[demo.py] the robot keeps rotating after moved near table #30

umhan35 · 2019-02-14T14:16:54Z

Hi, I am using Ubuntu 18.04 and ROS Melodic (to align with the version that the FetchIt robot has).

I followed the gazabo tutorial (https://docs.fetchrobotics.com/gazebo.html) and run the following:

roslaunch fetch_gazebo playground.launch
roslaunch fetch_gazebo_demo demo.launch

I expect the robot will move to the table and pick the blue cube, but after the robot moved near table keeps rotating it self. During 2 times, I waited 10 minutes before "Aborting because a valid control could not be found. Even after executing all recovery behaviors"

Rotating:

Here is the excerpt of the log:

[INFO] [1550152825.917689, 39.862000]: Waiting for basic_grasping_perception/find_objects...
[INFO] [1550152826.167008, 39.894000]: Moving to table...
[ WARN] [1550152935.337158025, 67.535000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550152980.132707802, 77.615000000]: Rotate recovery behavior started.
[ WARN] [1550153058.423329389, 95.251000000]: Invalid Trajectory 0.093694, 0.000000, 0.025957, cost: -1.000000
[ WARN] [1550153093.262487593, 103.608000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550153134.243491509, 113.969000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550153178.759782706, 125.168000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550153254.751576573, 143.775000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550153327.259470285, 162.218000000]: Rotate recovery behavior started.
[ WARN] [1550153366.278706368, 172.399000000]: Clearing costmap to unstuck robot (0.500000m).
[ERROR] [1550153366.753323531, 172.518000000]: Aborting because a valid control could not be found. Even after executing all recovery behaviors
[INFO] [1550153366.785093, 172.526000]: Raising torso...
Get block to pick...
[INFO] [1550153391.572698, 178.801000]: Picking object...
[WARN] [1550153392.673143, 178.988000]: ObjectManager: object0 not added yet
[WARN] [1550153392.681189, 178.992000]: ObjectManager: object1 not added yet
[WARN] [1550153392.688479, 178.993000]: ObjectManager: object2 not added yet
[WARN] [1550153392.695956, 178.993000]: ObjectManager: surface0 not added yet
[WARN] [1550153392.702579, 178.996000]: ObjectManager: surface1 not added yet
[INFO] [1550153393.662212, 179.207000]: Beginning to pick.
[ INFO] [1550153393.710886642, 179.216000000]: Planning attempt 1 of at most 1

Here is the full log:

$ roslaunch fetch_gazebo_demo demo.launch
... logging to /home/zhao/.ros/log/968350b0-3060-11e9-8d6a-04d3b0082825/roslaunch-zhao1804-15024.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://zhao1804:45795/

SUMMARY
========

PARAMETERS
 * /amcl/base_frame_id: base_link
 * /amcl/global_frame_id: map
 * /amcl/gui_publish_rate: 10.0
 * /amcl/kld_err: 0.01
 * /amcl/kld_z: 0.99
 * /amcl/laser_likelihood_max_dist: 2.0
 * /amcl/laser_max_beams: 30
 * /amcl/laser_max_range: -1.0
 * /amcl/laser_min_range: -1.0
 * /amcl/laser_model_type: likelihood_field
 * /amcl/laser_sigma_hit: 0.2
 * /amcl/laser_z_hit: 0.95
 * /amcl/laser_z_rand: 0.05
 * /amcl/max_particles: 2000
 * /amcl/min_particles: 500
 * /amcl/odom_alpha1: 0.15
 * /amcl/odom_alpha2: 0.5
 * /amcl/odom_alpha3: 0.5
 * /amcl/odom_alpha4: 0.15
 * /amcl/odom_frame_id: odom
 * /amcl/odom_model_type: diff-corrected
 * /amcl/recovery_alpha_fast: 0.0
 * /amcl/recovery_alpha_slow: 0.0
 * /amcl/resample_interval: 2
 * /amcl/save_pose_rate: 0.5
 * /amcl/transform_tolerance: 0.5
 * /amcl/update_min_a: 0.25
 * /amcl/update_min_d: 0.1
 * /amcl/use_map_topic: False
 * /basic_grasping_perception/gripper/approach/desired: 0.15
 * /basic_grasping_perception/gripper/approach/min: 0.145
 * /basic_grasping_perception/gripper/finger_depth: 0.02
 * /basic_grasping_perception/gripper/gripper_tolerance: 0.05
 * /basic_grasping_perception/gripper/retreat/desired: 0.15
 * /basic_grasping_perception/gripper/retreat/min: 0.145
 * /basic_grasping_perception/gripper/tool_to_planning_frame: 0.18
 * /move_base/NavfnROS/allow_unknown: True
 * /move_base/NavfnROS/default_tolerance: 0.0
 * /move_base/NavfnROS/planner_window_x: 0.0
 * /move_base/NavfnROS/planner_window_y: 0.0
 * /move_base/NavfnROS/visualize_potential: False
 * /move_base/TrajectoryPlannerROS/acc_lim_theta: 0.8
 * /move_base/TrajectoryPlannerROS/acc_lim_x: 0.35
 * /move_base/TrajectoryPlannerROS/acc_lim_y: 0.0
 * /move_base/TrajectoryPlannerROS/angular_sim_granularity: 0.025
 * /move_base/TrajectoryPlannerROS/dwa: False
 * /move_base/TrajectoryPlannerROS/gdist_scale: 12.0
 * /move_base/TrajectoryPlannerROS/heading_lookahead: 0.325
 * /move_base/TrajectoryPlannerROS/heading_scoring: True
 * /move_base/TrajectoryPlannerROS/heading_scoring_timestep: 0.8
 * /move_base/TrajectoryPlannerROS/holonomic_robot: False
 * /move_base/TrajectoryPlannerROS/latch_xy_goal_tolerance: True
 * /move_base/TrajectoryPlannerROS/max_vel_theta: 1.5
 * /move_base/TrajectoryPlannerROS/max_vel_x: 1.0
 * /move_base/TrajectoryPlannerROS/max_vel_y: 0.0
 * /move_base/TrajectoryPlannerROS/meter_scoring: True
 * /move_base/TrajectoryPlannerROS/min_in_place_vel_theta: 0.3
 * /move_base/TrajectoryPlannerROS/min_vel_theta: -1.5
 * /move_base/TrajectoryPlannerROS/min_vel_x: 0.15
 * /move_base/TrajectoryPlannerROS/min_vel_y: 0.0
 * /move_base/TrajectoryPlannerROS/occdist_scale: 0.1
 * /move_base/TrajectoryPlannerROS/oscillation_reset_dist: 0.05
 * /move_base/TrajectoryPlannerROS/pdist_scale: 8.0
 * /move_base/TrajectoryPlannerROS/publish_cost_grid_pc: False
 * /move_base/TrajectoryPlannerROS/sim_granularity: 0.025
 * /move_base/TrajectoryPlannerROS/sim_time: 1.0
 * /move_base/TrajectoryPlannerROS/vtheta_samples: 10
 * /move_base/TrajectoryPlannerROS/vx_samples: 3
 * /move_base/TrajectoryPlannerROS/xy_goal_tolerance: 0.1
 * /move_base/TrajectoryPlannerROS/yaw_goal_tolerance: 0.1
 * /move_base/aggressive_reset/reset_distance: 0.5
 * /move_base/base_global_planner: navfn/NavfnROS
 * /move_base/base_local_planner: base_local_planne...
 * /move_base/conservative_reset/reset_distance: 3.0
 * /move_base/controller_frequency: 25.0
 * /move_base/controller_patience: 15.0
 * /move_base/global_costmap/global_frame: map
 * /move_base/global_costmap/inflater/inflation_radius: 0.7
 * /move_base/global_costmap/inflater/robot_radius: 0.3
 * /move_base/global_costmap/obstacles/base_scan/clearing: True
 * /move_base/global_costmap/obstacles/base_scan/data_type: LaserScan
 * /move_base/global_costmap/obstacles/base_scan/marking: True
 * /move_base/global_costmap/obstacles/base_scan/max_obstacle_height: 0.3
 * /move_base/global_costmap/obstacles/base_scan/min_obstacle_height: 0.25
 * /move_base/global_costmap/obstacles/base_scan/obstacle_range: 4.0
 * /move_base/global_costmap/obstacles/base_scan/raytrace_range: 5.0
 * /move_base/global_costmap/obstacles/base_scan/topic: base_scan
 * /move_base/global_costmap/obstacles/max_obstacle_height: 2.0
 * /move_base/global_costmap/obstacles/observation_sources: base_scan
 * /move_base/global_costmap/obstacles/z_resolution: 0.125
 * /move_base/global_costmap/obstacles/z_voxels: 16
 * /move_base/global_costmap/plugins: [{'type': 'costma...
 * /move_base/global_costmap/publish_frequency: 0.0
 * /move_base/global_costmap/robot_base_frame: base_link
 * /move_base/global_costmap/robot_radius: 0.3
 * /move_base/global_costmap/static_map: True
 * /move_base/global_costmap/transform_tolerance: 0.5
 * /move_base/global_costmap/update_frequency: 5.0
 * /move_base/local_costmap/global_frame: odom
 * /move_base/local_costmap/height: 4.0
 * /move_base/local_costmap/inflater/inflation_radius: 0.7
 * /move_base/local_costmap/inflater/robot_radius: 0.3
 * /move_base/local_costmap/obstacles/base_scan/clearing: True
 * /move_base/local_costmap/obstacles/base_scan/data_type: LaserScan
 * /move_base/local_costmap/obstacles/base_scan/marking: True
 * /move_base/local_costmap/obstacles/base_scan/max_obstacle_height: 0.3
 * /move_base/local_costmap/obstacles/base_scan/min_obstacle_height: 0.25
 * /move_base/local_costmap/obstacles/base_scan/obstacle_range: 4.0
 * /move_base/local_costmap/obstacles/base_scan/raytrace_range: 5.0
 * /move_base/local_costmap/obstacles/base_scan/topic: base_scan
 * /move_base/local_costmap/obstacles/max_obstacle_height: 2.0
 * /move_base/local_costmap/obstacles/observation_sources: base_scan
 * /move_base/local_costmap/obstacles/publish_observations: False
 * /move_base/local_costmap/obstacles/z_resolution: 0.125
 * /move_base/local_costmap/obstacles/z_voxels: 16
 * /move_base/local_costmap/plugins: [{'type': 'costma...
 * /move_base/local_costmap/publish_frequency: 2.0
 * /move_base/local_costmap/resolution: 0.025
 * /move_base/local_costmap/robot_base_frame: base_link
 * /move_base/local_costmap/robot_radius: 0.3
 * /move_base/local_costmap/rolling_window: True
 * /move_base/local_costmap/transform_tolerance: 0.5
 * /move_base/local_costmap/width: 4.0
 * /move_base/oscillation_distance: 0.5
 * /move_base/oscillation_timeout: 10.0
 * /move_base/planner_frequency: 0.0
 * /move_base/planner_patience: 5.0
 * /move_base/recovery_behavior_enabled: True
 * /move_base/recovery_behaviors: [{'type': 'clear_...
 * /move_base/rotate_recovery/frequency: 20.0
 * /move_base/rotate_recovery/sim_granularity: 0.017
 * /move_group/allow_trajectory_execution: True
 * /move_group/allowed_execution_duration_scaling: 1.2
 * /move_group/allowed_goal_duration_margin: 0.5
 * /move_group/arm/longest_valid_segment_fraction: 0.05
 * /move_group/arm/planner_configs: ['SBLkConfigDefau...
 * /move_group/arm/projection_evaluator: joints(shoulder_p...
 * /move_group/arm_with_torso/longest_valid_segment_fraction: 0.05
 * /move_group/arm_with_torso/planner_configs: ['SBLkConfigDefau...
 * /move_group/arm_with_torso/projection_evaluator: joints(torso_lift...
 * /move_group/capabilities: move_group/MoveGr...
 * /move_group/controller_list: [{'default': True...
 * /move_group/gripper/planner_configs: ['SBLkConfigDefau...
 * /move_group/jiggle_fraction: 0.05
 * /move_group/max_safe_path_cost: 1
 * /move_group/moveit_controller_manager: moveit_simple_con...
 * /move_group/moveit_manage_controllers: True
 * /move_group/planner_configs/BKPIECEkConfigDefault/type: geometric::BKPIECE
 * /move_group/planner_configs/ESTkConfigDefault/type: geometric::EST
 * /move_group/planner_configs/KPIECEkConfigDefault/type: geometric::KPIECE
 * /move_group/planner_configs/LBKPIECEkConfigDefault/type: geometric::LBKPIECE
 * /move_group/planner_configs/PRMkConfigDefault/type: geometric::PRM
 * /move_group/planner_configs/PRMstarkConfigDefault/type: geometric::PRMstar
 * /move_group/planner_configs/RRTConnectkConfigDefault/type: geometric::RRTCon...
 * /move_group/planner_configs/RRTkConfigDefault/type: geometric::RRT
 * /move_group/planner_configs/RRTstarkConfigDefault/type: geometric::RRTstar
 * /move_group/planner_configs/SBLkConfigDefault/type: geometric::SBL
 * /move_group/planner_configs/TRRTkConfigDefault/type: geometric::TRRT
 * /move_group/planning_plugin: ompl_interface/OM...
 * /move_group/planning_scene_monitor/publish_geometry_updates: True
 * /move_group/planning_scene_monitor/publish_planning_scene: True
 * /move_group/planning_scene_monitor/publish_state_updates: True
 * /move_group/planning_scene_monitor/publish_transforms_updates: True
 * /move_group/request_adapters: default_planner_r...
 * /move_group/start_state_max_bounds_error: 0.1
 * /robot_description_kinematics/arm/kinematics_solver: fetch_arm_kinemat...
 * /robot_description_kinematics/arm/kinematics_solver_attempts: 3
 * /robot_description_kinematics/arm/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/arm/kinematics_solver_timeout: 0.005
 * /robot_description_kinematics/arm_with_torso/kinematics_solver: kdl_kinematics_pl...
 * /robot_description_kinematics/arm_with_torso/kinematics_solver_attempts: 3
 * /robot_description_kinematics/arm_with_torso/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/arm_with_torso/kinematics_solver_timeout: 0.005
 * /robot_description_planning/joint_limits/elbow_flex_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/elbow_flex_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/elbow_flex_joint/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/elbow_flex_joint/max_velocity: 1.5
 * /robot_description_planning/joint_limits/forearm_roll_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/forearm_roll_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/forearm_roll_joint/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/forearm_roll_joint/max_velocity: 1.57
 * /robot_description_planning/joint_limits/left_gripper_joint/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/left_gripper_joint/has_velocity_limits: False
 * /robot_description_planning/joint_limits/left_gripper_joint/max_acceleration: 0
 * /robot_description_planning/joint_limits/left_gripper_joint/max_velocity: 0
 * /robot_description_planning/joint_limits/right_gripper_joint/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/right_gripper_joint/has_velocity_limits: False
 * /robot_description_planning/joint_limits/right_gripper_joint/max_acceleration: 0
 * /robot_description_planning/joint_limits/right_gripper_joint/max_velocity: 0
 * /robot_description_planning/joint_limits/shoulder_lift_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/shoulder_lift_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/shoulder_lift_joint/max_acceleration: 1.0
 * /robot_description_planning/joint_limits/shoulder_lift_joint/max_velocity: 1.0
 * /robot_description_planning/joint_limits/shoulder_pan_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/shoulder_pan_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/shoulder_pan_joint/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/shoulder_pan_joint/max_velocity: 1.256
 * /robot_description_planning/joint_limits/torso_lift_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/torso_lift_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/torso_lift_joint/max_acceleration: 0.5
 * /robot_description_planning/joint_limits/torso_lift_joint/max_velocity: 0.1
 * /robot_description_planning/joint_limits/upperarm_roll_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/upperarm_roll_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/upperarm_roll_joint/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/upperarm_roll_joint/max_velocity: 1.57
 * /robot_description_planning/joint_limits/wrist_flex_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/wrist_flex_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/wrist_flex_joint/max_acceleration: 2.5
 * /robot_description_planning/joint_limits/wrist_flex_joint/max_velocity: 2.26
 * /robot_description_planning/joint_limits/wrist_roll_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/wrist_roll_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/wrist_roll_joint/max_acceleration: 2.5
 * /robot_description_planning/joint_limits/wrist_roll_joint/max_velocity: 2.26
 * /robot_description_semantic: <?xml version="1....
 * /rosdistro: melodic
 * /rosversion: 1.14.3

NODES
  /
    amcl (amcl/amcl)
    basic_grasping_perception (simple_grasping/basic_grasping_perception)
    demo (fetch_gazebo_demo/demo.py)
    map_server (map_server/map_server)
    move_base (move_base/move_base)
    move_group (moveit_ros_move_group/move_group)
    tilt_head_node (fetch_navigation/tilt_head.py)

ROS_MASTER_URI=http://localhost:11311

running rosparam delete /move_group/sensors
ERROR: parameter [/move_group/sensors] is not set
process[map_server-1]: started with pid [15068]
process[amcl-2]: started with pid [15069]
process[move_base-3]: started with pid [15074]
process[tilt_head_node-4]: started with pid [15081]
process[move_group-5]: started with pid [15086]
process[basic_grasping_perception-6]: started with pid [15088]
process[demo-7]: started with pid [15089]
[ INFO] [1550152754.618718786]: Loading robot model 'fetch'...
[ INFO] [1550152754.623928359]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1550152755.474892647, 21.301000000]: IK plugin for group 'arm' relies on deprecated API. Please implement initialize(RobotModel, ...).
[ WARN] [1550152755.521153797, 21.306000000]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[ INFO] [1550152756.164802904, 21.364000000]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1550152756.184662846, 21.367000000]: MoveGroup debug mode is ON
Starting planning scene monitors...
[ INFO] [1550152756.184855454, 21.367000000]: Starting planning scene monitor
[ INFO] [1550152756.196402249, 21.368000000]: Listening to '/planning_scene'
[ INFO] [1550152756.196794411, 21.368000000]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1550152756.218372224, 21.371000000]: Listening to '/collision_object' using message notifier with target frame 'base_link '
[ INFO] [1550152756.233318116, 21.373000000]: Listening to '/planning_scene_world' for planning scene world geometry
[ WARN] [1550152756.235112436, 21.374000000]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[ INFO] [1550152756.429797862, 21.400000000]: Listening to '/attached_collision_object' for attached collision objects
Planning scene monitors started.
[ INFO] [1550152756.560505984, 21.408000000]: Initializing OMPL interface using ROS parameters
[ INFO] [1550152756.684014541, 21.416000000]: Using planning interface 'OMPL'
[ INFO] [1550152756.705880057, 21.418000000]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1550152756.710598888, 21.418000000]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1550152756.720895651, 21.419000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1550152756.726043575, 21.419000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1550152756.728372573, 21.419000000]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1550152756.730569858, 21.419000000]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1550152756.731436529, 21.419000000]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1550152756.732194740, 21.419000000]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1550152756.732704201, 21.420000000]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1550152756.733383308, 21.420000000]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1550152756.733822725, 21.420000000]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1550152757.035567134, 21.454000000]: Added FollowJointTrajectory controller for arm_controller
[ INFO] [1550152757.292258046, 21.492000000]: Added FollowJointTrajectory controller for arm_with_torso_controller
[ INFO] [1550152757.548852007, 21.519000000]: Added GripperCommand controller for gripper_controller
[ INFO] [1550152757.549141987, 21.519000000]: Returned 3 controllers in list
[ INFO] [1550152757.616081913, 21.525000000]: Trajectory execution is managing controllers
Loading 'move_group/ApplyPlanningSceneService'...
Loading 'move_group/ClearOctomapService'...
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
Loading 'move_group/MoveGroupGetPlanningSceneService'...
Loading 'move_group/MoveGroupKinematicsService'...
Loading 'move_group/MoveGroupMoveAction'...
Loading 'move_group/MoveGroupPickPlaceAction'...
Loading 'move_group/MoveGroupPlanService'...
Loading 'move_group/MoveGroupQueryPlannersService'...
Loading 'move_group/MoveGroupStateValidationService'...
[ INFO] [1550152757.942615278, 21.566000000]: 

********************************************************
* MoveGroup using: 
*     - ApplyPlanningSceneService
*     - ClearOctomapService
*     - CartesianPathService
*     - ExecuteTrajectoryAction
*     - GetPlanningSceneService
*     - KinematicsService
*     - MoveAction
*     - PickPlaceAction
*     - MotionPlanService
*     - QueryPlannersService
*     - StateValidationService
********************************************************

[ INFO] [1550152757.942731993, 21.566000000]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1550152757.942764210, 21.566000000]: MoveGroup context initialization complete

You can start planning now!

[ INFO] [1550152759.487356168, 21.810000000]: Using plugin "static_map"
[ INFO] [1550152759.528604885, 21.815000000]: Requesting the map...
[ INFO] [1550152760.131376396, 21.918000000]: Resizing costmap to 401 X 444 at 0.050000 m/pix
[ INFO] [1550152760.625641725, 22.018000000]: Received a 401 X 444 map at 0.050000 m/pix
[ INFO] [1550152760.636009950, 22.018000000]: Using plugin "obstacles"
[ INFO] [1550152760.951294745, 22.052000000]:     Subscribed to Topics: base_scan
[INFO] [1550152761.129958, 22.093000]: Waiting for move_base...
[ INFO] [1550152761.214008339, 22.106000000]: Using plugin "inflater"
[ INFO] [1550152761.424443860, 22.147000000]: Using plugin "obstacles"
[ INFO] [1550152761.438196843, 22.149000000]:     Subscribed to Topics: base_scan
[ INFO] [1550152761.605159864, 22.173000000]: Using plugin "inflater"
[ INFO] [1550152761.788268598, 22.211000000]: Created local_planner base_local_planner/TrajectoryPlannerROS
[ INFO] [1550152761.829801498, 22.220000000]: Sim period is set to 0.04
[ INFO] [1550152762.578927711, 22.404000000]: Recovery behavior will clear layer obstacles
[ INFO] [1550152762.631120344, 22.417000000]: Recovery behavior will clear layer obstacles
[ INFO] [1550152762.796226307, 22.443000000]: odom received!
[INFO] [1550152762.949895, 22.481000]: Waiting for torso_controller...
[INFO] [1550152763.131925, 22.523000]: Waiting for head_controller...
[INFO] [1550152763.396435, 22.593000]: Waiting for get_planning_scene
[INFO] [1550152763.426339, 22.600000]: Connecting to pickup action...
[INFO] [1550152763.729734, 22.676000]: ...connected
[INFO] [1550152763.732230, 22.677000]: Connecting to place action...
[INFO] [1550152763.958943, 22.731000]: ...connected
[INFO] [1550152764.263868, 22.806000]: Waiting for basic_grasping_perception/find_objects...
Get block to pick...
[INFO] [1550152764.535517, 22.855000]: Picking object...
[WARN] [1550152765.726596, 23.118000]: Perception failed.
[INFO] [1550152765.731775, 23.118000]: Picking object...
[WARN] [1550152766.800970, 23.412000]: Perception failed.
[INFO] [1550152766.805500, 23.412000]: Picking object...
[WARN] [1550152767.817863, 23.676000]: Perception failed.
[INFO] [1550152767.831221, 23.679000]: Picking object...
[WARN] [1550152768.789852, 23.945000]: Perception failed.
[INFO] [1550152768.792502, 23.945000]: Picking object...
[WARN] [1550152769.793836, 24.212000]: Perception failed.
[INFO] [1550152769.796557, 24.212000]: Picking object...
[WARN] [1550152770.793096, 24.480000]: Perception failed.
[INFO] [1550152770.796807, 24.481000]: Picking object...
^C[basic_grasping_perception-6] killing on exit
[demo-7] killing on exit
[move_group-5] killing on exit
[tilt_head_node-4] killing on exit
[move_base-3] killing on exit
[amcl-2] killing on exit
[map_server-1] killing on exit
Traceback (most recent call last):
  File "/home/zhao/Dropbox/fetchit/catkin_ws/src/fetch_gazebo/fetch_gazebo_demo/scripts/demo.py", line 279, in <module>
    grasping_client.updateScene()
  File "/home/zhao/Dropbox/fetchit/catkin_ws/src/fetch_gazebo/fetch_gazebo_demo/scripts/demo.py", line 172, in updateScene
    self.scene.waitForSync()
  File "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_python/planning_scene_interface.py", line 410, in waitForSync
    rospy.sleep(0.1)
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/timer.py", line 165, in sleep
    raise rospy.exceptions.ROSInterruptException("ROS shutdown request")
rospy.exceptions.ROSInterruptException: ROS shutdown request
Traceback (most recent call last):
  File "/home/zhao/Dropbox/fetchit/catkin_ws/src/fetch_ros/fetch_navigation/scripts/tilt_head.py", line 138, in <module>
    h.loop()
  File "/home/zhao/Dropbox/fetchit/catkin_ws/src/fetch_ros/fetch_navigation/scripts/tilt_head.py", line 133, in loop
    rospy.sleep(1.0)
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/timer.py", line 165, in sleep
    raise rospy.exceptions.ROSInterruptException("ROS shutdown request")
rospy.exceptions.ROSInterruptException: ROS shutdown request
shutting down processing monitor...
... shutting down processing monitor complete
done
zhao@zhao1804:~/Dropbox/fetchit/catkin_ws$ roslaunch fetch_gazebo_demo demo.launch
... logging to /home/zhao/.ros/log/968350b0-3060-11e9-8d6a-04d3b0082825/roslaunch-zhao1804-15691.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://zhao1804:37927/

SUMMARY
========

PARAMETERS
 * /amcl/base_frame_id: base_link
 * /amcl/global_frame_id: map
 * /amcl/gui_publish_rate: 10.0
 * /amcl/kld_err: 0.01
 * /amcl/kld_z: 0.99
 * /amcl/laser_likelihood_max_dist: 2.0
 * /amcl/laser_max_beams: 30
 * /amcl/laser_max_range: -1.0
 * /amcl/laser_min_range: -1.0
 * /amcl/laser_model_type: likelihood_field
 * /amcl/laser_sigma_hit: 0.2
 * /amcl/laser_z_hit: 0.95
 * /amcl/laser_z_rand: 0.05
 * /amcl/max_particles: 2000
 * /amcl/min_particles: 500
 * /amcl/odom_alpha1: 0.15
 * /amcl/odom_alpha2: 0.5
 * /amcl/odom_alpha3: 0.5
 * /amcl/odom_alpha4: 0.15
 * /amcl/odom_frame_id: odom
 * /amcl/odom_model_type: diff-corrected
 * /amcl/recovery_alpha_fast: 0.0
 * /amcl/recovery_alpha_slow: 0.0
 * /amcl/resample_interval: 2
 * /amcl/save_pose_rate: 0.5
 * /amcl/transform_tolerance: 0.5
 * /amcl/update_min_a: 0.25
 * /amcl/update_min_d: 0.1
 * /amcl/use_map_topic: False
 * /basic_grasping_perception/gripper/approach/desired: 0.15
 * /basic_grasping_perception/gripper/approach/min: 0.145
 * /basic_grasping_perception/gripper/finger_depth: 0.02
 * /basic_grasping_perception/gripper/gripper_tolerance: 0.05
 * /basic_grasping_perception/gripper/retreat/desired: 0.15
 * /basic_grasping_perception/gripper/retreat/min: 0.145
 * /basic_grasping_perception/gripper/tool_to_planning_frame: 0.18
 * /move_base/NavfnROS/allow_unknown: True
 * /move_base/NavfnROS/default_tolerance: 0.0
 * /move_base/NavfnROS/planner_window_x: 0.0
 * /move_base/NavfnROS/planner_window_y: 0.0
 * /move_base/NavfnROS/visualize_potential: False
 * /move_base/TrajectoryPlannerROS/acc_lim_theta: 0.8
 * /move_base/TrajectoryPlannerROS/acc_lim_x: 0.35
 * /move_base/TrajectoryPlannerROS/acc_lim_y: 0.0
 * /move_base/TrajectoryPlannerROS/angular_sim_granularity: 0.025
 * /move_base/TrajectoryPlannerROS/dwa: False
 * /move_base/TrajectoryPlannerROS/gdist_scale: 12.0
 * /move_base/TrajectoryPlannerROS/heading_lookahead: 0.325
 * /move_base/TrajectoryPlannerROS/heading_scoring: True
 * /move_base/TrajectoryPlannerROS/heading_scoring_timestep: 0.8
 * /move_base/TrajectoryPlannerROS/holonomic_robot: False
 * /move_base/TrajectoryPlannerROS/latch_xy_goal_tolerance: True
 * /move_base/TrajectoryPlannerROS/max_vel_theta: 1.5
 * /move_base/TrajectoryPlannerROS/max_vel_x: 1.0
 * /move_base/TrajectoryPlannerROS/max_vel_y: 0.0
 * /move_base/TrajectoryPlannerROS/meter_scoring: True
 * /move_base/TrajectoryPlannerROS/min_in_place_vel_theta: 0.3
 * /move_base/TrajectoryPlannerROS/min_vel_theta: -1.5
 * /move_base/TrajectoryPlannerROS/min_vel_x: 0.15
 * /move_base/TrajectoryPlannerROS/min_vel_y: 0.0
 * /move_base/TrajectoryPlannerROS/occdist_scale: 0.1
 * /move_base/TrajectoryPlannerROS/oscillation_reset_dist: 0.05
 * /move_base/TrajectoryPlannerROS/pdist_scale: 8.0
 * /move_base/TrajectoryPlannerROS/publish_cost_grid_pc: False
 * /move_base/TrajectoryPlannerROS/sim_granularity: 0.025
 * /move_base/TrajectoryPlannerROS/sim_time: 1.0
 * /move_base/TrajectoryPlannerROS/vtheta_samples: 10
 * /move_base/TrajectoryPlannerROS/vx_samples: 3
 * /move_base/TrajectoryPlannerROS/xy_goal_tolerance: 0.1
 * /move_base/TrajectoryPlannerROS/yaw_goal_tolerance: 0.1
 * /move_base/aggressive_reset/reset_distance: 0.5
 * /move_base/base_global_planner: navfn/NavfnROS
 * /move_base/base_local_planner: base_local_planne...
 * /move_base/conservative_reset/reset_distance: 3.0
 * /move_base/controller_frequency: 25.0
 * /move_base/controller_patience: 15.0
 * /move_base/global_costmap/global_frame: map
 * /move_base/global_costmap/inflater/inflation_radius: 0.7
 * /move_base/global_costmap/inflater/robot_radius: 0.3
 * /move_base/global_costmap/obstacles/base_scan/clearing: True
 * /move_base/global_costmap/obstacles/base_scan/data_type: LaserScan
 * /move_base/global_costmap/obstacles/base_scan/marking: True
 * /move_base/global_costmap/obstacles/base_scan/max_obstacle_height: 0.3
 * /move_base/global_costmap/obstacles/base_scan/min_obstacle_height: 0.25
 * /move_base/global_costmap/obstacles/base_scan/obstacle_range: 4.0
 * /move_base/global_costmap/obstacles/base_scan/raytrace_range: 5.0
 * /move_base/global_costmap/obstacles/base_scan/topic: base_scan
 * /move_base/global_costmap/obstacles/max_obstacle_height: 2.0
 * /move_base/global_costmap/obstacles/observation_sources: base_scan
 * /move_base/global_costmap/obstacles/z_resolution: 0.125
 * /move_base/global_costmap/obstacles/z_voxels: 16
 * /move_base/global_costmap/plugins: [{'type': 'costma...
 * /move_base/global_costmap/publish_frequency: 0.0
 * /move_base/global_costmap/robot_base_frame: base_link
 * /move_base/global_costmap/robot_radius: 0.3
 * /move_base/global_costmap/static_map: True
 * /move_base/global_costmap/transform_tolerance: 0.5
 * /move_base/global_costmap/update_frequency: 5.0
 * /move_base/local_costmap/global_frame: odom
 * /move_base/local_costmap/height: 4.0
 * /move_base/local_costmap/inflater/inflation_radius: 0.7
 * /move_base/local_costmap/inflater/robot_radius: 0.3
 * /move_base/local_costmap/obstacles/base_scan/clearing: True
 * /move_base/local_costmap/obstacles/base_scan/data_type: LaserScan
 * /move_base/local_costmap/obstacles/base_scan/marking: True
 * /move_base/local_costmap/obstacles/base_scan/max_obstacle_height: 0.3
 * /move_base/local_costmap/obstacles/base_scan/min_obstacle_height: 0.25
 * /move_base/local_costmap/obstacles/base_scan/obstacle_range: 4.0
 * /move_base/local_costmap/obstacles/base_scan/raytrace_range: 5.0
 * /move_base/local_costmap/obstacles/base_scan/topic: base_scan
 * /move_base/local_costmap/obstacles/max_obstacle_height: 2.0
 * /move_base/local_costmap/obstacles/observation_sources: base_scan
 * /move_base/local_costmap/obstacles/publish_observations: False
 * /move_base/local_costmap/obstacles/z_resolution: 0.125
 * /move_base/local_costmap/obstacles/z_voxels: 16
 * /move_base/local_costmap/plugins: [{'type': 'costma...
 * /move_base/local_costmap/publish_frequency: 2.0
 * /move_base/local_costmap/resolution: 0.025
 * /move_base/local_costmap/robot_base_frame: base_link
 * /move_base/local_costmap/robot_radius: 0.3
 * /move_base/local_costmap/rolling_window: True
 * /move_base/local_costmap/transform_tolerance: 0.5
 * /move_base/local_costmap/width: 4.0
 * /move_base/oscillation_distance: 0.5
 * /move_base/oscillation_timeout: 10.0
 * /move_base/planner_frequency: 0.0
 * /move_base/planner_patience: 5.0
 * /move_base/recovery_behavior_enabled: True
 * /move_base/recovery_behaviors: [{'type': 'clear_...
 * /move_base/rotate_recovery/frequency: 20.0
 * /move_base/rotate_recovery/sim_granularity: 0.017
 * /move_group/allow_trajectory_execution: True
 * /move_group/allowed_execution_duration_scaling: 1.2
 * /move_group/allowed_goal_duration_margin: 0.5
 * /move_group/arm/longest_valid_segment_fraction: 0.05
 * /move_group/arm/planner_configs: ['SBLkConfigDefau...
 * /move_group/arm/projection_evaluator: joints(shoulder_p...
 * /move_group/arm_with_torso/longest_valid_segment_fraction: 0.05
 * /move_group/arm_with_torso/planner_configs: ['SBLkConfigDefau...
 * /move_group/arm_with_torso/projection_evaluator: joints(torso_lift...
 * /move_group/capabilities: move_group/MoveGr...
 * /move_group/controller_list: [{'default': True...
 * /move_group/gripper/planner_configs: ['SBLkConfigDefau...
 * /move_group/jiggle_fraction: 0.05
 * /move_group/max_safe_path_cost: 1
 * /move_group/moveit_controller_manager: moveit_simple_con...
 * /move_group/moveit_manage_controllers: True
 * /move_group/planner_configs/BKPIECEkConfigDefault/type: geometric::BKPIECE
 * /move_group/planner_configs/ESTkConfigDefault/type: geometric::EST
 * /move_group/planner_configs/KPIECEkConfigDefault/type: geometric::KPIECE
 * /move_group/planner_configs/LBKPIECEkConfigDefault/type: geometric::LBKPIECE
 * /move_group/planner_configs/PRMkConfigDefault/type: geometric::PRM
 * /move_group/planner_configs/PRMstarkConfigDefault/type: geometric::PRMstar
 * /move_group/planner_configs/RRTConnectkConfigDefault/type: geometric::RRTCon...
 * /move_group/planner_configs/RRTkConfigDefault/type: geometric::RRT
 * /move_group/planner_configs/RRTstarkConfigDefault/type: geometric::RRTstar
 * /move_group/planner_configs/SBLkConfigDefault/type: geometric::SBL
 * /move_group/planner_configs/TRRTkConfigDefault/type: geometric::TRRT
 * /move_group/planning_plugin: ompl_interface/OM...
 * /move_group/planning_scene_monitor/publish_geometry_updates: True
 * /move_group/planning_scene_monitor/publish_planning_scene: True
 * /move_group/planning_scene_monitor/publish_state_updates: True
 * /move_group/planning_scene_monitor/publish_transforms_updates: True
 * /move_group/request_adapters: default_planner_r...
 * /move_group/start_state_max_bounds_error: 0.1
 * /robot_description_kinematics/arm/kinematics_solver: fetch_arm_kinemat...
 * /robot_description_kinematics/arm/kinematics_solver_attempts: 3
 * /robot_description_kinematics/arm/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/arm/kinematics_solver_timeout: 0.005
 * /robot_description_kinematics/arm_with_torso/kinematics_solver: kdl_kinematics_pl...
 * /robot_description_kinematics/arm_with_torso/kinematics_solver_attempts: 3
 * /robot_description_kinematics/arm_with_torso/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/arm_with_torso/kinematics_solver_timeout: 0.005
 * /robot_description_planning/joint_limits/elbow_flex_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/elbow_flex_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/elbow_flex_joint/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/elbow_flex_joint/max_velocity: 1.5
 * /robot_description_planning/joint_limits/forearm_roll_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/forearm_roll_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/forearm_roll_joint/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/forearm_roll_joint/max_velocity: 1.57
 * /robot_description_planning/joint_limits/left_gripper_joint/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/left_gripper_joint/has_velocity_limits: False
 * /robot_description_planning/joint_limits/left_gripper_joint/max_acceleration: 0
 * /robot_description_planning/joint_limits/left_gripper_joint/max_velocity: 0
 * /robot_description_planning/joint_limits/right_gripper_joint/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/right_gripper_joint/has_velocity_limits: False
 * /robot_description_planning/joint_limits/right_gripper_joint/max_acceleration: 0
 * /robot_description_planning/joint_limits/right_gripper_joint/max_velocity: 0
 * /robot_description_planning/joint_limits/shoulder_lift_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/shoulder_lift_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/shoulder_lift_joint/max_acceleration: 1.0
 * /robot_description_planning/joint_limits/shoulder_lift_joint/max_velocity: 1.0
 * /robot_description_planning/joint_limits/shoulder_pan_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/shoulder_pan_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/shoulder_pan_joint/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/shoulder_pan_joint/max_velocity: 1.256
 * /robot_description_planning/joint_limits/torso_lift_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/torso_lift_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/torso_lift_joint/max_acceleration: 0.5
 * /robot_description_planning/joint_limits/torso_lift_joint/max_velocity: 0.1
 * /robot_description_planning/joint_limits/upperarm_roll_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/upperarm_roll_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/upperarm_roll_joint/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/upperarm_roll_joint/max_velocity: 1.57
 * /robot_description_planning/joint_limits/wrist_flex_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/wrist_flex_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/wrist_flex_joint/max_acceleration: 2.5
 * /robot_description_planning/joint_limits/wrist_flex_joint/max_velocity: 2.26
 * /robot_description_planning/joint_limits/wrist_roll_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/wrist_roll_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/wrist_roll_joint/max_acceleration: 2.5
 * /robot_description_planning/joint_limits/wrist_roll_joint/max_velocity: 2.26
 * /robot_description_semantic: <?xml version="1....
 * /rosdistro: melodic
 * /rosversion: 1.14.3

NODES
  /
    amcl (amcl/amcl)
    basic_grasping_perception (simple_grasping/basic_grasping_perception)
    demo (fetch_gazebo_demo/demo.py)
    map_server (map_server/map_server)
    move_base (move_base/move_base)
    move_group (moveit_ros_move_group/move_group)
    tilt_head_node (fetch_navigation/tilt_head.py)

ROS_MASTER_URI=http://localhost:11311

running rosparam delete /move_group/sensors
ERROR: parameter [/move_group/sensors] is not set
process[map_server-1]: started with pid [15744]
process[amcl-2]: started with pid [15745]
process[move_base-3]: started with pid [15751]
process[tilt_head_node-4]: started with pid [15754]
process[move_group-5]: started with pid [15758]
process[basic_grasping_perception-6]: started with pid [15764]
process[demo-7]: started with pid [15766]
[ INFO] [1550152816.255272910]: Loading robot model 'fetch'...
[ INFO] [1550152816.264961261]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1550152817.193265549, 38.395000000]: IK plugin for group 'arm' relies on deprecated API. Please implement initialize(RobotModel, ...).
[ WARN] [1550152817.226394094, 38.397000000]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[ INFO] [1550152817.673541007, 38.430000000]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1550152817.685287940, 38.431000000]: MoveGroup debug mode is ON
Starting planning scene monitors...
[ INFO] [1550152817.685621569, 38.431000000]: Starting planning scene monitor
[ INFO] [1550152817.693314597, 38.432000000]: Listening to '/planning_scene'
[ INFO] [1550152817.693583912, 38.432000000]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1550152817.706299892, 38.433000000]: Listening to '/collision_object' using message notifier with target frame 'base_link '
[ INFO] [1550152817.710822605, 38.433000000]: Listening to '/planning_scene_world' for planning scene world geometry
[ WARN] [1550152817.711709211, 38.433000000]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[ INFO] [1550152817.820010986, 38.443000000]: Listening to '/attached_collision_object' for attached collision objects
Planning scene monitors started.
[ INFO] [1550152817.986319777, 38.457000000]: Initializing OMPL interface using ROS parameters
[ INFO] [1550152818.126242942, 38.461000000]: Using planning interface 'OMPL'
[ INFO] [1550152818.131157432, 38.461000000]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1550152818.132135258, 38.461000000]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1550152818.133559413, 38.462000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1550152818.136698062, 38.462000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1550152818.137947254, 38.462000000]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1550152818.139511116, 38.462000000]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1550152818.139873359, 38.462000000]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1550152818.139994481, 38.462000000]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1550152818.140084337, 38.462000000]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1550152818.140180906, 38.462000000]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1550152818.140330353, 38.462000000]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1550152818.445939430, 38.499000000]: Added FollowJointTrajectory controller for arm_controller
[ INFO] [1550152818.755643478, 38.530000000]: Added FollowJointTrajectory controller for arm_with_torso_controller
[ INFO] [1550152819.057558648, 38.558000000]: Added GripperCommand controller for gripper_controller
[ INFO] [1550152819.057960954, 38.558000000]: Returned 3 controllers in list
[ INFO] [1550152819.157349446, 38.564000000]: Trajectory execution is managing controllers
Loading 'move_group/ApplyPlanningSceneService'...
Loading 'move_group/ClearOctomapService'...
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
Loading 'move_group/MoveGroupGetPlanningSceneService'...
Loading 'move_group/MoveGroupKinematicsService'...
Loading 'move_group/MoveGroupMoveAction'...
Loading 'move_group/MoveGroupPickPlaceAction'...
Loading 'move_group/MoveGroupPlanService'...
Loading 'move_group/MoveGroupQueryPlannersService'...
Loading 'move_group/MoveGroupStateValidationService'...
[ INFO] [1550152819.604622392, 38.596000000]: 

********************************************************
* MoveGroup using: 
*     - ApplyPlanningSceneService
*     - ClearOctomapService
*     - CartesianPathService
*     - ExecuteTrajectoryAction
*     - GetPlanningSceneService
*     - KinematicsService
*     - MoveAction
*     - PickPlaceAction
*     - MotionPlanService
*     - QueryPlannersService
*     - StateValidationService
********************************************************

[ INFO] [1550152819.604780873, 38.596000000]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1550152819.604842572, 38.596000000]: MoveGroup context initialization complete

You can start planning now!

[ INFO] [1550152821.238896216, 38.897000000]: Using plugin "static_map"
[ INFO] [1550152821.271485169, 38.898000000]: Requesting the map...
[ INFO] [1550152821.797832030, 38.999000000]: Resizing costmap to 401 X 444 at 0.050000 m/pix
[ INFO] [1550152822.318402372, 39.099000000]: Received a 401 X 444 map at 0.050000 m/pix
[ INFO] [1550152822.336528615, 39.102000000]: Using plugin "obstacles"
[INFO] [1550152822.717252, 39.164000]: Waiting for move_base...
[ INFO] [1550152822.724583598, 39.166000000]:     Subscribed to Topics: base_scan
[ INFO] [1550152822.868011344, 39.193000000]: Using plugin "inflater"
[ INFO] [1550152823.077351848, 39.230000000]: Using plugin "obstacles"
[ INFO] [1550152823.092972954, 39.234000000]:     Subscribed to Topics: base_scan
[ INFO] [1550152823.291826867, 39.275000000]: Using plugin "inflater"
[ INFO] [1550152823.465695819, 39.309000000]: Created local_planner base_local_planner/TrajectoryPlannerROS
[ INFO] [1550152823.524523393, 39.323000000]: Sim period is set to 0.04
[ INFO] [1550152824.266918156, 39.506000000]: Recovery behavior will clear layer obstacles
[ INFO] [1550152824.302552200, 39.515000000]: Recovery behavior will clear layer obstacles
[ INFO] [1550152824.497807764, 39.554000000]: odom received!
[INFO] [1550152824.632219, 39.586000]: Waiting for torso_controller...
[INFO] [1550152824.819133, 39.625000]: Waiting for head_controller...
[INFO] [1550152825.082742, 39.690000]: Waiting for get_planning_scene
[INFO] [1550152825.118656, 39.698000]: Connecting to pickup action...
[INFO] [1550152825.435118, 39.776000]: ...connected
[INFO] [1550152825.437498, 39.776000]: Connecting to place action...
[INFO] [1550152825.670877, 39.824000]: ...connected
[INFO] [1550152825.917689, 39.862000]: Waiting for basic_grasping_perception/find_objects...
[INFO] [1550152826.167008, 39.894000]: Moving to table...
[ WARN] [1550152935.337158025, 67.535000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550152980.132707802, 77.615000000]: Rotate recovery behavior started.
[ WARN] [1550153058.423329389, 95.251000000]: Invalid Trajectory 0.093694, 0.000000, 0.025957, cost: -1.000000
[ WARN] [1550153093.262487593, 103.608000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550153134.243491509, 113.969000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550153178.759782706, 125.168000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550153254.751576573, 143.775000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550153327.259470285, 162.218000000]: Rotate recovery behavior started.
[ WARN] [1550153366.278706368, 172.399000000]: Clearing costmap to unstuck robot (0.500000m).
[ERROR] [1550153366.753323531, 172.518000000]: Aborting because a valid control could not be found. Even after executing all recovery behaviors
[INFO] [1550153366.781150, 172.524000]: Done moving to table...
[INFO] [1550153366.785093, 172.526000]: Raising torso...
Get block to pick...
[INFO] [1550153391.572698, 178.801000]: Picking object...
[WARN] [1550153392.673143, 178.988000]: ObjectManager: object0 not added yet
[WARN] [1550153392.681189, 178.992000]: ObjectManager: object1 not added yet
[WARN] [1550153392.688479, 178.993000]: ObjectManager: object2 not added yet
[WARN] [1550153392.695956, 178.993000]: ObjectManager: surface0 not added yet
[WARN] [1550153392.702579, 178.996000]: ObjectManager: surface1 not added yet
[INFO] [1550153393.662212, 179.207000]: Beginning to pick.
[ INFO] [1550153393.710886642, 179.216000000]: Planning attempt 1 of at most 1
[ INFO] [1550153393.747439878, 179.224000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153393.748480877, 179.224000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153393.748852458, 179.224000000]: Added plan for pipeline 'pick'. Queue is now of size 2
[ INFO] [1550153393.749536260, 179.224000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153393.750028521, 179.224000000]: Added plan for pipeline 'pick'. Queue is now of size 2
[ INFO] [1550153393.750879788, 179.224000000]: Added plan for pipeline 'pick'. Queue is now of size 3
[ INFO] [1550153393.751895365, 179.224000000]: Added plan for pipeline 'pick'. Queue is now of size 4
[ INFO] [1550153393.752869652, 179.224000000]: Added plan for pipeline 'pick'. Queue is now of size 5
[ INFO] [1550153393.753795769, 179.224000000]: Added plan for pipeline 'pick'. Queue is now of size 6
[ INFO] [1550153393.754273214, 179.225000000]: Added plan for pipeline 'pick'. Queue is now of size 7
[ INFO] [1550153393.754462461, 179.225000000]: Added plan for pipeline 'pick'. Queue is now of size 8
[ INFO] [1550153393.754598282, 179.225000000]: Added plan for pipeline 'pick'. Queue is now of size 9
[ INFO] [1550153393.754749107, 179.225000000]: Added plan for pipeline 'pick'. Queue is now of size 10
[ INFO] [1550153393.754867200, 179.225000000]: Added plan for pipeline 'pick'. Queue is now of size 11
[ INFO] [1550153393.755051772, 179.225000000]: Added plan for pipeline 'pick'. Queue is now of size 12
[ INFO] [1550153393.755189438, 179.225000000]: Added plan for pipeline 'pick'. Queue is now of size 13
[ INFO] [1550153393.755334915, 179.225000000]: Added plan for pipeline 'pick'. Queue is now of size 14
[ INFO] [1550153393.755452999, 179.225000000]: Added plan for pipeline 'pick'. Queue is now of size 15
[ INFO] [1550153393.780708026, 179.227000000]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153393.786793545, 179.228000000]: Manipulation plan 7 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153393.804705611, 179.231000000]: Manipulation plan 5 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153393.808533135, 179.231000000]: Manipulation plan 11 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153393.808729677, 179.231000000]: Manipulation plan 2 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153393.815110041, 179.231000000]: Manipulation plan 10 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153393.830177394, 179.234000000]: Manipulation plan 15 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153393.830652548, 179.234000000]: Manipulation plan 1 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153393.838803211, 179.235000000]: Manipulation plan 6 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153393.844853167, 179.235000000]: Manipulation plan 16 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153393.859924986, 179.237000000]: Manipulation plan 3 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153393.863973966, 179.238000000]: Manipulation plan 14 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153393.864321996, 179.238000000]: Manipulation plan 8 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153393.872887149, 179.238000000]: Manipulation plan 17 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153393.878539880, 179.239000000]: Manipulation plan 9 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153393.881859741, 179.239000000]: Manipulation plan 12 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153393.891787588, 179.239000000]: Manipulation plan 13 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153393.891941139, 179.239000000]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 2
[ WARN] [1550153393.892547257, 179.239000000]: All supplied grasps failed. Retrying last grasp in verbose mode.
[ INFO] [1550153393.892858277, 179.239000000]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153393.895074774, 179.239000000]: IK failed
[ INFO] [1550153393.896964234, 179.239000000]: IK failed
[ INFO] [1550153393.903994475, 179.239000000]: IK failed
[ INFO] [1550153393.904274886, 179.239000000]: Sampler failed to produce a state
[ INFO] [1550153393.904446527, 179.239000000]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153393.905806261, 179.239000000]: Pickup planning completed after 0.181321 seconds
[ERROR] [1550153393.940272, 179.246000]: Pick failed in the planning stage, try again...
[ INFO] [1550153396.107112117, 179.756000000]: Planning attempt 1 of at most 1
[ INFO] [1550153396.107461923, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153396.107523632, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 2
[ INFO] [1550153396.107551375, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 3
[ INFO] [1550153396.107576767, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 4
[ INFO] [1550153396.107599795, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 5
[ INFO] [1550153396.107623829, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 6
[ INFO] [1550153396.107646277, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 7
[ INFO] [1550153396.107668341, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 8
[ INFO] [1550153396.107692867, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 9
[ INFO] [1550153396.107722079, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 10
[ INFO] [1550153396.107759870, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 11
[ INFO] [1550153396.107791824, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 12
[ INFO] [1550153396.107824336, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 13
[ INFO] [1550153396.107865058, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 14
[ INFO] [1550153396.107955347, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 15
[ INFO] [1550153396.108011277, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 16
[ INFO] [1550153396.108055433, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 17
[ INFO] [1550153396.108102517, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 18
[ INFO] [1550153396.118587283, 179.756000000]: Manipulation plan 2 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153396.120641004, 179.756000000]: Manipulation plan 5 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153396.124350701, 179.756000000]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153396.130405914, 179.756000000]: Manipulation plan 11 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153396.133675837, 179.756000000]: Manipulation plan 15 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153396.139437541, 179.756000000]: Manipulation plan 6 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153396.154993244, 179.760000000]: Manipulation plan 7 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153396.159013530, 179.760000000]: Manipulation plan 10 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153396.159131229, 179.760000000]: Manipulation plan 1 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153396.163847635, 179.761000000]: Manipulation plan 14 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153396.171108347, 179.763000000]: Manipulation plan 16 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153396.173989059, 179.763000000]: Manipulation plan 3 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153396.178834195, 179.764000000]: Manipulation plan 8 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153396.180172085, 179.764000000]: Manipulation plan 17 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153396.187044569, 179.764000000]: Manipulation plan 12 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153396.200858775, 179.768000000]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153396.201879521, 179.768000000]: Manipulation plan 13 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153396.207803285, 179.768000000]: Manipulation plan 9 failed at stage 'reachable & valid pose filter' on thread 1
[ WARN] [1550153396.210395812, 179.768000000]: All supplied grasps failed. Retrying last grasp in verbose mode.
[ INFO] [1550153396.210934814, 179.768000000]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153396.213506176, 179.770000000]: IK failed
[ INFO] [1550153396.215496819, 179.770000000]: IK failed
[ INFO] [1550153396.218012068, 179.770000000]: IK failed
[ INFO] [1550153396.221173104, 179.771000000]: Sampler failed to produce a state
[ INFO] [1550153396.221560718, 179.771000000]: Manipulation plan 9 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153396.224782682, 179.772000000]: Pickup planning completed after 0.103031 seconds
[ERROR] [1550153396.235715, 179.773000]: Pick failed in the planning stage, try again...
[ INFO] [1550153398.378370518, 180.284000000]: Planning attempt 1 of at most 1
[ INFO] [1550153398.378739755, 180.284000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153398.378807529, 180.284000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153398.382805380, 180.285000000]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153398.384706454, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 2
[ INFO] [1550153398.384853428, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 3
[ INFO] [1550153398.384914989, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 4
[ INFO] [1550153398.384955950, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 5
[ INFO] [1550153398.384992605, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 6
[ INFO] [1550153398.385033004, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 7
[ INFO] [1550153398.385074768, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 8
[ INFO] [1550153398.385121558, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 9
[ INFO] [1550153398.385181272, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 10
[ INFO] [1550153398.385233268, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 11
[ INFO] [1550153398.385285612, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 12
[ INFO] [1550153398.385340240, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 13
[ INFO] [1550153398.385391106, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 14
[ INFO] [1550153398.385451967, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 15
[ INFO] [1550153398.385513089, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 16
[ INFO] [1550153398.385569822, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 17
[ INFO] [1550153398.403623454, 180.286000000]: Manipulation plan 2 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153398.408771049, 180.286000000]: Manipulation plan 5 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153398.410371499, 180.286000000]: Manipulation plan 15 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153398.416666540, 180.288000000]: Manipulation plan 11 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153398.420468852, 180.288000000]: Manipulation plan 7 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153398.430312991, 180.290000000]: Manipulation plan 6 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153398.436804121, 180.290000000]: Manipulation plan 10 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153398.442740915, 180.290000000]: Manipulation plan 3 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153398.447288611, 180.291000000]: Manipulation plan 16 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153398.448312415, 180.292000000]: Manipulation plan 14 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153398.448692486, 180.292000000]: Manipulation plan 1 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153398.451544899, 180.292000000]: Manipulation plan 12 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153398.453659137, 180.292000000]: Manipulation plan 13 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153398.458705982, 180.292000000]: Manipulation plan 9 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153398.469929670, 180.293000000]: Manipulation plan 17 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153398.470030426, 180.293000000]: Manipulation plan 8 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153398.480887883, 180.294000000]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 1
[ WARN] [1550153398.481222248, 180.294000000]: All supplied grasps failed. Retrying last grasp in verbose mode.
[ INFO] [1550153398.481454257, 180.294000000]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153398.483513377, 180.294000000]: IK failed
[ INFO] [1550153398.489574325, 180.296000000]: IK failed
[ INFO] [1550153398.498495097, 180.297000000]: IK failed
[ INFO] [1550153398.499202991, 180.297000000]: Sampler failed to produce a state
[ INFO] [1550153398.500163399, 180.297000000]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153398.501141181, 180.298000000]: Pickup planning completed after 0.102676 seconds
[ERROR] [1550153398.509153, 180.298000]: Pick failed in the planning stage, try again...
[ INFO] [1550153400.571026984, 180.814000000]: Planning attempt 1 of at most 1
[ INFO] [1550153400.571347382, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153400.571399242, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 2
[ INFO] [1550153400.571433898, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 3
[ INFO] [1550153400.571470866, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 4
[ INFO] [1550153400.571501954, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 5
[ INFO] [1550153400.571534917, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 6
[ INFO] [1550153400.571565750, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 7
[ INFO] [1550153400.571599444, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 8
[ INFO] [1550153400.571632666, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 9
[ INFO] [1550153400.571665406, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 10
[ INFO] [1550153400.571697967, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 11
[ INFO] [1550153400.571732038, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 12
[ INFO] [1550153400.571767846, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 12
[ INFO] [1550153400.571800534, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 13
[ INFO] [1550153400.571831835, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 14
[ INFO] [1550153400.571869107, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 15
[ INFO] [1550153400.571898067, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 16
[ INFO] [1550153400.571928476, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 17
[ INFO] [1550153400.586055423, 180.818000000]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153400.591401481, 180.818000000]: Manipulation plan 5 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153400.601141103, 180.818000000]: Manipulation plan 11 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153400.607128709, 180.819000000]: Manipulation plan 1 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153400.608128440, 180.819000000]: Manipulation plan 7 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153400.624258590, 180.820000000]: Manipulation plan 2 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153400.626616510, 180.820000000]: Manipulation plan 6 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153400.630235842, 180.820000000]: Manipulation plan 15 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153400.639196048, 180.821000000]: Manipulation plan 10 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153400.649360544, 180.823000000]: Manipulation plan 14 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153400.656643976, 180.823000000]: Manipulation plan 3 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153400.657404738, 180.823000000]: Manipulation plan 8 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153400.668111743, 180.826000000]: Manipulation plan 16 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153400.679405593, 180.826000000]: Manipulation plan 12 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153400.687058933, 180.827000000]: Manipulation plan 13 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153400.690214022, 180.827000000]: Manipulation plan 17 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153400.694703205, 180.827000000]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153400.696004253, 180.827000000]: Manipulation plan 9 failed at stage 'reachable & valid pose filter' on thread 3
[ WARN] [1550153400.708756540, 180.830000000]: All supplied grasps failed. Retrying last grasp in verbose mode.
[ INFO] [1550153400.708940172, 180.830000000]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153400.711116214, 180.830000000]: IK failed
[ INFO] [1550153400.719230453, 180.830000000]: IK failed
[ INFO] [1550153400.722598089, 180.830000000]: IK failed
[ INFO] [1550153400.722787301, 180.830000000]: Sampler failed to produce a state
[ INFO] [1550153400.722872173, 180.830000000]: Manipulation plan 9 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153400.727197412, 180.830000000]: Pickup planning completed after 0.137375 seconds
[ERROR] [1550153400.736623, 180.835000]: Pick failed in the planning stage, try again...
^C

The text was updated successfully, but these errors were encountered:

moriarty · 2019-02-14T15:46:00Z

Sounds like the same issue reported here https://github.com/fetchrobotics/fetch_ros/issues/102

@narora1 @safrimus

moriarty · 2019-04-05T21:07:59Z

We've made some improvements but I think this is still a bit of an issue.

I've added Help Wanted and More Info Needed, similar to #36

slugflow · 2019-07-04T01:24:45Z

@umhan35 Hi, it might work better if you reduce the inflation_radius specified in costmap_common.yaml under fetch_navigation/config. (the default value is 0.7m)
In your case, the robot has to be close to the table to pick up the cube, while large inflation_radius keeps the robot away from the obstacles, and the robot starts rotation recovery if it is within the inflation layer of the table.

umhan35 · 2019-07-04T02:39:43Z

@xujingren We actually have another map and move_base config that works in our test course but not in Gazebo 9. The root cause is #102. We tested on Ubuntu 14.04 with Gazebo 2 and it works.

umhan35 mentioned this issue Feb 14, 2019

[pick_place_demo.py] cannot grasp the cube #31

Closed

ben-fido mentioned this issue Feb 27, 2019

Bug: Regression in simulated Fetch robot (Melodic, Gazebo 9) #35

Closed

moriarty mentioned this issue Feb 28, 2019

Gazebo9 Issues :-( #37

Closed

This was referenced Mar 22, 2019

fetch_gazebo: add casters to fetch.gazebo.xacro #49

Merged

Improved pick place frictions #59

Merged

moriarty added More Info Needed More information is needed to help us address this issue help wanted labels Apr 5, 2019

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[demo.py] the robot keeps rotating after moved near table #30

[demo.py] the robot keeps rotating after moved near table #30

umhan35 commented Feb 14, 2019

moriarty commented Feb 14, 2019

moriarty commented Apr 5, 2019

slugflow commented Jul 4, 2019

umhan35 commented Jul 4, 2019

[demo.py] the robot keeps rotating after moved near table #30

[demo.py] the robot keeps rotating after moved near table #30

Comments

umhan35 commented Feb 14, 2019

moriarty commented Feb 14, 2019

moriarty commented Apr 5, 2019

slugflow commented Jul 4, 2019

umhan35 commented Jul 4, 2019