diff --git a/controllers/ocs2_quadruped_controller/CMakeLists.txt b/controllers/ocs2_quadruped_controller/CMakeLists.txt index 5d4f59b..43768d1 100644 --- a/controllers/ocs2_quadruped_controller/CMakeLists.txt +++ b/controllers/ocs2_quadruped_controller/CMakeLists.txt @@ -56,6 +56,7 @@ add_library(${PROJECT_NAME} SHARED src/control/GaitManager.cpp src/control/TargetManager.cpp + src/control/CtrlComponent.cpp ) target_include_directories(${PROJECT_NAME} diff --git a/controllers/ocs2_quadruped_controller/README.md b/controllers/ocs2_quadruped_controller/README.md index 26a6819..de57a32 100644 --- a/controllers/ocs2_quadruped_controller/README.md +++ b/controllers/ocs2_quadruped_controller/README.md @@ -10,7 +10,8 @@ Tested environment: * Ubuntu 22.04 * ROS2 Humble -*[x] **[2025-01-16]** Add support for ground truth estimator. +* [x] **[2025-01-16]** Add support for ground truth estimator. +* [x] **[2025-03-15]** OCS2 Controller now can switch between passive and MPC mode. [![](http://i0.hdslb.com/bfs/archive/e758ce019587032449a153cf897a543443b64bba.jpg)](https://www.bilibili.com/video/BV1UcxieuEmH/) diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/FSM/StateOCS2.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/FSM/StateOCS2.h index d5bae79..8c077f5 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/FSM/StateOCS2.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/FSM/StateOCS2.h @@ -8,97 +8,49 @@ #include #include #include -#include -#include -#include -#include -#include -#include +#include #include #include -#include #include "controller_common/FSM/FSMState.h" -namespace ocs2 { +namespace ocs2 +{ class MPC_MRT_Interface; class MPC_BASE; - class PinocchioEndEffectorKinematics; } -struct CtrlComponent; - -namespace ocs2::legged_robot { - class StateOCS2 final : public FSMState { +namespace ocs2::legged_robot +{ + class StateOCS2 final : public FSMState + { public: - StateOCS2(CtrlInterfaces &ctrl_interfaces, - const std::shared_ptr &node, - const std::string &package_share_directory, - const std::vector &joint_names, - const std::vector &feet_names, - double default_kp, - double default_kd + StateOCS2(CtrlInterfaces& ctrl_interfaces, + const std::shared_ptr& ctrl_component ); void enter() override; - void run(const rclcpp::Time &time, - const rclcpp::Duration &period) 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 ctrl_component_; std::shared_ptr node_; - rclcpp::Publisher::SharedPtr observation_publisher_; - std::shared_ptr estimator_; - std::shared_ptr target_manager_; - std::shared_ptr visualizer_; - - std::shared_ptr legged_interface_; - std::shared_ptr eeKinematicsPtr_; // Whole Body Control std::shared_ptr wbc_; std::shared_ptr safety_checker_; - - // Nonlinear MPC - std::shared_ptr mpc_; - std::shared_ptr mpc_mrt_interface_; - - std::shared_ptr 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_; + double default_kp_ = 0; + double default_kd_ = 6; - std::vector joint_names_; - std::vector feet_names_; - double default_kp_; - double default_kd_; - - bool verbose_; - bool mpc_need_updated_; vector_t optimized_state_, optimized_input_; }; } diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h new file mode 100644 index 0000000..3a5d55e --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h @@ -0,0 +1,80 @@ +// +// Created by biao on 3/15/25. +// + +#ifndef CTRLCOMPONENT_H +#define CTRLCOMPONENT_H +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "TargetManager.h" + + +namespace ocs2 +{ + class MPC_BASE; + class MPC_MRT_Interface; + class CentroidalModelRbdConversions; +} + +namespace ocs2::legged_robot +{ + + class CtrlComponent + { + public: + explicit CtrlComponent(const std::shared_ptr& node, + CtrlInterfaces& ctrl_interfaces); + + void setupStateEstimate(const std::string& estimator_type); + void updateState(const rclcpp::Time& time, const rclcpp::Duration& period); + void init(); + + std::shared_ptr node_; + std::shared_ptr legged_interface_; + std::shared_ptr ee_kinematics_; + std::shared_ptr visualizer_; + std::shared_ptr mpc_; + std::shared_ptr mpc_mrt_interface_; + + SystemObservation observation_; + vector_t measured_rbd_state_; + std::atomic_bool mpc_running_{}; + + bool verbose_ = false; + + std::string task_file_; + std::string urdf_file_; + std::string reference_file_; + std::string gait_file_; + + private: + void setupLeggedInterface(); + void setupMpc(); + void setupMrt(); + + CtrlInterfaces& ctrl_interfaces_; + std::shared_ptr estimator_; + std::shared_ptr rbd_conversions_; + std::shared_ptr target_manager_; + + std::vector joint_names_; + std::vector feet_names_; + std::string robot_pkg_; + + // Nonlinear MPC + benchmark::RepeatedTimer mpc_timer_; + std::thread mpc_thread_; + std::atomic_bool controller_running_{}; + }; +} + + +#endif //CTRLCOMPONENT_H diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/GaitManager.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/GaitManager.h index 2e86ad6..2cbdd66 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/GaitManager.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/GaitManager.h @@ -8,25 +8,28 @@ #include #include -namespace ocs2::legged_robot { - class GaitManager final : public SolverSynchronizedModule { +namespace ocs2::legged_robot +{ + class GaitManager final : public SolverSynchronizedModule + { public: - GaitManager(CtrlInterfaces &ctrl_interfaces, + GaitManager(CtrlInterfaces& ctrl_interfaces, std::shared_ptr gait_schedule_ptr); void preSolverRun(scalar_t initTime, scalar_t finalTime, - const vector_t ¤tState, - const ReferenceManagerInterface &referenceManager) override; + const vector_t& currentState, + const ReferenceManagerInterface& referenceManager) override; - void postSolverRun(const PrimalSolution &primalSolution) override { + void postSolverRun(const PrimalSolution&/**primalSolution**/) override + { } - void init(const std::string &gait_file); + void init(const std::string& gait_file); private: void getTargetGait(); - CtrlInterfaces &ctrl_interfaces_; + CtrlInterfaces& ctrl_interfaces_; std::shared_ptr gait_schedule_ptr_; ModeSequenceTemplate target_gait_; diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h index 5fffbfd..4dbb5b6 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h @@ -11,22 +11,25 @@ #include #include -namespace ocs2::legged_robot { - class TargetManager { +namespace ocs2::legged_robot +{ + class TargetManager + { public: - TargetManager(CtrlInterfaces &ctrl_component, - const std::shared_ptr &referenceManagerPtr, - const std::string &task_file, - const std::string &reference_file); + TargetManager(CtrlInterfaces& ctrl_component, + const std::shared_ptr& referenceManagerPtr, + const std::string& task_file, + const std::string& reference_file); ~TargetManager() = default; - void update(SystemObservation &observation); + void update(SystemObservation& observation); private: - TargetTrajectories targetPoseToTargetTrajectories(const vector_t &targetPose, - const SystemObservation &observation, - const scalar_t &targetReachingTime) { + TargetTrajectories targetPoseToTargetTrajectories(const vector_t& targetPose, + const SystemObservation& observation, + const scalar_t& targetReachingTime) + { // desired time trajectory const scalar_array_t timeTrajectory{observation.time, targetReachingTime}; @@ -45,7 +48,7 @@ namespace ocs2::legged_robot { return {timeTrajectory, stateTrajectory, inputTrajectory}; } - CtrlInterfaces &ctrl_component_; + CtrlInterfaces& ctrl_component_; std::shared_ptr referenceManagerPtr_; vector_t default_joint_state_{}; diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h index 2a47aaa..1c98aee 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h @@ -4,7 +4,7 @@ #pragma once #include "StateEstimateBase.h" -#include +#include namespace ocs2::legged_robot { diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h index c6f634f..9c4916c 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h @@ -16,12 +16,14 @@ #include #include -namespace ocs2::legged_robot { - class StateEstimateBase { +namespace ocs2::legged_robot +{ + class StateEstimateBase + { public: virtual ~StateEstimateBase() = default; - StateEstimateBase(CentroidalModelInfo info, CtrlInterfaces &ctrl_component, + StateEstimateBase(CentroidalModelInfo info, CtrlInterfaces& ctrl_component, rclcpp_lifecycle::LifecycleNode::SharedPtr node); virtual void updateJointStates(); @@ -30,25 +32,27 @@ namespace ocs2::legged_robot { virtual void updateImu(); - virtual vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) = 0; + virtual vector_t update(const rclcpp::Time& time, const rclcpp::Duration& period) = 0; - size_t getMode() { return stanceLeg2ModeNumber(contact_flag_); } + [[nodiscard]] size_t getMode() const { return stanceLeg2ModeNumber(contact_flag_); } protected: void initPublishers(); - void updateAngular(const vector3_t &zyx, const vector_t &angularVel); + void updateAngular(const vector3_t& zyx, const vector_t& angularVel); - void updateLinear(const vector_t &pos, const vector_t &linearVel); + void updateLinear(const vector_t& pos, const vector_t& linearVel); - void publishMsgs(const nav_msgs::msg::Odometry &odom) const; + void publishMsgs(const nav_msgs::msg::Odometry& odom) const; - CtrlInterfaces &ctrl_component_; + CtrlInterfaces& ctrl_component_; CentroidalModelInfo info_; + contact_flag_t contact_flag_{}; + double feet_force_threshold_ = 5.0; + vector3_t zyx_offset_ = vector3_t::Zero(); vector_t rbd_state_; - contact_flag_t contact_flag_{}; Eigen::Quaternion quat_; vector3_t angular_vel_local_, linear_accel_local_; matrix3_t orientationCovariance_, angularVelCovariance_, linearAccelCovariance_; @@ -58,13 +62,15 @@ namespace ocs2::legged_robot { rclcpp_lifecycle::LifecycleNode::SharedPtr node_; }; - template - T square(T a) { + template + T square(T a) + { return a * a; } - template - Eigen::Matrix quatToZyx(const Eigen::Quaternion &q) { + template + Eigen::Matrix quatToZyx(const Eigen::Quaternion& q) + { Eigen::Matrix zyx; SCALAR_T as = std::min(-2. * (q.x() * q.z() - q.w() * q.y()), .99999); diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h index 12194f6..a56ce3a 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h @@ -1,11 +1,9 @@ -#pragma once -#pragma clang diagnostic push -#pragma ide diagnostic ignored "misc-non-private-member-variables-in-classes" // // Created by qiayuan on 2022/7/16. // +#ifndef LEGGEDINTERFACE_H +#define LEGGEDINTERFACE_H -#pragma once #include #include @@ -24,85 +22,89 @@ #include "SwitchedModelReferenceManager.h" -namespace ocs2::legged_robot { - class LeggedInterface final : public RobotInterface { +namespace ocs2::legged_robot +{ + class LeggedInterface final : public RobotInterface + { public: - LeggedInterface(const std::string &task_file, - const std::string &urdf_file, - const std::string &reference_file, + LeggedInterface(const std::string& task_file, + const std::string& urdf_file, + const std::string& reference_file, bool use_hard_friction_cone_constraint = false); ~LeggedInterface() override = default; - void setupJointNames(const std::vector &joint_names, - const std::vector &foot_names); + void setupJointNames(const std::vector& joint_names, + const std::vector& foot_names); - void setupOptimalControlProblem(const std::string &task_file, - const std::string &urdf_file, - const std::string &reference_file, + void setupOptimalControlProblem(const std::string& task_file, + const std::string& urdf_file, + const std::string& reference_file, bool verbose); - const OptimalControlProblem &getOptimalControlProblem() const override { return *problem_ptr_; } + const OptimalControlProblem& getOptimalControlProblem() const override { return *problem_ptr_; } - const ModelSettings &modelSettings() const { return model_settings_; } - const ddp::Settings &ddpSettings() const { return ddp_settings_; } - const mpc::Settings &mpcSettings() const { return mpc_settings_; } - const sqp::Settings &sqpSettings() { return sqp_settings_; } + const ModelSettings& modelSettings() const { return model_settings_; } + const ddp::Settings& ddpSettings() const { return ddp_settings_; } + const mpc::Settings& mpcSettings() const { return mpc_settings_; } + const sqp::Settings& sqpSettings() { return sqp_settings_; } - const RolloutBase &getRollout() const { return *rollout_ptr_; } - PinocchioInterface &getPinocchioInterface() { return *pinocchio_interface_ptr_; } - const CentroidalModelInfo &getCentroidalModelInfo() const { return centroidal_model_info_; } + const RolloutBase& getRollout() const { return *rollout_ptr_; } + PinocchioInterface& getPinocchioInterface() { return *pinocchio_interface_ptr_; } + const CentroidalModelInfo& getCentroidalModelInfo() const { return centroidal_model_info_; } - std::shared_ptr getSwitchedModelReferenceManagerPtr() const { + std::shared_ptr getSwitchedModelReferenceManagerPtr() const + { return reference_manager_ptr_; } - const Initializer &getInitializer() const override { return *initializer_ptr_; } + const Initializer& getInitializer() const override { return *initializer_ptr_; } - std::shared_ptr getReferenceManagerPtr() const override { + std::shared_ptr getReferenceManagerPtr() const override + { return reference_manager_ptr_; } protected: - void setupModel(const std::string &task_file, const std::string &urdf_file, - const std::string &reference_file); + void setupModel(const std::string& task_file, const std::string& urdf_file, + const std::string& reference_file); - void setupReferenceManager(const std::string &taskFile, const std::string &urdfFile, - const std::string &referenceFile, + void setupReferenceManager(const std::string& taskFile, const std::string& urdfFile, + const std::string& referenceFile, bool verbose); - void setupPreComputation(const std::string &taskFile, const std::string &urdfFile, - const std::string &referenceFile, + void setupPreComputation(const std::string& taskFile, const std::string& urdfFile, + const std::string& referenceFile, bool verbose); - std::shared_ptr loadGaitSchedule(const std::string &file, bool verbose) const; + std::shared_ptr loadGaitSchedule(const std::string& file, bool verbose) const; - std::unique_ptr getBaseTrackingCost(const std::string &taskFile, - const CentroidalModelInfo &info, bool verbose); + std::unique_ptr getBaseTrackingCost(const std::string& taskFile, + const CentroidalModelInfo& info, bool verbose); - matrix_t initializeInputCostWeight(const std::string &taskFile, const CentroidalModelInfo &info); + matrix_t initializeInputCostWeight(const std::string& taskFile, const CentroidalModelInfo& info); static std::pair loadFrictionConeSettings( - const std::string &taskFile, bool verbose); + const std::string& taskFile, bool verbose); std::unique_ptr getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient); std::unique_ptr getFrictionConeSoftConstraint(size_t contactPointIndex, scalar_t frictionCoefficient, - const RelaxedBarrierPenalty::Config & + const RelaxedBarrierPenalty::Config& barrierPenaltyConfig); - std::unique_ptr > getEeKinematicsPtr(const std::vector &foot_names, - const std::string &model_name); + std::unique_ptr> getEeKinematicsPtr(const std::vector& foot_names, + const std::string& model_name); std::unique_ptr getZeroVelocityConstraint( - const EndEffectorKinematics &end_effector_kinematics, + const EndEffectorKinematics& end_effector_kinematics, size_t contact_point_index); - std::unique_ptr getSelfCollisionConstraint(const PinocchioInterface &pinocchioInterface, - const std::string &taskFile, - const std::string &prefix, bool verbose); + std::unique_ptr getSelfCollisionConstraint(const PinocchioInterface& pinocchioInterface, + const std::string& taskFile, + const std::string& prefix, bool verbose); ModelSettings model_settings_; mpc::Settings mpc_settings_; @@ -125,5 +127,4 @@ namespace ocs2::legged_robot { vector_t initial_state_; }; } // namespace legged - -#pragma clang diagnostic pop +#endif // LEGGEDINTERFACE_H diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HierarchicalWbc.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HierarchicalWbc.h index 2da0308..a640d9d 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HierarchicalWbc.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HierarchicalWbc.h @@ -1,17 +1,21 @@ // // Created by qiayuan on 22-12-23. // -#pragma once +#ifndef HIERARCHICALWBC_H +#define HIERARCHICALWBC_H #include "WbcBase.h" -namespace ocs2::legged_robot { - class HierarchicalWbc final : public WbcBase { +namespace ocs2::legged_robot +{ + class HierarchicalWbc final : public WbcBase + { public: using WbcBase::WbcBase; - vector_t update(const vector_t &stateDesired, const vector_t &inputDesired, const vector_t &rbdStateMeasured, + vector_t update(const vector_t& stateDesired, const vector_t& inputDesired, const vector_t& rbdStateMeasured, size_t mode, scalar_t period) override; }; } // namespace legged +#endif // HIERARCHICALWBC_H diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HoQp.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HoQp.h index 6778724..1d0dc25 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HoQp.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HoQp.h @@ -4,22 +4,25 @@ // // Ref: https://github.com/bernhardpg/quadruped_locomotion // - -#pragma once +#ifndef HOQP_H +#define HOQP_H #include "Task.h" #include -namespace ocs2::legged_robot{ +namespace ocs2::legged_robot +{ // Hierarchical Optimization Quadratic Program - class HoQp { + class HoQp + { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW using HoQpPtr = std::shared_ptr; - explicit HoQp(const Task &task) : HoQp(task, nullptr) { + explicit HoQp(const Task& task) : HoQp(task, nullptr) + { } HoQp(Task task, HoQpPtr higherProblem); @@ -30,7 +33,8 @@ namespace ocs2::legged_robot{ vector_t getStackedSlackSolutions() const { return stackedSlackVars_; } - vector_t getSolutions() const { + vector_t getSolutions() const + { vector_t x = xPrev_ + stackedZPrev_ * decisionVarsSolutions_; return x; } @@ -74,3 +78,4 @@ namespace ocs2::legged_robot{ matrix_t zeroNvNx_; }; } // namespace legged +#endif // HOQP_H diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/Task.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/Task.h index 4080018..ddb22ad 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/Task.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/Task.h @@ -4,38 +4,43 @@ // // Ref: https://github.com/bernhardpg/quadruped_locomotion // - -#pragma once +#ifndef TASK_H +#define TASK_H #include #include -namespace ocs2::legged_robot { - - class Task { +namespace ocs2::legged_robot +{ + class Task + { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW Task() = default; Task(matrix_t a, vector_t b, matrix_t d, vector_t f) : a_(std::move(a)), d_(std::move(d)), b_(std::move(b)), - f_(std::move(f)) { + f_(std::move(f)) + { } explicit Task(size_t numDecisionVars) : Task(matrix_t::Zero(0, numDecisionVars), vector_t::Zero(0), matrix_t::Zero(0, numDecisionVars), - vector_t::Zero(0)) { + vector_t::Zero(0)) + { } - Task operator+(const Task &rhs) const { + Task operator+(const Task& rhs) const + { return { concatenateMatrices(a_, rhs.a_), concatenateVectors(b_, rhs.b_), concatenateMatrices(d_, rhs.d_), concatenateVectors(f_, rhs.f_) }; } - Task operator*(scalar_t rhs) const { + Task operator*(scalar_t rhs) const + { // clang-format off return { a_.cols() > 0 ? rhs * a_ : a_, @@ -48,11 +53,14 @@ namespace ocs2::legged_robot { matrix_t a_, d_; vector_t b_, f_; - static matrix_t concatenateMatrices(matrix_t m1, matrix_t m2) { - if (m1.cols() <= 0) { + static matrix_t concatenateMatrices(matrix_t m1, matrix_t m2) + { + if (m1.cols() <= 0) + { return m2; } - if (m2.cols() <= 0) { + if (m2.cols() <= 0) + { return m1; } assert(m1.cols() == m2.cols()); @@ -61,11 +69,14 @@ namespace ocs2::legged_robot { return res; } - static vector_t concatenateVectors(const vector_t &v1, const vector_t &v2) { - if (v1.cols() <= 0) { + static vector_t concatenateVectors(const vector_t& v1, const vector_t& v2) + { + if (v1.cols() <= 0) + { return v2; } - if (v2.cols() <= 0) { + if (v2.cols() <= 0) + { return v1; } assert(v1.cols() == v2.cols()); @@ -75,3 +86,4 @@ namespace ocs2::legged_robot { } }; } // namespace legged +#endif // TASK_H diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WbcBase.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WbcBase.h index b8b7c98..673a5a9 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WbcBase.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WbcBase.h @@ -1,8 +1,8 @@ // // Created by qiayuan on 2022/7/1. // - -#pragma once +#ifndef WBCBASE_H +#define WBCBASE_H #include "Task.h" @@ -10,28 +10,30 @@ #include #include -namespace ocs2::legged_robot { +namespace ocs2::legged_robot +{ // Decision Variables: x = [\dot u^T, F^T, \tau^T]^T - class WbcBase { + class WbcBase + { using Vector6 = Eigen::Matrix; using Matrix6 = Eigen::Matrix; public: virtual ~WbcBase() = default; - WbcBase(const PinocchioInterface &pinocchioInterface, CentroidalModelInfo info, - const PinocchioEndEffectorKinematics &eeKinematics); + WbcBase(const PinocchioInterface& pinocchioInterface, CentroidalModelInfo info, + const PinocchioEndEffectorKinematics& eeKinematics); - virtual void loadTasksSetting(const std::string &taskFile, bool verbose); + virtual void loadTasksSetting(const std::string& taskFile, bool verbose); - virtual vector_t update(const vector_t &stateDesired, const vector_t &inputDesired, - const vector_t &rbdStateMeasured, size_t mode, + virtual vector_t update(const vector_t& stateDesired, const vector_t& inputDesired, + const vector_t& rbdStateMeasured, size_t mode, scalar_t period); protected: - void updateMeasured(const vector_t &rbdStateMeasured); + void updateMeasured(const vector_t& rbdStateMeasured); - void updateDesired(const vector_t &stateDesired, const vector_t &inputDesired); + void updateDesired(const vector_t& stateDesired, const vector_t& inputDesired); size_t getNumDecisionVars() const { return num_decision_vars_; } @@ -43,11 +45,11 @@ namespace ocs2::legged_robot { Task formulateFrictionConeTask(); - Task formulateBaseAccelTask(const vector_t &stateDesired, const vector_t &inputDesired, scalar_t period); + Task formulateBaseAccelTask(const vector_t& stateDesired, const vector_t& inputDesired, scalar_t period); Task formulateSwingLegTask(); - Task formulateContactForceTask(const vector_t &inputDesired) const; + Task formulateContactForceTask(const vector_t& inputDesired) const; size_t num_decision_vars_; PinocchioInterface pinocchio_interface_measured_, pinocchio_interface_desired_; @@ -66,3 +68,4 @@ namespace ocs2::legged_robot { scalar_t friction_coeff_{}, swing_kp_{}, swing_kd_{}; }; } // namespace legged +#endif // WBCBASE_H diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WeightedWbc.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WeightedWbc.h index 0b23e5f..068d8c7 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WeightedWbc.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WeightedWbc.h @@ -5,22 +5,24 @@ #include "WbcBase.h" -namespace ocs2::legged_robot { - class WeightedWbc : public WbcBase { +namespace ocs2::legged_robot +{ + class WeightedWbc final : public WbcBase + { public: using WbcBase::WbcBase; - vector_t update(const vector_t &stateDesired, const vector_t &inputDesired, const vector_t &rbdStateMeasured, + vector_t update(const vector_t& stateDesired, const vector_t& inputDesired, const vector_t& rbdStateMeasured, size_t mode, scalar_t period) override; - void loadTasksSetting(const std::string &taskFile, bool verbose) override; + void loadTasksSetting(const std::string& taskFile, bool verbose) override; protected: - virtual Task formulateConstraints(); + Task formulateConstraints(); - virtual Task formulateWeightedTasks(const vector_t &stateDesired, const vector_t &inputDesired, - scalar_t period); + Task formulateWeightedTasks(const vector_t& stateDesired, const vector_t& inputDesired, + scalar_t period); private: scalar_t weightSwingLeg_, weightBaseAccel_, weightContactForce_; diff --git a/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py b/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py index edfcef7..5def5ea 100644 --- a/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py +++ b/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py @@ -1,17 +1,15 @@ import os - +import xacro from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription -from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler, TimerAction +from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler, \ + TimerAction +from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import PathJoinSubstitution from launch_ros.actions import Node +from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -from launch.event_handlers import OnProcessExit - -import xacro def launch_setup(context, *args, **kwargs): diff --git a/controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp b/controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp index 1f03474..f60df3c 100644 --- a/controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp +++ b/controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp @@ -5,144 +5,80 @@ #include "ocs2_quadruped_controller/FSM/StateOCS2.h" #include -#include -#include -#include #include -#include #include -#include -#include -#include -#include -#include -#include -#include #include #include -namespace ocs2::legged_robot { - StateOCS2::StateOCS2(CtrlInterfaces &ctrl_interfaces, - const std::shared_ptr &node, - const std::string &package_share_directory, - const std::vector &joint_names, - const std::vector &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( - legged_interface_->getPinocchioInterface(), pinocchio_mapping, - legged_interface_->modelSettings().contactNames3DoF); - - visualizer_ = std::make_shared( - legged_interface_->getPinocchioInterface(), legged_interface_->getCentroidalModelInfo(), *eeKinematicsPtr_, - node_); +namespace ocs2::legged_robot +{ + StateOCS2::StateOCS2(CtrlInterfaces& ctrl_interfaces, + const std::shared_ptr& ctrl_component) + : FSMState(FSMStateName::OCS2, "OCS2 State", ctrl_interfaces), + ctrl_component_(ctrl_component), + node_(ctrl_component->node_) + { + node_->declare_parameter("default_kp", default_kp_); + node_->declare_parameter("default_kd", default_kd_); + default_kp_ = node_->get_parameter("default_kp").as_double(); + default_kd_ = node_->get_parameter("default_kd").as_double(); // selfCollisionVisualization_.reset(new LeggedSelfCollisionVisualization(leggedInterface_->getPinocchioInterface(), // leggedInterface_->getGeometryInterface(), pinocchioMapping, nh)); + // Whole body control - wbc_ = std::make_shared(legged_interface_->getPinocchioInterface(), - legged_interface_->getCentroidalModelInfo(), - *eeKinematicsPtr_); - wbc_->loadTasksSetting(task_file_, verbose_); + wbc_ = std::make_shared(ctrl_component_->legged_interface_->getPinocchioInterface(), + ctrl_component_->legged_interface_->getCentroidalModelInfo(), + *ctrl_component_->ee_kinematics_); + wbc_->loadTasksSetting(ctrl_component_->task_file_, ctrl_component_->verbose_); // Safety Checker - safety_checker_ = std::make_shared(legged_interface_->getCentroidalModelInfo()); - - observation_publisher_ = node_->create_publisher( - "legged_robot_mpc_observation", 10); + safety_checker_ = std::make_shared(ctrl_component_->legged_interface_->getCentroidalModelInfo()); } - void StateOCS2::enter() { - if (mpc_running_ == false) { - // Initial state - observation_.state.setZero( - static_cast(legged_interface_->getCentroidalModelInfo().stateDim)); - updateStateEstimation(rclcpp::Duration(0, 1 / ctrl_interfaces_.frequency_ * 1000000000)); - observation_.input.setZero( - static_cast(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::enter() + { + ctrl_component_->init(); } - void StateOCS2::run(const rclcpp::Time &time, - const rclcpp::Duration &period) { - if (mpc_running_ == false) { + void StateOCS2::run(const rclcpp::Time& /**time**/, + const rclcpp::Duration& period) + { + if (ctrl_component_->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; + ctrl_component_->mpc_mrt_interface_->updatePolicy(); // 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); + ctrl_component_->mpc_mrt_interface_->evaluatePolicy(ctrl_component_->observation_.time, + ctrl_component_->observation_.state, + optimized_state_, + optimized_input_, planned_mode); // Whole body control - observation_.input = optimized_input_; + ctrl_component_->observation_.input = optimized_input_; wbc_timer_.startTimer(); - vector_t x = wbc_->update(optimized_state_, optimized_input_, measured_rbd_state_, planned_mode, + vector_t x = wbc_->update(optimized_state_, optimized_input_, ctrl_component_->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()); + ctrl_component_->legged_interface_-> + getCentroidalModelInfo()); vector_t vel_des = centroidal_model::getJointVelocities(optimized_input_, - legged_interface_->getCentroidalModelInfo()); + ctrl_component_->legged_interface_-> + getCentroidalModelInfo()); - for (int i = 0; i < joint_names_.size(); i++) { + for (int i = 0; i < 12; 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)); @@ -151,118 +87,29 @@ namespace ocs2::legged_robot { } // Visualization - visualizer_->update(observation_, mpc_mrt_interface_->getPolicy(), - mpc_mrt_interface_->getCommand()); - - observation_publisher_->publish(ros_msg_conversions::createObservationMsg(observation_)); + ctrl_component_->visualizer_->update(ctrl_component_->mpc_mrt_interface_->getPolicy(), + ctrl_component_->mpc_mrt_interface_->getCommand()); } - void StateOCS2::exit() { - mpc_running_ = false; - mpc_thread_.join(); - RCLCPP_INFO(node_->get_logger(), "MRT thread stopped."); + void StateOCS2::exit() + { } - FSMStateName StateOCS2::checkChange() { + 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); - } + if (!safety_checker_->check(ctrl_component_->observation_, optimized_state_, optimized_input_)) + { + RCLCPP_ERROR(node_->get_logger(), + "[Legged Controller] Safety check failed, stopping the controller."); return FSMStateName::PASSIVE; } - return FSMStateName::OCS2; - } - - void StateOCS2::setupStateEstimate(const std::string &estimator_type) { - if (estimator_type == "ground_truth") { - estimator_ = std::make_shared(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( - legged_interface_->getPinocchioInterface(), - legged_interface_->getCentroidalModelInfo(), - *eeKinematicsPtr_, ctrl_interfaces_, - node_); - dynamic_cast(*estimator_).loadSettings(task_file_, verbose_); - RCLCPP_INFO(node_->get_logger(), "Using Kalman Filter Estimator"); - } else { - estimator_ = std::make_shared( - legged_interface_->getCentroidalModelInfo(), ctrl_interfaces_, node_); - RCLCPP_INFO(node_->get_logger(), "Using Odom Topic Based Estimator"); + switch (ctrl_interfaces_.control_inputs_.command) + { + case 1: + return FSMStateName::PASSIVE; + default: + return FSMStateName::OCS2; } - 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_); - 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(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(legged_interface_->mpcSettings(), legged_interface_->sqpSettings(), - legged_interface_->getOptimalControlProblem(), - legged_interface_->getInitializer()); - rbd_conversions_ = std::make_shared(legged_interface_->getPinocchioInterface(), - legged_interface_->getCentroidalModelInfo()); - - // Initialize the reference manager - const auto gait_manager_ptr = std::make_shared( - 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(ctrl_interfaces_, - legged_interface_->getReferenceManagerPtr(), - task_file_, reference_file_); } } diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp index f11e760..70332bf 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp @@ -12,18 +12,25 @@ #include #include -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()); - for (const auto &joint_name: joint_names_) { - for (const auto &interface_type: command_interface_types_) { - if (!command_prefix_.empty()) { + for (const auto& joint_name : joint_names_) + { + for (const auto& interface_type : command_interface_types_) + { + if (!command_prefix_.empty()) + { conf.names.push_back(command_prefix_ + "/" + joint_name + "/" += interface_type); - } else { + } + else + { conf.names.push_back(joint_name + "/" += interface_type); } } @@ -32,45 +39,58 @@ 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_) { - conf.names.push_back(foot_force_name_ + "/" += interface_type); - } + } + for (const auto& interface_type : foot_force_interface_types_) + { + conf.names.push_back(foot_force_name_ + "/" += interface_type); } return conf; } - controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time &time, - const rclcpp::Duration &period) { - if (mode_ == FSMMode::NORMAL) { + controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time& time, + const rclcpp::Duration& period) + { + ctrl_comp_->updateState(time, period); + + if (mode_ == FSMMode::NORMAL) + { current_state_->run(time, period); next_state_name_ = current_state_->checkChange(); - if (next_state_name_ != current_state_->state_name) { + 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()); } - } else if (mode_ == FSMMode::CHANGE) { + } + else if (mode_ == FSMMode::CHANGE) + { current_state_->exit(); current_state_ = next_state_; @@ -81,56 +101,50 @@ namespace ocs2::legged_robot { return controller_interface::return_type::OK; } - controller_interface::CallbackReturn Ocs2QuadrupedController::on_init() { - // Initialize OCS2 - robot_pkg_ = auto_declare("robot_pkg", robot_pkg_); - const std::string package_share_directory = ament_index_cpp::get_package_share_directory(robot_pkg_); - + controller_interface::CallbackReturn Ocs2QuadrupedController::on_init() + { 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("command_prefix", command_prefix_); - joint_names_ = auto_declare >("joints", joint_names_); - feet_names_ = auto_declare >("feet", feet_names_); + joint_names_ = auto_declare>("joints", joint_names_); command_interface_types_ = - auto_declare >("command_interfaces", command_interface_types_); + auto_declare>("command_interfaces", command_interface_types_); state_interface_types_ = - auto_declare >("state_interfaces", state_interface_types_); + auto_declare>("state_interfaces", state_interface_types_); // IMU Sensor imu_name_ = auto_declare("imu_name", imu_name_); - imu_interface_types_ = auto_declare >("imu_interfaces", state_interface_types_); + imu_interface_types_ = auto_declare>("imu_interfaces", state_interface_types_); // Odometer Sensor (Ground Truth) estimator_type_ = auto_declare("estimator_type", estimator_type_); - if (estimator_type_ == "ground_truth") { + if (estimator_type_ == "ground_truth") + { odom_name_ = auto_declare("odom_name", odom_name_); - odom_interface_types_ = auto_declare >("odom_interfaces", state_interface_types_); - } else { - // Foot Force Sensor - foot_force_name_ = auto_declare("foot_force_name", foot_force_name_); - foot_force_interface_types_ = - auto_declare >("foot_force_interfaces", state_interface_types_); + odom_interface_types_ = auto_declare>("odom_interfaces", state_interface_types_); } + // Foot Force Sensor + foot_force_name_ = auto_declare("foot_force_name", foot_force_name_); + foot_force_interface_types_ = + auto_declare>("foot_force_interfaces", state_interface_types_); - // PD gains - default_kp_ = auto_declare("default_kp", default_kp_); - default_kd_ = auto_declare("default_kd", default_kd_); + ctrl_comp_ = std::make_shared(get_node(), ctrl_interfaces_); + ctrl_comp_->setupStateEstimate(estimator_type_); state_list_.passive = std::make_shared(ctrl_interfaces_); - state_list_.fixedDown = std::make_shared(ctrl_interfaces_, get_node(), - package_share_directory, joint_names_, feet_names_, - default_kp_, default_kd_); - state_list_.fixedDown->setupStateEstimate(estimator_type_); + state_list_.fixedDown = std::make_shared(ctrl_interfaces_, ctrl_comp_); 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", 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_interfaces_.control_inputs_.command = msg->command; ctrl_interfaces_.control_inputs_.lx = msg->lx; @@ -143,29 +157,42 @@ namespace ocs2::legged_robot { } 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_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_) { + 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_) { + } + 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_) { + } + else if (interface.get_prefix_name() == odom_name_) + { ctrl_interfaces_.odom_state_interface_.emplace_back(interface); - } else { + } + else + { state_interface_map_[interface.get_interface_name()]->push_back(interface); } } @@ -180,34 +207,40 @@ namespace ocs2::legged_robot { } 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; } - std::shared_ptr 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; + std::shared_ptr Ocs2QuadrupedController::getNextState(const FSMStateName stateName) const + { + switch (stateName) + { + case FSMStateName::PASSIVE: + return state_list_.passive; + case FSMStateName::FIXEDDOWN: + return state_list_.fixedDown; + default: + return state_list_.invalid; } } } diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h index 4908101..6d99d0f 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h @@ -8,11 +8,8 @@ #include #include #include -#include -#include -#include -#include #include +#include namespace ocs2::legged_robot { struct FSMStateList { @@ -66,8 +63,8 @@ namespace ocs2::legged_robot { std::shared_ptr next_state_; CtrlInterfaces ctrl_interfaces_; + std::shared_ptr ctrl_comp_; std::vector joint_names_; - std::vector feet_names_; std::vector command_interface_types_; std::vector state_interface_types_; @@ -88,7 +85,6 @@ namespace ocs2::legged_robot { {"velocity", &ctrl_interfaces_.joint_velocity_state_interface_} }; - std::string robot_pkg_; std::string command_prefix_; // IMU Sensor @@ -104,9 +100,6 @@ namespace ocs2::legged_robot { std::string odom_name_; std::vector odom_interface_types_; - double default_kp_ = 0; - double default_kd_ = 6; - rclcpp::Subscription::SharedPtr control_input_subscription_; }; } diff --git a/controllers/ocs2_quadruped_controller/src/control/CtrlComponent.cpp b/controllers/ocs2_quadruped_controller/src/control/CtrlComponent.cpp new file mode 100644 index 0000000..fe1724c --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/control/CtrlComponent.cpp @@ -0,0 +1,205 @@ +// +// Created by biao on 3/15/25. +// + +#include "ocs2_quadruped_controller/control/CtrlComponent.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace ocs2::legged_robot +{ + CtrlComponent::CtrlComponent(const std::shared_ptr& node, + CtrlInterfaces& ctrl_interfaces) : node_(node), ctrl_interfaces_(ctrl_interfaces) + { + node_->declare_parameter("robot_pkg", robot_pkg_); + node_->declare_parameter("feet", feet_names_); + + robot_pkg_ = node_->get_parameter("robot_pkg").as_string(); + joint_names_ = node_->get_parameter("joints").as_string_array(); + feet_names_ = node_->get_parameter("feet").as_string_array(); + + + 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"; + + loadData::loadCppDataType(task_file_, "legged_robot_interface.verbose", verbose_); + + setupLeggedInterface(); + setupMpc(); + setupMrt(); + + CentroidalModelPinocchioMapping pinocchio_mapping(legged_interface_->getCentroidalModelInfo()); + ee_kinematics_ = std::make_shared( + legged_interface_->getPinocchioInterface(), pinocchio_mapping, + legged_interface_->modelSettings().contactNames3DoF); + + rbd_conversions_ = std::make_shared(legged_interface_->getPinocchioInterface(), + legged_interface_->getCentroidalModelInfo()); + + // Init visulaizer + visualizer_ = std::make_shared( + legged_interface_->getPinocchioInterface(), + legged_interface_->getCentroidalModelInfo(), + *ee_kinematics_, + node_); + + // Init observation + observation_.state.setZero(static_cast(legged_interface_->getCentroidalModelInfo().stateDim)); + observation_.input.setZero( + static_cast(legged_interface_->getCentroidalModelInfo().inputDim)); + observation_.mode = STANCE; + } + + void CtrlComponent::setupStateEstimate(const std::string& estimator_type) + { + if (estimator_type == "ground_truth") + { + estimator_ = std::make_shared(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( + legged_interface_->getPinocchioInterface(), + legged_interface_->getCentroidalModelInfo(), + *ee_kinematics_, ctrl_interfaces_, + node_); + dynamic_cast(*estimator_).loadSettings(task_file_, verbose_); + RCLCPP_INFO(node_->get_logger(), "Using Kalman Filter Estimator"); + } + else + { + estimator_ = std::make_shared( + legged_interface_->getCentroidalModelInfo(), ctrl_interfaces_, node_); + RCLCPP_INFO(node_->get_logger(), "Using Odom Topic Based Estimator"); + } + observation_.time = 0; + } + + void CtrlComponent::updateState(const rclcpp::Time& time, const rclcpp::Duration& period) + { + // Update State Estimation + measured_rbd_state_ = estimator_->update(time, 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)); + observation_.mode = estimator_->getMode(); + + visualizer_->update(observation_); + + // Compute target trajectory + target_manager_->update(observation_); + // Update the current state of the system + mpc_mrt_interface_->setCurrentObservation(observation_); + } + + void CtrlComponent::init() + { + if (mpc_running_ == false) + { + 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 CtrlComponent::setupLeggedInterface() + { + legged_interface_ = std::make_shared(task_file_, urdf_file_, reference_file_); + legged_interface_->setupJointNames(joint_names_, feet_names_); + legged_interface_->setupOptimalControlProblem(task_file_, urdf_file_, reference_file_, verbose_); + } + + /** + * Set up the SQP MPC, Gait Manager and Reference Manager + */ + void CtrlComponent::setupMpc() + { + mpc_ = std::make_shared(legged_interface_->mpcSettings(), + legged_interface_->sqpSettings(), + legged_interface_->getOptimalControlProblem(), + legged_interface_->getInitializer()); + + // Initialize the reference manager + const auto gait_manager_ptr = std::make_shared( + 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(ctrl_interfaces_, + legged_interface_->getReferenceManagerPtr(), + task_file_, + reference_file_); + } + + void CtrlComponent::setupMrt() + { + mpc_mrt_interface_ = std::make_shared(*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_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."); + } +} diff --git a/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp b/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp index d160d1b..27d1181 100644 --- a/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp +++ b/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp @@ -19,8 +19,8 @@ namespace ocs2::legged_robot } void GaitManager::preSolverRun(const scalar_t initTime, const scalar_t finalTime, - const vector_t& currentState, - const ReferenceManagerInterface& referenceManager) + const vector_t& /**currentState**/, + const ReferenceManagerInterface& /**referenceManager**/) { getTargetGait(); if (gait_updated_) @@ -51,9 +51,10 @@ namespace ocs2::legged_robot 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]; + const int command = std::max(0, ctrl_interfaces_.control_inputs_.command - 2); + target_gait_ = gait_list_[command]; RCLCPP_INFO(rclcpp::get_logger("GaitManager"), "Switch to gait: %s", - gait_name_list_[ctrl_interfaces_.control_inputs_.command - 2].c_str()); + gait_name_list_[command].c_str()); gait_updated_ = true; } } diff --git a/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp b/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp index 4aae877..974eca9 100644 --- a/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp +++ b/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp @@ -24,7 +24,7 @@ namespace ocs2::legged_robot loadData::loadCppDataType(reference_file, "targetDisplacementVelocity", target_displacement_velocity_); } - void TargetManager::update(SystemObservation &observation) + void TargetManager::update(SystemObservation& observation) { vector_t cmdGoal = vector_t::Zero(6); cmdGoal[0] = ctrl_component_.control_inputs_.ly * target_displacement_velocity_; diff --git a/controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp b/controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp index 361591f..3b565f8 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp @@ -21,15 +21,15 @@ namespace ocs2::legged_robot 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() + ctrl_component_.odom_state_interface_[0].get().get_optional().value(), + ctrl_component_.odom_state_interface_[1].get().get_optional().value(), + ctrl_component_.odom_state_interface_[2].get().get_optional().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() + ctrl_component_.odom_state_interface_[3].get().get_optional().value(), + ctrl_component_.odom_state_interface_[4].get().get_optional().value(), + ctrl_component_.odom_state_interface_[5].get().get_optional().value() }; updateLinear(position_, linear_velocity_); diff --git a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp index 533aa1b..c64c767 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp @@ -9,55 +9,65 @@ #include #include +#include -namespace ocs2::legged_robot { +namespace ocs2::legged_robot +{ StateEstimateBase::StateEstimateBase(CentroidalModelInfo info, - CtrlInterfaces &ctrl_component, + CtrlInterfaces& ctrl_component, rclcpp_lifecycle::LifecycleNode::SharedPtr node) : ctrl_component_(ctrl_component), info_(std::move(info)), - rbd_state_(vector_t::Zero(2 * info_.generalizedCoordinatesNum)), node_(std::move(node)) { - + rbd_state_(vector_t::Zero(2 * info_.generalizedCoordinatesNum)), node_(std::move(node)) + { + node_->declare_parameter("feet_force_threshold", feet_force_threshold_); + feet_force_threshold_ = node_->get_parameter("feet_force_threshold").as_double(); } - void StateEstimateBase::updateJointStates() { + void StateEstimateBase::updateJointStates() + { const size_t size = ctrl_component_.joint_effort_state_interface_.size(); vector_t joint_pos(size), joint_vel(size); - for (int i = 0; i < size; i++) { - joint_pos(i) = ctrl_component_.joint_position_state_interface_[i].get().get_value(); - joint_vel(i) = ctrl_component_.joint_velocity_state_interface_[i].get().get_value(); + for (int i = 0; i < size; i++) + { + joint_pos(i) = ctrl_component_.joint_position_state_interface_[i].get().get_optional().value(); + joint_vel(i) = ctrl_component_.joint_velocity_state_interface_[i].get().get_optional().value(); } rbd_state_.segment(6, info_.actuatedDofNum) = joint_pos; rbd_state_.segment(6 + info_.generalizedCoordinatesNum, info_.actuatedDofNum) = joint_vel; } - void StateEstimateBase::updateContact() { + void StateEstimateBase::updateContact() + { const size_t size = ctrl_component_.foot_force_state_interface_.size(); - for (int i = 0; i < size; i++) { - contact_flag_[i] = ctrl_component_.foot_force_state_interface_[i].get().get_value() > 5.0; + for (int i = 0; i < size; i++) + { + contact_flag_[i] = ctrl_component_.foot_force_state_interface_[i].get().get_optional().value() > + feet_force_threshold_; } } - void StateEstimateBase::updateImu() { + void StateEstimateBase::updateImu() + { quat_ = { - ctrl_component_.imu_state_interface_[0].get().get_value(), - ctrl_component_.imu_state_interface_[1].get().get_value(), - ctrl_component_.imu_state_interface_[2].get().get_value(), - ctrl_component_.imu_state_interface_[3].get().get_value() + ctrl_component_.imu_state_interface_[0].get().get_optional().value(), + ctrl_component_.imu_state_interface_[1].get().get_optional().value(), + ctrl_component_.imu_state_interface_[2].get().get_optional().value(), + ctrl_component_.imu_state_interface_[3].get().get_optional().value() }; angular_vel_local_ = { - ctrl_component_.imu_state_interface_[4].get().get_value(), - ctrl_component_.imu_state_interface_[5].get().get_value(), - ctrl_component_.imu_state_interface_[6].get().get_value() + ctrl_component_.imu_state_interface_[4].get().get_optional().value(), + ctrl_component_.imu_state_interface_[5].get().get_optional().value(), + ctrl_component_.imu_state_interface_[6].get().get_optional().value() }; linear_accel_local_ = { - ctrl_component_.imu_state_interface_[7].get().get_value(), - ctrl_component_.imu_state_interface_[8].get().get_value(), - ctrl_component_.imu_state_interface_[9].get().get_value() + ctrl_component_.imu_state_interface_[7].get().get_optional().value(), + ctrl_component_.imu_state_interface_[8].get().get_optional().value(), + ctrl_component_.imu_state_interface_[9].get().get_optional().value() }; // orientationCovariance_ = orientationCovariance; @@ -77,17 +87,20 @@ namespace ocs2::legged_robot { pose_pub_ = node_->create_publisher("pose", 10); } - void StateEstimateBase::updateAngular(const vector3_t &zyx, const vector_t &angularVel) { + void StateEstimateBase::updateAngular(const vector3_t& zyx, const vector_t& angularVel) + { rbd_state_.segment<3>(0) = zyx; rbd_state_.segment<3>(info_.generalizedCoordinatesNum) = angularVel; } - void StateEstimateBase::updateLinear(const vector_t &pos, const vector_t &linearVel) { + void StateEstimateBase::updateLinear(const vector_t& pos, const vector_t& linearVel) + { rbd_state_.segment<3>(3) = pos; rbd_state_.segment<3>(info_.generalizedCoordinatesNum + 3) = linearVel; } - void StateEstimateBase::publishMsgs(const nav_msgs::msg::Odometry &odom) const { + void StateEstimateBase::publishMsgs(const nav_msgs::msg::Odometry& odom) const + { rclcpp::Time time = odom.header.stamp; odom_pub_->publish(odom); diff --git a/controllers/ocs2_quadruped_controller/src/wbc/HierarchicalWbc.cpp b/controllers/ocs2_quadruped_controller/src/wbc/HierarchicalWbc.cpp index e484c7f..6a97fb2 100644 --- a/controllers/ocs2_quadruped_controller/src/wbc/HierarchicalWbc.cpp +++ b/controllers/ocs2_quadruped_controller/src/wbc/HierarchicalWbc.cpp @@ -5,14 +5,16 @@ #include "ocs2_quadruped_controller/wbc/HierarchicalWbc.h" #include "ocs2_quadruped_controller/wbc/HoQp.h" -namespace ocs2::legged_robot { - vector_t HierarchicalWbc::update(const vector_t &stateDesired, const vector_t &inputDesired, - const vector_t &rbdStateMeasured, size_t mode, - scalar_t period) { +namespace ocs2::legged_robot +{ + vector_t HierarchicalWbc::update(const vector_t& stateDesired, const vector_t& inputDesired, + const vector_t& rbdStateMeasured, size_t mode, + scalar_t period) + { WbcBase::update(stateDesired, inputDesired, rbdStateMeasured, mode, period); Task task0 = formulateFloatingBaseEomTask() + formulateTorqueLimitsTask() + formulateFrictionConeTask() + - formulateNoContactMotionTask(); + formulateNoContactMotionTask(); Task task1 = formulateBaseAccelTask(stateDesired, inputDesired, period) + formulateSwingLegTask(); Task task2 = formulateContactForceTask(inputDesired); HoQp hoQp(task2, std::make_shared(task1, std::make_shared(task0))); diff --git a/controllers/ocs2_quadruped_controller/src/wbc/HoQp.cpp b/controllers/ocs2_quadruped_controller/src/wbc/HoQp.cpp index d5a29df..2161086 100644 --- a/controllers/ocs2_quadruped_controller/src/wbc/HoQp.cpp +++ b/controllers/ocs2_quadruped_controller/src/wbc/HoQp.cpp @@ -9,8 +9,10 @@ #include #include -namespace ocs2::legged_robot { - HoQp::HoQp(Task task, HoQpPtr higherProblem) : task_(std::move(task)), higherProblem_(std::move(higherProblem)) { +namespace ocs2::legged_robot +{ + HoQp::HoQp(Task task, HoQpPtr higherProblem) : task_(std::move(task)), higherProblem_(std::move(higherProblem)) + { initVars(); formulateProblem(); solveProblem(); @@ -19,14 +21,16 @@ namespace ocs2::legged_robot { stackSlackSolutions(); } - void HoQp::initVars() { + void HoQp::initVars() + { // Task variables numSlackVars_ = task_.d_.rows(); hasEqConstraints_ = task_.a_.rows() > 0; hasIneqConstraints_ = numSlackVars_ > 0; // Pre-Task variables - if (higherProblem_ != nullptr) { + if (higherProblem_ != nullptr) + { stackedZPrev_ = higherProblem_->getStackedZMatrix(); stackedTasksPrev_ = higherProblem_->getStackedTasks(); stackedSlackSolutionsPrev_ = higherProblem_->getStackedSlackSolutions(); @@ -34,7 +38,9 @@ namespace ocs2::legged_robot { numPrevSlackVars_ = higherProblem_->getSlackedNumVars(); numDecisionVars_ = stackedZPrev_.cols(); - } else { + } + else + { numDecisionVars_ = std::max(task_.a_.cols(), task_.d_.cols()); stackedTasksPrev_ = Task(numDecisionVars_); @@ -51,53 +57,66 @@ namespace ocs2::legged_robot { zeroNvNx_ = matrix_t::Zero(numSlackVars_, numDecisionVars_); } - void HoQp::formulateProblem() { + void HoQp::formulateProblem() + { buildHMatrix(); buildCVector(); buildDMatrix(); buildFVector(); } - void HoQp::buildHMatrix() { + void HoQp::buildHMatrix() + { matrix_t zTaTaz(numDecisionVars_, numDecisionVars_); - if (hasEqConstraints_) { + if (hasEqConstraints_) + { // Make sure that all eigenvalues of A_t_A are non-negative, which could arise due to numerical issues matrix_t aCurrZPrev = task_.a_ * stackedZPrev_; zTaTaz = aCurrZPrev.transpose() * aCurrZPrev + 1e-12 * matrix_t::Identity( - numDecisionVars_, numDecisionVars_); + numDecisionVars_, numDecisionVars_); // This way of splitting up the multiplication is about twice as fast as multiplying 4 matrices - } else { + } + else + { zTaTaz.setZero(); } h_ = (matrix_t(numDecisionVars_ + numSlackVars_, numDecisionVars_ + numSlackVars_) // clang-format off << zTaTaz, zeroNvNx_.transpose(), zeroNvNx_, eyeNvNv_) // clang-format on - .finished(); + .finished(); } - void HoQp::buildCVector() { + void HoQp::buildCVector() + { vector_t c = vector_t::Zero(numDecisionVars_ + numSlackVars_); vector_t zeroVec = vector_t::Zero(numSlackVars_); vector_t temp(numDecisionVars_); - if (hasEqConstraints_) { + if (hasEqConstraints_) + { temp = (task_.a_ * stackedZPrev_).transpose() * (task_.a_ * xPrev_ - task_.b_); - } else { + } + else + { temp.setZero(); } c_ = (vector_t(numDecisionVars_ + numSlackVars_) << temp, zeroVec).finished(); } - void HoQp::buildDMatrix() { + void HoQp::buildDMatrix() + { matrix_t stackedZero = matrix_t::Zero(numPrevSlackVars_, numSlackVars_); matrix_t dCurrZ; - if (hasIneqConstraints_) { + if (hasIneqConstraints_) + { dCurrZ = task_.d_ * stackedZPrev_; - } else { + } + else + { dCurrZ = matrix_t::Zero(0, numDecisionVars_); } @@ -107,34 +126,43 @@ namespace ocs2::legged_robot { << zeroNvNx_, -eyeNvNv_, stackedTasksPrev_.d_ * stackedZPrev_, stackedZero, dCurrZ, -eyeNvNv_) // clang-format on - .finished(); + .finished(); } - void HoQp::buildFVector() { + void HoQp::buildFVector() + { vector_t zeroVec = vector_t::Zero(numSlackVars_); vector_t fMinusDXPrev; - if (hasIneqConstraints_) { + if (hasIneqConstraints_) + { fMinusDXPrev = task_.f_ - task_.d_ * xPrev_; - } else { + } + else + { fMinusDXPrev = vector_t::Zero(0); } f_ = (vector_t(2 * numSlackVars_ + numPrevSlackVars_) << zeroVec, - stackedTasksPrev_.f_ - stackedTasksPrev_.d_ * xPrev_ + stackedSlackSolutionsPrev_, fMinusDXPrev) - .finished(); + stackedTasksPrev_.f_ - stackedTasksPrev_.d_ * xPrev_ + stackedSlackSolutionsPrev_, fMinusDXPrev) + .finished(); } - void HoQp::buildZMatrix() { - if (hasEqConstraints_) { + void HoQp::buildZMatrix() + { + if (hasEqConstraints_) + { assert((task_.a_.cols() > 0)); stackedZ_ = stackedZPrev_ * (task_.a_ * stackedZPrev_).fullPivLu().kernel(); - } else { + } + else + { stackedZ_ = stackedZPrev_; } } - void HoQp::solveProblem() { + void HoQp::solveProblem() + { auto qpProblem = qpOASES::QProblem(numDecisionVars_ + numSlackVars_, f_.size()); qpOASES::Options options; options.setToMPC(); @@ -151,11 +179,15 @@ namespace ocs2::legged_robot { slackVarsSolutions_ = qpSol.tail(numSlackVars_); } - void HoQp::stackSlackSolutions() { - if (higherProblem_ != nullptr) { + void HoQp::stackSlackSolutions() + { + if (higherProblem_ != nullptr) + { stackedSlackVars_ = Task::concatenateVectors(higherProblem_->getStackedSlackSolutions(), slackVarsSolutions_); - } else { + } + else + { stackedSlackVars_ = slackVarsSolutions_; } } diff --git a/controllers/ocs2_quadruped_controller/src/wbc/WbcBase.cpp b/controllers/ocs2_quadruped_controller/src/wbc/WbcBase.cpp index c3a5ca2..5fa00da 100644 --- a/controllers/ocs2_quadruped_controller/src/wbc/WbcBase.cpp +++ b/controllers/ocs2_quadruped_controller/src/wbc/WbcBase.cpp @@ -14,27 +14,32 @@ #include #include -namespace ocs2::legged_robot { - WbcBase::WbcBase(const PinocchioInterface &pinocchioInterface, CentroidalModelInfo info, - const PinocchioEndEffectorKinematics &eeKinematics) +namespace ocs2::legged_robot +{ + WbcBase::WbcBase(const PinocchioInterface& pinocchioInterface, CentroidalModelInfo info, + const PinocchioEndEffectorKinematics& eeKinematics) : pinocchio_interface_measured_(pinocchioInterface), pinocchio_interface_desired_(pinocchioInterface), info_(std::move(info)), ee_kinematics_(eeKinematics.clone()), mapping_(info_), - input_last_(vector_t::Zero(info_.inputDim)) { + input_last_(vector_t::Zero(info_.inputDim)) + { num_decision_vars_ = info_.generalizedCoordinatesNum + 3 * info_.numThreeDofContacts + info_.actuatedDofNum; q_measured_ = vector_t(info_.generalizedCoordinatesNum); v_measured_ = vector_t(info_.generalizedCoordinatesNum); } - vector_t WbcBase::update(const vector_t &stateDesired, const vector_t &inputDesired, - const vector_t &rbdStateMeasured, size_t mode, - scalar_t /*period*/) { + vector_t WbcBase::update(const vector_t& stateDesired, const vector_t& inputDesired, + const vector_t& rbdStateMeasured, size_t mode, + scalar_t /*period*/) + { contact_flag_ = modeNumber2StanceLeg(mode); num_contacts_ = 0; - for (const bool flag: contact_flag_) { - if (flag) { + for (const bool flag : contact_flag_) + { + if (flag) + { num_contacts_++; } } @@ -45,7 +50,8 @@ namespace ocs2::legged_robot { return {}; } - void WbcBase::updateMeasured(const vector_t &rbdStateMeasured) { + void WbcBase::updateMeasured(const vector_t& rbdStateMeasured) + { q_measured_.head<3>() = rbdStateMeasured.segment<3>(3); q_measured_.segment<3>(3) = rbdStateMeasured.head<3>(); q_measured_.tail(info_.actuatedDofNum) = rbdStateMeasured.segment(6, info_.actuatedDofNum); @@ -55,8 +61,8 @@ namespace ocs2::legged_robot { v_measured_.tail(info_.actuatedDofNum) = rbdStateMeasured.segment( info_.generalizedCoordinatesNum + 6, info_.actuatedDofNum); - const auto &model = pinocchio_interface_measured_.getModel(); - auto &data = pinocchio_interface_measured_.getData(); + const auto& model = pinocchio_interface_measured_.getModel(); + auto& data = pinocchio_interface_measured_.getData(); // For floating base EoM task forwardKinematics(model, data, q_measured_, v_measured_); @@ -66,7 +72,8 @@ namespace ocs2::legged_robot { data.M.triangularView() = data.M.transpose().triangularView(); nonLinearEffects(model, data, q_measured_, v_measured_); j_ = matrix_t(3 * info_.numThreeDofContacts, info_.generalizedCoordinatesNum); - for (size_t i = 0; i < info_.numThreeDofContacts; ++i) { + for (size_t i = 0; i < info_.numThreeDofContacts; ++i) + { Eigen::Matrix jac; jac.setZero(6, info_.generalizedCoordinatesNum); getFrameJacobian(model, data, info_.endEffectorFrameIndices[i], pinocchio::LOCAL_WORLD_ALIGNED, @@ -77,7 +84,8 @@ namespace ocs2::legged_robot { // For not contact motion task computeJointJacobiansTimeVariation(model, data, q_measured_, v_measured_); dj_ = matrix_t(3 * info_.numThreeDofContacts, info_.generalizedCoordinatesNum); - for (size_t i = 0; i < info_.numThreeDofContacts; ++i) { + for (size_t i = 0; i < info_.numThreeDofContacts; ++i) + { Eigen::Matrix jac; jac.setZero(6, info_.generalizedCoordinatesNum); getFrameJacobianTimeVariation(model, data, info_.endEffectorFrameIndices[i], @@ -86,9 +94,10 @@ namespace ocs2::legged_robot { } } - void WbcBase::updateDesired(const vector_t &stateDesired, const vector_t &inputDesired) { - const auto &model = pinocchio_interface_desired_.getModel(); - auto &data = pinocchio_interface_desired_.getData(); + void WbcBase::updateDesired(const vector_t& stateDesired, const vector_t& inputDesired) + { + const auto& model = pinocchio_interface_desired_.getModel(); + auto& data = pinocchio_interface_desired_.getData(); mapping_.setPinocchioInterface(pinocchio_interface_desired_); const auto qDesired = mapping_.getPinocchioJointPosition(stateDesired); @@ -100,21 +109,23 @@ namespace ocs2::legged_robot { forwardKinematics(model, data, qDesired, vDesired); } - Task WbcBase::formulateFloatingBaseEomTask() { - const auto &data = pinocchio_interface_measured_.getData(); + Task WbcBase::formulateFloatingBaseEomTask() + { + const auto& data = pinocchio_interface_measured_.getData(); matrix_t s(info_.actuatedDofNum, info_.generalizedCoordinatesNum); s.block(0, 0, info_.actuatedDofNum, 6).setZero(); s.block(0, 6, info_.actuatedDofNum, info_.actuatedDofNum).setIdentity(); matrix_t a = (matrix_t(info_.generalizedCoordinatesNum, num_decision_vars_) << data.M, -j_.transpose(), -s. - transpose()).finished(); + transpose()).finished(); vector_t b = -data.nle; return {a, b, matrix_t(), vector_t()}; } - Task WbcBase::formulateTorqueLimitsTask() { + Task WbcBase::formulateTorqueLimitsTask() + { matrix_t d(2 * info_.actuatedDofNum, num_decision_vars_); d.setZero(); matrix_t i = matrix_t::Identity(info_.actuatedDofNum, info_.actuatedDofNum); @@ -124,21 +135,25 @@ namespace ocs2::legged_robot { info_.actuatedDofNum, info_.actuatedDofNum) = -i; vector_t f(2 * info_.actuatedDofNum); - for (size_t l = 0; l < 2 * info_.actuatedDofNum / 3; ++l) { + for (size_t l = 0; l < 2 * info_.actuatedDofNum / 3; ++l) + { f.segment<3>(3 * l) = torque_limits_; } return {matrix_t(), vector_t(), d, f}; } - Task WbcBase::formulateNoContactMotionTask() { + Task WbcBase::formulateNoContactMotionTask() + { matrix_t a(3 * num_contacts_, num_decision_vars_); vector_t b(a.rows()); a.setZero(); b.setZero(); size_t j = 0; - for (size_t i = 0; i < info_.numThreeDofContacts; i++) { - if (contact_flag_[i]) { + for (size_t i = 0; i < info_.numThreeDofContacts; i++) + { + if (contact_flag_[i]) + { a.block(3 * j, 0, 3, info_.generalizedCoordinatesNum) = j_.block( 3 * i, 0, 3, info_.generalizedCoordinatesNum); b.segment(3 * j, 3) = -dj_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) * v_measured_; @@ -149,12 +164,15 @@ namespace ocs2::legged_robot { return {a, b, matrix_t(), vector_t()}; } - Task WbcBase::formulateFrictionConeTask() { + Task WbcBase::formulateFrictionConeTask() + { matrix_t a(3 * (info_.numThreeDofContacts - num_contacts_), num_decision_vars_); a.setZero(); size_t j = 0; - for (size_t i = 0; i < info_.numThreeDofContacts; ++i) { - if (!contact_flag_[i]) { + for (size_t i = 0; i < info_.numThreeDofContacts; ++i) + { + if (!contact_flag_[i]) + { a.block(3 * j++, info_.generalizedCoordinatesNum + 3 * i, 3, 3) = matrix_t::Identity(3, 3); } } @@ -171,8 +189,10 @@ namespace ocs2::legged_robot { matrix_t d(5 * num_contacts_ + 3 * (info_.numThreeDofContacts - num_contacts_), num_decision_vars_); d.setZero(); j = 0; - for (size_t i = 0; i < info_.numThreeDofContacts; ++i) { - if (contact_flag_[i]) { + for (size_t i = 0; i < info_.numThreeDofContacts; ++i) + { + if (contact_flag_[i]) + { d.block(5 * j++, info_.generalizedCoordinatesNum + 3 * i, 5, 3) = frictionPyramic; } } @@ -181,7 +201,8 @@ namespace ocs2::legged_robot { return {a, b, d, f}; } - Task WbcBase::formulateBaseAccelTask(const vector_t &stateDesired, const vector_t &inputDesired, scalar_t period) { + Task WbcBase::formulateBaseAccelTask(const vector_t& stateDesired, const vector_t& inputDesired, scalar_t period) + { matrix_t a(6, num_decision_vars_); a.setZero(); a.block(0, 0, 6, 6) = matrix_t::Identity(6, 6); @@ -190,18 +211,18 @@ namespace ocs2::legged_robot { input_last_ = inputDesired; mapping_.setPinocchioInterface(pinocchio_interface_desired_); - const auto &model = pinocchio_interface_desired_.getModel(); - auto &data = pinocchio_interface_desired_.getData(); + const auto& model = pinocchio_interface_desired_.getModel(); + auto& data = pinocchio_interface_desired_.getData(); const auto qDesired = mapping_.getPinocchioJointPosition(stateDesired); const vector_t vDesired = mapping_.getPinocchioJointVelocity(stateDesired, inputDesired); - const auto &A = getCentroidalMomentumMatrix(pinocchio_interface_desired_); + const auto& A = getCentroidalMomentumMatrix(pinocchio_interface_desired_); const Matrix6 Ab = A.template leftCols<6>(); const auto AbInv = computeFloatingBaseCentroidalMomentumMatrixInverse(Ab); const auto Aj = A.rightCols(info_.actuatedDofNum); const auto ADot = dccrba(model, data, qDesired, vDesired); Vector6 centroidalMomentumRate = info_.robotMass * getNormalizedCentroidalMomentumRate( - pinocchio_interface_desired_, info_, inputDesired); + pinocchio_interface_desired_, info_, inputDesired); centroidalMomentumRate.noalias() -= ADot * vDesired; centroidalMomentumRate.noalias() -= Aj * jointAccel; @@ -210,7 +231,8 @@ namespace ocs2::legged_robot { return {a, b, matrix_t(), vector_t()}; } - Task WbcBase::formulateSwingLegTask() { + Task WbcBase::formulateSwingLegTask() + { ee_kinematics_->setPinocchioInterface(pinocchio_interface_measured_); std::vector posMeasured = ee_kinematics_->getPosition(vector_t()); std::vector velMeasured = ee_kinematics_->getVelocity(vector_t(), vector_t()); @@ -223,10 +245,12 @@ namespace ocs2::legged_robot { a.setZero(); b.setZero(); size_t j = 0; - for (size_t i = 0; i < info_.numThreeDofContacts; ++i) { - if (!contact_flag_[i]) { + for (size_t i = 0; i < info_.numThreeDofContacts; ++i) + { + if (!contact_flag_[i]) + { vector3_t accel = swing_kp_ * (posDesired[i] - posMeasured[i]) + swing_kd_ * ( - velDesired[i] - velMeasured[i]); + velDesired[i] - velMeasured[i]); a.block(3 * j, 0, 3, info_.generalizedCoordinatesNum) = j_.block( 3 * i, 0, 3, info_.generalizedCoordinatesNum); b.segment(3 * j, 3) = accel - dj_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) * v_measured_; @@ -237,12 +261,14 @@ namespace ocs2::legged_robot { return {a, b, matrix_t(), vector_t()}; } - Task WbcBase::formulateContactForceTask(const vector_t &inputDesired) const { + Task WbcBase::formulateContactForceTask(const vector_t& inputDesired) const + { matrix_t a(3 * info_.numThreeDofContacts, num_decision_vars_); vector_t b(a.rows()); a.setZero(); - for (size_t i = 0; i < info_.numThreeDofContacts; ++i) { + for (size_t i = 0; i < info_.numThreeDofContacts; ++i) + { a.block(3 * i, info_.generalizedCoordinatesNum + 3 * i, 3, 3) = matrix_t::Identity(3, 3); } b = inputDesired.head(a.rows()); @@ -250,11 +276,13 @@ namespace ocs2::legged_robot { return {a, b, matrix_t(), vector_t()}; } - void WbcBase::loadTasksSetting(const std::string &taskFile, const bool verbose) { + void WbcBase::loadTasksSetting(const std::string& taskFile, const bool verbose) + { // Load task file torque_limits_ = vector_t(info_.actuatedDofNum / 4); loadData::loadEigenMatrix(taskFile, "torqueLimitsTask", torque_limits_); - if (verbose) { + if (verbose) + { std::cerr << "\n #### Torque Limits Task:"; std::cerr << "\n #### =============================================================================\n"; std::cerr << "\n #### Hip_joint Thigh_joint Calf_joint: " << torque_limits_.transpose() << "\n"; @@ -263,16 +291,19 @@ namespace ocs2::legged_robot { boost::property_tree::ptree pt; read_info(taskFile, pt); std::string prefix = "frictionConeTask."; - if (verbose) { + if (verbose) + { std::cerr << "\n #### Friction Cone Task:"; std::cerr << "\n #### =============================================================================\n"; } loadData::loadPtreeValue(pt, friction_coeff_, prefix + "frictionCoefficient", verbose); - if (verbose) { + if (verbose) + { std::cerr << " #### =============================================================================\n"; } prefix = "swingLegTask."; - if (verbose) { + if (verbose) + { std::cerr << "\n #### Swing Leg Task:"; std::cerr << "\n #### =============================================================================\n"; } diff --git a/controllers/ocs2_quadruped_controller/src/wbc/WeightedWbc.cpp b/controllers/ocs2_quadruped_controller/src/wbc/WeightedWbc.cpp index 6ea9634..e5e5ea3 100644 --- a/controllers/ocs2_quadruped_controller/src/wbc/WeightedWbc.cpp +++ b/controllers/ocs2_quadruped_controller/src/wbc/WeightedWbc.cpp @@ -10,10 +10,12 @@ #include #include -namespace ocs2::legged_robot { - vector_t WeightedWbc::update(const vector_t &stateDesired, const vector_t &inputDesired, - const vector_t &rbdStateMeasured, size_t mode, - scalar_t period) { +namespace ocs2::legged_robot +{ + vector_t WeightedWbc::update(const vector_t& stateDesired, const vector_t& inputDesired, + const vector_t& rbdStateMeasured, size_t mode, + scalar_t period) + { WbcBase::update(stateDesired, inputDesired, rbdStateMeasured, mode, period); // Constraints @@ -33,7 +35,7 @@ namespace ocs2::legged_robot { // Cost Task weighedTask = formulateWeightedTasks(stateDesired, inputDesired, period); Eigen::Matrix H = - weighedTask.a_.transpose() * weighedTask.a_; + weighedTask.a_.transpose() * weighedTask.a_; vector_t g = -weighedTask.a_.transpose() * weighedTask.b_; // Solve @@ -52,25 +54,29 @@ namespace ocs2::legged_robot { return qpSol; } - Task WeightedWbc::formulateConstraints() { + Task WeightedWbc::formulateConstraints() + { return formulateFloatingBaseEomTask() + formulateTorqueLimitsTask() + formulateFrictionConeTask() + - formulateNoContactMotionTask(); + formulateNoContactMotionTask(); } - Task WeightedWbc::formulateWeightedTasks(const vector_t &stateDesired, const vector_t &inputDesired, - scalar_t period) { + Task WeightedWbc::formulateWeightedTasks(const vector_t& stateDesired, const vector_t& inputDesired, + scalar_t period) + { return formulateSwingLegTask() * weightSwingLeg_ + formulateBaseAccelTask(stateDesired, inputDesired, period) * - weightBaseAccel_ + - formulateContactForceTask(inputDesired) * weightContactForce_; + weightBaseAccel_ + + formulateContactForceTask(inputDesired) * weightContactForce_; } - void WeightedWbc::loadTasksSetting(const std::string &taskFile, bool verbose) { + void WeightedWbc::loadTasksSetting(const std::string& taskFile, bool verbose) + { WbcBase::loadTasksSetting(taskFile, verbose); boost::property_tree::ptree pt; read_info(taskFile, pt); const std::string prefix = "weight."; - if (verbose) { + if (verbose) + { std::cerr << "\n #### WBC weight:"; std::cerr << "\n #### =============================================================================\n"; } diff --git a/descriptions/deep_robotics/lite3_description/config/robot_control.yaml b/descriptions/deep_robotics/lite3_description/config/robot_control.yaml index a41303f..2897b74 100644 --- a/descriptions/deep_robotics/lite3_description/config/robot_control.yaml +++ b/descriptions/deep_robotics/lite3_description/config/robot_control.yaml @@ -1,7 +1,7 @@ # Controller Manager configuration controller_manager: ros__parameters: - update_rate: 200 # Hz + update_rate: 1000 # Hz # Define the available controllers joint_state_broadcaster: diff --git a/descriptions/deep_robotics/x30_description/config/robot_control.yaml b/descriptions/deep_robotics/x30_description/config/robot_control.yaml index 82b85e2..f9d35f8 100644 --- a/descriptions/deep_robotics/x30_description/config/robot_control.yaml +++ b/descriptions/deep_robotics/x30_description/config/robot_control.yaml @@ -108,7 +108,8 @@ unitree_guide_controller: ocs2_quadruped_controller: ros__parameters: - update_rate: 100 # Hz + update_rate: 200 # Hz + robot_pkg: "x30_description" default_kd: 8.0 joints: - FL_HipX @@ -213,6 +214,8 @@ rl_quadruped_controller: - 0.0 - -1.0 - 1.8 + - -0.732 + - 1.361 command_interfaces: - effort @@ -253,92 +256,3 @@ rl_quadruped_controller: - linear_acceleration.x - linear_acceleration.y - linear_acceleration.z - -rl_quadruped_controller: - ros__parameters: - update_rate: 200 # Hz - robot_pkg: "x30_description" - model_folder: "legged_gym" - stand_kp: 500.0 - stand_kd: 20.0 - joints: - - FL_HipX - - FL_HipY - - FL_Knee - - FR_HipX - - FR_HipY - - FR_Knee - - HL_HipX - - HL_HipY - - HL_Knee - - HR_HipX - - HR_HipY - - HR_Knee - - down_pos: - - 0.0 - - -1.22 - - 2.61 - - 0.0 - - -1.22 - - 2.61 - - 0.0 - - -1.22 - - 2.61 - - 0.0 - - -1.22 - - 2.61 - - stand_pos: - - 0.0 - - -0.732 - - 1.361 - - 0.0 - - -0.732 - - 1.361 - - 0.0 - - -0.732 - - 1.361 - - 0.0 - - -0.732 - - 1.361 - - command_interfaces: - - effort - - position - - velocity - - kp - - kd - - state_interfaces: - - effort - - position - - velocity - - feet_names: - - FL_FOOT - - FR_FOOT - - HL_FOOT - - HR_FOOT - - foot_force_name: "foot_force" - foot_force_interfaces: - - FL - - FR - - HL - - HR - - imu_name: "imu_sensor" - base_name: "base" - - imu_interfaces: - - orientation.w - - orientation.x - - orientation.y - - orientation.z - - angular_velocity.x - - angular_velocity.y - - angular_velocity.z - - linear_acceleration.x - - linear_acceleration.y - - linear_acceleration.z \ No newline at end of file diff --git a/descriptions/unitree/a1_description/README.md b/descriptions/unitree/a1_description/README.md index eaca11e..dfb9175 100644 --- a/descriptions/unitree/a1_description/README.md +++ b/descriptions/unitree/a1_description/README.md @@ -52,7 +52,7 @@ ros2 launch a1_description visualize.launch.py source ~/ros2_ws/install/setup.bash ros2 launch unitree_guide_controller gazebo_classic.launch.py pkg_description:=a1_description height:=0.43 ``` - + ### Gazebo Harmonic (ROS2 Jazzy) * Unitree Guide Controller @@ -63,7 +63,7 @@ ros2 launch a1_description visualize.launch.py * OCS2 Quadruped Controller ```bash source ~/ros2_ws/install/setup.bash - ros2 launch ocs2_quadruped_controller gazebo.launch.py pkg_description:=a1_description + ros2 launch ocs2_quadruped_controller gazebo.launch.py pkg_description:=a1_description height:=0.43 ``` * RL Quadruped Controller ```bash diff --git a/descriptions/unitree/a1_description/config/gazebo.yaml b/descriptions/unitree/a1_description/config/gazebo.yaml index 9eb26b8..da39ceb 100644 --- a/descriptions/unitree/a1_description/config/gazebo.yaml +++ b/descriptions/unitree/a1_description/config/gazebo.yaml @@ -10,9 +10,6 @@ controller_manager: imu_sensor_broadcaster: type: imu_sensor_broadcaster/IMUSensorBroadcaster - leg_pd_controller: - type: leg_pd_controller/LegPdController - unitree_guide_controller: type: unitree_guide_controller/UnitreeGuideController @@ -27,33 +24,9 @@ imu_sensor_broadcaster: sensor_name: "imu_sensor" frame_id: "imu_link" -leg_pd_controller: - ros__parameters: - joints: - - FR_hip_joint - - FR_thigh_joint - - FR_calf_joint - - FL_hip_joint - - FL_thigh_joint - - FL_calf_joint - - RR_hip_joint - - RR_thigh_joint - - RR_calf_joint - - RL_hip_joint - - RL_thigh_joint - - RL_calf_joint - - command_interfaces: - - effort - - state_interfaces: - - position - - velocity - unitree_guide_controller: ros__parameters: - command_prefix: "leg_pd_controller" - update_rate: 200 # Hz + update_rate: 500 # Hz joints: - FR_hip_joint - FR_thigh_joint @@ -105,7 +78,9 @@ ocs2_quadruped_controller: ros__parameters: update_rate: 500 # Hz robot_pkg: "a1_description" - command_prefix: "leg_pd_controller" + feet_force_threshold: 5.0 +# default_kp: 1.0 + default_kd: 0.5 joints: - FL_hip_joint - FL_thigh_joint @@ -153,7 +128,12 @@ ocs2_quadruped_controller: - linear_acceleration.y - linear_acceleration.z - estimator_type: "odom_topic" + foot_force_name: "foot_force" + foot_force_interfaces: + - FL_foot_force + - RL_foot_force + - FR_foot_force + - RR_foot_force rl_quadruped_controller: ros__parameters: @@ -164,12 +144,12 @@ rl_quadruped_controller: - FL_hip_joint - FL_thigh_joint - FL_calf_joint - - RL_hip_joint - - RL_thigh_joint - - RL_calf_joint - FR_hip_joint - FR_thigh_joint - FR_calf_joint + - RL_hip_joint + - RL_thigh_joint + - RL_calf_joint - RR_hip_joint - RR_thigh_joint - RR_calf_joint diff --git a/descriptions/unitree/a1_description/config/ocs2/reference.info b/descriptions/unitree/a1_description/config/ocs2/reference.info index 2f69c51..f7fbfd2 100644 --- a/descriptions/unitree/a1_description/config/ocs2/reference.info +++ b/descriptions/unitree/a1_description/config/ocs2/reference.info @@ -5,18 +5,18 @@ comHeight 0.33 defaultJointState { - (0,0) -0.20 ; FL_hip_joint - (1,0) 0.72 ; FL_thigh_joint - (2,0) -1.44 ; FL_calf_joint - (3,0) 0.20 ; FR_hip_joint - (4,0) 0.72 ; FR_thigh_joint - (5,0) -1.44 ; FR_calf_joint - (6,0) -0.20 ; RL_hip_joint - (7,0) 0.72 ; RL_thigh_joint - (8,0) -1.44 ; RL_calf_joint - (9,0) 0.20 ; RR_hip_joint - (10,0) 0.72 ; RR_thigh_joint - (11,0) -1.44 ; RR_calf_joint + (0,0) -0.107 ; FL_hip_joint + (1,0) 0.77 ; FL_thigh_joint + (2,0) -1.68 ; FL_calf_joint + (3,0) 0.107 ; FR_hip_joint + (4,0) 0.77 ; FR_thigh_joint + (5,0) -1.68 ; FR_calf_joint + (6,0) -0.107 ; RL_hip_joint + (7,0) 0.77 ; RL_thigh_joint + (8,0) -1.68 ; RL_calf_joint + (9,0) 0.107 ; RR_hip_joint + (10,0) 0.77 ; RR_thigh_joint + (11,0) -1.68 ; RR_calf_joint } initialModeSchedule diff --git a/descriptions/unitree/a1_description/config/ocs2/task.info b/descriptions/unitree/a1_description/config/ocs2/task.info index 93f5ce4..0b53251 100644 --- a/descriptions/unitree/a1_description/config/ocs2/task.info +++ b/descriptions/unitree/a1_description/config/ocs2/task.info @@ -23,48 +23,6 @@ swing_trajectory_config swingTimeScale 0.15 } -; DDP settings -ddp -{ - algorithm SLQ - - nThreads 3 - threadPriority 50 - - maxNumIterations 1 - minRelCost 1e-1 - constraintTolerance 5e-3 - - displayInfo false - displayShortSummary false - checkNumericalStability false - debugPrintRollout false - debugCaching false - - AbsTolODE 1e-5 - RelTolODE 1e-3 - maxNumStepsPerSecond 10000 - timeStep 0.015 - backwardPassIntegratorType ODE45 - - constraintPenaltyInitialValue 20.0 - constraintPenaltyIncreaseRate 2.0 - - preComputeRiccatiTerms true - - useFeedbackPolicy false - - strategy LINE_SEARCH - lineSearch - { - minStepLength 1e-2 - maxStepLength 1.0 - hessianCorrectionStrategy DIAGONAL_SHIFT - hessianCorrectionMultiple 1e-5 - } -} - - ; Multiple_Shooting SQP settings sqp { @@ -85,39 +43,6 @@ sqp threadPriority 50 } -; Multiple_Shooting IPM settings -ipm -{ - nThreads 3 - dt 0.015 - ipmIteration 1 - deltaTol 1e-4 - g_max 10.0 - g_min 1e-6 - computeLagrangeMultipliers true - printSolverStatistics true - printSolverStatus false - printLinesearch false - useFeedbackPolicy false - integratorType RK2 - threadPriority 50 - - initialBarrierParameter 1e-4 - targetBarrierParameter 1e-4 - barrierLinearDecreaseFactor 0.2 - barrierSuperlinearDecreasePower 1.5 - barrierReductionCostTol 1e-3 - barrierReductionConstraintTol 1e-3 - - fractionToBoundaryMargin 0.995 - usePrimalStepSizeForDual false - - initialSlackLowerBound 1e-4 - initialDualLowerBound 1e-4 - initialSlackMarginRate 1e-2 - initialDualMarginRate 1e-2 -} - ; Rollout settings rollout { @@ -160,18 +85,18 @@ initialState (11,0) 0.0 ; theta_base_x ;; Leg Joint Positions: [FL, RL, FR, RR] ;; - (12,0) -0.20 ; FL_hip_joint - (13,0) 0.72 ; FL_thigh_joint - (14,0) -1.44 ; FL_calf_joint - (15,0) -0.20 ; RL_hip_joint - (16,0) 0.72 ; RL_thigh_joint - (17,0) -1.44 ; RL_calf_joint - (18,0) 0.20 ; FR_hip_joint - (19,0) 0.72 ; FR_thigh_joint - (20,0) -1.44 ; FR_calf_joint - (21,0) 0.20 ; RR_hip_joint - (22,0) 0.72 ; RR_thigh_joint - (23,0) -1.44 ; RR_calf_joint + (12,0) -0.107 ; FL_hip_joint + (13,0) 0.77 ; FL_thigh_joint + (14,0) -1.68 ; FL_calf_joint + (15,0) -0.107 ; RL_hip_joint + (16,0) 0.77 ; RL_thigh_joint + (17,0) -1.68 ; RL_calf_joint + (18,0) 0.107 ; FR_hip_joint + (19,0) 0.77 ; FR_thigh_joint + (20,0) -1.68 ; FR_calf_joint + (21,0) 0.107 ; RR_hip_joint + (22,0) 0.77 ; RR_thigh_joint + (23,0) -1.68 ; RR_calf_joint } ; standard state weight matrix @@ -295,15 +220,15 @@ frictionConeTask swingLegTask { - kp 350 - kd 37 + kp 300 + kd 30 } weight { swingLeg 100 baseAccel 1 - contactForce 0.01 + contactForce 0.05 } ; State Estimation diff --git a/descriptions/unitree/a1_description/config/robot_control.yaml b/descriptions/unitree/a1_description/config/robot_control.yaml index d42ed52..3c24e67 100644 --- a/descriptions/unitree/a1_description/config/robot_control.yaml +++ b/descriptions/unitree/a1_description/config/robot_control.yaml @@ -76,8 +76,9 @@ unitree_guide_controller: ocs2_quadruped_controller: ros__parameters: - update_rate: 100 # Hz - + update_rate: 500 # Hz + robot_pkg: "a1_description" + default_kd: 0.5 joints: - FL_hip_joint - FL_thigh_joint diff --git a/descriptions/unitree/a1_description/xacro/const.xacro b/descriptions/unitree/a1_description/xacro/const.xacro index acab77b..51a8d65 100644 --- a/descriptions/unitree/a1_description/xacro/const.xacro +++ b/descriptions/unitree/a1_description/xacro/const.xacro @@ -35,7 +35,7 @@ - + diff --git a/descriptions/unitree/a1_description/xacro/gazebo.xacro b/descriptions/unitree/a1_description/xacro/gazebo.xacro index 4f48485..0ce1728 100644 --- a/descriptions/unitree/a1_description/xacro/gazebo.xacro +++ b/descriptions/unitree/a1_description/xacro/gazebo.xacro @@ -162,21 +162,12 @@ - $(find go2_description)/config/gazebo.yaml + $(find a1_description)/config/gazebo.yaml - - odom - base - 1000 - odom - 3 - odom_with_covariance - tf - + diff --git a/descriptions/unitree/aliengo_description/config/robot_control.yaml b/descriptions/unitree/aliengo_description/config/robot_control.yaml index b2f2bec..e99423b 100644 --- a/descriptions/unitree/aliengo_description/config/robot_control.yaml +++ b/descriptions/unitree/aliengo_description/config/robot_control.yaml @@ -1,7 +1,7 @@ # Controller Manager configuration controller_manager: ros__parameters: - update_rate: 500 # Hz + update_rate: 1000 # Hz # Define the available controllers joint_state_broadcaster: @@ -78,7 +78,8 @@ unitree_guide_controller: ocs2_quadruped_controller: ros__parameters: - update_rate: 100 # Hz + update_rate: 500 # Hz + robot_pkg: "aliengo_description" default_kd: 5.0 joints: diff --git a/descriptions/unitree/b2_description/README.md b/descriptions/unitree/b2_description/README.md index 31aa2db..cdb5803 100644 --- a/descriptions/unitree/b2_description/README.md +++ b/descriptions/unitree/b2_description/README.md @@ -49,4 +49,9 @@ ros2 launch b2_description visualize.launch.py ```bash source ~/ros2_ws/install/setup.bash ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=b2_description height:=0.68 + ``` +* OCS2 Quadruped Controller + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch ocs2_quadruped_controller gazebo.launch.py pkg_description:=b2_description height:=0.68 ``` \ No newline at end of file diff --git a/descriptions/unitree/b2_description/config/gazebo.yaml b/descriptions/unitree/b2_description/config/gazebo.yaml index fc9f1e4..c3cabbc 100644 --- a/descriptions/unitree/b2_description/config/gazebo.yaml +++ b/descriptions/unitree/b2_description/config/gazebo.yaml @@ -10,8 +10,8 @@ controller_manager: imu_sensor_broadcaster: type: imu_sensor_broadcaster/IMUSensorBroadcaster - leg_pd_controller: - type: leg_pd_controller/LegPdController + ocs2_quadruped_controller: + type: ocs2_quadruped_controller/Ocs2QuadrupedController unitree_guide_controller: type: unitree_guide_controller/UnitreeGuideController @@ -24,33 +24,69 @@ imu_sensor_broadcaster: sensor_name: "imu_sensor" frame_id: "imu_link" -leg_pd_controller: +ocs2_quadruped_controller: ros__parameters: - update_rate: 1000 # Hz + update_rate: 500 # Hz + robot_pkg: "b2_description" + feet_force_threshold: 5.0 + default_kd: 3.5 + joints: - - FR_hip_joint - - FR_thigh_joint - - FR_calf_joint - FL_hip_joint - FL_thigh_joint - FL_calf_joint - - RR_hip_joint - - RR_thigh_joint - - RR_calf_joint + - FR_hip_joint + - FR_thigh_joint + - FR_calf_joint - RL_hip_joint - RL_thigh_joint - RL_calf_joint + - RR_hip_joint + - RR_thigh_joint + - RR_calf_joint command_interfaces: - effort + - position + - velocity + - kp + - kd state_interfaces: + - effort - position - velocity + feet: + - FL_foot + - FR_foot + - RL_foot + - RR_foot + + imu_name: "imu_sensor" + base_name: "base" + + imu_interfaces: + - orientation.w + - orientation.x + - orientation.y + - orientation.z + - angular_velocity.x + - angular_velocity.y + - angular_velocity.z + - linear_acceleration.x + - linear_acceleration.y + - linear_acceleration.z + + foot_force_name: "foot_force" + foot_force_interfaces: + - FL_foot_force + - RL_foot_force + - FR_foot_force + - RR_foot_force + unitree_guide_controller: ros__parameters: - command_prefix: "leg_pd_controller" update_rate: 500 # Hz stand_kp: 500.0 stand_kd: 40.0 diff --git a/descriptions/unitree/b2_description/config/robot_control.yaml b/descriptions/unitree/b2_description/config/robot_control.yaml index c2f0224..d004f52 100644 --- a/descriptions/unitree/b2_description/config/robot_control.yaml +++ b/descriptions/unitree/b2_description/config/robot_control.yaml @@ -75,8 +75,8 @@ unitree_guide_controller: ocs2_quadruped_controller: ros__parameters: - update_rate: 100 # Hz - + update_rate: 500 # Hz + robot_pkg: "b2_description" default_kd: 8.0 joints: diff --git a/descriptions/unitree/b2_description/xacro/const.xacro b/descriptions/unitree/b2_description/xacro/const.xacro index c42e36b..ca4d875 100755 --- a/descriptions/unitree/b2_description/xacro/const.xacro +++ b/descriptions/unitree/b2_description/xacro/const.xacro @@ -2,159 +2,159 @@ - - - + + + - - - - - - - - - - - - + + + + + + + + + + + + + + + - - - - + + + + - - - - - + + + + + - - - - - + + + + + + + - - - - + + + + + + - - - - + + + + + + - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + - - - - - - - - - - - - + + + + + + + + + + + + - - - - - - - - - - - + + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - - + + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - - + + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - + + diff --git a/descriptions/unitree/b2_description/xacro/gazebo.xacro b/descriptions/unitree/b2_description/xacro/gazebo.xacro index e1f532b..e2f66c4 100755 --- a/descriptions/unitree/b2_description/xacro/gazebo.xacro +++ b/descriptions/unitree/b2_description/xacro/gazebo.xacro @@ -1,188 +1,250 @@ + + + + gz_quadruped_hardware/GazeboSimSystem + - - - gz_ros2_control/GazeboSimSystem - + + + + + + - - - - - - + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - + + + + + + + + + + - - - - - - - - - - - - - + + + + + + + + + + + + - - - $(find b2_description)/config/gazebo.yaml - - - - + + + + + + + - - 0.6 - 0.6 - 1 - + + + $(find b2_description)/config/gazebo.yaml + + + + + - - - 1 - 500 - true - imu - - - - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - - - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - - - - true - + + 0.6 + 0.6 + 1 + + + + + 1 + 500 + true + imu + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + + + + true + + + + + + diff --git a/descriptions/unitree/b2_description/xacro/leg.xacro b/descriptions/unitree/b2_description/xacro/leg.xacro index 1ce3df1..0c98122 100755 --- a/descriptions/unitree/b2_description/xacro/leg.xacro +++ b/descriptions/unitree/b2_description/xacro/leg.xacro @@ -90,7 +90,6 @@ - @@ -152,7 +151,6 @@ - @@ -227,7 +225,6 @@ - diff --git a/descriptions/unitree/go1_description/config/robot_control.yaml b/descriptions/unitree/go1_description/config/robot_control.yaml index d8e3494..182c75a 100644 --- a/descriptions/unitree/go1_description/config/robot_control.yaml +++ b/descriptions/unitree/go1_description/config/robot_control.yaml @@ -1,7 +1,7 @@ # Controller Manager configuration controller_manager: ros__parameters: - update_rate: 200 # Hz + update_rate: 1000 # Hz # Define the available controllers joint_state_broadcaster: @@ -73,8 +73,8 @@ unitree_guide_controller: ocs2_quadruped_controller: ros__parameters: - update_rate: 100 # Hz - + update_rate: 500 # Hz + robot_pkg: "go1_description" joints: - FL_hip_joint - FL_thigh_joint diff --git a/descriptions/unitree/go2_description/xacro/const.xacro b/descriptions/unitree/go2_description/xacro/const.xacro index 5436b5c..bd02f6b 100755 --- a/descriptions/unitree/go2_description/xacro/const.xacro +++ b/descriptions/unitree/go2_description/xacro/const.xacro @@ -50,8 +50,8 @@ - - + + diff --git a/descriptions/unitree/go2_description/xacro/leg.xacro b/descriptions/unitree/go2_description/xacro/leg.xacro index 13150e2..374905d 100755 --- a/descriptions/unitree/go2_description/xacro/leg.xacro +++ b/descriptions/unitree/go2_description/xacro/leg.xacro @@ -2,202 +2,202 @@ - + - + - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - + + + + + + + + - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + - - - - - + + + + + - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + - - 0.2 - 0.2 - + + 0.2 + 0.2 + - - 0.2 - 0.2 - 0 - - - + + 0.2 + 0.2 + 0 + + + - - 0.6 - 0.6 - 1 - + + 0.6 + 0.6 + 1 + - - 0.6 - 0.6 - 1 - - - + + 0.6 + 0.6 + 1 + + + - - + + diff --git a/hardwares/gz_quadruped_hardware/xacro/foot_force_sensor.xacro b/hardwares/gz_quadruped_hardware/xacro/foot_force_sensor.xacro index d09d0c6..d69bfce 100644 --- a/hardwares/gz_quadruped_hardware/xacro/foot_force_sensor.xacro +++ b/hardwares/gz_quadruped_hardware/xacro/foot_force_sensor.xacro @@ -9,7 +9,7 @@ true 1 - 500 + 1000 true ${name}_foot_force