Add com-equality-task. Add unit tests for com-equality and se3 tasks. Add...

Add com-equality-task. Add unit tests for com-equality and se3 tasks. Add functions to compute pseudo-inverses.

CMakeLists.txt

@@ -95,6 +95,7 @@ SET(${PROJECT_NAME}_TASKS_HEADERS
     include/pininvdyn/tasks/task-motion.hpp
     include/pininvdyn/tasks/task-actuation.hpp
     include/pininvdyn/tasks/task-contact-force.hpp
+    include/pininvdyn/tasks/task-com-equality.hpp
     include/pininvdyn/tasks/task-se3-equality.hpp
     include/pininvdyn/tasks/task-contact-force-equality.hpp
     include/pininvdyn/tasks/task-actuation-equality.hpp

include/pininvdyn/math/utils.hpp

@@ -42,36 +42,60 @@ namespace pininvdyn
     typedef std::size_t Index;
 
     /**
-     * Convert the input SE3 object to a 7D tuple of floats [X,Y,Z,Q1,Q2,Q3,Q4].
+     * Convert the input SE3 object to a 7D vector of floats [X,Y,Z,Q1,Q2,Q3,Q4].
      */
-    static void se3ToXYZQUAT(const se3::SE3 & M, RefVector xyzQuat)
-    {
-      assert(xyzQuat.size()>=7);
-      xyzQuat.head<3>() = M.translation();
-      xyzQuat.tail<4>() = Eigen::Quaterniond(M.rotation()).coeffs();
-    }
+    void se3ToXYZQUAT(const se3::SE3 & M, RefVector xyzQuat);
 
-    static void se3ToVector(const se3::SE3 & M, RefVector vec)
-    {
-      assert(vec.size()>=12);
-      vec.head<3>() = M.translation();
-      typedef Eigen::Matrix<double,9,1> Vector9;
-      vec.tail<9>() = Eigen::Map<const Vector9>(&M.rotation()(0), 9);
-    }
+    /**
+     * Convert the input SE3 object to a 12D vector of floats [X,Y,Z,R11,R12,R13,R14,...].
+     */
+    void se3ToVector(const se3::SE3 & M, RefVector vec);
 
-    static void vectorToSE3(RefVector vec, se3::SE3 & M)
-    {
-      assert(vec.size()>=12);
-      M.translation( vec.head<3>() );
-      typedef Eigen::Matrix<double,3,3> Matrix3;
-      M.rotation( Eigen::Map<const Matrix3>(&vec(3), 3, 3) );
-    }
+    void vectorToSE3(RefVector vec, se3::SE3 & M);
+
+    void errorInSE3 (const se3::SE3 & M,
+                     const se3::SE3 & Mdes,
+                     se3::Motion & error);
+
+
+    void pseudoInverse(ConstRefMatrix A,
+                       RefMatrix Apinv,
+                       double tolerance,
+                       unsigned int computationOptions = Eigen::ComputeThinU | Eigen::ComputeThinV);
+
+    void pseudoInverse(ConstRefMatrix A,
+                       Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject>& svdDecomposition,
+                       RefMatrix Apinv,
+                       double tolerance,
+                       unsigned int computationOptions);
+
+    void pseudoInverse(ConstRefMatrix A,
+                       Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject>& svdDecomposition,
+                       RefMatrix Apinv,
+                       double tolerance,
+                       double * nullSpaceBasisOfA,
+                       int &nullSpaceRows,
+                       int &nullSpaceCols,
+                       unsigned int computationOptions);
+
+    void dampedPseudoInverse(ConstRefMatrix A,
+                             Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject>& svdDecomposition,
+                             RefMatrix Apinv,
+                             double tolerance,
+                             double dampingFactor,
+                             unsigned int computationOptions = Eigen::ComputeThinU | Eigen::ComputeThinV,
+                             double * nullSpaceBasisOfA=0,
+                             int *nullSpaceRows=0, int *nullSpaceCols=0);
+
+    void nullSpaceBasisFromDecomposition(Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject>& svdDecomposition,
+                                         double tolerance,
+                                         double * nullSpaceBasisMatrix,
+                                         int &rows, int &cols);
 
-    static void errorInSE3 (const se3::SE3 & M,
-                            const se3::SE3 & Mdes,
-                            se3::Motion & error)
+    template<typename Derived>
+    inline bool is_finite(const Eigen::MatrixBase<Derived>& x)
     {
-      error = se3::log6(Mdes.inverse() * M);
+      return ( (x - x).array() == (x - x).array()).all();
     }
 
   }

include/pininvdyn/robot-wrapper.hpp

@@ -25,6 +25,7 @@
 #include "pinocchio/algorithm/jacobian.hpp"
 #include "pinocchio/algorithm/frames.hpp"
 #include "pinocchio/spatial/skew.hpp"
+#include <pininvdyn/math/utils.hpp>
 
 #include <string>
 
@@ -50,6 +51,7 @@ namespace pininvdyn
     typedef Eigen::Matrix<Scalar,6,1> Vector6;
     typedef Eigen::MatrixXd Matrix;
     typedef Eigen::Matrix<double,3,Eigen::Dynamic> Matrix3x;
+    typedef pininvdyn::math::RefVector RefVector;
 
 
     RobotWrapper(const std::string & filename,
@@ -73,6 +75,11 @@ namespace pininvdyn
 
     void computeAllTerms(Data & data, const Vector & q, const Vector & v) const;
 
+    const void com(const Data & data,
+                   RefVector com_pos,
+                   RefVector com_vel,
+                   RefVector com_acc) const;
+
     const Vector3 & com(const Data & data) const;
 
     const Vector3 & com_vel(const Data & data) const;

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

+//
+// Copyright (c) 2017 CNRS
+//
+// This file is part of PinInvDyn
+// pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+// pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#ifndef __invdyn_task_com_equality_hpp__
+#define __invdyn_task_com_equality_hpp__
+
+#include <pininvdyn/tasks/task-motion.hpp>
+#include <pininvdyn/trajectories/trajectory-base.hpp>
+#include <pininvdyn/math/constraint-equality.hpp>
+
+namespace pininvdyn
+{
+  namespace tasks
+  {
+
+    class TaskComEquality:
+        public TaskMotion
+    {
+    public:
+      typedef pininvdyn::RobotWrapper RobotWrapper;
+      typedef pininvdyn::math::Index Index;
+      typedef pininvdyn::trajectories::TrajectorySample TrajectorySample;
+      typedef pininvdyn::math::Vector Vector;
+      typedef pininvdyn::math::Vector3 Vector3;
+      typedef pininvdyn::math::ConstraintEquality ConstraintEquality;
+      typedef se3::Data Data;
+      typedef se3::Data::Matrix6x Matrix6x;
+      typedef se3::Motion Motion;
+      typedef se3::SE3 SE3;
+
+      TaskComEquality(const std::string & name,
+                      RobotWrapper & robot);
+
+      int dim() const;
+
+      const ConstraintBase & compute(const double t,
+                                     ConstRefVector q,
+                                     ConstRefVector v,
+                                     Data & data);
+
+      void setReference(const TrajectorySample & ref);
+
+      const Vector3 & position_error() const;
+      const Vector3 & velocity_error() const;
+
+      const Vector3 & Kp();
+      const Vector3 & Kd();
+      void Kp(ConstRefVector Kp);
+      void Kd(ConstRefVector Kp);
+
+    protected:
+      Vector3 m_Kp;
+      Vector3 m_Kd;
+      Vector3 m_p_error, m_v_error;
+      TrajectorySample m_ref;
+      ConstraintEquality m_constraint;
+    };
+    
+  }
+}
+
+#endif // ifndef __invdyn_task_com_equality_hpp__

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

@@ -54,6 +54,9 @@ namespace pininvdyn
 
       void setReference(TrajectorySample & ref);
 
+      const Motion & position_error() const;
+      const Motion & velocity_error() const;
+
       const Vector & Kp();
       const Vector & Kd();
       void Kp(ConstRefVector Kp);

src/CMakeLists.txt

@@ -18,6 +18,7 @@
 SET(LIBRARY_NAME ${PROJECT_NAME})
 
 SET(${LIBRARY_NAME}_MATH_SOURCES
+    math/utils.cpp
   )
 
 SET(${LIBRARY_NAME}_TASKS_SOURCES
@@ -25,6 +26,7 @@ SET(${LIBRARY_NAME}_TASKS_SOURCES
     tasks/task-actuation-bounds.cpp
     tasks/task-actuation-equality.cpp
     tasks/task-actuation.cpp
+    tasks/task-com-equality.cpp
     tasks/task-contact-force-equality.cpp
     tasks/task-contact-force.cpp
     tasks/task-joint-bounds.cpp

src/math/utils.cpp

+//
+// Copyright (c) 2017 CNRS
+//
+// This file is part of PinInvDyn
+// PinInvDyn is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+// PinInvDyn is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// PinInvDyn If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#include <pininvdyn/math/utils.hpp>
+
+void pininvdyn::math::se3ToXYZQUAT(const se3::SE3 & M, RefVector xyzQuat)
+{
+  assert(xyzQuat.size()>=7);
+  xyzQuat.head<3>() = M.translation();
+  xyzQuat.tail<4>() = Eigen::Quaterniond(M.rotation()).coeffs();
+}
+
+void pininvdyn::math::se3ToVector(const se3::SE3 & M, RefVector vec)
+{
+  assert(vec.size()>=12);
+  vec.head<3>() = M.translation();
+  typedef Eigen::Matrix<double,9,1> Vector9;
+  vec.tail<9>() = Eigen::Map<const Vector9>(&M.rotation()(0), 9);
+}
+
+void pininvdyn::math::vectorToSE3(RefVector vec, se3::SE3 & M)
+{
+  assert(vec.size()>=12);
+  M.translation( vec.head<3>() );
+  typedef Eigen::Matrix<double,3,3> Matrix3;
+  M.rotation( Eigen::Map<const Matrix3>(&vec(3), 3, 3) );
+}
+
+void pininvdyn::math::errorInSE3 (const se3::SE3 & M,
+                                  const se3::SE3 & Mdes,
+                                  se3::Motion & error)
+{
+  error = se3::log6(Mdes.inverse() * M);
+}
+
+
+void pininvdyn::math::pseudoInverse(ConstRefMatrix A,
+                                    RefMatrix Apinv,
+                                    double tolerance,
+                                    unsigned int computationOptions)
+
+{
+  Eigen::JacobiSVD<typename Eigen::MatrixXd::PlainObject> svdDecomposition(A.rows(), A.cols());
+  pininvdyn::math::pseudoInverse(A, svdDecomposition, Apinv, tolerance, computationOptions);
+}
+
+void pininvdyn::math::pseudoInverse(ConstRefMatrix A,
+                                    Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject>& svdDecomposition,
+                                    RefMatrix Apinv,
+                                    double tolerance,
+                                    unsigned int computationOptions)
+{
+  using namespace Eigen;
+  int nullSpaceRows = -1, nullSpaceCols = -1;
+  pseudoInverse(A, svdDecomposition, Apinv, tolerance,
+                (double*)0, nullSpaceRows, nullSpaceCols, computationOptions);
+}
+
+void pininvdyn::math::pseudoInverse(ConstRefMatrix A,
+                                    Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject>& svdDecomposition,
+                                    RefMatrix Apinv,
+                                    double tolerance,
+                                    double * nullSpaceBasisOfA,
+                                    int &nullSpaceRows, int &nullSpaceCols,
+                                    unsigned int computationOptions)
+{
+  using namespace Eigen;
+
+  if (computationOptions == 0) return; //if no computation options we cannot compute the pseudo inverse
+  svdDecomposition.compute(A, computationOptions);
+
+  JacobiSVD<MatrixXd::PlainObject>::SingularValuesType singularValues = svdDecomposition.singularValues();
+  int singularValuesSize = singularValues.size();
+  int rank = 0;
+  for (int idx = 0; idx < singularValuesSize; idx++) {
+    if (tolerance > 0 && singularValues(idx) > tolerance) {
+      singularValues(idx) = 1.0 / singularValues(idx);
+      rank++;
+    } else {
+      singularValues(idx) = 0.0;
+    }
+  }
+
+  //equivalent to this U/V matrix in case they are computed full
+  Apinv = svdDecomposition.matrixV().leftCols(singularValuesSize) * singularValues.asDiagonal() * svdDecomposition.matrixU().leftCols(singularValuesSize).adjoint();
+
+  if (nullSpaceBasisOfA && (computationOptions & ComputeFullV)) {
+    //we can compute the null space basis for A
+    nullSpaceBasisFromDecomposition(svdDecomposition, rank, nullSpaceBasisOfA, nullSpaceRows, nullSpaceCols);
+  }
+}
+
+void pininvdyn::math::dampedPseudoInverse(ConstRefMatrix A,
+                                          Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject>& svdDecomposition,
+                                          RefMatrix Apinv,
+                                          double tolerance,
+                                          double dampingFactor,
+                                          unsigned int computationOptions,
+                                          double * nullSpaceBasisOfA,
+                                          int *nullSpaceRows, int *nullSpaceCols)
+{
+  using namespace Eigen;
+
+  if (computationOptions == 0) return; //if no computation options we cannot compute the pseudo inverse
+  svdDecomposition.compute(A, computationOptions);
+
+  JacobiSVD<MatrixXd::PlainObject>::SingularValuesType singularValues = svdDecomposition.singularValues();
+
+  //rank will be used for the null space basis.
+  //not sure if this is correct
+  const int singularValuesSize = singularValues.size();
+  const double d2 = dampingFactor * dampingFactor;
+  int rank = 0;
+  for (int idx = 0; idx < singularValuesSize; idx++)
+  {
+    if(singularValues(idx) > tolerance)
+      rank++;
+    singularValues(idx) = singularValues(idx) / ((singularValues(idx) * singularValues(idx)) + d2);
+  }
+
+  //equivalent to this U/V matrix in case they are computed full
+  Apinv = svdDecomposition.matrixV().leftCols(singularValuesSize) * singularValues.asDiagonal() * svdDecomposition.matrixU().leftCols(singularValuesSize).adjoint();
+
+  if (nullSpaceBasisOfA && nullSpaceRows && nullSpaceCols
+      && (computationOptions & ComputeFullV))
+  {
+    //we can compute the null space basis for A
+    nullSpaceBasisFromDecomposition(svdDecomposition, rank, nullSpaceBasisOfA, *nullSpaceRows, *nullSpaceCols);
+  }
+}
+
+void pininvdyn::math::nullSpaceBasisFromDecomposition(Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject>& svdDecomposition,
+                                                      double tolerance,
+                                                      double * nullSpaceBasisMatrix,
+                                                      int &rows, int &cols)
+{
+  using namespace Eigen;
+  JacobiSVD<MatrixXd::PlainObject>::SingularValuesType singularValues = svdDecomposition.singularValues();
+  int rank = 0;
+  for (int idx = 0; idx < singularValues.size(); idx++) {
+    if (tolerance > 0 && singularValues(idx) > tolerance) {
+      rank++;
+    }
+  }
+  nullSpaceBasisFromDecomposition(svdDecomposition, rank, nullSpaceBasisMatrix, rows, cols);
+
+}
+
+void nullSpaceBasisFromDecomposition(Eigen::JacobiSVD<Eigen::MatrixXd::PlainObject>& svdDecomposition,
+                                     int rank,
+                                     double * nullSpaceBasisMatrix,
+                                     int &rows, int &cols)
+{
+  using namespace Eigen;
+  const MatrixXd &vMatrix = svdDecomposition.matrixV();
+  //A \in \mathbb{R}^{uMatrix.rows() \times vMatrix.cols()}
+  rows = vMatrix.cols();
+  cols = vMatrix.cols() - rank;
+  Map<MatrixXd> map(nullSpaceBasisMatrix, rows, cols);
+  map = vMatrix.rightCols(vMatrix.cols() - rank);
+}

src/robot-wrapper.cpp

@@ -54,6 +54,16 @@ namespace pininvdyn
     se3::framesForwardKinematics(m_model, data);
   }
 
+  const void RobotWrapper::com(const Data & data,
+                               RefVector com_pos,
+                               RefVector com_vel,
+                               RefVector com_acc) const
+  {
+    com_pos = data.com[0];
+    com_vel = data.vcom[0];
+    com_acc = data.acom[0];
+  }
+
   const Vector3 & RobotWrapper::com(const Data & data) const
   {
     return data.com[0];

src/tasks/task-com-equality.cpp

+//
+// Copyright (c) 2017 CNRS
+//
+// This file is part of PinInvDyn
+// PinInvDyn is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+// PinInvDyn is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// PinInvDyn If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#include <pininvdyn/tasks/task-com-equality.hpp>
+
+namespace pininvdyn
+{
+  namespace tasks
+  {
+    using namespace pininvdyn::math;
+    using namespace se3;
+
+    TaskComEquality::TaskComEquality(const std::string & name,
+                                     RobotWrapper & robot):
+      TaskMotion(name, robot),
+      m_constraint(name, 3, robot.nv())
+    {
+      m_Kp.setZero(3);
+      m_Kd.setZero(3);
+    }
+
+    int TaskComEquality::dim() const
+    {
+      //return self._mask.sum ()
+      return 3;
+    }
+
+    const Vector3 & TaskComEquality::Kp(){ return m_Kp; }
+
+    const Vector3 & TaskComEquality::Kd(){ return m_Kd; }
+
+    void TaskComEquality::Kp(ConstRefVector Kp)
+    {
+      assert(Kp.size()==3);
+      m_Kp = Kp;
+    }
+
+    void TaskComEquality::Kd(ConstRefVector Kd)
+    {
+      assert(Kd.size()==3);
+      m_Kd = Kd;
+    }
+
+    void TaskComEquality::setReference(const TrajectorySample & ref)
+    {
+      m_ref = ref;
+    }
+
+    const Vector3 & TaskComEquality::position_error() const
+    {
+      return m_p_error;
+    }
+
+    const Vector3 & TaskComEquality::velocity_error() const
+    {
+      return m_v_error;
+    }
+
+    const ConstraintBase & TaskComEquality::compute(const double t,
+                                                    ConstRefVector q,
+                                                    ConstRefVector v,
+                                                    Data & data)
+    {
+      Vector3 p_com, v_com, drift;
+      m_robot.com(data, p_com, v_com, 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;
+
+      // Get CoM jacobian
+      const Matrix3x & Jcom = m_robot.Jcom(data);
+
+      m_constraint.setMatrix(Jcom);
+      m_constraint.setVector(m_a_des - drift);
+      return m_constraint;
+    }
+    
+  }
+}

src/tasks/task-se3-equality.cpp

@@ -73,6 +73,16 @@ namespace pininvdyn
       m_a_ref = Motion(ref.acc);
     }
 
+    const Motion & TaskSE3Equality::position_error() const
+    {
+      return m_p_error;
+    }
+
+    const Motion & TaskSE3Equality::velocity_error() const
+    {
+      return m_v_error;
+    }
+
     const ConstraintBase & TaskSE3Equality::compute(const double t,
                                                     ConstRefVector q,
                                                     ConstRefVector v,
@@ -95,15 +105,10 @@ namespace pininvdyn
                 - m_Kd.cwiseProduct(m_v_error.toVector_impl())
                 + m_wMl.actInv(m_a_ref).toVector_impl();
 
+      // @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);
 
-//      if(local_frame==False):
-//        drift = self._gMl.act(drift);
-//      a_des[:3] = self._gMl.rotation * a_des[:3];
-//      a_des[3:] = self._gMl.rotation * a_des[3:];
-//      J[:3,:] = self._gMl.rotation * J[:3,:];
-//      J[3:,:] = self._gMl.rotation * J[3:,:];
-
       m_constraint.setMatrix(m_J);
       m_constraint.setVector(m_a_des - drift.toVector_impl());
       return m_constraint;

unittest/CMakeLists.txt

@@ -54,4 +54,4 @@ ADD_UNIT_TEST(constraints eigen3)
 ADD_UNIT_TEST(trajectories eigen3)
 ADD_UNIT_TEST(robot-wrapper eigen3)
 ADD_UNIT_TEST(tasks eigen3)
-
+ADD_UNIT_TEST(math_utils eigen3)

unittest/math_utils.cpp

+//
+// Copyright (c) 2017 CNRS
+//
+// This file is part of PinInvDyn
+// PinInvDyn is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+// PinInvDyn is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// PinInvDyn If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#include <iostream>
+
+#include <boost/test/unit_test.hpp>
+#include <boost/utility/binary.hpp>
+
+#include <pininvdyn/math/utils.hpp>
+
+BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
+
+BOOST_AUTO_TEST_CASE ( test_pseudoinverse)
+{
+  std::cout << "test_pseudoinverse\n";
+  using namespace pininvdyn::math;
+  using namespace Eigen;
+  using namespace std;
+  const unsigned int m = 3;
+  const unsigned int n = 5;
+
+  MatrixXd A = MatrixXd::Random(m,n);
+  MatrixXd Apinv = MatrixXd::Zero(n,m);
+  pseudoInverse(A, Apinv, 1e-5);
+
+  BOOST_CHECK(MatrixXd::Identity(m,m).isApprox(A*Apinv));
+}
+
+
+BOOST_AUTO_TEST_SUITE_END ()