0
$\begingroup$

first of all merry Christmas to whoever is celebrating!

I am working on adding a 7th prismatic axis to the UR10e robot and implementing a kinematic chain to synchronously control the robot. The hardware interfaces are working perfectly so far. Using each kinematic chain by itself (axis or robot) is no problem with the joint trajectory controller as well as all other controllers.

However when using the joint trajectory controller with the combined kinematic chain, i seem to get nothing but errors.

The current list of joints are

  • axis_joint1
  • UR_shoulder_pan_joint
  • UR_shoulder_lift_joint
  • UR_elbow_joint
  • UR_wrist_1_joint
  • UR_wrist_2_joint
  • UR_wrist_3_joint

When using a joint trajectory controller which tries to move all joints at once such as

joint_trajectory_controller:
  ros__parameters:
    joints:
      - axis_joint1
      - UR_shoulder_pan_joint
      - UR_shoulder_lift_joint
      - UR_elbow_joint
      - UR_wrist_1_joint
      - UR_wrist_2_joint
      - UR_wrist_3_joint
    command_interfaces:
      - velocity
    state_interfaces:
      - position
      - velocity
    state_publish_rate: 100.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: true

i receive this error:

[ur_ros2_control_node-1] [ERROR] [1703780467.945419329] [axis_joint_trajectory_controller]: Holding position due to state tolerance violation

which comes from this file: https://github.com/ros-controls/ros2_controllers/blob/humble/joint_trajectory_controller/src/joint_trajectory_controller.cpp (Line 401, caused by Line 263) which is checking for a state tolerance? I guess this is either the goal or trajectory tolerance of the joint? There is not much documentation on that. I'm also not sure which state it is violating when it doesn't even have a position it should be in.

The same error occurs when i try sending commands to the axis only using this controller setup:

axis_joint_trajectory_controller:
  ros__parameters:
    joints:
      - axis_joint1
    command_interfaces:
      - velocity
    state_interfaces:
      - position
      - velocity
    state_publish_rate: 100.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: true
    constraints:
      stopped_velocity_tolerance: 0.2
      goal_time: 0.0
      axis_joint1: { trajectory: 0.2, goal: 0.1 }

and these goals

publisher_joint_trajectory_controller:
  ros__parameters:

    controller_name: "axis_joint_trajectory_controller"
    wait_sec_between_publish: 5

    goal_names: ["pos1", "pos2", "pos3", "pos4"]
    pos1: [0.6]
    pos2: [0.2]
    pos3: [0.5]
    pos4: [0.4]

    joints:
      - axis_joint1


    check_starting_point: false
    starting_point_limits:
      axis_joint1: [-2.0,2.0]

The other thing is, when trying to move the UR Robot joints alone (while the axis is still in the urdf kinematic chain), i get an error in the sanity check of the publisher . Using this setup

ur_joint_trajectory_controller:
  ros__parameters:
    joints:
      - UR_shoulder_pan_joint
      - UR_shoulder_lift_joint
      - UR_elbow_joint
      - UR_wrist_1_joint
      - UR_wrist_2_joint
      - UR_wrist_3_joint
    command_interfaces:
      - velocity
    state_interfaces:
      - position
      - velocity
    state_publish_rate: 100.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: true
    constraints:
      stopped_velocity_tolerance: 0.2
      goal_time: 0.0
      UR_shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
      UR_shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
      UR_elbow_joint: { trajectory: 0.2, goal: 0.1 }
      UR_wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
      UR_wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
      UR_wrist_3_joint: { trajectory: 0.2, goal: 0.1 }

and this trajectory yaml file

publisher_urjoint_trajectory_controller:
  ros__parameters:

    controller_name: "ur_joint_trajectory_controller"
    wait_sec_between_publish: 6

    goal_names: ["pos1", "pos2", "pos3", "pos4"]
    pos1:
      positions: [0.785, -1.57, 0.785, 0.785, 0.785, 0.785]
    pos2:
      positions: [0.0, -1.57, 0.0, 0.0, 0.0, 0.0]
    pos3:
      positions: [0.0, -1.57, 0.0, 0.0, -0.785, 0.0]
    pos4:
      positions: [0.0, -1.57, 0.0, 0.0, 0.0, 0.0]

    joints:
      - UR_shoulder_pan_joint
      - UR_shoulder_lift_joint
      - UR_elbow_joint
      - UR_wrist_1_joint
      - UR_wrist_2_joint
      - UR_wrist_3_joint

    check_starting_point: true
    starting_point_limits:
      UR_shoulder_pan_joint: [-3.0,3.1]
      UR_shoulder_lift_joint: [-3.6,3.5]
      UR_elbow_joint: [-3.6,3.6]
      UR_wrist_1_joint: [-3.6,3.5]
      UR_wrist_2_joint: [-3.6,3.6]
      UR_wrist_3_joint: [-3.1,3.61]

I receive this error:

[publisher_joint_trajectory_controller-1]     if (msg.position[idx] < self.starting_point[enum][0]) or (
[publisher_joint_trajectory_controller-1] KeyError: 'axis_joint1'

which comes from this file https://github.com/ros-controls/ros2_controllers/blob/master/ros2_controllers_test_nodes/ros2_controllers_test_nodes/publisher_joint_trajectory_controller.py, (line 168) This means that somehow it is checking for "axis_joint1", which is within the kinematic chain but neither in the sent trajectory nor in the controller, meaning for some inexplicable reason it is checking against the joint states or the defined urdf (which it should NOT do since the only relevant joints that should be checked are the ones defined in the controller)

It seems the error is within the joint_trajectory_controller, because when i use the forward_velocity_controller everything works as intended and when I load a robot with EITHER the kinematic chain of the Robot alone OR the axis alone, the joint trajectory controller works without problem.

I hope this was not too confusing and I really hope someone has some ideas, this has cost me many restless nights.

$\endgroup$

2 Answers 2

0
$\begingroup$

You are on humble? Which version of JTC are you using? (ros2 pkg xml joint_trajectory_controller)

There is a PR waiting for a merge, introducing a more verbose output if a tolerance check failed. You can compile the PR from source.

JTC starts with holding the position at time of activation. Can it be that your axes are moving, or that there isn't a correct state information at time of activation?

Regarding publisher_joint_trajectory_controller.py: If check_starting_point is set, it parses all joints from the joint_states messages. And because the starting_point_limits map doesn't have a value for axis_joint1 it throws an error. I agree that it should only check the joints from the joints parameter. Do you want to fix that and open a PR?

$\endgroup$
1
  • $\begingroup$ thanks alot! i will try out to compile ROS from source with the and see how it work out. however even in the PR it is unclear which HTC paramemter " Position Tolerance" relates to. I also dont understand why it would receive a failed tolerance check without any movement being initiated in the first place. and why it works with one URDF and not the other. I can open a PR, not sure how to fix the publisher_joint_trajectory_controller.py though $\endgroup$
    – Adam
    Jan 1 at 21:22
0
$\begingroup$

after fiddling around for weeks I have found the solution to my problem!

The problem is not due to the holding position error but that I used velocity as a control interface which does not work in the aforementioned setup. It worked when i changed it to:

ur_joint_trajectory_controller:
  ros__parameters:
    joints:
      - UR_shoulder_pan_joint
      - UR_shoulder_lift_joint
      - UR_elbow_joint
      - UR_wrist_1_joint
      - UR_wrist_2_joint
      - UR_wrist_3_joint
    command_interfaces:
      - velocity
    state_interfaces:
      - position
      - velocity
    state_publish_rate: 100.0
    action_monitor_rate: 20.0
    open_loop_control: true
$\endgroup$

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.