MoveIt Pick Randomly Fails/Succeeds?
Update 3:
I am using a different updated arm with an additional Joint, making it a 6 DOF with 8 joints, 2 being mimic joints. Now planning always works after 1 attemps.
However, this is what i found out so far:
I think it turns out that Mimic Joints are not part of the problem. The Robot Arm I used had 7 joints in it's chain, but 2 of those were Mimic Joints. I think that makes it a 5 DOF robot.
I have found a few discussions about 5 DOF robots not properly being supported by the kinematics plugins, although these discussions where quite old. I do not know if that is still up to date. However, when I comment out one mimic joints (tested with both), making it a 6 DOF robot, a solution can easily be found by MoveIt.
So what I did before was just plan over and over again (up to 500 times with the same grasp position), and eventually it found a trajectory. Sometimes after the first attempt, sometimes none after 500 attemps. I think this is because MoveIt chooses a random start position (Link), and for my robot it was just very unlikely for a valid position to be found.
As I said before, when I set the PreGrasp, Graps etc. positions manually with setPoseTarget() and attach/detach the object manually with attachObject() / detachObject() all planning succeeds. However, I noticed that from the PreGrasp Position to the Graps position, the trajectory does not move in a straight line forward.
When using Pick and Place a pregraspapproach.direction.vector is given. Here it can be defined that the object to be grasped should be approached from the x direction, but no movement towards y and z for example. Maybe this is what makes my planning fail, but I am not sure about that.
Theoretically it should have been possible for the old arm to move in a straight line forward.
Update2:
I did more testing and I think it fails due to setting mindistance and desireddistance for the grasps wrong. I do not know why they would be wrong and what are the correct values though.
What I currently am doing, and what did not fail yet, is just calling pick over and over again till it does not fail. I do not know why it can find a solution by giving the same data, and after how many attempts that solution is found seems random to be. Sometime it works after the first try, sometimes after 15.
The Code looks like this, place is handled the same:
bool pick(moveit::planning_interface::MoveGroupInterface& move_group) {
int directions_tested = 51;
std::vector<moveit_msgs::Grasp> grasps;
grasps.resize(directions_tested);
for (int i = 0; i < directions_tested; i++) {
grasps[i].grasp_pose.header.frame_id = "root_link";
grasps[i].grasp_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0);
grasps[i].grasp_pose.pose.position.x = object_origin_x - DISTANCE_TO_PALM - object_size_x / 2;
grasps[i].grasp_pose.pose.position.y = object_origin_y;
grasps[i].grasp_pose.pose.position.z = object_origin_z;
grasps[i].pre_grasp_approach.direction.header.frame_id = "root_link";
grasps[i].pre_grasp_approach.direction.vector.x = 1.00;
grasps[i].pre_grasp_approach.min_distance = 0.0004 * i;
grasps[i].pre_grasp_approach.desired_distance = 0.0008 * i;
grasps[i].post_grasp_retreat.direction.header.frame_id = "root_link";
grasps[i].post_grasp_retreat.direction.vector.z = 1.00;
grasps[i].post_grasp_retreat.min_distance = 0.05;
grasps[i].post_grasp_retreat.desired_distance = 0.10;
openGripper(grasps[i].pre_grasp_posture); // set posture
closedGripper(grasps[i].grasp_posture); // set posture
}
move_group.setSupportSurfaceName("table1");
if( move_group.pick("object", grasps) == moveit::planning_interface::MoveItErrorCode::SUCCESS ) {
ROS_WARN(" ## SUCCESS ##");
return true;
}
else {
return false;
}
}
int main(int argc, char** argv) {
// ...
addCollisionObjects();
int max_plan_counter;
int plan_counter;
bool success;
max_plan_counter = 20;
plan_counter = 0;
success = false;
ROS_INFO("Pick:");
while (plan_counter < max_plan_counter) {
ROS_INFO("Attempt #%d:", plan_counter + 1);
success = pick(move_group);
plan_counter++;
if(success) {
break;
}
}
if(success) {
max_plan_counter = 20;
plan_counter = 0;
success = false;
ROS_INFO("Place:");
while (plan_counter < max_plan_counter) {
ROS_INFO("Attempt #%d:", plan_counter + 1);
success = place(move_group);
plan_counter++;
if(success) {
break;
}
}
} //...
return 0;
}
Update1:
It turns out that using normal Joints instead of Mimic Joints works with the same positions being provided. However, this plan cannot be executed on my real robot, because the Mimic Joint is not free to move.
What options do I have to use Mimic Joints within a move group?
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 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
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):
#include <moveit/move_group_interface/move_group_interface.h> // Move the Robot.
#include <moveit/planning_scene_interface/planning_scene_interface.h> // Modify Robot's world.
#include <moveit_visual_tools/moveit_visual_tools.h>
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<moveit_msgs::Grasp> 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<moveit_msgs::CollisionObject> 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<double> 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;
}
Asked by JulianR on 2018-09-18 12:48:31 UTC
Comments
I'm currently facing the same issue. My code is in Python though I have a grasp generator which is basically trying all the different combinations of orientations for the grasp attempt. Doesn't seem to make much difference. How many DOF is your arm and what is the kinematics solver?
Asked by Wintermute on 2018-09-18 22:54:19 UTC
There are 7 joints in my arm chain, but only 5 of them are actuated and 2 are mimic joints. That makes 5 DOF? On start I get an error about dynamic solvers not being initialized because of mimic, but I do not know if that causes something. I use the standard kdl solver and plan with RTTConnect
Asked by JulianR on 2018-09-19 01:09:25 UTC
I don't know about mimic joints, but I remember that the PickPlace tutorial did not work reliably for me until I increased the amount of planning attempts. I would suggest doing that and then digging into the manipulation pipeline. Would be glad to see this thread updated.
Asked by fvd on 2018-09-29 01:31:22 UTC
I updated what I found out so far. I changed my robot's hardware and now use a slightly different arm with one more joint and all works fine. So I think the main issue is using a 5 DOF robot instead of 6 or 7 DOF in MoveIt.
Asked by JulianR on 2018-09-29 02:24:13 UTC
Hi Julian, I am also working on a robot arm with mimic joints. But I am wondering how to create mimic joints by .xcaro file. Could you share your .xacro file or .urdf file? Many thanks! My email address is 997427575@qq.com
Asked by Xiaowei on 2018-12-29 01:58:14 UTC