From c2af82554c40b2246aeb06f396604364ae489f58 Mon Sep 17 00:00:00 2001 From: Alexis DUBURCQ Date: Mon, 26 Apr 2021 23:11:44 +0200 Subject: [PATCH] [core/python] Custom return policy to avoid conflits with external modules. (#332) * [core] Fix bug at Robot initialization when defining default flexible joints in the model. * [core/python] Fix wrong keyword arguments for 'EngineMultiRobot.reset' method. * [core/python] Add new Boost.Python return policy that can be used to by-pass converter registry if 'convertToPython' has been specified. * [python] Add some missing typing return types. Co-authored-by: Alexis Duburcq --- CMakeLists.txt | 2 +- core/include/jiminy/core/Macros.h | 10 +- core/src/robot/Model.cc | 3 +- python/jiminy_py/src/jiminy_py/robot.py | 6 +- .../include/jiminy/python/Utilities.h | 59 ++++++++++- python/jiminy_pywrap/src/Constraints.cc | 31 ++---- python/jiminy_pywrap/src/Engine.cc | 100 +++++------------- python/jiminy_pywrap/src/Module.cc | 25 ----- python/jiminy_pywrap/src/Robot.cc | 36 +++---- 9 files changed, 115 insertions(+), 157 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 382bfa9c5..e07f66691 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.10) # Set the build version -set(BUILD_VERSION 1.6.12) +set(BUILD_VERSION 1.6.13) # Extract major, minor and patch version string(REPLACE "." ";" _VERSION "${BUILD_VERSION}") diff --git a/core/include/jiminy/core/Macros.h b/core/include/jiminy/core/Macros.h index 2b1c3ca48..89060b5b2 100644 --- a/core/include/jiminy/core/Macros.h +++ b/core/include/jiminy/core/Macros.h @@ -92,7 +92,7 @@ namespace jiminy struct is_vector > : std::true_type {}; template - constexpr bool is_vector_v = is_vector::value; // `inline` variables are not supported by gcc<7.3 + constexpr bool is_vector_v = is_vector >::value; // `inline` variables are not supported by gcc<7.3 // ========================== is_map ============================ @@ -106,7 +106,7 @@ namespace jiminy } template - struct isMap : public decltype(isMapDetail::test(std::declval())) {}; + struct isMap : public decltype(isMapDetail::test(std::declval >())) {}; template struct is_map : std::false_type {}; @@ -131,7 +131,7 @@ namespace jiminy } template - struct isEigenObject : public decltype(isEigenObjectDetail::test(std::declval())) {}; + struct isEigenObject : public decltype(isEigenObjectDetail::test(std::declval >())) {}; template struct is_eigen : public std::false_type {}; @@ -165,7 +165,7 @@ namespace jiminy } template - struct isEigenVector : public decltype(isEigenVectorDetail::test(std::declval())) {}; + struct isEigenVector : public decltype(isEigenVectorDetail::test(std::declval >())) {}; template struct is_eigen_vector : std::false_type {}; @@ -183,7 +183,7 @@ namespace jiminy \ template \ struct isPinocchioJoint ## type : \ - public decltype(isPinocchioJoint ## type ## Detail ::test(std::declval())) {}; \ + public decltype(isPinocchioJoint ## type ## Detail ::test(std::declval >())) {}; \ \ template \ struct is_pinocchio_joint_ ## name : public std::false_type {}; \ diff --git a/core/src/robot/Model.cc b/core/src/robot/Model.cc index c3466224d..9c1ae47d1 100644 --- a/core/src/robot/Model.cc +++ b/core/src/robot/Model.cc @@ -973,8 +973,9 @@ namespace jiminy { // Check if joint name exists std::string const & frameName = flexibleJoint.frameName; - if (!pncModel_.existFrame(frameName)) + if (!pncModelFlexibleOrig_.existFrame(frameName)) { + PRINT_ERROR("Frame '", frameName, "' does not exists. Impossible to insert flexible joint on it."); return hresult_t::ERROR_GENERIC; } diff --git a/python/jiminy_py/src/jiminy_py/robot.py b/python/jiminy_py/src/jiminy_py/robot.py index f7561ab05..48b4eec07 100644 --- a/python/jiminy_py/src/jiminy_py/robot.py +++ b/python/jiminy_py/src/jiminy_py/robot.py @@ -510,7 +510,7 @@ class BaseJiminyRobot(jiminy.Robot): name than the URDF file will be detected automatically without requiring to manually specify its path. """ - def __init__(self): + def __init__(self) -> None: super().__init__() self.extra_info = {} self.urdf_path_orig = None @@ -521,7 +521,7 @@ def initialize(self, mesh_path: Optional[str] = None, has_freeflyer: bool = True, avoid_instable_collisions: bool = True, - verbose: bool = True): + verbose: bool = True) -> None: """Initialize the robot. :param urdf_path: Path of the URDF file of the robot. @@ -844,5 +844,5 @@ def __del__(self) -> None: pass @property - def name(self): + def name(self) -> str: return self.pinocchio_model.name diff --git a/python/jiminy_pywrap/include/jiminy/python/Utilities.h b/python/jiminy_pywrap/include/jiminy/python/Utilities.h index 6394670a9..6d1650dd3 100644 --- a/python/jiminy_pywrap/include/jiminy/python/Utilities.h +++ b/python/jiminy_pywrap/include/jiminy/python/Utilities.h @@ -1,6 +1,7 @@ #ifndef UTILITIES_PYTHON_H #define UTILITIES_PYTHON_H +#include #include "jiminy/core/Types.h" #include "jiminy/core/Macros.h" @@ -297,7 +298,7 @@ namespace python /////////////////////////////////////////////////////////////////////////////// template - std::enable_if_t::value + std::enable_if_t && !is_eigen::value, bp::object> convertToPython(T const & data, bool const & /* copy */ = true) { @@ -341,7 +342,7 @@ namespace python } template - std::enable_if_t::value, bp::object> + std::enable_if_t, bp::object> convertToPython(T & data, bool const & copy = true) { bp::list dataPy; @@ -353,7 +354,7 @@ namespace python } template - std::enable_if_t::value, bp::object> + std::enable_if_t, bp::object> convertToPython(T const & data, bool const & copy = true) { bp::list dataPy; @@ -398,6 +399,54 @@ namespace python return configPyDict; } + template + struct converterToPython + { + static PyObject * convert(T const & data) + { + return bp::incref(convertToPython(data, copy).ptr()); + } + + static PyTypeObject const * get_pytype() + { + if (is_vector_v) // constexpr + { + return &PyList_Type; + } + else if (std::is_same::value + || std::is_same::value) // constexpr + { + return &PyDict_Type; + } + std::type_info const * typeId(&typeid(bp::object)); + bp::converter::registration const * r = bp::converter::registry::query(*typeId); + return r ? r->to_python_target_type(): 0; + } + }; + + template + struct result_converter + { + template + struct apply + { + struct type + { + typedef typename std::remove_reference::type value_type; + + PyObject * operator()(T const & x) const + { + return converterToPython::convert(x); + } + + PyTypeObject const * get_pytype(void) const + { + return converterToPython::get_pytype(); + } + }; + }; + }; + // **************************************************************************** // **************************** PYTHON TO C++ ********************************* // **************************************************************************** @@ -439,7 +488,7 @@ namespace python /////////////////////////////////////////////////////////////////////////////// template - std::enable_if_t::value + std::enable_if_t && !is_map::value && !is_eigen::value && !(std::is_integral::value && !std::is_same::value) @@ -554,7 +603,7 @@ namespace python } template - std::enable_if_t::value, T> + std::enable_if_t, T> convertFromPython(bp::object const & dataPy) { using V = typename T::value_type; diff --git a/python/jiminy_pywrap/src/Constraints.cc b/python/jiminy_pywrap/src/Constraints.cc index cef772ceb..64c648fe3 100644 --- a/python/jiminy_pywrap/src/Constraints.cc +++ b/python/jiminy_pywrap/src/Constraints.cc @@ -71,8 +71,10 @@ namespace python &PyConstraintVisitor::setIsEnable) .add_property("baumgarte_freq", &AbstractConstraintBase::getBaumgarteFreq, &AbstractConstraintBase::setBaumgarteFreq) - .add_property("jacobian", &PyConstraintVisitor::getJacobian) - .add_property("drift", &PyConstraintVisitor::getDrift) + .add_property("jacobian", bp::make_function(&AbstractConstraintBase::getJacobian, + bp::return_value_policy >())) + .add_property("drift", bp::make_function(&AbstractConstraintBase::getDrift, + bp::return_value_policy >())) ; } @@ -96,24 +98,6 @@ namespace python } } - static bp::object getJacobian(AbstractConstraintBase const & self) - { - // Do not use automatic converter for efficiency - return convertToPython(self.getJacobian(), false); - } - - static bp::object getDrift(AbstractConstraintBase const & self) - { - // Do not use automatic converter for efficiency - return convertToPython(self.getDrift(), false); - } - - static bp::object getReferenceConfiguration(JointConstraint & self) - { - // Do not use automatic converter for efficiency - return convertToPython(self.getReferenceConfiguration(), false); - } - static void setReferenceConfiguration(JointConstraint & self, vectorN_t const & value) { self.setReferenceConfiguration(value); @@ -150,7 +134,8 @@ namespace python bp::return_value_policy())) .add_property("joint_idx", bp::make_function(&JointConstraint::getJointIdx, bp::return_value_policy())) - .add_property("reference_configuration", &PyConstraintVisitor::getReferenceConfiguration, + .add_property("reference_configuration", bp::make_function(&JointConstraint::getReferenceConfiguration, + bp::return_value_policy >()), &PyConstraintVisitor::setReferenceConfiguration); bp::class_, @@ -180,9 +165,9 @@ namespace python bp::args("self", "first_frame_name", "second_frame_name", "distance_reference"))) .def_readonly("type", &DistanceConstraint::type_) .add_property("frames_names", bp::make_function(&DistanceConstraint::getFramesNames, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("frames_idx", bp::make_function(&DistanceConstraint::getFramesIdx, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("reference_distance", bp::make_function(&DistanceConstraint::getReferenceDistance, bp::return_value_policy())); diff --git a/python/jiminy_pywrap/src/Engine.cc b/python/jiminy_pywrap/src/Engine.cc index 773584c31..9c25870fe 100644 --- a/python/jiminy_pywrap/src/Engine.cc +++ b/python/jiminy_pywrap/src/Engine.cc @@ -117,28 +117,16 @@ namespace python .def_readonly("iter_failed", &stepperState_t::iterFailed) .def_readonly("t", &stepperState_t::t) .def_readonly("dt", &stepperState_t::dt) - .add_property("q", &PyStepperStateVisitor::getPosition) - .add_property("v", &PyStepperStateVisitor::getVelocity) - .add_property("a", &PyStepperStateVisitor::getAcceleration) + .add_property("q", bp::make_getter(&stepperState_t::qSplit, + bp::return_value_policy >())) + .add_property("v", bp::make_getter(&stepperState_t::vSplit, + bp::return_value_policy >())) + .add_property("a", bp::make_getter(&stepperState_t::aSplit, + bp::return_value_policy >())) .def("__repr__", &PyStepperStateVisitor::repr) ; } - static bp::object getPosition(stepperState_t const & self) - { - return convertToPython >(self.qSplit, false); - } - - static bp::object getVelocity(stepperState_t const & self) - { - return convertToPython >(self.vSplit, false); - } - - static bp::object getAcceleration(stepperState_t const & self) - { - return convertToPython >(self.aSplit, false); - } - static std::string repr(stepperState_t const & self) { std::stringstream s; @@ -192,68 +180,28 @@ namespace python void visit(PyClass & cl) const { cl - .add_property("q", &PySystemStateVisitor::getPosition) - .add_property("v", &PySystemStateVisitor::getVelocity) - .add_property("a", &PySystemStateVisitor::getAcceleration) - .add_property("command", &PySystemStateVisitor::getCommand) - .add_property("u", &PySystemStateVisitor::getTotalEffort) - .add_property("u_motor", &PySystemStateVisitor::getMotorEffort) - .add_property("u_internal", &PySystemStateVisitor::getInternalEffort) - .add_property("u_custom", &PySystemStateVisitor::getCustomDynamicsEffort) + .add_property("q", bp::make_getter(&systemState_t::q, + bp::return_value_policy >())) + .add_property("v", bp::make_getter(&systemState_t::v, + bp::return_value_policy >())) + .add_property("a", bp::make_getter(&systemState_t::a, + bp::return_value_policy >())) + .add_property("command", bp::make_getter(&systemState_t::command, + bp::return_value_policy >())) + .add_property("u", bp::make_getter(&systemState_t::u, + bp::return_value_policy >())) + .add_property("u_motor", bp::make_getter(&systemState_t::uMotor, + bp::return_value_policy >())) + .add_property("u_internal", bp::make_getter(&systemState_t::uInternal, + bp::return_value_policy >())) + .add_property("u_custom", bp::make_getter(&systemState_t::uCustom, + bp::return_value_policy >())) .add_property("f_external", bp::make_getter(&systemState_t::fExternal, bp::return_internal_reference<>())) .def("__repr__", &PySystemStateVisitor::repr) ; } - static bp::object getPosition(systemState_t const & self) - { - // Do not use automatic converter for efficiency - return convertToPython(self.q, false); - } - - static bp::object getVelocity(systemState_t const & self) - { - // Do not use automatic converter for efficiency - return convertToPython(self.v, false); - } - - static bp::object getAcceleration(systemState_t const & self) - { - // Do not use automatic converter for efficiency - return convertToPython(self.a, false); - } - - static bp::object getCommand(systemState_t const & self) - { - // Do not use automatic converter for efficiency - return convertToPython(self.command, false); - } - - static bp::object getTotalEffort(systemState_t const & self) - { - // Do not use automatic converter for efficiency - return convertToPython(self.u, false); - } - - static bp::object getMotorEffort(systemState_t const & self) - { - // Do not use automatic converter for efficiency - return convertToPython(self.uMotor, false); - } - - static bp::object getInternalEffort(systemState_t const & self) - { - // Do not use automatic converter for efficiency - return convertToPython(self.uInternal, false); - } - - static bp::object getCustomDynamicsEffort(systemState_t const & self) - { - // Do not use automatic converter for efficiency - return convertToPython(self.uCustom, false); - } - static std::string repr(systemState_t & self) { std::stringstream s; @@ -426,7 +374,7 @@ namespace python void (EngineMultiRobot::*)(bool_t const &, bool_t const &) >(&EngineMultiRobot::reset), (bp::arg("self"), - bp::arg("remove_all_forces") = false, + bp::arg("reset_random_generator") = false, bp::arg("remove_all_forces") = false)) .def("start", &PyEngineMultiRobotVisitor::start, (bp::arg("self"), "q_init_list", "v_init_list", @@ -503,7 +451,7 @@ namespace python .add_property("systems", bp::make_getter(&EngineMultiRobot::systems_, bp::return_internal_reference<>())) .add_property("systems_names", bp::make_function(&EngineMultiRobot::getSystemsNames, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("stepper_state", bp::make_function(&EngineMultiRobot::getStepperState, bp::return_internal_reference<>())) .add_property("is_simulation_running", &PyEngineMultiRobotVisitor::getIsSimulationRunning) diff --git a/python/jiminy_pywrap/src/Module.cc b/python/jiminy_pywrap/src/Module.cc index 0c1e0a163..da08dd0dc 100755 --- a/python/jiminy_pywrap/src/Module.cc +++ b/python/jiminy_pywrap/src/Module.cc @@ -46,31 +46,6 @@ namespace python bp::return_value_policy(), \ (bp::arg("self"), bp::arg("t"), bp::arg("q"), bp::arg("v"))); - template - struct converterToPython - { - static PyObject * convert(T const & data) - { - return bp::incref(convertToPython(data).ptr()); - } - - static PyTypeObject const * get_pytype() - { - std::type_info const * typeId(&typeid(bp::object)); - if (is_vector::value) // constexpr - { - typeId = &typeid(bp::list); - } - else if (std::is_same::value - || std::is_same::value) // constexpr - { - typeId = &typeid(bp::dict); - } - bp::converter::registration const * r = bp::converter::registry::query(*typeId); - return r ? r->to_python_target_type(): 0; - } - }; - uint32_t getRandomSeed(void) { uint32_t seed; diff --git a/python/jiminy_pywrap/src/Robot.cc b/python/jiminy_pywrap/src/Robot.cc index e17428a00..07d7265ea 100644 --- a/python/jiminy_pywrap/src/Robot.cc +++ b/python/jiminy_pywrap/src/Robot.cc @@ -90,7 +90,7 @@ namespace python .add_property("is_initialized", bp::make_function(&Model::getIsInitialized, bp::return_value_policy())) .add_property("mesh_package_dirs", bp::make_function(&Model::getMeshPackageDirs, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("urdf_path", bp::make_function(&Model::getUrdfPath, bp::return_value_policy())) .add_property("has_freeflyer", bp::make_function(&Model::getHasFreeflyer, @@ -104,27 +104,27 @@ namespace python bp::return_value_policy())) .add_property("collision_bodies_names", bp::make_function(&Model::getCollisionBodiesNames, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("collision_bodies_idx", bp::make_function(&Model::getCollisionBodiesIdx, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("collision_pairs_idx_by_body", bp::make_function(&Model::getCollisionPairsIdx, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("contact_frames_names", bp::make_function(&Model::getContactFramesNames, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("contact_frames_idx", bp::make_function(&Model::getContactFramesIdx, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("rigid_joints_names", bp::make_function(&Model::getRigidJointsNames, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("rigid_joints_idx", bp::make_function(&Model::getRigidJointsModelIdx, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("rigid_joints_position_idx", bp::make_function(&Model::getRigidJointsPositionIdx, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("rigid_joints_velocity_idx", bp::make_function(&Model::getRigidJointsVelocityIdx, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("flexible_joints_names", bp::make_function(&Model::getFlexibleJointsNames, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("flexible_joints_idx", bp::make_function(&Model::getFlexibleJointsModelIdx, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("position_limit_lower", bp::make_function(&Model::getPositionLimitMin, bp::return_value_policy())) @@ -134,11 +134,11 @@ namespace python bp::return_value_policy())) .add_property("logfile_position_headers", bp::make_function(&Model::getPositionFieldnames, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("logfile_velocity_headers", bp::make_function(&Model::getVelocityFieldnames, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("logfile_acceleration_headers", bp::make_function(&Model::getAccelerationFieldnames, - bp::return_value_policy())) + bp::return_value_policy >())) ; } @@ -314,7 +314,7 @@ namespace python .add_property("nmotors", bp::make_function(&Robot::nmotors, bp::return_value_policy())) .add_property("motors_names", bp::make_function(&Robot::getMotorsNames, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("motors_position_idx", &Robot::getMotorsPositionIdx) .add_property("motors_velocity_idx", &Robot::getMotorsVelocityIdx) .add_property("sensors_names", &PyRobotVisitor::getSensorsNames) @@ -325,9 +325,9 @@ namespace python bp::return_value_policy())) .add_property("logfile_command_headers", bp::make_function(&Robot::getCommandFieldnames, - bp::return_value_policy())) + bp::return_value_policy >())) .add_property("logfile_motor_effort_headers", bp::make_function(&Robot::getMotorEffortFieldnames, - bp::return_value_policy())) + bp::return_value_policy >())) ; }