robot arm joint velocity controller gains not compatible with moveit

[jacknlliu]

Using moveit planning to control the robot arm to move, we get the trajectory module warning, and robot arm may not get the target position exactly.

[ WARN] [1507990293.095432374, 1856.648000000]: Controller assembler_arm_controller failed with error code PATH_TOLERANCE_VIOLATED
[ WARN] [1507990293.095526214, 1856.648000000]: Controller handle assembler_arm_controller reports status ABORTED
[ INFO] [1507990293.596524919, 1857.090000000]: Execution completed: ABORTED

I think we should blame arm controller gains.

[jacknlliu]

Velocity and effort controlled hardware interface in gazebo may cause this issue. See ros-industrial/universal_robot#231 (comment).

moveit config issue, please see: STOMP planner and moveit.

Another related trajectory execution error

Invalid Trajectory: start point deviates from current robot state more than 0.01
joint 'shoulder_lift_joint': expected: -0.560594, current: -0.535852

This issue caused by moveit planning aborted and the robot cannot reach the goal. Then a new trajectory will be affected. More details please sse ros-controls/ros_controllers#48, ros-controls/ros_controllers#144.

Gazebo controller issue, please see Controller failed with error code GOAL_TOLERANCE_VIOLATED. In real robot control, we may not get this issue, because the real robot may not use ros-control, so they will not report tolerance error to moveit, and the controller parameters are different from gazebo controller.

ROS control related issues, please see: ros-controls/ros_controllers#300, ros-controls/ros_controllers#48, ros-controls/ros_controllers#46.

[jacknlliu]

There are some relationships between joint controller config and the output error from joint trajectory controller which also reported by MoveIt!.

Our joint trajectory controller config file is:

arm_gazebo_controller:
  type: velocity_controllers/JointTrajectoryController
  joints:
     - shoulder_pan_joint
     - shoulder_lift_joint
     - elbow_joint
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint
  constraints:
      goal_time: 0.6
      stopped_velocity_tolerance: 0.5
      shoulder_pan_joint: {trajectory: 0.3, goal: 0.1}
      shoulder_lift_joint: {trajectory: 0.75, goal: 0.2}
      elbow_joint: {trajectory: 0.75, goal: 0.2}
      wrist_1_joint: {trajectory: 0.3, goal: 0.1}
      wrist_2_joint: {trajectory: 0.3, goal: 0.1}
      wrist_3_joint: {trajectory: 0.3, goal: 0.1}

  gains:
      shoulder_pan_joint: {p: 1.2,  i: 0.0, d: 0.1, i_clamp: 1}
      shoulder_lift_joint: {p: 1000,  i: 0.1, d: 0.2, i_clamp: 1}
      elbow_joint: {p: 1.2,  i: 0.0, d: 0.1, i_clamp: 1}
      wrist_1_joint: {p: 1.2,  i: 0.0, d: 0.1, i_clamp: 1}
      wrist_2_joint: {p: 1.2,  i: 0.0, d: 0.1, i_clamp: 1}
      wrist_3_joint: {p: 1.2,  i: 0.0, d: 0.1, i_clamp: 1}
  stop_trajectory_duration: 0.5
  state_publish_rate:  125
  action_monitor_rate: 10

The parameter trajectory of constraints related joint controller dynamic error of PID output, especially the overshot, too large dynamic error greater than trajectory will cause PATH_TOLERANCE_VIOLATED. So we should tune the PID parameters to keep dynamic error less than trajectory value of constraints all the time to avoid PATH_TOLERANCE_VIOLATED.

The parameter goal of constraints related joint controller steady-state error of PID output, though the large P will make this smaller, there will be overshot in the dynamic performance. Too large steady-state error of PID ouput will cause GOAL_TOLERANCE_VIOLATED. So we should tune the PID parameters to keep steady-state error less than goal value of constraints all the time to avoid GOAL_TOLERANCE_VIOLATED.

[jacknlliu]

Another thing related is about joint velocity controller and joint effort velocity controller with Gazebo. Details please see jacknlliu/development-issues#8.

[jacknlliu]

Resolved! Suggest using joint position controller currently.