Robotics StackExchange | Archived questions

Real UR5 Error RRTConnect: Unable to sample any valid states for goal tree

I'm using a UR5 which and am trying to move the robot forward 0.1m in the y direction. Here are my start up commands:

roslaunch ur_robot_driver ur5_bringup.launch robot_ip:=169.254.198.35 kinematics_config:="${HOME}/my_robot_calibration.yaml"
roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true
roslaunch ur5_moveit_config moveit_rviz.launch config:=true

Some information on the current starting position of the UR5 (Don't have the points to upload pictures unfortunately):

joint positions: [-1.5844348112689417, -0.9981825987445276, -1.9562018553363245, -1.8684194723712366, 1.4567383527755737, 2.39935302734375]

Coordinates of End Effector: (0.12241, 0.23884, 0.4266)

Orientation of End Effector: [0.265, 0.5945, -0.307, 0.694]

My controllers.yaml file inside ur5moveitconfig/config file looks like this:

controller_list:
 - name: /scaled_pos_traj_controller
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: true
   joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
  - wrist_2_joint
  - wrist_3_joint

My kinematics.yaml file also in ur5moveitconfig/config looks like this:

manipulator:
  kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.01
  kinematics_solver_attempts: 3

Here is the console output of running roslaunch ur_robot_driver ur5_bringup.launch robot_ip:=169.254.198.35 kinematics_config:="${HOME}/my_robot_calibration.yaml"

... logging to /home/shu/.ros/log/e301052a-0f7d-11ea-9d0c-3c918077fd81/roslaunch-sewts-ideapad-002-26797.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://sewts-ideapad-002:39421/

SUMMARY
========

PARAMETERS
 * /controller_stopper/consistent_controllers: ['joint_state_con...
 * /force_torque_sensor_controller/publish_rate: 125
 * /force_torque_sensor_controller/type: force_torque_sens...
 * /hardware_control_loop/loop_hz: 125
 * /hardware_interface/joints: ['shoulder_pan_jo...
 * /joint_state_controller/publish_rate: 125
 * /joint_state_controller/type: joint_state_contr...
 * /pos_traj_controller/action_monitor_rate: 10
 * /pos_traj_controller/constraints/elbow_joint/goal: 0.1
 * /pos_traj_controller/constraints/elbow_joint/trajectory: 0.2
 * /pos_traj_controller/constraints/goal_time: 0.6
 * /pos_traj_controller/constraints/shoulder_lift_joint/goal: 0.1
 * /pos_traj_controller/constraints/shoulder_lift_joint/trajectory: 0.2
 * /pos_traj_controller/constraints/shoulder_pan_joint/goal: 0.1
 * /pos_traj_controller/constraints/shoulder_pan_joint/trajectory: 0.2
 * /pos_traj_controller/constraints/stopped_velocity_tolerance: 0.05
 * /pos_traj_controller/constraints/wrist_1_joint/goal: 0.1
 * /pos_traj_controller/constraints/wrist_1_joint/trajectory: 0.2
 * /pos_traj_controller/constraints/wrist_2_joint/goal: 0.1
 * /pos_traj_controller/constraints/wrist_2_joint/trajectory: 0.2
 * /pos_traj_controller/constraints/wrist_3_joint/goal: 0.1
 * /pos_traj_controller/constraints/wrist_3_joint/trajectory: 0.2
 * /pos_traj_controller/joints: ['shoulder_pan_jo...
 * /pos_traj_controller/state_publish_rate: 125
 * /pos_traj_controller/stop_trajectory_duration: 0.5
 * /pos_traj_controller/type: position_controll...
 * /robot_description: <?xml version="1....
 * /rosdistro: kinetic
 * /rosversion: 1.12.14
 * /scaled_pos_traj_controller/action_monitor_rate: 10
 * /scaled_pos_traj_controller/constraints/elbow_joint/goal: 0.1
 * /scaled_pos_traj_controller/constraints/elbow_joint/trajectory: 0.2
 * /scaled_pos_traj_controller/constraints/goal_time: 0.6
 * /scaled_pos_traj_controller/constraints/shoulder_lift_joint/goal: 0.1
 * /scaled_pos_traj_controller/constraints/shoulder_lift_joint/trajectory: 0.2
 * /scaled_pos_traj_controller/constraints/shoulder_pan_joint/goal: 0.1
 * /scaled_pos_traj_controller/constraints/shoulder_pan_joint/trajectory: 0.2
 * /scaled_pos_traj_controller/constraints/stopped_velocity_tolerance: 0.05
 * /scaled_pos_traj_controller/constraints/wrist_1_joint/goal: 0.1
 * /scaled_pos_traj_controller/constraints/wrist_1_joint/trajectory: 0.2
 * /scaled_pos_traj_controller/constraints/wrist_2_joint/goal: 0.1
 * /scaled_pos_traj_controller/constraints/wrist_2_joint/trajectory: 0.2
 * /scaled_pos_traj_controller/constraints/wrist_3_joint/goal: 0.1
 * /scaled_pos_traj_controller/constraints/wrist_3_joint/trajectory: 0.2
 * /scaled_pos_traj_controller/joints: ['shoulder_pan_jo...
 * /scaled_pos_traj_controller/state_publish_rate: 125
 * /scaled_pos_traj_controller/stop_trajectory_duration: 0.5
 * /scaled_pos_traj_controller/type: position_controll...
 * /speed_scaling_state_controller/publish_rate: 125
 * /speed_scaling_state_controller/type: ur_controllers/Sp...
 * /ur_hardware_interface/headless_mode: False
 * /ur_hardware_interface/input_recipe_file: /home/shu/catkin_...
 * /ur_hardware_interface/kinematics/forearm/pitch: 0.000670340723934
 * /ur_hardware_interface/kinematics/forearm/roll: 0.000150389826896
 * /ur_hardware_interface/kinematics/forearm/x: -0.425316567681
 * /ur_hardware_interface/kinematics/forearm/y: 0
 * /ur_hardware_interface/kinematics/forearm/yaw: -0.000129296667033
 * /ur_hardware_interface/kinematics/forearm/z: 0
 * /ur_hardware_interface/kinematics/hash: calib_72470218391...
 * /ur_hardware_interface/kinematics/shoulder/pitch: 0
 * /ur_hardware_interface/kinematics/shoulder/roll: 0
 * /ur_hardware_interface/kinematics/shoulder/x: 0
 * /ur_hardware_interface/kinematics/shoulder/y: 0
 * /ur_hardware_interface/kinematics/shoulder/yaw: 8.77666344309e-05
 * /ur_hardware_interface/kinematics/shoulder/z: 0.0893072092318
 * /ur_hardware_interface/kinematics/upper_arm/pitch: 0
 * /ur_hardware_interface/kinematics/upper_arm/roll: 1.57027963108
 * /ur_hardware_interface/kinematics/upper_arm/x: 8.02597230883e-05
 * /ur_hardware_interface/kinematics/upper_arm/y: 0
 * /ur_hardware_interface/kinematics/upper_arm/yaw: -0.000256378975585
 * /ur_hardware_interface/kinematics/upper_arm/z: 0
 * /ur_hardware_interface/kinematics/wrist_1/pitch: 3.139985598
 * /ur_hardware_interface/kinematics/wrist_1/roll: 3.13404114506
 * /ur_hardware_interface/kinematics/wrist_1/x: -0.392232740578
 * /ur_hardware_interface/kinematics/wrist_1/y: 0.000835269385168
 * /ur_hardware_interface/kinematics/wrist_1/yaw: 3.14149191057
 * /ur_hardware_interface/kinematics/wrist_1/z: 0.110607503694
 * /ur_hardware_interface/kinematics/wrist_2/pitch: 0
 * /ur_hardware_interface/kinematics/wrist_2/roll: 1.57084155987
 * /ur_hardware_interface/kinematics/wrist_2/x: 3.88210393792e-05
 * /ur_hardware_interface/kinematics/wrist_2/y: -0.0949976987238
 * /ur_hardware_interface/kinematics/wrist_2/yaw: 5.57612237128e-05
 * /ur_hardware_interface/kinematics/wrist_2/z: -4.2970377454e-06
 * /ur_hardware_interface/kinematics/wrist_3/pitch: 3.14159265359
 * /ur_hardware_interface/kinematics/wrist_3/roll: 1.57047457779
 * /ur_hardware_interface/kinematics/wrist_3/x: 7.18408705395e-05
 * /ur_hardware_interface/kinematics/wrist_3/y: 0.0828382643859
 * /ur_hardware_interface/kinematics/wrist_3/yaw: -3.14148013852
 * /ur_hardware_interface/kinematics/wrist_3/z: -2.66531297376e-05
 * /ur_hardware_interface/output_recipe_file: /home/shu/catkin_...
 * /ur_hardware_interface/robot_ip: 169.254.198.35
 * /ur_hardware_interface/script_file: /home/shu/catkin_...
 * /ur_hardware_interface/tf_prefix: 
 * /ur_hardware_interface/tool_baud_rate: 115200
 * /ur_hardware_interface/tool_parity: 0
 * /ur_hardware_interface/tool_rx_idle_chars: 1.5
 * /ur_hardware_interface/tool_stop_bits: 1
 * /ur_hardware_interface/tool_tx_idle_chars: 3.5
 * /ur_hardware_interface/tool_voltage: 0
 * /ur_hardware_interface/use_tool_communication: False

NODES
  /
    controller_stopper (controller_stopper/node)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    ros_control_controller_spawner (controller_manager/spawner)
    ros_control_stopped_spawner (controller_manager/spawner)
    ur_hardware_interface (ur_robot_driver/ur_robot_driver_node)
  /ur_hardware_interface/
    ur_robot_state_helper (ur_robot_driver/robot_state_helper)

auto-starting new master
process[master]: started with pid [26814]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to e301052a-0f7d-11ea-9d0c-3c918077fd81
process[rosout-1]: started with pid [26830]
started core service [/rosout]
process[robot_state_publisher-2]: started with pid [26849]
process[ur_hardware_interface-3]: started with pid [26850]
process[ros_control_controller_spawner-4]: started with pid [26857]
process[ros_control_stopped_spawner-5]: started with pid [26871]
process[controller_stopper-6]: started with pid [26890]
[ INFO] [1574684395.131712683]: Initializing dashboard client
[ INFO] [1574684395.133002127]: Connected: Universal Robots Dashboard Server

process[ur_hardware_interface/ur_robot_state_helper-7]: started with pid [26907]
[ INFO] [1574684395.147398645]: Waiting for controller manager service to come up on /controller_manager/switch_controller
[ INFO] [1574684395.148325153]: waitForService: Service [/controller_manager/switch_controller] has not been advertised, waiting...
[ INFO] [1574684395.159114696]: Initializing urdriver
[ INFO] [1574684395.159460126]: Checking if calibration data matches connected robot.
[ WARN] [1574684395.159751885]: No realtime capabilities found. Consider using a realtime system for better performance
[INFO] [1574684395.385368]: Controller Spawner: Waiting for service controller_manager/load_controller
[INFO] [1574684395.386575]: Controller Spawner: Waiting for service controller_manager/load_controller
[ INFO] [1574684395.535172595]: Calibration checked successfully.
[ WARN] [1574684396.237150652]: No realtime capabilities found. Consider using a realtime system for better performance
[ INFO] [1574684396.312434961]: Setting up RTDE communication with frequency 125.000000
[ INFO] [1574684397.333403855]: Robot ready to receive control commands.
[ WARN] [1574684397.340061000]: No realtime capabilities found. Consider using a realtime system for better performance
[ INFO] [1574684397.340270528]: Loaded ur_robot_driver hardware_interface
[ INFO] [1574684397.391057035]: waitForService: Service [/controller_manager/switch_controller] is now available.
[ INFO] [1574684397.391091579]: Service available.
[ INFO] [1574684397.391111114]: Waiting for controller list service to come up on /controller_manager/list_controllers
[ INFO] [1574684397.391720306]: Service available.
[INFO] [1574684397.503742]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1574684397.504913]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1574684397.506956]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1574684397.507879]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1574684397.509686]: Loading controller: joint_state_controller
[INFO] [1574684397.510480]: Loading controller: pos_traj_controller
[INFO] [1574684397.525853]: Loading controller: scaled_pos_traj_controller
[INFO] [1574684397.613978]: Controller Spawner: Loaded controllers: pos_traj_controller
[ INFO] [1574684397.615694377]: Robot's safety mode is now NORMAL
[ INFO] [1574684397.616154860]: Robot mode is now RUNNING
[INFO] [1574684397.693950]: Loading controller: speed_scaling_state_controller
[INFO] [1574684397.709805]: Loading controller: force_torque_sensor_controller
[INFO] [1574684397.725947]: Controller Spawner: Loaded controllers: joint_state_controller, scaled_pos_traj_controller, speed_scaling_state_controller, force_torque_sensor_controller
[INFO] [1574684397.733998]: Started controllers: joint_state_controller, scaled_pos_traj_controller, speed_scaling_state_controller, force_torque_sensor_controller

and of roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true

... logging to /home/shu/.ros/log/e301052a-0f7d-11ea-9d0c-3c918077fd81/roslaunch-sewts-ideapad-002-27342.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://sewts-ideapad-002:37179/

SUMMARY
========

PARAMETERS
 * /move_group/allow_trajectory_execution: True
 * /move_group/capabilities: move_group/MoveGr...
 * /move_group/controller_list: [{'default': True...
 * /move_group/endeffector/planner_configs: ['SBLkConfigDefau...
 * /move_group/jiggle_fraction: 0.05
 * /move_group/manipulator/longest_valid_segment_fraction: 0.01
 * /move_group/manipulator/planner_configs: ['SBLkConfigDefau...
 * /move_group/max_range: 5.0
 * /move_group/max_safe_path_cost: 1
 * /move_group/moveit_controller_manager: moveit_simple_con...
 * /move_group/moveit_manage_controllers: True
 * /move_group/octomap_resolution: 0.025
 * /move_group/planner_configs/BKPIECEkConfigDefault/border_fraction: 0.9
 * /move_group/planner_configs/BKPIECEkConfigDefault/failed_expansion_score_factor: 0.5
 * /move_group/planner_configs/BKPIECEkConfigDefault/min_valid_path_fraction: 0.5
 * /move_group/planner_configs/BKPIECEkConfigDefault/range: 0.0
 * /move_group/planner_configs/BKPIECEkConfigDefault/type: geometric::BKPIECE
 * /move_group/planner_configs/ESTkConfigDefault/goal_bias: 0.05
 * /move_group/planner_configs/ESTkConfigDefault/range: 0.0
 * /move_group/planner_configs/ESTkConfigDefault/type: geometric::EST
 * /move_group/planner_configs/KPIECEkConfigDefault/border_fraction: 0.9
 * /move_group/planner_configs/KPIECEkConfigDefault/failed_expansion_score_factor: 0.5
 * /move_group/planner_configs/KPIECEkConfigDefault/goal_bias: 0.05
 * /move_group/planner_configs/KPIECEkConfigDefault/min_valid_path_fraction: 0.5
 * /move_group/planner_configs/KPIECEkConfigDefault/range: 0.0
 * /move_group/planner_configs/KPIECEkConfigDefault/type: geometric::KPIECE
 * /move_group/planner_configs/LBKPIECEkConfigDefault/border_fraction: 0.9
 * /move_group/planner_configs/LBKPIECEkConfigDefault/min_valid_path_fraction: 0.5
 * /move_group/planner_configs/LBKPIECEkConfigDefault/range: 0.0
 * /move_group/planner_configs/LBKPIECEkConfigDefault/type: geometric::LBKPIECE
 * /move_group/planner_configs/PRMkConfigDefault/max_nearest_neighbors: 10
 * /move_group/planner_configs/PRMkConfigDefault/type: geometric::PRM
 * /move_group/planner_configs/PRMstarkConfigDefault/type: geometric::PRMstar
 * /move_group/planner_configs/RRTConnectkConfigDefault/range: 0.0
 * /move_group/planner_configs/RRTConnectkConfigDefault/type: geometric::RRTCon...
 * /move_group/planner_configs/RRTkConfigDefault/goal_bias: 0.05
 * /move_group/planner_configs/RRTkConfigDefault/range: 0.0
 * /move_group/planner_configs/RRTkConfigDefault/type: geometric::RRT
 * /move_group/planner_configs/RRTstarkConfigDefault/delay_collision_checking: 1
 * /move_group/planner_configs/RRTstarkConfigDefault/goal_bias: 0.05
 * /move_group/planner_configs/RRTstarkConfigDefault/range: 0.0
 * /move_group/planner_configs/RRTstarkConfigDefault/type: geometric::RRTstar
 * /move_group/planner_configs/SBLkConfigDefault/range: 0.0
 * /move_group/planner_configs/SBLkConfigDefault/type: geometric::SBL
 * /move_group/planner_configs/TRRTkConfigDefault/frountierNodeRatio: 0.1
 * /move_group/planner_configs/TRRTkConfigDefault/frountier_threshold: 0.0
 * /move_group/planner_configs/TRRTkConfigDefault/goal_bias: 0.05
 * /move_group/planner_configs/TRRTkConfigDefault/init_temperature: 10e-6
 * /move_group/planner_configs/TRRTkConfigDefault/k_constant: 0.0
 * /move_group/planner_configs/TRRTkConfigDefault/max_states_failed: 10
 * /move_group/planner_configs/TRRTkConfigDefault/min_temperature: 10e-10
 * /move_group/planner_configs/TRRTkConfigDefault/range: 0.0
 * /move_group/planner_configs/TRRTkConfigDefault/temp_change_factor: 2.0
 * /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
 * /move_group/trajectory_execution/allowed_execution_duration_scaling: 1.2
 * /move_group/trajectory_execution/allowed_goal_duration_margin: 0.5
 * /move_group/trajectory_execution/execution_duration_monitoring: False
 * /move_group/use_controller_manager: False
 * /robot_description_kinematics/manipulator/kinematics_solver: trac_ik_kinematic...
 * /robot_description_kinematics/manipulator/kinematics_solver_attempts: 3
 * /robot_description_kinematics/manipulator/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/manipulator/kinematics_solver_timeout: 0.01
 * /robot_description_kinematics/manipulator/solve_type: Manipulation2
 * /robot_description_planning/joint_limits/elbow_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/elbow_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/elbow_joint/max_acceleration: 3.15
 * /robot_description_planning/joint_limits/elbow_joint/max_velocity: 3.15
 * /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: 3.15
 * /robot_description_planning/joint_limits/shoulder_lift_joint/max_velocity: 3.15
 * /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: 3.15
 * /robot_description_planning/joint_limits/shoulder_pan_joint/max_velocity: 3.15
 * /robot_description_planning/joint_limits/wrist_1_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/wrist_1_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/wrist_1_joint/max_acceleration: 3.2
 * /robot_description_planning/joint_limits/wrist_1_joint/max_velocity: 3.2
 * /robot_description_planning/joint_limits/wrist_2_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/wrist_2_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/wrist_2_joint/max_acceleration: 3.2
 * /robot_description_planning/joint_limits/wrist_2_joint/max_velocity: 3.2
 * /robot_description_planning/joint_limits/wrist_3_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/wrist_3_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/wrist_3_joint/max_acceleration: 3.2
 * /robot_description_planning/joint_limits/wrist_3_joint/max_velocity: 3.2
 * /robot_description_semantic: <?xml version="1....
 * /rosdistro: kinetic
 * /rosversion: 1.12.14

NODES
  /
    move_group (moveit_ros_move_group/move_group)

ROS_MASTER_URI=http://localhost:11311

process[move_group-1]: started with pid [27362]
[ INFO] [1574684401.833724797]: Loading robot model 'ur5'...
[ WARN] [1574684401.833771432]: Skipping virtual joint 'fixed_base' because its child frame 'base_link' does not match the URDF frame 'world'
[ INFO] [1574684401.833782142]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1574684401.873049056]: IK Using joint shoulder_link -6.28319 6.28319
[ INFO] [1574684401.873076502]: IK Using joint upper_arm_link -6.28319 6.28319
[ INFO] [1574684401.873092438]: IK Using joint forearm_link -3.14159 3.14159
[ INFO] [1574684401.873102406]: IK Using joint wrist_1_link -6.28319 6.28319
[ INFO] [1574684401.873111716]: IK Using joint wrist_2_link -6.28319 6.28319
[ INFO] [1574684401.873120892]: IK Using joint wrist_3_link -6.28319 6.28319
[ INFO] [1574684401.873134026]: Looking in common namespaces for param name: manipulator/position_only_ik
[ INFO] [1574684401.874291167]: Looking in common namespaces for param name: manipulator/solve_type
[ INFO] [1574684401.875881932]: Using solve type Manipulation2
[ INFO] [1574684401.898915187]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1574684401.901086160]: MoveGroup debug mode is OFF
Starting context monitors...
[ INFO] [1574684401.901115018]: Starting scene monitor
[ INFO] [1574684401.903142747]: Listening to '/planning_scene'
[ INFO] [1574684401.903166072]: Starting world geometry monitor
[ INFO] [1574684401.905106361]: Listening to '/collision_object' using message notifier with target frame '/world '
[ INFO] [1574684401.906997552]: Listening to '/planning_scene_world' for planning scene world geometry
[ INFO] [1574684401.913668092]: Listening to '/attached_collision_object' for attached collision objects
Context monitors started.
[ INFO] [1574684401.933429980]: Initializing OMPL interface using ROS parameters
[ INFO] [1574684401.952189659]: Using planning interface 'OMPL'
[ INFO] [1574684401.954236784]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1574684401.954594224]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1574684401.954915281]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1574684401.955231614]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1574684401.955527350]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1574684401.955822963]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1574684401.955860170]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1574684401.955874173]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1574684401.955890927]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1574684401.955904664]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1574684401.955918480]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1574684402.227946532]: Added FollowJointTrajectory controller for /scaled_pos_traj_controller
[ INFO] [1574684402.228107488]: Returned 1 controllers in list
[ INFO] [1574684402.263652579]: 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] [1574684402.431023705]: 

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

[ INFO] [1574684402.431186168]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1574684402.431233647]: MoveGroup context initialization complete

You can start planning now!

[ INFO] [1574684431.548546279]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1574684431.551236491]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1574684431.552088822]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1574684434.370715825]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1574684434.370752006]: Solution found in 2.818948 seconds
[ INFO] [1574684434.409350084]: SimpleSetup: Path simplification took 0.002416 seconds and changed from 3 to 2 states
[ INFO] [1574684450.899686941]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1574684450.900425061]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1574684450.900816206]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1574684450.926512730]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1574684450.926599523]: Solution found in 0.025874 seconds
[ INFO] [1574684450.942298582]: SimpleSetup: Path simplification took 0.011022 seconds and changed from 3 to 2 states
[ INFO] [1574684458.147771793]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1574684458.148691643]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1574684458.148949821]: RRTConnect: Starting planning with 1 states already in datastructure
[ERROR] [1574684463.150180926]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1574684463.150239704]: RRTConnect: Created 1 states (1 start + 0 goal)
[ INFO] [1574684463.150267946]: No solution found after 5.001417 seconds
[ INFO] [1574684463.158762281]: Unable to solve the planning problem

Here's the script I'm using to move the UR5. It's basically the MoveGroupPythonInterfaceTutorial:

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
import math
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
from moveit_msgs.msg import RobotState
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from tf.transformations import quaternion_from_euler

class MoveGroupPythonIntefaceTutorial(object):
  def __init__(self):
    super(MoveGroupPythonIntefaceTutorial, self).__init__()
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_python_interface_tutorial',
                    anonymous=True)
    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()

    group_name = "manipulator" 
    group = moveit_commander.MoveGroupCommander(group_name)
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                                   moveit_msgs.msg.DisplayTrajectory,
                                                   queue_size=20)

    planning_frame = group.get_planning_frame()
    #print "============ Reference frame: %s" % planning_frame

    eef_link = group.get_end_effector_link()
    #print "============ End effector: %s" % eef_link

    group_names = robot.get_group_names()
    #print "============ Robot Groups:", robot.get_group_names()

    # Sometimes for debugging it is useful to print the entire state of the
    # robot:
    #print "============ Printing robot state"
    print(robot.get_current_state())
    #print ""

    # Misc variables
    self.robot = robot
    self.scene = scene
    self.group = group
    self.display_trajectory_publisher = display_trajectory_publisher
    self.planning_frame = planning_frame
    self.eef_link = eef_link
    self.group_names = group_names

def go_to_pose_goal(self):
    group = self.group

    pose_goal.position.x = group.get_current_pose().pose.position.x;
    pose_goal.position.y = group.get_current_pose().pose.position.y +0.1;
    pose_goal.position.z = group.get_current_pose().pose.position.z;
    pose_goal.orientation.x = group.get_current_pose().pose.orientation.x;
    pose_goal.orientation.y = group.get_current_pose().pose.orientation.y;
    pose_goal.orientation.z = group.get_current_pose().pose.orientation.z;
    pose_goal.orientation.w = group.get_current_pose().pose.orientation.w;
    group.set_pose_target(pose_goal)
    print(pose_goal)

    plan = group.go(wait=True)
    group.plan()
    group.stop()
    group.clear_pose_targets()

    current_pose = self.group.get_current_pose().pose
    return all_close(pose_goal, current_pose, 0.01)

def main():
  counter = 0
  while(True):
      try:
        print("============ PUT RVIZ ON THE SCREEN AND THEN PRESS ENTER")
        raw_input()
        tutorial = MoveGroupPythonIntefaceTutorial()
        print("GOING")
        rospy.sleep(2)
        tutorial.go_to_pose_goal()

        print("============ DONE")

      #except rospy.ROSInterrDuptException:
        #return
      except KeyboardInterrupt:
        return

if __name__ == '__main__':
  main()

I am quite sure the goal position is reachable - could someone help me figure out what I'm doing wrong?

Asked by shu on 2019-11-25 07:55:06 UTC

Comments

Answers

Maybe the robot is starting beyond a ROS joint limit. The wrist on the UR5 can rotate infinitely, which confuses ROS if it is beyond -360*,360*.

It could be that it thinks the robot is in collision, too, but I think there would have been some more errors printed if that were the case.

Asked by AndyZe on 2019-11-26 09:29:25 UTC

Comments

Unfortunately I don't think this is the case as I've tried giving the robot goals that have joints within -180 and 180. I've set a joint goal of [-1.8963, -0.5800, -2.2339, -1.8976, 1.5629, 2.0322] that always works, but the planner often fails for finding solutions for pose goals after reaching this joint goal. I have enjoyed a bit more success adding goals between the final goal and the starting position, but if the planner fails to find the solution for one it will never reach its end position.

Asked by shu on 2019-11-28 05:14:04 UTC