Refactor TrajectorySample::pos, vel, acc to value, derivative, second_derivative (#101)

* Refactor TrajectorySample::pos, vel, acc to value, derivative, second_derivative

* [Tests] update tests to refactor of TrajectorySample

* Task-am-equality: correctly use reference value/derivative instead of derivative/second_derivative
Fix #77

* [Python] update python bindings of TrajectorySample

* [Python] deprecate TrajectorySample.{pos,vel,acc}

* [Tests] python < 3.2 doesn't have assertWarns

* trajectory: keep deprecated pos/vel/acc, add updated getters / setters instead

* [CMake] install macros.hpp

Co-authored-by: Guilhem Saurel <guilhem.saurel@laas.fr>

CMakeLists.txt

@@ -168,6 +168,7 @@ FILE(GLOB ${PYWRAP}_HEADERS
 
 SET(${PROJECT_NAME}_HEADERS
   include/tsid/config.hpp
+  include/tsid/macros.hpp
   include/tsid/deprecation.hpp
   include/tsid/utils/statistics.hpp
   include/tsid/utils/stop-watch.hpp

include/tsid/bindings/python/trajectories/trajectory-base.hpp

@@ -20,8 +20,11 @@
 
 #include "tsid/bindings/python/fwd.hpp"
 
+
 #include <tsid/math/utils.hpp>
 #include "tsid/trajectories/trajectory-base.hpp"
+
+#include <pinocchio/bindings/python/utils/deprecation.hpp>
 #include <assert.h>
 namespace tsid
 {
@@ -41,52 +44,72 @@ namespace tsid
       {
         cl
         .def(bp::init<unsigned int>((bp::arg("size")), "Default Constructor with size"))
-        .def(bp::init<unsigned int, unsigned int>((bp::arg("pos_size"), bp::arg("vel_size")), "Default Constructor with pos and vel size"))
+        .def(bp::init<unsigned int, unsigned int>((bp::arg("value_size"), bp::arg("derivative_size")), "Default Constructor with value and derivative size"))
 
         .def("resize", &TrajectorySamplePythonVisitor::resize, bp::arg("size"))
-        .def("resize", &TrajectorySamplePythonVisitor::resize2, bp::args("pos_size", "vel_size"))
+        .def("resize", &TrajectorySamplePythonVisitor::resize2, bp::args("value_size", "derivative_size"))
+
+        .def("value", &TrajectorySamplePythonVisitor::value)
+        .def("derivative", &TrajectorySamplePythonVisitor::derivative)
+        .def("second_derivative", &TrajectorySamplePythonVisitor::second_derivative)
+
+        .def("value", &TrajectorySamplePythonVisitor::setvalue_vec)
+        .def("value", &TrajectorySamplePythonVisitor::setvalue_se3)
+        .def("derivative", &TrajectorySamplePythonVisitor::setderivative)
+        .def("second_derivative", &TrajectorySamplePythonVisitor::setsecond_derivative)
 
-        .def("pos", &TrajectorySamplePythonVisitor::pos)
-        .def("vel", &TrajectorySamplePythonVisitor::vel)
-        .def("acc", &TrajectorySamplePythonVisitor::acc)
+        // Deprecated methods:
+        .def("pos", &TrajectorySamplePythonVisitor::value,
+            pinocchio::python::deprecated_function<>("This method is now deprecated. Please use .value"))
+        .def("vel", &TrajectorySamplePythonVisitor::derivative,
+            pinocchio::python::deprecated_function<>("This method is now deprecated. Please use .derivative"))
+        .def("acc", &TrajectorySamplePythonVisitor::second_derivative,
+            pinocchio::python::deprecated_function<>("This method is now deprecated. Please use .second_derivative"))
 
-        .def("pos", &TrajectorySamplePythonVisitor::setpos_vec)
-        .def("pos", &TrajectorySamplePythonVisitor::setpos_se3)
-        .def("vel", &TrajectorySamplePythonVisitor::setvel)
-        .def("acc", &TrajectorySamplePythonVisitor::setacc)
+        .def("pos", &TrajectorySamplePythonVisitor::setvalue_vec,
+            pinocchio::python::deprecated_function<>("This method is now deprecated. Please use .value"))
+        .def("pos", &TrajectorySamplePythonVisitor::setvalue_se3,
+            pinocchio::python::deprecated_function<>("This method is now deprecated. Please use .value"))
+        .def("vel", &TrajectorySamplePythonVisitor::setderivative,
+            pinocchio::python::deprecated_function<>("This method is now deprecated. Please use .derivative"))
+        .def("acc", &TrajectorySamplePythonVisitor::setsecond_derivative,
+            pinocchio::python::deprecated_function<>("This method is now deprecated. Please use .second_derivative"))
         ;
       }
 
-      static void setpos_vec(TrajSample & self, const Eigen::VectorXd pos){
-        assert (self.pos.size() == pos.size());
-        self.pos = pos;
+      static void setvalue_vec(TrajSample & self, const Eigen::VectorXd value){
+        assert (self.getValue().size() == value.size());
+        self.setValue(value);
       }
-      static void setpos_se3(TrajSample & self, const pinocchio::SE3 & pos){
-        assert (self.pos.size() == 12);
-        tsid::math::SE3ToVector(pos, self.pos);
+      static void setvalue_se3(TrajSample & self, const pinocchio::SE3 & value){
+        assert (self.getValue().size() == 12);
+TSID_DISABLE_WARNING_PUSH
+TSID_DISABLE_WARNING_DEPRECATED
+        tsid::math::SE3ToVector(value, self.pos);
+TSID_DISABLE_WARNING_POP
       }
-      static void setvel(TrajSample & self, const Eigen::VectorXd vel){
-        assert (self.vel.size() == vel.size());
-        self.vel = vel;
+      static void setderivative(TrajSample & self, const Eigen::VectorXd derivative){
+        assert (self.getDerivative().size() == derivative.size());
+        self.setDerivative(derivative);
       }
-      static void setacc(TrajSample & self, const Eigen::VectorXd acc){
-        assert (self.acc.size() == acc.size());
-        self.acc = acc;
+      static void setsecond_derivative(TrajSample & self, const Eigen::VectorXd second_derivative){
+        assert (self.getSecondDerivative().size() == second_derivative.size());
+        self.setSecondDerivative(second_derivative);
       }
       static void resize(TrajSample & self, const unsigned int & size){
           self.resize(size, size);
       }
-      static void resize2(TrajSample & self, const unsigned int & pos_size, const unsigned int & vel_size){
-          self.resize(pos_size, vel_size);
+      static void resize2(TrajSample & self, const unsigned int & value_size, const unsigned int & derivative_size){
+          self.resize(value_size, derivative_size);
       }
-      static Eigen::VectorXd pos(const TrajSample & self){
-          return self.pos;
+      static Eigen::VectorXd value(const TrajSample & self){
+          return self.getValue();
       }
-      static Eigen::VectorXd vel(const TrajSample & self){
-          return self.vel;
+      static Eigen::VectorXd derivative(const TrajSample & self){
+          return self.getDerivative();
       }
-      static Eigen::VectorXd acc(const TrajSample & self){
-          return self.acc;
+      static Eigen::VectorXd second_derivative(const TrajSample & self){
+          return self.getSecondDerivative();
       }
 
       static void expose(const std::string & class_name)
@@ -102,4 +125,4 @@ namespace tsid
 }
 
 
-#endif // ifndef __tsid_python_traj_euclidian_hpp__
\ No newline at end of file
+#endif // ifndef __tsid_python_traj_euclidian_hpp__

include/tsid/macros.hpp

+#ifndef __TSID_MACROS_HPP__
+#define __TSID_MACROS_HPP__
+
+// ref https://www.fluentcpp.com/2019/08/30/how-to-disable-a-warning-in-cpp/
+#if defined(_MSC_VER)
+
+#define TSID_DISABLE_WARNING_PUSH           __pragma(warning( push ))
+#define TSID_DISABLE_WARNING_POP            __pragma(warning( pop ))
+#define TSID_DISABLE_WARNING(warningNumber) __pragma(warning( disable : warningNumber ))
+#define TSID_DISABLE_WARNING_DEPRECATED     TSID_DISABLE_WARNING(4996)
+
+#elif defined(__GNUC__) || defined(__clang__)
+
+#define TSID_DO_PRAGMA(X) _Pragma(#X)
+#define TSID_DISABLE_WARNING_PUSH           TSID_DO_PRAGMA(GCC diagnostic push)
+#define TSID_DISABLE_WARNING_POP            TSID_DO_PRAGMA(GCC diagnostic pop)
+#define TSID_DISABLE_WARNING(warningName)   TSID_DO_PRAGMA(GCC diagnostic ignored #warningName)
+#define TSID_DISABLE_WARNING_DEPRECATED     TSID_DISABLE_WARNING(-Wdeprecated-declarations)
+
+#else
+
+#define TSID_DISABLE_WARNING_PUSH
+#define TSID_DISABLE_WARNING_POP
+#define TSID_DISABLE_WARNING_DEPRECATED
+
+#endif
+
+#endif

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

@@ -58,6 +58,7 @@ namespace tsid
       const ConstraintBase & getConstraint() const;
 
       void setReference(TrajectorySample & ref);
+      void setReference(const SE3 & ref);
       const TrajectorySample & getReference() const;
 
       /** Return the desired task acceleration (after applying the specified mask).

include/tsid/trajectories/trajectory-base.hpp

 //
-// Copyright (c) 2017 CNRS
+// Copyright (c) 2017-2021 CNRS
 //
 // This file is part of tsid
 // tsid is free software: you can redistribute it
@@ -18,7 +18,10 @@
 #ifndef __invdyn_trajectory_base_hpp__
 #define __invdyn_trajectory_base_hpp__
 
+#include "tsid/deprecation.hpp"
+#include "tsid/macros.hpp"
 #include "tsid/math/fwd.hpp"
+#include "tsid/math/utils.hpp"
 
 #include <string>
 
@@ -26,22 +29,35 @@ namespace tsid
 {
   namespace trajectories
   {
-    
+
+    typedef Eigen::Map<const Eigen::Matrix<double, 3, 3>> MapMatrix3;
+
     class TrajectorySample
     {
     public:
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-      
-      math::Vector pos, vel, acc;
+
+      // TODO rename pos, vel, acc → value, derivative, second_derivative
+      DEPRECATED math::Vector pos, vel, acc;
+
+TSID_DISABLE_WARNING_PUSH
+TSID_DISABLE_WARNING_DEPRECATED
+      // getters / setters with updated names for math::Vector
+      const math::Vector & getValue() const { return pos; }
+      const math::Vector & getDerivative() const { return vel; }
+      const math::Vector & getSecondDerivative() const { return acc; }
+      void setValue(const math::Vector & value) { pos = value; }
+      void setDerivative(const math::Vector & derivative) { vel = derivative; }
+      void setSecondDerivative(const math::Vector & second_derivative) { acc = second_derivative; }
 
       TrajectorySample(unsigned int size=0)
       {
-        resize(size, size);
+        resize(size);
       }
 
-      TrajectorySample(unsigned int size_pos, unsigned int size_vel)
+      TrajectorySample(unsigned int size_value, unsigned int size_derivative)
       {
-        resize(size_pos, size_vel);
+        resize(size_value, size_derivative);
       }
 
       void resize(unsigned int size)
@@ -49,12 +65,19 @@ namespace tsid
         resize(size, size);
       }
 
-      void resize(unsigned int size_pos, unsigned int size_vel)
+      void resize(unsigned int size_value, unsigned int size_derivative)
       {
-        pos.setZero(size_pos);
-        vel.setZero(size_vel);
-        acc.setZero(size_vel);
+        pos.setZero(size_value);
+        vel.setZero(size_derivative);
+        acc.setZero(size_derivative);
       }
+
+      // declare default constructors / destructors to disable the deprecation
+      // message for them. TODO: Remove this after the
+      // pos/vel/acc → value/derivative/second_derivative rename
+      ~TrajectorySample() = default;
+      TrajectorySample(const TrajectorySample&) = default;
+TSID_DISABLE_WARNING_POP
     };
 
 

src/contacts/contact-6d.cpp

@@ -226,9 +226,7 @@ void Contact6d::setForceReference(ConstRefVector & f_ref)
 
 void Contact6d::setReference(const SE3 & ref)
 {
-  TrajectorySample s(12, 6);
-  SE3ToVector(ref, s.pos);
-  m_motionTask.setReference(s);
+  m_motionTask.setReference(ref);
 }
 
 const ConstraintBase & Contact6d::computeMotionTask(const double t,

src/contacts/contact-point.cpp

@@ -124,16 +124,16 @@ void ContactPoint:: updateForceGeneratorMatrix()
 unsigned int ContactPoint::n_motion() const { return m_motionTask.dim(); }
 unsigned int ContactPoint::n_force() const { return 3; }
 
-const Vector & ContactPoint::Kp() 
-{ 
-  m_Kp3 = m_motionTask.Kp().head<3>(); 
-  return m_Kp3; 
+const Vector & ContactPoint::Kp()
+{
+  m_Kp3 = m_motionTask.Kp().head<3>();
+  return m_Kp3;
 }
 
-const Vector & ContactPoint::Kd() 
-{ 
-  m_Kd3 = m_motionTask.Kd().head<3>(); 
-  return m_Kd3; 
+const Vector & ContactPoint::Kd()
+{
+  m_Kd3 = m_motionTask.Kd().head<3>();
+  return m_Kd3;
 }
 
 void ContactPoint::Kp(ConstRefVector Kp)
@@ -202,9 +202,7 @@ void ContactPoint::setForceReference(ConstRefVector & f_ref)
 
 void ContactPoint::setReference(const SE3 & ref)
 {
-  TrajectorySample s(12, 6);
-  SE3ToVector(ref, s.pos);
-  m_motionTask.setReference(s);
+  m_motionTask.setReference(ref);
 }
 
 const ConstraintBase & ContactPoint::computeMotionTask(const double t,

src/tasks/task-angular-momentum-equality.cpp

@@ -96,12 +96,12 @@ namespace tsid
     }
     const Vector & TaskAMEquality::momentum_ref() const
     {
-      return m_ref.vel;
+      return m_ref.getValue();
     }
 
     const Vector & TaskAMEquality::dmomentum_ref() const
     {
-      return m_ref.acc;
+      return m_ref.getDerivative();
     }
 
     const ConstraintBase & TaskAMEquality::getConstraint() const
@@ -118,10 +118,10 @@ namespace tsid
       // Get momentum jacobian
       const Matrix6x & J_am = m_robot.momentumJacobian(data);
       m_L = J_am.bottomRows(3) * v;
-      m_L_error = m_L - m_ref.vel;
+      m_L_error = m_L - m_ref.getValue();
 
       m_dL_des = - m_Kp.cwiseProduct(m_L_error)
-                + m_ref.acc;
+                + m_ref.getDerivative();
 
 #ifndef NDEBUG
 //      std::cout<<m_name<<" errors: "<<m_L_error.norm()<<" "
@@ -134,6 +134,6 @@ namespace tsid
 
       return m_constraint;
     }
-    
+
   }
 }

src/tasks/task-com-equality.cpp

@@ -41,7 +41,7 @@ namespace tsid
       m_ref.resize(3);
       m_mask.resize(3);
       m_mask.fill(1.);
-      setMask(m_mask);      
+      setMask(m_mask);
     }
 
 
@@ -120,12 +120,12 @@ namespace tsid
 
     const Vector & TaskComEquality::position_ref() const
     {
-      return m_ref.pos;
+      return m_ref.getValue();
     }
 
     const Vector & TaskComEquality::velocity_ref() const
     {
-      return m_ref.vel;
+      return m_ref.getDerivative();
     }
 
     const ConstraintBase & TaskComEquality::getConstraint() const
@@ -141,11 +141,11 @@ namespace tsid
       m_robot.com(data, m_p_com, m_v_com, m_drift);
 
       // Compute errors
-      m_p_error = m_p_com - m_ref.pos;
-      m_v_error = m_v_com - m_ref.vel;
+      m_p_error = m_p_com - m_ref.getValue();
+      m_v_error = m_v_com - m_ref.getDerivative();
       m_a_des = - m_Kp.cwiseProduct(m_p_error)
                 - m_Kd.cwiseProduct(m_v_error)
-                + m_ref.acc;
+                + m_ref.getSecondDerivative();
 
       m_p_error_vec = m_p_error;
       m_v_error_vec = m_v_error;
@@ -164,7 +164,7 @@ namespace tsid
 
         m_constraint.matrix().row(idx) = Jcom.row(i);
         m_constraint.vector().row(idx) = (m_a_des - m_drift).row(i);
-       
+
         m_a_des_masked(idx)            = m_a_des(i);
         m_drift_masked(idx)            = m_drift(i);
         m_p_error_masked_vec(idx)      = m_p_error_vec(i);

src/tasks/task-joint-posture.cpp

@@ -92,9 +92,9 @@ namespace tsid
 
     void TaskJointPosture::setReference(const TrajectorySample & ref)
     {
-      assert(ref.pos.size()==m_robot.na());
-      assert(ref.vel.size()==m_robot.na());
-      assert(ref.acc.size()==m_robot.na());
+      assert(ref.getValue().size()==m_robot.na());
+      assert(ref.getDerivative().size()==m_robot.na());
+      assert(ref.getSecondDerivative().size()==m_robot.na());
       m_ref = ref;
     }
 
@@ -135,12 +135,12 @@ namespace tsid
 
     const Vector & TaskJointPosture::position_ref() const
     {
-      return m_ref.pos;
+      return m_ref.getValue();
     }
 
     const Vector & TaskJointPosture::velocity_ref() const
     {
-      return m_ref.vel;
+      return m_ref.getDerivative();
     }
 
     const ConstraintBase & TaskJointPosture::getConstraint() const
@@ -156,16 +156,16 @@ namespace tsid
       // Compute errors
       m_p = q.tail(m_robot.na());
       m_v = v.tail(m_robot.na());
-      m_p_error = m_p - m_ref.pos;
-      m_v_error = m_v - m_ref.vel;
+      m_p_error = m_p - m_ref.getValue();
+      m_v_error = m_v - m_ref.getDerivative();
       m_a_des = - m_Kp.cwiseProduct(m_p_error)
                 - m_Kd.cwiseProduct(m_v_error)
-                + m_ref.acc;
+                + m_ref.getSecondDerivative();
 
       for(unsigned int i=0; i<m_activeAxes.size(); i++)
         m_constraint.vector()(i) = m_a_des(m_activeAxes(i));
       return m_constraint;
     }
-    
+
   }
 }

src/tasks/task-se3-equality.cpp

@@ -97,11 +97,27 @@ namespace tsid
     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);
+TSID_DISABLE_WARNING_PUSH
+TSID_DISABLE_WARNING_DEPRECATED
+      assert(ref.pos.size() == 12);
+      m_M_ref.translation( ref.pos.head<3>());
+      m_M_ref.rotation(MapMatrix3(&ref.pos(3), 3, 3));
+TSID_DISABLE_WARNING_POP
+      m_v_ref = Motion(ref.getDerivative());
+      m_a_ref = Motion(ref.getSecondDerivative());
     }
 
+    void TaskSE3Equality::setReference(const SE3 & ref)
+    {
+      TrajectorySample s(12, 6);
+TSID_DISABLE_WARNING_PUSH
+TSID_DISABLE_WARNING_DEPRECATED
+      tsid::math::SE3ToVector(ref, s.pos);
+TSID_DISABLE_WARNING_POP
+      setReference(s);
+    }
+
+
     const TrajectorySample & TaskSE3Equality::getReference() const
     {
       return m_ref;
@@ -230,7 +246,7 @@ namespace tsid
 
         idx += 1;
       }
-      
+
       return m_constraint;
     }
   }

src/trajectories/trajectory-euclidian.cpp

@@ -35,14 +35,13 @@ namespace tsid
 
     void TrajectoryEuclidianConstant::setReference(ConstRefVector ref)
     {
-      m_sample.pos = ref;
-      m_sample.vel.setZero(ref.size());
-      m_sample.acc.setZero(ref.size());
+      m_sample.resize((unsigned int)ref.size());
+      m_sample.setValue(ref);
     }
 
     unsigned int TrajectoryEuclidianConstant::size() const
     {
-      return (unsigned int)m_sample.pos.size();
+      return (unsigned int)m_sample.getValue().size();
     }
 
     const TrajectorySample & TrajectoryEuclidianConstant::operator()(double )

src/trajectories/trajectory-se3.cpp

 //
-// Copyright (c) 2017 CNRS
+// Copyright (c) 2017-2021 CNRS
 //
 // This file is part of tsid
 // tsid is free software: you can redistribute it
@@ -36,7 +36,10 @@ namespace tsid
       :TrajectoryBase(name)
     {
       m_sample.resize(12, 6);
-      SE3ToVector(M, m_sample.pos);
+TSID_DISABLE_WARNING_PUSH
+TSID_DISABLE_WARNING_DEPRECATED
+      tsid::math::SE3ToVector(M, m_sample.pos);
+TSID_DISABLE_WARNING_POP
     }
 
     unsigned int TrajectorySE3Constant::size() const
@@ -47,7 +50,10 @@ namespace tsid
     void TrajectorySE3Constant::setReference(const pinocchio::SE3 & ref)
     {
       m_sample.resize(12, 6);
-      SE3ToVector(ref, m_sample.pos);
+TSID_DISABLE_WARNING_PUSH
+TSID_DISABLE_WARNING_DEPRECATED
+      tsid::math::SE3ToVector(ref, m_sample.pos);
+TSID_DISABLE_WARNING_POP
     }
 
     const TrajectorySample & TrajectorySE3Constant::operator()(double )

tests/python/CMakeLists.txt

@@ -8,6 +8,7 @@ SET(${PYWRAP}_TESTS
   #Solvers
   Tasks
   Trajectories
+  Deprecations
   )
 
 FOREACH(test ${${PYWRAP}_TESTS})

tests/python/test_Deprecations.py

+import unittest
+import sys
+import warnings
+
+import numpy as np
+
+import tsid
+
+
+class DeprecationTest(unittest.TestCase):
+    def test_trajectory(self):
+        q_ref = np.ones(5)
+        traj_euclidian = tsid.TrajectoryEuclidianConstant("traj_eucl", q_ref)
+
+        if sys.version_info >= (3, 2):
+            with self.assertWarns(UserWarning):
+                traj_euclidian.computeNext().pos()
+            with self.assertWarns(UserWarning):
+                traj_euclidian.computeNext().vel()
+            with self.assertWarns(UserWarning):
+                traj_euclidian.computeNext().acc()
+        else:
+            with warnings.catch_warnings(record=True) as w:
+                self.assertEqual(len(w), 0)
+
+                traj_euclidian.computeNext().pos()
+                self.assertEqual(len(w), 1)
+                self.assertEqual(w[-1].category, UserWarning)
+
+                traj_euclidian.computeNext().vel()
+                self.assertEqual(len(w), 2)
+                self.assertEqual(w[-1].category, UserWarning)
+
+                traj_euclidian.computeNext().acc()
+                self.assertEqual(len(w), 3)
+                self.assertEqual(w[-1].category, UserWarning)
+
+
+if __name__ == '__main__':
+    unittest.main()

tests/python/test_Formulation.py

@@ -103,7 +103,7 @@ H_rf_ref_vec = np.matrix(np.zeros(12)).transpose()
 H_rf_ref_vec[0:3] = H_rf_ref.translation
 for i in range(0, 3):
     H_rf_ref_vec[3 * i + 3:3 * i + 6] = H_rf_ref.rotation[:, i]
-s.pos(H_rf_ref_vec)
+s.value(H_rf_ref_vec)
 rightFootTask.setReference(s)
 
 com_ref = robot.com(data)

tests/python/test_Trajectories.py

@@ -16,14 +16,14 @@ zero = np.matrix(np.zeros(n)).transpose()
 
 traj_euclidian = tsid.TrajectoryEuclidianConstant("traj_eucl", q_ref)
 assert traj_euclidian.has_trajectory_ended()
-assert np.linalg.norm(traj_euclidian.computeNext().pos() - q_ref, 2) < tol
-assert np.linalg.norm(traj_euclidian.getSample(0.0).pos() - q_ref, 2) < tol
+assert np.linalg.norm(traj_euclidian.computeNext().value() - q_ref, 2) < tol
+assert np.linalg.norm(traj_euclidian.getSample(0.0).value() - q_ref, 2) < tol
 
 traj_sample = tsid.TrajectorySample(n)
 traj_euclidian.getLastSample(traj_sample)