Skip to content

Commit

Permalink
update
Browse files Browse the repository at this point in the history
  • Loading branch information
fabians committed Nov 9, 2021
1 parent 52aae4e commit ed03d33
Show file tree
Hide file tree
Showing 9 changed files with 124 additions and 158 deletions.
51 changes: 26 additions & 25 deletions core/include/jiminy/core/robot/AbstractMotor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
///////////////////////////////////////////////////////////////////////////////////////////////
Expand All @@ -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.
///////////////////////////////////////////////////////////////////////////////////////////////
Expand All @@ -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_.
///
Expand Down Expand Up @@ -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);
Expand All @@ -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:
///////////////////////////////////////////////////////////////////////////////////////////////
Expand All @@ -268,7 +268,7 @@ namespace jiminy
/// \details This method must be called before initializing the motor.
///////////////////////////////////////////////////////////////////////////////////////////////
hresult_t attach(std::weak_ptr<Robot const> robot,
std::function<hresult_t(AbstractMotorBase & /* motor */)> notifyRobot,
std::function<hresult_t(AbstractMotorBase & /*motor*/)> notifyRobot,
MotorSharedDataHolder_t * sharedHolder);

///////////////////////////////////////////////////////////////////////////////////////////////
Expand All @@ -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 const> robot_; ///< Robot for which the command and internal dynamics
std::function<hresult_t(AbstractMotorBase &)> 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
Expand Down
32 changes: 12 additions & 20 deletions core/include/jiminy/core/robot/AbstractTransmission.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string> const & jointName,
std::vector<std::string> const & motorName);

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Refresh the proxies.
Expand Down Expand Up @@ -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.
///
Expand Down Expand Up @@ -164,24 +160,24 @@ namespace jiminy
/// \details It is the name of the motors associated with the transmission.
///////////////////////////////////////////////////////////////////////////////////////////////
std::vector<std::string> 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,
Expand Down Expand Up @@ -212,30 +208,26 @@ namespace jiminy
virtual void computeInverseTransform(Eigen::VectorBlock<vectorN_t const> const & q,
Eigen::VectorBlock<vectorN_t const> const & v) = 0; /* copy on purpose */


///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Compute energy dissipation in the transmission.
///
///////////////////////////////////////////////////////////////////////////////////////////////
virtual void computeEffortTransmission() = 0;
virtual computeEffortTransmission(void) = 0;

private:
///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Attach the transmission to a robot
///
/// \details This method must be called before initializing the transmission.
///////////////////////////////////////////////////////////////////////////////////////////////
hresult_t attach(std::weak_ptr<Robot const> robot,
std::function<hresult_t(AbstractTransmissionBase & /*transmission*/)> notifyRobot);
hresult_t attach(std::weak_ptr<Robot const> robot);

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Detach the transmission from the robot
///////////////////////////////////////////////////////////////////////////////////////////////
hresult_t detach(void);

public:
std::unique_ptr<abstractTransmissionOptions_t const> 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
Expand Down
3 changes: 0 additions & 3 deletions core/include/jiminy/core/robot/BasicTransmissions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
4 changes: 2 additions & 2 deletions core/include/jiminy/core/robot/Model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<constraintsHolderType_t, 5> const constraintsHolderTypeRange = {{
Expand Down
11 changes: 7 additions & 4 deletions core/include/jiminy/core/robot/Robot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,7 @@ namespace jiminy
motorsHolder_t const & getMotors(void) const;
hresult_t detachMotor(std::string const & motorName);
hresult_t detachMotors(std::vector<std::string> const & motorsNames = {});
hresult_t detachTransmission(std::string const & transmissionName);
hresult_t detachTransmissions(std::vector<std::string> const & transmissionsNames = {});
hresult_t attachSensor(std::shared_ptr<AbstractSensorBase> sensor);
hresult_t getSensor(std::string const & sensorType,
Expand All @@ -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<AbstractTransmissionBase> & transmission);
transmissionsHolder_t const & getTransmissions(void) const;
hresult_t attachTransmission(std::shared_ptr<AbstractTransmissionBase> transmission);

void computeMotorsEfforts(float64_t const & t,
vectorN_t const & q,
Expand Down Expand Up @@ -127,8 +131,7 @@ namespace jiminy
std::vector<int32_t> getMotorsVelocityIdx(void) const;
std::unordered_map<std::string, std::vector<std::string> > const & getSensorsNames(void) const;
std::vector<std::string> const & getSensorsNames(std::string const & sensorType) const;
std::vector<std::string> const & getActuatedJoints(void) const;
hresult_t updateActuatedJoints(std::vector<std::string> const & jointNames);
std::vector<std::string> const & getActuatedJointNames(void) const;

vectorN_t const & getCommandLimit(void) const;
vectorN_t const & getArmatures(void) const;
Expand All @@ -151,15 +154,15 @@ namespace jiminy
bool_t isTelemetryConfigured_;
std::shared_ptr<TelemetryData> telemetryData_;
motorsHolder_t motorsHolder_;
transmissionsHolder_t tranmissionsHolder_;
transmissionsHolder_t transmissionsHolder_;
sensorsGroupHolder_t sensorsGroupHolder_;
std::unordered_map<std::string, bool_t> sensorTelemetryOptions_;
std::vector<std::string> motorsNames_; ///< Name of the motors
std::unordered_map<std::string, std::vector<std::string> > sensorsNames_; ///< Name of the sensors
std::vector<std::string> commandFieldnames_; ///< Fieldnames of the command
std::vector<std::string> motorEffortFieldnames_; ///< Fieldnames of the motors effort
uint64_t nmotors_; ///< The number of motors
std::vector<std::string> actuatedJoints_; ///< List of joints attached to a transmission
std::vector<std::string> actuatedJointNames_; ///< List of joints attached to a transmission

private:
std::unique_ptr<MutexLocal> mutexLocal_;
Expand Down
60 changes: 31 additions & 29 deletions core/src/robot/AbstractMotor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<vectorN_t &, 4>{{
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_;
Expand All @@ -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<vectorN_t &, 4>{{
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)
Expand Down Expand Up @@ -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<vectorN_t &, 4>{{
sharedHolder_->position_,
sharedHolder_->velocity_,
sharedHolder_->acceleration_,
sharedHolder_->effort_}})
{
data.setZero();
}

// Update motor scope information
for (AbstractMotorBase * motor : sharedHolder_->motors_)
Expand Down
Loading

0 comments on commit ed03d33

Please sign in to comment.