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 ...@@ -43,7 +43,7 @@ namespace se3
/// \param[in] gamma The drift of the constraints (dim nb_constraints). /// \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. /// \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, inline const Eigen::VectorXd & forwardDynamics(const Model & model,
Data & data, Data & data,
...@@ -62,7 +62,7 @@ namespace se3 ...@@ -62,7 +62,7 @@ namespace se3
assert(J.rows() == gamma.size()); assert(J.rows() == gamma.size());
Eigen::VectorXd & a = data.ddq; Eigen::VectorXd & a = data.ddq;
Eigen::VectorXd & lambda = data.lambda; Eigen::VectorXd & lambda_c = data.lambda_c;
if (updateKinematics) if (updateKinematics)
computeAllTerms(model, data, q, v); computeAllTerms(model, data, q, v);
...@@ -83,11 +83,11 @@ namespace se3 ...@@ -83,11 +83,11 @@ namespace se3
data.llt_JMinvJt.compute(data.JMinvJt); data.llt_JMinvJt.compute(data.JMinvJt);
// Compute the Lagrange Multipliers // Compute the Lagrange Multipliers
lambda = -gamma -J*data.torque_residual; lambda_c = -gamma -J*data.torque_residual;
data.llt_JMinvJt.solveInPlace (lambda); data.llt_JMinvJt.solveInPlace (lambda_c);
// Compute the joint acceleration // Compute the joint acceleration
a = J.transpose() * lambda; a = J.transpose() * lambda_c;
cholesky::solve (model, data, a); cholesky::solve (model, data, a);
a += data.torque_residual; a += data.torque_residual;
......
...@@ -482,7 +482,7 @@ namespace se3 ...@@ -482,7 +482,7 @@ namespace se3
Eigen::LLT<Eigen::MatrixXd> llt_JMinvJt; Eigen::LLT<Eigen::MatrixXd> llt_JMinvJt;
/// \brief Lagrange Multipliers corresponding to contact forces. /// \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$. /// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$.
Eigen::MatrixXd sDUiJt; Eigen::MatrixXd sDUiJt;
......
...@@ -281,7 +281,7 @@ namespace se3 ...@@ -281,7 +281,7 @@ namespace se3
,upperPositionLimit(ref.nq) ,upperPositionLimit(ref.nq)
,JMinvJt() ,JMinvJt()
,llt_JMinvJt() ,llt_JMinvJt()
,lambda() ,lambda_c()
,sDUiJt(ref.nv,ref.nv) ,sDUiJt(ref.nv,ref.nv)
,torque_residual(ref.nv) ,torque_residual(ref.nv)
{ {
......
...@@ -113,7 +113,7 @@ namespace se3 ...@@ -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,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(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 ...@@ -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,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(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 --------------------------------------------------------- */ /* --- Expose --------------------------------------------------------- */
static void expose() static void expose()
......
...@@ -78,7 +78,7 @@ BOOST_AUTO_TEST_CASE ( test_FD ) ...@@ -78,7 +78,7 @@ BOOST_AUTO_TEST_CASE ( test_FD )
BOOST_CHECK(H_ref.isApprox(JMinvJt,1-12)); BOOST_CHECK(H_ref.isApprox(JMinvJt,1-12));
VectorXd lambda_ref = -JMinvJt.inverse() * (J*Minv*(tau - data.nle) + gamma); 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); VectorXd a_ref = Minv*(tau - data.nle + J.transpose()*lambda_ref);
...@@ -88,7 +88,7 @@ BOOST_AUTO_TEST_CASE ( test_FD ) ...@@ -88,7 +88,7 @@ BOOST_AUTO_TEST_CASE ( test_FD )
Eigen::VectorXd constraint_residual (J * data.ddq + gamma); Eigen::VectorXd constraint_residual (J * data.ddq + gamma);
BOOST_CHECK(constraint_residual.norm() <= 1e-12); 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); 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