Discontinuous joint velocities with zero spikes despite correct desired commands in Joint Trajectory Controller #1800
Description
Describe the bug
Hello,
I am using ROS 2 Humble and I recently started my studies on robotic manipulators. I apologize in advance if my question is something simple to resolve.
My original goal is to implement a control law that computes, at each iteration, the desired velocity command to move a manipulator, based on the current joint positions. I initially thought the best way to do this using the joint_trajectory_controller/JointTrajectoryController was to send a new message at every iteration — though I'm definitely open to alternative solutions if there's a better approach!
Specifically, I have tested using two different arms: the Gen3 arm from ROS2 Kortex and the Franka Robotics arm.
The problem arises each time a new message is sent (in my case, every second). The actual joint velocities (from the /joint_states topic) exhibit sharp discontinuities with spikes going to zero every time a new message is received (this can be better seen in the "screenshots" section). This behavior only occurs in the actual velocities and not in the desired velocities of the joint trajectory controller, which seems correctly interpolated.
From what I understand, the issue seems to be the same, or at least similar, as the one discussed in #1455 and #84 . It appears to be related to the fact that a proper replacement for the old trajectory has not yet been migrated from ROS1 to ROS2.
I was wondering if anyone else has encountered the same issue related to the actual velocities (not the desired ones), or if someone has found a solution to the problem.
Any insights or suggestions would be greatly appreciated. Thank you very much for your help!
To Reproduce
Regarding the ROS2 Kortex arm, I have simply followed the "Commanding the arm (physically and in simulation)" section of the repository https://github.com/Kinovarobotics/ros2_kortex/tree/humble. Specifically, I cloned the repository and launched kortex_sim_control.launch.py with the default settings. I’m using the Gen3 arm model with 7 degrees of freedom, without initializing the gripper. The joint trajectory controller parameters I used are the default ones, with position command interface and position and velocity state interfaces, adding allow_integration_in_goal_trajectories: true in order to accept goals without position specified
joint_trajectory_controller:
ros__parameters:
joints:
- joint_1
- joint_2
- joint_3
- joint_4
- joint_5
- joint_6
- joint_7
command_interfaces:
- position
state_interfaces:
- position
- velocity
state_publish_rate: 100.0
action_monitor_rate: 20.0
allow_partial_joints_goal: false
allow_integration_in_goal_trajectories: true
constraints:
stopped_velocity_tolerance: 0.0
goal_time: 0.0
My goal is to move the arm by publishing a loop of joint trajectory messages, like this:
ros2 topic pub /joint_trajectory_controller/joint_trajectory trajectory_msgs/JointTrajectory "{
joint_names: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7],
points: [
{ velocities: [-1, -1, -1, -1, -1, -1, -1], time_from_start: { sec: 10 } },
]
}"
For testing the Franka Robotics arm (https://github.com/frankarobotics/franka_ros2), I modified the franka_gazebo_controllers.yaml file (while keeping position as the command interface and setting allow_integration_in_goal_trajectories: true) and the visualize_franka_robot.launch.py file to add and activate the controller. Both files are part of the franka_gazebo_bringup package.
Then, I sent the command loop following the same procedure as in the previous case, like this:
ros2 topic pub /joint_trajectory_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "joint_names: ['fr3_joint1', 'fr3_joint2', 'fr3_joint3', 'fr3_joint4', 'fr3_joint5', 'fr3_joint6', 'fr3_joint7']
points:
- velocities: [-0.1, -0.1, -0.1, -0.1, -0.1, -0.1, -0.1]
time_from_start:
sec: 10
nanosec: 0"
Expected behavior
Looking at the velocity profile, what I would have expected in both cases is a desired velocity with a smooth behavior (since I’m only providing velocity points, the sampled trajectory, as I understand it, is generated using cubic splines) and an actual velocity that gradually and smoothly converges to the desired one.
Screenshots
For clarity, i am plotting the /joint_states velocity values for joint_1, even if the same behavior occurs on all other joints.
The issue occurs in both the first case, with the ROS2 Kortex arm
and the second case, with the Franka Robotics arm
Environment:
- OS: Ubuntu
- Version: Humble
Additional context
One thing I noticed, which makes me think the issue is related to the choice of command interface, is that with the Franka Robotics arm, setting the command interface to both position and velocity (while keeping everything else unchanged) produces exactly the expected result.
However, I couldn’t test this with the ROS2 Kortex arm because, as far as I understand, it only accepts position command interface and not velocity.