moveit2
The MoveIt Motion Planning Framework for ROS 2.
|
Client class to conveniently use the ROS interfaces provided by the move_group node. More...
#include <moveit/planning_interface/move_group_interface.h>
Classes | |
class | MoveGroupInterfaceImpl |
struct | Options |
Specification of options to use when constructing the MoveGroupInterface class. More... | |
struct | Plan |
The representation of a motion plan (as ROS messages) More... | |
Public Member Functions | |
MOVEIT_STRUCT_FORWARD (Plan) | |
MoveGroupInterface (const rclcpp::Node::SharedPtr &node, const Options &opt, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer=std::shared_ptr< tf2_ros::Buffer >(), const rclcpp::Duration &wait_for_servers=rclcpp::Duration::from_seconds(-1)) | |
Construct a MoveGroupInterface instance call using a specified set of options opt. | |
MoveGroupInterface (const rclcpp::Node::SharedPtr &node, const std::string &group, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer=std::shared_ptr< tf2_ros::Buffer >(), const rclcpp::Duration &wait_for_servers=rclcpp::Duration::from_seconds(-1)) | |
Construct a client for the MoveGroup action for a particular group. | |
~MoveGroupInterface () | |
MoveGroupInterface (const MoveGroupInterface &)=delete | |
This class owns unique resources (e.g. action clients, threads) and its not very meaningful to copy. Pass by references, move it, or simply create multiple instances where required. | |
MoveGroupInterface & | operator= (const MoveGroupInterface &)=delete |
MoveGroupInterface (MoveGroupInterface &&other) noexcept | |
MoveGroupInterface & | operator= (MoveGroupInterface &&other) noexcept |
const std::string & | getName () const |
Get the name of the group this instance operates on. | |
const std::vector< std::string > & | getNamedTargets () const |
Get the names of the named robot states available as targets, both either remembered states or default states from srdf. | |
const std::shared_ptr< tf2_ros::Buffer > & | getTF () const |
Get the tf2_ros::Buffer. | |
moveit::core::RobotModelConstPtr | getRobotModel () const |
Get the RobotModel object. | |
const rclcpp::Node::SharedPtr & | getNode () const |
Get the ROS node handle of this instance operates on. | |
const std::string & | getPlanningFrame () const |
Get the name of the frame in which the robot is planning. | |
const std::vector< std::string > & | getJointModelGroupNames () const |
Get the available planning group names. | |
const std::vector< std::string > & | getJointNames () const |
Get vector of names of joints available in move group. | |
const std::vector< std::string > & | getLinkNames () const |
Get vector of names of links available in move group. | |
std::map< std::string, double > | getNamedTargetValues (const std::string &name) const |
Get the joint angles for targets specified by name. | |
const std::vector< std::string > & | getActiveJoints () const |
Get only the active (actuated) joints this instance operates on. | |
const std::vector< std::string > & | getJoints () const |
Get all the joints this instance operates on (including fixed joints) | |
unsigned int | getVariableCount () const |
Get the number of variables used to describe the state of this group. This is larger or equal to the number of DOF. | |
bool | getInterfaceDescriptions (std::vector< moveit_msgs::msg::PlannerInterfaceDescription > &desc) const |
Get the descriptions of all planning plugins loaded by the action server. | |
bool | getInterfaceDescription (moveit_msgs::msg::PlannerInterfaceDescription &desc) const |
Get the description of the default planning plugin loaded by the action server. | |
std::map< std::string, std::string > | getPlannerParams (const std::string &planner_id, const std::string &group="") const |
Get the planner parameters for given group and planner_id. | |
void | setPlannerParams (const std::string &planner_id, const std::string &group, const std::map< std::string, std::string > ¶ms, bool bReplace=false) |
Set the planner parameters for given group and planner_id. | |
std::string | getDefaultPlanningPipelineId () const |
void | setPlanningPipelineId (const std::string &pipeline_id) |
Specify a planning pipeline to be used for further planning. | |
const std::string & | getPlanningPipelineId () const |
Get the current planning_pipeline_id. | |
std::string | getDefaultPlannerId (const std::string &group="") const |
Get the default planner of the current planning pipeline for the given group (or the pipeline's default) | |
void | setPlannerId (const std::string &planner_id) |
Specify a planner to be used for further planning. | |
const std::string & | getPlannerId () const |
Get the current planner_id. | |
void | setPlanningTime (double seconds) |
Specify the maximum amount of time to use when planning. | |
void | setNumPlanningAttempts (unsigned int num_planning_attempts) |
Set the number of times the motion plan is to be computed from scratch before the shortest solution is returned. The default value is 1. | |
void | setMaxVelocityScalingFactor (double max_velocity_scaling_factor) |
Set a scaling factor for optionally reducing the maximum joint velocity. Allowed values are in (0,1]. The maximum joint velocity specified in the robot model is multiplied by the factor. If the value is 0, it is set to the default value, which is defined in joint_limits.yaml of the moveit_config. If the value is greater than 1, it is set to 1.0. | |
double | getMaxVelocityScalingFactor () const |
Get the max velocity scaling factor set by setMaxVelocityScalingFactor(). | |
void | setMaxAccelerationScalingFactor (double max_acceleration_scaling_factor) |
Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0,1]. The maximum joint acceleration specified in the robot model is multiplied by the factor. If the value is 0, it is set to the default value, which is defined in joint_limits.yaml of the moveit_config. If the value is greater than 1, it is set to 1.0. | |
double | getMaxAccelerationScalingFactor () const |
Get the max acceleration scaling factor set by setMaxAccelerationScalingFactor(). | |
double | getPlanningTime () const |
Get the number of seconds set by setPlanningTime() | |
double | getGoalJointTolerance () const |
Get the tolerance that is used for reaching a joint goal. This is distance for each joint in configuration space. | |
double | getGoalPositionTolerance () const |
Get the tolerance that is used for reaching a position goal. This is be the radius of a sphere where the end-effector must reach. | |
double | getGoalOrientationTolerance () const |
Get the tolerance that is used for reaching an orientation goal. This is the tolerance for roll, pitch and yaw, in radians. | |
void | setGoalTolerance (double tolerance) |
Set the tolerance that is used for reaching the goal. For joint state goals, this will be distance for each joint, in the configuration space (radians or meters depending on joint type). For pose goals this will be the radius of a sphere where the end-effector must reach. This function simply triggers calls to setGoalPositionTolerance(), setGoalOrientationTolerance() and setGoalJointTolerance(). | |
void | setGoalJointTolerance (double tolerance) |
Set the joint tolerance (for each joint) that is used for reaching the goal when moving to a joint value target. | |
void | setGoalPositionTolerance (double tolerance) |
Set the position tolerance that is used for reaching the goal when moving to a pose. | |
void | setGoalOrientationTolerance (double tolerance) |
Set the orientation tolerance that is used for reaching the goal when moving to a pose. | |
void | setWorkspace (double minx, double miny, double minz, double maxx, double maxy, double maxz) |
Specify the workspace bounding box. The box is specified in the planning frame (i.e. relative to the robot root link start position). This is useful when the planning group contains the root joint of the robot – i.e. when planning motion for the robot relative to the world. | |
void | setStartState (const moveit_msgs::msg::RobotState &start_state) |
If a different start state should be considered instead of the current state of the robot, this function sets that state. | |
void | setStartState (const moveit::core::RobotState &start_state) |
If a different start state should be considered instead of the current state of the robot, this function sets that state. | |
void | setStartStateToCurrentState () |
Set the starting state for planning to be that reported by the robot's joint state publication. | |
Setting a joint state target (goal) | |
There are 2 types of goal targets:
Only one or the other is used for planning. Calling any of the set*JointValueTarget() functions sets the current goal target to the JointValueTarget. Calling any of the setPoseTarget(), setOrientationTarget(), setRPYTarget(), setPositionTarget() functions sets the current goal target to the Pose target. | |
bool | setJointValueTarget (const std::vector< double > &group_variable_values) |
Set the JointValueTarget and use it for future planning requests. | |
bool | setJointValueTarget (const std::map< std::string, double > &variable_values) |
Set the JointValueTarget and use it for future planning requests. | |
bool | setJointValueTarget (const std::vector< std::string > &variable_names, const std::vector< double > &variable_values) |
Set the JointValueTarget and use it for future planning requests. | |
bool | setJointValueTarget (const moveit::core::RobotState &robot_state) |
Set the JointValueTarget and use it for future planning requests. | |
bool | setJointValueTarget (const std::string &joint_name, const std::vector< double > &values) |
Set the JointValueTarget and use it for future planning requests. | |
bool | setJointValueTarget (const std::string &joint_name, double value) |
Set the JointValueTarget and use it for future planning requests. | |
bool | setJointValueTarget (const sensor_msgs::msg::JointState &state) |
Set the JointValueTarget and use it for future planning requests. | |
bool | setJointValueTarget (const geometry_msgs::msg::Pose &eef_pose, const std::string &end_effector_link="") |
Set the joint state goal for a particular joint by computing IK. | |
bool | setJointValueTarget (const geometry_msgs::msg::PoseStamped &eef_pose, const std::string &end_effector_link="") |
Set the joint state goal for a particular joint by computing IK. | |
bool | setJointValueTarget (const Eigen::Isometry3d &eef_pose, const std::string &end_effector_link="") |
Set the joint state goal for a particular joint by computing IK. | |
bool | setApproximateJointValueTarget (const geometry_msgs::msg::Pose &eef_pose, const std::string &end_effector_link="") |
Set the joint state goal for a particular joint by computing IK. | |
bool | setApproximateJointValueTarget (const geometry_msgs::msg::PoseStamped &eef_pose, const std::string &end_effector_link="") |
Set the joint state goal for a particular joint by computing IK. | |
bool | setApproximateJointValueTarget (const Eigen::Isometry3d &eef_pose, const std::string &end_effector_link="") |
Set the joint state goal for a particular joint by computing IK. | |
void | setRandomTarget () |
Set the joint state goal to a random joint configuration. | |
bool | setNamedTarget (const std::string &name) |
Set the current joint values to be ones previously remembered by rememberJointValues() or, if not found, that are specified in the SRDF under the name name as a group state. | |
void | getJointValueTarget (std::vector< double > &group_variable_values) const |
Get the current joint state goal in a form compatible to setJointValueTarget() | |
Setting a pose target (goal) | |
Setting a Pose (or Position or Orientation) target disables any previously set JointValueTarget. For groups that have multiple end effectors, a pose can be set for each end effector in the group. End effectors which do not have a pose target set will end up in arbitrary positions. | |
bool | setPositionTarget (double x, double y, double z, const std::string &end_effector_link="") |
Set the goal position of the end-effector end_effector_link to be (x, y, z). | |
bool | setRPYTarget (double roll, double pitch, double yaw, const std::string &end_effector_link="") |
Set the goal orientation of the end-effector end_effector_link to be (roll,pitch,yaw) radians. | |
bool | setOrientationTarget (double x, double y, double z, double w, const std::string &end_effector_link="") |
Set the goal orientation of the end-effector end_effector_link to be the quaternion (x,y,z,w). | |
bool | setPoseTarget (const Eigen::Isometry3d &end_effector_pose, const std::string &end_effector_link="") |
Set the goal pose of the end-effector end_effector_link. | |
bool | setPoseTarget (const geometry_msgs::msg::Pose &target, const std::string &end_effector_link="") |
Set the goal pose of the end-effector end_effector_link. | |
bool | setPoseTarget (const geometry_msgs::msg::PoseStamped &target, const std::string &end_effector_link="") |
Set the goal pose of the end-effector end_effector_link. | |
bool | setPoseTargets (const EigenSTL::vector_Isometry3d &end_effector_pose, const std::string &end_effector_link="") |
Set goal poses for end_effector_link. | |
bool | setPoseTargets (const std::vector< geometry_msgs::msg::Pose > &target, const std::string &end_effector_link="") |
Set goal poses for end_effector_link. | |
bool | setPoseTargets (const std::vector< geometry_msgs::msg::PoseStamped > &target, const std::string &end_effector_link="") |
Set goal poses for end_effector_link. | |
void | setPoseReferenceFrame (const std::string &pose_reference_frame) |
Specify which reference frame to assume for poses specified without a reference frame. | |
bool | setEndEffectorLink (const std::string &end_effector_link) |
Specify the parent link of the end-effector. This end_effector_link will be used in calls to pose target functions when end_effector_link is not explicitly specified. | |
bool | setEndEffector (const std::string &eef_name) |
Specify the name of the end-effector to use. This is equivalent to setting the EndEffectorLink to the parent link of this end effector. | |
void | clearPoseTarget (const std::string &end_effector_link="") |
Forget pose(s) specified for end_effector_link. | |
void | clearPoseTargets () |
Forget any poses specified for all end-effectors. | |
const geometry_msgs::msg::PoseStamped & | getPoseTarget (const std::string &end_effector_link="") const |
const std::vector< geometry_msgs::msg::PoseStamped > & | getPoseTargets (const std::string &end_effector_link="") const |
const std::string & | getEndEffectorLink () const |
Get the current end-effector link. This returns the value set by setEndEffectorLink() (or indirectly by setEndEffector()). If setEndEffectorLink() was not called, this function reports the link name that serves as parent of an end-effector attached to this group. If there are multiple end-effectors, one of them is returned. If no such link is known, the empty string is returned. | |
const std::string & | getEndEffector () const |
Get the current end-effector name. This returns the value set by setEndEffector() (or indirectly by setEndEffectorLink()). If setEndEffector() was not called, this function reports an end-effector attached to this group. If there are multiple end-effectors, one of them is returned. If no end-effector is known, the empty string is returned. | |
const std::string & | getPoseReferenceFrame () const |
Get the reference frame set by setPoseReferenceFrame(). By default this is the reference frame of the robot model. | |
Planning a path from the start position to the Target (goal) position, and executing that plan. | |
moveit::core::MoveItErrorCode | asyncMove () |
Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified target. This call is not blocking (does not wait for the execution of the trajectory to complete). | |
rclcpp_action::Client< moveit_msgs::action::MoveGroup > & | getMoveGroupClient () const |
Get the move_group action client used by the MoveGroupInterface. The client can be used for querying the execution state of the trajectory and abort trajectory execution during asynchronous execution. | |
moveit::core::MoveItErrorCode | move () |
Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified target. This call is always blocking (waits for the execution of the trajectory to complete) and requires an asynchronous spinner to be started. | |
moveit::core::MoveItErrorCode | plan (Plan &plan) |
Compute a motion plan that takes the group declared in the constructor from the current state to the specified target. No execution is performed. The resulting plan is stored in plan. | |
moveit::core::MoveItErrorCode | asyncExecute (const Plan &plan, const std::vector< std::string > &controllers=std::vector< std::string >()) |
Given a plan, execute it without waiting for completion. | |
moveit::core::MoveItErrorCode | asyncExecute (const moveit_msgs::msg::RobotTrajectory &trajectory, const std::vector< std::string > &controllers=std::vector< std::string >()) |
Given a robot trajectory, execute it without waiting for completion. | |
moveit::core::MoveItErrorCode | execute (const Plan &plan, const std::vector< std::string > &controllers=std::vector< std::string >()) |
Given a plan, execute it while waiting for completion. | |
moveit::core::MoveItErrorCode | execute (const moveit_msgs::msg::RobotTrajectory &trajectory, const std::vector< std::string > &controllers=std::vector< std::string >()) |
Given a robot trajectory, execute it while waiting for completion. | |
double | computeCartesianPath (const std::vector< geometry_msgs::msg::Pose > &waypoints, double eef_step, double, moveit_msgs::msg::RobotTrajectory &trajectory, bool avoid_collisions=true, moveit_msgs::msg::MoveItErrorCodes *error_code=nullptr) |
Compute a Cartesian path that follows specified waypoints with a step size of at most eef_step meters between end effector configurations of consecutive points in the result trajectory. The reference frame for the waypoints is that specified by setPoseReferenceFrame(). No more than jump_threshold is allowed as change in distance in the configuration space of the robot (this is to prevent 'jumps' in IK solutions). Collisions are avoided if avoid_collisions is set to true. If collisions cannot be avoided, the function fails. Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the waypoints. Return -1.0 in case of error. | |
double | computeCartesianPath (const std::vector< geometry_msgs::msg::Pose > &waypoints, double eef_step, moveit_msgs::msg::RobotTrajectory &trajectory, bool avoid_collisions=true, moveit_msgs::msg::MoveItErrorCodes *error_code=nullptr) |
double | computeCartesianPath (const std::vector< geometry_msgs::msg::Pose > &waypoints, double eef_step, double, moveit_msgs::msg::RobotTrajectory &trajectory, const moveit_msgs::msg::Constraints &path_constraints, bool avoid_collisions=true, moveit_msgs::msg::MoveItErrorCodes *error_code=nullptr) |
Compute a Cartesian path that follows specified waypoints with a step size of at most eef_step meters between end effector configurations of consecutive points in the result trajectory. The reference frame for the waypoints is that specified by setPoseReferenceFrame(). No more than jump_threshold is allowed as change in distance in the configuration space of the robot (this is to prevent 'jumps' in IK solutions). Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. Constraints are checked (collision and kinematic) if avoid_collisions is set to true. If constraints cannot be met, the function fails. Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the waypoints. Return -1.0 in case of error. | |
double | computeCartesianPath (const std::vector< geometry_msgs::msg::Pose > &waypoints, double eef_step, moveit_msgs::msg::RobotTrajectory &trajectory, const moveit_msgs::msg::Constraints &path_constraints, bool avoid_collisions=true, moveit_msgs::msg::MoveItErrorCodes *error_code=nullptr) |
void | stop () |
Stop any trajectory execution, if one is active. | |
void | allowReplanning (bool flag) |
Specify whether the robot is allowed to replan if it detects changes in the environment. | |
void | setReplanAttempts (int32_t attempts) |
Maximum number of replanning attempts. | |
void | setReplanDelay (double delay) |
Sleep this duration between replanning attempts (in walltime seconds) | |
void | allowLooking (bool flag) |
Specify whether the robot is allowed to look around before moving if it determines it should (default is false) | |
void | setLookAroundAttempts (int32_t attempts) |
How often is the system allowed to move the camera to update environment model when looking. | |
void | constructRobotState (moveit_msgs::msg::RobotState &state) |
Build a RobotState message for use with plan() or computeCartesianPath() If the move_group has a custom set start state, this method will use that as the robot state. | |
void | constructMotionPlanRequest (moveit_msgs::msg::MotionPlanRequest &request) |
Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and store it in request. | |
High level actions that trigger a sequence of plans and actions. | |
bool | attachObject (const std::string &object, const std::string &link="") |
Given the name of an object in the planning scene, make the object attached to a link of the robot. If no link name is specified, the end-effector is used. If there is no end-effector, the first link in the group is used. If the object name does not exist an error will be produced in move_group, but the request made by this interface will succeed. | |
bool | attachObject (const std::string &object, const std::string &link, const std::vector< std::string > &touch_links) |
Given the name of an object in the planning scene, make the object attached to a link of the robot. The set of links the object is allowed to touch without considering that a collision is specified by touch_links. If link is empty, the end-effector link is used. If there is no end-effector, the first link in the group is used. If the object name does not exist an error will be produced in move_group, but the request made by this interface will succeed. | |
bool | detachObject (const std::string &name="") |
Detach an object. name specifies the name of the object attached to this group, or the name of the link the object is attached to. If there is no name specified, and there is only one attached object, that object is detached. An error is produced if no object to detach is identified. | |
Query current robot state | |
bool | startStateMonitor (double wait=1.0) |
When reasoning about the current state of a robot, a CurrentStateMonitor instance is automatically constructed. This function allows triggering the construction of that object from the beginning, so that future calls to functions such as getCurrentState() will not take so long and are less likely to fail. | |
std::vector< double > | getCurrentJointValues () const |
Get the current joint values for the joints planned for by this instance (see getJoints()) | |
moveit::core::RobotStatePtr | getCurrentState (double wait=1) const |
Get the current state of the robot within the duration specified by wait. | |
geometry_msgs::msg::PoseStamped | getCurrentPose (const std::string &end_effector_link="") const |
Get the pose for the end-effector end_effector_link. If end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is assumed. | |
std::vector< double > | getCurrentRPY (const std::string &end_effector_link="") const |
Get the roll-pitch-yaw (XYZ) for the end-effector end_effector_link. If end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is assumed. | |
std::vector< double > | getRandomJointValues () const |
Get random joint values for the joints planned for by this instance (see getJoints()) | |
geometry_msgs::msg::PoseStamped | getRandomPose (const std::string &end_effector_link="") const |
Get a random reachable pose for the end-effector end_effector_link. If end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is assumed. | |
Manage named joint configurations | |
void | rememberJointValues (const std::string &name) |
Remember the current joint values (of the robot being monitored) under name. These can be used by setNamedTarget(). These values are remembered locally in the client. Other clients will not have access to them. | |
void | rememberJointValues (const std::string &name, const std::vector< double > &values) |
Remember the specified joint values under name. These can be used by setNamedTarget(). These values are remembered locally in the client. Other clients will not have access to them. | |
const std::map< std::string, std::vector< double > > & | getRememberedJointValues () const |
Get the currently remembered map of names to joint values. | |
void | forgetJointValues (const std::string &name) |
Forget the joint values remembered under name. | |
Manage planning constraints | |
void | setConstraintsDatabase (const std::string &host, unsigned int port) |
Specify where the database server that holds known constraints resides. | |
std::vector< std::string > | getKnownConstraints () const |
Get the names of the known constraints as read from the Mongo database, if a connection was achieved. | |
moveit_msgs::msg::Constraints | getPathConstraints () const |
Get the actual set of constraints in use with this MoveGroupInterface. | |
bool | setPathConstraints (const std::string &constraint) |
Specify a set of path constraints to use. The constraints are looked up by name from the Mongo database server. This replaces any path constraints set in previous calls to setPathConstraints(). | |
void | setPathConstraints (const moveit_msgs::msg::Constraints &constraint) |
Specify a set of path constraints to use. This version does not require a database server. This replaces any path constraints set in previous calls to setPathConstraints(). | |
void | clearPathConstraints () |
Specify that no path constraints are to be used. This removes any path constraints set in previous calls to setPathConstraints(). | |
moveit_msgs::msg::TrajectoryConstraints | getTrajectoryConstraints () const |
void | setTrajectoryConstraints (const moveit_msgs::msg::TrajectoryConstraints &constraint) |
void | clearTrajectoryConstraints () |
Static Public Attributes | |
static const std::string | ROBOT_DESCRIPTION |
Default ROS parameter name from where to read the robot's URDF. Set to 'robot_description'. | |
Protected Member Functions | |
const moveit::core::RobotState & | getTargetRobotState () const |
Client class to conveniently use the ROS interfaces provided by the move_group node.
This class includes many default settings to make things easy to use.
Definition at line 75 of file move_group_interface.h.
moveit::planning_interface::MoveGroupInterface::MoveGroupInterface | ( | const rclcpp::Node::SharedPtr & | node, |
const Options & | opt, | ||
const std::shared_ptr< tf2_ros::Buffer > & | tf_buffer = std::shared_ptr<tf2_ros::Buffer>() , |
||
const rclcpp::Duration & | wait_for_servers = rclcpp::Duration::from_seconds(-1) |
||
) |
Construct a MoveGroupInterface instance call using a specified set of options opt.
opt. | A MoveGroupInterface::Options structure, if you pass a ros::NodeHandle with a specific callback queue, it has to be of type ros::CallbackQueue (which is the default type of callback queues used in ROS) |
tf_buffer. | Specify a TF2_ROS Buffer instance to use. If not specified, one will be constructed internally |
wait_for_servers. | Timeout for connecting to action servers. -1 time means unlimited waiting. |
Definition at line 1270 of file move_group_interface.cpp.
moveit::planning_interface::MoveGroupInterface::MoveGroupInterface | ( | const rclcpp::Node::SharedPtr & | node, |
const std::string & | group, | ||
const std::shared_ptr< tf2_ros::Buffer > & | tf_buffer = std::shared_ptr<tf2_ros::Buffer>() , |
||
const rclcpp::Duration & | wait_for_servers = rclcpp::Duration::from_seconds(-1) |
||
) |
Construct a client for the MoveGroup action for a particular group.
tf_buffer. | Specify a TF2_ROS Buffer instance to use. If not specified, one will be constructed internally |
wait_for_servers. | Timeout for connecting to action servers. -1 time means unlimited waiting. |
Definition at line 1259 of file move_group_interface.cpp.
moveit::planning_interface::MoveGroupInterface::~MoveGroupInterface | ( | ) |
Definition at line 1278 of file move_group_interface.cpp.
|
delete |
This class owns unique resources (e.g. action clients, threads) and its not very meaningful to copy. Pass by references, move it, or simply create multiple instances where required.
|
noexcept |
Definition at line 1283 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::allowLooking | ( | bool | flag | ) |
Specify whether the robot is allowed to look around before moving if it determines it should (default is false)
Definition at line 2082 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::allowReplanning | ( | bool | flag | ) |
Specify whether the robot is allowed to replan if it detects changes in the environment.
Definition at line 2101 of file move_group_interface.cpp.
moveit::core::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::asyncExecute | ( | const moveit_msgs::msg::RobotTrajectory & | trajectory, |
const std::vector< std::string > & | controllers = std::vector<std::string>() |
||
) |
Given a robot trajectory, execute it without waiting for completion.
[in] | trajectory | The trajectory to execute |
[in] | controllers | An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. |
Definition at line 1425 of file move_group_interface.cpp.
moveit::core::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::asyncExecute | ( | const Plan & | plan, |
const std::vector< std::string > & | controllers = std::vector<std::string>() |
||
) |
Given a plan, execute it without waiting for completion.
[in] | plan | The motion plan for which to execute |
[in] | controllers | An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. |
Definition at line 1419 of file move_group_interface.cpp.
moveit::core::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::asyncMove | ( | ) |
Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified target. This call is not blocking (does not wait for the execution of the trajectory to complete).
Definition at line 1404 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::attachObject | ( | const std::string & | object, |
const std::string & | link, | ||
const std::vector< std::string > & | touch_links | ||
) |
Given the name of an object in the planning scene, make the object attached to a link of the robot. The set of links the object is allowed to touch without considering that a collision is specified by touch_links. If link is empty, the end-effector link is used. If there is no end-effector, the first link in the group is used. If the object name does not exist an error will be produced in move_group, but the request made by this interface will succeed.
Definition at line 2215 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::attachObject | ( | const std::string & | object, |
const std::string & | link = "" |
||
) |
Given the name of an object in the planning scene, make the object attached to a link of the robot. If no link name is specified, the end-effector is used. If there is no end-effector, the first link in the group is used. If the object name does not exist an error will be produced in move_group, but the request made by this interface will succeed.
Definition at line 2210 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::clearPathConstraints | ( | ) |
Specify that no path constraints are to be used. This removes any path constraints set in previous calls to setPathConstraints().
Definition at line 2153 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::clearPoseTarget | ( | const std::string & | end_effector_link = "" | ) |
Forget pose(s) specified for end_effector_link.
Definition at line 1716 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::clearPoseTargets | ( | ) |
Forget any poses specified for all end-effectors.
Definition at line 1721 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::clearTrajectoryConstraints | ( | ) |
|
inline |
Compute a Cartesian path that follows specified waypoints with a step size of at most eef_step meters between end effector configurations of consecutive points in the result trajectory. The reference frame for the waypoints is that specified by setPoseReferenceFrame(). No more than jump_threshold is allowed as change in distance in the configuration space of the robot (this is to prevent 'jumps' in IK solutions). Collisions are avoided if avoid_collisions is set to true. If collisions cannot be avoided, the function fails. Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the waypoints. Return -1.0 in case of error.
Definition at line 765 of file move_group_interface.h.
|
inline |
Compute a Cartesian path that follows specified waypoints with a step size of at most eef_step meters between end effector configurations of consecutive points in the result trajectory. The reference frame for the waypoints is that specified by setPoseReferenceFrame(). No more than jump_threshold is allowed as change in distance in the configuration space of the robot (this is to prevent 'jumps' in IK solutions). Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. Constraints are checked (collision and kinematic) if avoid_collisions is set to true. If constraints cannot be met, the function fails. Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the waypoints. Return -1.0 in case of error.
Definition at line 788 of file move_group_interface.h.
double moveit::planning_interface::MoveGroupInterface::computeCartesianPath | ( | const std::vector< geometry_msgs::msg::Pose > & | waypoints, |
double | eef_step, | ||
moveit_msgs::msg::RobotTrajectory & | trajectory, | ||
bool | avoid_collisions = true , |
||
moveit_msgs::msg::MoveItErrorCodes * | error_code = nullptr |
||
) |
double moveit::planning_interface::MoveGroupInterface::computeCartesianPath | ( | const std::vector< geometry_msgs::msg::Pose > & | waypoints, |
double | eef_step, | ||
moveit_msgs::msg::RobotTrajectory & | trajectory, | ||
const moveit_msgs::msg::Constraints & | path_constraints, | ||
bool | avoid_collisions = true , |
||
moveit_msgs::msg::MoveItErrorCodes * | error_code = nullptr |
||
) |
void moveit::planning_interface::MoveGroupInterface::constructMotionPlanRequest | ( | moveit_msgs::msg::MotionPlanRequest & | request | ) |
Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and store it in request.
Definition at line 2231 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::constructRobotState | ( | moveit_msgs::msg::RobotState & | state | ) |
Build a RobotState message for use with plan() or computeCartesianPath() If the move_group has a custom set start state, this method will use that as the robot state.
Otherwise, the robot state will be with is_diff
set to true, causing it to be an offset from the current state of the robot at time of the state's use.
Definition at line 2226 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::detachObject | ( | const std::string & | name = "" | ) |
Detach an object. name specifies the name of the object attached to this group, or the name of the link the object is attached to. If there is no name specified, and there is only one attached object, that object is detached. An error is produced if no object to detach is identified.
Definition at line 2221 of file move_group_interface.cpp.
moveit::core::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::execute | ( | const moveit_msgs::msg::RobotTrajectory & | trajectory, |
const std::vector< std::string > & | controllers = std::vector<std::string>() |
||
) |
Given a robot trajectory, execute it while waiting for completion.
[in] | trajectory | The trajectory to execute |
[in] | controllers | An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. |
Definition at line 1436 of file move_group_interface.cpp.
moveit::core::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::execute | ( | const Plan & | plan, |
const std::vector< std::string > & | controllers = std::vector<std::string>() |
||
) |
Given a plan, execute it while waiting for completion.
[in] | plan | Contains trajectory info as well as metadata such as a RobotModel. |
[in] | controllers | An optional list of ros2_controllers to execute with. If none, MoveIt will attempt to find a controller. The exact behavior of finding a controller depends on which MoveItControllerManager plugin is active. |
Definition at line 1431 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::forgetJointValues | ( | const std::string & | name | ) |
Forget the joint values remembered under name.
Definition at line 2077 of file move_group_interface.cpp.
const std::vector< std::string > & moveit::planning_interface::MoveGroupInterface::getActiveJoints | ( | ) | const |
Get only the active (actuated) joints this instance operates on.
Definition at line 2050 of file move_group_interface.cpp.
std::vector< double > moveit::planning_interface::MoveGroupInterface::getCurrentJointValues | ( | ) | const |
Get the current joint values for the joints planned for by this instance (see getJoints())
Definition at line 1952 of file move_group_interface.cpp.
geometry_msgs::msg::PoseStamped moveit::planning_interface::MoveGroupInterface::getCurrentPose | ( | const std::string & | end_effector_link = "" | ) | const |
Get the pose for the end-effector end_effector_link. If end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is assumed.
Definition at line 1995 of file move_group_interface.cpp.
std::vector< double > moveit::planning_interface::MoveGroupInterface::getCurrentRPY | ( | const std::string & | end_effector_link = "" | ) | const |
Get the roll-pitch-yaw (XYZ) for the end-effector end_effector_link. If end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is assumed.
Definition at line 2021 of file move_group_interface.cpp.
moveit::core::RobotStatePtr moveit::planning_interface::MoveGroupInterface::getCurrentState | ( | double | wait = 1 | ) | const |
Get the current state of the robot within the duration specified by wait.
Definition at line 2065 of file move_group_interface.cpp.
std::string moveit::planning_interface::MoveGroupInterface::getDefaultPlannerId | ( | const std::string & | group = "" | ) | const |
Get the default planner of the current planning pipeline for the given group (or the pipeline's default)
Definition at line 1364 of file move_group_interface.cpp.
std::string moveit::planning_interface::MoveGroupInterface::getDefaultPlanningPipelineId | ( | ) | const |
const std::string & moveit::planning_interface::MoveGroupInterface::getEndEffector | ( | ) | const |
Get the current end-effector name. This returns the value set by setEndEffector() (or indirectly by setEndEffectorLink()). If setEndEffector() was not called, this function reports an end-effector attached to this group. If there are multiple end-effectors, one of them is returned. If no end-effector is known, the empty string is returned.
Definition at line 1694 of file move_group_interface.cpp.
const std::string & moveit::planning_interface::MoveGroupInterface::getEndEffectorLink | ( | ) | const |
Get the current end-effector link. This returns the value set by setEndEffectorLink() (or indirectly by setEndEffector()). If setEndEffectorLink() was not called, this function reports the link name that serves as parent of an end-effector attached to this group. If there are multiple end-effectors, one of them is returned. If no such link is known, the empty string is returned.
Definition at line 1689 of file move_group_interface.cpp.
double moveit::planning_interface::MoveGroupInterface::getGoalJointTolerance | ( | ) | const |
Get the tolerance that is used for reaching a joint goal. This is distance for each joint in configuration space.
Definition at line 1905 of file move_group_interface.cpp.
double moveit::planning_interface::MoveGroupInterface::getGoalOrientationTolerance | ( | ) | const |
Get the tolerance that is used for reaching an orientation goal. This is the tolerance for roll, pitch and yaw, in radians.
Definition at line 1915 of file move_group_interface.cpp.
double moveit::planning_interface::MoveGroupInterface::getGoalPositionTolerance | ( | ) | const |
Get the tolerance that is used for reaching a position goal. This is be the radius of a sphere where the end-effector must reach.
Definition at line 1910 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::getInterfaceDescription | ( | moveit_msgs::msg::PlannerInterfaceDescription & | desc | ) | const |
Get the description of the default planning plugin loaded by the action server.
Definition at line 1327 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::getInterfaceDescriptions | ( | std::vector< moveit_msgs::msg::PlannerInterfaceDescription > & | desc | ) | const |
Get the descriptions of all planning plugins loaded by the action server.
Definition at line 1332 of file move_group_interface.cpp.
const std::vector< std::string > & moveit::planning_interface::MoveGroupInterface::getJointModelGroupNames | ( | ) | const |
Get the available planning group names.
Definition at line 2205 of file move_group_interface.cpp.
const std::vector< std::string > & moveit::planning_interface::MoveGroupInterface::getJointNames | ( | ) | const |
Get vector of names of joints available in move group.
Definition at line 1500 of file move_group_interface.cpp.
const std::vector< std::string > & moveit::planning_interface::MoveGroupInterface::getJoints | ( | ) | const |
Get all the joints this instance operates on (including fixed joints)
Definition at line 2055 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::getJointValueTarget | ( | std::vector< double > & | group_variable_values | ) | const |
Get the current joint state goal in a form compatible to setJointValueTarget()
Definition at line 1552 of file move_group_interface.cpp.
std::vector< std::string > moveit::planning_interface::MoveGroupInterface::getKnownConstraints | ( | ) | const |
Get the names of the known constraints as read from the Mongo database, if a connection was achieved.
Definition at line 2133 of file move_group_interface.cpp.
const std::vector< std::string > & moveit::planning_interface::MoveGroupInterface::getLinkNames | ( | ) | const |
Get vector of names of links available in move group.
Definition at line 1505 of file move_group_interface.cpp.
double moveit::planning_interface::MoveGroupInterface::getMaxAccelerationScalingFactor | ( | ) | const |
Get the max acceleration scaling factor set by setMaxAccelerationScalingFactor().
Definition at line 1399 of file move_group_interface.cpp.
double moveit::planning_interface::MoveGroupInterface::getMaxVelocityScalingFactor | ( | ) | const |
Get the max velocity scaling factor set by setMaxVelocityScalingFactor().
Definition at line 1389 of file move_group_interface.cpp.
rclcpp_action::Client< moveit_msgs::action::MoveGroup > & moveit::planning_interface::MoveGroupInterface::getMoveGroupClient | ( | ) | const |
Get the move_group action client used by the MoveGroupInterface. The client can be used for querying the execution state of the trajectory and abort trajectory execution during asynchronous execution.
Definition at line 1409 of file move_group_interface.cpp.
const std::string & moveit::planning_interface::MoveGroupInterface::getName | ( | ) | const |
Get the name of the group this instance operates on.
Definition at line 1305 of file move_group_interface.cpp.
const std::vector< std::string > & moveit::planning_interface::MoveGroupInterface::getNamedTargets | ( | ) | const |
Get the names of the named robot states available as targets, both either remembered states or default states from srdf.
Definition at line 1310 of file move_group_interface.cpp.
std::map< std::string, double > moveit::planning_interface::MoveGroupInterface::getNamedTargetValues | ( | const std::string & | name | ) | const |
Get the joint angles for targets specified by name.
Definition at line 1510 of file move_group_interface.cpp.
const rclcpp::Node::SharedPtr & moveit::planning_interface::MoveGroupInterface::getNode | ( | ) | const |
Get the ROS node handle of this instance operates on.
Definition at line 2195 of file move_group_interface.cpp.
moveit_msgs::msg::Constraints moveit::planning_interface::MoveGroupInterface::getPathConstraints | ( | ) | const |
Get the actual set of constraints in use with this MoveGroupInterface.
Definition at line 2138 of file move_group_interface.cpp.
const std::string & moveit::planning_interface::MoveGroupInterface::getPlannerId | ( | ) | const |
Get the current planner_id.
Definition at line 1374 of file move_group_interface.cpp.
std::map< std::string, std::string > moveit::planning_interface::MoveGroupInterface::getPlannerParams | ( | const std::string & | planner_id, |
const std::string & | group = "" |
||
) | const |
Get the planner parameters for given group and planner_id.
Definition at line 1337 of file move_group_interface.cpp.
const std::string & moveit::planning_interface::MoveGroupInterface::getPlanningFrame | ( | ) | const |
Get the name of the frame in which the robot is planning.
Definition at line 2200 of file move_group_interface.cpp.
const std::string & moveit::planning_interface::MoveGroupInterface::getPlanningPipelineId | ( | ) | const |
Get the current planning_pipeline_id.
Definition at line 1359 of file move_group_interface.cpp.
double moveit::planning_interface::MoveGroupInterface::getPlanningTime | ( | ) | const |
Get the number of seconds set by setPlanningTime()
Get time allowed to planner to solve problem before aborting.
Definition at line 2190 of file move_group_interface.cpp.
const std::string & moveit::planning_interface::MoveGroupInterface::getPoseReferenceFrame | ( | ) | const |
Get the reference frame set by setPoseReferenceFrame(). By default this is the reference frame of the robot model.
Definition at line 1900 of file move_group_interface.cpp.
const geometry_msgs::msg::PoseStamped & moveit::planning_interface::MoveGroupInterface::getPoseTarget | ( | const std::string & | end_effector_link = "" | ) | const |
Get the currently set pose goal for the end-effector end_effector_link. If end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is assumed. If multiple targets are set for end_effector_link this will return the first one. If no pose target is set for this end_effector_link then an empty pose will be returned (check for orientation.xyzw == 0).
Definition at line 1795 of file move_group_interface.cpp.
const std::vector< geometry_msgs::msg::PoseStamped > & moveit::planning_interface::MoveGroupInterface::getPoseTargets | ( | const std::string & | end_effector_link = "" | ) | const |
Get the currently set pose goal for the end-effector end_effector_link. The pose goal can consist of multiple poses, if corresponding setPoseTarget() calls were made. Otherwise, only one pose is returned in the vector. If end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is assumed
Definition at line 1801 of file move_group_interface.cpp.
std::vector< double > moveit::planning_interface::MoveGroupInterface::getRandomJointValues | ( | ) | const |
Get random joint values for the joints planned for by this instance (see getJoints())
Definition at line 1961 of file move_group_interface.cpp.
geometry_msgs::msg::PoseStamped moveit::planning_interface::MoveGroupInterface::getRandomPose | ( | const std::string & | end_effector_link = "" | ) | const |
Get a random reachable pose for the end-effector end_effector_link. If end_effector_link is empty (the default value) then the end-effector reported by getEndEffectorLink() is assumed.
Definition at line 1968 of file move_group_interface.cpp.
|
inline |
Get the currently remembered map of names to joint values.
Definition at line 921 of file move_group_interface.h.
moveit::core::RobotModelConstPtr moveit::planning_interface::MoveGroupInterface::getRobotModel | ( | ) | const |
Get the RobotModel object.
Definition at line 1322 of file move_group_interface.cpp.
|
protected |
return the full RobotState of the joint-space target, only for internal use
Definition at line 1684 of file move_group_interface.cpp.
const std::shared_ptr< tf2_ros::Buffer > & moveit::planning_interface::MoveGroupInterface::getTF | ( | ) | const |
Get the tf2_ros::Buffer.
Definition at line 1317 of file move_group_interface.cpp.
moveit_msgs::msg::TrajectoryConstraints moveit::planning_interface::MoveGroupInterface::getTrajectoryConstraints | ( | ) | const |
unsigned int moveit::planning_interface::MoveGroupInterface::getVariableCount | ( | ) | const |
Get the number of variables used to describe the state of this group. This is larger or equal to the number of DOF.
Definition at line 2060 of file move_group_interface.cpp.
moveit::core::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::move | ( | ) |
Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified target. This call is always blocking (waits for the execution of the trajectory to complete) and requires an asynchronous spinner to be started.
Definition at line 1414 of file move_group_interface.cpp.
moveit::planning_interface::MoveGroupInterface::MOVEIT_STRUCT_FORWARD | ( | Plan | ) |
|
delete |
|
noexcept |
Definition at line 1291 of file move_group_interface.cpp.
moveit::core::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::plan | ( | Plan & | plan | ) |
Compute a motion plan that takes the group declared in the constructor from the current state to the specified target. No execution is performed. The resulting plan is stored in plan.
Definition at line 1442 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::rememberJointValues | ( | const std::string & | name | ) |
Remember the current joint values (of the robot being monitored) under name. These can be used by setNamedTarget(). These values are remembered locally in the client. Other clients will not have access to them.
Definition at line 1942 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::rememberJointValues | ( | const std::string & | name, |
const std::vector< double > & | values | ||
) |
Remember the specified joint values under name. These can be used by setNamedTarget(). These values are remembered locally in the client. Other clients will not have access to them.
Definition at line 2072 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setApproximateJointValueTarget | ( | const Eigen::Isometry3d & | eef_pose, |
const std::string & | end_effector_link = "" |
||
) |
Set the joint state goal for a particular joint by computing IK.
This is different from setPoseTarget() in that a single IK state (aka JointValueTarget) is computed using IK, and the resulting JointValueTarget is used as the target for planning.
After this call, the JointValueTarget is used instead of any previously set Position, Orientation, or Pose targets.
If IK fails to find a solution then an approximation is used.
Definition at line 1677 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setApproximateJointValueTarget | ( | const geometry_msgs::msg::Pose & | eef_pose, |
const std::string & | end_effector_link = "" |
||
) |
Set the joint state goal for a particular joint by computing IK.
This is different from setPoseTarget() in that a single IK state (aka JointValueTarget) is computed using IK, and the resulting JointValueTarget is used as the target for planning.
After this call, the JointValueTarget is used instead of any previously set Position, Orientation, or Pose targets.
If IK fails to find a solution then an approximation is used.
Definition at line 1665 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setApproximateJointValueTarget | ( | const geometry_msgs::msg::PoseStamped & | eef_pose, |
const std::string & | end_effector_link = "" |
||
) |
Set the joint state goal for a particular joint by computing IK.
This is different from setPoseTarget() in that a single IK state (aka JointValueTarget) is computed using IK, and the resulting JointValueTarget is used as the target for planning.
After this call, the JointValueTarget is used instead of any previously set Position, Orientation, or Pose targets.
If IK fails to find a solution then an approximation is used.
Definition at line 1671 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setConstraintsDatabase | ( | const std::string & | host, |
unsigned int | port | ||
) |
Specify where the database server that holds known constraints resides.
Definition at line 2173 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setEndEffector | ( | const std::string & | eef_name | ) |
Specify the name of the end-effector to use. This is equivalent to setting the EndEffectorLink to the parent link of this end effector.
Definition at line 1708 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setEndEffectorLink | ( | const std::string & | end_effector_link | ) |
Specify the parent link of the end-effector. This end_effector_link will be used in calls to pose target functions when end_effector_link is not explicitly specified.
Definition at line 1699 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setGoalJointTolerance | ( | double | tolerance | ) |
Set the joint tolerance (for each joint) that is used for reaching the goal when moving to a joint value target.
Definition at line 1927 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setGoalOrientationTolerance | ( | double | tolerance | ) |
Set the orientation tolerance that is used for reaching the goal when moving to a pose.
Definition at line 1937 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setGoalPositionTolerance | ( | double | tolerance | ) |
Set the position tolerance that is used for reaching the goal when moving to a pose.
Definition at line 1932 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setGoalTolerance | ( | double | tolerance | ) |
Set the tolerance that is used for reaching the goal. For joint state goals, this will be distance for each joint, in the configuration space (radians or meters depending on joint type). For pose goals this will be the radius of a sphere where the end-effector must reach. This function simply triggers calls to setGoalPositionTolerance(), setGoalOrientationTolerance() and setGoalJointTolerance().
Definition at line 1920 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget | ( | const Eigen::Isometry3d & | eef_pose, |
const std::string & | end_effector_link = "" |
||
) |
Set the joint state goal for a particular joint by computing IK.
This is different from setPoseTarget() in that a single IK state (aka JointValueTarget) is computed using IK, and the resulting JointValueTarget is used as the target for planning.
After this call, the JointValueTarget is used instead of any previously set Position, Orientation, or Pose targets.
If IK fails to find a solution then false is returned BUT THE PARTIAL RESULT OF IK IS STILL SET AS THE GOAL.
Definition at line 1659 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget | ( | const geometry_msgs::msg::Pose & | eef_pose, |
const std::string & | end_effector_link = "" |
||
) |
Set the joint state goal for a particular joint by computing IK.
This is different from setPoseTarget() in that a single IK state (aka JointValueTarget) is computed using IK, and the resulting JointValueTarget is used as the target for planning.
After this call, the JointValueTarget is used instead of any previously set Position, Orientation, or Pose targets.
If IK fails to find a solution then false is returned BUT THE PARTIAL RESULT OF IK IS STILL SET AS THE GOAL.
Definition at line 1647 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget | ( | const geometry_msgs::msg::PoseStamped & | eef_pose, |
const std::string & | end_effector_link = "" |
||
) |
Set the joint state goal for a particular joint by computing IK.
This is different from setPoseTarget() in that a single IK state (aka JointValueTarget) is computed using IK, and the resulting JointValueTarget is used as the target for planning.
After this call, the JointValueTarget is used instead of any previously set Position, Orientation, or Pose targets.
If IK fails to find a solution then false is returned BUT THE PARTIAL RESULT OF IK IS STILL SET AS THE GOAL.
Definition at line 1653 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget | ( | const moveit::core::RobotState & | robot_state | ) |
Set the JointValueTarget and use it for future planning requests.
The target for all joints in the group are set to the value in robot_state.
After this call, the JointValueTarget is used instead of any previously set Position, Orientation, or Pose targets.
If these values are out of bounds then false is returned BUT THE VALUES ARE STILL SET AS THE GOAL.
Definition at line 1614 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget | ( | const sensor_msgs::msg::JointState & | state | ) |
Set the JointValueTarget and use it for future planning requests.
state is used to set the target joint state values. Values not specified in state remain unchanged.
After this call, the JointValueTarget is used instead of any previously set Position, Orientation, or Pose targets.
If these values are out of bounds then false is returned BUT THE VALUES ARE STILL SET AS THE GOAL.
Definition at line 1642 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget | ( | const std::map< std::string, double > & | variable_values | ) |
Set the JointValueTarget and use it for future planning requests.
variable_values is a map of joint variable names to values. Joints in the group are used to set the JointValueTarget. Joints in the model but not in the group are ignored. An exception is thrown if a joint name is not found in the model. Joint variables in the group that are missing from variable_values remain unchanged (to reset all target variables to their current values in the robot use setJointValueTarget(getCurrentJointValues())).
After this call, the JointValueTarget is used instead of any previously set Position, Orientation, or Pose targets.
If these values are out of bounds then false is returned BUT THE VALUES ARE STILL SET AS THE GOAL.
Definition at line 1572 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget | ( | const std::string & | joint_name, |
const std::vector< double > & | values | ||
) |
Set the JointValueTarget and use it for future planning requests.
values MUST have one value for each variable in joint joint_name. values are set as the target for this joint. Other joint targets remain unchanged.
After this call, the JointValueTarget is used instead of any previously set Position, Orientation, or Pose targets.
If these values are out of bounds then false is returned BUT THE VALUES ARE STILL SET AS THE GOAL.
Definition at line 1627 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget | ( | const std::string & | joint_name, |
double | value | ||
) |
Set the JointValueTarget and use it for future planning requests.
Joint joint_name must be a 1-DOF joint. value is set as the target for this joint. Other joint targets remain unchanged.
After this call, the JointValueTarget is used instead of any previously set Position, Orientation, or Pose targets.
If these values are out of bounds then false is returned BUT THE VALUES ARE STILL SET AS THE GOAL.
Definition at line 1621 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget | ( | const std::vector< double > & | group_variable_values | ) |
Set the JointValueTarget and use it for future planning requests.
group_variable_values MUST exactly match the variable order as returned by getJointValueTarget(std::vector<double>&).
This always sets all of the group's joint values.
After this call, the JointValueTarget is used instead of any previously set Position, Orientation, or Pose targets.
If these values are out of bounds then false is returned BUT THE VALUES ARE STILL SET AS THE GOAL.
Definition at line 1557 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setJointValueTarget | ( | const std::vector< std::string > & | variable_names, |
const std::vector< double > & | variable_values | ||
) |
Set the JointValueTarget and use it for future planning requests.
variable_names are variable joint names and variable_values are variable joint positions. Joints in the group are used to set the JointValueTarget. Joints in the model but not in the group are ignored. An exception is thrown if a joint name is not found in the model. Joint variables in the group that are missing from variable_names remain unchanged (to reset all target variables to their current values in the robot use setJointValueTarget(getCurrentJointValues())).
After this call, the JointValueTarget is used instead of any previously set Position, Orientation, or Pose targets.
If these values are out of bounds then false is returned BUT THE VALUES ARE STILL SET AS THE GOAL.
Definition at line 1590 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setLookAroundAttempts | ( | int32_t | attempts | ) |
How often is the system allowed to move the camera to update environment model when looking.
Definition at line 2088 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setMaxAccelerationScalingFactor | ( | double | max_acceleration_scaling_factor | ) |
Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0,1]. The maximum joint acceleration specified in the robot model is multiplied by the factor. If the value is 0, it is set to the default value, which is defined in joint_limits.yaml of the moveit_config. If the value is greater than 1, it is set to 1.0.
Definition at line 1394 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setMaxVelocityScalingFactor | ( | double | max_velocity_scaling_factor | ) |
Set a scaling factor for optionally reducing the maximum joint velocity. Allowed values are in (0,1]. The maximum joint velocity specified in the robot model is multiplied by the factor. If the value is 0, it is set to the default value, which is defined in joint_limits.yaml of the moveit_config. If the value is greater than 1, it is set to 1.0.
Definition at line 1384 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setNamedTarget | ( | const std::string & | name | ) |
Set the current joint values to be ones previously remembered by rememberJointValues() or, if not found, that are specified in the SRDF under the name name as a group state.
Definition at line 1533 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setNumPlanningAttempts | ( | unsigned int | num_planning_attempts | ) |
Set the number of times the motion plan is to be computed from scratch before the shortest solution is returned. The default value is 1.
Definition at line 1379 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setOrientationTarget | ( | double | x, |
double | y, | ||
double | z, | ||
double | w, | ||
const std::string & | end_effector_link = "" |
||
) |
Set the goal orientation of the end-effector end_effector_link to be the quaternion (x,y,z,w).
If end_effector_link is empty then getEndEffectorLink() is used.
This new orientation target replaces any pre-existing JointValueTarget or pre-existing Position, Orientation, or Pose target for this end_effector_link.
Definition at line 1869 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setPathConstraints | ( | const moveit_msgs::msg::Constraints & | constraint | ) |
Specify a set of path constraints to use. This version does not require a database server. This replaces any path constraints set in previous calls to setPathConstraints().
Definition at line 2148 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setPathConstraints | ( | const std::string & | constraint | ) |
Specify a set of path constraints to use. The constraints are looked up by name from the Mongo database server. This replaces any path constraints set in previous calls to setPathConstraints().
Definition at line 2143 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setPlannerId | ( | const std::string & | planner_id | ) |
Specify a planner to be used for further planning.
Definition at line 1369 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setPlannerParams | ( | const std::string & | planner_id, |
const std::string & | group, | ||
const std::map< std::string, std::string > & | params, | ||
bool | bReplace = false |
||
) |
Set the planner parameters for given group and planner_id.
Definition at line 1343 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setPlanningPipelineId | ( | const std::string & | pipeline_id | ) |
Specify a planning pipeline to be used for further planning.
Definition at line 1354 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setPlanningTime | ( | double | seconds | ) |
Specify the maximum amount of time to use when planning.
Set time allowed to planner to solve problem before aborting.
Definition at line 2184 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setPoseReferenceFrame | ( | const std::string & | pose_reference_frame | ) |
Specify which reference frame to assume for poses specified without a reference frame.
Definition at line 1895 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setPoseTarget | ( | const Eigen::Isometry3d & | end_effector_pose, |
const std::string & | end_effector_link = "" |
||
) |
Set the goal pose of the end-effector end_effector_link.
If end_effector_link is empty then getEndEffectorLink() is used.
This new pose target replaces any pre-existing JointValueTarget or pre-existing Position, Orientation, or Pose target for this end_effector_link.
Definition at line 1726 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setPoseTarget | ( | const geometry_msgs::msg::Pose & | target, |
const std::string & | end_effector_link = "" |
||
) |
Set the goal pose of the end-effector end_effector_link.
If end_effector_link is empty then getEndEffectorLink() is used.
This new orientation target replaces any pre-existing JointValueTarget or pre-existing Position, Orientation, or Pose target for this end_effector_link.
Definition at line 1735 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setPoseTarget | ( | const geometry_msgs::msg::PoseStamped & | target, |
const std::string & | end_effector_link = "" |
||
) |
Set the goal pose of the end-effector end_effector_link.
If end_effector_link is empty then getEndEffectorLink() is used.
This new orientation target replaces any pre-existing JointValueTarget or pre-existing Position, Orientation, or Pose target for this end_effector_link.
Definition at line 1744 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setPoseTargets | ( | const EigenSTL::vector_Isometry3d & | end_effector_pose, |
const std::string & | end_effector_link = "" |
||
) |
Set goal poses for end_effector_link.
If end_effector_link is empty then getEndEffectorLink() is used.
When planning, the planner will find a path to one (arbitrarily chosen) pose from the list. If this group contains multiple end effectors then all end effectors in the group should have the same number of pose targets. If planning is successful then the result of the plan will place all end effectors at a pose from the same index in the list. (In other words, if one end effector ends up at the 3rd pose in the list then all end effectors in the group will end up at the 3rd pose in their respective lists. End effectors which do not matter (i.e. can end up in any position) can have their pose targets disabled by calling clearPoseTarget() for that end_effector_link.
This new orientation target replaces any pre-existing JointValueTarget or pre-existing Position, Orientation, or Pose target(s) for this end_effector_link.
Definition at line 1751 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setPoseTargets | ( | const std::vector< geometry_msgs::msg::Pose > & | target, |
const std::string & | end_effector_link = "" |
||
) |
Set goal poses for end_effector_link.
If end_effector_link is empty then getEndEffectorLink() is used.
When planning, the planner will find a path to one (arbitrarily chosen) pose from the list. If this group contains multiple end effectors then all end effectors in the group should have the same number of pose targets. If planning is successful then the result of the plan will place all end effectors at a pose from the same index in the list. (In other words, if one end effector ends up at the 3rd pose in the list then all end effectors in the group will end up at the 3rd pose in their respective lists. End effectors which do not matter (i.e. can end up in any position) can have their pose targets disabled by calling clearPoseTarget() for that end_effector_link.
This new orientation target replaces any pre-existing JointValueTarget or pre-existing Position, Orientation, or Pose target(s) for this end_effector_link.
Definition at line 1765 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setPoseTargets | ( | const std::vector< geometry_msgs::msg::PoseStamped > & | target, |
const std::string & | end_effector_link = "" |
||
) |
Set goal poses for end_effector_link.
If end_effector_link is empty then getEndEffectorLink() is used.
When planning, the planner will find a path to one (arbitrarily chosen) pose from the list. If this group contains multiple end effectors then all end effectors in the group should have the same number of pose targets. If planning is successful then the result of the plan will place all end effectors at a pose from the same index in the list. (In other words, if one end effector ends up at the 3rd pose in the list then all end effectors in the group will end up at the 3rd pose in their respective lists. End effectors which do not matter (i.e. can end up in any position) can have their pose targets disabled by calling clearPoseTarget() for that end_effector_link.
This new orientation target replaces any pre-existing JointValueTarget or pre-existing Position, Orientation, or Pose target(s) for this end_effector_link.
Definition at line 1780 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setPositionTarget | ( | double | x, |
double | y, | ||
double | z, | ||
const std::string & | end_effector_link = "" |
||
) |
Set the goal position of the end-effector end_effector_link to be (x, y, z).
If end_effector_link is empty then getEndEffectorLink() is used.
This new position target replaces any pre-existing JointValueTarget or pre-existing Position, Orientation, or Pose target for this end_effector_link.
Definition at line 1821 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setRandomTarget | ( | ) |
Set the joint state goal to a random joint configuration.
After this call, the JointValueTarget is used instead of any previously set Position, Orientation, or Pose targets.
Definition at line 1494 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setReplanAttempts | ( | int32_t | attempts | ) |
Maximum number of replanning attempts.
Definition at line 2107 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setReplanDelay | ( | double | delay | ) |
Sleep this duration between replanning attempts (in walltime seconds)
Definition at line 2120 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::setRPYTarget | ( | double | roll, |
double | pitch, | ||
double | yaw, | ||
const std::string & | end_effector_link = "" |
||
) |
Set the goal orientation of the end-effector end_effector_link to be (roll,pitch,yaw) radians.
If end_effector_link is empty then getEndEffectorLink() is used.
This new orientation target replaces any pre-existing JointValueTarget or pre-existing Position, Orientation, or Pose target for this end_effector_link.
Definition at line 1846 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setStartState | ( | const moveit::core::RobotState & | start_state | ) |
If a different start state should be considered instead of the current state of the robot, this function sets that state.
Definition at line 1484 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setStartState | ( | const moveit_msgs::msg::RobotState & | start_state | ) |
If a different start state should be considered instead of the current state of the robot, this function sets that state.
Definition at line 1479 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setStartStateToCurrentState | ( | ) |
Set the starting state for planning to be that reported by the robot's joint state publication.
Definition at line 1489 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::setTrajectoryConstraints | ( | const moveit_msgs::msg::TrajectoryConstraints & | constraint | ) |
void moveit::planning_interface::MoveGroupInterface::setWorkspace | ( | double | minx, |
double | miny, | ||
double | minz, | ||
double | maxx, | ||
double | maxy, | ||
double | maxz | ||
) |
Specify the workspace bounding box. The box is specified in the planning frame (i.e. relative to the robot root link start position). This is useful when the planning group contains the root joint of the robot – i.e. when planning motion for the robot relative to the world.
Definition at line 2178 of file move_group_interface.cpp.
bool moveit::planning_interface::MoveGroupInterface::startStateMonitor | ( | double | wait = 1.0 | ) |
When reasoning about the current state of a robot, a CurrentStateMonitor instance is automatically constructed. This function allows triggering the construction of that object from the beginning, so that future calls to functions such as getCurrentState() will not take so long and are less likely to fail.
Definition at line 1947 of file move_group_interface.cpp.
void moveit::planning_interface::MoveGroupInterface::stop | ( | ) |
Stop any trajectory execution, if one is active.
Definition at line 1474 of file move_group_interface.cpp.
|
static |
Default ROS parameter name from where to read the robot's URDF. Set to 'robot_description'.
Definition at line 79 of file move_group_interface.h.