crucial-vectorfield-fail.txt

$ grep -C4 vectorfield_planner, src/herbpy/src/herbpy/herbrobot.py
        actual_planner = Sequence(
            # First, try the straight-line trajectory.
            self.snap_planner,
            # Then, try a few simple (and fast!) heuristics.
            #self.vectorfield_planner,
            #self.greedyik_planner,
            # Next, try a trajectory optimizer.
            self.trajopt_planner or self.chomp_planner
        )
$ rosrun table_clearing run.py --perception-sim --viewer=none --hardcoded
[WARNING] [root:logger.py:71]:initialize_logging: Install termcolor to colorize log messages.
[ WARN] [1461259257.670606827]: Group 'head_hand' is empty.
[kinbody.cpp:1470 SetDOFValues] dof 4 value is not in limits 0.000000e+00<5.415927e-01
[kinbody.cpp:1470 SetDOFValues] dof 15 value is not in limits 0.000000e+00<5.415927e-01
[WARNING] [prpy.planning.base:base.py:168]:__init__: Please explicitly declare a ClonedPlanningMethod instead of using PlanningMethod.
[WARNING] [prpy.planning.base:base.py:168]:__init__: Please explicitly declare a ClonedPlanningMethod instead of using PlanningMethod.
[WARNING] [prpy.planning.base:base.py:168]:__init__: Please explicitly declare a ClonedPlanningMethod instead of using PlanningMethod.
[WARNING] [prpy.planning.base:base.py:168]:__init__: Please explicitly declare a ClonedPlanningMethod instead of using PlanningMethod.
[WARNING] [root:logger.py:71]:initialize_logging: Install termcolor to colorize log messages.
[INFO] [__main__:run.py:136]:<module>: Executing trial 1 of 1
[INFO] [hgpc.actions.Sequence:Sequence.py:119]:_plan_recursive: Planning action MovePlateToTableEdge
[INFO] [table_clearing.PlateActions:PlateActions.py:83]:_compute_plate_slide_attributes: Distance to edge: 0
[INFO] [table_clearing.PlateActions:PlateActions.py:84]:_compute_plate_slide_attributes: Direction (table frame): [0, 0, -1]
[INFO] [table_clearing.PlateActions:PlateActions.py:87]:_compute_plate_slide_attributes: Direction (world frame): [-1.  0.  0.]
[INFO] [hgpc.actions.Sequence:Sequence.py:119]:_plan_recursive: Planning action OpenHandForPlateSlide
[INFO] [hgpc.actions.Sequence:Sequence.py:119]:_plan_recursive: Planning action MoveNearPlateAction
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "Sequence(SnapPlanner, Trajopt)".
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "SnapPlanner".
[WARNING] [prpy.planning.base:base.py:313]:plan: Planning with SnapPlanner failed: Detected collision: <table, padding_conference_table> x <herb, /right/finger1_2>.
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "Trajopt".
[INFO] [prpy.planning.base:base.py:305]:plan: Sequence - Planning succeeded after 0.861 seconds with "Trajopt".
[INFO] [prpy.planning.base:base.py:305]:plan: Sequence - Planning succeeded after 0.905 seconds with "Sequence(SnapPlanner, Trajopt)".
[INFO] [planning_metrics:logging_utils.py:34]:_log_data: P MoveNearPlateAction 0.905719041824 TrajoptPlanner PlanToEndEffectorPose
[INFO] [hgpc.actions.Sequence:Sequence.py:119]:_plan_recursive: Planning action TouchPlateAction
/home/cdellin/ws-tableclearing/src/table_clearing/src/hgpc/actions/MoveUntilTouch.py:172: DeprecationWarning: With-statements now directly support multiple context managers
  with nested(*ssavers),\
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "CBiRRT".
[openravepy_robot.cpp:901 GetActiveManipulatorIndex] GetActiveManipulatorIndex is deprecated
[openravepy_robot.cpp:901 GetActiveManipulatorIndex] GetActiveManipulatorIndex is deprecated
[GeneralIK.cpp:58 Init] Initializing GeneralIK Solver
[cbirrtproblem.cpp:112 main] IKsolver initialized
[cbirrtproblem.cpp:771 RunCBirrt] Doing Goal sampling
[cbirrtproblem.cpp:843 RunCBirrt] Setting default init config to current config
[TaskSpaceRegion.cpp:1031 RobotizeTSRChain] This is a point TSR, no robotized TSR needed
[GeneralIK.cpp:58 Init] Initializing GeneralIK Solver
[cbirrt.cpp:260 InitPlan] numdofs: 7
[cbirrt.cpp:305 InitPlan] grabbed: 0
[GeneralIK.cpp:58 Init] Initializing GeneralIK Solver
[cbirrt.cpp:314 InitPlan] Psample: 0.100000
[cbirrt.cpp:392 InitPlan] Checking for start projection...
[cbirrt.cpp:427 InitPlan] Start Node(s) Created
[cbirrt.cpp:860 AddRootConfiguration] Attached Solver Found a Solution!
[cbirrt.cpp:946 AddRootConfiguration] Constrained IK solution(s) found!
[cbirrt.cpp:622 PlanPath] Got first goal ik solution!
[cbirrt.cpp:746 PlanPath] Planning time: 0.454758s
[cbirrt.cpp:1183 _OptimizePath] Smoothing...
[cbirrt.cpp:1240 _OptimizePath] Done
[INFO] [prpy.planning.base:base.py:305]:plan: Sequence - Planning succeeded after 0.773 seconds with "CBiRRT".
[INFO] [hgpc.actions.Sequence:Sequence.py:119]:_plan_recursive: Planning action GrabPlateAction
/home/cdellin/ws-tableclearing/src/table_clearing/src/hgpc/actions/base.py:22: DeprecationWarning: With-statements now directly support multiple context managers
  self.cm.__enter__()
/home/cdellin/ws-tableclearing/src/table_clearing/src/hgpc/actions/MoveHand.py:132: RuntimeWarning: invalid value encountered in divide
  durations = (q_goal - q_prev) / qd
[INFO] [hgpc.actions.Sequence:Sequence.py:119]:_plan_recursive: Planning action SlidePlateToTableEdgeAction
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "CBiRRT".
[openravepy_robot.cpp:901 GetActiveManipulatorIndex] GetActiveManipulatorIndex is deprecated
[openravepy_robot.cpp:901 GetActiveManipulatorIndex] GetActiveManipulatorIndex is deprecated
[GeneralIK.cpp:58 Init] Initializing GeneralIK Solver
[cbirrtproblem.cpp:112 main] IKsolver initialized
[cbirrtproblem.cpp:771 RunCBirrt] Doing Goal sampling
[cbirrtproblem.cpp:843 RunCBirrt] Setting default init config to current config
[TaskSpaceRegion.cpp:1031 RobotizeTSRChain] This is a point TSR, no robotized TSR needed
[GeneralIK.cpp:58 Init] Initializing GeneralIK Solver
[cbirrt.cpp:260 InitPlan] numdofs: 7
[cbirrt.cpp:305 InitPlan] grabbed: 0
[GeneralIK.cpp:58 Init] Initializing GeneralIK Solver
[cbirrt.cpp:314 InitPlan] Psample: 0.100000
[cbirrt.cpp:392 InitPlan] Checking for start projection...
[cbirrt.cpp:427 InitPlan] Start Node(s) Created
[cbirrt.cpp:860 AddRootConfiguration] Attached Solver Found a Solution!
[cbirrt.cpp:946 AddRootConfiguration] Constrained IK solution(s) found!
[cbirrt.cpp:622 PlanPath] Got first goal ik solution!
[cbirrt.cpp:746 PlanPath] Planning time: 0.148630s
[cbirrt.cpp:1183 _OptimizePath] Smoothing...
[cbirrt.cpp:1240 _OptimizePath] Done
[INFO] [prpy.planning.base:base.py:305]:plan: Sequence - Planning succeeded after 0.305 seconds with "CBiRRT".
[INFO] [planning_metrics:logging_utils.py:34]:_log_data: P SlidePlateToTableEdgeAction 0.30511713028 CBiRRTPlanner PlanToEndEffectorOffset
[INFO] [hgpc.actions.Sequence:Sequence.py:119]:_plan_recursive: Planning action ReleasePlateAction
/home/cdellin/ws-tableclearing/src/table_clearing/src/hgpc/actions/base.py:22: DeprecationWarning: With-statements now directly support multiple context managers
  self.cm.__enter__()
[INFO] [hgpc.actions.Sequence:Sequence.py:119]:_plan_recursive: Planning action MoveAwayFromPlateAction
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "CBiRRT".
[openravepy_robot.cpp:901 GetActiveManipulatorIndex] GetActiveManipulatorIndex is deprecated
[openravepy_robot.cpp:901 GetActiveManipulatorIndex] GetActiveManipulatorIndex is deprecated
[GeneralIK.cpp:58 Init] Initializing GeneralIK Solver
[cbirrtproblem.cpp:112 main] IKsolver initialized
[cbirrtproblem.cpp:771 RunCBirrt] Doing Goal sampling
[cbirrtproblem.cpp:843 RunCBirrt] Setting default init config to current config
[TaskSpaceRegion.cpp:1031 RobotizeTSRChain] This is a point TSR, no robotized TSR needed
[GeneralIK.cpp:58 Init] Initializing GeneralIK Solver
[cbirrt.cpp:260 InitPlan] numdofs: 7
[cbirrt.cpp:305 InitPlan] grabbed: 0
[GeneralIK.cpp:58 Init] Initializing GeneralIK Solver
[cbirrt.cpp:314 InitPlan] Psample: 0.100000
[cbirrt.cpp:392 InitPlan] Checking for start projection...
[cbirrt.cpp:427 InitPlan] Start Node(s) Created
[cbirrt.cpp:860 AddRootConfiguration] Attached Solver Found a Solution!
[cbirrt.cpp:946 AddRootConfiguration] Constrained IK solution(s) found!
[cbirrt.cpp:622 PlanPath] Got first goal ik solution!
[cbirrt.cpp:746 PlanPath] Planning time: 0.474179s
[cbirrt.cpp:1183 _OptimizePath] Smoothing...
[cbirrt.cpp:1240 _OptimizePath] Done
[INFO] [prpy.planning.base:base.py:305]:plan: Sequence - Planning succeeded after 0.688 seconds with "CBiRRT".
[INFO] [planning_metrics:logging_utils.py:34]:_log_data: P MoveAwayFromPlateAction 0.688264846802 CBiRRTPlanner PlanToEndEffectorOffset
/home/cdellin/ws-tableclearing/src/table_clearing/src/hgpc/actions/Sequence.py:121: DeprecationWarning: With-statements now directly support multiple context managers
  with solution.save_and_jump(env):
/home/cdellin/ws-tableclearing/src/table_clearing/src/hgpc/actions/base.py:22: DeprecationWarning: With-statements now directly support multiple context managers
  self.cm.__enter__()
/home/cdellin/ws-tableclearing/src/table_clearing/src/hgpc/actions/base.py:22: DeprecationWarning: With-statements now directly support multiple context managers
  self.cm.__enter__()
[INFO] [hgpc.actions.Sequence:Sequence.py:119]:_plan_recursive: Planning action GrabPlateActionSequence
[INFO] [hgpc.actions.Sequence:Sequence.py:119]:_plan_recursive: Planning action OpenHandForPlateGrabAction
[INFO] [hgpc.actions.Sequence:Sequence.py:119]:_plan_recursive: Planning action PlanToPoseNearPlateAction
[openravepy_robot.cpp:901 GetActiveManipulatorIndex] GetActiveManipulatorIndex is deprecated
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "TSRPlanner(Sequence(SnapPlanner, Trajopt))".
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "SnapPlanner".
[WARNING] [prpy.planning.base:base.py:313]:plan: Planning with SnapPlanner failed: Detected collision: <table, padding_conference_table> x <herb, /right/hand_base>.
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "Trajopt".
[INFO] [prpy.planning.base:base.py:305]:plan: Sequence - Planning succeeded after 0.713 seconds with "Trajopt".
[INFO] [prpy.planning.tsr:tsr.py:171]:PlanToTSR: Planned to IK solution set 1 of 3.
[INFO] [prpy.planning.base:base.py:305]:plan: Sequence - Planning succeeded after 2.819 seconds with "TSRPlanner(Sequence(SnapPlanner, Trajopt))".
[INFO] [planning_metrics:logging_utils.py:34]:_log_data: P PlanToPoseNearPlateAction 2.81908202171 TrajoptPlanner PlanToConfiguration
[INFO] [hgpc.actions.Sequence:Sequence.py:119]:_plan_recursive: Planning action MoveToPlateGraspPreshapeAction
[INFO] [hgpc.actions.Sequence:Sequence.py:119]:_plan_recursive: Planning action GrabAndVerifyPlateAction
/home/cdellin/ws-tableclearing/src/table_clearing/src/hgpc/actions/base.py:22: DeprecationWarning: With-statements now directly support multiple context managers
  self.cm.__enter__()
[INFO] [hgpc.actions.Sequence:Sequence.py:119]:_plan_recursive: Planning action LiftPlateOffTableAction
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "CBiRRT".
[openravepy_robot.cpp:901 GetActiveManipulatorIndex] GetActiveManipulatorIndex is deprecated
[openravepy_robot.cpp:901 GetActiveManipulatorIndex] GetActiveManipulatorIndex is deprecated
[GeneralIK.cpp:58 Init] Initializing GeneralIK Solver
[cbirrtproblem.cpp:112 main] IKsolver initialized
[cbirrtproblem.cpp:771 RunCBirrt] Doing Goal sampling
[cbirrtproblem.cpp:843 RunCBirrt] Setting default init config to current config
[TaskSpaceRegion.cpp:1031 RobotizeTSRChain] This is a point TSR, no robotized TSR needed
[GeneralIK.cpp:58 Init] Initializing GeneralIK Solver
[cbirrt.cpp:260 InitPlan] numdofs: 7
[cbirrt.cpp:305 InitPlan] grabbed: 0
[GeneralIK.cpp:58 Init] Initializing GeneralIK Solver
[cbirrt.cpp:314 InitPlan] Psample: 0.100000
[cbirrt.cpp:392 InitPlan] Checking for start projection...
[cbirrt.cpp:427 InitPlan] Start Node(s) Created
[cbirrt.cpp:860 AddRootConfiguration] Attached Solver Found a Solution!
[cbirrt.cpp:946 AddRootConfiguration] Constrained IK solution(s) found!
[cbirrt.cpp:622 PlanPath] Got first goal ik solution!
[cbirrt.cpp:746 PlanPath] Planning time: 0.067231s
[cbirrt.cpp:1183 _OptimizePath] Smoothing...
[cbirrt.cpp:1240 _OptimizePath] Done
[INFO] [prpy.planning.base:base.py:305]:plan: Sequence - Planning succeeded after 0.105 seconds with "CBiRRT".
[INFO] [planning_metrics:logging_utils.py:34]:_log_data: P LiftPlateOffTableAction 0.105087041855 CBiRRTPlanner PlanToEndEffectorOffset
/home/cdellin/ws-tableclearing/src/table_clearing/src/hgpc/actions/Sequence.py:121: DeprecationWarning: With-statements now directly support multiple context managers
  with solution.save_and_jump(env):
/home/cdellin/ws-tableclearing/src/table_clearing/src/hgpc/actions/base.py:22: DeprecationWarning: With-statements now directly support multiple context managers
  self.cm.__enter__()
[INFO] [hgpc.actions.Sequence:Sequence.py:119]:_plan_recursive: Planning action PlacePlateInTrayActionSequence
[INFO] [hgpc.actions.Sequence:Sequence.py:119]:_plan_recursive: Planning action MovePlateOverTrayAction
[openravepy_robot.cpp:901 GetActiveManipulatorIndex] GetActiveManipulatorIndex is deprecated
[openravepy_robot.cpp:901 GetActiveManipulatorIndex] GetActiveManipulatorIndex is deprecated
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "TSRPlanner(Sequence(SnapPlanner, Trajopt))".
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "SnapPlanner".
[WARNING] [prpy.planning.base:base.py:313]:plan: Planning with SnapPlanner failed: Detected collision: <herb, /right/finger2_2> x <table, padding_conference_table>.
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "Trajopt".
[WARNING] [prpy.planning.base:base.py:313]:plan: Planning with Trajopt failed: Detected collision: <herb, /right/finger2_2> x <table, padding_conference_table>.
[WARNING] [prpy.planning.tsr:tsr.py:176]:PlanToTSR: Planning to IK solution set 1 of 3 failed: All planners failed.
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "SnapPlanner".
[WARNING] [prpy.planning.base:base.py:313]:plan: Planning with SnapPlanner failed: Detected collision: <herb, /right/finger2_2> x <table, padding_conference_table>.
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "Trajopt".
[WARNING] [prpy.planning.base:base.py:313]:plan: Planning with Trajopt failed: Detected collision: <herb, /right/finger2_2> x <table, padding_conference_table>.
[WARNING] [prpy.planning.tsr:tsr.py:176]:PlanToTSR: Planning to IK solution set 2 of 3 failed: All planners failed.
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "SnapPlanner".
[WARNING] [prpy.planning.base:base.py:313]:plan: Planning with SnapPlanner failed: Detected collision: <herb, /right/finger2_2> x <table, padding_conference_table>.
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "Trajopt".
[WARNING] [prpy.planning.base:base.py:313]:plan: Planning with Trajopt failed: Detected collision: <herb, /right/finger2_2> x <table, padding_conference_table>.
[WARNING] [prpy.planning.tsr:tsr.py:176]:PlanToTSR: Planning to IK solution set 3 of 3 failed: All planners failed.
[WARNING] [prpy.planning.base:base.py:313]:plan: Planning with TSRPlanner(Sequence(SnapPlanner, Trajopt)) failed: Planning to the top 3 of 337 IK solution sets failed.
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "CBiRRT".
[GeneralIK.cpp:58 Init] Initializing GeneralIK Solver
[cbirrtproblem.cpp:112 main] IKsolver initialized
[cbirrtproblem.cpp:771 RunCBirrt] Doing Goal sampling
[cbirrtproblem.cpp:843 RunCBirrt] Setting default init config to current config
[GeneralIK.cpp:58 Init] Initializing GeneralIK Solver
[cbirrt.cpp:260 InitPlan] numdofs: 7
[cbirrt.cpp:305 InitPlan] grabbed: 0
[GeneralIK.cpp:58 Init] Initializing GeneralIK Solver
[cbirrt.cpp:314 InitPlan] Psample: 0.100000
[cbirrt.cpp:376 InitPlan] CBirrtPlanner::InitPlan - Error: Start configuration in collision:
[cbirrt.cpp:380 InitPlan] Start 0: 4.81278 -1.62922 -0.2 2.2575 -2.43766 -0.118599 -2.4869 
[cbirrtproblem.cpp:892 RunCBirrt] InitPlan failed
[WARNING] [prpy.planning.base:base.py:313]:plan: Planning with CBiRRT failed: Unknown error: 0 InitPlan failed
Error: Start configuration in collision

[INFO] [__main__:run.py:197]:<module>: Failed to execute task: All planners failed.
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "SnapPlanner".
[INFO] [prpy.planning.base:base.py:305]:plan: Sequence - Planning succeeded after 0.416 seconds with "SnapPlanner".
[INFO] [planning_metrics:logging_utils.py:34]:_log_data: P RecoveryHomeAction 0.440494060516 SnapPlanner PlanToConfiguration
[WARNING] [prpy.base.robot:robot.py:352]:PostProcessPath: Post-processing smooth paths is not supported. Using the default post-processing logic; this may significantly change the geometric path.
[ParabolicSmoother.cpp:389 PlanPath] Terminated after 0 iterations and -1.000000 seconds with 1 ramps.
[INFO] [planning_metrics:logging_utils.py:34]:_log_data: S RecoveryHomeAction 0.0869250297546 HauserParabolicSmoother RetimeTrajectory
[INFO] [planning_metrics:logging_utils.py:34]:_log_data: E RecoveryHomeAction 3.11013388634
[INFO] [prpy.planning.base:base.py:297]:plan: Sequence - Calling planner "SnapPlanner".
[INFO] [prpy.planning.base:base.py:305]:plan: Sequence - Planning succeeded after 0.301 seconds with "SnapPlanner".
[INFO] [planning_metrics:logging_utils.py:34]:_log_data: P RecoveryHomeAction 0.31927895546 SnapPlanner PlanToConfiguration
[WARNING] [prpy.base.robot:robot.py:352]:PostProcessPath: Post-processing smooth paths is not supported. Using the default post-processing logic; this may significantly change the geometric path.
[ParabolicSmoother.cpp:389 PlanPath] Terminated after 0 iterations and -1.000000 seconds with 1 ramps.
[INFO] [planning_metrics:logging_utils.py:34]:_log_data: S RecoveryHomeAction 0.0443840026855 HauserParabolicSmoother RetimeTrajectory
[INFO] [planning_metrics:logging_utils.py:34]:_log_data: E RecoveryHomeAction 3.05877590179
[INFO] [planning_metrics:run.py:228]:<module>: T 0 17.1320798397 False All planners failed.
[INFO] [__main__:run.py:232]:<module>: Successfully completed 0 of 1 trials
/usr/lib/python2.7/dist-packages/IPython/frontend/terminal/embed.py:239: DeprecationWarning: With-statements now directly support multiple context managers
  with nested(self.builtin_trap, self.display_trap):
Python 2.7.3 (default, Mar 13 2014, 11:03:55) 
Type "copyright", "credits" or "license" for more information.

IPython 0.13.1 -- An enhanced Interactive Python.
?         -> Introduction and overview of IPython's features.
%quickref -> Quick reference.
help      -> Python's own help system.
object?   -> Details about 'object', use 'object??' for extra details.

In [1]: 
Do you really want to exit ([y]/n)? 

[INFO] [rospy.core:core.py:387]:signal_shutdown: signal_shutdown [atexit]
$ cat trial_20160421_1020.log 
20160421 10:21:00 P MoveNearPlateAction 0.905719041824 TrajoptPlanner PlanToEndEffectorPose
20160421 10:21:01 P SlidePlateToTableEdgeAction 0.30511713028 CBiRRTPlanner PlanToEndEffectorOffset
20160421 10:21:02 P MoveAwayFromPlateAction 0.688264846802 CBiRRTPlanner PlanToEndEffectorOffset
20160421 10:21:04 P PlanToPoseNearPlateAction 2.81908202171 TrajoptPlanner PlanToConfiguration
20160421 10:21:05 P LiftPlateOffTableAction 0.105087041855 CBiRRTPlanner PlanToEndEffectorOffset
20160421 10:21:09 P RecoveryHomeAction 0.440494060516 SnapPlanner PlanToConfiguration
20160421 10:21:09 S RecoveryHomeAction 0.0869250297546 HauserParabolicSmoother RetimeTrajectory
20160421 10:21:12 E RecoveryHomeAction 3.11013388634
20160421 10:21:12 P RecoveryHomeAction 0.31927895546 SnapPlanner PlanToConfiguration
20160421 10:21:12 S RecoveryHomeAction 0.0443840026855 HauserParabolicSmoother RetimeTrajectory
20160421 10:21:15 E RecoveryHomeAction 3.05877590179
20160421 10:21:15 T 0 17.1320798397 False All planners failed.