From ed03d33ab68205359c91fbcc3680f570aa7c3ca8 Mon Sep 17 00:00:00 2001 From: fabians Date: Tue, 9 Nov 2021 14:09:47 +0100 Subject: [PATCH] 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 ffd291716d..a8ea8b465d 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 7fa784674d..00e52d74eb 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 b335f566e0..95732da671 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 13bd47341b..e2d7acd76a 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 03185ec65b..470c8b48db 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 1d4b4ec830..ac52c30a98 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 1fe7c3d517..6bbccf1c89 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 26283a5f12..73459ce0f1 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 98b0c2eff5..502fa17444 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.jointNames_; + 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();