Commit 1247fe01 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

Merge pull request #139 from nim65s/devel

Lambda is a reserved keyword
parents 8e4ff872 514ac28b
......@@ -43,7 +43,7 @@ namespace se3
/// \param[in] gamma The drift of the constraints (dim nb_constraints).
/// \param[in] updateKinematics If true, the algorithm calls first se3::computeAllTerms. Otherwise, it uses the current dynamic values stored in data.
///
/// \return A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda vector.
/// \return A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers linked to the contact forces are available throw data.lambda_c vector.
///
inline const Eigen::VectorXd & forwardDynamics(const Model & model,
Data & data,
......@@ -62,7 +62,7 @@ namespace se3
assert(J.rows() == gamma.size());
Eigen::VectorXd & a = data.ddq;
Eigen::VectorXd & lambda = data.lambda;
Eigen::VectorXd & lambda_c = data.lambda_c;
if (updateKinematics)
computeAllTerms(model, data, q, v);
......@@ -83,11 +83,11 @@ namespace se3
data.llt_JMinvJt.compute(data.JMinvJt);
// Compute the Lagrange Multipliers
lambda = -gamma -J*data.torque_residual;
data.llt_JMinvJt.solveInPlace (lambda);
lambda_c = -gamma -J*data.torque_residual;
data.llt_JMinvJt.solveInPlace (lambda_c);
// Compute the joint acceleration
a = J.transpose() * lambda;
a = J.transpose() * lambda_c;
cholesky::solve (model, data, a);
a += data.torque_residual;
......
......@@ -482,7 +482,7 @@ namespace se3
Eigen::LLT<Eigen::MatrixXd> llt_JMinvJt;
/// \brief Lagrange Multipliers corresponding to contact forces.
Eigen::VectorXd lambda;
Eigen::VectorXd lambda_c;
/// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$.
Eigen::MatrixXd sDUiJt;
......
......@@ -281,7 +281,7 @@ namespace se3
,upperPositionLimit(ref.nq)
,JMinvJt()
,llt_JMinvJt()
,lambda()
,lambda_c()
,sDUiJt(ref.nv,ref.nv)
,torque_residual(ref.nv)
{
......
......@@ -113,7 +113,7 @@ namespace se3
.ADD_DATA_PROPERTY_CONST(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)")
.ADD_DATA_PROPERTY_CONST(double,potential_energy,"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,lambda,"Lagrange Multipliers linked to contact forces")
.ADD_DATA_PROPERTY_CONST(Eigen::VectorXd,lambda_c,"Lagrange Multipliers linked to contact forces")
;
}
......@@ -161,7 +161,7 @@ namespace se3
IMPL_DATA_PROPERTY_CONST(double,kinetic_energy,"Kinetic energy in [J] computed by kineticEnergy(model,data,q,v,True/False)")
IMPL_DATA_PROPERTY_CONST(double,potential_energy,"Potential energy in [J] computed by potentialEnergy(model,data,q,True/False)")
IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,lambda,"Lagrange Multipliers linked to contact forces")
IMPL_DATA_PROPERTY_CONST(Eigen::VectorXd,lambda_c,"Lagrange Multipliers linked to contact forces")
/* --- Expose --------------------------------------------------------- */
static void expose()
......
......@@ -78,7 +78,7 @@ BOOST_AUTO_TEST_CASE ( test_FD )
BOOST_CHECK(H_ref.isApprox(JMinvJt,1-12));
VectorXd lambda_ref = -JMinvJt.inverse() * (J*Minv*(tau - data.nle) + gamma);
BOOST_CHECK(data.lambda.isApprox(lambda_ref, 1e-12));
BOOST_CHECK(data.lambda_c.isApprox(lambda_ref, 1e-12));
VectorXd a_ref = Minv*(tau - data.nle + J.transpose()*lambda_ref);
......@@ -88,7 +88,7 @@ BOOST_AUTO_TEST_CASE ( test_FD )
Eigen::VectorXd constraint_residual (J * data.ddq + gamma);
BOOST_CHECK(constraint_residual.norm() <= 1e-12);
Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda);
Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda_c);
BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment