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

fix: code format

This commit is contained in:
fan-ziqi 2024-10-12 22:03:26 +08:00
parent bd2975223f
commit 4e4157d2b3
10 changed files with 295 additions and 389 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
src/rl_sar/include/rl_real_go2.hpp
View File

@ -4,45 +4,34 @@
#include "rl_sdk.hpp"
#include "observation_buffer.hpp"
#include "loop.hpp"
#include <unitree/robot/channel/channel_publisher.hpp>
#include <unitree/robot/channel/channel_publisher.hpp> // TODO go2的sdk没有传上来
#include <unitree/robot/channel/channel_subscriber.hpp>
#include <unitree/idl/go2/LowState_.hpp>
#include <unitree/idl/go2/LowCmd_.hpp>
#include <unitree/common/time/time_tool.hpp>
#include <unitree/common/thread/thread.hpp>
#include <unitree/robot/go2/robot_state/robot_state_client.hpp>
#include "unitree_joystick.h" // TODO 这里调用的是a1的，go2有自己的键盘接口吗
#include <csignal>
#include "matplotlibcpp.h"
#include "unitree_joystick.h"
namespace plt = matplotlibcpp;
using namespace unitree::common;
using namespace unitree::robot;
using namespace unitree::robot::go2;
#define TOPIC_LOWCMD "rt/lowcmd"
#define TOPIC_LOWSTATE "rt/lowstate"
constexpr double PosStopF = (2.146E+9f);
constexpr double VelStopF = (16000.0f);
class RL_Real : public RL
{
public:
RL_Real();
~RL_Real();
void Init();
void InitRobotStateClient();
int queryServiceStatus(const std::string& serviceName);
void activateService(const std::string& serviceName,int activate);
private:
void LowCmdwriteHandler();
uint32_t crc32_core(uint32_t* ptr, uint32_t len);
void InitLowCmd();
void LowStateMessageHandler(const void* messages);
// rl functions
torch::Tensor Forward() override;
void GetState(RobotState<double> *state) override;
@ -57,8 +46,6 @@ private:
// loop
std::shared_ptr<LoopFunc> loop_keyboard;
std::shared_ptr<LoopFunc> loop_control;
std::shared_ptr<LoopFunc> loop_udpSend;
std::shared_ptr<LoopFunc> loop_udpRecv;
std::shared_ptr<LoopFunc> loop_rl;
std::shared_ptr<LoopFunc> loop_plot;
@ -66,26 +53,23 @@ private:
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();
// unitree interface
void InitRobotStateClient();
int QueryServiceStatus(const std::string &serviceName);
void ActivateService(const std::string &serviceName, int activate);
void LowCmdwriteHandler();
uint32_t Crc32Core(uint32_t *ptr, uint32_t len);
void InitLowCmd();
void LowStateMessageHandler(const void *messages);
RobotStateClient rsc;
unitree_go::msg::dds_::LowCmd_ unitree_low_command{}; // default init
unitree_go::msg::dds_::LowState_ unitree_low_state{}; // default init
/*publisher*/
ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher;
/*subscriber*/
ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber;
unitree_go::msg::dds_::LowCmd_ unitree_low_command{}; // default init
unitree_go::msg::dds_::LowState_ unitree_low_state{}; // default init
ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher; // publisher
ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber; //subscriber
xRockerBtnDataStruct unitree_joy;
// others
int motiontime = 0;
std::vector<double> mapped_joint_positions;
@ -94,4 +78,4 @@ private:
int state_mapping[12] = {3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8};
};
#endif
#endif // RL_REAL_HPP

41
src/rl_sar/include/unitree_joystick.h
View File

@ -1,41 +0,0 @@
#ifndef UNITREE_JOYSTICK_H
#define UNITREE_JOYSTICK_H
#include <stdint.h>
// 16b
typedef union {
struct {
uint8_t R1 :1;
uint8_t L1 :1;
uint8_t start :1;
uint8_t select :1;
uint8_t R2 :1;
uint8_t L2 :1;
uint8_t F1 :1;
uint8_t F2 :1;
uint8_t A :1;
uint8_t B :1;
uint8_t X :1;
uint8_t Y :1;
uint8_t up :1;
uint8_t right :1;
uint8_t down :1;
uint8_t left :1;
} components;
uint16_t value;
} xKeySwitchUnion;
// 40 Byte (now used 24B)
typedef struct {
uint8_t head[2];
xKeySwitchUnion btn;
float lx;
float rx;
float ry;
float L2;
float ly;
uint8_t idle[16];
} xRockerBtnDataStruct;
#endif // UNITREE_JOYSTICK_H

4
src/rl_sar/launch/gazebo_go2_isaacgym.launch
View File

@ -14,7 +14,7 @@
<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)"/>
@ -26,7 +26,7 @@
<!-- Load the URDF into the ROS Parameter Server -->
<param name="robot_description"
command="$(find xacro)/xacro --inorder '$(arg dollar)$(arg robot_path)/xacro/robot.xacro'
command="$(find xacro)/xacro --inorder '$(arg dollar)$(arg robot_path)/xacro/robot.xacro'
DEBUG:=$(arg user_debug)"/>
<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->

68
src/rl_sar/models/go2_isaacgym/config.yaml
View File

@ -1,5 +1,5 @@
go2_isaacgym:
model_name: "himloco_3.pt"
model_name: "himloco.pt"
framework: "isaacgym"
rows: 4
cols: 3
@ -7,32 +7,32 @@ go2_isaacgym:
dt: 0.005
decimation: 4
num_observations: 45
observations: ["commands","ang_vel", "gravity_vec", "dof_pos", "dof_vel", "actions"]
observations: ["commands", "ang_vel", "gravity_vec", "dof_pos", "dof_vel", "actions"]
clip_obs: 100.0
clip_actions_lower: [-100, -100, -100,
-100, -100, -100,
-100, -100, -100,
-100, -100, -100]
clip_actions_upper: [100, 100, 100,
100, 100, 100,
100, 100, 100,
100, 100, 100]
rl_kp: [30, 30, 30,
30, 30, 30,
30, 30, 30,
30, 30, 30]
rl_kd: [0.75, 0.75, 0.75,
0.75, 0.75, 0.75,
0.75, 0.75, 0.75,
0.75, 0.75, 0.75]
fixed_kp: [60, 60, 60,
60, 60, 60,
60, 60, 60,
60, 60, 60]
fixed_kd: [5, 5, 5,
5, 5, 5,
5, 5, 5,
5, 5, 5]
clip_actions_lower: [-100, -100, -100,
-100, -100, -100,
-100, -100, -100,
-100, -100, -100]
clip_actions_upper: [100, 100, 100,
100, 100, 100,
100, 100, 100,
100, 100, 100]
rl_kp: [30, 30, 30,
30, 30, 30,
30, 30, 30,
30, 30, 30]
rl_kd: [0.75, 0.75, 0.75,
0.75, 0.75, 0.75,
0.75, 0.75, 0.75,
0.75, 0.75, 0.75]
fixed_kp: [60, 60, 60,
60, 60, 60,
60, 60, 60,
60, 60, 60]
fixed_kd: [5, 5, 5,
5, 5, 5,
5, 5, 5,
5, 5, 5]
hip_scale_reduction: 1.0
hip_scale_reduction_indices: [0, 3, 6, 9]
num_of_dofs: 12
@ -42,14 +42,14 @@ go2_isaacgym:
dof_pos_scale: 1.0
dof_vel_scale: 0.05
commands_scale: [2.0, 2.0, 0.25]
torque_limits: [33.5, 33.5, 33.5,
33.5, 33.5, 33.5,
33.5, 33.5, 33.5,
33.5, 33.5, 33.5]
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]
torque_limits: [33.5, 33.5, 33.5,
33.5, 33.5, 33.5,
33.5, 33.5, 33.5,
33.5, 33.5, 33.5]
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",

BIN
src/rl_sar/models/go2_isaacgym/himloco.pt
View File

Binary file not shown.

BIN
src/rl_sar/models/go2_isaacgym/himloco_2.pt
View File

Binary file not shown.

BIN
src/rl_sar/models/go2_isaacgym/himloco_3.pt
View File

Binary file not shown.

523
src/rl_sar/src/rl_real_go2.cpp
View File

@ -1,11 +1,229 @@
#include "../include/rl_real_go2.hpp"
#include "rl_real_go2.hpp"
// #define PLOT
// #define CSV_LOGGER
RL_Real rl_sar;
void RL_Real::RL_Real()
{
// read params from yaml
this->robot_name = "go2_isaacgym";
this->ReadYaml(this->robot_name);
uint32_t RL_Real::crc32_core(uint32_t* ptr, uint32_t len)
// history
this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6);
// init robot
this->InitRobotStateClient();
while (rl_sar.QueryServiceStatus("sport_mode"))
{
std::cout << "Try to deactivate the service: " << "sport_mode" << std::endl;
rl_sar.ActivateService("sport_mode", 0);
sleep(1);
}
this->InitLowCmd();
// create publisher
lowcmd_publisher.reset(new ChannelPublisher<unitree_go::msg::dds_::LowCmd_>(TOPIC_LOWCMD));
lowcmd_publisher->InitChannel();
// create subscriber
lowstate_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::LowState_>(TOPIC_LOWSTATE));
lowstate_subscriber->InitChannel(std::bind(&RL_Real::LowStateMessageHandler, this, std::placeholders::_1), 1);
// loop publishing thread TODO why?
// lowCmdWriteThreadPtr = CreateRecurrentThreadEx("writebasiccmd", UT_CPU_ID_NONE, 2000, &RL_Real::LowCmdwriteHandler, this);
// init rl
torch::autograd::GradMode::set_enabled(false);
this->InitObservations();
this->InitOutputs();
this->InitControl();
running_state = STATE_WAITING;
// model
std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + this->robot_name + "/" + this->params.model_name;
this->model = torch::jit::load(model_path);
// loop
this->loop_keyboard = std::make_shared<LoopFunc>("loop_keyboard", 0.05, std::bind(&RL_Real::KeyboardInterface, this));
this->loop_control = std::make_shared<LoopFunc>("loop_control", this->params.dt, std::bind(&RL_Real::RobotControl, this));
this->loop_rl = std::make_shared<LoopFunc>("loop_rl", this->params.dt * this->params.decimation, std::bind(&RL_Real::RunModel, this));
this->loop_keyboard->start();
this->loop_control->start();
this->loop_rl->start();
#ifdef PLOT
this->plot_t = std::vector<int>(this->plot_size, 0);
this->plot_real_joint_pos.resize(this->params.num_of_dofs);
this->plot_target_joint_pos.resize(this->params.num_of_dofs);
for (auto &vector : this->plot_real_joint_pos) { vector = std::vector<double>(this->plot_size, 0); }
for (auto &vector : this->plot_target_joint_pos) { vector = std::vector<double>(this->plot_size, 0); }
this->loop_plot = std::make_shared<LoopFunc>("loop_plot", 0.002, std::bind(&RL_Real::Plot, this));
this->loop_plot->start();
#endif
#ifdef CSV_LOGGER
this->CSVInit(this->robot_name);
#endif
}
RL_Real::~RL_Real()
{
this->loop_keyboard->shutdown();
this->loop_control->shutdown();
this->loop_rl->shutdown();
#ifdef PLOT
this->loop_plot->shutdown();
#endif
std::cout << LOGGER::INFO << "RL_Real exit" << std::endl;
}
void RL_Real::GetState(RobotState<double> *state)
{
// TODO 加锁
memcpy(&this->unitree_joy, &this->unitree_low_state.wireless_remote()[0], 40);
if ((int)this->unitree_joy.btn.components.R2 == 1)
{
this->control.control_state = STATE_POS_GETUP;
}
else if ((int)this->unitree_joy.btn.components.R1 == 1)
{
this->control.control_state = STATE_RL_INIT;
}
else if ((int)this->unitree_joy.btn.components.L2 == 1)
{
this->control.control_state = STATE_POS_GETDOWN;
}
if (this->params.framework == "isaacgym")
{
state->imu.quaternion[3] = this->unitree_low_state.imu_state().quaternion()[0]; // w
state->imu.quaternion[0] = this->unitree_low_state.imu_state().quaternion()[1]; // x
state->imu.quaternion[1] = this->unitree_low_state.imu_state().quaternion()[2]; // y
state->imu.quaternion[2] = this->unitree_low_state.imu_state().quaternion()[3]; // z
}
else if (this->params.framework == "isaacsim")
{
state->imu.quaternion[0] = this->unitree_low_state.imu_state().quaternion()[0]; // w
state->imu.quaternion[1] = this->unitree_low_state.imu_state().quaternion()[1]; // x
state->imu.quaternion[2] = this->unitree_low_state.imu_state().quaternion()[2]; // y
state->imu.quaternion[3] = this->unitree_low_state.imu_state().quaternion()[3]; // z
}
for (int i = 0; i < 3; ++i)
{
state->imu.gyroscope[i] = this->unitree_low_state.imu_state().gyroscope()[i];
}
for (int i = 0; i < this->params.num_of_dofs; ++i)
{
state->motor_state.q[i] = this->unitree_low_state.motor_state()[state_mapping[i]].q();
state->motor_state.dq[i] = this->unitree_low_state.motor_state()[state_mapping[i]].dq();
state->motor_state.tauEst[i] = this->unitree_low_state.motor_state()[state_mapping[i]].tau_est();
}
}
void RL_Real::SetCommand(const RobotCommand<double> *command)
{
for (int i = 0; i < this->params.num_of_dofs; ++i)
{
this->unitree_low_command.motor_cmd()[i].mode() = 0x01;
this->unitree_low_command.motor_cmd()[i].q() = command->motor_command.q[command_mapping[i]];
this->unitree_low_command.motor_cmd()[i].dq() = command->motor_command.dq[command_mapping[i]];
this->unitree_low_command.motor_cmd()[i].kp() = command->motor_command.kp[command_mapping[i]];
this->unitree_low_command.motor_cmd()[i].kd() = command->motor_command.kd[command_mapping[i]];
this->unitree_low_command.motor_cmd()[i].tau() = command->motor_command.tau[command_mapping[i]];
}
// 暂时不发 TODO Why?
this->unitree_low_command.crc() = Crc32Core((uint32_t *)&unitree_low_command, (sizeof(unitree_go::msg::dds_::LowCmd_) >> 2) - 1);
lowcmd_publisher->Write(unitree_low_command);
}
void RL_Real::RobotControl()
{
this->motiontime++;
this->GetState(&this->robot_state);
this->StateController(&this->robot_state, &this->robot_command);
this->SetCommand(&this->robot_command);
}
void RL_Real::RunModel()
{
if (this->running_state == STATE_RL_RUNNING)
{
this->obs.ang_vel = torch::tensor(this->robot_state.imu.gyroscope).unsqueeze(0);
// this->obs.commands = torch::tensor({{this->unitree_joy.ly, -this->unitree_joy.rx, -this->unitree_joy.lx}});
this->obs.commands = torch::tensor({{this->control.x, this->control.y, this->control.yaw}});
this->obs.base_quat = torch::tensor(this->robot_state.imu.quaternion).unsqueeze(0);
this->obs.dof_pos = torch::tensor(this->robot_state.motor_state.q).narrow(0, 0, this->params.num_of_dofs).unsqueeze(0);
this->obs.dof_vel = torch::tensor(this->robot_state.motor_state.dq).narrow(0, 0, this->params.num_of_dofs).unsqueeze(0);
torch::Tensor clamped_actions = this->Forward();
for (int i : this->params.hip_scale_reduction_indices)
{
clamped_actions[0][i] *= this->params.hip_scale_reduction;
}
this->obs.actions = clamped_actions;
torch::Tensor origin_output_torques = this->ComputeTorques(this->obs.actions);
this->TorqueProtect(origin_output_torques);
this->output_torques = torch::clamp(origin_output_torques, -(this->params.torque_limits), this->params.torque_limits);
this->output_dof_pos = this->ComputePosition(this->obs.actions);
#ifdef CSV_LOGGER
torch::Tensor tau_est = torch::tensor(this->robot_state.motor_state.tauEst).unsqueeze(0);
this->CSVLogger(this->output_torques, tau_est, this->obs.dof_pos, this->output_dof_pos, this->obs.dof_vel);
#endif
}
}
torch::Tensor RL_Real::Forward()
{
torch::autograd::GradMode::set_enabled(false);
torch::Tensor clamped_obs = this->ComputeObservation();
this->history_obs_buf.insert(clamped_obs);
this->history_obs = this->history_obs_buf.get_obs_vec({5, 4, 3, 2, 1, 0});
torch::Tensor actions = this->model.forward({this->history_obs}).toTensor();
if (this->params.clip_actions_upper.numel() != 0 && this->params.clip_actions_lower.numel() != 0)
{
return torch::clamp(actions, this->params.clip_actions_lower, this->params.clip_actions_upper);
}
else
{
return actions;
}
}
void RL_Real::Plot()
{
this->plot_t.erase(this->plot_t.begin());
this->plot_t.push_back(this->motiontime);
plt::cla();
plt::clf();
for (int i = 0; i < this->params.num_of_dofs; ++i)
{
this->plot_real_joint_pos[i].erase(this->plot_real_joint_pos[i].begin());
this->plot_target_joint_pos[i].erase(this->plot_target_joint_pos[i].begin());
this->plot_real_joint_pos[i].push_back(this->unitree_low_state.motor_state()[i].q());
this->plot_target_joint_pos[i].push_back(this->unitree_low_command.motor_cmd()[i].q());
plt::subplot(4, 3, i + 1);
plt::named_plot("_real_joint_pos", this->plot_t, this->plot_real_joint_pos[i], "r");
plt::named_plot("_target_joint_pos", this->plot_t, this->plot_target_joint_pos[i], "b");
plt::xlim(this->plot_t.front(), this->plot_t.back());
}
// plt::legend();
plt::pause(0.0001);
}
uint32_t RL_Real::Crc32Core(uint32_t *ptr, uint32_t len)
{
unsigned int xbit = 0;
unsigned int data = 0;
@ -37,69 +255,6 @@ uint32_t RL_Real::crc32_core(uint32_t* ptr, uint32_t len)
return CRC32;
}
void RL_Real::Init()
{
InitLowCmd();
/*create publisher*/
lowcmd_publisher.reset(new ChannelPublisher<unitree_go::msg::dds_::LowCmd_>(TOPIC_LOWCMD));
lowcmd_publisher->InitChannel();
/*create subscriber*/
lowstate_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::LowState_>(TOPIC_LOWSTATE));
lowstate_subscriber->InitChannel(std::bind(&RL_Real::LowStateMessageHandler, this, std::placeholders::_1), 1);
/*loop publishing thread*/
//lowCmdWriteThreadPtr = CreateRecurrentThreadEx("writebasiccmd", UT_CPU_ID_NONE, 2000, &RL_Real::LowCmdwriteHandler, this);
// read params from yaml
this->robot_name = "go2_isaacgym";
this->ReadYaml(this->robot_name);
// history
this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, 6);
// init
torch::autograd::GradMode::set_enabled(false);
this->InitObservations();
this->InitOutputs();
this->InitControl();
// model
std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + this->robot_name + "/" + this->params.model_name;
this->model = torch::jit::load(model_path);
// loop
// this->loop_udpSend = std::make_shared<LoopFunc>("loop_udpSend" , 0.002, std::bind(&RL_Real::UDPSend, this), 3);
// this->loop_udpRecv = std::make_shared<LoopFunc>("loop_udpRecv" , 0.002, std::bind(&RL_Real::UDPRecv, this), 3);
this->loop_keyboard = std::make_shared<LoopFunc>("loop_keyboard", 0.05, std::bind(&RL_Real::KeyboardInterface, this));
this->loop_control = std::make_shared<LoopFunc>("loop_control", this->params.dt, std::bind(&RL_Real::RobotControl, this));
this->loop_rl = std::make_shared<LoopFunc>("loop_rl", this->params.dt * this->params.decimation, std::bind(&RL_Real::RunModel, this));
// this->loop_udpSend->start();
// this->loop_udpRecv->start();
this->loop_keyboard->start();
this->loop_control->start();
this->loop_rl->start();
#ifdef PLOT
this->plot_t = std::vector<int>(this->plot_size, 0);
this->plot_real_joint_pos.resize(this->params.num_of_dofs);
this->plot_target_joint_pos.resize(this->params.num_of_dofs);
for(auto& vector : this->plot_real_joint_pos) { vector = std::vector<double>(this->plot_size, 0); }
for(auto& vector : this->plot_target_joint_pos) { vector = std::vector<double>(this->plot_size, 0); }
this->loop_plot = std::make_shared<LoopFunc>("loop_plot" , 0.002, std::bind(&RL_Real::Plot, this));
this->loop_plot->start();
#endif
#ifdef CSV_LOGGER
this->CSVInit(this->robot_name);
#endif
}
void RL_Real::InitLowCmd()
{
unitree_low_command.head()[0] = 0xFE;
@ -107,9 +262,9 @@ void RL_Real::InitLowCmd()
unitree_low_command.level_flag() = 0xFF;
unitree_low_command.gpio() = 0;
for(int i=0; i<20; i++)
for (int i = 0; i < 20; i++)
{
unitree_low_command.motor_cmd()[i].mode() = (0x01); // motor switch to servo (PMSM) mode
unitree_low_command.motor_cmd()[i].mode() = (0x01); // motor switch to servo (PMSM) mode
unitree_low_command.motor_cmd()[i].q() = (PosStopF);
unitree_low_command.motor_cmd()[i].kp() = (0);
unitree_low_command.motor_cmd()[i].dq() = (VelStopF);
@ -120,223 +275,52 @@ void RL_Real::InitLowCmd()
void RL_Real::InitRobotStateClient()
{
rsc.SetTimeout(10.0f);
rsc.SetTimeout(10.0f);
rsc.Init();
}
int RL_Real::queryServiceStatus(const std::string& serviceName)
int RL_Real::QueryServiceStatus(const std::string &serviceName)
{
std::vector<ServiceState> serviceStateList;
int ret,serviceStatus;
int ret, serviceStatus;
ret = rsc.ServiceList(serviceStateList);
size_t i, count=serviceStateList.size();
for (i=0; i<count; i++)
size_t i, count = serviceStateList.size();
for (i = 0; i < count; i++)
{
const ServiceState& serviceState = serviceStateList[i];
if(serviceState.name == serviceName)
const ServiceState &serviceState = serviceStateList[i];
if (serviceState.name == serviceName)
{
if(serviceState.status == 0)
if (serviceState.status == 0)
{
std::cout << "name: " << serviceState.name <<" is activate"<<std::endl;
std::cout << "name: " << serviceState.name << " is activate" << std::endl;
serviceStatus = 1;
}
else
{
std::cout << "name:" << serviceState.name <<" is deactivate"<<std::endl;
std::cout << "name:" << serviceState.name << " is deactivate" << std::endl;
serviceStatus = 0;
}
}
}
}
}
return serviceStatus;
}
void RL_Real::activateService(const std::string& serviceName,int activate)
void RL_Real::ActivateService(const std::string &serviceName, int activate)
{
rsc.ServiceSwitch(serviceName, activate);
rsc.ServiceSwitch(serviceName, activate);
}
void RL_Real::LowStateMessageHandler(const void* message)
void RL_Real::LowStateMessageHandler(const void *message)
{
unitree_low_state = *(unitree_go::msg::dds_::LowState_*)message;
unitree_low_state = *(unitree_go::msg::dds_::LowState_ *)message;
}
void RL_Real::LowCmdwriteHandler(){
// this->unitree_low_command.crc() = crc32_core((uint32_t *)&unitree_low_command, (sizeof(unitree_go::msg::dds_::LowCmd_)>>2)-1);
void RL_Real::LowCmdwriteHandler()
{
// this->unitree_low_command.crc() = Crc32Core((uint32_t *)&unitree_low_command, (sizeof(unitree_go::msg::dds_::LowCmd_)>>2)-1);
// this->lowcmd_publisher->Write(unitree_low_command);
}
RL_Real::RL_Real(){
}
RL_Real::~RL_Real()
{
// this->loop_udpSend->shutdown();
// this->loop_udpRecv->shutdown();
this->loop_keyboard->shutdown();
this->loop_control->shutdown();
this->loop_rl->shutdown();
#ifdef PLOT
this->loop_plot->shutdown();
#endif
std::cout << LOGGER::INFO << "RL_Real exit" << std::endl;
}
void RL_Real::GetState(RobotState<double> *state)
{
//! 这里是不是要加锁？
memcpy(&this->unitree_joy, &this->unitree_low_state.wireless_remote()[0], 40);
if((int)this->unitree_joy.btn.components.R2 == 1)
{
this->control.control_state = STATE_POS_GETUP;
}
else if((int)this->unitree_joy.btn.components.R1 == 1)
{
this->control.control_state = STATE_RL_INIT;
}
else if((int)this->unitree_joy.btn.components.L2 == 1)
{
this->control.control_state = STATE_POS_GETDOWN;
}
if(this->params.framework == "isaacgym")
{
state->imu.quaternion[3] = this->unitree_low_state.imu_state().quaternion()[0]; // w
state->imu.quaternion[0] = this->unitree_low_state.imu_state().quaternion()[1]; // x
state->imu.quaternion[1] = this->unitree_low_state.imu_state().quaternion()[2]; // y
state->imu.quaternion[2] = this->unitree_low_state.imu_state().quaternion()[3]; // z
}
else if(this->params.framework == "isaacsim")
{
state->imu.quaternion[0] = this->unitree_low_state.imu_state().quaternion()[0]; // w
state->imu.quaternion[1] = this->unitree_low_state.imu_state().quaternion()[1]; // x
state->imu.quaternion[2] = this->unitree_low_state.imu_state().quaternion()[2]; // y
state->imu.quaternion[3] = this->unitree_low_state.imu_state().quaternion()[3]; // z
}
for(int i = 0; i < 3; ++i)
{
state->imu.gyroscope[i] = this->unitree_low_state.imu_state().gyroscope()[i];
}
for(int i = 0; i < this->params.num_of_dofs; ++i)
{
state->motor_state.q[i] = this->unitree_low_state.motor_state()[state_mapping[i]].q();
state->motor_state.dq[i] = this->unitree_low_state.motor_state()[state_mapping[i]].dq();
state->motor_state.tauEst[i] = this->unitree_low_state.motor_state()[state_mapping[i]].tau_est();
}
//std::cout<<"(int)this->unitree_joy.btn.components.R2: "<<(int)this->unitree_joy.btn.components.R2<<std::endl;
// std::cout<<"quat: "<< this->unitree_low_state.imu_state().quaternion()[0]<<", "<< this->unitree_low_state.imu_state().quaternion()[1]<<", "<< this->unitree_low_state.imu_state().quaternion()[2]<<", "<< this->unitree_low_state.imu_state().quaternion()[3]<<std::endl;
}
void RL_Real::SetCommand(const RobotCommand<double> *command)
{
for(int i = 0; i < this->params.num_of_dofs; ++i)
{
this->unitree_low_command.motor_cmd()[i].mode() = 0x01;
this->unitree_low_command.motor_cmd()[i].q() = command->motor_command.q[command_mapping[i]];
this->unitree_low_command.motor_cmd()[i].dq() = command->motor_command.dq[command_mapping[i]];
this->unitree_low_command.motor_cmd()[i].kp() = command->motor_command.kp[command_mapping[i]];
this->unitree_low_command.motor_cmd()[i].kd() = command->motor_command.kd[command_mapping[i]];
this->unitree_low_command.motor_cmd()[i].tau() = command->motor_command.tau[command_mapping[i]];
//std::cout<<"q: "<<command->motor_command.q[command_mapping[i]]<<std::endl;
}
//暂时不发
this->unitree_low_command.crc() = crc32_core((uint32_t *)&unitree_low_command, (sizeof(unitree_go::msg::dds_::LowCmd_)>>2)-1);
lowcmd_publisher->Write(unitree_low_command);
}
void RL_Real::RobotControl()
{
this->motiontime++;
this->GetState(&this->robot_state);
this->StateController(&this->robot_state, &this->robot_command);
this->SetCommand(&this->robot_command);
}
void RL_Real::RunModel()
{
if(this->running_state == STATE_RL_RUNNING)
{
this->obs.ang_vel = torch::tensor(this->robot_state.imu.gyroscope).unsqueeze(0);
// this->obs.commands = torch::tensor({{this->unitree_joy.ly, -this->unitree_joy.rx, -this->unitree_joy.lx}});
this->obs.commands = torch::tensor({{this->control.x , this->control.y, this->control.yaw}});
this->obs.base_quat = torch::tensor(this->robot_state.imu.quaternion).unsqueeze(0);
this->obs.dof_pos = torch::tensor(this->robot_state.motor_state.q).narrow(0, 0, this->params.num_of_dofs).unsqueeze(0);
this->obs.dof_vel = torch::tensor(this->robot_state.motor_state.dq).narrow(0, 0, this->params.num_of_dofs).unsqueeze(0);
torch::Tensor clamped_actions = this->Forward();
for (int i : this->params.hip_scale_reduction_indices)
{
clamped_actions[0][i] *= this->params.hip_scale_reduction;
}
this->obs.actions = clamped_actions;
torch::Tensor origin_output_torques = this->ComputeTorques(this->obs.actions);
this->TorqueProtect(origin_output_torques);
this->output_torques = torch::clamp(origin_output_torques, -(this->params.torque_limits), this->params.torque_limits);
this->output_dof_pos = this->ComputePosition(this->obs.actions);
#ifdef CSV_LOGGER
torch::Tensor tau_est = torch::tensor(this->robot_state.motor_state.tauEst).unsqueeze(0);
this->CSVLogger(this->output_torques, tau_est, this->obs.dof_pos, this->output_dof_pos, this->obs.dof_vel);
#endif
}
}
torch::Tensor RL_Real::Forward()
{
torch::autograd::GradMode::set_enabled(false);
torch::Tensor clamped_obs = this->ComputeObservation();
this->history_obs_buf.insert(clamped_obs);
this->history_obs = this->history_obs_buf.get_obs_vec({5, 4, 3, 2, 1, 0});
torch::Tensor actions = this->model.forward({this->history_obs}).toTensor();
torch::Tensor clamped_actions = torch::clamp(actions, this->params.clip_actions_lower, this->params.clip_actions_upper);
return clamped_actions;
}
void RL_Real::Plot()
{
this->plot_t.erase(this->plot_t.begin());
this->plot_t.push_back(this->motiontime);
plt::cla();
plt::clf();
for(int i = 0; i < this->params.num_of_dofs; ++i)
{
this->plot_real_joint_pos[i].erase(this->plot_real_joint_pos[i].begin());
this->plot_target_joint_pos[i].erase(this->plot_target_joint_pos[i].begin());
this->plot_real_joint_pos[i].push_back(this->unitree_low_state.motor_state()[i].q());
this->plot_target_joint_pos[i].push_back(this->unitree_low_command.motor_cmd()[i].q());
plt::subplot(4, 3, i + 1);
plt::named_plot("_real_joint_pos", this->plot_t, this->plot_real_joint_pos[i], "r");
plt::named_plot("_target_joint_pos", this->plot_t, this->plot_target_joint_pos[i], "b");
plt::xlim(this->plot_t.front(), this->plot_t.back());
}
// plt::legend();
plt::pause(0.0001);
}
void signalHandler(int signum)
{
exit(0);
@ -344,30 +328,9 @@ void signalHandler(int signum)
int main(int argc, char **argv)
{
signal(SIGINT, signalHandler);
if (argc < 2)
{
std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
exit(-1);
}
std::cout << "WARNING: Make sure the robot is hung up or lying on the ground." << std::endl
<< "Press Enter to continue..." << std::endl;
std::cin.ignore();
ChannelFactory::Instance()->Init(0, argv[1]);
RL_Real rl_sar;
rl_sar.InitRobotStateClient();
while(rl_sar.queryServiceStatus("sport_mode"))
{
std::cout<<"Try to deactivate the service: "<<"sport_mode"<<std::endl;
rl_sar.activateService("sport_mode",0);
sleep(1);
}
rl_sar.Init();
while(1)
while (1)
{
sleep(10);
};

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

@ -252,6 +252,7 @@ torch::Tensor RL_Sim::Forward()
if (this->params.use_history)
{
this->history_obs_buf.insert(clamped_obs);
// TODO 这里要找一种方法适配不同的顺序，不能直接改这里，会导致a1的模型不可用
this->history_obs = this->history_obs_buf.get_obs_vec({5, 4, 3, 2, 1, 0});
actions = this->model.forward({this->history_obs}).toTensor();
}

1
src/robots/go2_description/config/joint_names_go2_description.yaml
View File

@ -1 +0,0 @@
controller_joint_names: ['', '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', ]