Height map and UWB API

[azazdeaz]

Experimental implementation of additional high-level DDS APIs .

Height Map API

I used the information from the message description and here: https://support.unitree.com/home/en/developer/LiDAR_service#heading-6
We would need a sample data or more detailed documentation to things like:

• What are the height measurements relative to? Currently it's the robots body plus one meter. Seems like it's relative to ground level (when the robot was turned on)
• How is the map oriented? Currently it follows the 2d top-down rotation of the robot body. Seems like it's in the world frame, but shifted is it centers around the robot. Not rotated.
• What is the message frequency? Currently it's 10hz. - This matches the differences of the timestamps from the example messages in the docs.

I added it as an optional feature to sport_mujoco.py . It can be enabled with --heightmap
Debug visualization can be enabled with --heightmap-debug

This data could be used for rail tracking and and as observation data for obstacle aware control policies.

height_map_test.mp4

UWB API

Unitree API reference: https://support.unitree.com/home/en/developer/UWB_service

For now, it only implements publishing the transform data in the message.
So fields related to buttons on the tag are not implemented, but we could add them if they use them to communicate with the robot. More on the controller here: https://support.unitree.com/home/en/developer/UWB%20Application

Locomotion controllers

Additionally to the WTW controller, it also enables to use RSL-RL based control policies.
sport_mujoco now has a --policy param to select wtw or rsl_rl
The model checkpoints will be automatically loaded from https://huggingface.co/Artefacts/go2-locomotion when used.

[fabid]

For the orientation, according to the docs:

The message topic is "rt/utlidar/height_map_array", and its coordinate system is the world coordinate system "odom" that is consistent with the body odometer

=> if I understand what you said, I think what you have is correct

What are the height measurements relative to? Currently it's the robots body plus one meter.

why 1 meter? I feel that I would expect that to be from ground level, but that would mean the API does some internal math to take into account the legs pose. Not sure about that
Checking docs further, I think you should use 33cm instead of 1meter

Function Name 	BodyHeight
Function Prototype 	int32_t BodyHeight (float height)
Function Overview 	Set the relative height of the body above the ground when standing and walking.
Parameter 	Height: Relative value to the default fuselage height, value [-0.18~0.03 ] (m)
Return Value 	If the call is successful, 0 will be returned. Otherwise, relevant error codes will be returned
Remarks 	Adjust the height of the body relative to the default state when standing or walking in balance. The default body height for Go2 is 0.33. For example, BodyHeight (-0.1) indicates adjusting the body height to 0.33-0.1=0.23 (m)

https://support.unitree.com/home/en/developer/sports_services

[fabid]

By the way, you are not using lidar rays for your current experimental implementation, right? I would expect the legs to block a large part of the field of view. Not sure what kind of processing they are doing

[azazdeaz]

By the way, you are not using lidar rays for your current experimental implementation, right? I would expect the legs to block a large part of the field of view. Not sure what kind of processing they are doing

I'm using the mj_ray API. It casts a ray (or rays) from a point and can return measurements like distance. The robot bodies are filtered out.
https://mujoco.readthedocs.io/en/latest/APIreference/APIfunctions.html#mj-ray

why 1 meter?

Just a random choice. It can be changed with a parameter.

Checking docs further, I think you should use 33cm instead of 1meter

Right, i can set the measurements relative to the body height.

[2lian]

Forgot the send my review last time

[2lian]

Quick question, how big/often are the heightmap messages?

[azazdeaz]

how big/often are the heightmap messages?

With the current settings, the bulk of the data is a 128x128 grid of float32 (16,384 floats = ~64 KB?). it's published at 10hz which can be changed with --heightmap-hz

[fabid]

@azazdeaz is this section about the uwb sensor?
if so to be more generic, maybe better to call it uwb_tag rather than human_marker

[azazdeaz]

Right, that would be better. I changed it in both repos. Thank you!