Pick and Place using Python
8,455 views
Skip to first unread message
Patrick Goebel
Jan 19, 2014, 2:47:22 PM1/19/14
to moveit...@googlegroups.com
Hello,
I am using the MoveIt! Python API in ROS Hydro (Debian install) with Ubuntu 12.04. My 6-dof arm is working well in demo mode and now I am trying to get a simple pick and place operation to work, also still in demo mode. My gripper has a single joint with one moving finger and one fixed finger. I am using the following script to test the pick() operation but it is failing with the following messages:
[ INFO] [1390160394.780762801]: Planning attempt 1 of at most 1
[ INFO] [1390160394.781137260]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1390160394.830357392]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0
[ WARN] [1390160394.830569822]: All supplied grasps failed. Retrying last grasp in verbose mode.
[ INFO] [1390160394.830677452]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1390160394.850568729]: IK failed
[ INFO] [1390160394.868562234]: IK failed
[ INFO] [1390160394.883219633]: IK failed
[ INFO] [1390160394.883286820]: Sampler failed to produce a state
[ INFO] [1390160394.883364968]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1390160394.883587365]: Pickup planning completed after 0.049711 seconds
My script tries to make it easy for the planner by initially positioning the gripper in nearly the exactly correct posture for grasping the target object named "part" in the script. (See the RViz screen capture below for where the arm and gripper are moved relative to the target object before the pick operation is run.) So I am not sure why IK is failing. I only vaguely understand the various parts of the grasp message and how to set the various fields properly so no doubt I am missing something essential.
Any hints would be appreciated!
Here now is my test script:
#!/usr/bin/env python
import sys
import rospy
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint
if __name__=='__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
right_gripper = MoveGroupCommander("right_gripper")
rospy.sleep(1)
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
right_arm.set_named_target("resting")
right_arm.go()
right_gripper.set_named_target("open")
right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
p.pose.position.x = 0.42
p.pose.position.y = -0.2
p.pose.position.z = 0.3
scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.205
p.pose.position.y = -0.12
p.pose.position.z = 0.7
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
grasps = []
g = Grasp()
g.id = "test"
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = "base_footprint"
grasp_pose.pose.position.x = 0.148554
grasp_pose.pose.position.y = -0.116075
grasp_pose.pose.position.z = 0.70493
grasp_pose.pose.orientation.x = -0.709103
grasp_pose.pose.orientation.y = 0.0137777
grasp_pose.pose.orientation.z = 0.0164031
grasp_pose.pose.orientation.w = 0.704779
right_arm.set_pose_target(grasp_pose)
right_arm.go()
rospy.sleep(2)
# set the grasp pose
g.grasp_pose = grasp_pose
# define the pre-grasp approach
g.pre_grasp_approach.direction.header.frame_id = "base_footprint"
g.pre_grasp_approach.direction.vector.x = 1.0
g.pre_grasp_approach.direction.vector.y = 0.0
g.pre_grasp_approach.direction.vector.z = 0.0
g.pre_grasp_approach.min_distance = 0.001
g.pre_grasp_approach.desired_distance = 0.1
g.pre_grasp_posture.header.frame_id = "right_gripper_base_link"
g.pre_grasp_posture.joint_names = ["right_gripper_finger_joint"]
pos = JointTrajectoryPoint()
pos.positions.append(0.0)
g.pre_grasp_posture.points.append(pos)
# set the grasp posture
g.grasp_posture.header.frame_id = "right_gripper_base_link"
g.grasp_posture.joint_names = ["right_gripper_finger_joint"]
pos = JointTrajectoryPoint()
pos.positions.append(0.2)
pos.effort.append(0.0)
g.grasp_posture.points.append(pos)
# set the post-grasp retreat
g.post_grasp_retreat.direction.header.frame_id = "base_footprint"
g.post_grasp_retreat.direction.vector.x = 0.0
g.post_grasp_retreat.direction.vector.y = 0.0
g.post_grasp_retreat.direction.vector.z = 1.0
g.post_grasp_retreat.desired_distance = 0.25
g.post_grasp_retreat.min_distance = 0.01
g.allowed_touch_objects = ["table"]
g.max_contact_force = 0
# append the grasp to the list of grasps
grasps.append(g)
rospy.sleep(2)
# pick the object
robot.right_arm.pick("part", grasps)
rospy.spin()
roscpp_shutdown()
I am using the MoveIt! Python API in ROS Hydro (Debian install) with Ubuntu 12.04. My 6-dof arm is working well in demo mode and now I am trying to get a simple pick and place operation to work, also still in demo mode. My gripper has a single joint with one moving finger and one fixed finger. I am using the following script to test the pick() operation but it is failing with the following messages:
[ INFO] [1390160394.780762801]: Planning attempt 1 of at most 1
[ INFO] [1390160394.781137260]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1390160394.830357392]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0
[ WARN] [1390160394.830569822]: All supplied grasps failed. Retrying last grasp in verbose mode.
[ INFO] [1390160394.830677452]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1390160394.850568729]: IK failed
[ INFO] [1390160394.868562234]: IK failed
[ INFO] [1390160394.883219633]: IK failed
[ INFO] [1390160394.883286820]: Sampler failed to produce a state
[ INFO] [1390160394.883364968]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1390160394.883587365]: Pickup planning completed after 0.049711 seconds
My script tries to make it easy for the planner by initially positioning the gripper in nearly the exactly correct posture for grasping the target object named "part" in the script. (See the RViz screen capture below for where the arm and gripper are moved relative to the target object before the pick operation is run.) So I am not sure why IK is failing. I only vaguely understand the various parts of the grasp message and how to set the various fields properly so no doubt I am missing something essential.
Any hints would be appreciated!
Here now is my test script:
#!/usr/bin/env python
import sys
import rospy
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint
if __name__=='__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
right_gripper = MoveGroupCommander("right_gripper")
rospy.sleep(1)
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
right_arm.set_named_target("resting")
right_arm.go()
right_gripper.set_named_target("open")
right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
p.pose.position.x = 0.42
p.pose.position.y = -0.2
p.pose.position.z = 0.3
scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.205
p.pose.position.y = -0.12
p.pose.position.z = 0.7
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
grasps = []
g = Grasp()
g.id = "test"
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = "base_footprint"
grasp_pose.pose.position.x = 0.148554
grasp_pose.pose.position.y = -0.116075
grasp_pose.pose.position.z = 0.70493
grasp_pose.pose.orientation.x = -0.709103
grasp_pose.pose.orientation.y = 0.0137777
grasp_pose.pose.orientation.z = 0.0164031
grasp_pose.pose.orientation.w = 0.704779
right_arm.set_pose_target(grasp_pose)
right_arm.go()
rospy.sleep(2)
# set the grasp pose
g.grasp_pose = grasp_pose
# define the pre-grasp approach
g.pre_grasp_approach.direction.header.frame_id = "base_footprint"
g.pre_grasp_approach.direction.vector.x = 1.0
g.pre_grasp_approach.direction.vector.y = 0.0
g.pre_grasp_approach.direction.vector.z = 0.0
g.pre_grasp_approach.min_distance = 0.001
g.pre_grasp_approach.desired_distance = 0.1
g.pre_grasp_posture.header.frame_id = "right_gripper_base_link"
g.pre_grasp_posture.joint_names = ["right_gripper_finger_joint"]
pos = JointTrajectoryPoint()
pos.positions.append(0.0)
g.pre_grasp_posture.points.append(pos)
# set the grasp posture
g.grasp_posture.header.frame_id = "right_gripper_base_link"
g.grasp_posture.joint_names = ["right_gripper_finger_joint"]
pos = JointTrajectoryPoint()
pos.positions.append(0.2)
pos.effort.append(0.0)
g.grasp_posture.points.append(pos)
# set the post-grasp retreat
g.post_grasp_retreat.direction.header.frame_id = "base_footprint"
g.post_grasp_retreat.direction.vector.x = 0.0
g.post_grasp_retreat.direction.vector.y = 0.0
g.post_grasp_retreat.direction.vector.z = 1.0
g.post_grasp_retreat.desired_distance = 0.25
g.post_grasp_retreat.min_distance = 0.01
g.allowed_touch_objects = ["table"]
g.max_contact_force = 0
# append the grasp to the list of grasps
grasps.append(g)
rospy.sleep(2)
# pick the object
robot.right_arm.pick("part", grasps)
rospy.spin()
roscpp_shutdown()
Sam Pfeiffer
Jan 21, 2014, 8:17:05 AM1/21/14
to moveit...@googlegroups.com
Hello Patrick,
I'm doing grasping (only on demo mode, still fighting some problems talking to real controllers) via Python in this file (with the robot REEM):
What I do different is that I send directly to the pickup action client a message that lets me fill more fields. I noticed that if I give it more replan attemps (10 in this case) it ends being able to grasp. What I must say is that giving only one possible grasp seems to be not too fine for MoveIt!. I've got better results giving it no grasps (so MoveIt! generates a default grasp which sometimes works). Or by using a block grasp generator (adapting it to our robot). We adapted it to be an action server here: https://github.com/bmagyar/block_grasp_generator and it's based on Dave Coleman work: https://github.com/davetcoleman/block_grasp_generator
I hope this helps you a bit!
Patrick Goebel
Jan 21, 2014, 9:37:35 PM1/21/14
to moveit...@googlegroups.com
Many thanks Sam--this looks promising. I'll give it a try over the
next few days and report back my results.
--patrick
--patrick
Martin Günther
Jan 28, 2014, 6:10:43 AM1/28/14
to moveit...@googlegroups.com
Hi Patrick,
we recently got Pick working with our 5DoF arm (whooo!), so I'm
supplying a few of my random notes and speculations inline.
On Sun, 19 Jan 2014 11:47:22 -0800
Patrick Goebel <pat...@pirobot.org> wrote:
> I am using the MoveIt! Python API in ROS Hydro (Debian install) with
> Ubuntu 12.04. My 6-dof arm is working well in demo mode [...]
Same setup I'm using, except I'm using either Gazebo or the real robot,
not the demo mode.
> [ INFO] [1390160394.780762801]: Planning attempt 1 of at most 1
> [ INFO] [1390160394.781137260]: Added plan for pipeline 'pick'. Queue
> is now of size 1
> [ INFO] [1390160394.830357392]: Manipulation plan 0 failed at stage
> 'reachable & valid pose filter' on thread 0
> [ WARN] [1390160394.830569822]: All supplied grasps failed. Retrying
> last grasp in verbose mode.
> [ INFO] [1390160394.830677452]: Re-added last failed plan for
> pipeline 'pick'. Queue is now of size 1
> [ INFO] [1390160394.850568729]: IK failed
> [ INFO] [1390160394.868562234]: IK failed
> [ INFO] [1390160394.883219633]: IK failed
> [ INFO] [1390160394.883286820]: Sampler failed to produce a state
> [ INFO] [1390160394.883364968]: Manipulation plan 0 failed at stage
> 'reachable & valid pose filter' on thread 0
> [ INFO] [1390160394.883587365]: Pickup planning completed after
> 0.049711 seconds
That's the same error I used to get until I fixed it (see below).
> My script tries to make it easy for the planner by initially
> positioning the gripper in nearly the exactly correct posture for
> grasping the target object named "part" in the script. (See the RViz
> screen capture below for where the arm and gripper are moved relative
> to the target object before the pick operation is run.) So I am not
> sure why IK is failing. I only vaguely understand the various parts
> of the grasp message and how to set the various fields properly so no
> doubt I am missing something essential.
>
> Any hints would be appreciated!
>
>
>
>
>
Maybe it's not super helpful that you're placing the gripper
initially at the grasp pose, since it has to go from there to the
pregrasp, then back to the grasp. This first movement is done with
collision checks with "part" enabled, so that may be a problem later.
On the other hand, you can already move there just fine, so there's no
reason it can't also move back from there. Also, this shows that the
grasp is reachable. Maybe you should calculate a pregrasp and move the
gripper there initially, just to make sure it can go there.
> rospy.sleep(2)
>
> # set the grasp pose
> g.grasp_pose = grasp_pose
>
> # define the pre-grasp approach
> g.pre_grasp_approach.direction.header.frame_id = "base_footprint"
This should be your gripper tool frame, since you usually want the
approach to happen along a specific axis of the gripper, not the base.
> g.pre_grasp_approach.direction.vector.x = 1.0
> g.pre_grasp_approach.direction.vector.y = 0.0
> g.pre_grasp_approach.direction.vector.z = 0.0
> g.pre_grasp_approach.min_distance = 0.001
This might be a little low. I guess the min_distance should be at least
big enough that the resulting pose is out of collision with the
"part" (but I might be wrong). For me, 0.05 m worked fine.
> g.pre_grasp_approach.desired_distance = 0.1
>
> g.pre_grasp_posture.header.frame_id = "right_gripper_base_link"
> g.pre_grasp_posture.joint_names = ["right_gripper_finger_joint"]
>
> pos = JointTrajectoryPoint()
> pos.positions.append(0.0)
>
> g.pre_grasp_posture.points.append(pos)
>
> # set the grasp posture
> g.grasp_posture.header.frame_id = "right_gripper_base_link"
> g.grasp_posture.joint_names = ["right_gripper_finger_joint"]
>
> pos = JointTrajectoryPoint()
> pos.positions.append(0.2)
> pos.effort.append(0.0)
>
> g.grasp_posture.points.append(pos)
>
> # set the post-grasp retreat
> g.post_grasp_retreat.direction.header.frame_id = "base_footprint"
This is okay, since you want the retreat to be "up", no matter how the
gripper is oriented.
> g.post_grasp_retreat.direction.vector.x = 0.0
> g.post_grasp_retreat.direction.vector.y = 0.0
> g.post_grasp_retreat.direction.vector.z = 1.0
> g.post_grasp_retreat.desired_distance = 0.25
> g.post_grasp_retreat.min_distance = 0.01
>
> g.allowed_touch_objects = ["table"]
This should definitely include "part", and I'm not sure it should have
"table". The allowed_touch_objects are only taken into account when
going from pre_grasp to grasp, and that's where we want to allow these
collisions.
Actually, adding "part" here made picking work for me; before, I used
to get the same errors you do, probably indicating that the gripper was
in collision with the "part" in the grasp pose.
> [...]
If you want to have a look, our (still very hacky) C++ code is here:
https://github.com/uos/calvin_robot/blob/hydro/calvin_pick_n_place/src/demo2.cpp
It usually picks successfully, but group.pick() never returns. If you
want, you can run it like this:
roslaunch calvin_gazebo calvin_table_world.launch
roslaunch calvin_moveit_config moveit_planning_execution.launch
roslaunch calvin_moveit_config moveit_rviz.launch config:=true
rosrun calvin_pick_n_place demo2
Cheers,
Martin
we recently got Pick working with our 5DoF arm (whooo!), so I'm
supplying a few of my random notes and speculations inline.
On Sun, 19 Jan 2014 11:47:22 -0800
Patrick Goebel <pat...@pirobot.org> wrote:
> I am using the MoveIt! Python API in ROS Hydro (Debian install) with
Same setup I'm using, except I'm using either Gazebo or the real robot,
not the demo mode.
> [ INFO] [1390160394.780762801]: Planning attempt 1 of at most 1
> [ INFO] [1390160394.781137260]: Added plan for pipeline 'pick'. Queue
> is now of size 1
> [ INFO] [1390160394.830357392]: Manipulation plan 0 failed at stage
> 'reachable & valid pose filter' on thread 0
> [ WARN] [1390160394.830569822]: All supplied grasps failed. Retrying
> last grasp in verbose mode.
> [ INFO] [1390160394.830677452]: Re-added last failed plan for
> pipeline 'pick'. Queue is now of size 1
> [ INFO] [1390160394.850568729]: IK failed
> [ INFO] [1390160394.868562234]: IK failed
> [ INFO] [1390160394.883219633]: IK failed
> [ INFO] [1390160394.883286820]: Sampler failed to produce a state
> [ INFO] [1390160394.883364968]: Manipulation plan 0 failed at stage
> 'reachable & valid pose filter' on thread 0
> [ INFO] [1390160394.883587365]: Pickup planning completed after
> 0.049711 seconds
> My script tries to make it easy for the planner by initially
> positioning the gripper in nearly the exactly correct posture for
> grasping the target object named "part" in the script. (See the RViz
> screen capture below for where the arm and gripper are moved relative
> to the target object before the pick operation is run.) So I am not
> sure why IK is failing. I only vaguely understand the various parts
> of the grasp message and how to set the various fields properly so no
> doubt I am missing something essential.
>
> Any hints would be appreciated!
>
>
>
>
>
initially at the grasp pose, since it has to go from there to the
pregrasp, then back to the grasp. This first movement is done with
collision checks with "part" enabled, so that may be a problem later.
On the other hand, you can already move there just fine, so there's no
reason it can't also move back from there. Also, this shows that the
grasp is reachable. Maybe you should calculate a pregrasp and move the
gripper there initially, just to make sure it can go there.
> rospy.sleep(2)
>
> # set the grasp pose
> g.grasp_pose = grasp_pose
>
> # define the pre-grasp approach
> g.pre_grasp_approach.direction.header.frame_id = "base_footprint"
approach to happen along a specific axis of the gripper, not the base.
> g.pre_grasp_approach.direction.vector.x = 1.0
> g.pre_grasp_approach.direction.vector.y = 0.0
> g.pre_grasp_approach.direction.vector.z = 0.0
> g.pre_grasp_approach.min_distance = 0.001
big enough that the resulting pose is out of collision with the
"part" (but I might be wrong). For me, 0.05 m worked fine.
> g.pre_grasp_approach.desired_distance = 0.1
>
> g.pre_grasp_posture.header.frame_id = "right_gripper_base_link"
> g.pre_grasp_posture.joint_names = ["right_gripper_finger_joint"]
>
> pos = JointTrajectoryPoint()
> pos.positions.append(0.0)
>
> g.pre_grasp_posture.points.append(pos)
>
> # set the grasp posture
> g.grasp_posture.header.frame_id = "right_gripper_base_link"
> g.grasp_posture.joint_names = ["right_gripper_finger_joint"]
>
> pos = JointTrajectoryPoint()
> pos.positions.append(0.2)
> pos.effort.append(0.0)
>
> g.grasp_posture.points.append(pos)
>
> # set the post-grasp retreat
> g.post_grasp_retreat.direction.header.frame_id = "base_footprint"
gripper is oriented.
> g.post_grasp_retreat.direction.vector.x = 0.0
> g.post_grasp_retreat.direction.vector.y = 0.0
> g.post_grasp_retreat.direction.vector.z = 1.0
> g.post_grasp_retreat.desired_distance = 0.25
> g.post_grasp_retreat.min_distance = 0.01
>
> g.allowed_touch_objects = ["table"]
"table". The allowed_touch_objects are only taken into account when
going from pre_grasp to grasp, and that's where we want to allow these
collisions.
Actually, adding "part" here made picking work for me; before, I used
to get the same errors you do, probably indicating that the gripper was
in collision with the "part" in the grasp pose.
> [...]
If you want to have a look, our (still very hacky) C++ code is here:
https://github.com/uos/calvin_robot/blob/hydro/calvin_pick_n_place/src/demo2.cpp
It usually picks successfully, but group.pick() never returns. If you
want, you can run it like this:
roslaunch calvin_gazebo calvin_table_world.launch
roslaunch calvin_moveit_config moveit_planning_execution.launch
roslaunch calvin_moveit_config moveit_rviz.launch config:=true
rosrun calvin_pick_n_place demo2
Cheers,
Martin
Patrick Goebel
Jan 28, 2014, 8:24:49 PM1/28/14
to moveit...@googlegroups.com
Thanks Martin,
This really helps my understanding of the process. I still haven't
gotten the grasping to work but now I am starting to wonder if it's
because I'm not giving the pick() command more than one grasp. Seems
everyone here (e.g. Sam, Fergs and you) are using a grasp generator to
submit multiple gasps to the pick() command. So I'll focus on that for
a bit...
--patrick
This really helps my understanding of the process. I still haven't
gotten the grasping to work but now I am starting to wonder if it's
because I'm not giving the pick() command more than one grasp. Seems
everyone here (e.g. Sam, Fergs and you) are using a grasp generator to
submit multiple gasps to the pick() command. So I'll focus on that for
a bit...
--patrick
Patrick Goebel
Jan 29, 2014, 10:05:28 AM1/29/14
to moveit...@googlegroups.com
So when a pick() attempt fails with the following message:
[ INFO] [1391005555.272147083]: Manipulation plan 0 failed at stage
failure to compute a grasp pose once the gripper is moved into
position? Or is it an IK failure when trying to move the gripper to the
correct location before grasping is even attempted?
The other INFO messages just before this one are:
[ INFO] [1391005555.219043035]: Re-added last failed plan for pipeline
[ INFO] [1391005555.253503013]: IK failed
[ INFO] [1391005555.271746294]: IK failed
[ INFO] [1391005555.271978401]: Sampler failed to produce a state
[ INFO] [1391005555.272147083]: Manipulation plan 0 failed at stage
patrick
[ INFO] [1391005555.272147083]: Manipulation plan 0 failed at stage
'reachable & valid pose filter' on thread 0
then exactly where in the planning is it actually failing? Is this a
failure to compute a grasp pose once the gripper is moved into
position? Or is it an IK failure when trying to move the gripper to the
correct location before grasping is even attempted?
The other INFO messages just before this one are:
[ INFO] [1391005555.219043035]: Re-added last failed plan for pipeline
'pick'. Queue is now of size 1
[ INFO] [1391005555.233861080]: IK failed
[ INFO] [1391005555.253503013]: IK failed
[ INFO] [1391005555.271746294]: IK failed
[ INFO] [1391005555.271978401]: Sampler failed to produce a state
[ INFO] [1391005555.272147083]: Manipulation plan 0 failed at stage
'reachable & valid pose filter' on thread 0
Thanks!
patrick
G.A. vd. Hoorn - 3ME
Jan 29, 2014, 10:08:31 AM1/29/14
to Patrick Goebel, moveit...@googlegroups.com
Patrick Goebel wrote:
> So when a pick() attempt fails with the following message:
>
> [ INFO] [1391005555.272147083]: Manipulation plan 0 failed at stage
> 'reachable & valid pose filter' on thread 0
>
> then exactly where in the planning is it actually failing? Is this a
> failure to compute a grasp pose once the gripper is moved into
> position? Or is it an IK failure when trying to move the gripper to the
> correct location before grasping is even attempted?
>
> The other INFO messages just before this one are:
>
> [ INFO] [1391005555.219043035]: Re-added last failed plan for pipeline
> 'pick'. Queue is now of size 1
> [ INFO] [1391005555.233861080]: IK failed
> [ INFO] [1391005555.253503013]: IK failed
> [ INFO] [1391005555.271746294]: IK failed
> [ INFO] [1391005555.271978401]: Sampler failed to produce a state
> [ INFO] [1391005555.272147083]: Manipulation plan 0 failed at stage
> 'reachable & valid pose filter' on thread 0
Have you tried turning up the logging level? Ie to DEBUG?
> So when a pick() attempt fails with the following message:
>
> [ INFO] [1391005555.272147083]: Manipulation plan 0 failed at stage
> 'reachable & valid pose filter' on thread 0
>
> then exactly where in the planning is it actually failing? Is this a
> failure to compute a grasp pose once the gripper is moved into
> position? Or is it an IK failure when trying to move the gripper to the
> correct location before grasping is even attempted?
>
> The other INFO messages just before this one are:
>
> [ INFO] [1391005555.219043035]: Re-added last failed plan for pipeline
> 'pick'. Queue is now of size 1
> [ INFO] [1391005555.233861080]: IK failed
> [ INFO] [1391005555.253503013]: IK failed
> [ INFO] [1391005555.271746294]: IK failed
> [ INFO] [1391005555.271978401]: Sampler failed to produce a state
> [ INFO] [1391005555.272147083]: Manipulation plan 0 failed at stage
> 'reachable & valid pose filter' on thread 0
Also, IIRC MoveIt itself has a debug option that can be enabled, either
through one of the launch files, or on the command line.
Gijs
> mailboxcluster.tudelft.net (131.180.6.102) with Microsoft SMTP Server id
> 14.3.174.1; Wed, 29 Jan 2014 16:05:45 +0100
> Received: from localhost (localhost [127.0.0.1]) by amavis (Postfix)
> with
> ESMTP id E2DA52B8045 for <gavand...@mailboxcluster.tudelft.net>;
> Wed, 29
> Jan 2014 16:05:44 +0100 (CET)
> X-Virus-Scanned: amavisd-new at tudelft.nl
> X-Spam-Flag: NO
> X-Spam-Score: -1.601
> X-Spam-Level:
> X-Spam-Status: No, score=-1.601 tagged_above=-99 required=5
> tests=[BAYES_00=-1.9, RCVD_IN_DNSWL_LOW=-0.7, RCVD_IN_SORBS_SPAM=1,
> SPF_PASS=-0.001] autolearn=no
> Received: from mailservice.tudelft.nl ([130.161.131.73]) by localhost
> (tudelft.nl [127.0.0.1]) (amavisd-new, port 10026) with ESMTP id
> KYK0J7kCmwKA
> for <gavand...@mailboxcluster.tudelft.net>; Wed, 29 Jan 2014
> 16:05:43
> +0100 (CET)
> Received: from mail-qc0-f185.google.com (mail-qc0-f185.google.com
> [209.85.216.185]) by mx2.tudelft.nl (Postfix) with ESMTP id
> 3E5632B8024 for
> <g.a.van...@tudelft.nl>; Wed, 29 Jan 2014 16:05:43 +0100 (CET)
> Received: by mail-qc0-f185.google.com with SMTP id
> m20sf472802qcx.2 for
> <g.a.van...@tudelft.nl>; Wed, 29 Jan 2014 07:05:42 -0800 (PST)
> DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed;
> d=googlegroups.com; s=20120806;
> h=message-id:date:from:user-agent:mime-version:to:subject:references
> :in-reply-to:x-original-sender:x-original-authentication-results
> :precedence:mailing-list:list-id:list-post:list-help:list-archive
> :sender:list-unsubscribe:content-type:content-transfer-encoding;
> bh=NoaCF0WSESmChgDfbKM5vTjo8urk4/SRvHii97Kv+BU=;
> b=IHdqXaBjKMQz6olZ1YCsbodBM5eD3hp9E8KHp6Pj4spWQcwcxU6AtXvUbhdWi7mQol
>
> Wj9KegOptqkVRpF6vklwPQNyGCjKkLkzIiCZrjvAcW3N44xFv6Hdk9lzdxxuVdvPQTs3
>
> 3Vl3yZKi4rj87z2O5hJ7GyC/575DA4WHxGE3ltpHDuq6+f4NAxpTUe4nswilXN9J+Hem
>
> 92uggqjsuJmmdlbW0xFZ38WdTDARdvW8iRe+wtJZsWZNHhxxcFXnq+9lZBJKpkbKY1q1
>
> H2LOvhnvObLje3DbfIRn4jYssoLpW7Rf8acgUO09ud7/RWhYkXepFT6X9ZdByLufiLcN
> 3zTw==
> X-Received: by 10.140.47.43 with SMTP id l40mr57207qga.11.1391007942772;
> Wed, 29 Jan 2014 07:05:42 -0800 (PST)
> X-BeenThere: moveit...@googlegroups.com
> Received: by 10.140.82.49 with SMTP id g46ls150961qgd.88.gmail; Wed, 29 Jan
> 2014 07:05:42 -0800 (PST)
> X-Received: by 10.52.107.162 with SMTP id hd2mr2644289vdb.7.1391007942345;
> Wed, 29 Jan 2014 07:05:42 -0800 (PST)
> Received: from smtp.stanford.edu (smtp1.Stanford.EDU. [171.67.219.81])
> by gmr-mx.google.com with ESMTPS id
> g1si848586pbw.2.2014.01.29.07.05.41 for
> <moveit...@googlegroups.com> (version=TLSv1.2
> cipher=ECDHE-RSA-AES128-GCM-SHA256 bits=128/128); Wed, 29 Jan 2014
> 07:05:42 -0800 (PST)
> Received-SPF: neutral (google.com: 171.67.219.81 is neither permitted
> nor denied by domain of pat...@pirobot.org) client-ip=171.67.219.81;
> Received: from smtp.stanford.edu (localhost [127.0.0.1]) by localhost
> (Postfix) with SMTP id 8BEF023141 for
> <moveit...@googlegroups.com>; Wed,
> 29 Jan 2014 07:05:41 -0800 (PST)
> Received: from [10.0.0.2] (c-98-207-152-210.hsd1.ca.comcast.net
> [98.207.152.210]) (using TLSv1 with cipher ECDHE-RSA-AES256-SHA (256/256
> bits)) (No client certificate requested) (Authenticated sender:
> pgoebel) by
> smtp.stanford.edu (Postfix) with ESMTPSA id 9A56B23138 for
> <moveit...@googlegroups.com>; Wed, 29 Jan 2014 07:05:28 -0800 (PST)
> Message-ID: <52E918B8...@pirobot.org>
> Date: Wed, 29 Jan 2014 07:05:28 -0800
> From: Patrick Goebel <pat...@pirobot.org>
> User-Agent: Mozilla/5.0 (X11; Linux i686; rv:24.0) Gecko/20100101
> Thunderbird/24.2.0
> To: <moveit...@googlegroups.com>
> Subject: Re: Pick and Place using Python
> References: <52DC2BCA...@pirobot.org>
> <20140128121...@gemenon.kbs> <52E85861...@pirobot.org>
> In-Reply-To: <52E85861...@pirobot.org>
> X-Original-Sender: pat...@pirobot.org
> X-Original-Authentication-Results: gmr-mx.google.com; spf=neutral
> (google.com: 171.67.219.81 is neither permitted nor denied by domain of
> pat...@pirobot.org) smtp.mail=pat...@pirobot.org
> Precedence: list
> Mailing-list: list moveit...@googlegroups.com; contact
> moveit-us...@googlegroups.com
> List-ID: <moveit-users.googlegroups.com>
> X-Google-Group-Id: 351344880618
> List-Post: <http://groups.google.com/group/moveit-users/post>,
> <mailto:moveit...@googlegroups.com>
> List-Help: <http://groups.google.com/support/>,
> <mailto:moveit-u...@googlegroups.com>
> List-Archive: <http://groups.google.com/group/moveit-users>
> Sender: <moveit...@googlegroups.com>
> List-Unsubscribe:
> <http://groups.google.com/group/moveit-users/subscribe>,
> <mailto:googlegroups-manage+35...@googlegroups.com>
> Content-Type: text/plain; charset="ISO-8859-1"; format=flowed
> Content-Transfer-Encoding: 8bit
> Return-Path:
> moveit-users+bncBCDZHZ7...@googlegroups.com
> X-MS-Exchange-Organization-AuthSource: srv352.tudelft.net
> X-MS-Exchange-Organization-AuthAs: Anonymous
> MIME-Version: 1.0
Patrick Goebel
Jan 29, 2014, 12:41:59 PM1/29/14
to moveit...@googlegroups.com
Thanks Gijs--good idea. I just tried that now and I only get one more
message in the output:
[ WARN] [1391008505.326694025]: All supplied grasps failed. Retrying
general so I'm still not sure exactly where the failure is occurring.
--patrick
message in the output:
[ WARN] [1391008505.326694025]: All supplied grasps failed. Retrying
last grasp in verbose mode.
which sounds like a problem in the final grasping but it is still fairly
general so I'm still not sure exactly where the failure is occurring.
--patrick
Sam Pfeiffer
Jan 29, 2014, 1:55:49 PM1/29/14
to Patrick Goebel, moveit...@googlegroups.com
It t your grasps failed. All the ones you specified. Which was one!
You can even try giving many times that grasp and I got it to work sometime like that at the start.
Also not specifying grasps it defaults to one that sometimes works.
Steffen P
Jan 30, 2014, 4:54:41 AM1/30/14
to moveit...@googlegroups.com, Patrick Goebel
After going throug the Pickup Manipulation Pipeline code I have found the following:
The MoveIt! Pickup Manipulation Pipeline calculates a complete pick up trajectory
ready to be executed based on the following information:
• The Planning Scene as provided by the Planning Scene Monitor.
• The id of the object to pick up
• The grasps to try for this object which have been generated by a grasp
planner.
A grasp contains the following information:
• The pose of the end effector for the grasp.
• The estimated probability of success for this grasp.
• The approach motion defined as a direction vector, a minimum approach
distance and a desired approach distance.
• The retreat motion defined as a direction vector, a minimum retreat distance
and a desired retreat distance.
• The maximum contact force to use while grasping.
• An optional list of obstacles (allowed touch objects) that have been seg-
mented and added to the environment as collision objects and that can be
touched, pushed or moved in the course of grasping.
The grasps are first ordered by their quality as estimated by the grasp planner
and then fed into the Pickup Manipulation Pipeline.
The Pickup Manipulation Pipeline consists of three stages which are executed in
the order of appearance. During these stages different trajectories are added to the
final pickup plan. Only if a grasp successfully passes all stages the resulting plan
can be executed. The diagram gives an overview of the complete pickup procedure.
Reachable and Valid Pose Filter The Reachable and Valid Pose Filter only ac-
cepts grasp poses which are reachable. This means a IK solution needs to
be found.
Approach and Translate Stage During this stage collisions between the gripper
and the object to grasp are disabled. If allowed touch objects contains
objects, collisions between these objects and the gripper are disabled as
well.
• A Cartesian path is calculated starting from the grasp posture into nega-
tive approach direction until the desired approach distance is reached or no
IK solution was found for a trajectory point. If the path is longer than
the minimum approach distance, the approach planning was successful and
the generated trajectory is added to the plan in the reverse order as ap-
proach trajectory. The first point of this reversed trajectory is saved as the
pre-grasp posture.
• The trajectory to close the gripper, the close gripper trajectory is added to
the plan.
• The collision object of the object to grasp (determined by the passed object
id) is attached to the gripper. (This is used for collision detection while
motion planning after the pickup task has finished.)
• Then a Cartesian path from the grasp posture into the retreat direction is
generated respectively. This time using the minimum retreat distance and
the desired retreat distance as specified in the grasp message. The resulting
trajectory is added to the plan as retreat trajectory.
Plan Stage In the Plan Stage the pre approach trajectory from the starting arm
posture to the pre-grasp posture and an open gripper trajectory are gener-
ated and added to the plan.
The generated plan containing all needed trajectories can now be executed.
The debug Markers to show which grasps were tested never worked for me using Groovy.
They were always displayed at the initial arm position as can be seen in the first image. (Red Gripper next to platform)
Because of the very low success rate of the Pickup Pipeline I have implemented my own pick function.
(https://github.com/fivef/kate_apps/tree/master/pick_and_place but the code is everything but nice)
First the success rate was similar to the Pickup Pipeline's but then I have increased the allowed planning time (move_group->setPlanningTime()) and
the results were much better:
I generated 105 grasp poses in front of the robot and let the robot execute grasps at these poses.
If the whole grasping process was executed successfully the pose is marked green, else red.
The first image shows the results of the Pickup Pipeline, the second image shows the results using my own implementation.
The drawback of my own implementation is that it is much slower.
Another hint for the planning time being the problem is that if the gripper is already near to the target grasp pose the success rate of the Pickup Pipeline increases.
So my plan is to test the Pickup Pipeline in Hydro to check if it has improved.
As far as I know the move_group->pick() function doesn't care about the planning time set with setPlanningTime().
I'll try to increase the planning time in the Pickup Pipeline code.
Additionally I am going to try IKFast with our Jaco arm.
Steffen P
Jan 30, 2014, 5:07:40 AM1/30/14
to moveit...@googlegroups.com
Hey Martin,
see here how to make pick return:
Martin Günther
Jan 30, 2014, 6:04:01 AM1/30/14
to moveit...@googlegroups.com
Hi Patrick,
On Wed, 29 Jan 2014 09:41:59 -0800
Patrick Goebel <pat...@pirobot.org> wrote:
> Thanks Gijs--good idea. I just tried that now and I only get one
> more message in the output:
>
> [ WARN] [1391008505.326694025]: All supplied grasps failed. Retrying
> last grasp in verbose mode.
>
> which sounds like a problem in the final grasping but it is still
> fairly general so I'm still not sure exactly where the failure is
> occurring.
Actually, the error message you got earlier is a bit more specific:
> [ INFO] [1391005555.272147083]: Manipulation plan 0 failed at stage
> 'reachable & valid pose filter' on thread 0
The pick code is here:
https://github.com/ros-planning/moveit_ros/blob/hydro-devel/manipulation/pick_place/src/pick.cpp
The first manipulation stage is this one:
https://github.com/ros-planning/moveit_ros/blob/hydro-devel/manipulation/pick_place/src/reachable_valid_pose_filter.cpp
... and that fails for all grasps that you supplied (one). I can't
claim to understand all that's going on, but I guess it checks that it
can actually move the end effector to the pose that you you specify in
your grasp message (`grasp.grasp_pose`). This fails. There can be a
number of reasons for that:
1. The pose isn't actually reachable by your arm, so there's no IK
solution at all.
2. The pose is reachable, but there's a collision there, probably with
the collision object "part". I guess that can also cause "no IK
solution" errors, since there's no *collision free* IK solution.
What I'd do is:
a) Remove the collision object, plan a path to your `grasp_pose`. If
that works, you can rule out (1).
b) Add the collision object back, plan a path again. If that fails,
it's (2). For me, what worked was adding the collision object to
`grasp.allowed_touch_objects`. This is probably a good idea anyway.
The weird thing is that from what I read in your code, you already did
"b)" and it worked.
I think you can ignore the later stages of the grasp message for now
and focus on getting the `grasp_pose` right.
Oh, and you should probably also remove the "table" for now to
eliminate collisions with that. The `setSupportSurface("table")` isn't
strictly required. AFAIK, all it does is ignore collisions with the
"part" object once it's attached to your gripper, but those collisions
can only happen *after* it's attached.
One more thing: I'd call the `pick()` function from a state where your
gripper is far away from everything so there are no collisions in the
start state. Don't try to make it easy by already moving the gripper
to the grasp pose.
@Ioan, Sachin: Please correct me where I'm wrong. Like everybody here,
I'm still in the process of figuring out all of this. :)
Cheers,
Martin
On Wed, 29 Jan 2014 09:41:59 -0800
Patrick Goebel <pat...@pirobot.org> wrote:
> Thanks Gijs--good idea. I just tried that now and I only get one
> more message in the output:
>
> [ WARN] [1391008505.326694025]: All supplied grasps failed. Retrying
> last grasp in verbose mode.
>
> which sounds like a problem in the final grasping but it is still
> fairly general so I'm still not sure exactly where the failure is
> occurring.
> [ INFO] [1391005555.272147083]: Manipulation plan 0 failed at stage
> 'reachable & valid pose filter' on thread 0
https://github.com/ros-planning/moveit_ros/blob/hydro-devel/manipulation/pick_place/src/pick.cpp
The first manipulation stage is this one:
https://github.com/ros-planning/moveit_ros/blob/hydro-devel/manipulation/pick_place/src/reachable_valid_pose_filter.cpp
... and that fails for all grasps that you supplied (one). I can't
claim to understand all that's going on, but I guess it checks that it
can actually move the end effector to the pose that you you specify in
your grasp message (`grasp.grasp_pose`). This fails. There can be a
number of reasons for that:
1. The pose isn't actually reachable by your arm, so there's no IK
solution at all.
2. The pose is reachable, but there's a collision there, probably with
the collision object "part". I guess that can also cause "no IK
solution" errors, since there's no *collision free* IK solution.
What I'd do is:
a) Remove the collision object, plan a path to your `grasp_pose`. If
that works, you can rule out (1).
b) Add the collision object back, plan a path again. If that fails,
it's (2). For me, what worked was adding the collision object to
`grasp.allowed_touch_objects`. This is probably a good idea anyway.
The weird thing is that from what I read in your code, you already did
"b)" and it worked.
I think you can ignore the later stages of the grasp message for now
and focus on getting the `grasp_pose` right.
Oh, and you should probably also remove the "table" for now to
eliminate collisions with that. The `setSupportSurface("table")` isn't
strictly required. AFAIK, all it does is ignore collisions with the
"part" object once it's attached to your gripper, but those collisions
can only happen *after* it's attached.
One more thing: I'd call the `pick()` function from a state where your
gripper is far away from everything so there are no collisions in the
start state. Don't try to make it easy by already moving the gripper
to the grasp pose.
@Ioan, Sachin: Please correct me where I'm wrong. Like everybody here,
I'm still in the process of figuring out all of this. :)
Cheers,
Martin
Sam Pfeiffer
Jan 30, 2014, 7:32:19 AM1/30/14
to Martin Günther, moveit...@googlegroups.com
I love you guys for this very detailed description, thank you so much!
Patrick Goebel
Jan 30, 2014, 2:32:58 PM1/30/14
to Steffen P, moveit-users
Hi Steffen,
This is an awesome summary! (I've printed it out and stapled it to my forehead.) Thanks to this and Martin and Gijs comments, I have managed partial success but as you will see from my video below, the result is quite odd.
The key factors that got me to this (weird) solution are:
* placing the pick() operation in a while loop until it succeeds
* keeping the minimum approach distance fairly small (e.g. 0.05)
* setting the start pose of the arm further back from the target part
* including the target "part" in g.allowed_touch_objects
Some other observations:
* If I use only a single grasp pose, the number of tries for the pick to succeed is wildly variable. Sometimes it gets it after one or two tries, other times it can take over 100
* it doesn't seem to matter if the table is included in the scene or not
* I can get a successful grasp on the first try if I generate a collection of grasp poses with varying pitch and yaw. I stole the make_grasps function (line 659) from Fergs' chess player package.
Here is a link to a video of what is happening when I get a successful "grasp". You can even see the gripper closing even though the part is not between the fingers:
http://www.youtube.com/watch?v=JBwrjvEuJqk&feature=youtu.be
Any idea why this behavior is happening?
--patrick
This is an awesome summary! (I've printed it out and stapled it to my forehead.) Thanks to this and Martin and Gijs comments, I have managed partial success but as you will see from my video below, the result is quite odd.
The key factors that got me to this (weird) solution are:
* placing the pick() operation in a while loop until it succeeds
* keeping the minimum approach distance fairly small (e.g. 0.05)
* setting the start pose of the arm further back from the target part
* including the target "part" in g.allowed_touch_objects
Some other observations:
* If I use only a single grasp pose, the number of tries for the pick to succeed is wildly variable. Sometimes it gets it after one or two tries, other times it can take over 100
* it doesn't seem to matter if the table is included in the scene or not
* I can get a successful grasp on the first try if I generate a collection of grasp poses with varying pitch and yaw. I stole the make_grasps function (line 659) from Fergs' chess player package.
Here is a link to a video of what is happening when I get a successful "grasp". You can even see the gripper closing even though the part is not between the fingers:
http://www.youtube.com/watch?v=JBwrjvEuJqk&feature=youtu.be
Any idea why this behavior is happening?
--patrick
Patrick Goebel
Jan 30, 2014, 6:28:41 PM1/30/14
to moveit-users
Just in case it isn't clear what is happening in the video, these
are the steps:
* The target is added to the scene. The target is the green "plank" that is 20cm tall, 7cm deep and 1cm wide so the gripper (3.5 cm open) should be able to grasp it edge on.
* The arm is moved part way to the target.
* Several grasp poses are tried and fail (the terminal window to the left shows the failure messages in orange)
* Eventually one of the grasp poses causes the gripper to point nearly straight down and apparently this is considered a successful pose because the gripper closes, the plank turns purple, and there is a message in the demo.launch terminal that says the target has been attached to the gripper link.
* The red, green and blue lines pointing out of the gripper are the x, y and z axes attached to the gripper frame. The gripper frame is an extra frame attached to the gripper base but with the origin in between the gripper fingers.
I have no idea why the gripper would be oriented parallel to the target object the way it is and why this pose would allow the object to attach to the gripper.
Here also is the test script including Fergs' grasp generator.
* The target is added to the scene. The target is the green "plank" that is 20cm tall, 7cm deep and 1cm wide so the gripper (3.5 cm open) should be able to grasp it edge on.
* The arm is moved part way to the target.
* Several grasp poses are tried and fail (the terminal window to the left shows the failure messages in orange)
* Eventually one of the grasp poses causes the gripper to point nearly straight down and apparently this is considered a successful pose because the gripper closes, the plank turns purple, and there is a message in the demo.launch terminal that says the target has been attached to the gripper link.
* The red, green and blue lines pointing out of the gripper are the x, y and z axes attached to the gripper frame. The gripper frame is an extra frame attached to the gripper base but with the origin in between the gripper fingers.
I have no idea why the gripper would be oriented parallel to the target object the way it is and why this pose would allow the object to attach to the gripper.
Here also is the test script including Fergs' grasp generator.
#!/usr/bin/env python
import sys
import rospy
import copy, math
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation,
PlaceLocation, MoveItErrorCodes
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from tf.transformations import euler_from_quaternion, quaternion_from_euler
import random
ROBOT_NAME = "pi_robot"
if ROBOT_NAME == "pi_robot":
GROUP_NAME_ARM = 'right_arm'
GROUP_NAME_GRIPPER = 'right_gripper'
GRIPPER_FRAME = 'right_gripper_link'
FIXED_FRAME = 'base_footprint'
GRIPPER_CLOSED = 0.3
GRIPPER_OPEN = 0.0
GRIPPER_JOINT_NAMES = ['right_gripper_finger_joint']
GRIPPER_EFFORT = [1.0]
class TestPick():
def __init__(self):
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from tf.transformations import euler_from_quaternion, quaternion_from_euler
import random
ROBOT_NAME = "pi_robot"
if ROBOT_NAME == "pi_robot":
GROUP_NAME_ARM = 'right_arm'
GROUP_NAME_GRIPPER = 'right_gripper'
GRIPPER_FRAME = 'right_gripper_link'
FIXED_FRAME = 'base_footprint'
GRIPPER_CLOSED = 0.3
GRIPPER_OPEN = 0.0
GRIPPER_JOINT_NAMES = ['right_gripper_finger_joint']
GRIPPER_EFFORT = [1.0]
class TestPick():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
scene.remove_attached_object(GRIPPER_FRAME, "part")
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
scene.remove_attached_object(GRIPPER_FRAME, "part")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
right_arm.set_named_target("resting")
right_arm.go()
right_gripper.set_named_target("open")
right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
#p.pose.position.x = 0.42
#p.pose.position.y = -0.2
#p.pose.position.z = 0.3
#scene.add_box("table", p, (0.5, 1.5, 0.6))
#p.pose.position.y = -0.2
#p.pose.position.z = 0.3
#scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.15
p.pose.position.y = -0.12
p.pose.position.z = 0.7
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
start_pose = PoseStamped()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = 0.0130178
start_pose.pose.position.y = -0.125155
start_pose.pose.position.z = 0.597653
start_pose.pose.orientation.x = 0.0
start_pose.pose.orientation.y = 0.388109
start_pose.pose.orientation.z = 0.0
start_pose.pose.orientation.w = 0.921613
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(2)
# generate a list of grasps
grasps = self.make_grasps(start_pose)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = robot.right_arm.pick("part", grasps)
n_attempts += 1
print "Attempts: ", n_attempts
rospy.sleep(0.2)
rospy.spin()
roscpp_shutdown()
# Get the gripper posture as a JointTrajectory
def make_gripper_posture(self, pose):
t = JointTrajectory()
t.joint_names = GRIPPER_JOINT_NAMES
tp = JointTrajectoryPoint()
tp.positions = [pose for j in t.joint_names]
tp.effort = GRIPPER_EFFORT
t.points.append(tp)
return t
def make_gripper_translation(self, min_dist, desired, axis=1.0):
g = GripperTranslation()
g.direction.vector.x = axis
g.direction.header.frame_id = GRIPPER_FRAME
g.min_distance = min_dist
g.desired_distance = desired
return g
def make_grasps(self, pose_stamped, mega_angle=False):
# setup defaults for the grasp
g = Grasp()
g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
g.pre_grasp_approach = self.make_gripper_translation(0.05, 0.1)
g.post_grasp_retreat = self.make_gripper_translation(0.05, 0.1, -1.0)
g.grasp_pose = pose_stamped
pitch_vals = [0, 0.2, -0.2, 0.4, -0.4]
#pitch_vals = [0]
yaw_vals = [-0.2, -0.1, 0, 0.1, 0.2]
#yaw_vals = [0]
if mega_angle:
pitch_vals += [0.78, -0.78, 0.3, -0.3, 0.5, -0.5, 0.6, -0.6]
# generate list of grasps
grasps = []
#for y in [-1.57, -0.78, 0, 0.78, 1.57]:
for y in yaw_vals:
for p in pitch_vals:
q = quaternion_from_euler(0, 1.57-p, y)
g.grasp_pose.pose.orientation.x = q[0]
g.grasp_pose.pose.orientation.y = q[1]
g.grasp_pose.pose.orientation.z = q[2]
g.grasp_pose.pose.orientation.w = q[3]
g.id = str(len(grasps))
g.allowed_touch_objects = ["part"]
g.max_contact_force = 0
#g.grasp_quality = 1.0 - abs(p/2.0)
grasps.append(copy.deepcopy(g))
return grasps
if __name__=='__main__':
TestPick()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = 0.0130178
start_pose.pose.position.y = -0.125155
start_pose.pose.position.z = 0.597653
start_pose.pose.orientation.x = 0.0
start_pose.pose.orientation.y = 0.388109
start_pose.pose.orientation.z = 0.0
start_pose.pose.orientation.w = 0.921613
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(2)
# generate a list of grasps
grasps = self.make_grasps(start_pose)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = robot.right_arm.pick("part", grasps)
n_attempts += 1
print "Attempts: ", n_attempts
rospy.sleep(0.2)
rospy.spin()
roscpp_shutdown()
# Get the gripper posture as a JointTrajectory
def make_gripper_posture(self, pose):
t = JointTrajectory()
t.joint_names = GRIPPER_JOINT_NAMES
tp = JointTrajectoryPoint()
tp.positions = [pose for j in t.joint_names]
tp.effort = GRIPPER_EFFORT
t.points.append(tp)
return t
def make_gripper_translation(self, min_dist, desired, axis=1.0):
g = GripperTranslation()
g.direction.vector.x = axis
g.direction.header.frame_id = GRIPPER_FRAME
g.min_distance = min_dist
g.desired_distance = desired
return g
def make_grasps(self, pose_stamped, mega_angle=False):
# setup defaults for the grasp
g = Grasp()
g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
g.pre_grasp_approach = self.make_gripper_translation(0.05, 0.1)
g.post_grasp_retreat = self.make_gripper_translation(0.05, 0.1, -1.0)
g.grasp_pose = pose_stamped
pitch_vals = [0, 0.2, -0.2, 0.4, -0.4]
#pitch_vals = [0]
yaw_vals = [-0.2, -0.1, 0, 0.1, 0.2]
#yaw_vals = [0]
if mega_angle:
pitch_vals += [0.78, -0.78, 0.3, -0.3, 0.5, -0.5, 0.6, -0.6]
# generate list of grasps
grasps = []
#for y in [-1.57, -0.78, 0, 0.78, 1.57]:
for y in yaw_vals:
for p in pitch_vals:
q = quaternion_from_euler(0, 1.57-p, y)
g.grasp_pose.pose.orientation.x = q[0]
g.grasp_pose.pose.orientation.y = q[1]
g.grasp_pose.pose.orientation.z = q[2]
g.grasp_pose.pose.orientation.w = q[3]
g.id = str(len(grasps))
g.allowed_touch_objects = ["part"]
g.max_contact_force = 0
#g.grasp_quality = 1.0 - abs(p/2.0)
grasps.append(copy.deepcopy(g))
return grasps
if __name__=='__main__':
TestPick()
Martin Günther
Jan 31, 2014, 5:45:42 AM1/31/14
to moveit...@googlegroups.com
On Thu, 30 Jan 2014 15:28:41 -0800
Patrick Goebel <pat...@pirobot.org> wrote:
> I have no idea why the gripper would be oriented parallel to the
> target object the way it is and why this pose would allow the object
> to attach to the gripper.
Hi Patrick,
Patrick Goebel <pat...@pirobot.org> wrote:
> I have no idea why the gripper would be oriented parallel to the
> target object the way it is and why this pose would allow the object
> to attach to the gripper.
I guess the grasps you're generating cause this. Basically, the
`grasp_pose`s you put in are orientations for the EEF where the object
can be grasped. It's the caller's responsibility to make sure that the
gripper is actually positioned correctly to grasp the object in those
poses; MoveIt doesn't double check this. All MoveIt does is check
whether it can find an IK solution and a plan to one of those poses,
and if yes, it moves there, closes the gripper and attaches the object.
What I would do is publish the grasp_poses you're generating on some
topic, visualize that in RViz and then manually move the arm in RViz
until the EEF frame matches up with those poses. That way, you can see
what's going on.
Cheers,
Martin
Patrick Goebel
Jan 31, 2014, 9:00:59 AM1/31/14
to moveit...@googlegroups.com
Thanks Martin--that was exactly the last piece missing in my
understanding. For some reason I thought that MoveIt was doing some
magic to check and finish the grasp once we got it close. I'll
visualize the grasp poses as you suggest and that should get me on track.
--patrick
understanding. For some reason I thought that MoveIt was doing some
magic to check and finish the grasp once we got it close. I'll
visualize the grasp poses as you suggest and that should get me on track.
--patrick
Patrick Goebel
Jan 31, 2014, 2:47:24 PM1/31/14
to moveit...@googlegroups.com
OK, don't laugh but I finally figured out the last piece of my lack
of understanding and perhaps it will help someone else.
After carefully reviewing Steffen's summary below, it finally occurred to me that the grasp poses I was generating had to be centered on the part I wanted to pick up. I kept thinking that the pick() function just had some magic capability to bring the gripper to the part since after all, I give the name of the part to pick() function. Now that I am generating the potential grasp poses at the position of the target part, the whole pipeline works beautifully.
After carefully reviewing Steffen's summary below, it finally occurred to me that the grasp poses I was generating had to be centered on the part I wanted to pick up. I kept thinking that the pick() function just had some magic capability to bring the gripper to the part since after all, I give the name of the part to pick() function. Now that I am generating the potential grasp poses at the position of the target part, the whole pipeline works beautifully.
--patrick
On 01/30/2014 01:54 AM, Steffen P
wrote:
Patrick Goebel
Mar 13, 2014, 1:45:06 PM3/13/14
to moveit...@googlegroups.com
OK, I'm baffled. I had Python pick and place working perfectly a
month ago--I even made a video to
prove it. Now, a month later I am running the exact same script and
it fails on the pick operation every time. I am using the latest
Debian install of MoveIt under Ubuntu 12.04. Is anyone still doing
successful pick and place with this latest installation?
--patrick
--patrick
Patrick Goebel
Mar 14, 2014, 9:34:50 AM3/14/14
to moveit...@googlegroups.com
False alarm. I found a small change I had made in my URDF and this
was messing up the transform I was assuming between my gripper
planning frame and the point of attachment to the arm. Sorry for
the wasted bandwidth.
--patrick
--patrick
Dina Youakim
Feb 19, 2015, 9:50:40 AM2/19/15
to moveit...@googlegroups.com
Hello,
First thanks a lot for this thread , it helped me a lot to understand the pick operation and I managed to make it work, but something weird happened today as I updated ros/moveit to fix the bug reported here https://github.com/ros-planning/moveit_commander/issues/22#issuecomment-74931884
Since then nothing is working and I am getting the following log:
[ INFO] [1424357416.156909040]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1424357416.206779560]: IK failed
[ INFO] [1424357416.208389840]: Found a contact between 'target' (type 'Object') and 'forearm' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1424357416.210989280]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1424357416.263241960]: IK failed
[ INFO] [1424357416.322580680]: IK failed
[ INFO] [1424357416.323931000]: Sampler failed to produce a state
[ INFO] [1424357416.324100560]: Manipulation plan 24 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1424357416.324591240]: Pickup planning completed after 3.214549 seconds
[ WARN] [1424357416.325952520]: Fail: ABORTED: No motion plan found. No execution attempted.
so it is claiming collision with the target it is trying to pick, I keep trying to change so many parameters, and sometimes the collision error disappears but still IK failed all the time. Any help would be much appreciated.
First thanks a lot for this thread , it helped me a lot to understand the pick operation and I managed to make it work, but something weird happened today as I updated ros/moveit to fix the bug reported here https://github.com/ros-planning/moveit_commander/issues/22#issuecomment-74931884
Since then nothing is working and I am getting the following log:
[ INFO] [1424357416.156909040]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1424357416.206779560]: IK failed
[ INFO] [1424357416.208389840]: Found a contact between 'target' (type 'Object') and 'forearm' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1424357416.210989280]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1424357416.263241960]: IK failed
[ INFO] [1424357416.322580680]: IK failed
[ INFO] [1424357416.323931000]: Sampler failed to produce a state
[ INFO] [1424357416.324100560]: Manipulation plan 24 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1424357416.324591240]: Pickup planning completed after 3.214549 seconds
[ WARN] [1424357416.325952520]: Fail: ABORTED: No motion plan found. No execution attempted.
so it is claiming collision with the target it is trying to pick, I keep trying to change so many parameters, and sometimes the collision error disappears but still IK failed all the time. Any help would be much appreciated.
Reply all
Reply to author
Forward
Message has been deleted
0 new messages