Add task for joint velocity and acceleration bounds.

include/tsid/tasks/task-joint-bounds.hpp

@@ -15,15 +15,61 @@
 // <http://www.gnu.org/licenses/>.
 //
 
-#ifndef __invdyn_tasks_fwd_hpp__
-#define __invdyn_tasks_fwd_hpp__
+#ifndef __invdyn_task_joint_bounds_hpp__
+#define __invdyn_task_joint_bounds_hpp__
+
+#include <tsid/tasks/task-motion.hpp>
+#include <tsid/math/constraint-bound.hpp>
 
 namespace tsid
 {
   namespace tasks
   {
-    
+
+    class TaskJointBounds : public TaskMotion
+    {
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+      typedef math::Vector Vector;
+      typedef math::ConstraintBound ConstraintBound;
+      typedef pinocchio::Data Data;
+
+      TaskJointBounds(const std::string & name,
+                      RobotWrapper & robot,
+                      double dt);
+
+      int dim() const;
+
+      const ConstraintBase & compute(const double t,
+                                     ConstRefVector q,
+                                     ConstRefVector v,
+                                     const Data & data);
+
+      const ConstraintBase & getConstraint() const;
+
+      void setTimeStep(double dt);
+      void setVelocityBounds(ConstRefVector lower, ConstRefVector upper);
+      void setAccelerationBounds(ConstRefVector lower, ConstRefVector upper);
+      const Vector & getAccelerationLowerBounds() const;
+      const Vector & getAccelerationUpperBounds() const;
+      const Vector & getVelocityLowerBounds() const;
+      const Vector & getVelocityUpperBounds() const;
+
+//      const Vector & mask() const;
+//      void mask(const Vector & mask);
+
+    protected:
+      double m_dt;
+      int m_na, m_nv;
+      Vector m_v_lb, m_v_ub;
+      Vector m_a_lb, m_a_ub;
+      Vector m_ddq_max_due_to_vel, m_ddq_min_due_to_vel;
+//      Vector m_mask;
+      ConstraintBound m_constraint;
+    };
+
   }
 }
 
-#endif // ifndef __invdyn_tasks_fwd_hpp__
+#endif // ifndef __invdyn_task_joint_bounds_hpp__

include/tsid/tasks/task-motion.hpp

@@ -36,24 +36,25 @@ namespace tsid
       TaskMotion(const std::string & name,
                  RobotWrapper & robot);
 
-      virtual const TrajectorySample & getReference() const = 0;
+      virtual const TrajectorySample & getReference() const;
 
-      virtual const Vector & getDesiredAcceleration() const = 0;
+      virtual const Vector & getDesiredAcceleration() const;
 
-      virtual Vector getAcceleration(ConstRefVector dv) const = 0;
+      virtual Vector getAcceleration(ConstRefVector dv) const;
 
-      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;
+      virtual const Vector & position_error() const;
+      virtual const Vector & velocity_error() const;
+      virtual const Vector & position() const;
+      virtual const Vector & velocity() const;
+      virtual const Vector & position_ref() const;
+      virtual const Vector & velocity_ref() const;
 
       virtual void setMask(math::ConstRefVector mask);
       virtual bool hasMask();
 
     protected:
       math::Vector m_mask;
+      math::Vector m_dummy;
     };
   }
 }

src/contacts/contact-point.cpp

@@ -119,8 +119,21 @@ unsigned int ContactPoint::n_force() const { return 3; }
 
 const Vector & ContactPoint::Kp() const { return m_motionTask.Kp().head<3>(); }
 const Vector & ContactPoint::Kd() const { return m_motionTask.Kd().head<3>(); }
-void ContactPoint::Kp(ConstRefVector Kp){ Vector6 Kp6; Kp6.head<3>() = Kp; m_motionTask.Kp(Kp6); }
-void ContactPoint::Kd(ConstRefVector Kd){ Vector6 Kd6; Kd6.head<3>() = Kd; m_motionTask.Kd(Kd6); }
+void ContactPoint::Kp(ConstRefVector Kp)
+{
+  assert(Kp.size()==3);
+  Vector6 Kp6;
+  Kp6.head<3>() = Kp;
+  m_motionTask.Kp(Kp6);
+}
+
+void ContactPoint::Kd(ConstRefVector Kd)
+{
+  assert(Kd.size()==3);
+  Vector6 Kd6;
+  Kd6.head<3>() = Kd;
+  m_motionTask.Kd(Kd6);
+}
 
 bool ContactPoint::setContactNormal(ConstRefVector contactNormal)
 {

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

@@ -107,14 +107,15 @@ void InverseDynamicsFormulationAccForce::addTask(TaskLevel* tl,
     if(priorityLevel==0)
       m_eq += c.rows();
   }
-  else if(c.isInequality())
+  else //if(c.isInequality())
   {
     tl->constraint = new ConstraintInequality(c.name(), c.rows(), m_v+m_k);
     if(priorityLevel==0)
       m_in += c.rows();
   }
-  else
-    tl->constraint = new ConstraintBound(c.name(), m_v+m_k);
+  // don't use bounds for now because EiQuadProg doesn't exploit them anyway
+//  else
+//    tl->constraint = new ConstraintBound(c.name(), m_v+m_k);
   m_hqpData[priorityLevel].push_back(make_pair<double, ConstraintBase*>(weight, tl->constraint));
 }
 
@@ -376,8 +377,9 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
     }
     else
     {
-      (*it)->constraint->lowerBound().head(m_v) = c.lowerBound();
-      (*it)->constraint->upperBound().head(m_v) = c.upperBound();
+      (*it)->constraint->matrix().leftCols(m_v) = Matrix::Identity(m_v, m_v);
+      (*it)->constraint->lowerBound() = c.lowerBound();
+      (*it)->constraint->upperBound() = c.upperBound();
     }
   }
 

src/tasks/task-joint-bounds.cpp

@@ -15,14 +15,102 @@
 // <http://www.gnu.org/licenses/>.
 //
 
-#include <Eigen/Dense>
-#include <pinocchio/multibody/model.hpp>
+#include <tsid/tasks/task-joint-bounds.hpp>
+#include "tsid/robots/robot-wrapper.hpp"
 
 namespace tsid
 {
   namespace tasks
   {
-    
-    
+    using namespace math;
+    using namespace trajectories;
+    using namespace pinocchio;
+
+    TaskJointBounds::TaskJointBounds(const std::string & name,
+                                     RobotWrapper & robot,
+                                     double dt):
+      TaskMotion(name, robot),
+      m_constraint(name, robot.nv()),
+      m_dt(dt),
+      m_nv(robot.nv()),
+      m_na(robot.na())
+    {
+      assert(dt>0.0);
+      m_v_lb = -1e10*Vector::Ones(m_na);
+      m_v_ub = +1e10*Vector::Ones(m_na);
+      m_a_lb = -1e10*Vector::Ones(m_na);
+      m_a_ub = +1e10*Vector::Ones(m_na);
+      m_ddq_max_due_to_vel.setZero(m_na);
+      m_ddq_max_due_to_vel.setZero(m_na);
+
+      int offset = m_nv-m_na;
+      for(int i=0; i<offset; i++)
+      {
+        m_constraint.upperBound()(i) = 1e10;
+        m_constraint.lowerBound()(i) = -1e10;
+      }
+    }
+
+    int TaskJointBounds::dim() const
+    { return m_nv; }
+
+    const Vector & TaskJointBounds::getAccelerationLowerBounds() const
+    { return m_a_lb; }
+
+    const Vector & TaskJointBounds::getAccelerationUpperBounds() const
+    { return m_a_ub; }
+
+    const Vector & TaskJointBounds::getVelocityLowerBounds() const
+    { return m_v_lb; }
+
+    const Vector & TaskJointBounds::getVelocityUpperBounds() const
+    { return m_v_ub; }
+
+    void TaskJointBounds::setTimeStep(double dt)
+    {
+      assert(dt>0);
+      m_dt = dt;
+    }
+
+    void TaskJointBounds::setVelocityBounds(ConstRefVector lower, ConstRefVector upper)
+    {
+      assert(lower.size()==m_na);
+      assert(upper.size()==m_na);
+      m_v_lb = lower;
+      m_v_ub = upper;
+    }
+
+    void TaskJointBounds::setAccelerationBounds(ConstRefVector lower, ConstRefVector upper)
+    {
+      assert(lower.size()==m_na);
+      assert(upper.size()==m_na);
+      m_a_lb = lower;
+      m_a_ub = upper;
+    }
+
+    const ConstraintBase & TaskJointBounds::getConstraint() const
+    {
+      return m_constraint;
+    }
+
+    const ConstraintBase & TaskJointBounds::compute(const double ,
+                                                    ConstRefVector ,
+                                                    ConstRefVector v,
+                                                    const Data & )
+    {
+      // compute min/max joint acc imposed by velocity limits
+      m_ddq_max_due_to_vel = (m_v_ub - v.tail(m_na))/m_dt;
+      m_ddq_min_due_to_vel = (m_v_lb - v.tail(m_na))/m_dt;
+
+      // take most conservative limit between vel and acc
+      int offset = m_nv-m_na;
+      for(int i=0; i<m_na; i++)
+      {
+        m_constraint.upperBound()(offset+i) = std::min(m_ddq_max_due_to_vel(i), m_a_ub(i));
+        m_constraint.lowerBound()(offset+i) = std::max(m_ddq_min_due_to_vel(i), m_a_lb(i));
+      }
+      return m_constraint;
+    }
+
   }
 }

src/tasks/task-motion.cpp

@@ -21,6 +21,10 @@ namespace tsid
 {
   namespace tasks
   {
+
+    typedef math::Vector Vector;
+    typedef trajectories::TrajectorySample TrajectorySample;
+
     TaskMotion::TaskMotion(const std::string & name,
                            RobotWrapper & robot):
       TaskBase(name, robot)
@@ -35,5 +39,19 @@ namespace tsid
     {
       return m_mask.size() > 0;
     }
+
+    const TrajectorySample & TaskMotion::getReference() const { return TrajectorySample(); }
+
+    const Vector & TaskMotion::getDesiredAcceleration() const  { return m_dummy; }
+
+    Vector TaskMotion::getAcceleration(ConstRefVector ) const  { return m_dummy; }
+
+    const Vector & TaskMotion::position_error() const { return m_dummy; }
+    const Vector & TaskMotion::velocity_error() const  { return m_dummy; }
+    const Vector & TaskMotion::position() const  { return m_dummy; }
+    const Vector & TaskMotion::velocity() const  { return m_dummy; }
+    const Vector & TaskMotion::position_ref() const  { return m_dummy; }
+    const Vector & TaskMotion::velocity_ref() const  { return m_dummy; }
+
   }
 }