Unverified Commit ff427110 authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub
Browse files

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: default avatarGuilhem Saurel <guilhem.saurel@laas.fr>
parent 17d12e7b
Pipeline #15720 passed with stage
in 21 minutes and 35 seconds
......@@ -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
......
......@@ -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__
#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
......@@ -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).
......
//
// 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
};
......
......@@ -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,
......
......@@ -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,
......
......@@ -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;
}
}
}
......@@ -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);
......
......@@ -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;
}
}
}
......@@ -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;
}
}
......
......@@ -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 )
......
//
// 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 )
......
......@@ -8,6 +8,7 @@ SET(${PYWRAP}_TESTS
#Solvers
Tasks
Trajectories
Deprecations
)
FOREACH(test ${${PYWRAP}_TESTS})
......
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()
......@@ -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)
......
......@@ -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)
assert np.linalg.norm(traj_sample.pos() - q_ref, 2) < tol
assert np.linalg.norm(traj_sample.vel() - zero, 2) < tol