Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

WIP: [core] Add transmission abstraction. #432

Draft
wants to merge 10 commits into
base: master
Choose a base branch
from
Draft

WIP: [core] Add transmission abstraction. #432

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
3047673
init commit transmission
Oct 29, 2021
06417c6
Merge branch 'master' into integrate_transmission
duburcqa Oct 29, 2021
52aae4e
update
Nov 8, 2021
500b569
update
Nov 9, 2021
b16bb60
update
Dec 8, 2021
6007e33
modifs
Dec 8, 2021
de296b4
comment all stuff to make it compile, put TODO
Dec 14, 2021
8ad2aa4
update
Dec 15, 2021
edec510
update
Dec 16, 2021
2b5dc0d
update
Dec 16, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
51 changes: 26 additions & 25 deletions core/include/jiminy/core/robot/AbstractMotor.h
View file
Open in desktop
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
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

same.

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
Open in desktop
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;
fabinsch marked this conversation as resolved.
Show resolved Hide resolved

///////////////////////////////////////////////////////////////////////////////////////////////
/// \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;
fabinsch marked this conversation as resolved.
Show resolved Hide resolved

///////////////////////////////////////////////////////////////////////////////////////////////
/// \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,
fabinsch marked this conversation as resolved.
Show resolved Hide resolved
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,
fabinsch marked this conversation as resolved.
Show resolved Hide resolved
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 */
fabinsch marked this conversation as resolved.
Show resolved Hide resolved


///////////////////////////////////////////////////////////////////////////////////////////////
/// \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
Open in desktop
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
Open in desktop
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
Open in desktop
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 = {});
fabinsch marked this conversation as resolved.
Show resolved Hide resolved
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;
fabinsch marked this conversation as resolved.
Show resolved Hide resolved
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
Open in desktop
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