From 411a62b725bb6d24f483d46a7e729dc95913a3b9 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Thu, 18 Apr 2024 15:50:59 +0200 Subject: [PATCH] [core] Add backlash support at motor-level. --- core/include/jiminy/core/fwd.h | 1 + .../jiminy/core/hardware/abstract_motor.h | 21 ++- core/include/jiminy/core/robot/model.h | 17 +- core/include/jiminy/core/robot/robot.h | 9 +- .../include/jiminy/core/utilities/pinocchio.h | 4 + core/src/engine/engine.cc | 71 ++++----- core/src/hardware/abstract_motor.cc | 47 ++++-- core/src/robot/model.cc | 110 ++++++++----- core/src/robot/robot.cc | 150 ++++++++++++++---- core/src/utilities/pinocchio.cc | 74 +++++++-- core/unit/engine_sanity_check.cc | 1 - .../atlas/atlas_v4_options.toml | 1 - .../bipedal_robots/cassie/cassie_options.toml | 1 - data/bipedal_robots/digit/digit_options.toml | 1 - .../anymal/anymal_options.toml | 1 - data/toys_models/ant/ant_options.toml | 1 - .../common/gym_jiminy/common/envs/generic.py | 5 - .../unit_py/test_deformation_estimator.py | 1 - .../unit_py/data/simple_pendulum.urdf | 9 ++ python/jiminy_py/unit_py/test_dense_pole.py | 1 - .../jiminy_py/unit_py/test_simple_pendulum.py | 99 +++++++++++- python/jiminy_py/unit_py/utilities.py | 1 - python/jiminy_pywrap/src/motors.cc | 3 + python/jiminy_pywrap/src/robot.cc | 10 +- 24 files changed, 471 insertions(+), 168 deletions(-) diff --git a/core/include/jiminy/core/fwd.h b/core/include/jiminy/core/fwd.h index 8b9bd5313..3cd45f272 100644 --- a/core/include/jiminy/core/fwd.h +++ b/core/include/jiminy/core/fwd.h @@ -211,6 +211,7 @@ namespace jiminy using FlexibilityConfig = std::vector; + // Generic configuration holder using GenericConfig = std::unordered_map(options.at("mechanicalReduction"))), @@ -68,7 +72,9 @@ namespace jiminy commandLimitFromUrdf(boost::get(options.at("commandLimitFromUrdf"))), commandLimit(boost::get(options.at("commandLimit"))), enableArmature(boost::get(options.at("enableArmature"))), - armature(boost::get(options.at("armature"))) + armature(boost::get(options.at("armature"))), + enableBacklash(boost::get(options.at("enableBacklash"))), + backlash(boost::get(options.at("backlash"))) { } }; @@ -144,6 +150,9 @@ namespace jiminy /// \brief Rotor inertia of the motor. double getArmature() const; + /// \brief Backlash of the transmission. + double getBacklash() const; + /// \brief Request the motor to update its actual effort based of the input data. /// /// \details It assumes that the internal state of the robot is consistent with the input @@ -190,7 +199,8 @@ namespace jiminy /// /// \details This method must be called before initializing the sensor. void attach(std::weak_ptr robot, - std::function notifyRobot, + std::function notifyRobot, MotorSharedStorage * sharedStorage); /// \brief Detach the sensor from the robot. @@ -216,14 +226,17 @@ namespace jiminy Eigen::Index jointVelocityIndex_{0}; double commandLimit_{0.0}; double armature_{0.0}; + double backlash_{0.0}; private: /// \brief Name of the motor. std::string name_; /// \brief Flag to determine whether the motor is attached to a robot. bool isAttached_{false}; - /// \brief Notify the robot that the configuration of the sensors have changed. - std::function notifyRobot_{}; + /// \brief Whether the robot must be notified. + bool mustNotifyRobot_{false}; + /// \brief Notify the robot that the configuration of the motor have changed. + std::function notifyRobot_{}; /// \brief Shared data between every motors associated with the robot. MotorSharedStorage * sharedStorage_{nullptr}; }; diff --git a/core/include/jiminy/core/robot/model.h b/core/include/jiminy/core/robot/model.h index e09b31f69..de5efce07 100644 --- a/core/include/jiminy/core/robot/model.h +++ b/core/include/jiminy/core/robot/model.h @@ -16,6 +16,7 @@ namespace jiminy inline constexpr std::string_view JOINT_PREFIX_BASE{"current"}; inline constexpr std::string_view FREE_FLYER_NAME{"Freeflyer"}; inline constexpr std::string_view FLEXIBLE_JOINT_SUFFIX{"Flexibility"}; + inline constexpr std::string_view BACKLASH_JOINT_SUFFIX{"Backlash"}; class AbstractConstraintBase; class FrameConstraint; @@ -135,7 +136,6 @@ namespace jiminy virtual GenericConfig getDefaultJointOptions() { GenericConfig config; - config["enablePositionLimit"] = true; config["positionLimitFromUrdf"] = true; config["positionLimitMin"] = Eigen::VectorXd{}; config["positionLimitMax"] = Eigen::VectorXd{}; @@ -182,7 +182,6 @@ namespace jiminy struct JointOptions { - const bool enablePositionLimit; const bool positionLimitFromUrdf; /// \brief Min position limit of all the mechanical joints of the theoretical model. const Eigen::VectorXd positionLimitMin; @@ -192,7 +191,6 @@ namespace jiminy const Eigen::VectorXd velocityLimit; JointOptions(const GenericConfig & options) : - enablePositionLimit{boost::get(options.at("enablePositionLimit"))}, positionLimitFromUrdf{boost::get(options.at("positionLimitFromUrdf"))}, positionLimitMin{boost::get(options.at("positionLimitMin"))}, positionLimitMax{boost::get(options.at("positionLimitMax"))}, @@ -350,6 +348,8 @@ namespace jiminy const std::vector & getMechanicalJointVelocityIndices() const; const std::vector & getFlexibilityJointNames() const; const std::vector & getFlexibilityJointIndices() const; + const std::vector & getBacklashJointNames() const; + const std::vector & getBacklashJointIndices() const; const Eigen::VectorXd & getPositionLimitMin() const; const Eigen::VectorXd & getPositionLimitMax() const; @@ -372,6 +372,7 @@ namespace jiminy protected: void generateModelExtended(const uniform_random_bit_generator_ref & g); + virtual void initializeExtendedModel(); void addFlexibilityJointsToExtendedModel(); void addBiasedToExtendedModel(const uniform_random_bit_generator_ref & g); @@ -440,12 +441,14 @@ namespace jiminy /// \brief All the indices of the mechanical joints in the velocity vector of the robot, /// ie including all their respective degrees of freedom. std::vector mechanicalJointVelocityIndices_{}; - /// \brief Name of the flexibility joints of the robot regardless of whether the - /// flexibilities are enabled. + /// \brief Name of the flexibility joints of the robot if enabled. std::vector flexibilityJointNames_{}; - /// \brief Index of the flexibility joints in the pinocchio robot regardless of whether the - /// flexibilities are enabled. + /// \brief Index of the flexibility joints in the pinocchio robot if enabled. std::vector flexibilityJointIndices_{}; + /// \brief Name of the backlash joints of the robot if enabled. + std::vector backlashJointNames_{}; + /// \brief Index of the backlash joints in the pinocchio robot if enabled. + std::vector backlashJointIndices_{}; /// \brief Store constraints. ConstraintTree constraints_{}; diff --git a/core/include/jiminy/core/robot/robot.h b/core/include/jiminy/core/robot/robot.h index 280b296a7..b4a3f0572 100644 --- a/core/include/jiminy/core/robot/robot.h +++ b/core/include/jiminy/core/robot/robot.h @@ -65,7 +65,7 @@ namespace jiminy std::weak_ptr getMotor(const std::string & motorName) const; const MotorVector & getMotors() const; void detachMotor(const std::string & motorName); - void detachMotors(std::vector motorsNames = {}); + void detachMotors(std::vector motorNames = {}); void attachSensor(std::shared_ptr sensor); std::shared_ptr getSensor(const std::string & sensorType, const std::string & sensorName); @@ -134,7 +134,12 @@ namespace jiminy protected: void refreshMotorProxies(); void refreshSensorProxies(); - virtual void refreshProxies() override; + void refreshProxies() override; + + void initializeExtendedModel() override; + + private: + void detachMotor(const std::string & motorName, bool triggerReset); protected: bool isTelemetryConfigured_{false}; diff --git a/core/include/jiminy/core/utilities/pinocchio.h b/core/include/jiminy/core/utilities/pinocchio.h index 4bb3a11cc..c28550226 100644 --- a/core/include/jiminy/core/utilities/pinocchio.h +++ b/core/include/jiminy/core/utilities/pinocchio.h @@ -78,6 +78,10 @@ namespace jiminy void addFlexibilityJointAtFixedFrame(pinocchio::Model & model, const std::string & frameName); + void addBacklashJointAfterMechanicalJoint(pinocchio::Model & model, + const std::string & parentJointName, + const std::string & newJointName); + Eigen::MatrixXd JIMINY_DLLAPI interpolatePositions(const pinocchio::Model & model, const Eigen::VectorXd & timesIn, const Eigen::MatrixXd & positionsIn, diff --git a/core/src/engine/engine.cc b/core/src/engine/engine.cc index 110495779..336f435d1 100644 --- a/core/src/engine/engine.cc +++ b/core/src/engine/engine.cc @@ -1005,8 +1005,7 @@ namespace jiminy /* Check that the initial configuration is not out-of-bounds if appropriate. Note that EPS allows to be very slightly out-of-bounds, which may occurs because of rounding errors. */ - if ((robot->modelOptions_->joints.enablePositionLimit && - (contactModel_ == ContactModelType::CONSTRAINT) && + if (((contactModel_ == ContactModelType::CONSTRAINT) && ((EPS < q.array() - robot->getPositionLimitMax().array()).any() || (EPS < robot->getPositionLimitMin().array() - q.array()).any()))) { @@ -1264,7 +1263,6 @@ namespace jiminy const ConstraintTree & constraints = (*robotIt)->getConstraints(); constraints.foreach( [&contactModel = contactModel_, - &enablePositionLimit = (*robotIt)->modelOptions_->joints.enablePositionLimit, &freq = engineOptions_->contacts.stabilizationFreq]( const std::shared_ptr & constraint, ConstraintNodeType node) @@ -1281,15 +1279,11 @@ namespace jiminy switch (node) { case ConstraintNodeType::BOUNDS_JOINTS: - if (!enablePositionLimit) - { - return; - } - { - auto & jointConstraint = - static_cast(*constraint.get()); - jointConstraint.setRotationDir(false); - } + { + auto & jointConstraint = + static_cast(*constraint.get()); + jointConstraint.setRotationDir(false); + } [[fallthrough]]; case ConstraintNodeType::CONTACT_FRAMES: case ConstraintNodeType::COLLISION_BODIES: @@ -3519,38 +3513,39 @@ namespace jiminy const pinocchio::Data & data = robot->pinocchioData_; const ConstraintTree & constraints = robot->getConstraints(); - // Enforce the position limit (mechanical joints only) - if (robot->modelOptions_->joints.enablePositionLimit) + /* Enforce position limits for all joints having bounds constraints, ie mechanical and + backlash joints. */ + const Eigen::VectorXd & positionLimitMin = robot->getPositionLimitMin(); + const Eigen::VectorXd & positionLimitMax = robot->getPositionLimitMax(); + for (auto & constraintItem : constraints.boundJoints) { - const Eigen::VectorXd & positionLimitMin = robot->getPositionLimitMin(); - const Eigen::VectorXd & positionLimitMax = robot->getPositionLimitMax(); - const std::vector & mechanicalJointIndices = - robot->getMechanicalJointIndices(); - for (std::size_t i = 0; i < mechanicalJointIndices.size(); ++i) - { - auto & constraint = constraints.boundJoints[i].second; - computePositionLimitsForcesAlgo::run( - model.joints[mechanicalJointIndices[i]], - typename computePositionLimitsForcesAlgo::ArgsType(data, - q, - v, - positionLimitMin, - positionLimitMax, - engineOptions_, - contactModel_, - constraint, - uInternal)); - } - } - - // Enforce the velocity limit (mechanical joints only) + auto & constraint = constraintItem.second; + const auto jointConstraint = std::static_pointer_cast(constraint); + const pinocchio::JointIndex jointIndex = jointConstraint->getJointIndex(); + computePositionLimitsForcesAlgo::run( + model.joints[jointIndex], + typename computePositionLimitsForcesAlgo::ArgsType(data, + q, + v, + positionLimitMin, + positionLimitMax, + engineOptions_, + contactModel_, + constraint, + uInternal)); + } + + // Enforce velocity limits for all joints having bounds constraints if requested if (robot->modelOptions_->joints.enableVelocityLimit) { const Eigen::VectorXd & velocityLimitMax = robot->getVelocityLimit(); - for (pinocchio::JointIndex mechanicalJointIndex : robot->getMechanicalJointIndices()) + for (auto & constraintItem : constraints.boundJoints) { + auto & constraint = constraintItem.second; + const auto jointConstraint = std::static_pointer_cast(constraint); + const pinocchio::JointIndex jointIndex = jointConstraint->getJointIndex(); computeVelocityLimitsForcesAlgo::run( - model.joints[mechanicalJointIndex], + model.joints[jointIndex], typename computeVelocityLimitsForcesAlgo::ArgsType( data, v, velocityLimitMax, engineOptions_, contactModel_, uInternal)); } diff --git a/core/src/hardware/abstract_motor.cc b/core/src/hardware/abstract_motor.cc index fd825cbfa..13fbc2c2c 100644 --- a/core/src/hardware/abstract_motor.cc +++ b/core/src/hardware/abstract_motor.cc @@ -23,9 +23,10 @@ namespace jiminy } } - void AbstractMotorBase::attach(std::weak_ptr robot, - std::function notifyRobot, - MotorSharedStorage * sharedStorage) + void AbstractMotorBase::attach( + std::weak_ptr robot, + std::function notifyRobot, + MotorSharedStorage * sharedStorage) { // Make sure the motor is not already attached if (isAttached_) @@ -163,28 +164,37 @@ namespace jiminy } // Check if the internal buffers must be updated - bool internalBuffersMustBeUpdated = false; if (isInitialized_) { // Check if armature has changed const bool enableArmature = boost::get(motorOptions.at("enableArmature")); - internalBuffersMustBeUpdated |= (baseMotorOptions_->enableArmature != enableArmature); + mustNotifyRobot_ |= (baseMotorOptions_->enableArmature != enableArmature); if (enableArmature) { const double armature = boost::get(motorOptions.at("armature")); - internalBuffersMustBeUpdated |= // + mustNotifyRobot_ |= // std::abs(armature - baseMotorOptions_->armature) > EPS; } + // Check if backlash has changed + const bool enableBacklash = boost::get(motorOptions.at("enableBacklash")); + mustNotifyRobot_ |= (baseMotorOptions_->enableBacklash != enableBacklash); + if (enableBacklash) + { + const double backlash = boost::get(motorOptions.at("backlash")); + mustNotifyRobot_ |= // + std::abs(backlash - baseMotorOptions_->backlash) > EPS; + } + // Check if command limit has changed const bool commandLimitFromUrdf = boost::get(motorOptions.at("commandLimitFromUrdf")); - internalBuffersMustBeUpdated |= + mustNotifyRobot_ |= (baseMotorOptions_->commandLimitFromUrdf != commandLimitFromUrdf); if (!commandLimitFromUrdf) { const double commandLimit = boost::get(motorOptions.at("commandLimit")); - internalBuffersMustBeUpdated |= + mustNotifyRobot_ |= std::abs(commandLimit - baseMotorOptions_->commandLimit) > EPS; } } @@ -198,7 +208,7 @@ namespace jiminy // Refresh the proxies if the robot is initialized if available if (robot) { - if (internalBuffersMustBeUpdated && robot->getIsInitialized() && isAttached_) + if (mustNotifyRobot_ && robot->getIsInitialized() && isAttached_) { refreshProxies(); } @@ -274,10 +284,22 @@ namespace jiminy armature_ = 0.0; } + // Get the transmission backlash + if (baseMotorOptions_->enableBacklash) + { + backlash_ = baseMotorOptions_->backlash; + } + else + { + backlash_ = 0.0; + } + // Propagate the user-defined motor inertia at Pinocchio model level if (notifyRobot_) { - notifyRobot_(*this); + const bool mustNotifyRobot = mustNotifyRobot_; + mustNotifyRobot_ = false; + notifyRobot_(*this, mustNotifyRobot); } } @@ -370,6 +392,11 @@ namespace jiminy return armature_; } + double AbstractMotorBase::getBacklash() const + { + return backlash_; + } + void AbstractMotorBase::computeEffortAll(double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, diff --git a/core/src/robot/model.cc b/core/src/robot/model.cc index 5279ff9dc..3195ce6e2 100644 --- a/core/src/robot/model.cc +++ b/core/src/robot/model.cc @@ -210,11 +210,22 @@ namespace jiminy const std::optional & collisionModel, const std::optional & visualModel) { + // Make sure that the pinocchio model is valid if (pinocchioModel.nq == 0) { JIMINY_THROW(std::invalid_argument, "Pinocchio model must not be empty."); } + // Make sure that the model is actually managed by a shared ptr + try + { + (void)shared_from_this(); + } + catch (std::bad_weak_ptr & e) + { + JIMINY_THROW(bad_control_flow, "Model must be managed by a std::shared_ptr."); + } + // Clear existing constraints constraints_.clear(); jointSpatialAccelerations_.clear(); @@ -891,20 +902,35 @@ namespace jiminy JIMINY_THROW(bad_control_flow, "Model not initialized."); } - // Initialize the extended model from the theoretical one - pinocchioModel_ = pinocchioModelTh_; + // Initialize the extended model + initializeExtendedModel(); // Add flexibility joints to the extended model if requested - flexibilityJointIndices_.clear(); - flexibilityJointNames_.clear(); if (modelOptions_->dynamics.enableFlexibility) { addFlexibilityJointsToExtendedModel(); } - /* Add biases to the dynamics properties of the model. - Note that is also refresh all proxies automatically. */ + // Add biases to the dynamics properties of the model addBiasedToExtendedModel(g); + + // Refresh internal proxies + refreshProxies(); + } + + void Model::initializeExtendedModel() + { + // Clear some proxies + backlashJointIndices_.clear(); + backlashJointNames_.clear(); + flexibilityJointIndices_.clear(); + flexibilityJointNames_.clear(); + + // Simply copy the theoretical model by default + pinocchioModel_ = pinocchioModelTh_; + + // Initially set effortLimit to zero systematically + pinocchioModel_.effortLimit.setZero(); } void Model::addFlexibilityJointsToExtendedModel() @@ -987,9 +1013,6 @@ namespace jiminy void Model::addBiasedToExtendedModel(const uniform_random_bit_generator_ref & g) { - // Initially set effortLimit to zero systematically - pinocchioModel_.effortLimit.setZero(); - for (const std::string & jointName : mechanicalJointNames_) { const pinocchio::JointIndex jointIndex = @@ -1058,9 +1081,6 @@ namespace jiminy // Re-allocate data to be consistent with new extended model initializePinocchioData(pinocchioModel_, pinocchioData_); - - // Refresh internal proxies - refreshProxies(); } void Model::computeConstraints(const Eigen::VectorXd & q, const Eigen::VectorXd & v) @@ -1116,6 +1136,7 @@ namespace jiminy void Model::refreshProxies() { + // Make sure that the model is initialized if (!isInitialized_) { JIMINY_THROW(bad_control_flow, "Model not initialized."); @@ -1188,34 +1209,38 @@ namespace jiminy } } - /* Get the joint position limits from the URDF or the user options. - Do NOT use robot_->pinocchioModel_.(lower|upper)PositionLimit. */ + // Get mechanical joint position limits from the URDF or user options positionLimitMin_.setConstant(pinocchioModel_.nq, -INF); positionLimitMax_.setConstant(pinocchioModel_.nq, +INF); - if (modelOptions_->joints.enablePositionLimit) + if (modelOptions_->joints.positionLimitFromUrdf) { - if (modelOptions_->joints.positionLimitFromUrdf) + for (Eigen::Index positionIndex : mechanicalJointPositionIndices_) { - for (Eigen::Index positionIndex : mechanicalJointPositionIndices_) - { - positionLimitMin_[positionIndex] = - pinocchioModel_.lowerPositionLimit[positionIndex]; - positionLimitMax_[positionIndex] = - pinocchioModel_.upperPositionLimit[positionIndex]; - } + positionLimitMin_[positionIndex] = + pinocchioModel_.lowerPositionLimit[positionIndex]; + positionLimitMax_[positionIndex] = + pinocchioModel_.upperPositionLimit[positionIndex]; } - else + } + else + { + for (std::size_t i = 0; i < mechanicalJointPositionIndices_.size(); ++i) { - for (std::size_t i = 0; i < mechanicalJointPositionIndices_.size(); ++i) - { - Eigen::Index positionIndex = mechanicalJointPositionIndices_[i]; - positionLimitMin_[positionIndex] = modelOptions_->joints.positionLimitMin[i]; - positionLimitMax_[positionIndex] = modelOptions_->joints.positionLimitMax[i]; - } + Eigen::Index positionIndex = mechanicalJointPositionIndices_[i]; + positionLimitMin_[positionIndex] = modelOptions_->joints.positionLimitMin[i]; + positionLimitMax_[positionIndex] = modelOptions_->joints.positionLimitMax[i]; } } + // Get the backlash joint position limits + for (pinocchio::JointIndex jointIndex : backlashJointIndices_) + { + const Eigen::Index positionIndex = pinocchioModel_.idx_qs[jointIndex]; + positionLimitMin_[positionIndex] = pinocchioModel_.lowerPositionLimit[positionIndex]; + positionLimitMax_[positionIndex] = pinocchioModel_.upperPositionLimit[positionIndex]; + } + /* Overwrite the position bounds for some specific joint type, mainly due to quaternion normalization and cos/sin representation. */ Eigen::Index idx_q, nq; @@ -1232,11 +1257,11 @@ namespace jiminy idx_q = joint.idx_q() + 3; nq = 4; break; - case JointModelType::UNSUPPORTED: case JointModelType::LINEAR: case JointModelType::ROTARY: case JointModelType::PLANAR: case JointModelType::TRANSLATION: + case JointModelType::UNSUPPORTED: default: continue; } @@ -1500,16 +1525,9 @@ namespace jiminy } // Check if the position or velocity limits have changed, and refresh proxies if so - bool enablePositionLimit = - boost::get(jointOptionsHolder.at("enablePositionLimit")); bool enableVelocityLimit = boost::get(jointOptionsHolder.at("enableVelocityLimit")); - if (enablePositionLimit != modelOptions_->joints.enablePositionLimit) - { - internalBuffersMustBeUpdated = true; - } - else if (enablePositionLimit && - (positionLimitFromUrdf != modelOptions_->joints.positionLimitFromUrdf)) + if (positionLimitFromUrdf != modelOptions_->joints.positionLimitFromUrdf) { internalBuffersMustBeUpdated = true; } @@ -1523,7 +1541,7 @@ namespace jiminy internalBuffersMustBeUpdated = true; } - // Check if the flexibility model and its proxies must be regenerated + // Check if the extended model must be regenerated bool enableFlexibility = boost::get(dynOptionsHolder.at("enableFlexibility")); if (modelOptions_ && ((enableFlexibility != modelOptions_->dynamics.enableFlexibility) || @@ -1581,7 +1599,7 @@ namespace jiminy if (isExtendedModelInvalid) { - // Trigger models regeneration + // Trigger extended regeneration reset(std::random_device{}); } else if (internalBuffersMustBeUpdated) @@ -1858,6 +1876,16 @@ namespace jiminy return flexibilityJointIndices_; } + const std::vector & Model::getBacklashJointNames() const + { + return backlashJointNames_; + } + + const std::vector & Model::getBacklashJointIndices() const + { + return backlashJointIndices_; + } + /// \brief Returns true if at least one constraint is active on the robot. bool Model::hasConstraints() const { diff --git a/core/src/robot/robot.cc b/core/src/robot/robot.cc index 13158af73..55709b2aa 100644 --- a/core/src/robot/robot.cc +++ b/core/src/robot/robot.cc @@ -9,6 +9,8 @@ #include "jiminy/core/hardware/abstract_sensor.h" #include "jiminy/core/control/abstract_controller.h" #include "jiminy/core/control/controller_functor.h" +#include "jiminy/core/constraints/abstract_constraint.h" +#include "jiminy/core/constraints/joint_constraint.h" #include "jiminy/core/robot/robot.h" @@ -183,8 +185,8 @@ namespace jiminy // Define robot notification method, responsible for updating the robot if // necessary after changing the motor parameters, for example the armature. - auto notifyRobot = - [robot_ = std::weak_ptr(shared_from_this())](AbstractMotorBase & motorIn) + auto notifyRobot = [robot_ = std::weak_ptr(shared_from_this())]( + AbstractMotorBase & motorIn, bool hasChanged) mutable { // Make sure the robot still exists auto robot = robot_.lock(); @@ -194,14 +196,23 @@ namespace jiminy "Robot has been deleted. Impossible to notify motor update."); } - // Update rotor inertia and effort limit of pinocchio model - const Eigen::Index mechanicalJointVelocityIndex = - getJointVelocityFirstIndex(robot->pinocchioModelTh_, motorIn.getJointName()); - robot->pinocchioModel_.rotorInertia[motorIn.getJointVelocityIndex()] = - robot->pinocchioModelTh_.rotorInertia[mechanicalJointVelocityIndex] + - motorIn.getArmature(); - robot->pinocchioModel_.effortLimit[motorIn.getJointVelocityIndex()] = - motorIn.getCommandLimit(); + /* Trigger extended model regeneration. + * The whole robot must be reset as joint and frame indices would be corrupted. + * Skip reset if nothing has changed from this motor point of view. This is necessary + to prevent infinite recursive reset loop. */ + if (hasChanged) + { + robot->reset(std::random_device{}); + } + + // Update rotor inertia + const std::string & mechanicaljointName = motorIn.getJointName(); + std::string inertiaJointName = mechanicaljointName; + const Eigen::Index motorVelocityIndex = motorIn.getJointVelocityIndex(); + robot->pinocchioModel_.rotorInertia[motorVelocityIndex] += motorIn.getArmature(); + + // Update effort limit + robot->pinocchioModel_.effortLimit[motorVelocityIndex] = motorIn.getCommandLimit(); }; // Attach the motor @@ -214,7 +225,7 @@ namespace jiminy refreshMotorProxies(); } - void Robot::detachMotor(const std::string & motorName) + void Robot::detachMotor(const std::string & motorName, bool triggerReset) { // The robot must be initialized if (!isInitialized_) @@ -230,36 +241,55 @@ namespace jiminy "Please stop it before removing motors."); } - auto motorIt = std::find_if(motors_.cbegin(), - motors_.cend(), + // Get motor if it exists + auto motorIt = std::find_if(motors_.begin(), + motors_.end(), [&motorName](const auto & elem) { return (elem->getName() == motorName); }); - if (motorIt == motors_.cend()) + if (motorIt == motors_.end()) { JIMINY_THROW(std::logic_error, "No motor with name '", motorName, "' is attached."); } - // Reset effortLimit and rotorInertia - const std::shared_ptr & motor = *motorIt; - const Eigen::Index mechanicalJointVelocityIndex = - ::jiminy::getJointVelocityFirstIndex(pinocchioModelTh_, motor->getJointName()); - pinocchioModel_.rotorInertia[motor->getJointVelocityIndex()] = - pinocchioModelTh_.rotorInertia[mechanicalJointVelocityIndex]; - pinocchioModel_.effortLimit[motor->getJointVelocityIndex()] = 0.0; + // Hold the motor alive before removing it + std::shared_ptr motor = *motorIt; + + // Remove the motor from the holder + motors_.erase(motorIt); // Detach the motor motor->detach(); - // Remove the motor from the holder - motors_.erase(motorIt); + // Skip reset and refresh if postponed + if (triggerReset) + { + // Early return if the model has been deleted already + try + { + (void)shared_from_this(); + } + catch (std::bad_weak_ptr & e) + { + return; + } - // Refresh the motors proxies - refreshMotorProxies(); + // Trigger extended model regeneration + reset(std::random_device{}); + + // Refresh the motors proxies + refreshMotorProxies(); + } + } + + void Robot::detachMotor(const std::string & motorName) + { + // Detach motor + detachMotor(motorName, true); } - void Robot::detachMotors(std::vector motorsNames) + void Robot::detachMotors(std::vector motorNames) { - if (motorsNames.empty()) + if (motorNames.empty()) { // Remove all sensors if none is specified if (!motorNames_.empty()) @@ -270,22 +300,25 @@ namespace jiminy else { // Make sure that no motor names are duplicates - if (checkDuplicates(motorsNames)) + if (checkDuplicates(motorNames)) { JIMINY_THROW(std::invalid_argument, "Duplicated motor names found."); } // Make sure that every motor name exist - if (!checkInclusion(motorNames_, motorsNames)) + if (!checkInclusion(motorNames_, motorNames)) { JIMINY_THROW(std::invalid_argument, "At least one of the motor names does not exist."); } - // Detach motors one-by-one - for (const std::string & name : motorsNames) + /* Detach motors one-by-one, and reset proxies at the very end only. + Beware the motor names must be stored locally because the list + of motors will be updated in-place. */ + const std::size_t nMotors = motorNames.size(); + for (std::size_t i = 0; i < nMotors; ++i) { - detachMotor(name); + detachMotor(motorNames[i], i == (nMotors - 1)); } } } @@ -571,6 +604,59 @@ namespace jiminy } } + void Robot::initializeExtendedModel() + { + // Call base implementation + Model::initializeExtendedModel(); + + // Add backlash joint for each motor + for (const auto & motor : motors_) + { + // Extract some proxies + const std::string & jointName = motor->getJointName(); + std::string backlashName = jointName; + backlashName += BACKLASH_JOINT_SUFFIX; + + // Check if constraint a joint bounds constraint exist + const bool hasConstraint = + constraints_.exist(backlashName, ConstraintNodeType::BOUNDS_JOINTS); + + // Skip adding joint if no backlash + const double backlash = motor->getBacklash(); + if (backlash < EPS) + { + // Remove joint bounds constraint if any + if (hasConstraint) + { + removeConstraint(backlashName, ConstraintNodeType::BOUNDS_JOINTS); + } + + continue; + } + + // Add joint bounds constraint if necessary + if (!hasConstraint) + { + std::shared_ptr constraint = + std::make_shared(backlashName); + addConstraint(backlashName, constraint, ConstraintNodeType::BOUNDS_JOINTS); + } + + // Add backlash joint to the model + addBacklashJointAfterMechanicalJoint(pinocchioModel_, jointName, backlashName); + backlashJointNames_.push_back(backlashName); + + // Update position limits in model + const Eigen::Index positionIndex = + getJointPositionFirstIndex(pinocchioModel_, backlashName); + pinocchioModel_.lowerPositionLimit[positionIndex] = -backlash / 2.0; + pinocchioModel_.upperPositionLimit[positionIndex] = backlash / 2.0; + } + + // Compute backlash joint indices + backlashJointIndices_ = getJointIndices(pinocchioModel_, backlashJointNames_); + } + std::shared_ptr Robot::getMotor(const std::string & motorName) { if (!isInitialized_) diff --git a/core/src/utilities/pinocchio.cc b/core/src/utilities/pinocchio.cc index f7cfaa26d..d94092794 100644 --- a/core/src/utilities/pinocchio.cc +++ b/core/src/utilities/pinocchio.cc @@ -461,16 +461,14 @@ namespace jiminy const std::string & childJointName, const std::string & newJointName) { - using namespace pinocchio; - - const pinocchio::JointIndex childJointIndex = getJointIndex(model, childJointName); - // Flexible joint is placed at the same position as the child joint, in its parent frame - const SE3 & jointPlacement = model.jointPlacements[childJointIndex]; + const pinocchio::JointIndex childJointIndex = getJointIndex(model, childJointName); + const pinocchio::SE3 & jointPlacement = model.jointPlacements[childJointIndex]; - // Create flexibility joint + // Add new joint before the original joint + pinocchio::JointModel newJointModel = pinocchio::JointModelSpherical(); const pinocchio::JointIndex newJointIndex = model.addJoint( - model.parents[childJointIndex], JointModelSpherical(), jointPlacement, newJointName); + model.parents[childJointIndex], newJointModel, jointPlacement, newJointName); // Set child joint to be a child of the new joint, at the origin model.parents[childJointIndex] = newJointIndex; @@ -485,24 +483,68 @@ namespace jiminy model.frames[childFrameIndex].previousFrame = newFrameIndex; model.frames[childFrameIndex].placement.setIdentity(); - // Update new joint subtree to include all the joints below it - for (std::size_t i = 0; i < model.subtrees[childJointIndex].size(); ++i) - { - model.subtrees[newJointIndex].push_back(model.subtrees[childJointIndex][i]); - } + // Update new joint subtree to append all the joints below it + const std::vector & childSubtree = model.subtrees[childJointIndex]; + std::vector & newSubtree = model.subtrees[newJointIndex]; + newSubtree.insert(newSubtree.end(), childSubtree.begin(), childSubtree.end()); // Add weightless body - model.appendBodyToJoint(newJointIndex, pinocchio::Inertia::Zero(), SE3::Identity()); + model.appendBodyToJoint( + newJointIndex, pinocchio::Inertia::Zero(), pinocchio::SE3::Identity()); - /* Pinocchio requires that joints are in increasing order as we move to the leaves of the - kinematic tree. Here this is no longer the case, as an intermediate joint was appended - at the end. We put the joint back in order by doing successive permutations. */ + /* Pinocchio expects joint indices to be sorted as we move from the root to the leaves of + the kinematic tree. This is no longer the case, as an intermediate joint was appended at + the end. Therefore, we put the joints back in order by doing successive permutations. */ for (pinocchio::JointIndex i = childJointIndex; i < newJointIndex; ++i) { swapJoints(model, i, newJointIndex); } } + void addBacklashJointAfterMechanicalJoint(pinocchio::Model & model, + const std::string & parentJointName, + const std::string & newJointName) + { + // Get parent joint model + const pinocchio::JointIndex parentJointIndex = getJointIndex(model, parentJointName); + const pinocchio::JointModel & parentJointModel = model.joints[parentJointIndex]; + + // Backlash are only supported for 1DoF joints (same as motors) + const JointModelType parentJointType = getJointType(parentJointModel); + if (parentJointType != JointModelType::LINEAR && + parentJointType != JointModelType::ROTARY && + parentJointType != JointModelType::ROTARY_UNBOUNDED) + { + JIMINY_THROW(std::logic_error, + "Backlash can only be associated with a 1-dof linear or rotary joint."); + } + + // Add new joint after the original joint + const pinocchio::JointIndex newJointIndex = model.addJoint( + parentJointIndex, parentJointModel, pinocchio::SE3::Identity(), newJointName); + + // Add new joint to frame list + const pinocchio::FrameIndex parentFrameIndex = getFrameIndex(model, parentJointName); + model.addJointFrame(newJointIndex, static_cast(parentFrameIndex)); + + // Update original joint subtree to include the new joint + std::vector & parentSubtree = model.subtrees[parentJointIndex]; + std::vector & newSubtree = model.subtrees[newJointIndex]; + newSubtree.insert(newSubtree.end(), parentSubtree.begin() + 1, parentSubtree.end()); + parentSubtree.insert(parentSubtree.begin() + 1, newJointIndex); + + // Move the inertia of the orginal joint to the new joint, which is now weightless + model.appendBodyToJoint( + newJointIndex, model.inertias[parentJointIndex], pinocchio::SE3::Identity()); + model.inertias[parentJointIndex].setZero(); + + // Putting the joints back in order + for (pinocchio::JointIndex i = parentJointIndex + 1; i < newJointIndex; ++i) + { + swapJoints(model, i, newJointIndex); + } + } + void addFlexibilityJointAtFixedFrame(pinocchio::Model & model, const std::string & frameName) { using namespace pinocchio; diff --git a/core/unit/engine_sanity_check.cc b/core/unit/engine_sanity_check.cc index e9c82725f..ff0cab20c 100755 --- a/core/unit/engine_sanity_check.cc +++ b/core/unit/engine_sanity_check.cc @@ -67,7 +67,6 @@ TEST(EngineSanity, EnergyConservation) // Disable velocity and position limits GenericConfig & modelOptions = boost::get(robotOptions.at("model")); GenericConfig & jointsOptions = boost::get(modelOptions.at("joints")); - boost::get(jointsOptions.at("enablePositionLimit")) = false; boost::get(jointsOptions.at("enableVelocityLimit")) = false; // Disable torque limits diff --git a/data/bipedal_robots/atlas/atlas_v4_options.toml b/data/bipedal_robots/atlas/atlas_v4_options.toml index f86fea921..8a5bfa81a 100644 --- a/data/bipedal_robots/atlas/atlas_v4_options.toml +++ b/data/bipedal_robots/atlas/atlas_v4_options.toml @@ -26,5 +26,4 @@ torsion = 0.0 # ======== Joints bounds configuration ======== [robot.model.joints] -enablePositionLimit = true enableVelocityLimit = true diff --git a/data/bipedal_robots/cassie/cassie_options.toml b/data/bipedal_robots/cassie/cassie_options.toml index 9932dc2f3..7ba101ac3 100644 --- a/data/bipedal_robots/cassie/cassie_options.toml +++ b/data/bipedal_robots/cassie/cassie_options.toml @@ -25,5 +25,4 @@ friction = 0.5 # ======== Joints bounds configuration ======== [robot.model.joints] -enablePositionLimit = true enableVelocityLimit = true diff --git a/data/bipedal_robots/digit/digit_options.toml b/data/bipedal_robots/digit/digit_options.toml index 9932dc2f3..7ba101ac3 100644 --- a/data/bipedal_robots/digit/digit_options.toml +++ b/data/bipedal_robots/digit/digit_options.toml @@ -25,5 +25,4 @@ friction = 0.5 # ======== Joints bounds configuration ======== [robot.model.joints] -enablePositionLimit = true enableVelocityLimit = true diff --git a/data/quadrupedal_robots/anymal/anymal_options.toml b/data/quadrupedal_robots/anymal/anymal_options.toml index 6269f5675..eeeeb6fc0 100644 --- a/data/quadrupedal_robots/anymal/anymal_options.toml +++ b/data/quadrupedal_robots/anymal/anymal_options.toml @@ -25,5 +25,4 @@ friction = 1.0 # ======== Joints bounds configuration ======== [robot.model.joints] -enablePositionLimit = true enableVelocityLimit = true diff --git a/data/toys_models/ant/ant_options.toml b/data/toys_models/ant/ant_options.toml index 0a3debfef..4ae169e62 100644 --- a/data/toys_models/ant/ant_options.toml +++ b/data/toys_models/ant/ant_options.toml @@ -23,5 +23,4 @@ friction = 1.0 # ======== Joints bounds configuration ======== [robot.model.joints] -enablePositionLimit = true enableVelocityLimit = false diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py b/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py index 62940aa54..9d49ac2a6 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py @@ -332,7 +332,6 @@ def _get_agent_state_space(self) -> spaces.Dict: """ # Define some proxies for convenience model_options = self.robot.get_model_options() - joint_position_indices = self.robot.mechanical_joint_position_indices joint_velocity_indices = self.robot.mechanical_joint_velocity_indices position_limit_upper = self.robot.position_limit_upper position_limit_lower = self.robot.position_limit_lower @@ -352,10 +351,6 @@ def _get_agent_state_space(self) -> spaces.Dict: velocity_limit[ joint_velocity_index + np.arange(3)] = FLEX_VEL_ANG_MAX - if not model_options['joints']['enablePositionLimit']: - position_limit_lower[joint_position_indices] = -JOINT_POS_MAX - position_limit_upper[joint_position_indices] = JOINT_POS_MAX - if not model_options['joints']['enableVelocityLimit']: velocity_limit[joint_velocity_indices] = JOINT_VEL_MAX diff --git a/python/gym_jiminy/unit_py/test_deformation_estimator.py b/python/gym_jiminy/unit_py/test_deformation_estimator.py index 858b959c6..924e89777 100644 --- a/python/gym_jiminy/unit_py/test_deformation_estimator.py +++ b/python/gym_jiminy/unit_py/test_deformation_estimator.py @@ -92,7 +92,6 @@ def test_arm(self): 'inertia': np.array([1.0, 1.0, 0.0]) } for i in range(1, 5) ] - model_options['joints']['enablePositionLimit'] = False model_options['joints']['enableVelocityLimit'] = False robot.set_model_options(model_options) diff --git a/python/jiminy_py/unit_py/data/simple_pendulum.urdf b/python/jiminy_py/unit_py/data/simple_pendulum.urdf index 948a4771f..865854a53 100644 --- a/python/jiminy_py/unit_py/data/simple_pendulum.urdf +++ b/python/jiminy_py/unit_py/data/simple_pendulum.urdf @@ -34,5 +34,14 @@ + + + + + + + + + diff --git a/python/jiminy_py/unit_py/test_dense_pole.py b/python/jiminy_py/unit_py/test_dense_pole.py index 692ac57fd..0a32d8106 100644 --- a/python/jiminy_py/unit_py/test_dense_pole.py +++ b/python/jiminy_py/unit_py/test_dense_pole.py @@ -41,7 +41,6 @@ def setUp(self): model_options['joints']['positionLimitMin'] = [-self.joint_limit] model_options['joints']['positionLimitMax'] = [self.joint_limit] model_options['joints']['positionLimitFromUrdf'] = False - model_options['joints']['enablePositionLimit'] = True model_options['joints']['enableVelocityLimit'] = False self.robot.set_model_options(model_options) diff --git a/python/jiminy_py/unit_py/test_simple_pendulum.py b/python/jiminy_py/unit_py/test_simple_pendulum.py index ad076a306..d74e3b868 100644 --- a/python/jiminy_py/unit_py/test_simple_pendulum.py +++ b/python/jiminy_py/unit_py/test_simple_pendulum.py @@ -166,6 +166,99 @@ def dynamics(t, x): # using Scipy self.assertTrue(np.allclose(x_jiminy, x_rk_python, atol=TOLERANCE)) + def test_backlash(self): + """Test adding a backlash to a pendulum, and make sure that: + - while 'inside' the backlash, the pendulum behaves like no motor is + present + - once the backlash limit is reached, the pendulum behaves like a + single body, with rotor and body inertia summed up. + """ + # Configure the robot: add rotor inertia and backlash + J = 1.0 + BACKLASH = 1.1 + robot_options = self.robot.get_options() + robot_options["motors"]["PendulumJoint"]['enableArmature'] = True + robot_options["motors"]["PendulumJoint"]['armature'] = J + robot_options["motors"]["PendulumJoint"]["enableBacklash"] = True + robot_options["motors"]["PendulumJoint"]["backlash"] = 2 * BACKLASH + self.robot.set_options(robot_options) + + TAU = 5.0 + def ControllerConstant(t, q, v, sensor_measurements, command): + command[:] = - TAU + + engine = jiminy.Engine() + setup_controller_and_engine( + engine, self.robot, compute_command=ControllerConstant) + + engine_options = engine.get_options() + engine_options["constraints"]["regularization"] = 0.0 + engine.set_options(engine_options) + + # Run simulation and extract log data + x0 = np.array([0.0, 0.1, 0.0, 0.0]) + tf = 5.0 + time, x_jiminy = simulate_and_get_state_evolution( + engine, tf, x0, split=False) + + # Now we compare the two phases of the motion: + # - first phase: before impact: both bodies move independently + # - second phase: after impact: both bodies move as one (a tolerance + # of 0.4s is applied to make sure all rebounds are gone) + t_impact = np.sqrt(BACKLASH / (TAU / J) * 2) + t1, t2 = np.searchsorted(time, [t_impact - 0.02, t_impact + 0.4]) + time_phase_1 = time[:t1] + x_jiminy_phase_1 = x_jiminy[:t1] + def dynamics(t, x): + # The angle of the pendulum mass is actually x[0] + x[1], not x[1] + return np.array([x[2], + x[3], + -TAU / J, + self.g / self.l * np.sin(x[0] + x[1]) + TAU / J]) + x_rk_python = integrate_dynamics(time_phase_1, x0, dynamics) + self.assertTrue(np.allclose( + x_jiminy_phase_1, x_rk_python, atol=TOLERANCE)) + + time_phase_2 = time[t2:] - time[t2] + x_jiminy_phase_2 = x_jiminy[t2:] + + I_total = self.m * self.l ** 2 + J + G = self.m * self.g * self.l / I_total + def dynamics(t, x): + acc = G * np.sin(x[0] + x[1]) - TAU / I_total + return np.array([x[2], x[3], acc, 0.0]) + x_rk_python = integrate_dynamics( + time_phase_2, x_jiminy_phase_2[0], dynamics) + self.assertTrue(np.allclose( + x_jiminy_phase_2, x_rk_python, atol=TOLERANCE)) + + # Simulate with more damping on the constraint to prevent bouncing. + # Note that this needs to be changed after the simulation start. + engine_options["stepper"]["dtMax"] = 0.001 + engine.set_options(engine_options) + engine.start(x0[:self.robot.nq], x0[-self.robot.nv:]) + self.robot.constraints.bounds_joints["PendulumJointBacklash"].kp = 1e5 + self.robot.constraints.bounds_joints["PendulumJointBacklash"].kd = 1e3 + engine.step(t_impact + 1.0) + log_vars = engine.log_data["variables"] + time = log_vars['Global.Time'] + q_jiminy = np.stack([ + log_vars[field] for field in self.robot.log_position_fieldnames + ], axis=-1) + v_jiminy = np.stack([ + log_vars[field] for field in self.robot.log_velocity_fieldnames + ], axis=-1) + x_jiminy = np.concatenate((q_jiminy, v_jiminy), axis=-1) + + # Ignoring some uncertainty at impact time due to the pendulum motion + t2 = np.searchsorted(time, t_impact + 0.20) + time_phase_2 = time[t2:] - time[t2] + x_jiminy_phase_2 = x_jiminy[t2:] + x_rk_python = integrate_dynamics( + time_phase_2, x_jiminy_phase_2[0], dynamics) + self.assertTrue(np.allclose( + x_jiminy_phase_2, x_rk_python, atol=TOLERANCE)) + def test_imu_sensor(self): """Test IMU sensor on pendulum motion. @@ -500,12 +593,12 @@ def test_flexibility_armature(self): # Create an engine: PD controller on motor and no internal dynamics k_control, nu_control = 100.0, 1.0 - def ControllerPD(t, q, v, sensor_measurements, command): + def compute_command(t, q, v, sensor_measurements, command): command[:] = - k_control * q[4] - nu_control * v[3] engine = jiminy.Engine() setup_controller_and_engine( - engine, self.robot, compute_command=ControllerPD) + engine, self.robot, compute_command=compute_command) # Configure the engine engine_options = engine.get_options() @@ -622,7 +715,7 @@ def spring_force(t, q, v, sensor_measurements, u_custom): def test_flexibility_api(self): """ @brief Test the addition and disabling of a flexibility in the system. - + @details This function only tests that the flexibility API works, but performs no validation of the physics behind it. """ diff --git a/python/jiminy_py/unit_py/utilities.py b/python/jiminy_py/unit_py/utilities.py index 08777686a..e81d8dca1 100644 --- a/python/jiminy_py/unit_py/utilities.py +++ b/python/jiminy_py/unit_py/utilities.py @@ -50,7 +50,6 @@ def load_urdf_default(urdf_name: str, # Configure the robot robot_options = robot.get_options() - robot_options["model"]["joints"]["enablePositionLimit"] = False robot_options["model"]["joints"]["enableVelocityLimit"] = False for name in robot.motor_names: robot_options["motors"][name]['enableCommandLimit'] = False diff --git a/python/jiminy_pywrap/src/motors.cc b/python/jiminy_pywrap/src/motors.cc index dd323eda9..1c9cacf66 100644 --- a/python/jiminy_pywrap/src/motors.cc +++ b/python/jiminy_pywrap/src/motors.cc @@ -56,6 +56,9 @@ namespace jiminy::python .ADD_PROPERTY_GET_WITH_POLICY("armature", &AbstractMotorBase::getArmature, bp::return_value_policy()) + .ADD_PROPERTY_GET_WITH_POLICY("backlash", + &AbstractMotorBase::getBacklash, + bp::return_value_policy()) .def("set_options", &internal::abstract_motor::setOptions) .def("get_options", diff --git a/python/jiminy_pywrap/src/robot.cc b/python/jiminy_pywrap/src/robot.cc index e55933894..ba9dba8d5 100644 --- a/python/jiminy_pywrap/src/robot.cc +++ b/python/jiminy_pywrap/src/robot.cc @@ -316,6 +316,12 @@ namespace jiminy::python .ADD_PROPERTY_GET_WITH_POLICY("flexibility_joint_indices", &Model::getFlexibilityJointIndices, bp::return_value_policy>()) + .ADD_PROPERTY_GET_WITH_POLICY("backlash_joint_names", + &Model::getBacklashJointNames, + bp::return_value_policy>()) + .ADD_PROPERTY_GET_WITH_POLICY("backlash_joint_indices", + &Model::getBacklashJointIndices, + bp::return_value_policy>()) .ADD_PROPERTY_GET_WITH_POLICY("position_limit_lower", &Model::getPositionLimitMin, @@ -404,7 +410,9 @@ namespace jiminy::python static_cast (Robot::*)(const std::string &)>( &Robot::getMotor), (bp::arg("self"), "motor_name")) - .def("detach_motor", &Robot::detachMotor, (bp::arg("self"), "joint_name")) + .def("detach_motor", + static_cast(&Robot::detachMotor), + (bp::arg("self"), "joint_name")) .def("detach_motors", &internal::robot::detachMotors, (bp::arg("self"), bp::arg("joints_names") = bp::list()))