feat: add cyberdog interface

README.md

@@ -60,7 +60,7 @@ Open a new terminal, launch the gazebo simulation environment
 
 ```bash
 source devel/setup.bash
-roslaunch rl_sar start_env.launch
+roslaunch rl_sar start_a1.launch
 ```
 
 Open a new terminal, run the control program

README_CN.md

@@ -60,7 +60,7 @@ catkin build
 
 ```bash
 source devel/setup.bash
-roslaunch rl_sar start_env.launch
+roslaunch rl_sar start_a1.launch
 ```
 
 新建终端，启动控制程序

src/rl_sar/include/rl_sim.hpp

@@ -41,6 +41,8 @@ class RL_Sim : public RL
     std::vector<std::vector<double>> plot_real_joint_pos, plot_target_joint_pos;
     void Plot();
 private:
+    std::string ros_namespace;
+
     std::vector<std::string> torque_command_topics;
 
     ros::Subscriber model_state_subscriber_;

src/rl_sar/launch/start_cyberdog.launch

@@ -0,0 +1,59 @@
+<launch>
+    <arg name="wname" default="stairs"/>
+    <arg name="rname" default="cyberdog"/>
+    <param name="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
+    <arg name="robot_path" value="(find $(arg rname)_description)"/>
+    <arg name="dollar" value="$"/>
+
+    <arg name="paused" default="true"/>
+    <arg name="use_sim_time" default="true"/>
+    <arg name="gui" default="true"/>
+    <arg name="headless" default="false"/>
+    <arg name="debug" default="false"/>
+    <!-- Debug mode will hung up the robot, use "true" or "false" to switch it. -->
+    <arg name="user_debug" default="false"/>
+
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="world_name" value="$(find rl_sar)/worlds/$(arg wname).world"/>
+        <arg name="debug" value="$(arg debug)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="paused" value="$(arg paused)"/>
+        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+        <arg name="headless" value="$(arg headless)"/>
+    </include>
+
+    <!-- Load the URDF into the ROS Parameter Server -->
+    <param name="robot_description" textfile="$(find cyberdog_description)/urdf/cyberdog_description.urdf"/>
+
+    <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
+    <!-- Set trunk and joint positions at startup -->
+    <node pkg="gazebo_ros" type="spawn_model" name="urdf_spawner" respawn="false" output="screen"
+          args="-urdf -z 0.6 -model $(arg rname)_gazebo -param robot_description -unpause"/>
+
+    <!-- Load joint controller configurations from YAML file to parameter server -->
+    <rosparam file="$(arg dollar)$(arg robot_path)/config/robot_control.yaml" command="load"/>
+
+    <!-- <rosparam param="/a1_gazebo/joint_state_controller/publish_rate">5000</rosparam> -->
+
+    <!-- load the controllers -->
+    <node pkg="controller_manager" type="spawner" name="controller_spawner" respawn="false"
+          output="screen" ns="/$(arg rname)_gazebo" args="joint_state_controller
+          FL_hip_controller FL_thigh_controller FL_calf_controller
+          FR_hip_controller FR_thigh_controller FR_calf_controller
+          RL_hip_controller RL_thigh_controller RL_calf_controller
+          RR_hip_controller RR_thigh_controller RR_calf_controller "/>
+
+    <!-- convert joint states to TF transforms for rviz, etc -->
+    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"
+          respawn="false" output="screen">
+        <remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
+    </node>
+
+    <!-- <node pkg="unitree_gazebo" type="servo" name="servo" required="true" output="screen"/> -->
+
+    <!-- load the parameter unitree_controller -->
+    <include file="$(find unitree_controller)/launch/set_ctrl.launch">
+        <arg name="rname" value="$(arg rname)"/>
+    </include>
+
+</launch>

src/rl_sar/library/cyberdog_motor_sdk/CMakeLists.txt

@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.14)
+project(cyberdog_motor_sdk)
+
+add_compile_options(-std=c++14)
+set(CMAKE_CXX_FLAGS "-O3")
+
+FIND_PACKAGE(lcm REQUIRED)
+
+file(GLOB_RECURSE sources src/*.c src/*.cpp)
+FILE(GLOB_RECURSE headers include/*.h include/*.hpp)
+
+# generate cyber_dog_motor_sdk library
+add_library(cyberdog_motor_sdk SHARED ${sources} ${headers})
+target_include_directories(cyberdog_motor_sdk PUBLIC
+        "include"
+        )
+target_link_libraries(cyberdog_motor_sdk
+        pthread
+        lcm
+        )
+
+## compile use code
+#add_executable(Example_MotorCtrl src/Example_MotorCtrl.cpp)
+#target_link_libraries(Example_MotorCtrl cyberdog_motor_sdk)

src/rl_sar/library/cyberdog_motor_sdk/README.md

@@ -0,0 +1,140 @@
+CYBERDOG MOTOR SDK
+---
+此SDK开放了电机驱动器和机身IMU传感器接口，配合cyberdog 1.0.0.94及以上版本使用，方便用户进行运动控制的二次开发。具体接口使用可参照Example_MotorCtrl.cpp，按如下步骤在实际机器人上部署运行。
+
+### 准备工作
+#### 安装依赖
+安装lcm(本地部署时需要)
+```
+$ git clone https://github.com/lcm-proj/lcm.git
+$ cd lcm
+$ mkdir build && cd build
+$ cmake .. && make
+$ sudo make install
+```
+安装docker(运控部署时需要)
+
+按照链接所附步骤进行安装：https://docs.docker.com/engine/install/ubuntu/
+```
+$ sudo apt-get remove docker docker-engine docker.io containerd runc
+$ sudo apt-get update
+$ sudo apt-get install \
+    ca-certificates \
+    curl \
+    gnupg \
+    lsb-release
+$ sudo mkdir -p /etc/apt/keyrings
+$ curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo gpg --dearmor -o /etc/apt/keyrings/docker.gpg
+$ echo \
+  "deb [arch=$(dpkg --print-architecture) signed-by=/etc/apt/keyrings/docker.gpg] https://download.docker.com/linux/ubuntu \
+  $(lsb_release -cs) stable" | sudo tee /etc/apt/sources.list.d/docker.list > /dev/null
+$ sudo apt-get update
+$ sudo apt-get install docker-ce docker-ce-cli containerd.io docker-compose-plugin
+$ sudo docker run hello-world
+
+# 给docker设置root权限：
+$ sudo groupadd docker
+$ sudo usermod -aG docker $USER
+$ sudo gpasswd -a $USER docker     #将登陆用户加入到docker用户组中
+```
+下载交叉编译所需docker镜像
+```
+$ wget https://cdn.cnbj2m.fds.api.mi-img.com/os-temp/loco/loco_arm64_20220118.tar
+$ docker load --input loco_arm64_20220118.tar
+$ docker images
+```
+#### 连接机器人
+将本地PC连接至铁蛋的USB download type-c 接口(位于中间)，等待出现”L4T-README” 弹窗
+```
+$ ping 192.168.55.100     #本地PC被分配的ip
+$ ssh [email protected]     #登录nx应用板 ,密码123
+mi@lubuntu:~$ athena_version -v #核对当前版本>=1.0.0.94
+$ ssh [email protected] #登录运动控制板
+```
+#### 进入电机控制模式
+修改配置开关，激活用户控制模式，运行用户自己的控制器：
+```
+$ ssh [email protected] #登录运动控制板
+root@TinaLinux:~# cd /robot
+root@TinaLinux:~# ./initialize.sh #拷贝出厂代码到可读写的开发区（/mnt/UDISK/robot-software），切换到开发者模式，仅需执行一次
+root@TinaLinux:~# vi /mnt/UDISK/robot-software/config/user_code_ctrl_mode.txt #切换mode:1(0:默认模式，1用户代码控制电机模式),重启机器人生效
+```
+### 编译及部署
+
+#### 1、用户电脑侧部署
+运行在用户pc侧(linux)难以保证实时lcm通信，仅推荐编译验证和简单的位控测试  
+```
+$ ping 192.168.55.233 #通过type c线连接Cyberdog的Download接口后，确认通信正常
+$ ifconfig | grep -B 1 192.168.55.100 | grep "flags"| cut -d ':' -f1 #获取该ip对应网络设备，一般为usb0
+$ sudo ifconfig usb0 multicast #usb0替换为上文获取的168.55.100对应网络设备,并配为多播
+$ sudo route add -net 224.0.0.0 netmask 240.0.0.0 dev usb0 #添加路由表，usb0对应替换
+下载sdk
+$ cd cyberdog_motor_sdk/
+$ mkdir build && cd build
+$ cmake ..
+$ make -j4
+$ ./Example_MotorCtrl
+```
+注：lcm通信若不成功，无法正常激活电机控制模式，log提示：Motor control mode has not been activated successfully
+
+#### 2、铁蛋NX应用板部署
+因非实时系统，仅推荐编译验证和简单位控测试
+```
+$ scp -r {sdk_path}/cyberdog_motor_sdk [email protected]:/home/mi/ #sdk源码拷入应用板，密码123
+$ ssh [email protected] #登录应用板
+mi@lubuntu:~$ cd /home/mi/cyberdog_motor_sdk
+mi@lubuntu:~$ mkdir build && cd build
+mi@lubuntu:~$ cmake ..
+mi@lubuntu:~$ make -j2
+mi@lubuntu:~$ ping 192.168.55.233 #测试和运控板的通信
+mi@lubuntu:~$ ./Example_MotorCtrl
+```
+#### 3、铁蛋运控板交叉编译部署
+为了能使编译的文件可以直接在机器人上运行，需要在部署交叉编译工具链的docker镜像环境下编译，具体步骤如下：
+
+```
+$ docker run -it --rm --name cyberdog_motor_sdk -v /home/xxx/{sdk_path}:/work/build_farm/workspace/cyberdog cr.d.xiaomi.net/athena/athena_cheetah_arm64:2.0 /bin/bash
+
+docker run -it --rm --name cyberdog_motor_sdk -v /home/fzq614/ROS_Workspaces/cyberdog_motor_sdk:/work/build_farm/workspace/cyberdog cr.d.xiaomi.net/athena/athena_cheetah_arm64:2.0 /bin/bash
+
+[root:/work] # cd /work/build_farm/workspace/cyberdog/ #进入docker系统的代码仓
+[root:/work/build_farm/workspace/cyberdog] # mkdir onboard-build && cd onboard-build
+[root:/work/build_farm/workspace/cyberdog] # cmake -DCMAKE_TOOLCHAIN_FILE=/usr/xcc/aarch64-openwrt-linux-gnu/Toolchain.cmake ..
+[root:/work/build_farm/workspace/cyberdog] # make -j4 #指定交叉编译工具链并编译
+[root:/work/build_farm/workspace/cyberdog] # exit
+```
+编译成功后, 将生成的.so文件libcyber_dog_sdk.so和可执行文件Example_MotorCtrl拷贝到运控/mnt/UDISK目录下
+```
+$ cd ~/{sdk_path}/onboard-build
+
+cd ~/ROS_Workspaces/cyberdog_motor_sdk/onboard-build
+
+$ ssh [email protected] "mkdir /mnt/UDISK/cyberdog_motor_sdk" #在运控板内创建文件夹
+$ scp libcyber_dog_motor_sdk.so  Example_MotorCtrl [email protected]:/mnt/UDISK/cyberdog_motor_sdk
+$ ssh [email protected]
+root@TinaLinux:~# cd /mnt/UDISK/cyberdog_motor_sdk
+root@TinaLinux:~# export LD_LIBRARY_PATH=/mnt/UDISK/cyberdog_motor_sdk #设置so库路径变量
+root@TinaLinux:~# ./Example_MotorCtrl  #通过“nohup ./Example_MotorCtrl &”可后台运行，退出ssh连接不受影响
+```
+如何添加开机自启动:  
+配置/mnt/UDISK/manager_config/fork_para_conf_lists.json 进程管理文件(注意结尾逗号)后重启运控程序  
+例：  "600003": {"fork_config":{"name": "Example_MotorCtrl",  "object_path": "/cyberdog_motor_sdk/",  "log_path": "", "paraValues": ["", "", ""] }}  
+注：手动关闭程序时，请先关闭用户程序Example_MotorCtrl，触发主程序(ctrl)超时保护趴下，再关闭或重启主程序。同时关闭主程序和用户程序，电机会因CAN总线超时位置锁定，再次启动易发生危险。
+
+#### 错误标志位含义
+```
+//bit0: warning flag, lost communication between user code and robot over 10[ms]. For safety, commanded tau and qd_des will be forced to divide by (over_time[ms]/10.0);
+//bit1: error flag, lost communication between user code and robot over 500[ms]. Robot will enter high-damping mode by setting joint gains kp=0, kd=10, tau=0;
+//bit2: warning flag, position command of any abaduction joint changing more than 8 degrees from its previous will be truncated;
+//bit3: warning flag, position command of any hip joint changing more than 10 degrees from its previous will be truncated;
+//bit4: warning flag, position command of any knee joint changing more than 12 degrees from its previous will be truncated;
+```
+注：为了避免通信超时导致危险，报err_flag: 0x02 communicate lost over 500ms后先排除故障,关闭Example_MotorCtrl例程进程,再重启运控程序或者直接重启运控板才能清除错误.
+```
+# 重启运控程序:
+$ ssh [email protected] "ps | grep -E 'Example_MotorCtrl' | grep -v grep | awk '{print \$1}' | xargs kill -9" #需先于主进程暂停，避免急停
+$ ssh [email protected] "ps | grep -E 'manager|ctrl|imu_online' | grep -v grep | awk '{print \$1}' | xargs kill -9"
+$ ssh [email protected] "export LD_LIBRARY_PATH=/mnt/UDISK/robot-software/build;/mnt/UDISK/manager /mnt/UDISK/ >> /mnt/UDISK/manager_log/manager.log 2>&1 &"
+# 重启运控板系统:
+$ ssh [email protected] "reboot"
+```

src/rl_sar/library/cyberdog_motor_sdk/README_EN.md

@@ -0,0 +1,121 @@
+CYBERDOG MOTOR SDK
+---
+This SDK provides the interface of joint motors and IMU sensor mounted on CyberDog and gives users more freedom to develop their own controller. It is compatible with CyberDog firmware version 1.0.0.94 or higher. For more details, please refer to the example code Example_MotorCtrl.cpp. To deploy on real robots, please follow the following steps.
+
+### Preparatory work
+#### Dependency
+Install LCM
+```
+$ git clone https://github.com/lcm-proj/lcm.git
+$ cd lcm
+$ mkdir build && cd build
+$ cmake .. && make
+$ sudo make install
+```
+Install docker
+
+Follow the steps attached to the link: https://docs.docker.com/engine/install/ubuntu/
+```
+$ sudo groupadd docker # Set root permissions for docker
+$ sudo usermod -aG docker $USER
+```
+Download the docker image required for cross compilation
+```
+$ wget https://cdn.cnbj2m.fds.api.mi-img.com/os-temp/loco/loco_arm64_20220118.tar
+$ docker load --input loco_arm64_20220118.tar
+$ docker images
+```
+#### Connect robot
+Connect the local PC to the USB Download type-C interface of cyberdog (in the middle), and wait for the "L4T-README" pop-up window to appear
+```
+$ ping 192.168.55.100 # local PC assigned IP
+$ ssh [email protected] # Login NX application board, password 123
+mi@lubuntu:~$ athena_version -v # check current version >= 1.0.0.94
+$ ssh [email protected] # Log in to the motion control board
+```
+#### Enter motor control mode
+Modify the configuration switch to activate the user control mode:
+```
+$ ssh [email protected] # Log in to the motion control board
+root@TinaLinux:~# cd /robot
+root@TinaLinux:/robot# ./initialize.sh # copy the factory code to the r/w Development Zone (/mnt/UDISK/robot-software), switch to the developer mode and execute it only once
+root@TinaLinux:/robot# vi /mnt/UDISK/robot-software/config/user_code_ctrl_mode.txt # switch mode: 1 (0: default mode, 1 user code controls motor mode), and restart the robot to take effect
+```
+### Compilation and deployment
+#### 1. Deploy on user PC
+It is difficult to ensure real-time LCM communication when running on the user PC (Linux), so only compilation verification and simple position control tests are recommended.
+```
+$ ping 192.168.55.233 # connect cyberdog's download interface through type C cable and make sure the communication is okay
+$ ifconfig | grep -B 1 192.168.55.100 | grep "flags" | cut -d ':' -f1 # obtain the network device corresponding to the IP, generally usb0
+$ sudo ifconfig usb0 multicast # replace usb0 with the 168.55.100 network device obtained above and set to multicast
+$ sudo route add -net 224.0.0.0 netmask 240.0.0.0 dev usb0 # add a route table and replace usb0 accordingly.
+$ mkdir build && cd build
+$ cmake ..
+$ make -j4
+$ ./Example_MotorCtrl
+```
+Note: if the LCM communication is unsuccessful, the motor control mode cannot be activated normally. Log prompt: motor control mode has not been activated successfully.
+#### 2. Deploy on NX application board
+Due to non real-time system, only compilation verification and simple position control test are recommended.
+```
+$ scp -r {sdk_path}/cyberdog_motor_sdk [email protected]:/home/mi/ # copy the SDK source code into the application board, password 123
+$ ssh [email protected] # Login application board
+mi@lubuntu:~$ cd /home/mi/cyberdog_motor_sdk
+mi@lubuntu:~$ mkdir build && cd build
+mi@lubuntu:~$ cmake ..
+mi@lubuntu:~$ make -j2
+mi@lubuntu:~$ ping 192.168.55.233 # test communication with motion control board
+mi@lubuntu:~$ ./Example_MotorCtrl
+```
+#### 3. Cross compilation and deploy on motion control board
+In order to make the compiled file run directly on the robot, it needs to be compiled in the docker image environment where the cross compilation tool chain is deployed. The specific steps are as follows:
+```
+$ docker run -it --rm --name cyberdog_motor_sdk -v /home/xxx/{sdk_path}:/work/build_farm/workspace/cyberdog cr.d.xiaomi.net/athena/athena_cheetah_arm64:2.0 /bin/bash
+[root:/work] # cd /work/build_farm/workspace/cyberdog/ # enter the code warehouse of docker system
+[root:/work/build_farm/workspace/cyberdog] # mkdir onboard-build && cd onboard-build
+[root:/work/build_farm/workspace/cyberdog] # cmake -DCMAKE_TOOLCHAIN_FILE=/usr/xcc/aarch64-openwrt-linux-gnu/Toolchain.cmake ..
+[root:/work/build_farm/workspace/cyberdog] # make -j4 # specify cross compile tool chain and compile
+[root:/work/build_farm/workspace/cyberdog] # exit
+```
+After successful compilation, copy the generated library and executable files libcyber_dog_sdk.so and Example_MotorCtrl to the directory /mnt/UDISK of motion control board.
+```
+$ cd ~/{sdk_path}/onboard-build
+$ ssh [email protected] "mkdir /mnt/UDISK/cyberdog_motor_sdk" # create a folder in the motion control board
+$ scp libcyber_dog_motor_sdk.so  Example_MotorCtrl [email protected]:/mnt/UDISK/cyberdog_motor_sdk
+$ ssh [email protected]
+root@TinaLinux:~# cd /mnt/UDISK/cyberdog_motor_sdk
+root@TinaLinux:~# export LD_LIBRARY_PATH=/mnt/UDISK/cyberdog_motor_sdk # setting so library path variable
+root@TinaLinux:~# ./Example_MotorCtrl
+```
+To run in the background so that the process will not be interrupted by the exit of SSH connection, please use the command below instead:
+```
+nohup ./Example_MotorCtrl &
+```
+
+How to add boot auto start:
+
+Configure the JSON process management file /mnt/UDISK/manager_config/fork_para_conf_lists.json, and restart the robot.
+
+Example: "600003": {"fork_config":{"name": "Example_MotorCtrl",  "object_path": "/cyberdog_motor_sdk/",  "log_path": "", "paraValues": ["", "", ""] }}
+
+Note: when closing the program manually, please close the user program such as Example_MotorCtrl first which will trigger the timeout protection of main program and lie down the robot, and then close or restart the main program. If the main program and user program are closed at the same time, the motor will be locked due to the timeout of CAN bus and invite dangers during restart.
+#### Explanation of error flags
+```
+//bit0: warning flag, lost communication between user code and robot over 10[ms]. For safety, commanded tau and qd_des will be forced to divide by (over_time[ms]/10.0);
+//bit1: error flag, lost communication between user code and robot over 500[ms]. Robot will enter high-damping mode by setting joint gains kp=0, kd=10, tau=0;
+//bit2: warning flag, position command of any abaduction joint changing more than 8 degrees from its previous will be truncated;
+//bit3: warning flag, position command of any hip joint changing more than 10 degrees from its previous will be truncated;
+//bit4: warning flag, position command of any knee joint changing more than 12 degrees from its previous will be truncated;
+```
+Note: in order to avoid danger caused by communication timeout, when "err_flag: 0x02 communicate lost over 500ms" is reported, please identify the problem and close Example_MotorCtrl first. The error flag cannot be cleared until the main control program is restared or the control board is totally rebooted.
+
+How to restart the main control program:
+```
+$ ssh [email protected] "ps | grep -E 'Example_MotorCtrl' | grep -v grep | awk '{print \$1}' | xargs kill -9" # Example_MotorCtrl needs to stop earlier than the main control process to avoid emergency stop
+$ ssh [email protected] "ps | grep -E 'manager|ctrl|imu_online' | grep -v grep | awk '{print \$1}' | xargs kill -9"
+$ ssh [email protected] "export LD_LIBRARY_PATH=/mnt/UDISK/robot-software/build;/mnt/UDISK/manager /mnt/UDISK/ >> /mnt/UDISK/manager_log/manager.log 2>&1 &"
+```
+How to reboot the motion control board:
+```
+$ ssh [email protected] "reboot"
+```