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 a9a2c74
Show file tree
Hide file tree
Showing 7 changed files with 217 additions and 141 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
3 changes: 3 additions & 0 deletions core/include/jiminy/core/robot/Robot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,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 +126,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
154 changes: 113 additions & 41 deletions core/src/robot/AbstractTransmission.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand All @@ -38,8 +36,56 @@ namespace jiminy
}
}

hresult_t AbstractTransmissionBase::attach(std::weak_ptr<Robot const> robot,
std::function<hresult_t(AbstractTransmissionBase & /*transmission*/)> notifyRobot)
hresult_t AbstractTransmissionBase::initialize(void)
{
// Populate jointPositionIndices_
std::vector<int32_t> jointPositionIndices;
returnCode = hresult_t::SUCCESS;
for (std::string const & jointName : jointNames_)
{
std::vector<int32_t> 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<int32_t> jointVelocityIndices;
for (std::string const & jointName : jointNames_)
{
std::vector<int32_t> 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 const> robot)
{
// Make sure the transmission is not already attached
if (isAttached_)
Expand All @@ -56,23 +102,22 @@ namespace jiminy
}

// Make sure the joint is not already attached to a transmission
std::vector<std::string>::iterator transmissionJointsIt = AbstractTransmissionBase::getJointName().begin();

for ( ; transmissionJointsIt != AbstractTransmissionBase::getJointName().end(); ++transmissionJointsIt)
std::vector<std::string>::iterator actuatedJointIt = AbstractTransmissionBase::robot.getActuatedJoints.begin();

for ( ; actuatedJointIt != AbstractTransmissionBase::robot.getActuatedJoints.end(); ++actuatedJointIt)
std_vector<std::string> 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;
Expand Down Expand Up @@ -111,7 +156,6 @@ namespace jiminy

// Clear the references to the robot and shared data
robot_.reset();
notifyRobot_ = nullptr;
sharedHolder_ = nullptr;

// Unset the Id
Expand Down Expand Up @@ -158,12 +202,12 @@ namespace jiminy
bool_t internalBuffersMustBeUpdated = false;
if (isInitialized_)
{
// Check if armature has changed
bool_t const & enableArmature = boost::get<bool_t>(transmissionOptions.at("enableArmature"));
internalBuffersMustBeUpdated |= (baseTransmissionOptions_->enableArmature != enableArmature);
if (enableArmature)
// Check if reduction ratio has changed
float64_t const & mechanicalReduction = boost::get<float64_t>(transmissionOptions.at("mechanicalReduction"));
internalBuffersMustBeUpdated |= (baseTransmissionOptions_->mechanicalReduction != mechanicalReduction);
if (mechanicalReduction)
{
float64_t const & armature = boost::get<float64_t>(transmissionOptions.at("armature"));
float64_t const & mechanicalReduction = boost::get<float64_t>(transmissionOptions.at("mechanicalReduction"));
internalBuffersMustBeUpdated |= std::abs(armature - baseTransmissionOptions_->armature) > EPS;
}
}
Expand Down Expand Up @@ -320,32 +364,50 @@ namespace jiminy
return transmissionIdx_;
}

std::vector<std>::string const & AbstractTransmissionBase::getJointName(void) const
std::vector<std>::string const & AbstractTransmissionBase::getJointNames(void) const
{
return jointName_;
return jointNames_;
}

std::vector<jointIndex_t> const & AbstractTransmissionBase::getJointModelIdx(void) const
std::vector<jointIndex_t> 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<joint_t> const & AbstractTransmissionBase::getJointType(void) const
std::vector<joint_t> 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<int32_t> 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<std>::string const & AbstractTransmissionBase::getMotorName(void) const
std::vector<std>::string const & AbstractTransmissionBase::getMotorNames(void) const
{
return motorName_;
}
Expand All @@ -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,
Expand All @@ -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;
}
}
Loading

0 comments on commit a9a2c74

Please sign in to comment.