add support for ground truth estimator
This commit is contained in:
Huang Zhenbiao
parent
2a6af12ea7
commit
7ed46994e4
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -10,6 +10,9 @@ Tested environment:
|
|||
* Ubuntu 22.04
|
||||
* ROS2 Humble
|
||||
|
||||
*[x] **[2025-01-16]** Add support for ground truth estimator.
|
||||
|
||||
|
||||
[](https://www.bilibili.com/video/BV1UcxieuEmH/)
|
||||
|
||||
## 1. Interfaces
|
||||
|
|
|
|||
|
|
@ -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_;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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_;
|
||||
};
|
||||
}
|
||||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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_;
|
||||
|
|
|
|||
|
|
@ -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,9 +47,15 @@ namespace ocs2::legged_robot {
|
|||
conf.names.push_back(imu_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_);
|
||||
|
||||
// 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() {
|
||||
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_, this->get_node());
|
||||
*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();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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_;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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_;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
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(
|
||||
|
|
|
|||
Loading…
Reference in New Issue