From 3047673991720d956ddb698e73c25b1c851b71ee Mon Sep 17 00:00:00 2001 From: fabians Date: Fri, 29 Oct 2021 14:29:46 +0200 Subject: [PATCH 1/9] init commit transmission --- .../include/jiminy/core/robot/AbstractMotor.h | 141 +++---- .../jiminy/core/robot/AbstractTransmission.h | 260 ++++++++++++ core/include/jiminy/core/robot/BasicMotors.h | 7 +- .../jiminy/core/robot/BasicTransmissions.h | 67 ++++ core/include/jiminy/core/robot/Robot.h | 4 + core/src/robot/AbstractMotor.cc | 124 ++---- core/src/robot/AbstractTransmission.cc | 370 ++++++++++++++++++ core/src/robot/BasicMotors.cc | 32 +- core/src/robot/BasicTransmissions.cc | 112 ++++++ core/src/robot/Robot.cc | 122 ++++++ 10 files changed, 1032 insertions(+), 207 deletions(-) create mode 100644 core/include/jiminy/core/robot/AbstractTransmission.h create mode 100644 core/include/jiminy/core/robot/BasicTransmissions.h create mode 100644 core/src/robot/AbstractTransmission.cc create mode 100644 core/src/robot/BasicTransmissions.cc diff --git a/core/include/jiminy/core/robot/AbstractMotor.h b/core/include/jiminy/core/robot/AbstractMotor.h index 63fbfd789..c978a86c4 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"))), @@ -135,84 +139,65 @@ namespace jiminy configHolder_t getOptions(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get the actual effort of the motor at the current time. + /// \brief Get the actual position of the motor at the current time. /////////////////////////////////////////////////////////////////////////////////////////////// - float64_t const & get(void) const; + float64_t const & getPosition(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get the actual effort of all the motors at the current time. + /// \brief Get the actual velocity of the motor at the current time. /////////////////////////////////////////////////////////////////////////////////////////////// - vectorN_t const & getAll(void) const; + float64_t const & getVelocity(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Set the configuration options of the motor. - /// - /// \param[in] motorOptions Dictionary with the parameters of the motor - /////////////////////////////////////////////////////////////////////////////////////////////// - virtual hresult_t setOptions(configHolder_t const & motorOptions); - - /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Set the same configuration options for every motors. - /// - /// \param[in] motorOptions Dictionary with the parameters used for any motor + /// \brief Get the actual acc of the motor at the current time. /////////////////////////////////////////////////////////////////////////////////////////////// - hresult_t setOptionsAll(configHolder_t const & motorOptions); + float64_t const & getAcceleration(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get isInitialized_. - /// - /// \details It is a flag used to determine if the motor has been initialized. + /// \brief Get the actual effort of the motor at the current time. /////////////////////////////////////////////////////////////////////////////////////////////// - bool_t const & getIsInitialized(void) const; + float64_t const & getEffort(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get name_. - /// - /// \details It is the name of the motor. + /// \brief Get the actual position of all the motor at the current time. /////////////////////////////////////////////////////////////////////////////////////////////// - std::string const & getName(void) const; + vectorN_t const & getPositionAll(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get motorIdx_. - /// - /// \details It is the index of the motor. + /// \brief Get the actual velocity of all the motor at the current time. /////////////////////////////////////////////////////////////////////////////////////////////// - int32_t const & getIdx(void) const; + vectorN_t const & getVelocityAll(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get jointName_. - /// - /// \details It is the name of the joint associated with the motor. + /// \brief Get the actual acc of all the motor at the current time. /////////////////////////////////////////////////////////////////////////////////////////////// - std::string const & getJointName(void) const; + vectorN_t const & getAccelerationAll(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 all the motor at the current time. /////////////////////////////////////////////////////////////////////////////////////////////// - jointIndex_t const & getJointModelIdx(void) const; + vectorN_t const & getEffortAll(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get jointType_. + /// \brief Set the configuration options of the motor. /// - /// \details It is the type of joint associated with the motor. + /// \param[in] motorOptions Dictionary with the parameters of the motor /////////////////////////////////////////////////////////////////////////////////////////////// - joint_t const & getJointType(void) const; + virtual hresult_t setOptions(configHolder_t const & motorOptions); /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get jointPositionIdx_. + /// \brief Set the same configuration options for every motors. /// - /// \details It is the index of the joint associated with the motor in the configuration vector. + /// \param[in] motorOptions Dictionary with the parameters used for any motor /////////////////////////////////////////////////////////////////////////////////////////////// - int32_t const & getJointPositionIdx(void) const; + hresult_t setOptionsAll(configHolder_t const & motorOptions); /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get jointVelocityIdx_. + /// \brief Get name_. /// - /// \details It is the index of the joint associated with the motor in the velocity vector. + /// \details It is the name of the motor. /////////////////////////////////////////////////////////////////////////////////////////////// - int32_t const & getJointVelocityIdx(void) const; + std::string const & getName(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get commandLimit_. @@ -234,18 +219,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. @@ -256,39 +233,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 acc + /// 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, + 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,19 +284,13 @@ namespace jiminy protected: configHolder_t motorOptionsHolder_; ///< Dictionary with the parameters of the motor - 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_; + float64_t armature_; private: MotorSharedDataHolder_t * sharedHolder_; ///< Shared data between every motors associated with the robot diff --git a/core/include/jiminy/core/robot/AbstractTransmission.h b/core/include/jiminy/core/robot/AbstractTransmission.h new file mode 100644 index 000000000..e309b124e --- /dev/null +++ b/core/include/jiminy/core/robot/AbstractTransmission.h @@ -0,0 +1,260 @@ +/////////////////////////////////////////////////////////////////////////////////////////////// +/// +/// \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 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 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 Get the actual state of the transmission at the current time. + /////////////////////////////////////////////////////////////////////////////////////////////// + float64_t const & get(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get the actual state of all the transmissions at the current time. + /////////////////////////////////////////////////////////////////////////////////////////////// + vectorN_t const & getAll(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 & getJointName(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get jointModelIdx_. + /// + /// \details It is the index of the joints associated with the transmission in the kinematic tree. + /////////////////////////////////////////////////////////////////////////////////////////////// + std::vectorconst & getJointModelIdx(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get jointType_. + /// + /// \details It is the type of joints associated with the transmission. + /////////////////////////////////////////////////////////////////////////////////////////////// + std::vector const & getJointType(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get jointPositionIdx_. + /// + /// \details It is the index of the joints associated with the transmission in the configuration vector. + /////////////////////////////////////////////////////////////////////////////////////////////// + std::vector const & getJointPositionIdx(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get jointVelocityIdx_. + /// + /// \details It is the index of the joints associated with the transmission in the velocity vector. + /////////////////////////////////////////////////////////////////////////////////////////////// + std::vector const & getJointVelocityIdx(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get motorName_. + /// + /// \details It is the name of the motors associated with the transmission. + /////////////////////////////////////////////////////////////////////////////////////////////// + std::vector const & getMotorName(void) const; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get ActuatedJoints. + /// + /// \details It is a list of joints that are attached to a transmission + /////////////////////////////////////////////////////////////////////////////////////////////// + std::vector const & robot.getActuatedJoints(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) final; + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \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) final; + + 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. + /// + /// \param[in] q Current configuration of the motors. + /// \param[in] v Current velocity of the motors. + /////////////////////////////////////////////////////////////////////////////////////////////// + virtual void computeTransform(Eigen::VectorBlock const & q, + Eigen::VectorBlock const & v) = 0; /* copy on purpose */ + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \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. + /////////////////////////////////////////////////////////////////////////////////////////////// + virtual void computeInverseTransform(Eigen::VectorBlock const & q, + Eigen::VectorBlock const & v) = 0; /* copy on purpose */ + + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Compute energy dissipation in the transmission. + /// + /////////////////////////////////////////////////////////////////////////////////////////////// + virtual void computeEffortTransmission() = 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, + std::function notifyRobot); + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \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::function notifyRobot_; ///< Notify the robot that the configuration of the transmissions have changed + 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_; + std::vector jointPositionIndices_; + std::vector jointVelocityIndices_; + std::vector motorNames_; + 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..10e94d039 100644 --- a/core/include/jiminy/core/robot/BasicMotors.h +++ b/core/include/jiminy/core/robot/BasicMotors.h @@ -56,15 +56,10 @@ 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); - 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, + virtual hresult_t computeEffort(float64_t const & v, float64_t command) final override; private: diff --git a/core/include/jiminy/core/robot/BasicTransmissions.h b/core/include/jiminy/core/robot/BasicTransmissions.h new file mode 100644 index 000000000..401bbc987 --- /dev/null +++ b/core/include/jiminy/core/robot/BasicTransmissions.h @@ -0,0 +1,67 @@ +#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); } + + hresult_t initialize(std::string const & jointName, + std::string const & motorName); + + virtual hresult_t setOptions(configHolder_t const & transmissionOptions) final override; + + private: + virtual float64_t computeTransform(float64_t const & t, + Eigen::VectorBlock q, + float64_t v, + float64_t const & a, + float64_t command) final override; + + virtual float64_t computeInverseTransform(float64_t const & t, + Eigen::VectorBlock q, + float64_t v, + float64_t const & a, + float64_t command) final override; + + 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/Robot.h b/core/include/jiminy/core/robot/Robot.h index 4bdad1b1d..ab203851a 100644 --- a/core/include/jiminy/core/robot/Robot.h +++ b/core/include/jiminy/core/robot/Robot.h @@ -22,6 +22,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 @@ -61,6 +62,7 @@ namespace jiminy hresult_t detachSensor(std::string const & sensorType, std::string const & sensorName); hresult_t detachSensors(std::string const & sensorType = {}); + transmissionsHolder_t const & getTransmissions(void) const; void computeMotorsEfforts(float64_t const & t, vectorN_t const & q, @@ -142,6 +144,7 @@ namespace jiminy bool_t isTelemetryConfigured_; std::shared_ptr telemetryData_; motorsHolder_t motorsHolder_; + transmissionsHolder_t tranmissionsHolder_; sensorsGroupHolder_t sensorsGroupHolder_; std::unordered_map sensorTelemetryOptions_; std::vector motorsNames_; ///< Name of the motors @@ -149,6 +152,7 @@ namespace jiminy std::vector commandFieldnames_; ///< Fieldnames of the command std::vector motorEffortFieldnames_; ///< Fieldnames of the motors effort uint64_t nmotors_; ///< The number of motors + std::vector actuatedJoints_; ///< List of joints attached to a transmission private: std::unique_ptr mutexLocal_; diff --git a/core/src/robot/AbstractMotor.cc b/core/src/robot/AbstractMotor.cc index edd243cb4..6cdec5d48 100644 --- a/core/src/robot/AbstractMotor.cc +++ b/core/src/robot/AbstractMotor.cc @@ -10,17 +10,11 @@ namespace jiminy AbstractMotorBase::AbstractMotorBase(std::string const & name) : baseMotorOptions_(nullptr), motorOptionsHolder_(), - isInitialized_(false), isAttached_(false), robot_(), notifyRobot_(), name_(name), motorIdx_(-1), - jointName_(), - jointModelIdx_(-1), - jointType_(joint_t::NONE), - jointPositionIdx_(-1), - jointVelocityIdx_(-1), commandLimit_(0.0), armature_(0.0), sharedHolder_(nullptr) @@ -216,15 +210,6 @@ namespace jiminy } } - if (returnCode == hresult_t::SUCCESS) - { - if (!isInitialized_) - { - PRINT_ERROR("Motor not initialized. Impossible to refresh proxies."); - returnCode = hresult_t::ERROR_INIT_FAILED; - } - } - if (returnCode == hresult_t::SUCCESS) { if (!robot->getIsInitialized()) @@ -236,33 +221,12 @@ 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; + commandLimit_ = robot->pncModel_.effortLimit[jointVelocityIdx_]; } else { @@ -289,24 +253,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::v(void) + { + return sharedHolder_->velocity_[motorIdx_]; + } + float64_t & AbstractMotorBase::a(void) + { + return sharedHolder_->acceleration_[motorIdx_]; + } + float64_t & AbstractMotorBase::u(void) + { + return sharedHolder_->effort_[motorIdx_]; } - float64_t & AbstractMotorBase::data(void) + float64_t const & AbstractMotorBase::getPosition(void) { - return sharedHolder_->data_[motorIdx_]; + return sharedHolder_->position_[motorIdx_]; } - vectorN_t const & AbstractMotorBase::getAll(void) const + float64_t const & AbstractMotorBase::getVelocity(void); { - return sharedHolder_->data_; + return sharedHolder_->velocity_[motorIdx_]; + } + + float64_t const & AbstractMotorBase::getAcceleration(void); + { + return sharedHolder_->acceleration_[motorIdx_]; + } + + float64_t const & AbstractMotorBase::getEffort(void); + { + return sharedHolder_->effort_[motorIdx_]; } hresult_t AbstractMotorBase::setOptionsAll(configHolder_t const & motorOptions) @@ -346,31 +328,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 +338,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 +354,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..14209c2cc --- /dev/null +++ b/core/src/robot/AbstractTransmission.cc @@ -0,0 +1,370 @@ +#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_(), + notifyRobot_(), + name_(name), + transmissionIdx_(-1), + jointName_(), + jointModelIdx_(-1), + jointType_(joint_t::NONE), + jointPositionIdx_(-1), + jointVelocityIdx_(-1), + motorName_(), + armature_(0.0), + sharedHolder_(nullptr) + { + // Initialize the options + setOptions(getDefaultTransmissionOptions()); + } + + AbstractTransmissionBase::~AbstractTransmissionBase(void) + { + // Detach the transmission before deleting it if necessary + if (isAttached_) + { + detach(); + } + } + + hresult_t AbstractTransmissionBase::attach(std::weak_ptr robot, + std::function notifyRobot) + { + // 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; + } + + // Make sure the joint is not already attached to a transmission + std::vector::iterator transmissionJointsIt = AbstractTransmissionBase::getJointName().begin(); + + for ( ; transmissionJointsIt != AbstractTransmissionBase::getJointName().end(); ++transmissionJointsIt) + std::vector::iterator actuatedJointIt = AbstractTransmissionBase::robot.getActuatedJoints.begin(); + + for ( ; actuatedJointIt != AbstractTransmissionBase::robot.getActuatedJoints.end(); ++actuatedJointIt) + { + if (*transmissionJointsIt == *actuatedJointIt) + { + PRINT_ERROR("Joint already attached to another transmission"); + return hresult_t::ERROR_GENERIC; + } + } + + // Copy references to the robot and shared data + robot_ = robot; + notifyRobot_ = notifyRobot; + + // Update the flag + isAttached_ = true; + + return hresult_t::SUCCESS; + } + + hresult_t AbstractTransmissionBase::detach(void) + { + // Delete the part of the shared memory associated with the transmission + + if (!isAttached_) + { + PRINT_ERROR("Transmission not attached to any robot."); + return hresult_t::ERROR_GENERIC; + } + + // Remove associated col in the global data buffer + if (transmissionIdx_ < sharedHolder_->num_ - 1) + { + int32_t transmissionShift = sharedHolder_->num_ - transmissionIdx_ - 1; + sharedHolder_->data_.segment(transmissionIdx_, transmissionShift) = + sharedHolder_->data_.segment(transmissionIdx_ + 1, transmissionShift).eval(); // eval to avoid aliasing + } + sharedHolder_->data_.conservativeResize(sharedHolder_->num_ - 1); + + // Shift the transmission ids + for (int32_t i = transmissionIdx_ + 1; i < sharedHolder_->num_; ++i) + { + --sharedHolder_->transmissions_[i]->transmissionIdx_; + } + + // Remove the transmission to the shared memory + sharedHolder_->transmissions_.erase(sharedHolder_->transmissions_.begin() + transmissionIdx_); + --sharedHolder_->num_; + + // Clear the references to the robot and shared data + robot_.reset(); + notifyRobot_ = nullptr; + sharedHolder_ = nullptr; + + // Unset the Id + transmissionIdx_ = -1; + + // Update the flag + isAttached_ = false; + + return hresult_t::SUCCESS; + } + + hresult_t AbstractTransmissionBase::resetAll(void) + { + // Make sure the transmission is attached to a robot + if (!isAttached_) + { + PRINT_ERROR("Transmission not attached to any robot."); + return hresult_t::ERROR_GENERIC; + } + + // Make sure the robot still exists + if (robot_.expired()) + { + PRINT_ERROR("Robot has been deleted. Impossible to reset the transmissions."); + return hresult_t::ERROR_GENERIC; + } + + // Clear the shared data buffer + sharedHolder_->data_.setZero(); + + // Update transmission scope information + for (AbstractTransmissionBase * transmission : sharedHolder_->transmissions_) + { + // Refresh proxies that are robot-dependent + transmission->refreshProxies(); + } + + 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 armature has changed + bool_t const & enableArmature = boost::get(transmissionOptions.at("enableArmature")); + internalBuffersMustBeUpdated |= (baseTransmissionOptions_->enableArmature != enableArmature); + if (enableArmature) + { + float64_t const & armature = boost::get(transmissionOptions.at("armature")); + internalBuffersMustBeUpdated |= std::abs(armature - baseTransmissionOptions_->armature) > EPS; + } + } + + // 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 (i = 0; i < jointName_.size(); i++) + { + if (returnCode == hresult_t::SUCCESS) + { + returnCode = ::jiminy::getJointModelIdx(robot->pncModel_, jointName_[i], jointModelIdx_[i]); + } + } + for (i = 0; i < jointName_.size(); i++) + { + if (returnCode == hresult_t::SUCCESS) + { + returnCode = getJointTypeFromIdx(robot->pncModel_, jointModelIdx_[i], jointType_[i]); + } + } + + for (i = 0; i < jointName_.size(); i++) + { + if (returnCode == hresult_t::SUCCESS) + { + // Transmissions are only supported for linear and rotary joints + if (jointType_[i] != joint_t::LINEAR && jointType_[i] != joint_t::ROTARY && jointType_[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 (i = 0; i < jointName_.size(); i++) + { + if (returnCode == hresult_t::SUCCESS) + { + ::jiminy::getJointPositionIdx(robot->pncModel_, jointName_[i], jointPositionIdx_[i]); + ::jiminy::getJointVelocityIdx(robot->pncModel_, jointName_[i], jointVelocityIdx_[i]); + + // Get the rotor inertia + if (baseTransmissionOptions_->enableArmature) + { + armature_ = baseTransmissionOptions_->armature; + } + else + { + armature_ = 0.0; + } + + // Propagate the user-defined transmission inertia at Pinocchio model level + if (notifyRobot_) + { + returnCode = notifyRobot_(*this); + } + } + } + + return returnCode; + } + + hresult_t AbstractTransmissionBase::setOptionsAll(configHolder_t const & transmissionOptions) + { + hresult_t returnCode = hresult_t::SUCCESS; + + // Make sure the transmission is attached to a robot + if (!isAttached_) + { + PRINT_ERROR("Transmission not attached to any robot."); + returnCode = hresult_t::ERROR_GENERIC; + } + + for (AbstractTransmissionBase * transmission : sharedHolder_->transmissions_) + { + if (returnCode == hresult_t::SUCCESS) + { + returnCode = transmission->setOptions(transmissionOptions); + } + } + + 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::string const & AbstractTransmissionBase::getJointName(void) const + { + return jointName_; + } + + std::vector const & AbstractTransmissionBase::getJointModelIdx(void) const + { + return jointModelIdx_; + } + + std::vector const & AbstractTransmissionBase::getJointType(void) const + { + return jointType_; + } + + std::vector const & AbstractTransmissionBase::getJointPositionIdx(void) const + { + return jointPositionIdx_; + } + + int32_t const & AbstractTransmissionBase::getJointVelocityIdx(void) const + { + return jointVelocityIdx_; + } + + std::vector::string const & AbstractTransmissionBase::getMotorName(void) const + { + return motorName_; + } + + hresult_t AbstractTransmissionBase::computeForward(float64_t const & t, + vectorN_t & q, + vectorN_t & v, + vectorN_t & a, + vectorN_t & uJoint) + { + // TODO modify q v + } + + hresult_t AbstractTransmissionBase::computeBackward(float64_t const & t, + vectorN_t const & q, + vectorN_t const & v, + vectorN_t const & a, + vectorN_t const & uJoint) + { + // TODO modify the motors + } +} diff --git a/core/src/robot/BasicMotors.cc b/core/src/robot/BasicMotors.cc index be27e536f..6586ee37b 100644 --- a/core/src/robot/BasicMotors.cc +++ b/core/src/robot/BasicMotors.cc @@ -17,23 +17,6 @@ namespace jiminy setOptions(getDefaultMotorOptions()); } - hresult_t SimpleMotor::initialize(std::string const & jointName) - { - hresult_t returnCode = hresult_t::SUCCESS; - - jointName_ = jointName; - isInitialized_ = true; - returnCode = refreshProxies(); - - if (returnCode != hresult_t::SUCCESS) - { - jointName_.clear(); - isInitialized_ = false; - } - - return returnCode; - } - hresult_t SimpleMotor::setOptions(configHolder_t const & motorOptions) { hresult_t returnCode = hresult_t::SUCCESS; @@ -79,25 +62,16 @@ 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 const & v, + 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; + data() = command; /* Add friction to the joints associated with the motor if enable. It is computed on joint side instead of the motor. */ diff --git a/core/src/robot/BasicTransmissions.cc b/core/src/robot/BasicTransmissions.cc new file mode 100644 index 000000000..f9c9f8b00 --- /dev/null +++ b/core/src/robot/BasicTransmissions.cc @@ -0,0 +1,112 @@ +#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(getDefaultTransmissionOptions()); + } + + hresult_t SimpleTransmission::initialize(std::string const & jointName, std::string const & motorName) + { + hresult_t returnCode = hresult_t::SUCCESS; + + jointName_ = jointName; + motorName_ = motorName; + isInitialized_ = true; + returnCode = refreshProxies(); + + if (returnCode != hresult_t::SUCCESS) + { + jointName_.clear(); + motorName_.clear(); + isInitialized_ = false; + } + + return returnCode; + } + + hresult_t SimpleTransmission::setOptions(configHolder_t const & transmissionOptions) + { + hresult_t returnCode = hresult_t::SUCCESS; + + returnCode = AbstractTransmissionBase::setOptions(transmissionOptions); + + // Check if the friction parameters make sense + if (returnCode == hresult_t::SUCCESS) + { + // Make sure the user-defined position limit has the right dimension + if (boost::get(transmissionOptions.at("frictionViscousPositive")) > 0.0) + { + PRINT_ERROR("'frictionViscousPositive' must be negative."); + returnCode = hresult_t::ERROR_BAD_INPUT; + } + if (boost::get(transmissionOptions.at("frictionViscousNegative")) > 0.0) + { + PRINT_ERROR("'frictionViscousNegative' must be negative."); + returnCode = hresult_t::ERROR_BAD_INPUT; + } + if (boost::get(transmissionOptions.at("frictionDryPositive")) > 0.0) + { + PRINT_ERROR("'frictionDryPositive' must be negative."); + returnCode = hresult_t::ERROR_BAD_INPUT; + } + if (boost::get(transmissionOptions.at("frictionDryNegative")) > 0.0) + { + PRINT_ERROR("'frictionDryNegative' must be negative."); + returnCode = hresult_t::ERROR_BAD_INPUT; + } + if (boost::get(transmissionOptions.at("frictionDrySlope")) < 0.0) + { + PRINT_ERROR("'frictionDrySlope' must be positive."); + returnCode = hresult_t::ERROR_BAD_INPUT; + } + } + + if (returnCode == hresult_t::SUCCESS) + { + transmissionOptions_ = std::make_unique(transmissionOptions); + } + + return returnCode; + } + + float64_t SimpleTransmission::computeTransform(float64_t const & /* t */, + Eigen::VectorBlock q, + float64_t v, + float64_t const & /* a */, + float64_t command) + { + if (!isInitialized_) + { + PRINT_ERROR("Transmission not initialized. Impossible to compute actual transmission effort."); + return hresult_t::ERROR_INIT_FAILED; + } + + return transmissionOptions_->mechanicalReduction; + } + + float64_t SimpleTransmission::computeInverseTransform(float64_t const & /* t */, + Eigen::VectorBlock /* q */, + float64_t /* v */, + float64_t const & /* a */, + float64_t /* command */) + { + if (!isInitialized_) + { + PRINT_ERROR("Transmission not initialized. Impossible to compute actual transmission effort."); + return hresult_t::ERROR_INIT_FAILED; + } + return transmissionOptions_->mechanicalReduction; + } +} diff --git a/core/src/robot/Robot.cc b/core/src/robot/Robot.cc index b766f0408..61c2159e1 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,6 +22,7 @@ namespace jiminy isTelemetryConfigured_(false), telemetryData_(nullptr), motorsHolder_(), + tranmissionsHolder_(), sensorsGroupHolder_(), sensorTelemetryOptions_(), motorsNames_(), @@ -28,6 +30,7 @@ namespace jiminy commandFieldnames_(), motorEffortFieldnames_(), nmotors_(0U), + actuatedJoints(), mutexLocal_(std::make_unique()), motorsSharedHolder_(std::make_shared()), sensorsSharedHolder_() @@ -123,6 +126,115 @@ 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) + { + // Define robot notification method, responsible for updating the robot if + // necessary after changing the transmission parameters + auto notifyRobot = [robot_=std::weak_ptr(shared_from_this())](AbstractTransmissionBase & transmissionIn) + { + // Make sure the robot still exists + auto robot = robot_.lock(); + if (!robot) + { + PRINT_ERROR("Robot has been deleted. Impossible to notify transmission update."); + return hresult_t::ERROR_GENERIC; + } + + // TODO Update the list of transmission ? + + return hresult_t::SUCCESS; + }; + + // Attach the transmission + returnCode = transmission->attach(shared_from_this(), + notifyRobot); + } + + if (returnCode == hresult_t::SUCCESS) + { + // Add the transmission to the holder + transmissionsHolder_.push_back(transmission); + + // Refresh the transmissions proxies + refreshTransmissionsProxies(); + } + + 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 + refreshTransmissionsProxies(); + + return hresult_t::SUCCESS; + } + hresult_t Robot::attachMotor(std::shared_ptr motor) { hresult_t returnCode = hresult_t::SUCCESS; @@ -619,6 +731,11 @@ namespace jiminy return motorsHolder_; } + Robot::transmissionsHolder_t const & Robot::getTransmissions(void) const + { + return tranmissionsHolder_; + } + hresult_t Robot::getSensor(std::string const & sensorType, std::string const & sensorName, std::weak_ptr & sensor) const @@ -692,6 +809,11 @@ namespace jiminy return sensorsGroupHolder_; } + Robot::tranmissionssGroupHolder_t const & Robot::getTransmissions(void) const + { + return tranmissionssGroupHolder_; + } + hresult_t Robot::setOptions(configHolder_t const & robotOptions) { hresult_t returnCode = hresult_t::SUCCESS; From 52aae4efc05220555a0d6beb768711fb609a5b8c Mon Sep 17 00:00:00 2001 From: fabians Date: Mon, 8 Nov 2021 17:21:44 +0100 Subject: [PATCH 2/9] update --- .../jiminy/core/robot/AbstractTransmission.h | 30 ++-- .../jiminy/core/robot/BasicTransmissions.h | 14 +- core/include/jiminy/core/robot/Model.h | 13 +- core/include/jiminy/core/robot/Robot.h | 4 + core/src/robot/AbstractMotor.cc | 33 +++- core/src/robot/AbstractTransmission.cc | 154 +++++++++++++----- core/src/robot/BasicTransmissions.cc | 61 ++----- core/src/robot/Model.cc | 1 + core/src/robot/Robot.cc | 83 +++++++--- 9 files changed, 245 insertions(+), 148 deletions(-) diff --git a/core/include/jiminy/core/robot/AbstractTransmission.h b/core/include/jiminy/core/robot/AbstractTransmission.h index e309b124e..7fa784674 100644 --- a/core/include/jiminy/core/robot/AbstractTransmission.h +++ b/core/include/jiminy/core/robot/AbstractTransmission.h @@ -46,6 +46,13 @@ namespace jiminy AbstractTransmissionBase(std::string const & name); virtual ~AbstractTransmissionBase(void); + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Init + /// + /// \remark Init + /////////////////////////////////////////////////////////////////////////////////////////////// + virtual hresult_t initialize(void); + /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Refresh the proxies. /// @@ -121,49 +128,42 @@ namespace jiminy /// /// \details It is the name of the joints associated with the transmission. /////////////////////////////////////////////////////////////////////////////////////////////// - std::vector const & getJointName(void) const; + 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 & getJointModelIdx(void) const; + std::vectorconst & getJointModelIndices(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get jointType_. /// /// \details It is the type of joints associated with the transmission. /////////////////////////////////////////////////////////////////////////////////////////////// - std::vector const & getJointType(void) const; + 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. /////////////////////////////////////////////////////////////////////////////////////////////// - std::vector const & getJointPositionIdx(void) const; + std::vector const & getJointPositionIndices(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get jointVelocityIdx_. /// /// \details It is the index of the joints associated with the transmission in the velocity vector. /////////////////////////////////////////////////////////////////////////////////////////////// - std::vector const & getJointVelocityIdx(void) const; + std::vector const & getJointVelocityIndices(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get motorName_. /// /// \details It is the name of the motors associated with the transmission. /////////////////////////////////////////////////////////////////////////////////////////////// - std::vector const & getMotorName(void) const; - - /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get ActuatedJoints. - /// - /// \details It is a list of joints that are attached to a transmission - /////////////////////////////////////////////////////////////////////////////////////////////// - std::vector const & robot.getActuatedJoints(void) const; + std::vector const & getMotorNames(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Compute forward transmission. @@ -247,8 +247,8 @@ namespace jiminy std::vector jointNames_; std::vector jointModelIndices_; std::vector jointTypes_; - std::vector jointPositionIndices_; - std::vector jointVelocityIndices_; + vectorN_t jointPositionIndices_; + vectorN_t jointVelocityIndices_; std::vector motorNames_; std::vector > motors_; matrixN_t forwardTransform_; diff --git a/core/include/jiminy/core/robot/BasicTransmissions.h b/core/include/jiminy/core/robot/BasicTransmissions.h index 401bbc987..b335f566e 100644 --- a/core/include/jiminy/core/robot/BasicTransmissions.h +++ b/core/include/jiminy/core/robot/BasicTransmissions.h @@ -47,17 +47,11 @@ namespace jiminy virtual hresult_t setOptions(configHolder_t const & transmissionOptions) final override; private: - virtual float64_t computeTransform(float64_t const & t, - Eigen::VectorBlock q, - float64_t v, - float64_t const & a, - float64_t command) final override; + virtual float64_t computeTransform(Eigen::VectorBlock /* q */, + Eigen::VectorBlock /* v */) final override; - virtual float64_t computeInverseTransform(float64_t const & t, - Eigen::VectorBlock q, - float64_t v, - float64_t const & a, - float64_t command) final override; + virtual float64_t computeInverseTransform(Eigen::VectorBlock /* q */, + Eigen::VectorBlock /* v */) final override; private: std::unique_ptr transmissionOptions_; 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 fe1ee1117..03185ec65 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; @@ -54,6 +55,7 @@ namespace jiminy motorsHolder_t const & getMotors(void) const; hresult_t detachMotor(std::string const & motorName); hresult_t detachMotors(std::vector const & motorsNames = {}); + 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, @@ -125,6 +127,8 @@ 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 & getActuatedJoints(void) const; + hresult_t updateActuatedJoints(std::vector const & jointNames); vectorN_t const & getCommandLimit(void) const; vectorN_t const & getArmatures(void) const; diff --git a/core/src/robot/AbstractMotor.cc b/core/src/robot/AbstractMotor.cc index 6cdec5d48..1d4b4ec83 100644 --- a/core/src/robot/AbstractMotor.cc +++ b/core/src/robot/AbstractMotor.cc @@ -10,6 +10,7 @@ namespace jiminy AbstractMotorBase::AbstractMotorBase(std::string const & name) : baseMotorOptions_(nullptr), motorOptionsHolder_(), + isInitialized_(false), isAttached_(false), robot_(), notifyRobot_(), @@ -59,9 +60,14 @@ namespace jiminy 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(); - + sharedHolder_->position_.conservativeResize(sharedHolder_->num_ + 1); + sharedHolder_->position_.tail<1>().setZero(); + sharedHolder_->velocity_.conservativeResize(sharedHolder_->num_ + 1); + sharedHolder_->velocity_.tail<1>().setZero(); + sharedHolder_->acceleration_.conservativeResize(sharedHolder_->num_ + 1); + sharedHolder_->acceleration_.tail<1>().setZero(); + sharedHolder_->effort_.conservativeResize(sharedHolder_->num_ + 1); + sharedHolder_->position_.tail<1>().setZero(); // Add the motor to the shared memory sharedHolder_->motors_.push_back(this); ++sharedHolder_->num_; @@ -86,10 +92,20 @@ namespace jiminy if (motorIdx_ < sharedHolder_->num_ - 1) { int32_t motorShift = sharedHolder_->num_ - motorIdx_ - 1; - sharedHolder_->data_.segment(motorIdx_, motorShift) = - sharedHolder_->data_.segment(motorIdx_ + 1, motorShift).eval(); // eval to avoid aliasing + sharedHolder_->position_.segment(motorIdx_, motorShift) = + sharedHolder_->position_.segment(motorIdx_ + 1, motorShift).eval(); // eval to avoid aliasing + sharedHolder_->velocity_.segment(motorIdx_, motorShift) = + sharedHolder_->velocity_.segment(motorIdx_ + 1, motorShift).eval(); // eval to avoid aliasing + sharedHolder_->acceleration_.segment(motorIdx_, motorShift) = + sharedHolder_->acceleration_.segment(motorIdx_ + 1, motorShift).eval(); // eval to avoid aliasing + sharedHolder_->effort_.segment(motorIdx_, motorShift) = + sharedHolder_->effort_.segment(motorIdx_ + 1, motorShift).eval(); // eval to avoid aliasing + } - sharedHolder_->data_.conservativeResize(sharedHolder_->num_ - 1); + sharedHolder_->position_.conservativeResize(sharedHolder_->num_ - 1); + sharedHolder_->velocity_.conservativeResize(sharedHolder_->num_ - 1); + sharedHolder_->acceleration_.conservativeResize(sharedHolder_->num_ - 1); + sharedHolder_->effort_.conservativeResize(sharedHolder_->num_ - 1); // Shift the motor ids for (int32_t i = motorIdx_ + 1; i < sharedHolder_->num_; ++i) @@ -132,7 +148,10 @@ namespace jiminy } // Clear the shared data buffer - sharedHolder_->data_.setZero(); + sharedHolder_->position_.setZero(); + sharedHolder_->velocity_.setZero(); + sharedHolder_->acceleration_.setZero(); + sharedHolder_->effort_.setZero(); // Update motor scope information for (AbstractMotorBase * motor : sharedHolder_->motors_) diff --git a/core/src/robot/AbstractTransmission.cc b/core/src/robot/AbstractTransmission.cc index 14209c2cc..1fe7c3d51 100644 --- a/core/src/robot/AbstractTransmission.cc +++ b/core/src/robot/AbstractTransmission.cc @@ -16,14 +16,12 @@ namespace jiminy notifyRobot_(), name_(name), transmissionIdx_(-1), - jointName_(), - jointModelIdx_(-1), - jointType_(joint_t::NONE), - jointPositionIdx_(-1), - jointVelocityIdx_(-1), - motorName_(), - armature_(0.0), - sharedHolder_(nullptr) + jointNames_(), + jointModelIndices_(-1), + jointTypes_(joint_t::NONE), + jointPositionIndices_(-1), + jointVelocityIndices_(-1), + motorNames_(), { // Initialize the options setOptions(getDefaultTransmissionOptions()); @@ -38,8 +36,56 @@ namespace jiminy } } - hresult_t AbstractTransmissionBase::attach(std::weak_ptr robot, - std::function notifyRobot) + hresult_t AbstractTransmissionBase::initialize(void) + { + // Populate jointPositionIndices_ + std::vector jointPositionIndices; + returnCode = hresult_t::SUCCESS; + for (std::string const & jointName : jointNames_) + { + std::vector jointPositionIdx; + if (!robot->model.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->model, jointName, jointPositionIdx); + } + if (returnCode == hresult_t::SUCCESS) + { + jointPositionIndices.insert(jointPositionIndices.end(), jointPositionIdx.begin(), jointPositionIdx.end()); + } + } + 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->model.existJointName(jointName)) + { + PRINT_ERROR("Joint '", jointName, "' not found in robot model."); + return hresult_t::ERROR_BAD_INPUT; + } + jointIndex_t const & jointModelIdx = robot->model.getJointId(jointName); + int32_t const & jointVelocityFirstIdx = robot->model.joints[jointModelIdx].idx_v(); + int32_t const & jointNv = robot->model.joints[jointModelIdx].nv(); + jointVelocityIdx.resize(jointNv); + std::iota(jointVelocityIdx.begin(), jointVelocityIdx.end(), jointVelocityFirstIdx) + jointVelocityIndices.insert(jointVelocityIndices.end(), jointVelocityIdx.begin(), jointVelocityIdx.end()); + } + } + + hresult_t AbstractTransmissionBase::attach(std::weak_ptr robot) { // Make sure the transmission is not already attached if (isAttached_) @@ -56,23 +102,22 @@ namespace jiminy } // Make sure the joint is not already attached to a transmission - std::vector::iterator transmissionJointsIt = AbstractTransmissionBase::getJointName().begin(); - - for ( ; transmissionJointsIt != AbstractTransmissionBase::getJointName().end(); ++transmissionJointsIt) - std::vector::iterator actuatedJointIt = AbstractTransmissionBase::robot.getActuatedJoints.begin(); - - for ( ; actuatedJointIt != AbstractTransmissionBase::robot.getActuatedJoints.end(); ++actuatedJointIt) + std_vector actuatedJoints = robot->getActuatedJoints() + for (std::string const & transmissionJoint : getJointNames()) + { + auto transmissionJointIt = actuatedJoints.find(transmissionJoint); + if (transmissionJointIt != actuatedJoints.end()) { - if (*transmissionJointsIt == *actuatedJointIt) - { - PRINT_ERROR("Joint already attached to another transmission"); - return hresult_t::ERROR_GENERIC; - } + PRINT_ERROR("Joint already attached to another transmission"); + return hresult_t::ERROR_GENERIC; } + } // Copy references to the robot and shared data robot_ = robot; - notifyRobot_ = notifyRobot; + + // Update the actuated joints + robot_->updateActuatedJoints(jointNames_) // Update the flag isAttached_ = true; @@ -111,7 +156,6 @@ namespace jiminy // Clear the references to the robot and shared data robot_.reset(); - notifyRobot_ = nullptr; sharedHolder_ = nullptr; // Unset the Id @@ -158,12 +202,12 @@ namespace jiminy bool_t internalBuffersMustBeUpdated = false; if (isInitialized_) { - // Check if armature has changed - bool_t const & enableArmature = boost::get(transmissionOptions.at("enableArmature")); - internalBuffersMustBeUpdated |= (baseTransmissionOptions_->enableArmature != enableArmature); - if (enableArmature) + // Check if reduction ratio has changed + float64_t const & mechanicalReduction = boost::get(transmissionOptions.at("mechanicalReduction")); + internalBuffersMustBeUpdated |= (baseTransmissionOptions_->mechanicalReduction != mechanicalReduction); + if (mechanicalReduction) { - float64_t const & armature = boost::get(transmissionOptions.at("armature")); + float64_t const & mechanicalReduction = boost::get(transmissionOptions.at("mechanicalReduction")); internalBuffersMustBeUpdated |= std::abs(armature - baseTransmissionOptions_->armature) > EPS; } } @@ -320,32 +364,50 @@ namespace jiminy return transmissionIdx_; } - std::vector::string const & AbstractTransmissionBase::getJointName(void) const + std::vector::string const & AbstractTransmissionBase::getJointNames(void) const { - return jointName_; + return jointNames_; } - std::vector const & AbstractTransmissionBase::getJointModelIdx(void) const + std::vector const & AbstractTransmissionBase::getJointModelIndices(void) const { - return jointModelIdx_; + jointIndex_t jointModelIdx; + for (std::string const & jointName : jointNames_) + { + returnCode = ::jiminy::getJointModelIdx(robot->pncModel_, jointName, jointModelIdx); + if (returnCode == hresult_t::SUCCESS) + { + jointModelIndices_.push_back(jointModelIdx); + } + } + return jointModelIndices_; } - std::vector const & AbstractTransmissionBase::getJointType(void) const + std::vector const & AbstractTransmissionBase::getJointTypes(void) const { - return jointType_; + jointModelIndices = getJointModelIndices(); + for (jointIndex_t const & idx : jointModelIndices) + { + joint_t jointType; + getJointTypeFromIdx(robot->pncModel, idx, jointType); + jointTypes_.push_back(jointType); + } + return jointTypes_; } - std::vector const & AbstractTransmissionBase::getJointPositionIdx(void) const + vectorN_t const & AbstractTransmissionBase::getJointPositionIndices(void) { - return jointPositionIdx_; + return jointPositionIndices_; } - int32_t const & AbstractTransmissionBase::getJointVelocityIdx(void) const + + vectorN_t const & AbstractTransmissionBase::getJointVelocityIndices(void) { - return jointVelocityIdx_; + + return jointVelocityIndices_; } - std::vector::string const & AbstractTransmissionBase::getMotorName(void) const + std::vector::string const & AbstractTransmissionBase::getMotorNames(void) const { return motorName_; } @@ -356,7 +418,13 @@ namespace jiminy vectorN_t & a, vectorN_t & uJoint) { - // TODO modify q v + auto qMotors = q.segment<>(jointPositionIdx_, ); + auto vMotors = v.segment<>(jointVelocityIdx_, ); + computeTransform(qMotors, vMotors); + q.noalias() = forwardTransform_ * motors_->getPosition(); + v.noalias() = forwardTransform_ * motors_->getVelocity(); + a.noalias() = forwardTransform_ * motors_->getAcceleration(); + uJoint.noalias() = forwardTransform_ * motors_->getEffort(); } hresult_t AbstractTransmissionBase::computeBackward(float64_t const & t, @@ -365,6 +433,10 @@ namespace jiminy vectorN_t const & a, vectorN_t const & uJoint) { - // TODO modify the motors + computeInverseTransform(q, v); + motors_->q = backwardTransform_ * q; + motors_->v = backwardTransform_ * v; + motors_->a = backwardTransform_ * a; + motors_->u = backwardTransform_ * uJoint; } } diff --git a/core/src/robot/BasicTransmissions.cc b/core/src/robot/BasicTransmissions.cc index f9c9f8b00..26283a5f1 100644 --- a/core/src/robot/BasicTransmissions.cc +++ b/core/src/robot/BasicTransmissions.cc @@ -17,19 +17,21 @@ namespace jiminy setOptions(getDefaultTransmissionOptions()); } - hresult_t SimpleTransmission::initialize(std::string const & jointName, std::string const & motorName) + hresult_t SimpleTransmission::initialize(std::string const & jointNames, std::string const & motorNames) { hresult_t returnCode = hresult_t::SUCCESS; - jointName_ = jointName; - motorName_ = motorName; + jointNames_ = jointNames; + motorNames_ = motorNames; isInitialized_ = true; - returnCode = refreshProxies(); + AbstractTransmissionBase::initialize() + + returnCode = refreshProxies(); if (returnCode != hresult_t::SUCCESS) { - jointName_.clear(); - motorName_.clear(); + jointNames_.clear(); + motorNames_.clear(); isInitialized_ = false; } @@ -42,37 +44,6 @@ namespace jiminy returnCode = AbstractTransmissionBase::setOptions(transmissionOptions); - // Check if the friction parameters make sense - if (returnCode == hresult_t::SUCCESS) - { - // Make sure the user-defined position limit has the right dimension - if (boost::get(transmissionOptions.at("frictionViscousPositive")) > 0.0) - { - PRINT_ERROR("'frictionViscousPositive' must be negative."); - returnCode = hresult_t::ERROR_BAD_INPUT; - } - if (boost::get(transmissionOptions.at("frictionViscousNegative")) > 0.0) - { - PRINT_ERROR("'frictionViscousNegative' must be negative."); - returnCode = hresult_t::ERROR_BAD_INPUT; - } - if (boost::get(transmissionOptions.at("frictionDryPositive")) > 0.0) - { - PRINT_ERROR("'frictionDryPositive' must be negative."); - returnCode = hresult_t::ERROR_BAD_INPUT; - } - if (boost::get(transmissionOptions.at("frictionDryNegative")) > 0.0) - { - PRINT_ERROR("'frictionDryNegative' must be negative."); - returnCode = hresult_t::ERROR_BAD_INPUT; - } - if (boost::get(transmissionOptions.at("frictionDrySlope")) < 0.0) - { - PRINT_ERROR("'frictionDrySlope' must be positive."); - returnCode = hresult_t::ERROR_BAD_INPUT; - } - } - if (returnCode == hresult_t::SUCCESS) { transmissionOptions_ = std::make_unique(transmissionOptions); @@ -81,11 +52,8 @@ namespace jiminy return returnCode; } - float64_t SimpleTransmission::computeTransform(float64_t const & /* t */, - Eigen::VectorBlock q, - float64_t v, - float64_t const & /* a */, - float64_t command) + float64_t SimpleTransmission::computeTransform(Eigen::VectorBlock /* q */, + Eigen::VectorBlock /* v */)) { if (!isInitialized_) { @@ -96,17 +64,14 @@ namespace jiminy return transmissionOptions_->mechanicalReduction; } - float64_t SimpleTransmission::computeInverseTransform(float64_t const & /* t */, - Eigen::VectorBlock /* q */, - float64_t /* v */, - float64_t const & /* a */, - float64_t /* command */) + float64_t SimpleTransmission::computeInverseTransform(Eigen::VectorBlock /* q */, + Eigen::VectorBlock /* v */)) { if (!isInitialized_) { PRINT_ERROR("Transmission not initialized. Impossible to compute actual transmission effort."); return hresult_t::ERROR_INIT_FAILED; } - return transmissionOptions_->mechanicalReduction; + return transmissionOptions_-> 1.0 / mechanicalReduction; } } diff --git a/core/src/robot/Model.cc b/core/src/robot/Model.cc index ba4dee805..c2b66d2e0 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() { diff --git a/core/src/robot/Robot.cc b/core/src/robot/Robot.cc index e459fce2f..98b0c2eff 100644 --- a/core/src/robot/Robot.cc +++ b/core/src/robot/Robot.cc @@ -30,7 +30,7 @@ namespace jiminy commandFieldnames_(), motorEffortFieldnames_(), nmotors_(0U), - actuatedJoints(), + actuatedJoints_(), mutexLocal_(std::make_unique()), motorsSharedHolder_(std::make_shared()), sensorsSharedHolder_() @@ -40,18 +40,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. */ @@ -176,26 +178,8 @@ namespace jiminy if (returnCode == hresult_t::SUCCESS) { - // Define robot notification method, responsible for updating the robot if - // necessary after changing the transmission parameters - auto notifyRobot = [robot_=std::weak_ptr(shared_from_this())](AbstractTransmissionBase & transmissionIn) - { - // Make sure the robot still exists - auto robot = robot_.lock(); - if (!robot) - { - PRINT_ERROR("Robot has been deleted. Impossible to notify transmission update."); - return hresult_t::ERROR_GENERIC; - } - - // TODO Update the list of transmission ? - - return hresult_t::SUCCESS; - }; - // Attach the transmission - returnCode = transmission->attach(shared_from_this(), - notifyRobot); + returnCode = transmission->attach(shared_from_this()); } if (returnCode == hresult_t::SUCCESS) @@ -409,6 +393,51 @@ 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 + 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. @@ -1338,6 +1367,12 @@ namespace jiminy return motorEffortEmpty; } + hresult_t Robot::updateActuatedJoints(std::vector const & jointNames) + { + actuatedJoints_.insert(actuatedJoints_.end(), jointNames.begin(), jointNames.end()); + return hresult_t::SUCCESS; + } + void Robot::setSensorsData(float64_t const & t, vectorN_t const & q, vectorN_t const & v, @@ -1368,7 +1403,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()); @@ -1529,7 +1564,7 @@ namespace jiminy vectorN_t const & Robot::getArmatures(void) const { - static vectorN_t armatures; + static vectorN_t armatures_; armatures.resize(pncModel_.nv); armatures.setZero(); From 500b5699d5c0fc1abb91d1c1c762e9604565c9e1 Mon Sep 17 00:00:00 2001 From: fabians Date: Tue, 9 Nov 2021 14:09:47 +0100 Subject: [PATCH 3/9] update --- .../include/jiminy/core/robot/AbstractMotor.h | 51 ++++++++-------- .../jiminy/core/robot/AbstractTransmission.h | 32 ++++------ .../jiminy/core/robot/BasicTransmissions.h | 3 - core/include/jiminy/core/robot/Model.h | 4 +- core/include/jiminy/core/robot/Robot.h | 11 ++-- core/src/robot/AbstractMotor.cc | 60 +++++++++--------- core/src/robot/AbstractTransmission.cc | 61 +++++++++++-------- core/src/robot/BasicTransmissions.cc | 39 +----------- core/src/robot/Robot.cc | 21 +++---- 9 files changed, 124 insertions(+), 158 deletions(-) diff --git a/core/include/jiminy/core/robot/AbstractMotor.h b/core/include/jiminy/core/robot/AbstractMotor.h index ffd291716..a8ea8b465 100644 --- a/core/include/jiminy/core/robot/AbstractMotor.h +++ b/core/include/jiminy/core/robot/AbstractMotor.h @@ -135,6 +135,20 @@ namespace jiminy /////////////////////////////////////////////////////////////////////////////////////////////// configHolder_t getOptions(void) const; + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Set the configuration options of the motor. + /// + /// \param[in] motorOptions Dictionary with the parameters of the motor + /////////////////////////////////////////////////////////////////////////////////////////////// + virtual hresult_t setOptions(configHolder_t const & motorOptions); + + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Set the same configuration options for every motors. + /// + /// \param[in] motorOptions Dictionary with the parameters used for any motor + /////////////////////////////////////////////////////////////////////////////////////////////// + hresult_t setOptionsAll(configHolder_t const & motorOptions); + /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get the actual position of the motor at the current time. /////////////////////////////////////////////////////////////////////////////////////////////// @@ -155,6 +169,13 @@ namespace jiminy /////////////////////////////////////////////////////////////////////////////////////////////// float64_t const & getEffort(void) const; + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get name_. + /// + /// \details It is the name of the motor. + /////////////////////////////////////////////////////////////////////////////////////////////// + std::string const & getName(void) const; + /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get the actual position of all the motor at the current time. /////////////////////////////////////////////////////////////////////////////////////////////// @@ -175,27 +196,6 @@ namespace jiminy /////////////////////////////////////////////////////////////////////////////////////////////// vectorN_t const & getEffortAll(void) const; - /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Set the configuration options of the motor. - /// - /// \param[in] motorOptions Dictionary with the parameters of the motor - /////////////////////////////////////////////////////////////////////////////////////////////// - virtual hresult_t setOptions(configHolder_t const & motorOptions); - - /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Set the same configuration options for every motors. - /// - /// \param[in] motorOptions Dictionary with the parameters used for any motor - /////////////////////////////////////////////////////////////////////////////////////////////// - hresult_t setOptionsAll(configHolder_t const & motorOptions); - - /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get name_. - /// - /// \details It is the name of the motor. - /////////////////////////////////////////////////////////////////////////////////////////////// - std::string const & getName(void) const; - /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get commandLimit_. /// @@ -250,7 +250,7 @@ namespace jiminy float64_t & v(void); /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get a reference to the last data buffer corresponding to the actual acc + /// \brief Get a reference to the last data buffer corresponding to the actual acceleration /// of the motor. /////////////////////////////////////////////////////////////////////////////////////////////// float64_t & a(void); @@ -259,7 +259,7 @@ namespace jiminy /// \brief Get a reference to the last data buffer corresponding to the actual effort /// of the motor. /////////////////////////////////////////////////////////////////////////////////////////////// - float64_t & u(void); + float64_t & u(void); private: /////////////////////////////////////////////////////////////////////////////////////////////// @@ -268,7 +268,7 @@ namespace jiminy /// \details This method must be called before initializing the motor. /////////////////////////////////////////////////////////////////////////////////////////////// hresult_t attach(std::weak_ptr robot, - std::function notifyRobot, + std::function notifyRobot, MotorSharedDataHolder_t * sharedHolder); /////////////////////////////////////////////////////////////////////////////////////////////// @@ -281,13 +281,14 @@ namespace jiminy protected: configHolder_t motorOptionsHolder_; ///< Dictionary with the parameters of the motor + 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 motors have changed std::string name_; ///< Name of the motor int32_t motorIdx_; ///< Index of the motor in the measurement buffer float64_t commandLimit_; - float64_t armature_; + float64_t armature_; private: MotorSharedDataHolder_t * sharedHolder_; ///< Shared data between every motors associated with the robot diff --git a/core/include/jiminy/core/robot/AbstractTransmission.h b/core/include/jiminy/core/robot/AbstractTransmission.h index 7fa784674..00e52d74e 100644 --- a/core/include/jiminy/core/robot/AbstractTransmission.h +++ b/core/include/jiminy/core/robot/AbstractTransmission.h @@ -47,11 +47,12 @@ namespace jiminy virtual ~AbstractTransmissionBase(void); /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Init + /// \brief Initialize /// - /// \remark Init + /// \remark Initialize the transmission with the names of connected motors and actuated joits. /////////////////////////////////////////////////////////////////////////////////////////////// - virtual hresult_t initialize(void); + virtual hresult_t initialize(std::vector const & jointName, + std::vector const & motorName); /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Refresh the proxies. @@ -83,11 +84,6 @@ namespace jiminy /////////////////////////////////////////////////////////////////////////////////////////////// float64_t const & get(void) const; - /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get the actual state of all the transmissions at the current time. - /////////////////////////////////////////////////////////////////////////////////////////////// - vectorN_t const & getAll(void) const; - /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Set the configuration options of the transmission. /// @@ -164,24 +160,24 @@ namespace jiminy /// \details It is the name of the motors associated with the transmission. /////////////////////////////////////////////////////////////////////////////////////////////// std::vector const & getMotorNames(void) const; - + /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Compute forward transmission. /// /// \details Compute forward transmission from motor to joint. /////////////////////////////////////////////////////////////////////////////////////////////// - hresult_t computeForward(float64_t const & t, + virtual hresult_t computeForward(float64_t const & t, vectorN_t & q, vectorN_t & v, vectorN_t & a, vectorN_t & uJoint) final; - + /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Compute backward transmission. /// /// \details Compute backward transmission from joint to motor. - /////////////////////////////////////////////////////////////////////////////////////////////// - hresult_t computeBackward(float64_t const & t, + /////////////////////////////////////////////////////////////////////////////////////////////// + virtual hresult_t computeBackward(float64_t const & t, vectorN_t const & q, vectorN_t const & v, vectorN_t const & a, @@ -212,12 +208,12 @@ namespace jiminy virtual void computeInverseTransform(Eigen::VectorBlock const & q, Eigen::VectorBlock const & v) = 0; /* copy on purpose */ - + /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Compute energy dissipation in the transmission. /// /////////////////////////////////////////////////////////////////////////////////////////////// - virtual void computeEffortTransmission() = 0; + virtual computeEffortTransmission(void) = 0; private: /////////////////////////////////////////////////////////////////////////////////////////////// @@ -225,17 +221,13 @@ namespace jiminy /// /// \details This method must be called before initializing the transmission. /////////////////////////////////////////////////////////////////////////////////////////////// - hresult_t attach(std::weak_ptr robot, - std::function notifyRobot); + 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 diff --git a/core/include/jiminy/core/robot/BasicTransmissions.h b/core/include/jiminy/core/robot/BasicTransmissions.h index b335f566e..95732da67 100644 --- a/core/include/jiminy/core/robot/BasicTransmissions.h +++ b/core/include/jiminy/core/robot/BasicTransmissions.h @@ -41,9 +41,6 @@ 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, - std::string const & motorName); - virtual hresult_t setOptions(configHolder_t const & transmissionOptions) final override; private: diff --git a/core/include/jiminy/core/robot/Model.h b/core/include/jiminy/core/robot/Model.h index 13bd47341..e2d7acd76 100644 --- a/core/include/jiminy/core/robot/Model.h +++ b/core/include/jiminy/core/robot/Model.h @@ -27,8 +27,8 @@ namespace jiminy BOUNDS_JOINTS = 0, CONTACT_FRAMES = 1, COLLISION_BODIES = 2, - USER = 3, - TRANSMISSIONS = 4 + TRANSMISSIONS = 3, + USER = 4 }; std::array const constraintsHolderTypeRange = {{ diff --git a/core/include/jiminy/core/robot/Robot.h b/core/include/jiminy/core/robot/Robot.h index 03185ec65..470c8b48d 100644 --- a/core/include/jiminy/core/robot/Robot.h +++ b/core/include/jiminy/core/robot/Robot.h @@ -55,6 +55,7 @@ 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, @@ -67,7 +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, @@ -127,8 +131,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 & getActuatedJoints(void) const; - hresult_t updateActuatedJoints(std::vector const & jointNames); + std::vector const & getActuatedJointNames(void) const; vectorN_t const & getCommandLimit(void) const; vectorN_t const & getArmatures(void) const; @@ -151,7 +154,7 @@ namespace jiminy bool_t isTelemetryConfigured_; std::shared_ptr telemetryData_; motorsHolder_t motorsHolder_; - transmissionsHolder_t tranmissionsHolder_; + transmissionsHolder_t transmissionsHolder_; sensorsGroupHolder_t sensorsGroupHolder_; std::unordered_map sensorTelemetryOptions_; std::vector motorsNames_; ///< Name of the motors @@ -159,7 +162,7 @@ namespace jiminy std::vector commandFieldnames_; ///< Fieldnames of the command std::vector motorEffortFieldnames_; ///< Fieldnames of the motors effort uint64_t nmotors_; ///< The number of motors - std::vector actuatedJoints_; ///< List of joints attached to a transmission + std::vector actuatedJointNames_; ///< List of joints attached to a transmission private: std::unique_ptr mutexLocal_; diff --git a/core/src/robot/AbstractMotor.cc b/core/src/robot/AbstractMotor.cc index 1d4b4ec83..ac52c30a9 100644 --- a/core/src/robot/AbstractMotor.cc +++ b/core/src/robot/AbstractMotor.cc @@ -59,15 +59,16 @@ namespace jiminy // Get an index motorIdx_ = sharedHolder_->num_; - // Add a value for the motor to the shared data buffer - sharedHolder_->position_.conservativeResize(sharedHolder_->num_ + 1); - sharedHolder_->position_.tail<1>().setZero(); - sharedHolder_->velocity_.conservativeResize(sharedHolder_->num_ + 1); - sharedHolder_->velocity_.tail<1>().setZero(); - sharedHolder_->acceleration_.conservativeResize(sharedHolder_->num_ + 1); - sharedHolder_->acceleration_.tail<1>().setZero(); - sharedHolder_->effort_.conservativeResize(sharedHolder_->num_ + 1); - sharedHolder_->position_.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,24 +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_->position_.segment(motorIdx_, motorShift) = - sharedHolder_->position_.segment(motorIdx_ + 1, motorShift).eval(); // eval to avoid aliasing - sharedHolder_->velocity_.segment(motorIdx_, motorShift) = - sharedHolder_->velocity_.segment(motorIdx_ + 1, motorShift).eval(); // eval to avoid aliasing - sharedHolder_->acceleration_.segment(motorIdx_, motorShift) = - sharedHolder_->acceleration_.segment(motorIdx_ + 1, motorShift).eval(); // eval to avoid aliasing - sharedHolder_->effort_.segment(motorIdx_, motorShift) = - sharedHolder_->effort_.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_->position_.conservativeResize(sharedHolder_->num_ - 1); - sharedHolder_->velocity_.conservativeResize(sharedHolder_->num_ - 1); - sharedHolder_->acceleration_.conservativeResize(sharedHolder_->num_ - 1); - sharedHolder_->effort_.conservativeResize(sharedHolder_->num_ - 1); // Shift the motor ids for (int32_t i = motorIdx_ + 1; i < sharedHolder_->num_; ++i) @@ -148,10 +146,14 @@ namespace jiminy } // Clear the shared data buffer - sharedHolder_->position_.setZero(); - sharedHolder_->velocity_.setZero(); - sharedHolder_->acceleration_.setZero(); - sharedHolder_->effort_.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_) diff --git a/core/src/robot/AbstractTransmission.cc b/core/src/robot/AbstractTransmission.cc index 1fe7c3d51..6bbccf1c8 100644 --- a/core/src/robot/AbstractTransmission.cc +++ b/core/src/robot/AbstractTransmission.cc @@ -8,7 +8,6 @@ namespace jiminy { AbstractTransmissionBase::AbstractTransmissionBase(std::string const & name) : - baseTransmissionOptions_(nullptr), transmissionOptionsHolder_(), isInitialized_(false), isAttached_(false), @@ -18,13 +17,13 @@ namespace jiminy transmissionIdx_(-1), jointNames_(), jointModelIndices_(-1), - jointTypes_(joint_t::NONE), + jointTypes_(), jointPositionIndices_(-1), jointVelocityIndices_(-1), - motorNames_(), + motorNames_() { // Initialize the options - setOptions(getDefaultTransmissionOptions()); + setOptions(transmissionOptionsHolder_); } AbstractTransmissionBase::~AbstractTransmissionBase(void) @@ -36,42 +35,56 @@ namespace jiminy } } - hresult_t AbstractTransmissionBase::initialize(void) + 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; + } + // Populate jointPositionIndices_ std::vector jointPositionIndices; - returnCode = hresult_t::SUCCESS; + hresult_t returnCode = hresult_t::SUCCESS; for (std::string const & jointName : jointNames_) { std::vector jointPositionIdx; - if (!robot->model.existJointName(jointName)) + if (!robot_->model.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->model, jointName, jointPositionIdx); + returnCode = getJointPositionIdx(robot_->model, jointName, jointPositionIdx); } if (returnCode == hresult_t::SUCCESS) { jointPositionIndices.insert(jointPositionIndices.end(), jointPositionIdx.begin(), jointPositionIdx.end()); } } - jointPositionSize = jointPositionIndices.size() + 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->model.existJointName(jointName)) + if (!robot_->model.existJointName(jointName)) { PRINT_ERROR("Joint '", jointName, "' not found in robot model."); return hresult_t::ERROR_BAD_INPUT; @@ -80,9 +93,10 @@ namespace jiminy int32_t const & jointVelocityFirstIdx = robot->model.joints[jointModelIdx].idx_v(); int32_t const & jointNv = robot->model.joints[jointModelIdx].nv(); jointVelocityIdx.resize(jointNv); - std::iota(jointVelocityIdx.begin(), jointVelocityIdx.end(), jointVelocityFirstIdx) + std::iota(jointVelocityIdx.begin(), jointVelocityIdx.end(), jointVelocityFirstIdx); jointVelocityIndices.insert(jointVelocityIndices.end(), jointVelocityIdx.begin(), jointVelocityIdx.end()); } + return returnCode; } hresult_t AbstractTransmissionBase::attach(std::weak_ptr robot) @@ -100,13 +114,13 @@ namespace jiminy PRINT_ERROR("Robot pointer expired or unset."); return hresult_t::ERROR_GENERIC; } - + // Make sure the joint is not already attached to a transmission - std_vector actuatedJoints = robot->getActuatedJoints() + std::vector actuatedJointNames = robot->getActuatedJointNames(); for (std::string const & transmissionJoint : getJointNames()) { - auto transmissionJointIt = actuatedJoints.find(transmissionJoint); - if (transmissionJointIt != actuatedJoints.end()) + auto transmissionJointIt = actuatedJointNames.find(transmissionJoint); + if (transmissionJointIt != actuatedJointNames.end()) { PRINT_ERROR("Joint already attached to another transmission"); return hresult_t::ERROR_GENERIC; @@ -116,9 +130,6 @@ namespace jiminy // Copy references to the robot and shared data robot_ = robot; - // Update the actuated joints - robot_->updateActuatedJoints(jointNames_) - // Update the flag isAttached_ = true; @@ -287,7 +298,7 @@ namespace jiminy } for (i = 0; i < jointName_.size(); i++) - { + { if (returnCode == hresult_t::SUCCESS) { // Transmissions are only supported for linear and rotary joints @@ -300,7 +311,7 @@ namespace jiminy } for (i = 0; i < jointName_.size(); i++) - { + { if (returnCode == hresult_t::SUCCESS) { ::jiminy::getJointPositionIdx(robot->pncModel_, jointName_[i], jointPositionIdx_[i]); @@ -389,13 +400,13 @@ namespace jiminy for (jointIndex_t const & idx : jointModelIndices) { joint_t jointType; - getJointTypeFromIdx(robot->pncModel, idx, jointType); + getJointTypeFromIdx(robot->pncModel, idx, jointType); jointTypes_.push_back(jointType); } return jointTypes_; } - vectorN_t const & AbstractTransmissionBase::getJointPositionIndices(void) + vectorN_t const & AbstractTransmissionBase::getJointPositionIndices(void) { return jointPositionIndices_; } @@ -425,7 +436,7 @@ namespace jiminy v.noalias() = forwardTransform_ * motors_->getVelocity(); a.noalias() = forwardTransform_ * motors_->getAcceleration(); uJoint.noalias() = forwardTransform_ * motors_->getEffort(); - } + } hresult_t AbstractTransmissionBase::computeBackward(float64_t const & t, vectorN_t const & q, @@ -438,5 +449,5 @@ namespace jiminy motors_->v = backwardTransform_ * v; motors_->a = backwardTransform_ * a; motors_->u = backwardTransform_ * uJoint; - } + } } diff --git a/core/src/robot/BasicTransmissions.cc b/core/src/robot/BasicTransmissions.cc index 26283a5f1..73459ce0f 100644 --- a/core/src/robot/BasicTransmissions.cc +++ b/core/src/robot/BasicTransmissions.cc @@ -14,42 +14,7 @@ namespace jiminy /* 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(getDefaultTransmissionOptions()); - } - - hresult_t SimpleTransmission::initialize(std::string const & jointNames, std::string const & motorNames) - { - hresult_t returnCode = hresult_t::SUCCESS; - - jointNames_ = jointNames; - motorNames_ = motorNames; - isInitialized_ = true; - - AbstractTransmissionBase::initialize() - - returnCode = refreshProxies(); - if (returnCode != hresult_t::SUCCESS) - { - jointNames_.clear(); - motorNames_.clear(); - isInitialized_ = false; - } - - return returnCode; - } - - hresult_t SimpleTransmission::setOptions(configHolder_t const & transmissionOptions) - { - hresult_t returnCode = hresult_t::SUCCESS; - - returnCode = AbstractTransmissionBase::setOptions(transmissionOptions); - - if (returnCode == hresult_t::SUCCESS) - { - transmissionOptions_ = std::make_unique(transmissionOptions); - } - - return returnCode; + setOptions(getOptions()); } float64_t SimpleTransmission::computeTransform(Eigen::VectorBlock /* q */, @@ -72,6 +37,6 @@ namespace jiminy PRINT_ERROR("Transmission not initialized. Impossible to compute actual transmission effort."); return hresult_t::ERROR_INIT_FAILED; } - return transmissionOptions_-> 1.0 / mechanicalReduction; + return 1.0 / transmissionOptions_->mechanicalReduction; } } diff --git a/core/src/robot/Robot.cc b/core/src/robot/Robot.cc index 98b0c2eff..80662ca7d 100644 --- a/core/src/robot/Robot.cc +++ b/core/src/robot/Robot.cc @@ -30,7 +30,7 @@ namespace jiminy commandFieldnames_(), motorEffortFieldnames_(), nmotors_(0U), - actuatedJoints_(), + actuatedJointNames_(), mutexLocal_(std::make_unique()), motorsSharedHolder_(std::make_shared()), sensorsSharedHolder_() @@ -67,6 +67,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. */ @@ -191,6 +192,11 @@ namespace jiminy refreshTransmissionsProxies(); } + // update list of actuated joints + std::vector const & jointNames = transmission->getJointNames(); + actuatedJoints_.insert(actuatedJoints_.end(), jointNames.begin(), jointNames.end()); + + return returnCode; } @@ -851,11 +857,6 @@ namespace jiminy return sensorsGroupHolder_; } - Robot::tranmissionssGroupHolder_t const & Robot::getTransmissions(void) const - { - return tranmissionssGroupHolder_; - } - hresult_t Robot::setOptions(configHolder_t const & robotOptions) { hresult_t returnCode = hresult_t::SUCCESS; @@ -1367,12 +1368,6 @@ namespace jiminy return motorEffortEmpty; } - hresult_t Robot::updateActuatedJoints(std::vector const & jointNames) - { - actuatedJoints_.insert(actuatedJoints_.end(), jointNames.begin(), jointNames.end()); - return hresult_t::SUCCESS; - } - void Robot::setSensorsData(float64_t const & t, vectorN_t const & q, vectorN_t const & v, @@ -1564,7 +1559,7 @@ namespace jiminy vectorN_t const & Robot::getArmatures(void) const { - static vectorN_t armatures_; + static vectorN_t armatures; armatures.resize(pncModel_.nv); armatures.setZero(); From b16bb60ee5cfbbd86f38928405e23ac23f536562 Mon Sep 17 00:00:00 2001 From: fabians Date: Wed, 8 Dec 2021 11:17:47 +0100 Subject: [PATCH 4/9] update --- .../jiminy/core/robot/AbstractTransmission.h | 30 +++- core/include/jiminy/core/robot/Model.h | 4 +- core/include/jiminy/core/robot/Robot.h | 1 + core/src/robot/AbstractMotor.cc | 18 +-- core/src/robot/AbstractTransmission.cc | 151 ++++-------------- core/src/robot/Model.cc | 2 + core/src/robot/Robot.cc | 9 ++ 7 files changed, 78 insertions(+), 137 deletions(-) diff --git a/core/include/jiminy/core/robot/AbstractTransmission.h b/core/include/jiminy/core/robot/AbstractTransmission.h index 00e52d74e..a8fb78881 100644 --- a/core/include/jiminy/core/robot/AbstractTransmission.h +++ b/core/include/jiminy/core/robot/AbstractTransmission.h @@ -27,6 +27,28 @@ namespace jiminy /* 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 @@ -145,14 +167,14 @@ namespace jiminy /// /// \details It is the index of the joints associated with the transmission in the configuration vector. /////////////////////////////////////////////////////////////////////////////////////////////// - std::vector const & getJointPositionIndices(void) const; + 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. /////////////////////////////////////////////////////////////////////////////////////////////// - std::vector const & getJointVelocityIndices(void) const; + vectorN_t const & getJointVelocityIndices(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get motorName_. @@ -228,12 +250,14 @@ namespace jiminy /////////////////////////////////////////////////////////////////////////////////////////////// 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::function notifyRobot_; ///< Notify the robot that the configuration of the transmissions have changed std::string name_; ///< Name of the transmission int32_t transmissionIdx_; ///< Index of the transmission in the transmission buffer std::vector jointNames_; diff --git a/core/include/jiminy/core/robot/Model.h b/core/include/jiminy/core/robot/Model.h index e2d7acd76..13bd47341 100644 --- a/core/include/jiminy/core/robot/Model.h +++ b/core/include/jiminy/core/robot/Model.h @@ -27,8 +27,8 @@ namespace jiminy BOUNDS_JOINTS = 0, CONTACT_FRAMES = 1, COLLISION_BODIES = 2, - TRANSMISSIONS = 3, - USER = 4 + USER = 3, + TRANSMISSIONS = 4 }; std::array const constraintsHolderTypeRange = {{ diff --git a/core/include/jiminy/core/robot/Robot.h b/core/include/jiminy/core/robot/Robot.h index 470c8b48d..2a085fa50 100644 --- a/core/include/jiminy/core/robot/Robot.h +++ b/core/include/jiminy/core/robot/Robot.h @@ -80,6 +80,7 @@ namespace jiminy vectorN_t const & command); vectorN_t const & getMotorsEfforts(void) const; float64_t const & getMotorEffort(std::string const & motorName) const; + float64_t getMotorEffortLimit; void setSensorsData(float64_t const & t, vectorN_t const & q, vectorN_t const & v, diff --git a/core/src/robot/AbstractMotor.cc b/core/src/robot/AbstractMotor.cc index ac52c30a9..68dfac8fa 100644 --- a/core/src/robot/AbstractMotor.cc +++ b/core/src/robot/AbstractMotor.cc @@ -292,22 +292,22 @@ namespace jiminy return sharedHolder_->effort_[motorIdx_]; } - float64_t const & AbstractMotorBase::getPosition(void) + float64_t const & AbstractMotorBase::getPosition(void) const { return sharedHolder_->position_[motorIdx_]; } - float64_t const & AbstractMotorBase::getVelocity(void); + float64_t const & AbstractMotorBase::getVelocity(void) const { return sharedHolder_->velocity_[motorIdx_]; } - float64_t const & AbstractMotorBase::getAcceleration(void); + float64_t const & AbstractMotorBase::getAcceleration(void) const { return sharedHolder_->acceleration_[motorIdx_]; } - float64_t const & AbstractMotorBase::getEffort(void); + float64_t const & AbstractMotorBase::getEffort(void) const { return sharedHolder_->effort_[motorIdx_]; } @@ -334,21 +334,11 @@ namespace jiminy return returnCode; } - bool_t const & AbstractMotorBase::getIsInitialized(void) const - { - return isInitialized_; - } - std::string const & AbstractMotorBase::getName(void) const { return name_; } - int32_t const & AbstractMotorBase::getIdx(void) const - { - return motorIdx_; - } - float64_t const & AbstractMotorBase::getCommandLimit(void) const { return commandLimit_; diff --git a/core/src/robot/AbstractTransmission.cc b/core/src/robot/AbstractTransmission.cc index 6bbccf1c8..de24b4c44 100644 --- a/core/src/robot/AbstractTransmission.cc +++ b/core/src/robot/AbstractTransmission.cc @@ -8,11 +8,11 @@ namespace jiminy { AbstractTransmissionBase::AbstractTransmissionBase(std::string const & name) : + baseTransmissionOptions_(nullptr), transmissionOptionsHolder_(), isInitialized_(false), isAttached_(false), robot_(), - notifyRobot_(), name_(name), transmissionIdx_(-1), jointNames_(), @@ -55,17 +55,18 @@ namespace jiminy // Populate jointPositionIndices_ std::vector jointPositionIndices; hresult_t returnCode = hresult_t::SUCCESS; + auto robot = robot_.lock(); for (std::string const & jointName : jointNames_) { std::vector jointPositionIdx; - if (!robot_->model.existJointName(jointName)) + 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_->model, jointName, jointPositionIdx); + returnCode = getJointPositionIdx(robot->pncModel_, jointName, jointPositionIdx); } if (returnCode == hresult_t::SUCCESS) { @@ -84,18 +85,20 @@ namespace jiminy for (std::string const & jointName : jointNames_) { std::vector jointVelocityIdx; - if (!robot_->model.existJointName(jointName)) + 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->model.getJointId(jointName); - int32_t const & jointVelocityFirstIdx = robot->model.joints[jointModelIdx].idx_v(); - int32_t const & jointNv = robot->model.joints[jointModelIdx].nv(); + 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()); } + + // missing how to populate vectorN_t jointVelocityIndices_ return returnCode; } @@ -116,7 +119,8 @@ namespace jiminy } // Make sure the joint is not already attached to a transmission - std::vector actuatedJointNames = robot->getActuatedJointNames(); + auto robotTemp = robot.lock(); + std::vector actuatedJointNames = robotTemp->getActuatedJointNames(); for (std::string const & transmissionJoint : getJointNames()) { auto transmissionJointIt = actuatedJointNames.find(transmissionJoint); @@ -125,8 +129,9 @@ namespace jiminy PRINT_ERROR("Joint already attached to another transmission"); return hresult_t::ERROR_GENERIC; } + // Add joint to actuated joints + actuatedJointNames.push_back(transmissionJoint); } - // Copy references to the robot and shared data robot_ = robot; @@ -138,75 +143,26 @@ namespace jiminy hresult_t AbstractTransmissionBase::detach(void) { - // Delete the part of the shared memory associated with the transmission - if (!isAttached_) { PRINT_ERROR("Transmission not attached to any robot."); return hresult_t::ERROR_GENERIC; } - // Remove associated col in the global data buffer - if (transmissionIdx_ < sharedHolder_->num_ - 1) - { - int32_t transmissionShift = sharedHolder_->num_ - transmissionIdx_ - 1; - sharedHolder_->data_.segment(transmissionIdx_, transmissionShift) = - sharedHolder_->data_.segment(transmissionIdx_ + 1, transmissionShift).eval(); // eval to avoid aliasing - } - sharedHolder_->data_.conservativeResize(sharedHolder_->num_ - 1); - - // Shift the transmission ids - for (int32_t i = transmissionIdx_ + 1; i < sharedHolder_->num_; ++i) - { - --sharedHolder_->transmissions_[i]->transmissionIdx_; - } - - // Remove the transmission to the shared memory - sharedHolder_->transmissions_.erase(sharedHolder_->transmissions_.begin() + transmissionIdx_); - --sharedHolder_->num_; - - // Clear the references to the robot and shared data + // Clear the references to the robot robot_.reset(); - sharedHolder_ = nullptr; // Unset the Id transmissionIdx_ = -1; + // Delete motor and joint references + // Update the flag isAttached_ = false; return hresult_t::SUCCESS; } - hresult_t AbstractTransmissionBase::resetAll(void) - { - // Make sure the transmission is attached to a robot - if (!isAttached_) - { - PRINT_ERROR("Transmission not attached to any robot."); - return hresult_t::ERROR_GENERIC; - } - - // Make sure the robot still exists - if (robot_.expired()) - { - PRINT_ERROR("Robot has been deleted. Impossible to reset the transmissions."); - return hresult_t::ERROR_GENERIC; - } - - // Clear the shared data buffer - sharedHolder_->data_.setZero(); - - // Update transmission scope information - for (AbstractTransmissionBase * transmission : sharedHolder_->transmissions_) - { - // Refresh proxies that are robot-dependent - transmission->refreshProxies(); - } - - return hresult_t::SUCCESS; - } - hresult_t AbstractTransmissionBase::setOptions(configHolder_t const & transmissionOptions) { // Check if the internal buffers must be updated @@ -216,11 +172,6 @@ namespace jiminy // Check if reduction ratio has changed float64_t const & mechanicalReduction = boost::get(transmissionOptions.at("mechanicalReduction")); internalBuffersMustBeUpdated |= (baseTransmissionOptions_->mechanicalReduction != mechanicalReduction); - if (mechanicalReduction) - { - float64_t const & mechanicalReduction = boost::get(transmissionOptions.at("mechanicalReduction")); - internalBuffersMustBeUpdated |= std::abs(armature - baseTransmissionOptions_->armature) > EPS; - } } // Update the transmission's options @@ -282,27 +233,27 @@ namespace jiminy } } - for (i = 0; i < jointName_.size(); i++) + for (int i = 0; i < jointNames_.size(); i++) { if (returnCode == hresult_t::SUCCESS) { - returnCode = ::jiminy::getJointModelIdx(robot->pncModel_, jointName_[i], jointModelIdx_[i]); + returnCode = ::jiminy::getJointModelIdx(robot->pncModel_, jointNames_[i], jointModelIndices_[i]); } } - for (i = 0; i < jointName_.size(); i++) + for (int i = 0; i < jointNames_.size(); i++) { if (returnCode == hresult_t::SUCCESS) { - returnCode = getJointTypeFromIdx(robot->pncModel_, jointModelIdx_[i], jointType_[i]); + returnCode = getJointTypeFromIdx(robot->pncModel_, jointModelIndices_[i], jointTypes_[i]); } } - for (i = 0; i < jointName_.size(); i++) + for (int i = 0; i < jointNames_.size(); i++) { if (returnCode == hresult_t::SUCCESS) { // Transmissions are only supported for linear and rotary joints - if (jointType_[i] != joint_t::LINEAR && jointType_[i] != joint_t::ROTARY && jointType_[i] != joint_t::ROTARY_UNBOUNDED) + 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; @@ -310,50 +261,12 @@ namespace jiminy } } - for (i = 0; i < jointName_.size(); i++) + for (int i = 0; i < jointNames_.size(); i++) { if (returnCode == hresult_t::SUCCESS) { - ::jiminy::getJointPositionIdx(robot->pncModel_, jointName_[i], jointPositionIdx_[i]); - ::jiminy::getJointVelocityIdx(robot->pncModel_, jointName_[i], jointVelocityIdx_[i]); - - // Get the rotor inertia - if (baseTransmissionOptions_->enableArmature) - { - armature_ = baseTransmissionOptions_->armature; - } - else - { - armature_ = 0.0; - } - - // Propagate the user-defined transmission inertia at Pinocchio model level - if (notifyRobot_) - { - returnCode = notifyRobot_(*this); - } - } - } - - return returnCode; - } - - hresult_t AbstractTransmissionBase::setOptionsAll(configHolder_t const & transmissionOptions) - { - hresult_t returnCode = hresult_t::SUCCESS; - - // Make sure the transmission is attached to a robot - if (!isAttached_) - { - PRINT_ERROR("Transmission not attached to any robot."); - returnCode = hresult_t::ERROR_GENERIC; - } - - for (AbstractTransmissionBase * transmission : sharedHolder_->transmissions_) - { - if (returnCode == hresult_t::SUCCESS) - { - returnCode = transmission->setOptions(transmissionOptions); + ::jiminy::getJointPositionIdx(robot->pncModel_, jointNames_[i], jointPositionIndices_[i]); + ::jiminy::getJointVelocityIdx(robot->pncModel_, jointNames_[i], jointVelocityIndices_[i]); } } @@ -375,7 +288,7 @@ namespace jiminy return transmissionIdx_; } - std::vector::string const & AbstractTransmissionBase::getJointNames(void) const + std::vector const & AbstractTransmissionBase::getJointNames(void) const { return jointNames_; } @@ -385,7 +298,8 @@ namespace jiminy jointIndex_t jointModelIdx; for (std::string const & jointName : jointNames_) { - returnCode = ::jiminy::getJointModelIdx(robot->pncModel_, jointName, jointModelIdx); + auto robot = robot_.lock(); + hresult_t returnCode = ::jiminy::getJointModelIdx(robot->pncModel_, jointName, jointModelIdx); if (returnCode == hresult_t::SUCCESS) { jointModelIndices_.push_back(jointModelIdx); @@ -396,7 +310,8 @@ namespace jiminy std::vector const & AbstractTransmissionBase::getJointTypes(void) const { - jointModelIndices = getJointModelIndices(); + std::vector const & jointModelIndices = getJointModelIndices(); + auto robot = robot_.lock(); for (jointIndex_t const & idx : jointModelIndices) { joint_t jointType; @@ -418,9 +333,9 @@ namespace jiminy return jointVelocityIndices_; } - std::vector::string const & AbstractTransmissionBase::getMotorNames(void) const + std::vector const & AbstractTransmissionBase::getMotorNames(void) const { - return motorName_; + return motorNames_; } hresult_t AbstractTransmissionBase::computeForward(float64_t const & t, diff --git a/core/src/robot/Model.cc b/core/src/robot/Model.cc index c2b66d2e0..6b3276471 100644 --- a/core/src/robot/Model.cc +++ b/core/src/robot/Model.cc @@ -102,6 +102,7 @@ namespace jiminy break; case constraintsHolderType_t::USER: case constraintsHolderType_t::COLLISION_BODIES: + case constraintsHolderType_t::TRANSMISSIONS: default: constraintsMapPtr = ®istered; } @@ -172,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 80662ca7d..19f06ddb1 100644 --- a/core/src/robot/Robot.cc +++ b/core/src/robot/Robot.cc @@ -1368,6 +1368,11 @@ namespace jiminy return motorEffortEmpty; } + float64_t getMotorEffortLimit + { + // TODO combine robot information and transmission information + } + void Robot::setSensorsData(float64_t const & t, vectorN_t const & q, vectorN_t const & v, @@ -1532,6 +1537,10 @@ namespace jiminy return sensorsNamesEmpty; } } + std::vector const & Robot::getActuatedJointNames(void) const + { + return actuatedJointNames_; + } vectorN_t const & Robot::getCommandLimit(void) const { From 6007e332fd96ea97343a9956179706e2488d2980 Mon Sep 17 00:00:00 2001 From: fabians Date: Wed, 8 Dec 2021 17:11:12 +0100 Subject: [PATCH 5/9] modifs --- core/src/robot/AbstractTransmission.cc | 42 ++++++++------------------ core/src/robot/BasicMotors.cc | 2 +- 2 files changed, 14 insertions(+), 30 deletions(-) diff --git a/core/src/robot/AbstractTransmission.cc b/core/src/robot/AbstractTransmission.cc index de24b4c44..005ddc3dd 100644 --- a/core/src/robot/AbstractTransmission.cc +++ b/core/src/robot/AbstractTransmission.cc @@ -123,7 +123,7 @@ namespace jiminy std::vector actuatedJointNames = robotTemp->getActuatedJointNames(); for (std::string const & transmissionJoint : getJointNames()) { - auto transmissionJointIt = actuatedJointNames.find(transmissionJoint); + auto transmissionJointIt = std::find(actuatedJointNames.begin(), actuatedJointNames.end(), transmissionJoint); if (transmissionJointIt != actuatedJointNames.end()) { PRINT_ERROR("Joint already attached to another transmission"); @@ -295,39 +295,21 @@ namespace jiminy std::vector const & AbstractTransmissionBase::getJointModelIndices(void) const { - jointIndex_t jointModelIdx; - for (std::string const & jointName : jointNames_) - { - auto robot = robot_.lock(); - hresult_t returnCode = ::jiminy::getJointModelIdx(robot->pncModel_, jointName, jointModelIdx); - if (returnCode == hresult_t::SUCCESS) - { - jointModelIndices_.push_back(jointModelIdx); - } - } return jointModelIndices_; } std::vector const & AbstractTransmissionBase::getJointTypes(void) const { - std::vector const & jointModelIndices = getJointModelIndices(); - auto robot = robot_.lock(); - for (jointIndex_t const & idx : jointModelIndices) - { - joint_t jointType; - getJointTypeFromIdx(robot->pncModel, idx, jointType); - jointTypes_.push_back(jointType); - } return jointTypes_; } - vectorN_t const & AbstractTransmissionBase::getJointPositionIndices(void) + vectorN_t const & AbstractTransmissionBase::getJointPositionIndices(void) const { return jointPositionIndices_; } - vectorN_t const & AbstractTransmissionBase::getJointVelocityIndices(void) + vectorN_t const & AbstractTransmissionBase::getJointVelocityIndices(void) const { return jointVelocityIndices_; @@ -347,10 +329,11 @@ namespace jiminy auto qMotors = q.segment<>(jointPositionIdx_, ); auto vMotors = v.segment<>(jointVelocityIdx_, ); computeTransform(qMotors, vMotors); - q.noalias() = forwardTransform_ * motors_->getPosition(); - v.noalias() = forwardTransform_ * motors_->getVelocity(); - a.noalias() = forwardTransform_ * motors_->getAcceleration(); - uJoint.noalias() = forwardTransform_ * motors_->getEffort(); + auto motors = motors_.lock(); + q.noalias() = forwardTransform_ * motors->getPosition(); + v.noalias() = forwardTransform_ * motors->getVelocity(); + a.noalias() = forwardTransform_ * motors->getAcceleration(); + uJoint.noalias() = forwardTransform_ * motors->getEffort(); } hresult_t AbstractTransmissionBase::computeBackward(float64_t const & t, @@ -360,9 +343,10 @@ namespace jiminy vectorN_t const & uJoint) { computeInverseTransform(q, v); - motors_->q = backwardTransform_ * q; - motors_->v = backwardTransform_ * v; - motors_->a = backwardTransform_ * a; - motors_->u = backwardTransform_ * uJoint; + 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 6586ee37b..6b645b79d 100644 --- a/core/src/robot/BasicMotors.cc +++ b/core/src/robot/BasicMotors.cc @@ -63,7 +63,7 @@ namespace jiminy } hresult_t SimpleMotor::computeEffort(float64_t const & v, - float64_t command) + float64_t command) { /* 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. */ From de296b4757d43fc2a21c4fe02055e7e83e52e8e9 Mon Sep 17 00:00:00 2001 From: fabians Date: Tue, 14 Dec 2021 18:39:11 +0100 Subject: [PATCH 6/9] comment all stuff to make it compile, put TODO --- .../include/jiminy/core/robot/AbstractMotor.h | 8 + .../jiminy/core/robot/AbstractTransmission.h | 10 +- core/include/jiminy/core/robot/BasicMotors.h | 3 +- .../jiminy/core/robot/BasicTransmissions.h | 8 +- core/include/jiminy/core/robot/Robot.h | 4 +- core/src/engine/EngineMultiRobot.cc | 47 +++-- core/src/robot/AbstractMotor.cc | 34 ++-- core/src/robot/AbstractTransmission.cc | 36 ++-- core/src/robot/BasicMotors.cc | 39 ++-- core/src/robot/Robot.cc | 179 +++++++++++------- 10 files changed, 216 insertions(+), 152 deletions(-) diff --git a/core/include/jiminy/core/robot/AbstractMotor.h b/core/include/jiminy/core/robot/AbstractMotor.h index a8ea8b465..17bb535e9 100644 --- a/core/include/jiminy/core/robot/AbstractMotor.h +++ b/core/include/jiminy/core/robot/AbstractMotor.h @@ -149,6 +149,14 @@ namespace jiminy /////////////////////////////////////////////////////////////////////////////////////////////// hresult_t setOptionsAll(configHolder_t const & motorOptions); + /////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get motorIdx_. + /// + /// \details It is the index of the motor. + /// \brief Get the actual acc of the motor at the current time. + /////////////////////////////////////////////////////////////////////////////////////////////// + int32_t const & getIdx(void) const; + /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get the actual position of the motor at the current time. /////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/core/include/jiminy/core/robot/AbstractTransmission.h b/core/include/jiminy/core/robot/AbstractTransmission.h index a8fb78881..babb78deb 100644 --- a/core/include/jiminy/core/robot/AbstractTransmission.h +++ b/core/include/jiminy/core/robot/AbstractTransmission.h @@ -210,12 +210,14 @@ namespace jiminy /// \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. + /// 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. /////////////////////////////////////////////////////////////////////////////////////////////// - virtual void computeTransform(Eigen::VectorBlock const & q, + virtual float64_t computeTransform(Eigen::VectorBlock const & q, Eigen::VectorBlock const & v) = 0; /* copy on purpose */ /////////////////////////////////////////////////////////////////////////////////////////////// @@ -227,7 +229,7 @@ namespace jiminy /// \param[in] q Current configuration of the motors. /// \param[in] v Current velocity of the motors. /////////////////////////////////////////////////////////////////////////////////////////////// - virtual void computeInverseTransform(Eigen::VectorBlock const & q, + virtual float64_t computeInverseTransform(Eigen::VectorBlock const & q, Eigen::VectorBlock const & v) = 0; /* copy on purpose */ @@ -235,7 +237,7 @@ namespace jiminy /// \brief Compute energy dissipation in the transmission. /// /////////////////////////////////////////////////////////////////////////////////////////////// - virtual computeEffortTransmission(void) = 0; + virtual void computeEffortTransmission(void) = 0; private: /////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/core/include/jiminy/core/robot/BasicMotors.h b/core/include/jiminy/core/robot/BasicMotors.h index 10e94d039..69182f3bf 100644 --- a/core/include/jiminy/core/robot/BasicMotors.h +++ b/core/include/jiminy/core/robot/BasicMotors.h @@ -59,8 +59,7 @@ namespace jiminy virtual hresult_t setOptions(configHolder_t const & motorOptions) final override; private: - virtual hresult_t computeEffort(float64_t const & v, - float64_t command) final override; + virtual hresult_t computeEffort(float64_t command) final override; private: std::unique_ptr motorOptions_; diff --git a/core/include/jiminy/core/robot/BasicTransmissions.h b/core/include/jiminy/core/robot/BasicTransmissions.h index 95732da67..04b81ecf2 100644 --- a/core/include/jiminy/core/robot/BasicTransmissions.h +++ b/core/include/jiminy/core/robot/BasicTransmissions.h @@ -44,11 +44,11 @@ namespace jiminy virtual hresult_t setOptions(configHolder_t const & transmissionOptions) final override; private: - virtual float64_t computeTransform(Eigen::VectorBlock /* q */, - Eigen::VectorBlock /* v */) final override; + virtual float64_t computeTransform(Eigen::VectorBlock /* q */, + Eigen::VectorBlock /* v */) final override; - virtual float64_t computeInverseTransform(Eigen::VectorBlock /* q */, - Eigen::VectorBlock /* v */) final override; + virtual float64_t computeInverseTransform(Eigen::VectorBlock /* q */, + Eigen::VectorBlock /* v */) final override; private: std::unique_ptr transmissionOptions_; diff --git a/core/include/jiminy/core/robot/Robot.h b/core/include/jiminy/core/robot/Robot.h index 2a085fa50..c280f28b3 100644 --- a/core/include/jiminy/core/robot/Robot.h +++ b/core/include/jiminy/core/robot/Robot.h @@ -80,7 +80,7 @@ namespace jiminy vectorN_t const & command); vectorN_t const & getMotorsEfforts(void) const; float64_t const & getMotorEffort(std::string const & motorName) const; - float64_t getMotorEffortLimit; + float64_t getMotorEffortLimit(void); void setSensorsData(float64_t const & t, vectorN_t const & q, vectorN_t const & v, @@ -149,6 +149,7 @@ namespace jiminy protected: hresult_t refreshMotorsProxies(void); hresult_t refreshSensorsProxies(void); + hresult_t refreshTransmissionProxies(void); virtual hresult_t refreshProxies(void) override; protected: @@ -160,6 +161,7 @@ namespace jiminy 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 diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc index c6a660c72..4f7cd3747 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,8 @@ namespace jiminy pinocchio::Model const & model = system.robot->pncModel_; pinocchio::Data & data = system.robot->pncData_; + // TODO wtf + // data.rotorInertia = system.robot->getArmature(); if (system.robot->hasConstraints()) { // Define some proxies for convenience diff --git a/core/src/robot/AbstractMotor.cc b/core/src/robot/AbstractMotor.cc index 68dfac8fa..0a9d0f1b9 100644 --- a/core/src/robot/AbstractMotor.cc +++ b/core/src/robot/AbstractMotor.cc @@ -60,7 +60,7 @@ namespace jiminy motorIdx_ = sharedHolder_->num_; // Add values for the motor to the shared data buffer - for (vectorN_t & data : std::array{{ + for (vectorN_t & data : std::array{{ sharedHolder_->position_, sharedHolder_->velocity_, sharedHolder_->acceleration_, @@ -89,7 +89,7 @@ namespace jiminy return hresult_t::ERROR_GENERIC; } - for (vectorN_t & data : std::array{{ + for (vectorN_t & data : std::array{{ sharedHolder_->position_, sharedHolder_->velocity_, sharedHolder_->acceleration_, @@ -146,7 +146,7 @@ namespace jiminy } // Clear the shared data buffer - for (vectorN_t & data : std::array{{ + for (vectorN_t & data : std::array{{ sharedHolder_->position_, sharedHolder_->velocity_, sharedHolder_->acceleration_, @@ -242,17 +242,18 @@ namespace jiminy if (returnCode == hresult_t::SUCCESS) { - ::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; - } + // 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) @@ -339,6 +340,11 @@ namespace jiminy return name_; } + int32_t const & AbstractMotorBase::getIdx(void) const + { + return motorIdx_; + } + float64_t const & AbstractMotorBase::getCommandLimit(void) const { return commandLimit_; diff --git a/core/src/robot/AbstractTransmission.cc b/core/src/robot/AbstractTransmission.cc index 005ddc3dd..74c3701c4 100644 --- a/core/src/robot/AbstractTransmission.cc +++ b/core/src/robot/AbstractTransmission.cc @@ -119,19 +119,19 @@ namespace jiminy } // Make sure the joint is not already attached to a transmission - 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; - } - // Add joint to actuated joints - actuatedJointNames.push_back(transmissionJoint); - } + // 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; @@ -326,9 +326,15 @@ namespace jiminy vectorN_t & a, vectorN_t & uJoint) { - auto qMotors = q.segment<>(jointPositionIdx_, ); - auto vMotors = v.segment<>(jointVelocityIdx_, ); + // 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 computeTransform(qMotors, vMotors); + + // Apply transformation from motor to joint level auto motors = motors_.lock(); q.noalias() = forwardTransform_ * motors->getPosition(); v.noalias() = forwardTransform_ * motors->getVelocity(); diff --git a/core/src/robot/BasicMotors.cc b/core/src/robot/BasicMotors.cc index 6b645b79d..ee4d6dd75 100644 --- a/core/src/robot/BasicMotors.cc +++ b/core/src/robot/BasicMotors.cc @@ -62,8 +62,7 @@ namespace jiminy return returnCode; } - hresult_t SimpleMotor::computeEffort(float64_t const & v, - float64_t command) + hresult_t SimpleMotor::computeEffort(float64_t command) { /* 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. */ @@ -71,24 +70,26 @@ namespace jiminy { command = clamp(command, -getCommandLimit(), getCommandLimit()); } - 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); - } - } + // 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/Robot.cc b/core/src/robot/Robot.cc index 19f06ddb1..f428bb1c3 100644 --- a/core/src/robot/Robot.cc +++ b/core/src/robot/Robot.cc @@ -22,11 +22,12 @@ namespace jiminy isTelemetryConfigured_(false), telemetryData_(nullptr), motorsHolder_(), - tranmissionsHolder_(), + transmissionsHolder_(), sensorsGroupHolder_(), sensorTelemetryOptions_(), motorsNames_(), sensorsNames_(), + transmissionsNames_(), commandFieldnames_(), motorEffortFieldnames_(), nmotors_(0U), @@ -94,6 +95,12 @@ namespace jiminy } } + // Reset the transmissions + if (!transmissionsHolder_.empty()) + { + (*transmissionsHolder_.begin())->resetAll(); + } + // Reset the telemetry flag isTelemetryConfigured_ = false; } @@ -189,13 +196,12 @@ namespace jiminy transmissionsHolder_.push_back(transmission); // Refresh the transmissions proxies - refreshTransmissionsProxies(); + refreshTransmissionProxies(); } // update list of actuated joints std::vector const & jointNames = transmission->getJointNames(); - actuatedJoints_.insert(actuatedJoints_.end(), jointNames.begin(), jointNames.end()); - + actuatedJointNames_.insert(actuatedJointNames_.end(), jointNames.begin(), jointNames.end()); return returnCode; } @@ -233,7 +239,7 @@ namespace jiminy transmissionsHolder_.erase(transmissionIt); // Refresh the transmissions proxies - refreshTransmissionsProxies(); + refreshTransmissionProxies(); return hresult_t::SUCCESS; } @@ -288,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; }; @@ -415,11 +422,12 @@ namespace jiminy if (returnCode == hresult_t::SUCCESS) { // Make sure that every transmission name exist - if (!checkInclusion(transmissionsNames_, transmissionsNames)) - { - PRINT_ERROR("At least one of the transmission names does not exist."); - returnCode = hresult_t::ERROR_BAD_INPUT; - } + // 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) @@ -640,6 +648,11 @@ namespace jiminy returnCode = refreshSensorsProxies(); } + if (returnCode == hresult_t::SUCCESS) + { + returnCode = refreshTransmissionProxies(); + } + return returnCode; } @@ -724,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) { @@ -781,7 +800,7 @@ namespace jiminy Robot::transmissionsHolder_t const & Robot::getTransmissions(void) const { - return tranmissionsHolder_; + return transmissionsHolder_; } hresult_t Robot::getSensor(std::string const & sensorType, @@ -1327,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); } } @@ -1343,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; } @@ -1360,17 +1383,21 @@ 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 + float64_t getMotorEffortLimit(void) { // TODO combine robot information and transmission information + return 0.0; } void Robot::setSensorsData(float64_t const & t, @@ -1475,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; } @@ -1488,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; } @@ -1509,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; } @@ -1548,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; } @@ -1572,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; } From 8ad2aa41f971d94c5f6a32ce752e1db44354ce7e Mon Sep 17 00:00:00 2001 From: fabians Date: Wed, 15 Dec 2021 13:33:42 +0100 Subject: [PATCH 7/9] update --- .../jiminy/core/robot/AbstractTransmission.h | 4 +- .../jiminy/core/robot/BasicTransmissions.h | 8 +- core/include/jiminy/core/robot/Robot.h | 2 +- core/src/robot/BasicTransmissions.cc | 13 +- .../cpp/double_pendulum/double_pendulum.cc | 10 +- .../include/jiminy/python/Transmissions.h | 14 ++ python/jiminy_pywrap/src/Motors.cc | 30 ++--- python/jiminy_pywrap/src/Transmissions.cc | 123 ++++++++++++++++++ 8 files changed, 172 insertions(+), 32 deletions(-) create mode 100644 python/jiminy_pywrap/include/jiminy/python/Transmissions.h create mode 100644 python/jiminy_pywrap/src/Transmissions.cc diff --git a/core/include/jiminy/core/robot/AbstractTransmission.h b/core/include/jiminy/core/robot/AbstractTransmission.h index babb78deb..74acf4013 100644 --- a/core/include/jiminy/core/robot/AbstractTransmission.h +++ b/core/include/jiminy/core/robot/AbstractTransmission.h @@ -217,7 +217,7 @@ namespace jiminy /// \param[in] q Current configuration of the motors. /// \param[in] v Current velocity of the motors. /////////////////////////////////////////////////////////////////////////////////////////////// - virtual float64_t computeTransform(Eigen::VectorBlock const & q, + virtual void computeTransform(Eigen::VectorBlock const & q, Eigen::VectorBlock const & v) = 0; /* copy on purpose */ /////////////////////////////////////////////////////////////////////////////////////////////// @@ -229,7 +229,7 @@ namespace jiminy /// \param[in] q Current configuration of the motors. /// \param[in] v Current velocity of the motors. /////////////////////////////////////////////////////////////////////////////////////////////// - virtual float64_t computeInverseTransform(Eigen::VectorBlock const & q, + virtual void computeInverseTransform(Eigen::VectorBlock const & q, Eigen::VectorBlock const & v) = 0; /* copy on purpose */ diff --git a/core/include/jiminy/core/robot/BasicTransmissions.h b/core/include/jiminy/core/robot/BasicTransmissions.h index 04b81ecf2..6cf374543 100644 --- a/core/include/jiminy/core/robot/BasicTransmissions.h +++ b/core/include/jiminy/core/robot/BasicTransmissions.h @@ -44,11 +44,11 @@ namespace jiminy virtual hresult_t setOptions(configHolder_t const & transmissionOptions) final override; private: - virtual float64_t computeTransform(Eigen::VectorBlock /* q */, - Eigen::VectorBlock /* v */) final override; + virtual void computeTransform(Eigen::VectorBlock const & /*q*/ , + Eigen::VectorBlock const & /*v*/) final override; - virtual float64_t computeInverseTransform(Eigen::VectorBlock /* q */, - Eigen::VectorBlock /* v */) final override; + virtual void computeInverseTransform(Eigen::VectorBlock const & /*q*/, + Eigen::VectorBlock const & /*v*/) final override; private: std::unique_ptr transmissionOptions_; diff --git a/core/include/jiminy/core/robot/Robot.h b/core/include/jiminy/core/robot/Robot.h index c280f28b3..f2a806a03 100644 --- a/core/include/jiminy/core/robot/Robot.h +++ b/core/include/jiminy/core/robot/Robot.h @@ -161,7 +161,7 @@ namespace jiminy 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 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 diff --git a/core/src/robot/BasicTransmissions.cc b/core/src/robot/BasicTransmissions.cc index 73459ce0f..169025a5e 100644 --- a/core/src/robot/BasicTransmissions.cc +++ b/core/src/robot/BasicTransmissions.cc @@ -17,26 +17,21 @@ namespace jiminy setOptions(getOptions()); } - float64_t SimpleTransmission::computeTransform(Eigen::VectorBlock /* q */, - Eigen::VectorBlock /* v */)) + void SimpleTransmission::computeTransform(Eigen::VectorBlock const & /*q*/, + Eigen::VectorBlock const & /*v*/) { if (!isInitialized_) { PRINT_ERROR("Transmission not initialized. Impossible to compute actual transmission effort."); - return hresult_t::ERROR_INIT_FAILED; } - - return transmissionOptions_->mechanicalReduction; } - float64_t SimpleTransmission::computeInverseTransform(Eigen::VectorBlock /* q */, - Eigen::VectorBlock /* v */)) + void SimpleTransmission::computeInverseTransform(Eigen::VectorBlock const & /*q*/, + Eigen::VectorBlock const & /*v*/) { if (!isInitialized_) { PRINT_ERROR("Transmission not initialized. Impossible to compute actual transmission effort."); - return hresult_t::ERROR_INIT_FAILED; } - return 1.0 / transmissionOptions_->mechanicalReduction; } } 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..3863e281d 100644 --- a/python/jiminy_pywrap/src/Motors.cc +++ b/python/jiminy_pywrap/src/Motors.cc @@ -26,22 +26,22 @@ namespace python void visit(PyClass & cl) const { cl - .add_property("is_initialized", bp::make_function(&AbstractMotorBase::getIsInitialized, - bp::return_value_policy())) + // .add_property("is_initialized", bp::make_function(&AbstractMotorBase::getIsInitialized, + // bp::return_value_policy())) .add_property("name", bp::make_function(&AbstractMotorBase::getName, 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("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, @@ -93,9 +93,9 @@ namespace python static void visit(PyClass & cl) { - cl - .def("initialize", &TMotor::initialize) - ; + // cl + // .def("initialize", &TMotor::initialize) + // ; } }; diff --git a/python/jiminy_pywrap/src/Transmissions.cc b/python/jiminy_pywrap/src/Transmissions.cc new file mode 100644 index 000000000..00a5f0908 --- /dev/null +++ b/python/jiminy_pywrap/src/Transmissions.cc @@ -0,0 +1,123 @@ +#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())) + + .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. From edec51068880da7d932db101e8ab0f25d0e3eed8 Mon Sep 17 00:00:00 2001 From: fabians Date: Thu, 16 Dec 2021 15:03:25 +0100 Subject: [PATCH 8/9] update --- .../jiminy/core/robot/AbstractTransmission.h | 45 +++++++++++++------ core/include/jiminy/core/robot/BasicMotors.h | 1 + .../jiminy/core/robot/BasicTransmissions.h | 10 +++-- core/include/jiminy/core/robot/Robot.h | 2 +- core/src/engine/EngineMultiRobot.cc | 3 +- core/src/robot/AbstractMotor.cc | 10 +++++ core/src/robot/AbstractTransmission.cc | 39 ++++++++++++++-- core/src/robot/BasicMotors.cc | 14 ++++++ core/src/robot/BasicTransmissions.cc | 11 ++++- python/jiminy_pywrap/src/Motors.cc | 10 ++--- python/jiminy_pywrap/src/Transmissions.cc | 2 +- 11 files changed, 116 insertions(+), 31 deletions(-) diff --git a/core/include/jiminy/core/robot/AbstractTransmission.h b/core/include/jiminy/core/robot/AbstractTransmission.h index 74acf4013..6a22b45a2 100644 --- a/core/include/jiminy/core/robot/AbstractTransmission.h +++ b/core/include/jiminy/core/robot/AbstractTransmission.h @@ -101,11 +101,6 @@ namespace jiminy /////////////////////////////////////////////////////////////////////////////////////////////// configHolder_t getOptions(void) const; - /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get the actual state of the transmission at the current time. - /////////////////////////////////////////////////////////////////////////////////////////////// - float64_t const & get(void) const; - /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Set the configuration options of the transmission. /// @@ -177,33 +172,54 @@ namespace jiminy vectorN_t const & getJointVelocityIndices(void) const; /////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get motorName_. + /// \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. /////////////////////////////////////////////////////////////////////////////////////////////// - virtual hresult_t computeForward(float64_t const & t, + hresult_t computeForward(float64_t const & t, vectorN_t & q, vectorN_t & v, vectorN_t & a, - vectorN_t & uJoint) final; + vectorN_t & uJoint); /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Compute backward transmission. /// /// \details Compute backward transmission from joint to motor. /////////////////////////////////////////////////////////////////////////////////////////////// - virtual hresult_t computeBackward(float64_t const & t, + hresult_t computeBackward(float64_t const & t, vectorN_t const & q, vectorN_t const & v, vectorN_t const & a, - vectorN_t const & uJoint) final; + vectorN_t const & uJoint); protected: /////////////////////////////////////////////////////////////////////////////////////////////// @@ -218,7 +234,8 @@ namespace jiminy /// \param[in] v Current velocity of the motors. /////////////////////////////////////////////////////////////////////////////////////////////// virtual void computeTransform(Eigen::VectorBlock const & q, - Eigen::VectorBlock const & v) = 0; /* copy on purpose */ + Eigen::VectorBlock const & v, + matrixN_t & out) = 0; /* copy on purpose */ /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Request the transmission to update its actual state based of the input data. @@ -230,8 +247,8 @@ namespace jiminy /// \param[in] v Current velocity of the motors. /////////////////////////////////////////////////////////////////////////////////////////////// virtual void computeInverseTransform(Eigen::VectorBlock const & q, - Eigen::VectorBlock const & v) = 0; /* copy on purpose */ - + Eigen::VectorBlock const & v, + matrixN_t & out) = 0; /* copy on purpose */ /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Compute energy dissipation in the transmission. @@ -268,10 +285,10 @@ namespace jiminy vectorN_t jointPositionIndices_; vectorN_t jointVelocityIndices_; std::vector motorNames_; + std::vector motorIndices_; std::vector > motors_; matrixN_t forwardTransform_; matrixN_t backwardTransform_; - }; } diff --git a/core/include/jiminy/core/robot/BasicMotors.h b/core/include/jiminy/core/robot/BasicMotors.h index 69182f3bf..03cdf1a6d 100644 --- a/core/include/jiminy/core/robot/BasicMotors.h +++ b/core/include/jiminy/core/robot/BasicMotors.h @@ -56,6 +56,7 @@ namespace jiminy auto shared_from_this() { return shared_from(this); } auto shared_from_this() const { return shared_from(this); } + hresult_t initialize(void); virtual hresult_t setOptions(configHolder_t const & motorOptions) final override; private: diff --git a/core/include/jiminy/core/robot/BasicTransmissions.h b/core/include/jiminy/core/robot/BasicTransmissions.h index 6cf374543..2cee15109 100644 --- a/core/include/jiminy/core/robot/BasicTransmissions.h +++ b/core/include/jiminy/core/robot/BasicTransmissions.h @@ -44,11 +44,15 @@ namespace jiminy virtual hresult_t setOptions(configHolder_t const & transmissionOptions) final override; private: - virtual void computeTransform(Eigen::VectorBlock const & /*q*/ , - Eigen::VectorBlock const & /*v*/) final override; + 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*/) final override; + Eigen::VectorBlock const & /*v*/, + matrixN_t & out) final override; + + virtual void computeEffortTransmission(void); private: std::unique_ptr transmissionOptions_; diff --git a/core/include/jiminy/core/robot/Robot.h b/core/include/jiminy/core/robot/Robot.h index f2a806a03..fde1c92c4 100644 --- a/core/include/jiminy/core/robot/Robot.h +++ b/core/include/jiminy/core/robot/Robot.h @@ -80,7 +80,7 @@ namespace jiminy vectorN_t const & command); vectorN_t const & getMotorsEfforts(void) const; float64_t const & getMotorEffort(std::string const & motorName) const; - float64_t getMotorEffortLimit(void); + vectorN_t const & getMotorsEffortLimits(void) const; void setSensorsData(float64_t const & t, vectorN_t const & q, vectorN_t const & v, diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc index 4f7cd3747..ab0b37441 100644 --- a/core/src/engine/EngineMultiRobot.cc +++ b/core/src/engine/EngineMultiRobot.cc @@ -3687,8 +3687,7 @@ namespace jiminy pinocchio::Model const & model = system.robot->pncModel_; pinocchio::Data & data = system.robot->pncData_; - // TODO wtf - // data.rotorInertia = system.robot->getArmature(); + data.rotorInertia = system.robot->getArmatures(); if (system.robot->hasConstraints()) { // Define some proxies for convenience diff --git a/core/src/robot/AbstractMotor.cc b/core/src/robot/AbstractMotor.cc index 0a9d0f1b9..4d2e7ab34 100644 --- a/core/src/robot/AbstractMotor.cc +++ b/core/src/robot/AbstractMotor.cc @@ -231,6 +231,16 @@ namespace jiminy } } + if (returnCode == hresult_t::SUCCESS) + { + if (!isInitialized_) + { + PRINT_ERROR("Motor not initialized. Impossible to refresh proxies."); + returnCode = hresult_t::ERROR_INIT_FAILED; + } + } + + if (returnCode == hresult_t::SUCCESS) { if (!robot->getIsInitialized()) diff --git a/core/src/robot/AbstractTransmission.cc b/core/src/robot/AbstractTransmission.cc index 74c3701c4..0b0bead81 100644 --- a/core/src/robot/AbstractTransmission.cc +++ b/core/src/robot/AbstractTransmission.cc @@ -52,10 +52,22 @@ namespace jiminy isInitialized_ = false; } + auto robot = robot_.lock(); + + // TODO move this stuff to refresh Proxies ? + // Populate motorIndices_ + std::weak_ptr motor; + for (std::string const & motorName : motorNames) + { + returnCode = robot->getMotor(motorName, motor); + auto motorTemp = motor.lock(); + int32_t idx = motorTemp->getIdx(); + motorIndices_.push_back(idx); + } + // Populate jointPositionIndices_ std::vector jointPositionIndices; hresult_t returnCode = hresult_t::SUCCESS; - auto robot = robot_.lock(); for (std::string const & jointName : jointNames_) { std::vector jointPositionIdx; @@ -98,7 +110,12 @@ namespace jiminy jointVelocityIndices.insert(jointVelocityIndices.end(), jointVelocityIdx.begin(), jointVelocityIdx.end()); } - // missing how to populate vectorN_t jointVelocityIndices_ + int32_t jointVelocitySize = jointVelocityIndices.size(); + jointVelocityIndices_.resize(jointVelocitySize); + for (int32_t i = 0; i < jointVelocitySize; ++i) + { + jointVelocityIndices_(i) = jointVelocityIndices[i]; + } return returnCode; } @@ -118,7 +135,7 @@ namespace jiminy return hresult_t::ERROR_GENERIC; } - // Make sure the joint is not already attached to a transmission + // 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(); @@ -320,6 +337,22 @@ namespace jiminy return motorNames_; } + std::vector const & getMotorIndices(void) const + { + // TODO create and populate + return motorIndices_; + } + + matrixN_t const & getForwardTransform(void) const + { + return forwardTransform_; + } + + matrixN_t const & getInverseTransform(void) const + { + return backwardTransform_; + } + hresult_t AbstractTransmissionBase::computeForward(float64_t const & t, vectorN_t & q, vectorN_t & v, diff --git a/core/src/robot/BasicMotors.cc b/core/src/robot/BasicMotors.cc index ee4d6dd75..1e0c1fd46 100644 --- a/core/src/robot/BasicMotors.cc +++ b/core/src/robot/BasicMotors.cc @@ -17,6 +17,20 @@ namespace jiminy setOptions(getDefaultMotorOptions()); } + hresult_t initialize(void) + { + hresult_t returnCode = hresult_t::SUCCESS; + isInitialized_ = true; + returnCode = refreshProxies(); + + if (returnCode != hresult_t::SUCCESS) + { + isInitialized_ = false; + } + + return returnCode; + } + hresult_t SimpleMotor::setOptions(configHolder_t const & motorOptions) { hresult_t returnCode = hresult_t::SUCCESS; diff --git a/core/src/robot/BasicTransmissions.cc b/core/src/robot/BasicTransmissions.cc index 169025a5e..b3d80d823 100644 --- a/core/src/robot/BasicTransmissions.cc +++ b/core/src/robot/BasicTransmissions.cc @@ -18,7 +18,8 @@ namespace jiminy } void SimpleTransmission::computeTransform(Eigen::VectorBlock const & /*q*/, - Eigen::VectorBlock const & /*v*/) + Eigen::VectorBlock const & /*v*/, + matrixN_t &out) { if (!isInitialized_) { @@ -27,11 +28,17 @@ namespace jiminy } void SimpleTransmission::computeInverseTransform(Eigen::VectorBlock const & /*q*/, - Eigen::VectorBlock const & /*v*/) + Eigen::VectorBlock const & /*v*/, + matrixN_t &out) { if (!isInitialized_) { PRINT_ERROR("Transmission not initialized. Impossible to compute actual transmission effort."); } } + + void computeEffortTransmission(void) + { + // TODO + } } diff --git a/python/jiminy_pywrap/src/Motors.cc b/python/jiminy_pywrap/src/Motors.cc index 3863e281d..938875e36 100644 --- a/python/jiminy_pywrap/src/Motors.cc +++ b/python/jiminy_pywrap/src/Motors.cc @@ -26,8 +26,8 @@ namespace python void visit(PyClass & cl) const { cl - // .add_property("is_initialized", bp::make_function(&AbstractMotorBase::getIsInitialized, - // bp::return_value_policy())) + .add_property("is_initialized", bp::make_function(&AbstractMotorBase::getIsInitialized, + bp::return_value_policy())) .add_property("name", bp::make_function(&AbstractMotorBase::getName, bp::return_value_policy())) .add_property("idx", bp::make_function(&AbstractMotorBase::getIdx, @@ -93,9 +93,9 @@ namespace python static void visit(PyClass & cl) { - // cl - // .def("initialize", &TMotor::initialize) - // ; + cl + .def("initialize", &TMotor::initialize) + ; } }; diff --git a/python/jiminy_pywrap/src/Transmissions.cc b/python/jiminy_pywrap/src/Transmissions.cc index 00a5f0908..d1927c2c4 100644 --- a/python/jiminy_pywrap/src/Transmissions.cc +++ b/python/jiminy_pywrap/src/Transmissions.cc @@ -113,7 +113,7 @@ namespace python std::shared_ptr, boost::noncopyable>("SimpleTransmission", bp::init( - bp::args("self", "Transmission_name"))) + bp::args("self", "transmission_name"))) .def(PySimpleTransmissionVisitor()); } }; From 2b5dc0d7655b4116562a53af51966d77a2b735f9 Mon Sep 17 00:00:00 2001 From: fabians Date: Thu, 16 Dec 2021 17:58:08 +0100 Subject: [PATCH 9/9] update --- .../jiminy/core/robot/AbstractTransmission.h | 6 +- core/include/jiminy/core/robot/BasicMotors.h | 2 +- .../jiminy/core/robot/BasicTransmissions.h | 8 +- core/src/engine/EngineMultiRobot.cc | 2 +- core/src/robot/AbstractMotor.cc | 1 - core/src/robot/AbstractTransmission.cc | 193 ++++++++++-------- core/src/robot/BasicMotors.cc | 2 +- core/src/robot/BasicTransmissions.cc | 4 +- python/jiminy_pywrap/src/Motors.cc | 10 - python/jiminy_pywrap/src/Transmissions.cc | 4 + 10 files changed, 130 insertions(+), 102 deletions(-) diff --git a/core/include/jiminy/core/robot/AbstractTransmission.h b/core/include/jiminy/core/robot/AbstractTransmission.h index 6a22b45a2..00f328566 100644 --- a/core/include/jiminy/core/robot/AbstractTransmission.h +++ b/core/include/jiminy/core/robot/AbstractTransmission.h @@ -232,10 +232,11 @@ namespace jiminy /// /// \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; /* copy on purpose */ + matrixN_t & out) = 0; /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Request the transmission to update its actual state based of the input data. @@ -245,10 +246,11 @@ namespace jiminy /// /// \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; /* copy on purpose */ + matrixN_t & out) = 0; /////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Compute energy dissipation in the transmission. diff --git a/core/include/jiminy/core/robot/BasicMotors.h b/core/include/jiminy/core/robot/BasicMotors.h index 03cdf1a6d..cff00b28a 100644 --- a/core/include/jiminy/core/robot/BasicMotors.h +++ b/core/include/jiminy/core/robot/BasicMotors.h @@ -60,7 +60,7 @@ namespace jiminy virtual hresult_t setOptions(configHolder_t const & motorOptions) final override; private: - virtual hresult_t computeEffort(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 index 2cee15109..cddf441ed 100644 --- a/core/include/jiminy/core/robot/BasicTransmissions.h +++ b/core/include/jiminy/core/robot/BasicTransmissions.h @@ -44,12 +44,12 @@ namespace jiminy virtual hresult_t setOptions(configHolder_t const & transmissionOptions) final override; private: - virtual void computeTransform(Eigen::VectorBlock const & /*q*/, - Eigen::VectorBlock const & /*v*/, + 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*/, + virtual void computeInverseTransform(Eigen::VectorBlock const & q, + Eigen::VectorBlock const & v, matrixN_t & out) final override; virtual void computeEffortTransmission(void); diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc index ab0b37441..729ec4170 100644 --- a/core/src/engine/EngineMultiRobot.cc +++ b/core/src/engine/EngineMultiRobot.cc @@ -3687,7 +3687,7 @@ namespace jiminy pinocchio::Model const & model = system.robot->pncModel_; pinocchio::Data & data = system.robot->pncData_; - data.rotorInertia = system.robot->getArmatures(); + 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 4d2e7ab34..b77694113 100644 --- a/core/src/robot/AbstractMotor.cc +++ b/core/src/robot/AbstractMotor.cc @@ -240,7 +240,6 @@ namespace jiminy } } - if (returnCode == hresult_t::SUCCESS) { if (!robot->getIsInitialized()) diff --git a/core/src/robot/AbstractTransmission.cc b/core/src/robot/AbstractTransmission.cc index 0b0bead81..05e1b0c6a 100644 --- a/core/src/robot/AbstractTransmission.cc +++ b/core/src/robot/AbstractTransmission.cc @@ -52,70 +52,6 @@ namespace jiminy isInitialized_ = false; } - auto robot = robot_.lock(); - - // TODO move this stuff to refresh Proxies ? - // Populate motorIndices_ - std::weak_ptr motor; - for (std::string const & motorName : motorNames) - { - returnCode = robot->getMotor(motorName, motor); - auto motorTemp = motor.lock(); - int32_t idx = motorTemp->getIdx(); - motorIndices_.push_back(idx); - } - - // Populate jointPositionIndices_ - std::vector jointPositionIndices; - hresult_t returnCode = hresult_t::SUCCESS; - 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; } @@ -282,9 +218,74 @@ namespace jiminy { if (returnCode == hresult_t::SUCCESS) { - ::jiminy::getJointPositionIdx(robot->pncModel_, jointNames_[i], jointPositionIndices_[i]); - ::jiminy::getJointVelocityIdx(robot->pncModel_, jointNames_[i], jointVelocityIndices_[i]); + ::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; @@ -337,23 +338,22 @@ namespace jiminy return motorNames_; } - std::vector const & getMotorIndices(void) const + std::vector const & AbstractTransmissionBase::getMotorIndices(void) const { - // TODO create and populate return motorIndices_; } - matrixN_t const & getForwardTransform(void) const + matrixN_t const & AbstractTransmissionBase::getForwardTransform(void) const { return forwardTransform_; } - matrixN_t const & getInverseTransform(void) const + matrixN_t const & AbstractTransmissionBase::getInverseTransform(void) const { return backwardTransform_; } - hresult_t AbstractTransmissionBase::computeForward(float64_t const & t, + hresult_t AbstractTransmissionBase::computeForward(float64_t const & /*t*/, vectorN_t & q, vectorN_t & v, vectorN_t & a, @@ -361,18 +361,36 @@ namespace jiminy { // 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_, ); + // 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(qMotors, vMotors); + computeTransform(qMotor, vMotor, forwardTransform_); // Apply transformation from motor to joint level - auto motors = motors_.lock(); - q.noalias() = forwardTransform_ * motors->getPosition(); - v.noalias() = forwardTransform_ * motors->getVelocity(); - a.noalias() = forwardTransform_ * motors->getAcceleration(); - uJoint.noalias() = forwardTransform_ * motors->getEffort(); + // 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, @@ -381,7 +399,22 @@ namespace jiminy vectorN_t const & a, vectorN_t const & uJoint) { - computeInverseTransform(q, v); + // 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; diff --git a/core/src/robot/BasicMotors.cc b/core/src/robot/BasicMotors.cc index 1e0c1fd46..6792ae5a8 100644 --- a/core/src/robot/BasicMotors.cc +++ b/core/src/robot/BasicMotors.cc @@ -17,7 +17,7 @@ namespace jiminy setOptions(getDefaultMotorOptions()); } - hresult_t initialize(void) + hresult_t SimpleMotor::initialize(void) { hresult_t returnCode = hresult_t::SUCCESS; isInitialized_ = true; diff --git a/core/src/robot/BasicTransmissions.cc b/core/src/robot/BasicTransmissions.cc index b3d80d823..131bee6d2 100644 --- a/core/src/robot/BasicTransmissions.cc +++ b/core/src/robot/BasicTransmissions.cc @@ -23,7 +23,7 @@ namespace jiminy { if (!isInitialized_) { - PRINT_ERROR("Transmission not initialized. Impossible to compute actual transmission effort."); + PRINT_ERROR("Transmission not initialized. Impossible to compute transformation of transmission."); } } @@ -33,7 +33,7 @@ namespace jiminy { if (!isInitialized_) { - PRINT_ERROR("Transmission not initialized. Impossible to compute actual transmission effort."); + PRINT_ERROR("Transmission not initialized. Impossible to compute transformation of transmission."); } } diff --git a/python/jiminy_pywrap/src/Motors.cc b/python/jiminy_pywrap/src/Motors.cc index 938875e36..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 index d1927c2c4..f89ab5c9b 100644 --- a/python/jiminy_pywrap/src/Transmissions.cc +++ b/python/jiminy_pywrap/src/Transmissions.cc @@ -42,6 +42,10 @@ namespace python 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)