[cob_twist_controller] JointTrajectoryControllerInterface on Torso

[fmessmer]

further investigate behavior of twist_controller when using it with JointTrajectoryControllerInterface

in #66 it has been tested/verified that both the behavior of the utils MovingAverage as well as SimpsonIntegration are fine and perform acceptably
still during tests with hardware - torso in particular - resulting motion were quite jerky.

this can be seen in quite jerky joint_trajectory_controller/state/desired messages (velocities as well as positions).
assumption is that the joint_trajectory_controller is not meant to be commanded at high update_rates (50Hz) - it's much less jerky when commanding at only 10Hz
as the jerky motion has mainly be observed on Torso (Elmo), there also might be potential in firmware configuration and/or motor/sensor hardware optimization @ipa-mdl @ipa-tif @ipa-bdw

[mathias-luedtke]

I guess a spline interpolator is not the right controller for "high" rate control.
Have you tried the group forward controller with mode 7?
Did you turn off all velocity/acceleration limits in URDF/yaml? (ros-industrial/ros_canopen#141)

[fmessmer]

Note to myself:
Make plots comparing

• q_dot_ik
• q_dot_avg
• derived_integrated_q_dot
• jtc/state/desired/velocitites
• joint_states/velocities

Also, verify that period used in integrator is equal to period between two arriving twist commands #76

[fmessmer]

Testing the integration/execution period with the following LoC in SimpsonIntegrator::updateIntegration() and in CobTwistController::solveTwist()

ros::Time now = ros::Time::now();
ROS_INFO_STREAM("\t\tSIMPSON_INTEGRATOR: Period: " << (now - last_update_time_).toSec());
last_update_time_ = now;

shows that the periods are identical +/- 10^-4

[fmessmer]

Here are some plots from using the joint_trajectory interface with the torso (commit 717608e);
Publishing Twists at 10Hz
torso_2_joint

torso_3_joint

Publishing Twists at 50Hz:
torso_2_joint

torso_3_joint

bottom line: trajectory_interface is way too slow for...

[fmessmer]

Plots using VelocityInterface (joint_group_velocity_controller) Mode3 at 50hz
torso_2_joint

torso_3_joint

limits as given in torso_driver.yaml:

  - name: torso_2_joint
    id: 32
    dcf_overlay: # "ObjectID": "ParameterValue" (both as strings)
      "6083": "6000" # profile acceleration, mgrad/sec^2
      "6084": "6000" # profile deceleration, mgrad/sec^2
      "60C5": "6000" # max acceleration, mgrad/sec^2
      "60C6": "6000" # max deceleration, mgrad/sec^2
  - name: torso_3_joint
    id: 33
    dcf_overlay: # "ObjectID": "ParameterValue" (both as strings)
      "6083": "80000" # profile acceleration, mgrad/sec^2
      "6084": "80000" # profile deceleration, mgrad/sec^2
      "60C5": "80000" # max acceleration, mgrad/sec^2
      "60C6": "80000" # max deceleration, mgrad/sec^2

[fmessmer]

Plots using JointGroupInterpolatedPositionController (joint_group_position_controller) Mode 7:
torso_3_joint:

⚠️ motion is fine when smooth sine is commanded using test_forward_command_sine_node.cpp

[fmessmer]

Summary:

• joint_trajectory_controller (mode7): internal interpolation seems to reduce the absolute magnitude of the velocities (would need to look into it in ros_controllers/joint_trajectory_controller)
• joint_group_velocity_controller (mode3): somehow has internal acceleration limits and is not able to follow given (non-acceleration limited) velocity commands
• joint_group_position_controller (mode7): position command seems to be "too jerky" and torso starts to vibrate...

[fmessmer]

We found acceleration limits set wrong in the Elmo firmware (10000 for torso_3_joint) that explains the ramps as seen in the respective plot.
After setting the firmware acceleration limits to 100000 the limits from the torso_driver.yaml (80000) are applied...and motion looks much more reactive

However, when switching back to trajectoy_mode (after max_silence_time), we always fall into diagnostics_error:

Full Name: /Actuators/Torso/torso torso_driver: torso_3_joint
Component: torso torso_driver: torso_3_joint
Hardware ID: none
Level: ERROR
Message: Node has emergency error; Motor has fault

error_register: 33
errors: 8611#0

and (several!) recover are needed...

[fmessmer]

The above mentioned problem occurs when switching from mode 7 to mode 3, then moving the joint for more than 15° and then switch back from mode3 to mode7...
A fix is to add the following to the defaults of torso_driver.yaml

  motor_layer:
    switching_state: 4 # switched on - stop controller before switching modes

⚠️ In mode 3 end position limits are not considered!

⚠️ Also the movements have a significant overshoot
👉 needs more investigation - probably bad frame_tracking PID configuration...?

[fmessmer]

ToDo's:

1. investigate overshoot when using velocity_interface (mode3)
2. investigate smoothing the commanded positions when using position_(interpol)_interface (mode7)
3. investigate speedup/interpolation of joint_trajectory_interface (mode7)

[fmessmer]

We have decided to go with the velocity_interface (mode3) for now as it yields the most promising performance...with have various temporary "hacks" for the still-missing joint_limit_interface....e.g. directly in elmo firmware as well as in the limiters of the twist_controller...

[mathias-luedtke]

the new limit interface is tracked in #72