0
$\begingroup$

I'm very disappointed about MoveIt planning accuracy and hope you can help me with some useful advices.

Some tech. intro: Ubuntu 22.04.3 LTS ROS2 Rolling Gazebo Classic MoveIT2 (installed from source)

All I need for now is to set joint_2 angle to 20 degrees and angles of other joints to 0. At start there all zeros as Home position.

There are 2 ways I'm doing that:

  1. Use "planning" and "joints" tabs in MotionPlanning window from Rviz. Just set joint_2 angle to 20 degrees in "joints" tab and then press "plan and execute" button in "planning" tab.

  2. Send command from python code to MoveIt for motion planning end executing, using moveitpy.

I used to try almost all available OMPL planners and all they show same medium results +/- 0.3 degrees. So as result I have joint_2 angle from 19.5 to 20.5 degrees for each try! But I assume to get joint_2 angle from 19,99 to 20.01 degrees if I send request planning exactly this joint to angle 20 degrees.

I tried to change OMPL settings and values widely, add them and delete:

goal_bias: 0.05
max_velocity_scaling_factor: 0.1
max_acceleration_scaling_factor: 0.1
goal_joint_tolerance: 0.005  
goal_position_tolerance: 0.01  
goal_orientation_tolerance: 0.1

And so this is my ompl_planning.yaml file:

planning_plugin: ompl_interface/OMPLPlanner
request_adapters: >-
  default_planner_request_adapters/AddTimeOptimalParameterization
  default_planner_request_adapters/ResolveConstraintFrames
  default_planner_request_adapters/FixWorkspaceBounds
  default_planner_request_adapters/FixStartStateBounds
  default_planner_request_adapters/FixStartStateCollision
  default_planner_request_adapters/FixStartStatePathConstraints
start_state_max_bounds_error: 0.1
max_planning_duration: 10.0
goal_bias: 0.05
max_velocity_scaling_factor: 0.1
max_acceleration_scaling_factor: 0.1
goal_joint_tolerance: 0.005  
goal_position_tolerance: 0.01  
goal_orientation_tolerance: 0.1 
default_planner_config: RRTConnectkConfigDefault

planner_configs:
  default_planner_config: RRTConnectkConfigDefault
  ESTkConfigDefault:
    type: geometric::EST
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
  RRTkConfigDefault:
    type: geometric::RRT
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
  RRTConnectkConfigDefault:
    type: geometric::RRTConnect
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  TRRTkConfigDefault:
    type: geometric::TRRT
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
    max_states_failed: 10  # when to start increasing temp. default: 10
    temp_change_factor: 2.0  # how much to increase or decrease temp. default: 2.0
    min_temperature: 10e-10  # lower limit of temp change. default: 10e-10
    init_temperature: 10e-6  # initial temperature. default: 10e-6
    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
    frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
    k_constant: 0.0  # value used to normalize expresssion. default: 0.0 set in setup()
  PRMkConfigDefault:
    type: geometric::PRM
    max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
  PRMstarkConfigDefault:
    type: geometric::PRMstar
  BiTRRTkConfigDefault:
    type: geometric::BiTRRT
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
    temp_change_factor: 0.1  # how much to increase or decrease temp. default: 0.1
    init_temperature: 100  # initial temperature. default: 100
    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
    frountier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
    cost_threshold: 1e300  # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
  BiESTkConfigDefault:
    type: geometric::BiEST
    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
  SPARSkConfigDefault:
    type: geometric::SPARS
    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
    max_failures: 1000  # maximum consecutive failure limit. default: 1000
  SPARStwokConfigDefault:
    type: geometric::SPARStwo
    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
    max_failures: 5000  # maximum consecutive failure limit. default: 5000

robot_arm:
  default_planner_config: RRTConnectkConfigDefault
  planner_configs:
    - ESTkConfigDefault
    - RRTkConfigDefault
    - RRTConnectkConfigDefault
    - TRRTkConfigDefault
    - PRMkConfigDefault
    - PRMstarkConfigDefault
    - BiTRRTkConfigDefault
    - BiESTkConfigDefault
    - SPARSkConfigDefault
    - SPARStwokConfigDefault
    
robot_end_group:
  default_planner_config: RRTConnectkConfigDefault
  planner_configs:
    - ESTkConfigDefault
    - RRTkConfigDefault
    - RRTConnectkConfigDefault
    - TRRTkConfigDefault
    - PRMkConfigDefault
    - PRMstarkConfigDefault
    - BiTRRTkConfigDefault
    - BiESTkConfigDefault
    - SPARSkConfigDefault
    - SPARStwokConfigDefault

For me those settings do not change nothing. I have the same result with and without it.

Then I try to change ros2_controllers.yaml:

  controller_manager:
  ros__parameters:
    update_rate: 50  # Hz

    robot_arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController
      joints:
        - joint_1
        - joint_2
        - joint_3
        - joint_4
        - joint_5
        - joint_6
      publish_rate: 50

    joint_state_controller:
      # type: joint_state_controller/JointStateController
      type: joint_state_broadcaster/JointStateBroadcaster

robot_arm_controller:
  ros__parameters:
    joints:
      - joint_1
      - joint_2
      - joint_3
      - joint_4
      - joint_5
      - joint_6
    interface_name: position
    command_interfaces:
      - position
      - velocity
    state_interfaces:
      - position
      - velocity
    state_publish_rate: 100.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: false
    constraints:
      stopped_velocity_tolerance: 0.01
      goal_time: 5.0
      # joint1: { trajectory: 0.2, goal: 0.1 }
      # joint2: { trajectory: 0.2, goal: 0.1 }
      # joint3: { trajectory: 0.2, goal: 0.1 }
      # joint4: { trajectory: 0.2, goal: 0.1 }
      # joint5: { trajectory: 0.2, goal: 0.1 }
      # joint6: { trajectory: 0.2, goal: 0.1 }
      joint1:
        trajectory: 0.02
        goal: 0.01
      joint2: 
        trajectory: 0.02
        goal: 0.01
      joint3:
        trajectory: 0.02
        goal: 0.01
      joint4:
        trajectory: 0.02
        goal: 0.01
      joint5:
        trajectory: 0.02
        goal: 0.001
      joint6:
        trajectory: 0.02
        goal: 0.01

I tried 2 different formats to add settings in my file. I change trajectory and goal value from 0.1 to 0.0001 - and still seen same results - joint_2 angle from 19.5 to 20.5 degrees after plan and execute joint_2 to 20 degrees from Home and from other positions. Same problem with other joints, but I didn't test a lot to calculate accuracy.

Well looks like this setting also not working for me.

Please help me figure out how to adjust the accuracy of the target pose.

Improve this question
$\endgroup$
1
  • $\begingroup$ This could be a problem of the ros control node. The followJointTrajectoryAction has a goal tolerance field. You could try to play with it and see what happens. $\endgroup$
    – Rafael Rojas
    Commented Mar 2 at 16:41

1 Answer 1

Reset to default
0
$\begingroup$

Regarding

I tried 2 different formats to add settings in my file.

I suggest using rqt_reconfigure to see if the parameters are loaded correctly, just to be sure about that first.

The joint-names of the contraints structure have to be the same as in your URDF (which is not the case, joint1 vs joint_1).

Then use any visualization programm, plotjuggler or foxglove for example, to see if the planned trajectory is not reaching your target or if the joint_trajectory_controller is not configured properly to reach it in combination with your robot. (have a look at the ~/controller_state topic).

Improve this answer
$\endgroup$
2
  • $\begingroup$ Thanks for your help. $\endgroup$
    – Kirill Partala
    Commented Dec 13, 2023 at 9:43
  • $\begingroup$ rqt_reconfigure was new for me and very helpful! joint-names war really wrong and I fixed it. But the problem was not solved in python. The planned trajectory was not reaching my target, so It was not controller problem. I was rewrite all my code to c++ to made it work properly. $\endgroup$
    – Kirill Partala
    Commented Dec 14, 2023 at 9:00

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.