Skip to content

Commit

Permalink
[core] Add backlash support at motor-level.
Browse files Browse the repository at this point in the history
  • Loading branch information
@duburcqa
Alexis Duburcq authored and duburcqa committed Apr 18, 2024
1 parent f98b6e3 commit 40052fd
Show file tree
Hide file tree
Showing 24 changed files with 427 additions and 168 deletions.
1 change: 1 addition & 0 deletions core/include/jiminy/core/fwd.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -211,6 +211,7 @@ namespace jiminy

using FlexibilityConfig = std::vector<FlexibilityJointConfig>;

// Generic configuration holder
using GenericConfig =
std::unordered_map<std::string,
boost::make_recursive_variant<
Expand Down
21 changes: 17 additions & 4 deletions core/include/jiminy/core/hardware/abstract_motor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,8 @@ namespace jiminy
config["commandLimit"] = 0.0;
config["enableArmature"] = false;
config["armature"] = 0.0;
config["enableBacklash"] = false;
config["backlash"] = 0.0;

return config;
};
Expand All @@ -61,14 +63,18 @@ namespace jiminy
const double commandLimit;
const bool enableArmature;
const double armature;
const bool enableBacklash;
const double backlash;

AbstractMotorOptions(const GenericConfig & options) :
mechanicalReduction(boost::get<double>(options.at("mechanicalReduction"))),
enableCommandLimit(boost::get<bool>(options.at("enableCommandLimit"))),
commandLimitFromUrdf(boost::get<bool>(options.at("commandLimitFromUrdf"))),
commandLimit(boost::get<double>(options.at("commandLimit"))),
enableArmature(boost::get<bool>(options.at("enableArmature"))),
armature(boost::get<double>(options.at("armature")))
armature(boost::get<double>(options.at("armature"))),
enableBacklash(boost::get<bool>(options.at("enableBacklash"))),
backlash(boost::get<double>(options.at("backlash")))
{
}
};
Expand Down Expand Up @@ -144,6 +150,9 @@ namespace jiminy
/// \brief Rotor inertia of the motor.
double getArmature() const;

/// \brief Backlash of the transmission.
double getBacklash() const;

/// \brief Request the motor to update its actual effort based of the input data.
///
/// \details It assumes that the internal state of the robot is consistent with the input
Expand Down Expand Up @@ -190,7 +199,8 @@ namespace jiminy
///
/// \details This method must be called before initializing the sensor.
void attach(std::weak_ptr<const Robot> robot,
std::function<void(AbstractMotorBase & /*motor*/)> notifyRobot,
std::function<void(
AbstractMotorBase & /*motor*/, bool /*hasChanged*/)> notifyRobot,
MotorSharedStorage * sharedStorage);

/// \brief Detach the sensor from the robot.
Expand All @@ -216,14 +226,17 @@ namespace jiminy
Eigen::Index jointVelocityIndex_{0};
double commandLimit_{0.0};
double armature_{0.0};
double backlash_{0.0};

private:
/// \brief Name of the motor.
std::string name_;
/// \brief Flag to determine whether the motor is attached to a robot.
bool isAttached_{false};
/// \brief Notify the robot that the configuration of the sensors have changed.
std::function<void(AbstractMotorBase &)> notifyRobot_{};
/// \brief Whether the robot must be notified.
bool mustNotifyRobot_{false};
/// \brief Notify the robot that the configuration of the motor have changed.
std::function<void(AbstractMotorBase &, bool)> notifyRobot_{};
/// \brief Shared data between every motors associated with the robot.
MotorSharedStorage * sharedStorage_{nullptr};
};
Expand Down
17 changes: 10 additions & 7 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
inline constexpr std::string_view JOINT_PREFIX_BASE{"current"};
inline constexpr std::string_view FREE_FLYER_NAME{"Freeflyer"};
inline constexpr std::string_view FLEXIBLE_JOINT_SUFFIX{"Flexibility"};
inline constexpr std::string_view BACKLASH_JOINT_SUFFIX{"Backlash"};

class AbstractConstraintBase;
class FrameConstraint;
Expand Down Expand Up @@ -135,7 +136,6 @@ namespace jiminy
virtual GenericConfig getDefaultJointOptions()
{
GenericConfig config;
config["enablePositionLimit"] = true;
config["positionLimitFromUrdf"] = true;
config["positionLimitMin"] = Eigen::VectorXd{};
config["positionLimitMax"] = Eigen::VectorXd{};
Expand Down Expand Up @@ -182,7 +182,6 @@ namespace jiminy

struct JointOptions
{
const bool enablePositionLimit;
const bool positionLimitFromUrdf;
/// \brief Min position limit of all the mechanical joints of the theoretical model.
const Eigen::VectorXd positionLimitMin;
Expand All @@ -192,7 +191,6 @@ namespace jiminy
const Eigen::VectorXd velocityLimit;

JointOptions(const GenericConfig & options) :
enablePositionLimit{boost::get<bool>(options.at("enablePositionLimit"))},
positionLimitFromUrdf{boost::get<bool>(options.at("positionLimitFromUrdf"))},
positionLimitMin{boost::get<Eigen::VectorXd>(options.at("positionLimitMin"))},
positionLimitMax{boost::get<Eigen::VectorXd>(options.at("positionLimitMax"))},
Expand Down Expand Up @@ -350,6 +348,8 @@ namespace jiminy
const std::vector<Eigen::Index> & getMechanicalJointVelocityIndices() const;
const std::vector<std::string> & getFlexibilityJointNames() const;
const std::vector<pinocchio::JointIndex> & getFlexibilityJointIndices() const;
const std::vector<std::string> & getBacklashJointNames() const;
const std::vector<pinocchio::JointIndex> & getBacklashJointIndices() const;

const Eigen::VectorXd & getPositionLimitMin() const;
const Eigen::VectorXd & getPositionLimitMax() const;
Expand All @@ -372,6 +372,7 @@ namespace jiminy
protected:
void generateModelExtended(const uniform_random_bit_generator_ref<uint32_t> & g);

virtual void initializeExtendedModel();
void addFlexibilityJointsToExtendedModel();
void addBiasedToExtendedModel(const uniform_random_bit_generator_ref<uint32_t> & g);

Expand Down Expand Up @@ -440,12 +441,14 @@ namespace jiminy
/// \brief All the indices of the mechanical joints in the velocity vector of the robot,
/// ie including all their respective degrees of freedom.
std::vector<Eigen::Index> mechanicalJointVelocityIndices_{};
/// \brief Name of the flexibility joints of the robot regardless of whether the
/// flexibilities are enabled.
/// \brief Name of the flexibility joints of the robot if enabled.
std::vector<std::string> flexibilityJointNames_{};
/// \brief Index of the flexibility joints in the pinocchio robot regardless of whether the
/// flexibilities are enabled.
/// \brief Index of the flexibility joints in the pinocchio robot if enabled.
std::vector<pinocchio::JointIndex> flexibilityJointIndices_{};
/// \brief Name of the backlash joints of the robot if enabled.
std::vector<std::string> backlashJointNames_{};
/// \brief Index of the backlash joints in the pinocchio robot if enabled.
std::vector<pinocchio::JointIndex> backlashJointIndices_{};

/// \brief Store constraints.
ConstraintTree constraints_{};
Expand Down
9 changes: 7 additions & 2 deletions core/include/jiminy/core/robot/robot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ namespace jiminy
std::weak_ptr<const AbstractMotorBase> getMotor(const std::string & motorName) const;
const MotorVector & getMotors() const;
void detachMotor(const std::string & motorName);
void detachMotors(std::vector<std::string> motorsNames = {});
void detachMotors(std::vector<std::string> motorNames = {});
void attachSensor(std::shared_ptr<AbstractSensorBase> sensor);
std::shared_ptr<AbstractSensorBase> getSensor(const std::string & sensorType,
const std::string & sensorName);
Expand Down Expand Up @@ -134,7 +134,12 @@ namespace jiminy
protected:
void refreshMotorProxies();
void refreshSensorProxies();
virtual void refreshProxies() override;
void refreshProxies() override;

void initializeExtendedModel() override;

private:
void detachMotor(const std::string & motorName, bool triggerReset);

protected:
bool isTelemetryConfigured_{false};
Expand Down
4 changes: 4 additions & 0 deletions core/include/jiminy/core/utilities/pinocchio.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,10 @@ namespace jiminy

void addFlexibilityJointAtFixedFrame(pinocchio::Model & model, const std::string & frameName);

void addBacklashJointAfterMechanicalJoint(pinocchio::Model & model,
const std::string & parentJointName,
const std::string & newJointName);

Eigen::MatrixXd JIMINY_DLLAPI interpolatePositions(const pinocchio::Model & model,
const Eigen::VectorXd & timesIn,
const Eigen::MatrixXd & positionsIn,
Expand Down
71 changes: 33 additions & 38 deletions core/src/engine/engine.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1005,8 +1005,7 @@ namespace jiminy
/* Check that the initial configuration is not out-of-bounds if appropriate.
Note that EPS allows to be very slightly out-of-bounds, which may occurs because of
rounding errors. */
if ((robot->modelOptions_->joints.enablePositionLimit &&
(contactModel_ == ContactModelType::CONSTRAINT) &&
if (((contactModel_ == ContactModelType::CONSTRAINT) &&
((EPS < q.array() - robot->getPositionLimitMax().array()).any() ||
(EPS < robot->getPositionLimitMin().array() - q.array()).any())))
{
Expand Down Expand Up @@ -1264,7 +1263,6 @@ namespace jiminy
const ConstraintTree & constraints = (*robotIt)->getConstraints();
constraints.foreach(
[&contactModel = contactModel_,
&enablePositionLimit = (*robotIt)->modelOptions_->joints.enablePositionLimit,
&freq = engineOptions_->contacts.stabilizationFreq](
const std::shared_ptr<AbstractConstraintBase> & constraint,
ConstraintNodeType node)
Expand All @@ -1281,15 +1279,11 @@ namespace jiminy
switch (node)
{
case ConstraintNodeType::BOUNDS_JOINTS:
if (!enablePositionLimit)
{
return;
}
{
auto & jointConstraint =
static_cast<JointConstraint &>(*constraint.get());
jointConstraint.setRotationDir(false);
}
{
auto & jointConstraint =
static_cast<JointConstraint &>(*constraint.get());
jointConstraint.setRotationDir(false);
}
[[fallthrough]];
case ConstraintNodeType::CONTACT_FRAMES:
case ConstraintNodeType::COLLISION_BODIES:
Expand Down Expand Up @@ -3519,38 +3513,39 @@ namespace jiminy
const pinocchio::Data & data = robot->pinocchioData_;
const ConstraintTree & constraints = robot->getConstraints();

// Enforce the position limit (mechanical joints only)
if (robot->modelOptions_->joints.enablePositionLimit)
/* Enforce position limits for all joints having bounds constraints, ie mechanical and
backlash joints. */
const Eigen::VectorXd & positionLimitMin = robot->getPositionLimitMin();
const Eigen::VectorXd & positionLimitMax = robot->getPositionLimitMax();
for (auto & constraintItem : constraints.boundJoints)
{
const Eigen::VectorXd & positionLimitMin = robot->getPositionLimitMin();
const Eigen::VectorXd & positionLimitMax = robot->getPositionLimitMax();
const std::vector<pinocchio::JointIndex> & mechanicalJointIndices =
robot->getMechanicalJointIndices();
for (std::size_t i = 0; i < mechanicalJointIndices.size(); ++i)
{
auto & constraint = constraints.boundJoints[i].second;
computePositionLimitsForcesAlgo::run(
model.joints[mechanicalJointIndices[i]],
typename computePositionLimitsForcesAlgo::ArgsType(data,
q,
v,
positionLimitMin,
positionLimitMax,
engineOptions_,
contactModel_,
constraint,
uInternal));
}
}

// Enforce the velocity limit (mechanical joints only)
auto & constraint = constraintItem.second;
const auto jointConstraint = std::static_pointer_cast<JointConstraint>(constraint);
const pinocchio::JointIndex jointIndex = jointConstraint->getJointIndex();
computePositionLimitsForcesAlgo::run(
model.joints[jointIndex],
typename computePositionLimitsForcesAlgo::ArgsType(data,
q,
v,
positionLimitMin,
positionLimitMax,
engineOptions_,
contactModel_,
constraint,
uInternal));
}

// Enforce velocity limits for all joints having bounds constraints if requested
if (robot->modelOptions_->joints.enableVelocityLimit)
{
const Eigen::VectorXd & velocityLimitMax = robot->getVelocityLimit();
for (pinocchio::JointIndex mechanicalJointIndex : robot->getMechanicalJointIndices())
for (auto & constraintItem : constraints.boundJoints)
{
auto & constraint = constraintItem.second;
const auto jointConstraint = std::static_pointer_cast<JointConstraint>(constraint);
const pinocchio::JointIndex jointIndex = jointConstraint->getJointIndex();
computeVelocityLimitsForcesAlgo::run(
model.joints[mechanicalJointIndex],
model.joints[jointIndex],
typename computeVelocityLimitsForcesAlgo::ArgsType(
data, v, velocityLimitMax, engineOptions_, contactModel_, uInternal));
}
Expand Down
47 changes: 37 additions & 10 deletions core/src/hardware/abstract_motor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,9 +23,10 @@ namespace jiminy
}
}

void AbstractMotorBase::attach(std::weak_ptr<const Robot> robot,
std::function<void(AbstractMotorBase & /*motor*/)> notifyRobot,
MotorSharedStorage * sharedStorage)
void AbstractMotorBase::attach(
std::weak_ptr<const Robot> robot,
std::function<void(AbstractMotorBase & /*motor*/, bool /*hasChanged*/)> notifyRobot,
MotorSharedStorage * sharedStorage)
{
// Make sure the motor is not already attached
if (isAttached_)
Expand Down Expand Up @@ -163,28 +164,37 @@ namespace jiminy
}

// Check if the internal buffers must be updated
bool internalBuffersMustBeUpdated = false;
if (isInitialized_)
{
// Check if armature has changed
const bool enableArmature = boost::get<bool>(motorOptions.at("enableArmature"));
internalBuffersMustBeUpdated |= (baseMotorOptions_->enableArmature != enableArmature);
mustNotifyRobot_ |= (baseMotorOptions_->enableArmature != enableArmature);
if (enableArmature)
{
const double armature = boost::get<double>(motorOptions.at("armature"));
internalBuffersMustBeUpdated |= //
mustNotifyRobot_ |= //
std::abs(armature - baseMotorOptions_->armature) > EPS;
}

// Check if backlash has changed
const bool enableBacklash = boost::get<bool>(motorOptions.at("enableBacklash"));
mustNotifyRobot_ |= (baseMotorOptions_->enableBacklash != enableBacklash);
if (enableBacklash)
{
const double backlash = boost::get<double>(motorOptions.at("backlash"));
mustNotifyRobot_ |= //
std::abs(backlash - baseMotorOptions_->backlash) > EPS;
}

// Check if command limit has changed
const bool commandLimitFromUrdf =
boost::get<bool>(motorOptions.at("commandLimitFromUrdf"));
internalBuffersMustBeUpdated |=
mustNotifyRobot_ |=
(baseMotorOptions_->commandLimitFromUrdf != commandLimitFromUrdf);
if (!commandLimitFromUrdf)
{
const double commandLimit = boost::get<double>(motorOptions.at("commandLimit"));
internalBuffersMustBeUpdated |=
mustNotifyRobot_ |=
std::abs(commandLimit - baseMotorOptions_->commandLimit) > EPS;
}
}
Expand All @@ -198,7 +208,7 @@ namespace jiminy
// Refresh the proxies if the robot is initialized if available
if (robot)
{
if (internalBuffersMustBeUpdated && robot->getIsInitialized() && isAttached_)
if (mustNotifyRobot_ && robot->getIsInitialized() && isAttached_)
{
refreshProxies();
}
Expand Down Expand Up @@ -274,10 +284,22 @@ namespace jiminy
armature_ = 0.0;
}

// Get the transmission backlash
if (baseMotorOptions_->enableBacklash)
{
backlash_ = baseMotorOptions_->backlash;
}
else
{
backlash_ = 0.0;
}

// Propagate the user-defined motor inertia at Pinocchio model level
if (notifyRobot_)
{
notifyRobot_(*this);
const bool mustNotifyRobot = mustNotifyRobot_;
mustNotifyRobot_ = false;
notifyRobot_(*this, mustNotifyRobot);
}
}

Expand Down Expand Up @@ -370,6 +392,11 @@ namespace jiminy
return armature_;
}

double AbstractMotorBase::getBacklash() const
{
return backlash_;
}

void AbstractMotorBase::computeEffortAll(double t,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
Expand Down
Loading

0 comments on commit 40052fd

Please sign in to comment.