moveit2
The MoveIt Motion Planning Framework for ROS 2.
|
Core components of MoveIt. More...
Classes | |
class | AABB |
Represents an axis-aligned bounding box. More... | |
class | AttachedBody |
Object defining bodies that can be attached to robot links. More... | |
class | CartesianInterpolator |
struct | CartesianPrecision |
class | FixedJointModel |
A fixed joint. More... | |
class | FloatingJointModel |
A floating joint. More... | |
class | JointModel |
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... | |
class | JointModelGroup |
struct | JumpThreshold |
Struct with options for defining joint-space jump thresholds. More... | |
class | LinkModel |
A link from the robot. Contains the constant transform applied to the link and its geometry. More... | |
struct | MaxEEFStep |
Struct for containing max_step for computeCartesianPath. More... | |
class | MoveItErrorCode |
a wrapper around moveit_msgs::MoveItErrorCodes to make it easier to return an error code message from a function More... | |
class | PlanarJointModel |
A planar joint. More... | |
class | PrismaticJointModel |
A prismatic joint. More... | |
class | RevoluteJointModel |
A revolute joint. More... | |
class | RobotModel |
Definition of a kinematic model. This class is not thread safe, however multiple instances can be created. More... | |
class | RobotModelBuilder |
Easily build different robot models for testing. Essentially a programmer-friendly light wrapper around URDF and SRDF. Best shown by an example: More... | |
class | RobotState |
Representation of a robot's state. This includes position, velocity, acceleration and effort. More... | |
class | Transforms |
Provides an implementation of a snapshot of a transform tree that can be easily queried for transforming different quantities. Transforms are maintained as a list of transforms to a particular frame. All stored transforms are considered fixed. More... | |
struct | VariableBounds |
Typedefs | |
typedef std::map< std::string, size_t > | VariableIndexMap |
Data type for holding mappings from variable names to their position in a state vector. | |
using | VariableBoundsMap = std::map< std::string, VariableBounds > |
Data type for holding mappings from variable names to their bounds. | |
using | JointModelMap = std::map< std::string, JointModel * > |
Map of names to instances for JointModel. | |
using | JointModelMapConst = std::map< std::string, const JointModel * > |
Map of names to const instances for JointModel. | |
typedef std::function< kinematics::KinematicsBasePtr(const JointModelGroup *)> | SolverAllocatorFn |
Function type that allocates a kinematics solver for a particular group. | |
using | SolverAllocatorMapFn = std::map< const JointModelGroup *, SolverAllocatorFn > |
Map from group instances to allocator functions & bijections. | |
using | JointModelGroupMap = std::map< std::string, JointModelGroup * > |
Map of names to instances for JointModelGroup. | |
using | JointModelGroupMapConst = std::map< std::string, const JointModelGroup * > |
Map of names to const instances for JointModelGroup. | |
using | JointBoundsVector = std::vector< const JointModel::Bounds * > |
typedef std::map< std::string, LinkModel * > | LinkModelMap |
Map of names to instances for LinkModel. | |
using | LinkModelMapConst = std::map< std::string, const LinkModel * > |
Map of names to const instances for LinkModel. | |
using | LinkTransformMap = std::map< const LinkModel *, Eigen::Isometry3d, std::less< const LinkModel * >, Eigen::aligned_allocator< std::pair< const LinkModel *const, Eigen::Isometry3d > > > |
Map from link model instances to Eigen transforms. | |
typedef std::function< void(AttachedBody *body, bool attached)> | AttachedBodyCallback |
typedef std::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> | GroupStateValidityCallbackFn |
Signature for functions that can verify that if the group joint_group in robot_state is set to joint_group_variable_values the state is valid or not. Returns true if the state is valid. This call is allowed to modify robot_state (e.g., set joint_group_variable_values) | |
using | FixedTransformsMap = std::map< std::string, Eigen::Isometry3d, std::less< std::string >, Eigen::aligned_allocator< std::pair< const std::string, Eigen::Isometry3d > > > |
Map frame names to the transformation matrix that can transform objects from the frame name to the planning frame. | |
Functions | |
MOVEIT_CLASS_FORWARD (JointModelGroup) | |
MOVEIT_CLASS_FORWARD (RobotState) | |
MOVEIT_CLASS_FORWARD (RobotModel) | |
std::ostream & | operator<< (std::ostream &out, const VariableBounds &b) |
Operator overload for printing variable bounds to a stream. | |
void | computeTurnDriveTurnGeometry (const double *from, const double *to, const double min_translational_distance, double &dx, double &dy, double &initial_turn, double &drive_angle, double &final_turn) |
Compute the geometry to turn toward the target point, drive straight and then turn to target orientation. | |
std::optional< int > | hasJointSpaceJump (const std::vector< moveit::core::RobotStatePtr > &waypoints, const moveit::core::JointModelGroup &group, const moveit::core::JumpThreshold &jump_threshold) |
Checks if a joint-space path has a jump larger than the given threshold. | |
bool | jointStateToRobotState (const sensor_msgs::msg::JointState &joint_state, RobotState &state) |
Convert a joint state to a MoveIt robot state. | |
bool | robotStateMsgToRobotState (const Transforms &tf, const moveit_msgs::msg::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true) |
Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state. | |
bool | robotStateMsgToRobotState (const moveit_msgs::msg::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true) |
Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state. | |
void | robotStateToRobotStateMsg (const RobotState &state, moveit_msgs::msg::RobotState &robot_state, bool copy_attached_bodies=true) |
Convert a MoveIt robot state to a robot state message. | |
void | attachedBodiesToAttachedCollisionObjectMsgs (const std::vector< const AttachedBody * > &attached_bodies, std::vector< moveit_msgs::msg::AttachedCollisionObject > &attached_collision_objs) |
Convert AttachedBodies to AttachedCollisionObjects. | |
void | robotStateToJointStateMsg (const RobotState &state, sensor_msgs::msg::JointState &joint_state) |
Convert a MoveIt robot state to a joint state message. | |
bool | jointTrajPointToRobotState (const trajectory_msgs::msg::JointTrajectory &trajectory, std::size_t point_id, RobotState &state) |
Convert a joint trajectory point to a MoveIt robot state. | |
void | robotStateToStream (const RobotState &state, std::ostream &out, bool include_header=true, const std::string &separator=",") |
Convert a MoveIt robot state to common separated values (CSV) on a single line that is outputted to a stream e.g. for file saving. | |
void | robotStateToStream (const RobotState &state, std::ostream &out, const std::vector< std::string > &joint_groups_ordering, bool include_header=true, const std::string &separator=",") |
Convert a MoveIt robot state to common separated values (CSV) on a single line that is outputted to a stream e.g. for file saving. This version can order by joint model groups. | |
void | streamToRobotState (RobotState &state, const std::string &line, const std::string &separator=",") |
Convert a string of joint values from a file (CSV) or input source into a RobotState. | |
std::ostream & | operator<< (std::ostream &out, const RobotState &s) |
Operator overload for printing variable bounds to a stream. | |
MOVEIT_CLASS_FORWARD (Transforms) | |
std::string | toString (double d) |
Convert a double to std::string using the classic C locale. | |
std::string | toString (float f) |
Convert a float to std::string using the classic C locale. | |
double | toDouble (const std::string &s) |
Converts a std::string to double using the classic C locale. | |
float | toFloat (const std::string &s) |
Converts a std::string to float using the classic C locale. | |
bool | isEmpty (const moveit_msgs::msg::PlanningScene &msg) |
Check if a message includes any information about a planning scene, or whether it is empty. | |
bool | isEmpty (const moveit_msgs::msg::PlanningSceneWorld &msg) |
Check if a message includes any information about a planning scene world, or whether it is empty. | |
bool | isEmpty (const moveit_msgs::msg::RobotState &msg) |
Check if a message includes any information about a robot state, or whether it is empty. | |
bool | isEmpty (const moveit_msgs::msg::Constraints &msg) |
Check if a message specifies constraints. | |
bool | isEmpty (const geometry_msgs::msg::Quaternion &msg) |
Check if the message contains any non-zero entries. | |
bool | isEmpty (const geometry_msgs::msg::Pose &msg) |
Check if the message contains any non-zero entries. | |
std::string | errorCodeToString (const MoveItErrorCode &error_code) |
Convenience function to translated error message into string. | |
moveit::core::RobotModelPtr | loadTestingRobotModel (const std::string &package_name, const std::string &urdf_relative_path, const std::string &srdf_relative_path) |
Loads a robot model given a URDF and SRDF file in a package. | |
moveit::core::RobotModelPtr | loadTestingRobotModel (const std::string &robot_name) |
Loads a robot from moveit_resources. | |
urdf::ModelInterfaceSharedPtr | loadModelInterface (const std::string &robot_name) |
Loads a URDF Model Interface from moveit_resources. | |
srdf::ModelSharedPtr | loadSRDFModel (const std::string &robot_name) |
Loads an SRDF Model from moveit_resources. | |
void | loadIKPluginForGroup (const rclcpp::Node::SharedPtr &node, JointModelGroup *jmg, const std::string &base_link, const std::string &tip_link, std::string plugin="KDL", double timeout=0.1) |
Load an IK solver plugin for the given joint model group. | |
template<class InType > | |
std::string | toStringImpl (InType t) |
template<class OutType > | |
OutType | toRealImpl (const std::string &s) |
Core components of MoveIt.
typedef std::function<void(AttachedBody* body, bool attached)> moveit::core::AttachedBodyCallback |
Definition at line 52 of file attached_body.h.
using moveit::core::FixedTransformsMap = typedef std::map<std::string, Eigen::Isometry3d, std::less<std::string>, Eigen::aligned_allocator<std::pair<const std::string, Eigen::Isometry3d> > > |
Map frame names to the transformation matrix that can transform objects from the frame name to the planning frame.
Definition at line 52 of file transforms.h.
typedef std::function<bool(RobotState* robot_state, const JointModelGroup* joint_group, const double* joint_group_variable_values)> moveit::core::GroupStateValidityCallbackFn |
Signature for functions that can verify that if the group joint_group in robot_state is set to joint_group_variable_values the state is valid or not. Returns true if the state is valid. This call is allowed to modify robot_state (e.g., set joint_group_variable_values)
Definition at line 70 of file robot_state.h.
using moveit::core::JointBoundsVector = typedef std::vector<const JointModel::Bounds*> |
Definition at line 67 of file joint_model_group.h.
using moveit::core::JointModelGroupMap = typedef std::map<std::string, JointModelGroup*> |
Map of names to instances for JointModelGroup.
Definition at line 62 of file joint_model_group.h.
using moveit::core::JointModelGroupMapConst = typedef std::map<std::string, const JointModelGroup*> |
Map of names to const instances for JointModelGroup.
Definition at line 65 of file joint_model_group.h.
using moveit::core::JointModelMap = typedef std::map<std::string, JointModel*> |
Map of names to instances for JointModel.
Definition at line 99 of file joint_model.h.
using moveit::core::JointModelMapConst = typedef std::map<std::string, const JointModel*> |
Map of names to const instances for JointModel.
Definition at line 102 of file joint_model.h.
typedef std::map<std::string, LinkModel*> moveit::core::LinkModelMap |
Map of names to instances for LinkModel.
Definition at line 61 of file link_model.h.
using moveit::core::LinkModelMapConst = typedef std::map<std::string, const LinkModel*> |
Map of names to const instances for LinkModel.
Definition at line 64 of file link_model.h.
using moveit::core::LinkTransformMap = typedef std::map<const LinkModel*, Eigen::Isometry3d, std::less<const LinkModel*>, Eigen::aligned_allocator<std::pair<const LinkModel* const, Eigen::Isometry3d> > > |
Map from link model instances to Eigen transforms.
Definition at line 67 of file link_model.h.
typedef std::function<kinematics::KinematicsBasePtr(const JointModelGroup*)> moveit::core::SolverAllocatorFn |
Function type that allocates a kinematics solver for a particular group.
Definition at line 56 of file joint_model_group.h.
using moveit::core::SolverAllocatorMapFn = typedef std::map<const JointModelGroup*, SolverAllocatorFn> |
Map from group instances to allocator functions & bijections.
Definition at line 59 of file joint_model_group.h.
using moveit::core::VariableBoundsMap = typedef std::map<std::string, VariableBounds> |
Data type for holding mappings from variable names to their bounds.
Definition at line 96 of file joint_model.h.
typedef std::map<std::string, size_t> moveit::core::VariableIndexMap |
Data type for holding mappings from variable names to their position in a state vector.
Definition at line 93 of file joint_model.h.
void moveit::core::attachedBodiesToAttachedCollisionObjectMsgs | ( | const std::vector< const AttachedBody * > & | attached_bodies, |
std::vector< moveit_msgs::msg::AttachedCollisionObject > & | attached_collision_objs | ||
) |
Convert AttachedBodies to AttachedCollisionObjects.
attached_bodies | The input MoveIt attached body objects |
attached_collision_objs | The resultant AttachedCollisionObject messages |
Definition at line 444 of file conversions.cpp.
void moveit::core::computeTurnDriveTurnGeometry | ( | const double * | from, |
const double * | to, | ||
const double | min_translational_distance, | ||
double & | dx, | ||
double & | dy, | ||
double & | initial_turn, | ||
double & | drive_angle, | ||
double & | final_turn | ||
) |
Compute the geometry to turn toward the target point, drive straight and then turn to target orientation.
[in] | from | A vector representing the initial position [x0, y0, theta0] |
[in] | to | A vector representing the target position [x1, y1, theta1] |
[in] | min_translational_distance | If the translational distance between from and to is less than this value the motion will be pure rotation (meters) |
[out] | dx | x1 - x0 (meters) |
[out] | dy | y1 - y0 (meters) |
[out] | initial_turn | The initial turn in radians to face the target |
[out] | drive_angle | The orientation in radians that the robot will be driving straight at |
[out] | final_turn | The final turn in radians to the target orientation |
Definition at line 166 of file planar_joint_model.cpp.
|
inline |
Convenience function to translated error message into string.
error_code | Error code to be translated to a string |
Definition at line 82 of file moveit_error_code.h.
std::optional< int > moveit::core::hasJointSpaceJump | ( | const std::vector< moveit::core::RobotStatePtr > & | waypoints, |
const moveit::core::JointModelGroup & | group, | ||
const moveit::core::JumpThreshold & | jump_threshold | ||
) |
Checks if a joint-space path has a jump larger than the given threshold.
This function computes the distance between every pair of adjacent waypoints (for the given group) and checks if that distance is larger than the threshold defined by jump_threshold
. If so, it is considered that the path has a jump, and the path index where the jump happens is returned as output. Otherwise the function returns a nullopt.
Definition at line 531 of file cartesian_interpolator.cpp.
bool moveit::core::isEmpty | ( | const geometry_msgs::msg::Pose & | msg | ) |
Check if the message contains any non-zero entries.
Definition at line 77 of file message_checks.cpp.
bool moveit::core::isEmpty | ( | const geometry_msgs::msg::Quaternion & | msg | ) |
Check if the message contains any non-zero entries.
Definition at line 72 of file message_checks.cpp.
bool moveit::core::isEmpty | ( | const moveit_msgs::msg::Constraints & | msg | ) |
Check if a message specifies constraints.
Definition at line 66 of file message_checks.cpp.
bool moveit::core::isEmpty | ( | const moveit_msgs::msg::PlanningScene & | msg | ) |
Check if a message includes any information about a planning scene, or whether it is empty.
Definition at line 43 of file message_checks.cpp.
bool moveit::core::isEmpty | ( | const moveit_msgs::msg::PlanningSceneWorld & | msg | ) |
Check if a message includes any information about a planning scene world, or whether it is empty.
Definition at line 49 of file message_checks.cpp.
bool moveit::core::isEmpty | ( | const moveit_msgs::msg::RobotState & | msg | ) |
Check if a message includes any information about a robot state, or whether it is empty.
Definition at line 54 of file message_checks.cpp.
bool moveit::core::jointStateToRobotState | ( | const sensor_msgs::msg::JointState & | joint_state, |
RobotState & | state | ||
) |
Convert a joint state to a MoveIt robot state.
joint_state | The input joint state to be converted |
state | The resultant MoveIt robot state |
Definition at line 406 of file conversions.cpp.
bool moveit::core::jointTrajPointToRobotState | ( | const trajectory_msgs::msg::JointTrajectory & | trajectory, |
std::size_t | point_id, | ||
RobotState & | state | ||
) |
Convert a joint trajectory point to a MoveIt robot state.
joint_trajectory | The input msg |
point_id | The index of the trajectory point in the joint trajectory. |
state | The resultant MoveIt robot state |
Definition at line 473 of file conversions.cpp.
void moveit::core::loadIKPluginForGroup | ( | const rclcpp::Node::SharedPtr & | node, |
JointModelGroup * | jmg, | ||
const std::string & | base_link, | ||
const std::string & | tip_link, | ||
std::string | plugin = "KDL" , |
||
double | timeout = 0.1 |
||
) |
Load an IK solver plugin for the given joint model group.
[in] | jmg | joint model group to load the plugin for |
[in] | base_link | base link of chain |
[in] | tip_link | tip link of chain |
[in] | plugin | name of the plugin ("KDL", or full name) |
[in] | timeout | default solver timeout |
Definition at line 139 of file robot_model_test_utils.cpp.
urdf::ModelInterfaceSharedPtr moveit::core::loadModelInterface | ( | const std::string & | robot_name | ) |
Loads a URDF Model Interface from moveit_resources.
[in] | robot_name | The name of the robot in moveit_resources to load. This should be the prefix to many of the robot packages. For example, "pr2", "panda", or "fanuc". |
Definition at line 95 of file robot_model_test_utils.cpp.
srdf::ModelSharedPtr moveit::core::loadSRDFModel | ( | const std::string & | robot_name | ) |
Loads an SRDF Model from moveit_resources.
[in] | robot_name | The name of the robot in moveit_resources to load. This should be the prefix to many of the robot packages. For example, "pr2", "panda", or "fanuc". |
Definition at line 118 of file robot_model_test_utils.cpp.
moveit::core::RobotModelPtr moveit::core::loadTestingRobotModel | ( | const std::string & | package_name, |
const std::string & | urdf_relative_path, | ||
const std::string & | srdf_relative_path | ||
) |
Loads a robot model given a URDF and SRDF file in a package.
[in] | package_name | The name of the package containing the URDF and SRDF files. |
[in] | urdf_relative_path | The relative path to the URDF file in the package installed directory. |
[in] | srdf_relative_path | The relative path to the SRDF file in the package installed directory. |
Definition at line 63 of file robot_model_test_utils.cpp.
moveit::core::RobotModelPtr moveit::core::loadTestingRobotModel | ( | const std::string & | robot_name | ) |
Loads a robot from moveit_resources.
[in] | robot_name | The name of the robot in moveit_resources to load. This should be the prefix to many of the robot packages. For example, "pr2", "panda", or "fanuc". |
Definition at line 87 of file robot_model_test_utils.cpp.
moveit::core::MOVEIT_CLASS_FORWARD | ( | JointModelGroup | ) |
moveit::core::MOVEIT_CLASS_FORWARD | ( | RobotModel | ) |
moveit::core::MOVEIT_CLASS_FORWARD | ( | RobotState | ) |
moveit::core::MOVEIT_CLASS_FORWARD | ( | Transforms | ) |
std::ostream & moveit::core::operator<< | ( | std::ostream & | out, |
const RobotState & | s | ||
) |
Operator overload for printing variable bounds to a stream.
Definition at line 2465 of file robot_state.cpp.
std::ostream & moveit::core::operator<< | ( | std::ostream & | out, |
const VariableBounds & | b | ||
) |
Operator overload for printing variable bounds to a stream.
Definition at line 291 of file joint_model.cpp.
bool moveit::core::robotStateMsgToRobotState | ( | const moveit_msgs::msg::RobotState & | robot_state, |
RobotState & | state, | ||
bool | copy_attached_bodies = true |
||
) |
Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state.
robot_state | The input robot state msg |
state | The resultant MoveIt robot state |
copy_attached_bodies | Flag to include attached objects in robot state copy |
Definition at line 413 of file conversions.cpp.
bool moveit::core::robotStateMsgToRobotState | ( | const Transforms & | tf, |
const moveit_msgs::msg::RobotState & | robot_state, | ||
RobotState & | state, | ||
bool | copy_attached_bodies = true |
||
) |
Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state.
tf | An instance of a transforms object |
robot_state | The input robot state msg |
state | The resultant MoveIt robot state |
copy_attached_bodies | Flag to include attached objects in robot state copy |
Definition at line 421 of file conversions.cpp.
void moveit::core::robotStateToJointStateMsg | ( | const RobotState & | state, |
sensor_msgs::msg::JointState & | joint_state | ||
) |
Convert a MoveIt robot state to a joint state message.
state | The input MoveIt robot state object |
robot_state | The resultant JointState message |
Definition at line 453 of file conversions.cpp.
void moveit::core::robotStateToRobotStateMsg | ( | const RobotState & | state, |
moveit_msgs::msg::RobotState & | robot_state, | ||
bool | copy_attached_bodies = true |
||
) |
Convert a MoveIt robot state to a robot state message.
state | The input MoveIt robot state object |
robot_state | The resultant RobotState *message |
copy_attached_bodies | Flag to include attached objects in robot state copy |
Definition at line 429 of file conversions.cpp.
void moveit::core::robotStateToStream | ( | const RobotState & | state, |
std::ostream & | out, | ||
bool | include_header = true , |
||
const std::string & | separator = "," |
||
) |
Convert a MoveIt robot state to common separated values (CSV) on a single line that is outputted to a stream e.g. for file saving.
state | - The input MoveIt robot state object |
out | - a file stream, or any other stream |
include_header | - flag to prefix the output with a line of joint names. |
separator | - allows to override the comma separator with any symbol, such as a white space |
Definition at line 498 of file conversions.cpp.
void moveit::core::robotStateToStream | ( | const RobotState & | state, |
std::ostream & | out, | ||
const std::vector< std::string > & | joint_groups_ordering, | ||
bool | include_header = true , |
||
const std::string & | separator = "," |
||
) |
Convert a MoveIt robot state to common separated values (CSV) on a single line that is outputted to a stream e.g. for file saving. This version can order by joint model groups.
state | - The input MoveIt robot state object |
out | - a file stream, or any other stream |
joint_group_ordering | - output joints based on ordering of joint groups |
include_header | - flag to prefix the output with a line of joint names. |
separator | - allows to override the comma separator with any symbol, such as a white space |
Definition at line 526 of file conversions.cpp.
void moveit::core::streamToRobotState | ( | RobotState & | state, |
const std::string & | line, | ||
const std::string & | separator = "," |
||
) |
Convert a string of joint values from a file (CSV) or input source into a RobotState.
state | - the output MoveIt robot state object |
line | - the input string of joint values |
separator | - allows to override the comma separator with any symbol, such as a white space |
Definition at line 563 of file conversions.cpp.
double moveit::core::toDouble | ( | const std::string & | s | ) |
Converts a std::string to double using the classic C locale.
std::runtime_exception | if not a valid number |
Definition at line 80 of file lexical_casts.cpp.
float moveit::core::toFloat | ( | const std::string & | s | ) |
Converts a std::string to float using the classic C locale.
std::runtime_exception | if not a valid number |
Definition at line 85 of file lexical_casts.cpp.
OutType moveit::core::toRealImpl | ( | const std::string & | s | ) |
Definition at line 66 of file lexical_casts.cpp.
std::string moveit::core::toString | ( | double | d | ) |
Convert a double to std::string using the classic C locale.
Definition at line 55 of file lexical_casts.cpp.
std::string moveit::core::toString | ( | float | f | ) |
Convert a float to std::string using the classic C locale.
Definition at line 60 of file lexical_casts.cpp.
std::string moveit::core::toStringImpl | ( | InType | t | ) |