├── .github └── ISSUE_TEMPLATE │ ├── bug_report.md │ ├── custom.md │ └── feature_request.md ├── CMakeLists.txt ├── README.md ├── README_EN.md ├── include ├── CustomInterface.h ├── leg_control_data_lcmt.hpp ├── motor_ctrl_lcmt.hpp ├── motor_ctrl_state_lcmt.hpp └── state_estimator_lcmt.hpp └── src ├── CustomInterface.cpp └── Example_MotorCtrl.cpp /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **To Reproduce** 14 | Steps to reproduce the behavior: 15 | 1. Go to '...' 16 | 2. Click on '....' 17 | 3. Scroll down to '....' 18 | 4. See error 19 | 20 | **Expected behavior** 21 | A clear and concise description of what you expected to happen. 22 | 23 | **Screenshots** 24 | If applicable, add screenshots to help explain your problem. 25 | 26 | **Desktop (please complete the following information):** 27 | - OS: [e.g. iOS] 28 | - Browser [e.g. chrome, safari] 29 | - Version [e.g. 22] 30 | 31 | **Smartphone (please complete the following information):** 32 | - Device: [e.g. iPhone6] 33 | - OS: [e.g. iOS8.1] 34 | - Browser [e.g. stock browser, safari] 35 | - Version [e.g. 22] 36 | 37 | **Additional context** 38 | Add any other context about the problem here. 39 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/custom.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Custom issue template 3 | about: Describe this issue template's purpose here. 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | 11 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for this project 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Is your feature request related to a problem? Please describe.** 11 | A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] 12 | 13 | **Describe the solution you'd like** 14 | A clear and concise description of what you want to happen. 15 | 16 | **Describe alternatives you've considered** 17 | A clear and concise description of any alternative solutions or features you've considered. 18 | 19 | **Additional context** 20 | Add any other context or screenshots about the feature request here. 21 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(cyber_dog_motor_sdk) 3 | 4 | add_compile_options(-std=c++14) 5 | set(CMAKE_CXX_FLAGS "-O3") 6 | 7 | FIND_PACKAGE(lcm REQUIRED) 8 | 9 | file(GLOB_RECURSE sources src/*.c src/*.cpp) 10 | 11 | # generate cyber_dog_motor_sdk library 12 | add_library(cyber_dog_motor_sdk SHARED ${sources}) 13 | target_include_directories(cyber_dog_motor_sdk PUBLIC 14 | "include" 15 | ) 16 | target_link_libraries(cyber_dog_motor_sdk 17 | pthread 18 | lcm 19 | ) 20 | 21 | # compile use code 22 | add_executable(Example_MotorCtrl src/Example_MotorCtrl.cpp) 23 | target_link_libraries(Example_MotorCtrl cyber_dog_motor_sdk) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | CYBERDOG MOTOR SDK 2 | --- 3 | 此SDK开放了电机驱动器和机身IMU传感器接口，配合cyberdog 1.0.0.94及以上版本使用，方便用户进行运动控制的二次开发。具体接口使用可参照Example_MotorCtrl.cpp，按如下步骤在实际机器人上部署运行。 4 | 5 | ### 准备工作 6 | #### 安装依赖 7 | 安装lcm(本地部署时需要) 8 | ``` 9 | $ git clone https://github.com/lcm-proj/lcm.git 10 | $ cd lcm 11 | $ mkdir build && cd build 12 | $ cmake .. && make 13 | $ sudo make install 14 | ``` 15 | 安装docker(运控部署时需要) 16 | 17 | 按照链接所附步骤进行安装：https://docs.docker.com/engine/install/ubuntu/ 18 | ``` 19 | # 给docker设置root权限： 20 | $ sudo groupadd docker 21 | $ sudo usermod -aG docker $USER 22 | ``` 23 | 下载交叉编译所需docker镜像 24 | ``` 25 | $ wget https://cdn.cnbj2m.fds.api.mi-img.com/os-temp/loco/loco_arm64_20220118.tar 26 | $ docker load --input loco_arm64_20220118.tar 27 | $ docker images 28 | ``` 29 | #### 连接机器人 30 | 将本地PC连接至铁蛋的USB download type-c 接口(位于中间)，等待出现”L4T-README” 弹窗 31 | ``` 32 | $ ping 192.168.55.100 #本地PC被分配的ip 33 | $ ssh mi@192.168.55.1 #登录nx应用板 ,密码123 34 | mi@lubuntu:~$ athena_version -v #核对当前版本>=1.0.0.94 35 | $ ssh root@192.168.55.233 #登录运动控制板 36 | ``` 37 | #### 进入电机控制模式 38 | 修改配置开关，激活用户控制模式，运行用户自己的控制器： 39 | ``` 40 | $ ssh root@192.168.55.233 #登录运动控制板 41 | root@TinaLinux:~# cd /robot 42 | root@TinaLinux:~# ./initialize.sh #拷贝出厂代码到可读写的开发区（/mnt/UDISK/robot-software），切换到开发者模式，仅需执行一次 43 | root@TinaLinux:~# vi /mnt/UDISK/robot-software/config/user_code_ctrl_mode.txt #切换mode:1(0:默认模式，1用户代码控制电机模式),重启机器人生效 44 | ``` 45 | ### 编译及部署 46 | 47 | #### 1、用户电脑侧部署 48 | 运行在用户pc侧(linux)难以保证实时lcm通信，仅推荐编译验证和简单的位控测试 49 | ``` 50 | $ ping 192.168.55.233 #通过type c线连接Cyberdog的Download接口后，确认通信正常 51 | $ ifconfig | grep -B 1 192.168.55.100 | grep "flags"| cut -d ':' -f1 #获取该ip对应网络设备，一般为usb0 52 | $ sudo ifconfig usb0 multicast #usb0替换为上文获取的168.55.100对应网络设备,并配为多播 53 | $ sudo route add -net 224.0.0.0 netmask 240.0.0.0 dev usb0 #添加路由表，usb0对应替换 54 | $ mkdir build && cd build #进入sdk代码仓后 55 | $ cmake .. 56 | $ make -j4 57 | $ ./Example_MotorCtrl 58 | ``` 59 | 注：lcm通信若不成功，无法正常激活电机控制模式，log提示：Motor control mode has not been activated successfully 60 | 61 | #### 2、铁蛋NX应用板部署 62 | 因非实时系统，仅推荐编译验证和简单位控测试 63 | ``` 64 | $ scp -r {sdk_path}/cyberdog_motor_sdk mi@192.168.55.1:/home/mi/ #sdk源码拷入应用板，密码123 65 | $ ssh mi@192.168.55.1 #登录应用板 66 | mi@lubuntu:~$ cd /home/mi/cyberdog_motor_sdk 67 | mi@lubuntu:~$ mkdir build && cd build 68 | mi@lubuntu:~$ cmake .. 69 | mi@lubuntu:~$ make -j2 70 | mi@lubuntu:~$ ping 192.168.55.233 #测试和运控板的通信 71 | mi@lubuntu:~$ ./Example_MotorCtrl 72 | ``` 73 | #### 3、铁蛋运控板交叉编译部署 74 | 为了能使编译的文件可以直接在机器人上运行，需要在部署交叉编译工具链的docker镜像环境下编译，具体步骤如下： 75 | 76 | ``` 77 | $ 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 78 | [root:/work] # cd /work/build_farm/workspace/cyberdog/ #进入docker系统的代码仓 79 | [root:/work/build_farm/workspace/cyberdog] # mkdir onboard-build && cd onboard-build 80 | [root:/work/build_farm/workspace/cyberdog] # cmake -DCMAKE_TOOLCHAIN_FILE=/usr/xcc/aarch64-openwrt-linux-gnu/Toolchain.cmake .. 81 | [root:/work/build_farm/workspace/cyberdog] # make -j4 #指定交叉编译工具链并编译 82 | [root:/work/build_farm/workspace/cyberdog] # exit 83 | ``` 84 | 编译成功后, 将生成的.so文件libcyber_dog_sdk.so和可执行文件Example_MotorCtrl拷贝到运控/mnt/UDISK目录下 85 | ``` 86 | $ cd ~/{sdk_path}/onboard-build 87 | $ ssh root@192.168.55.233 "mkdir /mnt/UDISK/cyberdog_motor_sdk" #在运控板内创建文件夹 88 | $ scp libcyber_dog_motor_sdk.so Example_MotorCtrl root@192.168.55.233:/mnt/UDISK/cyberdog_motor_sdk 89 | $ ssh root@192.168.55.233 90 | root@TinaLinux:~# cd /mnt/UDISK/cyberdog_motor_sdk 91 | root@TinaLinux:~# export LD_LIBRARY_PATH=/mnt/UDISK/cyberdog_motor_sdk #设置so库路径变量 92 | root@TinaLinux:~# ./Example_MotorCtrl #通过“nohup ./Example_MotorCtrl &”可后台运行，退出ssh连接不受影响 93 | ``` 94 | 如何添加开机自启动: 95 | 配置/mnt/UDISK/manager_config/fork_para_conf_lists.json 进程管理文件(注意结尾逗号)后重启运控程序 96 | 例： "600003": {"fork_config":{"name": "Example_MotorCtrl", "object_path": "/cyberdog_motor_sdk/", "log_path": "", "paraValues": ["", "", ""] }} 97 | 注：手动关闭程序时，请先关闭用户程序Example_MotorCtrl，触发主程序(ctrl)超时保护趴下，再关闭或重启主程序。同时关闭主程序和用户程序，电机会因CAN总线超时位置锁定，再次启动易发生危险。 98 | 99 | #### 错误标志位含义 100 | ``` 101 | //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); 102 | //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; 103 | //bit2: warning flag, position command of any abaduction joint changing more than 8 degrees from its previous will be truncated; 104 | //bit3: warning flag, position command of any hip joint changing more than 10 degrees from its previous will be truncated; 105 | //bit4: warning flag, position command of any knee joint changing more than 12 degrees from its previous will be truncated; 106 | ``` 107 | 注：为了避免通信超时导致危险，报err_flag: 0x02 communicate lost over 500ms后先排除故障,关闭Example_MotorCtrl例程进程,再重启运控程序或者直接重启运控板才能清除错误. 108 | ``` 109 | # 重启运控程序: 110 | $ ssh root@192.168.55.233 "ps | grep -E 'Example_MotorCtrl' | grep -v grep | awk '{print \$1}' | xargs kill -9" #需先于主进程暂停，避免急停 111 | $ ssh root@192.168.55.233 "ps | grep -E 'manager|ctrl|imu_online' | grep -v grep | awk '{print \$1}' | xargs kill -9" 112 | $ ssh root@192.168.55.233 "export LD_LIBRARY_PATH=/mnt/UDISK/robot-software/build;/mnt/UDISK/manager /mnt/UDISK/ >> /mnt/UDISK/manager_log/manager.log 2>&1 &" 113 | # 重启运控板系统: 114 | $ ssh root@192.168.55.233 "reboot" 115 | ``` 116 | -------------------------------------------------------------------------------- /README_EN.md: -------------------------------------------------------------------------------- 1 | CYBERDOG MOTOR SDK 2 | --- 3 | 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. 4 | 5 | ### Preparatory work 6 | #### Dependency 7 | Install LCM 8 | ``` 9 | $ git clone https://github.com/lcm-proj/lcm.git 10 | $ cd lcm 11 | $ mkdir build && cd build 12 | $ cmake .. && make 13 | $ sudo make install 14 | ``` 15 | Install docker 16 | 17 | Follow the steps attached to the link: https://docs.docker.com/engine/install/ubuntu/ 18 | ``` 19 | $ sudo groupadd docker # Set root permissions for docker 20 | $ sudo usermod -aG docker $USER 21 | ``` 22 | Download the docker image required for cross compilation 23 | ``` 24 | $ wget https://cdn.cnbj2m.fds.api.mi-img.com/os-temp/loco/loco_arm64_20220118.tar 25 | $ docker load --input loco_arm64_20220118.tar 26 | $ docker images 27 | ``` 28 | #### Connect robot 29 | 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 30 | ``` 31 | $ ping 192.168.55.100 # local PC assigned IP 32 | $ ssh mi@192.168.55.1 # Login NX application board, password 123 33 | mi@lubuntu:~$ athena_version -v # check current version >= 1.0.0.94 34 | $ ssh root@192.168.55.233 # Log in to the motion control board 35 | ``` 36 | #### Enter motor control mode 37 | Modify the configuration switch to activate the user control mode: 38 | ``` 39 | $ ssh root@192.168.55.233 # Log in to the motion control board 40 | root@TinaLinux:~# cd /robot 41 | 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 42 | 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 43 | ``` 44 | ### Compilation and deployment 45 | #### 1. Deploy on user PC 46 | 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. 47 | ``` 48 | $ ping 192.168.55.233 # connect cyberdog's download interface through type C cable and make sure the communication is okay 49 | $ ifconfig | grep -B 1 192.168.55.100 | grep "flags" | cut -d ':' -f1 # obtain the network device corresponding to the IP, generally usb0 50 | $ sudo ifconfig usb0 multicast # replace usb0 with the 168.55.100 network device obtained above and set to multicast 51 | $ sudo route add -net 224.0.0.0 netmask 240.0.0.0 dev usb0 # add a route table and replace usb0 accordingly. 52 | $ mkdir build && cd build 53 | $ cmake .. 54 | $ make -j4 55 | $ ./Example_MotorCtrl 56 | ``` 57 | 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. 58 | #### 2. Deploy on NX application board 59 | Due to non real-time system, only compilation verification and simple position control test are recommended. 60 | ``` 61 | $ scp -r {sdk_path}/cyberdog_motor_sdk mi@192.168.55.1:/home/mi/ # copy the SDK source code into the application board, password 123 62 | $ ssh mi@192.168.55.1 # Login application board 63 | mi@lubuntu:~$ cd /home/mi/cyberdog_motor_sdk 64 | mi@lubuntu:~$ mkdir build && cd build 65 | mi@lubuntu:~$ cmake .. 66 | mi@lubuntu:~$ make -j2 67 | mi@lubuntu:~$ ping 192.168.55.233 # test communication with motion control board 68 | mi@lubuntu:~$ ./Example_MotorCtrl 69 | ``` 70 | #### 3. Cross compilation and deploy on motion control board 71 | 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: 72 | ``` 73 | $ 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 74 | [root:/work] # cd /work/build_farm/workspace/cyberdog/ # enter the code warehouse of docker system 75 | [root:/work/build_farm/workspace/cyberdog] # mkdir onboard-build && cd onboard-build 76 | [root:/work/build_farm/workspace/cyberdog] # cmake -DCMAKE_TOOLCHAIN_FILE=/usr/xcc/aarch64-openwrt-linux-gnu/Toolchain.cmake .. 77 | [root:/work/build_farm/workspace/cyberdog] # make -j4 # specify cross compile tool chain and compile 78 | [root:/work/build_farm/workspace/cyberdog] # exit 79 | ``` 80 | 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. 81 | ``` 82 | $ cd ~/{sdk_path}/onboard-build 83 | $ ssh root@192.168.55.233 "mkdir /mnt/UDISK/cyberdog_motor_sdk" # create a folder in the motion control board 84 | $ scp libcyber_dog_motor_sdk.so Example_MotorCtrl root@192.168.55.233:/mnt/UDISK/cyberdog_motor_sdk 85 | $ ssh root@192.168.55.233 86 | root@TinaLinux:~# cd /mnt/UDISK/cyberdog_motor_sdk 87 | root@TinaLinux:~# export LD_LIBRARY_PATH=/mnt/UDISK/cyberdog_motor_sdk # setting so library path variable 88 | root@TinaLinux:~# ./Example_MotorCtrl 89 | ``` 90 | 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: 91 | ``` 92 | nohup ./Example_MotorCtrl & 93 | ``` 94 | 95 | How to add boot auto start: 96 | 97 | Configure the JSON process management file /mnt/UDISK/manager_config/fork_para_conf_lists.json, and restart the robot. 98 | 99 | Example: "600003": {"fork_config":{"name": "Example_MotorCtrl", "object_path": "/cyberdog_motor_sdk/", "log_path": "", "paraValues": ["", "", ""] }} 100 | 101 | 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. 102 | #### Explanation of error flags 103 | ``` 104 | //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); 105 | //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; 106 | //bit2: warning flag, position command of any abaduction joint changing more than 8 degrees from its previous will be truncated; 107 | //bit3: warning flag, position command of any hip joint changing more than 10 degrees from its previous will be truncated; 108 | //bit4: warning flag, position command of any knee joint changing more than 12 degrees from its previous will be truncated; 109 | ``` 110 | 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. 111 | 112 | How to restart the main control program: 113 | ``` 114 | $ ssh root@192.168.55.233 "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 115 | $ ssh root@192.168.55.233 "ps | grep -E 'manager|ctrl|imu_online' | grep -v grep | awk '{print \$1}' | xargs kill -9" 116 | $ ssh root@192.168.55.233 "export LD_LIBRARY_PATH=/mnt/UDISK/robot-software/build;/mnt/UDISK/manager /mnt/UDISK/ >> /mnt/UDISK/manager_log/manager.log 2>&1 &" 117 | ``` 118 | How to reboot the motion control board: 119 | ``` 120 | $ ssh root@192.168.55.233 "reboot" 121 | ``` 122 | -------------------------------------------------------------------------------- /include/CustomInterface.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef PROJECT_CUSTOMINTERFACE_H 16 | #define PROJECT_CUSTOMINTERFACE_H 17 | 18 | #include "leg_control_data_lcmt.hpp" 19 | #include "motor_ctrl_lcmt.hpp" 20 | #include "motor_ctrl_state_lcmt.hpp" 21 | #include "state_estimator_lcmt.hpp" 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | struct Robot_Data { 35 | float q[ 12 ]; // 12个关节电机角度，弧度制 36 | float qd[ 12 ]; //电机角速度，弧度制 37 | float tau[ 12 ]; //电机扭矩 N.M 38 | float quat[ 4 ]; //机身姿态四元数，右手坐标系 39 | float rpy[ 3 ]; //机身姿态横滚、俯仰、偏航角 弧度制 40 | float acc[ 3 ]; //加速度计值 41 | float omega[ 3 ]; //角速度计值 42 | float ctrl_topic_interval; //控制topic通信延迟 43 | int16_t err_flag; 44 | }; 45 | struct Motor_Cmd { 46 | //期望扭矩tau = tau_des + (q_des - q)*kp_des + (qd_des - qd)*kd_des 47 | float q_des[ 12 ]; // 12个关机电机期望角度，弧度制 0/3/6/9:-0.75~0.75 1/4:4.363 ~ -1.257 7/10:3.49 ~ -2.01 2/5/8/11:-0.506 ~ -2.478 48 | float qd_des[ 12 ]; //电机期望角速度，弧度制 +-12弧度/秒@24NM 49 | float kp_des[ 12 ]; //电机位置控制比例系数 0~200 50 | float kd_des[ 12 ]; //电机速度控制比例系数 0~10 51 | float tau_des[ 12 ]; //电机期望前馈扭矩 0/3/6/9:+-17N/M 1/2/4/5/7/8/10/11:+-24NM 52 | }; 53 | 54 | class CustomInterface { 55 | public: 56 | CustomInterface( const double& loop_rate ); 57 | void Spin(); 58 | void Stop(); 59 | 60 | protected: 61 | virtual void UserCode() = 0; 62 | Robot_Data robot_data; 63 | Motor_Cmd motor_cmd; 64 | 65 | private: 66 | double dt_; 67 | bool running_; 68 | bool all_thread_done_; 69 | bool mode_state; 70 | 71 | lcm::LCM _motor_ctrl_Lcm; 72 | lcm::LCM _motor_data_Lcm; 73 | lcm::LCM _robot_state_Lcm; 74 | lcm::LCM _motor_ctrl_state_Lcm; 75 | 76 | std::thread _motor_ctrl_state_LcmThread; 77 | std::thread _motor_data_LcmThread; 78 | std::thread _robot_state_LcmThread; 79 | std::thread _user_code_ControlThread; 80 | 81 | motor_ctrl_lcmt _motor_ctrl; 82 | 83 | std::string getLcmUrl_port( int64_t port, int64_t ttl ); 84 | 85 | void motor_data_LcmThread() { 86 | while ( running_ ) { 87 | _motor_data_Lcm.handleTimeout( 1000 ); 88 | } 89 | } 90 | void robot_state_LcmThread() { 91 | while ( running_ ) { 92 | _robot_state_Lcm.handleTimeout( 1000 ); 93 | } 94 | } 95 | void motor_ctrl_state_LcmThread() { 96 | while ( running_ ) { 97 | _motor_ctrl_state_Lcm.handleTimeout( 1000 ); 98 | } 99 | } 100 | 101 | void handle_motor_ctrl_state_LCM( const lcm::ReceiveBuffer* rbuf, const std::string& chan, const motor_ctrl_state_lcmt* msg ); 102 | void handle_motor_data_LCM( const lcm::ReceiveBuffer* rbuf, const std::string& chan, const leg_control_data_lcmt* msg ); 103 | void handle_robot_state_LCM( const lcm::ReceiveBuffer* rbuf, const std::string& chan, const state_estimator_lcmt* msg ); 104 | 105 | void motor_cmd_send(); 106 | void Control(); 107 | }; // CustomInterface 108 | #endif -------------------------------------------------------------------------------- /include/leg_control_data_lcmt.hpp: -------------------------------------------------------------------------------- 1 | /** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY 2 | * BY HAND!! 3 | * 4 | * Generated by lcm-gen 5 | **/ 6 | 7 | #ifndef __leg_control_data_lcmt_hpp__ 8 | #define __leg_control_data_lcmt_hpp__ 9 | 10 | #include 11 | 12 | class leg_control_data_lcmt { 13 | public: 14 | float q[ 12 ]; 15 | 16 | float qd[ 12 ]; 17 | 18 | float p[ 12 ]; 19 | 20 | float v[ 12 ]; 21 | 22 | float tau_est[ 12 ]; 23 | 24 | float force_est[ 12 ]; 25 | 26 | float force_desired[ 12 ]; 27 | 28 | int32_t q_abad_limit[ 4 ]; 29 | 30 | int32_t q_hip_limit[ 4 ]; 31 | 32 | int32_t q_knee_limit[ 4 ]; 33 | 34 | public: 35 | /** 36 | * Encode a message into binary form. 37 | * 38 | * @param buf The output buffer. 39 | * @param offset Encoding starts at thie byte offset into @p buf. 40 | * @param maxlen Maximum number of bytes to write. This should generally be 41 | * equal to getEncodedSize(). 42 | * @return The number of bytes encoded, or <0 on error. 43 | */ 44 | inline int encode( void* buf, int offset, int maxlen ) const; 45 | 46 | /** 47 | * Check how many bytes are required to encode this message. 48 | */ 49 | inline int getEncodedSize() const; 50 | 51 | /** 52 | * Decode a message from binary form into this instance. 53 | * 54 | * @param buf The buffer containing the encoded message. 55 | * @param offset The byte offset into @p buf where the encoded message starts. 56 | * @param maxlen The maximum number of bytes to read while decoding. 57 | * @return The number of bytes decoded, or <0 if an error occured. 58 | */ 59 | inline int decode( const void* buf, int offset, int maxlen ); 60 | 61 | /** 62 | * Retrieve the 64-bit fingerprint identifying the structure of the message. 63 | * Note that the fingerprint is the same for all instances of the same 64 | * message type, and is a fingerprint on the message type definition, not on 65 | * the message contents. 66 | */ 67 | inline static int64_t getHash(); 68 | 69 | /** 70 | * Returns "leg_control_data_lcmt" 71 | */ 72 | inline static const char* getTypeName(); 73 | 74 | // LCM support functions. Users should not call these 75 | inline int _encodeNoHash( void* buf, int offset, int maxlen ) const; 76 | inline int _getEncodedSizeNoHash() const; 77 | inline int _decodeNoHash( const void* buf, int offset, int maxlen ); 78 | inline static uint64_t _computeHash( const __lcm_hash_ptr* p ); 79 | }; 80 | 81 | int leg_control_data_lcmt::encode( void* buf, int offset, int maxlen ) const { 82 | int pos = 0, tlen; 83 | int64_t hash = getHash(); 84 | 85 | tlen = __int64_t_encode_array( buf, offset + pos, maxlen - pos, &hash, 1 ); 86 | if ( tlen < 0 ) 87 | return tlen; 88 | else 89 | pos += tlen; 90 | 91 | tlen = this->_encodeNoHash( buf, offset + pos, maxlen - pos ); 92 | if ( tlen < 0 ) 93 | return tlen; 94 | else 95 | pos += tlen; 96 | 97 | return pos; 98 | } 99 | 100 | int leg_control_data_lcmt::decode( const void* buf, int offset, int maxlen ) { 101 | int pos = 0, thislen; 102 | 103 | int64_t msg_hash; 104 | thislen = __int64_t_decode_array( buf, offset + pos, maxlen - pos, &msg_hash, 1 ); 105 | if ( thislen < 0 ) 106 | return thislen; 107 | else 108 | pos += thislen; 109 | if ( msg_hash != getHash() ) 110 | return -1; 111 | 112 | thislen = this->_decodeNoHash( buf, offset + pos, maxlen - pos ); 113 | if ( thislen < 0 ) 114 | return thislen; 115 | else 116 | pos += thislen; 117 | 118 | return pos; 119 | } 120 | 121 | int leg_control_data_lcmt::getEncodedSize() const { 122 | return 8 + _getEncodedSizeNoHash(); 123 | } 124 | 125 | int64_t leg_control_data_lcmt::getHash() { 126 | static int64_t hash = static_cast< int64_t >( _computeHash( NULL ) ); 127 | return hash; 128 | } 129 | 130 | const char* leg_control_data_lcmt::getTypeName() { 131 | return "leg_control_data_lcmt"; 132 | } 133 | 134 | int leg_control_data_lcmt::_encodeNoHash( void* buf, int offset, int maxlen ) const { 135 | int pos = 0, tlen; 136 | 137 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->q[ 0 ], 12 ); 138 | if ( tlen < 0 ) 139 | return tlen; 140 | else 141 | pos += tlen; 142 | 143 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->qd[ 0 ], 12 ); 144 | if ( tlen < 0 ) 145 | return tlen; 146 | else 147 | pos += tlen; 148 | 149 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->p[ 0 ], 12 ); 150 | if ( tlen < 0 ) 151 | return tlen; 152 | else 153 | pos += tlen; 154 | 155 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->v[ 0 ], 12 ); 156 | if ( tlen < 0 ) 157 | return tlen; 158 | else 159 | pos += tlen; 160 | 161 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->tau_est[ 0 ], 12 ); 162 | if ( tlen < 0 ) 163 | return tlen; 164 | else 165 | pos += tlen; 166 | 167 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->force_est[ 0 ], 12 ); 168 | if ( tlen < 0 ) 169 | return tlen; 170 | else 171 | pos += tlen; 172 | 173 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->force_desired[ 0 ], 12 ); 174 | if ( tlen < 0 ) 175 | return tlen; 176 | else 177 | pos += tlen; 178 | 179 | tlen = __int32_t_encode_array( buf, offset + pos, maxlen - pos, &this->q_abad_limit[ 0 ], 4 ); 180 | if ( tlen < 0 ) 181 | return tlen; 182 | else 183 | pos += tlen; 184 | 185 | tlen = __int32_t_encode_array( buf, offset + pos, maxlen - pos, &this->q_hip_limit[ 0 ], 4 ); 186 | if ( tlen < 0 ) 187 | return tlen; 188 | else 189 | pos += tlen; 190 | 191 | tlen = __int32_t_encode_array( buf, offset + pos, maxlen - pos, &this->q_knee_limit[ 0 ], 4 ); 192 | if ( tlen < 0 ) 193 | return tlen; 194 | else 195 | pos += tlen; 196 | 197 | return pos; 198 | } 199 | 200 | int leg_control_data_lcmt::_decodeNoHash( const void* buf, int offset, int maxlen ) { 201 | int pos = 0, tlen; 202 | 203 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->q[ 0 ], 12 ); 204 | if ( tlen < 0 ) 205 | return tlen; 206 | else 207 | pos += tlen; 208 | 209 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->qd[ 0 ], 12 ); 210 | if ( tlen < 0 ) 211 | return tlen; 212 | else 213 | pos += tlen; 214 | 215 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->p[ 0 ], 12 ); 216 | if ( tlen < 0 ) 217 | return tlen; 218 | else 219 | pos += tlen; 220 | 221 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->v[ 0 ], 12 ); 222 | if ( tlen < 0 ) 223 | return tlen; 224 | else 225 | pos += tlen; 226 | 227 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->tau_est[ 0 ], 12 ); 228 | if ( tlen < 0 ) 229 | return tlen; 230 | else 231 | pos += tlen; 232 | 233 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->force_est[ 0 ], 12 ); 234 | if ( tlen < 0 ) 235 | return tlen; 236 | else 237 | pos += tlen; 238 | 239 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->force_desired[ 0 ], 12 ); 240 | if ( tlen < 0 ) 241 | return tlen; 242 | else 243 | pos += tlen; 244 | 245 | tlen = __int32_t_decode_array( buf, offset + pos, maxlen - pos, &this->q_abad_limit[ 0 ], 4 ); 246 | if ( tlen < 0 ) 247 | return tlen; 248 | else 249 | pos += tlen; 250 | 251 | tlen = __int32_t_decode_array( buf, offset + pos, maxlen - pos, &this->q_hip_limit[ 0 ], 4 ); 252 | if ( tlen < 0 ) 253 | return tlen; 254 | else 255 | pos += tlen; 256 | 257 | tlen = __int32_t_decode_array( buf, offset + pos, maxlen - pos, &this->q_knee_limit[ 0 ], 4 ); 258 | if ( tlen < 0 ) 259 | return tlen; 260 | else 261 | pos += tlen; 262 | 263 | return pos; 264 | } 265 | 266 | int leg_control_data_lcmt::_getEncodedSizeNoHash() const { 267 | int enc_size = 0; 268 | enc_size += __float_encoded_array_size( NULL, 12 ); 269 | enc_size += __float_encoded_array_size( NULL, 12 ); 270 | enc_size += __float_encoded_array_size( NULL, 12 ); 271 | enc_size += __float_encoded_array_size( NULL, 12 ); 272 | enc_size += __float_encoded_array_size( NULL, 12 ); 273 | enc_size += __float_encoded_array_size( NULL, 12 ); 274 | enc_size += __float_encoded_array_size( NULL, 12 ); 275 | enc_size += __int32_t_encoded_array_size( NULL, 4 ); 276 | enc_size += __int32_t_encoded_array_size( NULL, 4 ); 277 | enc_size += __int32_t_encoded_array_size( NULL, 4 ); 278 | return enc_size; 279 | } 280 | 281 | uint64_t leg_control_data_lcmt::_computeHash( const __lcm_hash_ptr* ) { 282 | uint64_t hash = 0xa6b1824464a42a6bLL; 283 | return ( hash << 1 ) + ( ( hash >> 63 ) & 1 ); 284 | } 285 | 286 | #endif 287 | -------------------------------------------------------------------------------- /include/motor_ctrl_lcmt.hpp: -------------------------------------------------------------------------------- 1 | /** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY 2 | * BY HAND!! 3 | * 4 | * Generated by lcm-gen 5 | **/ 6 | 7 | #ifndef __motor_ctrl_lcmt_hpp__ 8 | #define __motor_ctrl_lcmt_hpp__ 9 | 10 | #include 11 | 12 | class motor_ctrl_lcmt { 13 | public: 14 | float q_des[ 12 ]; 15 | 16 | float qd_des[ 12 ]; 17 | 18 | float kp_des[ 12 ]; 19 | 20 | float kd_des[ 12 ]; 21 | 22 | float tau_des[ 12 ]; 23 | 24 | public: 25 | /** 26 | * Encode a message into binary form. 27 | * 28 | * @param buf The output buffer. 29 | * @param offset Encoding starts at thie byte offset into @p buf. 30 | * @param maxlen Maximum number of bytes to write. This should generally be 31 | * equal to getEncodedSize(). 32 | * @return The number of bytes encoded, or <0 on error. 33 | */ 34 | inline int encode( void* buf, int offset, int maxlen ) const; 35 | 36 | /** 37 | * Check how many bytes are required to encode this message. 38 | */ 39 | inline int getEncodedSize() const; 40 | 41 | /** 42 | * Decode a message from binary form into this instance. 43 | * 44 | * @param buf The buffer containing the encoded message. 45 | * @param offset The byte offset into @p buf where the encoded message starts. 46 | * @param maxlen The maximum number of bytes to read while decoding. 47 | * @return The number of bytes decoded, or <0 if an error occured. 48 | */ 49 | inline int decode( const void* buf, int offset, int maxlen ); 50 | 51 | /** 52 | * Retrieve the 64-bit fingerprint identifying the structure of the message. 53 | * Note that the fingerprint is the same for all instances of the same 54 | * message type, and is a fingerprint on the message type definition, not on 55 | * the message contents. 56 | */ 57 | inline static int64_t getHash(); 58 | 59 | /** 60 | * Returns "motor_ctrl_lcmt" 61 | */ 62 | inline static const char* getTypeName(); 63 | 64 | // LCM support functions. Users should not call these 65 | inline int _encodeNoHash( void* buf, int offset, int maxlen ) const; 66 | inline int _getEncodedSizeNoHash() const; 67 | inline int _decodeNoHash( const void* buf, int offset, int maxlen ); 68 | inline static uint64_t _computeHash( const __lcm_hash_ptr* p ); 69 | }; 70 | 71 | int motor_ctrl_lcmt::encode( void* buf, int offset, int maxlen ) const { 72 | int pos = 0, tlen; 73 | int64_t hash = getHash(); 74 | 75 | tlen = __int64_t_encode_array( buf, offset + pos, maxlen - pos, &hash, 1 ); 76 | if ( tlen < 0 ) 77 | return tlen; 78 | else 79 | pos += tlen; 80 | 81 | tlen = this->_encodeNoHash( buf, offset + pos, maxlen - pos ); 82 | if ( tlen < 0 ) 83 | return tlen; 84 | else 85 | pos += tlen; 86 | 87 | return pos; 88 | } 89 | 90 | int motor_ctrl_lcmt::decode( const void* buf, int offset, int maxlen ) { 91 | int pos = 0, thislen; 92 | 93 | int64_t msg_hash; 94 | thislen = __int64_t_decode_array( buf, offset + pos, maxlen - pos, &msg_hash, 1 ); 95 | if ( thislen < 0 ) 96 | return thislen; 97 | else 98 | pos += thislen; 99 | if ( msg_hash != getHash() ) 100 | return -1; 101 | 102 | thislen = this->_decodeNoHash( buf, offset + pos, maxlen - pos ); 103 | if ( thislen < 0 ) 104 | return thislen; 105 | else 106 | pos += thislen; 107 | 108 | return pos; 109 | } 110 | 111 | int motor_ctrl_lcmt::getEncodedSize() const { 112 | return 8 + _getEncodedSizeNoHash(); 113 | } 114 | 115 | int64_t motor_ctrl_lcmt::getHash() { 116 | static int64_t hash = static_cast< int64_t >( _computeHash( NULL ) ); 117 | return hash; 118 | } 119 | 120 | const char* motor_ctrl_lcmt::getTypeName() { 121 | return "motor_ctrl_lcmt"; 122 | } 123 | 124 | int motor_ctrl_lcmt::_encodeNoHash( void* buf, int offset, int maxlen ) const { 125 | int pos = 0, tlen; 126 | 127 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->q_des[ 0 ], 12 ); 128 | if ( tlen < 0 ) 129 | return tlen; 130 | else 131 | pos += tlen; 132 | 133 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->qd_des[ 0 ], 12 ); 134 | if ( tlen < 0 ) 135 | return tlen; 136 | else 137 | pos += tlen; 138 | 139 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->kp_des[ 0 ], 12 ); 140 | if ( tlen < 0 ) 141 | return tlen; 142 | else 143 | pos += tlen; 144 | 145 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->kd_des[ 0 ], 12 ); 146 | if ( tlen < 0 ) 147 | return tlen; 148 | else 149 | pos += tlen; 150 | 151 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->tau_des[ 0 ], 12 ); 152 | if ( tlen < 0 ) 153 | return tlen; 154 | else 155 | pos += tlen; 156 | 157 | return pos; 158 | } 159 | 160 | int motor_ctrl_lcmt::_decodeNoHash( const void* buf, int offset, int maxlen ) { 161 | int pos = 0, tlen; 162 | 163 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->q_des[ 0 ], 12 ); 164 | if ( tlen < 0 ) 165 | return tlen; 166 | else 167 | pos += tlen; 168 | 169 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->qd_des[ 0 ], 12 ); 170 | if ( tlen < 0 ) 171 | return tlen; 172 | else 173 | pos += tlen; 174 | 175 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->kp_des[ 0 ], 12 ); 176 | if ( tlen < 0 ) 177 | return tlen; 178 | else 179 | pos += tlen; 180 | 181 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->kd_des[ 0 ], 12 ); 182 | if ( tlen < 0 ) 183 | return tlen; 184 | else 185 | pos += tlen; 186 | 187 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->tau_des[ 0 ], 12 ); 188 | if ( tlen < 0 ) 189 | return tlen; 190 | else 191 | pos += tlen; 192 | 193 | return pos; 194 | } 195 | 196 | int motor_ctrl_lcmt::_getEncodedSizeNoHash() const { 197 | int enc_size = 0; 198 | enc_size += __float_encoded_array_size( NULL, 12 ); 199 | enc_size += __float_encoded_array_size( NULL, 12 ); 200 | enc_size += __float_encoded_array_size( NULL, 12 ); 201 | enc_size += __float_encoded_array_size( NULL, 12 ); 202 | enc_size += __float_encoded_array_size( NULL, 12 ); 203 | return enc_size; 204 | } 205 | 206 | uint64_t motor_ctrl_lcmt::_computeHash( const __lcm_hash_ptr* ) { 207 | uint64_t hash = 0x3ddbe622bc22d656LL; 208 | return ( hash << 1 ) + ( ( hash >> 63 ) & 1 ); 209 | } 210 | 211 | #endif 212 | -------------------------------------------------------------------------------- /include/motor_ctrl_state_lcmt.hpp: -------------------------------------------------------------------------------- 1 | /** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY 2 | * BY HAND!! 3 | * 4 | * Generated by lcm-gen 5 | **/ 6 | 7 | #ifndef __motor_ctrl_state_lcmt_hpp__ 8 | #define __motor_ctrl_state_lcmt_hpp__ 9 | 10 | #include 11 | 12 | class motor_ctrl_state_lcmt { 13 | public: 14 | int16_t err_flag; 15 | 16 | float ctrl_topic_interval; 17 | 18 | public: 19 | /** 20 | * Encode a message into binary form. 21 | * 22 | * @param buf The output buffer. 23 | * @param offset Encoding starts at thie byte offset into @p buf. 24 | * @param maxlen Maximum number of bytes to write. This should generally be 25 | * equal to getEncodedSize(). 26 | * @return The number of bytes encoded, or <0 on error. 27 | */ 28 | inline int encode( void* buf, int offset, int maxlen ) const; 29 | 30 | /** 31 | * Check how many bytes are required to encode this message. 32 | */ 33 | inline int getEncodedSize() const; 34 | 35 | /** 36 | * Decode a message from binary form into this instance. 37 | * 38 | * @param buf The buffer containing the encoded message. 39 | * @param offset The byte offset into @p buf where the encoded message starts. 40 | * @param maxlen The maximum number of bytes to read while decoding. 41 | * @return The number of bytes decoded, or <0 if an error occured. 42 | */ 43 | inline int decode( const void* buf, int offset, int maxlen ); 44 | 45 | /** 46 | * Retrieve the 64-bit fingerprint identifying the structure of the message. 47 | * Note that the fingerprint is the same for all instances of the same 48 | * message type, and is a fingerprint on the message type definition, not on 49 | * the message contents. 50 | */ 51 | inline static int64_t getHash(); 52 | 53 | /** 54 | * Returns "motor_ctrl_state_lcmt" 55 | */ 56 | inline static const char* getTypeName(); 57 | 58 | // LCM support functions. Users should not call these 59 | inline int _encodeNoHash( void* buf, int offset, int maxlen ) const; 60 | inline int _getEncodedSizeNoHash() const; 61 | inline int _decodeNoHash( const void* buf, int offset, int maxlen ); 62 | inline static uint64_t _computeHash( const __lcm_hash_ptr* p ); 63 | }; 64 | 65 | int motor_ctrl_state_lcmt::encode( void* buf, int offset, int maxlen ) const { 66 | int pos = 0, tlen; 67 | int64_t hash = getHash(); 68 | 69 | tlen = __int64_t_encode_array( buf, offset + pos, maxlen - pos, &hash, 1 ); 70 | if ( tlen < 0 ) 71 | return tlen; 72 | else 73 | pos += tlen; 74 | 75 | tlen = this->_encodeNoHash( buf, offset + pos, maxlen - pos ); 76 | if ( tlen < 0 ) 77 | return tlen; 78 | else 79 | pos += tlen; 80 | 81 | return pos; 82 | } 83 | 84 | int motor_ctrl_state_lcmt::decode( const void* buf, int offset, int maxlen ) { 85 | int pos = 0, thislen; 86 | 87 | int64_t msg_hash; 88 | thislen = __int64_t_decode_array( buf, offset + pos, maxlen - pos, &msg_hash, 1 ); 89 | if ( thislen < 0 ) 90 | return thislen; 91 | else 92 | pos += thislen; 93 | if ( msg_hash != getHash() ) 94 | return -1; 95 | 96 | thislen = this->_decodeNoHash( buf, offset + pos, maxlen - pos ); 97 | if ( thislen < 0 ) 98 | return thislen; 99 | else 100 | pos += thislen; 101 | 102 | return pos; 103 | } 104 | 105 | int motor_ctrl_state_lcmt::getEncodedSize() const { 106 | return 8 + _getEncodedSizeNoHash(); 107 | } 108 | 109 | int64_t motor_ctrl_state_lcmt::getHash() { 110 | static int64_t hash = static_cast< int64_t >( _computeHash( NULL ) ); 111 | return hash; 112 | } 113 | 114 | const char* motor_ctrl_state_lcmt::getTypeName() { 115 | return "motor_ctrl_state_lcmt"; 116 | } 117 | 118 | int motor_ctrl_state_lcmt::_encodeNoHash( void* buf, int offset, int maxlen ) const { 119 | int pos = 0, tlen; 120 | 121 | tlen = __int16_t_encode_array( buf, offset + pos, maxlen - pos, &this->err_flag, 1 ); 122 | if ( tlen < 0 ) 123 | return tlen; 124 | else 125 | pos += tlen; 126 | 127 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->ctrl_topic_interval, 1 ); 128 | if ( tlen < 0 ) 129 | return tlen; 130 | else 131 | pos += tlen; 132 | 133 | return pos; 134 | } 135 | 136 | int motor_ctrl_state_lcmt::_decodeNoHash( const void* buf, int offset, int maxlen ) { 137 | int pos = 0, tlen; 138 | 139 | tlen = __int16_t_decode_array( buf, offset + pos, maxlen - pos, &this->err_flag, 1 ); 140 | if ( tlen < 0 ) 141 | return tlen; 142 | else 143 | pos += tlen; 144 | 145 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->ctrl_topic_interval, 1 ); 146 | if ( tlen < 0 ) 147 | return tlen; 148 | else 149 | pos += tlen; 150 | 151 | return pos; 152 | } 153 | 154 | int motor_ctrl_state_lcmt::_getEncodedSizeNoHash() const { 155 | int enc_size = 0; 156 | enc_size += __int16_t_encoded_array_size( NULL, 1 ); 157 | enc_size += __float_encoded_array_size( NULL, 1 ); 158 | return enc_size; 159 | } 160 | 161 | uint64_t motor_ctrl_state_lcmt::_computeHash( const __lcm_hash_ptr* ) { 162 | uint64_t hash = 0x2afdf7bdf6035aeaLL; 163 | return ( hash << 1 ) + ( ( hash >> 63 ) & 1 ); 164 | } 165 | 166 | #endif 167 | -------------------------------------------------------------------------------- /include/state_estimator_lcmt.hpp: -------------------------------------------------------------------------------- 1 | /** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY 2 | * BY HAND!! 3 | * 4 | * Generated by lcm-gen 5 | **/ 6 | 7 | #ifndef __state_estimator_lcmt_hpp__ 8 | #define __state_estimator_lcmt_hpp__ 9 | 10 | #include 11 | 12 | class state_estimator_lcmt { 13 | public: 14 | float p[ 3 ]; 15 | 16 | float vWorld[ 3 ]; 17 | 18 | float vBody[ 3 ]; 19 | 20 | float vRemoter[ 3 ]; 21 | 22 | float rpy[ 3 ]; 23 | 24 | float omegaBody[ 3 ]; 25 | 26 | float omegaWorld[ 3 ]; 27 | 28 | float quat[ 4 ]; 29 | 30 | float aBody[ 3 ]; 31 | 32 | float aWorld[ 3 ]; 33 | 34 | float contactEstimate[ 4 ]; 35 | 36 | int64_t timestamp; 37 | 38 | public: 39 | /** 40 | * Encode a message into binary form. 41 | * 42 | * @param buf The output buffer. 43 | * @param offset Encoding starts at thie byte offset into @p buf. 44 | * @param maxlen Maximum number of bytes to write. This should generally be 45 | * equal to getEncodedSize(). 46 | * @return The number of bytes encoded, or <0 on error. 47 | */ 48 | inline int encode( void* buf, int offset, int maxlen ) const; 49 | 50 | /** 51 | * Check how many bytes are required to encode this message. 52 | */ 53 | inline int getEncodedSize() const; 54 | 55 | /** 56 | * Decode a message from binary form into this instance. 57 | * 58 | * @param buf The buffer containing the encoded message. 59 | * @param offset The byte offset into @p buf where the encoded message starts. 60 | * @param maxlen The maximum number of bytes to read while decoding. 61 | * @return The number of bytes decoded, or <0 if an error occured. 62 | */ 63 | inline int decode( const void* buf, int offset, int maxlen ); 64 | 65 | /** 66 | * Retrieve the 64-bit fingerprint identifying the structure of the message. 67 | * Note that the fingerprint is the same for all instances of the same 68 | * message type, and is a fingerprint on the message type definition, not on 69 | * the message contents. 70 | */ 71 | inline static int64_t getHash(); 72 | 73 | /** 74 | * Returns "state_estimator_lcmt" 75 | */ 76 | inline static const char* getTypeName(); 77 | 78 | // LCM support functions. Users should not call these 79 | inline int _encodeNoHash( void* buf, int offset, int maxlen ) const; 80 | inline int _getEncodedSizeNoHash() const; 81 | inline int _decodeNoHash( const void* buf, int offset, int maxlen ); 82 | inline static uint64_t _computeHash( const __lcm_hash_ptr* p ); 83 | }; 84 | 85 | int state_estimator_lcmt::encode( void* buf, int offset, int maxlen ) const { 86 | int pos = 0, tlen; 87 | int64_t hash = getHash(); 88 | 89 | tlen = __int64_t_encode_array( buf, offset + pos, maxlen - pos, &hash, 1 ); 90 | if ( tlen < 0 ) 91 | return tlen; 92 | else 93 | pos += tlen; 94 | 95 | tlen = this->_encodeNoHash( buf, offset + pos, maxlen - pos ); 96 | if ( tlen < 0 ) 97 | return tlen; 98 | else 99 | pos += tlen; 100 | 101 | return pos; 102 | } 103 | 104 | int state_estimator_lcmt::decode( const void* buf, int offset, int maxlen ) { 105 | int pos = 0, thislen; 106 | 107 | int64_t msg_hash; 108 | thislen = __int64_t_decode_array( buf, offset + pos, maxlen - pos, &msg_hash, 1 ); 109 | if ( thislen < 0 ) 110 | return thislen; 111 | else 112 | pos += thislen; 113 | if ( msg_hash != getHash() ) 114 | return -1; 115 | 116 | thislen = this->_decodeNoHash( buf, offset + pos, maxlen - pos ); 117 | if ( thislen < 0 ) 118 | return thislen; 119 | else 120 | pos += thislen; 121 | 122 | return pos; 123 | } 124 | 125 | int state_estimator_lcmt::getEncodedSize() const { 126 | return 8 + _getEncodedSizeNoHash(); 127 | } 128 | 129 | int64_t state_estimator_lcmt::getHash() { 130 | static int64_t hash = static_cast< int64_t >( _computeHash( NULL ) ); 131 | return hash; 132 | } 133 | 134 | const char* state_estimator_lcmt::getTypeName() { 135 | return "state_estimator_lcmt"; 136 | } 137 | 138 | int state_estimator_lcmt::_encodeNoHash( void* buf, int offset, int maxlen ) const { 139 | int pos = 0, tlen; 140 | 141 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->p[ 0 ], 3 ); 142 | if ( tlen < 0 ) 143 | return tlen; 144 | else 145 | pos += tlen; 146 | 147 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->vWorld[ 0 ], 3 ); 148 | if ( tlen < 0 ) 149 | return tlen; 150 | else 151 | pos += tlen; 152 | 153 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->vBody[ 0 ], 3 ); 154 | if ( tlen < 0 ) 155 | return tlen; 156 | else 157 | pos += tlen; 158 | 159 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->vRemoter[ 0 ], 3 ); 160 | if ( tlen < 0 ) 161 | return tlen; 162 | else 163 | pos += tlen; 164 | 165 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->rpy[ 0 ], 3 ); 166 | if ( tlen < 0 ) 167 | return tlen; 168 | else 169 | pos += tlen; 170 | 171 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->omegaBody[ 0 ], 3 ); 172 | if ( tlen < 0 ) 173 | return tlen; 174 | else 175 | pos += tlen; 176 | 177 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->omegaWorld[ 0 ], 3 ); 178 | if ( tlen < 0 ) 179 | return tlen; 180 | else 181 | pos += tlen; 182 | 183 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->quat[ 0 ], 4 ); 184 | if ( tlen < 0 ) 185 | return tlen; 186 | else 187 | pos += tlen; 188 | 189 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->aBody[ 0 ], 3 ); 190 | if ( tlen < 0 ) 191 | return tlen; 192 | else 193 | pos += tlen; 194 | 195 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->aWorld[ 0 ], 3 ); 196 | if ( tlen < 0 ) 197 | return tlen; 198 | else 199 | pos += tlen; 200 | 201 | tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->contactEstimate[ 0 ], 4 ); 202 | if ( tlen < 0 ) 203 | return tlen; 204 | else 205 | pos += tlen; 206 | 207 | tlen = __int64_t_encode_array( buf, offset + pos, maxlen - pos, &this->timestamp, 1 ); 208 | if ( tlen < 0 ) 209 | return tlen; 210 | else 211 | pos += tlen; 212 | 213 | return pos; 214 | } 215 | 216 | int state_estimator_lcmt::_decodeNoHash( const void* buf, int offset, int maxlen ) { 217 | int pos = 0, tlen; 218 | 219 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->p[ 0 ], 3 ); 220 | if ( tlen < 0 ) 221 | return tlen; 222 | else 223 | pos += tlen; 224 | 225 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->vWorld[ 0 ], 3 ); 226 | if ( tlen < 0 ) 227 | return tlen; 228 | else 229 | pos += tlen; 230 | 231 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->vBody[ 0 ], 3 ); 232 | if ( tlen < 0 ) 233 | return tlen; 234 | else 235 | pos += tlen; 236 | 237 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->vRemoter[ 0 ], 3 ); 238 | if ( tlen < 0 ) 239 | return tlen; 240 | else 241 | pos += tlen; 242 | 243 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->rpy[ 0 ], 3 ); 244 | if ( tlen < 0 ) 245 | return tlen; 246 | else 247 | pos += tlen; 248 | 249 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->omegaBody[ 0 ], 3 ); 250 | if ( tlen < 0 ) 251 | return tlen; 252 | else 253 | pos += tlen; 254 | 255 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->omegaWorld[ 0 ], 3 ); 256 | if ( tlen < 0 ) 257 | return tlen; 258 | else 259 | pos += tlen; 260 | 261 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->quat[ 0 ], 4 ); 262 | if ( tlen < 0 ) 263 | return tlen; 264 | else 265 | pos += tlen; 266 | 267 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->aBody[ 0 ], 3 ); 268 | if ( tlen < 0 ) 269 | return tlen; 270 | else 271 | pos += tlen; 272 | 273 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->aWorld[ 0 ], 3 ); 274 | if ( tlen < 0 ) 275 | return tlen; 276 | else 277 | pos += tlen; 278 | 279 | tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->contactEstimate[ 0 ], 4 ); 280 | if ( tlen < 0 ) 281 | return tlen; 282 | else 283 | pos += tlen; 284 | 285 | tlen = __int64_t_decode_array( buf, offset + pos, maxlen - pos, &this->timestamp, 1 ); 286 | if ( tlen < 0 ) 287 | return tlen; 288 | else 289 | pos += tlen; 290 | 291 | return pos; 292 | } 293 | 294 | int state_estimator_lcmt::_getEncodedSizeNoHash() const { 295 | int enc_size = 0; 296 | enc_size += __float_encoded_array_size( NULL, 3 ); 297 | enc_size += __float_encoded_array_size( NULL, 3 ); 298 | enc_size += __float_encoded_array_size( NULL, 3 ); 299 | enc_size += __float_encoded_array_size( NULL, 3 ); 300 | enc_size += __float_encoded_array_size( NULL, 3 ); 301 | enc_size += __float_encoded_array_size( NULL, 3 ); 302 | enc_size += __float_encoded_array_size( NULL, 3 ); 303 | enc_size += __float_encoded_array_size( NULL, 4 ); 304 | enc_size += __float_encoded_array_size( NULL, 3 ); 305 | enc_size += __float_encoded_array_size( NULL, 3 ); 306 | enc_size += __float_encoded_array_size( NULL, 4 ); 307 | enc_size += __int64_t_encoded_array_size( NULL, 1 ); 308 | return enc_size; 309 | } 310 | 311 | uint64_t state_estimator_lcmt::_computeHash( const __lcm_hash_ptr* ) { 312 | uint64_t hash = 0xeb390c8b8230c53fLL; 313 | return ( hash << 1 ) + ( ( hash >> 63 ) & 1 ); 314 | } 315 | 316 | #endif 317 | -------------------------------------------------------------------------------- /src/CustomInterface.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | std::string CustomInterface::getLcmUrl_port( int64_t port, int64_t ttl ) { 18 | assert( ttl >= 0 && ttl <= 255 ); 19 | return "udpm://239.255.76.67:" + std::to_string( port ) + "?ttl=" + std::to_string( ttl ); 20 | } 21 | 22 | CustomInterface::CustomInterface( const double& loop_rate ) 23 | : _motor_data_Lcm( getLcmUrl_port( 7667, 255 ) ), _motor_ctrl_state_Lcm( getLcmUrl_port( 7667, 255 ) ), _robot_state_Lcm( getLcmUrl_port( 7669, 255 ) ), 24 | _motor_ctrl_Lcm( getLcmUrl_port( 7667, 255 ) ) { 25 | 26 | running_ = true; 27 | all_thread_done_ = false; 28 | mode_state = false; 29 | 30 | if ( loop_rate > 0 ) { 31 | dt_ = 1.0 / loop_rate; 32 | } 33 | else { 34 | std::cout << "Loop rate should be more than zero! Set to default 500Hz." << std::endl; 35 | dt_ = 1.0 / 500; 36 | } 37 | 38 | _motor_ctrl_state_Lcm.subscribe( "motor_ctrl_state", &CustomInterface::handle_motor_ctrl_state_LCM, this ); 39 | _motor_data_Lcm.subscribe( "leg_control_data", &CustomInterface::handle_motor_data_LCM, this ); 40 | _robot_state_Lcm.subscribe( "state_estimator", &CustomInterface::handle_robot_state_LCM, this ); 41 | 42 | _motor_ctrl_state_LcmThread = std::thread( &CustomInterface::motor_ctrl_state_LcmThread, this ); 43 | _motor_data_LcmThread = std::thread( &CustomInterface::motor_data_LcmThread, this ); 44 | _robot_state_LcmThread = std::thread( &CustomInterface::robot_state_LcmThread, this ); 45 | _user_code_ControlThread = std::thread( &CustomInterface::Control, this ); 46 | } 47 | 48 | void CustomInterface::Control() { 49 | auto timerFd = timerfd_create( CLOCK_MONOTONIC, 0 ); 50 | int seconds = ( int )dt_; 51 | int nanoseconds = ( int )( 1e9 * std::fmod( dt_, 1.f ) ); 52 | int err_count = 0; 53 | 54 | itimerspec timerSpec; 55 | timerSpec.it_interval.tv_sec = seconds; 56 | timerSpec.it_value.tv_sec = seconds; 57 | timerSpec.it_value.tv_nsec = nanoseconds; 58 | timerSpec.it_interval.tv_nsec = nanoseconds; 59 | 60 | timerfd_settime( timerFd, 0, &timerSpec, nullptr ); 61 | unsigned long long missed = 0; 62 | while ( running_ ) { 63 | if ( !mode_state ) { 64 | sleep( 1 ); 65 | std::cout << "Motor control mode has not been activated successfully" << std::endl; 66 | continue; 67 | } 68 | 69 | UserCode(); 70 | 71 | if ( robot_data.err_flag & 0x02 ) { 72 | if ( err_count++ % 1000 == 0 ) { 73 | printf( "\nErr: 0x02 Communicate lost over 500ms!\n" ); 74 | printf( "The motor cmd has been disabled!!!\n" ); 75 | printf( "Cyberdog locomotion ctrl process need restart after fix the communication!\n\n" ); 76 | } 77 | } 78 | else { 79 | motor_cmd_send(); 80 | } 81 | 82 | int m = read( timerFd, &missed, sizeof( missed ) ); 83 | ( void )m; 84 | } 85 | } 86 | 87 | void CustomInterface::Spin() { 88 | while ( !all_thread_done_ ) { 89 | sleep( 1.0 ); 90 | } 91 | 92 | printf( "~ Exit ~\n" ); 93 | } 94 | 95 | void CustomInterface::Stop() { 96 | running_ = false; 97 | _motor_ctrl_state_LcmThread.join(); 98 | _motor_data_LcmThread.join(); 99 | _robot_state_LcmThread.join(); 100 | _user_code_ControlThread.join(); 101 | all_thread_done_ = true; 102 | } 103 | 104 | void CustomInterface::motor_cmd_send() { 105 | int sig[ 12 ] = { 1, -1, -1, 1, -1, -1, 1, -1, -1, 1, -1, -1 }; 106 | for ( int i = 0; i < 12; i++ ) { 107 | _motor_ctrl.q_des[ i ] = motor_cmd.q_des[ i ] * sig[ i ]; 108 | _motor_ctrl.qd_des[ i ] = motor_cmd.qd_des[ i ] * sig[ i ]; 109 | _motor_ctrl.kp_des[ i ] = motor_cmd.kp_des[ i ]; 110 | _motor_ctrl.kd_des[ i ] = motor_cmd.kd_des[ i ]; 111 | _motor_ctrl.tau_des[ i ] = motor_cmd.tau_des[ i ] * sig[ i ]; 112 | } 113 | _motor_ctrl_Lcm.publish( "motor_ctrl", &_motor_ctrl ); 114 | } 115 | 116 | void CustomInterface::handle_motor_ctrl_state_LCM( const lcm::ReceiveBuffer* rbuf, const std::string& chan, const motor_ctrl_state_lcmt* msg ) { 117 | ( void )rbuf; 118 | ( void )chan; 119 | robot_data.err_flag = msg->err_flag; 120 | robot_data.ctrl_topic_interval = msg->ctrl_topic_interval; 121 | mode_state = true; 122 | } 123 | void CustomInterface::handle_motor_data_LCM( const lcm::ReceiveBuffer* rbuf, const std::string& chan, const leg_control_data_lcmt* msg ) { 124 | ( void )rbuf; 125 | ( void )chan; 126 | int sig[ 12 ] = { 1, -1, -1, 1, -1, -1, 1, -1, -1, 1, -1, -1 }; 127 | for ( int i = 0; i < 12; i++ ) { 128 | robot_data.q[ i ] = msg->q[ i ] * sig[ i ]; 129 | robot_data.qd[ i ] = msg->qd[ i ] * sig[ i ]; 130 | robot_data.tau[ i ] = msg->tau_est[ i ] * sig[ i ]; 131 | } 132 | } 133 | void CustomInterface::handle_robot_state_LCM( const lcm::ReceiveBuffer* rbuf, const std::string& chan, const state_estimator_lcmt* msg ) { 134 | ( void )rbuf; 135 | ( void )chan; 136 | for ( int i = 0; i < 3; i++ ) { 137 | robot_data.omega[ i ] = msg->omegaWorld[ i ]; 138 | robot_data.rpy[ i ] = msg->rpy[ i ]; 139 | robot_data.acc[ i ] = msg->aWorld[ i ]; 140 | } 141 | for ( int i = 0; i < 4; i++ ) { 142 | robot_data.quat[ i ] = msg->quat[ i ]; 143 | } 144 | } -------------------------------------------------------------------------------- /src/Example_MotorCtrl.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | class CustomCtrl : public CustomInterface { 18 | public: 19 | CustomCtrl( const double& loop_rate ) : CustomInterface( loop_rate ){}; 20 | ~CustomCtrl(){}; 21 | 22 | private: 23 | bool first_run = true; 24 | long long count = 0; 25 | float init_q[ 12 ]; 26 | float target1_q[ 3 ] = { 0 / 57.3, 80 / 57.3, -135 / 57.3 }; 27 | float target2_q[ 3 ] = { 0 / 57.3, 45 / 57.3, -90 / 57.3 }; 28 | void UserCode() { 29 | // vertical view 30 | // leg 1 | | leg 0 31 | // | | 32 | // leg 3 | | leg 2 33 | // 34 | // Right hand coordinate system 35 | // ______ zero angle ______ 36 | // \ \ --> | | 37 | // / / | | 38 | float t = ( count / 1500.0 ) > 2 ? 2 : ( count / 1500.0 ); 39 | if ( first_run == true ) { 40 | for ( int i = 0; i < 12; i++ ) 41 | init_q[ i ] = robot_data.q[ i ]; 42 | if ( init_q[ 2 ] < -0.1 && init_q[ 5 ] < -0.1 && init_q[ 8 ] < -0.1 && init_q[ 11 ] < -0.1 ) { 43 | first_run = false; 44 | count = 0; 45 | } 46 | } 47 | else { 48 | for ( int i = 0; i < 12; i++ ) { 49 | if ( t < 1.0 ) 50 | motor_cmd.q_des[ i ] = target1_q[ i % 3 ] * t + init_q[ i ] * ( 1 - t ); 51 | else 52 | motor_cmd.q_des[ i ] = target2_q[ i % 3 ] * ( t - 1 ) + target1_q[ i % 3 ] * ( 2 - t ); 53 | motor_cmd.kp_des[ i ] = 100; 54 | motor_cmd.kd_des[ i ] = 2; 55 | motor_cmd.qd_des[ i ] = 0; 56 | motor_cmd.tau_des[ i ] = 0; 57 | } 58 | } 59 | if ( ( count++ ) % 1000 == 0 ) { 60 | printf( "interval:---------%.4f-------------\n", robot_data.ctrl_topic_interval ); 61 | printf( "rpy [3]:" ); 62 | for ( int i = 0; i < 3; i++ ) 63 | printf( " %.2f", robot_data.rpy[ i ] ); 64 | printf( "\nacc [3]:" ); 65 | for ( int i = 0; i < 3; i++ ) 66 | printf( " %.2f", robot_data.acc[ i ] ); 67 | printf( "\nquat[4]:" ); 68 | for ( int i = 0; i < 4; i++ ) 69 | printf( " %.2f", robot_data.quat[ i ] ); 70 | printf( "\nomeg[3]:" ); 71 | for ( int i = 0; i < 3; i++ ) 72 | printf( " %.2f", robot_data.omega[ i ] ); 73 | printf( "\nq [12]:" ); 74 | for ( int i = 0; i < 12; i++ ) 75 | printf( " %.2f", robot_data.q[ i ] ); 76 | printf( "\nqd [12]:" ); 77 | for ( int i = 0; i < 12; i++ ) 78 | printf( " %.2f", robot_data.qd[ i ] ); 79 | printf( "\ntau[12]:" ); 80 | for ( int i = 0; i < 12; i++ ) 81 | printf( " %.2f", robot_data.tau[ i ] ); 82 | printf( "\nctrl[12]:" ); 83 | for ( int i = 0; i < 12; i++ ) 84 | printf( " %.2f", motor_cmd.q_des[ i ] ); 85 | printf( "\n\n" ); 86 | } 87 | } 88 | }; 89 | 90 | std::shared_ptr< CustomCtrl > io; 91 | void signal_callback_handler( int signum ) { 92 | io->Stop(); 93 | ( void )signum; 94 | } 95 | 96 | int main() { 97 | /* user code ctrl mode:1 for motor ctrl */ 98 | signal( SIGINT, signal_callback_handler ); 99 | io = std::make_shared< CustomCtrl >( 500 ); 100 | io->Spin(); 101 | return 0; 102 | } --------------------------------------------------------------------------------