Topic/hrg

binding/python/include/qp_wrapper.hpp

@@ -19,5 +19,10 @@ JointsSelector* UnactiveJoints2Ptr(const std::vector<rbd::MultiBody>& mbs, int r
   return new JointsSelector(JointsSelector::UnactiveJoints(mbs, robotIndex, hl, unactiveJointsNames));
 }
 
+std::shared_ptr<rbd::ForwardDynamics> FD2ShPtr(rbd::ForwardDynamics & fd)
+{
+  return std::shared_ptr<rbd::ForwardDynamics>(&fd, [](rbd::ForwardDynamics *){});
+}
+
 }
 }

binding/python/tasks/qp/c_qp.pxd

@@ -7,6 +7,7 @@ from sva.c_sva cimport *
 from rbdyn.c_rbdyn cimport *
 from sch.c_sch cimport *
 cimport tasks.c_tasks as c_tasks
+from libcpp.memory cimport shared_ptr
 from libcpp.string cimport string
 from libcpp.vector cimport vector
 from libcpp cimport bool
@@ -494,7 +495,7 @@ cdef extern from "<Tasks/QPTasks.h>" namespace "tasks::qp":
 
 cdef extern from "<Tasks/QPMotionConstr.h>" namespace "tasks::qp":
   cdef cppclass MotionPolyConstr(ConstraintFunction[GenInequality], GenInequality, Constraint):
-    MotionPolyConstr(const vector[MultiBody]&, int, const c_tasks.PolyTorqueBound&)
+    MotionPolyConstr(const vector[MultiBody]&, int, const shared_ptr[ForwardDynamics], const c_tasks.PolyTorqueBound&)
     # Motion default
     void computeTorque(const VectorXd&, const VectorXd&)
     VectorXd torque() const
@@ -504,8 +505,8 @@ cdef extern from "<Tasks/QPMotionConstr.h>" namespace "tasks::qp":
     void removeFromSolver(QPSolver &)
 
   cdef cppclass MotionConstr(ConstraintFunction[GenInequality], GenInequality, Constraint):
-    MotionConstr(const vector[MultiBody]&, int, const c_tasks.TorqueBound&)
-    MotionConstr(const vector[MultiBody]&, int, const c_tasks.TorqueBound&, const c_tasks.TorqueDBound&, double)
+    MotionConstr(const vector[MultiBody]&, int, const shared_ptr[ForwardDynamics], const c_tasks.TorqueBound&)
+    MotionConstr(const vector[MultiBody]&, int, const shared_ptr[ForwardDynamics], const c_tasks.TorqueBound&, const c_tasks.TorqueDBound&, double)
     # Motion default
     void computeTorque(const VectorXd&, const VectorXd&)
     VectorXd torque() const
@@ -515,8 +516,8 @@ cdef extern from "<Tasks/QPMotionConstr.h>" namespace "tasks::qp":
     void removeFromSolver(QPSolver &)
 
   cdef cppclass MotionSpringConstr(ConstraintFunction[GenInequality], GenInequality, Constraint):
-    MotionSpringConstr(const vector[MultiBody]&, int, const c_tasks.TorqueBound&, const vector[SpringJoint]&)
-    MotionSpringConstr(const vector[MultiBody]&, int, const c_tasks.TorqueBound&, const c_tasks.TorqueDBound&, double, const vector[SpringJoint]&)
+    MotionSpringConstr(const vector[MultiBody]&, int, const shared_ptr[ForwardDynamics], const c_tasks.TorqueBound&, const vector[SpringJoint]&)
+    MotionSpringConstr(const vector[MultiBody]&, int, const shared_ptr[ForwardDynamics], const c_tasks.TorqueBound&, const c_tasks.TorqueDBound&, double, const vector[SpringJoint]&)
     # Motion default
     void computeTorque(const VectorXd&, const VectorXd&)
     VectorXd torque() const

binding/python/tasks/qp/c_qp_private.pxd

@@ -10,3 +10,4 @@ from libcpp.vector cimport vector
 cdef extern from "qp_wrapper.hpp" namespace "tasks::qp":
     JointsSelector* ActiveJoints2Ptr(const vector[MultiBody]&, int, HighLevelTask*, const vector[string])
     JointsSelector* UnactiveJoints2Ptr(const vector[MultiBody]&, int, HighLevelTask*, const vector[string])
+    shared_ptr[ForwardDynamics] FD2ShPtr(ForwardDynamics & fd)

binding/python/tasks/qp/qp.pxd

@@ -3,6 +3,7 @@
 #
 
 cimport c_qp
+cimport rbdyn.rbdyn as rbdyn
 
 from libcpp.vector cimport vector
 from libcpp cimport bool as cppbool
@@ -154,18 +155,21 @@ cdef class GripperTorqueTask(Task):
 cdef class MotionConstr(GenInequality):
   cdef c_qp.MotionConstr * impl
   cdef cppbool __own_impl
+  cdef rbdyn.ForwardDynamics fd_instance
 
 cdef MotionConstr MotionConstrFromPtr(c_qp.MotionConstr *)
 
 cdef class MotionPolyConstr(GenInequality):
   cdef c_qp.MotionPolyConstr * impl
   cdef cppbool __own_impl
+  cdef rbdyn.ForwardDynamics fd_instance
 
 cdef MotionPolyConstr MotionPolyConstrFromPtr(c_qp.MotionPolyConstr *)
 
 cdef class MotionSpringConstr(GenInequality):
   cdef c_qp.MotionSpringConstr * impl
   cdef cppbool __own_impl
+  cdef rbdyn.ForwardDynamics fd_instance
 
 cdef MotionSpringConstr MotionSpringConstrFromPtr(c_qp.MotionSpringConstr *)
 

binding/python/tasks/qp/qp.pyx

@@ -1614,13 +1614,15 @@ cdef class MotionConstr(GenInequality):
   def __dealloc__(self):
     if self.__own_impl:
       del self.impl
-  def __cinit__(self, MultiBodyVector mbs, int robotIndex, tasks.TorqueBound tb, tasks.TorqueDBound tdb = None, dt = None, skip_alloc = False):
+  def __cinit__(self, MultiBodyVector mbs, int robotIndex, ForwardDynamics fd, tasks.TorqueBound tb, tasks.TorqueDBound tdb = None, dt = None, skip_alloc = False):
     self.__own_impl = True
     if not skip_alloc:
+      # Keep FD alive
+      self.fd_instance = fd
       if tdb is None:
-        self.impl = new c_qp.MotionConstr(deref(mbs.v), robotIndex, tb.impl)
+        self.impl = new c_qp.MotionConstr(deref(mbs.v), robotIndex, c_qp_private.FD2ShPtr(fd.impl), tb.impl)
       else:
-        self.impl = new c_qp.MotionConstr(deref(mbs.v), robotIndex, tb.impl, tdb.impl, dt)
+        self.impl = new c_qp.MotionConstr(deref(mbs.v), robotIndex, c_qp_private.FD2ShPtr(fd.impl), tb.impl, tdb.impl, dt)
       self.cf_base = self.impl
       self.genineq_base = self.impl
       self.constraint_base = self.impl
@@ -1656,10 +1658,11 @@ cdef class MotionPolyConstr(GenInequality):
   def __dealloc__(self):
     if self.__own_impl:
       del self.impl
-  def __cinit__(self, MultiBodyVector mbs, int robotIndex, tasks.PolyTorqueBound tb, skip_alloc = False):
+  def __cinit__(self, MultiBodyVector mbs, int robotIndex, ForwardDynamics fd, tasks.PolyTorqueBound tb, skip_alloc = False):
     self.__own_impl = True
     if not skip_alloc:
-      self.impl = new c_qp.MotionPolyConstr(deref(mbs.v), robotIndex, tb.impl)
+      self.fd_instance = fd
+      self.impl = new c_qp.MotionPolyConstr(deref(mbs.v), robotIndex, c_qp_private.FD2ShPtr(fd.impl), tb.impl)
       self.cf_base = self.impl
       self.genineq_base = self.impl
       self.constraint_base = self.impl
@@ -1695,13 +1698,14 @@ cdef class MotionSpringConstr(GenInequality):
   def __dealloc__(self):
     if self.__own_impl:
       del self.impl
-  def __cinit__(self, MultiBodyVector mbs, int robotIndex, tasks.TorqueBound tb, sjs, tasks.TorqueDBound tdb = None, dt = None, skip_alloc = False):
+  def __cinit__(self, MultiBodyVector mbs, int robotIndex, ForwardDynamics fd, tasks.TorqueBound tb, sjs, tasks.TorqueDBound tdb = None, dt = None, skip_alloc = False):
     self.__own_impl = True
     if not skip_alloc:
+      self.fd_instance = fd
       if tdb is None:
-        self.impl = new c_qp.MotionSpringConstr(deref(mbs.v), robotIndex, tb.impl, SpringJointVector(sjs).v)
+        self.impl = new c_qp.MotionSpringConstr(deref(mbs.v), robotIndex, c_qp_private.FD2ShPtr(fd.impl), tb.impl, SpringJointVector(sjs).v)
       else:
-        self.impl = new c_qp.MotionSpringConstr(deref(mbs.v), robotIndex, tb.impl, tdb.impl, dt, SpringJointVector(sjs).v)
+        self.impl = new c_qp.MotionSpringConstr(deref(mbs.v), robotIndex, c_qp_private.FD2ShPtr(fd.impl), tb.impl, tdb.impl, dt, SpringJointVector(sjs).v)
       self.cf_base = self.impl
       self.genineq_base = self.impl
       self.constraint_base = self.impl

binding/python/tests/TestQPMultiRobot.py

@@ -152,6 +152,9 @@ def test(self):
     mb1, mbc1Init = arms.makeZXZArm()
     rbdyn.forwardKinematics(mb1, mbc1Init)
     rbdyn.forwardVelocity(mb1, mbc1Init)
+    self.fd1 = rbdyn.ForwardDynamics(mb1)
+    self.fd1.computeH(mb1, mbc1Init)
+    self.fd1.computeC(mb1, mbc1Init)
 
     mb2, mbc2Init = arms.makeZXZArm(False)
     if not LEGACY:
@@ -164,6 +167,9 @@ def test(self):
       mbc2Init.q[0] = [mb2InitOri.w(), mb2InitOri.x(), mb2InitOri.y(), mb2InitOri.z(), mb2InitPos.x(), mb2InitPos.y() + 1, mb2InitPos.z()]
     rbdyn.forwardKinematics(mb2, mbc2Init)
     rbdyn.forwardVelocity(mb2, mbc2Init)
+    self.fd2 = rbdyn.ForwardDynamics(mb2)
+    self.fd2.computeH(mb2, mbc2Init)
+    self.fd2.computeC(mb2, mbc2Init)
 
     if not LEGACY:
       X_0_b1 = sva.PTransformd(mbc1Init.bodyPosW[-1])
@@ -228,8 +234,8 @@ def test(self):
     torqueMax1 = [[], [Inf], [Inf], [Inf]]
     torqueMin2 = [[0,0,0,0,0,0], [-Inf], [-Inf], [-Inf]]
     torqueMax2 = [[0,0,0,0,0,0], [Inf], [Inf], [Inf]]
-    motion1 = tasks.qp.MotionConstr(mbs, 0, tasks.TorqueBound(torqueMin1, torqueMax1))
-    motion2 = tasks.qp.MotionConstr(mbs, 1, tasks.TorqueBound(torqueMin2, torqueMax2))
+    motion1 = tasks.qp.MotionConstr(mbs, 0, self.fd1, tasks.TorqueBound(torqueMin1, torqueMax1))
+    motion2 = tasks.qp.MotionConstr(mbs, 1, self.fd2, tasks.TorqueBound(torqueMin2, torqueMax2))
     plCstr = tasks.qp.PositiveLambda()
 
     motion1.addToSolver(solver)
@@ -264,6 +270,10 @@ def test(self):
         rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001)
         rbdyn.forwardKinematics(mbs[i], mbcs[i])
         rbdyn.forwardVelocity(mbs[i], mbcs[i])
+      self.fd1.computeH(mbs[0], mbcs[0])
+      self.fd1.computeC(mbs[0], mbcs[0])
+      self.fd2.computeH(mbs[1], mbcs[1])
+      self.fd2.computeC(mbs[1], mbcs[1])
 
       # Check that the link hold
       if not LEGACY:

binding/python/tests/TestQPSolver.py

@@ -171,6 +171,9 @@ def setUp(self):
     rbdyn.forwardVelocity(mb, self.mbcInit)
     rbdyn.forwardKinematics(mbEnv, mbcEnv)
     rbdyn.forwardVelocity(mbEnv, mbcEnv)
+    self.fd = rbdyn.ForwardDynamics(mb)
+    self.fd.computeH(mb, self.mbcInit)
+    self.fd.computeC(mb, self.mbcInit)
 
     if not LEGACY:
       self.mbcs = rbdyn.MultiBodyConfigVector([self.mbcInit, mbcEnv])
@@ -201,6 +204,8 @@ def run_solver(self):
       rbdyn.eulerIntegration(self.mbs[0], self.mbcs[0], 0.001)
       rbdyn.forwardKinematics(self.mbs[0], self.mbcs[0])
       rbdyn.forwardVelocity(self.mbs[0], self.mbcs[0])
+      self.fd.computeH(self.mbs[0], self.mbcs[0])
+      self.fd.computeC(self.mbs[0], self.mbcs[0])
 
   def check_equality_constr(self, ConstrClass, *args):
     self.contVec = [tasks.qp.UnilateralContact(0, 1, "b3", "b0", [eigen.Vector3d.Zero()], eigen.Matrix3d.Identity(), sva.PTransformd.Identity(), 3, math.tan(math.pi/4))]
@@ -255,7 +260,7 @@ def test_motion_constr(self):
     Inf = float("inf")
     torqueMin = [[], [-Inf], [-Inf], [-Inf]]
     torqueMax = [[], [Inf], [Inf], [Inf]]
-    motionCstr = tasks.qp.MotionConstr(self.mbs, 0, tasks.TorqueBound(torqueMin, torqueMax))
+    motionCstr = tasks.qp.MotionConstr(self.mbs, 0, self.fd, tasks.TorqueBound(torqueMin, torqueMax))
     plCstr = tasks.qp.PositiveLambda()
 
     motionCstr.addToSolver(self.solver)
@@ -301,7 +306,7 @@ def test_motion_constr_w_contact(self):
     Inf = float("inf")
     torqueMin = [[], [-Inf], [-Inf], [-Inf]]
     torqueMax = [[], [Inf], [Inf], [Inf]]
-    motionCstr = tasks.qp.MotionConstr(self.mbs, 0, tasks.TorqueBound(torqueMin, torqueMax))
+    motionCstr = tasks.qp.MotionConstr(self.mbs, 0, self.fd, tasks.TorqueBound(torqueMin, torqueMax))
     plCstr = tasks.qp.PositiveLambda()
 
     motionCstr.addToSolver(self.solver)
@@ -473,6 +478,9 @@ def setUp(self):
 
     rbdyn.forwardKinematics(mb, self.mbcInit)
     rbdyn.forwardVelocity(mb, self.mbcInit)
+    self.fd = rbdyn.ForwardDynamics(mb)
+    self.fd.computeH(mb, self.mbcInit)
+    self.fd.computeC(mb, self.mbcInit)
 
     if not LEGACY:
       self.mbcs = rbdyn.MultiBodyConfigVector([self.mbcInit])
@@ -499,7 +507,7 @@ def tearDown(self):
   def test_motion_constr(self):
     lBound = [[], [-30], [-30], [-30]]
     uBound = [[], [30], [30], [30]]
-    motionCstr = tasks.qp.MotionConstr(self.mbs, 0, tasks.TorqueBound(lBound, uBound))
+    motionCstr = tasks.qp.MotionConstr(self.mbs, 0, self.fd, tasks.TorqueBound(lBound, uBound))
 
     self.solver.addGenInequalityConstraint(motionCstr)
     self.assertEqual(self.solver.nrGenInequalityConstraints(), 1)
@@ -522,6 +530,8 @@ def test_motion_constr(self):
       rbdyn.eulerIntegration(self.mbs[0], self.mbcs[0], 0.001)
       rbdyn.forwardKinematics(self.mbs[0], self.mbcs[0])
       rbdyn.forwardVelocity(self.mbs[0], self.mbcs[0])
+      self.fd.computeH(self.mbs[0], self.mbcs[0])
+      self.fd.computeC(self.mbs[0], self.mbcs[0])
       motionCstr.computeTorque(self.solver.alphaDVec(), self.solver.lambdaVec())
       motionCstr.torque(self.mbs, self.mbcs)
       if not LEGACY:
@@ -579,7 +589,7 @@ def test_motion_poly_constr(self):
     null = eigen.VectorXd()
     lBoundPoly = [[null], [lpoly], [lpoly], [lpoly]]
     uBoundPoly = [[null], [upoly], [upoly], [upoly]]
-    motionPolyCstr = tasks.qp.MotionPolyConstr(self.mbs, 0, tasks.PolyTorqueBound(lBoundPoly, uBoundPoly))
+    motionPolyCstr = tasks.qp.MotionPolyConstr(self.mbs, 0, self.fd, tasks.PolyTorqueBound(lBoundPoly, uBoundPoly))
 
     motionPolyCstr.addToSolver(self.solver)
     self.assertEqual(self.solver.nrGenInequalityConstraints(), 1)
@@ -803,6 +813,9 @@ def test(self):
     rbdyn.forwardVelocity(mb, mbcInit)
     rbdyn.forwardKinematics(mbEnv, mbcEnv)
     rbdyn.forwardVelocity(mbEnv, mbcEnv)
+    self.fd = rbdyn.ForwardDynamics(mb)
+    self.fd.computeH(mb, mbcInit)
+    self.fd.computeC(mb, mbcInit)
 
     if not LEGACY:
       mbs = rbdyn.MultiBodyVector([mb, mbEnv])
@@ -816,7 +829,7 @@ def test(self):
     Inf = float("inf")
     torqueMin = [[0, 0, 0, 0, 0, 0], [-Inf], [-Inf], [-Inf]]
     torqueMax = [[0, 0, 0, 0, 0, 0], [Inf], [Inf], [Inf]]
-    motionCstr = tasks.qp.MotionConstr(mbs, 0, tasks.TorqueBound(torqueMin, torqueMax))
+    motionCstr = tasks.qp.MotionConstr(mbs, 0, self.fd, tasks.TorqueBound(torqueMin, torqueMax))
     plCstr = tasks.qp.PositiveLambda()
     contCstrAcc = tasks.qp.ContactAccConstr()
 
@@ -858,6 +871,8 @@ def test(self):
       rbdyn.eulerIntegration(mbs[0], mbcs[0], 0.001)
       rbdyn.forwardKinematics(mbs[0], mbcs[0])
       rbdyn.forwardVelocity(mbs[0], mbcs[0])
+      self.fd.computeH(mbs[0], mbcs[0])
+      self.fd.computeC(mbs[0], mbcs[0])
 
     plCstr.removeFromSolver(solver)
     contCstrAcc.removeFromSolver(solver)

src/GenQPUtils.h

@@ -21,7 +21,7 @@ namespace qp
 {
 
 // Value add to the diagonal to ensure positive matrix
-static const double DIAG_CONSTANT = 1e-4;
+  static const double DIAG_CONSTANT = 1e-4;
 
 /**
  * Fill the \f$ Q \f$ matrix and the \f$ c \f$ vector based on the
@@ -112,6 +112,8 @@ inline int fillEq(const std::vector<Equality *> & eq,
                   Eigen::VectorXd & AL,
                   Eigen::VectorXd & AU)
 {
+  // std::cout << "Rafa, in GenQPUtils::fillEq, nrALines = ";
+
   for(std::size_t i = 0; i < eq.size(); ++i)
   {
     // ineq constraint can return a matrix with more line
@@ -120,13 +122,19 @@ inline int fillEq(const std::vector<Equality *> & eq,
     const Eigen::MatrixXd & Ai = eq[i]->AEq();
     const Eigen::VectorXd & bi = eq[i]->bEq();
 
+    // std::cout << "(" << eq[i]->nameEq() << ") ";  // Added by Rafa
+
     A.block(nrALines, 0, nrConstr, nrVars) = Ai.block(0, 0, nrConstr, nrVars);
     AL.segment(nrALines, nrConstr) = bi.head(nrConstr);
     AU.segment(nrALines, nrConstr) = bi.head(nrConstr);
 
     nrALines += nrConstr;
+
+    // std::cout << nrALines << " ";  // Added by Rafa
   }
 
+  // std::cout << std::endl;  // Added by Rafa
+
   return nrALines;
 }
 
@@ -141,6 +149,8 @@ inline int fillInEq(const std::vector<Inequality *> & inEq,
                     Eigen::VectorXd & AL,
                     Eigen::VectorXd & AU)
 {
+  // std::cout << "Rafa, in GenQPUtils::fillInEq, nrALines = ";
+
   for(std::size_t i = 0; i < inEq.size(); ++i)
   {
     // ineq constraint can return a matrix with more line
@@ -149,13 +159,19 @@ inline int fillInEq(const std::vector<Inequality *> & inEq,
     const Eigen::MatrixXd & Ai = inEq[i]->AInEq();
     const Eigen::VectorXd & bi = inEq[i]->bInEq();
 
+    // std::cout << "(" << inEq[i]->nameInEq() << ") ";  // Added by Rafa
+
     A.block(nrALines, 0, nrConstr, nrVars) = Ai.block(0, 0, nrConstr, nrVars);
     AL.segment(nrALines, nrConstr).fill(-std::numeric_limits<double>::infinity());
     AU.segment(nrALines, nrConstr) = bi.head(nrConstr);
 
     nrALines += nrConstr;
+
+    // std::cout << nrALines << " ";  // Added by Rafa
   }
 
+  // std::cout << std::endl;  // Added by Rafa
+
   return nrALines;
 }
 
@@ -170,6 +186,8 @@ inline int fillGenInEq(const std::vector<GenInequality *> & genInEq,
                        Eigen::VectorXd & AL,
                        Eigen::VectorXd & AU)
 {
+  // std::cout << "Rafa, in GenQPUtils::fillGenInEq, nrALines = ";
+
   for(std::size_t i = 0; i < genInEq.size(); ++i)
   {
     // ineq constraint can return a matrix with more line
@@ -179,13 +197,19 @@ inline int fillGenInEq(const std::vector<GenInequality *> & genInEq,
     const Eigen::VectorXd & ALi = genInEq[i]->LowerGenInEq();
     const Eigen::VectorXd & AUi = genInEq[i]->UpperGenInEq();
 
+    // std::cout << "(" << genInEq[i]->nameGenInEq() << ") ";  // Added by Rafa
+
     A.block(nrALines, 0, nrConstr, nrVars) = Ai.block(0, 0, nrConstr, nrVars);
     AL.segment(nrALines, nrConstr) = ALi.head(nrConstr);
     AU.segment(nrALines, nrConstr) = AUi.head(nrConstr);
 
     nrALines += nrConstr;
-  }
 
+    // std::cout << nrALines << " ";  // Added by Rafa
+  }
+
+  // std::cout << std::endl;  // Added by Rafa
+
   return nrALines;
 }