diff --git a/CMakeLists.txt b/CMakeLists.txt index 3403dc88..1db60783 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -168,6 +168,8 @@ set(${PROJECT_NAME}_CONTACTS_HEADERS include/tsid/contacts/contact-6d.hpp include/tsid/contacts/contact-point.hpp include/tsid/contacts/measured-force-base.hpp + include/tsid/contacts/measured-3d-force.hpp + include/tsid/contacts/measured-6d-wrench.hpp include/tsid/contacts/measured-3Dforce.hpp include/tsid/contacts/measured-6Dwrench.hpp include/tsid/contacts/contact-two-frame-positions.hpp) @@ -272,8 +274,8 @@ set(${PROJECT_NAME}_CONTACTS_SOURCES src/contacts/contact-6d.cpp src/contacts/contact-point.cpp src/contacts/measured-force-base.cpp - src/contacts/measured-3Dforce.cpp - src/contacts/measured-6Dwrench.cpp + src/contacts/measured-3d-force.cpp + src/contacts/measured-6d-wrench.cpp src/contacts/contact-two-frame-positions.cpp) set(${PROJECT_NAME}_TRAJECTORIES_SOURCES diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index e73f927b..b912a765 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -19,6 +19,7 @@ set(${PYWRAP}_SOURCES contacts/contact-6d.cpp contacts/contact-point.cpp contacts/contact-two-frame-positions.cpp + contacts/measured-6d-wrench.cpp formulations/formulation.cpp math/utils.cpp module.cpp @@ -49,6 +50,7 @@ set(${PYWRAP}_HEADERS ../../include/tsid/bindings/python/contacts/contact-point.hpp ../../include/tsid/bindings/python/contacts/contact-two-frame-positions.hpp ../../include/tsid/bindings/python/contacts/contact-two-frames.hpp + ../../include/tsid/bindings/python/contacts/measured-6d-wrench.hpp ../../include/tsid/bindings/python/contacts/expose-contact.hpp ../../include/tsid/bindings/python/formulations/expose-formulations.hpp ../../include/tsid/bindings/python/formulations/formulation.hpp diff --git a/bindings/python/contacts/measured-6d-wrench.cpp b/bindings/python/contacts/measured-6d-wrench.cpp new file mode 100644 index 00000000..e2c597f1 --- /dev/null +++ b/bindings/python/contacts/measured-6d-wrench.cpp @@ -0,0 +1,11 @@ +#include "tsid/bindings/python/contacts/measured-6d-wrench.hpp" +#include "tsid/bindings/python/contacts/expose-contact.hpp" + +namespace tsid { +namespace python { +void exposeMeasured6dWrench() { + Measured6dWrenchPythonVisitor::expose( + "Measured6dWrench"); +} +} // namespace python +} // namespace tsid \ No newline at end of file diff --git a/include/tsid/bindings/python/contacts/expose-contact.hpp b/include/tsid/bindings/python/contacts/expose-contact.hpp index 01efac41..2278b6cc 100644 --- a/include/tsid/bindings/python/contacts/expose-contact.hpp +++ b/include/tsid/bindings/python/contacts/expose-contact.hpp @@ -21,19 +21,22 @@ #include "tsid/bindings/python/contacts/contact-6d.hpp" #include "tsid/bindings/python/contacts/contact-point.hpp" #include "tsid/bindings/python/contacts/contact-two-frame-positions.hpp" +#include "tsid/bindings/python/contacts/measured-6d-wrench.hpp" namespace tsid { namespace python { void exposeContact6d(); void exposeContactPoint(); void exposeContactTwoFramePositions(); +void exposeMeasured6dWrench(); inline void exposeContact() { exposeContact6d(); exposeContactPoint(); exposeContactTwoFramePositions(); + exposeMeasured6dWrench(); } } // namespace python } // namespace tsid -#endif // ifndef __tsid_python_expose_contact_hpp__ +#endif // ifndef __tsid_python_expose_contact_hpp__ \ No newline at end of file diff --git a/include/tsid/bindings/python/contacts/measured-6d-wrench.hpp b/include/tsid/bindings/python/contacts/measured-6d-wrench.hpp new file mode 100644 index 00000000..bbb87119 --- /dev/null +++ b/include/tsid/bindings/python/contacts/measured-6d-wrench.hpp @@ -0,0 +1,114 @@ +// +// Copyright (c) 2022 CNRS INRIA LORIA +// +// This file is part of tsid +// tsid 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. +// tsid 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 +// tsid If not, see . +// + +#ifndef __tsid_python_measured_6d_wrench_hpp__ +#define __tsid_python_measured_6d_wrench_hpp__ + +#include "tsid/bindings/python/fwd.hpp" +#include "tsid/contacts/measured-6d-wrench.hpp" +#include "tsid/robots/robot-wrapper.hpp" +#include "pinocchio/multibody/data.hpp" + +namespace tsid { +namespace python { +namespace bp = boost::python; + +template +struct Measured6dWrenchPythonVisitor + : public boost::python::def_visitor< + Measured6dWrenchPythonVisitor > { + template + void visit(PyClass &cl) const { + cl + // Expose the constructor: + .def(bp::init( + (bp::arg("name"), bp::arg("robot"), bp::arg("frameName")), + "Constructor for Measured6dwrench")) + + // Expose the name getter and setter: + .add_property("name", &Measured6dWrenchPythonVisitor::getName, + &Measured6dWrenchPythonVisitor::setName, + "Get or set the name of the measured-6Dwrench instance") + + // Expose computeJointTorques(Data &data) + .def("computeJointTorques", + &Measured6dWrenchPythonVisitor::computeJointTorques, + bp::args("data"), + "Compute the joint torques from the measured contact force") + + // Expose setMeasuredContactForce(const Vector6 &fext) + .def("setMeasuredContactForce", + &Measured6dWrenchPythonVisitor::setMeasuredContactForce, + bp::args("fext"), "Set the measured contact force") + + // Expose getMeasuredContactForce() as a read-only property. + .add_property( + "measuredContactForce", + bp::make_function( + &Measured6dWrenchPythonVisitor::getMeasuredContactForce), + "Get the measured contact force") + + // Expose useLocalFrame(bool local_frame) + .def("useLocalFrame", &Measured6dWrenchPythonVisitor::useLocalFrame, + bp::args("local_frame"), + "Specify whether to use the local frame for external force and " + "jacobian"); + } + + // Wrapper for name() getter. + static std::string getName(Measured6dWrench &self) { return self.name(); } + + // Wrapper for name(const std::string &) setter. + static void setName(Measured6dWrench &self, const std::string &name) { + self.name(name); + } + + // Wrapper for computeJointTorques(Data &data) returning by value. + static typename Measured6dWrench::Vector computeJointTorques( + Measured6dWrench &self, pinocchio::Data &data) { + return self.computeJointTorques(data); + } + + // Wrapper for setMeasuredContactForce(const Vector6 &fext) + static void setMeasuredContactForce( + Measured6dWrench &self, const typename Measured6dWrench::Vector6 &fext) { + self.setMeasuredContactForce(fext); + } + + // Wrapper for getMeasuredContactForce() returning by value. + static typename Measured6dWrench::Vector6 getMeasuredContactForce( + Measured6dWrench &self) { + return self.getMeasuredContactForce(); + } + + // Wrapper for useLocalFrame(bool local_frame) + static void useLocalFrame(Measured6dWrench &self, bool local_frame) { + self.useLocalFrame(local_frame); + } + + // Function to expose the binding. + static void expose(const std::string &class_name) { + std::string doc = "Bindings for tsid::contacts::Measured6dwrench"; + bp::class_(class_name.c_str(), doc.c_str(), bp::no_init) + .def(Measured6dWrenchPythonVisitor()); + } +}; + +} // namespace python +} // namespace tsid + +#endif // __tsid_python_measured_6d_wrench_hpp__ diff --git a/include/tsid/bindings/python/formulations/formulation.hpp b/include/tsid/bindings/python/formulations/formulation.hpp index 59652bfa..c00b4e88 100644 --- a/include/tsid/bindings/python/formulations/formulation.hpp +++ b/include/tsid/bindings/python/formulations/formulation.hpp @@ -27,6 +27,7 @@ #include "tsid/contacts/contact-6d.hpp" #include "tsid/contacts/contact-point.hpp" #include "tsid/contacts/contact-two-frame-positions.hpp" +#include "tsid/contacts/measured-6d-wrench.hpp" #include "tsid/tasks/task-joint-posture.hpp" #include "tsid/tasks/task-se3-equality.hpp" #include "tsid/tasks/task-com-equality.hpp" @@ -127,7 +128,9 @@ struct InvDynPythonVisitor .def("checkContact", &InvDynPythonVisitor::checkContact, bp::args("name", "HQPOutput")) .def("getContactForce", &InvDynPythonVisitor::getContactForce, - bp::args("name", "HQPOutput")); + bp::args("name", "HQPOutput")) + .def("addMeasuredForce", &InvDynPythonVisitor::addMeasuredForce, + bp::args("measured_force")); } static pinocchio::Data data(T& self) { pinocchio::Data data = self.data(); @@ -273,6 +276,10 @@ struct InvDynPythonVisitor const solvers::HQPOutput& sol) { return self.getContactForces(name, sol); } + static bool addMeasuredForce(T& self, + contacts::Measured6Dwrench& measured_force) { + return self.addMeasuredForce(measured_force); + } static void expose(const std::string& class_name) { std::string doc = "InvDyn info."; diff --git a/include/tsid/contacts/measured-3Dforce.hpp b/include/tsid/contacts/measured-3Dforce.hpp index 9a87f864..7c5b7ed9 100644 --- a/include/tsid/contacts/measured-3Dforce.hpp +++ b/include/tsid/contacts/measured-3Dforce.hpp @@ -1,71 +1,14 @@ -// -// Copyright (c) 2022 CNRS INRIA LORIA -// -// This file is part of tsid -// tsid 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. -// tsid 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 -// tsid If not, see -// . -// +#ifndef TSID_CONTACTS_MEASURED_3DFORCE_HPP +#define TSID_CONTACTS_MEASURED_3DFORCE_HPP -#ifndef __invdyn_measured_3Dforce_hpp__ -#define __invdyn_measured_3Dforce_hpp__ +#include "tsid/contacts/measured-3d-force.hpp" -#include +#ifdef _MSC_VER +#pragma message( \ + "TSID DEPRECATED: Please update your includes from 'measured-3Dforce.hpp' to 'measured-3d-force.hpp'") +#else +#warning \ + "TSID DEPRECATED: Please update your includes from 'measured-3Dforce.hpp' to 'measured-3d-force.hpp'" +#endif -#include "tsid/contacts/measured-force-base.hpp" - -namespace tsid { -namespace contacts { -class Measured3Dforce : public MeasuredForceBase { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - typedef math::Index Index; - typedef math::Vector3 Vector3; - typedef robots::RobotWrapper RobotWrapper; - typedef pinocchio::Data Data; - typedef pinocchio::Data::Matrix3x Matrix3x; - - Measured3Dforce(const std::string &name, RobotWrapper &robot, - const std::string &frameName); - - const Vector &computeJointTorques(Data &data); - - /** - * Set the value of the external wrench applied by the environment on the - * robot. - */ - void setMeasuredContactForce(const Vector3 &fext); - - const Vector3 &getMeasuredContactForce() const; - - /** - * @brief Specifies if the external force and jacobian is - * expressed in the local frame or the local world-oriented frame. - * - * @param local_frame If true, represent external force and jacobian in the - * local frame. If false, represent them in the local world-oriented frame. - */ - void useLocalFrame(bool local_frame); - - protected: - std::string m_frame_name; - Index m_frame_id; - Vector3 m_fext; - Matrix3x m_J; - Matrix3x m_J_rotated; - Vector m_computedTorques; - bool m_local_frame; -}; -} // namespace contacts -} // namespace tsid - -#endif // ifndef __invdyn_measured_6Dwrench_hpp__ +#endif // TSID_CONTACTS_MEASURED_3DFORCE_HPP diff --git a/include/tsid/contacts/measured-3d-force.hpp b/include/tsid/contacts/measured-3d-force.hpp new file mode 100644 index 00000000..6243c39e --- /dev/null +++ b/include/tsid/contacts/measured-3d-force.hpp @@ -0,0 +1,71 @@ +// +// Copyright (c) 2025 CNRS INRIA LORIA +// +// This file is part of tsid +// tsid 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. +// tsid 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 +// tsid If not, see +// . +// + +#ifndef __invdyn_measured_3d_force_hpp__ +#define __invdyn_measured_3d_force_hpp__ + +#include + +#include "tsid/contacts/measured-force-base.hpp" + +namespace tsid { +namespace contacts { +class Measured3Dforce : public MeasuredForceBase { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef math::Index Index; + typedef math::Vector3 Vector3; + typedef robots::RobotWrapper RobotWrapper; + typedef pinocchio::Data Data; + typedef pinocchio::Data::Matrix3x Matrix3x; + + Measured3Dforce(const std::string &name, RobotWrapper &robot, + const std::string &frameName); + + const Vector &computeJointTorques(Data &data); + + /** + * Set the value of the external wrench applied by the environment on the + * robot. + */ + void setMeasuredContactForce(const Vector3 &fext); + + const Vector3 &getMeasuredContactForce() const; + + /** + * @brief Specifies if the external force and jacobian is + * expressed in the local frame or the local world-oriented frame. + * + * @param local_frame If true, represent external force and jacobian in the + * local frame. If false, represent them in the local world-oriented frame. + */ + void useLocalFrame(bool local_frame); + + protected: + std::string m_frame_name; + Index m_frame_id; + Vector3 m_fext; + Matrix3x m_J; + Matrix3x m_J_rotated; + Vector m_computedTorques; + bool m_local_frame; +}; +} // namespace contacts +} // namespace tsid + +#endif // ifndef __invdyn_measured_3d_force_hpp__ diff --git a/include/tsid/contacts/measured-6Dwrench.hpp b/include/tsid/contacts/measured-6Dwrench.hpp index 24329230..0f218cb2 100644 --- a/include/tsid/contacts/measured-6Dwrench.hpp +++ b/include/tsid/contacts/measured-6Dwrench.hpp @@ -1,71 +1,14 @@ -// -// Copyright (c) 2022 CNRS INRIA LORIA -// -// This file is part of tsid -// tsid 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. -// tsid 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 -// tsid If not, see -// . -// +#ifndef TSID_CONTACTS_MEASURED_6DWRENCH_HPP +#define TSID_CONTACTS_MEASURED_6DWRENCH_HPP -#ifndef __invdyn_measured_6Dwrench_hpp__ -#define __invdyn_measured_6Dwrench_hpp__ +#include "tsid/contacts/measured-6d-wrench.hpp" -#include +#ifdef _MSC_VER +#pragma message( \ + "TSID DEPRECATED: Please update your includes from 'measured-6Dwrench.hpp' to 'measured-6d-wrench.hpp'") +#else +#warning \ + "TSID DEPRECATED: Please update your includes from 'measured-6Dwrench.hpp' to 'measured-6d-wrench.hpp'" +#endif -#include "tsid/contacts/measured-force-base.hpp" - -namespace tsid { -namespace contacts { -class Measured6Dwrench : public MeasuredForceBase { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - typedef math::Index Index; - typedef math::Vector6 Vector6; - typedef robots::RobotWrapper RobotWrapper; - typedef pinocchio::Data Data; - typedef pinocchio::Data::Matrix6x Matrix6x; - - Measured6Dwrench(const std::string &name, RobotWrapper &robot, - const std::string &frameName); - - const Vector &computeJointTorques(Data &data); - - /** - * Set the value of the external wrench applied by the environment on the - * robot. - */ - void setMeasuredContactForce(const Vector6 &fext); - - const Vector6 &getMeasuredContactForce() const; - - /** - * @brief Specifies if the external force and jacobian is - * expressed in the local frame or the local world-oriented frame. - * - * @param local_frame If true, represent external force and jacobian in the - * local frame. If false, represent them in the local world-oriented frame. - */ - void useLocalFrame(bool local_frame); - - protected: - std::string m_frame_name; - Index m_frame_id; - Vector6 m_fext; - Matrix6x m_J; - Matrix6x m_J_rotated; - Vector m_computedTorques; - bool m_local_frame; -}; -} // namespace contacts -} // namespace tsid - -#endif // ifndef __invdyn_measured_6Dwrench_hpp__ +#endif // TSID_CONTACTS_MEASURED_6DWRENCH_HPP diff --git a/include/tsid/contacts/measured-6d-wrench.hpp b/include/tsid/contacts/measured-6d-wrench.hpp new file mode 100644 index 00000000..10d9b035 --- /dev/null +++ b/include/tsid/contacts/measured-6d-wrench.hpp @@ -0,0 +1,71 @@ +// +// Copyright (c) 2025 CNRS INRIA LORIA +// +// This file is part of tsid +// tsid 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. +// tsid 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 +// tsid If not, see +// . +// + +#ifndef __invdyn_measured_6d_wrench_hpp__ +#define __invdyn_measured_6d_wrench_hpp__ + +#include + +#include "tsid/contacts/measured-force-base.hpp" + +namespace tsid { +namespace contacts { +class Measured6Dwrench : public MeasuredForceBase { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef math::Index Index; + typedef math::Vector6 Vector6; + typedef robots::RobotWrapper RobotWrapper; + typedef pinocchio::Data Data; + typedef pinocchio::Data::Matrix6x Matrix6x; + + Measured6Dwrench(const std::string &name, RobotWrapper &robot, + const std::string &frameName); + + const Vector &computeJointTorques(Data &data); + + /** + * Set the value of the external wrench applied by the environment on the + * robot. + */ + void setMeasuredContactForce(const Vector6 &fext); + + const Vector6 &getMeasuredContactForce() const; + + /** + * @brief Specifies if the external force and jacobian is + * expressed in the local frame or the local world-oriented frame. + * + * @param local_frame If true, represent external force and jacobian in the + * local frame. If false, represent them in the local world-oriented frame. + */ + void useLocalFrame(bool local_frame); + + protected: + std::string m_frame_name; + Index m_frame_id; + Vector6 m_fext; + Matrix6x m_J; + Matrix6x m_J_rotated; + Vector m_computedTorques; + bool m_local_frame; +}; +} // namespace contacts +} // namespace tsid + +#endif // ifndef __invdyn_measured_6d_wrench_hpp__ diff --git a/src/contacts/measured-3Dforce.cpp b/src/contacts/measured-3d-force.cpp similarity index 96% rename from src/contacts/measured-3Dforce.cpp rename to src/contacts/measured-3d-force.cpp index e1937f49..5f03de03 100644 --- a/src/contacts/measured-3Dforce.cpp +++ b/src/contacts/measured-3d-force.cpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2022 CNRS INRIA LORIA +// Copyright (c) 2025 CNRS INRIA LORIA // // This file is part of tsid // tsid is free software: you can redistribute it @@ -15,7 +15,7 @@ // . // -#include "tsid/contacts/measured-3Dforce.hpp" +#include "tsid/contacts/measured-3d-force.hpp" #include "tsid/robots/robot-wrapper.hpp" diff --git a/src/contacts/measured-6Dwrench.cpp b/src/contacts/measured-6d-wrench.cpp similarity index 96% rename from src/contacts/measured-6Dwrench.cpp rename to src/contacts/measured-6d-wrench.cpp index 1843ad68..2549fd20 100644 --- a/src/contacts/measured-6Dwrench.cpp +++ b/src/contacts/measured-6d-wrench.cpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2022 CNRS INRIA LORIA +// Copyright (c) 2025 CNRS INRIA LORIA // // This file is part of tsid // tsid is free software: you can redistribute it @@ -15,7 +15,7 @@ // . // -#include "tsid/contacts/measured-6Dwrench.hpp" +#include "tsid/contacts/measured-6d-wrench.hpp" #include "tsid/robots/robot-wrapper.hpp" diff --git a/tests/python/CMakeLists.txt b/tests/python/CMakeLists.txt index 06ea784c..ef8de258 100644 --- a/tests/python/CMakeLists.txt +++ b/tests/python/CMakeLists.txt @@ -8,7 +8,8 @@ set(${PYWRAP}_TESTS Solvers Tasks Trajectories - Deprecations) + Deprecations + Measured6DWrench) foreach(test ${${PYWRAP}_TESTS}) add_python_unit_test("py-${test}" "tests/python/test_${test}.py" diff --git a/tests/python/test_Measured6DWrench.py b/tests/python/test_Measured6DWrench.py new file mode 100644 index 00000000..7207380d --- /dev/null +++ b/tests/python/test_Measured6DWrench.py @@ -0,0 +1,30 @@ +from pathlib import Path + +import numpy as np +import pinocchio as se3 + +import tsid + +print("") +print("Test Measured6dWrench") +print("") + +tol = 1e-5 + +filename = str(Path(__file__).resolve().parent) +path = filename + "/../../models/romeo" +urdf = path + "/urdf/romeo.urdf" +vector = se3.StdVec_StdString() +vector.extend(item for item in path) +robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False) +model = robot.model() +data = robot.data() +frameName = "RAnkleRoll" +contact = tsid.Measured6dWrench("Measured6dwrench", robot, frameName) +wrench = np.asarray([1.5, 1.5, 1.5, 2.5, 2.5, 2.5]) # Some random wrench +contact.setMeasuredContactForce(wrench) +measured_wrench = contact.measuredContactForce +print(measured_wrench) + +invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False) +invdyn.addMeasuredForce(contact)