Minor improvement everywhere

include/pininvdyn/contacts/contact-6d.hpp

@@ -60,19 +60,19 @@ namespace pininvdyn
       virtual const ConstraintBase & computeMotionTask(const double t,
                                                        ConstRefVector q,
                                                        ConstRefVector v,
-                                                       Data & data);
+                                                       const Data & data);
 
       virtual const ConstraintInequality & computeForceTask(const double t,
                                                             ConstRefVector q,
                                                             ConstRefVector v,
-                                                            Data & data);
+                                                            const Data & data);
 
       virtual const Matrix & getForceGeneratorMatrix();
 
       virtual const ConstraintEquality & computeForceRegularizationTask(const double t,
                                                                         ConstRefVector q,
                                                                         ConstRefVector v,
-                                                                        Data & data);
+                                                                        const Data & data);
 
       const TaskMotion & getMotionTask() const;
       const ConstraintBase & getMotionConstraint() const;

include/pininvdyn/contacts/contact-base.hpp

@@ -61,19 +61,19 @@ namespace pininvdyn
       virtual const ConstraintBase & computeMotionTask(const double t,
                                                        ConstRefVector q,
                                                        ConstRefVector v,
-                                                       Data & data) = 0;
+                                                       const Data & data) = 0;
 
       virtual const ConstraintInequality & computeForceTask(const double t,
                                                             ConstRefVector q,
                                                             ConstRefVector v,
-                                                            Data & data) = 0;
+                                                            const Data & data) = 0;
 
       virtual const Matrix & getForceGeneratorMatrix() = 0;
 
       virtual const ConstraintEquality & computeForceRegularizationTask(const double t,
-                                                                    ConstRefVector q,
-                                                                    ConstRefVector v,
-                                                                    Data & data) = 0;
+                                                                        ConstRefVector q,
+                                                                        ConstRefVector v,
+                                                                        const Data & data) = 0;
 
       virtual const TaskMotion & getMotionTask() const = 0;
       virtual const ConstraintBase & getMotionConstraint() const = 0;

include/pininvdyn/tasks/task-base.hpp

@@ -53,7 +53,7 @@ namespace pininvdyn
       virtual const ConstraintBase & compute(const double t,
                                              ConstRefVector q,
                                              ConstRefVector v,
-                                             Data & data) = 0;
+                                             const Data & data) = 0;
 
       virtual const ConstraintBase & getConstraint() const = 0;
       

include/pininvdyn/tasks/task-com-equality.hpp

@@ -50,11 +50,15 @@ namespace pininvdyn
       const ConstraintBase & compute(const double t,
                                      ConstRefVector q,
                                      ConstRefVector v,
-                                     Data & data);
+                                     const Data & data);
 
       const ConstraintBase & getConstraint() const;
 
       void setReference(const TrajectorySample & ref);
+      const TrajectorySample & getReference() const;
+
+      const Vector & getDesiredAcceleration() const;
+      Vector getAcceleration(ConstRefVector dv) const;
 
       const Vector & position_error() const;
       const Vector & velocity_error() const;
@@ -72,6 +76,8 @@ namespace pininvdyn
       Vector3 m_Kp;
       Vector3 m_Kd;
       Vector3 m_p_error, m_v_error;
+      Vector3 m_a_des;
+      Vector3 m_drift;
       Vector m_p_error_vec, m_v_error_vec;
       TrajectorySample m_ref;
       ConstraintEquality m_constraint;

include/pininvdyn/tasks/task-joint-posture.hpp

@@ -47,11 +47,15 @@ namespace pininvdyn
       const ConstraintBase & compute(const double t,
                                      ConstRefVector q,
                                      ConstRefVector v,
-                                     Data & data);
+                                     const Data & data);
 
       const ConstraintBase & getConstraint() const;
 
       void setReference(const TrajectorySample & ref);
+      const TrajectorySample & getReference() const;
+
+      const Vector & getDesiredAcceleration() const;
+      Vector getAcceleration(ConstRefVector dv) const;
 
       const Vector & mask() const;
       bool mask(const Vector & mask);
@@ -73,6 +77,7 @@ namespace pininvdyn
       Vector m_Kd;
       Vector m_p_error, m_v_error;
       Vector m_p, m_v;
+      Vector m_a_des;
       Vector m_mask;
       VectorXi m_activeAxes;
       TrajectorySample m_ref;

include/pininvdyn/tasks/task-motion.hpp

@@ -19,6 +19,7 @@
 #define __invdyn_task_motion_hpp__
 
 #include <pininvdyn/tasks/task-base.hpp>
+#include <pininvdyn/trajectories/trajectory-base.hpp>
 
 namespace pininvdyn
 {
@@ -30,16 +31,24 @@ namespace pininvdyn
     public:
       typedef pininvdyn::RobotWrapper RobotWrapper;
       typedef pininvdyn::math::Vector Vector;
+      typedef pininvdyn::trajectories::TrajectorySample TrajectorySample;
 
       TaskMotion(const std::string & name,
                  RobotWrapper & robot);
 
+      virtual const TrajectorySample & getReference() const = 0;
+
+      virtual const Vector & getDesiredAcceleration() const = 0;
+
+      virtual Vector getAcceleration(ConstRefVector dv) const = 0;
+
       virtual const Vector & position_error() const = 0;
       virtual const Vector & velocity_error() const = 0;
       virtual const Vector & position() const = 0;
       virtual const Vector & velocity() const = 0;
       virtual const Vector & position_ref() const = 0;
       virtual const Vector & velocity_ref() const = 0;
+
     };
   }
 }

include/pininvdyn/tasks/task-se3-equality.hpp

@@ -50,11 +50,15 @@ namespace pininvdyn
       const ConstraintBase & compute(const double t,
                                      ConstRefVector q,
                                      ConstRefVector v,
-                                     Data & data);
+                                     const Data & data);
 
       const ConstraintBase & getConstraint() const;
 
       void setReference(TrajectorySample & ref);
+      const TrajectorySample & getReference() const;
+
+      const Vector & getDesiredAcceleration() const;
+      Vector getAcceleration(ConstRefVector dv) const;
 
       const Vector & position_error() const;
       const Vector & velocity_error() const;
@@ -80,8 +84,10 @@ namespace pininvdyn
       Vector m_Kp;
       Vector m_Kd;
       Vector m_a_des;
+      Motion m_drift;
       Matrix6x m_J;
       ConstraintEquality m_constraint;
+      TrajectorySample m_ref;
     };
     
   }

src/contacts/contact-6d.cpp

@@ -198,7 +198,7 @@ void Contact6d::setReference(const SE3 & ref)
 const ConstraintBase & Contact6d::computeMotionTask(const double t,
                                                     ConstRefVector q,
                                                     ConstRefVector v,
-                                                    Data & data)
+                                                    const Data & data)
 {
   return m_motionTask.compute(t, q, v, data);
 }
@@ -206,7 +206,7 @@ const ConstraintBase & Contact6d::computeMotionTask(const double t,
 const ConstraintInequality & Contact6d::computeForceTask(const double t,
                                                          ConstRefVector q,
                                                          ConstRefVector v,
-                                                         Data & data)
+                                                         const Data & data)
 {
   return m_forceInequality;
 }
@@ -219,7 +219,7 @@ const Matrix & Contact6d::getForceGeneratorMatrix()
 const ConstraintEquality & Contact6d::computeForceRegularizationTask(const double t,
                                                                      ConstRefVector q,
                                                                      ConstRefVector v,
-                                                                     Data & data)
+                                                                     const Data & data)
 {
   return m_forceRegTask;
 }

src/inverse-dynamics-formulation-acc-force.cpp

@@ -221,7 +221,7 @@ const HqpData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
   {
     const ContactTransitionInfo * c = *it_ct;
     assert(c->time_start <= m_t);
-    if(m_t < c->time_end)
+    if(m_t <= c->time_end)
     {
       const double alpha = (m_t - c->time_start) / (c->time_end - c->time_start);
       const double fMax = c->fMax_start + alpha*(c->fMax_end - c->fMax_start);
@@ -229,6 +229,8 @@ const HqpData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
     }
     else
     {
+      std::cout<<"[InverseDynamicsFormulationAccForce] Remove contact "<<
+                 c->contactLevel->contact.name()<<" at time "<<time<<std::endl;
       removeRigidContact(c->contactLevel->contact.name());
       // FIXME: this won't work if multiple contact transitions occur at the same time
       // because after erasing an element the iterator is invalid

src/tasks/task-com-equality.cpp

@@ -22,6 +22,7 @@ namespace pininvdyn
   namespace tasks
   {
     using namespace pininvdyn::math;
+    using namespace pininvdyn::trajectories;
     using namespace se3;
 
     TaskComEquality::TaskComEquality(const std::string & name,
@@ -60,6 +61,21 @@ namespace pininvdyn
       m_ref = ref;
     }
 
+    const TrajectorySample & TaskComEquality::getReference() const
+    {
+      return m_ref;
+    }
+
+    const Vector & TaskComEquality::getDesiredAcceleration() const
+    {
+      return m_a_des;
+    }
+
+    Vector TaskComEquality::getAcceleration(ConstRefVector dv) const
+    {
+      return m_constraint.matrix()*dv - m_drift;
+    }
+
     const Vector & TaskComEquality::position_error() const
     {
       return m_p_error_vec;
@@ -98,17 +114,17 @@ namespace pininvdyn
     const ConstraintBase & TaskComEquality::compute(const double t,
                                                     ConstRefVector q,
                                                     ConstRefVector v,
-                                                    Data & data)
+                                                    const Data & data)
     {
-      Vector3 p_com, v_com, drift;
-      m_robot.com(data, p_com, v_com, drift);
+      Vector3 p_com, v_com;
+      m_robot.com(data, p_com, v_com, m_drift);
 
       // Compute errors
       m_p_error = p_com - m_ref.pos;
       m_v_error = v_com - m_ref.vel;
-      Vector3 m_a_des = - m_Kp.cwiseProduct(m_p_error)
-                        - m_Kd.cwiseProduct(m_v_error)
-                        + m_ref.acc;
+      m_a_des = - m_Kp.cwiseProduct(m_p_error)
+                - m_Kd.cwiseProduct(m_v_error)
+                + m_ref.acc;
 
       m_p_error_vec = m_p_error;
       m_v_error_vec = m_v_error;
@@ -121,7 +137,7 @@ namespace pininvdyn
       const Matrix3x & Jcom = m_robot.Jcom(data);
 
       m_constraint.setMatrix(Jcom);
-      m_constraint.setVector(m_a_des - drift);
+      m_constraint.setVector(m_a_des - m_drift);
       return m_constraint;
     }
     

src/tasks/task-joint-posture.cpp

@@ -22,6 +22,7 @@ namespace pininvdyn
   namespace tasks
   {
     using namespace pininvdyn::math;
+    using namespace pininvdyn::trajectories;
     using namespace se3;
 
     TaskJointPosture::TaskJointPosture(const std::string & name,
@@ -90,6 +91,21 @@ namespace pininvdyn
       m_ref = ref;
     }
 
+    const TrajectorySample & TaskJointPosture::getReference() const
+    {
+      return m_ref;
+    }
+
+    const Vector & TaskJointPosture::getDesiredAcceleration() const
+    {
+      return m_a_des;
+    }
+
+    Vector TaskJointPosture::getAcceleration(ConstRefVector dv) const
+    {
+      return m_constraint.matrix()*dv;
+    }
+
     const Vector & TaskJointPosture::position_error() const
     {
       return m_p_error;
@@ -128,16 +144,16 @@ namespace pininvdyn
     const ConstraintBase & TaskJointPosture::compute(const double t,
                                                     ConstRefVector q,
                                                     ConstRefVector v,
-                                                    Data & data)
+                                                    const Data & data)
     {
       // Compute errors
       m_p = q.tail(m_robot.nv()-6);
       m_v = v.tail(m_robot.nv()-6);
       m_p_error = m_p - m_ref.pos;
       m_v_error = m_v - m_ref.vel;
-      Vector m_a_des = - m_Kp.cwiseProduct(m_p_error)
-                       - m_Kd.cwiseProduct(m_v_error)
-                       + m_ref.acc;
+      m_a_des = - m_Kp.cwiseProduct(m_p_error)
+                - m_Kd.cwiseProduct(m_v_error)
+                + m_ref.acc;
 
       for(unsigned int i=0; i<m_activeAxes.size(); i++)
         m_constraint.vector()(i) = m_a_des(m_activeAxes(i));

src/tasks/task-se3-equality.cpp

@@ -22,6 +22,7 @@ namespace pininvdyn
   namespace tasks
   {
     using namespace pininvdyn::math;
+    using namespace pininvdyn::trajectories;
     using namespace se3;
 
     TaskSE3Equality::TaskSE3Equality(const std::string & name,
@@ -29,7 +30,8 @@ namespace pininvdyn
                                      const std::string & frameName):
       TaskMotion(name, robot),
       m_frame_name(frameName),
-      m_constraint(name, 6, robot.nv())
+      m_constraint(name, 6, robot.nv()),
+      m_ref(12, 6)
     {
       assert(m_robot.model().existFrame(frameName));
       m_frame_id = m_robot.model().getFrameId(frameName);
@@ -74,11 +76,17 @@ namespace pininvdyn
 
     void TaskSE3Equality::setReference(TrajectorySample & ref)
     {
+      m_ref = ref;
       vectorToSE3(ref.pos, m_M_ref);
       m_v_ref = Motion(ref.vel);
       m_a_ref = Motion(ref.acc);
     }
 
+    const TrajectorySample & TaskSE3Equality::getReference() const
+    {
+      return m_ref;
+    }
+
     const Vector & TaskSE3Equality::position_error() const
     {
       return m_p_error_vec;
@@ -109,6 +117,16 @@ namespace pininvdyn
       return m_v_ref_vec;
     }
 
+    const Vector & TaskSE3Equality::getDesiredAcceleration() const
+    {
+      return m_a_des;
+    }
+
+    Vector TaskSE3Equality::getAcceleration(ConstRefVector dv) const
+    {
+      return m_constraint.matrix()*dv - m_drift.toVector();
+    }
+
     const ConstraintBase & TaskSE3Equality::getConstraint() const
     {
       return m_constraint;
@@ -117,13 +135,13 @@ namespace pininvdyn
     const ConstraintBase & TaskSE3Equality::compute(const double t,
                                                     ConstRefVector q,
                                                     ConstRefVector v,
-                                                    Data & data)
+                                                    const Data & data)
     {
       SE3 oMi;
-      Motion v_frame, drift;
+      Motion v_frame;
       m_robot.framePosition(data, m_frame_id, oMi);
       m_robot.frameVelocity(data, m_frame_id, v_frame);
-      m_robot.frameClassicAcceleration(data, m_frame_id, drift);
+      m_robot.frameClassicAcceleration(data, m_frame_id, m_drift);
 
       // Transformation from local to world
       m_wMl.rotation(oMi.rotation());
@@ -144,16 +162,16 @@ namespace pininvdyn
 #endif
 
       // desired acc in local frame
-      m_a_des = - m_Kp.cwiseProduct(m_p_error.toVector_impl())
-                - m_Kd.cwiseProduct(m_v_error.toVector_impl())
-                + m_wMl.actInv(m_a_ref).toVector_impl();
+      m_a_des = - m_Kp.cwiseProduct(m_p_error.toVector())
+                - m_Kd.cwiseProduct(m_v_error.toVector())
+                + m_wMl.actInv(m_a_ref).toVector();
 
       // @todo Since Jacobian computation is cheaper in world frame
       // we could do all computations in world frame
       m_robot.frameJacobianLocal(data, m_frame_id, m_J);