EmbodiedAI
/
quadruped_ros2_control
mirror of https://github.com/legubiao/quadruped_ros2_control.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

enhanced state switch for ocs2 quadruped controller

This commit is contained in:
Huang Zhenbiao 2025-03-15 19:42:35 +08:00
parent 9243a11e0e
commit 15b4b798d1
47 changed files with 1506 additions and 1360 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
controllers/ocs2_quadruped_controller/CMakeLists.txt
View File

@ -56,6 +56,7 @@ add_library(${PROJECT_NAME} SHARED
src/control/GaitManager.cpp src/control/GaitManager.cpp
src/control/TargetManager.cpp src/control/TargetManager.cpp
src/control/CtrlComponent.cpp
) )
target_include_directories(${PROJECT_NAME} target_include_directories(${PROJECT_NAME}

3
controllers/ocs2_quadruped_controller/README.md
View File

@ -10,7 +10,8 @@ Tested environment:
* Ubuntu 22.04 * Ubuntu 22.04
* ROS2 Humble * 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/) [![](http://i0.hdslb.com/bfs/archive/e758ce019587032449a153cf897a543443b64bba.jpg)](https://www.bilibili.com/video/BV1UcxieuEmH/)

76
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/FSM/StateOCS2.h
View File

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

80
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h Normal file
View File

@ -0,0 +1,80 @@
//
// Created by biao on 3/15/25.
//
#ifndef CTRLCOMPONENT_H
#define CTRLCOMPONENT_H
#include <memory>
#include <string>
#include <ocs2_mpc/SystemObservation.h>
#include <ocs2_quadruped_controller/estimator/StateEstimateBase.h>
#include <ocs2_quadruped_controller/interface/LeggedInterface.h>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <ocs2_core/misc/Benchmark.h>
#include <ocs2_mpc/MPC_MRT_Interface.h>
#include <ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h>
#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<rclcpp_lifecycle::LifecycleNode>& 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<rclcpp_lifecycle::LifecycleNode> node_;
std::shared_ptr<LeggedInterface> legged_interface_;
std::shared_ptr<PinocchioEndEffectorKinematics> ee_kinematics_;
std::shared_ptr<LeggedRobotVisualizer> visualizer_;
std::shared_ptr<MPC_BASE> mpc_;
std::shared_ptr<MPC_MRT_Interface> 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<StateEstimateBase> estimator_;
std::shared_ptr<CentroidalModelRbdConversions> rbd_conversions_;
std::shared_ptr<TargetManager> target_manager_;
std::vector<std::string> joint_names_;
std::vector<std::string> feet_names_;
std::string robot_pkg_;
// Nonlinear MPC
benchmark::RepeatedTimer mpc_timer_;
std::thread mpc_thread_;
std::atomic_bool controller_running_{};
};
}
#endif //CTRLCOMPONENT_H

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

@ -8,25 +8,28 @@
#include <ocs2_legged_robot/gait/GaitSchedule.h> #include <ocs2_legged_robot/gait/GaitSchedule.h>
#include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h> #include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h>
namespace ocs2::legged_robot { namespace ocs2::legged_robot
class GaitManager final : public SolverSynchronizedModule { {
class GaitManager final : public SolverSynchronizedModule
{
public: public:
GaitManager(CtrlInterfaces &ctrl_interfaces, GaitManager(CtrlInterfaces& ctrl_interfaces,
std::shared_ptr<GaitSchedule> gait_schedule_ptr); std::shared_ptr<GaitSchedule> gait_schedule_ptr);
void preSolverRun(scalar_t initTime, scalar_t finalTime, void preSolverRun(scalar_t initTime, scalar_t finalTime,
const vector_t &currentState, const vector_t& currentState,
const ReferenceManagerInterface &referenceManager) override; 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: private:
void getTargetGait(); void getTargetGait();
CtrlInterfaces &ctrl_interfaces_; CtrlInterfaces& ctrl_interfaces_;
std::shared_ptr<GaitSchedule> gait_schedule_ptr_; std::shared_ptr<GaitSchedule> gait_schedule_ptr_;
ModeSequenceTemplate target_gait_; ModeSequenceTemplate target_gait_;

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

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

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

@ -4,7 +4,7 @@
#pragma once #pragma once
#include "StateEstimateBase.h" #include "StateEstimateBase.h"
#include <realtime_tools/realtime_buffer.h> #include <realtime_tools/realtime_buffer.hpp>
namespace ocs2::legged_robot namespace ocs2::legged_robot
{ {

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

@ -16,12 +16,14 @@
#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h> #include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h>
#include <rclcpp_lifecycle/lifecycle_node.hpp> #include <rclcpp_lifecycle/lifecycle_node.hpp>
namespace ocs2::legged_robot { namespace ocs2::legged_robot
class StateEstimateBase { {
class StateEstimateBase
{
public: public:
virtual ~StateEstimateBase() = default; virtual ~StateEstimateBase() = default;
StateEstimateBase(CentroidalModelInfo info, CtrlInterfaces &ctrl_component, StateEstimateBase(CentroidalModelInfo info, CtrlInterfaces& ctrl_component,
rclcpp_lifecycle::LifecycleNode::SharedPtr node); rclcpp_lifecycle::LifecycleNode::SharedPtr node);
virtual void updateJointStates(); virtual void updateJointStates();
@ -30,25 +32,27 @@ namespace ocs2::legged_robot {
virtual void updateImu(); 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: protected:
void initPublishers(); 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_; CentroidalModelInfo info_;
contact_flag_t contact_flag_{};
double feet_force_threshold_ = 5.0;
vector3_t zyx_offset_ = vector3_t::Zero(); vector3_t zyx_offset_ = vector3_t::Zero();
vector_t rbd_state_; vector_t rbd_state_;
contact_flag_t contact_flag_{};
Eigen::Quaternion<scalar_t> quat_; Eigen::Quaternion<scalar_t> quat_;
vector3_t angular_vel_local_, linear_accel_local_; vector3_t angular_vel_local_, linear_accel_local_;
matrix3_t orientationCovariance_, angularVelCovariance_, linearAccelCovariance_; matrix3_t orientationCovariance_, angularVelCovariance_, linearAccelCovariance_;
@ -58,13 +62,15 @@ namespace ocs2::legged_robot {
rclcpp_lifecycle::LifecycleNode::SharedPtr node_; rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
}; };
template<typename T> template <typename T>
T square(T a) { T square(T a)
{
return a * a; return a * a;
} }
template<typename SCALAR_T> template <typename SCALAR_T>
Eigen::Matrix<SCALAR_T, 3, 1> quatToZyx(const Eigen::Quaternion<SCALAR_T> &q) { Eigen::Matrix<SCALAR_T, 3, 1> quatToZyx(const Eigen::Quaternion<SCALAR_T>& q)
{
Eigen::Matrix<SCALAR_T, 3, 1> zyx; Eigen::Matrix<SCALAR_T, 3, 1> zyx;
SCALAR_T as = std::min(-2. * (q.x() * q.z() - q.w() * q.y()), .99999); SCALAR_T as = std::min(-2. * (q.x() * q.z() - q.w() * q.y()), .99999);

91
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h
View File

@ -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. // Created by qiayuan on 2022/7/16.
// //
#ifndef LEGGEDINTERFACE_H
#define LEGGEDINTERFACE_H
#pragma once
#include <ocs2_centroidal_model/CentroidalModelInfo.h> #include <ocs2_centroidal_model/CentroidalModelInfo.h>
#include <ocs2_core/Types.h> #include <ocs2_core/Types.h>
@ -24,85 +22,89 @@
#include "SwitchedModelReferenceManager.h" #include "SwitchedModelReferenceManager.h"
namespace ocs2::legged_robot { namespace ocs2::legged_robot
class LeggedInterface final : public RobotInterface { {
class LeggedInterface final : public RobotInterface
{
public: public:
LeggedInterface(const std::string &task_file, LeggedInterface(const std::string& task_file,
const std::string &urdf_file, const std::string& urdf_file,
const std::string &reference_file, const std::string& reference_file,
bool use_hard_friction_cone_constraint = false); bool use_hard_friction_cone_constraint = false);
~LeggedInterface() override = default; ~LeggedInterface() override = default;
void setupJointNames(const std::vector<std::string> &joint_names, void setupJointNames(const std::vector<std::string>& joint_names,
const std::vector<std::string> &foot_names); const std::vector<std::string>& foot_names);
void setupOptimalControlProblem(const std::string &task_file, void setupOptimalControlProblem(const std::string& task_file,
const std::string &urdf_file, const std::string& urdf_file,
const std::string &reference_file, const std::string& reference_file,
bool verbose); 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 ModelSettings& modelSettings() const { return model_settings_; }
const ddp::Settings &ddpSettings() const { return ddp_settings_; } const ddp::Settings& ddpSettings() const { return ddp_settings_; }
const mpc::Settings &mpcSettings() const { return mpc_settings_; } const mpc::Settings& mpcSettings() const { return mpc_settings_; }
const sqp::Settings &sqpSettings() { return sqp_settings_; } const sqp::Settings& sqpSettings() { return sqp_settings_; }
const RolloutBase &getRollout() const { return *rollout_ptr_; } const RolloutBase& getRollout() const { return *rollout_ptr_; }
PinocchioInterface &getPinocchioInterface() { return *pinocchio_interface_ptr_; } PinocchioInterface& getPinocchioInterface() { return *pinocchio_interface_ptr_; }
const CentroidalModelInfo &getCentroidalModelInfo() const { return centroidal_model_info_; } const CentroidalModelInfo& getCentroidalModelInfo() const { return centroidal_model_info_; }
std::shared_ptr<SwitchedModelReferenceManager> getSwitchedModelReferenceManagerPtr() const { std::shared_ptr<SwitchedModelReferenceManager> getSwitchedModelReferenceManagerPtr() const
{
return reference_manager_ptr_; return reference_manager_ptr_;
} }
const Initializer &getInitializer() const override { return *initializer_ptr_; } const Initializer& getInitializer() const override { return *initializer_ptr_; }
std::shared_ptr<ReferenceManagerInterface> getReferenceManagerPtr() const override { std::shared_ptr<ReferenceManagerInterface> getReferenceManagerPtr() const override
{
return reference_manager_ptr_; return reference_manager_ptr_;
} }
protected: protected:
void setupModel(const std::string &task_file, const std::string &urdf_file, void setupModel(const std::string& task_file, const std::string& urdf_file,
const std::string &reference_file); const std::string& reference_file);
void setupReferenceManager(const std::string &taskFile, const std::string &urdfFile, void setupReferenceManager(const std::string& taskFile, const std::string& urdfFile,
const std::string &referenceFile, const std::string& referenceFile,
bool verbose); bool verbose);
void setupPreComputation(const std::string &taskFile, const std::string &urdfFile, void setupPreComputation(const std::string& taskFile, const std::string& urdfFile,
const std::string &referenceFile, const std::string& referenceFile,
bool verbose); bool verbose);
std::shared_ptr<GaitSchedule> loadGaitSchedule(const std::string &file, bool verbose) const; std::shared_ptr<GaitSchedule> loadGaitSchedule(const std::string& file, bool verbose) const;
std::unique_ptr<StateInputCost> getBaseTrackingCost(const std::string &taskFile, std::unique_ptr<StateInputCost> getBaseTrackingCost(const std::string& taskFile,
const CentroidalModelInfo &info, bool verbose); 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<scalar_t, RelaxedBarrierPenalty::Config> loadFrictionConeSettings( static std::pair<scalar_t, RelaxedBarrierPenalty::Config> loadFrictionConeSettings(
const std::string &taskFile, bool verbose); const std::string& taskFile, bool verbose);
std::unique_ptr<StateInputConstraint> getFrictionConeConstraint(size_t contactPointIndex, std::unique_ptr<StateInputConstraint> getFrictionConeConstraint(size_t contactPointIndex,
scalar_t frictionCoefficient); scalar_t frictionCoefficient);
std::unique_ptr<StateInputCost> getFrictionConeSoftConstraint(size_t contactPointIndex, std::unique_ptr<StateInputCost> getFrictionConeSoftConstraint(size_t contactPointIndex,
scalar_t frictionCoefficient, scalar_t frictionCoefficient,
const RelaxedBarrierPenalty::Config & const RelaxedBarrierPenalty::Config&
barrierPenaltyConfig); barrierPenaltyConfig);
std::unique_ptr<EndEffectorKinematics<scalar_t> > getEeKinematicsPtr(const std::vector<std::string> &foot_names, std::unique_ptr<EndEffectorKinematics<scalar_t>> getEeKinematicsPtr(const std::vector<std::string>& foot_names,
const std::string &model_name); const std::string& model_name);
std::unique_ptr<StateInputConstraint> getZeroVelocityConstraint( std::unique_ptr<StateInputConstraint> getZeroVelocityConstraint(
const EndEffectorKinematics<scalar_t> &end_effector_kinematics, const EndEffectorKinematics<scalar_t>& end_effector_kinematics,
size_t contact_point_index); size_t contact_point_index);
std::unique_ptr<StateCost> getSelfCollisionConstraint(const PinocchioInterface &pinocchioInterface, std::unique_ptr<StateCost> getSelfCollisionConstraint(const PinocchioInterface& pinocchioInterface,
const std::string &taskFile, const std::string& taskFile,
const std::string &prefix, bool verbose); const std::string& prefix, bool verbose);
ModelSettings model_settings_; ModelSettings model_settings_;
mpc::Settings mpc_settings_; mpc::Settings mpc_settings_;
@ -125,5 +127,4 @@ namespace ocs2::legged_robot {
vector_t initial_state_; vector_t initial_state_;
}; };
} // namespace legged } // namespace legged
#endif // LEGGEDINTERFACE_H
#pragma clang diagnostic pop

12
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HierarchicalWbc.h
View File

@ -1,17 +1,21 @@
// //
// Created by qiayuan on 22-12-23. // Created by qiayuan on 22-12-23.
// //
#pragma once #ifndef HIERARCHICALWBC_H
#define HIERARCHICALWBC_H
#include "WbcBase.h" #include "WbcBase.h"
namespace ocs2::legged_robot { namespace ocs2::legged_robot
class HierarchicalWbc final : public WbcBase { {
class HierarchicalWbc final : public WbcBase
{
public: public:
using WbcBase::WbcBase; 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, size_t mode,
scalar_t period) override; scalar_t period) override;
}; };
} // namespace legged } // namespace legged
#endif // HIERARCHICALWBC_H

17
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/HoQp.h
View File

@ -4,22 +4,25 @@
// //
// Ref: https://github.com/bernhardpg/quadruped_locomotion // Ref: https://github.com/bernhardpg/quadruped_locomotion
// //
#ifndef HOQP_H
#pragma once #define HOQP_H
#include "Task.h" #include "Task.h"
#include <memory> #include <memory>
namespace ocs2::legged_robot{ namespace ocs2::legged_robot
{
// Hierarchical Optimization Quadratic Program // Hierarchical Optimization Quadratic Program
class HoQp { class HoQp
{
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
using HoQpPtr = std::shared_ptr<HoQp>; using HoQpPtr = std::shared_ptr<HoQp>;
explicit HoQp(const Task &task) : HoQp(task, nullptr) { explicit HoQp(const Task& task) : HoQp(task, nullptr)
{
} }
HoQp(Task task, HoQpPtr higherProblem); HoQp(Task task, HoQpPtr higherProblem);
@ -30,7 +33,8 @@ namespace ocs2::legged_robot{
vector_t getStackedSlackSolutions() const { return stackedSlackVars_; } vector_t getStackedSlackSolutions() const { return stackedSlackVars_; }
vector_t getSolutions() const { vector_t getSolutions() const
{
vector_t x = xPrev_ + stackedZPrev_ * decisionVarsSolutions_; vector_t x = xPrev_ + stackedZPrev_ * decisionVarsSolutions_;
return x; return x;
} }
@ -74,3 +78,4 @@ namespace ocs2::legged_robot{
matrix_t zeroNvNx_; matrix_t zeroNvNx_;
}; };
} // namespace legged } // namespace legged
#endif // HOQP_H

42
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/Task.h
View File

@ -4,38 +4,43 @@
// //
// Ref: https://github.com/bernhardpg/quadruped_locomotion // Ref: https://github.com/bernhardpg/quadruped_locomotion
// //
#ifndef TASK_H
#pragma once #define TASK_H
#include <ocs2_core/Types.h> #include <ocs2_core/Types.h>
#include <utility> #include <utility>
namespace ocs2::legged_robot { namespace ocs2::legged_robot
{
class Task { class Task
{
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Task() = default; 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)), 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) explicit Task(size_t numDecisionVars)
: Task(matrix_t::Zero(0, numDecisionVars), vector_t::Zero(0), matrix_t::Zero(0, 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 { return {
concatenateMatrices(a_, rhs.a_), concatenateVectors(b_, rhs.b_), concatenateMatrices(d_, rhs.d_), concatenateMatrices(a_, rhs.a_), concatenateVectors(b_, rhs.b_), concatenateMatrices(d_, rhs.d_),
concatenateVectors(f_, rhs.f_) concatenateVectors(f_, rhs.f_)
}; };
} }
Task operator*(scalar_t rhs) const { Task operator*(scalar_t rhs) const
{
// clang-format off // clang-format off
return { return {
a_.cols() > 0 ? rhs * a_ : a_, a_.cols() > 0 ? rhs * a_ : a_,
@ -48,11 +53,14 @@ namespace ocs2::legged_robot {
matrix_t a_, d_; matrix_t a_, d_;
vector_t b_, f_; vector_t b_, f_;
static matrix_t concatenateMatrices(matrix_t m1, matrix_t m2) { static matrix_t concatenateMatrices(matrix_t m1, matrix_t m2)
if (m1.cols() <= 0) { {
if (m1.cols() <= 0)
{
return m2; return m2;
} }
if (m2.cols() <= 0) { if (m2.cols() <= 0)
{
return m1; return m1;
} }
assert(m1.cols() == m2.cols()); assert(m1.cols() == m2.cols());
@ -61,11 +69,14 @@ namespace ocs2::legged_robot {
return res; return res;
} }
static vector_t concatenateVectors(const vector_t &v1, const vector_t &v2) { static vector_t concatenateVectors(const vector_t& v1, const vector_t& v2)
if (v1.cols() <= 0) { {
if (v1.cols() <= 0)
{
return v2; return v2;
} }
if (v2.cols() <= 0) { if (v2.cols() <= 0)
{
return v1; return v1;
} }
assert(v1.cols() == v2.cols()); assert(v1.cols() == v2.cols());
@ -75,3 +86,4 @@ namespace ocs2::legged_robot {
} }
}; };
} // namespace legged } // namespace legged
#endif // TASK_H

29
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WbcBase.h
View File

@ -1,8 +1,8 @@
// //
// Created by qiayuan on 2022/7/1. // Created by qiayuan on 2022/7/1.
// //
#ifndef WBCBASE_H
#pragma once #define WBCBASE_H
#include "Task.h" #include "Task.h"
@ -10,28 +10,30 @@
#include <ocs2_legged_robot/gait/MotionPhaseDefinition.h> #include <ocs2_legged_robot/gait/MotionPhaseDefinition.h>
#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h> #include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h>
namespace ocs2::legged_robot { namespace ocs2::legged_robot
{
// Decision Variables: x = [\dot u^T, F^T, \tau^T]^T // Decision Variables: x = [\dot u^T, F^T, \tau^T]^T
class WbcBase { class WbcBase
{
using Vector6 = Eigen::Matrix<scalar_t, 6, 1>; using Vector6 = Eigen::Matrix<scalar_t, 6, 1>;
using Matrix6 = Eigen::Matrix<scalar_t, 6, 6>; using Matrix6 = Eigen::Matrix<scalar_t, 6, 6>;
public: public:
virtual ~WbcBase() = default; virtual ~WbcBase() = default;
WbcBase(const PinocchioInterface &pinocchioInterface, CentroidalModelInfo info, WbcBase(const PinocchioInterface& pinocchioInterface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics &eeKinematics); 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, virtual vector_t update(const vector_t& stateDesired, const vector_t& inputDesired,
const vector_t &rbdStateMeasured, size_t mode, const vector_t& rbdStateMeasured, size_t mode,
scalar_t period); scalar_t period);
protected: 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_; } size_t getNumDecisionVars() const { return num_decision_vars_; }
@ -43,11 +45,11 @@ namespace ocs2::legged_robot {
Task formulateFrictionConeTask(); 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 formulateSwingLegTask();
Task formulateContactForceTask(const vector_t &inputDesired) const; Task formulateContactForceTask(const vector_t& inputDesired) const;
size_t num_decision_vars_; size_t num_decision_vars_;
PinocchioInterface pinocchio_interface_measured_, pinocchio_interface_desired_; PinocchioInterface pinocchio_interface_measured_, pinocchio_interface_desired_;
@ -66,3 +68,4 @@ namespace ocs2::legged_robot {
scalar_t friction_coeff_{}, swing_kp_{}, swing_kd_{}; scalar_t friction_coeff_{}, swing_kp_{}, swing_kd_{};
}; };
} // namespace legged } // namespace legged
#endif // WBCBASE_H

14
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/wbc/WeightedWbc.h
View File

@ -5,21 +5,23 @@
#include "WbcBase.h" #include "WbcBase.h"
namespace ocs2::legged_robot { namespace ocs2::legged_robot
class WeightedWbc : public WbcBase { {
class WeightedWbc final : public WbcBase
{
public: public:
using WbcBase::WbcBase; 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, size_t mode,
scalar_t period) override; scalar_t period) override;
void loadTasksSetting(const std::string &taskFile, bool verbose) override; void loadTasksSetting(const std::string& taskFile, bool verbose) override;
protected: protected:
virtual Task formulateConstraints(); Task formulateConstraints();
virtual Task formulateWeightedTasks(const vector_t &stateDesired, const vector_t &inputDesired, Task formulateWeightedTasks(const vector_t& stateDesired, const vector_t& inputDesired,
scalar_t period); scalar_t period);
private: private:

12
controllers/ocs2_quadruped_controller/launch/gazebo.launch.py
View File

@ -1,17 +1,15 @@
import os import os
import xacro
from ament_index_python.packages import get_package_share_directory from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription from launch import LaunchDescription
from launch_ros.actions import Node from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler, \
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler, TimerAction TimerAction
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node from launch_ros.actions import Node
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare from launch_ros.substitutions import FindPackageShare
from launch.event_handlers import OnProcessExit
import xacro
def launch_setup(context, *args, **kwargs): def launch_setup(context, *args, **kwargs):

259
controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp
View File

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

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

@ -12,18 +12,25 @@
#include <angles/angles.h> #include <angles/angles.h>
#include <ocs2_quadruped_controller/control/GaitManager.h> #include <ocs2_quadruped_controller/control/GaitManager.h>
namespace ocs2::legged_robot { namespace ocs2::legged_robot
{
using config_type = controller_interface::interface_configuration_type; 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, {}}; controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
conf.names.reserve(joint_names_.size() * command_interface_types_.size()); conf.names.reserve(joint_names_.size() * command_interface_types_.size());
for (const auto &joint_name: joint_names_) { for (const auto& joint_name : joint_names_)
for (const auto &interface_type: command_interface_types_) { {
if (!command_prefix_.empty()) { for (const auto& interface_type : command_interface_types_)
{
if (!command_prefix_.empty())
{
conf.names.push_back(command_prefix_ + "/" + joint_name + "/" += interface_type); conf.names.push_back(command_prefix_ + "/" + joint_name + "/" += interface_type);
} else { }
else
{
conf.names.push_back(joint_name + "/" += interface_type); conf.names.push_back(joint_name + "/" += interface_type);
} }
} }
@ -32,45 +39,58 @@ namespace ocs2::legged_robot {
return conf; 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, {}}; controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
conf.names.reserve(joint_names_.size() * state_interface_types_.size()); conf.names.reserve(joint_names_.size() * state_interface_types_.size());
for (const auto &joint_name: joint_names_) { for (const auto& joint_name : joint_names_)
for (const auto &interface_type: state_interface_types_) { {
for (const auto& interface_type : state_interface_types_)
{
conf.names.push_back(joint_name + "/" += interface_type); 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); conf.names.push_back(imu_name_ + "/" += interface_type);
} }
if (estimator_type_ == "ground_truth") { if (estimator_type_ == "ground_truth")
for (const auto &interface_type: odom_interface_types_) { {
for (const auto& interface_type : odom_interface_types_)
{
conf.names.push_back(odom_name_ + "/" += interface_type); 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; return conf;
} }
controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time &time, controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time& time,
const rclcpp::Duration &period) { const rclcpp::Duration& period)
if (mode_ == FSMMode::NORMAL) { {
ctrl_comp_->updateState(time, period);
if (mode_ == FSMMode::NORMAL)
{
current_state_->run(time, period); current_state_->run(time, period);
next_state_name_ = current_state_->checkChange(); 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; mode_ = FSMMode::CHANGE;
next_state_ = getNextState(next_state_name_); next_state_ = getNextState(next_state_name_);
RCLCPP_INFO(get_node()->get_logger(), "Switched from %s to %s", 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()); 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_->exit();
current_state_ = next_state_; current_state_ = next_state_;
@ -81,56 +101,50 @@ namespace ocs2::legged_robot {
return controller_interface::return_type::OK; return controller_interface::return_type::OK;
} }
controller_interface::CallbackReturn Ocs2QuadrupedController::on_init() { controller_interface::CallbackReturn Ocs2QuadrupedController::on_init()
// Initialize OCS2 {
robot_pkg_ = auto_declare<std::string>("robot_pkg", robot_pkg_);
const std::string package_share_directory = ament_index_cpp::get_package_share_directory(robot_pkg_);
get_node()->get_parameter("update_rate", ctrl_interfaces_.frequency_); get_node()->get_parameter("update_rate", ctrl_interfaces_.frequency_);
RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_interfaces_.frequency_); RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_interfaces_.frequency_);
// Hardware Parameters // Hardware Parameters
command_prefix_ = auto_declare<std::string>("command_prefix", command_prefix_); command_prefix_ = auto_declare<std::string>("command_prefix", command_prefix_);
joint_names_ = auto_declare<std::vector<std::string> >("joints", joint_names_); joint_names_ = auto_declare<std::vector<std::string>>("joints", joint_names_);
feet_names_ = auto_declare<std::vector<std::string> >("feet", feet_names_);
command_interface_types_ = command_interface_types_ =
auto_declare<std::vector<std::string> >("command_interfaces", command_interface_types_); auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
state_interface_types_ = state_interface_types_ =
auto_declare<std::vector<std::string> >("state_interfaces", state_interface_types_); auto_declare<std::vector<std::string>>("state_interfaces", state_interface_types_);
// IMU Sensor // IMU Sensor
imu_name_ = auto_declare<std::string>("imu_name", imu_name_); imu_name_ = auto_declare<std::string>("imu_name", imu_name_);
imu_interface_types_ = auto_declare<std::vector<std::string> >("imu_interfaces", state_interface_types_); imu_interface_types_ = auto_declare<std::vector<std::string>>("imu_interfaces", state_interface_types_);
// Odometer Sensor (Ground Truth) // Odometer Sensor (Ground Truth)
estimator_type_ = auto_declare<std::string>("estimator_type", estimator_type_); estimator_type_ = auto_declare<std::string>("estimator_type", estimator_type_);
if (estimator_type_ == "ground_truth") { if (estimator_type_ == "ground_truth")
{
odom_name_ = auto_declare<std::string>("odom_name", odom_name_); odom_name_ = auto_declare<std::string>("odom_name", odom_name_);
odom_interface_types_ = auto_declare<std::vector<std::string> >("odom_interfaces", state_interface_types_); odom_interface_types_ = auto_declare<std::vector<std::string>>("odom_interfaces", state_interface_types_);
} else { }
// Foot Force Sensor // Foot Force Sensor
foot_force_name_ = auto_declare<std::string>("foot_force_name", foot_force_name_); foot_force_name_ = auto_declare<std::string>("foot_force_name", foot_force_name_);
foot_force_interface_types_ = foot_force_interface_types_ =
auto_declare<std::vector<std::string> >("foot_force_interfaces", state_interface_types_); auto_declare<std::vector<std::string>>("foot_force_interfaces", state_interface_types_);
}
// PD gains ctrl_comp_ = std::make_shared<CtrlComponent>(get_node(), ctrl_interfaces_);
default_kp_ = auto_declare<double>("default_kp", default_kp_); ctrl_comp_->setupStateEstimate(estimator_type_);
default_kd_ = auto_declare<double>("default_kd", default_kd_);
state_list_.passive = std::make_shared<StatePassive>(ctrl_interfaces_); state_list_.passive = std::make_shared<StatePassive>(ctrl_interfaces_);
state_list_.fixedDown = std::make_shared<StateOCS2>(ctrl_interfaces_, get_node(), state_list_.fixedDown = std::make_shared<StateOCS2>(ctrl_interfaces_, ctrl_comp_);
package_share_directory, joint_names_, feet_names_,
default_kp_, default_kd_);
state_list_.fixedDown->setupStateEstimate(estimator_type_);
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
controller_interface::CallbackReturn Ocs2QuadrupedController::on_configure( controller_interface::CallbackReturn Ocs2QuadrupedController::on_configure(
const rclcpp_lifecycle::State & /*previous_state*/) { const rclcpp_lifecycle::State& /*previous_state*/)
{
control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>( control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
"/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) { "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg)
{
// Handle message // Handle message
ctrl_interfaces_.control_inputs_.command = msg->command; ctrl_interfaces_.control_inputs_.command = msg->command;
ctrl_interfaces_.control_inputs_.lx = msg->lx; ctrl_interfaces_.control_inputs_.lx = msg->lx;
@ -143,29 +157,42 @@ namespace ocs2::legged_robot {
} }
controller_interface::CallbackReturn Ocs2QuadrupedController::on_activate( 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 // clear out vectors in case of restart
ctrl_interfaces_.clear(); ctrl_interfaces_.clear();
// assign command interfaces // assign command interfaces
for (auto &interface: command_interfaces_) { for (auto& interface : command_interfaces_)
{
std::string interface_name = interface.get_interface_name(); 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); command_interface_map_[interface_name.substr(pos + 1)]->push_back(interface);
} else { }
else
{
command_interface_map_[interface_name]->push_back(interface); command_interface_map_[interface_name]->push_back(interface);
} }
} }
// assign state interfaces // assign state interfaces
for (auto &interface: state_interfaces_) { for (auto& interface : state_interfaces_)
if (interface.get_prefix_name() == imu_name_) { {
if (interface.get_prefix_name() == imu_name_)
{
ctrl_interfaces_.imu_state_interface_.emplace_back(interface); 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); 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); ctrl_interfaces_.odom_state_interface_.emplace_back(interface);
} else { }
else
{
state_interface_map_[interface.get_interface_name()]->push_back(interface); state_interface_map_[interface.get_interface_name()]->push_back(interface);
} }
} }
@ -180,28 +207,34 @@ namespace ocs2::legged_robot {
} }
controller_interface::CallbackReturn Ocs2QuadrupedController::on_deactivate( controller_interface::CallbackReturn Ocs2QuadrupedController::on_deactivate(
const rclcpp_lifecycle::State & /*previous_state*/) { const rclcpp_lifecycle::State& /*previous_state*/)
{
release_interfaces(); release_interfaces();
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
controller_interface::CallbackReturn Ocs2QuadrupedController::on_cleanup( controller_interface::CallbackReturn Ocs2QuadrupedController::on_cleanup(
const rclcpp_lifecycle::State & /*previous_state*/) { const rclcpp_lifecycle::State& /*previous_state*/)
{
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
controller_interface::CallbackReturn Ocs2QuadrupedController::on_shutdown( controller_interface::CallbackReturn Ocs2QuadrupedController::on_shutdown(
const rclcpp_lifecycle::State & /*previous_state*/) { const rclcpp_lifecycle::State& /*previous_state*/)
{
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
controller_interface::CallbackReturn Ocs2QuadrupedController::on_error( controller_interface::CallbackReturn Ocs2QuadrupedController::on_error(
const rclcpp_lifecycle::State & /*previous_state*/) { const rclcpp_lifecycle::State& /*previous_state*/)
{
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
std::shared_ptr<FSMState> Ocs2QuadrupedController::getNextState(FSMStateName stateName) const { std::shared_ptr<FSMState> Ocs2QuadrupedController::getNextState(const FSMStateName stateName) const
switch (stateName) { {
switch (stateName)
{
case FSMStateName::PASSIVE: case FSMStateName::PASSIVE:
return state_list_.passive; return state_list_.passive;
case FSMStateName::FIXEDDOWN: case FSMStateName::FIXEDDOWN:

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

@ -8,11 +8,8 @@
#include <controller_common/FSM/StatePassive.h> #include <controller_common/FSM/StatePassive.h>
#include <controller_interface/controller_interface.hpp> #include <controller_interface/controller_interface.hpp>
#include <control_input_msgs/msg/inputs.hpp> #include <control_input_msgs/msg/inputs.hpp>
#include <ocs2_mpc/MPC_MRT_Interface.h>
#include <ocs2_quadruped_controller/estimator/StateEstimateBase.h>
#include <ocs2_quadruped_controller/interface/LeggedInterface.h>
#include <ocs2_quadruped_controller/wbc/WbcBase.h>
#include <ocs2_quadruped_controller/FSM/StateOCS2.h> #include <ocs2_quadruped_controller/FSM/StateOCS2.h>
#include <ocs2_quadruped_controller/control/CtrlComponent.h>
namespace ocs2::legged_robot { namespace ocs2::legged_robot {
struct FSMStateList { struct FSMStateList {
@ -66,8 +63,8 @@ namespace ocs2::legged_robot {
std::shared_ptr<FSMState> next_state_; std::shared_ptr<FSMState> next_state_;
CtrlInterfaces ctrl_interfaces_; CtrlInterfaces ctrl_interfaces_;
std::shared_ptr<CtrlComponent> ctrl_comp_;
std::vector<std::string> joint_names_; std::vector<std::string> joint_names_;
std::vector<std::string> feet_names_;
std::vector<std::string> command_interface_types_; std::vector<std::string> command_interface_types_;
std::vector<std::string> state_interface_types_; std::vector<std::string> state_interface_types_;
@ -88,7 +85,6 @@ namespace ocs2::legged_robot {
{"velocity", &ctrl_interfaces_.joint_velocity_state_interface_} {"velocity", &ctrl_interfaces_.joint_velocity_state_interface_}
}; };
std::string robot_pkg_;
std::string command_prefix_; std::string command_prefix_;
// IMU Sensor // IMU Sensor
@ -104,9 +100,6 @@ namespace ocs2::legged_robot {
std::string odom_name_; std::string odom_name_;
std::vector<std::string> odom_interface_types_; std::vector<std::string> odom_interface_types_;
double default_kp_ = 0;
double default_kd_ = 6;
rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_; rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_;
}; };
} }

205
controllers/ocs2_quadruped_controller/src/control/CtrlComponent.cpp Normal file
View File

@ -0,0 +1,205 @@
//
// Created by biao on 3/15/25.
//
#include "ocs2_quadruped_controller/control/CtrlComponent.h"
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <angles/angles.h>
#include <ocs2_core/misc/LoadData.h>
#include <ocs2_core/thread_support/SetThreadPriority.h>
#include <ocs2_quadruped_controller/estimator/FromOdomTopic.h>
#include <ocs2_quadruped_controller/estimator/GroundTruth.h>
#include <ocs2_quadruped_controller/estimator/LinearKalmanFilter.h>
#include <ocs2_centroidal_model/CentroidalModelRbdConversions.h>
#include <ocs2_core/thread_support/ExecuteAndSleep.h>
#include <ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h>
#include <ocs2_quadruped_controller/control/GaitManager.h>
#include <ocs2_sqp/SqpMpc.h>
namespace ocs2::legged_robot
{
CtrlComponent::CtrlComponent(const std::shared_ptr<rclcpp_lifecycle::LifecycleNode>& 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<PinocchioEndEffectorKinematics>(
legged_interface_->getPinocchioInterface(), pinocchio_mapping,
legged_interface_->modelSettings().contactNames3DoF);
rbd_conversions_ = std::make_shared<CentroidalModelRbdConversions>(legged_interface_->getPinocchioInterface(),
legged_interface_->getCentroidalModelInfo());
// Init visulaizer
visualizer_ = std::make_shared<LeggedRobotVisualizer>(
legged_interface_->getPinocchioInterface(),
legged_interface_->getCentroidalModelInfo(),
*ee_kinematics_,
node_);
// Init observation
observation_.state.setZero(static_cast<long>(legged_interface_->getCentroidalModelInfo().stateDim));
observation_.input.setZero(
static_cast<long>(legged_interface_->getCentroidalModelInfo().inputDim));
observation_.mode = STANCE;
}
void CtrlComponent::setupStateEstimate(const std::string& estimator_type)
{
if (estimator_type == "ground_truth")
{
estimator_ = std::make_shared<GroundTruth>(legged_interface_->getCentroidalModelInfo(),
ctrl_interfaces_,
node_);
RCLCPP_INFO(node_->get_logger(), "Using Ground Truth Estimator");
}
else if (estimator_type == "linear_kalman")
{
estimator_ = std::make_shared<KalmanFilterEstimate>(
legged_interface_->getPinocchioInterface(),
legged_interface_->getCentroidalModelInfo(),
*ee_kinematics_, ctrl_interfaces_,
node_);
dynamic_cast<KalmanFilterEstimate&>(*estimator_).loadSettings(task_file_, verbose_);
RCLCPP_INFO(node_->get_logger(), "Using Kalman Filter Estimator");
}
else
{
estimator_ = std::make_shared<FromOdomTopic>(
legged_interface_->getCentroidalModelInfo(), ctrl_interfaces_, node_);
RCLCPP_INFO(node_->get_logger(), "Using Odom Topic Based Estimator");
}
observation_.time = 0;
}
void 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<LeggedInterface>(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<SqpMpc>(legged_interface_->mpcSettings(),
legged_interface_->sqpSettings(),
legged_interface_->getOptimalControlProblem(),
legged_interface_->getInitializer());
// Initialize the reference manager
const auto gait_manager_ptr = std::make_shared<GaitManager>(
ctrl_interfaces_,
legged_interface_->getSwitchedModelReferenceManagerPtr()->
getGaitSchedule());
gait_manager_ptr->init(gait_file_);
mpc_->getSolverPtr()->addSynchronizedModule(gait_manager_ptr);
mpc_->getSolverPtr()->setReferenceManager(legged_interface_->getReferenceManagerPtr());
target_manager_ = std::make_shared<TargetManager>(ctrl_interfaces_,
legged_interface_->getReferenceManagerPtr(),
task_file_,
reference_file_);
}
void CtrlComponent::setupMrt()
{
mpc_mrt_interface_ = std::make_shared<MPC_MRT_Interface>(*mpc_);
mpc_mrt_interface_->initRollout(&legged_interface_->getRollout());
mpc_timer_.reset();
controller_running_ = true;
mpc_thread_ = std::thread([&]
{
while (controller_running_)
{
try
{
executeAndSleep(
[&]
{
if (mpc_running_)
{
mpc_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.");
}
}

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

@ -19,8 +19,8 @@ namespace ocs2::legged_robot
} }
void GaitManager::preSolverRun(const scalar_t initTime, const scalar_t finalTime, void GaitManager::preSolverRun(const scalar_t initTime, const scalar_t finalTime,
const vector_t& currentState, const vector_t& /**currentState**/,
const ReferenceManagerInterface& referenceManager) const ReferenceManagerInterface& /**referenceManager**/)
{ {
getTargetGait(); getTargetGait();
if (gait_updated_) 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 == 0) return;
if (ctrl_interfaces_.control_inputs_.command == last_command_) return; if (ctrl_interfaces_.control_inputs_.command == last_command_) return;
last_command_ = ctrl_interfaces_.control_inputs_.command; 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", 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; gait_updated_ = true;
} }
} }

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

@ -24,7 +24,7 @@ namespace ocs2::legged_robot
loadData::loadCppDataType(reference_file, "targetDisplacementVelocity", target_displacement_velocity_); 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); vector_t cmdGoal = vector_t::Zero(6);
cmdGoal[0] = ctrl_component_.control_inputs_.ly * target_displacement_velocity_; cmdGoal[0] = ctrl_component_.control_inputs_.ly * target_displacement_velocity_;

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

@ -21,15 +21,15 @@ namespace ocs2::legged_robot
updateImu(); updateImu();
position_ = { position_ = {
ctrl_component_.odom_state_interface_[0].get().get_value(), ctrl_component_.odom_state_interface_[0].get().get_optional().value(),
ctrl_component_.odom_state_interface_[1].get().get_value(), ctrl_component_.odom_state_interface_[1].get().get_optional().value(),
ctrl_component_.odom_state_interface_[2].get().get_value() ctrl_component_.odom_state_interface_[2].get().get_optional().value()
}; };
linear_velocity_ = { linear_velocity_ = {
ctrl_component_.odom_state_interface_[3].get().get_value(), ctrl_component_.odom_state_interface_[3].get().get_optional().value(),
ctrl_component_.odom_state_interface_[4].get().get_value(), ctrl_component_.odom_state_interface_[4].get().get_optional().value(),
ctrl_component_.odom_state_interface_[5].get().get_value() ctrl_component_.odom_state_interface_[5].get().get_optional().value()
}; };
updateLinear(position_, linear_velocity_); updateLinear(position_, linear_velocity_);

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

@ -9,55 +9,65 @@
#include <ocs2_robotic_tools/common/RotationDerivativesTransforms.h> #include <ocs2_robotic_tools/common/RotationDerivativesTransforms.h>
#include <memory> #include <memory>
#include <utility>
namespace ocs2::legged_robot { namespace ocs2::legged_robot
{
StateEstimateBase::StateEstimateBase(CentroidalModelInfo info, StateEstimateBase::StateEstimateBase(CentroidalModelInfo info,
CtrlInterfaces &ctrl_component, CtrlInterfaces& ctrl_component,
rclcpp_lifecycle::LifecycleNode::SharedPtr node) rclcpp_lifecycle::LifecycleNode::SharedPtr node)
: ctrl_component_(ctrl_component), : ctrl_component_(ctrl_component),
info_(std::move(info)), 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(); const size_t size = ctrl_component_.joint_effort_state_interface_.size();
vector_t joint_pos(size), joint_vel(size); vector_t joint_pos(size), joint_vel(size);
for (int i = 0; i < size; i++) { 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(); 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_.actuatedDofNum) = joint_pos;
rbd_state_.segment(6 + info_.generalizedCoordinatesNum, info_.actuatedDofNum) = joint_vel; 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(); const size_t size = ctrl_component_.foot_force_state_interface_.size();
for (int i = 0; i < size; i++) { for (int i = 0; i < size; i++)
contact_flag_[i] = ctrl_component_.foot_force_state_interface_[i].get().get_value() > 5.0; {
contact_flag_[i] = ctrl_component_.foot_force_state_interface_[i].get().get_optional().value() >
feet_force_threshold_;
} }
} }
void StateEstimateBase::updateImu() { void StateEstimateBase::updateImu()
{
quat_ = { quat_ = {
ctrl_component_.imu_state_interface_[0].get().get_value(), ctrl_component_.imu_state_interface_[0].get().get_optional().value(),
ctrl_component_.imu_state_interface_[1].get().get_value(), ctrl_component_.imu_state_interface_[1].get().get_optional().value(),
ctrl_component_.imu_state_interface_[2].get().get_value(), ctrl_component_.imu_state_interface_[2].get().get_optional().value(),
ctrl_component_.imu_state_interface_[3].get().get_value() ctrl_component_.imu_state_interface_[3].get().get_optional().value()
}; };
angular_vel_local_ = { angular_vel_local_ = {
ctrl_component_.imu_state_interface_[4].get().get_value(), ctrl_component_.imu_state_interface_[4].get().get_optional().value(),
ctrl_component_.imu_state_interface_[5].get().get_value(), ctrl_component_.imu_state_interface_[5].get().get_optional().value(),
ctrl_component_.imu_state_interface_[6].get().get_value() ctrl_component_.imu_state_interface_[6].get().get_optional().value()
}; };
linear_accel_local_ = { linear_accel_local_ = {
ctrl_component_.imu_state_interface_[7].get().get_value(), ctrl_component_.imu_state_interface_[7].get().get_optional().value(),
ctrl_component_.imu_state_interface_[8].get().get_value(), ctrl_component_.imu_state_interface_[8].get().get_optional().value(),
ctrl_component_.imu_state_interface_[9].get().get_value() ctrl_component_.imu_state_interface_[9].get().get_optional().value()
}; };
// orientationCovariance_ = orientationCovariance; // orientationCovariance_ = orientationCovariance;
@ -77,17 +87,20 @@ namespace ocs2::legged_robot {
pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("pose", 10); pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("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>(0) = zyx;
rbd_state_.segment<3>(info_.generalizedCoordinatesNum) = angularVel; 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>(3) = pos;
rbd_state_.segment<3>(info_.generalizedCoordinatesNum + 3) = linearVel; 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; rclcpp::Time time = odom.header.stamp;
odom_pub_->publish(odom); odom_pub_->publish(odom);

10
controllers/ocs2_quadruped_controller/src/wbc/HierarchicalWbc.cpp
View File

@ -5,10 +5,12 @@
#include "ocs2_quadruped_controller/wbc/HierarchicalWbc.h" #include "ocs2_quadruped_controller/wbc/HierarchicalWbc.h"
#include "ocs2_quadruped_controller/wbc/HoQp.h" #include "ocs2_quadruped_controller/wbc/HoQp.h"
namespace ocs2::legged_robot { namespace ocs2::legged_robot
vector_t HierarchicalWbc::update(const vector_t &stateDesired, const vector_t &inputDesired, {
const vector_t &rbdStateMeasured, size_t mode, vector_t HierarchicalWbc::update(const vector_t& stateDesired, const vector_t& inputDesired,
scalar_t period) { const vector_t& rbdStateMeasured, size_t mode,
scalar_t period)
{
WbcBase::update(stateDesired, inputDesired, rbdStateMeasured, mode, period); WbcBase::update(stateDesired, inputDesired, rbdStateMeasured, mode, period);
Task task0 = formulateFloatingBaseEomTask() + formulateTorqueLimitsTask() + formulateFrictionConeTask() + Task task0 = formulateFloatingBaseEomTask() + formulateTorqueLimitsTask() + formulateFrictionConeTask() +

82
controllers/ocs2_quadruped_controller/src/wbc/HoQp.cpp
View File

@ -9,8 +9,10 @@
#include <qpOASES.hpp> #include <qpOASES.hpp>
#include <utility> #include <utility>
namespace ocs2::legged_robot { namespace ocs2::legged_robot
HoQp::HoQp(Task task, HoQpPtr higherProblem) : task_(std::move(task)), higherProblem_(std::move(higherProblem)) { {
HoQp::HoQp(Task task, HoQpPtr higherProblem) : task_(std::move(task)), higherProblem_(std::move(higherProblem))
{
initVars(); initVars();
formulateProblem(); formulateProblem();
solveProblem(); solveProblem();
@ -19,14 +21,16 @@ namespace ocs2::legged_robot {
stackSlackSolutions(); stackSlackSolutions();
} }
void HoQp::initVars() { void HoQp::initVars()
{
// Task variables // Task variables
numSlackVars_ = task_.d_.rows(); numSlackVars_ = task_.d_.rows();
hasEqConstraints_ = task_.a_.rows() > 0; hasEqConstraints_ = task_.a_.rows() > 0;
hasIneqConstraints_ = numSlackVars_ > 0; hasIneqConstraints_ = numSlackVars_ > 0;
// Pre-Task variables // Pre-Task variables
if (higherProblem_ != nullptr) { if (higherProblem_ != nullptr)
{
stackedZPrev_ = higherProblem_->getStackedZMatrix(); stackedZPrev_ = higherProblem_->getStackedZMatrix();
stackedTasksPrev_ = higherProblem_->getStackedTasks(); stackedTasksPrev_ = higherProblem_->getStackedTasks();
stackedSlackSolutionsPrev_ = higherProblem_->getStackedSlackSolutions(); stackedSlackSolutionsPrev_ = higherProblem_->getStackedSlackSolutions();
@ -34,7 +38,9 @@ namespace ocs2::legged_robot {
numPrevSlackVars_ = higherProblem_->getSlackedNumVars(); numPrevSlackVars_ = higherProblem_->getSlackedNumVars();
numDecisionVars_ = stackedZPrev_.cols(); numDecisionVars_ = stackedZPrev_.cols();
} else { }
else
{
numDecisionVars_ = std::max(task_.a_.cols(), task_.d_.cols()); numDecisionVars_ = std::max(task_.a_.cols(), task_.d_.cols());
stackedTasksPrev_ = Task(numDecisionVars_); stackedTasksPrev_ = Task(numDecisionVars_);
@ -51,23 +57,28 @@ namespace ocs2::legged_robot {
zeroNvNx_ = matrix_t::Zero(numSlackVars_, numDecisionVars_); zeroNvNx_ = matrix_t::Zero(numSlackVars_, numDecisionVars_);
} }
void HoQp::formulateProblem() { void HoQp::formulateProblem()
{
buildHMatrix(); buildHMatrix();
buildCVector(); buildCVector();
buildDMatrix(); buildDMatrix();
buildFVector(); buildFVector();
} }
void HoQp::buildHMatrix() { void HoQp::buildHMatrix()
{
matrix_t zTaTaz(numDecisionVars_, numDecisionVars_); 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 // 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_; matrix_t aCurrZPrev = task_.a_ * stackedZPrev_;
zTaTaz = aCurrZPrev.transpose() * aCurrZPrev + 1e-12 * matrix_t::Identity( 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 // This way of splitting up the multiplication is about twice as fast as multiplying 4 matrices
} else { }
else
{
zTaTaz.setZero(); zTaTaz.setZero();
} }
@ -77,27 +88,35 @@ namespace ocs2::legged_robot {
.finished(); .finished();
} }
void HoQp::buildCVector() { void HoQp::buildCVector()
{
vector_t c = vector_t::Zero(numDecisionVars_ + numSlackVars_); vector_t c = vector_t::Zero(numDecisionVars_ + numSlackVars_);
vector_t zeroVec = vector_t::Zero(numSlackVars_); vector_t zeroVec = vector_t::Zero(numSlackVars_);
vector_t temp(numDecisionVars_); vector_t temp(numDecisionVars_);
if (hasEqConstraints_) { if (hasEqConstraints_)
{
temp = (task_.a_ * stackedZPrev_).transpose() * (task_.a_ * xPrev_ - task_.b_); temp = (task_.a_ * stackedZPrev_).transpose() * (task_.a_ * xPrev_ - task_.b_);
} else { }
else
{
temp.setZero(); temp.setZero();
} }
c_ = (vector_t(numDecisionVars_ + numSlackVars_) << temp, zeroVec).finished(); c_ = (vector_t(numDecisionVars_ + numSlackVars_) << temp, zeroVec).finished();
} }
void HoQp::buildDMatrix() { void HoQp::buildDMatrix()
{
matrix_t stackedZero = matrix_t::Zero(numPrevSlackVars_, numSlackVars_); matrix_t stackedZero = matrix_t::Zero(numPrevSlackVars_, numSlackVars_);
matrix_t dCurrZ; matrix_t dCurrZ;
if (hasIneqConstraints_) { if (hasIneqConstraints_)
{
dCurrZ = task_.d_ * stackedZPrev_; dCurrZ = task_.d_ * stackedZPrev_;
} else { }
else
{
dCurrZ = matrix_t::Zero(0, numDecisionVars_); dCurrZ = matrix_t::Zero(0, numDecisionVars_);
} }
@ -110,13 +129,17 @@ namespace ocs2::legged_robot {
.finished(); .finished();
} }
void HoQp::buildFVector() { void HoQp::buildFVector()
{
vector_t zeroVec = vector_t::Zero(numSlackVars_); vector_t zeroVec = vector_t::Zero(numSlackVars_);
vector_t fMinusDXPrev; vector_t fMinusDXPrev;
if (hasIneqConstraints_) { if (hasIneqConstraints_)
{
fMinusDXPrev = task_.f_ - task_.d_ * xPrev_; fMinusDXPrev = task_.f_ - task_.d_ * xPrev_;
} else { }
else
{
fMinusDXPrev = vector_t::Zero(0); fMinusDXPrev = vector_t::Zero(0);
} }
@ -125,16 +148,21 @@ namespace ocs2::legged_robot {
.finished(); .finished();
} }
void HoQp::buildZMatrix() { void HoQp::buildZMatrix()
if (hasEqConstraints_) { {
if (hasEqConstraints_)
{
assert((task_.a_.cols() > 0)); assert((task_.a_.cols() > 0));
stackedZ_ = stackedZPrev_ * (task_.a_ * stackedZPrev_).fullPivLu().kernel(); stackedZ_ = stackedZPrev_ * (task_.a_ * stackedZPrev_).fullPivLu().kernel();
} else { }
else
{
stackedZ_ = stackedZPrev_; stackedZ_ = stackedZPrev_;
} }
} }
void HoQp::solveProblem() { void HoQp::solveProblem()
{
auto qpProblem = qpOASES::QProblem(numDecisionVars_ + numSlackVars_, f_.size()); auto qpProblem = qpOASES::QProblem(numDecisionVars_ + numSlackVars_, f_.size());
qpOASES::Options options; qpOASES::Options options;
options.setToMPC(); options.setToMPC();
@ -151,11 +179,15 @@ namespace ocs2::legged_robot {
slackVarsSolutions_ = qpSol.tail(numSlackVars_); slackVarsSolutions_ = qpSol.tail(numSlackVars_);
} }
void HoQp::stackSlackSolutions() { void HoQp::stackSlackSolutions()
if (higherProblem_ != nullptr) { {
if (higherProblem_ != nullptr)
{
stackedSlackVars_ = Task::concatenateVectors(higherProblem_->getStackedSlackSolutions(), stackedSlackVars_ = Task::concatenateVectors(higherProblem_->getStackedSlackSolutions(),
slackVarsSolutions_); slackVarsSolutions_);
} else { }
else
{
stackedSlackVars_ = slackVarsSolutions_; stackedSlackVars_ = slackVarsSolutions_;
} }
} }

117
controllers/ocs2_quadruped_controller/src/wbc/WbcBase.cpp
View File

@ -14,27 +14,32 @@
#include <pinocchio/algorithm/frames.hpp> #include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/rnea.hpp> #include <pinocchio/algorithm/rnea.hpp>
namespace ocs2::legged_robot { namespace ocs2::legged_robot
WbcBase::WbcBase(const PinocchioInterface &pinocchioInterface, CentroidalModelInfo info, {
const PinocchioEndEffectorKinematics &eeKinematics) WbcBase::WbcBase(const PinocchioInterface& pinocchioInterface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics& eeKinematics)
: pinocchio_interface_measured_(pinocchioInterface), : pinocchio_interface_measured_(pinocchioInterface),
pinocchio_interface_desired_(pinocchioInterface), pinocchio_interface_desired_(pinocchioInterface),
info_(std::move(info)), info_(std::move(info)),
ee_kinematics_(eeKinematics.clone()), ee_kinematics_(eeKinematics.clone()),
mapping_(info_), 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; num_decision_vars_ = info_.generalizedCoordinatesNum + 3 * info_.numThreeDofContacts + info_.actuatedDofNum;
q_measured_ = vector_t(info_.generalizedCoordinatesNum); q_measured_ = vector_t(info_.generalizedCoordinatesNum);
v_measured_ = vector_t(info_.generalizedCoordinatesNum); v_measured_ = vector_t(info_.generalizedCoordinatesNum);
} }
vector_t WbcBase::update(const vector_t &stateDesired, const vector_t &inputDesired, vector_t WbcBase::update(const vector_t& stateDesired, const vector_t& inputDesired,
const vector_t &rbdStateMeasured, size_t mode, const vector_t& rbdStateMeasured, size_t mode,
scalar_t /*period*/) { scalar_t /*period*/)
{
contact_flag_ = modeNumber2StanceLeg(mode); contact_flag_ = modeNumber2StanceLeg(mode);
num_contacts_ = 0; num_contacts_ = 0;
for (const bool flag: contact_flag_) { for (const bool flag : contact_flag_)
if (flag) { {
if (flag)
{
num_contacts_++; num_contacts_++;
} }
} }
@ -45,7 +50,8 @@ namespace ocs2::legged_robot {
return {}; 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_.head<3>() = rbdStateMeasured.segment<3>(3);
q_measured_.segment<3>(3) = rbdStateMeasured.head<3>(); q_measured_.segment<3>(3) = rbdStateMeasured.head<3>();
q_measured_.tail(info_.actuatedDofNum) = rbdStateMeasured.segment(6, info_.actuatedDofNum); 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( v_measured_.tail(info_.actuatedDofNum) = rbdStateMeasured.segment(
info_.generalizedCoordinatesNum + 6, info_.actuatedDofNum); info_.generalizedCoordinatesNum + 6, info_.actuatedDofNum);
const auto &model = pinocchio_interface_measured_.getModel(); const auto& model = pinocchio_interface_measured_.getModel();
auto &data = pinocchio_interface_measured_.getData(); auto& data = pinocchio_interface_measured_.getData();
// For floating base EoM task // For floating base EoM task
forwardKinematics(model, data, q_measured_, v_measured_); forwardKinematics(model, data, q_measured_, v_measured_);
@ -66,7 +72,8 @@ namespace ocs2::legged_robot {
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>(); data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();
nonLinearEffects(model, data, q_measured_, v_measured_); nonLinearEffects(model, data, q_measured_, v_measured_);
j_ = matrix_t(3 * info_.numThreeDofContacts, info_.generalizedCoordinatesNum); 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<scalar_t, 6, Eigen::Dynamic> jac; Eigen::Matrix<scalar_t, 6, Eigen::Dynamic> jac;
jac.setZero(6, info_.generalizedCoordinatesNum); jac.setZero(6, info_.generalizedCoordinatesNum);
getFrameJacobian(model, data, info_.endEffectorFrameIndices[i], pinocchio::LOCAL_WORLD_ALIGNED, getFrameJacobian(model, data, info_.endEffectorFrameIndices[i], pinocchio::LOCAL_WORLD_ALIGNED,
@ -77,7 +84,8 @@ namespace ocs2::legged_robot {
// For not contact motion task // For not contact motion task
computeJointJacobiansTimeVariation(model, data, q_measured_, v_measured_); computeJointJacobiansTimeVariation(model, data, q_measured_, v_measured_);
dj_ = matrix_t(3 * info_.numThreeDofContacts, info_.generalizedCoordinatesNum); 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<scalar_t, 6, Eigen::Dynamic> jac; Eigen::Matrix<scalar_t, 6, Eigen::Dynamic> jac;
jac.setZero(6, info_.generalizedCoordinatesNum); jac.setZero(6, info_.generalizedCoordinatesNum);
getFrameJacobianTimeVariation(model, data, info_.endEffectorFrameIndices[i], 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) { void WbcBase::updateDesired(const vector_t& stateDesired, const vector_t& inputDesired)
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();
mapping_.setPinocchioInterface(pinocchio_interface_desired_); mapping_.setPinocchioInterface(pinocchio_interface_desired_);
const auto qDesired = mapping_.getPinocchioJointPosition(stateDesired); const auto qDesired = mapping_.getPinocchioJointPosition(stateDesired);
@ -100,8 +109,9 @@ namespace ocs2::legged_robot {
forwardKinematics(model, data, qDesired, vDesired); forwardKinematics(model, data, qDesired, vDesired);
} }
Task WbcBase::formulateFloatingBaseEomTask() { Task WbcBase::formulateFloatingBaseEomTask()
const auto &data = pinocchio_interface_measured_.getData(); {
const auto& data = pinocchio_interface_measured_.getData();
matrix_t s(info_.actuatedDofNum, info_.generalizedCoordinatesNum); matrix_t s(info_.actuatedDofNum, info_.generalizedCoordinatesNum);
s.block(0, 0, info_.actuatedDofNum, 6).setZero(); s.block(0, 0, info_.actuatedDofNum, 6).setZero();
@ -114,7 +124,8 @@ namespace ocs2::legged_robot {
return {a, b, matrix_t(), vector_t()}; return {a, b, matrix_t(), vector_t()};
} }
Task WbcBase::formulateTorqueLimitsTask() { Task WbcBase::formulateTorqueLimitsTask()
{
matrix_t d(2 * info_.actuatedDofNum, num_decision_vars_); matrix_t d(2 * info_.actuatedDofNum, num_decision_vars_);
d.setZero(); d.setZero();
matrix_t i = matrix_t::Identity(info_.actuatedDofNum, info_.actuatedDofNum); matrix_t i = matrix_t::Identity(info_.actuatedDofNum, info_.actuatedDofNum);
@ -124,21 +135,25 @@ namespace ocs2::legged_robot {
info_.actuatedDofNum, info_.actuatedDofNum,
info_.actuatedDofNum) = -i; info_.actuatedDofNum) = -i;
vector_t f(2 * info_.actuatedDofNum); 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_; f.segment<3>(3 * l) = torque_limits_;
} }
return {matrix_t(), vector_t(), d, f}; return {matrix_t(), vector_t(), d, f};
} }
Task WbcBase::formulateNoContactMotionTask() { Task WbcBase::formulateNoContactMotionTask()
{
matrix_t a(3 * num_contacts_, num_decision_vars_); matrix_t a(3 * num_contacts_, num_decision_vars_);
vector_t b(a.rows()); vector_t b(a.rows());
a.setZero(); a.setZero();
b.setZero(); b.setZero();
size_t j = 0; size_t j = 0;
for (size_t i = 0; i < info_.numThreeDofContacts; i++) { for (size_t i = 0; i < info_.numThreeDofContacts; i++)
if (contact_flag_[i]) { {
if (contact_flag_[i])
{
a.block(3 * j, 0, 3, info_.generalizedCoordinatesNum) = j_.block( a.block(3 * j, 0, 3, info_.generalizedCoordinatesNum) = j_.block(
3 * i, 0, 3, info_.generalizedCoordinatesNum); 3 * i, 0, 3, info_.generalizedCoordinatesNum);
b.segment(3 * j, 3) = -dj_.block(3 * i, 0, 3, info_.generalizedCoordinatesNum) * v_measured_; 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()}; return {a, b, matrix_t(), vector_t()};
} }
Task WbcBase::formulateFrictionConeTask() { Task WbcBase::formulateFrictionConeTask()
{
matrix_t a(3 * (info_.numThreeDofContacts - num_contacts_), num_decision_vars_); matrix_t a(3 * (info_.numThreeDofContacts - num_contacts_), num_decision_vars_);
a.setZero(); a.setZero();
size_t j = 0; size_t j = 0;
for (size_t i = 0; i < info_.numThreeDofContacts; ++i) { for (size_t i = 0; i < info_.numThreeDofContacts; ++i)
if (!contact_flag_[i]) { {
if (!contact_flag_[i])
{
a.block(3 * j++, info_.generalizedCoordinatesNum + 3 * i, 3, 3) = matrix_t::Identity(3, 3); 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_); matrix_t d(5 * num_contacts_ + 3 * (info_.numThreeDofContacts - num_contacts_), num_decision_vars_);
d.setZero(); d.setZero();
j = 0; j = 0;
for (size_t i = 0; i < info_.numThreeDofContacts; ++i) { for (size_t i = 0; i < info_.numThreeDofContacts; ++i)
if (contact_flag_[i]) { {
if (contact_flag_[i])
{
d.block(5 * j++, info_.generalizedCoordinatesNum + 3 * i, 5, 3) = frictionPyramic; d.block(5 * j++, info_.generalizedCoordinatesNum + 3 * i, 5, 3) = frictionPyramic;
} }
} }
@ -181,7 +201,8 @@ namespace ocs2::legged_robot {
return {a, b, d, f}; 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_); matrix_t a(6, num_decision_vars_);
a.setZero(); a.setZero();
a.block(0, 0, 6, 6) = matrix_t::Identity(6, 6); a.block(0, 0, 6, 6) = matrix_t::Identity(6, 6);
@ -190,12 +211,12 @@ namespace ocs2::legged_robot {
input_last_ = inputDesired; input_last_ = inputDesired;
mapping_.setPinocchioInterface(pinocchio_interface_desired_); mapping_.setPinocchioInterface(pinocchio_interface_desired_);
const auto &model = pinocchio_interface_desired_.getModel(); const auto& model = pinocchio_interface_desired_.getModel();
auto &data = pinocchio_interface_desired_.getData(); auto& data = pinocchio_interface_desired_.getData();
const auto qDesired = mapping_.getPinocchioJointPosition(stateDesired); const auto qDesired = mapping_.getPinocchioJointPosition(stateDesired);
const vector_t vDesired = mapping_.getPinocchioJointVelocity(stateDesired, inputDesired); 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 Matrix6 Ab = A.template leftCols<6>();
const auto AbInv = computeFloatingBaseCentroidalMomentumMatrixInverse(Ab); const auto AbInv = computeFloatingBaseCentroidalMomentumMatrixInverse(Ab);
const auto Aj = A.rightCols(info_.actuatedDofNum); const auto Aj = A.rightCols(info_.actuatedDofNum);
@ -210,7 +231,8 @@ namespace ocs2::legged_robot {
return {a, b, matrix_t(), vector_t()}; return {a, b, matrix_t(), vector_t()};
} }
Task WbcBase::formulateSwingLegTask() { Task WbcBase::formulateSwingLegTask()
{
ee_kinematics_->setPinocchioInterface(pinocchio_interface_measured_); ee_kinematics_->setPinocchioInterface(pinocchio_interface_measured_);
std::vector<vector3_t> posMeasured = ee_kinematics_->getPosition(vector_t()); std::vector<vector3_t> posMeasured = ee_kinematics_->getPosition(vector_t());
std::vector<vector3_t> velMeasured = ee_kinematics_->getVelocity(vector_t(), vector_t()); std::vector<vector3_t> velMeasured = ee_kinematics_->getVelocity(vector_t(), vector_t());
@ -223,8 +245,10 @@ namespace ocs2::legged_robot {
a.setZero(); a.setZero();
b.setZero(); b.setZero();
size_t j = 0; size_t j = 0;
for (size_t i = 0; i < info_.numThreeDofContacts; ++i) { for (size_t i = 0; i < info_.numThreeDofContacts; ++i)
if (!contact_flag_[i]) { {
if (!contact_flag_[i])
{
vector3_t accel = swing_kp_ * (posDesired[i] - posMeasured[i]) + swing_kd_ * ( 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( a.block(3 * j, 0, 3, info_.generalizedCoordinatesNum) = j_.block(
@ -237,12 +261,14 @@ namespace ocs2::legged_robot {
return {a, b, matrix_t(), vector_t()}; 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_); matrix_t a(3 * info_.numThreeDofContacts, num_decision_vars_);
vector_t b(a.rows()); vector_t b(a.rows());
a.setZero(); 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); a.block(3 * i, info_.generalizedCoordinatesNum + 3 * i, 3, 3) = matrix_t::Identity(3, 3);
} }
b = inputDesired.head(a.rows()); b = inputDesired.head(a.rows());
@ -250,11 +276,13 @@ namespace ocs2::legged_robot {
return {a, b, matrix_t(), vector_t()}; 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 // Load task file
torque_limits_ = vector_t(info_.actuatedDofNum / 4); torque_limits_ = vector_t(info_.actuatedDofNum / 4);
loadData::loadEigenMatrix(taskFile, "torqueLimitsTask", torque_limits_); loadData::loadEigenMatrix(taskFile, "torqueLimitsTask", torque_limits_);
if (verbose) { if (verbose)
{
std::cerr << "\n #### Torque Limits Task:"; std::cerr << "\n #### Torque Limits Task:";
std::cerr << "\n #### =============================================================================\n"; std::cerr << "\n #### =============================================================================\n";
std::cerr << "\n #### Hip_joint Thigh_joint Calf_joint: " << torque_limits_.transpose() << "\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; boost::property_tree::ptree pt;
read_info(taskFile, pt); read_info(taskFile, pt);
std::string prefix = "frictionConeTask."; std::string prefix = "frictionConeTask.";
if (verbose) { if (verbose)
{
std::cerr << "\n #### Friction Cone Task:"; std::cerr << "\n #### Friction Cone Task:";
std::cerr << "\n #### =============================================================================\n"; std::cerr << "\n #### =============================================================================\n";
} }
loadData::loadPtreeValue(pt, friction_coeff_, prefix + "frictionCoefficient", verbose); loadData::loadPtreeValue(pt, friction_coeff_, prefix + "frictionCoefficient", verbose);
if (verbose) { if (verbose)
{
std::cerr << " #### =============================================================================\n"; std::cerr << " #### =============================================================================\n";
} }
prefix = "swingLegTask."; prefix = "swingLegTask.";
if (verbose) { if (verbose)
{
std::cerr << "\n #### Swing Leg Task:"; std::cerr << "\n #### Swing Leg Task:";
std::cerr << "\n #### =============================================================================\n"; std::cerr << "\n #### =============================================================================\n";
} }

24
controllers/ocs2_quadruped_controller/src/wbc/WeightedWbc.cpp
View File

@ -10,10 +10,12 @@
#include <boost/property_tree/ptree.hpp> #include <boost/property_tree/ptree.hpp>
#include <ocs2_core/misc/LoadData.h> #include <ocs2_core/misc/LoadData.h>
namespace ocs2::legged_robot { namespace ocs2::legged_robot
vector_t WeightedWbc::update(const vector_t &stateDesired, const vector_t &inputDesired, {
const vector_t &rbdStateMeasured, size_t mode, vector_t WeightedWbc::update(const vector_t& stateDesired, const vector_t& inputDesired,
scalar_t period) { const vector_t& rbdStateMeasured, size_t mode,
scalar_t period)
{
WbcBase::update(stateDesired, inputDesired, rbdStateMeasured, mode, period); WbcBase::update(stateDesired, inputDesired, rbdStateMeasured, mode, period);
// Constraints // Constraints
@ -52,25 +54,29 @@ namespace ocs2::legged_robot {
return qpSol; return qpSol;
} }
Task WeightedWbc::formulateConstraints() { Task WeightedWbc::formulateConstraints()
{
return formulateFloatingBaseEomTask() + formulateTorqueLimitsTask() + formulateFrictionConeTask() + return formulateFloatingBaseEomTask() + formulateTorqueLimitsTask() + formulateFrictionConeTask() +
formulateNoContactMotionTask(); formulateNoContactMotionTask();
} }
Task WeightedWbc::formulateWeightedTasks(const vector_t &stateDesired, const vector_t &inputDesired, Task WeightedWbc::formulateWeightedTasks(const vector_t& stateDesired, const vector_t& inputDesired,
scalar_t period) { scalar_t period)
{
return formulateSwingLegTask() * weightSwingLeg_ + formulateBaseAccelTask(stateDesired, inputDesired, period) * return formulateSwingLegTask() * weightSwingLeg_ + formulateBaseAccelTask(stateDesired, inputDesired, period) *
weightBaseAccel_ + weightBaseAccel_ +
formulateContactForceTask(inputDesired) * weightContactForce_; 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); WbcBase::loadTasksSetting(taskFile, verbose);
boost::property_tree::ptree pt; boost::property_tree::ptree pt;
read_info(taskFile, pt); read_info(taskFile, pt);
const std::string prefix = "weight."; const std::string prefix = "weight.";
if (verbose) { if (verbose)
{
std::cerr << "\n #### WBC weight:"; std::cerr << "\n #### WBC weight:";
std::cerr << "\n #### =============================================================================\n"; std::cerr << "\n #### =============================================================================\n";
} }

2
descriptions/deep_robotics/lite3_description/config/robot_control.yaml
View File

@ -1,7 +1,7 @@
# Controller Manager configuration # Controller Manager configuration
controller_manager: controller_manager:
ros__parameters: ros__parameters:
update_rate: 200 # Hz update_rate: 1000 # Hz
# Define the available controllers # Define the available controllers
joint_state_broadcaster: joint_state_broadcaster:

90
descriptions/deep_robotics/x30_description/config/robot_control.yaml
View File

@ -108,7 +108,8 @@ unitree_guide_controller:
ocs2_quadruped_controller: ocs2_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 100 # Hz update_rate: 200 # Hz
robot_pkg: "x30_description"
default_kd: 8.0 default_kd: 8.0
joints: joints:
- FL_HipX - FL_HipX
@ -213,93 +214,6 @@ rl_quadruped_controller:
- 0.0 - 0.0
- -1.0 - -1.0
- 1.8 - 1.8
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
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 - -0.732
- 1.361 - 1.361

2
descriptions/unitree/a1_description/README.md
View File

@ -63,7 +63,7 @@ ros2 launch a1_description visualize.launch.py
* OCS2 Quadruped Controller * OCS2 Quadruped Controller
```bash ```bash
source ~/ros2_ws/install/setup.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 * RL Quadruped Controller
```bash ```bash

46
descriptions/unitree/a1_description/config/gazebo.yaml
View File

@ -10,9 +10,6 @@ controller_manager:
imu_sensor_broadcaster: imu_sensor_broadcaster:
type: imu_sensor_broadcaster/IMUSensorBroadcaster type: imu_sensor_broadcaster/IMUSensorBroadcaster
leg_pd_controller:
type: leg_pd_controller/LegPdController
unitree_guide_controller: unitree_guide_controller:
type: unitree_guide_controller/UnitreeGuideController type: unitree_guide_controller/UnitreeGuideController
@ -27,33 +24,9 @@ imu_sensor_broadcaster:
sensor_name: "imu_sensor" sensor_name: "imu_sensor"
frame_id: "imu_link" 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: unitree_guide_controller:
ros__parameters: ros__parameters:
command_prefix: "leg_pd_controller" update_rate: 500 # Hz
update_rate: 200 # Hz
joints: joints:
- FR_hip_joint - FR_hip_joint
- FR_thigh_joint - FR_thigh_joint
@ -105,7 +78,9 @@ ocs2_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 500 # Hz update_rate: 500 # Hz
robot_pkg: "a1_description" robot_pkg: "a1_description"
command_prefix: "leg_pd_controller" feet_force_threshold: 5.0
# default_kp: 1.0
default_kd: 0.5
joints: joints:
- FL_hip_joint - FL_hip_joint
- FL_thigh_joint - FL_thigh_joint
@ -153,7 +128,12 @@ ocs2_quadruped_controller:
- linear_acceleration.y - linear_acceleration.y
- linear_acceleration.z - 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: rl_quadruped_controller:
ros__parameters: ros__parameters:
@ -164,12 +144,12 @@ rl_quadruped_controller:
- FL_hip_joint - FL_hip_joint
- FL_thigh_joint - FL_thigh_joint
- FL_calf_joint - FL_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
- FR_hip_joint - FR_hip_joint
- FR_thigh_joint - FR_thigh_joint
- FR_calf_joint - FR_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
- RR_hip_joint - RR_hip_joint
- RR_thigh_joint - RR_thigh_joint
- RR_calf_joint - RR_calf_joint

24
descriptions/unitree/a1_description/config/ocs2/reference.info
View File

@ -5,18 +5,18 @@ comHeight 0.33
defaultJointState defaultJointState
{ {
(0,0) -0.20 ; FL_hip_joint (0,0) -0.107 ; FL_hip_joint
(1,0) 0.72 ; FL_thigh_joint (1,0) 0.77 ; FL_thigh_joint
(2,0) -1.44 ; FL_calf_joint (2,0) -1.68 ; FL_calf_joint
(3,0) 0.20 ; FR_hip_joint (3,0) 0.107 ; FR_hip_joint
(4,0) 0.72 ; FR_thigh_joint (4,0) 0.77 ; FR_thigh_joint
(5,0) -1.44 ; FR_calf_joint (5,0) -1.68 ; FR_calf_joint
(6,0) -0.20 ; RL_hip_joint (6,0) -0.107 ; RL_hip_joint
(7,0) 0.72 ; RL_thigh_joint (7,0) 0.77 ; RL_thigh_joint
(8,0) -1.44 ; RL_calf_joint (8,0) -1.68 ; RL_calf_joint
(9,0) 0.20 ; RR_hip_joint (9,0) 0.107 ; RR_hip_joint
(10,0) 0.72 ; RR_thigh_joint (10,0) 0.77 ; RR_thigh_joint
(11,0) -1.44 ; RR_calf_joint (11,0) -1.68 ; RR_calf_joint
} }
initialModeSchedule initialModeSchedule

105
descriptions/unitree/a1_description/config/ocs2/task.info
View File

@ -23,48 +23,6 @@ swing_trajectory_config
swingTimeScale 0.15 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 ; Multiple_Shooting SQP settings
sqp sqp
{ {
@ -85,39 +43,6 @@ sqp
threadPriority 50 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 settings
rollout rollout
{ {
@ -160,18 +85,18 @@ initialState
(11,0) 0.0 ; theta_base_x (11,0) 0.0 ; theta_base_x
;; Leg Joint Positions: [FL, RL, FR, RR] ;; ;; Leg Joint Positions: [FL, RL, FR, RR] ;;
(12,0) -0.20 ; FL_hip_joint (12,0) -0.107 ; FL_hip_joint
(13,0) 0.72 ; FL_thigh_joint (13,0) 0.77 ; FL_thigh_joint
(14,0) -1.44 ; FL_calf_joint (14,0) -1.68 ; FL_calf_joint
(15,0) -0.20 ; RL_hip_joint (15,0) -0.107 ; RL_hip_joint
(16,0) 0.72 ; RL_thigh_joint (16,0) 0.77 ; RL_thigh_joint
(17,0) -1.44 ; RL_calf_joint (17,0) -1.68 ; RL_calf_joint
(18,0) 0.20 ; FR_hip_joint (18,0) 0.107 ; FR_hip_joint
(19,0) 0.72 ; FR_thigh_joint (19,0) 0.77 ; FR_thigh_joint
(20,0) -1.44 ; FR_calf_joint (20,0) -1.68 ; FR_calf_joint
(21,0) 0.20 ; RR_hip_joint (21,0) 0.107 ; RR_hip_joint
(22,0) 0.72 ; RR_thigh_joint (22,0) 0.77 ; RR_thigh_joint
(23,0) -1.44 ; RR_calf_joint (23,0) -1.68 ; RR_calf_joint
} }
; standard state weight matrix ; standard state weight matrix
@ -295,15 +220,15 @@ frictionConeTask
swingLegTask swingLegTask
{ {
kp 350 kp 300
kd 37 kd 30
} }
weight weight
{ {
swingLeg 100 swingLeg 100
baseAccel 1 baseAccel 1
contactForce 0.01 contactForce 0.05
} }
; State Estimation ; State Estimation

5
descriptions/unitree/a1_description/config/robot_control.yaml
View File

@ -76,8 +76,9 @@ unitree_guide_controller:
ocs2_quadruped_controller: ocs2_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 100 # Hz update_rate: 500 # Hz
robot_pkg: "a1_description"
default_kd: 0.5
joints: joints:
- FL_hip_joint - FL_hip_joint
- FL_thigh_joint - FL_thigh_joint

2
descriptions/unitree/a1_description/xacro/const.xacro
View File

@ -35,7 +35,7 @@
<!-- joint limits --> <!-- joint limits -->
<xacro:property name="damping" value="0.1"/> <xacro:property name="damping" value="0.1"/>
<xacro:property name="friction" value="0.1"/> <xacro:property name="friction" value="0.2"/>
<xacro:property name="hip_max" value="46"/> <xacro:property name="hip_max" value="46"/>
<xacro:property name="hip_min" value="-46"/> <xacro:property name="hip_min" value="-46"/>
<xacro:property name="hip_velocity_max" value="21"/> <xacro:property name="hip_velocity_max" value="21"/>

13
descriptions/unitree/a1_description/xacro/gazebo.xacro
View File

@ -162,21 +162,12 @@
<gazebo> <gazebo>
<plugin filename="gz_quadruped_hardware-system" name="gz_quadruped_hardware::GazeboSimQuadrupedPlugin"> <plugin filename="gz_quadruped_hardware-system" name="gz_quadruped_hardware::GazeboSimQuadrupedPlugin">
<parameters>$(find go2_description)/config/gazebo.yaml</parameters> <parameters>$(find a1_description)/config/gazebo.yaml</parameters>
</plugin> </plugin>
<plugin filename="gz-sim-imu-system" <plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu"> name="gz::sim::systems::Imu">
</plugin> </plugin>
<plugin filename="gz-sim-odometry-publisher-system" <plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"/>
name="gz::sim::systems::OdometryPublisher">
<odom_frame>odom</odom_frame>
<robot_base_frame>base</robot_base_frame>
<odom_publish_frequency>1000</odom_publish_frequency>
<odom_topic>odom</odom_topic>
<dimensions>3</dimensions>
<odom_covariance_topic>odom_with_covariance</odom_covariance_topic>
<tf_topic>tf</tf_topic>
</plugin>
</gazebo> </gazebo>
<gazebo reference="trunk"> <gazebo reference="trunk">

5
descriptions/unitree/aliengo_description/config/robot_control.yaml
View File

@ -1,7 +1,7 @@
# Controller Manager configuration # Controller Manager configuration
controller_manager: controller_manager:
ros__parameters: ros__parameters:
update_rate: 500 # Hz update_rate: 1000 # Hz
# Define the available controllers # Define the available controllers
joint_state_broadcaster: joint_state_broadcaster:
@ -78,7 +78,8 @@ unitree_guide_controller:
ocs2_quadruped_controller: ocs2_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 100 # Hz update_rate: 500 # Hz
robot_pkg: "aliengo_description"
default_kd: 5.0 default_kd: 5.0
joints: joints:

5
descriptions/unitree/b2_description/README.md
View File

@ -50,3 +50,8 @@ ros2 launch b2_description visualize.launch.py
source ~/ros2_ws/install/setup.bash source ~/ros2_ws/install/setup.bash
ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=b2_description height:=0.68 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
```

58
descriptions/unitree/b2_description/config/gazebo.yaml
View File

@ -10,8 +10,8 @@ controller_manager:
imu_sensor_broadcaster: imu_sensor_broadcaster:
type: imu_sensor_broadcaster/IMUSensorBroadcaster type: imu_sensor_broadcaster/IMUSensorBroadcaster
leg_pd_controller: ocs2_quadruped_controller:
type: leg_pd_controller/LegPdController type: ocs2_quadruped_controller/Ocs2QuadrupedController
unitree_guide_controller: unitree_guide_controller:
type: unitree_guide_controller/UnitreeGuideController type: unitree_guide_controller/UnitreeGuideController
@ -24,33 +24,69 @@ imu_sensor_broadcaster:
sensor_name: "imu_sensor" sensor_name: "imu_sensor"
frame_id: "imu_link" frame_id: "imu_link"
leg_pd_controller: ocs2_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 1000 # Hz update_rate: 500 # Hz
robot_pkg: "b2_description"
feet_force_threshold: 5.0
default_kd: 3.5
joints: joints:
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- FL_hip_joint - FL_hip_joint
- FL_thigh_joint - FL_thigh_joint
- FL_calf_joint - FL_calf_joint
- RR_hip_joint - FR_hip_joint
- RR_thigh_joint - FR_thigh_joint
- RR_calf_joint - FR_calf_joint
- RL_hip_joint - RL_hip_joint
- RL_thigh_joint - RL_thigh_joint
- RL_calf_joint - RL_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
command_interfaces: command_interfaces:
- effort - effort
- position
- velocity
- kp
- kd
state_interfaces: state_interfaces:
- effort
- position - position
- velocity - 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: unitree_guide_controller:
ros__parameters: ros__parameters:
command_prefix: "leg_pd_controller"
update_rate: 500 # Hz update_rate: 500 # Hz
stand_kp: 500.0 stand_kp: 500.0
stand_kd: 40.0 stand_kd: 40.0

4
descriptions/unitree/b2_description/config/robot_control.yaml
View File

@ -75,8 +75,8 @@ unitree_guide_controller:
ocs2_quadruped_controller: ocs2_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 100 # Hz update_rate: 500 # Hz
robot_pkg: "b2_description"
default_kd: 8.0 default_kd: 8.0
joints: joints:

26
descriptions/unitree/b2_description/xacro/const.xacro
View File

@ -4,7 +4,7 @@
<!-- Constants for robot dimensions --> <!-- Constants for robot dimensions -->
<xacro:property name="PI" value="3.1415926535897931"/> <xacro:property name="PI" value="3.1415926535897931"/>
<!-- <xacro:property name="stick_mass" value="0.00001"/> --> <xacro:property name="stick_mass" value="0.00001"/>
<!-- simplified collision value --> <!-- simplified collision value -->
<xacro:property name="trunk_width" value="0.28"/> <xacro:property name="trunk_width" value="0.28"/>
@ -12,15 +12,15 @@
<xacro:property name="trunk_height" value="0.15"/> <xacro:property name="trunk_height" value="0.15"/>
<xacro:property name="hip_radius" value="0.07"/> <xacro:property name="hip_radius" value="0.07"/>
<xacro:property name="hip_length" value="0.05"/> <xacro:property name="hip_length" value="0.05"/>
<!-- <xacro:property name="thigh_shoulder_radius" value="0.044"/> <xacro:property name="thigh_shoulder_radius" value="0.044"/>
<xacro:property name="thigh_shoulder_length" value="0.08"/> --> <xacro:property name="thigh_shoulder_length" value="0.08"/>
<xacro:property name="thigh_width" value="0.0455"/> <xacro:property name="thigh_width" value="0.0455"/>
<xacro:property name="thigh_height" value="0.054"/> <xacro:property name="thigh_height" value="0.054"/>
<!-- <xacro:property name="calf_width" value="0.04"/> <xacro:property name="calf_width" value="0.04"/>
<xacro:property name="calf_height" value="0.05"/> --> <xacro:property name="calf_height" value="0.05"/>
<xacro:property name="foot_radius" value="0.04"/> <xacro:property name="foot_radius" value="0.04"/>
<!-- <xacro:property name="stick_radius" value="0.01"/> <xacro:property name="stick_radius" value="0.01"/>
<xacro:property name="stick_length" value="0.2"/> --> <xacro:property name="stick_length" value="0.2"/>
<!-- kinematic value --> <!-- kinematic value -->
<xacro:property name="thigh_offset" value="0.11973"/> <xacro:property name="thigh_offset" value="0.11973"/>
@ -34,23 +34,23 @@
<xacro:property name="hip_offset" value="0.11973"/> <xacro:property name="hip_offset" value="0.11973"/>
<!-- offset of link and rotor locations (left front) --> <!-- offset of link and rotor locations (left front) -->
<!-- <xacro:property name="hip_offset_x" value="0.3285"/> <xacro:property name="hip_offset_x" value="0.3285"/>
<xacro:property name="hip_offset_y" value="0.072"/> <xacro:property name="hip_offset_y" value="0.072"/>
<xacro:property name="hip_offset_z" value="0.0"/> --> <xacro:property name="hip_offset_z" value="0.0"/>
<xacro:property name="hip_rotor_offset_x" value="0.20205"/> <xacro:property name="hip_rotor_offset_x" value="0.20205"/>
<xacro:property name="hip_rotor_offset_y" value="0.072"/> <xacro:property name="hip_rotor_offset_y" value="0.072"/>
<xacro:property name="hip_rotor_offset_z" value="0.0"/> <xacro:property name="hip_rotor_offset_z" value="0.0"/>
<!-- <xacro:property name="thigh_offset_x" value="0"/> <xacro:property name="thigh_offset_x" value="0"/>
<xacro:property name="thigh_offset_y" value="0.11973"/> <xacro:property name="thigh_offset_y" value="0.11973"/>
<xacro:property name="thigh_offset_z" value="0.0"/> --> <xacro:property name="thigh_offset_z" value="0.0"/>
<xacro:property name="thigh_rotor_offset_x" value="0.0"/> <xacro:property name="thigh_rotor_offset_x" value="0.0"/>
<xacro:property name="thigh_rotor_offset_y" value="0.00798"/> <xacro:property name="thigh_rotor_offset_y" value="0.00798"/>
<xacro:property name="thigh_rotor_offset_z" value="0.0"/> <xacro:property name="thigh_rotor_offset_z" value="0.0"/>
<!-- <xacro:property name="calf_offset_x" value="0.0"/> <xacro:property name="calf_offset_x" value="0.0"/>
<xacro:property name="calf_offset_y" value="0.0"/> <xacro:property name="calf_offset_y" value="0.0"/>
<xacro:property name="calf_offset_z" value="-0.35"/> --> <xacro:property name="calf_offset_z" value="-0.35"/>
<xacro:property name="calf_rotor_offset_x" value="0.0"/> <xacro:property name="calf_rotor_offset_x" value="0.0"/>
<xacro:property name="calf_rotor_offset_y" value="-0.05788"/> <xacro:property name="calf_rotor_offset_y" value="-0.05788"/>
<xacro:property name="calf_rotor_offset_z" value="0.0"/> <xacro:property name="calf_rotor_offset_z" value="0.0"/>

92
descriptions/unitree/b2_description/xacro/gazebo.xacro
View File

@ -1,90 +1,139 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"> <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find gz_quadruped_hardware)/xacro/foot_force_sensor.xacro"/>
<ros2_control name="GazeboSystem" type="system"> <ros2_control name="GazeboSystem" type="system">
<hardware> <hardware>
<plugin>gz_ros2_control/GazeboSimSystem</plugin> <plugin>gz_quadruped_hardware/GazeboSimSystem</plugin>
</hardware> </hardware>
<joint name="FR_hip_joint"> <joint name="FR_hip_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="FR_thigh_joint"> <joint name="FR_thigh_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="FR_calf_joint"> <joint name="FR_calf_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="FL_hip_joint"> <joint name="FL_hip_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="FL_thigh_joint"> <joint name="FL_thigh_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="FL_calf_joint"> <joint name="FL_calf_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="RR_hip_joint"> <joint name="RR_hip_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="RR_thigh_joint"> <joint name="RR_thigh_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="RR_calf_joint"> <joint name="RR_calf_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="RL_hip_joint"> <joint name="RL_hip_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="RL_thigh_joint"> <joint name="RL_thigh_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
</joint> </joint>
<joint name="RL_calf_joint"> <joint name="RL_calf_joint">
<command_interface name="effort" initial_value="0.0"/> <command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/> <state_interface name="velocity"/>
<state_interface name="effort"/> <state_interface name="effort"/>
@ -102,15 +151,23 @@
<state_interface name="linear_acceleration.y"/> <state_interface name="linear_acceleration.y"/>
<state_interface name="linear_acceleration.z"/> <state_interface name="linear_acceleration.z"/>
</sensor> </sensor>
<sensor name="foot_force">
<state_interface name="FR_foot_force"/>
<state_interface name="FL_foot_force"/>
<state_interface name="RR_foot_force"/>
<state_interface name="RL_foot_force"/>
</sensor>
</ros2_control> </ros2_control>
<gazebo> <gazebo>
<plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin"> <plugin filename="gz_quadruped_hardware-system" name="gz_quadruped_hardware::GazeboSimQuadrupedPlugin">
<parameters>$(find b2_description)/config/gazebo.yaml</parameters> <parameters>$(find b2_description)/config/gazebo.yaml</parameters>
</plugin> </plugin>
<plugin filename="gz-sim-imu-system" <plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu"> name="gz::sim::systems::Imu">
</plugin> </plugin>
<plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"/>
</gazebo> </gazebo>
<gazebo reference="trunk"> <gazebo reference="trunk">
@ -185,4 +242,9 @@
<disableFixedJointLumping>true</disableFixedJointLumping> <disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo> </gazebo>
<xacro:foot_force_sensor name="FL" suffix="foot_fixed"/>
<xacro:foot_force_sensor name="RL" suffix="foot_fixed"/>
<xacro:foot_force_sensor name="FR" suffix="foot_fixed"/>
<xacro:foot_force_sensor name="RR" suffix="foot_fixed"/>
</robot> </robot>

3
descriptions/unitree/b2_description/xacro/leg.xacro
View File

@ -90,7 +90,6 @@
<geometry> <geometry>
<cylinder length="0.02" radius="0.05"/> <cylinder length="0.02" radius="0.05"/>
</geometry> </geometry>
<material name="green"/>
</visual> </visual>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="${hip_rotor_com_x} ${hip_rotor_com_y} ${hip_rotor_com_z}"/> <origin rpy="0 0 0" xyz="${hip_rotor_com_x} ${hip_rotor_com_y} ${hip_rotor_com_z}"/>
@ -152,7 +151,6 @@
<geometry> <geometry>
<cylinder length="0.02" radius="0.05"/> <cylinder length="0.02" radius="0.05"/>
</geometry> </geometry>
<material name="green"/>
</visual> </visual>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="${thigh_rotor_com_x} ${thigh_rotor_com_y} ${thigh_rotor_com_z}"/> <origin rpy="0 0 0" xyz="${thigh_rotor_com_x} ${thigh_rotor_com_y} ${thigh_rotor_com_z}"/>
@ -227,7 +225,6 @@
<geometry> <geometry>
<cylinder length="0.02" radius="0.05"/> <cylinder length="0.02" radius="0.05"/>
</geometry> </geometry>
<material name="green"/>
</visual> </visual>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="${calf_rotor_com_x} ${calf_rotor_com_y} ${calf_rotor_com_z}"/> <origin rpy="0 0 0" xyz="${calf_rotor_com_x} ${calf_rotor_com_y} ${calf_rotor_com_z}"/>

6
descriptions/unitree/go1_description/config/robot_control.yaml
View File

@ -1,7 +1,7 @@
# Controller Manager configuration # Controller Manager configuration
controller_manager: controller_manager:
ros__parameters: ros__parameters:
update_rate: 200 # Hz update_rate: 1000 # Hz
# Define the available controllers # Define the available controllers
joint_state_broadcaster: joint_state_broadcaster:
@ -73,8 +73,8 @@ unitree_guide_controller:
ocs2_quadruped_controller: ocs2_quadruped_controller:
ros__parameters: ros__parameters:
update_rate: 100 # Hz update_rate: 500 # Hz
robot_pkg: "go1_description"
joints: joints:
- FL_hip_joint - FL_hip_joint
- FL_thigh_joint - FL_thigh_joint

4
descriptions/unitree/go2_description/xacro/const.xacro
View File

@ -50,8 +50,8 @@
<!-- joint limits --> <!-- joint limits -->
<xacro:property name="damping" value="0.0"/> <xacro:property name="damping" value="0.1"/>
<xacro:property name="friction" value="0.01"/> <xacro:property name="friction" value="0.2"/>
<xacro:property name="hip_position_max" value="1.0472"/> <xacro:property name="hip_position_max" value="1.0472"/>
<xacro:property name="hip_position_min" value="-1.0472"/> <xacro:property name="hip_position_min" value="-1.0472"/>
<xacro:property name="hip_velocity_max" value="30.1"/> <xacro:property name="hip_velocity_max" value="30.1"/>

2
hardwares/gz_quadruped_hardware/xacro/foot_force_sensor.xacro
View File

@ -9,7 +9,7 @@
<disableFixedJointLumping>true</disableFixedJointLumping> <disableFixedJointLumping>true</disableFixedJointLumping>
<sensor name="${name}_foot_force" type="force_torque"> <sensor name="${name}_foot_force" type="force_torque">
<always_on>1</always_on> <always_on>1</always_on>
<update_rate>500</update_rate> <update_rate>1000</update_rate>
<visualize>true</visualize> <visualize>true</visualize>
<topic>${name}_foot_force</topic> <topic>${name}_foot_force</topic>
<force_torque> <force_torque>