Collective motion ticket

[philipniklas-r]

As discussed on the monday meeting, here are some of my thoughts as a collective ticket.

Main suggestions from the monday meeting

Walk Step Adjustment

https://github.com/HULKs/hulk/blob/main/etc/configuration/default.json#L381
Reduce max_step_adjustment down to 0.0018.

Gyro

https://github.com/bhuman/BHumanCodeRelease/blob/coderelease2022/Src/Modules/MotionControl/WalkingEngine/WalkLibs/WalkPhaseBase.cpp#L784
Experiment with a similar approach with the gyro balancing. The difference for us on worn-out robots are worlds apart, even though the adjustment is just conditional for up to 2 degrees on the KneePitch and HipPitch

https://github.com/HULKs/hulk/blob/main/etc/configuration/default.json#L357
And do not always balance with the Hip and KneePitch (only conditionally like above). Atleast from experience I only had bad observations.

General walking suggestions

Step duration

The current duration is configured here: https://github.com/HULKs/hulk/blob/main/etc/configuration/default.json#L351
The increase of duration here: https://github.com/HULKs/hulk/blob/main/etc/configuration/default.json#L388
And used here: https://github.com/HULKs/hulk/blob/main/crates/control/src/motion/walking_engine.rs#L450

What works great for us is having a base duration of 250 ms, always.
For bigger side steps this does not work great. Here the idea (back in 2019; And also used in the original rUNSWift walk I think) is to increase the step duration for side steps.

What we are currently doing is to still keep the 250 ms for side steps, but to increase the interpolation duration for the step height. So we are still using the parabolic_return function, but use 125 ms (half of the 250 ms) to interpolate to the max height, and use the longer duration (like 290 ms) to interpolate back to 0. This causes the swing foot to keep a little bit of height for a few motion frames longer, which is enough to have side steps of the correct lenghts. Otherwise the swing foot will touch the ground too early and the side step will be very short.

Turn speed

The speed is currently 1.2 rad: https://github.com/HULKs/hulk/blob/main/etc/configuration/default.json#L336
The speed is modified here: https://github.com/HULKs/hulk/blob/main/crates/control/src/motion/walking_engine.rs#L733
With the current value of 0.05 for inside_turn_ratio (https://github.com/HULKs/hulk/blob/main/etc/configuration/default.json#L367), the result for outter turn steps (left foot is swing -> 95% left rotation is allowed; right foot is swing -> 95% right rotation is allowed) are 1.2 rad * 0.95 = 1.14 rad -> 64 degree rotation in one step.
This explains those huge rotation step like here in the video (green 6): https://www.youtube.com/live/XLSFgg2ssYU?feature=share&t=18582
I assume, the value inside_turn_ratio should be something else? (like 0.5, or like in the rUNSWift walk 0.3 or something like that).

As a side note, for why those huge rotation steps result in falling robots (especially on worn-out ones): when rotating the feet outwards (so left foot counter clockwise, right foot clockwise), the yaw-rotation is done only by the joint HipYawPitch. This joint does not only rotate in the yaw axis, but also on the pitch axis. (http://doc.aldebaran.com/2-8/family/nao_technical/joints_naov6.html#pelvis-joints). This means would you only change the HipYawPitch (without doing propper InverseKinematic), the robots torso would rotate forward.
Normally the InverseKinematic corrects this and the code commands correct joint positions. Here the pitch joints (and rolls joints for high yaw rotations) of the legs need to rotate the torso further backwards.

And here is the problem: if the robot is tilting forward from some previous motion (or is just worn-out), those pitch joints get stuck. Meanwhile the HipYawPitch can move freely. As a result the robot just tilts more and more forward. This is the main reason (atleast from my current opinion) why the robots fall very quickly is such situations.

So I suggest to adjust the value inside_turn_ratio, to not have that much rotation in one direction. Theoretically I would also suggest to reduce the overall rotation (e.g. we use 120 degree per second -> 19.8 degree for inner rotations, 39.6 egree for outer rotations; reduced for worn-out robots). On the other hand, use as high rotations as possible. As long as your walk supports that, there is not need to reduce it. As speed is always an advantage.

Foot Leveling

I noticed a lot that your robots like to "crash" their swing foot toes into the ground (especially during kicks), which often destabilizes the robot. For that you already have a foot leveling approach.

https://github.com/HULKs/hulk/blob/main/crates/control/src/motion/walking_engine/balancing.rs#L51

Our original foot leveling for the swing foot does lots of extra complicated stuff. Most of it is probably overkill (and for the GORE in april I rewrote it completly). But from the experiments a few years ago I noticed, that the compensation works a lot better, if most of it is removed close to the end.

https://github.com/bhuman/BHumanCodeRelease/blob/coderelease2022/Src/Modules/MotionControl/WalkingEngine/WalkLibs/WalkStepAdjustment.cpp#L486
The full compensation is applied as long as the percentage of the step is smaller than soleForwardCompensationReturnZeroRation or soleBackwardsCompensationReturnZeroRatio (depending on the direction of the compensation). For forward compensation (so the torso is tilting forward), the full compensation is applied until 75% of the step is executed. Afterwards it is reduced to 1.f - std::min(1.f, stepRatio). This changes the compensation from one frame to another down to 25% (and slowly to 0 afterwards).

When I first tested this idea for the compensation, this calculation was a bug. Then I tested around a lot with it (also just this year again) and I observed the stability of the robot is significantly better if the compensation is reduced a lot before the next support foot switch. So unless you did not test it yet, I recommend to test this interpolation version.

In our implementation we also interpolate the compensation based on the torso and the sole rotation. The idea is to do no compensation if the error is just small, and interpolate it to 100% if the error is big enough.
We also use the torso orientation. In case you are using the imu angles directly, without an own filter, I recommend to not use the imu_adjustment. On the other hand I dont quite understand your current use of imu_adjustment.

Position resets

As the robots (especially worn-out ones) like to have differences between the commanded and measured positions, some kind of reset at the moment of a support foot switch is beneficial.
Our version from 2022: https://github.com/bhuman/BHumanCodeRelease/blob/coderelease2022/Src/Modules/MotionControl/WalkingEngine/WalkLibs/WalkPhaseBase.cpp#L381

If you want some explanation, I can go deeper about the reasoning. I do not recommend to do the exact same code, but I recommend to do some kind of reset where you think it would help.

For the GORE in april I also added a reset for the HipRolls when walking sideways, which increased the stability by a lot for worn-out robots.

In general the idea is just to make sure, that your request of the joint positions does not differ too much when starting a new walking step (and yes, they can differ by a lot sometimes). And as it is a new walking step, jumps in the joint positions are not really a problem (as long as it does not effect all 15 leg joints :P )

Other general suggestions

Energy saving mode

Berlin United - Nao Team Humboldt:
https://www.naoteamhumboldt.de/wp-content/papercite-data/pdf/naoth-report18.pdf (6.2)
https://github.com/BerlinUnited/NaoTH/blob/develop/NaoTHSoccer/Source/Motion/Engine/InverseKinematicsMotion/Motions/StandMotion.cpp#LL338C37-L338C37 (I hope it is the correct code)

B-Human:
https://github.com/bhuman/BHumanCodeRelease/tree/coderelease2022/Src/Modules/MotionControl/EnergySaving
https://github.com/bhuman/BHumanCodeRelease/blob/coderelease2022/Src/Modules/MotionControl/WalkingEngine/WalkingEngine.cpp#L631

The idea from berlin that we took is to check once within a time window, which joints have a filtered current above 300 mA. Berlin noticed that joints with above this current value heat up, those under it cool down.

The joints with above 300 mA are then adjusted by:

• calculating the difference between measured and requested positions
• adjust requested position by the minimal position step (https://github.com/BerlinUnited/NaoTH/blob/develop/NaoTHSoccer/Config/scheme/RC19/IKParameters.cfg#L23)
• over a short time the joints should start using a lot less current
• have some reset condition like the center of mass position, to reset the offsets to prevent a fall

Berlin also adjusts the stiffness.

We are not changing the stiffness, but are doing a bit more overengineering.

• Using larger adjustmens if the difference is larger
• Adjusting multiple joints at once (as long as not much is adjusted yet)
• Adjustment direction of the knee depends on the stand (walk height or high stand) -> otherwise the knee will heat up even faster ;)

Until last year, Berlins version worked better for the normal walk height stand. But their code is probably still more readable.

As you mentioned you had problems with your transitions:
Implement the adjustment as a helper object. Then you can use it in your StandMotion (our whatever generates your standing motion). This can just generate the same joint request in every frame, but at the end of its code execution it will call the helper object. This helper object holds the information about the current offsets, applies further changes and just adds the offsets in every motion frame on top of the positions. Afterwards the result is send to the robot as usual.

The only thing you need to consider is to stop calling the helper object after the robot starts walking again (or does other motions). Also the start positions for the walk need to be adjusted based on the applied offsets OR you need to implement a transition phase to interpolate the offsets back to 0.

The impact of such energy saving mode is quite high. A robot in the high stand can stand for hours without heating up. In the walk height stand it depends. Normally our goalie legs are still under 40°C, even tho it is standing in the walk height stand. Which means we can swap a field player with the goalie in the halftime pause and the field player will cool down even further in the second half.

Get Up Motions

There is a lot of stuff I could write, so if you are interested in more details just give me a notice.
But here are some points of interest:

• When transitioning from the sitting position into the standing position, you can use the knee pitches (and also ankle and hip pitches) for some gyro balancing. The knees (and hips) should use smaller factors than the ankles. But using those joints here could remove a lot of problems (like falling backwards at the very end). Atleast it improved our get up routine a lot.
• Do some kind of compensation for stuck joints. The HipYawPitch loves to get stuck, while the other joints still move according to plan. This causes the robots to just fall forward
• As more teams start to use wait times: from experience I know that the HipYawPitch can not move on the gras fields when the interpolation time is too slow. Some kind of body wobbling is unfortunately needed
• Implement break up conditions. I am not sure what the state is for you here, I think you started with it? But close to every keyframe should have conditions to break up. Whenever it is possible that the robot could fall on the body parts again with full stiffness -> a break up should be defined. One bad fall and something could be broken. And if not, the joints just got a few months older from that one fall.
  • A simple definition of angles for the torso (or imu) rotation should be enough

Fall motion

Correct me if I am wrong, but I think if your robots fall forward the legs stiffness is set to 0? https://github.com/HULKs/hulk/blob/main/etc/configuration/default.json#L238
I recommend to set it to 0.1 or 0.2 for the legs (assuming the max value is 1. I don't know your value range). Otherwise the ankles will just close fast and the robot falls even faster forward. Also in case for the arms 0.8 is actually used: are you really using 80% stiffness for the arms? If so: why did they not break yet? :D

Apart from that: for the backward fall a simple fall motion that overwrites the leg joints is enough (like a sitting position, with a lot forward torso rotation). So it is quite fast to implement. But (as a tipp) do not close the knees completely, instead set it to something like 105 degree (123 degree is fully closer). Combined with 0.2 (or 20 %) stiffness some of the fall energy is taken by this buffer zone in the knees.

High Stand

Just a reminder to not instantly interpolate into the high stand after walking. A simple delay is enough for most cases. Maybe add a check with the gyros or something like that.