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 support for ground truth estimator

This commit is contained in:
Huang Zhenbiao 2025-01-16 16:56:04 +08:00
parent 2a6af12ea7
commit 7ed46994e4
23 changed files with 506 additions and 357 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
controllers/ocs2_quadruped_controller/CMakeLists.txt
View File

@ -12,7 +12,6 @@ set(CONTROLLER_INCLUDE_DEPENDS
pluginlib
rcpputils
controller_interface
realtime_tools
ocs2_legged_robot_ros
ocs2_self_collision
@ -31,7 +30,7 @@ endforeach ()
add_library(${PROJECT_NAME} SHARED
src/Ocs2QuadrupedController.cpp
# src/estimator/FromTopicEstimate.cpp
src/estimator/GroundTruth.cpp
src/estimator/LinearKalmanFilter.cpp
src/estimator/StateEstimateBase.cpp

3
controllers/ocs2_quadruped_controller/README.md
View File

@ -10,6 +10,9 @@ Tested environment:
* Ubuntu 22.04
* ROS2 Humble
*[x] **[2025-01-16]** Add support for ground truth estimator.
[![](http://i0.hdslb.com/bfs/archive/e758ce019587032449a153cf897a543443b64bba.jpg)](https://www.bilibili.com/video/BV1UcxieuEmH/)
## 1. Interfaces

4
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h
View File

@ -41,9 +41,13 @@ struct CtrlComponent {
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
foot_force_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
odom_state_interface_;
control_input_msgs::msg::Inputs control_inputs_;
ocs2::SystemObservation observation_;
int frequency_{};
bool reset = true;
std::shared_ptr<ocs2::legged_robot::StateEstimateBase> estimator_;
std::shared_ptr<ocs2::legged_robot::TargetManager> target_manager_;

33
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromTopiceEstimate.h
View File

@ -1,33 +0,0 @@
//
// Created by qiayuan on 2022/7/24.
//
#pragma once
#include "StateEstimateBase.h"
#include <realtime_tools/realtime_tools/realtime_buffer.h>
#pragma once
namespace ocs2::legged_robot {
using namespace ocs2;
class FromTopicStateEstimate : public StateEstimateBase {
public:
FromTopicStateEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics &eeKinematics);
void updateImu(const Eigen::Quaternion<scalar_t> &quat, const vector3_t &angularVelLocal,
const vector3_t &linearAccelLocal,
const matrix3_t &orientationCovariance, const matrix3_t &angularVelCovariance,
const matrix3_t &linearAccelCovariance) override {
};
vector_t update(const ros::Time &time, const ros::Duration &period) override;
private:
void callback(const nav_msgs::Odometry::ConstPtr &msg);
ros::Subscriber sub_;
realtime_tools::RealtimeBuffer<nav_msgs::Odometry> buffer_;
};
} // namespace legged

24
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/GroundTruth.h Normal file
View File

@ -0,0 +1,24 @@
//
// Created by qiayuan on 2022/7/24.
//
#pragma once
#include "StateEstimateBase.h"
#include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h>
#include <tf2_ros/transform_broadcaster.h>
namespace ocs2::legged_robot {
class GroundTruth final : public StateEstimateBase {
public:
GroundTruth(CentroidalModelInfo info, CtrlComponent &ctrl_component,
const rclcpp_lifecycle::LifecycleNode::SharedPtr &node);
vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) override;
protected:
nav_msgs::msg::Odometry getOdomMsg();
vector3_t position_;
vector3_t linear_velocity_;
};
}

5
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h
View File

@ -26,6 +26,9 @@ namespace ocs2::legged_robot {
protected:
nav_msgs::msg::Odometry getOdomMsg();
PinocchioInterface pinocchio_interface_;
std::unique_ptr<PinocchioEndEffectorKinematics> ee_kinematics_;
vector_t feet_heights_;
// Config
@ -43,4 +46,4 @@ namespace ocs2::legged_robot {
matrix_t a_, b_, c_, q_, p_, r_;
vector_t xHat_, ps_, vs_;
};
} // namespace legged
}

7
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h
View File

@ -22,9 +22,7 @@ namespace ocs2::legged_robot {
public:
virtual ~StateEstimateBase() = default;
StateEstimateBase(PinocchioInterface pinocchio_interface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics &ee_kinematics,
CtrlComponent &ctrl_component,
StateEstimateBase(CentroidalModelInfo info, CtrlComponent &ctrl_component,
rclcpp_lifecycle::LifecycleNode::SharedPtr node);
virtual void updateJointStates();
@ -45,10 +43,7 @@ namespace ocs2::legged_robot {
void publishMsgs(const nav_msgs::msg::Odometry &odom) const;
CtrlComponent &ctrl_component_;
PinocchioInterface pinocchio_interface_;
CentroidalModelInfo info_;
std::unique_ptr<PinocchioEndEffectorKinematics> ee_kinematics_;
vector3_t zyx_offset_ = vector3_t::Zero();
vector_t rbd_state_;

51
controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp
View File

@ -15,6 +15,7 @@
#include <ocs2_sqp/SqpMpc.h>
#include <angles/angles.h>
#include <ocs2_quadruped_controller/control/GaitManager.h>
#include <ocs2_quadruped_controller/estimator/GroundTruth.h>
namespace ocs2::legged_robot {
using config_type = controller_interface::interface_configuration_type;
@ -46,8 +47,14 @@ namespace ocs2::legged_robot {
conf.names.push_back(imu_name_ + "/" += interface_type);
}
for (const auto &interface_type: foot_force_interface_types_) {
conf.names.push_back(foot_force_name_ + "/" += interface_type);
if (use_odom_) {
for (const auto &interface_type: odom_interface_types_) {
conf.names.push_back(odom_name_ + "/" += interface_type);
}
} else {
for (const auto &interface_type: foot_force_interface_types_) {
conf.names.push_back(foot_force_name_ + "/" += interface_type);
}
}
return conf;
@ -139,11 +146,22 @@ namespace ocs2::legged_robot {
auto_declare<std::vector<std::string> >("command_interfaces", command_interface_types_);
state_interface_types_ =
auto_declare<std::vector<std::string> >("state_interfaces", state_interface_types_);
// 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_);
foot_force_name_ = auto_declare<std::string>("foot_force_name", foot_force_name_);
foot_force_interface_types_ =
auto_declare<std::vector<std::string> >("foot_force_interfaces", state_interface_types_);
// Odometer Sensor (Ground Truth)
use_odom_ = auto_declare<bool>("use_odom", use_odom_);
if (use_odom_) {
odom_name_ = auto_declare<std::string>("odom_name", odom_name_);
odom_interface_types_ = auto_declare<std::vector<std::string> >("odom_interfaces", state_interface_types_);
} else {
// Foot Force Sensor
foot_force_name_ = auto_declare<std::string>("foot_force_name", foot_force_name_);
foot_force_interface_types_ =
auto_declare<std::vector<std::string> >("foot_force_interfaces", state_interface_types_);
}
// PD gains
default_kp_ = auto_declare<double>("default_kp", default_kp_);
@ -220,6 +238,8 @@ namespace ocs2::legged_robot {
ctrl_comp_.imu_state_interface_.emplace_back(interface);
} else if (interface.get_prefix_name() == foot_force_name_) {
ctrl_comp_.foot_force_state_interface_.emplace_back(interface);
} else if (interface.get_prefix_name() == odom_name_) {
ctrl_comp_.odom_state_interface_.emplace_back(interface);
} else {
state_interface_map_[interface.get_interface_name()]->push_back(interface);
}
@ -229,7 +249,7 @@ namespace ocs2::legged_robot {
// Initial state
ctrl_comp_.observation_.state.setZero(
static_cast<long>(legged_interface_->getCentroidalModelInfo().stateDim));
updateStateEstimation(get_node()->now(), rclcpp::Duration(0, 1/ctrl_comp_.frequency_*1000000000));
updateStateEstimation(get_node()->now(), rclcpp::Duration(0, 1 / ctrl_comp_.frequency_ * 1000000000));
ctrl_comp_.observation_.input.setZero(
static_cast<long>(legged_interface_->getCentroidalModelInfo().inputDim));
ctrl_comp_.observation_.mode = STANCE;
@ -330,10 +350,19 @@ namespace ocs2::legged_robot {
}
void Ocs2QuadrupedController::setupStateEstimate() {
ctrl_comp_.estimator_ = std::make_shared<KalmanFilterEstimate>(legged_interface_->getPinocchioInterface(),
legged_interface_->getCentroidalModelInfo(),
*eeKinematicsPtr_, ctrl_comp_, this->get_node());
dynamic_cast<KalmanFilterEstimate &>(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_);
if (use_odom_) {
ctrl_comp_.estimator_ = std::make_shared<GroundTruth>(legged_interface_->getCentroidalModelInfo(),
ctrl_comp_,
get_node());
RCLCPP_INFO(get_node()->get_logger(), "Using Ground Truth Estimator");
} else {
ctrl_comp_.estimator_ = std::make_shared<KalmanFilterEstimate>(legged_interface_->getPinocchioInterface(),
legged_interface_->getCentroidalModelInfo(),
*eeKinematicsPtr_, ctrl_comp_,
get_node());
dynamic_cast<KalmanFilterEstimate &>(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_);
RCLCPP_INFO(get_node()->get_logger(), "Using Kalman Filter Estimator");
}
ctrl_comp_.observation_.time = 0;
}
@ -344,7 +373,7 @@ namespace ocs2::legged_robot {
ctrl_comp_.observation_.state = rbd_conversions_->computeCentroidalStateFromRbdModel(measured_rbd_state_);
ctrl_comp_.observation_.state(9) = yaw_last + angles::shortest_angular_distance(
yaw_last, ctrl_comp_.observation_.state(9));
ctrl_comp_.observation_.mode = ctrl_comp_.estimator_->getMode();
// ctrl_comp_.observation_.mode = ctrl_comp_.estimator_->getMode();
}
}

6
controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h
View File

@ -97,10 +97,16 @@ namespace ocs2::legged_robot {
// IMU Sensor
std::string imu_name_;
std::vector<std::string> imu_interface_types_;
// Foot Force Sensor
std::string foot_force_name_;
std::vector<std::string> foot_force_interface_types_;
// Odometer Sensor (Ground Truth)
bool use_odom_ = false;
std::string odom_name_;
std::vector<std::string> odom_interface_types_;
double default_kp_ = 0;
double default_kd_ = 6;

1
controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp
View File

@ -48,5 +48,6 @@ namespace ocs2::legged_robot {
RCLCPP_INFO(rclcpp::get_logger("GaitManager"), "Switch to gait: %s",
gait_name_list_[ctrl_component_.control_inputs_.command - 1].c_str());
gait_updated_ = true;
ctrl_component_.reset = false;
}
}

1
controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp
View File

@ -25,6 +25,7 @@ namespace ocs2::legged_robot {
}
void TargetManager::update() {
if (ctrl_component_.reset) return;
vector_t cmdGoal = vector_t::Zero(6);
cmdGoal[0] = ctrl_component_.control_inputs_.ly * target_displacement_velocity_;
cmdGoal[1] = -ctrl_component_.control_inputs_.lx * target_displacement_velocity_;

36
controllers/ocs2_quadruped_controller/src/estimator/FromTopicEstimate.cpp
View File

@ -1,36 +0,0 @@
//
// Created by qiayuan on 2022/7/24.
//
#include "ocs2_quadruped_controller/estimator/FromTopiceEstimate.h"
namespace ocs2::legged_robot {
FromTopicStateEstimate::FromTopicStateEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics &eeKinematics)
: StateEstimateBase(std::move(pinocchioInterface), std::move(info), eeKinematics) {
ros::NodeHandle nh;
sub_ = nh.subscribe<nav_msgs::Odometry>("/ground_truth/state", 10, &FromTopicStateEstimate::callback, this);
}
void FromTopicStateEstimate::callback(const nav_msgs::Odometry::ConstPtr &msg) {
buffer_.writeFromNonRT(*msg);
}
vector_t FromTopicStateEstimate::update(const ros::Time & /*time*/, const ros::Duration & /*period*/) {
nav_msgs::Odometry odom = *buffer_.readFromRT();
updateAngular(quatToZyx(Eigen::Quaternion<scalar_t>(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,
odom.pose.pose.orientation.y,
odom.pose.pose.orientation.z)),
Eigen::Matrix<scalar_t, 3, 1>(odom.twist.twist.angular.x, odom.twist.twist.angular.y,
odom.twist.twist.angular.z));
updateLinear(Eigen::Matrix<scalar_t, 3, 1>(odom.pose.pose.position.x, odom.pose.pose.position.y,
odom.pose.pose.position.z),
Eigen::Matrix<scalar_t, 3, 1>(odom.twist.twist.linear.x, odom.twist.twist.linear.y,
odom.twist.twist.linear.z));
publishMsgs(odom);
return rbdState_;
}
} // namespace legged

64
controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp Normal file
View File

@ -0,0 +1,64 @@
//
// Created by qiayuan on 2022/7/24.
//
#include "ocs2_quadruped_controller/estimator/GroundTruth.h"
#include <ocs2_quadruped_controller/control/CtrlComponent.h>
namespace ocs2::legged_robot {
GroundTruth::GroundTruth(CentroidalModelInfo info, CtrlComponent &ctrl_component,
const rclcpp_lifecycle::LifecycleNode::SharedPtr &node)
: StateEstimateBase(
std::move(info), ctrl_component,
node) {
}
vector_t GroundTruth::update(const rclcpp::Time &time, const rclcpp::Duration &period) {
updateJointStates();
updateContact();
updateImu();
position_ = {
ctrl_component_.odom_state_interface_[0].get().get_value(),
ctrl_component_.odom_state_interface_[1].get().get_value(),
ctrl_component_.odom_state_interface_[2].get().get_value()
};
linear_velocity_ = {
ctrl_component_.odom_state_interface_[3].get().get_value(),
ctrl_component_.odom_state_interface_[4].get().get_value(),
ctrl_component_.odom_state_interface_[5].get().get_value()
};
updateLinear(position_, linear_velocity_);
auto odom = getOdomMsg();
odom.header.stamp = time;
odom.header.frame_id = "odom";
odom.child_frame_id = "base";
publishMsgs(odom);
return rbd_state_;
}
nav_msgs::msg::Odometry GroundTruth::getOdomMsg() {
nav_msgs::msg::Odometry odom;
odom.pose.pose.position.x = position_(0);
odom.pose.pose.position.y = position_(1);
odom.pose.pose.position.z = position_(2);
odom.pose.pose.orientation.x = quat_.x();
odom.pose.pose.orientation.y = quat_.y();
odom.pose.pose.orientation.z = quat_.z();
odom.pose.pose.orientation.w = quat_.w();
odom.twist.twist.linear.x = linear_velocity_(0);
odom.twist.twist.linear.y = linear_velocity_(1);
odom.twist.twist.linear.z = linear_velocity_(2);
odom.twist.twist.angular.x = angular_vel_local_.x();
odom.twist.twist.angular.y = angular_vel_local_.y();
odom.twist.twist.angular.z = angular_vel_local_.z();
return odom;
}
} // namespace legged

5
controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp
View File

@ -18,8 +18,10 @@ namespace ocs2::legged_robot {
const PinocchioEndEffectorKinematics &ee_kinematics,
CtrlComponent &ctrl_component,
const rclcpp_lifecycle::LifecycleNode::SharedPtr &node)
: StateEstimateBase(std::move(pinocchio_interface), std::move(info), ee_kinematics, ctrl_component,
: StateEstimateBase(std::move(info), ctrl_component,
node),
pinocchio_interface_(std::move(pinocchio_interface)),
ee_kinematics_(ee_kinematics.clone()),
numContacts_(info_.numThreeDofContacts + info_.numSixDofContacts),
dimContacts_(3 * numContacts_),
numState_(6 + dimContacts_),
@ -167,7 +169,6 @@ namespace ocs2::legged_robot {
odom.pose.pose.orientation.y = quat_.y();
odom.pose.pose.orientation.z = quat_.z();
odom.pose.pose.orientation.w = quat_.w();
odom.pose.pose.orientation.x = quat_.x();
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
odom.pose.covariance[i * 6 + j] = p_(i, j);

6
controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp
View File

@ -12,15 +12,11 @@
#include "ocs2_quadruped_controller/control/CtrlComponent.h"
namespace ocs2::legged_robot {
StateEstimateBase::StateEstimateBase(PinocchioInterface pinocchio_interface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics &ee_kinematics,
StateEstimateBase::StateEstimateBase(CentroidalModelInfo info,
CtrlComponent &ctrl_component,
rclcpp_lifecycle::LifecycleNode::SharedPtr node)
: ctrl_component_(ctrl_component),
pinocchio_interface_(std::move(pinocchio_interface)),
info_(std::move(info)),
ee_kinematics_(ee_kinematics.clone()),
rbd_state_(vector_t::Zero(2 * info_.generalizedCoordinatesNum)), node_(std::move(node)) {
odom_pub_ = node_->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("pose", 10);

21
descriptions/unitree/go1_description/xacro/ft_sensor.xacro Normal file
View File

@ -0,0 +1,21 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="ft_sensor" params="name">
<gazebo reference="${name}_foot_fixed">
<!-- Enable feedback for this joint -->
<provideFeedback>true</provideFeedback>
<!-- Prevent ros2_control from lumping this fixed joint with others -->
<disableFixedJointLumping>true</disableFixedJointLumping>
<sensor name="${name}_ft_sensor" type="force_torque">
<always_on>1</always_on>
<update_rate>500</update_rate>
<visualize>true</visualize>
<force_torque>
<frame>child</frame>
<!-- <measure_direction>child_to_parent</measure_direction>-->
</force_torque>
</sensor>
</gazebo>
</xacro:macro>
</robot>

6
descriptions/unitree/go1_description/xacro/gazebo.xacro
View File

@ -1,5 +1,6 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find go1_description)/xacro/ft_sensor.xacro"/>
<ros2_control name="GazeboSystem" type="system">
<hardware>
@ -185,4 +186,9 @@
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
<xacro:ft_sensor name="FR"/>
<xacro:ft_sensor name="FL"/>
<xacro:ft_sensor name="RR"/>
<xacro:ft_sensor name="RL"/>
</robot>

512
descriptions/unitree/go1_description/xacro/leg.xacro
View File

@ -2,280 +2,280 @@
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find go1_description)/xacro/transmission.xacro"/>
<xacro:include filename="$(find go1_description)/xacro/transmission.xacro"/>
<xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
<xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
<joint name="${name}_hip_joint" type="revolute">
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if>
<parent link="trunk"/>
<child link="${name}_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<xacro:if value="${(mirror_dae == True)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}"
upper="${hip_position_max}"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}"
upper="${-hip_position_min}"/>
</xacro:if>
</joint>
<joint name="${name}_hip_joint" type="revolute">
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if>
<parent link="trunk"/>
<child link="${name}_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<xacro:if value="${(mirror_dae == True)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}"
upper="${hip_position_max}"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}"
upper="${-hip_position_min}"/>
</xacro:if>
</joint>
<joint name="${name}_hip_rotor_joint" type="fixed">
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
</xacro:if>
<parent link="trunk"/>
<child link="${name}_hip_rotor"/>
</joint>
<joint name="${name}_hip_rotor_joint" type="fixed">
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${-hip_rotor_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-hip_rotor_offset_x} ${hip_rotor_offset_y} 0"/>
</xacro:if>
<parent link="trunk"/>
<child link="${name}_hip_rotor"/>
</joint>
<link name="${name}_hip">
<visual>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="${PI} 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<geometry>
<mesh filename="file://$(find go1_description)/meshes/hip.dae" scale="1 1 1"/>
</geometry>
<material name="deep-grey"/>
</visual>
<collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
<geometry>
<cylinder length="${hip_length}" radius="${hip_radius}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
<mass value="${hip_mass}"/>
<inertia
ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
izz="${hip_izz}"/>
</inertial>
</link>
<link name="${name}_hip">
<visual>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="${PI} 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<geometry>
<mesh filename="file://$(find go1_description)/meshes/hip.dae" scale="1 1 1"/>
</geometry>
<material name="deep-grey"/>
</visual>
<collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
<geometry>
<cylinder length="${hip_length}" radius="${hip_radius}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
<mass value="${hip_mass}"/>
<inertia
ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
izz="${hip_izz}"/>
</inertial>
</link>
<link name="${name}_hip_rotor">
<visual>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.035"/>
</geometry>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="${hip_rotor_com_x} ${hip_rotor_com_y} ${hip_rotor_com_z}"/>
<mass value="${hip_rotor_mass}"/>
<inertia
ixx="${hip_rotor_ixx}" ixy="${hip_rotor_ixy}" ixz="${hip_rotor_ixz}"
iyy="${hip_rotor_iyy}" iyz="${hip_rotor_iyz}"
izz="${hip_rotor_izz}"/>
</inertial>
</link>
<link name="${name}_hip_rotor">
<visual>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.035"/>
</geometry>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="${hip_rotor_com_x} ${hip_rotor_com_y} ${hip_rotor_com_z}"/>
<mass value="${hip_rotor_mass}"/>
<inertia
ixx="${hip_rotor_ixx}" ixy="${hip_rotor_ixy}" ixz="${hip_rotor_ixz}"
iyy="${hip_rotor_iyy}" iyz="${hip_rotor_iyz}"
izz="${hip_rotor_izz}"/>
</inertial>
</link>
<joint name="${name}_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
<parent link="${name}_hip"/>
<child link="${name}_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}"
upper="${thigh_position_max}"/>
</joint>
<joint name="${name}_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
<parent link="${name}_hip"/>
<child link="${name}_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}"
upper="${thigh_position_max}"/>
</joint>
<joint name="${name}_thigh_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="${thigh_rotor_offset_x} ${thigh_rotor_offset_y*mirror} 0"/>
<parent link="${name}_hip"/>
<child link="${name}_thigh_rotor"/>
</joint>
<joint name="${name}_thigh_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="${thigh_rotor_offset_x} ${thigh_rotor_offset_y*mirror} 0"/>
<parent link="${name}_hip"/>
<child link="${name}_thigh_rotor"/>
</joint>
<link name="${name}_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<xacro:if value="${mirror_dae == True}">
<mesh filename="file://$(find go1_description)/meshes/thigh.dae" scale="1 1 1"/>
</xacro:if>
<xacro:if value="${mirror_dae == False}">
<mesh filename="file://$(find go1_description)/meshes/thigh_mirror.dae" scale="1 1 1"/>
</xacro:if>
</geometry>
<material name="grey"/>
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
<geometry>
<box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
<mass value="${thigh_mass}"/>
<inertia
ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
izz="${thigh_izz}"/>
</inertial>
</link>
<link name="${name}_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<xacro:if value="${mirror_dae == True}">
<mesh filename="file://$(find go1_description)/meshes/thigh.dae" scale="1 1 1"/>
</xacro:if>
<xacro:if value="${mirror_dae == False}">
<mesh filename="file://$(find go1_description)/meshes/thigh_mirror.dae" scale="1 1 1"/>
</xacro:if>
</geometry>
<material name="grey"/>
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
<geometry>
<box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
<mass value="${thigh_mass}"/>
<inertia
ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
izz="${thigh_izz}"/>
</inertial>
</link>
<link name="${name}_thigh_rotor">
<visual>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.035"/>
</geometry>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="${thigh_rotor_com_x} ${thigh_rotor_com_y} ${thigh_rotor_com_z}"/>
<mass value="${thigh_rotor_mass}"/>
<inertia
ixx="${thigh_rotor_ixx}" ixy="${thigh_rotor_ixy}" ixz="${thigh_rotor_ixz}"
iyy="${thigh_rotor_iyy}" iyz="${thigh_rotor_iyz}"
izz="${thigh_rotor_izz}"/>
</inertial>
</link>
<link name="${name}_thigh_rotor">
<visual>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.035"/>
</geometry>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="${thigh_rotor_com_x} ${thigh_rotor_com_y} ${thigh_rotor_com_z}"/>
<mass value="${thigh_rotor_mass}"/>
<inertia
ixx="${thigh_rotor_ixx}" ixy="${thigh_rotor_ixy}" ixz="${thigh_rotor_ixz}"
iyy="${thigh_rotor_iyy}" iyz="${thigh_rotor_iyz}"
izz="${thigh_rotor_izz}"/>
</inertial>
</link>
<joint name="${name}_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
<parent link="${name}_thigh"/>
<child link="${name}_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}"
upper="${calf_position_max}"/>
</joint>
<joint name="${name}_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
<parent link="${name}_thigh"/>
<child link="${name}_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}"
upper="${calf_position_max}"/>
</joint>
<joint name="${name}_calf_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="${calf_rotor_offset_x} ${calf_rotor_offset_y*mirror} 0"/>
<parent link="${name}_thigh"/>
<child link="${name}_calf_rotor"/>
</joint>
<joint name="${name}_calf_rotor_joint" type="fixed">
<origin rpy="0 0 0" xyz="${calf_rotor_offset_x} ${calf_rotor_offset_y*mirror} 0"/>
<parent link="${name}_thigh"/>
<child link="${name}_calf_rotor"/>
</joint>
<link name="${name}_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://$(find go1_description)/meshes/calf.dae" scale="1 1 1"/>
</geometry>
<material name="light-grey"/>
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
<geometry>
<box size="${calf_length} ${calf_width} ${calf_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
<mass value="${calf_mass}"/>
<inertia
ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
iyy="${calf_iyy}" iyz="${calf_iyz}"
izz="${calf_izz}"/>
</inertial>
</link>
<link name="${name}_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://$(find go1_description)/meshes/calf.dae" scale="1 1 1"/>
</geometry>
<material name="light-grey"/>
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
<geometry>
<box size="${calf_length} ${calf_width} ${calf_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
<mass value="${calf_mass}"/>
<inertia
ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
iyy="${calf_iyy}" iyz="${calf_iyz}"
izz="${calf_izz}"/>
</inertial>
</link>
<link name="${name}_calf_rotor">
<visual>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.035"/>
</geometry>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="${calf_rotor_com_x} ${calf_rotor_com_y} ${calf_rotor_com_z}"/>
<mass value="${calf_rotor_mass}"/>
<inertia
ixx="${calf_rotor_ixx}" ixy="${calf_rotor_ixy}" ixz="${calf_rotor_ixz}"
iyy="${calf_rotor_iyy}" iyz="${calf_rotor_iyz}"
izz="${calf_rotor_izz}"/>
</inertial>
</link>
<link name="${name}_calf_rotor">
<visual>
<origin rpy="${PI/2.0} 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.02" radius="0.035"/>
</geometry>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="${calf_rotor_com_x} ${calf_rotor_com_y} ${calf_rotor_com_z}"/>
<mass value="${calf_rotor_mass}"/>
<inertia
ixx="${calf_rotor_ixx}" ixy="${calf_rotor_ixy}" ixz="${calf_rotor_ixz}"
iyy="${calf_rotor_iyy}" iyz="${calf_rotor_iyz}"
izz="${calf_rotor_izz}"/>
</inertial>
</link>
<joint name="${name}_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
<parent link="${name}_calf"/>
<child link="${name}_foot"/>
</joint>
<joint name="${name}_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
<parent link="${name}_calf"/>
<child link="${name}_foot"/>
</joint>
<link name="${name}_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius-0.01}"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius}"/>
</geometry>
</collision>
<inertial>
<mass value="${foot_mass}"/>
<inertia
ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
</inertial>
</link>
<link name="${name}_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius-0.01}"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius}"/>
</geometry>
</collision>
<inertial>
<mass value="${foot_mass}"/>
<inertia
ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
</inertial>
</link>
<gazebo reference="{name}_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<gazebo reference="{name}_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<gazebo reference="{name}_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>0</self_collide>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="{name}_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>0</self_collide>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="${name}_calf">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="${name}_calf">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="{name}_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<gazebo reference="{name}_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="1.0"/>
</gazebo>
<xacro:leg_transmission name="${name}"/>
</xacro:macro>
<xacro:leg_transmission name="${name}"/>
</xacro:macro>
</robot>

10
descriptions/unitree/go2_description/config/robot_control.yaml
View File

@ -125,6 +125,16 @@ ocs2_quadruped_controller:
- linear_acceleration.y
- linear_acceleration.z
use_odom: True
odom_name: "odometer"
odom_interfaces:
- position.x
- position.y
- position.z
- velocity.x
- velocity.y
- velocity.z
foot_force_name: "foot_force"
foot_force_interfaces:
- FL

9
descriptions/unitree/go2_description/xacro/ros2_control.xacro
View File

@ -171,6 +171,15 @@
<state_interface name="RL"/>
</sensor>
<sensor name="odometer">
<state_interface name="position.x"/>
<state_interface name="position.y"/>
<state_interface name="position.z"/>
<state_interface name="velocity.x"/>
<state_interface name="velocity.y"/>
<state_interface name="velocity.z"/>
</sensor>
</ros2_control>
</robot>

2
hardwares/hardware_unitree_mujoco/README.md
View File

@ -4,6 +4,8 @@ This package contains the hardware interface based on [unitree_sdk2](https://git
In theory, it also can communicate with real robot, but it is not tested yet. You can use go2 simulation in [unitree_mujoco](https://github.com/legubiao/unitree_mujoco). In this simulation, I add foot force sensor support.
*[x] **[2025-01-16]** Add odometer states for simulation.
## 1. Interfaces
Required hardware interfaces:

6
hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h
View File

@ -10,6 +10,7 @@
#include <unitree/idl/go2/LowState_.hpp>
#include <unitree/idl/go2/LowCmd_.hpp>
#include <unitree/idl/go2/SportModeState_.hpp>
#include <unitree/robot/channel/channel_publisher.hpp>
#include <unitree/robot/channel/channel_subscriber.hpp>
@ -38,6 +39,7 @@ protected:
std::vector<double> imu_states_;
std::vector<double> foot_force_;
std::vector<double> high_states_;
std::unordered_map<std::string, std::vector<std::string> > joint_interfaces = {
{"position", {}},
@ -50,13 +52,17 @@ protected:
void lowStateMessageHandle(const void *messages);
void highStateMessageHandle(const void *messages);
unitree_go::msg::dds_::LowCmd_ low_cmd_{}; // default init
unitree_go::msg::dds_::LowState_ low_state_{}; // default init
unitree_go::msg::dds_::SportModeState_ high_state_{}; // default init
/*publisher*/
unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> low_cmd_publisher_;
/*subscriber*/
unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lows_tate_subscriber_;
unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::SportModeState_> high_state_subscriber_;
};

48
hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp
View File

@ -8,6 +8,7 @@
#define TOPIC_LOWCMD "rt/lowcmd"
#define TOPIC_LOWSTATE "rt/lowstate"
#define TOPIC_HIGHSTATE "rt/sportmodestate"
using namespace unitree::robot;
using hardware_interface::return_type;
@ -30,6 +31,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa
imu_states_.assign(10, 0);
foot_force_.assign(4, 0);
high_states_.assign(6, 0);
for (const auto &joint: info_.joints) {
for (const auto &interface: joint.state_interfaces) {
@ -38,12 +40,12 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa
}
ChannelFactory::Instance()->Init(1, "lo");
low_cmd_publisher_ =
std::make_shared<ChannelPublisher<unitree_go::msg::dds_::LowCmd_> >(
TOPIC_LOWCMD);
low_cmd_publisher_->InitChannel();
/*create subscriber*/
lows_tate_subscriber_ =
std::make_shared<ChannelSubscriber<unitree_go::msg::dds_::LowState_> >(
TOPIC_LOWSTATE);
@ -54,6 +56,15 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa
1);
initLowCmd();
high_state_subscriber_ =
std::make_shared<ChannelSubscriber<unitree_go::msg::dds_::SportModeState_> >(
TOPIC_HIGHSTATE);
high_state_subscriber_->InitChannel(
[this](auto &&PH1) {
highStateMessageHandle(std::forward<decltype(PH1)>(PH1));
},
1);
return SystemInterface::on_init(info);
}
@ -83,13 +94,25 @@ std::vector<hardware_interface::StateInterface> HardwareUnitree::export_state_in
}
// export foot force sensor state interface
for (uint i = 0; i < info_.sensors[1].state_interfaces.size(); i++) {
state_interfaces.emplace_back(
info_.sensors[1].name, info_.sensors[1].state_interfaces[i].name, &foot_force_[i]);
if (info_.sensors.size() > 1) {
for (uint i = 0; i < info_.sensors[1].state_interfaces.size(); i++) {
state_interfaces.emplace_back(
info_.sensors[1].name, info_.sensors[1].state_interfaces[i].name, &foot_force_[i]);
}
}
// export odometer state interface
if (info_.sensors.size() > 2) {
// export high state interface
for (uint i = 0; i < info_.sensors[2].state_interfaces.size(); i++) {
state_interfaces.emplace_back(
info_.sensors[2].name, info_.sensors[2].state_interfaces[i].name, &high_states_[i]);
}
}
return state_interfaces;
return
state_interfaces;
}
std::vector<hardware_interface::CommandInterface> HardwareUnitree::export_command_interfaces() {
@ -141,6 +164,17 @@ return_type HardwareUnitree::read(const rclcpp::Time & /*time*/, const rclcpp::D
foot_force_[2] = low_state_.foot_force()[2];
foot_force_[3] = low_state_.foot_force()[3];
// high states
high_states_[0] = high_state_.position()[0];
high_states_[1] = high_state_.position()[1];
high_states_[2] = high_state_.position()[2];
high_states_[3] = high_state_.velocity()[0];
high_states_[4] = high_state_.velocity()[1];
high_states_[5] = high_state_.velocity()[2];
// RCLCPP_INFO(get_logger(), "high state: %f %f %f %f %f %f", high_states_[0], high_states_[1], high_states_[2],
// high_states_[3], high_states_[4], high_states_[5]);
return return_type::OK;
}
@ -182,6 +216,10 @@ void HardwareUnitree::lowStateMessageHandle(const void *messages) {
low_state_ = *static_cast<const unitree_go::msg::dds_::LowState_ *>(messages);
}
void HardwareUnitree::highStateMessageHandle(const void *messages) {
high_state_ = *static_cast<const unitree_go::msg::dds_::SportModeState_ *>(messages);
}
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(