Test small movements in joint space

[nbbrooks]

Test tolerance failing of trajectory completion with small movements

[MGBla]

Ok just tested it out on a single joint (wrist_1). Up till 0.1 radians the action terminates successfully. Everything above result in the following message:

[ros2_control_node-1] [INFO] [1612449853.661850170] [ur_joint_trajectory_controller]: Received new action goal
[ros2_control_node-1] [INFO] [1612449853.661883916] [ur_joint_trajectory_controller]: Accepted new action goal
[ros2_control_node-1] [WARN] [1612449863.664272377] [ur_joint_trajectory_controller]: Aborted due to goal tolerance violation

For reproducing the error, I started the robot at home pose [0.0, -1.57, 0.0, -1.57, 0.0, 0.0] and send the following ActionGoal

control_msgs.action.FollowJointTrajectory_Goal(trajectory=trajectory_msgs.msg.JointTrajectory(
header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), 
joint_names=['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], 
points=[trajectory_msgs.msg.JointTrajectoryPoint(positions=[0.0, -1.57, 0.0, -1.3, 0.0, 0.0], 
velocities=[], 
accelerations=[], 
effort=[], 
time_from_start=builtin_interfaces.msg.Duration(sec=10, nanosec=0))]), 
path_tolerance=[control_msgs.msg.JointTolerance(name='elbow_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='shoulder_lift_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='shoulder_pan_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_1_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_2_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_3_joint', position=3.14, velocity=3.14, acceleration=3.14)], 
goal_tolerance=[control_msgs.msg.JointTolerance(name='elbow_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='shoulder_lift_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='shoulder_pan_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_1_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_2_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_3_joint', position=3.14, velocity=3.14, acceleration=3.14)], 
goal_time_tolerance=builtin_interfaces.msg.Duration(sec=3, nanosec=0))

[MGBla]

As stated in teams #37 does not fix the Problem. It seems like the parameters are not correctly transfered into the JTC. Also I can't find them on the parameter server. I dug a little bit deeper into the error and it seems that the stopped_velocity_tolerance is the cause of the issue since. When the action fails the velocity is most of the time at around 0.1 (rad/s i guess). I increased it hardcoded in the tolerance.hpp which "fixed" the error. I will now have a closer look into the parameter server.

[MGBla]

Ok removed the hardcoded stuff and set the stopped_velocity_tolerance int the ur_ros2_control.yaml to 0.2 which works to prevent the error. However I guess the robot shouldn't have a velocity of 0.2 rad/s at the end of its trajectory.

[livanov93]

Ok just tested it out on a single joint (wrist_1). Up till 0.1 radians the action terminates successfully. Everything above result in the following message:

[ros2_control_node-1] [INFO] [1612449853.661850170] [ur_joint_trajectory_controller]: Received new action goal
[ros2_control_node-1] [INFO] [1612449853.661883916] [ur_joint_trajectory_controller]: Accepted new action goal
[ros2_control_node-1] [WARN] [1612449863.664272377] [ur_joint_trajectory_controller]: Aborted due to goal tolerance violation

For reproducing the error, I started the robot at home pose [0.0, -1.57, 0.0, -1.57, 0.0, 0.0] and send the following ActionGoal

control_msgs.action.FollowJointTrajectory_Goal(trajectory=trajectory_msgs.msg.JointTrajectory(
header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), 
joint_names=['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], 
points=[trajectory_msgs.msg.JointTrajectoryPoint(positions=[0.0, -1.57, 0.0, -1.3, 0.0, 0.0], 
velocities=[], 
accelerations=[], 
effort=[], 
time_from_start=builtin_interfaces.msg.Duration(sec=10, nanosec=0))]), 
path_tolerance=[control_msgs.msg.JointTolerance(name='elbow_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='shoulder_lift_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='shoulder_pan_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_1_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_2_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_3_joint', position=3.14, velocity=3.14, acceleration=3.14)], 
goal_tolerance=[control_msgs.msg.JointTolerance(name='elbow_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='shoulder_lift_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='shoulder_pan_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_1_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_2_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_3_joint', position=3.14, velocity=3.14, acceleration=3.14)], 
goal_time_tolerance=builtin_interfaces.msg.Duration(sec=3, nanosec=0))

Only place where that message can be outputed is here which actually depends on the outside_goal_tolerance flag. This flag can then only be set here where the check for the last point is done.

If we take a closer look this function is setting the flag and it is based on these parameters.0
They contain goal and state tolerances which are on default zero.

This function will not change them if they are not specified - they remain zero.

Function that is actually using them is only checking tolerance error if the parameters are non-zero.

The quick fix would be to set stopped_velocity_tolerance to zero.

[livanov93]

The issue has been created on the ros2_control.