Revision history [back]

Now the robot was able to find and grasp the object. But it either fails to lift the object, or cannot find the trajectory to drop off the object and stuck there(I was thinking maybe it's because the robot is picking up the object with right arm and supposed to drop it off at the left part of the table, which is too far for the right arm to accomplish). And then rviz window always crashes.

rviz: /tmp/buildd/ros-diamondback-visualization-common-1.4.2/debian/ros-diamondback-visualization-common/opt/ros/diamondback/stacks/visualization_common/ogre/build/ogre_src_v1-7-1/OgreMain/src/OgreRenderQueueSortingGrouping.cpp:386: void Ogre::QueuedRenderableCollection::addRenderable(Ogre::Pass*, Ogre::Renderable*): Assertion `retPair.second && "Error inserting new pass entry into PassGroupRenderableMap"' failed.
Aborted

This following image shows how the gazebo world was set up. The front edge of the base is flush with the front edge of the middle area of the table.

Output in the pick_and_place_demo.py window:

[INFO] [WallTime: 1313364634.696529] [482.448000] try_hard_to_move_joint try number: 0
[INFO] [WallTime: 1313364634.709388] [482.448000] asking move_arm to go to angles: -2.135 0.803 -1.732 -1.905 -2.369 -1.680 1.398
[ERROR] [WallTime: 1313364634.721057] [482.448000] Got a feedback callback when we're not tracking a goal. (id: /pick_and_place_demo_32161_1313362798776-3-482.448)
[INFO] [WallTime: 1313364635.324415] [482.525000] move_arm succeeded
[INFO] [WallTime: 1313364635.324947] [482.525000] move arm reported success
[INFO] [WallTime: 1313364635.325349] [482.525000] try_hard_to_move_joint try number: 0
[INFO] [WallTime: 1313364635.381088] [482.539000] asking move_arm to go to angles: 2.135 0.803 1.732 -1.905 2.369 -1.680 1.398
[INFO] [WallTime: 1313364636.054058] [482.578000] move_arm succeeded
[INFO] [WallTime: 1313364636.054517] [482.578000] move arm reported success
[INFO] [WallTime: 1313364648.031121] [484.016000] calling tabletop detection
[INFO] [WallTime: 1313364648.765651] [484.072000] tabletop detection reports success
[INFO] [WallTime: 1313364648.781044] [484.072000] table front edge x: 0.355
[INFO] [WallTime: 1313364648.781535] [484.072000] table height: 0.504
[INFO] [WallTime: 1313364648.782079] [484.072000] detection finished, finding bounding boxes for clusters and sorting objects
[INFO] [WallTime: 1313364651.297679] [484.380000] calling tabletop detection
[INFO] [WallTime: 1313364652.627787] [484.523000] tabletop detection reports success
[INFO] [WallTime: 1313364652.676713] [484.524000] detection finished, finding bounding boxes for clusters and sorting objects
[INFO] [WallTime: 1313364652.683333] [484.524000] object 0, red: recognized object with id 18665 and col name graspable_object_0
[INFO] [WallTime: 1313364652.684626] [484.524000] object dist from back edge is -0.259
[INFO] [WallTime: 1313364657.426376] [485.041000] calling tabletop detection
[INFO] [WallTime: 1313364658.520358] [485.143000] tabletop detection reports success
[INFO] [WallTime: 1313364658.530696] [485.143000] detection finished, finding bounding boxes for clusters and sorting objects
[INFO] [WallTime: 1313364658.531799] [485.143000] saw 1 objects on the right and 0 objects on the left
[INFO] [WallTime: 1313364658.532192] [485.143000] setting pick_up_side to the right side, put_down_side to the left
[INFO] [WallTime: 1313364658.572135] [485.146000] starting to move objects from r side to l side
[INFO] [WallTime: 1313364658.573262] [485.146000] object_tries:0
[INFO] [WallTime: 1313364662.196818] [485.575000] calling tabletop detection
[INFO] [WallTime: 1313364663.702122] [485.723000] tabletop detection reports success
[INFO] [WallTime: 1313364716.505898] [491.833000] detection finished, finding bounding boxes for clusters and sorting objects
[INFO] [WallTime: 1313364716.513570] [491.834000] object 0, red: recognized object with id 18665 and col name graspable_object_1
[INFO] [WallTime: 1313364716.514906] [491.834000] object dist from back edge is -0.259
[INFO] [WallTime: 1313364716.516770] [491.834000] dist_from_point:0.278
[INFO] [WallTime: 1313364716.518296] [491.834000] refining object detection
[INFO] [WallTime: 1313364720.205352] [492.276000] calling tabletop detection
[INFO] [WallTime: 1313364721.262789] [492.374000] tabletop detection reports success
[INFO] [WallTime: 1313364780.966603] [499.387000] detection finished, finding bounding boxes for clusters and sorting objects
[INFO] [WallTime: 1313364780.969839] [499.387000] object 0, red: recognized object with id 18665 and col name graspable_object_2
[INFO] [WallTime: 1313364780.971412] [499.387000] refined object's collision name: graspable_object_2
[INFO] [WallTime: 1313364780.972028] [499.387000] attempting to grasp an object, whicharm = 0
sending object collision name of: graspable_object_2
[INFO] [WallTime: 1313364989.277827] [520.049000] grasp result: LIFT_FAILED
[INFO] [WallTime: 1313364989.278367] [520.049000] gripper opening acceptable: 0.074
[INFO] [WallTime: 1313364989.278757] [520.049000] just lift failed; lifting with Cartesian controllers
[INFO] [WallTime: 1313364989.469111] [520.067000] switched joint to Cartesian successfully
[INFO] [WallTime: 1313364989.513563] [520.070000] preempt unnecessary, robot not moving
[INFO] [WallTime: 1313365021.611225] [523.077000] settling time ran out
[INFO] [WallTime: 1313365021.611739] [523.077000] gripper opening acceptable: 0.065
[INFO] [WallTime: 1313365021.612113] [523.077000] grasp succeeded, adding object to held_objects for the r arm
[INFO] [WallTime: 1313365021.612448] [523.077000] Planning for the right arm
[INFO] [WallTime: 1313365021.613123] [523.077000] constrained move try number: 0
[INFO] [WallTime: 1313365021.943647] [523.112000] switched Cartesian to joint successfully
[INFO] [WallTime: 1313365021.999843] [523.115000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.035449] [523.115000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.074307] [523.118000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.112305] [523.118000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.151379] [523.130000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.188451] [523.130000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.228465] [523.141000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.273477] [523.141000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.314318] [523.141000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.359442] [523.150000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.403116] [523.150000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.445460] [523.160000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.474817] [523.167000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.519203] [523.168000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.596135] [523.174000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.643130] [523.179000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.685139] [523.184000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.710940] [523.184000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.863950] [523.195000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.923393] [523.212000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365022.960267] [523.212000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365023.001745] [523.224000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365023.045301] [523.233000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365023.090343] [523.241000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365023.159151] [523.247000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365023.220828] [523.253000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365023.273206] [523.258000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365023.314306] [523.262000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365023.363586] [523.270000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365023.440091] [523.281000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365023.536171] [523.291000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365023.609245] [523.298000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365023.663707] [523.300000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365023.758056] [523.303000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365023.827110] [523.306000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365023.890588] [523.306000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365023.946781] [523.306000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.014032] [523.308000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.054859] [523.311000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.086341] [523.313000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.170739] [523.318000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.270349] [523.322000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.312205] [523.322000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.346509] [523.336000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.392579] [523.336000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.434996] [523.336000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.471660] [523.349000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.509148] [523.349000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.580348] [523.360000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.643699] [523.364000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.699716] [523.376000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.741815] [523.376000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.782939] [523.382000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.819593] [523.394000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.856188] [523.396000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.910541] [523.398000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.942529] [523.398000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365024.985155] [523.398000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365025.030514] [523.400000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365025.079391] [523.402000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365025.123099] [523.403000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365025.171790] [523.406000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365025.211429] [523.408000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365025.247262] [523.410000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365025.308161] [523.412000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365025.350162] [523.414000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365025.410094] [523.416000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365025.458478] [523.418000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365025.508302] [523.421000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365025.574811] [523.425000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365025.619626] [523.426000] IK error code: NO_IK_SOLUTION
[INFO] [WallTime: 1313365025.677421] [523.429000] IK error code: NO_IK_SOLUTION
Exception in thread Thread-78:
Traceback (most recent call last):
  File "/usr/lib/python2.6/threading.py", line 532, in __bootstrap_inner
    self.run()
  File "/usr/lib/python2.6/threading.py", line 484, in run
    self.__target(*self.__args, **self.__kwargs)
  File "/home/vincent/pr2_newcontroller/pr2_tabletop_manipulation_apps_new/pr2_pick_and_place_demos/src/pr2_pick_and_place_demos/pick_and_place_demo.py", line 300, in run_demo
    result = self.move_objects_to_other_side()
  File "/home/vincent/pr2_newcontroller/pr2_tabletop_manipulation_apps_new/pr2_pick_and_place_demos/src/pr2_pick_and_place_demos/pick_and_place_demo.py", line 149, in move_objects_to_other_side
    (result, arm_used) = self.detect_and_pick_up_object(head_point, arms_to_try = arms_to_try, constrained = self.constrained)
  File "/home/vincent/pr2_newcontroller/pr2_tabletop_manipulation_apps_new/pr2_pick_and_place_demos/src/pr2_pick_and_place_demos/pick_and_place_manager.py", line 1614, in detect_and_pick_up_object
    (success, arm_used) = self.pick_up_nearest_object(arms_to_try = arms_to_try)
  File "/home/vincent/pr2_newcontroller/pr2_tabletop_manipulation_apps_new/pr2_pick_and_place_demos/src/pr2_pick_and_place_demos/pick_and_place_manager.py", line 1447, in pick_up_nearest_object
    (result, arm_used) = self.grasp_object_and_check_success(object_to_grasp, whicharm)
  File "/home/vincent/pr2_newcontroller/pr2_tabletop_manipulation_apps_new/pr2_pick_and_place_demos/src/pr2_pick_and_place_demos/pick_and_place_manager.py", line 1358, in grasp_object_and_check_success
    self.move_arm_to_side(arm_used, try_constrained = 1)
  File "/home/vincent/pr2_newcontroller/pr2_tabletop_manipulation_apps_new/pr2_pick_and_place_demos/src/pr2_pick_and_place_demos/pick_and_place_manager.py", line 1664, in move_arm_to_side
    result = self.try_to_move_constrained(whicharm,constraint,3,start_angles,location)
  File "/home/vincent/pr2_newcontroller/pr2_tabletop_manipulation_apps_new/pr2_pick_and_place_demos/src/pr2_pick_and_place_demos/pick_and_place_manager.py", line 1633, in try_to_move_constrained
    error_code = self.cms[whicharm].move_arm_constrained(constraint, start_angles, location)
  File "/opt/ros/diamondback/stacks/pr2_object_manipulation/pr2_gripper_reactive_approach/src/pr2_gripper_reactive_approach/controller_manager.py", line 599, in move_arm_constrained
    joint_constraint.position = solution[2]
IndexError: tuple index out of range