[Unitree A1] Using sport mode with mbs_unitree_ros wrapper

nhooram · October 11, 2021, 9:36am

Hello Mybotshop team,

I’m trying to get the A1 work with ROS. I’ve successfully installed the unitree_ros, unitree_ros_to_real, and the mbs_unitree* packages on a Jetson AGX board that sits on top of the robot, connected through the ethernet cable. I’m running unitree_legged_sdk 3.2.

On the bot, there are total of three machines I can ssh into:

192.168.123.12: the upboard
192.168.123.161: “slam board”, the one the d435 is connected to
192.168.123.162: extra jetson AGX board, also known as 192.168.178.36 in the local wifi network

I am running ROS on the 3rd board, with the address 192.168.123.162.

So far, I’ve been able to do the following:

Turn on the robot. It stands up and stays still.
I don’t touch anything on the joystick controller that came with the robot. I think this means that the robot is in “normal mode”
I ssh into the AGX board (machine #3) and run the following commands:

sudo su
roslaunch mbs_unitree_ros high_level_mode.launch

In another terminal, I run the teleop_keyboard node, and can control the movement of the robot.

The issue with the above is that the robot seems to drift quite a bit. I think this has to do with it being in Normal Mode. Therefore I tried to run the steps 3 and 4 after putting the robot into “sport mode”, by pressing L2+B -> L1+start -> start, like written on the joypad controller.

The problem is, when the robot is in sport mode, controlling the robot via the /cmd_vel topic through the mbs wrapper doesn’t seem to work.

The full stdout is the following:

started roslaunch server http://192.168.178.36:35179/

SUMMARY
========

PARAMETERS
 * /control_level: highlevel
 * /firmwork: 3_2
 * /robot_name: a1
 * /rosdistro: melodic
 * /rosversion: 1.14.11

NODES
  /
    high_level_driver (mbs_unitree_ros/high_level_mode)
    node_lcm_server (unitree_legged_real/lcm_server_3_2)

auto-starting new master
process[master]: started with pid [10013]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 70b25c32-2a77-11ec-9a59-48b02d3dc095
process[rosout-1]: started with pid [10054]
started core service [/rosout]
process[node_lcm_server-2]: started with pid [10061]
UDP Initialized. Port: 8080
LCM Initialized. Subscribe channel: LCM_High_Cmd, Publish channel: LCM_High_State
[Loop Start] named: UDP_Send, period: 2(ms), run at cpu: 3
[Loop Start] named: UDP_Recv, period: 2(ms), run at cpu: 3
[Loop Start] named: LCM_Recv, period: 2(ms), cpu unspecified
[Loop Start] named: control_loop, period: 2(ms), cpu unspecified
process[high_level_driver-3]: started with pid [10068]


Error! LCM Time out.
[node_lcm_server-2] process has died [pid 10061, exit code 255, cmd /home/hooram/a1_ws/devel/lib/unitree_legged_real/l
cm_server_3_2 a1 highlevel __name:=node_lcm_server __log:=/root/.ros/log/70b25c32-2a77-11ec-9a59-48b02d3dc095/node_lcm
_server-2.log].
log file: /root/.ros/log/70b25c32-2a77-11ec-9a59-48b02d3dc095/node_lcm_server-2*.log

^[[A
^C[high_level_driver-3] killing on exit
*** buffer overflow detected ***: /home/hooram/a1_ws/devel/lib/mbs_unitree_ros/high_level_mode terminated

I would appreciate any pointers in solving this issue.

Regards,

Rammi

tahir.mehmood · October 11, 2021, 11:06am

Great question. Two things to tell you:

The robot driver in ROS by default is set to work in normal mode. So when the robot is turned on, its in normal mode. Here at this point the provided ROS driver should work.
ROS control in sports mode will be released soon in our next version. But you can do this if you want by chaning the ip for the message communication.
For example here you can have a look for ip address and its class for UDP communication(All communication on A1 is UDP based using LCM).

What you have to do to get this working:

Setup a communication with UDP and LCM(an example of this you can see in mbs_unitree_ros/src/llm.cpp ) and specify the ip which is mentioned in here rest you can take the code from mbs_unitree_ros/hlm.cpp as in hlm.cpp not specifically communication is used which in fact is done in llm.cpp.

If you have any question/problem please do not hesitate to contact support@mybotshop.de or post here.

nhooram · October 11, 2021, 11:41am

Hi Tahir,

Thanks for the insight. This is in the direction I was thinking, noting that there seems to be a processed called A1_sport_1, running on port 8018 and 8082 on the upboard.

Below is the result of sudo netstat -tulpn on machines 192.168.123.12 and 192.168.123.161:

192.168.123.161, upboard

Proto Recv-Q Send-Q Local Address           Foreign Address         State       PID/Program name
tcp        0      0 0.0.0.0:22              0.0.0.0:*               LISTEN      986/sshd        
tcp        0      0 127.0.0.1:631           0.0.0.0:*               LISTEN      801/cupsd       
tcp6       0      0 :::22                   :::*                    LISTEN      986/sshd        
tcp6       0      0 ::1:631                 :::*                    LISTEN      801/cupsd       
udp        0      0 0.0.0.0:631             0.0.0.0:*                           855/cups-browsed
udp        0      0 0.0.0.0:5353            0.0.0.0:*                           815/avahi-daemon: r
udp        0      0 0.0.0.0:8018            0.0.0.0:*                           1547/A1_sport_1 
udp    83200      0 0.0.0.0:8082            0.0.0.0:*                           1547/A1_sport_1 
udp        0      0 0.0.0.0:47035           0.0.0.0:*                           815/avahi-daemon: r
udp6       0      0 :::33877                :::*                                815/avahi-daemon: r
udp6       0      0 :::5353                 :::*                                815/avahi-daemon: r

192.168.123.12, “slam” board

Proto Recv-Q Send-Q Local Address           Foreign Address         State       PID/Program name    
tcp        0      0 0.0.0.0:111             0.0.0.0:*               LISTEN      4561/rpcbind        
tcp        0      0 0.0.0.0:8080            0.0.0.0:*               LISTEN      7827/./build/RobotV 
tcp        0      0 127.0.0.53:53           0.0.0.0:*               LISTEN      4525/systemd-resolv 
tcp        0      0 0.0.0.0:22              0.0.0.0:*               LISTEN      6690/sshd           
tcp        0      0 127.0.0.1:46721         0.0.0.0:*               LISTEN      6651/containerd     
tcp        0      0 127.0.0.1:2947          0.0.0.0:*               LISTEN      1/init              
tcp6       0      0 :::111                  :::*                    LISTEN      4561/rpcbind        
tcp6       0      0 :::8080                 :::*                    LISTEN      7827/./build/RobotV 
tcp6       0      0 :::1716                 :::*                    LISTEN      7792/kdeconnectd    
tcp6       0      0 :::22                   :::*                    LISTEN      6690/sshd           
tcp6       0      0 ::1:2947                :::*                    LISTEN      1/init              
udp        0      0 0.0.0.0:9011            0.0.0.0:*                           7827/./build/RobotV 
udp        0      0 0.0.0.0:921             0.0.0.0:*                           4561/rpcbind        
udp        0      0 0.0.0.0:5353            0.0.0.0:*                           5096/avahi-daemon:  
udp        0      0 0.0.0.0:54917           0.0.0.0:*                           5096/avahi-daemon:  
udp        0      0 127.0.0.53:53           0.0.0.0:*                           4525/systemd-resolv 
udp        0      0 0.0.0.0:111             0.0.0.0:*                           4561/rpcbind        
udp6       0      0 :::921                  :::*                                4561/rpcbind        
udp6       0      0 :::5353                 :::*                                5096/avahi-daemon:  
udp6       0      0 :::58815                :::*                                5096/avahi-daemon:  
udp6       0      0 :::1716                 :::*                                7792/kdeconnectd    
udp6       0      0 :::111                  :::*                                4561/rpcbind

Since I can’t ssh into the realtime controller board (192.168.123.10), I can only nmap to see the open ports on that machine. It seems the normal mode comm channel is UDP 192.168.123.10:8007:

hooram@jetson-agx:~$ sudo nmap -sU -p 8000-8100 192.168.123.10

Starting Nmap 7.60 ( https://nmap.org ) at 2021-10-11 13:37 CEST
Nmap scan report for 192.168.123.10
Host is up (0.00027s latency).
Not shown: 100 closed ports
PORT     STATE SERVICE
8007/udp open  unknown
MAC Address: 00:80:E1:00:00:00 (STMicroelectronics SRL)

Nmap done: 1 IP address (1 host up) scanned in 3.06 seconds

Some follow up questions:

Noting that on the upboard, there are two ports that has to do with the sport mode, namely 8018 and 8082. Which is the correct one?
Is the following what you mean by “by chaning the ip for the message communication.”?

change something in the four lines in unitree_legged_sdk/include/unitree_legged_sdk/udp.h#L19

constexpr int UDP_CLIENT_PORT = 8080;                       // local port
constexpr int UDP_SERVER_PORT = 8007;                       // target port
constexpr char UDP_SERVER_IP_BASIC[] = "192.168.123.10";    // target IP address
constexpr char UDP_SERVER_IP_SPORT[] = "192.168.123.161";   // target IP address

Is the idea that we “fake” the server for the basic (I suppose it means normal here) mode by changing the middle two lines to the following?

constexpr int UDP_SERVER_PORT = 8018;      //or 8082, which one?
constexpr char UDP_SERVER_IP_BASIC[] = 192.168.123.161

And of course

# in the sdk folder
rm -rf build
mkdir build
cd build 
cmake ..
make

Thanks,

Rammi

tahir.mehmood · October 12, 2021, 7:33am

In response to your questions:

Noting that on the upboard, there are two ports that has to do with the sport mode, namely 8018 and 8082. Which is the correct one?
- I am not sure about that, I think these are just the comm ports mode change should be done only by the ips. But I can be wrong, we need to confirm this from Unitree.
Is the following what you mean by “by chaning the ip for the message communication.”?
- Well I didn’t thought about changing the ip in here but that could be a starting point. The answer which I stated above means that when you look inside the code for low level control(mbs_unitree_ros/src/llm.cpp), in this node communication is setup from the bottom up handling every message by binding LCM and UDP. Which is not the case in high level control(mbs_unitree_ros/src/hlm.cpp) so adopt the code in hlm.cpp in a communication fashion done in llm.cpp. And in this adoption while creating a class instance for UDP communication you can specify the ip for sport mode in the constructor.

Constructor for UDP Comm

This default constructor is used at the moment by mbs_unitree_ros driver. Without changing the default ip and ports for the robot.
For you, the solution is to use this or this initialization which than will demand the robot to be in sport mode and use that. Here you can see a few examples for UDP communication setup.

nhooram · October 12, 2021, 2:17pm

Here’s what I tried

// in hlm.cpp
49a50,55
>     UniTreeDriver(uint8_t level, ros::NodeHandle *nh) : udp(8080, "192.168.123.161", 8082, sizeof(SendHighROS), sizeof(RecvHighROS))
>     {
>         state_pub = nh->advertise<unitree_legged_msgs::HighState>("/state", 10);
>         twist_sub = nh->subscribe("/cmd_vel", 500, &UniTreeDriver::twistCallback, this);
>         set_body_pose = nh->advertiseService("/set_body_pose", &UniTreeDriver::poseCallback, this);
>     }
56,62d61
< UniTreeDriver::UniTreeDriver(ros::NodeHandle *nh)
< {
<     state_pub = nh->advertise<unitree_legged_msgs::HighState>("/state", 10);
<     twist_sub = nh->subscribe("/cmd_vel", 500, &UniTreeDriver::twistCallback, this);
<     set_body_pose = nh->advertiseService("/set_body_pose", &UniTreeDriver::poseCallback, this);
< }
< 
147c146
<     UniTreeDriver driver = UniTreeDriver(&nh);
---
>     UniTreeDriver driver = UniTreeDriver(UNITREE_LEGGED_SDK::HIGHLEVEL, &nh);

and

// in unitree_legged_sdk-3.2/include/lcm_server.h
65c65,66
<         Lcm_Server_High(LeggedType rname) : udp(HIGHLEVEL), mylcm(HIGHLEVEL)
---
>         Lcm_Server_High(LeggedType rname) : udp(8080, "192.168.123.161", 8082, sizeof(HighCmd), sizeof(HighState)), mylcm(HIGHLEVEL)

After rebuilding the sdk and the catkin workspace, running

sudo su
roslaunch mbs_unitree_ros high_level_mode.launch

still doesn’t seem to do the trick. I also tried to comment out

<---<include file="$(find unitree_legged_real)/launch/real.launch" />---!>

in the high_level_mode.launch file, still to no avail.

Unfortunately that’s the extent of my cpp-fu, and I’ll play around a bit more. Also, fyi, I saw that the sdk version 3.3 supports A1, and yesterday was a commit to the unitree_legged_sdk v3.3 with an example python script.

Is there any timeline on the release of the next version of mbs that supports sport mode?

tahir.mehmood · October 12, 2021, 3:04pm

I think its hard to comment without looking into the code for the robot or not by seeing the errors. Was there any error?

If you think a review will be helpfull, you can send your package to info@mybotshop.de

MYBOTSHOP_Support · October 12, 2021, 3:18pm

Hi Rammi,

Unfortunately I can’t tell you any exactly date yet. We are still improving our code and working on new features. At the moment our highest priority has the stairs climbing issue. But I will mention your wish in our internal project list and will try to have something for you asap!

nhooram · October 13, 2021, 2:53pm

Lemme nuke my environment and try again. If I need further assistance I’ll make another thread or email you.

nhooram · October 14, 2021, 11:35am

I was able to sort out the issue. Basically what I did was:

decided to use unitree_legged_sdk_3.3.x with sport mode v1.19 (the folder /home/unitree/Unitree in the upboard - shoot an email to them for the file)
- no modification to the sdk code was made
change the ROS <-> LCM bindings in catkin_ws/src/mbs_driver/mbs_unitree_ros/include/mbs_unitree_ros/convert.h to reflect the changes made to unitree_legged_sdk-3.3.x (in the sdk/include/comm.h file)

I run unitree_legged_sdk-3.3.x/build/lcm_server_high and roslaunch the mbs high_level_mode.launch as sudo.

Then I can control the robot through the /cmd_vel topic in sport mode.

One issue I found was that the ./lcm_server_high process keeps quitting because of LCM timeout error. A workaround I found was to keep restarting the process, like so:

until ./lcm_server_high; do
    echo "LCM high level server crashed with exit code $?.  Respawning.." >&2
    # sleep 1 // if you want
done

This workaround is super janky, but for indoor navigation it seems to work alright.

Also it seems they added more “modes” and gait types (trot / trot running / stair climbing) in sdk v3.3.x, so I’ll see if I can control this from ROS:

// in unitree_legged_sdk-3.3.x/include/unitree_legged_sdk/comm.h
		uint8_t mode;                       // 0. idle, default stand  1. force stand (controlled by dBodyHeight + ypr)
											// 2. target velocity walking (controlled by velocity + yawSpeed)
											// 3. target position walking (controlled by position + ypr[0])
											// 4. path mode walking (reserve for future release)
											// 5. position stand down. 
											// 6. position stand up 
											// 7. damping mode 
											// 8. recovery stand
											// 9. backflip
											// 10. jumpYaw
											// 11. straightHand
											// 12. dance1
											// 13. dance2
											
		uint8_t gaitType;                  // 0.idle  1.trot  2.trot running  3.climb stair
		uint8_t speedLevel;                // 0. default low speed. 1. medium speed 2. high speed. during walking, only respond MODE 3

Hope this helps.