Real UR5 doesn't stop moving when it should
In gazebo the giving the ur5 coordinate goals is working perfectly. However, with the real ur5 there are some existing disparities with its movements.
Here are my start up commands:
roslaunch ur_modern_driver ur5_bringup.launch robot_ip:=ROBOTIP [reverse_port:=REVERSE_PORT]
roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true
roslaunch ur5_moveit_config moveit_rviz.launch config:=true
I am using the TracIK plugin for my kinematics solver. The following code is how i define the ur5 to move 0.1 meters in the y direction.
pose_goal = geometry_msgs.msg.Pose()
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)
The ur5 is moving in the correct direction every time, but there's quite a lot of inaccuracy in its expected and actual end-effector positions after stopping, the largest of which I've seen is an actual y coordinate of 0.43 versus an expected y coordinate of 0.508. The disparity between the actual and expected coordinates is not the same every time.
Can someone point me in the right direction of where I should be looking to fix this issue? Please let me know if I need to post more information.
Edit - now with full planning execution log after switching to UniversalRobots/UniversalRobotsROS_Driver:
shu@sewts-ideapad-002:/opt/ros/kinetic/share/ur5_moveit_config/config$ roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true
... logging to /home/shu/.ros/log/0c4ff900-0c7b-11ea-abbf-98fa9b14f60a/roslaunch-sewts-ideapad-002-28607.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:43151/
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.0gz1
* /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 [28627]
[ INFO] [1574353327.897032338]: Loading robot model 'ur5'...
[ WARN] [1574353327.897081694]: Skipping virtual joint 'fixed_base' because its child frame 'base_link' does not match the URDF frame 'world'
[ INFO] [1574353327.897097681]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1574353327.938687844]: IK Using joint shoulder_link -6.28319 6.28319
[ INFO] [1574353327.938711653]: IK Using joint upper_arm_link -6.28319 6.28319
[ INFO] [1574353327.938720071]: IK Using joint forearm_link -3.14159 3.14159
[ INFO] [1574353327.938727794]: IK Using joint wrist_1_link -6.28319 6.28319
[ INFO] [1574353327.938738967]: IK Using joint wrist_2_link -6.28319 6.28319
[ INFO] [1574353327.938748256]: IK Using joint wrist_3_link -6.28319 6.28319
[ INFO] [1574353327.938762155]: Looking in common namespaces for param name: manipulator/position_only_ik
[ INFO] [1574353327.940070404]: Looking in common namespaces for param name: manipulator/solve_type
[ INFO] [1574353327.942067769]: Using solve type Manipulation2
[ INFO] [1574353327.965545026]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1574353327.967543475]: MoveGroup debug mode is OFF
Starting context monitors...
[ INFO] [1574353327.967565320]: Starting scene monitor
[ INFO] [1574353327.969323588]: Listening to '/planning_scene'
[ INFO] [1574353327.969339594]: Starting world geometry monitor
[ INFO] [1574353327.971398606]: Listening to '/collision_object' using message notifier with target frame '/world '
[ INFO] [1574353327.973437948]: Listening to '/planning_scene_world' for planning scene world geometry
[ INFO] [1574353327.980282358]: Listening to '/attached_collision_object' for attached collision objects
Context monitors started.
[ INFO] [1574353327.999511248]: Initializing OMPL interface using ROS parameters
[ INFO] [1574353328.018657218]: Using planning interface 'OMPL'
[ INFO] [1574353328.020718940]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1574353328.021076216]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1574353328.021416209]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1574353328.021768318]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1574353328.022097351]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1574353328.022381381]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1574353328.022409932]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1574353328.022420776]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1574353328.022431443]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1574353328.022441401]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1574353328.022451397]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1574353328.326573883]: Added FollowJointTrajectory controller for /scaled_pos_traj_controller
[ INFO] [1574353328.326746161]: Returned 1 controllers in list
[ INFO] [1574353328.360978302]: 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] [1574353328.525438728]:
********************************************************
* MoveGroup using:
* - ApplyPlanningSceneService
* - ClearOctomapService
* - CartesianPathService
* - ExecuteTrajectoryAction
* - GetPlanningSceneService
* - KinematicsService
* - MoveAction
* - PickPlaceAction
* - MotionPlanService
* - QueryPlannersService
* - StateValidationService
********************************************************
[ INFO] [1574353328.525594788]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1574353328.525663853]: MoveGroup context initialization complete
You can start planning now!
[ INFO] [1574353360.647292706]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1574353360.650081281]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1574353360.651026382]: RRTConnect: Starting planning with 1 states already in datastructure
[ERROR] [1574353365.653833286]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1574353365.653884587]: RRTConnect: Created 1 states (1 start + 0 goal)
[ INFO] [1574353365.653916727]: No solution found after 5.003178 seconds
[ INFO] [1574353366.663765610]: Unable to solve the planning problem
Asked by shu on 2019-11-21 05:21:41 UTC
Answers
Please use UniversalRobots/Universal_Robots_ROS_Driver with current versions of Polyscope and CB3 or e-series controllers.
Asked by gvdhoorn on 2019-11-21 10:24:00 UTC
Comments
Thank you for pointing me to the right package.
I've done the following:
1. Followed the building instructions in the Universal_robots_ROS_Driver. ur_robot_driver wasn't being recognized as a package so I put this in the ~/catkin_ws/src and rebuilt my workspace.
2. Extracted calibration information about the UR5.
3. Changed my ur5_moveit_config/config/controllers.yaml to look for the action server /scaled_pos_traj_controller/follow_joint_trajectory
4. Launched the ur5_bringup file in ur_robot_driver with the calibration file passed in
5. roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true
6. Ran my script to try and move the robot in the y coordinate direction.
I am now getting RRTConnect: Unable to sample any valid states for goal tree
as an Error message while trying to run my script. I will update the question to include the log as it's too long for the comment.
Asked by shu on 2019-11-21 11:34:59 UTC
This should be asked in a new question.
Edit: and it has: #q338484.
Asked by gvdhoorn on 2019-11-25 05:00:08 UTC
You may try the new driver from FZI https://github.com/UniversalRobots/Universal_Robots_ROS_Driver
Asked by duck-development on 2019-11-21 11:54:14 UTC
Comments
I've given that a try and now am getting a RRTConnect: Unable to sample any valid states for goal tree
error (see my comment on the question)
Asked by shu on 2019-11-21 11:56:47 UTC
Comments