diff --git a/CMakeLists.txt b/CMakeLists.txt index 96a2b31997..cf984477e1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -124,13 +124,15 @@ IF(BUILD_WITH_CASADI_SUPPORT) ENDIF(BUILD_WITH_CASADI_SUPPORT) SET(BOOST_REQUIRED_COMPONENTS filesystem serialization system) -SET(BOOST_BUILD_COMPONENTS unit_test_framework) -SET(BOOST_OPTIONAL_COMPONENTS "") + +SET_BOOST_DEFAULT_OPTIONS() +EXPORT_BOOST_DEFAULT_OPTIONS() +ADD_PROJECT_DEPENDENCY(Boost REQUIRED COMPONENTS ${BOOST_REQUIRED_COMPONENTS}) IF(BUILD_PYTHON_INTERFACE) - SET(BOOST_OPTIONAL_COMPONENTS ${BOOST_OPTIONAL_COMPONENTS} python) FINDPYTHON() - ADD_PROJECT_DEPENDENCY(eigenpy 2.2.0 REQUIRED) + SEARCH_FOR_BOOST_PYTHON(REQUIRED) + ADD_PROJECT_DEPENDENCY(eigenpy 2.5.0 REQUIRED) ENDIF(BUILD_PYTHON_INTERFACE) IF(BUILD_WITH_HPP_FCL_SUPPORT) @@ -154,8 +156,6 @@ IF(BUILD_WITH_HPP_FCL_SUPPORT) ENDIF(BUILD_PYTHON_INTERFACE) ENDIF(BUILD_WITH_HPP_FCL_SUPPORT) -SET(BOOST_COMPONENTS ${BOOST_REQUIRED_COMPONENTS} ${BOOST_OPTIONAL_COMPONENTS} ${BOOST_BUILD_COMPONENTS}) -SEARCH_FOR_BOOST() # Enforce the preprocessed version of boost::list and boost::vector # This information is redundant with the content of include/pinocchio/container/boost-container-limits.hpp # but it avoids any compilation issue. diff --git a/bindings/python/multibody/data.hpp b/bindings/python/multibody/data.hpp index fecbf190c9..f6ba17da83 100644 --- a/bindings/python/multibody/data.hpp +++ b/bindings/python/multibody/data.hpp @@ -2,8 +2,8 @@ // Copyright (c) 2015-2020 CNRS INRIA // -#ifndef __pinocchio_python_data_hpp__ -#define __pinocchio_python_data_hpp__ +#ifndef __pinocchio_python_multibody_data_hpp__ +#define __pinocchio_python_multibody_data_hpp__ #include @@ -11,6 +11,7 @@ #include "pinocchio/serialization/data.hpp" #include +#include #include #include "pinocchio/bindings/python/serialization/serializable.hpp" @@ -114,56 +115,56 @@ namespace pinocchio .ADD_DATA_PROPERTY(oMi,"Body absolute placement (wrt world)") .ADD_DATA_PROPERTY(oMf,"frames absolute placement (wrt world)") .ADD_DATA_PROPERTY(liMi,"Body relative placement (wrt parent)") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(tau,"Joint torques (output of RNEA)") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(nle,"Non Linear Effects (output of nle algorithm)") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(ddq,"Joint accelerations (output of ABA)") + .ADD_DATA_PROPERTY(tau,"Joint torques (output of RNEA)") + .ADD_DATA_PROPERTY(nle,"Non Linear Effects (output of nle algorithm)") + .ADD_DATA_PROPERTY(ddq,"Joint accelerations (output of ABA)") .ADD_DATA_PROPERTY(Ycrb,"Inertia of the sub-tree composit rigid body") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(M,"The joint space inertia matrix") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(Minv,"The inverse of the joint space inertia matrix") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(C,"The Coriolis C(q,v) matrix such that the Coriolis effects are given by c(q,v) = C(q,v)v") + .ADD_DATA_PROPERTY(M,"The joint space inertia matrix") + .ADD_DATA_PROPERTY(Minv,"The inverse of the joint space inertia matrix") + .ADD_DATA_PROPERTY(C,"The Coriolis C(q,v) matrix such that the Coriolis effects are given by c(q,v) = C(q,v)v") .ADD_DATA_PROPERTY(Fcrb,"Spatial forces set, used in CRBA") .ADD_DATA_PROPERTY(lastChild,"Index of the last child (for CRBA)") .ADD_DATA_PROPERTY(nvSubtree,"Dimension of the subtree motion space (for CRBA)") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(U,"Joint Inertia square root (upper triangle)") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(D,"Diagonal of UDUT inertia decomposition") + .ADD_DATA_PROPERTY(U,"Joint Inertia square root (upper triangle)") + .ADD_DATA_PROPERTY(D,"Diagonal of UDUT inertia decomposition") .ADD_DATA_PROPERTY(parents_fromRow,"First previous non-zero row in M (used in Cholesky)") .ADD_DATA_PROPERTY(nvSubtree_fromRow,"Subtree of the current row index (used in Cholesky)") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(J,"Jacobian of joint placement") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(dJ,"Time variation of the Jacobian of joint placement (data.J).") + .ADD_DATA_PROPERTY(J,"Jacobian of joint placement") + .ADD_DATA_PROPERTY(dJ,"Time variation of the Jacobian of joint placement (data.J).") .ADD_DATA_PROPERTY(iMf,"Body placement wrt to algorithm end effector.") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(Ag, - "Centroidal matrix which maps from joint velocity to the centroidal momentum.") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(dAg, - "Time derivative of the centroidal momentum matrix Ag.") - .ADD_DATA_PROPERTY_READONLY(hg, - "Centroidal momentum (expressed in the frame centered at the CoM and aligned with the world frame).") - .ADD_DATA_PROPERTY_READONLY(dhg, - "Centroidal momentum time derivative (expressed in the frame centered at the CoM and aligned with the world frame).") - .ADD_DATA_PROPERTY_READONLY(Ig, - "Centroidal Composite Rigid Body Inertia.") + .ADD_DATA_PROPERTY(Ag, + "Centroidal matrix which maps from joint velocity to the centroidal momentum.") + .ADD_DATA_PROPERTY(dAg, + "Time derivative of the centroidal momentum matrix Ag.") + .ADD_DATA_PROPERTY(hg, + "Centroidal momentum (expressed in the frame centered at the CoM and aligned with the world frame).") + .ADD_DATA_PROPERTY(dhg, + "Centroidal momentum time derivative (expressed in the frame centered at the CoM and aligned with the world frame).") + .ADD_DATA_PROPERTY(Ig, + "Centroidal Composite Rigid Body Inertia.") .ADD_DATA_PROPERTY(com,"CoM position of the subtree starting at joint index i.") .ADD_DATA_PROPERTY(vcom,"CoM velocity of the subtree starting at joint index i.") .ADD_DATA_PROPERTY(acom,"CoM acceleration of the subtree starting at joint index i..") .ADD_DATA_PROPERTY(mass,"Mass of the subtree starting at joint index i.") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(Jcom,"Jacobian of center of mass.") + .ADD_DATA_PROPERTY(Jcom,"Jacobian of center of mass.") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(C,"Joint space Coriolis matrix.") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(dtau_dq,"Partial derivative of the joint torque vector with respect to the joint configuration.") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(dtau_dv,"Partial derivative of the joint torque vector with respect to the joint velocity.") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(ddq_dq,"Partial derivative of the joint acceleration vector with respect to the joint configuration.") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(ddq_dv,"Partial derivative of the joint acceleration vector with respect to the joint velocity.") + .ADD_DATA_PROPERTY(C,"Joint space Coriolis matrix.") + .ADD_DATA_PROPERTY(dtau_dq,"Partial derivative of the joint torque vector with respect to the joint configuration.") + .ADD_DATA_PROPERTY(dtau_dv,"Partial derivative of the joint torque vector with respect to the joint velocity.") + .ADD_DATA_PROPERTY(ddq_dq,"Partial derivative of the joint acceleration vector with respect to the joint configuration.") + .ADD_DATA_PROPERTY(ddq_dv,"Partial derivative of the joint acceleration vector with respect to the joint velocity.") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(kinetic_energy,"Kinetic energy in [J] computed by computeKineticEnergy") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(potential_energy,"Potential energy in [J] computed by computePotentialEnergy") + .ADD_DATA_PROPERTY(kinetic_energy,"Kinetic energy in [J] computed by computeKineticEnergy") + .ADD_DATA_PROPERTY(potential_energy,"Potential energy in [J] computed by computePotentialEnergy") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(lambda_c,"Lagrange Multipliers linked to contact forces") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(impulse_c,"Lagrange Multipliers linked to contact impulses") + .ADD_DATA_PROPERTY(lambda_c,"Lagrange Multipliers linked to contact forces") + .ADD_DATA_PROPERTY(impulse_c,"Lagrange Multipliers linked to contact impulses") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(dq_after,"Generalized velocity after the impact.") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(staticRegressor,"Static regressor.") - .ADD_DATA_PROPERTY_READONLY_BYVALUE(jointTorqueRegressor,"Joint torque regressor.") + .ADD_DATA_PROPERTY(dq_after,"Generalized velocity after the impact.") + .ADD_DATA_PROPERTY(staticRegressor,"Static regressor.") + .ADD_DATA_PROPERTY(jointTorqueRegressor,"Joint torque regressor.") .def(bp::self == bp::self) .def(bp::self != bp::self) @@ -182,8 +183,13 @@ namespace pinocchio .def(SerializableVisitor()) .def_pickle(PickleData()); - StdAlignedVectorPythonVisitor::expose("StdVec_vec3d"); - StdAlignedVectorPythonVisitor::expose("StdVec_Matrix6x"); + typedef PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) StdVec_Vector3; + typedef PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) StdVec_Matrix6x; + + StdAlignedVectorPythonVisitor::expose("StdVec_vec3d") + .def(details::overload_base_get_item_for_std_vector()); + StdAlignedVectorPythonVisitor::expose("StdVec_Matrix6x") + .def(details::overload_base_get_item_for_std_vector()); StdVectorPythonVisitor::expose("StdVec_int"); } @@ -191,4 +197,4 @@ namespace pinocchio }} // namespace pinocchio::python -#endif // ifndef __pinocchio_python_data_hpp__ +#endif // ifndef __pinocchio_python_multibody_data_hpp__ diff --git a/bindings/python/multibody/model.hpp b/bindings/python/multibody/model.hpp index 0f5f003283..66e3878b78 100644 --- a/bindings/python/multibody/model.hpp +++ b/bindings/python/multibody/model.hpp @@ -3,13 +3,14 @@ // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_python_model_hpp__ -#define __pinocchio_python_model_hpp__ +#ifndef __pinocchio_python_multibody_model_hpp__ +#define __pinocchio_python_multibody_model_hpp__ + +#include #include "pinocchio/multibody/model.hpp" #include "pinocchio/serialization/model.hpp" -#include #include #include #include @@ -18,6 +19,7 @@ #include "pinocchio/bindings/python/serialization/serializable.hpp" #include "pinocchio/bindings/python/utils/printable.hpp" #include "pinocchio/bindings/python/utils/copyable.hpp" +#include "pinocchio/bindings/python/utils/std-map.hpp" #include "pinocchio/bindings/python/utils/pickle-map.hpp" #include "pinocchio/bindings/python/utils/std-vector.hpp" @@ -164,7 +166,8 @@ namespace pinocchio void visit(PyClass& cl) const { cl - .def(bp::init<>(bp::arg("self"),"Default constructor. Constructs an empty model.")) + .def(bp::init<>(bp::arg("self"), + "Default constructor. Constructs an empty model.")) // Class Members .add_property("nq", &Model::nq) @@ -183,32 +186,22 @@ namespace pinocchio .add_property("names",&Model::names) .add_property("name",&Model::name) .add_property("referenceConfigurations", &Model::referenceConfigurations) - .add_property("rotorInertia", - make_getter(&Model::rotorInertia, bp::return_value_policy()), - make_setter(&Model::rotorInertia, bp::return_value_policy()), - "Vector of rotor inertia parameters.") - .add_property("rotorGearRatio", - make_getter(&Model::rotorGearRatio, bp::return_value_policy()), - make_setter(&Model::rotorGearRatio, bp::return_value_policy()), - "Vector of rotor gear ratio parameters.") - .add_property("effortLimit", - make_getter(&Model::effortLimit, bp::return_value_policy()), - make_setter(&Model::effortLimit, bp::return_value_policy()), - "Joint max effort.") - .add_property("velocityLimit", - make_getter(&Model::velocityLimit, bp::return_value_policy()), - make_setter(&Model::velocityLimit, bp::return_value_policy()), - "Joint max velocity.") - .add_property("lowerPositionLimit", - make_getter(&Model::lowerPositionLimit, bp::return_value_policy()), - make_setter(&Model::lowerPositionLimit, bp::return_value_policy()), - "Limit for joint lower position.") - .add_property("upperPositionLimit", - make_getter(&Model::upperPositionLimit, bp::return_value_policy()), - make_setter(&Model::upperPositionLimit, bp::return_value_policy()), - "Limit for joint upper position.") - .def_readwrite("frames",&Model::frames,"Vector of frames contained in the model.") + .def_readwrite("rotorInertia",&Model::rotorInertia, + "Vector of rotor inertia parameters.") + .def_readwrite("rotorGearRatio",&Model::rotorGearRatio, + "Vector of rotor gear ratio parameters.") + .def_readwrite("effortLimit",&Model::effortLimit, + "Joint max effort.") + .def_readwrite("velocityLimit",&Model::velocityLimit, + "Joint max velocity.") + .def_readwrite("lowerPositionLimit",&Model::lowerPositionLimit, + "Limit for joint lower position.") + .def_readwrite("upperPositionLimit",&Model::upperPositionLimit, + "Limit for joint upper position.") + + .def_readwrite("frames",&Model::frames, + "Vector of frames contained in the model.") .def_readwrite("supports", &Model::supports, @@ -219,41 +212,43 @@ namespace pinocchio &Model::subtrees, "Vector of subtrees. subtree[j] corresponds to the subtree supported by the joint j.") - .def_readwrite("gravity",&Model::gravity,"Motion vector corresponding to the gravity field expressed in the world Frame.") + .def_readwrite("gravity",&Model::gravity, + "Motion vector corresponding to the gravity field expressed in the world Frame.") // Class Methods .def("addJoint",&ModelPythonVisitor::addJoint, - bp::args("parent_id","joint_model","joint_placement","joint_name"), + bp::args("self","parent_id","joint_model","joint_placement","joint_name"), "Adds a joint to the kinematic tree. The joint is defined by its placement relative to its parent joint and its name.") .def("addJoint",&ModelPythonVisitor::addJointWithLimits, - bp::args("parent_id","joint_model","joint_placement","joint_name", + bp::args("self","parent_id","joint_model","joint_placement","joint_name", "max_effort","max_velocity","min_config","max_config"), "Adds a joint to the kinematic tree with given bounds. The joint is defined by its placement relative to its parent joint and its name.") .def("addJointFrame", &Model::addJointFrame, - addJointFrame_overload(bp::args("joint_id", "frame_id"), + addJointFrame_overload(bp::args("self","joint_id", "frame_id"), "Add the joint provided by its joint_id as a frame to the frame tree.\n" "The frame_id may be optionally provided.")) .def("appendBodyToJoint",&Model::appendBodyToJoint, - bp::args("joint_id","body_inertia","body_placement"), + bp::args("self","joint_id","body_inertia","body_placement"), "Appends a body to the joint given by its index. The body is defined by its inertia, its relative placement regarding to the joint and its name.") - .def("addBodyFrame", &Model::addBodyFrame, bp::args("body_name", "parentJoint", "body_placement", "previous_frame(parent frame)"), "add a body to the frame tree") - .def("getBodyId",&Model::getBodyId, bp::args("name"), "Return the index of a frame of type BODY given by its name") - .def("existBodyName", &Model::existBodyName, bp::args("name"), "Check if a frame of type BODY exists, given its name") - .def("getJointId",&Model::getJointId, bp::args("name"), "Return the index of a joint given by its name") - .def("existJointName", &Model::existJointName, bp::args("name"), "Check if a joint given by its name exists") + .def("addBodyFrame", &Model::addBodyFrame, bp::args("self","body_name", "parentJoint", "body_placement", "previous_frame(parent frame)"), "add a body to the frame tree") + .def("getBodyId",&Model::getBodyId, bp::args("self","name"), "Return the index of a frame of type BODY given by its name") + .def("existBodyName", &Model::existBodyName, bp::args("self","name"), "Check if a frame of type BODY exists, given its name") + .def("getJointId",&Model::getJointId, bp::args("self","name"), "Return the index of a joint given by its name") + .def("existJointName", &Model::existJointName, bp::args("self","name"), "Check if a joint given by its name exists") - .def("getFrameId",&Model::getFrameId,getFrameId_overload(bp::args("name","type"),"Returns the index of the frame given by its name and its type. If the frame is not in the frames vector, it returns the current size of the frames vector.")) + .def("getFrameId",&Model::getFrameId,getFrameId_overload(bp::args("self","name","type"),"Returns the index of the frame given by its name and its type. If the frame is not in the frames vector, it returns the current size of the frames vector.")) - .def("existFrame",&Model::existFrame,existFrame_overload(bp::args("name","type"),"Returns true if the frame given by its name exists inside the Model with the given type.")) + .def("existFrame",&Model::existFrame,existFrame_overload(bp::args("self","name","type"),"Returns true if the frame given by its name exists inside the Model with the given type.")) - .def("addFrame",(std::size_t (Model::*)(const Frame &)) &Model::addFrame,bp::args("frame"),"Add a frame to the vector of frames.") + .def("addFrame",(std::size_t (Model::*)(const Frame &)) &Model::addFrame,bp::args("self","frame"),"Add a frame to the vector of frames.") .def("createData", - &ModelPythonVisitor::createData, + &ModelPythonVisitor::createData,bp::arg("self"), "Create a Data object for the given model.") - .def("check",(bool (Model::*)(const Data &) const) &Model::check,bp::arg("data"),"Check consistency of data wrt model.") + .def("check",(bool (Model::*)(const Data &) const) &Model::check,bp::args("self","data"), + "Check consistency of data wrt model.") .def(bp::self == bp::self) .def(bp::self != bp::self) @@ -314,14 +309,17 @@ namespace pinocchio /* --- Expose --------------------------------------------------------- */ static void expose() { + typedef typename Model::ConfigVectorMap ConfigVectorMap; + typedef bp::map_indexing_suite map_indexing_suite; StdVectorPythonVisitor::expose("StdVec_Index"); StdVectorPythonVisitor::expose("StdVec_IndexVector"); StdVectorPythonVisitor::expose("StdVec_StdString"); StdVectorPythonVisitor::expose("StdVec_Bool"); StdVectorPythonVisitor::expose("StdVec_double"); bp::class_("StdMap_String_EigenVectorXd") - .def(bp::map_indexing_suite< typename Model::ConfigVectorMap, true >()) - .def_pickle(PickleMap()); + .def(map_indexing_suite()) + .def_pickle(PickleMap()) + .def(details::overload_base_get_item_for_std_map()); bp::class_("Model", "Articulated Rigid Body model", @@ -337,4 +335,4 @@ namespace pinocchio }} // namespace pinocchio::python -#endif // ifndef __pinocchio_python_model_hpp__ +#endif // ifndef __pinocchio_python_multibody_model_hpp__ diff --git a/bindings/python/spatial/force.hpp b/bindings/python/spatial/force.hpp index c8ffd63f74..c929ddc08a 100644 --- a/bindings/python/spatial/force.hpp +++ b/bindings/python/spatial/force.hpp @@ -3,10 +3,11 @@ // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_python_force_hpp__ -#define __pinocchio_python_force_hpp__ +#ifndef __pinocchio_python_spatial_force_hpp__ +#define __pinocchio_python_spatial_force_hpp__ #include +#include #include #include "pinocchio/spatial/se3.hpp" @@ -55,6 +56,9 @@ namespace pinocchio typedef typename Force::Vector3 Vector3; typedef typename Force::Scalar Scalar; + typedef typename Eigen::Map MapVector3; + typedef typename Eigen::Ref RefVector3; + template void visit(PyClass& cl) const { @@ -67,18 +71,23 @@ namespace pinocchio .def(bp::init((bp::arg("other")),"Copy constructor.")) .add_property("linear", - &ForcePythonVisitor::getLinear, + bp::make_function(&ForcePythonVisitor::getLinear, + bp::with_custodian_and_ward_postcall<0,1>()), &ForcePythonVisitor::setLinear, "Linear part of a *this, corresponding to the linear velocity in case of a Spatial velocity.") .add_property("angular", - &ForcePythonVisitor::getAngular, + bp::make_function(&ForcePythonVisitor::getAngular, + bp::with_custodian_and_ward_postcall<0,1>()), &ForcePythonVisitor::setAngular, "Angular part of a *this, corresponding to the angular velocity in case of a Spatial velocity.") .add_property("vector", - &ForcePythonVisitor::getVector, + bp::make_function((typename Force::ToVectorReturnType (Force::*)())&Force::toVector, + bp::return_internal_reference<>()), &ForcePythonVisitor::setVector, "Returns the components of *this as a 6d vector.") - .add_property("np",&ForcePythonVisitor::getVector) + .add_property("np", + bp::make_function((typename Force::ToVectorReturnType (Force::*)())&Force::toVector, + bp::return_internal_reference<>())) .def("se3Action",&Force::template se3Action, bp::args("self","M"),"Returns the result of the dual action of M on *this.") @@ -118,7 +127,8 @@ namespace pinocchio .def("Zero",&Force::Zero,"Returns a zero Force.") .staticmethod("Zero") - .def("__array__",&ForcePythonVisitor::getVector) + .def("__array__",bp::make_function((typename Force::ToVectorReturnType (Force::*)())&Force::toVector, + bp::return_internal_reference<>())) .def_pickle(Pickle()) ; @@ -147,20 +157,18 @@ namespace pinocchio { return bp::make_tuple((Vector3)f.linear(),(Vector3)f.angular()); } }; - static Vector3 getLinear(const Force & self ) { return self.linear(); } + static RefVector3 getLinear(Force & self ) { return self.linear(); } static void setLinear(Force & self, const Vector3 & f) { self.linear(f); } - static Vector3 getAngular(const Force & self) { return self.angular(); } + static RefVector3 getAngular(Force & self) { return self.angular(); } static void setAngular(Force & self, const Vector3 & n) { self.angular(n); } static void setZero(Force & self) { self.setZero(); } static void setRandom(Force & self) { self.setRandom(); } - static Vector6 getVector(const Force & self) { return self.toVector(); } static void setVector(Force & self, const Vector6 & f) { self = f; } }; } // namespace python } // namespace pinocchio -#endif // ifndef __pinocchio_python_se3_hpp__ - +#endif // ifndef __pinocchio_python_spatial_force_hpp__ diff --git a/bindings/python/spatial/inertia.hpp b/bindings/python/spatial/inertia.hpp index d3cdf57589..73650d411f 100644 --- a/bindings/python/spatial/inertia.hpp +++ b/bindings/python/spatial/inertia.hpp @@ -3,11 +3,12 @@ // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_python_inertia_hpp__ -#define __pinocchio_python_inertia_hpp__ +#ifndef __pinocchio_python_spatial_inertia_hpp__ +#define __pinocchio_python_spatial_inertia_hpp__ #include #include +#include #include #include "pinocchio/spatial/inertia.hpp" @@ -74,7 +75,8 @@ namespace pinocchio &InertiaPythonVisitor::setMass, "Mass of the Spatial Inertia.") .add_property("lever", - &InertiaPythonVisitor::getLever, + bp::make_function((typename Inertia::Vector3 & (Inertia::*)())&Inertia::lever, + bp::return_internal_reference<>()), &InertiaPythonVisitor::setLever, "Center of mass location of the Spatial Inertia. It corresponds to the location of the center of mass regarding to the frame where the Spatial Inertia is expressed.") .add_property("inertia", @@ -169,7 +171,6 @@ namespace pinocchio static Scalar getMass( const Inertia & self ) { return self.mass(); } static void setMass( Inertia & self, Scalar mass ) { self.mass() = mass; } - static Vector3 getLever( const Inertia & self ) { return self.lever(); } static void setLever( Inertia & self, const Vector3 & lever ) { self.lever() = lever; } static Matrix3 getInertia( const Inertia & self ) { return self.inertia().matrix(); } @@ -233,4 +234,4 @@ namespace pinocchio } // namespace python } // namespace pinocchio -#endif // ifndef __pinocchio_python_se3_hpp__ +#endif // ifndef __pinocchio_python_spatial_inertia_hpp__ diff --git a/bindings/python/spatial/motion.hpp b/bindings/python/spatial/motion.hpp index 06eb146c4b..25defbb751 100644 --- a/bindings/python/spatial/motion.hpp +++ b/bindings/python/spatial/motion.hpp @@ -3,10 +3,11 @@ // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_python_motion_hpp__ -#define __pinocchio_python_motion_hpp__ +#ifndef __pinocchio_python_spatial_motion_hpp__ +#define __pinocchio_python_spatial_motion_hpp__ #include +#include #include #include "pinocchio/spatial/se3.hpp" @@ -56,6 +57,9 @@ namespace pinocchio typedef ForceTpl::Options> Force; typedef typename Motion::Vector6 Vector6; typedef typename Motion::Vector3 Vector3; + + typedef typename Eigen::Map MapVector3; + typedef typename Eigen::Ref RefVector3; public: @@ -66,23 +70,28 @@ namespace pinocchio .def(bp::init<>("Default constructor")) .def(bp::init ((bp::arg("linear"),bp::arg("angular")), - "Initialize from linear and angular components of a Motion vector (don(t mix the order).")) + "Initialize from linear and angular components of a Motion vector (don't mix the order).")) .def(bp::init((bp::arg("vec")),"Init from a vector 6 [linear velocity, angular velocity]")) .def(bp::init((bp::arg("other")),"Copy constructor.")) .add_property("linear", - &MotionPythonVisitor::getLinear, + bp::make_function(&MotionPythonVisitor::getLinear, + bp::with_custodian_and_ward_postcall<0,1>()), &MotionPythonVisitor::setLinear, "Linear part of a *this, corresponding to the linear velocity in case of a Spatial velocity.") .add_property("angular", - &MotionPythonVisitor::getAngular, + bp::make_function(&MotionPythonVisitor::getAngular, + bp::with_custodian_and_ward_postcall<0,1>()), &MotionPythonVisitor::setAngular, "Angular part of a *this, corresponding to the angular velocity in case of a Spatial velocity.") .add_property("vector", - &MotionPythonVisitor::getVector, + bp::make_function((typename Motion::ToVectorReturnType (Motion::*)())&Motion::toVector, + bp::return_internal_reference<>()), &MotionPythonVisitor::setVector, "Returns the components of *this as a 6d vector.") - .add_property("np",&MotionPythonVisitor::getVector) + .add_property("np", + bp::make_function((typename Motion::ToVectorReturnType (Motion::*)())&Motion::toVector, + bp::return_internal_reference<>())) .def("se3Action",&Motion::template se3Action, bp::args("self","M"),"Returns the result of the action of M on *this.") @@ -132,7 +141,8 @@ namespace pinocchio .def("Zero",&Motion::Zero,"Returns a zero Motion.") .staticmethod("Zero") - .def("__array__",&MotionPythonVisitor::getVector) + .def("__array__",bp::make_function((typename Motion::ToVectorReturnType (Motion::*)())&Motion::toVector, + bp::return_internal_reference<>())) .def_pickle(Pickle()) ; @@ -160,12 +170,11 @@ namespace pinocchio { return bp::make_tuple((Vector3)m.linear(),(Vector3)m.angular()); } }; - static Vector3 getLinear(const Motion & self) { return self.linear(); } + static RefVector3 getLinear(Motion & self) { return self.linear(); } static void setLinear (Motion & self, const Vector3 & v) { self.linear(v); } - static Vector3 getAngular(const Motion & self) { return self.angular(); } + static RefVector3 getAngular(Motion & self) { return self.angular(); } static void setAngular(Motion & self, const Vector3 & w) { self.angular(w); } - static Vector6 getVector(const Motion & self) { return self.toVector(); } static void setVector(Motion & self, const Vector6 & v) { self = v; } static void setZero(Motion & self) { self.setZero(); } @@ -173,9 +182,6 @@ namespace pinocchio }; - - }} // namespace pinocchio::python -#endif // ifndef __pinocchio_python_se3_hpp__ - +#endif // ifndef __pinocchio_python_spatial_motion_hpp__ diff --git a/bindings/python/spatial/se3.hpp b/bindings/python/spatial/se3.hpp index 5ec653c08b..08234c6403 100644 --- a/bindings/python/spatial/se3.hpp +++ b/bindings/python/spatial/se3.hpp @@ -3,10 +3,11 @@ // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. // -#ifndef __pinocchio_python_se3_hpp__ -#define __pinocchio_python_se3_hpp__ +#ifndef __pinocchio_python_spatial_se3_hpp__ +#define __pinocchio_python_spatial_se3_hpp__ #include +#include #include #include "pinocchio/spatial/se3.hpp" @@ -46,7 +47,7 @@ namespace pinocchio template struct SE3PythonVisitor - : public boost::python::def_visitor< SE3PythonVisitor > + : public boost::python::def_visitor< SE3PythonVisitor > { typedef typename SE3::Scalar Scalar; typedef typename SE3::Matrix3 Matrix3; @@ -61,51 +62,49 @@ namespace pinocchio { cl .def(bp::init - ((bp::arg("Rotation matrix"),bp::arg("Translation vector")), + ((bp::arg("rotation"),bp::arg("translation")), "Initialize from a rotation matrix and a translation vector.")) .def(bp::init - ((bp::arg("Quaternion"),bp::arg("Translation vector")), + ((bp::arg("quat"),bp::arg("translation")), "Initialize from a quaternion and a translation vector.")) - .def(bp::init((bp::arg("trivial arg (should be 1)")),"Init to identity.")) + .def(bp::init((bp::arg("int")),"Init to identity.")) .def(bp::init((bp::arg("other")), "Copy constructor.")) .def(bp::init - ((bp::arg("Homogeneous matrix")), - "Initialize from a homogeneous matrix.")) + ((bp::arg("homegeneous_matrix")), + "Initialize from an homogeneous matrix.")) .add_property("rotation", - &getRotation, + bp::make_function((typename SE3::AngularRef (SE3::*)()) &SE3::rotation,bp::return_internal_reference<>()), (void (SE3::*)(const Matrix3 &)) &SE3::rotation, - "The rotation part of the transformation." - ) + "The rotation part of the transformation.") .add_property("translation", - &getTranslation, + bp::make_function((typename SE3::LinearRef (SE3::*)()) &SE3::translation,bp::return_internal_reference<>()), (void (SE3::*)(const Vector3 &)) &SE3::translation, - "The translation part of the transformation." - ) + "The translation part of the transformation.") .add_property("homogeneous",&SE3::toHomogeneousMatrix, - "Returns the homegeneous matrix of *this (acting on SE3).") + "Returns the equivalent homegeneous matrix (acting on SE3).") .add_property("action",&SE3::toActionMatrix, - "Returns the action matrix of *this (acting on Motion).") + "Returns the related action matrix (acting on Motion).") .def("toActionMatrix",&SE3::toActionMatrix,bp::arg("self"), - "Returns the action matrix of *this (acting on Motion).") + "Returns the related action matrix (acting on Motion).") .add_property("actionInverse",&SE3::toActionMatrixInverse, - "Returns the inverse of the action matrix of *this (acting on Motion).\n" + "Returns the inverse of the action matrix (acting on Motion).\n" "This is equivalent to do m.inverse().action") .def("toActionMatrixInverse",&SE3::toActionMatrixInverse,bp::arg("self"), - "Returns the inverse of the action matrix of *this (acting on Motion).\n" + "Returns the inverse of the action matrix (acting on Motion).\n" "This is equivalent to do m.inverse().toActionMatrix()") .add_property("dualAction",&SE3::toDualActionMatrix, - "Returns the dual action matrix of *this (acting on Force).") + "Returns the related dual action matrix (acting on Force).") .def("toDualActionMatrix",&SE3::toDualActionMatrix,bp::arg("self"), - "Returns the dual action matrix of *this (acting on Force).") + "Returns the related dual action matrix (acting on Force).") .def("setIdentity",&SE3PythonVisitor::setIdentity,bp::arg("self"), "Set *this to the identity placement.") .def("setRandom",&SE3PythonVisitor::setRandom,bp::arg("self"), "Set *this to a random placement.") - .def("inverse", &SE3::inverse, bp::arg("self"), + .def("inverse", &SE3::inverse, bp::arg("self"), "Returns the inverse transform") .def("act", (Vector3 (SE3::*)(const Vector3 &) const) &SE3::act, @@ -180,7 +179,7 @@ namespace pinocchio static void expose() { bp::class_("SE3", - "SE3 transformation composed defined by its translation and its rotation", + "SE3 transformation defined by a 3d vector and a rotation matrix.", bp::init<>()) .def(SE3PythonVisitor()) .def(CopyableVisitor()) @@ -198,9 +197,6 @@ namespace pinocchio { return bp::make_tuple((Matrix3)M.rotation(),(Vector3)M.translation()); } }; - static Vector3 getTranslation(const SE3 & self) { return self.translation(); } - static Matrix3 getRotation(const SE3 & self) { return self.rotation(); } - static void setIdentity(SE3 & self) { self.setIdentity(); } static void setRandom(SE3 & self) { self.setRandom(); } @@ -212,5 +208,4 @@ namespace pinocchio } // namespace python } // namespace pinocchio -#endif // ifndef __pinocchio_python_se3_hpp__ - +#endif // ifndef __pinocchio_python_spatial_se3_hpp__ diff --git a/bindings/python/utils/std-aligned-vector.hpp b/bindings/python/utils/std-aligned-vector.hpp index 4294693d12..c228fdb098 100644 --- a/bindings/python/utils/std-aligned-vector.hpp +++ b/bindings/python/utils/std-aligned-vector.hpp @@ -35,12 +35,13 @@ namespace pinocchio typedef container::aligned_vector vector_type; typedef StdContainerFromPythonList FromPythonListConverter; - static void expose(const std::string & class_name, - const std::string & doc_string = "") + static ::boost::python::class_ expose(const std::string & class_name, + const std::string & doc_string = "") { namespace bp = boost::python; - bp::class_(class_name.c_str(),doc_string.c_str()) + bp::class_ cl(class_name.c_str(),doc_string.c_str()); + cl .def(StdAlignedVectorPythonVisitor()) .def("tolist",&FromPythonListConverter::tolist,bp::arg("self"), "Returns the aligned_vector as a Python list.") @@ -49,6 +50,8 @@ namespace pinocchio // Register conversion if(EnableFromPythonListConverter) FromPythonListConverter::register_converter(); + + return cl; } }; } // namespace python diff --git a/bindings/python/utils/std-map.hpp b/bindings/python/utils/std-map.hpp new file mode 100644 index 0000000000..c5a5340f5e --- /dev/null +++ b/bindings/python/utils/std-map.hpp @@ -0,0 +1,77 @@ +// +// Copyright (c) 2020 INRIA +// + +#ifndef __pinocchio_python_utils_map_hpp__ +#define __pinocchio_python_utils_map_hpp__ + +#include + +namespace pinocchio +{ + namespace python + { + namespace details + { + template + struct overload_base_get_item_for_std_map + : public boost::python::def_visitor< overload_base_get_item_for_std_map > + { + typedef typename Container::value_type value_type; + typedef typename Container::value_type::second_type data_type; + typedef typename Container::key_type key_type; + typedef typename Container::key_type index_type; + + template + void visit(Class& cl) const + { + cl + .def("__getitem__", &base_get_item); + } + + private: + + static boost::python::object + base_get_item(boost::python::back_reference container, PyObject* i_) + { + namespace bp = ::boost::python; + + index_type idx = convert_index(container.get(), i_); + typename Container::iterator i = container.get().find(idx); + if (i == container.get().end()) + { + PyErr_SetString(PyExc_KeyError, "Invalid key"); + bp::throw_error_already_set(); + } + + typename bp::to_python_indirect convert; + return bp::object(bp::handle<>(convert(i->second))); + } + + static index_type + convert_index(Container& /*container*/, PyObject* i_) + { + namespace bp = ::boost::python; + bp::extract i(i_); + if (i.check()) + { + return i(); + } + else + { + bp::extract i(i_); + if (i.check()) + return i(); + } + + PyErr_SetString(PyExc_TypeError, "Invalid index type"); + bp::throw_error_already_set(); + return index_type(); + } + }; + + } + } +} + +#endif // ifndef __pinocchio_python_utils_map_hpp__ diff --git a/bindings/python/utils/std-vector.hpp b/bindings/python/utils/std-vector.hpp index 6cfff0377d..b41d295495 100644 --- a/bindings/python/utils/std-vector.hpp +++ b/bindings/python/utils/std-vector.hpp @@ -8,8 +8,10 @@ #include #include #include + #include #include +#include #include "pinocchio/bindings/python/utils/pickle-vector.hpp" @@ -17,7 +19,69 @@ namespace pinocchio { namespace python { - + + namespace details + { + template + struct overload_base_get_item_for_std_vector + : public boost::python::def_visitor< overload_base_get_item_for_std_vector > + { + typedef typename Container::value_type value_type; + typedef typename Container::value_type data_type; + typedef size_t index_type; + + template + void visit(Class& cl) const + { + cl + .def("__getitem__", &base_get_item); + } + + private: + + static boost::python::object + base_get_item(boost::python::back_reference container, PyObject* i_) + { + namespace bp = ::boost::python; + + index_type idx = convert_index(container.get(), i_); + typename Container::iterator i = container.get().begin(); + std::advance(i, idx); + if (i == container.get().end()) + { + PyErr_SetString(PyExc_KeyError, "Invalid index"); + bp::throw_error_already_set(); + } + + typename bp::to_python_indirect convert; + return bp::object(bp::handle<>(convert(*i))); + } + + static index_type + convert_index(Container & container, PyObject* i_) + { + namespace bp = boost::python; + bp::extract i(i_); + if (i.check()) + { + long index = i(); + if (index < 0) + index += container.size(); + if (index >= long(container.size()) || index < 0) + { + PyErr_SetString(PyExc_IndexError, "Index out of range"); + bp::throw_error_already_set(); + } + return index; + } + + PyErr_SetString(PyExc_TypeError, "Invalid index type"); + bp::throw_error_already_set(); + return index_type(); + } + }; + } + /// /// \brief Register the conversion from a Python list to a std::vector /// @@ -110,12 +174,13 @@ namespace pinocchio typedef std::vector vector_type; typedef StdContainerFromPythonList FromPythonListConverter; - static void expose(const std::string & class_name, - const std::string & doc_string = "") + static ::boost::python::class_ expose(const std::string & class_name, + const std::string & doc_string = "") { namespace bp = boost::python; - bp::class_(class_name.c_str(),doc_string.c_str()) + bp::class_ cl(class_name.c_str(),doc_string.c_str()); + cl .def(StdVectorPythonVisitor()) .def("tolist",&FromPythonListConverter::tolist,bp::arg("self"), "Returns the std::vector as a Python list.") @@ -124,6 +189,8 @@ namespace pinocchio // Register conversion if(EnableFromPythonListConverter) FromPythonListConverter::register_converter(); + + return cl; } }; diff --git a/cmake b/cmake index b12791b89a..4a79907faf 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit b12791b89aa99c88bd24326daaa4b4add27d3a33 +Subproject commit 4a79907faf1038e91d1fad7e2502166831ec6520 diff --git a/src/autodiff/casadi/utils/static-if.hpp b/src/autodiff/casadi/utils/static-if.hpp index e727d7db99..f0f53aa637 100644 --- a/src/autodiff/casadi/utils/static-if.hpp +++ b/src/autodiff/casadi/utils/static-if.hpp @@ -27,21 +27,21 @@ namespace pinocchio { switch(op) { - case LT: - return ::casadi::Matrix::if_else(lhs_value < rhs_value,then_value,else_value); - break; - case LE: - return ::casadi::Matrix::if_else(lhs_value <= rhs_value,then_value,else_value); - break; - case EQ: - return ::casadi::Matrix::if_else(lhs_value == rhs_value,then_value,else_value); - break; - case GE: - return ::casadi::Matrix::if_else(lhs_value >= rhs_value,then_value,else_value); - break; - case GT: - return ::casadi::Matrix::if_else(lhs_value > rhs_value,then_value,else_value); - break; + case LT: + return ::casadi::Matrix::if_else(lhs_value < rhs_value,then_value,else_value); + break; + case LE: + return ::casadi::Matrix::if_else(lhs_value <= rhs_value,then_value,else_value); + break; + case EQ: + return ::casadi::Matrix::if_else(lhs_value == rhs_value,then_value,else_value); + break; + case GE: + return ::casadi::Matrix::if_else(lhs_value >= rhs_value,then_value,else_value); + break; + case GT: + return ::casadi::Matrix::if_else(lhs_value > rhs_value,then_value,else_value); + break; } } }; diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt index 5be42b655e..7839e5c26e 100644 --- a/unittest/CMakeLists.txt +++ b/unittest/CMakeLists.txt @@ -37,9 +37,8 @@ MACRO(ADD_OPTIONAL_COMPILE_DEPENDENCY PKG_CONFIG_STRING) ADD_DEPENDENCY(0 1 ${PKG_CONFIG_STRING} "${PKG_CONFIG_DEBUG_STRING}") ENDMACRO() -# --- RULES ------------------------------------------------------------------- -# --- RULES ------------------------------------------------------------------- -# --- RULES ------------------------------------------------------------------- +# Find Boost.UnitTestFramework +FIND_PACKAGE(Boost COMPONENTS unit_test_framework) # Math components ADD_PINOCCHIO_UNIT_TEST(eigen-basic-op) diff --git a/unittest/python/bindings_SE3.py b/unittest/python/bindings_SE3.py index 2f5228ed02..f30dbf20dc 100644 --- a/unittest/python/bindings_SE3.py +++ b/unittest/python/bindings_SE3.py @@ -111,6 +111,13 @@ def test_point_action(self): bma = amb.inverse() self.assertTrue(np.allclose(bma.act(p), (bMa.dot(p_homogeneous))[0:3])) + def test_member(self): + M = pin.SE3.Random() + trans = M.translation + M.translation[2] = 1. + + self.assertTrue(trans[2] == M.translation[2]) + def test_conversions(self): def compute (m): tq_vec = pin.SE3ToXYZQUAT (m) diff --git a/unittest/python/bindings_com_velocity_derivatives.py b/unittest/python/bindings_com_velocity_derivatives.py index 801113358d..b52e5e2212 100644 --- a/unittest/python/bindings_com_velocity_derivatives.py +++ b/unittest/python/bindings_com_velocity_derivatives.py @@ -45,7 +45,7 @@ def test_numdiff(self): def calc_vc(q,vq): """ Compute COM velocity """ pin.centerOfMass(rmodel,rdata,q,vq) - return rdata.vcom[0] + return rdata.vcom[0].copy() dvc_dqn = df_dq(rmodel,lambda _q: calc_vc(_q,vq),q) self.assertTrue(np.allclose(dvc_dq,dvc_dqn,atol=np.sqrt(self.precision))) diff --git a/unittest/python/bindings_force.py b/unittest/python/bindings_force.py index 6d8a8c83e6..ebcef18a60 100644 --- a/unittest/python/bindings_force.py +++ b/unittest/python/bindings_force.py @@ -33,12 +33,18 @@ def test_set_linear(self): f.linear = lin self.assertTrue(np.allclose(f.linear, lin)) + f.linear[1] = 1. + self.assertTrue(f.linear[1] == 1.) + def test_set_angular(self): f = pin.Force.Zero() ang = rand(3) f.angular = ang self.assertTrue(np.allclose(f.angular, ang)) + f.angular[1] = 1. + self.assertTrue(f.angular[1] == 1.) + def test_set_vector(self): f = pin.Force.Zero() vec = rand(6) diff --git a/unittest/python/bindings_motion.py b/unittest/python/bindings_motion.py index 4f7f59a902..f5fc0c00fa 100644 --- a/unittest/python/bindings_motion.py +++ b/unittest/python/bindings_motion.py @@ -33,12 +33,18 @@ def test_set_linear(self): v.linear = lin self.assertTrue(np.allclose(v.linear, lin)) + v.linear[1] = 1. + self.assertTrue(v.linear[1] == 1.) + def test_set_angular(self): v = pin.Motion.Zero() ang = rand(3) v.angular = ang self.assertTrue(np.allclose(v.angular, ang)) + v.angular[1] = 1. + self.assertTrue(v.angular[1] == 1.) + def test_set_vector(self): v = pin.Motion.Zero() vec = rand(6)