diff --git a/README.md b/README.md index d0b949f..2efaa8b 100644 --- a/README.md +++ b/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 diff --git a/README_CN.md b/README_CN.md index e8550e9..279f950 100644 --- a/README_CN.md +++ b/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 ``` 新建终端，启动控制程序 diff --git a/src/rl_sar/include/rl_sim.hpp b/src/rl_sar/include/rl_sim.hpp index 7fa12a0..917f50f 100644 --- a/src/rl_sar/include/rl_sim.hpp +++ b/src/rl_sar/include/rl_sim.hpp @@ -41,6 +41,8 @@ public: std::vector> plot_real_joint_pos, plot_target_joint_pos; void Plot(); private: + std::string ros_namespace; + std::vector torque_command_topics; ros::Subscriber model_state_subscriber_; diff --git a/src/rl_sar/launch/start_env.launch b/src/rl_sar/launch/start_a1.launch similarity index 100% rename from src/rl_sar/launch/start_env.launch rename to src/rl_sar/launch/start_a1.launch diff --git a/src/rl_sar/launch/start_cyberdog.launch b/src/rl_sar/launch/start_cyberdog.launch new file mode 100644 index 0000000..142ab95 --- /dev/null +++ b/src/rl_sar/launch/start_cyberdog.launch @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/rl_sar/library/cyberdog_motor_sdk/CMakeLists.txt b/src/rl_sar/library/cyberdog_motor_sdk/CMakeLists.txt new file mode 100644 index 0000000..c2a90b3 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/src/rl_sar/library/cyberdog_motor_sdk/README.md b/src/rl_sar/library/cyberdog_motor_sdk/README.md new file mode 100644 index 0000000..dddcae6 --- /dev/null +++ b/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 mi@192.168.55.1 #登录nx应用板 ,密码123 +mi@lubuntu:~$ athena_version -v #核对当前版本>=1.0.0.94 +$ ssh root@192.168.55.233 #登录运动控制板 +``` +#### 进入电机控制模式 +修改配置开关，激活用户控制模式，运行用户自己的控制器： +``` +$ ssh root@192.168.55.233 #登录运动控制板 +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 mi@192.168.55.1:/home/mi/ #sdk源码拷入应用板，密码123 +$ ssh mi@192.168.55.1 #登录应用板 +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 root@192.168.55.233 "mkdir /mnt/UDISK/cyberdog_motor_sdk" #在运控板内创建文件夹 +$ scp libcyber_dog_motor_sdk.so Example_MotorCtrl root@192.168.55.233:/mnt/UDISK/cyberdog_motor_sdk +$ ssh root@192.168.55.233 +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 root@192.168.55.233 "ps | grep -E 'Example_MotorCtrl' | grep -v grep | awk '{print \$1}' | xargs kill -9" #需先于主进程暂停，避免急停 +$ ssh root@192.168.55.233 "ps | grep -E 'manager|ctrl|imu_online' | grep -v grep | awk '{print \$1}' | xargs kill -9" +$ 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 &" +# 重启运控板系统: +$ ssh root@192.168.55.233 "reboot" +``` diff --git a/src/rl_sar/library/cyberdog_motor_sdk/README_EN.md b/src/rl_sar/library/cyberdog_motor_sdk/README_EN.md new file mode 100644 index 0000000..7d58a42 --- /dev/null +++ b/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 mi@192.168.55.1 # Login NX application board, password 123 +mi@lubuntu:~$ athena_version -v # check current version >= 1.0.0.94 +$ ssh root@192.168.55.233 # Log in to the motion control board +``` +#### Enter motor control mode +Modify the configuration switch to activate the user control mode: +``` +$ ssh root@192.168.55.233 # 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 mi@192.168.55.1:/home/mi/ # copy the SDK source code into the application board, password 123 +$ ssh mi@192.168.55.1 # 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 root@192.168.55.233 "mkdir /mnt/UDISK/cyberdog_motor_sdk" # create a folder in the motion control board +$ scp libcyber_dog_motor_sdk.so Example_MotorCtrl root@192.168.55.233:/mnt/UDISK/cyberdog_motor_sdk +$ ssh root@192.168.55.233 +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 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 +$ ssh root@192.168.55.233 "ps | grep -E 'manager|ctrl|imu_online' | grep -v grep | awk '{print \$1}' | xargs kill -9" +$ 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 &" +``` +How to reboot the motion control board: +``` +$ ssh root@192.168.55.233 "reboot" +``` diff --git a/src/rl_sar/library/cyberdog_motor_sdk/include/CustomInterface.h b/src/rl_sar/library/cyberdog_motor_sdk/include/CustomInterface.h new file mode 100644 index 0000000..727e842 --- /dev/null +++ b/src/rl_sar/library/cyberdog_motor_sdk/include/CustomInterface.h @@ -0,0 +1,117 @@ +// Copyright (c) 2021 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PROJECT_CUSTOMINTERFACE_H +#define PROJECT_CUSTOMINTERFACE_H + +#include "leg_control_data_lcmt.hpp" +#include "motor_ctrl_lcmt.hpp" +#include "motor_ctrl_state_lcmt.hpp" +#include "state_estimator_lcmt.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct Robot_Data +{ + float q[12]; // 12个关节电机角度，弧度制 + float qd[12]; //电机角速度，弧度制 + float tau[12]; //电机扭矩 N.M + float quat[4]; //机身姿态四元数，右手坐标系 w, x, y, z + float rpy[3]; //机身姿态横滚、俯仰、偏航角 弧度制 + float acc[3]; //加速度计值 + float omega[3]; //角速度计值 + float ctrl_topic_interval; //控制topic通信延迟 + int16_t err_flag; +}; +struct Motor_Cmd +{ + //期望扭矩tau = tau_des + (q_des - q)*kp_des + (qd_des - qd)*kd_des + float q_des[12]; // 12个关机电机期望角度，弧度制 0/3/6/9:-0.75~0.75 1/4:-1.257 ~ 4.363 7/10:-2.01 ~ 3.49 2/5/8/11:-2.478 ~ -0.506 + float qd_des[12]; //电机期望角速度，弧度制 +-12弧度/秒@24NM + float kp_des[12]; //电机位置控制比例系数 0~200 + float kd_des[12]; //电机速度控制比例系数 0~10 + float tau_des[12]; //电机期望前馈扭矩 0/3/6/9:+-17N/M 1/2/4/5/7/8/10/11:+-24NM +}; + +class CustomInterface +{ +public: + CustomInterface(const double &loop_rate); + void Spin(); + void Stop(); + +protected: + virtual void UserCode() = 0; + Robot_Data robot_data; + Motor_Cmd motor_cmd; + +private: + double dt_; + bool running_; + bool all_thread_done_; + bool mode_state; + + lcm::LCM _motor_ctrl_Lcm; + lcm::LCM _motor_data_Lcm; + lcm::LCM _robot_state_Lcm; + lcm::LCM _motor_ctrl_state_Lcm; + + std::thread _motor_ctrl_state_LcmThread; + std::thread _motor_data_LcmThread; + std::thread _robot_state_LcmThread; + std::thread _user_code_ControlThread; + + motor_ctrl_lcmt _motor_ctrl; + + std::string getLcmUrl_port(int64_t port, int64_t ttl); + + void motor_data_LcmThread() + { + while(running_) + { + _motor_data_Lcm.handleTimeout(1000); + } + } + void robot_state_LcmThread() + { + while(running_) + { + _robot_state_Lcm.handleTimeout(1000); + } + } + void motor_ctrl_state_LcmThread() + { + while(running_) + { + _motor_ctrl_state_Lcm.handleTimeout(1000); + } + } + + void handle_motor_ctrl_state_LCM(const lcm::ReceiveBuffer *rbuf, const std::string &chan, const motor_ctrl_state_lcmt *msg); + void handle_motor_data_LCM(const lcm::ReceiveBuffer *rbuf, const std::string &chan, const leg_control_data_lcmt *msg); + void handle_robot_state_LCM(const lcm::ReceiveBuffer *rbuf, const std::string &chan, const state_estimator_lcmt *msg); + + void motor_cmd_send(); + void Control(); +}; // CustomInterface +#endif \ No newline at end of file diff --git a/src/rl_sar/library/cyberdog_motor_sdk/include/leg_control_data_lcmt.hpp b/src/rl_sar/library/cyberdog_motor_sdk/include/leg_control_data_lcmt.hpp new file mode 100644 index 0000000..1470a6b --- /dev/null +++ b/src/rl_sar/library/cyberdog_motor_sdk/include/leg_control_data_lcmt.hpp @@ -0,0 +1,286 @@ +/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY + * BY HAND!! + * + * Generated by lcm-gen + **/ + +#ifndef __leg_control_data_lcmt_hpp__ +#define __leg_control_data_lcmt_hpp__ + +#include + +class leg_control_data_lcmt { +public: + float q[ 12 ]; + + float qd[ 12 ]; + + float p[ 12 ]; + + float v[ 12 ]; + + float tau_est[ 12 ]; + + float force_est[ 12 ]; + + float force_desired[ 12 ]; + + int32_t q_abad_limit[ 4 ]; + + int32_t q_hip_limit[ 4 ]; + + int32_t q_knee_limit[ 4 ]; + +public: + /** + * Encode a message into binary form. + * + * @param buf The output buffer. + * @param offset Encoding starts at thie byte offset into @p buf. + * @param maxlen Maximum number of bytes to write. This should generally be + * equal to getEncodedSize(). + * @return The number of bytes encoded, or <0 on error. + */ + inline int encode( void* buf, int offset, int maxlen ) const; + + /** + * Check how many bytes are required to encode this message. + */ + inline int getEncodedSize() const; + + /** + * Decode a message from binary form into this instance. + * + * @param buf The buffer containing the encoded message. + * @param offset The byte offset into @p buf where the encoded message starts. + * @param maxlen The maximum number of bytes to read while decoding. + * @return The number of bytes decoded, or <0 if an error occured. + */ + inline int decode( const void* buf, int offset, int maxlen ); + + /** + * Retrieve the 64-bit fingerprint identifying the structure of the message. + * Note that the fingerprint is the same for all instances of the same + * message type, and is a fingerprint on the message type definition, not on + * the message contents. + */ + inline static int64_t getHash(); + + /** + * Returns "leg_control_data_lcmt" + */ + inline static const char* getTypeName(); + + // LCM support functions. Users should not call these + inline int _encodeNoHash( void* buf, int offset, int maxlen ) const; + inline int _getEncodedSizeNoHash() const; + inline int _decodeNoHash( const void* buf, int offset, int maxlen ); + inline static uint64_t _computeHash( const __lcm_hash_ptr* p ); +}; + +int leg_control_data_lcmt::encode( void* buf, int offset, int maxlen ) const { + int pos = 0, tlen; + int64_t hash = getHash(); + + tlen = __int64_t_encode_array( buf, offset + pos, maxlen - pos, &hash, 1 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = this->_encodeNoHash( buf, offset + pos, maxlen - pos ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + return pos; +} + +int leg_control_data_lcmt::decode( const void* buf, int offset, int maxlen ) { + int pos = 0, thislen; + + int64_t msg_hash; + thislen = __int64_t_decode_array( buf, offset + pos, maxlen - pos, &msg_hash, 1 ); + if ( thislen < 0 ) + return thislen; + else + pos += thislen; + if ( msg_hash != getHash() ) + return -1; + + thislen = this->_decodeNoHash( buf, offset + pos, maxlen - pos ); + if ( thislen < 0 ) + return thislen; + else + pos += thislen; + + return pos; +} + +int leg_control_data_lcmt::getEncodedSize() const { + return 8 + _getEncodedSizeNoHash(); +} + +int64_t leg_control_data_lcmt::getHash() { + static int64_t hash = static_cast< int64_t >( _computeHash( NULL ) ); + return hash; +} + +const char* leg_control_data_lcmt::getTypeName() { + return "leg_control_data_lcmt"; +} + +int leg_control_data_lcmt::_encodeNoHash( void* buf, int offset, int maxlen ) const { + int pos = 0, tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->q[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->qd[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->p[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->v[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->tau_est[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->force_est[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->force_desired[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __int32_t_encode_array( buf, offset + pos, maxlen - pos, &this->q_abad_limit[ 0 ], 4 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __int32_t_encode_array( buf, offset + pos, maxlen - pos, &this->q_hip_limit[ 0 ], 4 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __int32_t_encode_array( buf, offset + pos, maxlen - pos, &this->q_knee_limit[ 0 ], 4 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + return pos; +} + +int leg_control_data_lcmt::_decodeNoHash( const void* buf, int offset, int maxlen ) { + int pos = 0, tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->q[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->qd[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->p[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->v[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->tau_est[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->force_est[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->force_desired[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __int32_t_decode_array( buf, offset + pos, maxlen - pos, &this->q_abad_limit[ 0 ], 4 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __int32_t_decode_array( buf, offset + pos, maxlen - pos, &this->q_hip_limit[ 0 ], 4 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __int32_t_decode_array( buf, offset + pos, maxlen - pos, &this->q_knee_limit[ 0 ], 4 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + return pos; +} + +int leg_control_data_lcmt::_getEncodedSizeNoHash() const { + int enc_size = 0; + enc_size += __float_encoded_array_size( NULL, 12 ); + enc_size += __float_encoded_array_size( NULL, 12 ); + enc_size += __float_encoded_array_size( NULL, 12 ); + enc_size += __float_encoded_array_size( NULL, 12 ); + enc_size += __float_encoded_array_size( NULL, 12 ); + enc_size += __float_encoded_array_size( NULL, 12 ); + enc_size += __float_encoded_array_size( NULL, 12 ); + enc_size += __int32_t_encoded_array_size( NULL, 4 ); + enc_size += __int32_t_encoded_array_size( NULL, 4 ); + enc_size += __int32_t_encoded_array_size( NULL, 4 ); + return enc_size; +} + +uint64_t leg_control_data_lcmt::_computeHash( const __lcm_hash_ptr* ) { + uint64_t hash = 0xa6b1824464a42a6bLL; + return ( hash << 1 ) + ( ( hash >> 63 ) & 1 ); +} + +#endif diff --git a/src/rl_sar/library/cyberdog_motor_sdk/include/motor_ctrl_lcmt.hpp b/src/rl_sar/library/cyberdog_motor_sdk/include/motor_ctrl_lcmt.hpp new file mode 100644 index 0000000..add044a --- /dev/null +++ b/src/rl_sar/library/cyberdog_motor_sdk/include/motor_ctrl_lcmt.hpp @@ -0,0 +1,211 @@ +/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY + * BY HAND!! + * + * Generated by lcm-gen + **/ + +#ifndef __motor_ctrl_lcmt_hpp__ +#define __motor_ctrl_lcmt_hpp__ + +#include + +class motor_ctrl_lcmt { +public: + float q_des[ 12 ]; + + float qd_des[ 12 ]; + + float kp_des[ 12 ]; + + float kd_des[ 12 ]; + + float tau_des[ 12 ]; + +public: + /** + * Encode a message into binary form. + * + * @param buf The output buffer. + * @param offset Encoding starts at thie byte offset into @p buf. + * @param maxlen Maximum number of bytes to write. This should generally be + * equal to getEncodedSize(). + * @return The number of bytes encoded, or <0 on error. + */ + inline int encode( void* buf, int offset, int maxlen ) const; + + /** + * Check how many bytes are required to encode this message. + */ + inline int getEncodedSize() const; + + /** + * Decode a message from binary form into this instance. + * + * @param buf The buffer containing the encoded message. + * @param offset The byte offset into @p buf where the encoded message starts. + * @param maxlen The maximum number of bytes to read while decoding. + * @return The number of bytes decoded, or <0 if an error occured. + */ + inline int decode( const void* buf, int offset, int maxlen ); + + /** + * Retrieve the 64-bit fingerprint identifying the structure of the message. + * Note that the fingerprint is the same for all instances of the same + * message type, and is a fingerprint on the message type definition, not on + * the message contents. + */ + inline static int64_t getHash(); + + /** + * Returns "motor_ctrl_lcmt" + */ + inline static const char* getTypeName(); + + // LCM support functions. Users should not call these + inline int _encodeNoHash( void* buf, int offset, int maxlen ) const; + inline int _getEncodedSizeNoHash() const; + inline int _decodeNoHash( const void* buf, int offset, int maxlen ); + inline static uint64_t _computeHash( const __lcm_hash_ptr* p ); +}; + +int motor_ctrl_lcmt::encode( void* buf, int offset, int maxlen ) const { + int pos = 0, tlen; + int64_t hash = getHash(); + + tlen = __int64_t_encode_array( buf, offset + pos, maxlen - pos, &hash, 1 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = this->_encodeNoHash( buf, offset + pos, maxlen - pos ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + return pos; +} + +int motor_ctrl_lcmt::decode( const void* buf, int offset, int maxlen ) { + int pos = 0, thislen; + + int64_t msg_hash; + thislen = __int64_t_decode_array( buf, offset + pos, maxlen - pos, &msg_hash, 1 ); + if ( thislen < 0 ) + return thislen; + else + pos += thislen; + if ( msg_hash != getHash() ) + return -1; + + thislen = this->_decodeNoHash( buf, offset + pos, maxlen - pos ); + if ( thislen < 0 ) + return thislen; + else + pos += thislen; + + return pos; +} + +int motor_ctrl_lcmt::getEncodedSize() const { + return 8 + _getEncodedSizeNoHash(); +} + +int64_t motor_ctrl_lcmt::getHash() { + static int64_t hash = static_cast< int64_t >( _computeHash( NULL ) ); + return hash; +} + +const char* motor_ctrl_lcmt::getTypeName() { + return "motor_ctrl_lcmt"; +} + +int motor_ctrl_lcmt::_encodeNoHash( void* buf, int offset, int maxlen ) const { + int pos = 0, tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->q_des[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->qd_des[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->kp_des[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->kd_des[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->tau_des[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + return pos; +} + +int motor_ctrl_lcmt::_decodeNoHash( const void* buf, int offset, int maxlen ) { + int pos = 0, tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->q_des[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->qd_des[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->kp_des[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->kd_des[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->tau_des[ 0 ], 12 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + return pos; +} + +int motor_ctrl_lcmt::_getEncodedSizeNoHash() const { + int enc_size = 0; + enc_size += __float_encoded_array_size( NULL, 12 ); + enc_size += __float_encoded_array_size( NULL, 12 ); + enc_size += __float_encoded_array_size( NULL, 12 ); + enc_size += __float_encoded_array_size( NULL, 12 ); + enc_size += __float_encoded_array_size( NULL, 12 ); + return enc_size; +} + +uint64_t motor_ctrl_lcmt::_computeHash( const __lcm_hash_ptr* ) { + uint64_t hash = 0x3ddbe622bc22d656LL; + return ( hash << 1 ) + ( ( hash >> 63 ) & 1 ); +} + +#endif diff --git a/src/rl_sar/library/cyberdog_motor_sdk/include/motor_ctrl_state_lcmt.hpp b/src/rl_sar/library/cyberdog_motor_sdk/include/motor_ctrl_state_lcmt.hpp new file mode 100644 index 0000000..a452571 --- /dev/null +++ b/src/rl_sar/library/cyberdog_motor_sdk/include/motor_ctrl_state_lcmt.hpp @@ -0,0 +1,166 @@ +/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY + * BY HAND!! + * + * Generated by lcm-gen + **/ + +#ifndef __motor_ctrl_state_lcmt_hpp__ +#define __motor_ctrl_state_lcmt_hpp__ + +#include + +class motor_ctrl_state_lcmt { +public: + int16_t err_flag; + + float ctrl_topic_interval; + +public: + /** + * Encode a message into binary form. + * + * @param buf The output buffer. + * @param offset Encoding starts at thie byte offset into @p buf. + * @param maxlen Maximum number of bytes to write. This should generally be + * equal to getEncodedSize(). + * @return The number of bytes encoded, or <0 on error. + */ + inline int encode( void* buf, int offset, int maxlen ) const; + + /** + * Check how many bytes are required to encode this message. + */ + inline int getEncodedSize() const; + + /** + * Decode a message from binary form into this instance. + * + * @param buf The buffer containing the encoded message. + * @param offset The byte offset into @p buf where the encoded message starts. + * @param maxlen The maximum number of bytes to read while decoding. + * @return The number of bytes decoded, or <0 if an error occured. + */ + inline int decode( const void* buf, int offset, int maxlen ); + + /** + * Retrieve the 64-bit fingerprint identifying the structure of the message. + * Note that the fingerprint is the same for all instances of the same + * message type, and is a fingerprint on the message type definition, not on + * the message contents. + */ + inline static int64_t getHash(); + + /** + * Returns "motor_ctrl_state_lcmt" + */ + inline static const char* getTypeName(); + + // LCM support functions. Users should not call these + inline int _encodeNoHash( void* buf, int offset, int maxlen ) const; + inline int _getEncodedSizeNoHash() const; + inline int _decodeNoHash( const void* buf, int offset, int maxlen ); + inline static uint64_t _computeHash( const __lcm_hash_ptr* p ); +}; + +int motor_ctrl_state_lcmt::encode( void* buf, int offset, int maxlen ) const { + int pos = 0, tlen; + int64_t hash = getHash(); + + tlen = __int64_t_encode_array( buf, offset + pos, maxlen - pos, &hash, 1 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = this->_encodeNoHash( buf, offset + pos, maxlen - pos ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + return pos; +} + +int motor_ctrl_state_lcmt::decode( const void* buf, int offset, int maxlen ) { + int pos = 0, thislen; + + int64_t msg_hash; + thislen = __int64_t_decode_array( buf, offset + pos, maxlen - pos, &msg_hash, 1 ); + if ( thislen < 0 ) + return thislen; + else + pos += thislen; + if ( msg_hash != getHash() ) + return -1; + + thislen = this->_decodeNoHash( buf, offset + pos, maxlen - pos ); + if ( thislen < 0 ) + return thislen; + else + pos += thislen; + + return pos; +} + +int motor_ctrl_state_lcmt::getEncodedSize() const { + return 8 + _getEncodedSizeNoHash(); +} + +int64_t motor_ctrl_state_lcmt::getHash() { + static int64_t hash = static_cast< int64_t >( _computeHash( NULL ) ); + return hash; +} + +const char* motor_ctrl_state_lcmt::getTypeName() { + return "motor_ctrl_state_lcmt"; +} + +int motor_ctrl_state_lcmt::_encodeNoHash( void* buf, int offset, int maxlen ) const { + int pos = 0, tlen; + + tlen = __int16_t_encode_array( buf, offset + pos, maxlen - pos, &this->err_flag, 1 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->ctrl_topic_interval, 1 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + return pos; +} + +int motor_ctrl_state_lcmt::_decodeNoHash( const void* buf, int offset, int maxlen ) { + int pos = 0, tlen; + + tlen = __int16_t_decode_array( buf, offset + pos, maxlen - pos, &this->err_flag, 1 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->ctrl_topic_interval, 1 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + return pos; +} + +int motor_ctrl_state_lcmt::_getEncodedSizeNoHash() const { + int enc_size = 0; + enc_size += __int16_t_encoded_array_size( NULL, 1 ); + enc_size += __float_encoded_array_size( NULL, 1 ); + return enc_size; +} + +uint64_t motor_ctrl_state_lcmt::_computeHash( const __lcm_hash_ptr* ) { + uint64_t hash = 0x2afdf7bdf6035aeaLL; + return ( hash << 1 ) + ( ( hash >> 63 ) & 1 ); +} + +#endif diff --git a/src/rl_sar/library/cyberdog_motor_sdk/include/state_estimator_lcmt.hpp b/src/rl_sar/library/cyberdog_motor_sdk/include/state_estimator_lcmt.hpp new file mode 100644 index 0000000..a7399b9 --- /dev/null +++ b/src/rl_sar/library/cyberdog_motor_sdk/include/state_estimator_lcmt.hpp @@ -0,0 +1,316 @@ +/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY + * BY HAND!! + * + * Generated by lcm-gen + **/ + +#ifndef __state_estimator_lcmt_hpp__ +#define __state_estimator_lcmt_hpp__ + +#include + +class state_estimator_lcmt { +public: + float p[ 3 ]; + + float vWorld[ 3 ]; + + float vBody[ 3 ]; + + float vRemoter[ 3 ]; + + float rpy[ 3 ]; + + float omegaBody[ 3 ]; + + float omegaWorld[ 3 ]; + + float quat[ 4 ]; + + float aBody[ 3 ]; + + float aWorld[ 3 ]; + + float contactEstimate[ 4 ]; + + int64_t timestamp; + +public: + /** + * Encode a message into binary form. + * + * @param buf The output buffer. + * @param offset Encoding starts at thie byte offset into @p buf. + * @param maxlen Maximum number of bytes to write. This should generally be + * equal to getEncodedSize(). + * @return The number of bytes encoded, or <0 on error. + */ + inline int encode( void* buf, int offset, int maxlen ) const; + + /** + * Check how many bytes are required to encode this message. + */ + inline int getEncodedSize() const; + + /** + * Decode a message from binary form into this instance. + * + * @param buf The buffer containing the encoded message. + * @param offset The byte offset into @p buf where the encoded message starts. + * @param maxlen The maximum number of bytes to read while decoding. + * @return The number of bytes decoded, or <0 if an error occured. + */ + inline int decode( const void* buf, int offset, int maxlen ); + + /** + * Retrieve the 64-bit fingerprint identifying the structure of the message. + * Note that the fingerprint is the same for all instances of the same + * message type, and is a fingerprint on the message type definition, not on + * the message contents. + */ + inline static int64_t getHash(); + + /** + * Returns "state_estimator_lcmt" + */ + inline static const char* getTypeName(); + + // LCM support functions. Users should not call these + inline int _encodeNoHash( void* buf, int offset, int maxlen ) const; + inline int _getEncodedSizeNoHash() const; + inline int _decodeNoHash( const void* buf, int offset, int maxlen ); + inline static uint64_t _computeHash( const __lcm_hash_ptr* p ); +}; + +int state_estimator_lcmt::encode( void* buf, int offset, int maxlen ) const { + int pos = 0, tlen; + int64_t hash = getHash(); + + tlen = __int64_t_encode_array( buf, offset + pos, maxlen - pos, &hash, 1 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = this->_encodeNoHash( buf, offset + pos, maxlen - pos ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + return pos; +} + +int state_estimator_lcmt::decode( const void* buf, int offset, int maxlen ) { + int pos = 0, thislen; + + int64_t msg_hash; + thislen = __int64_t_decode_array( buf, offset + pos, maxlen - pos, &msg_hash, 1 ); + if ( thislen < 0 ) + return thislen; + else + pos += thislen; + if ( msg_hash != getHash() ) + return -1; + + thislen = this->_decodeNoHash( buf, offset + pos, maxlen - pos ); + if ( thislen < 0 ) + return thislen; + else + pos += thislen; + + return pos; +} + +int state_estimator_lcmt::getEncodedSize() const { + return 8 + _getEncodedSizeNoHash(); +} + +int64_t state_estimator_lcmt::getHash() { + static int64_t hash = static_cast< int64_t >( _computeHash( NULL ) ); + return hash; +} + +const char* state_estimator_lcmt::getTypeName() { + return "state_estimator_lcmt"; +} + +int state_estimator_lcmt::_encodeNoHash( void* buf, int offset, int maxlen ) const { + int pos = 0, tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->p[ 0 ], 3 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->vWorld[ 0 ], 3 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->vBody[ 0 ], 3 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->vRemoter[ 0 ], 3 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->rpy[ 0 ], 3 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->omegaBody[ 0 ], 3 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->omegaWorld[ 0 ], 3 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->quat[ 0 ], 4 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->aBody[ 0 ], 3 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->aWorld[ 0 ], 3 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_encode_array( buf, offset + pos, maxlen - pos, &this->contactEstimate[ 0 ], 4 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __int64_t_encode_array( buf, offset + pos, maxlen - pos, &this->timestamp, 1 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + return pos; +} + +int state_estimator_lcmt::_decodeNoHash( const void* buf, int offset, int maxlen ) { + int pos = 0, tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->p[ 0 ], 3 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->vWorld[ 0 ], 3 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->vBody[ 0 ], 3 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->vRemoter[ 0 ], 3 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->rpy[ 0 ], 3 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->omegaBody[ 0 ], 3 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->omegaWorld[ 0 ], 3 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->quat[ 0 ], 4 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->aBody[ 0 ], 3 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->aWorld[ 0 ], 3 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __float_decode_array( buf, offset + pos, maxlen - pos, &this->contactEstimate[ 0 ], 4 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + tlen = __int64_t_decode_array( buf, offset + pos, maxlen - pos, &this->timestamp, 1 ); + if ( tlen < 0 ) + return tlen; + else + pos += tlen; + + return pos; +} + +int state_estimator_lcmt::_getEncodedSizeNoHash() const { + int enc_size = 0; + enc_size += __float_encoded_array_size( NULL, 3 ); + enc_size += __float_encoded_array_size( NULL, 3 ); + enc_size += __float_encoded_array_size( NULL, 3 ); + enc_size += __float_encoded_array_size( NULL, 3 ); + enc_size += __float_encoded_array_size( NULL, 3 ); + enc_size += __float_encoded_array_size( NULL, 3 ); + enc_size += __float_encoded_array_size( NULL, 3 ); + enc_size += __float_encoded_array_size( NULL, 4 ); + enc_size += __float_encoded_array_size( NULL, 3 ); + enc_size += __float_encoded_array_size( NULL, 3 ); + enc_size += __float_encoded_array_size( NULL, 4 ); + enc_size += __int64_t_encoded_array_size( NULL, 1 ); + return enc_size; +} + +uint64_t state_estimator_lcmt::_computeHash( const __lcm_hash_ptr* ) { + uint64_t hash = 0xeb390c8b8230c53fLL; + return ( hash << 1 ) + ( ( hash >> 63 ) & 1 ); +} + +#endif diff --git a/src/rl_sar/library/cyberdog_motor_sdk/src/CustomInterface.cpp b/src/rl_sar/library/cyberdog_motor_sdk/src/CustomInterface.cpp new file mode 100644 index 0000000..df343f4 --- /dev/null +++ b/src/rl_sar/library/cyberdog_motor_sdk/src/CustomInterface.cpp @@ -0,0 +1,171 @@ +// Copyright (c) 2021 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +std::string CustomInterface::getLcmUrl_port(int64_t port, int64_t ttl) +{ + assert(ttl >= 0 && ttl <= 255); + return "udpm://239.255.76.67:" + std::to_string(port) + "?ttl=" + std::to_string(ttl); +} + +CustomInterface::CustomInterface(const double &loop_rate) + : _motor_data_Lcm(getLcmUrl_port(7667, 255)), _motor_ctrl_state_Lcm(getLcmUrl_port(7667, 255)), + _robot_state_Lcm(getLcmUrl_port(7669, 255)), + _motor_ctrl_Lcm(getLcmUrl_port(7667, 255)) +{ + + running_ = true; + all_thread_done_ = false; + mode_state = false; + + if(loop_rate > 0) + { + dt_ = 1.0 / loop_rate; + } + else + { + std::cout << "Loop rate should be more than zero! Set to default 500Hz." << std::endl; + dt_ = 1.0 / 500; + } + + _motor_ctrl_state_Lcm.subscribe("motor_ctrl_state", &CustomInterface::handle_motor_ctrl_state_LCM, this); + _motor_data_Lcm.subscribe("leg_control_data", &CustomInterface::handle_motor_data_LCM, this); + _robot_state_Lcm.subscribe("state_estimator", &CustomInterface::handle_robot_state_LCM, this); + + _motor_ctrl_state_LcmThread = std::thread(&CustomInterface::motor_ctrl_state_LcmThread, this); + _motor_data_LcmThread = std::thread(&CustomInterface::motor_data_LcmThread, this); + _robot_state_LcmThread = std::thread(&CustomInterface::robot_state_LcmThread, this); + _user_code_ControlThread = std::thread(&CustomInterface::Control, this); +} + +void CustomInterface::Control() +{ + auto timerFd = timerfd_create(CLOCK_MONOTONIC, 0); + int seconds = (int) dt_; + int nanoseconds = (int) (1e9 * std::fmod(dt_, 1.f)); + int err_count = 0; + + itimerspec timerSpec; + timerSpec.it_interval.tv_sec = seconds; + timerSpec.it_value.tv_sec = seconds; + timerSpec.it_value.tv_nsec = nanoseconds; + timerSpec.it_interval.tv_nsec = nanoseconds; + + timerfd_settime(timerFd, 0, &timerSpec, nullptr); + unsigned long long missed = 0; + while(running_) + { + if(!mode_state) + { + sleep(1); + std::cout << "Motor control mode has not been activated successfully" << std::endl; + continue; + } + + UserCode(); + + if(robot_data.err_flag & 0x02) + { + if(err_count++ % 1000 == 0) + { + printf("\nErr: 0x02 Communicate lost over 500ms!\n"); + printf("The motor cmd has been disabled!!!\n"); + printf("Cyberdog locomotion ctrl process need restart after fix the communication!\n\n"); + } + } + else + { + motor_cmd_send(); + } + + int m = read(timerFd, &missed, sizeof(missed)); + (void) m; + } +} + +void CustomInterface::Spin() +{ + while(!all_thread_done_) + { + sleep(1.0); + } + + printf("~ Exit ~\n"); +} + +void CustomInterface::Stop() +{ + running_ = false; + _motor_ctrl_state_LcmThread.join(); + _motor_data_LcmThread.join(); + _robot_state_LcmThread.join(); + _user_code_ControlThread.join(); + all_thread_done_ = true; +} + +void CustomInterface::motor_cmd_send() +{ + int sig[12] = {1, -1, -1, 1, -1, -1, 1, -1, -1, 1, -1, -1}; + for(int i = 0; i < 12; i++) + { + _motor_ctrl.q_des[i] = motor_cmd.q_des[i] * sig[i]; + _motor_ctrl.qd_des[i] = motor_cmd.qd_des[i] * sig[i]; + _motor_ctrl.kp_des[i] = motor_cmd.kp_des[i]; + _motor_ctrl.kd_des[i] = motor_cmd.kd_des[i]; + _motor_ctrl.tau_des[i] = motor_cmd.tau_des[i] * sig[i]; + } + _motor_ctrl_Lcm.publish("motor_ctrl", &_motor_ctrl); +} + +void CustomInterface::handle_motor_ctrl_state_LCM(const lcm::ReceiveBuffer *rbuf, const std::string &chan, + const motor_ctrl_state_lcmt *msg) +{ + (void) rbuf; + (void) chan; + robot_data.err_flag = msg->err_flag; + robot_data.ctrl_topic_interval = msg->ctrl_topic_interval; + mode_state = true; +} + +void CustomInterface::handle_motor_data_LCM(const lcm::ReceiveBuffer *rbuf, const std::string &chan, + const leg_control_data_lcmt *msg) +{ + (void) rbuf; + (void) chan; + int sig[12] = {1, -1, -1, 1, -1, -1, 1, -1, -1, 1, -1, -1}; + for(int i = 0; i < 12; i++) + { + robot_data.q[i] = msg->q[i] * sig[i]; + robot_data.qd[i] = msg->qd[i] * sig[i]; + robot_data.tau[i] = msg->tau_est[i] * sig[i]; + } +} + +void CustomInterface::handle_robot_state_LCM(const lcm::ReceiveBuffer *rbuf, const std::string &chan, + const state_estimator_lcmt *msg) +{ + (void) rbuf; + (void) chan; + for(int i = 0; i < 3; i++) + { + robot_data.omega[i] = msg->omegaWorld[i]; + robot_data.rpy[i] = msg->rpy[i]; + robot_data.acc[i] = msg->aWorld[i]; + } + for(int i = 0; i < 4; i++) + { + robot_data.quat[i] = msg->quat[i]; + } +} \ No newline at end of file diff --git a/src/rl_sar/library/cyberdog_motor_sdk/src/Example_MotorCtrl.cpp b/src/rl_sar/library/cyberdog_motor_sdk/src/Example_MotorCtrl.cpp new file mode 100644 index 0000000..fffd411 --- /dev/null +++ b/src/rl_sar/library/cyberdog_motor_sdk/src/Example_MotorCtrl.cpp @@ -0,0 +1,115 @@ +// Copyright (c) 2021 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +class CustomCtrl : public CustomInterface +{ +public: + CustomCtrl(const double &loop_rate) + : CustomInterface(loop_rate) {}; + + ~CustomCtrl() {}; + +private: + bool first_run = true; + long long count = 0; + float init_q[12]; + float target1_q[3] = {0 / 57.3, 80 / 57.3, -135 / 57.3}; + float target2_q[3] = {0 / 57.3, 45 / 57.3, -90 / 57.3}; + + void UserCode() + { + // vertical view + // leg 1 | | leg 0 + // | | + // leg 3 | | leg 2 + // + // Right hand coordinate system + // ______ zero angle ______ + // \ \ --> | | + // / / | | + float t = (count / 1500.0) > 2 ? 2 : (count / 1500.0); + if(first_run == true) + { + for(int i = 0; i < 12; i++) + init_q[i] = robot_data.q[i]; + if(init_q[2] > 0.1 && init_q[5] > 0.1 && init_q[8] > 0.1 && init_q[11] > 0.1) + { + first_run = false; + count = 0; + } + } + else + { + for(int i = 0; i < 12; i++) + { + if(t < 1.0) + motor_cmd.q_des[i] = target1_q[i % 3] * t + init_q[i] * (1 - t); + else + motor_cmd.q_des[i] = target2_q[i % 3] * (t - 1) + target1_q[i % 3] * (2 - t); + motor_cmd.kp_des[i] = 100; + motor_cmd.kd_des[i] = 2; + motor_cmd.qd_des[i] = 0; + motor_cmd.tau_des[i] = 0; + } + } + if((count++) % 1000 == 0) + { + printf("interval:---------%.4f-------------\n", robot_data.ctrl_topic_interval); + printf("rpy [3]:"); + for(int i = 0; i < 3; i++) + printf(" %.2f", robot_data.rpy[i]); + printf("\nacc [3]:"); + for(int i = 0; i < 3; i++) + printf(" %.2f", robot_data.acc[i]); + printf("\nquat[4]:"); + for(int i = 0; i < 4; i++) + printf(" %.2f", robot_data.quat[i]); + printf("\nomeg[3]:"); + for(int i = 0; i < 3; i++) + printf(" %.2f", robot_data.omega[i]); + printf("\nq [12]:"); + for(int i = 0; i < 12; i++) + printf(" %.2f", robot_data.q[i]); + printf("\nqd [12]:"); + for(int i = 0; i < 12; i++) + printf(" %.2f", robot_data.qd[i]); + printf("\ntau[12]:"); + for(int i = 0; i < 12; i++) + printf(" %.2f", robot_data.tau[i]); + printf("\nctrl[12]:"); + for(int i = 0; i < 12; i++) + printf(" %.2f", motor_cmd.q_des[i]); + printf("\n\n"); + } + } +}; + +std::shared_ptr io; + +void signal_callback_handler(int signum) +{ + io->Stop(); + (void) signum; +} + +int main() +{ + /* user code ctrl mode:1 for motor ctrl */ + signal(SIGINT, signal_callback_handler); + io = std::make_shared(500); + io->Spin(); + return 0; +} \ No newline at end of file diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp index 323737c..9528332 100644 --- a/src/rl_sar/src/rl_sim.cpp +++ b/src/rl_sar/src/rl_sim.cpp @@ -58,7 +58,7 @@ RL_Sim::RL_Sim() cmd_vel_subscriber_ = nh.subscribe("/cmd_vel", 10, &RL_Sim::CmdvelCallback, this); - std::string ros_namespace = "/a1_gazebo/"; + nh.param("ros_namespace", ros_namespace, ""); joint_names = { "FL_hip_joint", "FL_thigh_joint", "FL_calf_joint", @@ -77,7 +77,7 @@ RL_Sim::RL_Sim() "/gazebo/model_states", 10, &RL_Sim::ModelStatesCallback, this); joint_state_subscriber_ = nh.subscribe( - "/a1_gazebo/joint_states", 10, &RL_Sim::JointStatesCallback, this); + ros_namespace + "joint_states", 10, &RL_Sim::JointStatesCallback, this); loop_control = std::make_shared("loop_control", 0.002, boost::bind(&RL_Sim::RobotControl, this)); loop_rl = std::make_shared("loop_rl" , 0.02 , boost::bind(&RL_Sim::RunModel, this)); diff --git a/src/robots/cyberdog_description/CMakeLists.txt b/src/robots/cyberdog_description/CMakeLists.txt new file mode 100644 index 0000000..57d84b9 --- /dev/null +++ b/src/robots/cyberdog_description/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 2.8.3) +project(cyberdog_description) + +find_package(catkin REQUIRED COMPONENTS + genmsg + roscpp + std_msgs + tf +) + +catkin_package( + CATKIN_DEPENDS +) + +include_directories( + # include + ${Boost_INCLUDE_DIR} + ${catkin_INCLUDE_DIRS} +) diff --git a/src/robots/cyberdog_description/config/joint_names_cyberdog_urdf_test.yaml b/src/robots/cyberdog_description/config/joint_names_cyberdog_urdf_test.yaml new file mode 100644 index 0000000..5e625d2 --- /dev/null +++ b/src/robots/cyberdog_description/config/joint_names_cyberdog_urdf_test.yaml @@ -0,0 +1 @@ +controller_joint_names: ['joint_abad_bl', 'joint_abad_fr', 'joint_hip_fr', 'joint_knee_fr', 'joint_abad_fl', 'joint_hip_fl', 'joint_knee_fl', 'joint_abad_br', 'joint_hip_br', 'joint_knee_br', 'joint_abad_bl', 'joint_hip_bl', 'joint_knee_bl', ] diff --git a/src/robots/cyberdog_description/config/robot_control.yaml b/src/robots/cyberdog_description/config/robot_control.yaml new file mode 100644 index 0000000..0981d25 --- /dev/null +++ b/src/robots/cyberdog_description/config/robot_control.yaml @@ -0,0 +1,70 @@ +cyberdog_gazebo: + # Publish all joint states ----------------------------------- + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 1000 + + # FL Controllers --------------------------------------- + FL_hip_controller: + type: unitree_legged_control/UnitreeJointController + joint: FL_hip_joint + pid: {p: 100.0, i: 0.0, d: 5.0} + + FL_thigh_controller: + type: unitree_legged_control/UnitreeJointController + joint: FL_thigh_joint + pid: {p: 300.0, i: 0.0, d: 8.0} + + FL_calf_controller: + type: unitree_legged_control/UnitreeJointController + joint: FL_calf_joint + pid: {p: 300.0, i: 0.0, d: 8.0} + + # FR Controllers --------------------------------------- + FR_hip_controller: + type: unitree_legged_control/UnitreeJointController + joint: FR_hip_joint + pid: {p: 100.0, i: 0.0, d: 5.0} + + FR_thigh_controller: + type: unitree_legged_control/UnitreeJointController + joint: FR_thigh_joint + pid: {p: 300.0, i: 0.0, d: 8.0} + + FR_calf_controller: + type: unitree_legged_control/UnitreeJointController + joint: FR_calf_joint + pid: {p: 300.0, i: 0.0, d: 8.0} + + # RL Controllers --------------------------------------- + RL_hip_controller: + type: unitree_legged_control/UnitreeJointController + joint: RL_hip_joint + pid: {p: 100.0, i: 0.0, d: 5.0} + + RL_thigh_controller: + type: unitree_legged_control/UnitreeJointController + joint: RL_thigh_joint + pid: {p: 300.0, i: 0.0, d: 8.0} + + RL_calf_controller: + type: unitree_legged_control/UnitreeJointController + joint: RL_calf_joint + pid: {p: 300.0, i: 0.0, d: 8.0} + + # RR Controllers --------------------------------------- + RR_hip_controller: + type: unitree_legged_control/UnitreeJointController + joint: RR_hip_joint + pid: {p: 100.0, i: 0.0, d: 5.0} + + RR_thigh_controller: + type: unitree_legged_control/UnitreeJointController + joint: RR_thigh_joint + pid: {p: 300.0, i: 0.0, d: 8.0} + + RR_calf_controller: + type: unitree_legged_control/UnitreeJointController + joint: RR_calf_joint + pid: {p: 300.0, i: 0.0, d: 8.0} + diff --git a/src/robots/cyberdog_description/launch/display.launch b/src/robots/cyberdog_description/launch/display.launch new file mode 100644 index 0000000..e2e84bf --- /dev/null +++ b/src/robots/cyberdog_description/launch/display.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/robots/cyberdog_description/launch/gazebo.launch b/src/robots/cyberdog_description/launch/gazebo.launch new file mode 100644 index 0000000..58f1bfc --- /dev/null +++ b/src/robots/cyberdog_description/launch/gazebo.launch @@ -0,0 +1,19 @@ + + + + + + \ No newline at end of file diff --git a/src/robots/cyberdog_description/launch/test.rviz b/src/robots/cyberdog_description/launch/test.rviz new file mode 100644 index 0000000..fe255bb --- /dev/null +++ b/src/robots/cyberdog_description/launch/test.rviz @@ -0,0 +1,245 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 555 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + FL_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RL_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + trunk: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: trunk + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.3364639282226562 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.16539815068244934 + Target Frame: + Yaw: 1.430397391319275 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000030700fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 739 + Y: -14 diff --git a/src/robots/cyberdog_description/meshes/body.STL b/src/robots/cyberdog_description/meshes/body.STL new file mode 100644 index 0000000..dd2254e Binary files /dev/null and b/src/robots/cyberdog_description/meshes/body.STL differ diff --git a/src/robots/cyberdog_description/meshes/link_abad_bl.STL b/src/robots/cyberdog_description/meshes/link_abad_bl.STL new file mode 100644 index 0000000..c9701e7 Binary files /dev/null and b/src/robots/cyberdog_description/meshes/link_abad_bl.STL differ diff --git a/src/robots/cyberdog_description/meshes/link_abad_br.STL b/src/robots/cyberdog_description/meshes/link_abad_br.STL new file mode 100644 index 0000000..6dbf0a1 Binary files /dev/null and b/src/robots/cyberdog_description/meshes/link_abad_br.STL differ diff --git a/src/robots/cyberdog_description/meshes/link_abad_fl.STL b/src/robots/cyberdog_description/meshes/link_abad_fl.STL new file mode 100644 index 0000000..c9701e7 Binary files /dev/null and b/src/robots/cyberdog_description/meshes/link_abad_fl.STL differ diff --git a/src/robots/cyberdog_description/meshes/link_abad_fr.STL b/src/robots/cyberdog_description/meshes/link_abad_fr.STL new file mode 100644 index 0000000..ca0aabe Binary files /dev/null and b/src/robots/cyberdog_description/meshes/link_abad_fr.STL differ diff --git a/src/robots/cyberdog_description/meshes/link_hip_bl.STL b/src/robots/cyberdog_description/meshes/link_hip_bl.STL new file mode 100644 index 0000000..fc867b4 Binary files /dev/null and b/src/robots/cyberdog_description/meshes/link_hip_bl.STL differ diff --git a/src/robots/cyberdog_description/meshes/link_hip_br.STL b/src/robots/cyberdog_description/meshes/link_hip_br.STL new file mode 100644 index 0000000..9ae1784 Binary files /dev/null and b/src/robots/cyberdog_description/meshes/link_hip_br.STL differ diff --git a/src/robots/cyberdog_description/meshes/link_hip_fl.STL b/src/robots/cyberdog_description/meshes/link_hip_fl.STL new file mode 100644 index 0000000..9ae1784 Binary files /dev/null and b/src/robots/cyberdog_description/meshes/link_hip_fl.STL differ diff --git a/src/robots/cyberdog_description/meshes/link_hip_fr.STL b/src/robots/cyberdog_description/meshes/link_hip_fr.STL new file mode 100644 index 0000000..9ae1784 Binary files /dev/null and b/src/robots/cyberdog_description/meshes/link_hip_fr.STL differ diff --git a/src/robots/cyberdog_description/meshes/link_knee_bl.STL b/src/robots/cyberdog_description/meshes/link_knee_bl.STL new file mode 100644 index 0000000..025e006 Binary files /dev/null and b/src/robots/cyberdog_description/meshes/link_knee_bl.STL differ diff --git a/src/robots/cyberdog_description/meshes/link_knee_br.STL b/src/robots/cyberdog_description/meshes/link_knee_br.STL new file mode 100644 index 0000000..025e006 Binary files /dev/null and b/src/robots/cyberdog_description/meshes/link_knee_br.STL differ diff --git a/src/robots/cyberdog_description/meshes/link_knee_fl.STL b/src/robots/cyberdog_description/meshes/link_knee_fl.STL new file mode 100644 index 0000000..5d505f1 Binary files /dev/null and b/src/robots/cyberdog_description/meshes/link_knee_fl.STL differ diff --git a/src/robots/cyberdog_description/meshes/link_knee_fr.STL b/src/robots/cyberdog_description/meshes/link_knee_fr.STL new file mode 100644 index 0000000..025e006 Binary files /dev/null and b/src/robots/cyberdog_description/meshes/link_knee_fr.STL differ diff --git a/src/robots/cyberdog_description/package.xml b/src/robots/cyberdog_description/package.xml new file mode 100644 index 0000000..9127a6e --- /dev/null +++ b/src/robots/cyberdog_description/package.xml @@ -0,0 +1,19 @@ + + + cyberdog_description + 1.0.0 + The cyberdog_description package + TODO + + BSD + catkin + roscpp + std_msgs + + roslaunch + robot_state_publisher + rviz + joint_state_publisher_gui + gazebo + + \ No newline at end of file diff --git a/src/robots/cyberdog_description/urdf/cyberdog_description.csv b/src/robots/cyberdog_description/urdf/cyberdog_description.csv new file mode 100644 index 0000000..ce6ed29 --- /dev/null +++ b/src/robots/cyberdog_description/urdf/cyberdog_description.csv @@ -0,0 +1,14 @@ +Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity +body,0,0,0,0,0,0,7.18,0.032051,-0.00023217,0.002728,0.13707,5.6623E-05,0.14946,0,0,0,0,0,0,package://cyberdog_description/meshes/body.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/body.STL,,body^Cyberdog-1;cyber_dog_body-1,coor_body,axis_left_abad,joint_abad_bl,revolute,-0.23536,0.05,0,0,0,0,body,1,0,0,17,12,-0.75,0.75,,,0.01,0.2,,,, +link_abad_fr,0,-0.053575,0,0,0,0,0.509,0.00038097,1.2523E-05,-1.1653E-05,0.00069383,5.071E-06,0.00047336,0,0,0,0,0,0,package://cyberdog_description/meshes/link_abad_fr.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_abad_fr.STL,,abad_link^Cyberdog-8,coor_leg0_abad,axis_right_abad,joint_abad_fr,revolute,0.23536,-0.05,0,0,0,0,body,1,0,0,17,12,-0.75,0.75,,,0.01,0.2,,,, +link_hip_fr,0,0,-0.1,0,0,0,0.664,0.0033376,-7.1504E-07,-0.00019282,0.0026385,-9.3033E-06,0.0013093,0,0,0,0,0,0,package://cyberdog_description/meshes/link_hip_fr.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_hip_fr.STL,,hip_link^Cyberdog-6,coor_leg0_hip,axis_leg0_hip,joint_hip_fr,revolute,0,-0.10715,0,0,0,0,link_abad_fr,0,-1,0,24,12,4.363,-1.257,,,0.01,0.2,,,, +link_knee_fr,0,0,-0.11439,0,0,0,0.114,0.0014553,-3.2376E-08,8.3885E-05,0.0021522,5.1259E-07,0.00070545,0,0,0,0,0,0,package://cyberdog_description/meshes/link_knee_fr.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_knee_fr.STL,,knee_link^Cyberdog-6,coor_leg0_knee,axis_leg0_knee,joint_knee_fr,revolute,0,0,-0.2,0,0,0,link_hip_fr,0,-1,0,24,12,-0.506,-2.478,,,0.01,0.2,,,, +link_abad_fl,0,0.053575,0,0,0,0,0.509,0.00038097,1.2523E-05,-1.1653E-05,0.00069383,5.071E-06,0.00047336,0,0,0,0,0,0,package://cyberdog_description/meshes/link_abad_fl.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_abad_fl.STL,,abad_link^Cyberdog-4,coor_leg1_abad,axis_left_abad,joint_abad_fl,revolute,0.23536,0.05,0,0,0,0,body,1,0,0,17,12,-0.75,0.75,,,0.01,0.2,,,, +link_hip_fl,0,0,-0.1,0,0,0,0.664,0.0033376,-7.150375E-07,-0.00019282,0.0026385,-9.3033E-06,0.0013093,0,0,0,0,0,0,package://cyberdog_description/meshes/link_hip_fl.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_hip_fl.STL,,hip_link^Cyberdog-2,coor_leg1_hip,axis_leg1_hip,joint_hip_fl,revolute,0,0.10715,0,0,0,0,link_abad_fl,0,-1,0,24,12,4.363,-1.257,,,0.01,0.2,,,, +link_knee_fl,0,0,-0.11439,0,0,0,0.114,0.0014553,-3.2376E-08,8.3885E-05,0.0021522,5.1259E-07,0.00070545,0,0,0,0,0,0,package://cyberdog_description/meshes/link_knee_fl.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_knee_fl.STL,,knee_link^Cyberdog-2,coor_leg1_knee,axis_leg1_knee,joint_knee_fl,revolute,0,0,-0.2,0,0,0,link_hip_fl,0,-1,0,24,12,-0.506,-2.478,,,0.01,0.2,,,, +link_abad_br,0,-0.053575,0,0,0,0,0.509,0.00038097,1.2523E-05,-1.1653E-05,0.00069383,5.071E-06,0.00047336,0,0,0,0,0,0,package://cyberdog_description/meshes/link_abad_br.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_abad_br.STL,,abad_link^Cyberdog-9,coor_leg2_abad,axis_right_abad,joint_abad_br,revolute,-0.23536,-0.05,0,0,0,0,body,1,0,0,17,12,-0.75,0.75,,,0.01,0.2,,,, +link_hip_br,0,0,-0.1,0,0,0,0.664,0.0033376,-7.150375E-07,-0.00019282,0.0026385,-9.3033E-06,0.0013093,0,0,0,0,0,0,package://cyberdog_description/meshes/link_hip_br.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_hip_br.STL,,hip_link^Cyberdog-7,coor_leg2_hip,axis_leg2_hip,joint_hip_br,revolute,0,-0.10715,0,0,0,0,link_abad_br,0,-1,0,24,12,3.49,-2.01,,,0.01,0.2,,,, +link_knee_br,0,0,-0.11439,0,0,0,0.114,0.0014553,-3.2376E-08,8.3885E-05,0.0021522,5.1259E-07,0.00070545,0,0,0,0,0,0,package://cyberdog_description/meshes/link_knee_br.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_knee_br.STL,,knee_link^Cyberdog-7,coor_leg2_knee,axis_leg2_knee,joint_knee_br,revolute,0,0,-0.2,0,0,0,link_hip_br,0,-1,0,24,12,-0.506,-2.478,,,0.01,0.2,,,, +link_abad_bl,0,0.053575,0,0,0,0,0.509,0.00038097,1.2523E-05,-1.1653E-05,0.00069383,5.071E-06,0.00047336,0,0,0,0,0,0,package://cyberdog_description/meshes/link_abad_bl.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_abad_bl.STL,,abad_link^Cyberdog-5,coor_leg3_abad,axis_left_abad,joint_abad_bl,revolute,-0.23536,0.05,0,0,0,0,body,1,0,0,17,12,-0.75,0.75,,,0.01,0.2,,,, +link_hip_bl,0,0,-0.1,0,0,0,0.664,0.0033376,-7.1504E-07,-0.00019282,0.0026385,-9.3033E-06,0.0013093,0,0,0,0,0,0,package://cyberdog_description/meshes/link_hip_bl.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_hip_bl.STL,,hip_link^Cyberdog-3,coor_leg3_hip,axis_leg3_hip,joint_hip_bl,revolute,0,0.10715,0,0,0,0,link_abad_bl,0,-1,0,24,12,3.49,-2.01,,,0.01,0.2,,,, +link_knee_bl,0,0,-0.11439,0,0,0,0.114,0.0014553,-3.2376E-08,8.3885E-05,0.0021522,5.1259E-07,0.00070545,0,0,0,0,0,0,package://cyberdog_description/meshes/link_knee_bl.STL,1,1,1,1,0,0,0,0,0,0,package://cyberdog_description/meshes/link_knee_bl.STL,,knee_link^Cyberdog-3,coor_leg3_knee,axis_leg3_knee,joint_knee_bl,revolute,0,0,-0.2,0,0,0,link_hip_bl,0,-1,0,24,12,-0.506,-2.478,,,0.01,0.2,,,, diff --git a/src/robots/cyberdog_description/urdf/cyberdog_description.urdf b/src/robots/cyberdog_description/urdf/cyberdog_description.urdf new file mode 100644 index 0000000..4d7c2ff --- /dev/null +++ b/src/robots/cyberdog_description/urdf/cyberdog_description.urdf @@ -0,0 +1,1207 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + /cyberdog_gazebo + gazebo_ros_control/DefaultRobotHWSim + + + + + + 10 + + base + 0 0 0 0 0 0 + Gazebo/Yellow + + + + + + + 1000 + + FR_calf + 0 0 -0.217 0 0 0 + Gazebo/Yellow + + + + + + trunk + /apply_force/trunk + + + + true + + true + 1000 + true + __default_topic__ + + trunk_imu + imu_link + 1000.0 + 0.0 + 0 0 0 + 0 0 0 + imu_link + + 0 0 0 0 0 0 + + + + + + 100 + + + FR_calf_fixed_joint_lump__FR_foot_collision_1 + + + + + + 100 + + + FL_calf_fixed_joint_lump__FL_foot_collision_1 + + + + + + 100 + + + RR_calf_fixed_joint_lump__RR_foot_collision_1 + + + + + + 100 + + + RL_calf_fixed_joint_lump__RL_foot_collision_1 + + + + + + + + FR_foot_contact + + + + + + + FL_foot_contact + + + + + + + RR_foot_contact + + + + + + + RL_foot_contact + + + + + Gazebo/Green + false + + + 0.2 + 0.2 + + + + + 0.2 + 0.2 + Gazebo/Red + + + + 0.2 + 0.2 + Gazebo/DarkGrey + + + 0.2 + 0.2 + 1 + Gazebo/DarkGrey + + + + + 0.2 + 0.2 + 1 + + + 0.9 + 0.9 + 1 + Gazebo/DarkGrey + + + + + + 0.2 + 0.2 + Gazebo/DarkGrey + + + 0.2 + 0.2 + 1 + Gazebo/DarkGrey + + + + + 0.2 + 0.2 + 1 + + + 0.9 + 0.9 + 1 + Gazebo/DarkGrey + + + + + + 0.2 + 0.2 + Gazebo/DarkGrey + + + 0.2 + 0.2 + 1 + Gazebo/DarkGrey + + + + + 0.2 + 0.2 + 1 + + + 0.9 + 0.9 + 1 + Gazebo/DarkGrey + + + + + + 0.2 + 0.2 + Gazebo/DarkGrey + + + 0.2 + 0.2 + 1 + Gazebo/DarkGrey + + + + + 0.2 + 0.2 + 1 + + + 0.9 + 0.9 + 1 + Gazebo/DarkGrey + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + \ No newline at end of file