Wheel reference tracking problem in simulation (undesired rolling)

[IoannisDadiotis]

As rolling of the simulated centauro has troubled me and other people quite a lot of times and this is a major issue that we cannot solve easily, I create this issue in order to share here some information which may be useful for anyone experiencing or tackling this problem.

Lately as part of implementing in simulation the scenario of our demo I have found some tracking errors of the wheel joints which in many cases lead to inaccurate tracking of cartesian trajectories by the wheel links. I conducted some simple rolling motion experiments using the branch demo-stepping of the model and this .config for the SoT. I, also, checked the effect of the motion duration (i.e. larger or lower wheel velocities) and the 2 different physics engines: ode and bullet. The simulations are done in this .world where some properties of the physics engines are defined. Below you can see the errors of the wheels global positions (topic /gazebo/link_states) in the rolling direction for 2 different motions:

1. All wheels roll 1 m forward
2. FL, HL, HR wheel roll 1.2 m and FR wheel rolls 1.0 m forward. This case corresponds to the first rolling motion in our demo and represents the case when we want to have simultaneous reshaping of the support polygon.

The commanded reference trajectories are smooth and, also, include a trajectory of the CoM, i.e. the CoM is also following a trajectory relative to the wheels From the results we can see that in the 2nd experiments the errors for the FR wheel (the one rolling a smaller distance) are much higher, reaching 7-8 % for the ode and 5-6 % for the bullet. This is in symphony with the errors I have noticed in the past in other simulations (here 3 wheels are commanded to follow a trajectory but FR wheel is, also, observed to roll while it is not supposed to). The problem exists regardless the physics engines and the durations of the commanded motion.

@alaurenzi , For the first scenario of rolling all wheels by 1 m in 4 sec using ODE, I recorded the wheels motor velocity and position (reference and real value) which you can see below. Wheels motor velocity: Deviation between reference and real motor velocity at peak point = 0.1 - 0.15 rad/sec for all wheel motors Wheels motor position: In the following, I recorded the initial and final values of reference and real motor positions, computed the difference between them (difference ref/real), converted this difference to the arc that the wheel has covered (that is distance travelled by the wheel if we assume zero slip). Also, you can see the deviation of this arc from the commanded one (ref error for the references and real error for the real motor position). The difference between the latter two is the tracking error. Finally, it is obvious that more than 70% (in some wheels even 99 %) of the error is because of a wrong reference generation rather than tracking error. One would expect the reference motor positions to result in a reference arc equal to the commanded rolling distance.

Notice that the wheel motors are velocity controlled.

UPDATE: This issue was solved for the rolling phases where all legs roll the same distance (no support polygon reshaping). It was indeed a problem related with cartesio and the reference generation and was due to low weight of the Rolling tasks. However for rolling phases which include modification of the support polygon there exist tracking errors. In particular for a rolling phase of 1.2 m except FR foot that rolls 1.0 m we have the following: which is again significant errors especially for the FR leg that rolls different distance. The error of the FR leg is 90 % tracking error and only 10 % error from wrong reference generation. The gains for the wheel actuator controller are

<kp>2100000</kp>
<kd>1</kd>

I also tested the same scenario for increased P gain and an I gain

<kp>4200000</kp>
<kd>1</kd>
<ki>500</ki>

and the results are:

[IoannisDadiotis]

The undesired rolling of the wheels during stance phases (constant position of a foot) is an important issue which may tightly related to the reference tracking issue during roll phases (see my previous comment).

I recorded the amount of undesired rolling we have during the sequence of movements of our demo. Below you can see these errors for FR and HR wheel (from /gazebo/link_states) during the phases that each leg should remain at a constant position (stance phases). The undesired rolling for each foot is computed only for the stance phases and, thus, for the rest the we depict zero errors for clarity in the diagram. We can see that the error increases with time, as expected as the wheels deviate from the initial positions as the rolling continues and time passes. The frequency of the recorded data is 1000 Hz. Roughly speaking, we can notice that the wheels can roll up to 6-7 cm in almost 10 secs.

[IoannisDadiotis]

FYI @liesrock

[alaurenzi]

As suggested by @aled96, we could add an integral controller for the wheels and see if it improves anything..

[aled96]

After changing line 24 of https://github.com/ADVRHumanoids/iit-centauro-ros-pkg/blob/velodyne-realsense/centauro_urdf/urdf/gazebo/xbot.gazebo.xacro from: <gain name="wheel_mot" p="0" d="10"/> to <gain name="wheel_mot" p="250" d="10"/> I run few experiments with my planner, sending position references to the wheels (using cartesio2.0) and the results seem nice !

In fact I was able to step on/down a platform without experiencing the rolling problem.

[alaurenzi]

Implementing the integral controller will be even better, since there will be no need to send position references to the wheels (as is on the real platform)!

[aled96]

Perfect, that would be great !

[IoannisDadiotis]

After changing line 24 of https://github.com/ADVRHumanoids/iit-centauro-ros-pkg/blob/velodyne-realsense/centauro_urdf/urdf/gazebo/xbot.gazebo.xacro from: <gain name="wheel_mot" p="0" d="10"/> to <gain name="wheel_mot" p="250" d="10"/> I run few experiments with my planner, sending position references to the wheels (using cartesio2.0) and the results seem nice !

In fact I was able to step on/down a platform without experiencing the rolling problem.

This eliminated undesired wheel rolling when I performed pure locomotin but has caused me some new problems. The wheels are not controlled properly when sending velocity references to the wheel joints through /xbotcore/command topic

rostopic pub /xbotcore/command xbot_msgs/JointCommand "header:
  seq: 0
  stamp: {secs: 0, nsecs: 0}
  frame_id: ''
name: ['j_wheel_1', 'j_wheel_2', 'j_wheel_3', 'j_wheel_4']
position: [0]
velocity: [5, -5, 5, -5]
effort: [0]
stiffness: [0]
damping: [0]
ctrl_mode: [2,2,2,2]
aux_name: ''
aux: [0]" -r 100