joint trajectory controller: issues with arm_controller/state/desired/velocities

edit

Hello,

I am working on a co-simulation between gazebo and simulink. Therfore I am using the Matlab ROS Toolbox to publish and subscribe to topics and the Robotics System Toolbox to synchronize gazebo with simulink. I am simulating the Franka Emika Panda using the ROS joint trajectory controller with the topic interface. So every timestep, which is 0.01s, a new trajectory message is published on the /arm_controller/command topic from simulink. Each message contains only one waypoint with joint positions and velocities specified and a time_from_start value of 0.01s.

I am confused by the behaviour of the joint trajectory controller dealing with the specified joint velocities. I will provide details and diagrams in the following.

Here you can see the joint position messages of the /arm_controller/command topic. Here you can see the joint velocity messages of the /arm_controller/command topic. So this is the desired trajectory I am calculating in Simulink and publishing to the command topic. The velocities are limited by 2 rad/s, which is less than the limits specified in ROS.

Now, the joint trajectory controller provides information about the state of a joint with the /arm_controller/state topic. You can analyse the desired and actual state of a joint. Here you can see the desired joint positions within the joint trajectory controller. The profile is the same as the one from the /arm_controller/command topic, which should be the case, right? However, the desired joint velocities are not the same as the ones from the command topic, as you can see here. There are 2 issues. Firstly, the profiles don't align and secondly, there are high jumps. Shouldn't it be the exact same profiles as the ones from the command topic?

Finally, I show you the actual states of the joints by subscribing to the /joint_states topic. Here, you can see the joint position profiles, which look really good. And here, you can see the joint velocity profiles with some high jumps. That's why I assume, that the desired velocities of the trajectory controller are the problem. I am really confused why the positon profiles look very good, but the velocity ones don't.

Has anybody an idea what the issue could be? I would be very happy for any kind of help.

My yaml-file looks like the following:

panda
    joint_state_controller:
        type: joint_state_controller/JointStateController
        publish_rate: 100

    arm_controller:
        type: effort_controllers/JointTrajectoryController
        joints:
            -joint1
            -joint2
            -joint3
            -joint4
            -joint5
            -joint6
            -joint7
        gains: 
            joint1: {p:12000, d:50, i:0, i_clamp:10000}
            joint2: {p:30000, d:100, i:0.02, i_clamp:10000}
            joint3: {p:18000, d:50, i:0.01, i_clamp:1}
            joint4: {p:18000, d:70, i:0.01, i_clamp:10000}
            joint5: {p:12000, d:70, i:0.01, i_clamp:1}
            joint6: {p:7000, d:50, i:0.01, i_clamp:1}
            joint1: {p:2000, d:20, i:0, i_clamp:1}
        state_publish_rate: 100

And here is an example of a joint configuration in my urdf-file:

<joint name="joint1" type="revolute">
    <safety_controller k_position="100 ...