Add unit tests for joint bounds task

unittest/tasks.cpp

@@ -26,6 +26,7 @@
 #include <tsid/tasks/task-se3-equality.hpp>
 #include <tsid/tasks/task-com-equality.hpp>
 #include <tsid/tasks/task-joint-posture.hpp>
+#include <tsid/tasks/task-joint-bounds.hpp>
 
 #include <tsid/trajectories/trajectory-se3.hpp>
 #include <tsid/trajectories/trajectory-euclidian.hpp>
@@ -272,4 +273,50 @@ BOOST_AUTO_TEST_CASE ( test_task_joint_posture )
   }
 }
 
+
+BOOST_AUTO_TEST_CASE ( test_task_joint_bounds )
+{
+  cout<<"\n\n*********** TEST TASK JOINT BOUNDS ***********\n";
+  vector<string> package_dirs;
+  package_dirs.push_back(romeo_model_path);
+  string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
+  RobotWrapper robot(urdfFileName,
+                     package_dirs,
+                     pinocchio::JointModelFreeFlyer(),
+                     false);
+  const unsigned int na = robot.nv()-6;
+  const double dt = 0.001;
+
+  cout<<"Gonna create task\n";
+  TaskJointBounds task("task-joint-bounds", robot, dt);
+
+  cout<<"Gonna set limits\n"<<na<<endl;
+  VectorXd dq_max = VectorXd::Ones(na);
+  VectorXd dq_min = -dq_max;
+  task.setVelocityBounds(dq_min, dq_max);
+
+  BOOST_CHECK(task.getVelocityLowerBounds().isApprox(dq_min));
+  BOOST_CHECK(task.getVelocityUpperBounds().isApprox(dq_max));
+
+  cout<<"Gonna set up for simulation\n";
+  double t = 0.0;
+
+  VectorXd q = neutral(robot.model());
+  VectorXd v = VectorXd::Zero(robot.nv());
+  pinocchio::Data data(robot.model());
+  for(int i=0; i<max_it; i++)
+  {
+    robot.computeAllTerms(data, q, v);
+    const ConstraintBase & constraint = task.compute(t, q, v, data);
+    BOOST_CHECK(constraint.rows()==robot.nv());
+    BOOST_CHECK(static_cast<tsid::math::Index>(constraint.cols())==static_cast<tsid::math::Index>(robot.nv()));
+    BOOST_REQUIRE(isFinite(constraint.lowerBound()));
+    BOOST_REQUIRE(isFinite(constraint.upperBound()));
+
+    BOOST_REQUIRE(isFinite(v));
+    BOOST_REQUIRE(isFinite(q));
+    t += dt;
+  }
+}
+
 BOOST_AUTO_TEST_SUITE_END ()

unittest/tsid-formulation.cpp

@@ -26,6 +26,7 @@
 #include <tsid/tasks/task-com-equality.hpp>
 #include <tsid/tasks/task-se3-equality.hpp>
 #include <tsid/tasks/task-joint-posture.hpp>
+#include <tsid/tasks/task-joint-bounds.hpp>
 #include <tsid/trajectories/trajectory-euclidian.hpp>
 #include <tsid/solvers/solver-HQP-factory.hxx>
 #include <tsid/solvers/utils.hpp>
@@ -99,12 +100,13 @@ class StandardRomeoInvDynCtrl
   Contact6d * contactLF;
   TaskComEquality * comTask;
   TaskJointPosture * postureTask;
+  TaskJointBounds * jointBoundsTask;
   Vector q;
   Vector v;
   pinocchio::SE3 H_rf_ref;
   pinocchio::SE3 H_lf_ref;
 
-  StandardRomeoInvDynCtrl() : t(0.)
+  StandardRomeoInvDynCtrl(double dt) : t(0.)
   {
     vector<string> package_dirs;
     package_dirs.push_back(romeo_model_path);
@@ -162,6 +164,13 @@ class StandardRomeoInvDynCtrl
     postureTask->Kp(kp_posture*Vector::Ones(nv-6));
     postureTask->Kd(2.0*postureTask->Kp().cwiseSqrt());
     tsid->addMotionTask(*postureTask, w_posture, 1);
+
+    // Add the joint bounds task
+    jointBoundsTask = new TaskJointBounds("task-joint-bounds", *robot, dt);
+    Vector dq_max = 10*Vector::Ones(robot->na());
+    Vector dq_min = -dq_max;
+    jointBoundsTask->setVelocityBounds(dq_min, dq_max);
+    tsid->addMotionTask(*jointBoundsTask, 1.0, 0);
   }
 };
 
@@ -194,7 +203,7 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_remove_contact )
   const double w_RF = 1e3;
   double t = 0.0;
 
-  StandardRomeoInvDynCtrl romeo_inv_dyn;
+  StandardRomeoInvDynCtrl romeo_inv_dyn(dt);
   RobotWrapper & robot = *(romeo_inv_dyn.robot);
   InverseDynamicsFormulationAccForce * tsid = romeo_inv_dyn.tsid;
   Contact6d & contactRF = *(romeo_inv_dyn.contactRF);
@@ -323,7 +332,7 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force )
   const unsigned int PRINT_N = 100;
   double t = 0.0;
 
-  StandardRomeoInvDynCtrl romeo_inv_dyn;
+  StandardRomeoInvDynCtrl romeo_inv_dyn(dt);
   RobotWrapper & robot = *(romeo_inv_dyn.robot);
   InverseDynamicsFormulationAccForce * tsid = romeo_inv_dyn.tsid;
   Contact6d & contactRF = *(romeo_inv_dyn.contactRF);
@@ -555,8 +564,8 @@ BOOST_AUTO_TEST_CASE ( test_contact_point_invdyn_formulation_acc_force )
   for (int i = 0; i < 4; i++) {
     ContactPoint* cp = new ContactPoint("contact_" + contactFrames[i], robot,
         contactFrames[i], contactNormal, mu, fMin, fMax);
-    cp->Kp(kp_contact*Vector::Ones(6));
-    cp->Kd(2.0*cp->Kp().cwiseSqrt());
+    cp->Kp(kp_contact*Vector::Ones(3));
+    cp->Kd(2.0*sqrt(kp_contact)*Vector::Ones(3));
     cp->setReference(robot.framePosition(data, robot.model().getFrameId(contactFrames[i])));
     cp->useLocalFrame(false);
     tsid->addRigidContact(*cp, w_forceReg, 1.0, 1);
@@ -648,7 +657,7 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_computation_time )
   const double dt = 0.001;
   double t = 0.0;
 
-  StandardRomeoInvDynCtrl romeo_inv_dyn;
+  StandardRomeoInvDynCtrl romeo_inv_dyn(dt);
   RobotWrapper & robot = *(romeo_inv_dyn.robot);
   InverseDynamicsFormulationAccForce * tsid = romeo_inv_dyn.tsid;
   TaskComEquality & comTask = *(romeo_inv_dyn.comTask);
@@ -672,7 +681,7 @@ BOOST_AUTO_TEST_CASE ( test_invdyn_formulation_acc_force_computation_time )
   SolverHQPBase * solver_fast = SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG_FAST,
                                                                   "eiquadprog-fast");
   SolverHQPBase * solver_rt =
-      SolverHQPFactory::createNewSolver<61,18,34>(SOLVER_HQP_EIQUADPROG_RT,
+      SolverHQPFactory::createNewSolver<61,18,71>(SOLVER_HQP_EIQUADPROG_RT,
                                                   "eiquadprog-rt");
 
   solver->resize(tsid->nVar(), tsid->nEq(), tsid->nIn());