Giters

ros-industrial / motoman

ROS-Industrial Motoman support (http://wiki.ros.org/motoman)

Geek Repo:Geek Repo

Github PK Tool:Github PK Tool

HC10DTP + MoveIt - Running into singularities

tall1 opened this issue · comments

commented

Hi all,
I'm using #541 and MoveIt for controlling my HC10DTP motoman cobot.

After playing with this setup for a while I noticed that I'm running into singularities from time to time, meaning that my robot halts when it reaches a singularity and then I have to move it manually (via teach mode) out of that position and re-plan (sometimes multiple times).

I want to know if there is a way to tell MoveIt to avoid singularities.

The output from the console

setting /run_id to ccedee38-0da8-11ee-9d24-6f0e410225a1
process[rosout-1]: started with pid [10839]
started core service [/rosout]
process[joint_state-2]: started with pid [10846]
process[motion_streaming_interface-3]: started with pid [10847]
process[io_relay-4]: started with pid [10848]
process[joint_trajectory_action-5]: started with pid [10853]
process[robot_state_publisher-6]: started with pid [10860]
process[move_group-7]: started with pid [10866]
process[rviz_hvmr_Latitude_5430_10821_5853370145707960593-8]: started with pid [10873]
[ WARN] [1687072914.439785643] [/motion_streaming_interface]: Tried to connect when socket already in connected state
[ INFO] [1687072914.474269963] [/move_group]: Loading robot model 'motoman_hc10dtp_b00'...
[ INFO] [1687072914.535150088] [${node}]: rviz version 1.14.20
[ INFO] [1687072914.535200509] [${node}]: compiled against Qt version 5.12.8
[ INFO] [1687072914.535211185] [${node}]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1687072914.542837509] [/rviz_hvmr_Latitude_5430_10821_5853370145707960593]: Forcing OpenGl version 0.
[ INFO] [1687072914.702082546] [/rviz_hvmr_Latitude_5430_10821_5853370145707960593]: Stereo is NOT SUPPORTED
[ INFO] [1687072914.702119674] [/rviz_hvmr_Latitude_5430_10821_5853370145707960593]: OpenGL device: Mesa Intel(R) Graphics (ADL GT2)
[ INFO] [1687072914.702129836] [/rviz_hvmr_Latitude_5430_10821_5853370145707960593]: OpenGl version: 4.6 (GLSL 4.6) limited to GLSL 1.4 on Mesa system.
[ INFO] [1687072914.730777976] [/move_group]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1687072914.733231759] [/move_group]: Listening to 'joint_states' for joint states
[ INFO] [1687072914.735647529] [/move_group]: Listening to '/attached_collision_object' for attached collision objects
[ INFO] [1687072914.735671185] [/move_group]: Starting planning scene monitor
[ INFO] [1687072914.736737685] [/move_group]: Listening to '/planning_scene'
[ INFO] [1687072914.736757899] [/move_group]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1687072914.737773719] [/move_group]: Listening to '/collision_object'
[ INFO] [1687072914.738993430] [/move_group]: Listening to '/planning_scene_world' for planning scene world geometry
[ INFO] [1687072914.774418372] [/move_group]: Listening to '/camera1/depth/color/points' using message filter with target frame 'base_link '
[ INFO] [1687072914.776274876] [/move_group]: Listening to '/camera2/depth/color/points' using message filter with target frame 'base_link '
[ INFO] [1687072914.776315477] [/move_group]: Loading planning pipeline 'chomp'
[ INFO] [1687072914.790762696] [/move_group]: Using planning interface 'CHOMP'
[ INFO] [1687072914.792600072] [/move_group]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1687072914.792770972] [/move_group]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1687072914.792921069] [/move_group]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1687072914.793078184] [/move_group]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1687072914.793234209] [/move_group]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1687072914.793388008] [/move_group]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1687072914.793403058] [/move_group]: Using planning request adapter 'Limiting Max Cartesian Speed'
[ INFO] [1687072914.793409594] [/move_group]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1687072914.793414524] [/move_group]: Using planning request adapter 'Resolve constraint frames to robot links'
[ INFO] [1687072914.793418993] [/move_group]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1687072914.793423218] [/move_group]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1687072914.793427386] [/move_group]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1687072914.793431670] [/move_group]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1687072914.794093767] [/move_group]: Loading planning pipeline 'ompl'
[ INFO] [1687072914.821287137] [/move_group]: Using planning interface 'OMPL'
[ INFO] [1687072914.822714650] [/move_group]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1687072914.823032605] [/move_group]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1687072914.823308337] [/move_group]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1687072914.823586887] [/move_group]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1687072914.823868240] [/move_group]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1687072914.824137788] [/move_group]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1687072914.824166788] [/move_group]: Using planning request adapter 'Limiting Max Cartesian Speed'
[ INFO] [1687072914.824178628] [/move_group]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1687072914.824191020] [/move_group]: Using planning request adapter 'Resolve constraint frames to robot links'
[ INFO] [1687072914.824204378] [/move_group]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1687072914.824216061] [/move_group]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1687072914.824226802] [/move_group]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1687072914.824236167] [/move_group]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1687072914.824283269] [/move_group]: Loading planning pipeline 'pilz_industrial_motion_planner'
[ INFO] [1687072914.826834505] [/move_group]: Reading limits from namespace /robot_description_planning
[ INFO] [1687072914.837408846] [/move_group]: Available plugins: pilz_industrial_motion_planner::PlanningContextLoaderCIRC pilz_industrial_motion_planner::PlanningContextLoaderLIN pilz_industrial_motion_planner::PlanningContextLoaderPTP
[ INFO] [1687072914.837430628] [/move_group]: About to load: pilz_industrial_motion_planner::PlanningContextLoaderCIRC
[ INFO] [1687072914.838415606] [/move_group]: Registered Algorithm [CIRC]
[ INFO] [1687072914.838429106] [/move_group]: About to load: pilz_industrial_motion_planner::PlanningContextLoaderLIN
[ INFO] [1687072914.839159964] [/move_group]: Registered Algorithm [LIN]
[ INFO] [1687072914.839170815] [/move_group]: About to load: pilz_industrial_motion_planner::PlanningContextLoaderPTP
[ INFO] [1687072914.839858014] [/move_group]: Registered Algorithm [PTP]
[ INFO] [1687072914.839868357] [/move_group]: Using planning interface 'Pilz Industrial Motion Planner'
[ INFO] [1687072915.129882046] [/move_group]: Added FollowJointTrajectory controller for
[ INFO] [1687072915.130255353] [/move_group]: Returned 1 controllers in list
[ INFO] [1687072915.147172027] [/move_group]: Trajectory execution is managing controllers
[ INFO] [1687072915.147236105] [/move_group]: MoveGroup debug mode is OFF
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'...
Loading 'pilz_industrial_motion_planner/MoveGroupSequenceAction'...
[ INFO] [1687072915.200248432] [/move_group]: initialize move group sequence action
[ INFO] [1687072915.203482398] [/move_group]: Reading limits from namespace /robot_description_planning
Loading 'pilz_industrial_motion_planner/MoveGroupSequenceService'...
[ INFO] [1687072915.210598357] [/move_group]: Reading limits from namespace /robot_description_planning
[ INFO] [1687072915.216388819] [/move_group]:

  • MoveGroup using:
  • - ApplyPlanningSceneService

  • - ClearOctomapService

  • - CartesianPathService

  • - ExecuteTrajectoryAction

  • - GetPlanningSceneService

  • - KinematicsService

  • - MoveAction

  • - PickPlaceAction

  • - MotionPlanService

  • - QueryPlannersService

  • - StateValidationService

  • - SequenceAction

  • - SequenceService

[ INFO] [1687072915.216629619] [/move_group]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1687072915.216650479] [/move_group]: MoveGroup context initialization complete

You can start planning now!

[ INFO] [1687072918.070266561] [/rviz_hvmr_Latitude_5430_10821_5853370145707960593]: Loading robot model 'motoman_hc10dtp_b00'...
[ INFO] [1687072918.326276779] [/rviz_hvmr_Latitude_5430_10821_5853370145707960593]: Starting planning scene monitor
[ INFO] [1687072918.330781936] [/rviz_hvmr_Latitude_5430_10821_5853370145707960593]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1687072918.396908300] [/rviz_hvmr_Latitude_5430_10821_5853370145707960593]: Constructing new MoveGroup connection for group 'arm' in namespace ''
[ INFO] [1687072919.543792784] [/rviz_hvmr_Latitude_5430_10821_5853370145707960593]: Ready to take commands for planning group arm.
[ WARN] [1687072952.841977358] [/motion_streaming_interface]: Motoman robot is now enabled and will accept motion commands.
[ WARN] [1687072969.357615971] [/motion_streaming_interface]: Motoman robot is now disabled and will NOT accept motion commands.
[ WARN] [1687073251.293560528] [/motion_streaming_interface]: Motoman robot is now enabled and will accept motion commands.
[ INFO] [1687073274.455576904] [/move_group]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1687073274.455900482] [/move_group]: Planning attempt 1 of at most 5
[ INFO] [1687073274.456039125] [/move_group]: Using planning pipeline 'ompl'
[ INFO] [1687073274.461878179] [/move_group]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1687073274.463478337] [/move_group]: arm/arm: Starting planning with 1 states already in datastructure
[ INFO] [1687073274.463636223] [/move_group]: arm/arm: Starting planning with 1 states already in datastructure
[ INFO] [1687073274.463786397] [/move_group]: arm/arm: Starting planning with 1 states already in datastructure
[ INFO] [1687073274.463947960] [/move_group]: arm/arm: Starting planning with 1 states already in datastructure
[ INFO] [1687073274.478148656] [/move_group]: arm/arm: Created 5 states (2 start + 3 goal)
[ INFO] [1687073274.480533198] [/move_group]: arm/arm: Created 4 states (2 start + 2 goal)
[ INFO] [1687073274.482913494] [/move_group]: arm/arm: Created 4 states (2 start + 2 goal)
[ INFO] [1687073274.483472607] [/move_group]: arm/arm: Created 5 states (2 start + 3 goal)
[ INFO] [1687073274.483647842] [/move_group]: ParallelPlan::solve(): Solution found by one or more threads in 0.020850 seconds
[ INFO] [1687073274.490607650] [/move_group]: SimpleSetup: Path simplification took 0.006779 seconds and changed from 3 to 2 states
[ INFO] [1687073282.545287838] [/move_group]: Controller '' successfully finished
[ INFO] [1687073282.625873297] [/move_group]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1687073304.082469519] [/move_group]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1687073304.082654404] [/move_group]: Planning attempt 1 of at most 5
[ INFO] [1687073304.082814041] [/move_group]: Using planning pipeline 'ompl'
[ INFO] [1687073304.087980981] [/move_group]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1687073304.088640733] [/move_group]: arm/arm: Starting planning with 1 states already in datastructure
[ INFO] [1687073304.088749434] [/move_group]: arm/arm: Starting planning with 1 states already in datastructure
[ INFO] [1687073304.088933592] [/move_group]: arm/arm: Starting planning with 1 states already in datastructure
[ INFO] [1687073304.089095119] [/move_group]: arm/arm: Starting planning with 1 states already in datastructure
[ INFO] [1687073304.103207870] [/move_group]: arm/arm: Created 4 states (2 start + 2 goal)
[ INFO] [1687073304.104367071] [/move_group]: arm/arm: Created 4 states (2 start + 2 goal)
[ INFO] [1687073304.104433363] [/move_group]: arm/arm: Created 4 states (2 start + 2 goal)
[ INFO] [1687073304.106851019] [/move_group]: arm/arm: Created 5 states (2 start + 3 goal)
[ INFO] [1687073304.106985735] [/move_group]: ParallelPlan::solve(): Solution found by one or more threads in 0.018693 seconds
[ INFO] [1687073304.118872812] [/move_group]: SimpleSetup: Path simplification took 0.011809 seconds and changed from 3 to 2 states
[ERROR] [1687073308.485797991] [/move_group]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 4.347495 seconds). Stopping trajectory.
[ INFO] [1687073308.485954577] [/move_group]: Cancelling execution for
[ INFO] [1687073308.486212413] [/move_group]: Completed trajectory execution with status TIMED_OUT ...
[ INFO] [1687073308.487382285] [/move_group]: Controller '' successfully finished
[ INFO] [1687073308.518144698] [/rviz_hvmr_Latitude_5430_10821_5853370145707960593]: ABORTED: TIMED_OUT
[ INFO] [1687073327.223681511] [/move_group]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1687073327.223756951] [/move_group]: Using planning pipeline 'ompl'
[ INFO] [1687073327.225250636] [/move_group]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1687073327.225704239] [/move_group]: arm/arm: Starting planning with 1 states already in datastructure
[ INFO] [1687073327.225746838] [/move_group]: arm/arm: Starting planning with 1 states already in datastructure
[ INFO] [1687073327.225862453] [/move_group]: arm/arm: Starting planning with 1 states already in datastructure
[ INFO] [1687073327.225913701] [/move_group]: arm/arm: Starting planning with 1 states already in datastructure
[ INFO] [1687073327.240405922] [/move_group]: arm/arm: Created 4 states (2 start + 2 goal)
[ INFO] [1687073327.241255129] [/move_group]: arm/arm: Created 4 states (2 start + 2 goal)
[ INFO] [1687073327.241462291] [/move_group]: arm/arm: Created 4 states (2 start + 2 goal)
[ INFO] [1687073327.241755841] [/move_group]: arm/arm: Created 4 states (2 start + 2 goal)
[ INFO] [1687073327.241827912] [/move_group]: ParallelPlan::solve(): Solution found by one or more threads in 0.016382 seconds
[ INFO] [1687073327.247741062] [/move_group]: SimpleSetup: Path simplification took 0.005835 seconds and changed from 3 to 2 states
[ INFO] [1687073351.935858682] [/move_group]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1687073351.935977305] [/move_group]: Planning attempt 1 of at most 5
[ INFO] [1687073351.936061490] [/move_group]: Using planning pipeline 'ompl'
[ INFO] [1687073351.938994802] [/move_group]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1687073351.939974298] [/move_group]: arm/arm: Starting planning with 1 states already in datastructure
[ INFO] [1687073351.940124541] [/move_group]: arm/arm: Starting planning with 1 states already in datastructure
[ INFO] [1687073351.940290720] [/move_group]: arm/arm: Starting planning with 1 states already in datastructure
[ INFO] [1687073351.940474931] [/move_group]: arm/arm: Starting planning with 1 states already in datastructure
[ INFO] [1687073351.957124670] [/move_group]: arm/arm: Created 5 states (2 start + 3 goal)
[ INFO] [1687073351.957226333] [/move_group]: arm/arm: Created 5 states (2 start + 3 goal)
[ INFO] [1687073351.957302597] [/move_group]: arm/arm: Created 4 states (2 start + 2 goal)
[ INFO] [1687073351.957698877] [/move_group]: arm/arm: Created 5 states (2 start + 3 goal)
[ INFO] [1687073351.957809902] [/move_group]: ParallelPlan::solve(): Solution found by one or more threads in 0.018395 seconds
[ INFO] [1687073351.961470291] [/move_group]: SimpleSetup: Path simplification took 0.003614 seconds and changed from 3 to 2 states
[ERROR] [1687073353.659364727] [/motion_streaming_interface]: Aborting Trajectory. Failed to send point (#5): Not Ready (5) : Power and Force Limiting (PFL) was activated: please reset PFL on the robot, re-enable the driver (call the service) and send a new trajectory (5011)
[ERROR] [1687073356.349640297] [/move_group]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 4.386707 seconds). Stopping trajectory.
[ INFO] [1687073356.349777061] [/move_group]: Cancelling execution for
[ INFO] [1687073356.349882765] [/move_group]: Completed trajectory execution with status TIMED_OUT ...
[ INFO] [1687073356.351082334] [/move_group]: Controller '' successfully finished
[ INFO] [1687073356.358188252] [/rviz_hvmr_Latitude_5430_10821_5853370145707960593]: ABORTED: TIMED_OUT

======================================================

I'm adding some photos and a link to a video that shows the robot reaching a singularity point and halting:

WhatsApp Image 2023-06-18 at 10 51 53