moveit2
The MoveIt Motion Planning Framework for ROS 2.
|
Classes | |
class | CollisionMonitor |
struct | JointJogCommand |
struct | KinematicState |
struct | PoseCommand |
class | Servo |
class | ServoNode |
struct | TwistCommand |
Typedefs | |
typedef std::pair< StatusCode, Eigen::VectorXd > | JointDeltaResult |
typedef std::variant< JointJogCommand, TwistCommand, PoseCommand > | ServoInput |
typedef std::unordered_map< std::string, std::size_t > | JointNameToMoveGroupIndexMap |
Enumerations | |
enum class | StatusCode : int8_t { INVALID = -1 , NO_WARNING = 0 , DECELERATE_FOR_APPROACHING_SINGULARITY = 1 , HALT_FOR_SINGULARITY = 2 , DECELERATE_FOR_LEAVING_SINGULARITY = 3 , DECELERATE_FOR_COLLISION = 4 , HALT_FOR_COLLISION = 5 , JOINT_BOUND = 6 } |
enum class | CommandType : int8_t { JOINT_JOG = 0 , TWIST = 1 , POSE = 2 , MIN = JOINT_JOG , MAX = POSE } |
Functions | |
JointDeltaResult | jointDeltaFromJointJog (const JointJogCommand &command, const moveit::core::RobotStatePtr &robot_state, const servo::Params &servo_params, const JointNameToMoveGroupIndexMap &joint_name_group_index_map) |
Compute the change in joint position for the given joint jog command. | |
JointDeltaResult | jointDeltaFromTwist (const TwistCommand &command, const moveit::core::RobotStatePtr &robot_state, const servo::Params &servo_params, const std::string &planning_frame, const JointNameToMoveGroupIndexMap &joint_name_group_index_map) |
Compute the change in joint position for the given twist command. | |
JointDeltaResult | jointDeltaFromPose (const PoseCommand &command, const moveit::core::RobotStatePtr &robot_state, const servo::Params &servo_params, const std::string &planning_frame, const std::string &ee_frame, const JointNameToMoveGroupIndexMap &joint_name_group_index_map) |
Compute the change in joint position for the given pose command. | |
JointDeltaResult | jointDeltaFromIK (const Eigen::VectorXd &cartesian_position_delta, const moveit::core::RobotStatePtr &robot_state, const servo::Params &servo_params, const JointNameToMoveGroupIndexMap &joint_name_group_index_map) |
Computes the required change in joint angles for given Cartesian change, using the robot's IK solver. | |
std::optional< std::string > | getIKSolverBaseFrame (const moveit::core::RobotStatePtr &robot_state, const std::string &group_name) |
Get the base frame of the current active joint group or subgroup's IK solver. | |
std::optional< std::string > | getIKSolverTipFrame (const moveit::core::RobotStatePtr &robot_state, const std::string &group_name) |
Get the tip (end-effector) frame of the current active joint group or subgroup's IK solver. | |
bool | isValidCommand (const Eigen::VectorXd &command) |
Checks if a given VectorXd is a valid command. | |
bool | isValidCommand (const Eigen::Isometry3d &command) |
Checks if a given Isometry3d (pose) is a valid command. | |
bool | isValidCommand (const TwistCommand &command) |
Checks if a given Twist command is valid. | |
bool | isValidCommand (const PoseCommand &command) |
Checks if a given Pose command is valid. | |
geometry_msgs::msg::Pose | poseFromCartesianDelta (const Eigen::VectorXd &delta_x, const Eigen::Isometry3d &base_to_tip_frame_transform) |
Create a pose message for the provided change in Cartesian position. | |
std::optional< trajectory_msgs::msg::JointTrajectory > | composeTrajectoryMessage (const servo::Params &servo_params, const std::deque< KinematicState > &joint_cmd_rolling_window) |
Create a trajectory message from a rolling window queue of joint state commands. Method optionally returns a trajectory message if one can be created. | |
void | updateSlidingWindow (KinematicState &next_joint_state, std::deque< KinematicState > &joint_cmd_rolling_window, double max_expected_latency, const rclcpp::Time &cur_time) |
Adds a new joint state command to a queue containing commands over a time window. Also modifies the velocities of the commands to help avoid overshooting. | |
std_msgs::msg::Float64MultiArray | composeMultiArrayMessage (const servo::Params &servo_params, const KinematicState &joint_state) |
Create a Float64MultiArray message from given joint state. | |
std::pair< double, StatusCode > | velocityScalingFactorForSingularity (const moveit::core::RobotStatePtr &robot_state, const Eigen::VectorXd &target_delta_x, const servo::Params &servo_params) |
Computes scaling factor for velocity when the robot is near a singularity. | |
double | jointLimitVelocityScalingFactor (const Eigen::VectorXd &velocities, const moveit::core::JointBoundsVector &joint_bounds, double scaling_override) |
Apply velocity scaling based on joint limits. If the robot model does not have velocity limits defined, then a scale factor of 1.0 will be returned. | |
std::vector< size_t > | jointVariablesToHalt (const Eigen::VectorXd &positions, const Eigen::VectorXd &velocities, const moveit::core::JointBoundsVector &joint_bounds, const std::vector< double > &margins) |
Finds the joint variable indices corresponding to joints exceeding allowable position limits. | |
geometry_msgs::msg::TransformStamped | convertIsometryToTransform (const Eigen::Isometry3d &eigen_tf, const std::string &parent_frame, const std::string &child_frame) |
Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped. | |
PoseCommand | poseFromPoseStamped (const geometry_msgs::msg::PoseStamped &msg) |
Convert a PoseStamped message to a Servo Pose. | |
planning_scene_monitor::PlanningSceneMonitorPtr | createPlanningSceneMonitor (const rclcpp::Node::SharedPtr &node, const servo::Params &servo_params) |
Creates the planning scene monitor used by servo. | |
KinematicState | extractRobotState (const moveit::core::RobotStatePtr &robot_state, const std::string &move_group_name) |
Extract the state from a RobotStatePtr instance. | |
const std::unordered_map< StatusCode, std::string > | SERVO_STATUS_CODE_MAP ({ { StatusCode::INVALID, "Invalid" }, { StatusCode::NO_WARNING, "No warnings" }, { StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY, "Moving closer to a singularity, decelerating" }, { StatusCode::HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" }, { StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY, "Moving away from a singularity, decelerating" }, { StatusCode::DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" }, { StatusCode::HALT_FOR_COLLISION, "Collision detected, emergency stop" }, { StatusCode::JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } }) |
Variables | |
constexpr int | MIN_POINTS_FOR_TRAJ_MSG = 3 |
typedef std::pair<StatusCode, Eigen::VectorXd> moveit_servo::JointDeltaResult |
Definition at line 85 of file datatypes.hpp.
typedef std::unordered_map<std::string, std::size_t> moveit_servo::JointNameToMoveGroupIndexMap |
Definition at line 134 of file datatypes.hpp.
typedef std::variant<JointJogCommand, TwistCommand, PoseCommand> moveit_servo::ServoInput |
Definition at line 111 of file datatypes.hpp.
|
strong |
Enumerator | |
---|---|
JOINT_JOG | |
TWIST | |
POSE | |
MIN | |
MAX |
Definition at line 74 of file datatypes.hpp.
|
strong |
Enumerator | |
---|---|
INVALID | |
NO_WARNING | |
DECELERATE_FOR_APPROACHING_SINGULARITY | |
HALT_FOR_SINGULARITY | |
DECELERATE_FOR_LEAVING_SINGULARITY | |
DECELERATE_FOR_COLLISION | |
HALT_FOR_COLLISION | |
JOINT_BOUND |
Definition at line 51 of file datatypes.hpp.
std_msgs::msg::Float64MultiArray moveit_servo::composeMultiArrayMessage | ( | const servo::Params & | servo_params, |
const KinematicState & | joint_state | ||
) |
Create a Float64MultiArray message from given joint state.
servo_params | The configuration used by servo, required for selecting position vs velocity. |
joint_state | The joint state to be added into the Float64MultiArray. |
Definition at line 258 of file common.cpp.
std::optional< trajectory_msgs::msg::JointTrajectory > moveit_servo::composeTrajectoryMessage | ( | const servo::Params & | servo_params, |
const std::deque< KinematicState > & | joint_cmd_rolling_window | ||
) |
Create a trajectory message from a rolling window queue of joint state commands. Method optionally returns a trajectory message if one can be created.
servo_params | The configuration used by servo, required for setting some field of the trajectory message. |
joint_cmd_rolling_window | A rolling window queue of joint state commands. |
Definition at line 151 of file common.cpp.
geometry_msgs::msg::TransformStamped moveit_servo::convertIsometryToTransform | ( | const Eigen::Isometry3d & | eigen_tf, |
const std::string & | parent_frame, | ||
const std::string & | child_frame | ||
) |
Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped.
eigen_tf | The isometry to be converted to TransformStamped. |
parent_frame | The target frame. |
child_frame | The current frame. |
Definition at line 454 of file common.cpp.
planning_scene_monitor::PlanningSceneMonitorPtr moveit_servo::createPlanningSceneMonitor | ( | const rclcpp::Node::SharedPtr & | node, |
const servo::Params & | servo_params | ||
) |
Creates the planning scene monitor used by servo.
Definition at line 480 of file common.cpp.
KinematicState moveit_servo::extractRobotState | ( | const moveit::core::RobotStatePtr & | robot_state, |
const std::string & | move_group_name | ||
) |
Extract the state from a RobotStatePtr instance.
robot_state | A RobotStatePtr instance. |
move_group_name | The name of the planning group. |
Definition at line 516 of file common.cpp.
std::optional< std::string > moveit_servo::getIKSolverBaseFrame | ( | const moveit::core::RobotStatePtr & | robot_state, |
const std::string & | group_name | ||
) |
Get the base frame of the current active joint group or subgroup's IK solver.
robot_state | A pointer to the current robot state. |
group_name | The currently active joint group name. |
Definition at line 57 of file common.cpp.
std::optional< std::string > moveit_servo::getIKSolverTipFrame | ( | const moveit::core::RobotStatePtr & | robot_state, |
const std::string & | group_name | ||
) |
Get the tip (end-effector) frame of the current active joint group or subgroup's IK solver.
robot_state | A pointer to the current robot state. |
group_name | The currently active joint group name. |
Definition at line 72 of file common.cpp.
bool moveit_servo::isValidCommand | ( | const Eigen::Isometry3d & | command | ) |
Checks if a given Isometry3d (pose) is a valid command.
command | The command to be checked. |
Definition at line 93 of file common.cpp.
bool moveit_servo::isValidCommand | ( | const Eigen::VectorXd & | command | ) |
Checks if a given VectorXd is a valid command.
command | The command to be checked. |
Definition at line 87 of file common.cpp.
bool moveit_servo::isValidCommand | ( | const PoseCommand & | command | ) |
Checks if a given Pose command is valid.
command | The command to be checked. |
Definition at line 110 of file common.cpp.
bool moveit_servo::isValidCommand | ( | const TwistCommand & | command | ) |
Checks if a given Twist command is valid.
command | The command to be checked. |
Definition at line 105 of file common.cpp.
JointDeltaResult moveit_servo::jointDeltaFromIK | ( | const Eigen::VectorXd & | cartesian_position_delta, |
const moveit::core::RobotStatePtr & | robot_state, | ||
const servo::Params & | servo_params, | ||
const JointNameToMoveGroupIndexMap & | joint_name_group_index_map | ||
) |
Computes the required change in joint angles for given Cartesian change, using the robot's IK solver.
cartesian_position_delta | The change in Cartesian position. |
robot_state_ | The current robot state as obtained from PlanningSceneMonitor. |
servo_params | The servo parameters. |
joint_name_group_index_map | Mapping between joint subgroup name and move group joint vector position. |
Definition at line 261 of file command.cpp.
JointDeltaResult moveit_servo::jointDeltaFromJointJog | ( | const JointJogCommand & | command, |
const moveit::core::RobotStatePtr & | robot_state, | ||
const servo::Params & | servo_params, | ||
const JointNameToMoveGroupIndexMap & | joint_name_group_index_map | ||
) |
Compute the change in joint position for the given joint jog command.
command | The joint jog command. |
robot_state_ | The current robot state as obtained from PlanningSceneMonitor. |
servo_params | The servo parameters. |
joint_name_group_index_map | Mapping between joint subgroup name and move group joint vector position. |
Definition at line 82 of file command.cpp.
JointDeltaResult moveit_servo::jointDeltaFromPose | ( | const PoseCommand & | command, |
const moveit::core::RobotStatePtr & | robot_state, | ||
const servo::Params & | servo_params, | ||
const std::string & | planning_frame, | ||
const std::string & | ee_frame, | ||
const JointNameToMoveGroupIndexMap & | joint_name_group_index_map | ||
) |
Compute the change in joint position for the given pose command.
command | The pose command. |
robot_state_ | The current robot state as obtained from PlanningSceneMonitor. |
servo_params | The servo parameters. |
planning_frame | The planning frame name. |
ee_frame | The end effector frame name. |
joint_name_group_index_map | Mapping between sub group joint name and move group joint vector position |
Definition at line 211 of file command.cpp.
JointDeltaResult moveit_servo::jointDeltaFromTwist | ( | const TwistCommand & | command, |
const moveit::core::RobotStatePtr & | robot_state, | ||
const servo::Params & | servo_params, | ||
const std::string & | planning_frame, | ||
const JointNameToMoveGroupIndexMap & | joint_name_group_index_map | ||
) |
Compute the change in joint position for the given twist command.
command | The twist command. |
robot_state_ | The current robot state as obtained from PlanningSceneMonitor. |
servo_params | The servo parameters. |
planning_frame | The planning frame name. |
joint_name_group_index_map | Mapping between joint subgroup name and move group joint vector position. |
Definition at line 147 of file command.cpp.
double moveit_servo::jointLimitVelocityScalingFactor | ( | const Eigen::VectorXd & | velocities, |
const moveit::core::JointBoundsVector & | joint_bounds, | ||
double | scaling_override | ||
) |
Apply velocity scaling based on joint limits. If the robot model does not have velocity limits defined, then a scale factor of 1.0 will be returned.
velocities | The commanded velocities. |
joint_bounds | The bounding information for the robot joints. |
scaling_override | The user defined velocity scaling override. |
Definition at line 380 of file common.cpp.
std::vector< size_t > moveit_servo::jointVariablesToHalt | ( | const Eigen::VectorXd & | positions, |
const Eigen::VectorXd & | velocities, | ||
const moveit::core::JointBoundsVector & | joint_bounds, | ||
const std::vector< double > & | margins | ||
) |
Finds the joint variable indices corresponding to joints exceeding allowable position limits.
positions | The joint positions. |
velocities | The current commanded velocities. |
joint_bounds | The allowable limits for the robot joints. |
margins | Additional buffer on the actual joint limits. |
Definition at line 411 of file common.cpp.
geometry_msgs::msg::Pose moveit_servo::poseFromCartesianDelta | ( | const Eigen::VectorXd & | delta_x, |
const Eigen::Isometry3d & | base_to_tip_frame_transform | ||
) |
Create a pose message for the provided change in Cartesian position.
delta_x | The change in Cartesian position. |
base_to_tip_frame_transform | The transformation from robot base to ee frame. |
Definition at line 115 of file common.cpp.
PoseCommand moveit_servo::poseFromPoseStamped | ( | const geometry_msgs::msg::PoseStamped & | msg | ) |
Convert a PoseStamped message to a Servo Pose.
msg | The PoseStamped message. |
Definition at line 464 of file common.cpp.
const std::unordered_map< StatusCode, std::string > moveit_servo::SERVO_STATUS_CODE_MAP | ( | { { StatusCode::INVALID, "Invalid" }, { StatusCode::NO_WARNING, "No warnings" }, { StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY, "Moving closer to a singularity, decelerating" }, { StatusCode::HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" }, { StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY, "Moving away from a singularity, decelerating" }, { StatusCode::DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" }, { StatusCode::HALT_FOR_COLLISION, "Collision detected, emergency stop" }, { StatusCode::JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } } | ) |
void moveit_servo::updateSlidingWindow | ( | KinematicState & | next_joint_state, |
std::deque< KinematicState > & | joint_cmd_rolling_window, | ||
double | max_expected_latency, | ||
const rclcpp::Time & | cur_time | ||
) |
Adds a new joint state command to a queue containing commands over a time window. Also modifies the velocities of the commands to help avoid overshooting.
next_joint_state | The next commanded joint state. |
joint_cmd_rolling_window | Queue of containing a rolling window of joint commands. |
max_expected_latency | The next_joint_state will be added to the joint_cmd_rolling_window with a time stamp of |
cur_time | The current time stamp when the method is called. This value is used to update the time stamp of next_joint_state |
Definition at line 203 of file common.cpp.
std::pair< double, StatusCode > moveit_servo::velocityScalingFactorForSingularity | ( | const moveit::core::RobotStatePtr & | robot_state, |
const Eigen::VectorXd & | target_delta_x, | ||
const servo::Params & | servo_params | ||
) |
Computes scaling factor for velocity when the robot is near a singularity.
robot_state | A pointer to the current robot state. |
target_delta_x | The vector containing the required change in Cartesian position. |
servo_params | The servo parameters, contains the singularity thresholds. |
Definition at line 282 of file common.cpp.
|
constexpr |
Definition at line 58 of file common.hpp.