initial commit
This commit is contained in:
huangweifeng
@@ -0,0 +1,311 @@
|
||||
/**
|
||||
* @file dds_hardware_interface.hpp
|
||||
* @brief hardware interface for dds
|
||||
* @author DeepRobotics
|
||||
* @version 1.0
|
||||
* @date 2025-11-07
|
||||
*
|
||||
* @copyright Copyright (c) 2025 DeepRobotics
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include "common_types.h"
|
||||
#include "robot_interface.h"
|
||||
#include "dds_types.h"
|
||||
|
||||
#include "drdds/msg/joints_data.hpp"
|
||||
#include "drdds/msg/joints_data_cmd.hpp"
|
||||
#include "drdds/msg/imu_data.hpp"
|
||||
#include "drdds/msg/battery_data.hpp"
|
||||
|
||||
using namespace dds;
|
||||
|
||||
struct JointConfig {
|
||||
float dir;
|
||||
float offset;
|
||||
};
|
||||
|
||||
|
||||
class DdsInterface : public RobotInterface{
|
||||
protected:
|
||||
double ri_ts_, imu_ts_;
|
||||
Vec3f omega_body_, rpy_, acc_, base_lin_vel_;
|
||||
VecXf joint_pos_, joint_vel_, joint_tau_;
|
||||
VecXf contact_force_;
|
||||
VecXf motor_temperture_, driver_temperture_;
|
||||
float *pos_offset_;
|
||||
float *joint_dir_;
|
||||
bool *data_updated_;
|
||||
uint16_t *control_word;
|
||||
|
||||
JointConfig *joint_config_;
|
||||
std::vector<uint16_t> driver_status_;
|
||||
std::vector<uint16_t> joint_data_id_;
|
||||
BatteryInfo_dds battery_info_[2];
|
||||
std::vector<uint16_t> battery_data_;
|
||||
|
||||
double time_stamp_;
|
||||
float remain_bat;
|
||||
|
||||
rclcpp::Publisher<drdds::msg::JointsDataCmd>::SharedPtr joint_cmd_pub_;
|
||||
rclcpp::Subscription<drdds::msg::JointsData>::SharedPtr joint_data_sub_;
|
||||
rclcpp::Subscription<drdds::msg::ImuData>::SharedPtr imu_data_sub_;
|
||||
rclcpp::Subscription<drdds::msg::BatteryData>::SharedPtr health_data_sub_;
|
||||
|
||||
|
||||
bool IsDataUpdatedFinished() {
|
||||
bool res = data_updated_[0];
|
||||
for (int i = 0; i < dof_num_; ++i) {
|
||||
res = res & data_updated_[i];
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
void ResetPositionOffset() {
|
||||
memset(data_updated_, 0, dof_num_ * sizeof(bool));
|
||||
this->SetJointCommand(MatXf::Zero(dof_num_, 5));
|
||||
usleep(100 * 1000);
|
||||
VecXf last_joint_pos = this->GetJointPosition();
|
||||
VecXf current_joint_pos = this->GetJointPosition();
|
||||
int cnt = 0;
|
||||
while (!IsDataUpdatedFinished()) {
|
||||
++cnt;
|
||||
this->RefreshRobotData();
|
||||
rclcpp::spin_some(this->get_node());
|
||||
current_joint_pos = this->GetJointPosition();
|
||||
|
||||
for (int i = 0; i < dof_num_; ++i) {
|
||||
if (!data_updated_[i] && current_joint_pos(i) != last_joint_pos(i)) {
|
||||
data_updated_[i] = true;
|
||||
}
|
||||
}
|
||||
last_joint_pos = current_joint_pos;
|
||||
usleep(1000);
|
||||
if (cnt == 10000) {
|
||||
std::cout << data_updated_ << std::endl;
|
||||
std::cout << "joint data update is not finished\n";
|
||||
}
|
||||
}
|
||||
for (int i = 1; i < dof_num_; i += 4) {
|
||||
if (current_joint_pos(i) < -210. / 180. * M_PI) {
|
||||
pos_offset_[i] = pos_offset_[i] + 360.;
|
||||
std::cout << "joint " << i << " offset is changed to " << pos_offset_[i] << "\n";
|
||||
} else if (current_joint_pos(i) > 90. / 180. * M_PI) {
|
||||
pos_offset_[i] = pos_offset_[i] - 360.;
|
||||
std::cout << "joint " << i << " offset is changed to " << pos_offset_[i] << "\n";
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < dof_num_; ++i) {
|
||||
joint_config_[i].dir = joint_dir_[i];
|
||||
joint_config_[i].offset = Deg2Rad(pos_offset_[i]);
|
||||
}
|
||||
}
|
||||
|
||||
public:
|
||||
int run_cnt_ = 0;
|
||||
|
||||
DdsInterface(const std::string &robot_name, const int &dof_num) : RobotInterface(robot_name, dof_num) {
|
||||
std::cout << robot_name << " is using DDS Interface" << std::endl;
|
||||
joint_pos_ = VecXf::Zero(dof_num_);
|
||||
joint_vel_ = VecXf::Zero(dof_num_);
|
||||
joint_tau_ = VecXf::Zero(dof_num_);
|
||||
motor_temperture_ = VecXf::Zero(dof_num_);
|
||||
driver_temperture_ = VecXf::Zero(dof_num_);
|
||||
omega_body_.setZero();
|
||||
rpy_.setZero();
|
||||
acc_.setZero();
|
||||
base_lin_vel_.setZero();
|
||||
contact_force_ = VecXf::Zero(9);
|
||||
ri_ts_ = 0.0;
|
||||
imu_ts_ = 0.0;
|
||||
time_stamp_ = 0.0;
|
||||
remain_bat = 0.0f;
|
||||
|
||||
pos_offset_ = new float[dof_num_];
|
||||
joint_dir_ = new float[dof_num_];
|
||||
data_updated_ = new bool[dof_num_];
|
||||
joint_config_ = new JointConfig[dof_num_];
|
||||
control_word = new uint16_t[4];
|
||||
|
||||
for (int i = 0; i < dof_num_; ++i) {
|
||||
pos_offset_[i] = 0.0f;
|
||||
joint_dir_[i] = 1.0f;
|
||||
data_updated_[i] = false;
|
||||
joint_config_[i].dir = 1.0f;
|
||||
joint_config_[i].offset = 0.0f;
|
||||
}
|
||||
|
||||
driver_status_.assign(dof_num_, 1);
|
||||
joint_data_id_.resize(dof_num_);
|
||||
battery_data_.resize(BATTERY_DATA_SIZE);
|
||||
|
||||
joint_cmd_pub_ = node_->create_publisher<drdds::msg::JointsDataCmd>("/JOINTS_CMD", 10);
|
||||
joint_data_sub_ = node_->create_subscription<drdds::msg::JointsData>("/JOINTS_DATA", 10,
|
||||
std::bind(&DdsInterface::Handler, this,std::placeholders::_1));
|
||||
imu_data_sub_ = node_->create_subscription<drdds::msg::ImuData>("/IMU_DATA", 10,
|
||||
std::bind(&DdsInterface::HandlerIMU, this, std::placeholders::_1));
|
||||
health_data_sub_ = node_->create_subscription<drdds::msg::BatteryData>("/BATTERY_DATA", 10,
|
||||
std::bind(&DdsInterface::HandlerHealth, this, std::placeholders::_1));
|
||||
|
||||
sleep(1);
|
||||
|
||||
control_word[0] = kIndexDisable;
|
||||
control_word[1] = kIndexErrorReset;
|
||||
control_word[2] = kIndexEnable;
|
||||
control_word[3] = kIndexGetStatusWord;
|
||||
ResetJointError();
|
||||
}
|
||||
|
||||
virtual ~DdsInterface() {
|
||||
delete[] pos_offset_;
|
||||
delete[] joint_dir_;
|
||||
delete[] data_updated_;
|
||||
delete[] joint_config_;
|
||||
}
|
||||
|
||||
virtual void Start() {
|
||||
}
|
||||
|
||||
virtual void Stop() {
|
||||
}
|
||||
|
||||
virtual double GetInterfaceTimeStamp() {
|
||||
return ri_ts_;
|
||||
}
|
||||
|
||||
virtual VecXf GetJointPosition() {
|
||||
return joint_pos_;
|
||||
}
|
||||
|
||||
virtual VecXf GetJointVelocity() {
|
||||
return joint_vel_;
|
||||
}
|
||||
|
||||
virtual VecXf GetJointTorque() {
|
||||
return joint_tau_;
|
||||
}
|
||||
|
||||
virtual Vec3f GetImuRpy() {
|
||||
return rpy_;
|
||||
}
|
||||
|
||||
virtual Vec3f GetImuAcc() {
|
||||
return acc_;
|
||||
}
|
||||
|
||||
virtual Vec3f GetImuOmega() {
|
||||
return omega_body_;
|
||||
}
|
||||
|
||||
virtual Vec3f GetBaseLinearVelocity() {
|
||||
return base_lin_vel_;
|
||||
}
|
||||
|
||||
virtual VecXf GetContactForce() {
|
||||
return contact_force_;
|
||||
}
|
||||
|
||||
virtual void ResetJointError() {
|
||||
auto msg = drdds::msg::JointsDataCmd();
|
||||
|
||||
for (int j = 0; j < 4; ++j) {
|
||||
for (int i = 0; i < dof_num_; ++i) {
|
||||
msg.data.joints_data[i].position = 0;
|
||||
msg.data.joints_data[i].velocity = 0;
|
||||
msg.data.joints_data[i].torque = 0;
|
||||
msg.data.joints_data[i].kp = 0;
|
||||
msg.data.joints_data[i].kd = 0;
|
||||
msg.data.joints_data[i].control_word = control_word[j];
|
||||
}
|
||||
std::cout << "ResetJointError publish!! \n" << std::endl;
|
||||
joint_cmd_pub_->publish(msg);
|
||||
usleep(10 * 1000);
|
||||
}
|
||||
}
|
||||
|
||||
virtual void SetJointCommand(Eigen::Matrix<float, Eigen::Dynamic, 5> input) {
|
||||
joint_cmd_ = input;
|
||||
auto msg = drdds::msg::JointsDataCmd();
|
||||
|
||||
for (int i = 0; i < dof_num_; ++i) {
|
||||
msg.data.joints_data[i].position =
|
||||
(input(i, 1) - joint_config_[i].offset) * joint_config_[i].dir;
|
||||
msg.data.joints_data[i].velocity = input(i, 3) * joint_dir_[i];
|
||||
msg.data.joints_data[i].torque = input(i, 4) * joint_dir_[i];
|
||||
msg.data.joints_data[i].kp = input(i, 0);
|
||||
msg.data.joints_data[i].kd = input(i, 2);
|
||||
msg.data.joints_data[i].control_word = kIndexMotorControl;
|
||||
}
|
||||
std::cout << "SetJointCommand publish!! \n" << std::endl;
|
||||
joint_cmd_pub_->publish(msg);
|
||||
}
|
||||
|
||||
virtual VecXf GetMotorTemperture() {
|
||||
return motor_temperture_;
|
||||
}
|
||||
|
||||
virtual VecXf GetDriverTemperture() {
|
||||
return driver_temperture_;
|
||||
}
|
||||
|
||||
virtual double GetImuTimestamp() {
|
||||
return imu_ts_;
|
||||
}
|
||||
|
||||
virtual std::vector<uint16_t> GetBatteryData() {
|
||||
return battery_data_;
|
||||
}
|
||||
|
||||
virtual std::vector<uint16_t> GetDriverStatusWord() {
|
||||
return driver_status_;
|
||||
}
|
||||
|
||||
virtual std::vector<uint16_t> GetJointDataID() {
|
||||
return joint_data_id_;
|
||||
}
|
||||
|
||||
virtual void RefreshRobotData() {
|
||||
}
|
||||
|
||||
virtual void Handler(const drdds::msg::JointsData::SharedPtr msg) {
|
||||
++run_cnt_;
|
||||
for (int i = 0; i < dof_num_; ++i) {
|
||||
joint_pos_(i) = msg->data.joints_data[i].position * joint_dir_[i] + pos_offset_[i] / 180. * M_PI;
|
||||
joint_vel_(i) = msg->data.joints_data[i].velocity * joint_dir_[i];
|
||||
joint_tau_(i) = msg->data.joints_data[i].torque * joint_dir_[i];
|
||||
motor_temperture_(i) = float(msg->data.joints_data[i].motion_temp);
|
||||
driver_temperture_(i) = float(msg->data.joints_data[i].driver_temp);
|
||||
driver_status_[i] = msg->data.joints_data[i].status_word;
|
||||
joint_data_id_[i] = uint16_t(run_cnt_);
|
||||
}
|
||||
// FireAnt sim2sim telemetry is packed into unused entries of the fixed
|
||||
// 16-joint DDS message by mujoco_simulation_ros2.py.
|
||||
if (dof_num_ == 8 && msg->data.joints_data.size() >= 16) {
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
contact_force_(i) = msg->data.joints_data[8 + i].position;
|
||||
contact_force_(5 + i) = msg->data.joints_data[12 + i].position;
|
||||
}
|
||||
contact_force_(4) = msg->data.joints_data[12].velocity;
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
base_lin_vel_(i) = msg->data.joints_data[8 + i].torque;
|
||||
}
|
||||
}
|
||||
ri_ts_ = rclcpp::Time(msg->header.stamp).seconds();
|
||||
}
|
||||
|
||||
void HandlerIMU(const drdds::msg::ImuData::SharedPtr msg) {
|
||||
rpy_ = Vec3f(Deg2Rad(msg->data.roll), Deg2Rad(msg->data.pitch), Deg2Rad(msg->data.yaw));
|
||||
acc_ << msg->data.acc_x, msg->data.acc_y, msg->data.acc_z;
|
||||
omega_body_ << msg->data.omega_x, msg->data.omega_y, msg->data.omega_z;
|
||||
imu_ts_ = rclcpp::Time(msg->header.stamp).seconds();
|
||||
}
|
||||
|
||||
void HandlerHealth(const drdds::msg::BatteryData::SharedPtr msg) {
|
||||
battery_data_[0] = uint8_t(msg->data[0].battery_level);
|
||||
remain_bat = battery_data_[0] / 100;
|
||||
battery_info_[0].battery_level = msg->data[0].battery_level;
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,41 @@
|
||||
#pragma once
|
||||
|
||||
#include "dds_interface.hpp"
|
||||
|
||||
/** Identity-calibrated DDS interface for the 8-DoF FireAnt MuJoCo model. */
|
||||
class FireAntInterface : public DdsInterface {
|
||||
public:
|
||||
explicit FireAntInterface(const std::string &robot_name)
|
||||
: DdsInterface(robot_name, 8) {
|
||||
for (int i = 0; i < dof_num_; ++i) {
|
||||
pos_offset_[i] = 0.0f;
|
||||
joint_dir_[i] = 1.0f;
|
||||
data_updated_[i] = false;
|
||||
joint_config_[i].dir = 1.0f;
|
||||
joint_config_[i].offset = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
void Start() override {
|
||||
time_stamp_ = GetTimestampMs();
|
||||
}
|
||||
|
||||
void Stop() override {}
|
||||
|
||||
void SetJointCommand(Eigen::Matrix<float, Eigen::Dynamic, 5> input) override {
|
||||
if (input.rows() != dof_num_ || input.cols() != 5) {
|
||||
throw std::runtime_error("FireAnt joint command must be 8x5");
|
||||
}
|
||||
joint_cmd_ = input;
|
||||
auto msg = drdds::msg::JointsDataCmd();
|
||||
for (int i = 0; i < dof_num_; ++i) {
|
||||
msg.data.joints_data[i].position = input(i, 1);
|
||||
msg.data.joints_data[i].velocity = input(i, 3);
|
||||
msg.data.joints_data[i].torque = input(i, 4);
|
||||
msg.data.joints_data[i].kp = (i % 2 == 0) ? input(i, 0) : 0.0f;
|
||||
msg.data.joints_data[i].kd = input(i, 2);
|
||||
msg.data.joints_data[i].control_word = kIndexMotorControl;
|
||||
}
|
||||
joint_cmd_pub_->publish(msg);
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,121 @@
|
||||
#pragma once
|
||||
|
||||
#include "robot_interface.h"
|
||||
#include "dds_interface.hpp"
|
||||
|
||||
class M20Interface : public DdsInterface {
|
||||
protected:
|
||||
void ResetPositionOffset() {
|
||||
memset(data_updated_, 0, dof_num_ * sizeof(bool));
|
||||
this->SetJointCommand(MatXf::Zero(dof_num_, 5));
|
||||
|
||||
VecXf last_joint_pos = this->GetJointPosition();
|
||||
VecXf current_joint_pos = this->GetJointPosition();
|
||||
int cnt = 0;
|
||||
while (!IsDataUpdatedFinished()) {
|
||||
++cnt;
|
||||
usleep(1000);
|
||||
|
||||
rclcpp::spin_some(this->get_node());
|
||||
current_joint_pos = this->GetJointPosition();
|
||||
for (int i = 0; i < dof_num_; ++i) {
|
||||
if (!data_updated_[i] && current_joint_pos(i) != last_joint_pos(i) &&
|
||||
!std::isnan(current_joint_pos(i))) {
|
||||
data_updated_[i] = true;
|
||||
std::cout << "joint " << i << " data updated at " << cnt << " cnt!" << std::endl;
|
||||
}
|
||||
}
|
||||
last_joint_pos = current_joint_pos;
|
||||
|
||||
if (cnt > 10000) {
|
||||
for (int i = 0; i < dof_num_; ++i) {
|
||||
std::cout << i << " :" << data_updated_[i] << std::endl;
|
||||
}
|
||||
std::cout << "joint data update is not finished\n";
|
||||
}
|
||||
}
|
||||
for (int i = 1; i < dof_num_; i += 4) {
|
||||
if (current_joint_pos(i) < -140. / 180. * M_PI) {
|
||||
pos_offset_[i] = pos_offset_[i] + 360.;
|
||||
std::cout << "joint " << i << " offset is changed to " << pos_offset_[i] << "\n";
|
||||
} else if (current_joint_pos(i) > 140. / 180. * M_PI) {
|
||||
pos_offset_[i] = pos_offset_[i] - 360.;
|
||||
std::cout << "joint " << i << " offset is changed to " << pos_offset_[i] << "\n";
|
||||
}
|
||||
}
|
||||
for (int i = 2; i < dof_num_; i += 4) {
|
||||
if (current_joint_pos(i) < -164. / 180. * M_PI) {
|
||||
pos_offset_[i] = pos_offset_[i] + 360.;
|
||||
std::cout << "joint " << i << " offset is changed to " << pos_offset_[i] << "\n";
|
||||
} else if (current_joint_pos(i) > 164. / 180. * M_PI) {
|
||||
pos_offset_[i] = pos_offset_[i] - 360.;
|
||||
std::cout << "joint " << i << " offset is changed to " << pos_offset_[i] << "\n";
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < dof_num_; ++i) {
|
||||
joint_config_[i].dir = joint_dir_[i];
|
||||
joint_config_[i].offset = Deg2Rad(pos_offset_[i]);
|
||||
}
|
||||
}
|
||||
|
||||
public:
|
||||
M20Interface(const std::string &robot_name) : DdsInterface(robot_name, 16) {
|
||||
battery_data_.resize(2 * BATTERY_DATA_SIZE);
|
||||
|
||||
float init_pos_offset[16] = {-25, -131, 160, 0.,
|
||||
25, -131, 160, 0,
|
||||
-25, 131, -160, 0,
|
||||
25, 131, -160, 0};
|
||||
float joint_dir[16] = {1, 1, -1, 1,
|
||||
1, -1, 1, -1,
|
||||
-1, 1, -1, 1.,
|
||||
-1, -1, 1, -1};
|
||||
for (int i = 0; i < dof_num_; ++i) {
|
||||
pos_offset_[i] = init_pos_offset[i];
|
||||
joint_dir_[i] = joint_dir[i];
|
||||
data_updated_[i] = false;
|
||||
joint_config_[i].dir = joint_dir_[i];
|
||||
joint_config_[i].offset = Deg2Rad(pos_offset_[i]);
|
||||
}
|
||||
}
|
||||
|
||||
~M20Interface() {
|
||||
}
|
||||
|
||||
virtual void Start() {
|
||||
time_stamp_ = GetTimestampMs();
|
||||
ResetPositionOffset();
|
||||
}
|
||||
|
||||
virtual void Stop() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Sends joint commands to the robot.
|
||||
* Input matrix layout: [dof × 5] → [kp, pos, kd, vel, torque_ff]
|
||||
*
|
||||
* ⚠ Torque values (column 4) are protected by low-level hardware torque limits
|
||||
* → commands exceeding limits are saturated at the firmware/hardware level
|
||||
*
|
||||
* Joints with index % 4 == 3 have kp forced to 0 (velocity/torque mode)
|
||||
*/
|
||||
virtual void SetJointCommand(Eigen::Matrix<float, Eigen::Dynamic, 5> input) {
|
||||
auto msg = drdds::msg::JointsDataCmd();
|
||||
for (int i = 0; i < dof_num_; ++i) {
|
||||
msg.data.joints_data[i].position =
|
||||
(input(i, 1) - joint_config_[i].offset) * joint_config_[i].dir;
|
||||
msg.data.joints_data[i].velocity = input(i, 3) * joint_dir_[i];
|
||||
msg.data.joints_data[i].torque = input(i, 4) * joint_dir_[i];
|
||||
msg.data.joints_data[i].kp = input(i, 0);
|
||||
if (i % 4 == 3) {
|
||||
msg.data.joints_data[i].kp = 0;
|
||||
}
|
||||
msg.data.joints_data[i].kd = input(i, 2);
|
||||
msg.data.joints_data[i].control_word = kIndexMotorControl;
|
||||
}
|
||||
joint_cmd_pub_->publish(msg);
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
@@ -0,0 +1,175 @@
|
||||
/**
|
||||
* @file robot_interface.h
|
||||
* @brief this file is for applying robot's control interface
|
||||
* @author DeepRobotics
|
||||
* @version 1.0
|
||||
* @date 2025-11-07
|
||||
*
|
||||
* @copyright Copyright (c) 2025 DeepRobotics
|
||||
*
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "common_types.h"
|
||||
#include <atomic>
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
using namespace types;
|
||||
|
||||
namespace interface{
|
||||
const int BATTERY_DATA_SIZE=8;
|
||||
|
||||
class RobotInterface{
|
||||
private:
|
||||
/* data */
|
||||
public:
|
||||
/**
|
||||
* @brief Construct a new Robot Interface object
|
||||
* @param robot_name robot name
|
||||
* @param dof_num The number of degrees of freedom of the robot.
|
||||
*/
|
||||
RobotInterface(const std::string& robot_name, int dof_num):robot_name_(robot_name), dof_num_(dof_num){
|
||||
start_flag_ = true;
|
||||
joint_cmd_ = Eigen::Matrix<float, Eigen::Dynamic, 5>::Zero(dof_num_, 5);
|
||||
node_ = std::make_shared<rclcpp::Node>("rl_deploy");
|
||||
|
||||
}
|
||||
virtual ~RobotInterface(){};
|
||||
|
||||
std::string robot_name_;
|
||||
const int dof_num_;
|
||||
Eigen::Matrix<float, Eigen::Dynamic, 5> joint_cmd_;
|
||||
std::atomic<bool> start_flag_;
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
|
||||
rclcpp::Node::SharedPtr get_node() {
|
||||
return node_;
|
||||
}
|
||||
/**
|
||||
* @brief Start to get robot's state and send control command
|
||||
*/
|
||||
virtual void Start() = 0;
|
||||
|
||||
/**
|
||||
* @brief Stop to control the robot
|
||||
*/
|
||||
virtual void Stop() = 0;
|
||||
|
||||
/**
|
||||
* @brief Get the time stamp of the robot
|
||||
* @return double time stamp
|
||||
*/
|
||||
virtual double GetInterfaceTimeStamp() = 0;
|
||||
|
||||
/**
|
||||
* @brief Get the joint position of the robot
|
||||
* @return VecXf joint position vector
|
||||
*/
|
||||
virtual VecXf GetJointPosition() = 0;
|
||||
|
||||
/**
|
||||
* @brief Get the joint velocity of the robot
|
||||
* @return VecXf joint velocity vector
|
||||
*/
|
||||
virtual VecXf GetJointVelocity() = 0;
|
||||
|
||||
/**
|
||||
* @brief Get the joint torque of the robot
|
||||
* @return VecXf joint torque vector
|
||||
*/
|
||||
virtual VecXf GetJointTorque() = 0;
|
||||
|
||||
/**
|
||||
* @brief Get the roll-pitch-yaw angle of the robot base
|
||||
* @return Vec3f roll-pitch-yaw(unit rad)
|
||||
*/
|
||||
virtual Vec3f GetImuRpy() = 0;
|
||||
|
||||
/**
|
||||
* @brief Get the accleration of the robot base
|
||||
* @return Vec3f accleration(unit m*s^-2)
|
||||
*/
|
||||
virtual Vec3f GetImuAcc() = 0;
|
||||
|
||||
/**
|
||||
* @brief Get the angular velocity of robot base
|
||||
* @return Vec3f angular velocity in body coordinate(unit rad/s)
|
||||
*/
|
||||
virtual Vec3f GetImuOmega() = 0;
|
||||
|
||||
/** Body-frame base linear velocity, when supplied by the simulator/estimator. */
|
||||
virtual Vec3f GetBaseLinearVelocity(){
|
||||
return Vec3f::Zero();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Set the joint command in standard form
|
||||
* torque = kp * (qDes - q) + kd * (vDes - v) + tff
|
||||
* @param input a dof_num*5 matrix and each column represent kp, goal_angle_pos, kd, goal_vel, torque_feedforward
|
||||
*/
|
||||
virtual void SetJointCommand(Eigen::Matrix<float, Eigen::Dynamic, 5> input) = 0;
|
||||
|
||||
virtual MatXf GetJointCommand(){
|
||||
return joint_cmd_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the contact force if robot have any force sensor
|
||||
* @return VecXf force vector
|
||||
*/
|
||||
virtual VecXf GetContactForce() = 0;
|
||||
|
||||
|
||||
/**
|
||||
* @brief Get the motor temperture
|
||||
* @return VecXf motor temperture
|
||||
*/
|
||||
virtual VecXf GetMotorTemperture(){
|
||||
return 30.*VecXf::Ones(dof_num_);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the driver temperture
|
||||
* @return VecXf driver temperture
|
||||
*/
|
||||
virtual VecXf GetDriverTemperture(){
|
||||
return VecXf::Zero(dof_num_);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the Battery Data
|
||||
* @return std::vector<uint16_t> level(0~100), voltage(mV), current(mA), cycles, temperture*4(C)
|
||||
*/
|
||||
virtual std::vector<uint16_t> GetBatteryData(){
|
||||
return std::vector<uint16_t>(BATTERY_DATA_SIZE, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the Imu Timestamp
|
||||
* @return float
|
||||
*/
|
||||
virtual double GetImuTimestamp(){
|
||||
return 0.;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the Driver Status Word
|
||||
* @return std::vector<uint16_t>
|
||||
*/
|
||||
virtual std::vector<uint16_t> GetDriverStatusWord(){
|
||||
return std::vector<uint16_t>(dof_num_, 1);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the Joint Data ID object
|
||||
* @return std::vector<uint16_t>
|
||||
*/
|
||||
virtual std::vector<uint16_t> GetJointDataID(){
|
||||
return std::vector<uint16_t>(dof_num_, 0);
|
||||
}
|
||||
|
||||
virtual void RefreshRobotData(){
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
};
|
||||
Binary file not shown.
@@ -0,0 +1,361 @@
|
||||
#!/usr/bin/env python3
|
||||
"""ROS 2 MuJoCo bridge for the original M20 and the 8-DoF FireAnt model."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import mujoco
|
||||
import mujoco.viewer
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from builtin_interfaces.msg import Time
|
||||
from drdds.msg import (
|
||||
ImuData,
|
||||
ImuDataValue,
|
||||
JointData,
|
||||
JointsData,
|
||||
JointsDataCmd,
|
||||
JointsDataValue,
|
||||
MetaType,
|
||||
)
|
||||
from rclpy.node import Node
|
||||
|
||||
|
||||
CURRENT_DIR = Path(__file__).resolve().parent
|
||||
PROJECT_ROOT = CURRENT_DIR.parents[2]
|
||||
MESSAGE_JOINT_COUNT = 16
|
||||
DT = 0.001
|
||||
STATE_PUBLISH_DECIMATION = 5 # 200 Hz, matching the C++ controller loop.
|
||||
|
||||
FIRE_ANT_JOINTS = (
|
||||
"fl_knee_joint", "fl_wheel_joint",
|
||||
"fr_knee_joint", "fr_wheel_joint",
|
||||
"hl_knee_joint", "hl_wheel_joint",
|
||||
"hr_knee_joint", "hr_wheel_joint",
|
||||
)
|
||||
FIRE_ANT_HOME = np.array([0.5, 0.0, 0.5, 0.0, -0.5, 0.0, -0.5, 0.0], dtype=np.float64)
|
||||
|
||||
M20_DIR = np.array(
|
||||
[1, 1, -1, 1, 1, -1, 1, -1, -1, 1, -1, 1, -1, -1, 1, -1],
|
||||
dtype=np.float64,
|
||||
)
|
||||
M20_OFFSET = np.deg2rad(
|
||||
np.array([-25, 229, 160, 0, 25, -131, -200, 0, -25, -229, -160, 0, 25, 131, 200, 0])
|
||||
)
|
||||
M20_HOME = np.array(
|
||||
[-0.438, -1.16, 2.76, 0, 0.438, -1.16, 2.76, 0,
|
||||
-0.438, 1.16, -2.76, 0, 0.438, 1.16, -2.76, 0],
|
||||
dtype=np.float64,
|
||||
)
|
||||
|
||||
|
||||
def model_path(robot: str, terrain: str) -> Path:
|
||||
if robot == "fire_ant":
|
||||
name = "fire_ant_rough.xml" if terrain == "rough" else "fire_ant.xml"
|
||||
return PROJECT_ROOT / "M20_description" / "fire_ant_mjcf" / "mjcf" / name
|
||||
name = "M20_stair.xml" if terrain == "rough" else "M20.xml"
|
||||
return PROJECT_ROOT / "M20_description" / "m20_mjcf" / "mjcf" / name
|
||||
|
||||
|
||||
def quaternion_to_euler(q: np.ndarray) -> np.ndarray:
|
||||
"""Convert a MuJoCo wxyz quaternion to roll, pitch and yaw."""
|
||||
w, x, y, z = q
|
||||
roll = np.arctan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))
|
||||
pitch = np.arcsin(np.clip(2.0 * (w * y - z * x), -1.0, 1.0))
|
||||
yaw = np.arctan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))
|
||||
return np.array([roll, pitch, yaw], dtype=np.float64)
|
||||
|
||||
|
||||
class MuJoCoSimulationNode(Node):
|
||||
def __init__(
|
||||
self,
|
||||
robot: str,
|
||||
xml_path: Path,
|
||||
use_viewer: bool,
|
||||
randomize: bool,
|
||||
seed: int,
|
||||
realtime_factor: float,
|
||||
) -> None:
|
||||
super().__init__("mujoco_simulation")
|
||||
if not xml_path.is_file():
|
||||
raise FileNotFoundError(f"Cannot find MJCF: {xml_path}")
|
||||
|
||||
self.robot = robot
|
||||
self.model = mujoco.MjModel.from_xml_path(str(xml_path))
|
||||
self.model.opt.timestep = DT
|
||||
self.data = mujoco.MjData(self.model)
|
||||
self.realtime_factor = realtime_factor
|
||||
|
||||
self.dof_num = self.model.nu
|
||||
expected_dof = 8 if robot == "fire_ant" else 16
|
||||
if self.dof_num != expected_dof:
|
||||
raise RuntimeError(f"{robot} expected {expected_dof} actuators, model has {self.dof_num}")
|
||||
|
||||
self.joint_names = list(FIRE_ANT_JOINTS) if robot == "fire_ant" else [
|
||||
mujoco.mj_id2name(
|
||||
self.model,
|
||||
mujoco.mjtObj.mjOBJ_JOINT,
|
||||
int(self.model.actuator_trnid[i, 0]),
|
||||
)
|
||||
for i in range(self.dof_num)
|
||||
]
|
||||
self.joint_ids = np.array(
|
||||
[mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, name) for name in self.joint_names],
|
||||
dtype=np.int32,
|
||||
)
|
||||
if np.any(self.joint_ids < 0):
|
||||
raise RuntimeError(f"Missing joints in MJCF: {self.joint_names}")
|
||||
self.qpos_adr = self.model.jnt_qposadr[self.joint_ids]
|
||||
self.dof_adr = self.model.jnt_dofadr[self.joint_ids]
|
||||
self.actuator_ids = np.array(
|
||||
[
|
||||
next(i for i in range(self.model.nu) if int(self.model.actuator_trnid[i, 0]) == joint_id)
|
||||
for joint_id in self.joint_ids
|
||||
],
|
||||
dtype=np.int32,
|
||||
)
|
||||
|
||||
self.joint_dir = np.ones(self.dof_num) if robot == "fire_ant" else M20_DIR.copy()
|
||||
self.pos_offset = np.zeros(self.dof_num) if robot == "fire_ant" else M20_OFFSET.copy()
|
||||
self.kp_cmd = np.zeros(self.dof_num)
|
||||
self.kd_cmd = np.zeros(self.dof_num)
|
||||
self.pos_cmd = np.zeros(self.dof_num)
|
||||
self.vel_cmd = np.zeros(self.dof_num)
|
||||
self.tau_ff = np.zeros(self.dof_num)
|
||||
self.applied_tau = np.zeros(self.dof_num)
|
||||
self.velocity_limit = (
|
||||
np.array([17.8024, 109.956] * 4)
|
||||
if robot == "fire_ant"
|
||||
else np.array([45.0, 22.4, 22.4, 100.0] * 4)
|
||||
)
|
||||
self.kp_scale = np.ones(self.dof_num)
|
||||
self.kd_scale = np.ones(self.dof_num)
|
||||
self.timestamp = 0.0
|
||||
|
||||
self.base_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "base_link")
|
||||
self.wheel_body_ids: list[int] = []
|
||||
self.knee_body_ids: list[int] = []
|
||||
if robot == "fire_ant":
|
||||
self.wheel_body_ids = [self._body_id(f"{leg}_wheel") for leg in ("fl", "fr", "hl", "hr")]
|
||||
self.knee_body_ids = [self._body_id(f"{leg}_knee") for leg in ("fl", "fr", "hl", "hr")]
|
||||
if randomize:
|
||||
self._apply_training_randomization(seed)
|
||||
|
||||
self._set_initial_pose()
|
||||
self.imu_pub = self.create_publisher(ImuData, "/IMU_DATA", 200)
|
||||
self.joints_pub = self.create_publisher(JointsData, "/JOINTS_DATA", 200)
|
||||
self.cmd_sub = self.create_subscription(JointsDataCmd, "/JOINTS_CMD", self._cmd_callback, 50)
|
||||
self.viewer = mujoco.viewer.launch_passive(self.model, self.data) if use_viewer else None
|
||||
|
||||
mode = "randomized" if randomize else "nominal"
|
||||
self.get_logger().info(
|
||||
f"Loaded {robot} ({mode}) from {xml_path}; dof={self.dof_num}, dt={DT:.4f}s"
|
||||
)
|
||||
|
||||
def _body_id(self, name: str) -> int:
|
||||
body_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, name)
|
||||
if body_id < 0:
|
||||
raise RuntimeError(f"Missing body in MJCF: {name}")
|
||||
return body_id
|
||||
|
||||
def _apply_training_randomization(self, seed: int) -> None:
|
||||
"""Apply one reproducible sample from the training-time domain ranges."""
|
||||
rng = np.random.default_rng(seed)
|
||||
original_mass = self.model.body_mass.copy()
|
||||
for body_id in [*self.knee_body_ids, *self.wheel_body_ids]:
|
||||
mass_scale = rng.uniform(0.85, 1.15)
|
||||
inertia_scale = mass_scale * rng.uniform(0.85, 1.15)
|
||||
self.model.body_mass[body_id] *= mass_scale
|
||||
self.model.body_inertia[body_id] *= inertia_scale
|
||||
|
||||
base_mass = max(0.1, original_mass[self.base_id] + rng.uniform(-1.0, 3.0))
|
||||
base_mass_scale = base_mass / original_mass[self.base_id]
|
||||
self.model.body_mass[self.base_id] = base_mass
|
||||
self.model.body_inertia[self.base_id] *= base_mass_scale * rng.uniform(0.85, 1.15)
|
||||
self.model.body_ipos[self.base_id] += rng.uniform((-0.03, -0.03, -0.02), (0.03, 0.03, 0.02))
|
||||
|
||||
for body_id in self.wheel_body_ids:
|
||||
geom_start = self.model.body_geomadr[body_id]
|
||||
geom_count = self.model.body_geomnum[body_id]
|
||||
self.model.geom_friction[geom_start:geom_start + geom_count, 0] = rng.uniform(0.3, 1.0)
|
||||
self.kp_scale = rng.uniform(0.85, 1.15, self.dof_num)
|
||||
self.kd_scale = rng.uniform(0.85, 1.15, self.dof_num)
|
||||
|
||||
def _set_initial_pose(self) -> None:
|
||||
self.data.qpos[:] = 0.0
|
||||
self.data.qvel[:] = 0.0
|
||||
self.data.qpos[0:3] = (0.0, 0.0, 0.306 if self.robot == "fire_ant" else 0.2)
|
||||
self.data.qpos[3:7] = (1.0, 0.0, 0.0, 0.0)
|
||||
self.data.qpos[self.qpos_adr] = FIRE_ANT_HOME if self.robot == "fire_ant" else M20_HOME
|
||||
mujoco.mj_forward(self.model, self.data)
|
||||
|
||||
def _cmd_callback(self, msg: JointsDataCmd) -> None:
|
||||
if len(msg.data.joints_data) < self.dof_num:
|
||||
self.get_logger().warn(
|
||||
f"Ignoring short JointsDataCmd ({len(msg.data.joints_data)} < {self.dof_num})"
|
||||
)
|
||||
return
|
||||
pub_pos = np.empty(self.dof_num)
|
||||
pub_vel = np.empty(self.dof_num)
|
||||
for i in range(self.dof_num):
|
||||
command = msg.data.joints_data[i]
|
||||
self.kp_cmd[i] = command.kp
|
||||
self.kd_cmd[i] = command.kd
|
||||
pub_pos[i] = command.position
|
||||
pub_vel[i] = command.velocity
|
||||
self.tau_ff[i] = command.torque
|
||||
self.pos_cmd = pub_pos * self.joint_dir + self.pos_offset
|
||||
self.vel_cmd = pub_vel * self.joint_dir
|
||||
|
||||
def _joint_state(self) -> tuple[np.ndarray, np.ndarray]:
|
||||
return self.data.qpos[self.qpos_adr].copy(), self.data.qvel[self.dof_adr].copy()
|
||||
|
||||
def _apply_joint_torque(self) -> None:
|
||||
q, dq = self._joint_state()
|
||||
torque = (
|
||||
self.kp_cmd * self.kp_scale * (self.pos_cmd - q)
|
||||
+ self.kd_cmd * self.kd_scale * (self.vel_cmd - dq)
|
||||
+ self.tau_ff
|
||||
)
|
||||
ctrl_range = self.model.actuator_ctrlrange[self.actuator_ids]
|
||||
torque = np.clip(torque, ctrl_range[:, 0], ctrl_range[:, 1])
|
||||
# The MuJoCo motor has a torque limit but no built-in speed limit. Prevent
|
||||
# positive work beyond the datasheet/training actuator velocity limit.
|
||||
overspeed_accelerating = (np.abs(dq) >= self.velocity_limit) & (torque * dq > 0.0)
|
||||
torque[overspeed_accelerating] = 0.0
|
||||
self.data.ctrl[self.actuator_ids] = torque
|
||||
|
||||
def _body_linear_velocity(self) -> np.ndarray:
|
||||
# Free-joint translational qvel is in world axes; the policy uses body axes.
|
||||
rotation_world_from_body = self.data.xmat[self.base_id].reshape(3, 3)
|
||||
return rotation_world_from_body.T @ self.data.qvel[0:3]
|
||||
|
||||
def _contact_force_norms(self, body_ids: list[int]) -> np.ndarray:
|
||||
# cfrc_ext stores [torque, force]; norm is frame invariant.
|
||||
return np.linalg.norm(self.data.cfrc_ext[body_ids, 3:6], axis=1)
|
||||
|
||||
def _make_stamp(self) -> Time:
|
||||
stamp = Time()
|
||||
stamp.sec = int(self.timestamp)
|
||||
stamp.nanosec = int((self.timestamp - stamp.sec) * 1e9)
|
||||
return stamp
|
||||
|
||||
def _publish_robot_state(self) -> None:
|
||||
q_world = self.data.sensordata[0:4]
|
||||
rpy_deg = np.rad2deg(quaternion_to_euler(q_world))
|
||||
body_acc = self.data.sensordata[4:7]
|
||||
angular_velocity_body = self.data.sensordata[7:10]
|
||||
|
||||
imu_msg = ImuData()
|
||||
imu_msg.header = MetaType()
|
||||
imu_msg.header.frame_id = 0
|
||||
imu_msg.header.stamp = self._make_stamp()
|
||||
imu_msg.data = ImuDataValue()
|
||||
imu_msg.data.roll, imu_msg.data.pitch, imu_msg.data.yaw = map(float, rpy_deg)
|
||||
imu_msg.data.omega_x, imu_msg.data.omega_y, imu_msg.data.omega_z = map(
|
||||
float, angular_velocity_body
|
||||
)
|
||||
imu_msg.data.acc_x, imu_msg.data.acc_y, imu_msg.data.acc_z = map(float, body_acc)
|
||||
self.imu_pub.publish(imu_msg)
|
||||
|
||||
q, dq = self._joint_state()
|
||||
pub_pos = (q - self.pos_offset) * self.joint_dir
|
||||
pub_vel = dq * self.joint_dir
|
||||
self.applied_tau = self.data.actuator_force[self.actuator_ids] * self.joint_dir
|
||||
|
||||
joints_msg = JointsData()
|
||||
joints_msg.header = MetaType()
|
||||
joints_msg.header.frame_id = 0
|
||||
joints_msg.header.stamp = self._make_stamp()
|
||||
joints_msg.data = JointsDataValue()
|
||||
joints_msg.data.joints_data = [JointData() for _ in range(MESSAGE_JOINT_COUNT)]
|
||||
for i in range(self.dof_num):
|
||||
joint = joints_msg.data.joints_data[i]
|
||||
joint.name = [32, 32, 32, 32]
|
||||
joint.data_id = 0
|
||||
joint.status_word = 1
|
||||
joint.position = float(pub_pos[i])
|
||||
joint.velocity = float(pub_vel[i])
|
||||
joint.torque = float(self.applied_tau[i])
|
||||
joint.motion_temp = 40.0
|
||||
joint.driver_temp = 45.0
|
||||
|
||||
if self.robot == "fire_ant":
|
||||
# Sim-only extension inside the eight unused fields of the fixed 16-joint DDS message.
|
||||
wheel_force = self._contact_force_norms(self.wheel_body_ids)
|
||||
knee_force = self._contact_force_norms(self.knee_body_ids)
|
||||
base_force = self._contact_force_norms([self.base_id])[0]
|
||||
base_velocity = self._body_linear_velocity()
|
||||
for i in range(4):
|
||||
joints_msg.data.joints_data[8 + i].position = float(wheel_force[i])
|
||||
joints_msg.data.joints_data[12 + i].position = float(knee_force[i])
|
||||
for i in range(3):
|
||||
joints_msg.data.joints_data[8 + i].torque = float(base_velocity[i])
|
||||
joints_msg.data.joints_data[12].velocity = float(base_force)
|
||||
|
||||
self.joints_pub.publish(joints_msg)
|
||||
|
||||
def start(self) -> None:
|
||||
step = 0
|
||||
next_step_time = time.perf_counter()
|
||||
while rclpy.ok() and (self.viewer is None or self.viewer.is_running()):
|
||||
rclpy.spin_once(self, timeout_sec=0.0)
|
||||
self._apply_joint_torque()
|
||||
mujoco.mj_step(self.model, self.data)
|
||||
mujoco.mj_rnePostConstraint(self.model, self.data)
|
||||
step += 1
|
||||
self.timestamp = step * DT
|
||||
if step % STATE_PUBLISH_DECIMATION == 0:
|
||||
self._publish_robot_state()
|
||||
if self.viewer is not None and step % 20 == 0:
|
||||
self.viewer.sync()
|
||||
|
||||
if self.realtime_factor > 0.0:
|
||||
next_step_time += DT / self.realtime_factor
|
||||
remaining = next_step_time - time.perf_counter()
|
||||
if remaining > 0:
|
||||
time.sleep(remaining)
|
||||
elif remaining < -0.1:
|
||||
next_step_time = time.perf_counter()
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--robot", choices=("fire_ant", "m20"), default="fire_ant")
|
||||
parser.add_argument("--terrain", choices=("flat", "rough"), default="rough")
|
||||
parser.add_argument("--xml", type=Path, help="Override the selected MJCF path")
|
||||
parser.add_argument("--headless", action="store_true", help="Run without the MuJoCo viewer")
|
||||
parser.add_argument("--randomize", action="store_true", help="Sample training mass/friction/gain ranges once")
|
||||
parser.add_argument("--seed", type=int, default=0)
|
||||
parser.add_argument("--realtime-factor", type=float, default=1.0, help="0 runs as fast as possible")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def main() -> None:
|
||||
args = parse_args()
|
||||
xml_path = args.xml.resolve() if args.xml else model_path(args.robot, args.terrain).resolve()
|
||||
rclpy.init()
|
||||
node = MuJoCoSimulationNode(
|
||||
robot=args.robot,
|
||||
xml_path=xml_path,
|
||||
use_viewer=not args.headless,
|
||||
randomize=args.randomize,
|
||||
seed=args.seed,
|
||||
realtime_factor=args.realtime_factor,
|
||||
)
|
||||
try:
|
||||
node.start()
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
np.set_printoptions(precision=4, suppress=True)
|
||||
main()
|
||||
Reference in New Issue
Block a user