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

ocs2 controller adding

This commit is contained in:
Huang Zhenbiao 2024-09-24 21:50:46 +08:00
parent ad680af0f1
commit bfc5532ce3
11 changed files with 238 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
controllers/ocs2_quadruped_controller/CMakeLists.txt
View File

@ -31,7 +31,7 @@ add_library(${PROJECT_NAME} SHARED
src/Ocs2QuadrupedController.cpp
# src/estimator/FromTopicEstimate.cpp
# src/estimator/LinearKalmanFilter.cpp
src/estimator/LinearKalmanFilter.cpp
src/estimator/StateEstimateBase.cpp
src/wbc/HierarchicalWbc.cpp

2
controllers/ocs2_quadruped_controller/README.md
View File

@ -1,5 +1,7 @@
# OCS2 Quadruped Controller
This is a ros2-control controller based on [legged_control](https://github.com/qiayuanl/legged_control) and [ocs2_ros2](https://github.com/legubiao/ocs2_ros2).
## 2. Build
```bash
cd ~/ros2_ws

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

@ -18,7 +18,8 @@ namespace ocs2::legged_robot {
class KalmanFilterEstimate final : public StateEstimateBase {
public:
KalmanFilterEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics &eeKinematics);
const PinocchioEndEffectorKinematics &eeKinematics,
const rclcpp_lifecycle::LifecycleNode::SharedPtr &node);
vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) override;
@ -49,10 +50,6 @@ namespace ocs2::legged_robot {
vector_t xHat_, ps_, vs_;
// Topic
ros::Subscriber sub_;
realtime_tools::RealtimeBuffer<nav_msgs::Odometry> buffer_;
tf2_ros::Buffer tfBuffer_;
tf2_ros::TransformListener tfListener_;
tf2::Transform world2odom_;
std::string frameOdom_, frameGuess_;
bool topicUpdated_;

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

@ -14,6 +14,7 @@
#include <ocs2_legged_robot/common/Types.h>
#include <ocs2_legged_robot/gait/MotionPhaseDefinition.h>
#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
namespace ocs2::legged_robot {
class StateEstimateBase {
@ -21,7 +22,8 @@ namespace ocs2::legged_robot {
virtual ~StateEstimateBase() = default;
StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics &eeKinematics, rclcpp::Node::SharedPtr node);
const PinocchioEndEffectorKinematics &eeKinematics,
rclcpp_lifecycle::LifecycleNode::SharedPtr node);
virtual void updateJointStates(const vector_t &jointPos, const vector_t &jointVel);
@ -58,7 +60,7 @@ namespace ocs2::legged_robot {
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr posePub_;
rclcpp::Time lastPub_;
rclcpp::Node::SharedPtr node_;
rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
};
template<typename T>

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

@ -3,3 +3,109 @@
//
#include "Ocs2QuadrupedController.h"
#include <ocs2_core/misc/LoadData.h>
#include <ocs2_core/thread_support/ExecuteAndSleep.h>
#include <ocs2_core/thread_support/SetThreadPriority.h>
#include <ocs2_legged_robot_ros/gait/GaitReceiver.h>
#include <ocs2_quadruped_controller/estimator/LinearKalmanFilter.h>
#include <ocs2_quadruped_controller/wbc/WeightedWbc.h>
#include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h>
#include <ocs2_sqp/SqpMpc.h>
namespace ocs2::legged_robot {
controller_interface::CallbackReturn Ocs2QuadrupedController::on_init() {
verbose_ = false;
loadData::loadCppDataType(task_file_, "legged_robot_interface.verbose", verbose_);
setUpLeggedInterface();
setupMpc();
setupMrt();
// Visualization
CentroidalModelPinocchioMapping pinocchioMapping(legged_interface_->getCentroidalModelInfo());
eeKinematicsPtr_ = std::make_shared<PinocchioEndEffectorKinematics>(
legged_interface_->getPinocchioInterface(), pinocchioMapping,
legged_interface_->modelSettings().contactNames3DoF);
// robotVisualizer_ = std::make_shared<LeggedRobotVisualizer>(leggedInterface_->getPinocchioInterface(),
// leggedInterface_->getCentroidalModelInfo(), *eeKinematicsPtr_, nh);
// selfCollisionVisualization_.reset(new LeggedSelfCollisionVisualization(leggedInterface_->getPinocchioInterface(),
// leggedInterface_->getGeometryInterface(), pinocchioMapping, nh));
// State estimation
setupStateEstimate();
// Whole body control
wbc_ = std::make_shared<WeightedWbc>(legged_interface_->getPinocchioInterface(),
legged_interface_->getCentroidalModelInfo(),
*eeKinematicsPtr_);
wbc_->loadTasksSetting(task_file_, verbose_);
// Safety Checker
safetyChecker_ = std::make_shared<SafetyChecker>(legged_interface_->getCentroidalModelInfo());
return CallbackReturn::SUCCESS;
}
void Ocs2QuadrupedController::setUpLeggedInterface() {
legged_interface_ = std::make_shared<LeggedInterface>(task_file_, urdf_file_, reference_file_);
legged_interface_->setupOptimalControlProblem(task_file_, urdf_file_, reference_file_, verbose_);
}
void Ocs2QuadrupedController::setupMpc() {
mpc_ = std::make_shared<SqpMpc>(legged_interface_->mpcSettings(), legged_interface_->sqpSettings(),
legged_interface_->getOptimalControlProblem(),
legged_interface_->getInitializer());
rbdConversions_ = std::make_shared<CentroidalModelRbdConversions>(legged_interface_->getPinocchioInterface(),
legged_interface_->getCentroidalModelInfo());
const std::string robotName = "legged_robot";
// Gait receiver
auto gaitReceiverPtr =
std::make_shared<GaitReceiver>(this->get_node(),
legged_interface_->getSwitchedModelReferenceManagerPtr()->
getGaitSchedule(), robotName);
// ROS ReferenceManager
auto rosReferenceManagerPtr = std::make_shared<RosReferenceManager>(
robotName, legged_interface_->getReferenceManagerPtr());
rosReferenceManagerPtr->subscribe(this->get_node());
mpc_->getSolverPtr()->addSynchronizedModule(gaitReceiverPtr);
mpc_->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);
observationPublisher_ = nh.advertise<ocs2_msgs::msg::mpc_observation>(robotName + "_mpc_observation", 1);
}
void Ocs2QuadrupedController::setupMrt() {
mpcMrtInterface_ = std::make_shared<MPC_MRT_Interface>(*mpc_);
mpcMrtInterface_->initRollout(&legged_interface_->getRollout());
mpcTimer_.reset();
controllerRunning_ = true;
mpcThread_ = std::thread([&]() {
while (controllerRunning_) {
try {
executeAndSleep(
[&]() {
if (mpcRunning_) {
mpcTimer_.startTimer();
mpcMrtInterface_->advanceMpc();
mpcTimer_.endTimer();
}
},
legged_interface_->mpcSettings().mpcDesiredFrequency_);
} catch (const std::exception &e) {
controllerRunning_ = false;
RCLCPP_WARN(get_node()->get_logger(), "[Ocs2 MPC thread] Error : %s", e.what());
}
}
});
setThreadPriority(legged_interface_->sqpSettings().threadPriority, mpcThread_);
}
void Ocs2QuadrupedController::setupStateEstimate() {
stateEstimate_ = std::make_shared<KalmanFilterEstimate>(legged_interface_->getPinocchioInterface(),
legged_interface_->getCentroidalModelInfo(),
*eeKinematicsPtr_, this->get_node());
dynamic_cast<KalmanFilterEstimate &>(*stateEstimate_).loadSettings(task_file_, verbose_);
currentObservation_.time = 0;
}
}

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

@ -4,54 +4,94 @@
#ifndef OCS2QUADRUPEDCONTROLLER_H
#define OCS2QUADRUPEDCONTROLLER_H
#include "SafetyChecker.h"
#include <controller_interface/controller_interface.hpp>
#include <ocs2_quadruped_controller/interface/LeggedInterface.h>
#include <ocs2_mpc/MPC_MRT_Interface.h>
#include <ocs2_centroidal_model/CentroidalModelRbdConversions.h>
#include <ocs2_quadruped_controller/estimator/StateEstimateBase.h>
#include <ocs2_quadruped_controller/wbc/WbcBase.h>
namespace ocs2::legged_robot {
class Ocs2QuadrupedController final : public controller_interface::ControllerInterface {
public:
CONTROLLER_INTERFACE_PUBLIC
Ocs2QuadrupedController() = default;
class Ocs2QuadrupedController final : public controller_interface::ControllerInterface {
CONTROLLER_INTERFACE_PUBLIC
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
public:
CONTROLLER_INTERFACE_PUBLIC
Ocs2QuadrupedController() = default;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::return_type update(
const rclcpp::Time &time, const rclcpp::Duration &period) override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_init() override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::return_type update(
const rclcpp::Time &time, const rclcpp::Duration &period) override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State &previous_state) override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_init() override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State &previous_state) override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State &previous_state) override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State &previous_state) override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State &previous_state) override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_cleanup(
const rclcpp_lifecycle::State &previous_state) override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State &previous_state) override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_shutdown(
const rclcpp_lifecycle::State &previous_state) override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_cleanup(
const rclcpp_lifecycle::State &previous_state) override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_error(
const rclcpp_lifecycle::State &previous_state) override;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_shutdown(
const rclcpp_lifecycle::State &previous_state) override;
protected:
void setUpLeggedInterface();
CONTROLLER_INTERFACE_PUBLIC
controller_interface::CallbackReturn on_error(
const rclcpp_lifecycle::State &previous_state) override;
void setupMpc();
};
void setupMrt();
void setupStateEstimate();
SystemObservation currentObservation_;
std::string task_file_;
std::string urdf_file_;
std::string reference_file_;
bool verbose_;
std::shared_ptr<LeggedInterface> legged_interface_;
std::shared_ptr<PinocchioEndEffectorKinematics> eeKinematicsPtr_;
// Whole Body Control
std::shared_ptr<WbcBase> wbc_;
std::shared_ptr<SafetyChecker> safetyChecker_;
// Nonlinear MPC
std::shared_ptr<MPC_BASE> mpc_;
std::shared_ptr<MPC_MRT_Interface> mpcMrtInterface_;
std::shared_ptr<CentroidalModelRbdConversions> rbdConversions_;
std::shared_ptr<StateEstimateBase> stateEstimate_;
private:
std::thread mpcThread_;
std::atomic_bool controllerRunning_{}, mpcRunning_{};
benchmark::RepeatedTimer mpcTimer_;
benchmark::RepeatedTimer wbcTimer_;
};
}
#endif //OCS2QUADRUPEDCONTROLLER_H

34
controllers/ocs2_quadruped_controller/src/SafetyChecker.h Normal file
View File

@ -0,0 +1,34 @@
//
// Created by qiayuan on 2022/7/26.
//
#pragma once
#include <ocs2_centroidal_model/AccessHelperFunctions.h>
#include <ocs2_legged_robot/common/Types.h>
#include <ocs2_mpc/SystemObservation.h>
namespace ocs2::legged_robot {
class SafetyChecker {
public:
explicit SafetyChecker(const CentroidalModelInfo &info) : info_(info) {
}
bool check(const SystemObservation &observation, const vector_t & /*optimized_state*/,
const vector_t & /*optimized_input*/) {
return checkOrientation(observation);
}
protected:
bool checkOrientation(const SystemObservation &observation) {
vector_t pose = centroidal_model::getBasePose(observation.state, info_);
if (pose(5) > M_PI_2 || pose(5) < -M_PI_2) {
std::cerr << "[SafetyChecker] Orientation safety check failed!" << std::endl;
return false;
}
return true;
}
const CentroidalModelInfo &info_;
};
} // namespace legged

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

@ -4,6 +4,7 @@
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <utility>
#include <ocs2_quadruped_controller/estimator/LinearKalmanFilter.h>
#include <ocs2_core/misc/LoadData.h>
@ -13,13 +14,13 @@
namespace ocs2::legged_robot {
KalmanFilterEstimate::KalmanFilterEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics &eeKinematics)
: StateEstimateBase(std::move(pinocchioInterface), std::move(info), eeKinematics),
const PinocchioEndEffectorKinematics &eeKinematics,
const rclcpp_lifecycle::LifecycleNode::SharedPtr &node)
: StateEstimateBase(std::move(pinocchioInterface), std::move(info), eeKinematics, std::move(node)),
numContacts_(info_.numThreeDofContacts + info_.numSixDofContacts),
dimContacts_(3 * numContacts_),
numState_(6 + dimContacts_),
numObserve_(2 * dimContacts_ + numContacts_),
tfListener_(tfBuffer_),
topicUpdated_(false) {
xHat_.setZero(numState_);
ps_.setZero(dimContacts_);
@ -45,12 +46,10 @@ namespace ocs2::legged_robot {
eeKinematics_->setPinocchioInterface(pinocchioInterface_);
world2odom_.setRotation(tf2::Quaternion::getIdentity());
sub_ = ros::NodeHandle().subscribe<nav_msgs::msg::Odometry>("/tracking_camera/odom/sample", 10,
&KalmanFilterEstimate::callback, this);
}
vector_t KalmanFilterEstimate::update(const ros::Time &time, const ros::Duration &period) {
scalar_t dt = period.toSec();
vector_t KalmanFilterEstimate::update(const rclcpp::Time &time, const rclcpp::Duration &period) {
scalar_t dt = period.seconds();
a_.block(0, 3, 3, 3) = dt * matrix3_t::Identity();
b_.block(0, 0, 3, 3) = 0.5 * dt * dt * matrix3_t::Identity();
b_.block(3, 0, 3, 3) = dt * matrix3_t::Identity();
@ -75,8 +74,8 @@ namespace ocs2::legged_robot {
// Only set angular velocity, let linear velocity be zero
vPino.tail(actuatedDofNum) = rbdState_.segment(6 + info_.generalizedCoordinatesNum, actuatedDofNum);
pinocchio::forwardKinematics(model, data, qPino, vPino);
pinocchio::updateFramePlacements(model, data);
forwardKinematics(model, data, qPino, vPino);
updateFramePlacements(model, data);
const auto eePos = eeKinematics_->getPosition(vector_t());
const auto eeVel = eeKinematics_->getVelocity(vector_t(), vector_t());
@ -159,71 +158,6 @@ namespace ocs2::legged_robot {
return rbdState_;
}
void KalmanFilterEstimate::updateFromTopic() {
auto *msg = buffer_.readFromRT();
tf2::Transform world2sensor;
world2sensor.setOrigin(tf2::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y,
msg->pose.pose.position.z));
world2sensor.setRotation(tf2::Quaternion(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y,
msg->pose.pose.orientation.z,
msg->pose.pose.orientation.w));
if (world2odom_.getRotation() == tf2::Quaternion::getIdentity()) // First received
{
tf2::Transform odom2sensor;
try {
geometry_msgs::TransformStamped tf_msg = tfBuffer_.lookupTransform(
"odom", msg->child_frame_id, msg->header.stamp);
tf2::fromMsg(tf_msg.transform, odom2sensor);
} catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what());
return;
}
world2odom_ = world2sensor * odom2sensor.inverse();
}
tf2::Transform base2sensor;
try {
geometry_msgs::TransformStamped tf_msg = tfBuffer_.lookupTransform(
"base", msg->child_frame_id, msg->header.stamp);
tf2::fromMsg(tf_msg.transform, base2sensor);
} catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what());
return;
}
tf2::Transform odom2base = world2odom_.inverse() * world2sensor * base2sensor.inverse();
vector3_t newPos(odom2base.getOrigin().x(), odom2base.getOrigin().y(), odom2base.getOrigin().z());
const auto &model = pinocchioInterface_.getModel();
auto &data = pinocchioInterface_.getData();
vector_t qPino(info_.generalizedCoordinatesNum);
qPino.head<3>() = newPos;
qPino.segment<3>(3) = rbdState_.head<3>();
qPino.tail(info_.actuatedDofNum) = rbdState_.segment(6, info_.actuatedDofNum);
forwardKinematics(model, data, qPino);
updateFramePlacements(model, data);
xHat_.segment<3>(0) = newPos;
for (size_t i = 0; i < numContacts_; ++i) {
xHat_.segment<3>(6 + i * 3) = eeKinematics_->getPosition(vector_t())[i];
xHat_(6 + i * 3 + 2) -= footRadius_;
if (contactFlag_[i]) {
feetHeights_[i] = xHat_(6 + i * 3 + 2);
}
}
auto odom = getOdomMsg();
odom.header = msg->header;
odom.child_frame_id = "base";
publishMsgs(odom);
}
void KalmanFilterEstimate::callback(const nav_msgs::Odometry::ConstPtr &msg) {
buffer_.writeFromNonRT(*msg);
topicUpdated_ = true;
}
nav_msgs::msg::Odometry KalmanFilterEstimate::getOdomMsg() {
nav_msgs::msg::Odometry odom;
odom.pose.pose.position.x = xHat_.segment<3>(0)(0);

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

@ -15,7 +15,7 @@ namespace ocs2::legged_robot {
StateEstimateBase::StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics &eeKinematics,
rclcpp::Node::SharedPtr node)
rclcpp_lifecycle::LifecycleNode::SharedPtr node)
: pinocchioInterface_(std::move(pinocchioInterface)),
info_(std::move(info)),
eeKinematics_(eeKinematics.clone()),

7
descriptions/go1_description/xacro/ros2_control.xacro
View File

@ -164,6 +164,13 @@
<state_interface name="linear_acceleration.z"/>
</sensor>
<sensor name="foot_force">
<state_interface name="FR"/>
<state_interface name="FL"/>
<state_interface name="RR"/>
<state_interface name="RL"/>
</sensor>
</ros2_control>
</robot>

2
hardwares/hardware_unitree_mujoco/README.md
View File

@ -2,7 +2,7 @@
This package contains the hardware interface based on [unitree_sdk2](https://github.com/unitreerobotics/unitree_sdk2) to control the Unitree robot in Mujoco simulator.
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/unitreerobotics/unitree_mujoco).
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.
## 1. Interfaces