Skip to content

Commit

Permalink
update
Browse files Browse the repository at this point in the history
  • Loading branch information
fabians committed Nov 8, 2021
1 parent 06417c6 commit 225724e
Show file tree
Hide file tree
Showing 8 changed files with 243 additions and 148 deletions.
30 changes: 15 additions & 15 deletions core/include/jiminy/core/robot/AbstractTransmission.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
///
Expand Down Expand Up @@ -121,49 +128,42 @@ namespace jiminy
///
/// \details It is the name of the joints associated with the transmission.
///////////////////////////////////////////////////////////////////////////////////////////////
std::vector<std::string> const & getJointName(void) const;
std::vector<std::string> const & getJointNames(void) const;

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get jointModelIdx_.
///
/// \details It is the index of the joints associated with the transmission in the kinematic tree.
///////////////////////////////////////////////////////////////////////////////////////////////
std::vector<jointIndex_t >const & getJointModelIdx(void) const;
std::vector<jointIndex_t >const & getJointModelIndices(void) const;

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get jointType_.
///
/// \details It is the type of joints associated with the transmission.
///////////////////////////////////////////////////////////////////////////////////////////////
std::vector<joint_t> const & getJointType(void) const;
std::vector<joint_t> 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<int32_t> const & getJointPositionIdx(void) const;
std::vector<int32_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<int32_t> const & getJointVelocityIdx(void) const;
std::vector<int32_t> const & getJointVelocityIndices(void) const;

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get motorName_.
///
/// \details It is the name of the motors associated with the transmission.
///////////////////////////////////////////////////////////////////////////////////////////////
std::vector<std::string> const & getMotorName(void) const;

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Get ActuatedJoints.
///
/// \details It is a list of joints that are attached to a transmission
///////////////////////////////////////////////////////////////////////////////////////////////
std::vector<std::string> const & robot.getActuatedJoints(void) const;
std::vector<std::string> const & getMotorNames(void) const;

///////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Compute forward transmission.
Expand Down Expand Up @@ -247,8 +247,8 @@ namespace jiminy
std::vector<std::string> jointNames_;
std::vector<jointIndex_t> jointModelIndices_;
std::vector<joint_t> jointTypes_;
std::vector<int32_t> jointPositionIndices_;
std::vector<int32_t> jointVelocityIndices_;
vectorN_t jointPositionIndices_;
vectorN_t jointVelocityIndices_;
std::vector<std::string> motorNames_;
std::vector<std::weak_ptr<AbstractMotorBase> > motors_;
matrixN_t forwardTransform_;
Expand Down
14 changes: 4 additions & 10 deletions core/include/jiminy/core/robot/BasicTransmissions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<vectorN_t> q,
float64_t v,
float64_t const & a,
float64_t command) final override;
virtual float64_t computeTransform(Eigen::VectorBlock<vectorN_t> /* q */,
Eigen::VectorBlock<vectorN_t> /* v */) final override;

virtual float64_t computeInverseTransform(float64_t const & t,
Eigen::VectorBlock<vectorN_t> q,
float64_t v,
float64_t const & a,
float64_t command) final override;
virtual float64_t computeInverseTransform(Eigen::VectorBlock<vectorN_t> /* q */,
Eigen::VectorBlock<vectorN_t> /* v */) final override;

private:
std::unique_ptr<transmissionOptions_t const> transmissionOptions_;
Expand Down
13 changes: 10 additions & 3 deletions core/include/jiminy/core/robot/Model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@ namespace jiminy
class AbstractConstraintBase;
class FixedFrameConstraint;
class JointConstraint;
class TransmisionConstraint;

template<typename DerivedConstraint>
using constraintsMapTpl_t = static_map_t<std::string, std::shared_ptr<DerivedConstraint> >;
Expand All @@ -26,14 +27,16 @@ namespace jiminy
BOUNDS_JOINTS = 0,
CONTACT_FRAMES = 1,
COLLISION_BODIES = 2,
USER = 3
USER = 3,
TRANSMISSIONS = 4
};

std::array<constraintsHolderType_t, 4> const constraintsHolderTypeRange = {{
std::array<constraintsHolderType_t, 5> 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
Expand Down Expand Up @@ -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:
Expand All @@ -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<constraintsMap_t> collisionBodies; ///< Store internal constraints related to collision bounds
constraintsMap_t registered; ///< Store internal constraints registered by user
};
Expand Down
4 changes: 4 additions & 0 deletions core/include/jiminy/core/robot/Robot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@ namespace jiminy
{
struct MotorSharedDataHolder_t;
class AbstractMotorBase;
class AbstractTransmissionBase;
struct SensorSharedDataHolder_t;
class AbstractSensorBase;
class TelemetryData;
Expand Down Expand Up @@ -54,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 detachTransmissions(std::vector<std::string> const & transmissionsNames = {});
hresult_t attachSensor(std::shared_ptr<AbstractSensorBase> sensor);
hresult_t getSensor(std::string const & sensorType,
std::string const & sensorName,
Expand Down Expand Up @@ -125,6 +127,8 @@ 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);

vectorN_t const & getCommandLimit(void) const;
vectorN_t const & getArmatures(void) const;
Expand Down
32 changes: 25 additions & 7 deletions core/src/robot/AbstractMotor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,9 +59,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_;
Expand All @@ -86,10 +91,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)
Expand Down Expand Up @@ -132,7 +147,10 @@ namespace jiminy
}

// Clear the shared data buffer
sharedHolder_->data_.setZero();
sharedHolder_->position_.setZero();
sharedHolder_->veloctiy_.setZero();
sharedHolder_->acceleration_.setZero();
sharedHolder_->effort_.setZero();

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

0 comments on commit 225724e

Please sign in to comment.