[cob_twist_controller] verify integration behavior and moving average (for position controller interfaces)

[fmessmer]

Debug, verify and investigate:

• mathematical formula of moving_average DONE
• mathematical formula of simpson_integration
• reset condition for integration
• initiall value of positional command in comparison to actual current joint position (there appears to be a jump in the first command message)
• smoothness of positional command (values)
• equidistance-ness of positional command (timestamps)

[fmessmer]

I made a small test script that publishes a input signal (sine) with some noise as well as a test node using various moving_average instances to smooth the given input signal.

Here is the result of ma(1, true), ma(3, true), ma(10, true), ma(100, true):

As it can be seen, setting a higher size does not really have any big effect on the smoothness....

[fmessmer]

Apparently, the old implementation used an exponential moving average with a constant decay factor (about 0.7?). As the weighting for "older" values (i.e. higher indices in the deque) tend towards 0.0, increasing the size does not have much of an effect. Beside it had some implementation and performance deficiencies!

Thus I re-implemented the MovingAverage in https://github.com/ipa-fxm/cob_control/commit/9b425a9f8772d96bc096f27cc148516e142f2514 implementing MovingAverageSimple (all weights 1), MoviingAverageSimple (weights according to triangle function) as well as MovingAverageExponential (with configurable decay factor).

Running the test script against the new classes looks like this (Input pink, Simple green, Triangle cyan, Exponential0.5 blue, Exponential0.3 red):

This shows that a MovingAverageExponential with a smaller factor results in a smoother output....I will use this within SimpsonIntegrator!

[fmessmer]

• ToDo: Reset() for MovingAverage :point_right: done in https://github.com/ipa-fxm/cob_control/commit/29acbd61a4473e758a5ef8032f69030d481b84fa
• Idea for reset condition: Use MovingAverage for the period :point_right: using max_command_silence_time seems to be a plausible timeout/reset strategy

[fmessmer]

As a second part of this issue, I had a look at the SimpsonIntegrator Again, using a test_node, I tested various inputs (would-be velocity commands) and how they are integrated (would-be position commands). When using a really noise sine velocity input signal, the integrated position output looks quite smooth now:

Also, I think the new reset strategy should fix the problems (jumps) in case the twist_controller is being re-activated after being idle for a while (i.e. control_mode_adapter had switched back to joint_trajectory mode)

I will test this on the robot on Tuesday!

[fmessmer]

When testing it with real hardware, ControllerInterfacePosition was still not usable with (Elmo) Torso. However, ControllerInterfaceTrajectory worked quite ok.

Still, it can be observed that all ControllerInterfaces derived from ControllerInterfacePositionBase, i.e. ControllerInterfacePosition, ControllerInterfaceTrajectory and ControllerInterfaceJointStates, significantly "lag" (i.e. "less dynamic") in comparison to ControllerInterfaceVelocity.

We need to investigate different Integration concepts - in addition to Simpson - and/or try to tune the MovingAverage configuration used within the Integrator! Maybe we might want to introduce some D part for the cob_frame_tracker PID controller??? @ipa-fxm-cm could you please have a look after you have finalized the cob_cartesian_controller?

Improving the Integration behavior might also lead to a better performance when using KinematicsExtensionLookat!

[fmessmer]

@ipa-fxm-cm is doing some experiments regarding verification/evaluation of integration/smoothing strategy as well as trying to (dynamical) speed-ups for the position-based controller interfaces...

[MaChristoph]

I made several tests to verify the smoothing methods. Firstly, I disabled the filtering of the position output to obtain a reference trajectory. Each plot compares the absolute value of the output velocity from the inverse kinematic solver (q_dot_ik) and the absolute value of the derived velocity from the integrated position (derived_q_dot_ik). I used the Euler differentiation for calculating the derivative, which may cause an inaccurate result (maybe I try with a better formula). As we can see, the derived_q_dot_ik value is kinda noisy around the q_dot_ik value.

In the following picture I used an exponential average (0.5) for the position output. This basicly shifts the noisy output by a constant value along the y-Axis. To proof this I made another test with an exponential average (0.3).

In the next plot I used the simple average over 3 elements which leads to almost the same result as in the exponential case.

Simple average (10)

So far we can see that averaging reduces the spread of the noise but shifts the mean absolute value below the actual velocity which results in a slower dynamical behavior of the system.

The next step is to smooth the derived velocity (just for visualization). So far I only considered the smoothness of the position data. So again I plotted the absolute value of the actual velocity (q_dot_ik) and the integrated position (pos) without filtering. As we can see, the position without filtering is not really "smooth". To fix this problem I have set the position filter to Simple Average over 2 values and for visualization purpose the derived velocity filter also to Simple Average over 2 values.

Conclusion: Until now, we can see that the Simple Average renders the derived position data very close to the actual velocity value and this is also visible in the simulation. Another positive fact is that the position curve is much smoother than before which may fix the chattering effect of the torso.

[fmessmer]

Thanks for the summary! Could you please commit your test code, too? Maybe also open a CodeReview PR against my ipa-fxm/indigo_dev branch (not ipa320) for on-code discussion.

So, pos/data is the joint position being calculated by the integration, q_dot_ik/data is the joint velocity i.e. input for the integration i.e. the IK solver result and derived_q_dot_ik/data is the joint velocity as differentiated from the joint position, right? Shouldn't the output be a vector, i.e. with one value per DoF? Did you just plot one DoF here? And beside the MovingAverage "smoothing" q_dot_ik before being used in the integration, you added another MovingAverage for "smoothing" pos i.e. the output of the integration, right?

As you said that you implemented a Euler differentiation maybe you could move this implementation into an according class within the util folder - similar to integration....maybe you could even prepare that integration and differentiation classes similar to how I did it for the MovingAverage, i.e. introducing an abstract BaseClass so that we could test various integration and differentiation approaches...

Also, please try these exeriments on real hardware - if possible...maybe @ipa-mdl does not need the lwa4d anymore and can set it back up in the appartment....

[MaChristoph]

So, pos/data is the joint position being calculated by the integration, q_dot_ik/data is the joint velocity i.e. input for the integration i.e. the IK solver result and derived_q_dot_ik/data is the joint velocity as differentiated from the joint position, right?

• pos/data is the integrated q_dot_ik value, for visualization I have used the absolute value of the position data (I didn't want too many curves in the plot).
• q_dot_ik is the result from the IK solver
• derived_q_dot_ik is the velocity obtained form the pos/data value derived by the euler derivation.

Shouldn't the output be a vector, i.e. with one value per DoF? Did you just plot one DoF here?

• answered above

And beside the MovingAverage "smoothing" q_dot_ik before being used in the integration, you added another MovingAverage for "smoothing" pos i.e. the output of the integration, right?

• The position output was smoothed with MovingAverage out of the box. I added another MovingAverage for smoothing the derived position data ( derived_q_dot_ik ). This has no effect in the dynamical performance of the system, it's just for comparing the q_dot_ik and derived_q_dot_ik values visually.

As you said that you implemented a Euler differentiation maybe you could move this implementation into an according class within the util folder - similar to integration....maybe you could even prepare that integration and differentiation classes similar to how I did it for the MovingAverage, i.e. introducing an abstract BaseClass so that we could test various integration and differentiation approaches...

• Do we really need the derivation stuff? I guess we don't need to derive data very precisely because we don't work with accelerations in TwistControl. So far we only need to integrate from velocities to positions.

[MaChristoph]

I made few more tests and found out some performance issues in the trajectory mode. The setup was the following. Moving Average Simple (2) for the position smoothing and for the derived position (derived_q_dot_ik).

First the position mode.

and then the trajectory mode.

I made another test with Simple Average (3) for the derived position to reduce the noise of the signal a bit

Position Mode

Trajectory Mode

We can see that the trajectory mode is about factor 3 slower than the position mode. I guess the problem is in the controller_interface.cpp. I try to fix this issue as soon as possible.

[MaChristoph]

I improved the system dynamics by reducing the time_to_start value to 0.01 from 0.05... (controller_interface.cpp) The trajectory mode is now as fast as the position mode

I have also tried to set the trajectory_velocity to the current velocity but this has effected the system dynamics negative. Adding the acceleration vector ended in the same negative result. So the best configuration so far is Simple Average (2) for the position filter and 0.01 for the time_to_start duration. The position curve is very smooth which is necessary to prevent the chattering of the joints.

[MaChristoph]

Found something interesting how the concept of trajectories works.

http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement

[fmessmer]

Indeed, that's a good find. So it seems we would want the "Zero trajectory start time (start now)" case, right? As I understand it, the time_from_start should be set to 0.0 (or not set at all) for this case. Could you test this, too? We only need to take care/make sure that the trajectory is not discarded because the TimeStamp being in the past! So we might need to be checking this and what to set for the trajectory.header.stamp.... You might want to make log outputs within the joint_trajectory_controller package in order to see what's going on....luckily, you have it in your catkin_ws :wink:

[fmessmer]

Regarding the filtering (MovingAverage): As you are now using SimpleAverage(2), have you also tried to use no MovingAverage at all? I.e. not for input and not for output...

[fmessmer]

Regarding setting of velocities and accelerations: According to http://wiki.ros.org/joint_trajectory_controller#Trajectory_representation, this would only affect the interpolation, linear, cubic, quintic.... I guess as we do not have the accelerations, i.e. we would need to derive them with a likely faulty differentiation method, we do not want to calculate and set them.... But as we have the desired velocities from the IK solver, could you investigate a bit further, why it appears to have a negative effect on the trajectory performance?

[mathias-luedtke]

We only need to take care/make sure that the trajectory is not discarded because the TimeStamp being in the past! So we might need to be checking this and what to set for the trajectory.header.stamp

Not the whole trajectory but outdated points will be discarded. If the resulting trajectory is empty, nothing will happen. Do you send full trajectories or single-point trajectories?

For debugging purposes I suggest to set header.stamp = now() and send the current positions with time_from_start=0. The commanded positions should follow with feasible time offsets. The result is the same as for header.stamp = now() + offset, but it represents a complete trajectory in time. So you are able to inspect the supposed starting point of the trajectory.

[fmessmer]

@ipa-mdl We are sending single-point trajectories as we only calculate the setpoint for the current control cycle...

[MaChristoph]

Today I made more tests and found a measurement method for accuracy of the input velocity and the derived position. I simply derived the norm of q_dot and the norm of the derivative of the integrated position (derived_q_dot). Afterwards I derived the ratio norm_derived_q_dot / norm_q_dot and summed the ratios up over all iterations until a hold_twist getting published. This allows me to check the actual velocities compared to the derived velocities. An optimal solution would be a 1.0. In the following table I will compare different input types and different position averaging methods.

Derived position with Simple Average over 2 values ( For derived position accuracy )

• In the first test I set the actual velocity and position for each trajectory point

  Filter for the position output	Mouse input	Cartesian path input
  No averaging	0.978865	1.071654
  Exponential Average(0.7)	0.737349	0.838607
  Simple Average(2)	0.664983	0.995245
  Exponential Average(0.5)	0.640396	0.667388

• Now I only set the position for each trajectory point

  Filter for the position output	Mouse input	Cartesian path input
  No averaging	0.942192	1.24865
  Exponential Average(0.7)	0.723221	1.01426
  Simple Average(2)	0.773926	0.791266
  Exponential Average(0.5)	0.591823	0.764932

We can see that the 'no averaging' case in the first table is the best one (This one is a bit above the 1.0, this is okay since the averaging and Euler method have some error). That wasn't really hard to guess, but again if we look at the plot of the joint position we can see the chattering problem.

The position is too noisy which results in even more noisy joint velocities (chattering effect). In the next plot you can see the same results with Simple Average (2).

Here we can see that the slopes are smoother but we have less dynamical behavior. We need to make some compromises and go one step further to the hardware tests and see how the real system behaves with the new findings. :grin:

[fmessmer]

the tests and investigations showed that the mathematical behavior of both MovingAverage and SimpsonIntegration is fine

however, motion using the JointTrajectoryControllerInterface (in particular Torso) is still jerky...but this is more likely to be related to the joint_trajectory_controller implementation or hardware :point_right: closing in favor of #83