moveit2
The MoveIt Motion Planning Framework for ROS 2.
|
A joint from the robot. Models the transform that this joint applies in the kinematic chain. A joint consists of multiple variables. In the simplest case, when the joint is a single DOF, there is only one variable and its name is the same as the joint's name. For multi-DOF joints, each variable has a local name (e.g., x, y) but the full variable name as seen from the outside of this class is a concatenation of the "joint name"/"local name" (e.g., a joint named 'base' with local variables 'x' and 'y' will store its full variable names as 'base/x' and 'base/y'). Local names are never used to reference variables directly. More...
#include <joint_model.h>
Public Types | |
enum | JointType { UNKNOWN , REVOLUTE , PRISMATIC , PLANAR , FLOATING , FIXED } |
The different types of joints we support. More... | |
using | Bounds = std::vector< VariableBounds > |
The datatype for the joint bounds. | |
Public Member Functions | |
JointModel (const std::string &name, size_t joint_index, size_t first_variable_index) | |
Constructs a joint named name. | |
virtual | ~JointModel () |
const std::string & | getName () const |
Get the name of the joint. | |
JointType | getType () const |
Get the type of joint. | |
std::string | getTypeName () const |
Get the type of joint as a string. | |
const LinkModel * | getParentLinkModel () const |
Get the link that this joint connects to. The robot is assumed to start with a joint, so the root joint will return nullptr here. | |
const LinkModel * | getChildLinkModel () const |
Get the link that this joint connects to. There will always be such a link. | |
void | setParentLinkModel (const LinkModel *link) |
void | setChildLinkModel (const LinkModel *link) |
virtual double | distance (const double *value1, const double *value2) const =0 |
Compute the distance between two joint states of the same model (represented by the variable values) | |
double | getDistanceFactor () const |
Get the factor that should be applied to the value returned by distance() when that value is used in compound distances. | |
void | setDistanceFactor (double factor) |
Set the factor that should be applied to the value returned by distance() when that value is used in compound distances. | |
virtual unsigned int | getStateSpaceDimension () const =0 |
Get the dimension of the state space that corresponds to this joint. | |
const JointModel * | getMimic () const |
Get the joint this one is mimicking. | |
double | getMimicOffset () const |
If mimicking a joint, this is the offset added to that joint's value. | |
double | getMimicFactor () const |
If mimicking a joint, this is the multiplicative factor for that joint's value. | |
void | setMimic (const JointModel *mimic, double factor, double offset) |
Mark this joint as mimicking mimic using factor and offset. | |
const std::vector< const JointModel * > & | getMimicRequests () const |
The joint models whose values would be modified if the value of this joint changed. | |
void | addMimicRequest (const JointModel *joint) |
Notify this joint that there is another joint that mimics it. | |
void | addDescendantJointModel (const JointModel *joint) |
void | addDescendantLinkModel (const LinkModel *link) |
const std::vector< const LinkModel * > & | getDescendantLinkModels () const |
Get all the link models that descend from this joint, in the kinematic tree. | |
const std::vector< const JointModel * > & | getDescendantJointModels () const |
Get all the joint models that descend from this joint, in the kinematic tree. | |
const std::vector< const JointModel * > & | getNonFixedDescendantJointModels () const |
Get all the non-fixed joint models that descend from this joint, in the kinematic tree. | |
bool | isPassive () const |
Check if this joint is passive. | |
void | setPassive (bool flag) |
virtual void | interpolate (const double *from, const double *to, const double t, double *state) const =0 |
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state. The memory location of state is not required to be different from the memory of either from or to. | |
virtual double | getMaximumExtent (const Bounds &other_bounds) const =0 |
Get the extent of the state space (the maximum value distance() can ever report) | |
double | getMaximumExtent () const |
Reason about the variables that make up this joint | |
const std::vector< std::string > & | getVariableNames () const |
Get the names of the variables that make up this joint, in the order they appear in corresponding states. For single DOF joints, this will be just the joint name. For multi-DOF joints these will be the joint name followed by "/", followed by the local names of the variables. | |
const std::vector< std::string > & | getLocalVariableNames () const |
Get the local names of the variable that make up the joint (suffixes that are attached to joint names to construct the variable names). For single DOF joints, this will be empty. | |
bool | hasVariable (const std::string &variable) const |
Check if a particular variable is known to this joint. | |
std::size_t | getVariableCount () const |
Get the number of variables that describe this joint. | |
size_t | getFirstVariableIndex () const |
Get the index of this joint's first variable within the full robot state. | |
size_t | getJointIndex () const |
Get the index of this joint within the robot model. | |
size_t | getLocalVariableIndex (const std::string &variable) const |
Get the index of the variable within this joint. | |
Functionality specific to computing state values | |
void | getVariableDefaultPositions (double *values) const |
Provide a default value for the joint given the default joint variable bounds (maintained internally). Most joints will use the default implementation provided in this base class, but the quaternion for example needs a different implementation. Enough memory is assumed to be allocated. | |
virtual void | getVariableDefaultPositions (double *values, const Bounds &other_bounds) const =0 |
Provide a default value for the joint given the joint variable bounds. Most joints will use the default implementation provided in this base class, but the quaternion for example needs a different implementation. Enough memory is assumed to be allocated. | |
void | getVariableRandomPositions (random_numbers::RandomNumberGenerator &rng, double *values) const |
Provide random values for the joint variables (within default bounds). Enough memory is assumed to be allocated. | |
virtual void | getVariableRandomPositions (random_numbers::RandomNumberGenerator &rng, double *values, const Bounds &other_bounds) const =0 |
Provide random values for the joint variables (within specified bounds). Enough memory is assumed to be allocated. | |
void | getVariableRandomPositionsNearBy (random_numbers::RandomNumberGenerator &rng, double *values, const double *near, const double distance) const |
Provide random values for the joint variables (within default bounds). Enough memory is assumed to be allocated. | |
virtual void | getVariableRandomPositionsNearBy (random_numbers::RandomNumberGenerator &rng, double *values, const Bounds &other_bounds, const double *near, const double distance) const =0 |
Provide random values for the joint variables (within specified bounds). Enough memory is assumed to be allocated. | |
Functionality specific to verifying bounds | |
bool | satisfiesPositionBounds (const double *values, double margin=0.0) const |
Check if the set of values for the variables of this joint are within bounds. | |
virtual bool | satisfiesPositionBounds (const double *values, const Bounds &other_bounds, double margin) const =0 |
Check if the set of position values for the variables of this joint are within bounds, up to some margin. | |
bool | enforcePositionBounds (double *values) const |
Force the specified values to be inside bounds and normalized. Quaternions are normalized, continuous revolute joints are made between -Pi and Pi. Returns true if changes were made. | |
virtual bool | enforcePositionBounds (double *values, const Bounds &other_bounds) const =0 |
Force the specified values to be inside bounds and normalized. Quaternions are normalized, continuous revolute joints are made between -Pi and Pi. Return true if changes were made. | |
virtual bool | harmonizePosition (double *values, const Bounds &other_bounds) const |
bool | harmonizePosition (double *values) const |
bool | satisfiesVelocityBounds (const double *values, double margin=0.0) const |
Check if the set of velocities for the variables of this joint are within bounds. | |
virtual bool | satisfiesVelocityBounds (const double *values, const Bounds &other_bounds, double margin) const |
Check if the set of velocities for the variables of this joint are within bounds, up to some margin. | |
bool | enforceVelocityBounds (double *values) const |
Force the specified velocities to be within bounds. Return true if changes were made. | |
virtual bool | enforceVelocityBounds (double *values, const Bounds &other_bounds) const |
Force the specified velocities to be inside bounds. Return true if changes were made. | |
bool | satisfiesAccelerationBounds (const double *values, double margin=0.0) const |
Check if the set of accelerations for the variables of this joint are within bounds. | |
virtual bool | satisfiesAccelerationBounds (const double *values, const Bounds &other_bounds, double margin) const |
Check if the set of accelerations for the variables of this joint are within bounds, up to some margin. | |
bool | satisfiesJerkBounds (const double *values, double margin=0.0) const |
Check if the set of jerks for the variables of this joint are within bounds. | |
virtual bool | satisfiesJerkBounds (const double *values, const Bounds &other_bounds, double margin) const |
Check if the set of jerks for the variables of this joint are within bounds, up to some margin. | |
const VariableBounds & | getVariableBounds (const std::string &variable) const |
Get the bounds for a variable. Throw an exception if the variable was not found. | |
const Bounds & | getVariableBounds () const |
Get the variable bounds for this joint, in the same order as the names returned by getVariableNames() | |
void | setVariableBounds (const std::string &variable, const VariableBounds &bounds) |
Set the lower and upper bounds for a variable. Throw an exception if the variable was not found. | |
void | setVariableBounds (const std::vector< moveit_msgs::msg::JointLimits > &jlim) |
Override joint limits loaded from URDF. Unknown variables are ignored. | |
const std::vector< moveit_msgs::msg::JointLimits > & | getVariableBoundsMsg () const |
Get the joint limits known to this model, as a message. | |
Computing transforms | |
virtual void | computeTransform (const double *joint_values, Eigen::Isometry3d &transf) const =0 |
Given the joint values for a joint, compute the corresponding transform. The computed transform is guaranteed to be a valid isometry. | |
virtual void | computeVariablePositions (const Eigen::Isometry3d &transform, double *joint_values) const =0 |
Given the transform generated by joint, compute the corresponding joint values. Make sure the passed transform is a valid isometry. | |
Protected Member Functions | |
void | computeVariableBoundsMsg () |
Protected Attributes | |
JointType | type_ |
The type of joint. | |
std::vector< std::string > | local_variable_names_ |
The local names to use for the variables that make up this joint. | |
std::vector< std::string > | variable_names_ |
The full names to use for the variables that make up this joint. | |
Bounds | variable_bounds_ |
The bounds for each variable (low, high) in the same order as variable_names_. | |
std::vector< moveit_msgs::msg::JointLimits > | variable_bounds_msg_ |
VariableIndexMap | variable_index_map_ |
Map from variable names to the corresponding index in variable_names_ (indexing makes sense within the JointModel only) | |
const LinkModel * | parent_link_model_ |
The link before this joint. | |
const LinkModel * | child_link_model_ |
The link after this joint. | |
const JointModel * | mimic_ |
The joint this one mimics (nullptr for joints that do not mimic) | |
double | mimic_factor_ |
The multiplier to the mimic joint. | |
double | mimic_offset_ |
The offset to the mimic joint. | |
std::vector< const JointModel * > | mimic_requests_ |
The set of joints that should get a value copied to them when this joint changes. | |
std::vector< const LinkModel * > | descendant_link_models_ |
Pointers to all the links that will be moved if this joint changes value. | |
std::vector< const JointModel * > | descendant_joint_models_ |
Pointers to all the joints that follow this one in the kinematic tree (including mimic joints) | |
std::vector< const JointModel * > | non_fixed_descendant_joint_models_ |
Pointers to all the joints that follow this one in the kinematic tree, including mimic joints, but excluding fixed joints. | |
bool | passive_ |
Specify whether this joint is marked as passive in the SRDF. | |
double | distance_factor_ |
The factor applied to the distance between two joint states. | |
A joint from the robot. Models the transform that this joint applies in the kinematic chain. A joint consists of multiple variables. In the simplest case, when the joint is a single DOF, there is only one variable and its name is the same as the joint's name. For multi-DOF joints, each variable has a local name (e.g., x, y) but the full variable name as seen from the outside of this class is a concatenation of the "joint name"/"local name" (e.g., a joint named 'base' with local variables 'x' and 'y' will store its full variable names as 'base/x' and 'base/y'). Local names are never used to reference variables directly.
Definition at line 116 of file joint_model.h.
using moveit::core::JointModel::Bounds = std::vector<VariableBounds> |
The datatype for the joint bounds.
Definition at line 131 of file joint_model.h.
The different types of joints we support.
Enumerator | |
---|---|
UNKNOWN | |
REVOLUTE | |
PRISMATIC | |
PLANAR | |
FLOATING | |
FIXED |
Definition at line 120 of file joint_model.h.
moveit::core::JointModel::JointModel | ( | const std::string & | name, |
size_t | joint_index, | ||
size_t | first_variable_index | ||
) |
Constructs a joint named name.
[in] | name | The joint name |
[in] | index | The index of the joint in the RobotModel |
[in] | first_variable_index | The index of the first variable in the RobotModel |
Definition at line 47 of file joint_model.cpp.
|
virtualdefault |
void moveit::core::JointModel::addDescendantJointModel | ( | const JointModel * | joint | ) |
void moveit::core::JointModel::addDescendantLinkModel | ( | const LinkModel * | link | ) |
Definition at line 267 of file joint_model.cpp.
void moveit::core::JointModel::addMimicRequest | ( | const JointModel * | joint | ) |
Notify this joint that there is another joint that mimics it.
Definition at line 255 of file joint_model.cpp.
|
pure virtual |
Given the joint values for a joint, compute the corresponding transform. The computed transform is guaranteed to be a valid isometry.
Implemented in moveit::core::FixedJointModel, moveit::core::FloatingJointModel, moveit::core::PlanarJointModel, moveit::core::PrismaticJointModel, and moveit::core::RevoluteJointModel.
|
protected |
|
pure virtual |
Given the transform generated by joint, compute the corresponding joint values. Make sure the passed transform is a valid isometry.
Implemented in moveit::core::FixedJointModel, moveit::core::FloatingJointModel, moveit::core::PlanarJointModel, moveit::core::PrismaticJointModel, and moveit::core::RevoluteJointModel.
|
pure virtual |
Compute the distance between two joint states of the same model (represented by the variable values)
Implemented in moveit::core::FixedJointModel, moveit::core::FloatingJointModel, moveit::core::PlanarJointModel, moveit::core::PrismaticJointModel, and moveit::core::RevoluteJointModel.
|
inline |
Force the specified values to be inside bounds and normalized. Quaternions are normalized, continuous revolute joints are made between -Pi and Pi. Returns true if changes were made.
Definition at line 292 of file joint_model.h.
|
pure virtual |
Force the specified values to be inside bounds and normalized. Quaternions are normalized, continuous revolute joints are made between -Pi and Pi. Return true if changes were made.
Implemented in moveit::core::FixedJointModel, moveit::core::FloatingJointModel, moveit::core::PlanarJointModel, moveit::core::PrismaticJointModel, and moveit::core::RevoluteJointModel.
|
inline |
Force the specified velocities to be within bounds. Return true if changes were made.
Definition at line 320 of file joint_model.h.
|
virtual |
Force the specified velocities to be inside bounds. Return true if changes were made.
Definition at line 98 of file joint_model.cpp.
|
inline |
Get the link that this joint connects to. There will always be such a link.
Definition at line 168 of file joint_model.h.
|
inline |
Get all the joint models that descend from this joint, in the kinematic tree.
Definition at line 428 of file joint_model.h.
|
inline |
Get all the link models that descend from this joint, in the kinematic tree.
Definition at line 422 of file joint_model.h.
|
inline |
Get the factor that should be applied to the value returned by distance() when that value is used in compound distances.
Definition at line 374 of file joint_model.h.
|
inline |
Get the index of this joint's first variable within the full robot state.
Definition at line 216 of file joint_model.h.
|
inline |
Get the index of this joint within the robot model.
Definition at line 222 of file joint_model.h.
size_t moveit::core::JointModel::getLocalVariableIndex | ( | const std::string & | variable | ) | const |
Get the index of the variable within this joint.
Definition at line 85 of file joint_model.cpp.
|
inline |
Get the local names of the variable that make up the joint (suffixes that are attached to joint names to construct the variable names). For single DOF joints, this will be empty.
Definition at line 198 of file joint_model.h.
|
inline |
Definition at line 459 of file joint_model.h.
|
pure virtual |
Get the extent of the state space (the maximum value distance() can ever report)
Implemented in moveit::core::FixedJointModel, moveit::core::FloatingJointModel, moveit::core::PlanarJointModel, moveit::core::PrismaticJointModel, and moveit::core::RevoluteJointModel.
|
inline |
Get the joint this one is mimicking.
Definition at line 390 of file joint_model.h.
|
inline |
If mimicking a joint, this is the multiplicative factor for that joint's value.
Definition at line 402 of file joint_model.h.
|
inline |
If mimicking a joint, this is the offset added to that joint's value.
Definition at line 396 of file joint_model.h.
|
inline |
The joint models whose values would be modified if the value of this joint changed.
Definition at line 411 of file joint_model.h.
|
inline |
Get the name of the joint.
Definition at line 145 of file joint_model.h.
|
inline |
Get all the non-fixed joint models that descend from this joint, in the kinematic tree.
Definition at line 434 of file joint_model.h.
|
inline |
Get the link that this joint connects to. The robot is assumed to start with a joint, so the root joint will return nullptr here.
Definition at line 162 of file joint_model.h.
|
pure virtual |
Get the dimension of the state space that corresponds to this joint.
Implemented in moveit::core::FixedJointModel, moveit::core::FloatingJointModel, moveit::core::PlanarJointModel, moveit::core::PrismaticJointModel, and moveit::core::RevoluteJointModel.
|
inline |
Get the type of joint.
Definition at line 151 of file joint_model.h.
std::string moveit::core::JointModel::getTypeName | ( | ) | const |
Get the type of joint as a string.
Definition at line 64 of file joint_model.cpp.
|
inline |
Get the variable bounds for this joint, in the same order as the names returned by getVariableNames()
Definition at line 350 of file joint_model.h.
const VariableBounds & moveit::core::JointModel::getVariableBounds | ( | const std::string & | variable | ) | const |
Get the bounds for a variable. Throw an exception if the variable was not found.
Definition at line 177 of file joint_model.cpp.
|
inline |
Get the joint limits known to this model, as a message.
Definition at line 362 of file joint_model.h.
|
inline |
Get the number of variables that describe this joint.
Definition at line 210 of file joint_model.h.
|
inline |
Provide a default value for the joint given the default joint variable bounds (maintained internally). Most joints will use the default implementation provided in this base class, but the quaternion for example needs a different implementation. Enough memory is assumed to be allocated.
Definition at line 238 of file joint_model.h.
|
pure virtual |
Provide a default value for the joint given the joint variable bounds. Most joints will use the default implementation provided in this base class, but the quaternion for example needs a different implementation. Enough memory is assumed to be allocated.
Implemented in moveit::core::FixedJointModel, moveit::core::FloatingJointModel, moveit::core::PlanarJointModel, moveit::core::PrismaticJointModel, and moveit::core::RevoluteJointModel.
|
inline |
Get the names of the variables that make up this joint, in the order they appear in corresponding states. For single DOF joints, this will be just the joint name. For multi-DOF joints these will be the joint name followed by "/", followed by the local names of the variables.
Definition at line 190 of file joint_model.h.
|
inline |
Provide random values for the joint variables (within default bounds). Enough memory is assumed to be allocated.
Definition at line 250 of file joint_model.h.
|
pure virtual |
Provide random values for the joint variables (within specified bounds). Enough memory is assumed to be allocated.
Implemented in moveit::core::FixedJointModel, moveit::core::FloatingJointModel, moveit::core::PlanarJointModel, moveit::core::PrismaticJointModel, and moveit::core::RevoluteJointModel.
|
pure virtual |
Provide random values for the joint variables (within specified bounds). Enough memory is assumed to be allocated.
Implemented in moveit::core::FixedJointModel, moveit::core::FloatingJointModel, moveit::core::PlanarJointModel, moveit::core::PrismaticJointModel, and moveit::core::RevoluteJointModel.
|
inline |
Provide random values for the joint variables (within default bounds). Enough memory is assumed to be allocated.
Definition at line 262 of file joint_model.h.
|
inline |
|
virtual |
Harmonize position of revolute joints, adding/subtracting multiples of 2*Pi to bring them back into bounds. Return true if changes were made.
Reimplemented in moveit::core::RevoluteJointModel.
Definition at line 93 of file joint_model.cpp.
|
inline |
Check if a particular variable is known to this joint.
Definition at line 204 of file joint_model.h.
|
pure virtual |
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state. The memory location of state is not required to be different from the memory of either from or to.
Implemented in moveit::core::FixedJointModel, moveit::core::FloatingJointModel, moveit::core::PlanarJointModel, moveit::core::PrismaticJointModel, and moveit::core::RevoluteJointModel.
|
inline |
Check if this joint is passive.
Definition at line 440 of file joint_model.h.
|
virtual |
Check if the set of accelerations for the variables of this joint are within bounds, up to some margin.
Definition at line 137 of file joint_model.cpp.
|
inline |
Check if the set of accelerations for the variables of this joint are within bounds.
Definition at line 329 of file joint_model.h.
|
virtual |
Check if the set of jerks for the variables of this joint are within bounds, up to some margin.
Definition at line 157 of file joint_model.cpp.
|
inline |
Check if the set of jerks for the variables of this joint are within bounds.
Definition at line 338 of file joint_model.h.
|
pure virtual |
Check if the set of position values for the variables of this joint are within bounds, up to some margin.
Implemented in moveit::core::FixedJointModel, moveit::core::FloatingJointModel, moveit::core::PlanarJointModel, moveit::core::PrismaticJointModel, and moveit::core::RevoluteJointModel.
|
inline |
Check if the set of values for the variables of this joint are within bounds.
Definition at line 280 of file joint_model.h.
|
virtual |
Check if the set of velocities for the variables of this joint are within bounds, up to some margin.
Definition at line 117 of file joint_model.cpp.
|
inline |
Check if the set of velocities for the variables of this joint are within bounds.
Definition at line 311 of file joint_model.h.
|
inline |
Definition at line 178 of file joint_model.h.
|
inline |
Set the factor that should be applied to the value returned by distance() when that value is used in compound distances.
Definition at line 381 of file joint_model.h.
void moveit::core::JointModel::setMimic | ( | const JointModel * | mimic, |
double | factor, | ||
double | offset | ||
) |
Mark this joint as mimicking mimic using factor and offset.
Definition at line 248 of file joint_model.cpp.
|
inline |
Definition at line 173 of file joint_model.h.
|
inline |
Definition at line 445 of file joint_model.h.
void moveit::core::JointModel::setVariableBounds | ( | const std::string & | variable, |
const VariableBounds & | bounds | ||
) |
Set the lower and upper bounds for a variable. Throw an exception if the variable was not found.
Definition at line 182 of file joint_model.cpp.
void moveit::core::JointModel::setVariableBounds | ( | const std::vector< moveit_msgs::msg::JointLimits > & | jlim | ) |
Override joint limits loaded from URDF. Unknown variables are ignored.
Definition at line 188 of file joint_model.cpp.
|
protected |
The link after this joint.
Definition at line 512 of file joint_model.h.
|
protected |
Pointers to all the joints that follow this one in the kinematic tree (including mimic joints)
Definition at line 530 of file joint_model.h.
|
protected |
Pointers to all the links that will be moved if this joint changes value.
Definition at line 527 of file joint_model.h.
|
protected |
The factor applied to the distance between two joint states.
Definition at line 540 of file joint_model.h.
|
protected |
The local names to use for the variables that make up this joint.
Definition at line 494 of file joint_model.h.
|
protected |
The joint this one mimics (nullptr for joints that do not mimic)
Definition at line 515 of file joint_model.h.
|
protected |
The multiplier to the mimic joint.
Definition at line 518 of file joint_model.h.
|
protected |
The offset to the mimic joint.
Definition at line 521 of file joint_model.h.
|
protected |
The set of joints that should get a value copied to them when this joint changes.
Definition at line 524 of file joint_model.h.
|
protected |
Pointers to all the joints that follow this one in the kinematic tree, including mimic joints, but excluding fixed joints.
Definition at line 534 of file joint_model.h.
|
protected |
The link before this joint.
Definition at line 509 of file joint_model.h.
|
protected |
Specify whether this joint is marked as passive in the SRDF.
Definition at line 537 of file joint_model.h.
|
protected |
The type of joint.
Definition at line 491 of file joint_model.h.
|
protected |
The bounds for each variable (low, high) in the same order as variable_names_.
Definition at line 500 of file joint_model.h.
|
protected |
Definition at line 502 of file joint_model.h.
|
protected |
Map from variable names to the corresponding index in variable_names_ (indexing makes sense within the JointModel only)
Definition at line 506 of file joint_model.h.
|
protected |
The full names to use for the variables that make up this joint.
Definition at line 497 of file joint_model.h.