Verified Commit f2cf5aef authored by Justin Carpentier's avatar Justin Carpentier
Browse files

python/multibody: allows modifications of Eigen objects

parent a9c2f230
Pipeline #10931 failed with stage
in 70 minutes and 57 seconds
......@@ -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 <boost/python.hpp>
......@@ -11,6 +11,7 @@
#include "pinocchio/serialization/data.hpp"
#include <eigenpy/memory.hpp>
#include <eigenpy/eigen-to-python.hpp>
#include <eigenpy/exception.hpp>
#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<Data>())
.def_pickle(PickleData<Data>());
StdAlignedVectorPythonVisitor<Vector3, true>::expose("StdVec_vec3d");
StdAlignedVectorPythonVisitor<Matrix6x, true>::expose("StdVec_Matrix6x");
typedef PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) StdVec_Vector3;
typedef PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) StdVec_Matrix6x;
StdAlignedVectorPythonVisitor<Vector3,false>::expose("StdVec_vec3d")
.def(details::overload_base_get_item_for_std_vector<StdVec_Vector3>());
StdAlignedVectorPythonVisitor<Matrix6x,false>::expose("StdVec_Matrix6x")
.def(details::overload_base_get_item_for_std_vector<StdVec_Matrix6x>());
StdVectorPythonVisitor<int>::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__
......@@ -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 <eigenpy/eigen-to-python.hpp>
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/serialization/model.hpp"
#include <boost/python/suite/indexing/map_indexing_suite.hpp>
#include <boost/python/overloads.hpp>
#include <eigenpy/memory.hpp>
#include <eigenpy/exception.hpp>
......@@ -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<bp::return_by_value>()),
make_setter(&Model::rotorInertia, bp::return_value_policy<bp::return_by_value>()),
"Vector of rotor inertia parameters.")
.add_property("rotorGearRatio",
make_getter(&Model::rotorGearRatio, bp::return_value_policy<bp::return_by_value>()),
make_setter(&Model::rotorGearRatio, bp::return_value_policy<bp::return_by_value>()),
"Vector of rotor gear ratio parameters.")
.add_property("effortLimit",
make_getter(&Model::effortLimit, bp::return_value_policy<bp::return_by_value>()),
make_setter(&Model::effortLimit, bp::return_value_policy<bp::return_by_value>()),
"Joint max effort.")
.add_property("velocityLimit",
make_getter(&Model::velocityLimit, bp::return_value_policy<bp::return_by_value>()),
make_setter(&Model::velocityLimit, bp::return_value_policy<bp::return_by_value>()),
"Joint max velocity.")
.add_property("lowerPositionLimit",
make_getter(&Model::lowerPositionLimit, bp::return_value_policy<bp::return_by_value>()),
make_setter(&Model::lowerPositionLimit, bp::return_value_policy<bp::return_by_value>()),
"Limit for joint lower position.")
.add_property("upperPositionLimit",
make_getter(&Model::upperPositionLimit, bp::return_value_policy<bp::return_by_value>()),
make_setter(&Model::upperPositionLimit, bp::return_value_policy<bp::return_by_value>()),
"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<ConfigVectorMap,false> map_indexing_suite;
StdVectorPythonVisitor<Index>::expose("StdVec_Index");
StdVectorPythonVisitor<IndexVector>::expose("StdVec_IndexVector");
StdVectorPythonVisitor<std::string>::expose("StdVec_StdString");
StdVectorPythonVisitor<bool>::expose("StdVec_Bool");
StdVectorPythonVisitor<Scalar>::expose("StdVec_double");
bp::class_<typename Model::ConfigVectorMap>("StdMap_String_EigenVectorXd")
.def(bp::map_indexing_suite< typename Model::ConfigVectorMap, true >())
.def_pickle(PickleMap<typename Model::ConfigVectorMap>());
.def(map_indexing_suite())
.def_pickle(PickleMap<typename Model::ConfigVectorMap>())
.def(details::overload_base_get_item_for_std_map<typename Model::ConfigVectorMap>());
bp::class_<Model>("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__
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment