EmbodiedAI
/
quadruped_ros2_control
mirror of https://github.com/legubiao/quadruped_ros2_control.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add gazebo simulation for A1, tested rl controller passed

This commit is contained in:
Huang Zhenbiao 2024-10-08 21:36:42 +08:00
parent 09ec0abde4
commit eaab6d7a69
18 changed files with 821 additions and 941 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
README.md
View File

@ -53,4 +53,5 @@ For more details, please refer to the [unitree guide controller](controllers/uni
### Miscellaneous
[1] Unitree Robotics. *unitree\_guide: An open source project for controlling the quadruped robot of Unitree Robotics, and it is also the software project accompanying 《四足机器人控制算法--建模、控制与实践》 published by Unitree Robotics*. [Online]. Available: [https://github.com/unitreerobotics/unitree_guide](https://github.com/unitreerobotics/unitree_guide)
[2] Qiayuan Liao. *legged\_control: An open-source NMPC, WBC, state estimation, and sim2real framework for legged robots*. [Online]. Available: [https://github.com/qiayuanl/legged_control](https://github.com/qiayuanl/legged_control)
[2] Qiayuan Liao. *legged\_control: An open-source NMPC, WBC, state estimation, and sim2real framework for legged robots*. [Online]. Available: [https://github.com/qiayuanl/legged_control](https://github.com/qiayuanl/legged_control)

48
controllers/legged_gym_controller/include/legged_gym_controller/FSM/StateRL.h
View File

@ -10,6 +10,23 @@
#include "FSMState.h"
template <typename Functor>
void executeAndSleep(Functor f, const double frequency) {
using clock = std::chrono::high_resolution_clock;
const auto start = clock::now();
// Execute wrapped function
f();
// Compute desired duration rounded to clock decimation
const std::chrono::duration<double> desiredDuration(1.0 / frequency);
const auto dt = std::chrono::duration_cast<clock::duration>(desiredDuration);
// Sleep
const auto sleepTill = start + dt;
std::this_thread::sleep_until(sleepTill);
}
template<typename T>
struct RobotCommand {
struct MotorCommand {
@ -38,11 +55,17 @@ struct RobotState {
} motor_state;
};
struct Control
{
double x = 0.0;
double y = 0.0;
double yaw = 0.0;
};
struct ModelParams {
std::string model_name;
std::string framework;
bool use_history;
double dt;
int decimation;
int num_observations;
std::vector<std::string> observations;
@ -66,7 +89,6 @@ struct ModelParams {
torch::Tensor fixed_kd;
torch::Tensor commands_scale;
torch::Tensor default_dof_pos;
std::vector<std::string> joint_controller_names;
};
struct Observations
@ -98,26 +120,40 @@ private:
void loadYaml(const std::string &config_path);
static torch::Tensor quatRotateInverse(torch::Tensor q, torch::Tensor v, const std::string& framework);
static torch::Tensor quatRotateInverse(const torch::Tensor &q, const torch::Tensor& v, const std::string& framework);
/**
* @brief Forward the RL model to get the action
*/
torch::Tensor forward();
void getState();
void runModel();
void setCommand() const;
// Parameters
ModelParams params_;
Observations obs;
Observations obs_;
Control control_;
RobotState<double> robot_state_;
RobotCommand<double> robot_command_;
// history buffer
std::shared_ptr<ObservationBuffer> history_obs_buf_;
torch::Tensor history_obs_;
// rl module
torch::jit::script::Module model;
torch::jit::script::Module model_;
std::thread rl_thread_;
bool running_ = false;
bool updated_ = false;
// output buffer
torch::Tensor output_torques;
torch::Tensor output_dof_pos;
torch::Tensor output_dof_pos_;
};

467
controllers/legged_gym_controller/library/rl_sdk/rl_sdk.cpp
View File

@ -1,467 +0,0 @@
#include "rl_sdk.hpp"
/* You may need to override this Forward() function
torch::Tensor RL_XXX::Forward()
{
torch::autograd::GradMode::set_enabled(false);
torch::Tensor clamped_obs = this->ComputeObservation();
torch::Tensor actions = this->model.forward({clamped_obs}).toTensor();
torch::Tensor clamped_actions = torch::clamp(actions, this->params.clip_actions_lower, this->params.clip_actions_upper);
return clamped_actions;
}
*/
torch::Tensor RL::ComputeObservation()
{
std::vector<torch::Tensor> obs_list;
for(const std::string& observation : this->params.observations)
{
if(observation == "lin_vel")
{
obs_list.push_back(this->obs.lin_vel * this->params.lin_vel_scale);
}
else if(observation == "ang_vel")
{
// obs_list.push_back(this->obs.ang_vel * this->params.ang_vel_scale); // TODO is QuatRotateInverse necessery?
obs_list.push_back(this->QuatRotateInverse(this->obs.base_quat, this->obs.ang_vel, this->params.framework) * this->params.ang_vel_scale);
}
else if(observation == "gravity_vec")
{
obs_list.push_back(this->QuatRotateInverse(this->obs.base_quat, this->obs.gravity_vec, this->params.framework));
}
else if(observation == "commands")
{
obs_list.push_back(this->obs.commands * this->params.commands_scale);
}
else if(observation == "dof_pos")
{
obs_list.push_back((this->obs.dof_pos - this->params.default_dof_pos) * this->params.dof_pos_scale);
}
else if(observation == "dof_vel")
{
obs_list.push_back(this->obs.dof_vel * this->params.dof_vel_scale);
}
else if(observation == "actions")
{
obs_list.push_back(this->obs.actions);
}
}
torch::Tensor obs = torch::cat(obs_list, 1);
torch::Tensor clamped_obs = torch::clamp(obs, -this->params.clip_obs, this->params.clip_obs);
return clamped_obs;
}
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, this->params.num_of_dofs});
this->obs.actions = torch::zeros({1, this->params.num_of_dofs});
}
void RL::InitOutputs()
{
this->output_torques = torch::zeros({1, this->params.num_of_dofs});
this->output_dof_pos = this->params.default_dof_pos;
}
void RL::InitControl()
{
this->control.control_state = STATE_WAITING;
this->control.x = 0.0;
this->control.y = 0.0;
this->control.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;
return output_torques;
}
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, const std::string& framework)
{
torch::Tensor q_w;
torch::Tensor q_vec;
if(framework == "isaacsim")
{
q_w = q.index({torch::indexing::Slice(), 0});
q_vec = q.index({torch::indexing::Slice(), torch::indexing::Slice(1, 4)});
}
else if(framework == "isaacgym")
{
q_w = q.index({torch::indexing::Slice(), 3});
q_vec = q.index({torch::indexing::Slice(), torch::indexing::Slice(0, 3)});
}
c10::IntArrayRef shape = q.sizes();
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)
{
static RobotState<double> start_state;
static RobotState<double> now_state;
static float getup_percent = 0.0;
static float getdown_percent = 0.0;
// waiting
if(this->running_state == STATE_WAITING)
{
for(int i = 0; i < this->params.num_of_dofs; ++i)
{
command->motor_command.q[i] = state->motor_state.q[i];
}
if(this->control.control_state == STATE_POS_GETUP)
{
this->control.control_state = STATE_WAITING;
getup_percent = 0.0;
for(int i = 0; i < this->params.num_of_dofs; ++i)
{
now_state.motor_state.q[i] = state->motor_state.q[i];
start_state.motor_state.q[i] = now_state.motor_state.q[i];
}
this->running_state = STATE_POS_GETUP;
std::cout << std::endl << LOGGER::INFO << "Switching to STATE_POS_GETUP" << std::endl;
}
}
// stand up (position control)
else if(this->running_state == STATE_POS_GETUP)
{
if(getup_percent < 1.0)
{
getup_percent += 1 / 500.0;
getup_percent = getup_percent > 1.0 ? 1.0 : getup_percent;
for(int i = 0; i < this->params.num_of_dofs; ++i)
{
command->motor_command.q[i] = (1 - getup_percent) * now_state.motor_state.q[i] + getup_percent * this->params.default_dof_pos[0][i].item<double>();
command->motor_command.dq[i] = 0;
command->motor_command.kp[i] = this->params.fixed_kp[0][i].item<double>();
command->motor_command.kd[i] = this->params.fixed_kd[0][i].item<double>();
command->motor_command.tau[i] = 0;
}
std::cout << "\r" << std::flush << LOGGER::INFO << "Getting up " << std::fixed << std::setprecision(2) << getup_percent * 100.0 << std::flush;
}
if(this->control.control_state == STATE_RL_INIT)
{
this->control.control_state = STATE_WAITING;
this->running_state = STATE_RL_INIT;
std::cout << std::endl << LOGGER::INFO << "Switching to STATE_RL_INIT" << std::endl;
}
else if(this->control.control_state == STATE_POS_GETDOWN)
{
this->control.control_state = STATE_WAITING;
getdown_percent = 0.0;
for(int i = 0; i < this->params.num_of_dofs; ++i)
{
now_state.motor_state.q[i] = state->motor_state.q[i];
}
this->running_state = STATE_POS_GETDOWN;
std::cout << std::endl << LOGGER::INFO << "Switching to STATE_POS_GETDOWN" << std::endl;
}
}
// init obs and start rl loop
else if(this->running_state == STATE_RL_INIT)
{
if(getup_percent == 1)
{
this->InitObservations();
this->InitOutputs();
this->InitControl();
this->running_state = STATE_RL_RUNNING;
std::cout << std::endl << LOGGER::INFO << "Switching to STATE_RL_RUNNING" << std::endl;
}
}
// rl loop
else if(this->running_state == STATE_RL_RUNNING)
{
std::cout << "\r" << std::flush << LOGGER::INFO << "RL Controller x:" << this->control.x << " y:" << this->control.y << " yaw:" << this->control.yaw << std::flush;
for(int i = 0; i < this->params.num_of_dofs; ++i)
{
command->motor_command.q[i] = this->output_dof_pos[0][i].item<double>();
command->motor_command.dq[i] = 0;
command->motor_command.kp[i] = this->params.rl_kp[0][i].item<double>();
command->motor_command.kd[i] = this->params.rl_kd[0][i].item<double>();
command->motor_command.tau[i] = 0;
}
if(this->control.control_state == STATE_POS_GETDOWN)
{
this->control.control_state = STATE_WAITING;
getdown_percent = 0.0;
for(int i = 0; i < this->params.num_of_dofs; ++i)
{
now_state.motor_state.q[i] = state->motor_state.q[i];
}
this->running_state = STATE_POS_GETDOWN;
std::cout << std::endl << LOGGER::INFO << "Switching to STATE_POS_GETDOWN" << std::endl;
}
else if(this->control.control_state == STATE_POS_GETUP)
{
this->control.control_state = STATE_WAITING;
getup_percent = 0.0;
for(int i = 0; i < this->params.num_of_dofs; ++i)
{
now_state.motor_state.q[i] = state->motor_state.q[i];
}
this->running_state = STATE_POS_GETUP;
std::cout << std::endl << LOGGER::INFO << "Switching to STATE_POS_GETUP" << std::endl;
}
}
// get down (position control)
else if(this->running_state == STATE_POS_GETDOWN)
{
if(getdown_percent < 1.0)
{
getdown_percent += 1 / 500.0;
getdown_percent = getdown_percent > 1.0 ? 1.0 : getdown_percent;
for(int i = 0; i < this->params.num_of_dofs; ++i)
{
command->motor_command.q[i] = (1 - getdown_percent) * now_state.motor_state.q[i] + getdown_percent * start_state.motor_state.q[i];
command->motor_command.dq[i] = 0;
command->motor_command.kp[i] = this->params.fixed_kp[0][i].item<double>();
command->motor_command.kd[i] = this->params.fixed_kd[0][i].item<double>();
command->motor_command.tau[i] = 0;
}
std::cout << "\r" << std::flush << LOGGER::INFO << "Getting down " << std::fixed << std::setprecision(2) << getdown_percent * 100.0 << std::flush;
}
if(getdown_percent == 1)
{
this->InitObservations();
this->InitOutputs();
this->InitControl();
this->running_state = STATE_WAITING;
std::cout << std::endl << LOGGER::INFO << "Switching to STATE_WAITING" << std::endl;
}
}
}
void RL::TorqueProtect(torch::Tensor origin_output_torques)
{
std::vector<int> out_of_range_indices;
std::vector<double> out_of_range_values;
for(int i = 0; i < origin_output_torques.size(1); ++i)
{
double torque_value = origin_output_torques[0][i].item<double>();
double limit_lower = -this->params.torque_limits[0][i].item<double>();
double limit_upper = this->params.torque_limits[0][i].item<double>();
if(torque_value < limit_lower || torque_value > limit_upper)
{
out_of_range_indices.push_back(i);
out_of_range_values.push_back(torque_value);
}
}
if(!out_of_range_indices.empty())
{
for(int i = 0; i < out_of_range_indices.size(); ++i)
{
int index = out_of_range_indices[i];
double value = out_of_range_values[i];
double limit_lower = -this->params.torque_limits[0][index].item<double>();
double limit_upper = this->params.torque_limits[0][index].item<double>();
std::cout << LOGGER::WARNING << "Torque(" << index+1 << ")=" << value << " out of range(" << limit_lower << ", " << limit_upper << ")" << std::endl;
}
// Just a reminder, no protection
// this->control.control_state = STATE_POS_GETDOWN;
// std::cout << LOGGER::INFO << "Switching to STATE_POS_GETDOWN"<< std::endl;
}
}
#include <termios.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::KeyboardInterface()
{
if(kbhit())
{
int c = fgetc(stdin);
switch(c)
{
case '0': this->control.control_state = STATE_POS_GETUP; break;
case 'p': this->control.control_state = STATE_RL_INIT; break;
case '1': this->control.control_state = STATE_POS_GETDOWN; break;
case 'q': break;
case 'w': this->control.x += 0.1; break;
case 's': this->control.x -= 0.1; break;
case 'a': this->control.yaw += 0.1; break;
case 'd': this->control.yaw -= 0.1; break;
case 'i': break;
case 'k': break;
case 'j': this->control.y += 0.1; break;
case 'l': this->control.y -= 0.1; break;
case ' ': this->control.x = 0; this->control.y = 0; this->control.yaw = 0; break;
case 'r': this->control.control_state = STATE_RESET_SIMULATION; break;
case '\n': this->control.control_state = STATE_TOGGLE_SIMULATION; break;
default: break;
}
}
}
template<typename T>
std::vector<T> ReadVectorFromYaml(const YAML::Node& node)
{
std::vector<T> values;
for(const auto& val : node)
{
values.push_back(val.as<T>());
}
return values;
}
template<typename T>
std::vector<T> ReadVectorFromYaml(const YAML::Node& node, const std::string& framework, const int& rows, const int& cols)
{
std::vector<T> values;
for(const auto& val : node)
{
values.push_back(val.as<T>());
}
if(framework == "isaacsim")
{
std::vector<T> transposed_values(cols * rows);
for(int r = 0; r < rows; ++r)
{
for(int c = 0; c < cols; ++c)
{
transposed_values[c * rows + r] = values[r * cols + c];
}
}
return transposed_values;
}
else if(framework == "isaacgym")
{
return values;
}
else
{
throw std::invalid_argument("Unsupported framework: " + framework);
}
}
void RL::ReadYaml(std::string robot_name)
{
// // The config file is located at "rl_sar/src/rl_sar/models/<robot_name>/config.yaml"
// std::string config_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + robot_name + "/config.yaml";
// YAML::Node config;
// try
// {
// config = YAML::LoadFile(config_path)[robot_name];
// } catch(YAML::BadFile &e)
// {
// std::cout << LOGGER::ERROR << "The file '" << config_path << "' does not exist" << std::endl;
// return;
// }
//
// this->params.model_name = config["model_name"].as<std::string>();
// this->params.framework = config["framework"].as<std::string>();
// int rows = config["rows"].as<int>();
// int cols = config["cols"].as<int>();
// this->params.use_history = config["use_history"].as<bool>();
// this->params.dt = config["dt"].as<double>();
// this->params.decimation = config["decimation"].as<int>();
// this->params.num_observations = config["num_observations"].as<int>();
// this->params.observations = ReadVectorFromYaml<std::string>(config["observations"]);
// this->params.clip_obs = config["clip_obs"].as<double>();
// if(config["clip_actions_lower"].IsNull() && config["clip_actions_upper"].IsNull())
// {
// this->params.clip_actions_upper = torch::tensor({}).view({1, -1});
// this->params.clip_actions_lower = torch::tensor({}).view({1, -1});
// }
// else
// {
// this->params.clip_actions_upper = torch::tensor(ReadVectorFromYaml<double>(config["clip_actions_upper"], this->params.framework, rows, cols)).view({1, -1});
// this->params.clip_actions_lower = torch::tensor(ReadVectorFromYaml<double>(config["clip_actions_lower"], this->params.framework, rows, cols)).view({1, -1});
// }
// this->params.action_scale = config["action_scale"].as<double>();
// this->params.hip_scale_reduction = config["hip_scale_reduction"].as<double>();
// this->params.hip_scale_reduction_indices = ReadVectorFromYaml<int>(config["hip_scale_reduction_indices"]);
// this->params.num_of_dofs = config["num_of_dofs"].as<int>();
// this->params.lin_vel_scale = config["lin_vel_scale"].as<double>();
// this->params.ang_vel_scale = config["ang_vel_scale"].as<double>();
// this->params.dof_pos_scale = config["dof_pos_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({this->params.lin_vel_scale, this->params.lin_vel_scale, this->params.ang_vel_scale});
// this->params.rl_kp = torch::tensor(ReadVectorFromYaml<double>(config["rl_kp"], this->params.framework, rows, cols)).view({1, -1});
// this->params.rl_kd = torch::tensor(ReadVectorFromYaml<double>(config["rl_kd"], this->params.framework, rows, cols)).view({1, -1});
// this->params.fixed_kp = torch::tensor(ReadVectorFromYaml<double>(config["fixed_kp"], this->params.framework, rows, cols)).view({1, -1});
// this->params.fixed_kd = torch::tensor(ReadVectorFromYaml<double>(config["fixed_kd"], this->params.framework, rows, cols)).view({1, -1});
// this->params.torque_limits = torch::tensor(ReadVectorFromYaml<double>(config["torque_limits"], this->params.framework, rows, cols)).view({1, -1});
// this->params.default_dof_pos = torch::tensor(ReadVectorFromYaml<double>(config["default_dof_pos"], this->params.framework, rows, cols)).view({1, -1});
// this->params.joint_controller_names = ReadVectorFromYaml<std::string>(config["joint_controller_names"], this->params.framework, rows, cols);
}
void RL::CSVInit(std::string robot_name)
{
csv_filename = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + robot_name + "/motor";
// Uncomment these lines if need timestamp for file name
// auto now = std::chrono::system_clock::now();
// std::time_t now_c = std::chrono::system_clock::to_time_t(now);
// std::stringstream ss;
// ss << std::put_time(std::localtime(&now_c), "%Y%m%d%H%M%S");
// std::string timestamp = ss.str();
// csv_filename += "_" + timestamp;
csv_filename += ".csv";
std::ofstream file(csv_filename.c_str());
for(int i = 0; i < 12; ++i) {file << "tau_cal_" << i << ",";}
for(int i = 0; i < 12; ++i) {file << "tau_est_" << i << ",";}
for(int i = 0; i < 12; ++i) {file << "joint_pos_" << i << ",";}
for(int i = 0; i < 12; ++i) {file << "joint_pos_target_" << i << ",";}
for(int i = 0; i < 12; ++i) {file << "joint_vel_" << i << ",";}
file << std::endl;
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)
{
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 << 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_target[0][i].item<double>() << ",";}
for(int i = 0; i < 12; ++i) {file << joint_vel[0][i].item<double>() << ",";}
file << std::endl;
file.close();
}

164
controllers/legged_gym_controller/library/rl_sdk/rl_sdk.hpp
View File

@ -1,164 +0,0 @@
#ifndef RL_SDK_HPP
#define RL_SDK_HPP
#include <torch/script.h>
#include <string>
namespace LOGGER {
const char* const INFO = "\033[0;37m[INFO]\033[0m ";
const char* const WARNING = "\033[0;33m[WARNING]\033[0m ";
const char* const ERROR = "\033[0;31m[ERROR]\033[0m ";
const char* const DEBUG = "\033[0;32m[DEBUG]\033[0m ";
}
template<typename T>
struct RobotCommand
{
struct MotorCommand
{
std::vector<T> q = std::vector<T>(32, 0.0);
std::vector<T> dq = std::vector<T>(32, 0.0);
std::vector<T> tau = std::vector<T>(32, 0.0);
std::vector<T> kp = std::vector<T>(32, 0.0);
std::vector<T> kd = std::vector<T>(32, 0.0);
} motor_command;
};
template<typename T>
struct RobotState
{
struct IMU
{
std::vector<T> quaternion = {1.0, 0.0, 0.0, 0.0}; // w, x, y, z
std::vector<T> gyroscope = {0.0, 0.0, 0.0};
std::vector<T> accelerometer = {0.0, 0.0, 0.0};
} imu;
struct MotorState
{
std::vector<T> q = std::vector<T>(32, 0.0);
std::vector<T> dq = std::vector<T>(32, 0.0);
std::vector<T> ddq = std::vector<T>(32, 0.0);
std::vector<T> tauEst = std::vector<T>(32, 0.0);
std::vector<T> cur = std::vector<T>(32, 0.0);
} motor_state;
};
enum STATE {
STATE_WAITING = 0,
STATE_POS_GETUP,
STATE_RL_INIT,
STATE_RL_RUNNING,
STATE_POS_GETDOWN,
STATE_RESET_SIMULATION,
STATE_TOGGLE_SIMULATION,
};
struct Control
{
STATE control_state;
double x = 0.0;
double y = 0.0;
double yaw = 0.0;
};
struct ModelParams
{
std::string model_name;
std::string framework;
bool use_history;
double dt;
int decimation;
int num_observations;
std::vector<std::string> observations;
double damping;
double stiffness;
double action_scale;
double hip_scale_reduction;
std::vector<int> hip_scale_reduction_indices;
int num_of_dofs;
double lin_vel_scale;
double ang_vel_scale;
double dof_pos_scale;
double dof_vel_scale;
double clip_obs;
torch::Tensor clip_actions_upper;
torch::Tensor clip_actions_lower;
torch::Tensor torque_limits;
torch::Tensor rl_kd;
torch::Tensor rl_kp;
torch::Tensor fixed_kp;
torch::Tensor fixed_kd;
torch::Tensor commands_scale;
torch::Tensor default_dof_pos;
std::vector<std::string> joint_controller_names;
};
struct Observations
{
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;
};
class RL
{
public:
RL(){};
~RL(){};
ModelParams params;
Observations obs;
RobotState<double> robot_state;
RobotCommand<double> robot_command;
// init
void InitObservations();
void InitOutputs();
void InitControl();
// rl functions
virtual torch::Tensor Forward() = 0;
torch::Tensor ComputeObservation();
virtual void GetState(RobotState<double> *state) = 0;
virtual void SetCommand(const RobotCommand<double> *command) = 0;
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, const std::string& framework);
// 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);
// control
Control control;
void KeyboardInterface();
// others
std::string robot_name;
STATE running_state = STATE_RL_RUNNING; // default running_state set to STATE_RL_RUNNING
bool simulation_running = false;
// protect func
void TorqueProtect(torch::Tensor origin_output_torques);
protected:
// rl module
torch::jit::script::Module model;
// output buffer
torch::Tensor output_torques;
torch::Tensor output_dof_pos;
};
#endif

2
controllers/legged_gym_controller/src/FSM/StateFixedDown.cpp
View File

@ -44,6 +44,8 @@ FSMStateName StateFixedDown::checkChange() {
return FSMStateName::PASSIVE;
case 2:
return FSMStateName::FIXEDSTAND;
case 3:
return FSMStateName::RL;
default:
return FSMStateName::FIXEDDOWN;
}

2
controllers/legged_gym_controller/src/FSM/StateFixedStand.cpp
View File

@ -45,6 +45,8 @@ FSMStateName StateFixedStand::checkChange() {
return FSMStateName::PASSIVE;
case 2:
return FSMStateName::FIXEDDOWN;
case 3:
return FSMStateName::RL;
default:
return FSMStateName::FIXEDSTAND;
}

148
controllers/legged_gym_controller/src/FSM/StateRL.cpp
View File

@ -49,18 +49,57 @@ StateRL::StateRL(CtrlComponent &ctrl_component, const std::string &config_path)
history_obs_buf_ = std::make_shared<ObservationBuffer>(1, params_.num_observations, 6);
}
model = torch::jit::load(config_path + "/" + params_.model_name);
model_ = torch::jit::load(config_path + "/" + params_.model_name);
std::cout << "Model loaded: " << config_path + "/" + params_.model_name << std::endl;
rl_thread_ = std::thread([&] {
while (true) {
try {
executeAndSleep(
[&] {
if (running_) {
runModel();
}
},
ctrl_comp_.frequency_ / params_.decimation);
} catch (const std::exception &e) {
running_ = false;
RCLCPP_ERROR(rclcpp::get_logger("StateRL"), "Error in RL thread: %s", e.what());
}
}
});
}
void StateRL::enter() {
// Init observations
obs_.lin_vel = torch::tensor({{0.0, 0.0, 0.0}});
obs_.ang_vel = torch::tensor({{0.0, 0.0, 0.0}});
obs_.gravity_vec = torch::tensor({{0.0, 0.0, -1.0}});
obs_.commands = torch::tensor({{0.0, 0.0, 0.0}});
obs_.base_quat = torch::tensor({{0.0, 0.0, 0.0, 1.0}});
obs_.dof_pos = params_.default_dof_pos;
obs_.dof_vel = torch::zeros({1, params_.num_of_dofs});
obs_.actions = torch::zeros({1, params_.num_of_dofs});
// Init output
output_torques = torch::zeros({1, params_.num_of_dofs});
output_dof_pos_ = params_.default_dof_pos;
// Init control
control_.x = 0.0;
control_.y = 0.0;
control_.yaw = 0.0;
running_ = true;
}
void StateRL::run() {
getState();
setCommand();
}
void StateRL::exit() {
running_ = false;
}
FSMStateName StateRL::checkChange() {
@ -79,25 +118,25 @@ torch::Tensor StateRL::computeObservation() {
for (const std::string &observation: params_.observations) {
if (observation == "lin_vel") {
obs_list.push_back(obs.lin_vel * params_.lin_vel_scale);
obs_list.push_back(obs_.lin_vel * params_.lin_vel_scale);
} else if (observation == "ang_vel") {
// obs_list.push_back(obs.ang_vel * params_.ang_vel_scale); // TODO is QuatRotateInverse necessery?
obs_list.push_back(
this->QuatRotateInverse(obs.base_quat, obs.ang_vel, params_.framework) * params_.ang_vel_scale);
quatRotateInverse(obs_.base_quat, obs_.ang_vel, params_.framework) * params_.ang_vel_scale);
} else if (observation == "gravity_vec") {
obs_list.push_back(this->QuatRotateInverse(obs.base_quat, obs.gravity_vec, params_.framework));
obs_list.push_back(quatRotateInverse(obs_.base_quat, obs_.gravity_vec, params_.framework));
} else if (observation == "commands") {
obs_list.push_back(obs.commands * params_.commands_scale);
obs_list.push_back(obs_.commands * params_.commands_scale);
} else if (observation == "dof_pos") {
obs_list.push_back((obs.dof_pos - params_.default_dof_pos) * params_.dof_pos_scale);
obs_list.push_back((obs_.dof_pos - params_.default_dof_pos) * params_.dof_pos_scale);
} else if (observation == "dof_vel") {
obs_list.push_back(obs.dof_vel * params_.dof_vel_scale);
obs_list.push_back(obs_.dof_vel * params_.dof_vel_scale);
} else if (observation == "actions") {
obs_list.push_back(obs.actions);
obs_list.push_back(obs_.actions);
}
}
torch::Tensor obs = cat(obs_list, 1);
const torch::Tensor obs = cat(obs_list, 1);
torch::Tensor clamped_obs = clamp(obs, -params_.clip_obs, params_.clip_obs);
return clamped_obs;
}
@ -118,7 +157,6 @@ void StateRL::loadYaml(const std::string &config_path) {
const int rows = config["rows"].as<int>();
const int cols = config["cols"].as<int>();
params_.use_history = config["use_history"].as<bool>();
params_.dt = config["dt"].as<double>();
params_.decimation = config["decimation"].as<int>();
params_.num_observations = config["num_observations"].as<int>();
params_.observations = ReadVectorFromYaml<std::string>(config["observations"]);
@ -156,11 +194,9 @@ void StateRL::loadYaml(const std::string &config_path) {
ReadVectorFromYaml<double>(config["torque_limits"], params_.framework, rows, cols)).view({1, -1});
params_.default_dof_pos = torch::tensor(
ReadVectorFromYaml<double>(config["default_dof_pos"], params_.framework, rows, cols)).view({1, -1});
params_.joint_controller_names = ReadVectorFromYaml<std::string>(config["joint_controller_names"],
params_.framework, rows, cols);
}
torch::Tensor StateRL::quatRotateInverse(torch::Tensor q, torch::Tensor v, const std::string &framework) {
torch::Tensor StateRL::quatRotateInverse(const torch::Tensor &q, const torch::Tensor &v, const std::string &framework) {
torch::Tensor q_w;
torch::Tensor q_vec;
if (framework == "isaacsim") {
@ -170,11 +206,11 @@ torch::Tensor StateRL::quatRotateInverse(torch::Tensor q, torch::Tensor v, const
q_w = q.index({torch::indexing::Slice(), 3});
q_vec = q.index({torch::indexing::Slice(), torch::indexing::Slice(0, 3)});
}
c10::IntArrayRef shape = q.sizes();
const c10::IntArrayRef shape = q.sizes();
const torch::Tensor a = v * (2.0 * torch::pow(q_w, 2) - 1.0).unsqueeze(-1);
const torch::Tensor b = torch::cross(q_vec, v, -1) * q_w.unsqueeze(-1) * 2.0;
const torch::Tensor c = q_vec * torch::bmm(q_vec.view({shape[0], 1, 3}), v.view({shape[0], 3, 1})).squeeze(-1) * 2.0;
const torch::Tensor b = cross(q_vec, v, -1) * q_w.unsqueeze(-1) * 2.0;
const torch::Tensor c = q_vec * bmm(q_vec.view({shape[0], 1, 3}), v.view({shape[0], 3, 1})).squeeze(-1) * 2.0;
return a - b + c;
}
@ -186,9 +222,9 @@ torch::Tensor StateRL::forward() {
if (params_.use_history) {
history_obs_buf_->insert(clamped_obs);
history_obs_ = history_obs_buf_->getObsVec({0, 1, 2, 3, 4, 5});
actions = model.forward({history_obs_}).toTensor();
actions = model_.forward({history_obs_}).toTensor();
} else {
actions = model.forward({clamped_obs}).toTensor();
actions = model_.forward({clamped_obs}).toTensor();
}
if (params_.clip_actions_upper.numel() != 0 && params_.clip_actions_lower.numel() != 0) {
@ -196,3 +232,77 @@ torch::Tensor StateRL::forward() {
}
return actions;
}
void StateRL::getState() {
if (params_.framework == "isaacgym") {
robot_state_.imu.quaternion[3] = ctrl_comp_.imu_state_interface_[0].get().get_value();
robot_state_.imu.quaternion[0] = ctrl_comp_.imu_state_interface_[1].get().get_value();
robot_state_.imu.quaternion[1] = ctrl_comp_.imu_state_interface_[2].get().get_value();
robot_state_.imu.quaternion[2] = ctrl_comp_.imu_state_interface_[3].get().get_value();
} else if (params_.framework == "isaacsim") {
robot_state_.imu.quaternion[0] = ctrl_comp_.imu_state_interface_[0].get().get_value();
robot_state_.imu.quaternion[1] = ctrl_comp_.imu_state_interface_[1].get().get_value();
robot_state_.imu.quaternion[2] = ctrl_comp_.imu_state_interface_[2].get().get_value();
robot_state_.imu.quaternion[3] = ctrl_comp_.imu_state_interface_[3].get().get_value();
}
robot_state_.imu.gyroscope[0] = ctrl_comp_.imu_state_interface_[4].get().get_value();
robot_state_.imu.gyroscope[1] = ctrl_comp_.imu_state_interface_[5].get().get_value();
robot_state_.imu.gyroscope[2] = ctrl_comp_.imu_state_interface_[6].get().get_value();
robot_state_.imu.accelerometer[0] = ctrl_comp_.imu_state_interface_[7].get().get_value();
robot_state_.imu.accelerometer[1] = ctrl_comp_.imu_state_interface_[8].get().get_value();
robot_state_.imu.accelerometer[2] = ctrl_comp_.imu_state_interface_[9].get().get_value();
for (int i = 0; i < 12; i++) {
robot_state_.motor_state.q[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
robot_state_.motor_state.dq[i] = ctrl_comp_.joint_velocity_state_interface_[i].get().get_value();
robot_state_.motor_state.tauEst[i] = ctrl_comp_.joint_effort_state_interface_[i].get().get_value();
}
control_.x = ctrl_comp_.control_inputs_.ly;
control_.y = -ctrl_comp_.control_inputs_.lx;
control_.yaw = -ctrl_comp_.control_inputs_.rx;
updated_ = true;
}
void StateRL::runModel() {
obs_.ang_vel = torch::tensor(robot_state_.imu.gyroscope).unsqueeze(0);
obs_.commands = torch::tensor({{control_.x, control_.y, control_.yaw}});
obs_.base_quat = torch::tensor(robot_state_.imu.quaternion).unsqueeze(0);
obs_.dof_pos = torch::tensor(robot_state_.motor_state.q).narrow(0, 0, params_.num_of_dofs).unsqueeze(0);
obs_.dof_vel = torch::tensor(robot_state_.motor_state.dq).narrow(0, 0, params_.num_of_dofs).unsqueeze(0);
const torch::Tensor clamped_actions = forward();
for (const int i: params_.hip_scale_reduction_indices) {
clamped_actions[0][i] *= params_.hip_scale_reduction;
}
obs_.actions = clamped_actions;
const torch::Tensor actions_scaled = clamped_actions * params_.action_scale;
// torch::Tensor output_torques = params_.rl_kp * (actions_scaled + params_.default_dof_pos - obs_.dof_pos) - params_.rl_kd * obs_.dof_vel;
// output_torques = clamp(output_torques, -(params_.torque_limits), params_.torque_limits);
output_dof_pos_ = actions_scaled + params_.default_dof_pos;
for (int i = 0; i < params_.num_of_dofs; ++i) {
robot_command_.motor_command.q[i] = output_dof_pos_[0][i].item<double>();
robot_command_.motor_command.dq[i] = 0;
robot_command_.motor_command.kp[i] = params_.rl_kp[0][i].item<double>();
robot_command_.motor_command.kd[i] = params_.rl_kd[0][i].item<double>();
robot_command_.motor_command.tau[i] = 0;
}
}
void StateRL::setCommand() const {
for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_position_command_interface_[i].get().set_value(robot_command_.motor_command.q[i]);
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(robot_command_.motor_command.dq[i]);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(robot_command_.motor_command.kp[i]);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(robot_command_.motor_command.kd[i]);
ctrl_comp_.joint_torque_command_interface_[i].get().set_value(robot_command_.motor_command.tau[i]);
}
}

13
controllers/legged_gym_controller/src/LeggedGymController.cpp
View File

@ -13,7 +13,11 @@ namespace legged_gym_controller {
conf.names.reserve(joint_names_.size() * command_interface_types_.size());
for (const auto &joint_name: joint_names_) {
for (const auto &interface_type: command_interface_types_) {
conf.names.push_back(joint_name + "/" += interface_type);
if (!command_prefix_.empty()) {
conf.names.push_back(command_prefix_ + "/" + joint_name + "/" += interface_type);
} else {
conf.names.push_back(joint_name + "/" += interface_type);
}
}
}
@ -72,6 +76,8 @@ namespace legged_gym_controller {
state_interface_types_ =
auto_declare<std::vector<std::string> >("state_interfaces", state_interface_types_);
command_prefix_ = auto_declare<std::string>("command_prefix", command_prefix_);
// imu sensor
imu_name_ = auto_declare<std::string>("imu_name", imu_name_);
imu_interface_types_ = auto_declare<std::vector<std::string> >("imu_interfaces", state_interface_types_);
@ -79,6 +85,8 @@ namespace legged_gym_controller {
// rl config folder
rl_config_folder_ = auto_declare<std::string>("config_folder", rl_config_folder_);
get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
RCLCPP_INFO(get_node()->get_logger(), "Controller Update Rate: %d Hz", ctrl_comp_.frequency_);
} catch (const std::exception &e) {
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
return controller_interface::CallbackReturn::ERROR;
@ -99,9 +107,6 @@ namespace legged_gym_controller {
ctrl_comp_.control_inputs_.ry = msg->ry;
});
get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_);
return CallbackReturn::SUCCESS;
}

2
controllers/legged_gym_controller/src/LeggedGymController.h
View File

@ -72,6 +72,8 @@ namespace legged_gym_controller {
std::vector<std::string> command_interface_types_;
std::vector<std::string> state_interface_types_;
std::string command_prefix_;
// IMU Sensor
std::string imu_name_;
std::vector<std::string> imu_interface_types_;

15
descriptions/unitree/a1_description/README.md
View File

@ -10,7 +10,7 @@ Tested environment:
## Build
```bash
cd ~/ros2_ws
colcon build --packages-up-to a1_description
colcon build --packages-up-to a1_description --symlink-install
```
## Visualize the robot
@ -21,6 +21,7 @@ ros2 launch a1_description visualize.launch.py
```
## Launch ROS2 Control
### Mujoco Simulator
* Unitree Guide Controller
```bash
source ~/ros2_ws/install/setup.bash
@ -36,3 +37,15 @@ ros2 launch a1_description visualize.launch.py
source ~/ros2_ws/install/setup.bash
ros2 launch a1_description rl_control.launch.py
```
### Gazebo Simulator
* Unitree Guide Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch a1_description gazebo_unitree_guide.launch.py
```
* Legged Gym Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch a1_description gazebo_rl_control.launch.py
```

142
descriptions/unitree/a1_description/config/gazebo.yaml Normal file
View File

@ -0,0 +1,142 @@
# Controller Manager configuration
controller_manager:
ros__parameters:
update_rate: 2000 # Hz
# Define the available controllers
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
imu_sensor_broadcaster:
type: imu_sensor_broadcaster/IMUSensorBroadcaster
leg_pd_controller:
type: leg_pd_controller/LegPdController
unitree_guide_controller:
type: unitree_guide_controller/UnitreeGuideController
legged_gym_controller:
type: legged_gym_controller/LeggedGymController
imu_sensor_broadcaster:
ros__parameters:
sensor_name: "imu_sensor"
frame_id: "imu_link"
leg_pd_controller:
ros__parameters:
joints:
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- FL_hip_joint
- FL_thigh_joint
- FL_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
command_interfaces:
- effort
state_interfaces:
- position
- velocity
unitree_guide_controller:
ros__parameters:
command_prefix: "leg_pd_controller"
joints:
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- FL_hip_joint
- FL_thigh_joint
- FL_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
command_interfaces:
- effort
- position
- velocity
- kp
- kd
state_interfaces:
- effort
- position
- velocity
feet_names:
- FR_foot
- FL_foot
- RR_foot
- RL_foot
imu_name: "imu_sensor"
base_name: "base"
imu_interfaces:
- orientation.w
- orientation.x
- orientation.y
- orientation.z
- angular_velocity.x
- angular_velocity.y
- angular_velocity.z
- linear_acceleration.x
- linear_acceleration.y
- linear_acceleration.z
legged_gym_controller:
ros__parameters:
update_rate: 200 # Hz
config_folder: "/home/tlab-uav/ros2_ws/install/a1_description/share/a1_description/config/issacgym"
command_prefix: "leg_pd_controller"
joints:
- FL_hip_joint
- FL_thigh_joint
- FL_calf_joint
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
command_interfaces:
- effort
- position
- velocity
- kp
- kd
state_interfaces:
- effort
- position
- velocity
imu_name: "imu_sensor"
imu_interfaces:
- orientation.w
- orientation.x
- orientation.y
- orientation.z
- angular_velocity.x
- angular_velocity.y
- angular_velocity.z
- linear_acceleration.x
- linear_acceleration.y
- linear_acceleration.z

21
descriptions/unitree/a1_description/config/issacgym/config.yaml
View File

@ -3,7 +3,6 @@ framework: "isaacgym"
rows: 4
cols: 3
use_history: True
dt: 0.005
decimation: 4
num_observations: 45
observations: ["ang_vel", "gravity_vec", "commands", "dof_pos", "dof_vel", "actions"]
@ -16,14 +15,14 @@ clip_actions_upper: [100, 100, 100,
100, 100, 100,
100, 100, 100,
100, 100, 100]
rl_kp: [20, 20, 20,
20, 20, 20,
20, 20, 20,
20, 20, 20]
rl_kd: [0.5, 0.5, 0.5,
0.5, 0.5, 0.5,
0.5, 0.5, 0.5,
0.5, 0.5, 0.5]
rl_kp: [13.5, 13.5, 13.5,
13.5, 13.5, 13.5,
13.5, 13.5, 13.5,
13.5, 13.5, 13.5]
rl_kd: [0.41, 0.41, 0.41,
0.41, 0.41, 0.41,
0.41, 0.41, 0.41,
0.41, 0.41, 0.41]
fixed_kp: [80, 80, 80,
80, 80, 80,
80, 80, 80,
@ -49,7 +48,3 @@ default_dof_pos: [ 0.1000, 0.8000, -1.5000,
-0.1000, 0.8000, -1.5000,
0.1000, 1.0000, -1.5000,
-0.1000, 1.0000, -1.5000]
joint_controller_names: ["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"]

14
descriptions/unitree/a1_description/config/robot_control.yaml
View File

@ -135,20 +135,20 @@ ocs2_quadruped_controller:
legged_gym_controller:
ros__parameters:
update_rate: 500 # Hz
update_rate: 100 # Hz
joints:
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- FL_hip_joint
- FL_thigh_joint
- FL_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
command_interfaces:
- effort

111
descriptions/unitree/a1_description/launch/gazebo_rl_control.launch.py Normal file
View File

@ -0,0 +1,111 @@
import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.event_handlers import OnProcessExit
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
package_description = "a1_description"
def process_xacro():
pkg_path = os.path.join(get_package_share_directory(package_description))
xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
robot_description_config = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true'})
return robot_description_config.toxml()
def generate_launch_description():
rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
robot_description = process_xacro()
gz_spawn_entity = Node(
package='ros_gz_sim',
executable='create',
output='screen',
arguments=['-topic', 'robot_description', '-name',
'a1', '-allow_renaming', 'true', '-z', '0.4'],
)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[
{
'publish_frequency': 20.0,
'use_tf_static': True,
'robot_description': robot_description,
'ignore_timestamp': True
}
],
)
joint_state_publisher = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster",
"--controller-manager", "/controller_manager"],
)
imu_sensor_broadcaster = Node(
package="controller_manager",
executable="spawner",
arguments=["imu_sensor_broadcaster",
"--controller-manager", "/controller_manager"],
)
leg_pd_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["leg_pd_controller",
"--controller-manager", "/controller_manager"],
)
controller = Node(
package="controller_manager",
executable="spawner",
arguments=["legged_gym_controller", "--controller-manager", "/controller_manager"],
parameters=[
{
'config_folder': os.path.join(get_package_share_directory(package_description), 'config',
'issacgym'),
}],
)
return LaunchDescription([
Node(
package='rviz2',
executable='rviz2',
name='rviz_ocs2',
output='screen',
arguments=["-d", rviz_config_file]
),
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
output='screen'
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
'launch',
'gz_sim.launch.py'])]),
launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
robot_state_publisher,
gz_spawn_entity,
leg_pd_controller,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=leg_pd_controller,
on_exit=[controller, imu_sensor_broadcaster, joint_state_publisher],
)
),
])

106
descriptions/unitree/a1_description/launch/gazebo_unitree_guide.launch.py Normal file
View File

@ -0,0 +1,106 @@
import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.event_handlers import OnProcessExit
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
package_description = "a1_description"
def process_xacro():
pkg_path = os.path.join(get_package_share_directory(package_description))
xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
robot_description_config = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true'})
return robot_description_config.toxml()
def generate_launch_description():
rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
robot_description = process_xacro()
gz_spawn_entity = Node(
package='ros_gz_sim',
executable='create',
output='screen',
arguments=['-topic', 'robot_description', '-name',
'a1', '-allow_renaming', 'true', '-z', '0.4'],
)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[
{
'publish_frequency': 20.0,
'use_tf_static': True,
'robot_description': robot_description,
'ignore_timestamp': True
}
],
)
joint_state_publisher = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster",
"--controller-manager", "/controller_manager"],
)
imu_sensor_broadcaster = Node(
package="controller_manager",
executable="spawner",
arguments=["imu_sensor_broadcaster",
"--controller-manager", "/controller_manager"],
)
leg_pd_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["leg_pd_controller",
"--controller-manager", "/controller_manager"],
)
unitree_guide_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
)
return LaunchDescription([
Node(
package='rviz2',
executable='rviz2',
name='rviz_ocs2',
output='screen',
arguments=["-d", rviz_config_file]
),
Node(
package='ros_gz_bridge',
executable='parameter_bridge',
arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
output='screen'
),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
'launch',
'gz_sim.launch.py'])]),
launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
robot_state_publisher,
gz_spawn_entity,
leg_pd_controller,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=leg_pd_controller,
on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
)
),
])

427
descriptions/unitree/a1_description/xacro/gazebo.xacro
View File

@ -1,266 +1,195 @@
<?xml version="1.0"?>
<robot>
<!-- ros_control plugin -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/a1_gazebo</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
</plugin>
</gazebo>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 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>
<ros2_control name="GazeboSystem" type="system">
<hardware>
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
</hardware>
<!-- 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> -->
<joint name="FR_hip_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<gazebo>
<plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
<bodyName>trunk</bodyName>
<topicName>/apply_force/trunk</topicName>
</plugin>
</gazebo>
<joint name="FR_thigh_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<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>
<joint name="FR_calf_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<!-- 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>
<joint name="FL_hip_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<!-- 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>
<joint name="FL_thigh_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<gazebo reference="base">
<material>Gazebo/Green</material>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<joint name="FL_calf_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<gazebo reference="trunk">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<joint name="RR_hip_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<gazebo reference="stick_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/White</material>
</gazebo>
<joint name="RR_thigh_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<gazebo reference="imu_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Red</material>
</gazebo>
<joint name="RR_calf_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<!-- 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>
<joint name="RL_hip_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<!-- 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>
<joint name="RL_thigh_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<!-- 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>
<joint name="RL_calf_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<!-- 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>
<sensor name="imu_sensor">
<state_interface name="orientation.x"/>
<state_interface name="orientation.y"/>
<state_interface name="orientation.z"/>
<state_interface name="orientation.w"/>
<state_interface name="angular_velocity.x"/>
<state_interface name="angular_velocity.y"/>
<state_interface name="angular_velocity.z"/>
<state_interface name="linear_acceleration.x"/>
<state_interface name="linear_acceleration.y"/>
<state_interface name="linear_acceleration.z"/>
</sensor>
</ros2_control>
<gazebo>
<plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
<parameters>$(find a1_description)/config/gazebo.yaml</parameters>
</plugin>
<plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu">
</plugin>
</gazebo>
<gazebo reference="trunk">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<visual>
<material>
<ambient>.05 .05 .05 1.0</ambient>
<diffuse>.05 .05 .05 1.0</diffuse>
<specular>.05 .05 .05 1.0</specular>
</material>
</visual>
</gazebo>
<gazebo reference="imu_link">
<sensor name="imu_sensor" type="imu">
<always_on>1</always_on>
<update_rate>500</update_rate>
<visualize>true</visualize>
<topic>imu</topic>
<imu>
<angular_velocity>
<x>
<!-- <noise type="gaussian">-->
<!-- <mean>0.0</mean>-->
<!-- <stddev>2e-4</stddev>-->
<!-- <bias_mean>0.0000075</bias_mean>-->
<!-- <bias_stddev>0.0000008</bias_stddev>-->
<!-- </noise>-->
</x>
<y>
<!-- <noise type="gaussian">-->
<!-- <mean>0.0</mean>-->
<!-- <stddev>2e-4</stddev>-->
<!-- <bias_mean>0.0000075</bias_mean>-->
<!-- <bias_stddev>0.0000008</bias_stddev>-->
<!-- </noise>-->
</y>
<z>
<!-- <noise type="gaussian">-->
<!-- <mean>0.0</mean>-->
<!-- <stddev>2e-4</stddev>-->
<!-- <bias_mean>0.0000075</bias_mean>-->
<!-- <bias_stddev>0.0000008</bias_stddev>-->
<!-- </noise>-->
</z>
</angular_velocity>
<linear_acceleration>
<x>
<!-- <noise type="gaussian">-->
<!-- <mean>0.0</mean>-->
<!-- <stddev>1.7e-2</stddev>-->
<!-- <bias_mean>0.1</bias_mean>-->
<!-- <bias_stddev>0.001</bias_stddev>-->
<!-- </noise>-->
</x>
<y>
<!-- <noise type="gaussian">-->
<!-- <mean>0.0</mean>-->
<!-- <stddev>1.7e-2</stddev>-->
<!-- <bias_mean>0.1</bias_mean>-->
<!-- <bias_stddev>0.001</bias_stddev>-->
<!-- </noise>-->
</y>
<z>
<!-- <noise type="gaussian">-->
<!-- <mean>0.0</mean>-->
<!-- <stddev>1.7e-2</stddev>-->
<!-- <bias_mean>0.1</bias_mean>-->
<!-- <bias_stddev>0.001</bias_stddev>-->
<!-- </noise>-->
</z>
</linear_acceleration>
</imu>
</sensor>
</gazebo>
<gazebo reference="imu_joint">
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
</robot>

66
descriptions/unitree/a1_description/xacro/leg.xacro
View File

@ -35,9 +35,8 @@
<origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<geometry>
<mesh filename="package://a1_description/meshes/hip.dae" scale="1 1 1"/>
<mesh filename="file://$(find a1_description)/meshes/hip.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
@ -85,13 +84,12 @@
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<xacro:if value="${mirror_dae == True}">
<mesh filename="package://a1_description/meshes/thigh.dae" scale="1 1 1"/>
<mesh filename="file://$(find a1_description)/meshes/thigh.dae" scale="1 1 1"/>
</xacro:if>
<xacro:if value="${mirror_dae == False}">
<mesh filename="package://a1_description/meshes/thigh_mirror.dae" scale="1 1 1"/>
<mesh filename="file://$(find a1_description)/meshes/thigh_mirror.dae" scale="1 1 1"/>
</xacro:if>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
@ -122,9 +120,8 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://a1_description/meshes/calf.dae" scale="1 1 1"/>
<mesh filename="file://$(find a1_description)/meshes/calf.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
@ -154,7 +151,6 @@
<geometry>
<sphere radius="${foot_radius-0.01}"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
@ -171,6 +167,60 @@
</inertial>
</link>
<gazebo reference="${name}_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<visual>
<material>
<ambient>.5 .5 .5 1.0</ambient>
<diffuse>.5 .5 .5 1.0</diffuse>
<specular>.5 .5 .5 1.0</specular>
</material>
</visual>
</gazebo>
<gazebo reference="${name}_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>0</self_collide>
<visual>
<material>
<ambient>.175 .175 .175 1.0</ambient>
<diffuse>.175 .175 .175 1.0</diffuse>
<specular>.175 .175 .175 1.0</specular>
</material>
</visual>
<kp value="1000000.0"/>
<kd value="100.0"/>
</gazebo>
<gazebo reference="${name}_calf">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<visual>
<material>
<ambient>.5 .5 .5 1.0</ambient>
<diffuse>.5 .5 .5 1.0</diffuse>
<specular>.5 .5 .5 1.0</specular>
</material>
</visual>
</gazebo>
<gazebo reference="${name}_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<visual>
<material>
<ambient>.175 .175 .175 1.0</ambient>
<diffuse>.175 .175 .175 1.0</diffuse>
<specular>.175 .175 .175 1.0</specular>
</material>
</visual>
<kp value="1000000.0"/>
<kd value="100.0"/>
</gazebo>
<xacro:leg_transmission name="${name}"/>
</xacro:macro>
</robot>

11
descriptions/unitree/a1_description/xacro/robot.xacro
View File

@ -3,12 +3,19 @@
<robot name="a1" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:arg name="DEBUG" default="false"/>
<xacro:arg name="GAZEBO" default="false"/>
<xacro:include filename="$(find a1_description)/xacro/const.xacro"/>
<xacro:include filename="$(find a1_description)/xacro/materials.xacro"/>
<xacro:include filename="$(find a1_description)/xacro/leg.xacro"/>
<xacro:include filename="$(find a1_description)/xacro/stairs.xacro"/>
<xacro:include filename="$(find a1_description)/xacro/ros2_control.xacro"/>
<xacro:if value="$(arg GAZEBO)">
<xacro:include filename="$(find a1_description)/xacro/gazebo.xacro"/>
</xacro:if>
<xacro:unless value="$(arg GAZEBO)">
<xacro:include filename="$(find a1_description)/xacro/ros2_control.xacro"/>
</xacro:unless>
<!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->
@ -44,7 +51,7 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://a1_description/meshes/trunk.dae" scale="1 1 1"/>
<mesh filename="file://$(find a1_description)/meshes/trunk.dae" scale="1 1 1"/>
</geometry>
<material name="orange"/>
</visual>