Hello,
I am not able to Pick up objects in RViz using MoveIt with my Robot.
I did the official tutorial about [Pick and Place](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/pick_place/pick_place_tutorial.html) on the MoveIt website. I played around with the Code and was able to change the object and table positions and still make the Panda Robot grab and place the objects where I wanted.
Now, I tried to do the same with my Robot, however, every time I try to pick up an object up I get the following error:
[ INFO] [1537290395.155352985]: Planning attempt 1 of at most 1
[ INFO] [1537290395.186375463]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1537290395.229833831]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0
[ WARN] [1537290395.229998325]: All supplied grasps failed. Retrying last grasp in verbose mode.
[ INFO] [1537290395.230079900]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1537290395.239498324]: IK failed
[ INFO] [1537290395.244838125]: IK failed
[ INFO] [1537290395.254101979]: IK failed
[ INFO] [1537290395.254125795]: Sampler failed to produce a state
[ INFO] [1537290395.254139738]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1537290395.254280163]: Pickup planning completed after 0.074341 seconds
To confirm I was giving the correct, reachable grasp positions, I moved the robot to the Pre-Grasp, Grasp and Post-Grasp positions. It turns out I can plan a Path to those by setting them as Pose Targets, so I do think they are valid. [Result](https://media.giphy.com/media/1APbT1kuq1A71Uy6QT/giphy.gif)
I do not understand why planning that path with the pick method does not work and would appreciate it, if anyone could help me out here.
My Code ([GitHub for whole file](https://github.com/JulianRi/boris_debug/blob/master/MoveitInterface.cpp)):
#include // Move the Robot.
#include // Modify Robot's world.
#include
const float OPEN_GRIPPER = - 1.00;
const float CLOSE_GRIPPER = 0.00;
const float DISTANCE_TO_PALM = 0.09; // Distance from last link (hand_link origin) to grapable area of the gripper.
const float GRIPPER_LENGTH = 0.06; // From Palm to the gripper end.
float table_size_x = 0.20;
float table_size_y = 0.20;
float table_size_z = 0.04;
float object_size_x = 0.02;
float object_size_y = 0.02;
float object_size_z = 0.08;
float table_origin_x = 0.40;
float table_origin_y = 0.00;
float table_origin_z = table_size_z / 2;
float object_origin_x = table_origin_x;
float object_origin_y = table_origin_y;
float object_origin_z = table_size_z + object_size_z / 2;
void openGripper(trajectory_msgs::JointTrajectory& posture) {
posture.joint_names.resize(6);
posture.joint_names[0] = "gripper_right_joint";
posture.joint_names[1] = "gripper_right_front_joint";
posture.joint_names[2] = "gripper_right_connection_joint";
posture.joint_names[3] = "gripper_left_joint";
posture.joint_names[4] = "gripper_left_front_joint";
posture.joint_names[5] = "gripper_left_connection_joint";
/* Set them as open. */
posture.points.resize(1);
posture.points[0].positions.resize(6);
posture.points[0].positions[0] = - 1.00;
posture.points[0].positions[1] = - 1.00;
posture.points[0].positions[2] = - 1.00;
posture.points[0].positions[3] = - 1.00;
posture.points[0].positions[4] = - 1.00;
posture.points[0].positions[5] = - 1.00;
posture.points[0].time_from_start = ros::Duration(1.0);
}
void closedGripper(trajectory_msgs::JointTrajectory& posture) {
posture.joint_names.resize(6);
posture.joint_names[0] = "gripper_right_joint";
posture.joint_names[1] = "gripper_right_front_joint";
posture.joint_names[2] = "gripper_right_connection_joint";
posture.joint_names[3] = "gripper_left_joint";
posture.joint_names[4] = "gripper_left_front_joint";
posture.joint_names[5] = "gripper_left_connection_joint";
/* Set them as closed. */
posture.points.resize(1);
posture.points[0].positions.resize(6);
posture.points[0].positions[0] = 0;
posture.points[0].positions[1] = 0;
posture.points[0].positions[2] = 0;
posture.points[0].positions[3] = 0;
posture.points[0].positions[4] = 0;
posture.points[0].positions[5] = 0;
posture.points[0].time_from_start = ros::Duration(1.0);
}
void pick(moveit::planning_interface::MoveGroupInterface& move_group) {
std::vector grasps;
grasps.resize(1);
// Position of the last link in that chain. (Connected to the End Effector.)
grasps[0].grasp_pose.header.frame_id = "root_link";
grasps[0].grasp_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0);
grasps[0].grasp_pose.pose.position.x = object_origin_x - DISTANCE_TO_PALM - object_size_x / 2;
grasps[0].grasp_pose.pose.position.y = object_origin_y;
grasps[0].grasp_pose.pose.position.z = object_origin_z;
grasps[0].pre_grasp_approach.direction.header.frame_id = "root_link";
grasps[0].pre_grasp_approach.direction.vector.x = 1.00;
grasps[0].pre_grasp_approach.min_distance = GRIPPER_LENGTH * 1;
grasps[0].pre_grasp_approach.desired_distance = GRIPPER_LENGTH * 1.2;
grasps[0].post_grasp_retreat.direction.header.frame_id = "root_link";
grasps[0].post_grasp_retreat.direction.vector.z = 1.00;
grasps[0].post_grasp_retreat.min_distance = 0.05;
grasps[0].post_grasp_retreat.desired_distance = 0.10;
// Setting posture of Effector before grasp
openGripper(grasps[0].pre_grasp_posture);
// Setting posture of Effector during grasp
closedGripper(grasps[0].grasp_posture);
move_group.setSupportSurfaceName("table1");
move_group.pick("object", grasps);
}
void addCollisionObjects() {
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
std::vector collision_objects;
collision_objects.resize(2);
collision_objects[0].id = "table1";
collision_objects[0].header.frame_id = "root_link";
collision_objects[0].primitives.resize(1);
collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;
collision_objects[0].primitives[0].dimensions.resize(3);
collision_objects[0].primitives[0].dimensions[0] = table_size_x;
collision_objects[0].primitives[0].dimensions[1] = table_size_y;
collision_objects[0].primitives[0].dimensions[2] = table_size_z;
collision_objects[0].primitive_poses.resize(1);
collision_objects[0].primitive_poses[0].position.x = table_origin_x;
collision_objects[0].primitive_poses[0].position.y = table_origin_y;
collision_objects[0].primitive_poses[0].position.z = table_origin_z;
collision_objects[0].operation = collision_objects[0].ADD;
collision_objects[1].id = "object";
collision_objects[1].header.frame_id = "root_link";
collision_objects[1].primitives.resize(1);
collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX;
collision_objects[1].primitives[0].dimensions.resize(3);
collision_objects[1].primitives[0].dimensions[0] = object_size_x;
collision_objects[1].primitives[0].dimensions[1] = object_size_y;
collision_objects[1].primitives[0].dimensions[2] = object_size_z;
collision_objects[1].primitive_poses.resize(1);
collision_objects[1].primitive_poses[0].position.x = object_origin_x;
collision_objects[1].primitive_poses[0].position.y = object_origin_y;
collision_objects[1].primitive_poses[0].position.z = object_origin_z;
collision_objects[1].operation = collision_objects[1].ADD;
planning_scene_interface.applyCollisionObjects(collision_objects);
}
void planJointStateGoalGripper(
moveit::planning_interface::MoveGroupInterface& move_group,
double angle) {
moveit::core::RobotStatePtr current_state;
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
std::vector joint_group_positions;
current_state = move_group.getCurrentState();
current_state -> copyJointGroupPositions(move_group.getCurrentState()
-> getJointModelGroup("boris_gripper"), joint_group_positions);
joint_group_positions[0] = angle;
joint_group_positions[1] = angle;
joint_group_positions[2] = angle;
joint_group_positions[3] = angle;
joint_group_positions[4] = angle;
joint_group_positions[5] = angle;
move_group.setJointValueTarget(joint_group_positions);
if (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS) {
move_group.execute(my_plan);
ROS_INFO("Planning Done.");
}
else {
ROS_INFO("Planning Failed.");
}
}
void planPositionPoseGoalArm(moveit::planning_interface::MoveGroupInterface& move_group, double x, double y, double z, double roll, double pitch, double yaw) {
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
geometry_msgs::Pose target_pose;
target_pose.position.x = x;
target_pose.position.y = y;
target_pose.position.z = z;
target_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
move_group.setPoseTarget(target_pose);
if (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS) {
move_group.execute(my_plan);
ROS_INFO("Planning Done.");
}
else {
ROS_INFO("Planning Failed.");
}
}
int main(int argc, char** argv) {
ros::init(argc, argv, "moveit_interface_boris");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
moveit_visual_tools::MoveItVisualTools visual_tools("root_link");
moveit::planning_interface::MoveGroupInterface move_group("boris_arm");
moveit::planning_interface::MoveGroupInterface move_group_gripper("boris_gripper");
// Initial Position before starting:
planJointStateGoalGripper(move_group_gripper, OPEN_GRIPPER);
planPositionPoseGoalArm(move_group,
0.17,
0,
0.07,
0, 0, 0);
addCollisionObjects();
// Start planning for the pick:
visual_tools.prompt("Press 'next' to start.");
//pick(move_group); // This fails!!!
// Pre Grab Position:
planPositionPoseGoalArm(move_group,
object_origin_x - DISTANCE_TO_PALM - object_size_x / 2 - GRIPPER_LENGTH * 1,
object_origin_y,
object_origin_z,
0, 0, 0);
// Grasp Position:
planPositionPoseGoalArm(move_group,
object_origin_x - DISTANCE_TO_PALM - object_size_x / 2,
object_origin_y,
object_origin_z,
0, 0, 0);
// Post Grab Position.
planPositionPoseGoalArm(move_group,
object_origin_x - DISTANCE_TO_PALM - object_size_x / 2,
object_origin_y,
object_origin_z + 0.05 * 1,
0, 0, 0);
return 0;
}
↧