search

guihomework / dexterous-manipulation-tutorial

11 stars 13 forks source link

Hand not picking the pen issue #12

Closed kirantokala closed 7 years ago

kirantokala commented 7 years ago

Hello,

I don't have too much experience using MoveIt, so maybe I'm missing something obvious but I can't see it. I'm using a shadow robot hand with ROS Indigo, I'm basically following the tutorial and trying to adapt the code to this particular case.

For me, it is working fine still Tutorial2 part1. But Tutorial2 part2 is not working.

I tried to solve using other issue #9. But still error coming

Thanks

kirantokala commented 7 years ago

I reached upto here.

robot_hand_img

kirantokala commented 7 years ago

Logs for grasp_app.py command line:

[ INFO] [1505565216.601559113, 57.104000000]: IK Using joint ra_wrist_3_link -3.14159 3.14159 [ INFO] [1505565216.601583005, 57.104000000]: IK Using joint rh_wrist -0.523599 0.174533 [ INFO] [1505565216.601605199, 57.104000000]: IK Using joint rh_palm -0.698132 0.488692 [ INFO] [1505565216.601625779, 57.104000000]: Looking in private handle: /move_group_commander_wrappers_1505565212254276075 for param name: right_arm_and_wrist/position_only_ik [ INFO] [1505565216.602838017, 57.104000000]: Looking in private handle: /move_group_commander_wrappers_1505565212254276075 for param name: right_arm_and_wrist/solve_type [ INFO] [1505565216.603668343, 57.104000000]: Using solve type Speed [ INFO] [1505565216.622458920, 57.111000000]: IK Using joint rh_wrist -0.523599 0.174533 [ INFO] [1505565216.622542696, 57.111000000]: IK Using joint rh_palm -0.698132 0.488692 [ INFO] [1505565216.622567405, 57.111000000]: Looking in private handle: /move_group_commander_wrappers_1505565212254276075 for param name: rh_wrist/position_only_ik [ INFO] [1505565216.623618105, 57.111000000]: Looking in private handle: /move_group_commander_wrappers_1505565212254276075 for param name: rh_wrist/solve_type [ INFO] [1505565216.624683421, 57.112000000]: Using solve type Speed [ INFO] [1505565216.672109758, 57.130000000]: Started IK for rh_fftip [ WARN] [1505565216.706143339, 57.146000000]: No maxIterations on param server, using 1000 as default [ WARN] [1505565216.707444885, 57.146000000]: No epsilon on param server, using 0.010000 as default [ WARN] [1505565216.710069170, 57.146000000]: No lambda on param server, using 0.010000 as default [ INFO] [1505565216.711272045, 57.147000000]: Started IK for rh_mftip [ WARN] [1505565216.739895503, 57.158000000]: No maxIterations on param server, using 1000 as default [ WARN] [1505565216.743065218, 57.158000000]: No epsilon on param server, using 0.010000 as default [ WARN] [1505565216.744236304, 57.158000000]: No lambda on param server, using 0.010000 as default [ INFO] [1505565216.745129684, 57.158000000]: Started IK for rh_rftip [ WARN] [1505565216.777538107, 57.176000000]: No maxIterations on param server, using 1000 as default [ WARN] [1505565216.778403146, 57.176000000]: No epsilon on param server, using 0.010000 as default [ WARN] [1505565216.779474961, 57.176000000]: No lambda on param server, using 0.010000 as default [ INFO] [1505565216.780160468, 57.176000000]: Started IK for rh_lftip [ WARN] [1505565216.808325464, 57.189000000]: No maxIterations on param server, using 1000 as default [ WARN] [1505565216.810639330, 57.189000000]: No epsilon on param server, using 0.010000 as default [ WARN] [1505565216.811594444, 57.190000000]: No lambda on param server, using 0.010000 as default [ INFO] [1505565216.812735004, 57.190000000]: Started IK for rh_thtip [ WARN] [1505565216.847296839, 57.204000000]: No maxIterations on param server, using 1000 as default [ WARN] [1505565216.848544521, 57.205000000]: No epsilon on param server, using 0.010000 as default [ WARN] [1505565216.849587095, 57.205000000]: No lambda on param server, using 0.010000 as default Available groups: ['rh_fingers', 'rh_first_finger', 'rh_little_finger', 'rh_middle_finger', 'rh_ring_finger', 'rh_thumb', 'rh_wrist', 'right_arm', 'right_arm_and_hand', 'right_arm_and_wrist', 'right_hand'] [ INFO] [1505565217.766007136, 57.709000000]: TrajectoryExecution will use old service capability. [ INFO] [1505565217.766082509, 57.709000000]: Ready to take MoveGroup commands for group right_arm. [ INFO] [1505565225.506257223, 61.885000000]: TrajectoryExecution will use old service capability. [ INFO] [1505565225.506311756, 61.885000000]: Ready to take MoveGroup commands for group right_hand. [ WARN] [1505565229.610971401, 64.066000000]: Fail: ABORTED: Must specify group in motion plan request

Logs for "roslaunch sr_robot_launch sr_ur_arm_hand.launch" command line

[ INFO] [1505565142.166576685]: Loading robot model 'ur10srh'... [ INFO] [1505565142.166699381]: No root/virtual joint specified in SRDF. Assuming fixed joint [ INFO] [1505565142.460104035, 13.157000000]: IK Using joint ra_shoulder_link -3.14159 3.14159 [ INFO] [1505565142.460165133, 13.157000000]: IK Using joint ra_upper_arm_link -3.14159 3.14159 [ INFO] [1505565142.460190539, 13.157000000]: IK Using joint ra_forearm_link -3.14159 3.14159 [ INFO] [1505565142.460214206, 13.157000000]: IK Using joint ra_wrist_1_link -3.14159 3.14159 [ INFO] [1505565142.460238578, 13.157000000]: IK Using joint ra_wrist_2_link -3.14159 3.14159 [ INFO] [1505565142.460260729, 13.157000000]: IK Using joint ra_wrist_3_link -3.14159 3.14159 [ INFO] [1505565142.460294023, 13.157000000]: Looking in private handle: /moveit_python_wrappers_1505565142144747925 for param name: right_arm/position_only_ik [ INFO] [1505565142.461106436, 13.158000000]: Looking in private handle: /moveit_python_wrappers_1505565142144747925 for param name: right_arm/solve_type [ INFO] [1505565142.461622115, 13.158000000]: Using solve type Speed [ INFO] [1505565142.470807186, 13.164000000]: IK Using joint ra_shoulder_link -3.14159 3.14159 [ INFO] [1505565142.471024700, 13.164000000]: IK Using joint ra_upper_arm_link -3.14159 3.14159 [ INFO] [1505565142.471051392, 13.164000000]: IK Using joint ra_forearm_link -3.14159 3.14159 [ INFO] [1505565142.471072925, 13.164000000]: IK Using joint ra_wrist_1_link -3.14159 3.14159 [ INFO] [1505565142.471147726, 13.164000000]: IK Using joint ra_wrist_2_link -3.14159 3.14159 [ INFO] [1505565142.471179118, 13.164000000]: IK Using joint ra_wrist_3_link -3.14159 3.14159 [ INFO] [1505565142.471205288, 13.164000000]: IK Using joint rh_wrist -0.523599 0.174533 [ INFO] [1505565142.471226102, 13.164000000]: IK Using joint rh_palm -0.698132 0.488692 [ INFO] [1505565142.471244750, 13.164000000]: Looking in private handle: /moveit_python_wrappers_1505565142144747925 for param name: right_arm_and_wrist/position_only_ik [ INFO] [1505565142.472346949, 13.165000000]: Looking in private handle: /moveit_python_wrappers_1505565142144747925 for param name: right_arm_and_wrist/solve_type [ INFO] [1505565142.472936832, 13.165000000]: Using solve type Speed [ INFO] [1505565142.483283700, 13.172000000]: IK Using joint rh_wrist -0.523599 0.174533 [ INFO] [1505565142.483443386, 13.172000000]: IK Using joint rh_palm -0.698132 0.488692 [ INFO] [1505565142.483509019, 13.172000000]: Looking in private handle: /moveit_python_wrappers_1505565142144747925 for param name: rh_wrist/position_only_ik [ INFO] [1505565142.484467788, 13.172000000]: Looking in private handle: /moveit_python_wrappers_1505565142144747925 for param name: rh_wrist/solve_type [ INFO] [1505565142.484947203, 13.172000000]: Using solve type Speed [ INFO] [1505565142.518969508, 13.193000000]: Started IK for rh_fftip [ WARN] [1505565142.541801424, 13.208000000]: No maxIterations on param server, using 1000 as default [ WARN] [1505565142.542486741, 13.209000000]: No epsilon on param server, using 0.010000 as default [ WARN] [1505565142.542956726, 13.209000000]: No lambda on param server, using 0.010000 as default [ INFO] [1505565142.543634268, 13.209000000]: Started IK for rh_mftip [ WARN] [1505565142.565053738, 13.223000000]: No maxIterations on param server, using 1000 as default [ WARN] [1505565142.565613775, 13.223000000]: No epsilon on param server, using 0.010000 as default [ WARN] [1505565142.566135768, 13.223000000]: No lambda on param server, using 0.010000 as default [ INFO] [1505565142.566487649, 13.223000000]: Started IK for rh_rftip [ WARN] [1505565142.588529635, 13.237000000]: No maxIterations on param server, using 1000 as default [ WARN] [1505565142.589085080, 13.238000000]: No epsilon on param server, using 0.010000 as default [ WARN] [1505565142.589561430, 13.238000000]: No lambda on param server, using 0.010000 as default [ INFO] [1505565142.589938804, 13.238000000]: Started IK for rh_lftip [ WARN] [1505565142.612948466, 13.253000000]: No maxIterations on param server, using 1000 as default [ WARN] [1505565142.614005950, 13.254000000]: No epsilon on param server, using 0.010000 as default [ WARN] [1505565142.614645788, 13.254000000]: No lambda on param server, using 0.010000 as default [ INFO] [1505565142.615081807, 13.254000000]: Started IK for rh_thtip [ WARN] [1505565142.636509769, 13.268000000]: No maxIterations on param server, using 1000 as default [ WARN] [1505565142.637033891, 13.268000000]: No epsilon on param server, using 0.010000 as default [ WARN] [1505565142.638131000, 13.269000000]: No lambda on param server, using 0.010000 as default [ INFO] [1505565145.018234125, 14.865000000]: Loading robot model 'ur10srh'... [ INFO] [1505565145.018489228, 14.865000000]: No root/virtual joint specified in SRDF. Assuming fixed joint [ INFO] [1505565145.745541239, 15.288000000]: Loading robot model 'ur10srh'... [ INFO] [1505565145.745660034, 15.288000000]: No root/virtual joint specified in SRDF. Assuming fixed joint [ INFO] [1505565146.037342970, 15.435000000]: IK Using joint ra_shoulder_link -3.14159 3.14159 [ INFO] [1505565146.037467134, 15.435000000]: IK Using joint ra_upper_arm_link -3.14159 3.14159 [ INFO] [1505565146.037494356, 15.435000000]: IK Using joint ra_forearm_link -3.14159 3.14159 [ INFO] [1505565146.037514652, 15.435000000]: IK Using joint ra_wrist_1_link -3.14159 3.14159 [ INFO] [1505565146.037534824, 15.435000000]: IK Using joint ra_wrist_2_link -3.14159 3.14159 [ INFO] [1505565146.037555435, 15.435000000]: IK Using joint ra_wrist_3_link -3.14159 3.14159 [ INFO] [1505565146.037595486, 15.435000000]: Looking in private handle: /rviz_root1_ThinkCenter_26375_4848893127691406725 for param name: right_arm/position_only_ik [ INFO] [1505565146.038265763, 15.436000000]: Looking in private handle: /rviz_root1_ThinkCenter_26375_4848893127691406725 for param name: right_arm/solve_type [ INFO] [1505565146.040229841, 15.437000000]: Using solve type Distance [ INFO] [1505565146.054659005, 15.444000000]: IK Using joint ra_shoulder_link -3.14159 3.14159 [ INFO] [1505565146.054772187, 15.444000000]: IK Using joint ra_upper_arm_link -3.14159 3.14159 [ INFO] [1505565146.054842615, 15.444000000]: IK Using joint ra_forearm_link -3.14159 3.14159 [ INFO] [1505565146.054908122, 15.444000000]: IK Using joint ra_wrist_1_link -3.14159 3.14159 [ INFO] [1505565146.054972617, 15.444000000]: IK Using joint ra_wrist_2_link -3.14159 3.14159 [ INFO] [1505565146.055035603, 15.444000000]: IK Using joint ra_wrist_3_link -3.14159 3.14159 [ INFO] [1505565146.055100519, 15.444000000]: IK Using joint rh_wrist -0.523599 0.174533 [ INFO] [1505565146.055140515, 15.444000000]: IK Using joint rh_palm -0.698132 0.488692 [ INFO] [1505565146.055168330, 15.444000000]: Looking in private handle: /rviz_root1_ThinkCenter_26375_4848893127691406725 for param name: right_arm_and_wrist/position_only_ik [ INFO] [1505565146.056189626, 15.445000000]: Looking in private handle: /rviz_root1_ThinkCenter_26375_4848893127691406725 for param name: right_arm_and_wrist/solve_type [ INFO] [1505565146.056740902, 15.445000000]: Using solve type Speed [ INFO] [1505565146.069581871, 15.452000000]: IK Using joint rh_wrist -0.523599 0.174533 [ INFO] [1505565146.069630532, 15.452000000]: IK Using joint rh_palm -0.698132 0.488692 [ INFO] [1505565146.069699645, 15.452000000]: Looking in private handle: /rviz_root1_ThinkCenter_26375_4848893127691406725 for param name: rh_wrist/position_only_ik [ INFO] [1505565146.071076240, 15.453000000]: Looking in private handle: /rviz_root1_ThinkCenter_26375_4848893127691406725 for param name: rh_wrist/solve_type [ INFO] [1505565146.071713040, 15.453000000]: Using solve type Speed [ INFO] [1505565146.114773036, 15.473000000]: Started IK for rh_fftip [ INFO] [1505565146.145898259, 15.491000000]: Started IK for rh_mftip [ INFO] [1505565146.173650468, 15.507000000]: Started IK for rh_rftip [ INFO] [1505565146.198303532, 15.518000000]: Started IK for rh_lftip [ INFO] [1505565146.224158015, 15.531000000]: Started IK for rh_thtip [ INFO] [1505565146.306713184, 15.578000000]: Starting scene monitor [ INFO] [1505565146.310589235, 15.579000000]: Listening to '/move_group/monitored_planning_scene' [ INFO] [1505565146.384034666, 15.626000000]: Constructing new MoveGroup connection for group 'right_arm' in namespace '' [ INFO] [1505565146.661330426, 15.795000000]: TrajectoryExecution will use old service capability. [ INFO] [1505565146.661388660, 15.795000000]: Ready to take MoveGroup commands for group right_arm. [ INFO] [1505565147.252446697, 16.175000000]: TrajectoryExecution will use old service capability. [ INFO] [1505565147.252516678, 16.175000000]: Ready to take MoveGroup commands for group right_arm. [ INFO] [1505565147.252551459, 16.175000000]: Looking around: no [ INFO] [1505565147.252576675, 16.175000000]: Replanning: no [INFO] [WallTime: 1505565153.352106] [19.796000] Waiting for warehouse services... [ WARN] [1505565214.234878265, 55.782000000]: MessageFilter [target=/world ]: Discarding message from [/moveit_grasp_app_28259_1505565212265] due to empty frame_id. This message will only print once. [ INFO] [1505565217.768217214, 57.709000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline. [ERROR] [1505565217.768795569, 57.709000000]: Found empty JointState message [ERROR] [1505565217.768950929, 57.709000000]: Found empty JointState message [ERROR] [1505565217.769670512, 57.709000000]: Found empty JointState message [ WARN] [1505565217.770283224, 57.709000000]: Cannot find planning configuration for group 'right_arm' using planner 'RRTConnectConfigDefault'. Will use defaults instead. [ERROR] [1505565217.770755034, 57.709000000]: Found empty JointState message [ INFO] [1505565217.771220728, 57.709000000]: No optimization objective specified, defaulting to PathLengthOptimizationObjective [ INFO] [1505565217.771329054, 57.710000000]: No planner specified. Using default. [ INFO] [1505565217.772011251, 57.710000000]: RRTConnect: Starting planning with 1 states already in datastructure [ INFO] [1505565217.802732451, 57.723000000]: RRTConnect: Created 4 states (2 start + 2 goal) [ INFO] [1505565217.802795489, 57.723000000]: Solution found in 0.031143 seconds [ INFO] [1505565217.806572898, 57.726000000]: SimpleSetup: Path simplification took 0.003655 seconds and changed from 3 to 2 states [ INFO] [1505565217.814207254, 57.731000000]: Received new trajectory execution service request... [ WARN] [1505565217.817513600, 57.733000000]: Dropping first 1 trajectory point(s) out of 10, as they occur before the current time. First valid point will be reached in 0.409s. [ INFO] [1505565221.112572995, 59.533000000]: Execution completed: SUCCEEDED [ INFO] [1505565221.114934504, 59.534000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline. [ INFO] [1505565221.115046799, 59.534000000]: Planning attempt 1 of at most 1 [ERROR] [1505565221.115119097, 59.534000000]: Found empty JointState message [ERROR] [1505565221.115142786, 59.534000000]: Found empty JointState message [ERROR] [1505565221.115439141, 59.534000000]: Found empty JointState message [ WARN] [1505565221.115801816, 59.534000000]: Cannot find planning configuration for group 'right_arm' using planner 'RRTConnectConfigDefault'. Will use defaults instead. [ERROR] [1505565221.115918537, 59.534000000]: Found empty JointState message [ INFO] [1505565221.116061670, 59.534000000]: No optimization objective specified, defaulting to PathLengthOptimizationObjective [ INFO] [1505565221.116098336, 59.534000000]: No planner specified. Using default. [ INFO] [1505565221.116360931, 59.534000000]: RRTConnect: Starting planning with 1 states already in datastructure [ INFO] [1505565221.156254871, 59.555000000]: RRTConnect: Created 5 states (3 start + 2 goal) [ INFO] [1505565221.156618992, 59.555000000]: Solution found in 0.040439 seconds [ INFO] [1505565221.168355175, 59.560000000]: SimpleSetup: Path simplification took 0.011498 seconds and changed from 4 to 2 states [ WARN] [1505565221.175923931, 59.563000000]: Dropping first 1 trajectory point(s) out of 20, as they occur before the current time. First valid point will be reached in 0.306s. [ INFO] [1505565225.506733586, 61.885000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline. [ INFO] [1505565225.506904601, 61.885000000]: Planning attempt 1 of at most 1 [ERROR] [1505565225.507004499, 61.885000000]: Found empty JointState message [ INFO] [1505565225.507101389, 61.885000000]: Starting state is just outside bounds (joint 'rh_LFJ5'). Assuming within bounds. [ INFO] [1505565225.507119079, 61.885000000]: Starting state is just outside bounds (joint 'rh_THJ4'). Assuming within bounds. [ INFO] [1505565225.507136387, 61.885000000]: Starting state is just outside bounds (joint 'rh_THJ3'). Assuming within bounds. [ WARN] [1505565225.508345130, 61.885000000]: Cannot find planning configuration for group 'right_hand' using planner 'LBKPIECEkConfigDefault'. Will use defaults instead. [ INFO] [1505565225.508953452, 61.885000000]: No planner specified. Using default. [ INFO] [1505565225.509550206, 61.885000000]: RRTConnect: Starting planning with 1 states already in datastructure [ INFO] [1505565225.543308338, 61.899000000]: RRTConnect: Created 5 states (2 start + 3 goal) [ INFO] [1505565225.543448962, 61.899000000]: Solution found in 0.034380 seconds [ INFO] [1505565225.546762213, 61.902000000]: SimpleSetup: Path simplification took 0.003205 seconds and changed from 4 to 2 states [ INFO] [1505565225.548337192, 61.902000000]: Planning adapters have added states at index positions: [ 0 ] [ WARN] [1505565225.561022291, 61.907000000]: Dropping first 1 trajectory point(s) out of 15, as they occur before the current time. First valid point will be reached in 0.212s. [ WARN] [1505565225.561757963, 61.907000000]: Dropping first 1 trajectory point(s) out of 15, as they occur before the current time. First valid point will be reached in 0.212s. [ERROR] [1505565229.602839035, 64.063000000]: Attached body 'pen' not found [ INFO] [1505565229.607965819, 64.064000000]: Planning attempt 1 of at most 1 [ERROR] [1505565229.608096837, 64.064000000]: No end-effector specified for pick action

guihomework commented 7 years ago

Seems you encounter the same error as before, with the group being wrongly defined. https://github.com/guihomework/dexterous-manipulation-tutorial/issues/9#issuecomment-321502525 Shadow fixed their sr_multi_moveit_config and either my patch or their fix solves this particular problem. Why create a new bug for the same error that was fixed ?

Again here, it seems you start the sr_ur_arm_hand.launch from sr_robot_launch not the sr_multi_moveit_config, which I did not try and this is not coming from my tutorial. I cannot test my tutorial for every launch file that exist at shadow.

The tutorial is meant to learn how to use moveit in the context of dexterous manipulation, if the sr_ur_arm_hand.launch starts wrong things, it is up to you to compare with what sr_multi_moveit_config does then, it probably loads a wrong srdf (since the problem is the same as before).

kirantokala commented 7 years ago

Hi,

Now it is executing fine.

Mistake is from my side only. I run the file from sr_robot_launch package from shadow robot.

Now, I run the demo.launch file from sr_multi_moveit_config, So It is working fine.

Thank you

guihomework commented 7 years ago

Good. Then you have a starting point to compare and get your things working later. Good luck.