Demo: WallGrasp not working

[joaobimbo]

When I try running the demo with a WallGrasp, I get the following:

soma@soma-iit:~$ rosrun ec_grasp_planner planner.py --grasp WallGrasp --ros_service_call --rviz --handarm RBOHand2Kuka
Received graph with 13 nodes and 20 edges.
States Explored: 49
Time per state: 0.224 ms
Plan length: 4
move_hand(l0, l1)
move_object(l1, l0)
move_object(l0, l9)
grasp_object(l9)
Press Cntrl-C to stop sending visualization_msgs/MarkerArray on topic '/planned_grasp_path' ...

And on the hybrid_automaton_manager_kuka this comes up:

$ rosrun hybrid_automaton_manager_kuka hybrid_automaton_manager_kuka 
[ INFO] [1508158199.870548399]: Ready to manage hybrid automatons for the Kuka system
[ForceTorqueSensor.deserialize] WARN: The attribute frame_id for the force torque sensor is not implemented! (/home/soma/code/hybrid-automaton-library/src/ForceTorqueSensor.cpp:83)
[ForceTorqueSensor.deserialize] WARN: The attribute frame_id for the force torque sensor is not implemented! (/home/soma/code/hybrid-automaton-library/src/ForceTorqueSensor.cpp:83)
[HybridAutomaton._activeCurrentControlMode] INFO: Current mode: Mode0 (/home/soma/code/hybrid-automaton-library/src/HybridAutomaton.cpp:393)
goal R:   0.00369 -0.999987  0.003451
 0.540212  0.004898  0.841514
-0.841521 -0.001241  0.540224
goal t: 0.659577
-0.350956
0.555792
[ INFO] [1508158207.489617050, 12.831000000]: Loading robot model 'iiwa7_kinect_ft'...
[ INFO] [1508158207.489660120, 12.831000000]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1508158207.677154455, 12.991000000]: Loading robot model 'iiwa7_kinect_ft'...
[ INFO] [1508158207.677185393, 12.991000000]: No root/virtual joint specified in SRDF. Assuming fixed joint
Initializing psm
[ INFO] [1508158207.753289275, 13.058000000]: Loading robot model 'iiwa7_kinect_ft'...
[ INFO] [1508158207.753411208, 13.058000000]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1508158207.899846895, 13.190000000]: Loading robot model 'iiwa7_kinect_ft'...
[ INFO] [1508158207.899877624, 13.190000000]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1508158208.231255007, 13.414000000]: Starting scene monitor
[ INFO] [1508158208.234788294, 13.419000000]: Listening to '/planning_scene'
[ INFO] [1508158208.234857747, 13.419000000]: Starting world geometry monitor
[ INFO] [1508158208.239650183, 13.424000000]: Listening to '/collision_object' using message notifier with target frame '/world '
[ INFO] [1508158208.244411157, 13.426000000]: Listening to '/planning_scene_world' for planning scene world geometry
[ WARN] [1508158208.246040981, 13.428000000]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[ INFO] [1508158208.395072720, 13.554000000]: Listening to '/attached_collision_object' for attached collision objects
Collision World Objects:
     box
     table

Attached Bodies:

iiwa_joint_1: -9.41122e-06
iiwa_joint_2: -1.37033e-06
iiwa_joint_3: -6.79043e-06
iiwa_joint_4: 2.61174e-06
iiwa_joint_5: 2.42811e-06
iiwa_joint_6: -1.27199e-06
iiwa_joint_7: -7.11956e-07
[ INFO] [1508158208.475601981, 13.628000000]: Initializing OMPL interface using ROS parameters
[ INFO] [1508158208.493743820, 13.649000000]: Using planning interface 'OMPL'
[ INFO] [1508158208.542680987, 13.697000000]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1508158208.543145870, 13.698000000]: Param 'start_state_max_bounds_error' was not set. Using default value: 0.05
[ INFO] [1508158208.543545993, 13.698000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1508158208.543980463, 13.699000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1508158208.544373796, 13.699000000]: Param 'jiggle_fraction' was not set. Using default value: 0.02
[ INFO] [1508158208.544772666, 13.700000000]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1508158208.544819678, 13.700000000]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1508158208.544833523, 13.700000000]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1508158208.544843613, 13.700000000]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1508158208.544856878, 13.700000000]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1508158208.544867150, 13.700000000]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1508158208.555213467, 13.712000000]: The timeout for planning must be positive (0.000000 specified). Assuming one second instead.
[ INFO] [1508158208.556048516, 13.713000000]: No planner specified. Using default.
[ INFO] [1508158208.557285898, 13.713000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ERROR] [1508158209.563971881, 14.631000000]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1508158209.564016302, 14.631000000]: RRTConnect: Created 1 states (1 start + 0 goal)
[ INFO] [1508158209.564033442, 14.631000000]: No solution found after 1.007750 seconds
[ INFO] [1508158209.589103511, 14.658000000]: Unable to solve the planning problem
[ERROR] [1508158209.589197774, 14.658000000]: Could not compute plan successfully
[ WARN] [1508158209.594011416, 14.663000000]: SEVERE WARNING!!! Attempting to unload library while objects created by this loader exist in the heap! You should delete your objects before attempting to unload the library or destroying the ClassLoader. The library will NOT be unloaded.

And the robot doesn't move.

[zweistein]

Hi,

We mentioned that the Kuka manager uses Moveit to plan tool space motion. As the error message says: [ERROR] [1508158209.563971881, 14.631000000]: RRTConnect: Unable to sample any valid states for goal tree -> the trajectory planner was not able to find a valid plan to reach the goal. The goal is one of the waypoints in the plan.

You can move the object in Gazebo around the table so you can overcome the limitations of the arm/planner: