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

take FSM out as separate library

This commit is contained in:
Huang Zhenbiao 2025-02-27 21:43:10 +08:00
parent 2b395cd2f3
commit aefd4fb3dd
28 changed files with 537 additions and 506 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
controllers/ocs2_quadruped_controller/CMakeLists.txt
View File

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

107
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/FSM/StateOCS2.h Normal file
View File

@ -0,0 +1,107 @@
//
// Created by tlab-uav on 25-2-27.
//
#ifndef STATEOCS2_H
#define STATEOCS2_H
#include <SafetyChecker.h>
#include <ocs2_centroidal_model/CentroidalModelRbdConversions.h>
#include <ocs2_core/misc/Benchmark.h>
#include <ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h>
#include <ocs2_mpc/SystemObservation.h>
#include <ocs2_msgs/msg/detail/mpc_observation__struct.hpp>
#include <ocs2_quadruped_controller/control/TargetManager.h>
#include <ocs2_quadruped_controller/estimator/StateEstimateBase.h>
#include <ocs2_quadruped_controller/interface/LeggedInterface.h>
#include <ocs2_quadruped_controller/wbc/WbcBase.h>
#include <rclcpp/duration.hpp>
#include <ocs2_ros_interfaces/common/RosMsgConversions.h>
#include "controller_common/FSM/FSMState.h"
namespace ocs2 {
class MPC_MRT_Interface;
class MPC_BASE;
class PinocchioEndEffectorKinematics;
}
struct CtrlComponent;
namespace ocs2::legged_robot {
class StateOCS2 final : public FSMState {
public:
StateOCS2(CtrlInterfaces &ctrl_interfaces,
const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> &node,
const std::string &package_share_directory,
const std::vector<std::string> &joint_names,
const std::vector<std::string> &feet_names,
double default_kp,
double default_kd
);
void enter() override;
void run(const rclcpp::Time &time,
const rclcpp::Duration &period) override;
void exit() override;
FSMStateName checkChange() override;
void setupStateEstimate(const std::string &estimator_type);
private:
void updateStateEstimation(const rclcpp::Duration &period);
void setupLeggedInterface();
void setupMpc();
void setupMrt();
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
rclcpp::Publisher<ocs2_msgs::msg::MpcObservation>::SharedPtr observation_publisher_;
std::shared_ptr<StateEstimateBase> estimator_;
std::shared_ptr<TargetManager> target_manager_;
std::shared_ptr<LeggedRobotVisualizer> visualizer_;
std::shared_ptr<LeggedInterface> legged_interface_;
std::shared_ptr<PinocchioEndEffectorKinematics> eeKinematicsPtr_;
// Whole Body Control
std::shared_ptr<WbcBase> wbc_;
std::shared_ptr<SafetyChecker> safety_checker_;
// Nonlinear MPC
std::shared_ptr<MPC_BASE> mpc_;
std::shared_ptr<MPC_MRT_Interface> mpc_mrt_interface_;
std::shared_ptr<CentroidalModelRbdConversions> rbd_conversions_;
SystemObservation observation_;
vector_t measured_rbd_state_;
std::thread mpc_thread_;
std::atomic_bool controller_running_{}, mpc_running_{};
benchmark::RepeatedTimer mpc_timer_;
benchmark::RepeatedTimer wbc_timer_;
std::string task_file_;
std::string urdf_file_;
std::string reference_file_;
std::string gait_file_;
std::vector<std::string> joint_names_;
std::vector<std::string> feet_names_;
double default_kp_;
double default_kd_;
bool verbose_;
bool mpc_need_updated_;
vector_t optimized_state_, optimized_input_;
};
}
#endif //STATEOCS2_H

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

@ -1,75 +0,0 @@
//
// Created by biao on 24-9-10.
//
#ifndef INTERFACE_H
#define INTERFACE_H
#include <vector>
#include <hardware_interface/loaned_command_interface.hpp>
#include <hardware_interface/loaned_state_interface.hpp>
#include <control_input_msgs/msg/inputs.hpp>
#include <ocs2_mpc/SystemObservation.h>
#include <ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h>
#include "TargetManager.h"
#include "ocs2_quadruped_controller/estimator/StateEstimateBase.h"
struct CtrlComponent {
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_torque_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_position_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_velocity_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_kp_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_kd_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
joint_effort_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
joint_position_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
joint_velocity_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
imu_state_interface_;
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_;
std::shared_ptr<ocs2::legged_robot::LeggedRobotVisualizer> visualizer_;
CtrlComponent() {
}
void clear() {
joint_torque_command_interface_.clear();
joint_position_command_interface_.clear();
joint_velocity_command_interface_.clear();
joint_kd_command_interface_.clear();
joint_kp_command_interface_.clear();
joint_effort_state_interface_.clear();
joint_position_state_interface_.clear();
joint_velocity_state_interface_.clear();
imu_state_interface_.clear();
foot_force_state_interface_.clear();
}
};
#endif //INTERFACE_H

7
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/GaitManager.h
View File

@ -4,15 +4,14 @@
#ifndef GAITMANAGER_H
#define GAITMANAGER_H
#include <controller_common/CtrlInterfaces.h>
#include <ocs2_legged_robot/gait/GaitSchedule.h>
#include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h>
#include "CtrlComponent.h"
namespace ocs2::legged_robot {
class GaitManager final : public SolverSynchronizedModule {
public:
GaitManager(CtrlComponent &ctrl_component,
GaitManager(CtrlInterfaces &ctrl_interfaces,
std::shared_ptr<GaitSchedule> gait_schedule_ptr);
void preSolverRun(scalar_t initTime, scalar_t finalTime,
@ -27,7 +26,7 @@ namespace ocs2::legged_robot {
private:
void getTargetGait();
CtrlComponent &ctrl_component_;
CtrlInterfaces &ctrl_interfaces_;
std::shared_ptr<GaitSchedule> gait_schedule_ptr_;
ModeSequenceTemplate target_gait_;

18
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h
View File

@ -7,27 +7,26 @@
#include <memory>
#include <controller_common/CtrlInterfaces.h>
#include <ocs2_mpc/SystemObservation.h>
#include <ocs2_oc/synchronized_module/ReferenceManagerInterface.h>
struct CtrlComponent;
namespace ocs2::legged_robot {
class TargetManager {
public:
TargetManager(CtrlComponent &ctrl_component,
TargetManager(CtrlInterfaces &ctrl_component,
const std::shared_ptr<ReferenceManagerInterface> &referenceManagerPtr,
const std::string& task_file,
const std::string& reference_file);
const std::string &task_file,
const std::string &reference_file);
~TargetManager() = default;
void update();
void update(SystemObservation &observation);
private:
TargetTrajectories targetPoseToTargetTrajectories(const vector_t &targetPose,
const SystemObservation &observation,
const scalar_t &targetReachingTime) {
const SystemObservation &observation,
const scalar_t &targetReachingTime) {
// desired time trajectory
const scalar_array_t timeTrajectory{observation.time, targetReachingTime};
@ -45,7 +44,8 @@ namespace ocs2::legged_robot {
return {timeTrajectory, stateTrajectory, inputTrajectory};
}
CtrlComponent &ctrl_component_;
CtrlInterfaces &ctrl_component_;
std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_;
vector_t default_joint_state_{};

2
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h
View File

@ -11,7 +11,7 @@ namespace ocs2::legged_robot
class FromOdomTopic final : public StateEstimateBase
{
public:
FromOdomTopic(CentroidalModelInfo info, CtrlComponent& ctrl_component,
FromOdomTopic(CentroidalModelInfo info, CtrlInterfaces& ctrl_component,
const rclcpp_lifecycle::LifecycleNode::SharedPtr& node);
vector_t update(const rclcpp::Time& time, const rclcpp::Duration& period) override;

2
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/GroundTruth.h
View File

@ -11,7 +11,7 @@
namespace ocs2::legged_robot {
class GroundTruth final : public StateEstimateBase {
public:
GroundTruth(CentroidalModelInfo info, CtrlComponent &ctrl_component,
GroundTruth(CentroidalModelInfo info, CtrlInterfaces &ctrl_component,
const rclcpp_lifecycle::LifecycleNode::SharedPtr &node);
vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) override;

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

@ -16,7 +16,7 @@ namespace ocs2::legged_robot {
public:
KalmanFilterEstimate(PinocchioInterface pinocchio_interface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics &ee_kinematics,
CtrlComponent &ctrl_component,
CtrlInterfaces &ctrl_component,
const rclcpp_lifecycle::LifecycleNode::SharedPtr &node);
vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) override;

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

@ -3,6 +3,7 @@
//
#pragma once
#include <controller_common/CtrlInterfaces.h>
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
@ -15,14 +16,12 @@
#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
struct CtrlComponent;
namespace ocs2::legged_robot {
class StateEstimateBase {
public:
virtual ~StateEstimateBase() = default;
StateEstimateBase(CentroidalModelInfo info, CtrlComponent &ctrl_component,
StateEstimateBase(CentroidalModelInfo info, CtrlInterfaces &ctrl_component,
rclcpp_lifecycle::LifecycleNode::SharedPtr node);
virtual void updateJointStates();
@ -44,7 +43,7 @@ namespace ocs2::legged_robot {
void publishMsgs(const nav_msgs::msg::Odometry &odom) const;
CtrlComponent &ctrl_component_;
CtrlInterfaces &ctrl_component_;
CentroidalModelInfo info_;
vector3_t zyx_offset_ = vector3_t::Zero();

1
controllers/ocs2_quadruped_controller/package.xml
View File

@ -11,6 +11,7 @@
<depend>backward_ros</depend>
<depend>controller_interface</depend>
<depend>controller_common</depend>
<depend>pluginlib</depend>
<depend>control_input_msgs</depend>
<depend>qpoases_colcon</depend>

268
controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp Normal file
View File

@ -0,0 +1,268 @@
//
// Created by tlab-uav on 25-2-27.
//
#include "ocs2_quadruped_controller/FSM/StateOCS2.h"
#include <angles/angles.h>
#include <ocs2_core/reference/TargetTrajectories.h>
#include <ocs2_core/thread_support/ExecuteAndSleep.h>
#include <ocs2_core/thread_support/SetThreadPriority.h>
#include <ocs2_ros_interfaces/common/RosMsgConversions.h>
#include <rclcpp/rate.hpp>
#include <ocs2_core/misc/LoadData.h>
#include <ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h>
#include <ocs2_mpc/MPC_MRT_Interface.h>
#include <ocs2_quadruped_controller/control/GaitManager.h>
#include <ocs2_quadruped_controller/control/TargetManager.h>
#include <ocs2_quadruped_controller/estimator/FromOdomTopic.h>
#include <ocs2_quadruped_controller/estimator/GroundTruth.h>
#include <ocs2_quadruped_controller/estimator/LinearKalmanFilter.h>
#include <ocs2_quadruped_controller/wbc/WeightedWbc.h>
#include <ocs2_sqp/SqpMpc.h>
namespace ocs2::legged_robot {
StateOCS2::StateOCS2(CtrlInterfaces &ctrl_interfaces,
const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> &node,
const std::string &package_share_directory,
const std::vector<std::string> &joint_names,
const std::vector<std::string> &feet_names,
double default_kp,
double default_kd)
: FSMState(
FSMStateName::OCS2, "OCS2 State", ctrl_interfaces),
node_(node),
joint_names_(joint_names),
feet_names_(feet_names),
default_kp_(default_kp),
default_kd_(default_kd) {
urdf_file_ = package_share_directory + "/urdf/robot.urdf";
task_file_ = package_share_directory + "/config/ocs2/task.info";
reference_file_ = package_share_directory + "/config/ocs2/reference.info";
gait_file_ = package_share_directory + "/config/ocs2/gait.info";
// Load verbose parameter from the task file
verbose_ = false;
loadData::loadCppDataType(task_file_, "legged_robot_interface.verbose", verbose_);
setupLeggedInterface();
setupMpc();
setupMrt();
// Visualization
CentroidalModelPinocchioMapping pinocchio_mapping(legged_interface_->getCentroidalModelInfo());
eeKinematicsPtr_ = std::make_shared<PinocchioEndEffectorKinematics>(
legged_interface_->getPinocchioInterface(), pinocchio_mapping,
legged_interface_->modelSettings().contactNames3DoF);
visualizer_ = std::make_shared<LeggedRobotVisualizer>(
legged_interface_->getPinocchioInterface(), legged_interface_->getCentroidalModelInfo(), *eeKinematicsPtr_,
node_);
// selfCollisionVisualization_.reset(new LeggedSelfCollisionVisualization(leggedInterface_->getPinocchioInterface(),
// leggedInterface_->getGeometryInterface(), pinocchioMapping, nh));
// Whole body control
wbc_ = std::make_shared<WeightedWbc>(legged_interface_->getPinocchioInterface(),
legged_interface_->getCentroidalModelInfo(),
*eeKinematicsPtr_);
wbc_->loadTasksSetting(task_file_, verbose_);
// Safety Checker
safety_checker_ = std::make_shared<SafetyChecker>(legged_interface_->getCentroidalModelInfo());
observation_publisher_ = node_->create_publisher<ocs2_msgs::msg::MpcObservation>(
"legged_robot_mpc_observation", 10);
}
void StateOCS2::enter() {
if (mpc_running_ == false) {
// Initial state
observation_.state.setZero(
static_cast<long>(legged_interface_->getCentroidalModelInfo().stateDim));
updateStateEstimation(rclcpp::Duration(0, 1 / ctrl_interfaces_.frequency_ * 1000000000));
observation_.input.setZero(
static_cast<long>(legged_interface_->getCentroidalModelInfo().inputDim));
observation_.mode = STANCE;
const TargetTrajectories target_trajectories({observation_.time},
{observation_.state},
{observation_.input});
// Set the first observation and command and wait for optimization to finish
mpc_mrt_interface_->setCurrentObservation(observation_);
mpc_mrt_interface_->getReferenceManager().setTargetTrajectories(target_trajectories);
RCLCPP_INFO(node_->get_logger(), "Waiting for the initial policy ...");
while (!mpc_mrt_interface_->initialPolicyReceived()) {
mpc_mrt_interface_->advanceMpc();
rclcpp::WallRate(legged_interface_->mpcSettings().mrtDesiredFrequency_).sleep();
}
RCLCPP_INFO(node_->get_logger(), "Initial policy has been received.");
mpc_running_ = true;
}
}
void StateOCS2::run(const rclcpp::Time &time,
const rclcpp::Duration &period) {
if (mpc_running_ == false) {
return;
}
// State Estimate
updateStateEstimation(period);
// Compute target trajectory
target_manager_->update(observation_);
// Update the current state of the system
mpc_mrt_interface_->setCurrentObservation(observation_);
// Load the latest MPC policy
mpc_mrt_interface_->updatePolicy();
mpc_need_updated_ = true;
// Evaluate the current policy
size_t planned_mode = 0; // The mode that is active at the time the policy is evaluated at.
mpc_mrt_interface_->evaluatePolicy(observation_.time, observation_.state,
optimized_state_,
optimized_input_, planned_mode);
// Whole body control
observation_.input = optimized_input_;
wbc_timer_.startTimer();
vector_t x = wbc_->update(optimized_state_, optimized_input_, measured_rbd_state_, planned_mode,
period.seconds());
wbc_timer_.endTimer();
vector_t torque = x.tail(12);
vector_t pos_des = centroidal_model::getJointAngles(optimized_state_,
legged_interface_->getCentroidalModelInfo());
vector_t vel_des = centroidal_model::getJointVelocities(optimized_input_,
legged_interface_->getCentroidalModelInfo());
for (int i = 0; i < joint_names_.size(); i++) {
std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(torque(i));
std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(pos_des(i));
std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(vel_des(i));
std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(default_kp_);
std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(default_kd_);
}
// Visualization
visualizer_->update(observation_, mpc_mrt_interface_->getPolicy(),
mpc_mrt_interface_->getCommand());
observation_publisher_->publish(ros_msg_conversions::createObservationMsg(observation_));
}
void StateOCS2::exit() {
mpc_running_ = false;
mpc_thread_.join();
RCLCPP_INFO(node_->get_logger(), "MRT thread stopped.");
}
FSMStateName StateOCS2::checkChange() {
// Safety check, if failed, stop the controller
if (!safety_checker_->check(observation_, optimized_state_, optimized_input_)) {
RCLCPP_ERROR(node_->get_logger(), "[Legged Controller] Safety check failed, stopping the controller.");
for (int i = 0; i < joint_names_.size(); i++) {
std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(0);
std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(0);
std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(0);
std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(0.0);
std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(0.35);
}
return FSMStateName::PASSIVE;
}
return FSMStateName::OCS2;
}
void StateOCS2::setupStateEstimate(const std::string &estimator_type) {
if (estimator_type == "ground_truth") {
estimator_ = std::make_shared<GroundTruth>(legged_interface_->getCentroidalModelInfo(),
ctrl_interfaces_,
node_);
RCLCPP_INFO(node_->get_logger(), "Using Ground Truth Estimator");
} else if (estimator_type == "linear_kalman") {
estimator_ = std::make_shared<KalmanFilterEstimate>(
legged_interface_->getPinocchioInterface(),
legged_interface_->getCentroidalModelInfo(),
*eeKinematicsPtr_, ctrl_interfaces_,
node_);
dynamic_cast<KalmanFilterEstimate &>(*estimator_).loadSettings(task_file_, verbose_);
RCLCPP_INFO(node_->get_logger(), "Using Kalman Filter Estimator");
} else {
estimator_ = std::make_shared<FromOdomTopic>(
legged_interface_->getCentroidalModelInfo(), ctrl_interfaces_, node_);
RCLCPP_INFO(node_->get_logger(), "Using Odom Topic Based Estimator");
}
observation_.time = 0;
}
void StateOCS2::updateStateEstimation(const rclcpp::Duration &period) {
measured_rbd_state_ = estimator_->update(node_->now(), period);
observation_.time += period.seconds();
const scalar_t yaw_last = observation_.state(9);
observation_.state = rbd_conversions_->computeCentroidalStateFromRbdModel(measured_rbd_state_);
observation_.state(9) = yaw_last + angles::shortest_angular_distance(
yaw_last, observation_.state(9));
// ctrl_comp_.observation_.mode = ctrl_comp_.estimator_->getMode();
}
void StateOCS2::setupMrt() {
mpc_mrt_interface_ = std::make_shared<MPC_MRT_Interface>(*mpc_);
mpc_mrt_interface_->initRollout(&legged_interface_->getRollout());
mpc_timer_.reset();
controller_running_ = true;
mpc_thread_ = std::thread([&] {
while (controller_running_) {
try {
executeAndSleep(
[&] {
if (mpc_running_ && mpc_need_updated_) {
mpc_need_updated_ = false;
mpc_timer_.startTimer();
mpc_mrt_interface_->advanceMpc();
mpc_timer_.endTimer();
}
},
legged_interface_->mpcSettings().mpcDesiredFrequency_);
} catch (const std::exception &e) {
controller_running_ = false;
RCLCPP_WARN(node_->get_logger(), "[Ocs2 MPC thread] Error : %s", e.what());
}
}
});
setThreadPriority(legged_interface_->sqpSettings().threadPriority, mpc_thread_);
RCLCPP_INFO(node_->get_logger(), "MRT initialized. MPC thread started.");
}
void StateOCS2::setupLeggedInterface() {
legged_interface_ = std::make_shared<LeggedInterface>(task_file_, urdf_file_, reference_file_);
legged_interface_->setupJointNames(joint_names_, feet_names_);
legged_interface_->setupOptimalControlProblem(task_file_, urdf_file_, reference_file_, verbose_);
}
void StateOCS2::setupMpc() {
mpc_ = std::make_shared<SqpMpc>(legged_interface_->mpcSettings(), legged_interface_->sqpSettings(),
legged_interface_->getOptimalControlProblem(),
legged_interface_->getInitializer());
rbd_conversions_ = std::make_shared<CentroidalModelRbdConversions>(legged_interface_->getPinocchioInterface(),
legged_interface_->getCentroidalModelInfo());
// Initialize the reference manager
const auto gait_manager_ptr = std::make_shared<GaitManager>(
ctrl_interfaces_, legged_interface_->getSwitchedModelReferenceManagerPtr()->
getGaitSchedule());
gait_manager_ptr->init(gait_file_);
mpc_->getSolverPtr()->addSynchronizedModule(gait_manager_ptr);
mpc_->getSolverPtr()->setReferenceManager(legged_interface_->getReferenceManagerPtr());
target_manager_ = std::make_shared<TargetManager>(ctrl_interfaces_,
legged_interface_->getReferenceManagerPtr(),
task_file_, reference_file_);
}
}

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

@ -6,25 +6,16 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#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_ros_interfaces/common/RosMsgConversions.h>
#include <ocs2_sqp/SqpMpc.h>
#include <angles/angles.h>
#include <ocs2_quadruped_controller/control/GaitManager.h>
#include <ocs2_quadruped_controller/estimator/GroundTruth.h>
#include <ocs2_quadruped_controller/estimator/FromOdomTopic.h>
namespace ocs2::legged_robot
{
namespace ocs2::legged_robot {
using config_type = controller_interface::interface_configuration_type;
controller_interface::InterfaceConfiguration Ocs2QuadrupedController::command_interface_configuration() const
{
controller_interface::InterfaceConfiguration Ocs2QuadrupedController::command_interface_configuration() const {
controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
conf.names.reserve(joint_names_.size() * command_interface_types_.size());
@ -41,35 +32,26 @@ namespace ocs2::legged_robot
return conf;
}
controller_interface::InterfaceConfiguration Ocs2QuadrupedController::state_interface_configuration() const
{
controller_interface::InterfaceConfiguration Ocs2QuadrupedController::state_interface_configuration() const {
controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
conf.names.reserve(joint_names_.size() * state_interface_types_.size());
for (const auto& joint_name : joint_names_)
{
for (const auto& interface_type : state_interface_types_)
{
for (const auto &joint_name: joint_names_) {
for (const auto &interface_type: state_interface_types_) {
conf.names.push_back(joint_name + "/" += interface_type);
}
}
for (const auto& interface_type : imu_interface_types_)
{
for (const auto &interface_type: imu_interface_types_) {
conf.names.push_back(imu_name_ + "/" += interface_type);
}
if (estimator_type_ == "ground_truth")
{
for (const auto& interface_type : odom_interface_types_)
{
if (estimator_type_ == "ground_truth") {
for (const auto &interface_type: odom_interface_types_) {
conf.names.push_back(odom_name_ + "/" += interface_type);
}
}
else if (estimator_type_ == "linear_kalman")
{
for (const auto& interface_type : foot_force_interface_types_)
{
} else if (estimator_type_ == "linear_kalman") {
for (const auto &interface_type: foot_force_interface_types_) {
conf.names.push_back(foot_force_name_ + "/" += interface_type);
}
}
@ -77,375 +59,156 @@ namespace ocs2::legged_robot
return conf;
}
controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time& time,
const rclcpp::Duration& period)
{
// State Estimate
updateStateEstimation(period);
// Compute target trajectory
ctrl_comp_.target_manager_->update();
// Update the current state of the system
mpc_mrt_interface_->setCurrentObservation(ctrl_comp_.observation_);
// Load the latest MPC policy
mpc_mrt_interface_->updatePolicy();
mpc_need_updated_ = true;
// Evaluate the current policy
vector_t optimized_state, optimized_input;
size_t planned_mode = 0; // The mode that is active at the time the policy is evaluated at.
mpc_mrt_interface_->evaluatePolicy(ctrl_comp_.observation_.time, ctrl_comp_.observation_.state, optimized_state,
optimized_input, planned_mode);
// Whole body control
ctrl_comp_.observation_.input = optimized_input;
wbc_timer_.startTimer();
vector_t x = wbc_->update(optimized_state, optimized_input, measured_rbd_state_, planned_mode,
period.seconds());
wbc_timer_.endTimer();
vector_t torque = x.tail(12);
vector_t pos_des = centroidal_model::getJointAngles(optimized_state,
legged_interface_->getCentroidalModelInfo());
vector_t vel_des = centroidal_model::getJointVelocities(optimized_input,
legged_interface_->getCentroidalModelInfo());
// Safety check, if failed, stop the controller
if (!safety_checker_->check(ctrl_comp_.observation_, optimized_state, optimized_input))
{
RCLCPP_ERROR(get_node()->get_logger(), "[Legged Controller] Safety check failed, stopping the controller.");
for (int i = 0; i < joint_names_.size(); i++)
{
ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_position_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(0.0);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(0.35);
controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time &time,
const rclcpp::Duration &period) {
if (mode_ == FSMMode::NORMAL) {
current_state_->run(time, period);
next_state_name_ = current_state_->checkChange();
if (next_state_name_ != current_state_->state_name) {
mode_ = FSMMode::CHANGE;
next_state_ = getNextState(next_state_name_);
RCLCPP_INFO(get_node()->get_logger(), "Switched from %s to %s",
current_state_->state_name_string.c_str(), next_state_->state_name_string.c_str());
}
return controller_interface::return_type::ERROR;
} else if (mode_ == FSMMode::CHANGE) {
current_state_->exit();
current_state_ = next_state_;
current_state_->enter();
mode_ = FSMMode::NORMAL;
}
for (int i = 0; i < joint_names_.size(); i++)
{
ctrl_comp_.joint_torque_command_interface_[i].get().set_value(torque(i));
ctrl_comp_.joint_position_command_interface_[i].get().set_value(pos_des(i));
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(vel_des(i));
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(default_kp_);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(default_kd_);
}
// Visualization
ctrl_comp_.visualizer_->update(ctrl_comp_.observation_, mpc_mrt_interface_->getPolicy(),
mpc_mrt_interface_->getCommand());
observation_publisher_->publish(ros_msg_conversions::createObservationMsg(ctrl_comp_.observation_));
return controller_interface::return_type::OK;
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_init()
{
controller_interface::CallbackReturn Ocs2QuadrupedController::on_init() {
// Initialize OCS2
robot_pkg_ = auto_declare<std::string>("robot_pkg", robot_pkg_);
const std::string package_share_directory = ament_index_cpp::get_package_share_directory(robot_pkg_);
urdf_file_ = package_share_directory + "/urdf/robot.urdf";
task_file_ = package_share_directory + "/config/ocs2/task.info";
reference_file_ = package_share_directory + "/config/ocs2/reference.info";
gait_file_ = package_share_directory + "/config/ocs2/gait.info";
get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_);
// Load verbose parameter from the task file
verbose_ = false;
loadData::loadCppDataType(task_file_, "legged_robot_interface.verbose", verbose_);
get_node()->get_parameter("update_rate", ctrl_interfaces_.frequency_);
RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_interfaces_.frequency_);
// Hardware Parameters
command_prefix_ = auto_declare<std::string>("command_prefix", command_prefix_);
joint_names_ = auto_declare<std::vector<std::string>>("joints", joint_names_);
feet_names_ = auto_declare<std::vector<std::string>>("feet", feet_names_);
joint_names_ = auto_declare<std::vector<std::string> >("joints", joint_names_);
feet_names_ = auto_declare<std::vector<std::string> >("feet", feet_names_);
command_interface_types_ =
auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
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_);
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_);
imu_interface_types_ = auto_declare<std::vector<std::string> >("imu_interfaces", state_interface_types_);
// Odometer Sensor (Ground Truth)
estimator_type_ = auto_declare<std::string>("estimator_type", estimator_type_);
if (estimator_type_ == "ground_truth")
{
if (estimator_type_ == "ground_truth") {
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
{
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_);
auto_declare<std::vector<std::string> >("foot_force_interfaces", state_interface_types_);
}
// PD gains
default_kp_ = auto_declare<double>("default_kp", default_kp_);
default_kd_ = auto_declare<double>("default_kd", default_kd_);
setupLeggedInterface();
setupMpc();
setupMrt();
// Visualization
CentroidalModelPinocchioMapping pinocchio_mapping(legged_interface_->getCentroidalModelInfo());
eeKinematicsPtr_ = std::make_shared<PinocchioEndEffectorKinematics>(
legged_interface_->getPinocchioInterface(), pinocchio_mapping,
legged_interface_->modelSettings().contactNames3DoF);
ctrl_comp_.visualizer_ = std::make_shared<LeggedRobotVisualizer>(
legged_interface_->getPinocchioInterface(), legged_interface_->getCentroidalModelInfo(), *eeKinematicsPtr_,
get_node());
// 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
safety_checker_ = std::make_shared<SafetyChecker>(legged_interface_->getCentroidalModelInfo());
state_list_.passive = std::make_shared<StatePassive>(ctrl_interfaces_);
state_list_.fixedDown = std::make_shared<StateOCS2>(ctrl_interfaces_, get_node(),
package_share_directory, joint_names_, feet_names_,
default_kp_, default_kd_);
state_list_.fixedDown->setupStateEstimate(estimator_type_);
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_configure(
const rclcpp_lifecycle::State& /*previous_state*/)
{
const rclcpp_lifecycle::State & /*previous_state*/) {
control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
"/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg)
{
"/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) {
// Handle message
ctrl_comp_.control_inputs_.command = msg->command;
ctrl_comp_.control_inputs_.lx = msg->lx;
ctrl_comp_.control_inputs_.ly = msg->ly;
ctrl_comp_.control_inputs_.rx = msg->rx;
ctrl_comp_.control_inputs_.ry = msg->ry;
ctrl_interfaces_.control_inputs_.command = msg->command;
ctrl_interfaces_.control_inputs_.lx = msg->lx;
ctrl_interfaces_.control_inputs_.ly = msg->ly;
ctrl_interfaces_.control_inputs_.rx = msg->rx;
ctrl_interfaces_.control_inputs_.ry = msg->ry;
});
observation_publisher_ = get_node()->create_publisher<ocs2_msgs::msg::MpcObservation>(
"legged_robot_mpc_observation", 10);
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_activate(
const rclcpp_lifecycle::State& /*previous_state*/)
{
const rclcpp_lifecycle::State & /*previous_state*/) {
// clear out vectors in case of restart
ctrl_comp_.clear();
ctrl_interfaces_.clear();
// assign command interfaces
for (auto& interface : command_interfaces_)
{
for (auto &interface: command_interfaces_) {
std::string interface_name = interface.get_interface_name();
if (const size_t pos = interface_name.find('/'); pos != std::string::npos)
{
if (const size_t pos = interface_name.find('/'); pos != std::string::npos) {
command_interface_map_[interface_name.substr(pos + 1)]->push_back(interface);
}
else
{
} else {
command_interface_map_[interface_name]->push_back(interface);
}
}
// assign state interfaces
for (auto& interface : state_interfaces_)
{
if (interface.get_prefix_name() == imu_name_)
{
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
{
for (auto &interface: state_interfaces_) {
if (interface.get_prefix_name() == imu_name_) {
ctrl_interfaces_.imu_state_interface_.emplace_back(interface);
} else if (interface.get_prefix_name() == foot_force_name_) {
ctrl_interfaces_.foot_force_state_interface_.emplace_back(interface);
} else if (interface.get_prefix_name() == odom_name_) {
ctrl_interfaces_.odom_state_interface_.emplace_back(interface);
} else {
state_interface_map_[interface.get_interface_name()]->push_back(interface);
}
}
if (mpc_running_ == false)
{
// Initial state
ctrl_comp_.observation_.state.setZero(
static_cast<long>(legged_interface_->getCentroidalModelInfo().stateDim));
updateStateEstimation(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;
const TargetTrajectories target_trajectories({ctrl_comp_.observation_.time},
{ctrl_comp_.observation_.state},
{ctrl_comp_.observation_.input});
// Set the first observation and command and wait for optimization to finish
mpc_mrt_interface_->setCurrentObservation(ctrl_comp_.observation_);
mpc_mrt_interface_->getReferenceManager().setTargetTrajectories(target_trajectories);
RCLCPP_INFO(get_node()->get_logger(), "Waiting for the initial policy ...");
while (!mpc_mrt_interface_->initialPolicyReceived())
{
mpc_mrt_interface_->advanceMpc();
rclcpp::WallRate(legged_interface_->mpcSettings().mrtDesiredFrequency_).sleep();
}
RCLCPP_INFO(get_node()->get_logger(), "Initial policy has been received.");
mpc_running_ = true;
}
current_state_ = state_list_.passive;
current_state_->enter();
next_state_ = current_state_;
next_state_name_ = current_state_->state_name;
mode_ = FSMMode::NORMAL;
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_deactivate(
const rclcpp_lifecycle::State& /*previous_state*/)
{
const rclcpp_lifecycle::State & /*previous_state*/) {
release_interfaces();
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_cleanup(
const rclcpp_lifecycle::State& /*previous_state*/)
{
const rclcpp_lifecycle::State & /*previous_state*/) {
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_shutdown(
const rclcpp_lifecycle::State& /*previous_state*/)
{
const rclcpp_lifecycle::State & /*previous_state*/) {
return CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn Ocs2QuadrupedController::on_error(
const rclcpp_lifecycle::State& /*previous_state*/)
{
const rclcpp_lifecycle::State & /*previous_state*/) {
return CallbackReturn::SUCCESS;
}
void Ocs2QuadrupedController::setupLeggedInterface()
{
legged_interface_ = std::make_shared<LeggedInterface>(task_file_, urdf_file_, reference_file_);
legged_interface_->setupJointNames(joint_names_, feet_names_);
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());
rbd_conversions_ = std::make_shared<CentroidalModelRbdConversions>(legged_interface_->getPinocchioInterface(),
legged_interface_->getCentroidalModelInfo());
// Initialize the reference manager
const auto gait_manager_ptr = std::make_shared<GaitManager>(
ctrl_comp_, legged_interface_->getSwitchedModelReferenceManagerPtr()->
getGaitSchedule());
gait_manager_ptr->init(gait_file_);
mpc_->getSolverPtr()->addSynchronizedModule(gait_manager_ptr);
mpc_->getSolverPtr()->setReferenceManager(legged_interface_->getReferenceManagerPtr());
ctrl_comp_.target_manager_ = std::make_shared<TargetManager>(ctrl_comp_,
legged_interface_->getReferenceManagerPtr(),
task_file_, reference_file_);
}
void Ocs2QuadrupedController::setupMrt()
{
mpc_mrt_interface_ = std::make_shared<MPC_MRT_Interface>(*mpc_);
mpc_mrt_interface_->initRollout(&legged_interface_->getRollout());
mpc_timer_.reset();
controller_running_ = true;
mpc_thread_ = std::thread([&]
{
while (controller_running_)
{
try
{
executeAndSleep(
[&]
{
if (mpc_running_ && mpc_need_updated_)
{
mpc_need_updated_ = false;
mpc_timer_.startTimer();
mpc_mrt_interface_->advanceMpc();
mpc_timer_.endTimer();
}
},
legged_interface_->mpcSettings().mpcDesiredFrequency_);
}
catch (const std::exception& e)
{
controller_running_ = false;
RCLCPP_WARN(get_node()->get_logger(), "[Ocs2 MPC thread] Error : %s", e.what());
}
}
});
setThreadPriority(legged_interface_->sqpSettings().threadPriority, mpc_thread_);
RCLCPP_INFO(get_node()->get_logger(), "MRT initialized. MPC thread started.");
}
void Ocs2QuadrupedController::setupStateEstimate()
{
if (estimator_type_ == "ground_truth")
{
ctrl_comp_.estimator_ = std::make_shared<GroundTruth>(legged_interface_->getCentroidalModelInfo(),
ctrl_comp_,
get_node());
RCLCPP_INFO(get_node()->get_logger(), "Using Ground Truth Estimator");
std::shared_ptr<FSMState> Ocs2QuadrupedController::getNextState(FSMStateName stateName) const {
switch (stateName) {
case FSMStateName::PASSIVE:
return state_list_.passive;
case FSMStateName::FIXEDDOWN:
return state_list_.fixedDown;
default:
return state_list_.invalid;
}
else if (estimator_type_ == "linear_kalman")
{
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");
}
else
{
ctrl_comp_.estimator_ = std::make_shared<FromOdomTopic>(legged_interface_->getCentroidalModelInfo(),ctrl_comp_,get_node());
RCLCPP_INFO(get_node()->get_logger(), "Using Odom Topic Based Estimator");
}
ctrl_comp_.observation_.time = 0;
}
void Ocs2QuadrupedController::updateStateEstimation(const rclcpp::Duration& period)
{
measured_rbd_state_ = ctrl_comp_.estimator_->update(get_node()->now(), period);
ctrl_comp_.observation_.time += period.seconds();
const scalar_t yaw_last = ctrl_comp_.observation_.state(9);
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();
}
}

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

@ -5,18 +5,22 @@
#ifndef OCS2QUADRUPEDCONTROLLER_H
#define OCS2QUADRUPEDCONTROLLER_H
#include <controller_common/FSM/StatePassive.h>
#include <controller_interface/controller_interface.hpp>
#include <control_input_msgs/msg/inputs.hpp>
#include <ocs2_centroidal_model/CentroidalModelRbdConversions.h>
#include <ocs2_mpc/MPC_MRT_Interface.h>
#include <ocs2_quadruped_controller/estimator/StateEstimateBase.h>
#include <ocs2_quadruped_controller/interface/LeggedInterface.h>
#include <ocs2_quadruped_controller/wbc/WbcBase.h>
#include <ocs2_msgs/msg/mpc_observation.hpp>
#include "SafetyChecker.h"
#include "ocs2_quadruped_controller/control/CtrlComponent.h"
#include <ocs2_quadruped_controller/FSM/StateOCS2.h>
namespace ocs2::legged_robot {
struct FSMStateList {
std::shared_ptr<FSMState> invalid;
std::shared_ptr<StatePassive> passive;
std::shared_ptr<StateOCS2> fixedDown;
};
class Ocs2QuadrupedController final : public controller_interface::ControllerInterface {
public:
Ocs2QuadrupedController() = default;
@ -51,17 +55,17 @@ namespace ocs2::legged_robot {
const rclcpp_lifecycle::State &previous_state) override;
protected:
void setupLeggedInterface();
void setupMpc();
std::shared_ptr<FSMState> getNextState(FSMStateName stateName) const;
void setupMrt();
FSMMode mode_ = FSMMode::NORMAL;
std::string state_name_;
FSMStateName next_state_name_ = FSMStateName::INVALID;
FSMStateList state_list_;
std::shared_ptr<FSMState> current_state_;
std::shared_ptr<FSMState> next_state_;
void setupStateEstimate();
void updateStateEstimation(const rclcpp::Duration &period);
CtrlComponent ctrl_comp_;
CtrlInterfaces ctrl_interfaces_;
std::vector<std::string> joint_names_;
std::vector<std::string> feet_names_;
std::vector<std::string> command_interface_types_;
@ -70,18 +74,18 @@ namespace ocs2::legged_robot {
std::unordered_map<
std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> > *>
command_interface_map_ = {
{"effort", &ctrl_comp_.joint_torque_command_interface_},
{"position", &ctrl_comp_.joint_position_command_interface_},
{"velocity", &ctrl_comp_.joint_velocity_command_interface_},
{"kp", &ctrl_comp_.joint_kp_command_interface_},
{"kd", &ctrl_comp_.joint_kd_command_interface_}
{"effort", &ctrl_interfaces_.joint_torque_command_interface_},
{"position", &ctrl_interfaces_.joint_position_command_interface_},
{"velocity", &ctrl_interfaces_.joint_velocity_command_interface_},
{"kp", &ctrl_interfaces_.joint_kp_command_interface_},
{"kd", &ctrl_interfaces_.joint_kd_command_interface_}
};
std::unordered_map<
std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> > *>
state_interface_map_ = {
{"position", &ctrl_comp_.joint_position_state_interface_},
{"effort", &ctrl_comp_.joint_effort_state_interface_},
{"velocity", &ctrl_comp_.joint_velocity_state_interface_}
{"position", &ctrl_interfaces_.joint_position_state_interface_},
{"effort", &ctrl_interfaces_.joint_effort_state_interface_},
{"velocity", &ctrl_interfaces_.joint_velocity_state_interface_}
};
std::string robot_pkg_;
@ -104,35 +108,6 @@ namespace ocs2::legged_robot {
double default_kd_ = 6;
rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_;
rclcpp::Publisher<ocs2_msgs::msg::MpcObservation>::SharedPtr observation_publisher_;
std::string task_file_;
std::string urdf_file_;
std::string reference_file_;
std::string gait_file_;
bool verbose_;
bool mpc_need_updated_;
std::shared_ptr<LeggedInterface> legged_interface_;
std::shared_ptr<PinocchioEndEffectorKinematics> eeKinematicsPtr_;
// Whole Body Control
std::shared_ptr<WbcBase> wbc_;
std::shared_ptr<SafetyChecker> safety_checker_;
// Nonlinear MPC
std::shared_ptr<MPC_BASE> mpc_;
std::shared_ptr<MPC_MRT_Interface> mpc_mrt_interface_;
std::shared_ptr<CentroidalModelRbdConversions> rbd_conversions_;
private:
vector_t measured_rbd_state_;
std::thread mpc_thread_;
std::atomic_bool controller_running_{}, mpc_running_{};
benchmark::RepeatedTimer mpc_timer_;
benchmark::RepeatedTimer wbc_timer_;
};
}

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

@ -10,9 +10,9 @@
namespace ocs2::legged_robot
{
GaitManager::GaitManager(CtrlComponent& ctrl_component,
GaitManager::GaitManager(CtrlInterfaces& ctrl_interfaces,
std::shared_ptr<GaitSchedule> gait_schedule_ptr)
: ctrl_component_(ctrl_component),
: ctrl_interfaces_(ctrl_interfaces),
gait_schedule_ptr_(std::move(gait_schedule_ptr)),
target_gait_({0.0, 1.0}, {STANCE})
{
@ -48,13 +48,12 @@ namespace ocs2::legged_robot
void GaitManager::getTargetGait()
{
if (ctrl_component_.control_inputs_.command == 0) return;
if (ctrl_component_.control_inputs_.command == last_command_) return;
last_command_ = ctrl_component_.control_inputs_.command;
target_gait_ = gait_list_[ctrl_component_.control_inputs_.command - 1];
if (ctrl_interfaces_.control_inputs_.command == 0) return;
if (ctrl_interfaces_.control_inputs_.command == last_command_) return;
last_command_ = ctrl_interfaces_.control_inputs_.command;
target_gait_ = gait_list_[ctrl_interfaces_.control_inputs_.command -2];
RCLCPP_INFO(rclcpp::get_logger("GaitManager"), "Switch to gait: %s",
gait_name_list_[ctrl_component_.control_inputs_.command - 1].c_str());
gait_name_list_[ctrl_interfaces_.control_inputs_.command - 2].c_str());
gait_updated_ = true;
ctrl_component_.reset = false;
}
}

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

@ -7,11 +7,9 @@
#include <ocs2_core/misc/LoadData.h>
#include <ocs2_robotic_tools/common/RotationTransforms.h>
#include "ocs2_quadruped_controller/control/CtrlComponent.h"
namespace ocs2::legged_robot
{
TargetManager::TargetManager(CtrlComponent& ctrl_component,
TargetManager::TargetManager(CtrlInterfaces& ctrl_component,
const std::shared_ptr<ReferenceManagerInterface>& referenceManagerPtr,
const std::string& task_file,
const std::string& reference_file)
@ -26,16 +24,15 @@ namespace ocs2::legged_robot
loadData::loadCppDataType(reference_file, "targetDisplacementVelocity", target_displacement_velocity_);
}
void TargetManager::update()
void TargetManager::update(SystemObservation &observation)
{
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_;
cmdGoal[2] = ctrl_component_.control_inputs_.ry;
cmdGoal[3] = -ctrl_component_.control_inputs_.rx * target_rotation_velocity_;
const vector_t currentPose = ctrl_component_.observation_.state.segment<6>(6);
const vector_t currentPose = observation.state.segment<6>(6);
const Eigen::Matrix<scalar_t, 3, 1> zyx = currentPose.tail(3);
vector_t cmd_vel_rot = getRotationMatrixFromZyxEulerAngles(zyx) * cmdGoal.head(3);
@ -51,9 +48,9 @@ namespace ocs2::legged_robot
return target;
}();
const scalar_t targetReachingTime = ctrl_component_.observation_.time + time_to_target_;
const scalar_t targetReachingTime = observation.time + time_to_target_;
auto trajectories =
targetPoseToTargetTrajectories(targetPose, ctrl_component_.observation_, targetReachingTime);
targetPoseToTargetTrajectories(targetPose, observation, targetReachingTime);
trajectories.stateTrajectory[0].head(3) = cmd_vel_rot;
trajectories.stateTrajectory[1].head(3) = cmd_vel_rot;

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

@ -5,7 +5,7 @@
#include "ocs2_quadruped_controller/estimator/FromOdomTopic.h"
namespace ocs2::legged_robot {
FromOdomTopic::FromOdomTopic(CentroidalModelInfo info, CtrlComponent &ctrl_component,
FromOdomTopic::FromOdomTopic(CentroidalModelInfo info, CtrlInterfaces &ctrl_component,
const rclcpp_lifecycle::LifecycleNode::SharedPtr &node) : StateEstimateBase(
std::move(info), ctrl_component,
node) {

4
controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp
View File

@ -4,11 +4,9 @@
#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,
GroundTruth::GroundTruth(CentroidalModelInfo info, CtrlInterfaces& ctrl_component,
const rclcpp_lifecycle::LifecycleNode::SharedPtr& node)
: StateEstimateBase(
std::move(info), ctrl_component,

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

@ -16,7 +16,7 @@ namespace ocs2::legged_robot {
KalmanFilterEstimate::KalmanFilterEstimate(PinocchioInterface pinocchio_interface,
CentroidalModelInfo info,
const PinocchioEndEffectorKinematics &ee_kinematics,
CtrlComponent &ctrl_component,
CtrlInterfaces &ctrl_component,
const rclcpp_lifecycle::LifecycleNode::SharedPtr &node)
: StateEstimateBase(std::move(info), ctrl_component,
node),

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

@ -9,11 +9,10 @@
#include <ocs2_robotic_tools/common/RotationDerivativesTransforms.h>
#include <memory>
#include "ocs2_quadruped_controller/control/CtrlComponent.h"
namespace ocs2::legged_robot {
StateEstimateBase::StateEstimateBase(CentroidalModelInfo info,
CtrlComponent &ctrl_component,
CtrlInterfaces &ctrl_component,
rclcpp_lifecycle::LifecycleNode::SharedPtr node)
: ctrl_component_(ctrl_component),
info_(std::move(info)),

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

@ -125,16 +125,6 @@ ocs2_quadruped_controller:
- linear_acceleration.y
- linear_acceleration.z
estimator_type: "ground_truth"
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
libraries/controller_common/include/controller_common/CtrlInterfaces.h
View File

@ -33,6 +33,13 @@ struct CtrlInterfaces {
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
imu_state_interface_;
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_;
int frequency_{};
@ -50,6 +57,8 @@ struct CtrlInterfaces {
joint_velocity_state_interface_.clear();
imu_state_interface_.clear();
imu_state_interface_.clear();
foot_force_state_interface_.clear();
}
};

5
libraries/controller_common/include/controller_common/FSM/BaseFixedStand.h
View File

@ -5,8 +5,6 @@
#ifndef BASEFIXEDSTAND_H
#define BASEFIXEDSTAND_H
#include <rclcpp/time.hpp>
#include "FSMState.h"
class BaseFixedStand : public FSMState {
@ -18,7 +16,8 @@ public:
void enter() override;
void run() override;
void run(const rclcpp::Time &time,
const rclcpp::Duration &period) override;
void exit() override;

4
libraries/controller_common/include/controller_common/FSM/FSMState.h
View File

@ -9,6 +9,7 @@
#include <utility>
#include <controller_common/common/enumClass.h>
#include <controller_common/CtrlInterfaces.h>
#include <rclcpp/time.hpp>
class FSMState {
public:
@ -22,7 +23,8 @@ public:
virtual void enter() = 0;
virtual void run() = 0;
virtual void run(const rclcpp::Time &time,
const rclcpp::Duration &period) = 0;
virtual void exit() = 0;

5
libraries/controller_common/include/controller_common/FSM/StateFixedDown.h
View File

@ -5,8 +5,6 @@
#ifndef STATEFIXEDDOWN_H
#define STATEFIXEDDOWN_H
#include <rclcpp/time.hpp>
#include "FSMState.h"
class StateFixedDown final : public FSMState {
@ -19,7 +17,8 @@ public:
void enter() override;
void run() override;
void run(const rclcpp::Time &time,
const rclcpp::Duration &period) override;
void exit() override;

4
libraries/controller_common/include/controller_common/FSM/StatePassive.h
View File

@ -6,14 +6,14 @@
#define STATEPASSIVE_H
#include "FSMState.h"
class StatePassive final : public FSMState {
public:
explicit StatePassive(CtrlInterfaces &ctrl_interfaces);
void enter() override;
void run() override;
void run(const rclcpp::Time &time,
const rclcpp::Duration &period) override;
void exit() override;

2
libraries/controller_common/src/FSM/BaseFixedStand.cpp
View File

@ -31,7 +31,7 @@ void BaseFixedStand::enter() {
ctrl_interfaces_.control_inputs_.command = 0;
}
void BaseFixedStand::run() {
void BaseFixedStand::run(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) {
percent_ += 1 / duration_;
phase = std::tanh(percent_);
for (int i = 0; i < 12; i++) {

2
libraries/controller_common/src/FSM/StateFixedDown.cpp
View File

@ -32,7 +32,7 @@ void StateFixedDown::enter() {
}
}
void StateFixedDown::run() {
void StateFixedDown::run(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) {
percent_ += 1 / duration_;
phase = std::tanh(percent_);
for (int i = 0; i < 12; i++) {

2
libraries/controller_common/src/FSM/StatePassive.cpp
View File

@ -29,7 +29,7 @@ void StatePassive::enter() {
ctrl_interfaces_.control_inputs_.command = 0;
}
void StatePassive::run() {
void StatePassive::run(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) {
}
void StatePassive::exit() {