search

moveit / moveit

:robot: The MoveIt motion planning framework
http://moveit.ros.org
BSD 3-Clause "New" or "Revised" License
1.67k stars 947 forks source link

Motoman HC10DTP + MoveIt - Running into singularities #3452

Open tall1 opened 1 year ago

tall1 commented 1 year ago

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

I've opened this issue in the motoman github and been advised to move here since my issue is more MoveIt oriented.

welcome[bot] commented 1 year ago

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

gavanderhoorn commented 1 year ago

It's important to mention that singularities per se are not the problem. MoveIt plans in joint space, and outputs JointTrajectorys. Moving through singularities with joint motions is perfectly possible.

I believe the main question would be how to take certain kinematic constraints into account (coming from the robot that's going to execute those trajectories) when asking MoveIt to plan certain motions which are not easily expressed using just the joint position limits.

As I wrote in the issue linked, perhaps a custom constraint sampler, position and/or orientation constraints or OMPL constrained planning could be used.

That was no longer a robot / driver issue, hence the suggestion to post here.