Merge pull request #139 from nim65s/devel

Lambda is a reserved keyword

src/algorithm/dynamics.hpp

@@ -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;
     

src/multibody/model.hpp

@@ -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;

src/multibody/model.hxx

@@ -281,7 +281,7 @@ namespace se3
     ,upperPositionLimit(ref.nq)
     ,JMinvJt()
     ,llt_JMinvJt()
-    ,lambda()
+    ,lambda_c()
     ,sDUiJt(ref.nv,ref.nv)
     ,torque_residual(ref.nv)
   {

src/python/data.hpp

@@ -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()

unittest/dynamics.cpp

@@ -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);
 }