motor oscillates after diff_drive_controller configuration

edit

Hello,

I have configured the canopen_motor_node of my robot to use a DiffDriveController, but once I start publishing angular velocities in the /cmd_vel topic the movement is not smooth, i.e. the robot turns then stops, then continues the angular movement, then stops and so on. After a while it gets stable.

I thought it could be a problem related to the pid controller embedded inside the motors (Dunkermotoren BG75CI) because the movement seems to be like an underdamped second order response trying to reach the desired velocity value but when I used socketcan_bridge_node to send velocity values it works properly. So now i think it could be related to the differential drive controller or to the frequency of publishing. I put here my controller configuration

# The joint state controller handles publishing transforms for any moving joints
joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

# Differential Controllers ---------------------------------------

diff_drive_controller:
    type: "diff_drive_controller/DiffDriveController"
    left_wheel: 'left_wheel_joint'
    right_wheel: 'right_wheel_joint'
    publish_rate: 1000.0
    pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
    twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
    wheel_separation: 0.60125
    wheel_radius: 0.1

    required_drive_mode: 2

    # Wheel separation and radius multipliers
    wheel_separation_multiplier: 1.0
    wheel_radius_multiplier: 1.0

    # Velocity commands timeout [s]
    cmd_vel_timeout: 0.25

    # Base frame_id
    base_frame_id: base_link

    # Velocity and acceleration limits
    linear:
      x:
        has_velocity_limits    : true
        max_velocity           : 4.0  # m/s
        min_velocity           : -4.0 # m/s
        has_acceleration_limits: true
        max_acceleration       : 1  # m/s^2
        min_acceleration       : -1 # m/s^2
        has_jerk_limits        : true
        max_jerk               : 5.0  # m/s^3
    angular:
      z:
        has_velocity_limits    : true
        max_velocity           : 3  # rad/s
        has_acceleration_limits: true
        max_acceleration       : 1  # rad/s^2
        has_jerk_limits        : true
        max_jerk               : 1  # rad/s^3

Also my canopen configuration

bus:
  device: can0
  loopback: false
  driver_plugin: can::SocketCANInterface
  master_allocator: canopen::SimpleMaster::Allocator

sync:
  interval_ms: 0 # set to 0 to disable sync
  update_ms: 10 #update interval of control loop, must be set explecitly if sync is disabled
  overflow: 0 # overflow sync counter at value or do not set it (0, default)
# heartbeat: # simple heartbeat producer, optional!
  # rate: 20 # heartbeat rate
  # msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started

# struct syntax
nodes:
  node1:
    id: 1
    eds_pkg: bluerobot_basic # optionals package  name for relative path
    name: right_wheel_joint
    eds_file: "/eds/Dunker_BG75mi.eds" # path to EDS/DCF file

  node2:
    id: 2
    eds_pkg: bluerobot_basic
    name: left_wheel_joint # optional name
    eds_file: "/eds/Dunker_BG75mi.eds" # absolute name
    dcf_overlay:
        "4911sub1" : "0x1B"   #Reverse flag

use_realtime_period: true

defaults:
  motor_allocator: canopen::Motor402::Allocator # select allocator for motor layer
  motor_layer: # settings passed to motor layer (plugin-specific)
    switching_state: 5 # (Operation_Enable), state for mode switching
  pos_from_device: "(obj6063*2*pi)/(4096*23)" # actual position [m] -> rad
  vel_to_device: "rint(23*(vel*60/(2*pi)))" # rad/s -> rev/min
  vel_from_device: "(obj606C*2*pi)/(60*23)" # actual velocity [rev/min] -> rad/s
#  pos_to_device: "rint(rad2deg(pos)*1000)" # rad -> m
#  eff_to_device: "rint(eff)" # just round to integer
#  eff_from_device: "0" # unset

Edit1:

I also let you the EDS dictionary of my motors.

Dictionary object

Edit 2:

I plot the target velocity value, the actual velocity ... (more)