false Goal reached, success! with joint trajectory controller

[FrGe2016]

I have a slow home made robot, On every moves, the succes, for the slower joint, is confirmed a few seconds before the joint target is reached ? At this moment, there is still a significant distance to travel, ( we are not talking of oscillation around the target due to a not well adjusted PID)

[christophfroehlich]

Please give us some information: your ROS distro, ros2_control version, your controller configuration yaml etc.

[FrGe2016]

I have ros2 Humble.

I have a real home made robot, controlled by a micro controller, the micro controller has a pid loop command on position, one tcp server stream the actual position and velocity to the hardware interface while another receive the position command. Of all the joints, the lateral joint is the slowest, it is a prismatic joint ( rack and pinion). The JTC confirm success while this joint is not at his destination ( time remaining 1,5 to 2 seconds )

g_controllers.yaml

controller_manager:
  ros__parameters:
    update_rate: 50  # Hz


    arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController  
   
    joint_state_broadcaster:                                 
      type: joint_state_broadcaster/JointStateBroadcaster     
          
arm_controller:
  ros__parameters:
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    allow_partial_joints_goal: true
    joints:
      - elbow
      - wrist
      - rotator
      - lateral
      - gripper
      - gripper_top

g.ros2_control_arm.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:macro name="g_ros2_control" params="
               name
               use_mock_hardware:=^|false
               mock_sensor_commands:=^|false
               sim_gazebo_classic:=^|false
               sim_gazebo:=^|false"
               >
    <ros2_control name="${name}" type="system">
      <hardware>
        <xacro:if value="${use_mock_hardware}">
          <plugin>mock_components/GenericSystem</plugin>
          <param name="mock_sensor_commands">${mock_sensor_commands}</param>
        </xacro:if>
        <xacro:if value="${sim_gazebo_classic}">
          <plugin>gazebo_ros2_control/GazeboSystem</plugin>
        </xacro:if>
        <xacro:if value="${sim_gazebo}">
          <plugin>ign_ros2_control/IgnitionSystem</plugin>
        </xacro:if>
        <xacro:unless value="${use_mock_hardware or sim_gazebo_classic or sim_gazebo}">
          <plugin>f_hardware_interface/FBotHardwareInterface</plugin>
                  
        </xacro:unless>
      </hardware>
      <joint name="elbow">
         <command_interface name="position"/>
         <command_interface name="velocity"/>
         <state_interface name="position">
             <param name="initial_value">0.0</param>
          </state_interface>
          <state_interface name="velocity">
             <param name="initial_value">0.0</param>
          </state_interface>    
      </joint>
      <joint name="wrist">
         <command_interface name="position"/>
         <command_interface name="velocity"/>
         <state_interface name="position">
             <param name="initial_value">0.0</param>
          </state_interface>
          <state_interface name="velocity">
             <param name="initial_value">0.0</param>
          </state_interface> 
      </joint>
      <joint name="rotator">
         <command_interface name="position"/>
         <command_interface name="velocity"/>
         <state_interface name="position">
             <param name="initial_value">0.0</param>
          </state_interface>
          <state_interface name="velocity">
             <param name="initial_value">0.0</param>
          </state_interface> 
      </joint>
      <joint name="lateral">
         <command_interface name="position"/>
         <state_interface name="position">
             <param name="initial_value">0.0</param>
          </state_interface>
          <state_interface name="velocity">
             <param name="initial_value">0.0</param>
          </state_interface> 
      </joint>
      <joint name="gripper">
         <command_interface name="position"/>
         <command_interface name="velocity"/>
         <state_interface name="position">
             <param name="initial_value">0.0</param>
          </state_interface>
          <state_interface name="velocity">
             <param name="initial_value">0.0</param>
          </state_interface> 
      </joint>
      <joint name="gripper_top">
         <command_interface name="position"/>
         <command_interface name="velocity"/>
         <state_interface name="position">
             <param name="initial_value">0.0</param>
          </state_interface>
          <state_interface name="velocity">
             <param name="initial_value">0.0</param>
          </state_interface> 
      </joint>

    </ros2_control>
    
  </xacro:macro>
</robot>

Velocity and acceleration limits are defined in the URDF

[christophfroehlich]

It would be very helpful if you format your comments appropriately.

Have a look at the documentation. You don't specify any of the constraints parameters, which means that JTC succeeds if every joint velocity is below 0.01units (default setting). This could be the case with your "slow actuator".

However, consider sending a feasible trajectory to your system which can be reached on time at least in the nominal case.

[gavanderhoorn]

one tcp server stream the actual position and velocity to the hardware interface

as it's a common pattern I see with 'cheap servos' (which can't report the actual current state): is this really the current state, or the last commanded state?

[FrGe2016]

I have good feedback's with absolute angle measurements for my revolute joints an an encoder for my slow prismatic joint.I missed that part of the documentation, my starting point configuration files generated by moveit setup assistant. I will try to fix my problem with the available parameters. ( but my slow joint is not that slow and there is a factor of 70 compared with the other joints.) Setting a global parameter could be an issue. In last resort, i will modify the joint trajectory controller but i will try to avoid it .