Setting a joint state target (goal)
setJointValueTarget
There are 2 types of goal targets:
- a JointValueTarget (aka JointStateTarget) specifies an absolute value for each joint (angle for rotational joints or position for prismatic joints).
- a PoseTarget (Position, Orientation, or Pose) specifies the pose of one or more end effectors (and the planner can use any joint values that reaches the pose(s)).
Only one or the other is used for planning.
Calling any of the setJointValueTarget()
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.
setJointValueTarget
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.
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.
使用
// Create the MoveIt MoveGroup Interface
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "ur_gripper"); //change
// Set a Joint value
move_group_interface.setJointValueTarget(std::vector<double>({-0.02, -0.02})); //打开
move_group_interface.setJointValueTarget(std::vector<double>({0.0, 0.0})); //关闭
// Create a plan to that target pose
auto const [success, plan] = [&move_group_interface]{
moveit::planning_interface::MoveGroupInterface::Plan msg;
auto const ok = static_cast<bool>(move_group_interface.plan(msg));
return std::make_pair(ok, msg);
}();
// Execute the plan
if(success) {
move_group_interface.execute(plan);
} else {
RCLCPP_ERROR(logger, "Planing failed!");
}