Not Picking the Pen - Githubissues

kirantokala commented 7 years ago

When I run the grasp_app.py file,

It is executing fine. Cup and pen are drawing in Moveit.

But I am getting error like

[ WARN] [1504365375.873329728, 35.582000000]: Fail: ABORTED: Solution found but controller failed during execution

kirantokala commented 7 years ago

Because of this,

Hand is going to that pen. But it is not picking. Just Plan found. But Execution not happening.

Can you please tell me, If any one knows this issue?

kirantokala commented 7 years ago

picking_error

kirantokala commented 7 years ago

It's like above picture, Plan found using four grasp pose, But execution not happening.

guihomework commented 7 years ago

sorry for delayed answering, i was busy in a summer school. I will have a look

kirantokala commented 7 years ago

ok, Thanks.

I stuck at this part. Waiting for your reply. Please have a look and replay as soon as possible.

guihomework commented 7 years ago

I can reproduce it and it worked in the past, so there is something which changed since my last test for you.

[ INFO] [1505303740.498642497]: Ready to take commands for planning group right_hand.
[ WARN] [1505303741.045817834]: Fail: ABORTED: Solution found but controller failed during execution

what you did not tell me, is that the move_group.launch shows error before you even start your app I am pretty sure you have something similar to this in your move_group.launch

[ERROR] [1505303413.706983462]: MoveItFakeControllerManager: No controller_list specified.

this is why at execution you moveit planner says something like this

[ INFO] [1505303741.044619348]: Found successful manipulation plan!
[ INFO] [1505303741.044756911]: Pickup planning completed after 0.081335 seconds
[ INFO] [1505303741.045203447]: Returned 0 controllers in list
[ERROR] [1505303741.045241103]: Unable to identify any set of controllers that can actuate the specified joints: [ ra_elbow_joint ra_shoulder_lift_joint ra_shoulder_pan_joint ra_wrist_1_joint ra_wrist_2_joint ra_wrist_3_joint ]
[ERROR] [1505303741.045304168]: Known controllers and their joints: 
(empty list shown here)

Please for next time you report issues

include all errors (or try track the errors on your own first), to not force the maintainer to first reproduce the exact error, but give all the info for faster help.
don't ask for "reply as soon as possible" this makes it worse usually. A community help is not a dedicated service that you pay to support you, so it happens when the maintainer or other members has time to help.

I think the problem is related to the latest sr_multi_moveit package (which version/branch/commit do you use ?) I will investigate by tonight (cannot do better, have other stuff on my list that was already planned)

kirantokala commented 7 years ago

Sorry for troubles you.

guihomework commented 7 years ago

After looking deeply into sr_multi_moveit commit history I did not find changes that would affect the controller_list. I gave it a second try, and this time I got

[INFO] [1505334794.074686]: Successfully loaded fake_controllers params
[ INFO] [1505334794.084820921]: Fake controller 'fake_ra_controller' with joints [ ra_shoulder_pan_joint ra_shoulder_lift_joint ra_elbow_joint ra_wrist_1_joint ra_wrist_2_joint ra_wrist_3_joint rh_WRJ2 rh_WRJ1 ]
[ INFO] [1505334794.085369724]: Fake controller 'fake_rh_controller' with joints [ rh_THJ5 rh_THJ4 rh_THJ3 rh_THJ2 rh_THJ1 rh_FFJ4 rh_FFJ3 rh_FFJ2 rh_FFJ1 rh_MFJ4 rh_MFJ3 rh_MFJ2 rh_MFJ1 rh_RFJ4 rh_RFJ3 rh_RFJ2 rh_RFJ1 rh_LFJ5 rh_LFJ4 rh_LFJ3 rh_LFJ2 rh_LFJ1 ]
[ INFO] [1505334794.085871805]: Returned 2 controllers in list

which is correct and then the pen is picked correctly at my side.

I suppose the problem is a runtime race condition when moveit wants the controller list to be available, but the generator did not generate it yet for some reason. After a second launch at my side it worked all the time. Race conditions are hard to solve in launch files, and this is something Shadow Robot team should solve. I will contact them tomorrow. In the mean time the only solution I can see for you is to use a saved fake_controllers.yaml that works for the tutorial.

Please put the attached file fake_controllers.txt to this folder sr_interface/sr_multi_moveit/sr_multi_moveit_config/config AND rename its extension to yaml (github does not accept yaml directly)
modify this file sr_interface/sr_multi_moveit/sr_multi_moveit_config/launch/fake_moveit_controller_manager.launch.xml comment out the generator node
```

```
and add this <rosparam command="load" file="$(find sr_multi_moveit_config)/config/fake_controllers.yaml" />

In general there should be no error when loading the demo, otherwise things do not work properly.

If this solves your problem, please close the issue.

kirantokala commented 7 years ago

Hi,

I tried as you told. But Still error not solved.

Logs for grasp_app.py file command line: [ INFO] [1505624032.233821358, 24.147000000]: Started IK for rh_thtip [ WARN] [1505624032.264939253, 24.159000000]: No maxIterations on param server, using 1000 as default [ WARN] [1505624032.265619896, 24.160000000]: No epsilon on param server, using 0.010000 as default [ WARN] [1505624032.266427957, 24.161000000]: 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] [1505624033.076777429, 24.506000000]: TrajectoryExecution will use old service capability. [ INFO] [1505624033.076889601, 24.506000000]: Ready to take MoveGroup commands for group right_arm. [ INFO] [1505624042.426801220, 28.705000000]: TrajectoryExecution will use old service capability. [ INFO] [1505624042.426879237, 28.705000000]: Ready to take MoveGroup commands for group right_hand. [ INFO] [1505624048.648165535, 31.527000000]: ABORTED: Solution found but controller failed during execution [ WARN] [1505624062.461489491, 37.406000000]: Fail: ABORTED: Solution found but controller failed during execution

Logs for sr_robot_launch file: [ INFO] [1505624033.078765723, 24.506000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline. [ERROR] [1505624033.079404478, 24.506000000]: Found empty JointState message [ERROR] [1505624033.079570984, 24.506000000]: Found empty JointState message [ERROR] [1505624033.081350038, 24.506000000]: Found empty JointState message [ WARN] [1505624033.082148015, 24.506000000]: Cannot find planning configuration for group 'right_arm' using planner 'RRTConnectConfigDefault'. Will use defaults instead. [ERROR] [1505624033.082979388, 24.506000000]: Found empty JointState message [ INFO] [1505624033.084325291, 24.506000000]: No optimization objective specified, defaulting to PathLengthOptimizationObjective [ INFO] [1505624033.084620380, 24.506000000]: No planner specified. Using default. [ INFO] [1505624033.087845194, 24.506000000]: RRTConnect: Starting planning with 1 states already in datastructure [ INFO] [1505624033.122677082, 24.525000000]: RRTConnect: Created 5 states (3 start + 2 goal) [ INFO] [1505624033.122744507, 24.525000000]: Solution found in 0.036261 seconds [ INFO] [1505624033.126533130, 24.526000000]: SimpleSetup: Path simplification took 0.003544 seconds and changed from 4 to 2 states [ INFO] [1505624033.141854967, 24.529000000]: Received new trajectory execution service request... [ WARN] [1505624033.147980755, 24.534000000]: 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] [1505624037.130700053, 26.336000000]: Execution completed: SUCCEEDED [ INFO] [1505624037.135694967, 26.338000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline. [ INFO] [1505624037.135938940, 26.338000000]: Planning attempt 1 of at most 1 [ERROR] [1505624037.136028220, 26.338000000]: Found empty JointState message [ERROR] [1505624037.136055062, 26.338000000]: Found empty JointState message [ERROR] [1505624037.136383300, 26.338000000]: Found empty JointState message [ WARN] [1505624037.136775441, 26.338000000]: Cannot find planning configuration for group 'right_arm' using planner 'RRTConnectConfigDefault'. Will use defaults instead. [ERROR] [1505624037.136906999, 26.338000000]: Found empty JointState message [ INFO] [1505624037.137091247, 26.338000000]: No optimization objective specified, defaulting to PathLengthOptimizationObjective [ INFO] [1505624037.137134333, 26.338000000]: No planner specified. Using default. [ INFO] [1505624037.137469425, 26.338000000]: RRTConnect: Starting planning with 1 states already in datastructure [ INFO] [1505624037.179850038, 26.356000000]: RRTConnect: Created 4 states (2 start + 2 goal) [ INFO] [1505624037.180113582, 26.357000000]: Solution found in 0.042892 seconds [ INFO] [1505624037.191296178, 26.363000000]: SimpleSetup: Path simplification took 0.011085 seconds and changed from 3 to 2 states [ WARN] [1505624037.203104387, 26.368000000]: 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] [1505624042.442879080, 28.710000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline. [ INFO] [1505624042.443557586, 28.710000000]: Planning attempt 1 of at most 1 [ERROR] [1505624042.444109001, 28.710000000]: Found empty JointState message [ INFO] [1505624042.444631739, 28.710000000]: Starting state is just outside bounds (joint 'rh_WRJ1'). Assuming within bounds. [ INFO] [1505624042.444684013, 28.710000000]: Starting state is just outside bounds (joint 'rh_LFJ2'). Assuming within bounds. [ INFO] [1505624042.444717673, 28.710000000]: Starting state is just outside bounds (joint 'rh_THJ4'). Assuming within bounds. [ INFO] [1505624042.444748875, 28.710000000]: Starting state is just outside bounds (joint 'rh_THJ3'). Assuming within bounds. [ WARN] [1505624042.446407806, 28.710000000]: Cannot find planning configuration for group 'right_hand' using planner 'LBKPIECEkConfigDefault'. Will use defaults instead. [ INFO] [1505624042.447428182, 28.710000000]: No planner specified. Using default. [ INFO] [1505624042.448213257, 28.710000000]: RRTConnect: Starting planning with 1 states already in datastructure [ INFO] [1505624042.476651312, 28.721000000]: RRTConnect: Created 5 states (2 start + 3 goal) [ INFO] [1505624042.477276584, 28.721000000]: Solution found in 0.029593 seconds [ INFO] [1505624042.483746049, 28.723000000]: SimpleSetup: Path simplification took 0.006018 seconds and changed from 4 to 2 states [ INFO] [1505624042.487027474, 28.724000000]: Planning adapters have added states at index positions: [ 0 ] [ WARN] [1505624042.496305038, 28.728000000]: Dropping first 1 trajectory point(s) out of 16, as they occur before the current time. First valid point will be reached in 0.252s. [ WARN] [1505624042.496404520, 28.728000000]: Dropping first 1 trajectory point(s) out of 16, as they occur before the current time. First valid point will be reached in 0.252s. [ WARN] [1505624042.721930308, 28.828000000]: Controller ra_trajectory_controller failed with error code PATH_TOLERANCE_VIOLATED [ WARN] [1505624042.722369784, 28.828000000]: Controller handle ra_trajectory_controller reports status ABORTED [ERROR] [1505624049.017038126, 31.667000000]: Attached body 'pen' not found [ INFO] [1505624049.017319569, 31.667000000]: Planning attempt 1 of at most 1 [ INFO] [1505624049.030001819, 31.673000000]: Added plan for pipeline 'pick'. Queue is now of size 1 [ INFO] [1505624049.624799069, 31.865000000]: Attaching world object 'pen' to link 'ra_ee_link' [ INFO] [1505624049.625093834, 31.865000000]: Attached object 'pen' to link 'ra_ee_link' [ERROR] [1505624050.410199597, 32.100000000]: Found empty JointState message [ERROR] [1505624050.410267237, 32.100000000]: Found empty JointState message [ERROR] [1505624050.410667221, 32.100000000]: Found empty JointState message [ WARN] [1505624050.411322228, 32.100000000]: Cannot find planning configuration for group 'right_arm' using planner 'RRTConnectConfigDefault'. Will use defaults instead. [ERROR] [1505624050.411517311, 32.100000000]: Found empty JointState message [ INFO] [1505624050.411934909, 32.100000000]: No optimization objective specified, defaulting to PathLengthOptimizationObjective [ INFO] [1505624050.412700592, 32.101000000]: RRTConnect: Starting planning with 1 states already in datastructure [ INFO] [1505624050.455130622, 32.118000000]: RRTConnect: Created 5 states (2 start + 3 goal) [ INFO] [1505624050.455243431, 32.118000000]: Solution found in 0.043083 seconds [ INFO] [1505624050.657066247, 32.202000000]: SimpleSetup: Path simplification took 0.201707 seconds and changed from 4 to 37 states [ INFO] [1505624050.669940885, 32.206000000]: Found successful manipulation plan! [ INFO] [1505624050.670585663, 32.206000000]: Pickup planning completed after 1.653023 seconds [ WARN] [1505624050.673907676, 32.207000000]: Dropping first 1 trajectory point(s) out of 37, as they occur before the current time. First valid point will be reached in 0.145s. [ERROR] [1505624062.241798722, 37.308000000]: Trajectory message contains waypoints that are not strictly increasing in time. [ERROR] [1505624062.241922004, 37.308000000]: Trajectory message contains waypoints that are not strictly increasing in time. [ WARN] [1505624062.242281204, 37.308000000]: Controller ra_trajectory_controller failed with error code INVALID_GOAL [ WARN] [1505624062.242356336, 37.308000000]: Controller handle ra_trajectory_controller reports status FAILED [ WARN] [1505624062.242385875, 37.308000000]: Controller handle rh_trajectory_controller reports status RUNNING [ WARN] [1505624062.242419161, 37.308000000]: Controller rh_trajectory_controller failed with error code INVALID_GOAL

guihomework commented 7 years ago

Hi, at least with the more complete log, one can see that your controllers are running, but the trajectory are wrong for some reasons as these lines show

[ERROR] [1505624062.241798722, 37.308000000]: Trajectory message contains waypoints that are not strictly increasing in time.
[ERROR] [1505624062.241922004, 37.308000000]: Trajectory message contains waypoints that are not strictly increasing in time.

I am surprised by the name you give of the log sr_robot_launch In the tutorial I suggest to start the demo app with sr_multi_moveit_config

If you don't use the fake execution, then you differ from my tutorial (and my suggested fix will be of no help). You should look at the goal request sent to the ra_trajectory_controller and rh_trajectory_controller to see why it tells the waypoints do not increase in time. MoveIt! is responsible for generating trajectories. If they are wrong this is a bug in moveit but because I don't see that, it means your version of moveit has this bug. You might want to use a more recent version then.

guihomework / dexterous-manipulation-tutorial

Not Picking the Pen #11