Detail on replicating Safety functions
MAVProxyUser opened this issue · 3 comments
For anyone that wishes to try to implement them on their own (before we can) here are some notes.
https://github.com/unitreerobotics/unitree_legged_sdk/blob/master/include/unitree_legged_sdk/safety.h
Start here: unitreerobotics/unitree_legged_sdk#65
Long story short we can simply make use of https://github.com/Bin4ry/free-dog-sdk/blob/main/ucl/lowState.py#L58
return motorState(mode, q, dq, ddq, tauEst, q_raw, dq_raw, ddq_raw, temperature, reserve)
Looks to me like in order to add safety support, we need to utilize the above MotorState. Seemingly all "Safety()" does is check the static values depicted in the disassembled code attached for each joint vs the motor state.
I think we should add this dynamic (overrideable), usually we send an empty packet to the dog, this will return the SN of the dog and then we know the type: AlienGo, Go1, A1 , so based on this we can tell the sdk to not go over the said safety values instead of having the user to code it hard into the usage of the sdk.
The Safety function itself should be initialized by the unitree connection object, it can hold an bool property to check for it and adapt dynamically. And do as they do, just need to find time for it. Should be doable very easy by including an wrapper layer to the buildCmd and holding the current state in the connection object, so we can ask for it. Little modifications needed.
I have tried to implement something similar: i tried to detect low level and high level modes by sending something "empty". In practice this causes interference of high and low level locomotion. Also do we really need to support go1? Also power protect doesn't depend on model, so no detection is needed. Position protect is weird, since it requires perfect initial calibration and better to solve on higher level anyway.
position safety seems simple enough, not really sure what is the calculation they are doing for the power safety over there.
I think I will try writing something because running DRL policies without safeties sounds like a bad idea.
Although I see an advantage to run it on the MCU so the safeties would work even if connection is lost or the program crashes/hangs, so if you do have code to activate unitree safeties it can be useful.