diff --git a/core/include/jiminy/core/robot/AbstractMotor.h b/core/include/jiminy/core/robot/AbstractMotor.h index 1310ff025..17bb535e9 100644 --- a/core/include/jiminy/core/robot/AbstractMotor.h +++ b/core/include/jiminy/core/robot/AbstractMotor.h @@ -33,7 +33,10 @@ namespace jiminy struct MotorSharedDataHolder_t { MotorSharedDataHolder_t(void) : - data_(), + position_(), + velocity_(), + acceleration_(), + effort_(), motors_(), num_(0) { @@ -42,7 +45,11 @@ namespace jiminy ~MotorSharedDataHolder_t(void) = default; - vectorN_t data_; ///< Buffer with current actual motor effort + vectorN_t position_; + vectorN_t velocity_; + vectorN_t acceleration_; + vectorN_t effort_; + std::vector motors_; ///< Vector of pointers to the motors. int32_t num_; ///< Number of motors }; @@ -59,7 +66,6 @@ namespace jiminy virtual configHolder_t getDefaultMotorOptions(void) { configHolder_t config; - config["mechanicalReduction"] = 1.0; config["enableCommandLimit"] = true; config["commandLimitFromUrdf"] = true; config["commandLimit"] = 0.0; @@ -71,7 +77,6 @@ namespace jiminy struct abstractMotorOptions_t { - float64_t const mechanicalReduction; ///< Mechanical reduction ratio of the transmission (joint / motor, usually >= 1.0 bool_t const enableCommandLimit; bool_t const commandLimitFromUrdf; float64_t const commandLimit; @@ -79,7 +84,6 @@ namespace jiminy float64_t const armature; abstractMotorOptions_t(configHolder_t const & options) : - mechanicalReduction(boost::get(options.at("mechanicalReduction"))), enableCommandLimit(boost::get(options.at("enableCommandLimit"))), commandLimitFromUrdf(boost::get(options.at("commandLimitFromUrdf"))), commandLimit(boost::get(options.at("commandLimit"))), @@ -131,16 +135,6 @@ namespace jiminy /////////////////////////////////////////////////////////////////////////////////////////////// configHolder_t getOptions(void) const; - /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get the actual effort of the motor at the current time. - /////////////////////////////////////////////////////////////////////////////////////////////// - float64_t const & get(void) const; - - /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get the actual effort of all the motors at the current time. - /////////////////////////////////////////////////////////////////////////////////////////////// - vectorN_t const & getAll(void) const; - /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Set the configuration options of the motor. /// @@ -156,60 +150,59 @@ namespace jiminy hresult_t setOptionsAll(configHolder_t const & motorOptions); /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get isInitialized_. + /// \brief Get motorIdx_. /// - /// \details It is a flag used to determine if the motor has been initialized. + /// \details It is the index of the motor. + /// \brief Get the actual acc of the motor at the current time. /////////////////////////////////////////////////////////////////////////////////////////////// - bool_t const & getIsInitialized(void) const; + int32_t const & getIdx(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get name_. - /// - /// \details It is the name of the motor. + /// \brief Get the actual position of the motor at the current time. /////////////////////////////////////////////////////////////////////////////////////////////// - std::string const & getName(void) const; + float64_t const & getPosition(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get motorIdx_. - /// - /// \details It is the index of the motor. + /// \brief Get the actual velocity of the motor at the current time. /////////////////////////////////////////////////////////////////////////////////////////////// - int32_t const & getIdx(void) const; + float64_t const & getVelocity(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get jointName_. - /// - /// \details It is the name of the joint associated with the motor. + /// \brief Get the actual acc of the motor at the current time. /////////////////////////////////////////////////////////////////////////////////////////////// - std::string const & getJointName(void) const; + float64_t const & getAcceleration(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get jointModelIdx_. - /// - /// \details It is the index of the joint associated with the motor in the kinematic tree. + /// \brief Get the actual effort of the motor at the current time. /////////////////////////////////////////////////////////////////////////////////////////////// - jointIndex_t const & getJointModelIdx(void) const; + float64_t const & getEffort(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get jointType_. + /// \brief Get name_. /// - /// \details It is the type of joint associated with the motor. + /// \details It is the name of the motor. /////////////////////////////////////////////////////////////////////////////////////////////// - joint_t const & getJointType(void) const; + std::string const & getName(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get jointPositionIdx_. - /// - /// \details It is the index of the joint associated with the motor in the configuration vector. + /// \brief Get the actual position of all the motor at the current time. /////////////////////////////////////////////////////////////////////////////////////////////// - int32_t const & getJointPositionIdx(void) const; + vectorN_t const & getPositionAll(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get jointVelocityIdx_. - /// - /// \details It is the index of the joint associated with the motor in the velocity vector. + /// \brief Get the actual velocity of all the motor at the current time. /////////////////////////////////////////////////////////////////////////////////////////////// - int32_t const & getJointVelocityIdx(void) const; + vectorN_t const & getVelocityAll(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get the actual acc of all the motor at the current time. + /////////////////////////////////////////////////////////////////////////////////////////////// + vectorN_t const & getAccelerationAll(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get the actual effort of all the motor at the current time. + /////////////////////////////////////////////////////////////////////////////////////////////// + vectorN_t const & getEffortAll(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get commandLimit_. @@ -231,18 +224,10 @@ namespace jiminy /// \details It assumes that the internal state of the robot is consistent with the /// input arguments. /// - /// \param[in] t Current time. - /// \param[in] q Current configuration of the motor. - /// \param[in] v Current velocity of the motor. - /// \param[in] a Current acceleration of the motor. /// \param[in] command Current command effort of the motor. /// /////////////////////////////////////////////////////////////////////////////////////////////// - virtual hresult_t computeEffort(float64_t const & t, - Eigen::VectorBlock const & q, - float64_t const & v, - float64_t const & a, - float64_t command) = 0; /* copy on purpose */ + virtual hresult_t computeEffort(float64_t command) = 0; /* copy on purpose */ /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Request every motors to update their actual effort based of the input data. @@ -253,39 +238,49 @@ namespace jiminy /// \remark This method is not intended to be called manually. The Robot to which the /// motor is added is taking care of it while updating the state of the motors. /// - /// \param[in] t Current time. - /// \param[in] q Current configuration vector of the robot. - /// \param[in] v Current velocity vector of the robot. - /// \param[in] a Current acceleration vector of the robot. /// \param[in] command Current command effort vector of the robot. /// /// \return Return code to determine whether the execution of the method was successful. /////////////////////////////////////////////////////////////////////////////////////////////// - hresult_t computeEffortAll(float64_t const & t, - vectorN_t const & q, - vectorN_t const & v, - vectorN_t const & a, - vectorN_t const & command); + hresult_t computeEffortAll(vectorN_t const & command); protected: + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get a reference to the last data buffer corresponding to the actual position + /// of the motor. + /////////////////////////////////////////////////////////////////////////////////////////////// + float64_t & q(void); + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get a reference to the last data buffer corresponding to the actual velocity + /// of the motor. + /////////////////////////////////////////////////////////////////////////////////////////////// + float64_t & v(void); + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get a reference to the last data buffer corresponding to the actual acceleration + /// of the motor. + /////////////////////////////////////////////////////////////////////////////////////////////// + float64_t & a(void); + /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get a reference to the last data buffer corresponding to the actual effort /// of the motor. /////////////////////////////////////////////////////////////////////////////////////////////// - float64_t & data(void); + float64_t & u(void); private: /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Attach the sensor to a robot + /// \brief Attach the motor to a robot /// - /// \details This method must be called before initializing the sensor. + /// \details This method must be called before initializing the motor. /////////////////////////////////////////////////////////////////////////////////////////////// hresult_t attach(std::weak_ptr robot, std::function notifyRobot, MotorSharedDataHolder_t * sharedHolder); /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Detach the sensor from the robot + /// \brief Detach the motor from the robot /////////////////////////////////////////////////////////////////////////////////////////////// hresult_t detach(void); @@ -297,14 +292,9 @@ namespace jiminy bool_t isInitialized_; ///< Flag to determine whether the controller has been initialized or not bool_t isAttached_; ///< Flag to determine whether the motor is attached to a robot std::weak_ptr robot_; ///< Robot for which the command and internal dynamics - std::function notifyRobot_; ///< Notify the robot that the configuration of the sensors have changed + std::function notifyRobot_; ///< Notify the robot that the configuration of the motors have changed std::string name_; ///< Name of the motor int32_t motorIdx_; ///< Index of the motor in the measurement buffer - std::string jointName_; - jointIndex_t jointModelIdx_; - joint_t jointType_; - int32_t jointPositionIdx_; - int32_t jointVelocityIdx_; float64_t commandLimit_; float64_t armature_; diff --git a/core/include/jiminy/core/robot/AbstractTransmission.h b/core/include/jiminy/core/robot/AbstractTransmission.h new file mode 100644 index 000000000..00f328566 --- /dev/null +++ b/core/include/jiminy/core/robot/AbstractTransmission.h @@ -0,0 +1,297 @@ +/////////////////////////////////////////////////////////////////////////////////////////////// +/// +/// \brief Generic interface for any transmission. +/// +/// \details Any transmission must inherit from this base class and implement its virtual methods. +/// +/// \remark Each transmission added to a Jiminy Robot is downcasted as an instance of +/// AbstractTransmissionBase and polymorphism is used to call the actual implementations. +/// +/////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef JIMINY_ABSTRACT_TRANSMISSION_H +#define JIMINY_ABSTRACT_TRANSMISSION_H + +#include + +#include "jiminy/core/Macros.h" +#include "jiminy/core/Types.h" + + +namespace jiminy +{ + class Robot; + + class AbstractTransmissionBase : public std::enable_shared_from_this + { + /* AKA AbstractSensorBase */ + friend Robot; + + public: + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Dictionary gathering the configuration options shared between transmissions + /////////////////////////////////////////////////////////////////////////////////////////////// + virtual configHolder_t getDefaultTransmissionOptions(void) + { + configHolder_t config; + config["mechanicalReduction"] = 0.0; + return config; + }; + + struct abstractTransmissionOptions_t + { + float64_t const mechanicalReduction; + + abstractTransmissionOptions_t(configHolder_t const & options) : + mechanicalReduction(boost::get(options.at("mechanicalReduction"))) + { + // Empty. + } + }; + + public: + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Forbid the copy of the class + /////////////////////////////////////////////////////////////////////////////////////////////// + AbstractTransmissionBase(AbstractTransmissionBase const & abstractTransmission) = delete; + AbstractTransmissionBase & operator = (AbstractTransmissionBase const & other) = delete; + + auto shared_from_this() { return shared_from(this); } + auto shared_from_this() const { return shared_from(this); } + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + /// + /// \param[in] robot Robot + /// \param[in] name Name of the transmission + /////////////////////////////////////////////////////////////////////////////////////////////// + AbstractTransmissionBase(std::string const & name); + virtual ~AbstractTransmissionBase(void); + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Initialize + /// + /// \remark Initialize the transmission with the names of connected motors and actuated joits. + /////////////////////////////////////////////////////////////////////////////////////////////// + virtual hresult_t initialize(std::vector const & jointName, + std::vector const & motorName); + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Refresh the proxies. + /// + /// \remark This method is not intended to be called manually. The Robot to which the + /// transmission is added is taking care of it when its own `refresh` method is called. + /////////////////////////////////////////////////////////////////////////////////////////////// + virtual hresult_t refreshProxies(void); + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Reset the internal state of the transmissions. + /// + /// \details This method resets the internal state of the transmission. + /// + /// \remark This method is not intended to be called manually. The Robot to which the + /// transmission is added is taking care of it when its own `reset` method is called. + /////////////////////////////////////////////////////////////////////////////////////////////// + virtual hresult_t resetAll(void); + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get the configuration options of the transmission. + /// + /// \return Dictionary with the parameters of the transmission + /////////////////////////////////////////////////////////////////////////////////////////////// + configHolder_t getOptions(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Set the configuration options of the transmission. + /// + /// \param[in] transmissionOptions Dictionary with the parameters of the transmission + /////////////////////////////////////////////////////////////////////////////////////////////// + virtual hresult_t setOptions(configHolder_t const & transmissionOptions); + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Set the same configuration options for every transmissions. + /// + /// \param[in] transmissionOptions Dictionary with the parameters used for any transmission + /////////////////////////////////////////////////////////////////////////////////////////////// + hresult_t setOptionsAll(configHolder_t const & transmissionOptions); + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get isInitialized_. + /// + /// \details It is a flag used to determine if the transmission has been initialized. + /////////////////////////////////////////////////////////////////////////////////////////////// + bool_t const & getIsInitialized(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get name_. + /// + /// \details It is the name of the transmission. + /////////////////////////////////////////////////////////////////////////////////////////////// + std::string const & getName(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get transmissionIdx_. + /// + /// \details It is the index of the transmission. + /////////////////////////////////////////////////////////////////////////////////////////////// + int32_t const & getIdx(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get jointName_. + /// + /// \details It is the name of the joints associated with the transmission. + /////////////////////////////////////////////////////////////////////////////////////////////// + std::vector const & getJointNames(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get jointModelIdx_. + /// + /// \details It is the index of the joints associated with the transmission in the kinematic tree. + /////////////////////////////////////////////////////////////////////////////////////////////// + std::vectorconst & getJointModelIndices(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get jointType_. + /// + /// \details It is the type of joints associated with the transmission. + /////////////////////////////////////////////////////////////////////////////////////////////// + std::vector const & getJointTypes(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get jointPositionIdx_. + /// + /// \details It is the index of the joints associated with the transmission in the configuration vector. + /////////////////////////////////////////////////////////////////////////////////////////////// + vectorN_t const & getJointPositionIndices(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get jointVelocityIdx_. + /// + /// \details It is the index of the joints associated with the transmission in the velocity vector. + /////////////////////////////////////////////////////////////////////////////////////////////// + vectorN_t const & getJointVelocityIndices(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get motorNames_. + /// + /// \details It is the name of the motors associated with the transmission. + /////////////////////////////////////////////////////////////////////////////////////////////// + std::vector const & getMotorNames(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get getMotorIndices. + /// + /// \details It is the name of the motors associated with the transmission. + /////////////////////////////////////////////////////////////////////////////////////////////// + std::vector const & getMotorIndices(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get forwardTransform_. + /// + /// \details It is the transformation matrix to convert quantities from motor to joint-level. + /////////////////////////////////////////////////////////////////////////////////////////////// + matrixN_t const & getForwardTransform(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get backwardTransform_. + /// + /// \details It is the transformation matrix to convert quantities from joint to motor-level. + /////////////////////////////////////////////////////////////////////////////////////////////// + matrixN_t const & getInverseTransform(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Compute forward transmission. + /// + /// \details Compute forward transmission from motor to joint. + /////////////////////////////////////////////////////////////////////////////////////////////// + hresult_t computeForward(float64_t const & t, + vectorN_t & q, + vectorN_t & v, + vectorN_t & a, + vectorN_t & uJoint); + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Compute backward transmission. + /// + /// \details Compute backward transmission from joint to motor. + /////////////////////////////////////////////////////////////////////////////////////////////// + hresult_t computeBackward(float64_t const & t, + vectorN_t const & q, + vectorN_t const & v, + vectorN_t const & a, + vectorN_t const & uJoint); + + protected: + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Request the transmission to update its actual state based of the input data. + /// + /// \details It assumes that the internal state of the robot is consistent with the + /// input arguments. This transformation is used to compute the forward transformation + /// of the transmission transforming motor position, velocity and torque to joint + /// position, velocity and torque. + /// + /// \param[in] q Current configuration of the motors. + /// \param[in] v Current velocity of the motors. + /// \param[out] out Matrix transforming quantities from motor to joint level + /////////////////////////////////////////////////////////////////////////////////////////////// + virtual void computeTransform(Eigen::VectorBlock const & q, + Eigen::VectorBlock const & v, + matrixN_t & out) = 0; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Request the transmission to update its actual state based of the input data. + /// + /// \details It assumes that the internal state of the robot is consistent with the + /// input arguments. + /// + /// \param[in] q Current configuration of the motors. + /// \param[in] v Current velocity of the motors. + /// \param[out] out Matrix transforming quantities from joint to motor level + /////////////////////////////////////////////////////////////////////////////////////////////// + virtual void computeInverseTransform(Eigen::VectorBlock const & q, + Eigen::VectorBlock const & v, + matrixN_t & out) = 0; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Compute energy dissipation in the transmission. + /// + /////////////////////////////////////////////////////////////////////////////////////////////// + virtual void computeEffortTransmission(void) = 0; + + private: + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Attach the transmission to a robot + /// + /// \details This method must be called before initializing the transmission. + /////////////////////////////////////////////////////////////////////////////////////////////// + hresult_t attach(std::weak_ptr robot); + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Detach the transmission from the robot + /////////////////////////////////////////////////////////////////////////////////////////////// + hresult_t detach(void); + + public: + std::unique_ptr baseTransmissionOptions_; ///< Structure with the parameters of the transmission + + protected: + configHolder_t transmissionOptionsHolder_; ///< Dictionary with the parameters of the transmission + bool_t isInitialized_; ///< Flag to determine whether the transmission has been initialized or not + bool_t isAttached_; ///< Flag to determine whether the transmission is attached to a robot + std::weak_ptr robot_; ///< Robot for which the command and internal dynamics + std::string name_; ///< Name of the transmission + int32_t transmissionIdx_; ///< Index of the transmission in the transmission buffer + std::vector jointNames_; + std::vector jointModelIndices_; + std::vector jointTypes_; + vectorN_t jointPositionIndices_; + vectorN_t jointVelocityIndices_; + std::vector motorNames_; + std::vector motorIndices_; + std::vector > motors_; + matrixN_t forwardTransform_; + matrixN_t backwardTransform_; + }; +} + +#endif //end of JIMINY_ABSTRACT_TRANSMISSION_H diff --git a/core/include/jiminy/core/robot/BasicMotors.h b/core/include/jiminy/core/robot/BasicMotors.h index 13fca9c02..cff00b28a 100644 --- a/core/include/jiminy/core/robot/BasicMotors.h +++ b/core/include/jiminy/core/robot/BasicMotors.h @@ -56,16 +56,11 @@ namespace jiminy auto shared_from_this() { return shared_from(this); } auto shared_from_this() const { return shared_from(this); } - hresult_t initialize(std::string const & jointName); - + hresult_t initialize(void); virtual hresult_t setOptions(configHolder_t const & motorOptions) final override; private: - virtual hresult_t computeEffort(float64_t const & t, - Eigen::VectorBlock const & q, - float64_t const & v, - float64_t const & a, - float64_t command) final override; + virtual hresult_t computeEffort(float64_t command) final override; /* copy on purpose */ private: std::unique_ptr motorOptions_; diff --git a/core/include/jiminy/core/robot/BasicTransmissions.h b/core/include/jiminy/core/robot/BasicTransmissions.h new file mode 100644 index 000000000..cddf441ed --- /dev/null +++ b/core/include/jiminy/core/robot/BasicTransmissions.h @@ -0,0 +1,62 @@ +#ifndef JIMINY_BASIC_TRANSMISSIONS_H +#define JIMINY_BASIC_TRANSMISSIONS_H + +#include "jiminy/core/robot/AbstractTransmission.h" + + +namespace jiminy +{ + class SimpleTransmission : public AbstractTransmissionBase + { + public: + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Dictionary gathering the configuration options shared between transmissions + /////////////////////////////////////////////////////////////////////////////////////////////// + virtual configHolder_t getDefaultTransmissionOptions(void) override + { + // Add extra options or update default values + configHolder_t config = AbstractTransmissionBase::getDefaultTransmissionOptions(); + + config["mechanicalReduction"] = 0.0; + + return config; + }; + + struct transmissionOptions_t : public abstractTransmissionOptions_t + { + float64_t const mechanicalReduction; ///< Gear reduction ratio motor to joint + + transmissionOptions_t(configHolder_t const & options) : + abstractTransmissionOptions_t(options), + mechanicalReduction(boost::get(options.at("mechanicalReduction"))) + { + // Empty. + } + }; + + public: + SimpleTransmission(std::string const & name); + virtual ~SimpleTransmission(void) = default; + + auto shared_from_this() { return shared_from(this); } + auto shared_from_this() const { return shared_from(this); } + + virtual hresult_t setOptions(configHolder_t const & transmissionOptions) final override; + + private: + virtual void computeTransform(Eigen::VectorBlock const & q, + Eigen::VectorBlock const & v, + matrixN_t & out) final override; + + virtual void computeInverseTransform(Eigen::VectorBlock const & q, + Eigen::VectorBlock const & v, + matrixN_t & out) final override; + + virtual void computeEffortTransmission(void); + + private: + std::unique_ptr transmissionOptions_; + }; +} + +#endif //end of JIMINY_BASIC_TRANSMISSIONS_H \ No newline at end of file diff --git a/core/include/jiminy/core/robot/Model.h b/core/include/jiminy/core/robot/Model.h index e3189edfe..13bd47341 100644 --- a/core/include/jiminy/core/robot/Model.h +++ b/core/include/jiminy/core/robot/Model.h @@ -16,6 +16,7 @@ namespace jiminy class AbstractConstraintBase; class FixedFrameConstraint; class JointConstraint; + class TransmisionConstraint; template using constraintsMapTpl_t = static_map_t >; @@ -26,14 +27,16 @@ namespace jiminy BOUNDS_JOINTS = 0, CONTACT_FRAMES = 1, COLLISION_BODIES = 2, - USER = 3 + USER = 3, + TRANSMISSIONS = 4 }; - std::array const constraintsHolderTypeRange = {{ + std::array const constraintsHolderTypeRange = {{ constraintsHolderType_t::BOUNDS_JOINTS, constraintsHolderType_t::CONTACT_FRAMES, constraintsHolderType_t::COLLISION_BODIES, - constraintsHolderType_t::USER + constraintsHolderType_t::USER, + constraintsHolderType_t::TRANSMISSIONS }}; struct constraintsHolder_t @@ -91,6 +94,9 @@ namespace jiminy case constraintsHolderType_t::CONTACT_FRAMES: constraintsMapPtr = &contactFrames; break; + case constraintsHolderType_t::TRANSMISSIONS: + constraintsMapPtr = &transmissionConstraints; + break; case constraintsHolderType_t::USER: case constraintsHolderType_t::COLLISION_BODIES: default: @@ -115,6 +121,7 @@ namespace jiminy public: constraintsMap_t boundJoints; ///< Store internal constraints related to joint bounds constraintsMap_t contactFrames; ///< Store internal constraints related to contact frames + constraintsMap_t transmissionConstraints; ///< Store internal constraints related to the transmissions std::vector collisionBodies; ///< Store internal constraints related to collision bounds constraintsMap_t registered; ///< Store internal constraints registered by user }; diff --git a/core/include/jiminy/core/robot/Robot.h b/core/include/jiminy/core/robot/Robot.h index cd579e155..fde1c92c4 100644 --- a/core/include/jiminy/core/robot/Robot.h +++ b/core/include/jiminy/core/robot/Robot.h @@ -9,6 +9,7 @@ namespace jiminy { struct MotorSharedDataHolder_t; class AbstractMotorBase; + class AbstractTransmissionBase; struct SensorSharedDataHolder_t; class AbstractSensorBase; class TelemetryData; @@ -22,6 +23,7 @@ namespace jiminy using sensorsHolder_t = std::vector >; using sensorsGroupHolder_t = std::unordered_map; using sensorsSharedHolder_t = std::unordered_map >; + using transmissionsHolder_t = std::vector >; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -53,6 +55,8 @@ namespace jiminy motorsHolder_t const & getMotors(void) const; hresult_t detachMotor(std::string const & motorName); hresult_t detachMotors(std::vector const & motorsNames = {}); + hresult_t detachTransmission(std::string const & transmissionName); + hresult_t detachTransmissions(std::vector const & transmissionsNames = {}); hresult_t attachSensor(std::shared_ptr sensor); hresult_t getSensor(std::string const & sensorType, std::string const & sensorName, @@ -64,6 +68,10 @@ namespace jiminy hresult_t detachSensor(std::string const & sensorType, std::string const & sensorName); hresult_t detachSensors(std::string const & sensorType = {}); + hresult_t getTransmission(std::string const & transmissionName, + std::shared_ptr & transmission); + transmissionsHolder_t const & getTransmissions(void) const; + hresult_t attachTransmission(std::shared_ptr transmission); void computeMotorsEfforts(float64_t const & t, vectorN_t const & q, @@ -72,6 +80,7 @@ namespace jiminy vectorN_t const & command); vectorN_t const & getMotorsEfforts(void) const; float64_t const & getMotorEffort(std::string const & motorName) const; + vectorN_t const & getMotorsEffortLimits(void) const; void setSensorsData(float64_t const & t, vectorN_t const & q, vectorN_t const & v, @@ -123,6 +132,7 @@ namespace jiminy std::vector getMotorsVelocityIdx(void) const; std::unordered_map > const & getSensorsNames(void) const; std::vector const & getSensorsNames(std::string const & sensorType) const; + std::vector const & getActuatedJointNames(void) const; vectorN_t const & getCommandLimit(void) const; vectorN_t const & getArmatures(void) const; @@ -139,19 +149,23 @@ namespace jiminy protected: hresult_t refreshMotorsProxies(void); hresult_t refreshSensorsProxies(void); + hresult_t refreshTransmissionProxies(void); virtual hresult_t refreshProxies(void) override; protected: bool_t isTelemetryConfigured_; std::shared_ptr telemetryData_; motorsHolder_t motorsHolder_; + transmissionsHolder_t transmissionsHolder_; sensorsGroupHolder_t sensorsGroupHolder_; std::unordered_map sensorTelemetryOptions_; std::vector motorsNames_; ///< Name of the motors std::unordered_map > sensorsNames_; ///< Name of the sensors + std::vector transmissionsNames_; ///< Name of the motors std::vector commandFieldnames_; ///< Fieldnames of the command std::vector motorEffortFieldnames_; ///< Fieldnames of the motors effort uint64_t nmotors_; ///< The number of motors + std::vector actuatedJointNames_; ///< List of joints attached to a transmission private: std::unique_ptr mutexLocal_; diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc index c6a660c72..729ec4170 100644 --- a/core/src/engine/EngineMultiRobot.cc +++ b/core/src/engine/EngineMultiRobot.cc @@ -1143,14 +1143,15 @@ namespace jiminy } } - for (auto const & motor : system.robot->getMotors()) - { - if (!motor->getIsInitialized()) - { - PRINT_ERROR("At least a motor of a robot is not initialized."); - return hresult_t::ERROR_INIT_FAILED; - } - } + // TODO check with transmission + // for (auto const & motor : system.robot->getMotors()) + // { + // if (!motor->getIsInitialized()) + // { + // PRINT_ERROR("At least a motor of a robot is not initialized."); + // return hresult_t::ERROR_INIT_FAILED; + // } + // } } auto systemIt = systems_.begin(); @@ -1412,12 +1413,14 @@ namespace jiminy // Compute the total effort vector u = uInternal + uCustom; - for (auto const & motor : systemIt->robot->getMotors()) - { - int32_t const & motorIdx = motor->getIdx(); - int32_t const & motorVelocityIdx = motor->getJointVelocityIdx(); - u[motorVelocityIdx] += uMotor[motorIdx]; - } + + // TODO transmission + // for (auto const & motor : systemIt->robot->getMotors()) + // { + // int32_t const & motorIdx = motor->getIdx(); + // int32_t const & motorVelocityIdx = motor->getJointVelocityIdx(); + // u[motorVelocityIdx] += uMotor[motorIdx]; + // } // Compute dynamics a = computeAcceleration(*systemIt, *systemDataIt, q, v, u, fext); @@ -3658,12 +3661,14 @@ namespace jiminy // Compute the total effort vector u = uInternal + uCustom; - for (auto const & motor : systemIt->robot->getMotors()) - { - int32_t const & motorIdx = motor->getIdx(); - int32_t const & motorVelocityIdx = motor->getJointVelocityIdx(); - u[motorVelocityIdx] += uMotor[motorIdx]; - } + + // TODO Transmission + // for (auto const & motor : systemIt->robot->getMotors()) + // { + // int32_t const & motorIdx = motor->getIdx(); + // int32_t const & motorVelocityIdx = motor->getJointVelocityIdx(); + // u[motorVelocityIdx] += uMotor[motorIdx]; + // } // Compute the dynamics *aIt = computeAcceleration(*systemIt, *systemDataIt, *qIt, *vIt, u, fext); @@ -3682,6 +3687,7 @@ namespace jiminy pinocchio::Model const & model = system.robot->pncModel_; pinocchio::Data & data = system.robot->pncData_; + model.rotorInertia = system.robot->getArmatures().col(0); if (system.robot->hasConstraints()) { // Define some proxies for convenience diff --git a/core/src/robot/AbstractMotor.cc b/core/src/robot/AbstractMotor.cc index edd243cb4..b77694113 100644 --- a/core/src/robot/AbstractMotor.cc +++ b/core/src/robot/AbstractMotor.cc @@ -16,11 +16,6 @@ namespace jiminy notifyRobot_(), name_(name), motorIdx_(-1), - jointName_(), - jointModelIdx_(-1), - jointType_(joint_t::NONE), - jointPositionIdx_(-1), - jointVelocityIdx_(-1), commandLimit_(0.0), armature_(0.0), sharedHolder_(nullptr) @@ -64,10 +59,16 @@ namespace jiminy // Get an index motorIdx_ = sharedHolder_->num_; - // Add a value for the motor to the shared data buffer - sharedHolder_->data_.conservativeResize(sharedHolder_->num_ + 1); - sharedHolder_->data_.tail<1>().setZero(); - + // Add values for the motor to the shared data buffer + for (vectorN_t & data : std::array{{ + sharedHolder_->position_, + sharedHolder_->velocity_, + sharedHolder_->acceleration_, + sharedHolder_->effort_}}) + { + data.conservativeResize(sharedHolder_->num_ + 1); + data.tail<1>().setZero(); + } // Add the motor to the shared memory sharedHolder_->motors_.push_back(this); ++sharedHolder_->num_; @@ -88,14 +89,21 @@ namespace jiminy return hresult_t::ERROR_GENERIC; } - // Remove associated col in the global data buffer - if (motorIdx_ < sharedHolder_->num_ - 1) + for (vectorN_t & data : std::array{{ + sharedHolder_->position_, + sharedHolder_->velocity_, + sharedHolder_->acceleration_, + sharedHolder_->effort_}}) { - int32_t motorShift = sharedHolder_->num_ - motorIdx_ - 1; - sharedHolder_->data_.segment(motorIdx_, motorShift) = - sharedHolder_->data_.segment(motorIdx_ + 1, motorShift).eval(); // eval to avoid aliasing + // Remove associated col in the global data buffer + if (motorIdx_ < sharedHolder_->num_ - 1) + { + int32_t motorShift = sharedHolder_->num_ - motorIdx_ - 1; + data.segment(motorIdx_, motorShift) = + data.segment(motorIdx_ + 1, motorShift).eval(); // eval to avoid aliasing + } + data.conservativeResize(sharedHolder_->num_ - 1); } - sharedHolder_->data_.conservativeResize(sharedHolder_->num_ - 1); // Shift the motor ids for (int32_t i = motorIdx_ + 1; i < sharedHolder_->num_; ++i) @@ -138,7 +146,14 @@ namespace jiminy } // Clear the shared data buffer - sharedHolder_->data_.setZero(); + for (vectorN_t & data : std::array{{ + sharedHolder_->position_, + sharedHolder_->velocity_, + sharedHolder_->acceleration_, + sharedHolder_->effort_}}) + { + data.setZero(); + } // Update motor scope information for (AbstractMotorBase * motor : sharedHolder_->motors_) @@ -236,38 +251,18 @@ namespace jiminy if (returnCode == hresult_t::SUCCESS) { - returnCode = ::jiminy::getJointModelIdx(robot->pncModel_, jointName_, jointModelIdx_); - } - - if (returnCode == hresult_t::SUCCESS) - { - returnCode = getJointTypeFromIdx(robot->pncModel_, jointModelIdx_, jointType_); - } - - if (returnCode == hresult_t::SUCCESS) - { - // Motors are only supported for linear and rotary joints - if (jointType_ != joint_t::LINEAR && jointType_ != joint_t::ROTARY && jointType_ != joint_t::ROTARY_UNBOUNDED) - { - PRINT_ERROR("A motor can only be associated with a 1-dof linear or rotary joint."); - returnCode = hresult_t::ERROR_BAD_INPUT; - } - } - - if (returnCode == hresult_t::SUCCESS) - { - ::jiminy::getJointPositionIdx(robot->pncModel_, jointName_, jointPositionIdx_); - ::jiminy::getJointVelocityIdx(robot->pncModel_, jointName_, jointVelocityIdx_); - - // Get the motor effort limits from the URDF or the user options. - if (baseMotorOptions_->commandLimitFromUrdf) - { - commandLimit_ = robot->pncModel_.effortLimit[jointVelocityIdx_] / baseMotorOptions_->mechanicalReduction; - } - else - { - commandLimit_ = baseMotorOptions_->commandLimit; - } + // TODO shift this to transmission + // ::jiminy::getJointVelocityIdx(robot->pncModel_, jointName_, jointVelocityIdx_); + + // // Get the motor effort limits from the URDF or the user options. + // if (baseMotorOptions_->commandLimitFromUrdf) + // { + // commandLimit_ = robot->pncModel_.effortLimit[jointVelocityIdx_]; + // } + // else + // { + // commandLimit_ = baseMotorOptions_->commandLimit; + // } // Get the rotor inertia if (baseMotorOptions_->enableArmature) @@ -289,24 +284,42 @@ namespace jiminy return returnCode; } - float64_t const & AbstractMotorBase::get(void) const + float64_t & AbstractMotorBase::q(void) { - static float64_t dataEmpty; - if (isAttached_) - { - return sharedHolder_->data_[motorIdx_]; - } - return dataEmpty; + return sharedHolder_->position_[motorIdx_]; } - float64_t & AbstractMotorBase::data(void) + float64_t & AbstractMotorBase::v(void) { - return sharedHolder_->data_[motorIdx_]; + return sharedHolder_->velocity_[motorIdx_]; + } + float64_t & AbstractMotorBase::a(void) + { + return sharedHolder_->acceleration_[motorIdx_]; + } + float64_t & AbstractMotorBase::u(void) + { + return sharedHolder_->effort_[motorIdx_]; + } + + float64_t const & AbstractMotorBase::getPosition(void) const + { + return sharedHolder_->position_[motorIdx_]; } - vectorN_t const & AbstractMotorBase::getAll(void) const + float64_t const & AbstractMotorBase::getVelocity(void) const { - return sharedHolder_->data_; + return sharedHolder_->velocity_[motorIdx_]; + } + + float64_t const & AbstractMotorBase::getAcceleration(void) const + { + return sharedHolder_->acceleration_[motorIdx_]; + } + + float64_t const & AbstractMotorBase::getEffort(void) const + { + return sharedHolder_->effort_[motorIdx_]; } hresult_t AbstractMotorBase::setOptionsAll(configHolder_t const & motorOptions) @@ -331,11 +344,6 @@ namespace jiminy return returnCode; } - bool_t const & AbstractMotorBase::getIsInitialized(void) const - { - return isInitialized_; - } - std::string const & AbstractMotorBase::getName(void) const { return name_; @@ -346,31 +354,6 @@ namespace jiminy return motorIdx_; } - std::string const & AbstractMotorBase::getJointName(void) const - { - return jointName_; - } - - jointIndex_t const & AbstractMotorBase::getJointModelIdx(void) const - { - return jointModelIdx_; - } - - joint_t const & AbstractMotorBase::getJointType(void) const - { - return jointType_; - } - - int32_t const & AbstractMotorBase::getJointPositionIdx(void) const - { - return jointPositionIdx_; - } - - int32_t const & AbstractMotorBase::getJointVelocityIdx(void) const - { - return jointVelocityIdx_; - } - float64_t const & AbstractMotorBase::getCommandLimit(void) const { return commandLimit_; @@ -381,11 +364,7 @@ namespace jiminy return armature_; } - hresult_t AbstractMotorBase::computeEffortAll(float64_t const & t, - vectorN_t const & q, - vectorN_t const & v, - vectorN_t const & a, - vectorN_t const & command) + hresult_t AbstractMotorBase::computeEffortAll(vectorN_t const & command) { hresult_t returnCode = hresult_t::SUCCESS; @@ -401,20 +380,7 @@ namespace jiminy { if (returnCode == hresult_t::SUCCESS) { - uint8_t nq_motor; - if (motor->getJointType() == joint_t::ROTARY_UNBOUNDED) - { - nq_motor = 2; - } - else - { - nq_motor = 1; - } - returnCode = motor->computeEffort(t, - q.segment(motor->getJointPositionIdx(), nq_motor), - v[motor->getJointVelocityIdx()], - a[motor->getJointVelocityIdx()], - command[motor->getIdx()]); + returnCode = motor->computeEffort(command[motor->getIdx()]); } } diff --git a/core/src/robot/AbstractTransmission.cc b/core/src/robot/AbstractTransmission.cc new file mode 100644 index 000000000..05e1b0c6a --- /dev/null +++ b/core/src/robot/AbstractTransmission.cc @@ -0,0 +1,424 @@ +#include "jiminy/core/robot/Robot.h" +#include "jiminy/core/Macros.h" + +#include "jiminy/core/utilities/Pinocchio.h" +#include "jiminy/core/robot/AbstractTransmission.h" + + +namespace jiminy +{ + AbstractTransmissionBase::AbstractTransmissionBase(std::string const & name) : + baseTransmissionOptions_(nullptr), + transmissionOptionsHolder_(), + isInitialized_(false), + isAttached_(false), + robot_(), + name_(name), + transmissionIdx_(-1), + jointNames_(), + jointModelIndices_(-1), + jointTypes_(), + jointPositionIndices_(-1), + jointVelocityIndices_(-1), + motorNames_() + { + // Initialize the options + setOptions(transmissionOptionsHolder_); + } + + AbstractTransmissionBase::~AbstractTransmissionBase(void) + { + // Detach the transmission before deleting it if necessary + if (isAttached_) + { + detach(); + } + } + + hresult_t AbstractTransmissionBase::initialize(std::vector const & jointNames, + std::vector const & motorNames) + { + // Copy reference to joint and motors names + hresult_t returnCode = hresult_t::SUCCESS; + jointNames_ = jointNames; + motorNames_ = motorNames; + isInitialized_ = true; + + returnCode = refreshProxies(); + if (returnCode != hresult_t::SUCCESS) + { + jointNames_.clear(); + motorNames_.clear(); + isInitialized_ = false; + } + + return returnCode; + } + + hresult_t AbstractTransmissionBase::attach(std::weak_ptr robot) + { + // Make sure the transmission is not already attached + if (isAttached_) + { + PRINT_ERROR("Transmission already attached to a robot. Please 'detach' method before attaching it."); + return hresult_t::ERROR_GENERIC; + } + + // Make sure the robot still exists + if (robot.expired()) + { + PRINT_ERROR("Robot pointer expired or unset."); + return hresult_t::ERROR_GENERIC; + } + + // TODO Make sure the joint is not already attached to a transmission + // WARNING at this point it is still not know which joint or motor the transmision connects + // auto robotTemp = robot.lock(); + // std::vector actuatedJointNames = robotTemp->getActuatedJointNames(); + // for (std::string const & transmissionJoint : getJointNames()) + // { + // auto transmissionJointIt = std::find(actuatedJointNames.begin(), actuatedJointNames.end(), transmissionJoint); + // if (transmissionJointIt != actuatedJointNames.end()) + // { + // PRINT_ERROR("Joint already attached to another transmission"); + // return hresult_t::ERROR_GENERIC; + // } + // } + + // Copy references to the robot and shared data + robot_ = robot; + + // Update the flag + isAttached_ = true; + + return hresult_t::SUCCESS; + } + + hresult_t AbstractTransmissionBase::detach(void) + { + if (!isAttached_) + { + PRINT_ERROR("Transmission not attached to any robot."); + return hresult_t::ERROR_GENERIC; + } + + // Clear the references to the robot + robot_.reset(); + + // Unset the Id + transmissionIdx_ = -1; + + // Delete motor and joint references + + // Update the flag + isAttached_ = false; + + return hresult_t::SUCCESS; + } + + hresult_t AbstractTransmissionBase::setOptions(configHolder_t const & transmissionOptions) + { + // Check if the internal buffers must be updated + bool_t internalBuffersMustBeUpdated = false; + if (isInitialized_) + { + // Check if reduction ratio has changed + float64_t const & mechanicalReduction = boost::get(transmissionOptions.at("mechanicalReduction")); + internalBuffersMustBeUpdated |= (baseTransmissionOptions_->mechanicalReduction != mechanicalReduction); + } + + // Update the transmission's options + transmissionOptionsHolder_ = transmissionOptions; + baseTransmissionOptions_ = std::make_unique(transmissionOptionsHolder_); + + // Refresh the proxies if the robot is initialized if available + if (auto robot = robot_.lock()) + { + if (internalBuffersMustBeUpdated && robot->getIsInitialized() && isAttached_) + { + refreshProxies(); + } + } + + return hresult_t::SUCCESS; + } + + configHolder_t AbstractTransmissionBase::getOptions(void) const + { + return transmissionOptionsHolder_; + } + + hresult_t AbstractTransmissionBase::refreshProxies(void) + { + hresult_t returnCode = hresult_t::SUCCESS; + + if (!isAttached_) + { + PRINT_ERROR("Transmission not attached to any robot. Impossible to refresh proxies."); + returnCode = hresult_t::ERROR_INIT_FAILED; + } + + auto robot = robot_.lock(); + if (returnCode == hresult_t::SUCCESS) + { + if (!robot) + { + PRINT_ERROR("Robot has been deleted. Impossible to refresh proxies."); + returnCode = hresult_t::ERROR_GENERIC; + } + } + + if (returnCode == hresult_t::SUCCESS) + { + if (!isInitialized_) + { + PRINT_ERROR("Transmission not initialized. Impossible to refresh proxies."); + returnCode = hresult_t::ERROR_INIT_FAILED; + } + } + + if (returnCode == hresult_t::SUCCESS) + { + if (!robot->getIsInitialized()) + { + PRINT_ERROR("Robot not initialized. Impossible to refresh proxies."); + returnCode = hresult_t::ERROR_INIT_FAILED; + } + } + + for (int i = 0; i < jointNames_.size(); i++) + { + if (returnCode == hresult_t::SUCCESS) + { + returnCode = ::jiminy::getJointModelIdx(robot->pncModel_, jointNames_[i], jointModelIndices_[i]); + } + } + for (int i = 0; i < jointNames_.size(); i++) + { + if (returnCode == hresult_t::SUCCESS) + { + returnCode = getJointTypeFromIdx(robot->pncModel_, jointModelIndices_[i], jointTypes_[i]); + } + } + + for (int i = 0; i < jointNames_.size(); i++) + { + if (returnCode == hresult_t::SUCCESS) + { + // Transmissions are only supported for linear and rotary joints + if (jointTypes_[i] != joint_t::LINEAR && jointTypes_[i] != joint_t::ROTARY && jointTypes_[i] != joint_t::ROTARY_UNBOUNDED) + { + PRINT_ERROR("A transmission can only be associated with a 1-dof linear or rotary joint."); + returnCode = hresult_t::ERROR_BAD_INPUT; + } + } + } + + for (int i = 0; i < jointNames_.size(); i++) + { + if (returnCode == hresult_t::SUCCESS) + { + ::jiminy::getJointPositionIdx(robot->pncModel_, jointNames_[i], static_cast(jointPositionIndices_[i])); + ::jiminy::getJointVelocityIdx(robot->pncModel_, jointNames_[i], static_cast(jointVelocityIndices_[i])); + } + } + + // Populate motorIndices_ + std::weak_ptr motor; + for (std::string const & motorName : getMotorNames()) + { + if (returnCode == hresult_t::SUCCESS) + { + returnCode = robot->getMotor(motorName, motor); + auto motorTemp = motor.lock(); + int32_t idx = motorTemp->getIdx(); + motorIndices_.push_back(idx); + } + } + + // PopulatjointPositionIndices_ + std::vector jointPositionIndices; + for (std::string const & jointName : jointNames_) + { + std::vector jointPositionIdx; + if (!robot->pncModel_.existJointName(jointName)) + { + PRINT_ERROR("Joint '", jointName, "' not found in robot model."); + return hresult_t::ERROR_BAD_INPUT; + } + if (returnCode == hresult_t::SUCCESS) + { + returnCode = getJointPositionIdx(robot->pncModel_, jointName, jointPositionIdx); + } + if (returnCode == hresult_t::SUCCESS) + { + jointPositionIndices.insert(jointPositionIndices.end(), jointPositionIdx.begin(), jointPositionIdx.end()); + } + } + int32_t jointPositionSize = jointPositionIndices.size(); + jointPositionIndices_.resize(jointPositionSize); + for (int32_t i = 0; i < jointPositionSize; ++i) + { + jointPositionIndices_(i) = jointPositionIndices[i]; + } + + + // Populate jointVelocityIndices_ + std::vector jointVelocityIndices; + for (std::string const & jointName : jointNames_) + { + std::vector jointVelocityIdx; + if (!robot->pncModel_.existJointName(jointName)) + { + PRINT_ERROR("Joint '", jointName, "' not found in robot model."); + return hresult_t::ERROR_BAD_INPUT; + } + jointIndex_t const & jointModelIdx = robot->pncModel_.getJointId(jointName); + int32_t const & jointVelocityFirstIdx = robot->pncModel_.joints[jointModelIdx].idx_v(); + int32_t const & jointNv = robot->pncModel_.joints[jointModelIdx].nv(); + jointVelocityIdx.resize(jointNv); + std::iota(jointVelocityIdx.begin(), jointVelocityIdx.end(), jointVelocityFirstIdx); + jointVelocityIndices.insert(jointVelocityIndices.end(), jointVelocityIdx.begin(), jointVelocityIdx.end()); + } + + int32_t jointVelocitySize = jointVelocityIndices.size(); + jointVelocityIndices_.resize(jointVelocitySize); + for (int32_t i = 0; i < jointVelocitySize; ++i) + { + jointVelocityIndices_(i) = jointVelocityIndices[i]; + } + + return returnCode; + } + + bool_t const & AbstractTransmissionBase::getIsInitialized(void) const + { + return isInitialized_; + } + + std::string const & AbstractTransmissionBase::getName(void) const + { + return name_; + } + + int32_t const & AbstractTransmissionBase::getIdx(void) const + { + return transmissionIdx_; + } + + std::vector const & AbstractTransmissionBase::getJointNames(void) const + { + return jointNames_; + } + + std::vector const & AbstractTransmissionBase::getJointModelIndices(void) const + { + return jointModelIndices_; + } + + std::vector const & AbstractTransmissionBase::getJointTypes(void) const + { + return jointTypes_; + } + + vectorN_t const & AbstractTransmissionBase::getJointPositionIndices(void) const + { + return jointPositionIndices_; + } + + + vectorN_t const & AbstractTransmissionBase::getJointVelocityIndices(void) const + { + + return jointVelocityIndices_; + } + + std::vector const & AbstractTransmissionBase::getMotorNames(void) const + { + return motorNames_; + } + + std::vector const & AbstractTransmissionBase::getMotorIndices(void) const + { + return motorIndices_; + } + + matrixN_t const & AbstractTransmissionBase::getForwardTransform(void) const + { + return forwardTransform_; + } + + matrixN_t const & AbstractTransmissionBase::getInverseTransform(void) const + { + return backwardTransform_; + } + + hresult_t AbstractTransmissionBase::computeForward(float64_t const & /*t*/, + vectorN_t & q, + vectorN_t & v, + vectorN_t & a, + vectorN_t & uJoint) + { + // Extract motor configuration and velocity from all motors attached + // to the robot for this transmission + + // ujoint is effort on the level of the joint, not transmission + // if you know torque at motor level, use transmission to compute torque at joint level + // you know state of the system (joints), then compute the state of the motors + + // Gather the corresponding data of the motors attached to the transmission + int32_t numberOfMotors = motors_.size(); + vectorN_t qMotor(numberOfMotors); + vectorN_t vMotor(numberOfMotors); + vectorN_t aMotor(numberOfMotors); + vectorN_t uMotor(numberOfMotors); + + for (int32_t i = 0; i < numberOfMotors; i++) + { + auto motorTemp = motors_[i].lock(); + qMotor[i] = motorTemp->getPosition(); + vMotor[i] = motorTemp->getVelocity(); + aMotor[i] = motorTemp->getAcceleration(); + uMotor[i] = motorTemp->getEffort(); + } + // Compute the transmission effect based on the current configuration + computeTransform(qMotor, vMotor, forwardTransform_); + + // Apply transformation from motor to joint level + // TODO take care of motor position which can be 1 or 2 dimensional + q.noalias() = forwardTransform_ * qMotor; + v.noalias() = forwardTransform_ * vMotor; + a.noalias() = forwardTransform_ * aMotor; + uJoint.noalias() = forwardTransform_ * uMotor; + + } + + hresult_t AbstractTransmissionBase::computeBackward(float64_t const & t, + vectorN_t const & q, + vectorN_t const & v, + vectorN_t const & a, + vectorN_t const & uJoint) + { + // Extract motor configuration and velocity from all motors attached + // to the robot for this transmission + auto qMotors = q.segment(jointPositionIndices_); + auto vMotors = v.segment(jointVelocityIndices_); + + // Compute the transmission effect based on the current configuration + computeInverseTransform(qMotors, vMotors, backwardTransform_); + + // Gather the corresponding data of the motors attached to the transmission + int32_t numberOfMotors = motors_.size(); + vectorN_t qTemp(numberOfMotors); + vectorN_t vTemp(numberOfMotors); + vectorN_t aTemp(numberOfMotors); + vectorN_t uTemp(numberOfMotors); + + // TODO similar way than forward + auto motors = motors_.lock(); + motors->q = backwardTransform_ * q; + motors->v = backwardTransform_ * v; + motors->a = backwardTransform_ * a; + motors->u = backwardTransform_ * uJoint; + } +} diff --git a/core/src/robot/BasicMotors.cc b/core/src/robot/BasicMotors.cc index be27e536f..6792ae5a8 100644 --- a/core/src/robot/BasicMotors.cc +++ b/core/src/robot/BasicMotors.cc @@ -17,17 +17,14 @@ namespace jiminy setOptions(getDefaultMotorOptions()); } - hresult_t SimpleMotor::initialize(std::string const & jointName) + hresult_t SimpleMotor::initialize(void) { hresult_t returnCode = hresult_t::SUCCESS; - - jointName_ = jointName; isInitialized_ = true; returnCode = refreshProxies(); if (returnCode != hresult_t::SUCCESS) { - jointName_.clear(); isInitialized_ = false; } @@ -79,42 +76,34 @@ namespace jiminy return returnCode; } - hresult_t SimpleMotor::computeEffort(float64_t const & /* t */, - Eigen::VectorBlock const & /* q */, - float64_t const & v, - float64_t const & /* a */, - float64_t command) + hresult_t SimpleMotor::computeEffort(float64_t command) { - if (!isInitialized_) - { - PRINT_ERROR("Motor not initialized. Impossible to compute actual motor effort."); - return hresult_t::ERROR_INIT_FAILED; - } - /* Compute the motor effort, taking into account the limit, if any. It is the output of the motor on joint side, ie after the transmission. */ if (motorOptions_->enableCommandLimit) { command = clamp(command, -getCommandLimit(), getCommandLimit()); } - data() = motorOptions_->mechanicalReduction * command; - /* Add friction to the joints associated with the motor if enable. - It is computed on joint side instead of the motor. */ - if (motorOptions_->enableFriction) - { - float64_t const & vJoint = v; - if (vJoint > 0) - { - data() += motorOptions_->frictionViscousPositive * vJoint - + motorOptions_->frictionDryPositive * tanh(motorOptions_->frictionDrySlope * vJoint); - } - else - { - data() += motorOptions_->frictionViscousNegative * vJoint - + motorOptions_->frictionDryNegative * tanh(motorOptions_->frictionDrySlope * vJoint); - } - } + // TODO all of this here needs to be done properly taking into account the transmission + // data() = command; + + // /* Add friction to the joints associated with the motor if enable. + // It is computed on joint side instead of the motor. */ + // if (motorOptions_->enableFriction) + // { + // float64_t const & vJoint = v; + // if (vJoint > 0) + // { + // data() += motorOptions_->frictionViscousPositive * vJoint + // + motorOptions_->frictionDryPositive * tanh(motorOptions_->frictionDrySlope * vJoint); + // } + // else + // { + // data() += motorOptions_->frictionViscousNegative * vJoint + // + motorOptions_->frictionDryNegative * tanh(motorOptions_->frictionDrySlope * vJoint); + // } + // } return hresult_t::SUCCESS; } diff --git a/core/src/robot/BasicTransmissions.cc b/core/src/robot/BasicTransmissions.cc new file mode 100644 index 000000000..131bee6d2 --- /dev/null +++ b/core/src/robot/BasicTransmissions.cc @@ -0,0 +1,44 @@ +#include + +#include "jiminy/core/utilities/Helpers.h" + +#include "jiminy/core/robot/BasicTransmissions.h" + + +namespace jiminy +{ + SimpleTransmission::SimpleTransmission(std::string const & name) : + AbstractTransmissionBase(name), + transmissionOptions_(nullptr) + { + /* AbstractTransmissionBase constructor calls the base implementations of + the virtual methods since the derived class is not available at + this point. Thus it must be called explicitly in the constructor. */ + setOptions(getOptions()); + } + + void SimpleTransmission::computeTransform(Eigen::VectorBlock const & /*q*/, + Eigen::VectorBlock const & /*v*/, + matrixN_t &out) + { + if (!isInitialized_) + { + PRINT_ERROR("Transmission not initialized. Impossible to compute transformation of transmission."); + } + } + + void SimpleTransmission::computeInverseTransform(Eigen::VectorBlock const & /*q*/, + Eigen::VectorBlock const & /*v*/, + matrixN_t &out) + { + if (!isInitialized_) + { + PRINT_ERROR("Transmission not initialized. Impossible to compute transformation of transmission."); + } + } + + void computeEffortTransmission(void) + { + // TODO + } +} diff --git a/core/src/robot/Model.cc b/core/src/robot/Model.cc index ba4dee805..6b3276471 100644 --- a/core/src/robot/Model.cc +++ b/core/src/robot/Model.cc @@ -46,6 +46,7 @@ namespace jiminy constraintsHolder_t::constraintsHolder_t(void) : boundJoints(), contactFrames(), + transmissionConstraints(), collisionBodies(), registered() { @@ -101,6 +102,7 @@ namespace jiminy break; case constraintsHolderType_t::USER: case constraintsHolderType_t::COLLISION_BODIES: + case constraintsHolderType_t::TRANSMISSIONS: default: constraintsMapPtr = ®istered; } @@ -171,6 +173,7 @@ namespace jiminy collisionBodies.push_back(constraintsMap); break; case constraintsHolderType_t::USER: + case constraintsHolderType_t::TRANSMISSIONS: default: registered.insert(registered.end(), constraintsMap.begin(), constraintsMap.end()); } diff --git a/core/src/robot/Robot.cc b/core/src/robot/Robot.cc index 6975308d6..f428bb1c3 100644 --- a/core/src/robot/Robot.cc +++ b/core/src/robot/Robot.cc @@ -5,6 +5,7 @@ #include "jiminy/core/robot/AbstractMotor.h" #include "jiminy/core/robot/AbstractSensor.h" +#include "jiminy/core/robot/AbstractTransmission.h" #include "jiminy/core/telemetry/TelemetryData.h" #include "jiminy/core/io/FileDevice.h" #include "jiminy/core/utilities/Helpers.h" @@ -21,13 +22,16 @@ namespace jiminy isTelemetryConfigured_(false), telemetryData_(nullptr), motorsHolder_(), + transmissionsHolder_(), sensorsGroupHolder_(), sensorTelemetryOptions_(), motorsNames_(), sensorsNames_(), + transmissionsNames_(), commandFieldnames_(), motorEffortFieldnames_(), nmotors_(0U), + actuatedJointNames_(), mutexLocal_(std::make_unique()), motorsSharedHolder_(std::make_shared()), sensorsSharedHolder_() @@ -37,18 +41,20 @@ namespace jiminy Robot::~Robot(void) { - // Detach all the motors and sensors + // Detach all the motors, sensors and transmissions detachSensors({}); detachMotors({}); + detachTransmissions({}); } hresult_t Robot::initialize(std::string const & urdfPath, bool_t const & hasFreeflyer, std::vector const & meshPackageDirs) { - // Detach all the motors and sensors + // Detach all the motors, sensors and transmissions detachSensors({}); detachMotors({}); + detachTransmissions({}); /* Delete the current model and generate a new one. Note that is also refresh all proxies automatically. */ @@ -62,6 +68,7 @@ namespace jiminy // Detach all the motors and sensors detachSensors({}); detachMotors({}); + detachTransmissions({}); /* Delete the current model and generate a new one. Note that is also refresh all proxies automatically. */ @@ -88,6 +95,12 @@ namespace jiminy } } + // Reset the transmissions + if (!transmissionsHolder_.empty()) + { + (*transmissionsHolder_.begin())->resetAll(); + } + // Reset the telemetry flag isTelemetryConfigured_ = false; } @@ -136,6 +149,101 @@ namespace jiminy return returnCode; } + hresult_t Robot::attachTransmission(std::shared_ptr transmission) + { + hresult_t returnCode = hresult_t::SUCCESS; + + if (!isInitialized_) + { + PRINT_ERROR("The robot is not initialized."); + returnCode = hresult_t::ERROR_INIT_FAILED; + } + + if (returnCode == hresult_t::SUCCESS) + { + if (getIsLocked()) + { + PRINT_ERROR("Robot is locked, probably because a simulation is running. " + "Please stop it before adding motors."); + returnCode = hresult_t::ERROR_GENERIC; + } + } + + if (returnCode == hresult_t::SUCCESS) + { + std::string const & transmissionName = transmission->getName(); + auto transmissionIt = std::find_if(transmissionsHolder_.begin(), transmissionsHolder_.end(), + [&transmissionName](auto const & elem) + { + return (elem->getName() == transmissionName); + }); + if (transmissionIt != transmissionsHolder_.end()) + { + PRINT_ERROR("A transmission with the same name already exists."); + returnCode = hresult_t::ERROR_BAD_INPUT; + } + } + + if (returnCode == hresult_t::SUCCESS) + { + // Attach the transmission + returnCode = transmission->attach(shared_from_this()); + } + + if (returnCode == hresult_t::SUCCESS) + { + // Add the transmission to the holder + transmissionsHolder_.push_back(transmission); + + // Refresh the transmissions proxies + refreshTransmissionProxies(); + } + + // update list of actuated joints + std::vector const & jointNames = transmission->getJointNames(); + actuatedJointNames_.insert(actuatedJointNames_.end(), jointNames.begin(), jointNames.end()); + + return returnCode; + } + + hresult_t Robot::detachTransmission(std::string const & transmissionName) + { + if (!isInitialized_) + { + PRINT_ERROR("Robot not initialized."); + return hresult_t::ERROR_INIT_FAILED; + } + + if (getIsLocked()) + { + PRINT_ERROR("Robot is locked, probably because a simulation is running. " + "Please stop it before removing transmissions."); + return hresult_t::ERROR_GENERIC; + } + + auto transmissionIt = std::find_if(transmissionsHolder_.begin(), transmissionsHolder_.end(), + [&transmissionName](auto const & elem) + { + return (elem->getName() == transmissionName); + }); + if (transmissionIt == transmissionsHolder_.end()) + { + PRINT_ERROR("No transmission with this name exists."); + return hresult_t::ERROR_BAD_INPUT; + } + + // Detach the transmission + (*transmissionIt)->detach(); // It cannot fail at this point + + // Remove the transmission from the holder + transmissionsHolder_.erase(transmissionIt); + + // Refresh the transmissions proxies + refreshTransmissionProxies(); + + return hresult_t::SUCCESS; + } + hresult_t Robot::attachMotor(std::shared_ptr motor) { hresult_t returnCode = hresult_t::SUCCESS; @@ -186,13 +294,14 @@ namespace jiminy } // Update rotor inertia of pinocchio model - float64_t const & armature = motorIn.getArmature(); - std::string const & jointName = motorIn.getJointName(); - int32_t jointVelocityIdx; - ::jiminy::getJointVelocityIdx(robot->pncModel_, jointName, jointVelocityIdx); - robot->pncModel_.rotorInertia[jointVelocityIdx] = armature; - ::jiminy::getJointVelocityIdx(robot->pncModelRigidOrig_, jointName, jointVelocityIdx); - robot->pncModelRigidOrig_.rotorInertia[jointVelocityIdx] = armature; + // TODO should not be done here anymore , motor has no idea of joints + [[maybe_unused]] float64_t const & armature = motorIn.getArmature(); + // std::string const & jointName = motorIn.getJointName(); + // int32_t jointVelocityIdx; + // ::jiminy::getJointVelocityIdx(robot->pncModel_, jointName, jointVelocityIdx); + // robot->pncModel_.rotorInertia[jointVelocityIdx] = armature; + // ::jiminy::getJointVelocityIdx(robot->pncModelRigidOrig_, jointName, jointVelocityIdx); + // robot->pncModelRigidOrig_.rotorInertia[jointVelocityIdx] = armature; return hresult_t::SUCCESS; }; @@ -297,6 +406,52 @@ namespace jiminy return returnCode; } + hresult_t Robot::detachTransmissions(std::vector const & transmissionsNames) + { + hresult_t returnCode = hresult_t::SUCCESS; + + if (!transmissionsNames.empty()) + { + // Make sure that no transmission names are duplicates + if (checkDuplicates(transmissionsNames)) + { + PRINT_ERROR("Duplicated transmission names."); + returnCode = hresult_t::ERROR_BAD_INPUT; + } + + if (returnCode == hresult_t::SUCCESS) + { + // Make sure that every transmission name exist + // TODO adapt the template to include transmissions + // if (!checkInclusion(transmissionsNames_, transmissionsNames)) + // { + // PRINT_ERROR("At least one of the transmission names does not exist."); + // returnCode = hresult_t::ERROR_BAD_INPUT; + // } + } + + for (std::string const & name : transmissionsNames) + { + if (returnCode == hresult_t::SUCCESS) + { + returnCode = detachTransmission(name); + } + } + } + else + { + if (returnCode == hresult_t::SUCCESS) + { + if (!transmissionsNames_.empty()) + { + returnCode = detachTransmissions(std::vector(transmissionsNames_)); + } + } + } + + return returnCode; + } + hresult_t Robot::attachSensor(std::shared_ptr sensor) { // The sensors' names must be unique, even if their type is different. @@ -493,6 +648,11 @@ namespace jiminy returnCode = refreshSensorsProxies(); } + if (returnCode == hresult_t::SUCCESS) + { + returnCode = refreshTransmissionProxies(); + } + return returnCode; } @@ -577,6 +737,12 @@ namespace jiminy return returnCode; } + hresult_t Robot::refreshTransmissionProxies(void) + { + // TODO + return hresult_t::SUCCESS; + } + hresult_t Robot::getMotor(std::string const & motorName, std::shared_ptr & motor) { @@ -632,6 +798,11 @@ namespace jiminy return motorsHolder_; } + Robot::transmissionsHolder_t const & Robot::getTransmissions(void) const + { + return transmissionsHolder_; + } + hresult_t Robot::getSensor(std::string const & sensorType, std::string const & sensorName, std::weak_ptr & sensor) const @@ -1175,15 +1346,17 @@ namespace jiminy return isTelemetryConfigured_; } - void Robot::computeMotorsEfforts(float64_t const & t, - vectorN_t const & q, - vectorN_t const & v, - vectorN_t const & a, + void Robot::computeMotorsEfforts(float64_t const & /*t*/, + vectorN_t const & /*q*/, + vectorN_t const & /*v*/, + vectorN_t const & /*a*/, vectorN_t const & command) { if (!motorsHolder_.empty()) { - (*motorsHolder_.begin())->computeEffortAll(t, q, v, a, command); + // TODO this i would like to do + // (*motorsHolder_.begin())->computeEffortAll(t, q, v, a, command); + (*motorsHolder_.begin())->computeEffortAll(command); } } @@ -1191,10 +1364,12 @@ namespace jiminy { static vectorN_t const motorsEffortsEmpty; - if (!motorsHolder_.empty()) - { - return (*motorsHolder_.begin())->getAll(); - } + // TODO what to do here, its not just data anymore but 4 values + // q, v, a, u + // if (!motorsHolder_.empty()) + // { + // return (*motorsHolder_.begin())->getAll(); + // } return motorsEffortsEmpty; } @@ -1208,14 +1383,23 @@ namespace jiminy { return (elem->getName() == motorName); }); + // TODO what to do here, its not just data anymore but 4 values + // q, v, a, u if (motorIt != motorsHolder_.end()) { - return (*motorIt)->get(); + // return (*motorIt)->get(); + return (*motorIt)->getEffort(); } return motorEffortEmpty; } + float64_t getMotorEffortLimit(void) + { + // TODO combine robot information and transmission information + return 0.0; + } + void Robot::setSensorsData(float64_t const & t, vectorN_t const & q, vectorN_t const & v, @@ -1246,7 +1430,7 @@ namespace jiminy sensorDataTypeMap_t dataType(std::cref(sensorsSharedIt->second->dataMeasured_)); // Need explicit call to `std::reference_wrapper` for gcc<7.3 for (auto & sensor : sensorsGroupIt->second) { - auto & sensorConst = const_cast(*sensor); + auto const & sensorConst = const_cast(*sensor); dataType.emplace(sensorConst.getName(), sensorConst.getIdx(), sensorConst.get()); @@ -1318,12 +1502,13 @@ namespace jiminy { std::vector motorsModelIdx; motorsModelIdx.reserve(nmotors_); - std::transform(motorsHolder_.begin(), motorsHolder_.end(), - std::back_inserter(motorsModelIdx), - [](auto const & motor) -> jointIndex_t - { - return motor->getJointModelIdx(); - }); + // TODO use transmission + // std::transform(motorsHolder_.begin(), motorsHolder_.end(), + // std::back_inserter(motorsModelIdx), + // [](auto const & motor) -> jointIndex_t + // { + // return motor->getJointModelIdx(); + // }); return motorsModelIdx; } @@ -1331,20 +1516,21 @@ namespace jiminy { std::vector > motorsPositionIdx; motorsPositionIdx.reserve(nmotors_); - std::transform(motorsHolder_.begin(), motorsHolder_.end(), - std::back_inserter(motorsPositionIdx), - [](auto const & elem) -> std::vector - { - int32_t const & jointPositionIdx = elem->getJointPositionIdx(); - if (elem->getJointType() == joint_t::ROTARY_UNBOUNDED) - { - return {jointPositionIdx, jointPositionIdx + 1}; - } - else - { - return {jointPositionIdx}; - } - }); + // TODO not possible anymore, use transmission to get joint + // std::transform(motorsHolder_.begin(), motorsHolder_.end(), + // std::back_inserter(motorsPositionIdx), + // [](auto const & elem) -> std::vector + // { + // int32_t const & jointPositionIdx = elem->getJointPositionIdx(); + // if (elem->getJointType() == joint_t::ROTARY_UNBOUNDED) + // { + // return {jointPositionIdx, jointPositionIdx + 1}; + // } + // else + // { + // return {jointPositionIdx}; + // } + // }); return motorsPositionIdx; } @@ -1352,12 +1538,13 @@ namespace jiminy { std::vector motorsVelocityIdx; motorsVelocityIdx.reserve(nmotors_); - std::transform(motorsHolder_.begin(), motorsHolder_.end(), - std::back_inserter(motorsVelocityIdx), - [](auto const & elem) -> int32_t - { - return elem->getJointVelocityIdx(); - }); + // TODO transmission + // std::transform(motorsHolder_.begin(), motorsHolder_.end(), + // std::back_inserter(motorsVelocityIdx), + // [](auto const & elem) -> int32_t + // { + // return elem->getJointVelocityIdx(); + // }); return motorsVelocityIdx; } @@ -1380,6 +1567,10 @@ namespace jiminy return sensorsNamesEmpty; } } + std::vector const & Robot::getActuatedJointNames(void) const + { + return actuatedJointNames_; + } vectorN_t const & Robot::getCommandLimit(void) const { @@ -1387,20 +1578,21 @@ namespace jiminy commandLimit.resize(pncModel_.nv); commandLimit.setConstant(qNAN); - for (auto const & motor : motorsHolder_) - { - auto const & motorOptions = motor->baseMotorOptions_; - int32_t const & motorsVelocityIdx = motor->getJointVelocityIdx(); - if (motorOptions->enableCommandLimit) - { - commandLimit[motorsVelocityIdx] = motor->getCommandLimit(); - } - else - { - commandLimit[motorsVelocityIdx] = INF; - } - } + // TODO should be part of transmission + // for (auto const & motor : motorsHolder_) + // { + // auto const & motorOptions = motor->baseMotorOptions_; + // int32_t const & motorsVelocityIdx = motor->getJointVelocityIdx(); + // if (motorOptions->enableCommandLimit) + // { + // commandLimit[motorsVelocityIdx] = motor->getCommandLimit(); + // } + // else + // { + // commandLimit[motorsVelocityIdx] = INF; + // } + // } return commandLimit; } @@ -1411,11 +1603,13 @@ namespace jiminy armatures.resize(pncModel_.nv); armatures.setZero(); - for (auto const & motor : motorsHolder_) - { - int32_t const & motorsVelocityIdx = motor->getJointVelocityIdx(); - armatures[motorsVelocityIdx] = motor->getArmature(); - } + + // TODO part of transmission + // for (auto const & motor : motorsHolder_) + // { + // int32_t const & motorsVelocityIdx = motor->getJointVelocityIdx(); + // armatures[motorsVelocityIdx] = motor->getArmature(); + // } return armatures; } diff --git a/examples/cpp/double_pendulum/double_pendulum.cc b/examples/cpp/double_pendulum/double_pendulum.cc index 9285f7b8a..eddd39b2b 100644 --- a/examples/cpp/double_pendulum/double_pendulum.cc +++ b/examples/cpp/double_pendulum/double_pendulum.cc @@ -7,6 +7,7 @@ #include "jiminy/core/engine/Engine.h" #include "jiminy/core/robot/BasicMotors.h" +#include "jiminy/core/robot/BasicTransmissions.h" #include "jiminy/core/control/ControllerFunctor.h" #include "jiminy/core/io/FileDevice.h" #include "jiminy/core/utilities/Helpers.h" @@ -66,6 +67,7 @@ int main(int /* argc */, char_t * /* argv */[]) // Instantiate and configuration the robot std::vector motorJointNames{"SecondPendulumJoint"}; + std::vector transmissionNames{"SimpleTransmissionSecondPendulumJoint"}; auto robot = std::make_shared(); configHolder_t modelOptions = robot->getModelOptions(); @@ -77,7 +79,13 @@ int main(int /* argc */, char_t * /* argv */[]) { auto motor = std::make_shared(jointName); robot->attachMotor(motor); - motor->initialize(jointName); + // motor->initialize(jointName); + } + for (std::string const & transmissionName : transmissionNames) + { + auto transmission = std::make_shared(transmissionName); + // robot->attachTransmission(transmission); + // motor->initialize(jointName); } // Instantiate and configuration the controller diff --git a/python/jiminy_pywrap/include/jiminy/python/Transmissions.h b/python/jiminy_pywrap/include/jiminy/python/Transmissions.h new file mode 100644 index 000000000..ccbc32783 --- /dev/null +++ b/python/jiminy_pywrap/include/jiminy/python/Transmissions.h @@ -0,0 +1,14 @@ +#ifndef TRANSMISSIONS_PYTHON_H +#define TRANSMISSIONS_PYTHON_H + + +namespace jiminy +{ +namespace python +{ + void exposeAbstractTransmission(void); + void exposeSimpleTransmission(void); +} // End of namespace python. +} // End of namespace jiminy. + +#endif // TRANSMISSIONS_PYTHON_H diff --git a/python/jiminy_pywrap/src/Motors.cc b/python/jiminy_pywrap/src/Motors.cc index 129c1cc76..e365c94db 100644 --- a/python/jiminy_pywrap/src/Motors.cc +++ b/python/jiminy_pywrap/src/Motors.cc @@ -32,16 +32,6 @@ namespace python bp::return_value_policy())) .add_property("idx", bp::make_function(&AbstractMotorBase::getIdx, bp::return_value_policy())) - .add_property("joint_name", bp::make_function(&AbstractMotorBase::getJointName, - bp::return_value_policy())) - .add_property("joint_idx", bp::make_function(&AbstractMotorBase::getJointModelIdx, - bp::return_value_policy())) - .add_property("joint_type", bp::make_function(&AbstractMotorBase::getJointType, - bp::return_value_policy())) - .add_property("joint_position_idx", bp::make_function(&AbstractMotorBase::getJointPositionIdx, - bp::return_value_policy())) - .add_property("joint_velocity_idx", bp::make_function(&AbstractMotorBase::getJointVelocityIdx, - bp::return_value_policy())) .add_property("command_limit", bp::make_function(&AbstractMotorBase::getCommandLimit, bp::return_value_policy())) .add_property("armature", bp::make_function(&AbstractMotorBase::getArmature, diff --git a/python/jiminy_pywrap/src/Transmissions.cc b/python/jiminy_pywrap/src/Transmissions.cc new file mode 100644 index 000000000..f89ab5c9b --- /dev/null +++ b/python/jiminy_pywrap/src/Transmissions.cc @@ -0,0 +1,127 @@ +#include "jiminy/core/robot/BasicTransmissions.h" + +#include + +#include "jiminy/python/Utilities.h" +#include "jiminy/python/Transmissions.h" + + +namespace jiminy +{ +namespace python +{ + namespace bp = boost::python; + + // ***************************** PyAbstractTransmissionVisitor *********************************** + + struct PyAbstractTransmissionVisitor + : public bp::def_visitor + { + public: + /////////////////////////////////////////////////////////////////////////////// + /// \brief Expose C++ API through the visitor. + /////////////////////////////////////////////////////////////////////////////// + + template + void visit(PyClass & cl) const + { + cl + .add_property("is_initialized", bp::make_function(&AbstractTransmissionBase::getIsInitialized, + bp::return_value_policy())) + .add_property("name", bp::make_function(&AbstractTransmissionBase::getName, + bp::return_value_policy())) + .add_property("idx", bp::make_function(&AbstractTransmissionBase::getIdx, + bp::return_value_policy())) + .add_property("joint_names", bp::make_function(&AbstractTransmissionBase::getJointNames, + bp::return_value_policy())) + .add_property("joint_indices", bp::make_function(&AbstractTransmissionBase::getJointModelIndices, + bp::return_value_policy())) + .add_property("joint_types", bp::make_function(&AbstractTransmissionBase::getJointTypes, + bp::return_value_policy())) + .add_property("joint_position_indices", bp::make_function(&AbstractTransmissionBase::getJointPositionIndices, + bp::return_value_policy())) + .add_property("joint_velocity_indices", bp::make_function(&AbstractTransmissionBase::getJointVelocityIndices, + bp::return_value_policy())) + .add_property("forward_transform", bp::make_function(&AbstractTransmissionBase::getForward, + bp::return_value_policy >())) + .add_property("backward_transform", bp::make_function(&AbstractTransmissionBase::getBackward, + bp::return_value_policy >())) + + .def("set_options", &PyAbstractTransmissionVisitor::setOptions) + .def("get_options", &AbstractTransmissionBase::getOptions) + .def("compute_transform", &AbstractTransmissionBase::computeTransform) + .def("compute_inverse_transform", &AbstractTransmissionBase::computeInverseTransform) + ; + } + + public: + static void setOptions(AbstractTransmissionBase & self, + bp::dict const & configPy) + { + configHolder_t config = self.getOptions(); + convertFromPython(configPy, config); + self.setOptions(config); + } + + /////////////////////////////////////////////////////////////////////////////// + /// \brief Expose. + /////////////////////////////////////////////////////////////////////////////// + static void expose() + { + bp::class_, + boost::noncopyable>("AbstractTransmission", bp::no_init) + .def(PyAbstractTransmissionVisitor()); + } + }; + + BOOST_PYTHON_VISITOR_EXPOSE(AbstractTransmission) + + // ***************************** PySimpleTransmissionVisitor *********************************** + + struct PySimpleTransmissionVisitor + : public bp::def_visitor + { + public: + /////////////////////////////////////////////////////////////////////////////// + /// \brief Expose C++ API through the visitor. + /////////////////////////////////////////////////////////////////////////////// + + template + class PyTransmissionVisitorImpl + { + public: + using TTransmission = typename PyClass::wrapped_type; + + static void visit(PyClass & cl) + { + cl + .def("initialize", &TTransmission::initialize) + ; + } + }; + + public: + template + void visit(PyClass & cl) const + { + PyTransmissionVisitorImpl::visit(cl); + } + + /////////////////////////////////////////////////////////////////////////////// + /// \brief Expose. + /////////////////////////////////////////////////////////////////////////////// + static void expose() + { + bp::class_, + std::shared_ptr, + boost::noncopyable>("SimpleTransmission", + bp::init( + bp::args("self", "transmission_name"))) + .def(PySimpleTransmissionVisitor()); + } + }; + + BOOST_PYTHON_VISITOR_EXPOSE(SimpleTransmission) +} // End of namespace python. +} // End of namespace jiminy.