EmbodiedAI
/
rl_sar
mirror of https://github.com/fan-ziqi/rl_sar.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

feat: [Destructive Update] NEW VERSISON

This commit is contained in:
fan-ziqi 2024-05-24 16:24:14 +08:00
parent 75a943e9d8
commit 174a2686f7
69 changed files with 682 additions and 11172 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
README.md
View File

@ -75,21 +75,10 @@ Open a new terminal, launch the gazebo simulation environment
```bash ```bash
source devel/setup.bash source devel/setup.bash
roslaunch rl_sar start_a1.launch roslaunch rl_sar gazebo_a1.launch
``` ```
Open a new terminal, run the control program Press **0** on the keyboard to switch the robot to the default standing position, press **P** to switch to RL control mode, and press **1** in any state to switch to the initial lying position. WS controls x-axis, AD controls yaw, and JL controls y-axis.
```bash
source devel/setup.bash
rosrun rl_sar rl_sim
```
Open a new terminal, run the keyboard control program
```bash
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
```
### Physical Robots ### Physical Robots
@ -109,61 +98,7 @@ rosrun rl_sar rl_real_a1
Press the **R2** button on the controller to switch the robot to the default standing position, press **R1** to switch to RL control mode, and press **L2** in any state to switch to the initial lying position. The left stick controls x-axis up and down, controls yaw left and right, and the right stick controls y-axis left and right. Press the **R2** button on the controller to switch the robot to the default standing position, press **R1** to switch to RL control mode, and press **L2** in any state to switch to the initial lying position. The left stick controls x-axis up and down, controls yaw left and right, and the right stick controls y-axis left and right.
#### Cyberdog1 OR Press **0** on the keyboard to switch the robot to the default standing position, press **P** to switch to RL control mode, and press **1** in any state to switch to the initial lying position. WS controls x-axis, AD controls yaw, and JL controls y-axis.
1. Connect to the robot (only need to do this once)
Connect the local PC to the Cyberdog's USB download Type-C interface (located in the middle) and wait for the "L4T-README" pop-up to appear.
```bash
ping 192.168.55.100 # IP assigned to the local PC
ssh mi@192.168.55.1 # Log in to the NX application board, password 123
athena_version -v # Verify the current version is >=1.0.0.94
```
2. Enter motor control mode (only need to do this once)
Modify the configuration switch to activate user control mode and run the user's own controller:
```bash
ssh root@192.168.55.233 # Log in to the motion control board
cd /robot
./initialize.sh # Copy factory code to the readable and writable development area (/mnt/UDISK/robot-software), switch to developer mode, only need to be executed once
vi /mnt/UDISK/robot-software/config/user_code_ctrl_mode.txt # Switch mode: 1 (0: default mode, 1 user code control motor mode), reboot the robot to take effect
```
3. Use Ethernet cable to connect the computer and the motion control board
Due to the risk of damaging the interface and higher communication latency when using a Type-C connection, it is recommended to use an Ethernet cable for connection. Disconnect the cables between the main control and motion control board of the robot, and connect the computer and the motion control board directly with an Ethernet cable, then set the wired connection IPv4 of the computer to manual ` 192.168.55.100`. It is recommended to remove the head and lead the cable out of the head opening. Be careful not to damage the cables during disassembly and assembly.
Initialize the robot's connection (this step needs to be done every time the robot is reconnected)
```bash
cd src/rl_sar/scripts
bash init_cyberdog.sh
```
Start the control program
```bash
source devel/setup.bash
rosrun rl_sar rl_real_cyberdog
```
Press **0** on the keyboard to switch the robot to the default standing position, press **P** to switch to RL control mode, and press **1** in any state to switch to the initial lying position. WS controls x-axis, AD controls yaw, and JL controls y-axis.
4. Use a Type-C cable to connect the computer and the robot
If it is inconvenient to disassemble the robot, a Type-C cable can be temporarily used for debugging. The procedure after connecting the Type-C cable is the same as above.
5. After using Ctrl+C to end the program, the robot's motion control program will automatically reset. If the program goes out of control, the motion control program can also be manually restarted.
Note: After restarting the motion control program, there is a startup time of approximately 5-10 seconds. During this time, running programs may report `Motor control mode has not been activated successfully`. Wait until there are no errors before running the control program again.
```bash
cd src/rl_sar/scripts
bash kill_cyberdog.sh
```
## Add Your Robot ## Add Your Robot

71
README_CN.md
View File

@ -75,21 +75,10 @@ catkin build
```bash ```bash
source devel/setup.bash source devel/setup.bash
roslaunch rl_sar start_a1.launch roslaunch rl_sar gazebo_a1.launch
``` ```
新建终端，启动控制程序 按下键盘上的**0**键让机器人切换到默认站起姿态，按下**P**键切换到RL控制模式，任意状态按下**1**键切换到最初的趴下姿态。WS控制x，AD控制yaw，JL控制y。
```bash
source devel/setup.bash
rosrun rl_sar rl_sim
```
新建终端，键盘控制程序
```bash
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
```
### 实物 ### 实物
@ -109,61 +98,7 @@ rosrun rl_sar rl_real_a1
按下遥控器的**R2**键让机器人切换到默认站起姿态，按下**R1**键切换到RL控制模式，任意状态按下**L2**切换到最初的趴下姿态。左摇杆上下控制x左右控制yaw，右摇杆左右控制y。 按下遥控器的**R2**键让机器人切换到默认站起姿态，按下**R1**键切换到RL控制模式，任意状态按下**L2**切换到最初的趴下姿态。左摇杆上下控制x左右控制yaw，右摇杆左右控制y。
#### Cyberdog1 OR 按下键盘上的**0**键让机器人切换到默认站起姿态，按下**P**键切换到RL控制模式，任意状态按下**1**键切换到最初的趴下姿态。WS控制x，AD控制yaw，JL控制y。
1. 连接机器人(只需执行一次此步骤)
将本地PC连接至铁蛋的USB download type-c 接口(位于中间)，等待出现”L4T-README” 弹窗
```bash
ping 192.168.55.100 #本地PC被分配的ip
ssh mi@192.168.55.1 #登录nx应用板 ,密码123
athena_version -v #核对当前版本>=1.0.0.94
```
2. 进入电机控制模式(只需执行一次此步骤)
修改配置开关，激活用户控制模式，运行用户自己的控制器：
```bash
ssh root@192.168.55.233 #登录运动控制板
cd /robot
./initialize.sh #拷贝出厂代码到可读写的开发区（/mnt/UDISK/robot-software），切换到开发者模式，仅需执行一次
vi /mnt/UDISK/robot-software/config/user_code_ctrl_mode.txt #切换mode:1(0:默认模式，1用户代码控制电机模式),重启机器人生效
```
3. 使用网线连接电脑和运动控制板
由于使用Type-C连接时调试碰撞易损坏接口，而且通信延迟较高，故推荐使用网线进行连接。需要将机器人拆开，断开断开主控和运动控制板的网线，将电脑和运动控制板使用网线直接连接，并设置电脑的有线连接IPv4为手动`192.168.55.100`。推荐拆掉头部并将网线从头部的开口引出。拆装时候注意不要损坏排线。
初始化机器人的连接(每次重新连接机器人都要执行此步骤)
```bash
cd src/rl_sar/scripts
bash init_cyberdog.sh
```
启动控制程序
```bash
source devel/setup.bash
rosrun rl_sar rl_real_cyberdog
```
按下键盘上的**0**键让机器人切换到默认站起姿态，按下**P**键切换到RL控制模式，任意状态按下**1**键切换到最初的趴下姿态。WS控制x，AD控制yaw，JL控制y。
4. 使用Type-C线连接电脑与机器人
若不方便拆卸机器人，可以暂时使用Type-C线调试。接入Type-C线后运行方法同上。
5. 程序在使用Ctrl+C结束后会自动重置机器人的运控程序，如程序失控也可手动重启运控程序。
注：运控程序重启后大概有5-10秒的启动时间，在这段时间内运行程序会报`Motor control mode has not been activated successfully`，需等待不报错再运行控制程序。
```bash
cd src/rl_sar/scripts
bash kill_cyberdog.sh
```
## 添加你的机器人 ## 添加你的机器人

20
src/rl_sar/CMakeLists.txt
View File

@ -43,9 +43,6 @@ include_directories(library/unitree_legged_sdk_3.2/include)
link_directories(library/unitree_legged_sdk_3.2/lib) link_directories(library/unitree_legged_sdk_3.2/lib)
set(EXTRA_LIBS -pthread libunitree_legged_sdk_amd64.so lcm) set(EXTRA_LIBS -pthread libunitree_legged_sdk_amd64.so lcm)
add_subdirectory(library/cyberdog_motor_sdk)
include_directories(library/cyberdog_motor_sdk/include)
include_directories( include_directories(
include include
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
@ -53,6 +50,7 @@ include_directories(
library/matplotlibcpp library/matplotlibcpp
library/observation_buffer library/observation_buffer
library/rl_sdk library/rl_sdk
library/loop
) )
add_library(rl_sdk library/rl_sdk/rl_sdk.cpp) add_library(rl_sdk library/rl_sdk/rl_sdk.cpp)
@ -75,14 +73,8 @@ target_link_libraries(rl_sim
rl_sdk observation_buffer yaml-cpp rl_sdk observation_buffer yaml-cpp
) )
# add_executable(rl_real_a1 src/rl_real_a1.cpp ) add_executable(rl_real_a1 src/rl_real_a1.cpp )
# target_link_libraries(rl_real_a1 target_link_libraries(rl_real_a1
# ${catkin_LIBRARIES} ${EXTRA_LIBS} "${TORCH_LIBRARIES}" ${catkin_LIBRARIES} ${EXTRA_LIBS} "${TORCH_LIBRARIES}"
# rl_sdk observation_buffer yaml-cpp rl_sdk observation_buffer yaml-cpp
# ) )
# add_executable(rl_real_cyberdog src/rl_real_cyberdog.cpp )
# target_link_libraries(rl_real_cyberdog
# ${catkin_LIBRARIES} ${EXTRA_LIBS} "${TORCH_LIBRARIES}"
# rl_sdk observation_buffer cyberdog_motor_sdk yaml-cpp
# )

92
src/rl_sar/config.yaml
View File

@ -3,16 +3,22 @@ a1:
num_observations: 45 num_observations: 45
clip_obs: 100.0 clip_obs: 100.0
clip_actions: 100.0 clip_actions: 100.0
# damping: 0.5 rl_kp: [20, 20, 20, # FL
# stiffness: 20.0 20, 20, 20, # FR
p_gains: [20, 20, 20, # FL 20, 20, 20, # RL
20, 20, 20, # FR 20, 20, 20] # RR
20, 20, 20, # RL rl_kd: [0.5, 0.5, 0.5, # FL
20, 20, 20] # RR 0.5, 0.5, 0.5, # FR
d_gains: [0.5, 0.5, 0.5, # FL 0.5, 0.5, 0.5, # RL
0.5, 0.5, 0.5, # FR 0.5, 0.5, 0.5] # RR
0.5, 0.5, 0.5, # RL fixed_kp: [80, 80, 80, # FL
0.5, 0.5, 0.5] # RR 80, 80, 80, # FR
80, 80, 80, # RL
80, 80, 80] # RR
fixed_kd: [3, 3, 3, # FL
3, 3, 3, # FR
3, 3, 3, # RL
3, 3, 3] # RR
action_scale: 0.25 action_scale: 0.25
hip_scale_reduction: 0.5 hip_scale_reduction: 0.5
hip_scale_reduction_indices: [0, 3, 6, 9] hip_scale_reduction_indices: [0, 3, 6, 9]
@ -40,16 +46,22 @@ cyberdog1:
num_observations: 45 num_observations: 45
clip_obs: 100.0 clip_obs: 100.0
clip_actions: 100.0 clip_actions: 100.0
# damping: 0.5 rl_kp: [20, 20, 20, # FL
# stiffness: 20.0 20, 20, 20, # FR
p_gains: [20, 20, 20, # FL 20, 20, 20, # RL
20, 20, 20, # FR 20, 20, 20] # RR
20, 20, 20, # RL rl_kd: [0.5, 0.5, 0.5, # FL
20, 20, 20] # RR 0.5, 0.5, 0.5, # FR
d_gains: [0.5, 0.5, 0.5, # FL 0.5, 0.5, 0.5, # RL
0.5, 0.5, 0.5, # FR 0.5, 0.5, 0.5] # RR
0.5, 0.5, 0.5, # RL fixed_kp: [80, 80, 80, # FL
0.5, 0.5, 0.5] # RR 80, 80, 80, # FR
80, 80, 80, # RL
80, 80, 80] # RR
fixed_kd: [3, 3, 3, # FL
3, 3, 3, # FR
3, 3, 3, # RL
3, 3, 3] # RR
action_scale: 0.25 action_scale: 0.25
hip_scale_reduction: 0.5 hip_scale_reduction: 0.5
hip_scale_reduction_indices: [0, 3, 6, 9] hip_scale_reduction_indices: [0, 3, 6, 9]
@ -76,16 +88,22 @@ lite3_wheel:
num_observations: 45 num_observations: 45
clip_obs: 100.0 clip_obs: 100.0
clip_actions: 100.0 clip_actions: 100.0
# damping: 0.5 rl_kp: [20, 20, 20, 0, # FL
# stiffness: 20.0 20, 20, 20, 0, # FR
p_gains: [20, 20, 20, 0, # FL 20, 20, 20, 0, # RL
20, 20, 20, 0, # FR 20, 20, 20, 0] # RR
20, 20, 20, 0, # RL rl_kd: [0.5, 0.5, 0.5, 0.5, # FL
20, 20, 20, 0] # RR 0.5, 0.5, 0.5, 0.5, # FR
d_gains: [0.5, 0.5, 0.5, 0.5, # FL 0.5, 0.5, 0.5, 0.5, # RL
0.5, 0.5, 0.5, 0.5, # FR 0.5, 0.5, 0.5, 0.5] # RR
0.5, 0.5, 0.5, 0.5, # RL fixed_kp: [80, 80, 80, # FL
0.5, 0.5, 0.5, 0.5] # RR 80, 80, 80, # FR
80, 80, 80, # RL
80, 80, 80] # RR
fixed_kd: [3, 3, 3, # FL
3, 3, 3, # FR
3, 3, 3, # RL
3, 3, 3] # RR
action_scale: 0.25 action_scale: 0.25
hip_scale_reduction: 1.0 hip_scale_reduction: 1.0
hip_scale_reduction_indices: [0, 3, 6, 9] hip_scale_reduction_indices: [0, 3, 6, 9]
@ -113,12 +131,14 @@ gr1t1:
num_observations: 39 num_observations: 39
clip_obs: 100.0 clip_obs: 100.0
clip_actions: 100.0 clip_actions: 100.0
# damping: 0.5 rl_kp: [57.0, 43.0, 114.0, 114.0, 15.3,
# stiffness: 20.0 57.0, 43.0, 114.0, 114.0, 15.3]
p_gains: [57.0, 43.0, 114.0, 114.0, 15.3, rl_kd: [5.7, 4.3, 11.4, 11.4, 1.5,
57.0, 43.0, 114.0, 114.0, 15.3] 5.7, 4.3, 11.4, 11.4, 1.5]
d_gains: [5.7, 4.3, 11.4, 11.4, 1.5, fixed_kp: [57.0, 43.0, 114.0, 114.0, 15.3,
5.7, 4.3, 11.4, 11.4, 1.5] 57.0, 43.0, 114.0, 114.0, 15.3]
fixed_kd: [5.7, 4.3, 11.4, 11.4, 1.5,
5.7, 4.3, 11.4, 11.4, 1.5]
action_scale: 1.0 action_scale: 1.0
hip_scale_reduction: 1.0 hip_scale_reduction: 1.0
hip_scale_reduction_indices: [] hip_scale_reduction_indices: []

64
src/rl_sar/include/rl_real_a1.hpp
View File

@ -3,10 +3,10 @@
#include "rl_sdk.hpp" #include "rl_sdk.hpp"
#include "observation_buffer.hpp" #include "observation_buffer.hpp"
#include "loop.hpp"
#include "unitree_legged_sdk/unitree_legged_sdk.h" #include "unitree_legged_sdk/unitree_legged_sdk.h"
#include "unitree_legged_sdk/unitree_joystick.h" #include "unitree_legged_sdk/unitree_joystick.h"
#include <csignal> #include <csignal>
// #include <signal.h>
#include "matplotlibcpp.h" #include "matplotlibcpp.h"
namespace plt = matplotlibcpp; namespace plt = matplotlibcpp;
@ -16,51 +16,49 @@ class RL_Real : public RL
public: public:
RL_Real(); RL_Real();
~RL_Real(); ~RL_Real();
private:
void RunModel(); // rl functions
torch::Tensor Forward() override; torch::Tensor Forward() override;
torch::Tensor ComputeObservation() override; torch::Tensor ComputeObservation() override;
void GetState(RobotState<double> *state) override;
void SetCommand(const RobotCommand<double> *command) override;
void RunModel();
void RobotControl();
// history buffer
ObservationBuffer history_obs_buf; ObservationBuffer history_obs_buf;
torch::Tensor history_obs; torch::Tensor history_obs;
int motiontime = 0;
//udp // loop
void UDPSend(){udp.Send();} std::shared_ptr<LoopFunc> loop_keyboard;
void UDPRecv(){udp.Recv();} std::shared_ptr<LoopFunc> loop_control;
void RobotControl(); std::shared_ptr<LoopFunc> loop_udpSend;
UNITREE_LEGGED_SDK::Safety safe; std::shared_ptr<LoopFunc> loop_udpRecv;
UNITREE_LEGGED_SDK::UDP udp; std::shared_ptr<LoopFunc> loop_rl;
UNITREE_LEGGED_SDK::LowCmd cmd = {0}; std::shared_ptr<LoopFunc> loop_plot;
UNITREE_LEGGED_SDK::LowState state = {0};
xRockerBtnDataStruct _keyData;
std::shared_ptr<UNITREE_LEGGED_SDK::LoopFunc> loop_control;
std::shared_ptr<UNITREE_LEGGED_SDK::LoopFunc> loop_udpSend;
std::shared_ptr<UNITREE_LEGGED_SDK::LoopFunc> loop_udpRecv;
std::shared_ptr<UNITREE_LEGGED_SDK::LoopFunc> loop_rl;
std::shared_ptr<UNITREE_LEGGED_SDK::LoopFunc> loop_plot;
float getup_percent = 0.0;
float getdown_percent = 0.0;
float start_pos[12];
float now_pos[12];
int robot_state = STATE_WAITING;
// plot
const int plot_size = 100; const int plot_size = 100;
std::vector<int> plot_t; std::vector<int> plot_t;
std::vector<std::vector<double>> plot_real_joint_pos, plot_target_joint_pos; std::vector<std::vector<double>> plot_real_joint_pos, plot_target_joint_pos;
void Plot(); void Plot();
private:
std::vector<std::string> joint_names;
std::vector<double> joint_positions;
std::vector<double> joint_velocities;
int dof_mapping[12] = {3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8}; // unitree interface
void UDPSend(){unitree_udp.Send();}
void UDPRecv(){unitree_udp.Recv();}
UNITREE_LEGGED_SDK::Safety unitree_safe;
UNITREE_LEGGED_SDK::UDP unitree_udp;
UNITREE_LEGGED_SDK::LowCmd unitree_low_command = {0};
UNITREE_LEGGED_SDK::LowState unitree_low_state = {0};
xRockerBtnDataStruct unitree_joy;
// others
int motiontime = 0;
std::vector<double> mapped_joint_positions;
std::vector<double> mapped_joint_velocities;
int command_mapping[12] = {3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8};
int state_mapping[12] = {3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8};
int hip_scale_reduction_indices[4] = {0, 3, 6, 9}; int hip_scale_reduction_indices[4] = {0, 3, 6, 9};
std::chrono::high_resolution_clock::time_point start_time;
}; };
#endif #endif

60
src/rl_sar/include/rl_real_cyberdog.hpp
View File

@ -1,60 +0,0 @@
#ifndef RL_REAL_HPP
#define RL_REAL_HPP
#include "rl_sdk.hpp"
#include "observation_buffer.hpp"
#include "unitree_legged_sdk/loop.h"
#include <boost/bind.hpp>
#include <CustomInterface.h>
#include <csignal>
// #include <signal.h>
#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;
using CyberdogData = Robot_Data;
using CyberdogCmd = Motor_Cmd;
class RL_Real : public RL, public CustomInterface
{
public:
RL_Real();
~RL_Real();
void RunModel();
torch::Tensor Forward() override;
torch::Tensor ComputeObservation() override;
ObservationBuffer history_obs_buf;
torch::Tensor history_obs;
int motiontime = 0;
CyberdogData cyberdogData;
CyberdogCmd cyberdogCmd;
void UserCode();
long long count = 0;
void RobotControl();
std::shared_ptr<UNITREE_LEGGED_SDK::LoopFunc> loop_control;
std::shared_ptr<UNITREE_LEGGED_SDK::LoopFunc> loop_rl;
std::shared_ptr<UNITREE_LEGGED_SDK::LoopFunc> loop_plot;
const int plot_size = 100;
std::vector<int> plot_t;
std::vector<std::vector<double>> plot_real_joint_pos, plot_target_joint_pos;
void Plot();
std::thread _keyboardThread;
private:
std::vector<std::string> joint_names;
std::vector<double> joint_positions;
std::vector<double> joint_velocities;
int dof_mapping[12] = {3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8};
int hip_scale_reduction_indices[4] = {0, 3, 6, 9};
std::chrono::high_resolution_clock::time_point start_time;
};
#endif

70
src/rl_sar/include/rl_sim.hpp
View File

@ -3,16 +3,13 @@
#include "rl_sdk.hpp" #include "rl_sdk.hpp"
#include "observation_buffer.hpp" #include "observation_buffer.hpp"
#include "loop.hpp"
#include <ros/ros.h> #include <ros/ros.h>
#include <gazebo_msgs/ModelStates.h> #include <gazebo_msgs/ModelStates.h>
#include <sensor_msgs/JointState.h> #include <sensor_msgs/JointState.h>
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include "unitree_legged_sdk/loop.h"
#include <csignal>
#include "robot_msgs/MotorCommand.h" #include "robot_msgs/MotorCommand.h"
// #include "robot_msgs/RobotState.h" #include <csignal>
// #include "robot_msgs/RobotCommand.h"
#include "matplotlibcpp.h" #include "matplotlibcpp.h"
namespace plt = matplotlibcpp; namespace plt = matplotlibcpp;
@ -22,61 +19,54 @@ class RL_Sim : public RL
public: public:
RL_Sim(); RL_Sim();
~RL_Sim(); ~RL_Sim();
private:
void ModelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg); // rl functions
void JointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
void CmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg);
void RunModel();
void RobotControl();
torch::Tensor Forward() override; torch::Tensor Forward() override;
torch::Tensor ComputeObservation() override; torch::Tensor ComputeObservation() override;
void GetState(RobotState<double> *state) override; void GetState(RobotState<double> *state) override;
void SetCommand(const RobotCommand<double> *command) override; void SetCommand(const RobotCommand<double> *command) override;
void RunModel();
void RobotControl();
// history buffer
bool use_history = true;
ObservationBuffer history_obs_buf; ObservationBuffer history_obs_buf;
torch::Tensor history_obs; torch::Tensor history_obs;
int motiontime = 0; // loop
std::shared_ptr<LoopFunc> loop_keyboard;
std::shared_ptr<UNITREE_LEGGED_SDK::LoopFunc> loop_control; std::shared_ptr<LoopFunc> loop_control;
std::shared_ptr<UNITREE_LEGGED_SDK::LoopFunc> loop_rl; std::shared_ptr<LoopFunc> loop_rl;
std::shared_ptr<UNITREE_LEGGED_SDK::LoopFunc> loop_plot; std::shared_ptr<LoopFunc> loop_plot;
// plot
const int plot_size = 100; const int plot_size = 100;
std::vector<int> plot_t; std::vector<int> plot_t;
std::vector<std::vector<double>> plot_real_joint_pos, plot_target_joint_pos; std::vector<std::vector<double>> plot_real_joint_pos, plot_target_joint_pos;
void Plot(); void Plot();
std::thread _keyboardThread; // ros interface
private:
std::string ros_namespace; std::string ros_namespace;
std::vector<std::string> torque_command_topics;
ros::Subscriber model_state_subscriber_;
ros::Subscriber joint_state_subscriber_;
ros::Subscriber cmd_vel_subscriber_;
std::map<std::string, ros::Publisher> torque_publishers;
std::vector<robot_msgs::MotorCommand> motor_commands;
// robot_msgs::RobotState robot_state;
// robot_msgs::RobotCommand robot_command;
geometry_msgs::Twist vel; geometry_msgs::Twist vel;
geometry_msgs::Pose pose; geometry_msgs::Pose pose;
geometry_msgs::Twist cmd_vel; geometry_msgs::Twist cmd_vel;
std::vector<std::string> torque_command_topics;
ros::Subscriber model_state_subscriber;
ros::Subscriber joint_state_subscriber;
ros::Subscriber cmd_vel_subscriber;
std::map<std::string, ros::Publisher> torque_publishers;
void ModelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg);
void JointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
void CmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg);
std::vector<double> joint_positions; // others
std::vector<double> joint_velocities; int motiontime = 0;
std::vector<double> joint_efforts;
std::chrono::high_resolution_clock::time_point start_time;
void MapData(const std::vector<double>& source_data, std::vector<double>& target_data);
std::map<std::string, size_t> sorted_to_original_index; std::map<std::string, size_t> sorted_to_original_index;
std::vector<robot_msgs::MotorCommand> motor_commands;
std::vector<double> mapped_joint_positions;
std::vector<double> mapped_joint_velocities;
std::vector<double> mapped_joint_efforts;
void MapData(const std::vector<double>& source_data, std::vector<double>& target_data);
}; };
#endif #endif

2
src/rl_sar/launch/start_a1.launch → src/rl_sar/launch/gazebo_a1.launch
View File

@ -1,6 +1,8 @@
<launch> <launch>
<arg name="wname" default="stairs"/> <arg name="wname" default="stairs"/>
<arg name="rname" default="a1"/> <arg name="rname" default="a1"/>
<param name="robot_name" type="str" value="a1"/>
<param name="use_history" type="bool" value="true"/>
<param name="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/> <param name="ros_namespace" type="str" value="/$(arg rname)_gazebo/"/>
<arg name="robot_path" value="(find $(arg rname)_description)"/> <arg name="robot_path" value="(find $(arg rname)_description)"/>
<arg name="dollar" value="$"/> <arg name="dollar" value="$"/>

52
src/rl_sar/launch/start_cyberdog.launch
View File

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

24
src/rl_sar/library/cyberdog_motor_sdk/CMakeLists.txt
View File

@ -1,24 +0,0 @@
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)

140
src/rl_sar/library/cyberdog_motor_sdk/README.md
View File

@ -1,140 +0,0 @@
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"
```

121
src/rl_sar/library/cyberdog_motor_sdk/README_EN.md
View File

@ -1,121 +0,0 @@
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"
```

117
src/rl_sar/library/cyberdog_motor_sdk/include/CustomInterface.h
View File

@ -1,117 +0,0 @@
// 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 <assert.h>
#include <chrono>
#include <cmath>
#include <iostream>
#include <lcm/lcm-cpp.hpp>
#include <signal.h>
#include <string>
#include <sys/mman.h>
#include <sys/timerfd.h>
#include <thread>
#include <unistd.h>
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

286
src/rl_sar/library/cyberdog_motor_sdk/include/leg_control_data_lcmt.hpp
View File

@ -1,286 +0,0 @@
/** 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 <lcm/lcm_coretypes.h>
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

211
src/rl_sar/library/cyberdog_motor_sdk/include/motor_ctrl_lcmt.hpp
View File

@ -1,211 +0,0 @@
/** 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 <lcm/lcm_coretypes.h>
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

166
src/rl_sar/library/cyberdog_motor_sdk/include/motor_ctrl_state_lcmt.hpp
View File

@ -1,166 +0,0 @@
/** 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 <lcm/lcm_coretypes.h>
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

316
src/rl_sar/library/cyberdog_motor_sdk/include/state_estimator_lcmt.hpp
View File

@ -1,316 +0,0 @@
/** 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 <lcm/lcm_coretypes.h>
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

171
src/rl_sar/library/cyberdog_motor_sdk/src/CustomInterface.cpp
View File

@ -1,171 +0,0 @@
// 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 <CustomInterface.h>
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];
}
}

115
src/rl_sar/library/cyberdog_motor_sdk/src/Example_MotorCtrl.cpp
View File

@ -1,115 +0,0 @@
// 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 <CustomInterface.h>
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<CustomCtrl> 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<CustomCtrl>(500);
io->Spin();
return 0;
}

84
src/rl_sar/library/loop/loop.hpp Normal file
View File

@ -0,0 +1,84 @@
#ifndef LOOP_H
#define LOOP_H
#include <iostream>
#include <thread>
#include <mutex>
#include <chrono>
#include <functional>
typedef std::function<void()> Callback;
class Loop {
public:
Loop(std::string name, float period, int bindCPU = -1)
: _name(name), _period(period), _bindCPU(bindCPU) {}
~Loop() {
if (_isrunning) {
shutdown(); // Ensure the loop is stopped when the object is destroyed
}
}
void start() {
_isrunning = true;
_thread = std::thread([this]() {
if (_bindCPU >= 0) {
std::lock_guard<std::mutex> lock(_printMutex);
std::cout << "[Loop Start] named: " << _name << ", period: " << _period * 1000 << " (ms), run at cpu: " << _bindCPU << std::endl;
} else {
std::lock_guard<std::mutex> lock(_printMutex);
std::cout << "[Loop Start] named: " << _name << ", period: " << _period * 1000 << " (ms), cpu unspecified" << std::endl;
}
entryFunc();
}); // Start the loop in a new thread
}
void shutdown() {
_isrunning = false;
if (_thread.joinable()) {
_thread.join(); // Wait for the loop thread to finish
std::lock_guard<std::mutex> lock(_printMutex);
std::cout << "[Loop End] named: " << _name << std::endl;
}
}
virtual void functionCB() = 0;
protected:
void entryFunc() {
while (_isrunning) {
functionCB(); // Call the overridden functionCB in a loop
std::this_thread::sleep_for(std::chrono::duration<float>(_period)); // Wait for the specified period
}
}
std::string _name;
float _period;
int _bindCPU;
bool _isrunning = false;
std::thread _thread;
static std::mutex _printMutex;
};
std::mutex Loop::_printMutex;
class LoopFunc : public Loop {
public:
LoopFunc(std::string name, float period, const Callback& cb)
: Loop(name, period), _fp(cb) {}
LoopFunc(std::string name, float period, int bindCPU, const Callback& cb)
: Loop(name, period, bindCPU), _fp(cb) {}
void functionCB() override {
{
std::lock_guard<std::mutex> lock(_printMutex);
(_fp)(); // Call the provided callback function
}
}
private:
Callback _fp;
};
#endif

519
src/rl_sar/library/rl_sdk/rl_sdk.cpp
View File

@ -1,7 +1,251 @@
#include "rl_sdk.hpp" #include "rl_sdk.hpp"
/* You may need to override this ComputeObservation() function
torch::Tensor RL::ComputeObservation()
{
torch::Tensor obs = torch::cat({(this->QuatRotateInverse(this->base_quat, this->lin_vel)) * this->params.lin_vel_scale,
(this->QuatRotateInverse(this->obs.base_quat, this->obs.ang_vel)) * this->params.ang_vel_scale,
this->QuatRotateInverse(this->obs.base_quat, this->obs.gravity_vec),
this->obs.commands * this->params.commands_scale,
(this->obs.dof_pos - this->params.default_dof_pos) * this->params.dof_pos_scale,
this->obs.dof_vel * this->params.dof_vel_scale,
this->obs.actions},
1);
obs = torch::clamp(obs, -this->params.clip_obs, this->params.clip_obs);
printf("observation size: %d, %d\n", obs.sizes()[0], obs.sizes()[1]);
return obs;
}
*/
/* You may need to override this Forward() function
torch::Tensor RL::Forward()
{
torch::Tensor obs = this->ComputeObservation();
torch::Tensor actor_input = torch::cat({obs}, 1);
torch::Tensor actions = this->actor.forward({actor_input}).toTensor();
this->obs.actions = actions;
torch::Tensor clamped_actions = torch::clamp(actions, -this->params.clip_actions, this->params.clip_actions);
return clamped_actions;
}
*/
void RL::InitObservations()
{
this->obs.lin_vel = torch::tensor({{0.0, 0.0, 0.0}});
this->obs.ang_vel = torch::tensor({{0.0, 0.0, 0.0}});
this->obs.gravity_vec = torch::tensor({{0.0, 0.0, -1.0}});
this->obs.commands = torch::tensor({{0.0, 0.0, 0.0}});
this->obs.base_quat = torch::tensor({{0.0, 0.0, 0.0, 1.0}});
this->obs.dof_pos = this->params.default_dof_pos;
this->obs.dof_vel = torch::zeros({1, params.num_of_dofs});
this->obs.actions = torch::zeros({1, params.num_of_dofs});
}
void RL::InitOutputs()
{
this->output_torques = torch::zeros({1, params.num_of_dofs});
this->output_dof_pos = params.default_dof_pos;
}
void RL::InitKeyboard()
{
this->keyboard.keyboard_state = STATE_WAITING;
this->keyboard.x = 0.0;
this->keyboard.y = 0.0;
this->keyboard.yaw = 0.0;
}
torch::Tensor RL::ComputeTorques(torch::Tensor actions)
{
torch::Tensor actions_scaled = actions * this->params.action_scale;
torch::Tensor output_torques = this->params.rl_kp * (actions_scaled + this->params.default_dof_pos - this->obs.dof_pos) - this->params.rl_kd * this->obs.dof_vel;
torch::Tensor clamped = torch::clamp(output_torques, -(this->params.torque_limits), this->params.torque_limits);
return clamped;
}
torch::Tensor RL::ComputePosition(torch::Tensor actions)
{
torch::Tensor actions_scaled = actions * this->params.action_scale;
return actions_scaled + this->params.default_dof_pos;
}
torch::Tensor RL::QuatRotateInverse(torch::Tensor q, torch::Tensor v)
{
c10::IntArrayRef shape = q.sizes();
torch::Tensor q_w = q.index({torch::indexing::Slice(), -1});
torch::Tensor q_vec = q.index({torch::indexing::Slice(), torch::indexing::Slice(0, 3)});
torch::Tensor a = v * (2.0 * torch::pow(q_w, 2) - 1.0).unsqueeze(-1);
torch::Tensor b = torch::cross(q_vec, v, -1) * q_w.unsqueeze(-1) * 2.0;
torch::Tensor c = q_vec * torch::bmm(q_vec.view({shape[0], 1, 3}), v.view({shape[0], 3, 1})).squeeze(-1) * 2.0;
return a - b + c;
}
void RL::StateController(const RobotState<double> *state, RobotCommand<double> *command)
{
// waiting
if(running_state == STATE_WAITING)
{
for(int i = 0; i < params.num_of_dofs; ++i)
{
command->motor_command.q[i] = state->motor_state.q[i];
}
if(keyboard.keyboard_state == STATE_POS_GETUP)
{
keyboard.keyboard_state = STATE_WAITING;
getup_percent = 0.0;
for(int i = 0; i < params.num_of_dofs; ++i)
{
now_pos[i] = state->motor_state.q[i];
start_pos[i] = now_pos[i];
}
running_state = STATE_POS_GETUP;
}
}
// stand up (position control)
else if(running_state == STATE_POS_GETUP)
{
if(getup_percent != 1)
{
getup_percent += 1 / 1000.0;
getup_percent = getup_percent > 1 ? 1 : getup_percent;
for(int i = 0; i < params.num_of_dofs; ++i)
{
command->motor_command.q[i] = (1 - getup_percent) * now_pos[i] + getup_percent * params.default_dof_pos[0][i].item<double>();
command->motor_command.dq[i] = 0;
command->motor_command.kp[i] = params.fixed_kp[0][i].item<double>();
command->motor_command.kd[i] = params.fixed_kd[0][i].item<double>();
command->motor_command.tau[i] = 0;
}
printf("getting up %.3f%%\r", getup_percent*100.0);
}
if(keyboard.keyboard_state == STATE_RL_INIT)
{
keyboard.keyboard_state = STATE_WAITING;
running_state = STATE_RL_INIT;
}
else if(keyboard.keyboard_state == STATE_POS_GETDOWN)
{
keyboard.keyboard_state = STATE_WAITING;
getdown_percent = 0.0;
for(int i = 0; i < params.num_of_dofs; ++i)
{
now_pos[i] = state->motor_state.q[i];
}
running_state = STATE_POS_GETDOWN;
}
}
// init obs and start rl loop
else if(running_state == STATE_RL_INIT)
{
if(getup_percent == 1)
{
running_state = STATE_RL_RUNNING;
this->InitObservations();
this->InitOutputs();
this->InitKeyboard();
}
}
// rl loop
else if(running_state == STATE_RL_RUNNING)
{
for(int i = 0; i < params.num_of_dofs; ++i)
{
command->motor_command.q[i] = output_dof_pos[0][i].item<double>();
command->motor_command.dq[i] = 0;
command->motor_command.kp[i] = params.rl_kp[0][i].item<double>();
command->motor_command.kd[i] = params.rl_kd[0][i].item<double>();
command->motor_command.tau[i] = 0;
}
if(keyboard.keyboard_state == STATE_POS_GETDOWN)
{
keyboard.keyboard_state = STATE_WAITING;
getdown_percent = 0.0;
for(int i = 0; i < params.num_of_dofs; ++i)
{
now_pos[i] = state->motor_state.q[i];
}
running_state = STATE_POS_GETDOWN;
}
}
// get down (position control)
else if(running_state == STATE_POS_GETDOWN)
{
if(getdown_percent != 1)
{
getdown_percent += 1 / 1000.0;
getdown_percent = getdown_percent > 1 ? 1 : getdown_percent;
for(int i = 0; i < params.num_of_dofs; ++i)
{
command->motor_command.q[i] = (1 - getdown_percent) * now_pos[i] + getdown_percent * start_pos[i];
command->motor_command.dq[i] = 0;
command->motor_command.kp[i] = params.fixed_kp[0][i].item<double>();
command->motor_command.kd[i] = params.fixed_kd[0][i].item<double>();
command->motor_command.tau[i] = 0;
}
printf("getting down %.3f%%\r", getdown_percent*100.0);
}
if(getdown_percent == 1)
{
running_state = STATE_WAITING;
this->InitObservations();
this->InitOutputs();
this->InitKeyboard();
}
}
}
#include <termios.h> #include <termios.h>
#include <sys/ioctl.h> #include <sys/ioctl.h>
static bool kbhit()
{
termios term;
tcgetattr(0, &term);
termios term2 = term;
term2.c_lflag &= ~ICANON;
tcsetattr(0, TCSANOW, &term2);
int byteswaiting;
ioctl(0, FIONREAD, &byteswaiting);
tcsetattr(0, TCSANOW, &term);
return byteswaiting > 0;
}
void RL::RunKeyboard()
{
if(kbhit())
{
int c = fgetc(stdin);
switch(c)
{
case '0': keyboard.keyboard_state = STATE_POS_GETUP; break;
case 'p': keyboard.keyboard_state = STATE_RL_INIT; break;
case '1': keyboard.keyboard_state = STATE_POS_GETDOWN; break;
case 'q': break;
case 'w': keyboard.x += 0.1; break;
case 's': keyboard.x -= 0.1; break;
case 'a': keyboard.yaw += 0.1; break;
case 'd': keyboard.yaw -= 0.1; break;
case 'i': break;
case 'k': break;
case 'j': keyboard.y += 0.1; break;
case 'l': keyboard.y -= 0.1; break;
case ' ': keyboard.x = 0; keyboard.y = 0; keyboard.yaw = 0; break;
default: break;
}
}
}
template<typename T> template<typename T>
std::vector<T> ReadVectorFromYaml(const YAML::Node& node) std::vector<T> ReadVectorFromYaml(const YAML::Node& node)
@ -41,12 +285,10 @@ void RL::ReadYaml(std::string robot_name)
this->params.dof_vel_scale = config["dof_vel_scale"].as<double>(); this->params.dof_vel_scale = config["dof_vel_scale"].as<double>();
// this->params.commands_scale = torch::tensor(ReadVectorFromYaml<double>(config["commands_scale"])).view({1, -1}); // this->params.commands_scale = torch::tensor(ReadVectorFromYaml<double>(config["commands_scale"])).view({1, -1});
this->params.commands_scale = torch::tensor({this->params.lin_vel_scale, this->params.lin_vel_scale, this->params.ang_vel_scale}); this->params.commands_scale = torch::tensor({this->params.lin_vel_scale, this->params.lin_vel_scale, this->params.ang_vel_scale});
// this->params.damping = config["damping"].as<double>(); this->params.rl_kp = torch::tensor(ReadVectorFromYaml<double>(config["rl_kp"])).view({1, -1});
// this->params.stiffness = config["stiffness"].as<double>(); this->params.rl_kd = torch::tensor(ReadVectorFromYaml<double>(config["rl_kd"])).view({1, -1});
// this->params.d_gains = torch::ones(12) * this->params.damping; this->params.fixed_kp = torch::tensor(ReadVectorFromYaml<double>(config["fixed_kp"])).view({1, -1});
// this->params.p_gains = torch::ones(12) * this->params.stiffness; this->params.fixed_kd = torch::tensor(ReadVectorFromYaml<double>(config["fixed_kd"])).view({1, -1});
this->params.p_gains = torch::tensor(ReadVectorFromYaml<double>(config["p_gains"])).view({1, -1});
this->params.d_gains = torch::tensor(ReadVectorFromYaml<double>(config["d_gains"])).view({1, -1});
this->params.torque_limits = torch::tensor(ReadVectorFromYaml<double>(config["torque_limits"])).view({1, -1}); this->params.torque_limits = torch::tensor(ReadVectorFromYaml<double>(config["torque_limits"])).view({1, -1});
this->params.default_dof_pos = torch::tensor(ReadVectorFromYaml<double>(config["default_dof_pos"])).view({1, -1}); this->params.default_dof_pos = torch::tensor(ReadVectorFromYaml<double>(config["default_dof_pos"])).view({1, -1});
this->params.joint_names = ReadVectorFromYaml<std::string>(config["joint_names"]); this->params.joint_names = ReadVectorFromYaml<std::string>(config["joint_names"]);
@ -65,7 +307,7 @@ void RL::CSVInit(std::string robot_name)
// csv_filename += "_" + timestamp; // csv_filename += "_" + timestamp;
csv_filename += ".csv"; csv_filename += ".csv";
std::ofstream file(csv_filename.c_str()); // 创建新文件 std::ofstream file(csv_filename.c_str());
for(int i = 0; i < 12; ++i) {file << "torque_" << i << ",";} for(int i = 0; i < 12; ++i) {file << "torque_" << i << ",";}
for(int i = 0; i < 12; ++i) {file << "tau_est_" << i << ",";} for(int i = 0; i < 12; ++i) {file << "tau_est_" << i << ",";}
@ -75,14 +317,13 @@ void RL::CSVInit(std::string robot_name)
file << std::endl; file << std::endl;
file.close(); // 关闭文件 file.close();
} }
void RL::CSVLogger(torch::Tensor torque, torch::Tensor tau_est, torch::Tensor joint_pos, torch::Tensor joint_pos_target, torch::Tensor joint_vel) void RL::CSVLogger(torch::Tensor torque, torch::Tensor tau_est, torch::Tensor joint_pos, torch::Tensor joint_pos_target, torch::Tensor joint_vel)
{ {
std::ofstream file(csv_filename.c_str(), std::ios_base::app); // 打开文件以追加模式写入数据 std::ofstream file(csv_filename.c_str(), std::ios_base::app);
// 写入数据
for(int i = 0; i < 12; ++i) {file << torque[0][i].item<double>() << ",";} for(int i = 0; i < 12; ++i) {file << torque[0][i].item<double>() << ",";}
for(int i = 0; i < 12; ++i) {file << tau_est[0][i].item<double>() << ",";} for(int i = 0; i < 12; ++i) {file << tau_est[0][i].item<double>() << ",";}
for(int i = 0; i < 12; ++i) {file << joint_pos[0][i].item<double>() << ",";} for(int i = 0; i < 12; ++i) {file << joint_pos[0][i].item<double>() << ",";}
@ -91,261 +332,5 @@ void RL::CSVLogger(torch::Tensor torque, torch::Tensor tau_est, torch::Tensor jo
file << std::endl; file << std::endl;
file.close(); // 关闭文件 file.close();
}
torch::Tensor RL::QuatRotateInverse(torch::Tensor q, torch::Tensor v)
{
c10::IntArrayRef shape = q.sizes();
torch::Tensor q_w = q.index({torch::indexing::Slice(), -1});
torch::Tensor q_vec = q.index({torch::indexing::Slice(), torch::indexing::Slice(0, 3)});
torch::Tensor a = v * (2.0 * torch::pow(q_w, 2) - 1.0).unsqueeze(-1);
torch::Tensor b = torch::cross(q_vec, v, -1) * q_w.unsqueeze(-1) * 2.0;
torch::Tensor c = q_vec * torch::bmm(q_vec.view({shape[0], 1, 3}), v.view({shape[0], 3, 1})).squeeze(-1) * 2.0;
return a - b + c;
}
void RL::InitObservations()
{
this->obs.lin_vel = torch::tensor({{0.0, 0.0, 0.0}});
this->obs.ang_vel = torch::tensor({{0.0, 0.0, 0.0}});
this->obs.gravity_vec = torch::tensor({{0.0, 0.0, -1.0}});
this->obs.commands = torch::tensor({{0.0, 0.0, 0.0}});
this->obs.base_quat = torch::tensor({{0.0, 0.0, 0.0, 1.0}});
this->obs.dof_pos = this->params.default_dof_pos;
this->obs.dof_vel = torch::zeros({1, params.num_of_dofs});
this->obs.actions = torch::zeros({1, params.num_of_dofs});
}
void RL::InitOutputs()
{
this->output_torques = torch::zeros({1, params.num_of_dofs});
this->output_dof_pos = params.default_dof_pos;
}
void RL::InitKeyboard()
{
this->keyboard.keyboard_state = STATE_WAITING;
this->keyboard.x = 0.0;
this->keyboard.y = 0.0;
this->keyboard.yaw = 0.0;
}
torch::Tensor RL::ComputeTorques(torch::Tensor actions)
{
torch::Tensor actions_scaled = actions * this->params.action_scale;
torch::Tensor output_torques = this->params.p_gains * (actions_scaled + this->params.default_dof_pos - this->obs.dof_pos) - this->params.d_gains * this->obs.dof_vel;
torch::Tensor clamped = torch::clamp(output_torques, -(this->params.torque_limits), this->params.torque_limits);
return clamped;
}
torch::Tensor RL::ComputePosition(torch::Tensor actions)
{
torch::Tensor actions_scaled = actions * this->params.action_scale;
return actions_scaled + this->params.default_dof_pos;
}
/* You may need to override this ComputeObservation() function
torch::Tensor RL::ComputeObservation()
{
torch::Tensor obs = torch::cat({(this->QuatRotateInverse(this->base_quat, this->lin_vel)) * this->params.lin_vel_scale,
(this->QuatRotateInverse(this->obs.base_quat, this->obs.ang_vel)) * this->params.ang_vel_scale,
this->QuatRotateInverse(this->obs.base_quat, this->obs.gravity_vec),
this->obs.commands * this->params.commands_scale,
(this->obs.dof_pos - this->params.default_dof_pos) * this->params.dof_pos_scale,
this->obs.dof_vel * this->params.dof_vel_scale,
this->obs.actions},
1);
obs = torch::clamp(obs, -this->params.clip_obs, this->params.clip_obs);
printf("observation size: %d, %d\n", obs.sizes()[0], obs.sizes()[1]);
return obs;
}
*/
/* You may need to override this Forward() function
torch::Tensor RL::Forward()
{
torch::Tensor obs = this->ComputeObservation();
torch::Tensor actor_input = torch::cat({obs}, 1);
torch::Tensor action = this->actor.forward({actor_input}).toTensor();
this->obs.actions = action;
torch::Tensor clamped = torch::clamp(action, -this->params.clip_actions, this->params.clip_actions);
return clamped;
}
*/
static bool kbhit()
{
termios term;
tcgetattr(0, &term);
termios term2 = term;
term2.c_lflag &= ~ICANON;
tcsetattr(0, TCSANOW, &term2);
int byteswaiting;
ioctl(0, FIONREAD, &byteswaiting);
tcsetattr(0, TCSANOW, &term);
return byteswaiting > 0;
}
void RL::run_keyboard()
{
int c;
// Check for keyboard input
while(true)
{
if(kbhit())
{
c = fgetc(stdin);
switch(c)
{
case '0': keyboard.keyboard_state = STATE_POS_GETUP; break;
case 'p': keyboard.keyboard_state = STATE_RL_INIT; break;
case '1': keyboard.keyboard_state = STATE_POS_GETDOWN; break;
case 'q': break;
case 'w': keyboard.x += 0.1; break;
case 's': keyboard.x -= 0.1; break;
case 'a': keyboard.yaw += 0.1; break;
case 'd': keyboard.yaw -= 0.1; break;
case 'i': break;
case 'k': break;
case 'j': keyboard.y += 0.1; break;
case 'l': keyboard.y -= 0.1; break;
case ' ': keyboard.x = 0; keyboard.y = 0; keyboard.yaw = 0; break;
default: break;
}
}
usleep(10000);
}
}
void RL::StateController(const RobotState<double> *state, RobotCommand<double> *command)
{
// waiting
if(running_state == STATE_WAITING)
{
for(int i = 0; i < params.num_of_dofs; ++i)
{
command->motor_command.q[i] = state->motor_state.q[i];
}
if(keyboard.keyboard_state == STATE_POS_GETUP)
{
keyboard.keyboard_state = STATE_WAITING;
getup_percent = 0.0;
for(int i = 0; i < params.num_of_dofs; ++i)
{
now_pos[i] = state->motor_state.q[i];
start_pos[i] = now_pos[i];
}
running_state = STATE_POS_GETUP;
}
}
// stand up (position control)
else if(running_state == STATE_POS_GETUP)
{
if(getup_percent != 1)
{
getup_percent += 1 / 1000.0;
getup_percent = getup_percent > 1 ? 1 : getup_percent;
for(int i = 0; i < params.num_of_dofs; ++i)
{
command->motor_command.q[i] = (1 - getup_percent) * now_pos[i] + getup_percent * params.default_dof_pos[0][i].item<double>();
command->motor_command.dq[i] = 0;
command->motor_command.kp[i] = 200;
command->motor_command.kd[i] = 10;
command->motor_command.tau[i] = 0;
}
printf("getting up %.3f%%\r", getup_percent*100.0);
}
if(keyboard.keyboard_state == STATE_RL_INIT)
{
keyboard.keyboard_state = STATE_WAITING;
running_state = STATE_RL_INIT;
}
else if(keyboard.keyboard_state == STATE_POS_GETDOWN)
{
keyboard.keyboard_state = STATE_WAITING;
getdown_percent = 0.0;
for(int i = 0; i < params.num_of_dofs; ++i)
{
now_pos[i] = state->motor_state.q[i];
}
running_state = STATE_POS_GETDOWN;
}
}
// init obs and start rl loop
else if(running_state == STATE_RL_INIT)
{
if(getup_percent == 1)
{
running_state = STATE_RL_RUNNING;
this->InitObservations();
this->InitOutputs();
this->InitKeyboard();
// printf("\nstart rl loop\n");
// loop_rl->start();
}
}
// rl loop
else if(running_state == STATE_RL_RUNNING)
{
for(int i = 0; i < params.num_of_dofs; ++i)
{
command->motor_command.q[i] = output_dof_pos[0][i].item<double>();
command->motor_command.dq[i] = 0;
// command->motor_command.kp[i] = params.stiffness;
// command->motor_command.kd[i] = params.damping;
command->motor_command.kp[i] = params.p_gains[0][i].item<double>();
command->motor_command.kd[i] = params.d_gains[0][i].item<double>();
// command->motor_command.tau[i] = output_torques[0][i].item<double>();
command->motor_command.tau[i] = 0;
}
if(keyboard.keyboard_state == STATE_POS_GETDOWN)
{
keyboard.keyboard_state = STATE_WAITING;
getdown_percent = 0.0;
for(int i = 0; i < params.num_of_dofs; ++i)
{
now_pos[i] = state->motor_state.q[i];
}
running_state = STATE_POS_GETDOWN;
}
}
// get down (position control)
else if(running_state == STATE_POS_GETDOWN)
{
if(getdown_percent != 1)
{
getdown_percent += 1 / 1000.0;
getdown_percent = getdown_percent > 1 ? 1 : getdown_percent;
for(int i = 0; i < params.num_of_dofs; ++i)
{
command->motor_command.q[i] = (1 - getdown_percent) * now_pos[i] + getdown_percent * start_pos[i];
command->motor_command.dq[i] = 0;
command->motor_command.kp[i] = 200;
command->motor_command.kd[i] = 10;
command->motor_command.tau[i] = 0;
}
printf("getting down %.3f%%\r", getdown_percent*100.0);
}
if(getdown_percent == 1)
{
running_state = STATE_WAITING;
this->InitObservations();
this->InitOutputs();
this->InitKeyboard();
// printf("\nstop rl loop\n");
// loop_rl->shutdown();
}
}
} }

72
src/rl_sar/library/rl_sdk/rl_sdk.hpp
View File

@ -75,8 +75,10 @@ struct ModelParams
double clip_obs; double clip_obs;
double clip_actions; double clip_actions;
torch::Tensor torque_limits; torch::Tensor torque_limits;
torch::Tensor d_gains; torch::Tensor rl_kd;
torch::Tensor p_gains; torch::Tensor rl_kp;
torch::Tensor fixed_kp;
torch::Tensor fixed_kd;
torch::Tensor commands_scale; torch::Tensor commands_scale;
torch::Tensor default_dof_pos; torch::Tensor default_dof_pos;
std::vector<std::string> joint_names; std::vector<std::string> joint_names;
@ -102,51 +104,51 @@ public:
ModelParams params; ModelParams params;
Observations obs; Observations obs;
virtual torch::Tensor Forward() = 0;
virtual torch::Tensor ComputeObservation() = 0;
torch::Tensor ComputeTorques(torch::Tensor actions);
torch::Tensor ComputePosition(torch::Tensor actions);
torch::Tensor QuatRotateInverse(torch::Tensor q, torch::Tensor v);
void InitObservations();
void InitOutputs();
void InitKeyboard();
void ReadYaml(std::string robot_name);
std::string csv_filename;
void CSVInit(std::string robot_name);
void CSVLogger(torch::Tensor torque, torch::Tensor tau_est, torch::Tensor joint_pos, torch::Tensor joint_pos_target, torch::Tensor joint_vel);
void run_keyboard();
float getup_percent = 0.0;
float getdown_percent = 0.0;
std::vector<double> start_pos;
std::vector<double> now_pos;
int running_state = STATE_WAITING;
RobotState<double> robot_state; RobotState<double> robot_state;
RobotCommand<double> robot_command; RobotCommand<double> robot_command;
// init
void InitObservations();
void InitOutputs();
void InitKeyboard();
// rl functions
virtual torch::Tensor Forward() = 0;
virtual torch::Tensor ComputeObservation() = 0;
virtual void GetState(RobotState<double> *state) = 0; virtual void GetState(RobotState<double> *state) = 0;
virtual void SetCommand(const RobotCommand<double> *command) = 0; virtual void SetCommand(const RobotCommand<double> *command) = 0;
void StateController(const RobotState<double> *state, RobotCommand<double> *command); void StateController(const RobotState<double> *state, RobotCommand<double> *command);
torch::Tensor ComputeTorques(torch::Tensor actions);
torch::Tensor ComputePosition(torch::Tensor actions);
torch::Tensor QuatRotateInverse(torch::Tensor q, torch::Tensor v);
// yaml params
void ReadYaml(std::string robot_name);
// csv logger
std::string csv_filename;
void CSVInit(std::string robot_name);
void CSVLogger(torch::Tensor torque, torch::Tensor tau_est, torch::Tensor joint_pos, torch::Tensor joint_pos_target, torch::Tensor joint_vel);
// keyboard
KeyBoard keyboard;
void RunKeyboard();
// others
std::string robot_name;
STATE running_state = STATE_WAITING;
protected: protected:
// rl module // rl module
torch::jit::script::Module model; torch::jit::script::Module model;
// observation buffer
torch::Tensor lin_vel;
torch::Tensor ang_vel;
torch::Tensor gravity_vec;
torch::Tensor commands;
torch::Tensor base_quat;
torch::Tensor dof_pos;
torch::Tensor dof_vel;
torch::Tensor actions;
// output buffer // output buffer
torch::Tensor output_torques; torch::Tensor output_torques;
torch::Tensor output_dof_pos; torch::Tensor output_dof_pos;
// keyboard // getup getdown buffer
KeyBoard keyboard; float getup_percent = 0.0;
float getdown_percent = 0.0;
std::vector<double> start_pos;
std::vector<double> now_pos;
}; };
#endif // RL_SDK_HPP #endif

BIN
src/rl_sar/models/cyberdog1/model.pt
View File

Binary file not shown.

298
src/rl_sar/src/rl_real_a1.cpp
View File

@ -1,52 +1,56 @@
#include "../include/rl_real_a1.hpp" #include "../include/rl_real_a1.hpp"
#define ROBOT_NAME "a1"
// #define PLOT // #define PLOT
// #define CSV_LOGGER // #define CSV_LOGGER
RL_Real rl_sar; RL_Real rl_sar;
RL_Real::RL_Real() : safe(UNITREE_LEGGED_SDK::LeggedType::A1), udp(UNITREE_LEGGED_SDK::LOWLEVEL) RL_Real::RL_Real() : unitree_safe(UNITREE_LEGGED_SDK::LeggedType::A1), unitree_udp(UNITREE_LEGGED_SDK::LOWLEVEL)
{ {
torch::autograd::GradMode::set_enabled(false); // read params from yaml
robot_name = "a1";
ReadYaml(ROBOT_NAME); ReadYaml(robot_name);
udp.InitCmdData(cmd);
start_time = std::chrono::high_resolution_clock::now();
std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + ROBOT_NAME + "/" + this->params.model_name;
this->model = torch::jit::load(model_path);
this->InitObservations();
this->InitOutputs();
// history
this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6); this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6);
unitree_udp.InitCmdData(unitree_low_command);
// init
torch::autograd::GradMode::set_enabled(false);
start_pos.resize(params.num_of_dofs);
now_pos.resize(params.num_of_dofs);
this->InitObservations();
this->InitOutputs();
this->InitKeyboard();
// model
std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + robot_name + "/" + this->params.model_name;
this->model = torch::jit::load(model_path);
// loop
loop_keyboard = std::make_shared<LoopFunc>("loop_keyboard", 0.05 , boost::bind(&RL_Real::RunKeyboard, this));
loop_control = std::make_shared<LoopFunc>("loop_control" , 0.002, boost::bind(&RL_Real::RobotControl, this));
loop_udpSend = std::make_shared<LoopFunc>("loop_udpSend" , 0.002, 3, boost::bind(&RL_Real::UDPSend, this));
loop_udpRecv = std::make_shared<LoopFunc>("loop_udpRecv" , 0.002, 3, boost::bind(&RL_Real::UDPRecv, this));
loop_rl = std::make_shared<LoopFunc>("loop_rl" , 0.02 , boost::bind(&RL_Real::RunModel, this));
loop_keyboard->start();
loop_udpSend->start();
loop_udpRecv->start();
loop_control->start();
loop_rl->start();
#ifdef PLOT
plot_t = std::vector<int>(plot_size, 0); plot_t = std::vector<int>(plot_size, 0);
plot_real_joint_pos.resize(params.num_of_dofs); plot_real_joint_pos.resize(params.num_of_dofs);
plot_target_joint_pos.resize(params.num_of_dofs); plot_target_joint_pos.resize(params.num_of_dofs);
for(auto& vector : plot_real_joint_pos) { vector = std::vector<double>(plot_size, 0); } for(auto& vector : plot_real_joint_pos) { vector = std::vector<double>(plot_size, 0); }
for(auto& vector : plot_target_joint_pos) { vector = std::vector<double>(plot_size, 0); } for(auto& vector : plot_target_joint_pos) { vector = std::vector<double>(plot_size, 0); }
loop_plot = std::make_shared<LoopFunc>("loop_plot" , 0.002, boost::bind(&RL_Real::Plot, this));
loop_control = std::make_shared<UNITREE_LEGGED_SDK::LoopFunc>("loop_control", 0.002, boost::bind(&RL_Real::RobotControl, this));
loop_udpSend = std::make_shared<UNITREE_LEGGED_SDK::LoopFunc>("loop_udpSend", 0.002, 3, boost::bind(&RL_Real::UDPSend, this));
loop_udpRecv = std::make_shared<UNITREE_LEGGED_SDK::LoopFunc>("loop_udpRecv", 0.002, 3, boost::bind(&RL_Real::UDPRecv, this));
loop_rl = std::make_shared<UNITREE_LEGGED_SDK::LoopFunc>("loop_rl" , 0.02 , boost::bind(&RL_Real::RunModel, this));
loop_udpSend->start();
loop_udpRecv->start();
loop_control->start();
#ifdef PLOT
loop_plot = std::make_shared<UNITREE_LEGGED_SDK::LoopFunc>("loop_plot" , 0.002, boost::bind(&RL_Real::Plot, this));
loop_plot->start(); loop_plot->start();
#endif #endif
#ifdef CSV_LOGGER #ifdef CSV_LOGGER
CSVInit(ROBOT_NAME); CSVInit(robot_name);
#endif #endif
} }
@ -62,168 +66,84 @@ RL_Real::~RL_Real()
printf("exit\n"); printf("exit\n");
} }
void RL_Real::GetState(RobotState<double> *state)
{
unitree_udp.GetRecv(unitree_low_state);
memcpy(&unitree_joy, unitree_low_state.wirelessRemote, 40);
if((int)unitree_joy.btn.components.R2 == 1)
{
keyboard.keyboard_state = STATE_POS_GETUP;
}
else if((int)unitree_joy.btn.components.R1 == 1)
{
keyboard.keyboard_state = STATE_RL_INIT;
}
else if((int)unitree_joy.btn.components.L2 == 1)
{
keyboard.keyboard_state = STATE_POS_GETDOWN;
}
for(int i = 0; i < 4; ++i)
{
state->imu.quaternion[i] = unitree_low_state.imu.quaternion[i];
}
for(int i = 0; i < 3; ++i)
{
state->imu.gyroscope[i] = unitree_low_state.imu.gyroscope[i];
}
// state->imu.accelerometer
for(int i = 0; i < params.num_of_dofs; ++i)
{
state->motor_state.q[i] = unitree_low_state.motorState[state_mapping[i]].q;
state->motor_state.dq[i] = unitree_low_state.motorState[state_mapping[i]].dq;
state->motor_state.tauEst[i] = unitree_low_state.motorState[state_mapping[i]].tauEst;
}
}
void RL_Real::SetCommand(const RobotCommand<double> *command)
{
for(int i = 0; i < params.num_of_dofs; ++i)
{
unitree_low_command.motorCmd[i].mode = 0x0A;
unitree_low_command.motorCmd[i].q = command->motor_command.q[command_mapping[i]];
unitree_low_command.motorCmd[i].dq = command->motor_command.dq[command_mapping[i]];
unitree_low_command.motorCmd[i].Kp = command->motor_command.kp[command_mapping[i]];
unitree_low_command.motorCmd[i].Kd = command->motor_command.kd[command_mapping[i]];
unitree_low_command.motorCmd[i].tau = command->motor_command.tau[command_mapping[i]];
}
unitree_safe.PowerProtect(unitree_low_command, unitree_low_state, 8);
// safe.PositionProtect(unitree_low_command, unitree_low_state);
unitree_udp.SetSend(unitree_low_command);
}
void RL_Real::RobotControl() void RL_Real::RobotControl()
{ {
motiontime++; motiontime++;
udp.GetRecv(state);
memcpy(&_keyData, state.wirelessRemote, 40); GetState(&robot_state);
StateController(&robot_state, &robot_command);
// waiting SetCommand(&robot_command);
if(robot_state == STATE_WAITING)
{
for(int i = 0; i < params.num_of_dofs; ++i)
{
cmd.motorCmd[i].q = state.motorState[i].q;
}
if((int)_keyData.btn.components.R2 == 1)
{
getup_percent = 0.0;
for(int i = 0; i < params.num_of_dofs; ++i)
{
now_pos[i] = state.motorState[i].q;
start_pos[i] = now_pos[i];
}
robot_state = STATE_POS_GETUP;
}
}
// stand up (position control)
else if(robot_state == STATE_POS_GETUP)
{
if(getup_percent != 1)
{
getup_percent += 1 / 1000.0;
getup_percent = getup_percent > 1 ? 1 : getup_percent;
for(int i = 0; i < params.num_of_dofs; ++i)
{
cmd.motorCmd[i].mode = 0x0A;
cmd.motorCmd[i].q = (1 - getup_percent) * now_pos[i] + getup_percent * params.default_dof_pos[0][dof_mapping[i]].item<double>();
cmd.motorCmd[i].dq = 0;
cmd.motorCmd[i].Kp = 80;
cmd.motorCmd[i].Kd = 3;
cmd.motorCmd[i].tau = 0;
}
printf("getting up %.3f%%\r", getup_percent * 100.0);
}
if((int)_keyData.btn.components.R1 == 1)
{
robot_state = STATE_RL_INIT;
}
else if((int)_keyData.btn.components.L2 == 1)
{
getdown_percent = 0.0;
for(int i = 0; i < params.num_of_dofs; ++i)
{
now_pos[i] = state.motorState[i].q;
}
robot_state = STATE_POS_GETDOWN;
}
}
// init obs and start rl loop
else if(robot_state == STATE_RL_INIT)
{
if(getup_percent == 1)
{
robot_state = STATE_RL_RUNNING;
this->InitObservations();
this->InitOutputs();
printf("\nstart rl loop\n");
loop_rl->start();
}
}
// rl loop
else if(robot_state == STATE_RL_RUNNING)
{
for(int i = 0; i < params.num_of_dofs; ++i)
{
cmd.motorCmd[i].mode = 0x0A;
// cmd.motorCmd[i].q = 0;
cmd.motorCmd[i].q = output_dof_pos[0][dof_mapping[i]].item<double>();
cmd.motorCmd[i].dq = 0;
// cmd.motorCmd[i].Kp = params.stiffness;
// cmd.motorCmd[i].Kd = params.damping;
cmd.motorCmd[i].Kp = params.p_gains[0][dof_mapping[i]].item<double>();
cmd.motorCmd[i].Kd = params.d_gains[0][dof_mapping[i]].item<double>();
// cmd.motorCmd[i].tau = output_torques[0][dof_mapping[i]].item<double>();
cmd.motorCmd[i].tau = 0;
}
if((int)_keyData.btn.components.L2 == 1)
{
getdown_percent = 0.0;
for(int i = 0; i < params.num_of_dofs; ++i)
{
now_pos[i] = state.motorState[i].q;
}
robot_state = STATE_POS_GETDOWN;
}
}
// get down (position control)
else if(robot_state == STATE_POS_GETDOWN)
{
if(getdown_percent != 1)
{
getdown_percent += 1 / 1000.0;
getdown_percent = getdown_percent > 1 ? 1 : getdown_percent;
for(int i = 0; i < params.num_of_dofs; ++i)
{
cmd.motorCmd[i].mode = 0x0A;
cmd.motorCmd[i].q = (1 - getdown_percent) * now_pos[i] + getdown_percent * start_pos[i];
cmd.motorCmd[i].dq = 0;
cmd.motorCmd[i].Kp = 80;
cmd.motorCmd[i].Kd = 3;
cmd.motorCmd[i].tau = 0;
}
printf("getting down %.3f%%\r", getdown_percent * 100.0);
}
if(getdown_percent == 1)
{
robot_state = STATE_WAITING;
this->InitObservations();
this->InitOutputs();
printf("\nstop rl loop\n");
loop_rl->shutdown();
}
}
safe.PowerProtect(cmd, state, 8);
// safe.PositionProtect(cmd, state);
udp.SetSend(cmd);
} }
void RL_Real::RunModel() void RL_Real::RunModel()
{ {
if(robot_state == STATE_RL_RUNNING) if(running_state == STATE_RL_RUNNING)
{ {
// auto duration = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_time).count(); this->obs.ang_vel = torch::tensor({{unitree_low_state.imu.gyroscope[0], unitree_low_state.imu.gyroscope[1], unitree_low_state.imu.gyroscope[2]}});
// std::cout << "Execution time: " << duration << " microseconds" << std::endl; this->obs.commands = torch::tensor({{unitree_joy.ly, -unitree_joy.rx, -unitree_joy.lx}});
// start_time = std::chrono::high_resolution_clock::now(); this->obs.base_quat = torch::tensor({{unitree_low_state.imu.quaternion[1], unitree_low_state.imu.quaternion[2], unitree_low_state.imu.quaternion[3], unitree_low_state.imu.quaternion[0]}});
this->obs.dof_pos = torch::tensor({{unitree_low_state.motorState[3].q, unitree_low_state.motorState[4].q, unitree_low_state.motorState[5].q,
// printf("%f, %f, %f\n", unitree_low_state.motorState[0].q, unitree_low_state.motorState[1].q, unitree_low_state.motorState[2].q,
// state.imu.gyroscope[0], state.imu.gyroscope[1], state.imu.gyroscope[2]); unitree_low_state.motorState[9].q, unitree_low_state.motorState[10].q, unitree_low_state.motorState[11].q,
// printf("%f, %f, %f, %f\n", unitree_low_state.motorState[6].q, unitree_low_state.motorState[7].q, unitree_low_state.motorState[8].q}});
// state.imu.quaternion[1], state.imu.quaternion[2], state.imu.quaternion[3], state.imu.quaternion[0]); this->obs.dof_vel = torch::tensor({{unitree_low_state.motorState[3].dq, unitree_low_state.motorState[4].dq, unitree_low_state.motorState[5].dq,
// printf("%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n", unitree_low_state.motorState[0].dq, unitree_low_state.motorState[1].dq, unitree_low_state.motorState[2].dq,
// state.motorState[FL_0].q, state.motorState[FL_1].q, state.motorState[FL_2].q, unitree_low_state.motorState[9].dq, unitree_low_state.motorState[10].dq, unitree_low_state.motorState[11].dq,
// state.motorState[FR_0].q, state.motorState[FR_1].q, state.motorState[FR_2].q, unitree_low_state.motorState[6].dq, unitree_low_state.motorState[7].dq, unitree_low_state.motorState[8].dq}});
// state.motorState[RL_0].q, state.motorState[RL_1].q, state.motorState[RL_2].q,
// state.motorState[RR_0].q, state.motorState[RR_1].q, state.motorState[RR_2].q);
// printf("%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n",
// state.motorState[FL_0].dq, state.motorState[FL_1].dq, state.motorState[FL_2].dq,
// state.motorState[FR_0].dq, state.motorState[FR_1].dq, state.motorState[FR_2].dq,
// state.motorState[RL_0].dq, state.motorState[RL_1].dq, state.motorState[RL_2].dq,
// state.motorState[RR_0].dq, state.motorState[RR_1].dq, state.motorState[RR_2].dq);
this->obs.ang_vel = torch::tensor({{state.imu.gyroscope[0], state.imu.gyroscope[1], state.imu.gyroscope[2]}});
this->obs.commands = torch::tensor({{_keyData.ly, -_keyData.rx, -_keyData.lx}});
this->obs.base_quat = torch::tensor({{state.imu.quaternion[1], state.imu.quaternion[2], state.imu.quaternion[3], state.imu.quaternion[0]}});
this->obs.dof_pos = torch::tensor({{state.motorState[3].q, state.motorState[4].q, state.motorState[5].q,
state.motorState[0].q, state.motorState[1].q, state.motorState[2].q,
state.motorState[9].q, state.motorState[10].q, state.motorState[11].q,
state.motorState[6].q, state.motorState[7].q, state.motorState[8].q}});
this->obs.dof_vel = torch::tensor({{state.motorState[3].dq, state.motorState[4].dq, state.motorState[5].dq,
state.motorState[0].dq, state.motorState[1].dq, state.motorState[2].dq,
state.motorState[9].dq, state.motorState[10].dq, state.motorState[11].dq,
state.motorState[6].dq, state.motorState[7].dq, state.motorState[8].dq}});
torch::Tensor actions = this->Forward(); torch::Tensor actions = this->Forward();
@ -235,10 +155,10 @@ void RL_Real::RunModel()
output_torques = this->ComputeTorques(actions); output_torques = this->ComputeTorques(actions);
output_dof_pos = this->ComputePosition(actions); output_dof_pos = this->ComputePosition(actions);
#ifdef CSV_LOGGER #ifdef CSV_LOGGER
torch::Tensor tau_est = torch::tensor({{state.motorState[3].tauEst, state.motorState[4].tauEst, state.motorState[5].tauEst, torch::Tensor tau_est = torch::tensor({{unitree_low_state.motorState[3].tauEst, unitree_low_state.motorState[4].tauEst, unitree_low_state.motorState[5].tauEst,
state.motorState[0].tauEst, state.motorState[1].tauEst, state.motorState[2].tauEst, unitree_low_state.motorState[0].tauEst, unitree_low_state.motorState[1].tauEst, unitree_low_state.motorState[2].tauEst,
state.motorState[9].tauEst, state.motorState[10].tauEst, state.motorState[11].tauEst, unitree_low_state.motorState[9].tauEst, unitree_low_state.motorState[10].tauEst, unitree_low_state.motorState[11].tauEst,
state.motorState[6].tauEst, state.motorState[7].tauEst, state.motorState[8].tauEst}}); unitree_low_state.motorState[6].tauEst, unitree_low_state.motorState[7].tauEst, unitree_low_state.motorState[8].tauEst}});
CSVLogger(output_torques, tau_est, this->obs.dof_pos, output_dof_pos, this->obs.dof_vel); CSVLogger(output_torques, tau_est, this->obs.dof_pos, output_dof_pos, this->obs.dof_vel);
#endif #endif
} }
@ -283,8 +203,8 @@ void RL_Real::Plot()
{ {
plot_real_joint_pos[i].erase(plot_real_joint_pos[i].begin()); plot_real_joint_pos[i].erase(plot_real_joint_pos[i].begin());
plot_target_joint_pos[i].erase(plot_target_joint_pos[i].begin()); plot_target_joint_pos[i].erase(plot_target_joint_pos[i].begin());
plot_real_joint_pos[i].push_back(state.motorState[i].q); plot_real_joint_pos[i].push_back(unitree_low_state.motorState[i].q);
plot_target_joint_pos[i].push_back(cmd.motorCmd[i].q); plot_target_joint_pos[i].push_back(unitree_low_command.motorCmd[i].q);
plt::subplot(4, 3, i + 1); plt::subplot(4, 3, i + 1);
plt::named_plot("_real_joint_pos", plot_t, plot_real_joint_pos[i], "r"); plt::named_plot("_real_joint_pos", plot_t, plot_real_joint_pos[i], "r");
plt::named_plot("_target_joint_pos", plot_t, plot_target_joint_pos[i], "b"); plt::named_plot("_target_joint_pos", plot_t, plot_target_joint_pos[i], "b");

314
src/rl_sar/src/rl_real_cyberdog.cpp
View File

@ -1,314 +0,0 @@
#include "../include/rl_real_cyberdog.hpp"
#define ROBOT_NAME "cyberdog1"
// #define PLOT
// #define CSV_LOGGER
RL_Real rl_sar;
RL_Real::RL_Real() : CustomInterface(500)
{
torch::autograd::GradMode::set_enabled(false);
ReadYaml(ROBOT_NAME);
start_time = std::chrono::high_resolution_clock::now();
std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + ROBOT_NAME + "/" + this->params.model_name;
this->model = torch::jit::load(model_path);
this->InitObservations();
this->InitOutputs();
this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6);
plot_t = std::vector<int>(plot_size, 0);
plot_real_joint_pos.resize(params.num_of_dofs);
plot_target_joint_pos.resize(params.num_of_dofs);
for(auto& vector : plot_real_joint_pos) { vector = std::vector<double>(plot_size, 0); }
for(auto& vector : plot_target_joint_pos) { vector = std::vector<double>(plot_size, 0); }
loop_control = std::make_shared<UNITREE_LEGGED_SDK::LoopFunc>("loop_control", 0.002, boost::bind(&RL_Real::RobotControl, this));
loop_rl = std::make_shared<UNITREE_LEGGED_SDK::LoopFunc>("loop_rl" , 0.02 , boost::bind(&RL_Real::RunModel, this));
loop_control->start();
#ifdef PLOT
loop_plot = std::make_shared<UNITREE_LEGGED_SDK::LoopFunc>("loop_plot" , 0.002, boost::bind(&RL_Real::Plot, this));
loop_plot->start();
#endif
_keyboardThread = std::thread(&RL_Real::run_keyboard, this);
#ifdef CSV_LOGGER
CSVInit(ROBOT_NAME);
#endif
}
RL_Real::~RL_Real()
{
loop_control->shutdown();
loop_rl->shutdown();
#ifdef PLOT
loop_plot->shutdown();
#endif
printf("exit\n");
}
void RL_Real::RobotControl()
{
// printf("%f, %f, %f\n",
// cyberdogData.omega[0], cyberdogData.omega[1], cyberdogData.omega[2]);
// printf("%f, %f, %f, %f\n",
// cyberdogData.quat[1], cyberdogData.quat[2], cyberdogData.quat[3], cyberdogData.quat[0]);
// printf("%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n",
// cyberdogData.q[3], cyberdogData.q[4], cyberdogData.q[5],
// cyberdogData.q[0], cyberdogData.q[1], cyberdogData.q[2],
// cyberdogData.q[9], cyberdogData.q[10], cyberdogData.q[11],
// cyberdogData.q[6], cyberdogData.q[7], cyberdogData.q[8]);
// printf("%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n",
// cyberdogData.qd[3], cyberdogData.qd[4], cyberdogData.qd[5],
// cyberdogData.qd[0], cyberdogData.qd[1], cyberdogData.qd[2],
// cyberdogData.qd[9], cyberdogData.qd[10], cyberdogData.qd[11],
// cyberdogData.qd[6], cyberdogData.qd[7], cyberdogData.qd[8]);
std::cout << "robot_state" << keyboard.robot_state
<< " x" << keyboard.x << " y" << keyboard.y << " yaw" << keyboard.yaw
<< "\r";
motiontime++;
// waiting
if(robot_state == STATE_WAITING)
{
for(int i = 0; i < params.num_of_dofs; ++i)
{
cyberdogCmd.q_des[i] = cyberdogData.q[i];
}
if(keyboard.robot_state == STATE_POS_GETUP)
{
keyboard.robot_state = STATE_WAITING;
getup_percent = 0.0;
for(int i = 0; i < params.num_of_dofs; ++i)
{
now_pos[i] = cyberdogData.q[i];
start_pos[i] = now_pos[i];
}
robot_state = STATE_POS_GETUP;
}
}
// stand up (position control)
else if(robot_state == STATE_POS_GETUP)
{
if(getup_percent != 1)
{
getup_percent += 1 / 1000.0;
getup_percent = getup_percent > 1 ? 1 : getup_percent;
for(int i = 0; i < params.num_of_dofs; ++i)
{
cyberdogCmd.q_des[i] = (1 - getup_percent) * now_pos[i] + getup_percent * params.default_dof_pos[0][dof_mapping[i]].item<double>();
cyberdogCmd.qd_des[i] = 0;
cyberdogCmd.kp_des[i] = 200;
cyberdogCmd.kd_des[i] = 10;
cyberdogCmd.tau_des[i] = 0;
}
printf("getting up %.3f%%\r", getup_percent*100.0);
}
if(keyboard.robot_state == STATE_RL_INIT)
{
keyboard.robot_state = STATE_WAITING;
robot_state = STATE_RL_INIT;
}
else if(keyboard.robot_state == STATE_POS_GETDOWN)
{
keyboard.robot_state = STATE_WAITING;
getdown_percent = 0.0;
for(int i = 0; i < params.num_of_dofs; ++i)
{
now_pos[i] = cyberdogData.q[i];
}
robot_state = STATE_POS_GETDOWN;
}
}
// init obs and start rl loop
else if(robot_state == STATE_RL_INIT)
{
if(getup_percent == 1)
{
robot_state = STATE_RL_RUNNING;
this->InitObservations();
this->InitOutputs();
printf("\nstart rl loop\n");
loop_rl->start();
}
}
// rl loop
else if(robot_state == STATE_RL_RUNNING)
{
for(int i = 0; i < params.num_of_dofs; ++i)
{
// cyberdogCmd.q_des[i] = 0;
cyberdogCmd.q_des[i] = output_dof_pos[0][dof_mapping[i]].item<double>();
cyberdogCmd.qd_des[i] = 0;
// cyberdogCmd.kp_des[i] = params.stiffness;
// cyberdogCmd.kd_des[i] = params.damping;
cyberdogCmd.kp_des[i] = params.p_gains[0][dof_mapping[i]].item<double>();
cyberdogCmd.kd_des[i] = params.d_gains[0][dof_mapping[i]].item<double>();
// cyberdogCmd.tau_des[i] = output_torques[0][dof_mapping[i]].item<double>();
cyberdogCmd.tau_des[i] = 0;
}
if(keyboard.robot_state == STATE_POS_GETDOWN)
{
keyboard.robot_state = STATE_WAITING;
getdown_percent = 0.0;
for(int i = 0; i < params.num_of_dofs; ++i)
{
now_pos[i] = cyberdogData.q[i];
}
robot_state = STATE_POS_GETDOWN;
}
}
// get down (position control)
else if(robot_state == STATE_POS_GETDOWN)
{
if(getdown_percent != 1)
{
getdown_percent += 1 / 1000.0;
getdown_percent = getdown_percent > 1 ? 1 : getdown_percent;
for(int i = 0; i < params.num_of_dofs; ++i)
{
cyberdogCmd.q_des[i] = (1 - getdown_percent) * now_pos[i] + getdown_percent * start_pos[i];
cyberdogCmd.qd_des[i] = 0;
cyberdogCmd.kp_des[i] = 200;
cyberdogCmd.kd_des[i] = 10;
cyberdogCmd.tau_des[i] = 0;
}
printf("getting down %.3f%%\r", getdown_percent*100.0);
}
if(getdown_percent == 1)
{
robot_state = STATE_WAITING;
this->InitObservations();
this->InitOutputs();
printf("\nstop rl loop\n");
loop_rl->shutdown();
}
}
}
void RL_Real::UserCode()
{
cyberdogData = robot_data;
motor_cmd = cyberdogCmd;
}
void RL_Real::RunModel()
{
if(robot_state == STATE_RL_RUNNING)
{
// auto duration = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_time).count();
// std::cout << "Execution time: " << duration << " microseconds" << std::endl;
// start_time = std::chrono::high_resolution_clock::now();
this->obs.ang_vel = torch::tensor({{cyberdogData.omega[0], cyberdogData.omega[1], cyberdogData.omega[2]}});
this->obs.commands = torch::tensor({{keyboard.x, keyboard.y, keyboard.yaw}});
this->obs.base_quat = torch::tensor({{cyberdogData.quat[1], cyberdogData.quat[2], cyberdogData.quat[3], cyberdogData.quat[0]}});
this->obs.dof_pos = torch::tensor({{cyberdogData.q[3], cyberdogData.q[4], cyberdogData.q[5],
cyberdogData.q[0], cyberdogData.q[1], cyberdogData.q[2],
cyberdogData.q[9], cyberdogData.q[10], cyberdogData.q[11],
cyberdogData.q[6], cyberdogData.q[7], cyberdogData.q[8]}});
this->obs.dof_vel = torch::tensor({{cyberdogData.qd[3], cyberdogData.qd[4], cyberdogData.qd[5],
cyberdogData.qd[0], cyberdogData.qd[1], cyberdogData.qd[2],
cyberdogData.qd[9], cyberdogData.qd[10], cyberdogData.qd[11],
cyberdogData.qd[6], cyberdogData.qd[7], cyberdogData.qd[8]}});
torch::Tensor actions = this->Forward();
for (int i : hip_scale_reduction_indices)
{
actions[0][i] *= this->params.hip_scale_reduction;
}
output_torques = this->ComputeTorques(actions);
output_dof_pos = this->ComputePosition(actions);
#ifdef CSV_LOGGER
torch::Tensor tau_est = torch::tensor({{cyberdogData.tau[3], cyberdogData.tau[4], cyberdogData.tau[5],
cyberdogData.tau[0], cyberdogData.tau[1], cyberdogData.tau[2],
cyberdogData.tau[9], cyberdogData.tau[10], cyberdogData.tau[11],
cyberdogData.tau[6], cyberdogData.tau[7], cyberdogData.tau[8]}});
CSVLogger(output_torques, tau_est, this->obs.dof_pos, output_dof_pos, this->obs.dof_vel);
#endif
}
}
torch::Tensor RL_Real::ComputeObservation()
{
torch::Tensor obs = torch::cat({// this->QuatRotateInverse(this->obs.base_quat, this->obs.lin_vel) * this->params.lin_vel_scale,
this->QuatRotateInverse(this->obs.base_quat, this->obs.ang_vel) * this->params.ang_vel_scale,
this->QuatRotateInverse(this->obs.base_quat, this->obs.gravity_vec),
this->obs.commands * this->params.commands_scale,
(this->obs.dof_pos - this->params.default_dof_pos) * this->params.dof_pos_scale,
this->obs.dof_vel * this->params.dof_vel_scale,
this->obs.actions
},1);
obs = torch::clamp(obs, -this->params.clip_obs, this->params.clip_obs);
return obs;
}
torch::Tensor RL_Real::Forward()
{
torch::Tensor obs = this->ComputeObservation();
history_obs_buf.insert(obs);
history_obs = history_obs_buf.get_obs_vec({0, 1, 2, 3, 4, 5});
torch::Tensor action = this->model.forward({history_obs}).toTensor();
this->obs.actions = action;
torch::Tensor clamped = torch::clamp(action, -this->params.clip_actions, this->params.clip_actions);
return clamped;
}
void RL_Real::Plot()
{
plot_t.erase(plot_t.begin());
plot_t.push_back(motiontime);
plt::cla();
plt::clf();
for(int i = 0; i < params.num_of_dofs; ++i)
{
plot_real_joint_pos[i].erase(plot_real_joint_pos[i].begin());
plot_target_joint_pos[i].erase(plot_target_joint_pos[i].begin());
plot_real_joint_pos[i].push_back(cyberdogData.q[i]);
plot_target_joint_pos[i].push_back(cyberdogCmd.q_des[i]);
plt::subplot(4, 3, i+1);
plt::named_plot("_real_joint_pos", plot_t, plot_real_joint_pos[i], "r");
plt::named_plot("_target_joint_pos", plot_t, plot_target_joint_pos[i], "b");
plt::xlim(plot_t.front(), plot_t.back());
}
// plt::legend();
plt::pause(0.0001);
}
void signalHandler(int signum)
{
system("ssh root@192.168.55.233 \"ps | grep -E 'manager|ctrl|imu_online' | grep -v grep | awk '{print \\$1}' | xargs kill -9\"");
system("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 &\"");
exit(0);
}
int main(int argc, char **argv)
{
signal(SIGINT, signalHandler);
while(1)
{
sleep(10);
};
return 0;
}

136
src/rl_sar/src/rl_sim.cpp
View File

@ -1,17 +1,22 @@
#include "../include/rl_sim.hpp" #include "../include/rl_sim.hpp"
// #define ROBOT_NAME "a1"
#define ROBOT_NAME "gr1t1"
// #define PLOT // #define PLOT
// #define CSV_LOGGER // #define CSV_LOGGER
// #define USE_HISTORY
RL_Sim::RL_Sim() RL_Sim::RL_Sim()
{ {
torch::autograd::GradMode::set_enabled(false); ros::NodeHandle nh;
ReadYaml(ROBOT_NAME); // read params from yaml
nh.param<std::string>("robot_name", robot_name, "");
ReadYaml(robot_name);
// history
nh.param<bool>("use_history", use_history, "");
if(use_history)
{
this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6);
}
// Due to the fact that the robot_state_publisher sorts the joint names alphabetically, // Due to the fact that the robot_state_publisher sorts the joint names alphabetically,
// the mapping table is established according to the order defined in the YAML file // the mapping table is established according to the order defined in the YAML file
@ -21,66 +26,56 @@ RL_Sim::RL_Sim()
{ {
sorted_to_original_index[sorted_joint_names[i]] = i; sorted_to_original_index[sorted_joint_names[i]] = i;
} }
mapped_joint_positions = std::vector<double>(params.num_of_dofs, 0.0);
mapped_joint_velocities = std::vector<double>(params.num_of_dofs, 0.0);
mapped_joint_efforts = std::vector<double>(params.num_of_dofs, 0.0);
ros::NodeHandle nh; // init
start_time = std::chrono::high_resolution_clock::now(); torch::autograd::GradMode::set_enabled(false);
cmd_vel = geometry_msgs::Twist();
motor_commands.resize(params.num_of_dofs); motor_commands.resize(params.num_of_dofs);
start_pos.resize(params.num_of_dofs); start_pos.resize(params.num_of_dofs);
now_pos.resize(params.num_of_dofs); now_pos.resize(params.num_of_dofs);
std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + ROBOT_NAME + "/" + this->params.model_name;
this->model = torch::jit::load(model_path);
this->InitObservations(); this->InitObservations();
this->InitOutputs(); this->InitOutputs();
this->InitKeyboard(); this->InitKeyboard();
#ifdef USE_HISTORY // model
this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6); std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + robot_name + "/" + this->params.model_name;
#endif this->model = torch::jit::load(model_path);
joint_positions = std::vector<double>(params.num_of_dofs, 0.0); // publisher
joint_velocities = std::vector<double>(params.num_of_dofs, 0.0); nh.param<std::string>("ros_namespace", ros_namespace, "");
joint_efforts = std::vector<double>(params.num_of_dofs, 0.0); for (int i = 0; i < params.num_of_dofs; ++i)
{
// joint need to rename as xxx_joint
torque_publishers[params.joint_names[i]] = nh.advertise<robot_msgs::MotorCommand>(
ros_namespace + params.joint_names[i].substr(0, params.joint_names[i].size() - 6) + "_controller/command", 10);
}
// subscriber
cmd_vel_subscriber = nh.subscribe<geometry_msgs::Twist>("/cmd_vel", 10, &RL_Sim::CmdvelCallback, this);
model_state_subscriber = nh.subscribe<gazebo_msgs::ModelStates>("/gazebo/model_states", 10, &RL_Sim::ModelStatesCallback, this);
joint_state_subscriber = nh.subscribe<sensor_msgs::JointState>(ros_namespace + "joint_states", 10, &RL_Sim::JointStatesCallback, this);
// loop
loop_keyboard = std::make_shared<LoopFunc>("loop_keyboard", 0.05 , boost::bind(&RL_Sim::RunKeyboard, this));
loop_control = std::make_shared<LoopFunc>("loop_control" , 0.002, boost::bind(&RL_Sim::RobotControl, this));
loop_rl = std::make_shared<LoopFunc>("loop_rl" , 0.02 , boost::bind(&RL_Sim::RunModel, this));
loop_keyboard->start();
loop_control->start();
loop_rl->start();
#ifdef PLOT
plot_t = std::vector<int>(plot_size, 0); plot_t = std::vector<int>(plot_size, 0);
plot_real_joint_pos.resize(params.num_of_dofs); plot_real_joint_pos.resize(params.num_of_dofs);
plot_target_joint_pos.resize(params.num_of_dofs); plot_target_joint_pos.resize(params.num_of_dofs);
for(auto& vector : plot_real_joint_pos) { vector = std::vector<double>(plot_size, 0); } for(auto& vector : plot_real_joint_pos) { vector = std::vector<double>(plot_size, 0); }
for(auto& vector : plot_target_joint_pos) { vector = std::vector<double>(plot_size, 0); } for(auto& vector : plot_target_joint_pos) { vector = std::vector<double>(plot_size, 0); }
loop_plot = std::make_shared<LoopFunc>("loop_plot" , 0.002, boost::bind(&RL_Sim::Plot, this));
cmd_vel_subscriber_ = nh.subscribe<geometry_msgs::Twist>("/cmd_vel", 10, &RL_Sim::CmdvelCallback, this);
nh.param<std::string>("ros_namespace", ros_namespace, "");
for (int i = 0; i < params.num_of_dofs; ++i)
{
torque_publishers[params.joint_names[i]] = nh.advertise<robot_msgs::MotorCommand>(
ros_namespace + params.joint_names[i].substr(0, params.joint_names[i].size() - 6) + "_controller/command", 10);
}
model_state_subscriber_ = nh.subscribe<gazebo_msgs::ModelStates>(
"/gazebo/model_states", 10, &RL_Sim::ModelStatesCallback, this);
joint_state_subscriber_ = nh.subscribe<sensor_msgs::JointState>(
ros_namespace + "joint_states", 10, &RL_Sim::JointStatesCallback, this);
loop_control = std::make_shared<UNITREE_LEGGED_SDK::LoopFunc>("loop_control", 0.002, boost::bind(&RL_Sim::RobotControl, this));
loop_rl = std::make_shared<UNITREE_LEGGED_SDK::LoopFunc>("loop_rl" , 0.02 , boost::bind(&RL_Sim::RunModel, this));
loop_control->start();
loop_rl->start();
#ifdef PLOT
loop_plot = std::make_shared<UNITREE_LEGGED_SDK::LoopFunc>("loop_plot" , 0.002, boost::bind(&RL_Sim::Plot, this));
loop_plot->start(); loop_plot->start();
#endif #endif
_keyboardThread = std::thread(&RL_Sim::run_keyboard, this);
#ifdef CSV_LOGGER #ifdef CSV_LOGGER
CSVInit(ROBOT_NAME); CSVInit(robot_name);
#endif #endif
} }
@ -109,9 +104,9 @@ void RL_Sim::GetState(RobotState<double> *state)
for(int i = 0; i < params.num_of_dofs; ++i) for(int i = 0; i < params.num_of_dofs; ++i)
{ {
state->motor_state.q[i] = joint_positions[i]; state->motor_state.q[i] = mapped_joint_positions[i];
state->motor_state.dq[i] = joint_velocities[i]; state->motor_state.dq[i] = mapped_joint_velocities[i];
state->motor_state.tauEst[i] = joint_efforts[i]; state->motor_state.tauEst[i] = mapped_joint_efforts[i];
} }
} }
@ -136,7 +131,7 @@ void RL_Sim::RobotControl()
{ {
std::cout << "running_state " << keyboard.keyboard_state std::cout << "running_state " << keyboard.keyboard_state
<< " x" << keyboard.x << " y" << keyboard.y << " yaw" << keyboard.yaw << " x" << keyboard.x << " y" << keyboard.y << " yaw" << keyboard.yaw
<< " \r"; << " \r";
motiontime++; motiontime++;
@ -166,9 +161,9 @@ void RL_Sim::MapData(const std::vector<double>& source_data, std::vector<double>
void RL_Sim::JointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) void RL_Sim::JointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)
{ {
MapData(msg->position, joint_positions); MapData(msg->position, mapped_joint_positions);
MapData(msg->velocity, joint_velocities); MapData(msg->velocity, mapped_joint_velocities);
MapData(msg->effort, joint_efforts); MapData(msg->effort, mapped_joint_efforts);
} }
void RL_Sim::RunModel() void RL_Sim::RunModel()
@ -180,8 +175,8 @@ void RL_Sim::RunModel()
// this->obs.commands = torch::tensor({{cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z}}); // this->obs.commands = torch::tensor({{cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z}});
this->obs.commands = torch::tensor({{keyboard.x, keyboard.y, keyboard.yaw}}); this->obs.commands = torch::tensor({{keyboard.x, keyboard.y, keyboard.yaw}});
this->obs.base_quat = torch::tensor({{pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w}}); this->obs.base_quat = torch::tensor({{pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w}});
this->obs.dof_pos = torch::tensor(joint_positions).unsqueeze(0); this->obs.dof_pos = torch::tensor(mapped_joint_positions).unsqueeze(0);
this->obs.dof_vel = torch::tensor(joint_velocities).unsqueeze(0); this->obs.dof_vel = torch::tensor(mapped_joint_velocities).unsqueeze(0);
torch::Tensor clamped_actions = this->Forward(); torch::Tensor clamped_actions = this->Forward();
@ -194,7 +189,7 @@ void RL_Sim::RunModel()
output_dof_pos = this->ComputePosition(clamped_actions); output_dof_pos = this->ComputePosition(clamped_actions);
#ifdef CSV_LOGGER #ifdef CSV_LOGGER
torch::Tensor tau_est = torch::tensor(joint_efforts).unsqueeze(0); torch::Tensor tau_est = torch::tensor(mapped_joint_efforts).unsqueeze(0);
CSVLogger(output_torques, tau_est, this->obs.dof_pos, output_dof_pos, this->obs.dof_vel); CSVLogger(output_torques, tau_est, this->obs.dof_pos, output_dof_pos, this->obs.dof_vel);
#endif #endif
} }
@ -216,15 +211,22 @@ torch::Tensor RL_Sim::ComputeObservation()
torch::Tensor RL_Sim::Forward() torch::Tensor RL_Sim::Forward()
{ {
torch::autograd::GradMode::set_enabled(false);
torch::Tensor obs = this->ComputeObservation(); torch::Tensor obs = this->ComputeObservation();
#ifdef USE_HISTORY torch::Tensor actions;
history_obs_buf.insert(obs);
history_obs = history_obs_buf.get_obs_vec({0, 1, 2, 3, 4, 5}); if(use_history)
torch::Tensor action = this->model.forward({history_obs}).toTensor(); {
#else history_obs_buf.insert(obs);
torch::Tensor actions = this->model.forward({obs}).toTensor(); history_obs = history_obs_buf.get_obs_vec({0, 1, 2, 3, 4, 5});
#endif actions = this->model.forward({history_obs}).toTensor();
}
else
{
actions = this->model.forward({obs}).toTensor();
}
this->obs.actions = actions; this->obs.actions = actions;
torch::Tensor clamped_actions = torch::clamp(actions, -this->params.clip_actions, this->params.clip_actions); torch::Tensor clamped_actions = torch::clamp(actions, -this->params.clip_actions, this->params.clip_actions);
@ -242,7 +244,7 @@ void RL_Sim::Plot()
{ {
plot_real_joint_pos[i].erase(plot_real_joint_pos[i].begin()); plot_real_joint_pos[i].erase(plot_real_joint_pos[i].begin());
plot_target_joint_pos[i].erase(plot_target_joint_pos[i].begin()); plot_target_joint_pos[i].erase(plot_target_joint_pos[i].begin());
plot_real_joint_pos[i].push_back(joint_positions[i]); plot_real_joint_pos[i].push_back(mapped_joint_positions[i]);
plot_target_joint_pos[i].push_back(motor_commands[i].q); plot_target_joint_pos[i].push_back(motor_commands[i].q);
plt::subplot(4, 3, i+1); plt::subplot(4, 3, i+1);
plt::named_plot("_real_joint_pos", plot_t, plot_real_joint_pos[i], "r"); plt::named_plot("_real_joint_pos", plot_t, plot_real_joint_pos[i], "r");

19
src/robots/cyberdog1_description/CMakeLists.txt
View File

@ -1,19 +0,0 @@
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}
)

201
src/robots/cyberdog1_description/LICENCE
View File

@ -1,201 +0,0 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright 2024 Fan Ziqi
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.

2
src/robots/cyberdog1_description/README.md
View File

@ -1,2 +0,0 @@
# cyberdog1_description
URDF model for the first generation Cyberdog (unofficial)

70
src/robots/cyberdog1_description/config/robot_control.yaml
View File

@ -1,70 +0,0 @@
cyberdog_gazebo:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 1000
# FL Controllers ---------------------------------------
FL_hip_controller:
type: robot_joint_controller/RobotJointController
joint: FL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
FL_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: FL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
FL_calf_controller:
type: robot_joint_controller/RobotJointController
joint: FL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# FR Controllers ---------------------------------------
FR_hip_controller:
type: robot_joint_controller/RobotJointController
joint: FR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
FR_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: FR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
FR_calf_controller:
type: robot_joint_controller/RobotJointController
joint: FR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# RL Controllers ---------------------------------------
RL_hip_controller:
type: robot_joint_controller/RobotJointController
joint: RL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
RL_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: RL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
RL_calf_controller:
type: robot_joint_controller/RobotJointController
joint: RL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# RR Controllers ---------------------------------------
RR_hip_controller:
type: robot_joint_controller/RobotJointController
joint: RR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
RR_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: RR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
RR_calf_controller:
type: robot_joint_controller/RobotJointController
joint: RR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}

21
src/robots/cyberdog1_description/launch/display.launch
View File

@ -1,21 +0,0 @@
<launch>
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find cyberdog_description)/xacro/robot.xacro'"/>
<!-- send fake joint values -->
<!-- <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
<param name="use_gui" value="TRUE"/>
</node> -->
<node
name="joint_state_publisher_gui"
pkg="joint_state_publisher_gui"
type="joint_state_publisher_gui" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="1000.0"/>
</node>
<node pkg="rviz" type="rviz" name="rviz" respawn="false" output="screen"
args="-d $(find cyberdog_description)/launch/test.rviz"/>
</launch>

19
src/robots/cyberdog1_description/launch/gazebo.launch
View File

@ -1,19 +0,0 @@
<launch>
<include file="$(find gazebo_ros)/launch/empty_world.launch" />
<node
name="tf_footprint_base"
pkg="tf"
type="static_transform_publisher"
args="0 0 0 0 0 0 base_link base_footprint 40" />
<node
name="spawn_model"
pkg="gazebo_ros"
type="spawn_model"
args="-file $(find cyberdog_description)/urdf/cyberdog_description.urdf -urdf -model cyberdog_description -z 0.6 -unpause"
output="screen" />
<node
name="fake_joint_calibration"
pkg="rostopic"
type="rostopic"
args="pub /calibrated std_msgs/Bool true" />
</launch>

245
src/robots/cyberdog1_description/launch/test.rviz
View File

@ -1,245 +0,0 @@
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: <Fixed 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: <Fixed 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

14
src/robots/cyberdog1_description/package.xml
View File

@ -1,14 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>cyberdog_description</name>
<version>2.0.0</version>
<description>The a1_description package</description>
<maintainer email="laikago@unitree.cc">unitree</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>std_msgs</depend>
</package>

1387
src/robots/cyberdog1_description/urdf/cyberdog_description.urdf
View File

File diff suppressed because it is too large Load Diff

796
src/robots/cyberdog1_description/urdf_for_isaacgym/cyberdog.urdf
View File

@ -1,796 +0,0 @@
<?xml version="1.0" ?>
<robot name="cyberdog_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="silver">
<color rgba="0.913725490196 0.913725490196 0.847058823529 1.0"/>
</material>
<material name="orange">
<color rgba="0.12 0.15 0.2 1.0"/>
</material>
<material name="brown">
<color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<link name="base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base"/>
<child link="trunk"/>
</joint>
<link name="trunk">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.36 0.23 0.15"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.36 0.23 0.15"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.018785003 0.003291012 -0.000925646"/>
<mass value="6.248410872"/>
<inertia ixx="0.032143039" ixy="-0.000268204" ixz="-0.00241341" iyy="0.13578316" iyz="8.0244e-05" izz="0.14828095"/>
</inertial>
</link>
<joint name="imu_joint" type="fixed">
<parent link="trunk"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<material name="red"/>
</visual>
<!-- <collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision> -->
</link>
<joint name="FR_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="0.23035 -0.05 0"/>
<parent link="trunk"/>
<child link="FR_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="24" lower="-0.76096355387" upper="0.76096355387" velocity="31"/>
</joint>
<link name="FR_hip">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.052" radius="0.047"/>
</geometry>
<material name="silver"/>
</visual>
<!-- <collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.052" radius="0.047"/>
</geometry>
</collision> -->
<inertial>
<origin rpy="0 0 0" xyz="-0.001597914 -0.001680938 2.253e-06"/>
<mass value="0.537806728"/>
<inertia ixx="0.000403612" ixy="9.337e-06" ixz="5.36e-07" iyy="0.000600779" iyz="6.3e-08" izz="0.000417249"/>
</inertial>
</link>
<joint name="FR_hip_rotor_fix" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="-0.0832504 0 0"/>
<parent link="FR_hip"/>
<child link="FR_hip_rotor"/>
</joint>
<link name="FR_hip_rotor">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.076506382"/>
<inertia ixx="4.5843e-05" ixy="0" ixz="0" iyy="4.5871e-05" iyz="0" izz="8.9031e-05"/>
</inertial>
</link>
<joint name="FR_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 -0.053076000000000005 0"/>
<parent link="FR_hip"/>
<child link="FR_thigh_shoulder"/>
</joint>
<link name="FR_thigh_shoulder">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.052" radius="0.047"/>
</geometry>
<material name="silver"/>
</visual>
<!-- <collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.052" radius="0.047"/>
</geometry>
</collision> -->
</link>
<joint name="FR_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.105076 0"/>
<parent link="FR_hip"/>
<child link="FR_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="24" lower="-1.88495559215" upper="3.76991118431" velocity="31"/>
</joint>
<link name="FR_thigh">
<visual>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1"/>
<geometry>
<box size="0.2 0.035 0.05"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1"/>
<geometry>
<box size="0.2 0.035 0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003205778 0.027811623 -0.027767048"/>
<mass value="0.950266218"/>
<inertia ixx="0.004811898" ixy="-9.3647e-05" ixz="-0.000208709" iyy="0.004752366" iyz="-0.000813258" izz="0.001079691"/>
</inertial>
</link>
<joint name="FR_thigh_rotor_fix" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 0.107226 0"/>
<parent link="FR_thigh"/>
<child link="FR_thigh_rotor"/>
</joint>
<link name="FR_thigh_rotor">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.076506382"/>
<inertia ixx="4.5843e-05" ixy="0" ixz="0" iyy="4.5871e-05" iyz="0" izz="8.9031e-05"/>
</inertial>
</link>
<joint name="FR_calf_rotor_fix" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 0.0426259 0"/>
<parent link="FR_thigh"/>
<child link="FR_calf_rotor"/>
</joint>
<link name="FR_calf_rotor">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.076506382"/>
<inertia ixx="4.5843e-05" ixy="0" ixz="0" iyy="4.5871e-05" iyz="0" izz="8.9031e-05"/>
</inertial>
</link>
<joint name="FR_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
<parent link="FR_thigh"/>
<child link="FR_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="24" lower="-2.49582083035" upper="-0.717155789644" velocity="31"/>
</joint>
<link name="FR_calf">
<visual>
<origin rpy="0 3.141592653589793 0" xyz="0 0 -0.1045"/>
<geometry>
<box size="0.016 0.016 0.209"/>
</geometry>
<material name="silver"/>
</visual>
<visual name="FR_calf_rubber">
<origin rpy="0 3.141592653589793 0" xyz="-0.0125 0 -0.007"/>
<geometry>
<sphere radius="0.016"/>
</geometry>
</visual>
<collision>
<origin rpy="0 3.141592653589793 0" xyz="0 0 -0.1045"/>
<geometry>
<box size="0.016 0.016 0.209"/>
</geometry>
</collision>
<collision name="FR_calf_rubber">
<origin rpy="0 3.141592653589793 0" xyz="-0.0125 0 -0.007"/>
<geometry>
<sphere radius="0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.007372358 0.000122423 -0.10769301"/>
<mass value="0.1135916"/>
<inertia ixx="0.000667971" ixy="-1e-09" ixz="5.356e-06" iyy="0.000674399" iyz="-1.5e-08" izz="1.4181e-05"/>
</inertial>
</link>
<joint name="FR_foot_fixed" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 0 -0.19999999999999998"/>
<parent link="FR_calf"/>
<child link="FR_foot"/>
</joint>
<link name="FR_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.018"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.018"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
</inertial>
</link>
<joint name="FL_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="0.23035 0.05 0"/>
<parent link="trunk"/>
<child link="FL_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="24" lower="-0.76096355387" upper="0.76096355387" velocity="31"/>
</joint>
<link name="FL_hip">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.052" radius="0.047"/>
</geometry>
<material name="silver"/>
</visual>
<!-- <collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.052" radius="0.047"/>
</geometry>
</collision> -->
<inertial>
<origin rpy="0 0 0" xyz="-0.001597914 0.001680938 2.253e-06"/>
<mass value="0.537806728"/>
<inertia ixx="0.000403612" ixy="-9.337e-06" ixz="5.36e-07" iyy="0.000600779" iyz="-6.3e-08" izz="0.000417249"/>
</inertial>
</link>
<joint name="FL_hip_rotor_fix" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="-0.0832504 0 0"/>
<parent link="FL_hip"/>
<child link="FL_hip_rotor"/>
</joint>
<link name="FL_hip_rotor">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.076506382"/>
<inertia ixx="4.5843e-05" ixy="0" ixz="0" iyy="4.5871e-05" iyz="0" izz="8.9031e-05"/>
</inertial>
</link>
<joint name="FL_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0.053076000000000005 0"/>
<parent link="FL_hip"/>
<child link="FL_thigh_shoulder"/>
</joint>
<link name="FL_thigh_shoulder">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.052" radius="0.047"/>
</geometry>
<material name="silver"/>
</visual>
<!-- <collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.052" radius="0.047"/>
</geometry>
</collision> -->
</link>
<joint name="FL_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0.105076 0"/>
<parent link="FL_hip"/>
<child link="FL_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="24" lower="-1.88495559215" upper="3.76991118431" velocity="31"/>
</joint>
<link name="FL_thigh">
<visual>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1"/>
<geometry>
<box size="0.2 0.035 0.05"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1"/>
<geometry>
<box size="0.2 0.035 0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003205778 -0.027811623 -0.027767048"/>
<mass value="0.950266218"/>
<inertia ixx="0.004811898" ixy="9.3647e-05" ixz="-0.000208709" iyy="0.004752366" iyz="0.000813258" izz="0.001079691"/>
</inertial>
</link>
<joint name="FL_thigh_rotor_fix" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 -0.107226 0"/>
<parent link="FL_thigh"/>
<child link="FL_thigh_rotor"/>
</joint>
<link name="FL_thigh_rotor">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.076506382"/>
<inertia ixx="4.5843e-05" ixy="0" ixz="0" iyy="4.5871e-05" iyz="0" izz="8.9031e-05"/>
</inertial>
</link>
<joint name="FL_calf_rotor_fix" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 -0.0426259 0"/>
<parent link="FL_thigh"/>
<child link="FL_calf_rotor"/>
</joint>
<link name="FL_calf_rotor">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.076506382"/>
<inertia ixx="4.5843e-05" ixy="0" ixz="0" iyy="4.5871e-05" iyz="0" izz="8.9031e-05"/>
</inertial>
</link>
<joint name="FL_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
<parent link="FL_thigh"/>
<child link="FL_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="24" lower="-2.49582083035" upper="-0.717155789644" velocity="31"/>
</joint>
<link name="FL_calf">
<visual>
<origin rpy="0 3.141592653589793 0" xyz="0 0 -0.1045"/>
<geometry>
<box size="0.016 0.016 0.209"/>
</geometry>
<material name="silver"/>
</visual>
<visual name="FL_calf_rubber">
<origin rpy="0 3.141592653589793 0" xyz="-0.0125 0 -0.007"/>
<geometry>
<sphere radius="0.016"/>
</geometry>
</visual>
<collision>
<origin rpy="0 3.141592653589793 0" xyz="0 0 -0.1045"/>
<geometry>
<box size="0.016 0.016 0.209"/>
</geometry>
</collision>
<collision name="FL_calf_rubber">
<origin rpy="0 3.141592653589793 0" xyz="-0.0125 0 -0.007"/>
<geometry>
<sphere radius="0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.007372358 0.000122423 -0.10769301"/>
<mass value="0.1135916"/>
<inertia ixx="0.000667971" ixy="-1e-09" ixz="5.356e-06" iyy="0.000674399" iyz="-1.5e-08" izz="1.4181e-05"/>
</inertial>
</link>
<joint name="FL_foot_fixed" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 0 -0.19999999999999998"/>
<parent link="FL_calf"/>
<child link="FL_foot"/>
</joint>
<link name="FL_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.018"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.018"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
</inertial>
</link>
<joint name="RR_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="-0.23035 -0.05 0"/>
<parent link="trunk"/>
<child link="RR_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="24" lower="-0.76096355387" upper="0.76096355387" velocity="31"/>
</joint>
<link name="RR_hip">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.052" radius="0.047"/>
</geometry>
<material name="silver"/>
</visual>
<!-- <collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.052" radius="0.047"/>
</geometry>
</collision> -->
<inertial>
<origin rpy="0 0 0" xyz="0.001597914 -0.001680938 2.253e-06"/>
<mass value="0.537806728"/>
<inertia ixx="0.000403612" ixy="-9.337e-06" ixz="-5.36e-07" iyy="0.000600779" iyz="6.3e-08" izz="0.000417249"/>
</inertial>
</link>
<joint name="RR_hip_rotor_fix" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0.0832504 0 0"/>
<parent link="RR_hip"/>
<child link="RR_hip_rotor"/>
</joint>
<link name="RR_hip_rotor">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.076506382"/>
<inertia ixx="4.5843e-05" ixy="0" ixz="0" iyy="4.5871e-05" iyz="0" izz="8.9031e-05"/>
</inertial>
</link>
<joint name="RR_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 -0.053076000000000005 0"/>
<parent link="RR_hip"/>
<child link="RR_thigh_shoulder"/>
</joint>
<link name="RR_thigh_shoulder">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.052" radius="0.047"/>
</geometry>
<material name="silver"/>
</visual>
<!-- <collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.052" radius="0.047"/>
</geometry>
</collision> -->
</link>
<joint name="RR_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.105076 0"/>
<parent link="RR_hip"/>
<child link="RR_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="24" lower="-1.88495559215" upper="3.76991118431" velocity="31"/>
</joint>
<link name="RR_thigh">
<visual>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1"/>
<geometry>
<box size="0.2 0.035 0.05"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1"/>
<geometry>
<box size="0.2 0.035 0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003205778 0.027811623 -0.027767048"/>
<mass value="0.950266218"/>
<inertia ixx="0.004811898" ixy="-9.3647e-05" ixz="-0.000208709" iyy="0.004752366" iyz="-0.000813258" izz="0.001079691"/>
</inertial>
</link>
<joint name="RR_thigh_rotor_fix" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 0.107226 0"/>
<parent link="RR_thigh"/>
<child link="RR_thigh_rotor"/>
</joint>
<link name="RR_thigh_rotor">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.076506382"/>
<inertia ixx="4.5843e-05" ixy="0" ixz="0" iyy="4.5871e-05" iyz="0" izz="8.9031e-05"/>
</inertial>
</link>
<joint name="RR_calf_rotor_fix" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 0.0426259 0"/>
<parent link="RR_thigh"/>
<child link="RR_calf_rotor"/>
</joint>
<link name="RR_calf_rotor">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.076506382"/>
<inertia ixx="4.5843e-05" ixy="0" ixz="0" iyy="4.5871e-05" iyz="0" izz="8.9031e-05"/>
</inertial>
</link>
<joint name="RR_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
<parent link="RR_thigh"/>
<child link="RR_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="24" lower="-2.49582083035" upper="-0.717155789644" velocity="31"/>
</joint>
<link name="RR_calf">
<visual>
<origin rpy="0 3.141592653589793 0" xyz="0 0 -0.1045"/>
<geometry>
<box size="0.016 0.016 0.209"/>
</geometry>
<material name="silver"/>
</visual>
<visual name="RR_calf_rubber">
<origin rpy="0 3.141592653589793 0" xyz="-0.0125 0 -0.007"/>
<geometry>
<sphere radius="0.016"/>
</geometry>
</visual>
<collision>
<origin rpy="0 3.141592653589793 0" xyz="0 0 -0.1045"/>
<geometry>
<box size="0.016 0.016 0.209"/>
</geometry>
</collision>
<collision name="RR_calf_rubber">
<origin rpy="0 3.141592653589793 0" xyz="-0.0125 0 -0.007"/>
<geometry>
<sphere radius="0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.007372358 0.000122423 -0.10769301"/>
<mass value="0.1135916"/>
<inertia ixx="0.000667971" ixy="-1e-09" ixz="5.356e-06" iyy="0.000674399" iyz="-1.5e-08" izz="1.4181e-05"/>
</inertial>
</link>
<joint name="RR_foot_fixed" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 0 -0.19999999999999998"/>
<parent link="RR_calf"/>
<child link="RR_foot"/>
</joint>
<link name="RR_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.018"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.018"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
</inertial>
</link>
<joint name="RL_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="-0.23035 0.05 0"/>
<parent link="trunk"/>
<child link="RL_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="24" lower="-0.76096355387" upper="0.76096355387" velocity="31"/>
</joint>
<link name="RL_hip">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.052" radius="0.047"/>
</geometry>
<material name="silver"/>
</visual>
<!-- <collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.052" radius="0.047"/>
</geometry>
</collision> -->
<inertial>
<origin rpy="0 0 0" xyz="0.001597914 0.001680938 2.253e-06"/>
<mass value="0.537806728"/>
<inertia ixx="0.000403612" ixy="9.337e-06" ixz="-5.36e-07" iyy="0.000600779" iyz="-6.3e-08" izz="0.000417249"/>
</inertial>
</link>
<joint name="RL_hip_rotor_fix" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0.0832504 0 0"/>
<parent link="RL_hip"/>
<child link="RL_hip_rotor"/>
</joint>
<link name="RL_hip_rotor">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.076506382"/>
<inertia ixx="4.5843e-05" ixy="0" ixz="0" iyy="4.5871e-05" iyz="0" izz="8.9031e-05"/>
</inertial>
</link>
<joint name="RL_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0.053076000000000005 0"/>
<parent link="RL_hip"/>
<child link="RL_thigh_shoulder"/>
</joint>
<link name="RL_thigh_shoulder">
<visual>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.052" radius="0.047"/>
</geometry>
<material name="silver"/>
</visual>
<!-- <collision>
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.052" radius="0.047"/>
</geometry>
</collision> -->
</link>
<joint name="RL_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0.105076 0"/>
<parent link="RL_hip"/>
<child link="RL_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="24" lower="-1.88495559215" upper="3.76991118431" velocity="31"/>
</joint>
<link name="RL_thigh">
<visual>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1"/>
<geometry>
<box size="0.2 0.035 0.05"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="0 0 -0.1"/>
<geometry>
<box size="0.2 0.035 0.05"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003205778 -0.027811623 -0.027767048"/>
<mass value="0.950266218"/>
<inertia ixx="0.004811898" ixy="9.3647e-05" ixz="-0.000208709" iyy="0.004752366" iyz="0.000813258" izz="0.001079691"/>
</inertial>
</link>
<joint name="RL_thigh_rotor_fix" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 -0.107226 0"/>
<parent link="RL_thigh"/>
<child link="RL_thigh_rotor"/>
</joint>
<link name="RL_thigh_rotor">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.076506382"/>
<inertia ixx="4.5843e-05" ixy="0" ixz="0" iyy="4.5871e-05" iyz="0" izz="8.9031e-05"/>
</inertial>
</link>
<joint name="RL_calf_rotor_fix" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 -0.0426259 0"/>
<parent link="RL_thigh"/>
<child link="RL_calf_rotor"/>
</joint>
<link name="RL_calf_rotor">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.076506382"/>
<inertia ixx="4.5843e-05" ixy="0" ixz="0" iyy="4.5871e-05" iyz="0" izz="8.9031e-05"/>
</inertial>
</link>
<joint name="RL_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.2"/>
<parent link="RL_thigh"/>
<child link="RL_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="24" lower="-2.49582083035" upper="-0.717155789644" velocity="31"/>
</joint>
<link name="RL_calf">
<visual>
<origin rpy="0 3.141592653589793 0" xyz="0 0 -0.1045"/>
<geometry>
<box size="0.016 0.016 0.209"/>
</geometry>
<material name="silver"/>
</visual>
<visual name="RL_calf_rubber">
<origin rpy="0 3.141592653589793 0" xyz="-0.0125 0 -0.007"/>
<geometry>
<sphere radius="0.016"/>
</geometry>
</visual>
<collision>
<origin rpy="0 3.141592653589793 0" xyz="0 0 -0.1045"/>
<geometry>
<box size="0.016 0.016 0.209"/>
</geometry>
</collision>
<collision name="RL_calf_rubber">
<origin rpy="0 3.141592653589793 0" xyz="-0.0125 0 -0.007"/>
<geometry>
<sphere radius="0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.007372358 0.000122423 -0.10769301"/>
<mass value="0.1135916"/>
<inertia ixx="0.000667971" ixy="-1e-09" ixz="5.356e-06" iyy="0.000674399" iyz="-1.5e-08" izz="1.4181e-05"/>
</inertial>
</link>
<joint name="RL_foot_fixed" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 0 -0.19999999999999998"/>
<parent link="RL_calf"/>
<child link="RL_foot"/>
</joint>
<link name="RL_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.018"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.018"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
</inertial>
</link>
</robot>

124
src/robots/cyberdog1_description/xacro/const.xacro
View File

@ -1,124 +0,0 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="PI" value="3.1415926535897931" />
<!-- dimension -->
<!-- body -->
<xacro:property name="body_width" value="0.23" />
<xacro:property name="body_length" value="0.36" />
<xacro:property name="body_height" value="0.15" />
<!-- abad -->
<xacro:property name="abad_radius" value="0.047" />
<xacro:property name="abad_length" value="0.052" />
<xacro:property name="abad_offset_x" value="0.23035" />
<xacro:property name="abad_offset_y" value="0.05" />
<xacro:property name="abad_rotor_offset" value="0.0832504" />
<xacro:property name="abad_max" value="0.76096355387" />
<xacro:property name="abad_min" value="-0.76096355387" />
<!-- hip -->
<xacro:property name="hip_shoulder_radius" value="0.047" />
<xacro:property name="hip_shoulder_length" value="0.052" />
<xacro:property name="hip_width" value="0.035" />
<xacro:property name="hip_height" value="0.050" />
<xacro:property name="hip_offset" value="0.105076" />
<xacro:property name="hip_length" value="0.2" />
<xacro:property name="hip_rotor_offset" value="-0.107226" />
<xacro:property name="hip_f_max" value="3.76991118431" />
<xacro:property name="hip_f_min" value="-1.88495559215" />
<xacro:property name="hip_h_max" value="3.76991118431" />
<xacro:property name="hip_h_min" value="-1.88495559215" />
<!-- knee -->
<xacro:property name="knee_width" value="0.016" />
<xacro:property name="knee_height" value="0.016" />
<xacro:property name="knee_length" value="0.209" />
<xacro:property name="knee_rotor_offset" value="-0.0426259" />
<xacro:property name="knee_max" value="-0.717155789644" />
<xacro:property name="knee_min" value="-2.49582083035" />
<xacro:property name="knee_rubber" value="0.025" />
<!-- foot -->
<xacro:property name="foot_radius" value="0.018" />
<!-- acuator atribute -->
<xacro:property name="abadGearRatio" value="1" />
<xacro:property name="hipGearRatio" value="1" />
<xacro:property name="kneeGearRatio" value="1" />
<xacro:property name="abad_motorTauMax" value="24" />
<xacro:property name="hip_motorTauMax" value="24" />
<xacro:property name="knee_motorTauMax" value="24" />
<xacro:property name="abad_motorVelMax" value="31" />
<xacro:property name="hip_motorVelMax" value="31" />
<xacro:property name="knee_motorVelMax" value="31" />
<xacro:property name="abad_damping" value="0" />
<xacro:property name="hip_damping" value="0" />
<xacro:property name="knee_damping" value="0" />
<xacro:property name="abad_friction" value="0" />
<xacro:property name="hip_friction" value="0" />
<xacro:property name="knee_friction" value="0" />
<!-- inertial&mass value -->
<!-- body -->
<xacro:property name="body_mass" value="6.248410872" />
<xacro:property name="body_com_x" value="0.018785003" />
<xacro:property name="body_com_y" value="0.003291012" />
<xacro:property name="body_com_z" value="-0.000925646" />
<xacro:property name="body_ixx" value="0.032143039" />
<xacro:property name="body_ixy" value="-0.000268204" />
<xacro:property name="body_ixz" value="-0.00241341" />
<xacro:property name="body_iyy" value="0.13578316" />
<xacro:property name="body_iyz" value="0.000080244" />
<xacro:property name="body_izz" value="0.14828095" />
<!-- abad -->
<xacro:property name="abad_mass" value="0.537806728" />
<xacro:property name="abad_com_x" value="-0.001597914" />
<xacro:property name="abad_com_y" value="0.001680938" />
<xacro:property name="abad_com_z" value="0.000002253" />
<xacro:property name="abad_ixx" value="0.000403612" />
<xacro:property name="abad_ixy" value="-0.000009337" />
<xacro:property name="abad_ixz" value="0.000000536" />
<xacro:property name="abad_iyy" value="0.000600779" />
<xacro:property name="abad_iyz" value="-0.000000063" />
<xacro:property name="abad_izz" value="0.000417249" />
<!-- hip -->
<xacro:property name="hip_mass" value="0.950266218" />
<xacro:property name="hip_com_x" value="-0.003205778" />
<xacro:property name="hip_com_y" value="-0.027811623" />
<xacro:property name="hip_com_z" value="-0.027767048" />
<xacro:property name="hip_ixx" value="0.004811898" />
<xacro:property name="hip_ixy" value="0.000093647" />
<xacro:property name="hip_ixz" value="-0.000208709" />
<xacro:property name="hip_iyy" value="0.004752366" />
<xacro:property name="hip_iyz" value="0.000813258" />
<xacro:property name="hip_izz" value="0.001079691" />
<!-- knee -->
<xacro:property name="knee_mass" value="0.1135916" />
<xacro:property name="knee_com_x" value="0.007372358" />
<xacro:property name="knee_com_y" value="0.000122423" />
<xacro:property name="knee_com_z" value="-0.10769301" />
<xacro:property name="knee_ixx" value="0.000667971" />
<xacro:property name="knee_ixy" value="-0.000000001" />
<xacro:property name="knee_ixz" value="0.000005356" />
<xacro:property name="knee_iyy" value="0.000674399" />
<xacro:property name="knee_iyz" value="-0.000000015" />
<xacro:property name="knee_izz" value="0.000014181" />
<!-- rotor -->
<xacro:property name="rotor_mass" value="0.076506382" />
<xacro:property name="rotor_com_x" value="0" />
<xacro:property name="rotor_com_y" value="0" />
<xacro:property name="rotor_com_z" value="0" />
<xacro:property name="rotor_ixx" value="0.000045843" />
<xacro:property name="rotor_ixy" value="0" />
<xacro:property name="rotor_ixz" value="0" />
<xacro:property name="rotor_iyy" value="0.000045871" />
<xacro:property name="rotor_iyz" value="0" />
<xacro:property name="rotor_izz" value="0.000089031" />
</robot>

266
src/robots/cyberdog1_description/xacro/gazebo.xacro
View File

@ -1,266 +0,0 @@
<?xml version="1.0"?>
<robot>
<!-- ros_control plugin -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/cyberdog_gazebo</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
</plugin>
</gazebo>
<!-- Show the trajectory of trunk center. -->
<!-- <gazebo>
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
<frequency>10</frequency>
<plot>
<link>base</link>
<pose>0 0 0 0 0 0</pose>
<material>Gazebo/Yellow</material>
</plot>
</plugin>
</gazebo> -->
<!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
<!-- <gazebo>
<plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
<frequency>100</frequency>
<plot>
<link>FL_foot</link>
<pose>0 0 0 0 0 0</pose>
<material>Gazebo/Green</material>
</plot>
</plugin>
</gazebo> -->
<gazebo>
<plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
<bodyName>trunk</bodyName>
<topicName>/apply_force/trunk</topicName>
</plugin>
</gazebo>
<gazebo reference="imu_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>1000</update_rate>
<visualize>true</visualize>
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>trunk_imu</topicName>
<bodyName>imu_link</bodyName>
<updateRateHZ>1000.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_link</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
<!-- Foot contacts. -->
<!-- <gazebo reference="FR_calf">
<sensor name="FR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>FR_calf_fixed_joint_lump__FR_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="FL_calf">
<sensor name="FL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>FL_calf_fixed_joint_lump__FL_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RR_calf">
<sensor name="RR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>RR_calf_fixed_joint_lump__RR_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RL_calf">
<sensor name="RL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
<contact>
<collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
</contact>
</sensor>
</gazebo> -->
<!-- Visualization of Foot contacts. -->
<!-- <gazebo reference="FR_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>FR_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="FL_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>FL_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="RR_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>RR_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="RL_foot">
<visual>
<plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
<topicName>RL_foot_contact</topicName>
</plugin>
</visual>
</gazebo> -->
<gazebo reference="base">
<material>Gazebo/Green</material>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="trunk">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="stick_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/White</material>
</gazebo>
<gazebo reference="imu_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Red</material>
</gazebo>
<!-- FL leg -->
<gazebo reference="FL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="FL_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="FL_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FL_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- FR leg -->
<gazebo reference="FR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="FR_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="FR_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FR_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- RL leg -->
<gazebo reference="RL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="RL_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="RL_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="RL_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- RR leg -->
<gazebo reference="RR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="RR_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="RR_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="RR_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
</robot>

281
src/robots/cyberdog1_description/xacro/leg.xacro
View File

@ -1,281 +0,0 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="../xacro/transmission.xacro" />
<xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae *origin">
<joint name="${name}_hip_joint" type="revolute">
<xacro:insert_block name="origin" />
<parent link="trunk" />
<child link="${name}_hip" />
<axis xyz="1 0 0" />
<dynamics damping="${abad_damping}" friction="${abad_friction}" />
<xacro:if value="${(mirror_dae == True)}">
<limit effort="${abad_motorTauMax*abadGearRatio}"
velocity="${abad_motorVelMax}" lower="${abad_min}"
upper="${abad_max}" />
</xacro:if>
<xacro:if value="${(mirror_dae == False)}">
<limit effort="${abad_motorTauMax*abadGearRatio}"
velocity="${abad_motorVelMax}" lower="${-abad_max}"
upper="${-abad_min}" />
</xacro:if>
</joint>
<link name="${name}_hip">
<!-- <visual>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="0 0 0" />
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="${PI} 0 0" xyz="0 0 0" />
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 ${PI} 0" xyz="0 0 0" />
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="${PI} ${PI} 0" xyz="0 0 0" />
</xacro:if>
<geometry>
<mesh filename="package://cyberdog_new_description/meshes/abad.dae" scale="1 1 1" />
</geometry>
<material name="orange" />
</visual> -->
<visual>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0" />
<geometry>
<cylinder length="${abad_length}" radius="${abad_radius}" />
</geometry>
<material name="silver" />
</visual>
<collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0" />
<geometry>
<cylinder length="${abad_length}" radius="${abad_radius}" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0"
xyz="${abad_com_x*front_hind} ${abad_com_y*mirror} ${abad_com_z}" />
<mass value="${abad_mass}" />
<inertia ixx="${abad_ixx}" ixy="${abad_ixy*mirror*front_hind}"
ixz="${abad_ixz*front_hind}" iyy="${abad_iyy}" iyz="${abad_iyz*mirror}"
izz="${abad_izz}" />
</inertial>
</link>
<joint name="${name}_hip_rotor_fix" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="${-abad_rotor_offset*front_hind} 0 0" />
<parent link="${name}_hip" />
<child link="${name}_hip_rotor" />
</joint>
<!-- this link is only for abad rotor inertial -->
<link name="${name}_hip_rotor">
<inertial>
<origin rpy="0 0 0" xyz="${rotor_com_x} ${rotor_com_y} ${rotor_com_z}" />
<mass value="${rotor_mass}" />
<inertia ixx="${rotor_ixx}" ixy="${rotor_ixy}" ixz="${rotor_ixz}" iyy="${rotor_iyy}" iyz="${rotor_iyz}" izz="${rotor_izz}" />
</inertial>
</link>
<joint name="${name}_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 ${(hip_offset-hip_shoulder_length)*mirror} 0" />
<parent link="${name}_hip" />
<child link="${name}_thigh_shoulder" />
</joint>
<link name="${name}_thigh_shoulder">
<visual>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0" />
<geometry>
<cylinder length="${hip_shoulder_length}" radius="${hip_shoulder_radius}" />
</geometry>
<material name="silver" />
</visual>
<collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0" />
<geometry>
<cylinder length="${hip_shoulder_length}" radius="${hip_shoulder_radius}" />
</geometry>
</collision>
</link>
<joint name="${name}_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 ${hip_offset*mirror} 0" />
<parent link="${name}_hip" />
<child link="${name}_thigh" />
<axis xyz="0 1 0" />
<dynamics damping="${hip_damping}" friction="${hip_friction}" />
<xacro:if value="${front_hind_dae == True}">
<limit effort="${hip_motorTauMax*hipGearRatio}"
velocity="${hip_motorVelMax}" lower="${hip_f_min}"
upper="${hip_f_max}" />
</xacro:if>
<xacro:if value="${front_hind_dae == False}">
<limit effort="${hip_motorTauMax*hipGearRatio}"
velocity="${hip_motorVelMax}" lower="${hip_h_min}"
upper="${hip_h_max}" />
</xacro:if>
</joint>
<link name="${name}_thigh">
<!-- <visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<xacro:if value="${mirror_dae == True}">
<mesh filename="package://cyberdog_new_description/meshes/hip.dae"
scale="1 1 1" />
</xacro:if>
<xacro:if value="${mirror_dae == False}">
<mesh filename="package://cyberdog_new_description/meshes/hip_mirror.dae"
scale="1 1 1" />
</xacro:if>
</geometry>
<material name="orange" />
</visual> -->
<visual>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-hip_length/2.0}" />
<geometry>
<box size="${hip_length} ${hip_width} ${hip_height}" />
</geometry>
<material name="silver" />
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-hip_length/2.0}" />
<geometry>
<box size="${hip_length} ${hip_width} ${hip_height}" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${hip_com_x} ${hip_com_y*mirror} ${hip_com_z}" />
<mass value="${hip_mass}" />
<inertia ixx="${hip_ixx}" ixy="${hip_ixy*mirror}" ixz="${hip_ixz}" iyy="${hip_iyy}"
iyz="${hip_iyz*mirror}" izz="${hip_izz}" />
</inertial>
</link>
<joint name="${name}_thigh_rotor_fix" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 ${hip_rotor_offset*mirror} 0" />
<parent link="${name}_thigh" />
<child link="${name}_thigh_rotor" />
</joint>
<!-- this link is only for hip rotor inertial -->
<link name="${name}_thigh_rotor">
<inertial>
<origin rpy="0 0 0" xyz="${rotor_com_x} ${rotor_com_y} ${rotor_com_z}" />
<mass value="${rotor_mass}" />
<inertia ixx="${rotor_ixx}" ixy="${rotor_ixy}" ixz="${rotor_ixz}" iyy="${rotor_iyy}" iyz="${rotor_iyz}" izz="${rotor_izz}" />
</inertial>
</link>
<joint name="${name}_calf_rotor_fix" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 ${knee_rotor_offset*mirror} 0" />
<parent link="${name}_thigh" />
<child link="${name}_calf_rotor" />
</joint>
<!-- this link is only for knee rotor inertial -->
<link name="${name}_calf_rotor">
<inertial>
<origin rpy="0 0 0" xyz="${rotor_com_x} ${rotor_com_y} ${rotor_com_z}" />
<mass value="${rotor_mass}" />
<inertia ixx="${rotor_ixx}" ixy="${rotor_ixy}" ixz="${rotor_ixz}" iyy="${rotor_iyy}" iyz="${rotor_iyz}" izz="${rotor_izz}" />
</inertial>
</link>
<joint name="${name}_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 ${-hip_length}" />
<parent link="${name}_thigh" />
<child link="${name}_calf" />
<axis xyz="0 1 0" />
<dynamics damping="${hip_damping}" friction="${hip_friction}" />
<limit effort="${knee_motorTauMax*kneeGearRatio}"
velocity="${knee_motorVelMax}" lower="${knee_min}"
upper="${knee_max}" />
</joint>
<link name="${name}_calf">
<!-- <visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="package://cyberdog_new_description/meshes/knee.dae" scale="1 1 1" />
</geometry>
<material name="orange" />
</visual> -->
<visual>
<origin rpy="0 ${PI} 0" xyz="0 0 ${-knee_length/2.0}" />
<geometry>
<box size="${knee_height} ${knee_width} ${knee_length}" />
</geometry>
<material name="silver" />
</visual>
<visual name="${name}_calf_rubber">
<origin rpy="0 ${PI} 0" xyz="${-knee_rubber/2.0} 0 -0.007" />
<geometry>
<sphere radius="0.016" />
</geometry>
</visual>
<collision>
<origin rpy="0 ${PI} 0" xyz="0 0 ${-knee_length/2.0}" />
<geometry>
<box size="${knee_height} ${knee_width} ${knee_length}" />
</geometry>
</collision>
<collision name="${name}_calf_rubber">
<origin rpy="0 ${PI} 0" xyz="${-knee_rubber/2.0} 0 -0.007" />
<geometry>
<sphere radius="0.016" />
</geometry>
<surface>
<contact>
<ode>
<max_vel>0.00001</max_vel>
<min_depth>0.0</min_depth>
</ode>
</contact>
</surface>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${knee_com_x} ${knee_com_y} ${knee_com_z}" />
<mass value="${knee_mass}" />
<inertia ixx="${knee_ixx}" ixy="${knee_ixy}" ixz="${knee_ixz}" iyy="${knee_iyy}"
iyz="${knee_iyz}" izz="${knee_izz}" />
</inertial>
</link>
<joint name="${name}_foot_fixed" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 0 ${-knee_length+foot_radius/2.0}" />
<parent link="${name}_calf" />
<child link="${name}_foot" />
</joint>
<link name="${name}_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<sphere radius="${foot_radius}" />
</geometry>
<material name="silver" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<sphere radius="${foot_radius}" />
</geometry>
<surface>
<contact>
<ode>
<max_vel>0.001</max_vel>
<min_depth>0.0</min_depth>
</ode>
</contact>
</surface>
</collision>
</link>
<xacro:leg_transmission name="${name}" />
</xacro:macro>
</robot>

123
src/robots/cyberdog1_description/xacro/robot.xacro
View File

@ -1,123 +0,0 @@
<?xml version="1.0"?>
<robot name="cyberdog_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:arg name="ROBOT" default="cyberdog_description" />
<xacro:arg name="USE_LIDAR" default="false" />
<xacro:include filename="const.xacro" />
<xacro:include filename="leg.xacro" />
<xacro:include filename="gazebo.xacro" />
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="silver">
<color rgba="0.913725490196 0.913725490196 0.847058823529 1.0"/>
</material>
<material name="orange">
<color rgba="0.12 0.15 0.2 1.0"/>
</material>
<material name="brown">
<color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<link name="base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</visual>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0" />
<parent link="base" />
<child link="trunk" />
</joint>
<link name="trunk">
<!-- <visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="package://cyberdog_new_description/meshes/body.dae" scale="1 1 1" />
</geometry>
</visual> -->
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size="${body_length} ${body_width} ${body_height}" />
</geometry>
<material name="silver" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size="${body_length} ${body_width} ${body_height}" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${body_com_x} ${body_com_y} ${body_com_z}" />
<mass value="${body_mass}" />
<inertia ixx="${body_ixx}" ixy="${body_ixy}" ixz="${body_ixz}" iyy="${body_iyy}" iyz="${body_iyz}" izz="${body_izz}" />
</inertial>
</link>
<joint name="imu_joint" type="fixed">
<parent link="trunk" />
<child link="imu_link" />
<origin rpy="0 0 0" xyz="0 0 0" />
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001" />
<origin rpy="0 0 0" xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
<material name="red"/>
</visual>
<!-- <collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size=".001 .001 .001" />
</geometry>
</collision> -->
</link>
<xacro:leg name="FR" mirror="-1" mirror_dae="False" front_hind="1" front_hind_dae="True">
<origin rpy="0 0 0" xyz="${abad_offset_x} ${-abad_offset_y} 0" />
</xacro:leg>
<xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True">
<origin rpy="0 0 0" xyz="${abad_offset_x} ${abad_offset_y} 0" />
</xacro:leg>
<xacro:leg name="RR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False">
<origin rpy="0 0 0" xyz="${-abad_offset_x} ${-abad_offset_y} 0" />
</xacro:leg>
<xacro:leg name="RL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False">
<origin rpy="0 0 0" xyz="${-abad_offset_x} ${abad_offset_y} 0" />
</xacro:leg>
</robot>

41
src/robots/cyberdog1_description/xacro/transmission.xacro
View File

@ -1,41 +0,0 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="leg_transmission" params="name">
<transmission name="${name}_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>"${abadGearRatio}"</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${name}_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>"${hipGearRatio}"</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${name}_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>"${kneeGearRatio}"</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
</robot>

19
src/robots/cyberdog2_description/CMakeLists.txt
View File

@ -1,19 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(cyberdog2_description)
find_package(catkin REQUIRED COMPONENTS
genmsg
roscpp
std_msgs
tf
)
catkin_package(
CATKIN_DEPENDS
)
include_directories(
# include
${Boost_INCLUDE_DIR}
${catkin_INCLUDE_DIRS}
)

23
src/robots/cyberdog2_description/launch/display.launch
View File

@ -1,23 +0,0 @@
<launch>
<!-- <param name="robot_description" textfile="$(find cyberdog2_description)/urdf/cyberdog2_description.urdf"/> -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find cyberdog2_description)/xacro/robot.xacro'"/>
<!-- send fake joint values -->
<!-- <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
<param name="use_gui" value="TRUE"/>
</node> -->
<node
name="joint_state_publisher_gui"
pkg="joint_state_publisher_gui"
type="joint_state_publisher_gui" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="1000.0"/>
</node>
<node pkg="rviz" type="rviz" name="rviz" respawn="false" output="screen"
args="-d $(find cyberdog2_description)/launch/test.rviz"/>
</launch>

19
src/robots/cyberdog2_description/launch/gazebo.launch
View File

@ -1,19 +0,0 @@
<launch>
<include file="$(find gazebo_ros)/launch/empty_world.launch" />
<node
name="tf_footprint_base"
pkg="tf"
type="static_transform_publisher"
args="0 0 0 0 0 0 base_link base_footprint 40" />
<node
name="spawn_model"
pkg="gazebo_ros"
type="spawn_model"
args="-file $(find cyberdog_description)/urdf/cyberdog2_description.urdf -urdf -model cyberdog2_description -z 0.6 -unpause"
output="screen" />
<node
name="fake_joint_calibration"
pkg="rostopic"
type="rostopic"
args="pub /calibrated std_msgs/Bool true" />
</launch>

245
src/robots/cyberdog2_description/launch/test.rviz
View File

@ -1,245 +0,0 @@
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: <Fixed 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: <Fixed 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

188
src/robots/cyberdog2_description/meshes/abad.dae
View File

File diff suppressed because one or more lines are too long

710
src/robots/cyberdog2_description/meshes/body.dae
View File

File diff suppressed because one or more lines are too long

391
src/robots/cyberdog2_description/meshes/hip.dae
View File

File diff suppressed because one or more lines are too long

391
src/robots/cyberdog2_description/meshes/hip_mirror.dae
View File

File diff suppressed because one or more lines are too long

130
src/robots/cyberdog2_description/meshes/knee.dae
View File

File diff suppressed because one or more lines are too long

19
src/robots/cyberdog2_description/package.xml
View File

@ -1,19 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>cyberdog2_description</name>
<version>1.0.0</version>
<description>The cyberdog2_description package</description>
<author>TODO</author>
<maintainer email="TODO@email.com" />
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>std_msgs</depend>
<depend>roslaunch</depend>
<depend>robot_state_publisher</depend>
<depend>rviz</depend>
<depend>joint_state_publisher_gui</depend>
<depend>gazebo</depend>
</package>

293
src/robots/cyberdog2_description/rviz/cyberdog.rviz
View File

@ -1,293 +0,0 @@
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Grid1
- /RobotModel1
Splitter Ratio: 0.5558823347091675
Tree Height: 719
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz_common/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/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: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz_default_plugins/RobotModel
Collision Enabled: false
Description File: ""
Description Source: Topic
Description Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: robot_description
Enabled: true
Links:
AI_camera_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
All Links Enabled: true
D435_camera_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
FL_abad:
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_hip_shoulder:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FL_knee:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_abad:
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_hip_shoulder:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
FR_knee:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
RGB_camera_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_abad:
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_hip_shoulder:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RL_knee:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_abad:
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_hip_shoulder:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RR_knee:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
body:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
imu_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: base_link
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 1.469738483428955
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
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.9903974533081055
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 5.3835859298706055
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1016
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c70000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1848
X: 72
Y: 27

130
src/robots/cyberdog2_description/xacro/const.xacro
View File

@ -1,130 +0,0 @@
<?xml version="1.0"?>
<robot name="cyberdog2_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="PI" value="3.1415926535897931" />
<!-- dimension -->
<!-- body -->
<xacro:property name="body_width" value="0.190" />
<xacro:property name="body_length" value="0.236" />
<xacro:property name="body_height" value="0.109" />
<!-- abad -->
<xacro:property name="abad_radius" value="0.039" />
<xacro:property name="abad_length" value="0.035" />
<xacro:property name="abad_offset_x" value="0.164" />
<xacro:property name="abad_offset_y" value="0.042" />
<xacro:property name="abad_rotor_offset" value="0.0642303" />
<xacro:property name="abad_max" value="39" />
<xacro:property name="abad_min" value="-39" />
<!-- hip -->
<xacro:property name="hip_shoulder_radius" value="0.039" />
<xacro:property name="hip_shoulder_length" value="0.035" />
<xacro:property name="hip_width" value="0.0250" />
<xacro:property name="hip_height" value="0.034" />
<xacro:property name="hip_offset" value="0.094" />
<xacro:property name="hip_length" value="0.12" />
<xacro:property name="hip_rotor_offset" value="-0.07577" />
<xacro:property name="hip_f_max" value="160" />
<xacro:property name="hip_f_min" value="-76" />
<xacro:property name="hip_h_max" value="180" />
<xacro:property name="hip_h_min" value="-56" />
<!-- knee -->
<xacro:property name="knee_width" value="0.016" />
<xacro:property name="knee_height" value="0.016" />
<xacro:property name="knee_length" value="0.18341" />
<xacro:property name="knee_rotor_offset" value="-0.0342303" />
<xacro:property name="knee_max" value="-30" />
<xacro:property name="knee_min" value="-145" />
<xacro:property name="knee_rubber" value="0.02" />
<!-- foot -->
<xacro:property name="foot_radius" value="0.019" />
<!-- acuator atribute -->
<xacro:property name="abadGearRatio" value="7.75" />
<xacro:property name="hipGearRatio" value="7.75" />
<xacro:property name="kneeGearRatio" value="7.75" />
<xacro:property name="abad_motorTauMax" value="1.5484" />
<xacro:property name="hip_motorTauMax" value="1.5484" />
<xacro:property name="knee_motorTauMax" value="1.5484" />
<xacro:property name="abad_motorVelMax" value="30.9971" />
<xacro:property name="hip_motorVelMax" value="30.9971" />
<xacro:property name="knee_motorVelMax" value="30.9971" />
<!-- <xacro:property name="abad_damping" value="0.01" />
<xacro:property name="hip_damping" value="0.01" />
<xacro:property name="knee_damping" value="0.01" />
<xacro:property name="abad_friction" value="0.1" />
<xacro:property name="hip_friction" value="0.1" />
<xacro:property name="knee_friction" value="0.1" /> -->
<xacro:property name="abad_damping" value="0" />
<xacro:property name="hip_damping" value="0" />
<xacro:property name="knee_damping" value="0" />
<xacro:property name="abad_friction" value="0" />
<xacro:property name="hip_friction" value="0" />
<xacro:property name="knee_friction" value="0" />
<!-- inertial&mass value -->
<!-- body -->
<xacro:property name="body_mass" value="4.030000000" />
<xacro:property name="body_com_x" value="0.027300000" />
<xacro:property name="body_com_y" value="-0.000242000" />
<xacro:property name="body_com_z" value="0.014300000" />
<xacro:property name="body_ixx" value="0.018500000" />
<xacro:property name="body_ixy" value="-0.000173000" />
<xacro:property name="body_ixz" value="-0.010200000" />
<xacro:property name="body_iyy" value="0.051700000" />
<xacro:property name="body_iyz" value="-0.000028300" />
<xacro:property name="body_izz" value="0.048300000" />
<!-- abad -->
<xacro:property name="abad_mass" value="0.354000000" />
<xacro:property name="abad_com_x" value="-0.003920000" />
<xacro:property name="abad_com_y" value="0.015000000" />
<xacro:property name="abad_com_z" value="-0.000306000" />
<xacro:property name="abad_ixx" value="0.000190000" />
<xacro:property name="abad_ixy" value="-0.000027000" />
<xacro:property name="abad_ixz" value="-0.000000344" />
<xacro:property name="abad_iyy" value="0.000276000" />
<xacro:property name="abad_iyz" value="0.000001950" />
<xacro:property name="abad_izz" value="0.000233000" />
<!-- hip -->
<xacro:property name="hip_mass" value="0.482000000" />
<xacro:property name="hip_com_x" value="-0.002120000" />
<xacro:property name="hip_com_y" value="-0.021200000" />
<xacro:property name="hip_com_z" value="-0.018400000" />
<xacro:property name="hip_ixx" value="0.001010000" />
<xacro:property name="hip_ixy" value="0.000022300" />
<xacro:property name="hip_ixz" value="-0.000038500" />
<xacro:property name="hip_iyy" value="0.000983000" />
<xacro:property name="hip_iyz" value="0.000199000" />
<xacro:property name="hip_izz" value="0.000347000" />
<!-- knee -->
<xacro:property name="knee_mass" value="0.116000000" />
<xacro:property name="knee_com_x" value="0.000600000" />
<xacro:property name="knee_com_y" value="-0.000047200" />
<xacro:property name="knee_com_z" value="-0.089300000" />
<xacro:property name="knee_ixx" value="0.000668000" />
<xacro:property name="knee_ixy" value="0.000000003" />
<xacro:property name="knee_ixz" value="0.000023700" />
<xacro:property name="knee_iyy" value="0.000674000" />
<xacro:property name="knee_iyz" value="0.000000603" />
<xacro:property name="knee_izz" value="0.000015400" />
<!-- rotor -->
<xacro:property name="rotor_mass" value="0.056700000" />
<xacro:property name="rotor_com_x" value="0" />
<xacro:property name="rotor_com_y" value="0" />
<xacro:property name="rotor_com_z" value="0" />
<xacro:property name="rotor_ixx" value="0.000025300" />
<xacro:property name="rotor_ixy" value="0" />
<xacro:property name="rotor_ixz" value="0" />
<xacro:property name="rotor_iyy" value="0.000047800" />
<xacro:property name="rotor_iyz" value="0" />
<xacro:property name="rotor_izz" value="0.000025300" />
</robot>

252
src/robots/cyberdog2_description/xacro/gazebo.xacro
View File

@ -1,252 +0,0 @@
<?xml version="1.0"?>
<robot name="cyber_dog" xmlns:xacro="http://www.ros.org/wiki/xacro">
<gazebo>
<plugin name="gazebo_rt_control" filename="liblegged_plugin.so">
</plugin>
<plugin name="gazebo_rt_control" filename="libreal_time_control.so">
<robotName>$(arg ROBOT)</robotName>
</plugin>
</gazebo>
<gazebo reference="imu_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>500</update_rate>
<visualize>true</visualize>
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>body_imu</topicName>
<bodyName>imu_link</bodyName>
<updateRateHZ>500.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_link</frameName>
<ros>
<remapping>~/out:=imu</remapping>
</ros>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
<xacro:if value="$(arg USE_LIDAR)">
<gazebo reference="lidar_link">
<sensor name="realsense" type="ray">
<always_on>true</always_on>
<visualize>true</visualize>
<pose>0.0 0 0.0 0 0 0</pose>
<update_rate>5</update_rate>
<ray>
<scan>
<horizontal>
<samples>180</samples>
<resolution>1.000000</resolution>
<min_angle>-1.5700</min_angle>
<max_angle>1.5700</max_angle>
</horizontal>
</scan>
<range>
<min>0.01</min>
<max>12.00</max>
<resolution>0.015000</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="cyberdog_laserscan" filename="libgazebo_ros_ray_sensor.so">
<ros>
<remapping>~/out:=scan</remapping>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
<frame_name>lidar_link</frame_name>
</plugin>
</sensor>
</gazebo>
</xacro:if>
<!-- Foot contacts. -->
<gazebo reference="FR_knee">
<sensor name="FR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libfoot_contact_plugin.so"/>
<contact>
<collision>FR_knee_fixed_joint_lump__FR_foot_collision_2</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="FL_knee">
<sensor name="FL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libfoot_contact_plugin.so"/>
<contact>
<collision>FL_knee_fixed_joint_lump__FL_foot_collision_2</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RR_knee">
<sensor name="RR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libfoot_contact_plugin.so"/>
<contact>
<collision>RR_knee_fixed_joint_lump__RR_foot_collision_2</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RL_knee">
<sensor name="RL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin name="contactPlugin" filename="libfoot_contact_plugin.so"/>
<contact>
<collision>RL_knee_fixed_joint_lump__RL_foot_collision_2</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="base">
<material>Gazebo/Green</material>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="body">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<kp value="50000.0"/>
<kd value="5000.0"/>
</gazebo>
<gazebo reference="stick_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/White</material>
</gazebo>
<gazebo reference="imu_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Red</material>
</gazebo>
<!-- FL leg -->
<gazebo reference="FL_abad">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="FL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="50000.0"/>
<kd value="5000.0"/>
</gazebo>
<gazebo reference="FL_knee">
<mu1>0.7</mu1>
<mu2>0.7</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FL_foot">
<mu1>0.7</mu1>
<mu2>0.7</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="50000.0"/>
<kd value="5000.0"/>
</gazebo>
<!-- FR leg -->
<gazebo reference="FR_abad">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="FR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="50000.0"/>
<kd value="5000.0"/>
</gazebo>
<gazebo reference="FR_knee">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FR_foot">
<mu1>0.7</mu1>
<mu2>0.7</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="50000.0"/>
<kd value="5000.0"/>
</gazebo>
<!-- RL leg -->
<gazebo reference="RL_abad">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="RL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="50000.0"/>
<kd value="5000.0"/>
</gazebo>
<gazebo reference="RL_knee">
<mu1>0.7</mu1>
<mu2>0.7</mu2>
<self_collide>1</self_collide>
<kp value="5000000000.0"/>
<kd value="50000.0"/>
</gazebo>
<gazebo reference="RL_foot">
<mu1>0.7</mu1>
<mu2>0.7</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="50000.0"/>
<kd value="5000.0"/>
</gazebo>
<!-- RR leg -->
<gazebo reference="RR_abad">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="RR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="50000.0"/>
<kd value="5000.0"/>
</gazebo>
<gazebo reference="RR_knee">
<mu1>0.7</mu1>
<mu2>0.7</mu2>
<self_collide>1</self_collide>
<kp value="5000000000.0"/>
<kd value="50000.0"/>
</gazebo>
<gazebo reference="RR_foot">
<mu1>0.7</mu1>
<mu2>0.7</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="50000.0"/>
<kd value="5000.0"/>
</gazebo>
</robot>

247
src/robots/cyberdog2_description/xacro/leg.xacro
View File

@ -1,247 +0,0 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="../xacro/transmission.xacro" />
<xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae *origin">
<joint name="${name}_hip_joint" type="revolute">
<xacro:insert_block name="origin" />
<parent link="trunk" />
<child link="${name}_hip" />
<axis xyz="1 0 0" />
<dynamics damping="${abad_damping}" friction="${abad_friction}" />
<xacro:if value="${(mirror_dae == True)}">
<limit effort="${abad_motorTauMax*abadGearRatio}"
velocity="${abad_motorVelMax}" lower="${abad_min*PI/180.0}"
upper="${abad_max*PI/180.0}" />
</xacro:if>
<xacro:if value="${(mirror_dae == False)}">
<limit effort="${abad_motorTauMax*abadGearRatio}"
velocity="${abad_motorVelMax}" lower="${-abad_max*PI/180.0}"
upper="${-abad_min*PI/180.0}" />
</xacro:if>
</joint>
<link name="${name}_hip">
<visual>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="0 0 0" />
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="${PI} 0 0" xyz="0 0 0" />
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 ${PI} 0" xyz="0 0 0" />
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="${PI} ${PI} 0" xyz="0 0 0" />
</xacro:if>
<geometry>
<mesh filename="package://cyberdog2_description/meshes/abad.dae" scale="1 1 1" />
</geometry>
<material name="orange" />
</visual>
<collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0" />
<geometry>
<cylinder length="${abad_length}" radius="${abad_radius}" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0"
xyz="${abad_com_x*front_hind} ${abad_com_y*mirror} ${abad_com_z}" />
<mass value="${abad_mass}" />
<inertia ixx="${abad_ixx}" ixy="${abad_ixy*mirror*front_hind}"
ixz="${abad_ixz*front_hind}" iyy="${abad_iyy}" iyz="${abad_iyz*mirror}"
izz="${abad_izz}" />
</inertial>
</link>
<!-- <joint name="${name}_hip_rotor_fix" type="fixed">
<origin rpy="0 0 0" xyz="${-abad_rotor_offset*front_hind} 0 0" />
<parent link="${name}_hip" />
<child link="${name}_hip_rotor" />
</joint> -->
<!-- this link is only for abad rotor inertial -->
<!-- <link name="${name}_hip_rotor">
<inertial>
<origin rpy="0 0 0" xyz="${rotor_com_x} ${rotor_com_y} ${rotor_com_z}" />
<mass value="${rotor_mass}" />
<inertia ixx="${rotor_ixx}" ixy="${rotor_ixy}" ixz="${rotor_ixz}" iyy="${rotor_iyy}" iyz="${rotor_iyz}" izz="${rotor_izz}" />
</inertial>
</link> -->
<!-- <joint name="${name}_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 ${(hip_offset-hip_shoulder_length)*mirror} 0" />
<parent link="${name}_hip" />
<child link="${name}_thigh_shoulder" />
</joint> -->
<!-- <link name="${name}_thigh_shoulder">
<collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0" />
<geometry>
<cylinder length="${hip_shoulder_length}" radius="${hip_shoulder_radius}" />
</geometry>
</collision>
</link> -->
<joint name="${name}_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 ${hip_offset*mirror} 0" />
<parent link="${name}_hip" />
<child link="${name}_thigh" />
<axis xyz="0 1 0" />
<dynamics damping="${hip_damping}" friction="${hip_friction}" />
<xacro:if value="${front_hind_dae == True}">
<limit effort="${hip_motorTauMax*hipGearRatio}"
velocity="${hip_motorVelMax}" lower="${hip_f_min*PI/180.0}"
upper="${hip_f_max*PI/180.0}" />
</xacro:if>
<xacro:if value="${front_hind_dae == False}">
<limit effort="${hip_motorTauMax*hipGearRatio}"
velocity="${hip_motorVelMax}" lower="${hip_h_min*PI/180.0}"
upper="${hip_h_max*PI/180.0}" />
</xacro:if>
</joint>
<link name="${name}_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<xacro:if value="${mirror_dae == True}">
<mesh filename="package://cyberdog2_description/meshes/hip.dae"
scale="1 1 1" />
</xacro:if>
<xacro:if value="${mirror_dae == False}">
<mesh filename="package://cyberdog2_description/meshes/hip_mirror.dae"
scale="1 1 1" />
</xacro:if>
</geometry>
<material name="orange" />
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-hip_length/2.0}" />
<geometry>
<box size="${hip_length} ${hip_width} ${hip_height}" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${hip_com_x} ${hip_com_y*mirror} ${hip_com_z}" />
<mass value="${hip_mass}" />
<inertia ixx="${hip_ixx}" ixy="${hip_ixy*mirror}" ixz="${hip_ixz}" iyy="${hip_iyy}"
iyz="${hip_iyz*mirror}" izz="${hip_izz}" />
</inertial>
</link>
<!-- <joint name="${name}_thigh_rotor_fix" type="fixed">
<origin rpy="0 0 0" xyz="0 ${hip_rotor_offset*mirror} 0" />
<parent link="${name}_thigh" />
<child link="${name}_thigh_rotor" />
</joint> -->
<!-- this link is only for hip rotor inertial -->
<!-- <link name="${name}_thigh_rotor">
<inertial>
<origin rpy="0 0 0" xyz="${rotor_com_x} ${rotor_com_y} ${rotor_com_z}" />
<mass value="${rotor_mass}" />
<inertia ixx="${rotor_ixx}" ixy="${rotor_ixy}" ixz="${rotor_ixz}" iyy="${rotor_iyy}" iyz="${rotor_iyz}" izz="${rotor_izz}" />
</inertial>
</link> -->
<!-- <joint name="${name}_calf_rotor_fix" type="fixed">
<origin rpy="0 0 0" xyz="0 ${knee_rotor_offset*mirror} 0" />
<parent link="${name}_thigh" />
<child link="${name}_calf_rotor" />
</joint> -->
<!-- this link is only for knee rotor inertial -->
<!-- <link name="${name}_calf_rotor">
<inertial>
<origin rpy="0 0 0" xyz="${rotor_com_x} ${rotor_com_y} ${rotor_com_z}" />
<mass value="${rotor_mass}" />
<inertia ixx="${rotor_ixx}" ixy="${rotor_ixy}" ixz="${rotor_ixz}" iyy="${rotor_iyy}" iyz="${rotor_iyz}" izz="${rotor_izz}" />
</inertial>
</link> -->
<joint name="${name}_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 ${-hip_length}" />
<parent link="${name}_thigh" />
<child link="${name}_calf" />
<axis xyz="0 1 0" />
<dynamics damping="${hip_damping}" friction="${hip_friction}" />
<limit effort="${knee_motorTauMax*kneeGearRatio}"
velocity="${knee_motorVelMax}" lower="${knee_min*PI/180.0}"
upper="${knee_max*PI/180.0}" />
</joint>
<link name="${name}_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="package://cyberdog2_description/meshes/knee.dae" scale="1 1 1" />
</geometry>
<material name="orange" />
</visual>
<collision>
<origin rpy="0 ${PI} 0" xyz="0 0 ${-knee_length/2.0}" />
<geometry>
<box size="${knee_height} ${knee_width} ${knee_length}" />
</geometry>
</collision>
<!-- <collision name="${name}_calf_rubber">
<origin rpy="0 ${PI} 0" xyz="${-knee_rubber/2.0} 0 -0.007" />
<geometry>
<sphere radius="0.016" />
</geometry>
<surface>
<contact>
<ode>
<max_vel>0.00001</max_vel>
<min_depth>0.0</min_depth>
</ode>
</contact>
</surface>
</collision> -->
<inertial>
<origin rpy="0 0 0" xyz="${knee_com_x} ${knee_com_y} ${knee_com_z}" />
<mass value="${knee_mass}" />
<inertia ixx="${knee_ixx}" ixy="${knee_ixy}" ixz="${knee_ixz}" iyy="${knee_iyy}"
iyz="${knee_iyz}" izz="${knee_izz}" />
</inertial>
</link>
<joint name="${name}_foot_fixed" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0.0055 0 ${-knee_length+foot_radius/2.0}" />
<parent link="${name}_calf" />
<child link="${name}_foot" />
</joint>
<link name="${name}_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<sphere radius="${foot_radius-0.01}" />
</geometry>
<material name="orange" />
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<sphere radius="${foot_radius}" />
</geometry>
<surface>
<contact>
<ode>
<max_vel>0.001</max_vel>
<min_depth>0.0</min_depth>
</ode>
</contact>
</surface>
</collision>
</link>
<xacro:leg_transmission name="${name}" />
</xacro:macro>
</robot>

228
src/robots/cyberdog2_description/xacro/robot.xacro
View File

@ -1,228 +0,0 @@
<?xml version="1.0"?>
<robot name="cyberdog2_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:arg name="ROBOT" default="cyberdog2_description" />
<xacro:arg name="USE_LIDAR" default="false" />
<xacro:include filename="const.xacro" />
<xacro:include filename="leg.xacro" />
<!-- <xacro:include filename="gazebo.xacro" /> -->
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="silver">
<color rgba="0.913725490196 0.913725490196 0.847058823529 1.0"/>
</material>
<material name="orange">
<color rgba="0.12 0.15 0.2 1.0"/>
</material>
<material name="brown">
<color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<link name="base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</visual>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0" />
<parent link="base" />
<child link="trunk" />
</joint>
<link name="trunk">
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<mesh filename="package://cyberdog2_description/meshes/body.dae" scale="1 1 1" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size="${body_length} ${body_width} ${body_height}" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${body_com_x} ${body_com_y} ${body_com_z}" />
<mass value="${body_mass}" />
<inertia ixx="${body_ixx}" ixy="${body_ixy}" ixz="${body_ixz}" iyy="${body_iyy}" iyz="${body_iyz}" izz="${body_izz}" />
</inertial>
</link>
<joint name="imu_joint" type="fixed">
<parent link="trunk" />
<child link="imu_link" />
<origin rpy="0 0 0" xyz="33.4e-3 -17.2765e-3 51.0469e-3" />
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001" />
<origin rpy="0 0 0" xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</visual>
<!-- <collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size=".001 .001 .001" />
</geometry>
</collision> -->
</link>
<!-- <joint name="scan_joint" type="fixed">
<parent link="body" />
<child link="lidar_link" />
<origin rpy="0 0 0" xyz="0.21425 0 0.0908" />
</joint>
<link name="lidar_link">
<inertial>
<mass value="0.001" />
<origin rpy="0 0 0" xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
</inertial>
</link>
<joint name="D435_camera_joint" type="fixed">
<parent link="body" />
<child link="D435_camera_link" />
<origin rpy="0 0 0" xyz="271.994e-3 25e-3 114.912e-3" />
</joint>
<link name="D435_camera_link">
<inertial>
<mass value="0.001" />
<origin rpy="0 0 0" xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size=".001 .001 .001" />
</geometry>
</collision>
</link>
<joint name="RGB_camera_joint" type="fixed">
<parent link="body" />
<child link="RGB_camera_link" />
<origin rpy="0 0 0" xyz="275.76e-3 0 125.794e-3" />
</joint>
<link name="RGB_camera_link">
<inertial>
<mass value="0.001" />
<origin rpy="0 0 0" xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size=".001 .001 .001" />
</geometry>
</collision>
</link>
<joint name="AI_camera_joint" type="fixed">
<parent link="body" />
<child link="AI_camera_link" />
<origin rpy="0 0 0" xyz="290.228e-3 0 147.420e-3" />
</joint>
<link name="AI_camera_link">
<inertial>
<mass value="0.001" />
<origin rpy="0 0 0" xyz="0 0 0" />
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0" />
<geometry>
<box size=".001 .001 .001" />
</geometry>
</collision>
</link> -->
<xacro:leg name="FR" mirror="-1" mirror_dae="False" front_hind="1" front_hind_dae="True">
<origin rpy="0 0 0" xyz="${abad_offset_x} ${-abad_offset_y} 0" />
</xacro:leg>
<xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True">
<origin rpy="0 0 0" xyz="${abad_offset_x} ${abad_offset_y} 0" />
</xacro:leg>
<xacro:leg name="RR" mirror="-1" mirror_dae="False" front_hind="-1" front_hind_dae="False">
<origin rpy="0 0 0" xyz="${-abad_offset_x} ${-abad_offset_y} 0" />
</xacro:leg>
<xacro:leg name="RL" mirror="1" mirror_dae="True" front_hind="-1" front_hind_dae="False">
<origin rpy="0 0 0" xyz="${-abad_offset_x} ${abad_offset_y} 0" />
</xacro:leg>
<!-- This link is only for head collision -->
<!-- <joint name="head_joint" type="fixed">
<parent link="trunk" />
<child link="head" />
<origin rpy="0 0 0" xyz="0 0 0" />
</joint>
<link name="head">
<collision>
<origin rpy="0 0 0" xyz="0.256 0 0.120" />
<geometry>
<box size="0.076 0.060 0.040" />
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0.225 0 0.150" />
<geometry>
<box size="0.020 0.080 0.100" />
</geometry>
</collision>
</link> -->
</robot>

41
src/robots/cyberdog2_description/xacro/transmission.xacro
View File

@ -1,41 +0,0 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="leg_transmission" params="name">
<transmission name="${name}_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>"${abadGearRatio}"</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${name}_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>"${hipGearRatio}"</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${name}_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${name}_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>"${kneeGearRatio}"</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
</robot>

19
src/robots/cyberdog_old_description/CMakeLists.txt
View File

@ -1,19 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(cyberdog_old_description)
find_package(catkin REQUIRED COMPONENTS
genmsg
roscpp
std_msgs
tf
)
catkin_package(
CATKIN_DEPENDS
)
include_directories(
# include
${Boost_INCLUDE_DIR}
${catkin_INCLUDE_DIRS}
)

70
src/robots/cyberdog_old_description/config/robot_control.yaml
View File

@ -1,70 +0,0 @@
cyberdog_gazebo:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 1000
# FL Controllers ---------------------------------------
FL_hip_controller:
type: robot_joint_controller/RobotJointController
joint: FL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
FL_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: FL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
FL_calf_controller:
type: robot_joint_controller/RobotJointController
joint: FL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# FR Controllers ---------------------------------------
FR_hip_controller:
type: robot_joint_controller/RobotJointController
joint: FR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
FR_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: FR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
FR_calf_controller:
type: robot_joint_controller/RobotJointController
joint: FR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# RL Controllers ---------------------------------------
RL_hip_controller:
type: robot_joint_controller/RobotJointController
joint: RL_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
RL_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: RL_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
RL_calf_controller:
type: robot_joint_controller/RobotJointController
joint: RL_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
# RR Controllers ---------------------------------------
RR_hip_controller:
type: robot_joint_controller/RobotJointController
joint: RR_hip_joint
pid: {p: 100.0, i: 0.0, d: 5.0}
RR_thigh_controller:
type: robot_joint_controller/RobotJointController
joint: RR_thigh_joint
pid: {p: 300.0, i: 0.0, d: 8.0}
RR_calf_controller:
type: robot_joint_controller/RobotJointController
joint: RR_calf_joint
pid: {p: 300.0, i: 0.0, d: 8.0}

22
src/robots/cyberdog_old_description/launch/display.launch
View File

@ -1,22 +0,0 @@
<launch>
<param name="robot_description" textfile="$(find cyberdog_old_description)/urdf/cyberdog_old_description.urdf"/>
<!-- send fake joint values -->
<!-- <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
<param name="use_gui" value="TRUE"/>
</node> -->
<node
name="joint_state_publisher_gui"
pkg="joint_state_publisher_gui"
type="joint_state_publisher_gui" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="1000.0"/>
</node>
<node pkg="rviz" type="rviz" name="rviz" respawn="false" output="screen"
args="-d $(find cyberdog_old_description)/launch/test.rviz"/>
</launch>

19
src/robots/cyberdog_old_description/launch/gazebo.launch
View File

@ -1,19 +0,0 @@
<launch>
<include file="$(find gazebo_ros)/launch/empty_world.launch" />
<node
name="tf_footprint_base"
pkg="tf"
type="static_transform_publisher"
args="0 0 0 0 0 0 base_link base_footprint 40" />
<node
name="spawn_model"
pkg="gazebo_ros"
type="spawn_model"
args="-file $(find cyberdog_description)/urdf/cyberdog_old_description.urdf -urdf -model cyberdog_old_description -z 0.6 -unpause"
output="screen" />
<node
name="fake_joint_calibration"
pkg="rostopic"
type="rostopic"
args="pub /calibrated std_msgs/Bool true" />
</launch>

245
src/robots/cyberdog_old_description/launch/test.rviz
View File

@ -1,245 +0,0 @@
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: <Fixed 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: <Fixed 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

BIN
src/robots/cyberdog_old_description/meshes/calf.STL
View File

Binary file not shown.

BIN
src/robots/cyberdog_old_description/meshes/hip.STL
View File

Binary file not shown.

BIN
src/robots/cyberdog_old_description/meshes/hip_mirror.STL
View File

Binary file not shown.

BIN
src/robots/cyberdog_old_description/meshes/thigh.STL
View File

Binary file not shown.

BIN
src/robots/cyberdog_old_description/meshes/trunk.STL
View File

Binary file not shown.

19
src/robots/cyberdog_old_description/package.xml
View File

@ -1,19 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>cyberdog_old_description</name>
<version>1.0.0</version>
<description>The cyberdog_old_description package</description>
<author>TODO</author>
<maintainer email="TODO@email.com" />
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>std_msgs</depend>
<depend>roslaunch</depend>
<depend>robot_state_publisher</depend>
<depend>rviz</depend>
<depend>joint_state_publisher_gui</depend>
<depend>gazebo</depend>
</package>

933
src/robots/cyberdog_old_description/urdf/cyberdog_old_description.urdf
View File

@ -1,933 +0,0 @@
<?xml version="1.0" encoding="utf-8"?>
<robot name="cyberdog_old_description">
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="silver">
<color rgba="0.913725490196 0.913725490196 0.847058823529 1.0"/>
</material>
<material name="orange">
<!-- <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/> -->
<color rgba="0.12 0.15 0.2 1.0"/>
</material>
<material name="brown">
<color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<!-- ros_control plugin -->
<gazebo>
<plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
<robotNamespace>/cyberdog_gazebo</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
</plugin>
</gazebo>
<!-- Show the trajectory of body center. -->
<gazebo>
<plugin filename="libLinkPlot3DPlugin.so" name="3dplotbody">
<frequency>10</frequency>
<plot>
<link>base</link>
<pose>0 0 0 0 0 0</pose>
<material>Gazebo/Yellow</material>
</plot>
</plugin>
</gazebo>
<!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
<gazebo>
<plugin filename="libLinkPlot3DPlugin.so" name="3dplot">
<frequency>1000</frequency>
<plot>
<link>FR_calf</link>
<pose>0 0 -0.217 0 0 0</pose>
<material>Gazebo/Yellow</material>
</plot>
</plugin>
</gazebo>
<gazebo>
<plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
<bodyName>trunk</bodyName>
<topicName>/apply_force/trunk</topicName>
</plugin>
</gazebo>
<gazebo reference="imu_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>1000</update_rate>
<visualize>true</visualize>
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>trunk_imu</topicName>
<bodyName>imu_link</bodyName>
<updateRateHZ>1000.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_link</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
<!-- Foot contacts. -->
<!-- <gazebo reference="FR_calf">
<sensor name="FR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
<contact>
<collision>FR_calf_fixed_joint_lump__FR_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="FL_calf">
<sensor name="FL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
<contact>
<collision>FL_calf_fixed_joint_lump__FL_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RR_calf">
<sensor name="RR_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
<contact>
<collision>RR_calf_fixed_joint_lump__RR_foot_collision_1</collision>
</contact>
</sensor>
</gazebo>
<gazebo reference="RL_calf">
<sensor name="RL_foot_contact" type="contact">
<update_rate>100</update_rate>
<plugin filename="libunitreeFootContactPlugin.so" name="contactPlugin"/>
<contact>
<collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
</contact>
</sensor>
</gazebo> -->
<!-- Visualization of Foot contacts. -->
<!-- <gazebo reference="FR_foot">
<visual>
<plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
<topicName>FR_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="FL_foot">
<visual>
<plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
<topicName>FL_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="RR_foot">
<visual>
<plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
<topicName>RR_foot_contact</topicName>
</plugin>
</visual>
</gazebo>
<gazebo reference="RL_foot">
<visual>
<plugin filename="libunitreeDrawForcePlugin.so" name="drawForcePlugin">
<topicName>RL_foot_contact</topicName>
</plugin>
</visual>
</gazebo> -->
<gazebo reference="base">
<material>Gazebo/Green</material>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="trunk">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="imu_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Red</material>
</gazebo>
<!-- FR leg -->
<gazebo reference="FR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="FR_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="FR_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FR_foot">
<mu1>0.9</mu1>
<mu2>0.9</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- FL leg -->
<gazebo reference="FL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="FL_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="FL_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="FL_foot">
<mu1>0.9</mu1>
<mu2>0.9</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- RR leg -->
<gazebo reference="RR_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="RR_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="RR_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="RR_foot">
<mu1>0.9</mu1>
<mu2>0.9</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- RL leg -->
<gazebo reference="RL_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="RL_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="RL_calf">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="RL_foot">
<mu1>0.9</mu1>
<mu2>0.9</mu2>
<self_collide>1</self_collide>
<material>Gazebo/DarkGrey</material>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<!-- 悬挂 -->
<!-- <link name="world"/>
<joint name="base_static_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="base"/>
</joint> -->
<link name="base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base"/>
<child link="trunk"/>
</joint>
<link name="trunk">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/trunk.STL" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/trunk.STL"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="8.78"/>
<inertia ixx="0.032051" ixy="-0.00023217" ixz="0.002728" iyy="0.13707" iyz="5.6623E-05" izz="0.14946"/>
</inertial>
</link>
<joint name="imu_joint" type="fixed">
<parent link="trunk"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<material name="red"/>
</visual>
<!-- <collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision> -->
</link>
<joint name="FR_hip_joint" type="revolute">
<origin xyz="0.23536 -0.05 0" rpy="0 0 0"/>
<parent link="trunk"/>
<child link="FR_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="17" lower="-0.75" upper="0.75" velocity="12"/>
</joint>
<link name="FR_hip">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/hip_mirror.STL" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/hip_mirror.STL"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 -0.053575 0" rpy="0 0 0"/>
<mass value="0.509"/>
<inertia ixx="0.00038097" ixy="1.2523E-05" ixz="-1.1653E-05" iyy="0.00069383" iyz="5.071E-06" izz="0.00047336"/>
</inertial>
</link>
<joint name="FR_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 -0.10 0"/>
<parent link="FR_hip"/>
<child link="FR_thigh_shoulder"/>
</joint>
<link name="FR_thigh_shoulder">
</link>
<joint name="FR_thigh_joint" type="revolute">
<origin xyz="0 -0.10715 0" rpy="0 0 0"/>
<parent link="FR_hip"/>
<child link="FR_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit lower="-1.257" upper="4.363" effort="24" velocity="12"/>
</joint>
<link name="FR_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/thigh.STL" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/thigh.STL"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 -0.1" rpy="0 0 0"/>
<mass value="0.664"/>
<inertia ixx="0.0033376" ixy="-7.1504E-07" ixz="-0.00019282" iyy="0.0026385" iyz="-9.3033E-06" izz="0.0013093"/>
</inertial>
</link>
<joint name="FR_calf_joint" type="revolute">
<origin xyz="0 0 -0.2" rpy="0 0 0"/>
<parent link="FR_thigh"/>
<child link="FR_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit lower="-2.478" upper="-0.506" effort="24" velocity="12"/>
</joint>
<link name="FR_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/calf.STL" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/calf.STL"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 -0.11439" rpy="0 0 0"/>
<mass value="0.114"/>
<inertia ixx="0.0014553" ixy="-3.2376E-08" ixz="8.3885E-05" iyy="0.0021522" iyz="5.1259E-07" izz="0.00070545"/>
</inertial>
</link>
<joint name="FR_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.217"/>
<parent link="FR_calf"/>
<child link="FR_foot"/>
</joint>
<link name="FR_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.025"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.025"/>
</geometry>
</collision>
<inertial>
<mass value="0.026"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
</inertial>
</link>
<transmission name="FR_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FR_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FR_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FR_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FR_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FR_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FR_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FR_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FR_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<joint name="FL_hip_joint" type="revolute">
<origin xyz="0.23536 0.05 0" rpy="0 0 0"/>
<parent link="trunk"/>
<child link="FL_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0" friction="0"/>
<limit lower="-0.75" upper="0.75" effort="17" velocity="12"/>
</joint>
<link name="FL_hip">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/hip.STL" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/hip.STL"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0.053575 0" rpy="0 0 0"/>
<mass value="0.509"/>
<inertia ixx="0.00038097" ixy="1.2523E-05" ixz="-1.1653E-05" iyy="0.00069383" iyz="5.071E-06" izz="0.00047336"/>
</inertial>
</link>
<joint name="FL_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0.10 0"/>
<parent link="FL_hip"/>
<child link="FL_thigh_shoulder"/>
</joint>
<link name="FL_thigh_shoulder">
</link>
<joint name="FL_thigh_joint" type="revolute">
<origin xyz="0 0.10715 0" rpy="0 0 0"/>
<parent link="FL_hip"/>
<child link="FL_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit lower="-1.257" upper="4.363" effort="24" velocity="12"/>
</joint>
<link name="FL_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/thigh.STL" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/thigh.STL"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 -0.1" rpy="0 0 0"/>
<mass value="0.664"/>
<inertia ixx="0.0033376" ixy="-7.150375E-07" ixz="-0.00019282" iyy="0.0026385" iyz="-9.3033E-06" izz="0.0013093"/>
</inertial>
</link>
<joint name="FL_calf_joint" type="revolute">
<origin xyz="0 0 -0.2" rpy="0 0 0"/>
<parent link="FL_thigh"/>
<child link="FL_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit lower="-2.478" upper="-0.506" effort="24" velocity="12"/>
</joint>
<link name="FL_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/calf.STL" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/calf.STL"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 -0.11439" rpy="0 0 0"/>
<mass value="0.114"/>
<inertia ixx="0.0014553" ixy="-3.2376E-08" ixz="8.3885E-05" iyy="0.0021522" iyz="5.1259E-07" izz="0.00070545"/>
</inertial>
</link>
<joint name="FL_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.217"/>
<parent link="FL_calf"/>
<child link="FL_foot"/>
</joint>
<link name="FL_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.025"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.025"/>
</geometry>
</collision>
<inertial>
<mass value="0.026"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
</inertial>
</link>
<transmission name="FL_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FL_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FL_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FL_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FL_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FL_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="FL_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="FL_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="FL_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<joint name="RR_hip_joint" type="revolute">
<origin xyz="-0.23536 -0.05 0" rpy="0 0 0"/>
<parent link="trunk"/>
<child link="RR_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0" friction="0"/>
<limit lower="-0.75" upper="0.75" effort="17" velocity="12"/>
</joint>
<link name="RR_hip">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/hip_mirror.STL" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/hip_mirror.STL"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 -0.053575 0" rpy="0 0 0"/>
<mass value="0.509"/>
<inertia ixx="0.00038097" ixy="1.2523E-05" ixz="-1.1653E-05" iyy="0.00069383" iyz="5.071E-06" izz="0.00047336"/>
</inertial>
</link>
<joint name="RR_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 -0.10 0"/>
<parent link="RR_hip"/>
<child link="RR_thigh_shoulder"/>
</joint>
<link name="RR_thigh_shoulder">
</link>
<joint name="RR_thigh_joint" type="revolute">
<origin xyz="0 -0.10715 0" rpy="0 0 0"/>
<parent link="RR_hip"/>
<child link="RR_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit lower="-2.01" upper="3.49" effort="24" velocity="12"/>
</joint>
<link name="RR_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/thigh.STL" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/thigh.STL"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 -0.1" rpy="0 0 0"/>
<mass value="0.664"/>
<inertia ixx="0.0033376" ixy="-7.150375E-07" ixz="-0.00019282" iyy="0.0026385" iyz="-9.3033E-06" izz="0.0013093"/>
</inertial>
</link>
<joint name="RR_calf_joint" type="revolute">
<origin xyz="0 0 -0.2" rpy="0 0 0"/>
<parent link="RR_thigh"/>
<child link="RR_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit lower="-2.478" upper="-0.506" effort="24" velocity="12"/>
</joint>
<link name="RR_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/calf.STL" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/calf.STL"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 -0.11439" rpy="0 0 0"/>
<mass value="0.114"/>
<inertia ixx="0.0014553" ixy="-3.2376E-08" ixz="8.3885E-05" iyy="0.0021522" iyz="5.1259E-07" izz="0.00070545"/>
</inertial>
</link>
<joint name="RR_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.217"/>
<parent link="RR_calf"/>
<child link="RR_foot"/>
</joint>
<link name="RR_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.025"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.025"/>
</geometry>
</collision>
<inertial>
<mass value="0.026"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
</inertial>
</link>
<transmission name="BR_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RR_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="BR_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="BR_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RR_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="BR_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="BR_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RR_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="BR_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<joint name="RL_hip_joint" type="revolute">
<origin xyz="-0.23536 0.05 0" rpy="0 0 0"/>
<parent link="trunk"/>
<child link="RL_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0" friction="0"/>
<limit lower="-0.75" upper="0.75" effort="17" velocity="12"/>
</joint>
<link name="RL_hip">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/hip.STL" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/hip.STL"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0.053575 0" rpy="0 0 0"/>
<mass value="0.509"/>
<inertia ixx="0.00038097" ixy="1.2523E-05" ixz="-1.1653E-05" iyy="0.00069383" iyz="5.071E-06" izz="0.00047336"/>
</inertial>
</link>
<joint name="RL_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0.10 0"/>
<parent link="RL_hip"/>
<child link="RL_thigh_shoulder"/>
</joint>
<link name="RL_thigh_shoulder">
</link>
<joint name="RL_thigh_joint" type="revolute">
<origin xyz="0 0.10715 0" rpy="0 0 0"/>
<parent link="RL_hip"/>
<child link="RL_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit lower="-2.01" upper="3.49" effort="24" velocity="12"/>
</joint>
<link name="RL_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/thigh.STL" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/thigh.STL"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 -0.1" rpy="0 0 0"/>
<mass value="0.664"/>
<inertia ixx="0.0033376" ixy="-7.1504E-07" ixz="-0.00019282" iyy="0.0026385" iyz="-9.3033E-06" izz="0.0013093"/>
</inertial>
</link>
<joint name="RL_calf_joint" type="revolute">
<origin xyz="0 0 -0.2" rpy="0 0 0"/>
<parent link="RL_thigh"/>
<child link="RL_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit lower="-2.478" upper="-0.506" effort="24" velocity="12"/>
</joint>
<link name="RL_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/calf.STL" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://cyberdog_old_description/meshes/calf.STL"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 -0.11439" rpy="0 0 0"/>
<mass value="0.114"/>
<inertia ixx="0.0014553" ixy="-3.2376E-08" ixz="8.3885E-05" iyy="0.0021522" iyz="5.1259E-07" izz="0.00070545"/>
</inertial>
</link>
<joint name="RL_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 -0.217"/>
<parent link="RL_calf"/>
<child link="RL_foot"/>
</joint>
<link name="RL_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.025"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.025"/>
</geometry>
</collision>
<inertial>
<mass value="0.026"/>
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
</inertial>
</link>
<transmission name="BL_hip_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RL_hip_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="BL_hip_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="BL_thigh_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RL_thigh_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="BL_thigh_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="BL_calf_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="RL_calf_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="BL_calf_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</robot>