Compatility with pinocchio 3

Author: EtienneAr

src/inverse-dynamics/kinodynamics.cpp

@@ -77,15 +77,15 @@ KinodynamicsID::KinodynamicsID(const RobotModelHandler & model_handler, double c
   boundsTask_->setPositionBounds(
     model_handler_.getModel().lowerPositionLimit.tail(nq_actuated),
     model_handler_.getModel().upperPositionLimit.tail(nq_actuated));
-  boundsTask_->setVelocityBounds(model_handler_.getModel().upperVelocityLimit.tail(nu));
+  boundsTask_->setVelocityBounds(model_handler_.getModel().velocityLimit.tail(nu));
   boundsTask_->setImposeBounds(
     true, true, true, false); // For now do not impose acceleration bound as it is not provided in URDF
   formulation_.addMotionTask(*boundsTask_, 1.0, 0); // No weight needed as it is set as constraint
 
   // Add actuation limit task
   actuationTask_ = std::make_shared<tsid::tasks::TaskActuationBounds>("actuation-limits", robot_);
   actuationTask_->setBounds(
-    model_handler_.getModel().lowerEffortLimit.tail(nu), model_handler_.getModel().upperEffortLimit.tail(nu));
+    -model_handler_.getModel().effortLimit.tail(nu), model_handler_.getModel().effortLimit.tail(nu));
   formulation_.addActuationTask(*actuationTask_, 1.0, 0); // No weight needed as it is set as constraint
 
   // Create an HQP solver

tests/inverse-dynamics/centroidal.cpp

@@ -82,11 +82,11 @@ class TestCentroidalID
     {
       BOOST_CHECK_LE(q[7 + i], model.upperPositionLimit[7 + i]);
       BOOST_CHECK_GE(q[7 + i], model.lowerPositionLimit[7 + i]);
-      BOOST_CHECK_LE(dq[6 + i], model.upperVelocityLimit[6 + i]);
+      BOOST_CHECK_LE(dq[6 + i], model.velocityLimit[6 + i]);
       // Do not use lower velocity bound as TSID cannot handle it
-      BOOST_CHECK_GE(dq[6 + i], -model.upperVelocityLimit[6 + i]);
-      BOOST_CHECK_LE(tau[i], model.upperEffortLimit[6 + i]);
-      BOOST_CHECK_GE(tau[i], model.lowerEffortLimit[6 + i]);
+      BOOST_CHECK_GE(dq[6 + i], -model.velocityLimit[6 + i]);
+      BOOST_CHECK_LE(tau[i], model.effortLimit[6 + i]);
+      BOOST_CHECK_GE(tau[i], -model.effortLimit[6 + i]);
     }
   }
 

tests/inverse-dynamics/kinodynamics.cpp

@@ -82,11 +82,11 @@ class TestKinoID
     {
       BOOST_CHECK_LE(q[7 + i], model.upperPositionLimit[7 + i]);
       BOOST_CHECK_GE(q[7 + i], model.lowerPositionLimit[7 + i]);
-      BOOST_CHECK_LE(dq[6 + i], model.upperVelocityLimit[6 + i]);
+      BOOST_CHECK_LE(dq[6 + i], model.velocityLimit[6 + i]);
       // Do not use lower velocity bound as TSID cannot handle it
-      BOOST_CHECK_GE(dq[6 + i], -model.upperVelocityLimit[6 + i]);
-      BOOST_CHECK_LE(tau[i], model.upperEffortLimit[6 + i]);
-      BOOST_CHECK_GE(tau[i], model.lowerEffortLimit[6 + i]);
+      BOOST_CHECK_GE(dq[6 + i], -model.velocityLimit[6 + i]);
+      BOOST_CHECK_LE(tau[i], model.effortLimit[6 + i]);
+      BOOST_CHECK_GE(tau[i], -model.effortLimit[6 + i]);
     }
   }