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/TargetManager.cpp
src/control/CtrlComponent.cpp
)
target_include_directories(${PROJECT_NAME}

3
controllers/ocs2_quadruped_controller/README.md
View File

@ -10,7 +10,8 @@ Tested environment:
* Ubuntu 22.04
* ROS2 Humble
*[x] **[2025-01-16]** Add support for ground truth estimator.
* [x] **[2025-01-16]** Add support for ground truth estimator.
* [x] **[2025-03-15]** OCS2 Controller now can switch between passive and MPC mode.
[![](http://i0.hdslb.com/bfs/archive/e758ce019587032449a153cf897a543443b64bba.jpg)](https://www.bilibili.com/video/BV1UcxieuEmH/)

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

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

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_oc/synchronized_module/SolverSynchronizedModule.h>
namespace ocs2::legged_robot {
class GaitManager final : public SolverSynchronizedModule {
namespace ocs2::legged_robot
{
class GaitManager final : public SolverSynchronizedModule
{
public:
GaitManager(CtrlInterfaces &ctrl_interfaces,
GaitManager(CtrlInterfaces& ctrl_interfaces,
std::shared_ptr<GaitSchedule> gait_schedule_ptr);
void preSolverRun(scalar_t initTime, scalar_t finalTime,
const vector_t &currentState,
const ReferenceManagerInterface &referenceManager) override;
const vector_t& currentState,
const ReferenceManagerInterface& referenceManager) override;
void postSolverRun(const PrimalSolution &primalSolution) override {
void postSolverRun(const PrimalSolution&/**primalSolution**/) override
{
}
void init(const std::string &gait_file);
void init(const std::string& gait_file);
private:
void getTargetGait();
CtrlInterfaces &ctrl_interfaces_;
CtrlInterfaces& ctrl_interfaces_;
std::shared_ptr<GaitSchedule> gait_schedule_ptr_;
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_oc/synchronized_module/ReferenceManagerInterface.h>
namespace ocs2::legged_robot {
class TargetManager {
namespace ocs2::legged_robot
{
class TargetManager
{
public:
TargetManager(CtrlInterfaces &ctrl_component,
const std::shared_ptr<ReferenceManagerInterface> &referenceManagerPtr,
const std::string &task_file,
const std::string &reference_file);
TargetManager(CtrlInterfaces& ctrl_component,
const std::shared_ptr<ReferenceManagerInterface>& referenceManagerPtr,
const std::string& task_file,
const std::string& reference_file);
~TargetManager() = default;
void update(SystemObservation &observation);
void update(SystemObservation& observation);
private:
TargetTrajectories targetPoseToTargetTrajectories(const vector_t &targetPose,
const SystemObservation &observation,
const scalar_t &targetReachingTime) {
TargetTrajectories targetPoseToTargetTrajectories(const vector_t& targetPose,
const SystemObservation& observation,
const scalar_t& targetReachingTime)
{
// desired time trajectory
const scalar_array_t timeTrajectory{observation.time, targetReachingTime};
@ -45,7 +48,7 @@ namespace ocs2::legged_robot {
return {timeTrajectory, stateTrajectory, inputTrajectory};
}
CtrlInterfaces &ctrl_component_;
CtrlInterfaces& ctrl_component_;
std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_;
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
#include "StateEstimateBase.h"
#include <realtime_tools/realtime_buffer.h>
#include <realtime_tools/realtime_buffer.hpp>
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 <rclcpp_lifecycle/lifecycle_node.hpp>
namespace ocs2::legged_robot {
class StateEstimateBase {
namespace ocs2::legged_robot
{
class StateEstimateBase
{
public:
virtual ~StateEstimateBase() = default;
StateEstimateBase(CentroidalModelInfo info, CtrlInterfaces &ctrl_component,
StateEstimateBase(CentroidalModelInfo info, CtrlInterfaces& ctrl_component,
rclcpp_lifecycle::LifecycleNode::SharedPtr node);
virtual void updateJointStates();
@ -30,25 +32,27 @@ namespace ocs2::legged_robot {
virtual void updateImu();
virtual vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) = 0;
virtual vector_t update(const rclcpp::Time& time, const rclcpp::Duration& period) = 0;
size_t getMode() { return stanceLeg2ModeNumber(contact_flag_); }
[[nodiscard]] size_t getMode() const { return stanceLeg2ModeNumber(contact_flag_); }
protected:
void initPublishers();
void updateAngular(const vector3_t &zyx, const vector_t &angularVel);
void updateAngular(const vector3_t& zyx, const vector_t& angularVel);
void updateLinear(const vector_t &pos, const vector_t &linearVel);
void updateLinear(const vector_t& pos, const vector_t& linearVel);
void publishMsgs(const nav_msgs::msg::Odometry &odom) const;
void publishMsgs(const nav_msgs::msg::Odometry& odom) const;
CtrlInterfaces &ctrl_component_;
CtrlInterfaces& ctrl_component_;
CentroidalModelInfo info_;
contact_flag_t contact_flag_{};
double feet_force_threshold_ = 5.0;
vector3_t zyx_offset_ = vector3_t::Zero();
vector_t rbd_state_;
contact_flag_t contact_flag_{};
Eigen::Quaternion<scalar_t> quat_;
vector3_t angular_vel_local_, linear_accel_local_;
matrix3_t orientationCovariance_, angularVelCovariance_, linearAccelCovariance_;
@ -58,13 +62,15 @@ namespace ocs2::legged_robot {
rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
};
template<typename T>
T square(T a) {
template <typename T>
T square(T a)
{
return a * a;
}
template<typename SCALAR_T>
Eigen::Matrix<SCALAR_T, 3, 1> quatToZyx(const Eigen::Quaternion<SCALAR_T> &q) {
template <typename SCALAR_T>
Eigen::Matrix<SCALAR_T, 3, 1> quatToZyx(const Eigen::Quaternion<SCALAR_T>& q)
{
Eigen::Matrix<SCALAR_T, 3, 1> zyx;
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.
//
#ifndef LEGGEDINTERFACE_H
#define LEGGEDINTERFACE_H
#pragma once
#include <ocs2_centroidal_model/CentroidalModelInfo.h>
#include <ocs2_core/Types.h>
@ -24,85 +22,89 @@
#include "SwitchedModelReferenceManager.h"
namespace ocs2::legged_robot {
class LeggedInterface final : public RobotInterface {
namespace ocs2::legged_robot
{
class LeggedInterface final : public RobotInterface
{
public:
LeggedInterface(const std::string &task_file,
const std::string &urdf_file,
const std::string &reference_file,
LeggedInterface(const std::string& task_file,
const std::string& urdf_file,
const std::string& reference_file,
bool use_hard_friction_cone_constraint = false);
~LeggedInterface() override = default;
void setupJointNames(const std::vector<std::string> &joint_names,
const std::vector<std::string> &foot_names);
void setupJointNames(const std::vector<std::string>& joint_names,
const std::vector<std::string>& foot_names);
void setupOptimalControlProblem(const std::string &task_file,
const std::string &urdf_file,
const std::string &reference_file,
void setupOptimalControlProblem(const std::string& task_file,
const std::string& urdf_file,
const std::string& reference_file,
bool verbose);
const OptimalControlProblem &getOptimalControlProblem() const override { return *problem_ptr_; }
const OptimalControlProblem& getOptimalControlProblem() const override { return *problem_ptr_; }
const ModelSettings &modelSettings() const { return model_settings_; }
const ddp::Settings &ddpSettings() const { return ddp_settings_; }
const mpc::Settings &mpcSettings() const { return mpc_settings_; }
const sqp::Settings &sqpSettings() { return sqp_settings_; }
const ModelSettings& modelSettings() const { return model_settings_; }
const ddp::Settings& ddpSettings() const { return ddp_settings_; }
const mpc::Settings& mpcSettings() const { return mpc_settings_; }
const sqp::Settings& sqpSettings() { return sqp_settings_; }
const RolloutBase &getRollout() const { return *rollout_ptr_; }
PinocchioInterface &getPinocchioInterface() { return *pinocchio_interface_ptr_; }
const CentroidalModelInfo &getCentroidalModelInfo() const { return centroidal_model_info_; }
const RolloutBase& getRollout() const { return *rollout_ptr_; }
PinocchioInterface& getPinocchioInterface() { return *pinocchio_interface_ptr_; }
const CentroidalModelInfo& getCentroidalModelInfo() const { return centroidal_model_info_; }
std::shared_ptr<SwitchedModelReferenceManager> getSwitchedModelReferenceManagerPtr() const {
std::shared_ptr<SwitchedModelReferenceManager> getSwitchedModelReferenceManagerPtr() const
{
return reference_manager_ptr_;
}
const Initializer &getInitializer() const override { return *initializer_ptr_; }
const Initializer& getInitializer() const override { return *initializer_ptr_; }
std::shared_ptr<ReferenceManagerInterface> getReferenceManagerPtr() const override {
std::shared_ptr<ReferenceManagerInterface> getReferenceManagerPtr() const override
{
return reference_manager_ptr_;
}
protected:
void setupModel(const std::string &task_file, const std::string &urdf_file,
const std::string &reference_file);
void setupModel(const std::string& task_file, const std::string& urdf_file,
const std::string& reference_file);
void setupReferenceManager(const std::string &taskFile, const std::string &urdfFile,
const std::string &referenceFile,
void setupReferenceManager(const std::string& taskFile, const std::string& urdfFile,
const std::string& referenceFile,
bool verbose);
void setupPreComputation(const std::string &taskFile, const std::string &urdfFile,
const std::string &referenceFile,
void setupPreComputation(const std::string& taskFile, const std::string& urdfFile,
const std::string& referenceFile,
bool verbose);
std::shared_ptr<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,
const CentroidalModelInfo &info, bool verbose);
std::unique_ptr<StateInputCost> getBaseTrackingCost(const std::string& taskFile,
const CentroidalModelInfo& info, bool verbose);
matrix_t initializeInputCostWeight(const std::string &taskFile, const CentroidalModelInfo &info);
matrix_t initializeInputCostWeight(const std::string& taskFile, const CentroidalModelInfo& info);
static std::pair<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,
scalar_t frictionCoefficient);
std::unique_ptr<StateInputCost> getFrictionConeSoftConstraint(size_t contactPointIndex,
scalar_t frictionCoefficient,
const RelaxedBarrierPenalty::Config &
const RelaxedBarrierPenalty::Config&
barrierPenaltyConfig);
std::unique_ptr<EndEffectorKinematics<scalar_t> > getEeKinematicsPtr(const std::vector<std::string> &foot_names,
const std::string &model_name);
std::unique_ptr<EndEffectorKinematics<scalar_t>> getEeKinematicsPtr(const std::vector<std::string>& foot_names,
const std::string& model_name);
std::unique_ptr<StateInputConstraint> getZeroVelocityConstraint(
const EndEffectorKinematics<scalar_t> &end_effector_kinematics,
const EndEffectorKinematics<scalar_t>& end_effector_kinematics,
size_t contact_point_index);
std::unique_ptr<StateCost> getSelfCollisionConstraint(const PinocchioInterface &pinocchioInterface,
const std::string &taskFile,
const std::string &prefix, bool verbose);
std::unique_ptr<StateCost> getSelfCollisionConstraint(const PinocchioInterface& pinocchioInterface,
const std::string& taskFile,
const std::string& prefix, bool verbose);
ModelSettings model_settings_;
mpc::Settings mpc_settings_;
@ -125,5 +127,4 @@ namespace ocs2::legged_robot {
vector_t initial_state_;
};
} // namespace legged
#pragma clang diagnostic pop
#endif // LEGGEDINTERFACE_H

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.
//
#pragma once
#ifndef HIERARCHICALWBC_H
#define HIERARCHICALWBC_H
#include "WbcBase.h"
namespace ocs2::legged_robot {
class HierarchicalWbc final : public WbcBase {
namespace ocs2::legged_robot
{
class HierarchicalWbc final : public WbcBase
{
public:
using WbcBase::WbcBase;
vector_t update(const vector_t &stateDesired, const vector_t &inputDesired, const vector_t &rbdStateMeasured,
vector_t update(const vector_t& stateDesired, const vector_t& inputDesired, const vector_t& rbdStateMeasured,
size_t mode,
scalar_t period) override;
};
} // namespace legged
#endif // HIERARCHICALWBC_H

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

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

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.
//
#pragma once
#ifndef WBCBASE_H
#define WBCBASE_H
#include "Task.h"
@ -10,28 +10,30 @@
#include <ocs2_legged_robot/gait/MotionPhaseDefinition.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
class WbcBase {
class WbcBase
{
using Vector6 = Eigen::Matrix<scalar_t, 6, 1>;
using Matrix6 = Eigen::Matrix<scalar_t, 6, 6>;
public:
virtual ~WbcBase() = default;
WbcBase(const PinocchioInterface &pinocchioInterface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics &eeKinematics);
WbcBase(const PinocchioInterface& pinocchioInterface, CentroidalModelInfo info,
const PinocchioEndEffectorKinematics& eeKinematics);
virtual void loadTasksSetting(const std::string &taskFile, bool verbose);
virtual void loadTasksSetting(const std::string& taskFile, bool verbose);
virtual vector_t update(const vector_t &stateDesired, const vector_t &inputDesired,
const vector_t &rbdStateMeasured, size_t mode,
virtual vector_t update(const vector_t& stateDesired, const vector_t& inputDesired,
const vector_t& rbdStateMeasured, size_t mode,
scalar_t period);
protected:
void updateMeasured(const vector_t &rbdStateMeasured);
void updateMeasured(const vector_t& rbdStateMeasured);
void updateDesired(const vector_t &stateDesired, const vector_t &inputDesired);
void updateDesired(const vector_t& stateDesired, const vector_t& inputDesired);
size_t getNumDecisionVars() const { return num_decision_vars_; }
@ -43,11 +45,11 @@ namespace ocs2::legged_robot {
Task formulateFrictionConeTask();
Task formulateBaseAccelTask(const vector_t &stateDesired, const vector_t &inputDesired, scalar_t period);
Task formulateBaseAccelTask(const vector_t& stateDesired, const vector_t& inputDesired, scalar_t period);
Task formulateSwingLegTask();
Task formulateContactForceTask(const vector_t &inputDesired) const;
Task formulateContactForceTask(const vector_t& inputDesired) const;
size_t num_decision_vars_;
PinocchioInterface pinocchio_interface_measured_, pinocchio_interface_desired_;
@ -66,3 +68,4 @@ namespace ocs2::legged_robot {
scalar_t friction_coeff_{}, swing_kp_{}, swing_kd_{};
};
} // namespace legged
#endif // WBCBASE_H

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

@ -5,22 +5,24 @@
#include "WbcBase.h"
namespace ocs2::legged_robot {
class WeightedWbc : public WbcBase {
namespace ocs2::legged_robot
{
class WeightedWbc final : public WbcBase
{
public:
using WbcBase::WbcBase;
vector_t update(const vector_t &stateDesired, const vector_t &inputDesired, const vector_t &rbdStateMeasured,
vector_t update(const vector_t& stateDesired, const vector_t& inputDesired, const vector_t& rbdStateMeasured,
size_t mode,
scalar_t period) override;
void loadTasksSetting(const std::string &taskFile, bool verbose) override;
void loadTasksSetting(const std::string& taskFile, bool verbose) override;
protected:
virtual Task formulateConstraints();
Task formulateConstraints();
virtual Task formulateWeightedTasks(const vector_t &stateDesired, const vector_t &inputDesired,
scalar_t period);
Task formulateWeightedTasks(const vector_t& stateDesired, const vector_t& inputDesired,
scalar_t period);
private:
scalar_t weightSwingLeg_, weightBaseAccel_, weightContactForce_;

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

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

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

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

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

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

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

@ -8,11 +8,8 @@
#include <controller_common/FSM/StatePassive.h>
#include <controller_interface/controller_interface.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/control/CtrlComponent.h>
namespace ocs2::legged_robot {
struct FSMStateList {
@ -66,8 +63,8 @@ namespace ocs2::legged_robot {
std::shared_ptr<FSMState> next_state_;
CtrlInterfaces ctrl_interfaces_;
std::shared_ptr<CtrlComponent> ctrl_comp_;
std::vector<std::string> joint_names_;
std::vector<std::string> feet_names_;
std::vector<std::string> command_interface_types_;
std::vector<std::string> state_interface_types_;
@ -88,7 +85,6 @@ namespace ocs2::legged_robot {
{"velocity", &ctrl_interfaces_.joint_velocity_state_interface_}
};
std::string robot_pkg_;
std::string command_prefix_;
// IMU Sensor
@ -104,9 +100,6 @@ namespace ocs2::legged_robot {
std::string odom_name_;
std::vector<std::string> odom_interface_types_;
double default_kp_ = 0;
double default_kd_ = 6;
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,
const vector_t& currentState,
const ReferenceManagerInterface& referenceManager)
const vector_t& /**currentState**/,
const ReferenceManagerInterface& /**referenceManager**/)
{
getTargetGait();
if (gait_updated_)
@ -51,9 +51,10 @@ namespace ocs2::legged_robot
if (ctrl_interfaces_.control_inputs_.command == 0) return;
if (ctrl_interfaces_.control_inputs_.command == last_command_) return;
last_command_ = ctrl_interfaces_.control_inputs_.command;
target_gait_ = gait_list_[ctrl_interfaces_.control_inputs_.command -2];
const int command = std::max(0, ctrl_interfaces_.control_inputs_.command - 2);
target_gait_ = gait_list_[command];
RCLCPP_INFO(rclcpp::get_logger("GaitManager"), "Switch to gait: %s",
gait_name_list_[ctrl_interfaces_.control_inputs_.command - 2].c_str());
gait_name_list_[command].c_str());
gait_updated_ = true;
}
}

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_);
}
void TargetManager::update(SystemObservation &observation)
void TargetManager::update(SystemObservation& observation)
{
vector_t cmdGoal = vector_t::Zero(6);
cmdGoal[0] = ctrl_component_.control_inputs_.ly * target_displacement_velocity_;

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

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

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

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

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

@ -5,14 +5,16 @@
#include "ocs2_quadruped_controller/wbc/HierarchicalWbc.h"
#include "ocs2_quadruped_controller/wbc/HoQp.h"
namespace ocs2::legged_robot {
vector_t HierarchicalWbc::update(const vector_t &stateDesired, const vector_t &inputDesired,
const vector_t &rbdStateMeasured, size_t mode,
scalar_t period) {
namespace ocs2::legged_robot
{
vector_t HierarchicalWbc::update(const vector_t& stateDesired, const vector_t& inputDesired,
const vector_t& rbdStateMeasured, size_t mode,
scalar_t period)
{
WbcBase::update(stateDesired, inputDesired, rbdStateMeasured, mode, period);
Task task0 = formulateFloatingBaseEomTask() + formulateTorqueLimitsTask() + formulateFrictionConeTask() +
formulateNoContactMotionTask();
formulateNoContactMotionTask();
Task task1 = formulateBaseAccelTask(stateDesired, inputDesired, period) + formulateSwingLegTask();
Task task2 = formulateContactForceTask(inputDesired);
HoQp hoQp(task2, std::make_shared<HoQp>(task1, std::make_shared<HoQp>(task0)));

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

@ -9,8 +9,10 @@
#include <qpOASES.hpp>
#include <utility>
namespace ocs2::legged_robot {
HoQp::HoQp(Task task, HoQpPtr higherProblem) : task_(std::move(task)), higherProblem_(std::move(higherProblem)) {
namespace ocs2::legged_robot
{
HoQp::HoQp(Task task, HoQpPtr higherProblem) : task_(std::move(task)), higherProblem_(std::move(higherProblem))
{
initVars();
formulateProblem();
solveProblem();
@ -19,14 +21,16 @@ namespace ocs2::legged_robot {
stackSlackSolutions();
}
void HoQp::initVars() {
void HoQp::initVars()
{
// Task variables
numSlackVars_ = task_.d_.rows();
hasEqConstraints_ = task_.a_.rows() > 0;
hasIneqConstraints_ = numSlackVars_ > 0;
// Pre-Task variables
if (higherProblem_ != nullptr) {
if (higherProblem_ != nullptr)
{
stackedZPrev_ = higherProblem_->getStackedZMatrix();
stackedTasksPrev_ = higherProblem_->getStackedTasks();
stackedSlackSolutionsPrev_ = higherProblem_->getStackedSlackSolutions();
@ -34,7 +38,9 @@ namespace ocs2::legged_robot {
numPrevSlackVars_ = higherProblem_->getSlackedNumVars();
numDecisionVars_ = stackedZPrev_.cols();
} else {
}
else
{
numDecisionVars_ = std::max(task_.a_.cols(), task_.d_.cols());
stackedTasksPrev_ = Task(numDecisionVars_);
@ -51,53 +57,66 @@ namespace ocs2::legged_robot {
zeroNvNx_ = matrix_t::Zero(numSlackVars_, numDecisionVars_);
}
void HoQp::formulateProblem() {
void HoQp::formulateProblem()
{
buildHMatrix();
buildCVector();
buildDMatrix();
buildFVector();
}
void HoQp::buildHMatrix() {
void HoQp::buildHMatrix()
{
matrix_t zTaTaz(numDecisionVars_, numDecisionVars_);
if (hasEqConstraints_) {
if (hasEqConstraints_)
{
// Make sure that all eigenvalues of A_t_A are non-negative, which could arise due to numerical issues
matrix_t aCurrZPrev = task_.a_ * stackedZPrev_;
zTaTaz = aCurrZPrev.transpose() * aCurrZPrev + 1e-12 * matrix_t::Identity(
numDecisionVars_, numDecisionVars_);
numDecisionVars_, numDecisionVars_);
// This way of splitting up the multiplication is about twice as fast as multiplying 4 matrices
} else {
}
else
{
zTaTaz.setZero();
}
h_ = (matrix_t(numDecisionVars_ + numSlackVars_, numDecisionVars_ + numSlackVars_) // clang-format off
<< zTaTaz, zeroNvNx_.transpose(),
zeroNvNx_, eyeNvNv_) // clang-format on
.finished();
.finished();
}
void HoQp::buildCVector() {
void HoQp::buildCVector()
{
vector_t c = vector_t::Zero(numDecisionVars_ + numSlackVars_);
vector_t zeroVec = vector_t::Zero(numSlackVars_);
vector_t temp(numDecisionVars_);
if (hasEqConstraints_) {
if (hasEqConstraints_)
{
temp = (task_.a_ * stackedZPrev_).transpose() * (task_.a_ * xPrev_ - task_.b_);
} else {
}
else
{
temp.setZero();
}
c_ = (vector_t(numDecisionVars_ + numSlackVars_) << temp, zeroVec).finished();
}
void HoQp::buildDMatrix() {
void HoQp::buildDMatrix()
{
matrix_t stackedZero = matrix_t::Zero(numPrevSlackVars_, numSlackVars_);
matrix_t dCurrZ;
if (hasIneqConstraints_) {
if (hasIneqConstraints_)
{
dCurrZ = task_.d_ * stackedZPrev_;
} else {
}
else
{
dCurrZ = matrix_t::Zero(0, numDecisionVars_);
}
@ -107,34 +126,43 @@ namespace ocs2::legged_robot {
<< zeroNvNx_, -eyeNvNv_,
stackedTasksPrev_.d_ * stackedZPrev_, stackedZero,
dCurrZ, -eyeNvNv_) // clang-format on
.finished();
.finished();
}
void HoQp::buildFVector() {
void HoQp::buildFVector()
{
vector_t zeroVec = vector_t::Zero(numSlackVars_);
vector_t fMinusDXPrev;
if (hasIneqConstraints_) {
if (hasIneqConstraints_)
{
fMinusDXPrev = task_.f_ - task_.d_ * xPrev_;
} else {
}
else
{
fMinusDXPrev = vector_t::Zero(0);
}
f_ = (vector_t(2 * numSlackVars_ + numPrevSlackVars_) << zeroVec,
stackedTasksPrev_.f_ - stackedTasksPrev_.d_ * xPrev_ + stackedSlackSolutionsPrev_, fMinusDXPrev)
.finished();
stackedTasksPrev_.f_ - stackedTasksPrev_.d_ * xPrev_ + stackedSlackSolutionsPrev_, fMinusDXPrev)
.finished();
}
void HoQp::buildZMatrix() {
if (hasEqConstraints_) {
void HoQp::buildZMatrix()
{
if (hasEqConstraints_)
{
assert((task_.a_.cols() > 0));
stackedZ_ = stackedZPrev_ * (task_.a_ * stackedZPrev_).fullPivLu().kernel();
} else {
}
else
{
stackedZ_ = stackedZPrev_;
}
}
void HoQp::solveProblem() {
void HoQp::solveProblem()
{
auto qpProblem = qpOASES::QProblem(numDecisionVars_ + numSlackVars_, f_.size());
qpOASES::Options options;
options.setToMPC();
@ -151,11 +179,15 @@ namespace ocs2::legged_robot {
slackVarsSolutions_ = qpSol.tail(numSlackVars_);
}
void HoQp::stackSlackSolutions() {
if (higherProblem_ != nullptr) {
void HoQp::stackSlackSolutions()
{
if (higherProblem_ != nullptr)
{
stackedSlackVars_ = Task::concatenateVectors(higherProblem_->getStackedSlackSolutions(),
slackVarsSolutions_);
} else {
}
else
{
stackedSlackVars_ = slackVarsSolutions_;
}
}

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

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

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

@ -10,10 +10,12 @@
#include <boost/property_tree/ptree.hpp>
#include <ocs2_core/misc/LoadData.h>
namespace ocs2::legged_robot {
vector_t WeightedWbc::update(const vector_t &stateDesired, const vector_t &inputDesired,
const vector_t &rbdStateMeasured, size_t mode,
scalar_t period) {
namespace ocs2::legged_robot
{
vector_t WeightedWbc::update(const vector_t& stateDesired, const vector_t& inputDesired,
const vector_t& rbdStateMeasured, size_t mode,
scalar_t period)
{
WbcBase::update(stateDesired, inputDesired, rbdStateMeasured, mode, period);
// Constraints
@ -33,7 +35,7 @@ namespace ocs2::legged_robot {
// Cost
Task weighedTask = formulateWeightedTasks(stateDesired, inputDesired, period);
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> H =
weighedTask.a_.transpose() * weighedTask.a_;
weighedTask.a_.transpose() * weighedTask.a_;
vector_t g = -weighedTask.a_.transpose() * weighedTask.b_;
// Solve
@ -52,25 +54,29 @@ namespace ocs2::legged_robot {
return qpSol;
}
Task WeightedWbc::formulateConstraints() {
Task WeightedWbc::formulateConstraints()
{
return formulateFloatingBaseEomTask() + formulateTorqueLimitsTask() + formulateFrictionConeTask() +
formulateNoContactMotionTask();
formulateNoContactMotionTask();
}
Task WeightedWbc::formulateWeightedTasks(const vector_t &stateDesired, const vector_t &inputDesired,
scalar_t period) {
Task WeightedWbc::formulateWeightedTasks(const vector_t& stateDesired, const vector_t& inputDesired,
scalar_t period)
{
return formulateSwingLegTask() * weightSwingLeg_ + formulateBaseAccelTask(stateDesired, inputDesired, period) *
weightBaseAccel_ +
formulateContactForceTask(inputDesired) * weightContactForce_;
weightBaseAccel_ +
formulateContactForceTask(inputDesired) * weightContactForce_;
}
void WeightedWbc::loadTasksSetting(const std::string &taskFile, bool verbose) {
void WeightedWbc::loadTasksSetting(const std::string& taskFile, bool verbose)
{
WbcBase::loadTasksSetting(taskFile, verbose);
boost::property_tree::ptree pt;
read_info(taskFile, pt);
const std::string prefix = "weight.";
if (verbose) {
if (verbose)
{
std::cerr << "\n #### WBC weight:";
std::cerr << "\n #### =============================================================================\n";
}

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

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

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

@ -108,7 +108,8 @@ unitree_guide_controller:
ocs2_quadruped_controller:
ros__parameters:
update_rate: 100 # Hz
update_rate: 200 # Hz
robot_pkg: "x30_description"
default_kd: 8.0
joints:
- FL_HipX
@ -213,6 +214,8 @@ rl_quadruped_controller:
- 0.0
- -1.0
- 1.8
- -0.732
- 1.361
command_interfaces:
- effort
@ -253,92 +256,3 @@ rl_quadruped_controller:
- linear_acceleration.x
- linear_acceleration.y
- linear_acceleration.z
rl_quadruped_controller:
ros__parameters:
update_rate: 200 # Hz
robot_pkg: "x30_description"
model_folder: "legged_gym"
stand_kp: 500.0
stand_kd: 20.0
joints:
- FL_HipX
- FL_HipY
- FL_Knee
- FR_HipX
- FR_HipY
- FR_Knee
- HL_HipX
- HL_HipY
- HL_Knee
- HR_HipX
- HR_HipY
- HR_Knee
down_pos:
- 0.0
- -1.22
- 2.61
- 0.0
- -1.22
- 2.61
- 0.0
- -1.22
- 2.61
- 0.0
- -1.22
- 2.61
stand_pos:
- 0.0
- -0.732
- 1.361
- 0.0
- -0.732
- 1.361
- 0.0
- -0.732
- 1.361
- 0.0
- -0.732
- 1.361
command_interfaces:
- effort
- position
- velocity
- kp
- kd
state_interfaces:
- effort
- position
- velocity
feet_names:
- FL_FOOT
- FR_FOOT
- HL_FOOT
- HR_FOOT
foot_force_name: "foot_force"
foot_force_interfaces:
- FL
- FR
- HL
- HR
imu_name: "imu_sensor"
base_name: "base"
imu_interfaces:
- orientation.w
- orientation.x
- orientation.y
- orientation.z
- angular_velocity.x
- angular_velocity.y
- angular_velocity.z
- linear_acceleration.x
- linear_acceleration.y
- linear_acceleration.z

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

@ -52,7 +52,7 @@ ros2 launch a1_description visualize.launch.py
source ~/ros2_ws/install/setup.bash
ros2 launch unitree_guide_controller gazebo_classic.launch.py pkg_description:=a1_description height:=0.43
```
### Gazebo Harmonic (ROS2 Jazzy)
* Unitree Guide Controller
@ -63,7 +63,7 @@ ros2 launch a1_description visualize.launch.py
* OCS2 Quadruped Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch ocs2_quadruped_controller gazebo.launch.py pkg_description:=a1_description
ros2 launch ocs2_quadruped_controller gazebo.launch.py pkg_description:=a1_description height:=0.43
```
* RL Quadruped Controller
```bash

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

@ -10,9 +10,6 @@ controller_manager:
imu_sensor_broadcaster:
type: imu_sensor_broadcaster/IMUSensorBroadcaster
leg_pd_controller:
type: leg_pd_controller/LegPdController
unitree_guide_controller:
type: unitree_guide_controller/UnitreeGuideController
@ -27,33 +24,9 @@ imu_sensor_broadcaster:
sensor_name: "imu_sensor"
frame_id: "imu_link"
leg_pd_controller:
ros__parameters:
joints:
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- FL_hip_joint
- FL_thigh_joint
- FL_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
command_interfaces:
- effort
state_interfaces:
- position
- velocity
unitree_guide_controller:
ros__parameters:
command_prefix: "leg_pd_controller"
update_rate: 200 # Hz
update_rate: 500 # Hz
joints:
- FR_hip_joint
- FR_thigh_joint
@ -105,7 +78,9 @@ ocs2_quadruped_controller:
ros__parameters:
update_rate: 500 # Hz
robot_pkg: "a1_description"
command_prefix: "leg_pd_controller"
feet_force_threshold: 5.0
# default_kp: 1.0
default_kd: 0.5
joints:
- FL_hip_joint
- FL_thigh_joint
@ -153,7 +128,12 @@ ocs2_quadruped_controller:
- linear_acceleration.y
- linear_acceleration.z
estimator_type: "odom_topic"
foot_force_name: "foot_force"
foot_force_interfaces:
- FL_foot_force
- RL_foot_force
- FR_foot_force
- RR_foot_force
rl_quadruped_controller:
ros__parameters:
@ -164,12 +144,12 @@ rl_quadruped_controller:
- FL_hip_joint
- FL_thigh_joint
- FL_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint

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

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

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

@ -23,48 +23,6 @@ swing_trajectory_config
swingTimeScale 0.15
}
; DDP settings
ddp
{
algorithm SLQ
nThreads 3
threadPriority 50
maxNumIterations 1
minRelCost 1e-1
constraintTolerance 5e-3
displayInfo false
displayShortSummary false
checkNumericalStability false
debugPrintRollout false
debugCaching false
AbsTolODE 1e-5
RelTolODE 1e-3
maxNumStepsPerSecond 10000
timeStep 0.015
backwardPassIntegratorType ODE45
constraintPenaltyInitialValue 20.0
constraintPenaltyIncreaseRate 2.0
preComputeRiccatiTerms true
useFeedbackPolicy false
strategy LINE_SEARCH
lineSearch
{
minStepLength 1e-2
maxStepLength 1.0
hessianCorrectionStrategy DIAGONAL_SHIFT
hessianCorrectionMultiple 1e-5
}
}
; Multiple_Shooting SQP settings
sqp
{
@ -85,39 +43,6 @@ sqp
threadPriority 50
}
; Multiple_Shooting IPM settings
ipm
{
nThreads 3
dt 0.015
ipmIteration 1
deltaTol 1e-4
g_max 10.0
g_min 1e-6
computeLagrangeMultipliers true
printSolverStatistics true
printSolverStatus false
printLinesearch false
useFeedbackPolicy false
integratorType RK2
threadPriority 50
initialBarrierParameter 1e-4
targetBarrierParameter 1e-4
barrierLinearDecreaseFactor 0.2
barrierSuperlinearDecreasePower 1.5
barrierReductionCostTol 1e-3
barrierReductionConstraintTol 1e-3
fractionToBoundaryMargin 0.995
usePrimalStepSizeForDual false
initialSlackLowerBound 1e-4
initialDualLowerBound 1e-4
initialSlackMarginRate 1e-2
initialDualMarginRate 1e-2
}
; Rollout settings
rollout
{
@ -160,18 +85,18 @@ initialState
(11,0) 0.0 ; theta_base_x
;; Leg Joint Positions: [FL, RL, FR, RR] ;;
(12,0) -0.20 ; FL_hip_joint
(13,0) 0.72 ; FL_thigh_joint
(14,0) -1.44 ; FL_calf_joint
(15,0) -0.20 ; RL_hip_joint
(16,0) 0.72 ; RL_thigh_joint
(17,0) -1.44 ; RL_calf_joint
(18,0) 0.20 ; FR_hip_joint
(19,0) 0.72 ; FR_thigh_joint
(20,0) -1.44 ; FR_calf_joint
(21,0) 0.20 ; RR_hip_joint
(22,0) 0.72 ; RR_thigh_joint
(23,0) -1.44 ; RR_calf_joint
(12,0) -0.107 ; FL_hip_joint
(13,0) 0.77 ; FL_thigh_joint
(14,0) -1.68 ; FL_calf_joint
(15,0) -0.107 ; RL_hip_joint
(16,0) 0.77 ; RL_thigh_joint
(17,0) -1.68 ; RL_calf_joint
(18,0) 0.107 ; FR_hip_joint
(19,0) 0.77 ; FR_thigh_joint
(20,0) -1.68 ; FR_calf_joint
(21,0) 0.107 ; RR_hip_joint
(22,0) 0.77 ; RR_thigh_joint
(23,0) -1.68 ; RR_calf_joint
}
; standard state weight matrix
@ -295,15 +220,15 @@ frictionConeTask
swingLegTask
{
kp 350
kd 37
kp 300
kd 30
}
weight
{
swingLeg 100
baseAccel 1
contactForce 0.01
contactForce 0.05
}
; State Estimation

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

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

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

@ -35,7 +35,7 @@
<!-- joint limits -->
<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_min" value="-46"/>
<xacro:property name="hip_velocity_max" value="21"/>

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

@ -162,21 +162,12 @@
<gazebo>
<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 filename="gz-sim-imu-system"
name="gz::sim::systems::Imu">
</plugin>
<plugin filename="gz-sim-odometry-publisher-system"
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>
<plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"/>
</gazebo>
<gazebo reference="trunk">

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

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

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

@ -49,4 +49,9 @@ ros2 launch b2_description visualize.launch.py
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=b2_description height:=0.68
```
* OCS2 Quadruped Controller
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch ocs2_quadruped_controller gazebo.launch.py pkg_description:=b2_description height:=0.68
```

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

@ -10,8 +10,8 @@ controller_manager:
imu_sensor_broadcaster:
type: imu_sensor_broadcaster/IMUSensorBroadcaster
leg_pd_controller:
type: leg_pd_controller/LegPdController
ocs2_quadruped_controller:
type: ocs2_quadruped_controller/Ocs2QuadrupedController
unitree_guide_controller:
type: unitree_guide_controller/UnitreeGuideController
@ -24,33 +24,69 @@ imu_sensor_broadcaster:
sensor_name: "imu_sensor"
frame_id: "imu_link"
leg_pd_controller:
ocs2_quadruped_controller:
ros__parameters:
update_rate: 1000 # Hz
update_rate: 500 # Hz
robot_pkg: "b2_description"
feet_force_threshold: 5.0
default_kd: 3.5
joints:
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- FL_hip_joint
- FL_thigh_joint
- FL_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
command_interfaces:
- effort
- position
- velocity
- kp
- kd
state_interfaces:
- effort
- position
- velocity
feet:
- FL_foot
- FR_foot
- RL_foot
- RR_foot
imu_name: "imu_sensor"
base_name: "base"
imu_interfaces:
- orientation.w
- orientation.x
- orientation.y
- orientation.z
- angular_velocity.x
- angular_velocity.y
- angular_velocity.z
- linear_acceleration.x
- linear_acceleration.y
- linear_acceleration.z
foot_force_name: "foot_force"
foot_force_interfaces:
- FL_foot_force
- RL_foot_force
- FR_foot_force
- RR_foot_force
unitree_guide_controller:
ros__parameters:
command_prefix: "leg_pd_controller"
update_rate: 500 # Hz
stand_kp: 500.0
stand_kd: 40.0

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

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

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

@ -2,159 +2,159 @@
<robot name="b2_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Constants for robot dimensions -->
<xacro:property name="PI" value="3.1415926535897931"/>
<!-- <xacro:property name="stick_mass" value="0.00001"/> -->
<!-- Constants for robot dimensions -->
<xacro:property name="PI" value="3.1415926535897931"/>
<xacro:property name="stick_mass" value="0.00001"/>
<!-- simplified collision value -->
<xacro:property name="trunk_width" value="0.28"/>
<xacro:property name="trunk_length" value="0.5"/>
<xacro:property name="trunk_height" value="0.15"/>
<xacro:property name="hip_radius" value="0.07"/>
<xacro:property name="hip_length" value="0.05"/>
<!-- <xacro:property name="thigh_shoulder_radius" value="0.044"/>
<xacro:property name="thigh_shoulder_length" value="0.08"/> -->
<xacro:property name="thigh_width" value="0.0455"/>
<xacro:property name="thigh_height" value="0.054"/>
<!-- <xacro:property name="calf_width" value="0.04"/>
<xacro:property name="calf_height" value="0.05"/> -->
<xacro:property name="foot_radius" value="0.04"/>
<!-- <xacro:property name="stick_radius" value="0.01"/>
<xacro:property name="stick_length" value="0.2"/> -->
<!-- simplified collision value -->
<xacro:property name="trunk_width" value="0.28"/>
<xacro:property name="trunk_length" value="0.5"/>
<xacro:property name="trunk_height" value="0.15"/>
<xacro:property name="hip_radius" value="0.07"/>
<xacro:property name="hip_length" value="0.05"/>
<xacro:property name="thigh_shoulder_radius" value="0.044"/>
<xacro:property name="thigh_shoulder_length" value="0.08"/>
<xacro:property name="thigh_width" value="0.0455"/>
<xacro:property name="thigh_height" value="0.054"/>
<xacro:property name="calf_width" value="0.04"/>
<xacro:property name="calf_height" value="0.05"/>
<xacro:property name="foot_radius" value="0.04"/>
<xacro:property name="stick_radius" value="0.01"/>
<xacro:property name="stick_length" value="0.2"/>
<!-- kinematic value -->
<xacro:property name="thigh_offset" value="0.11973"/>
<xacro:property name="thigh_length" value="0.32"/>
<xacro:property name="calf_length" value="0.35"/>
<!-- kinematic value -->
<xacro:property name="thigh_offset" value="0.11973"/>
<xacro:property name="thigh_length" value="0.32"/>
<xacro:property name="calf_length" value="0.35"/>
<!-- leg offset from trunk center value -->
<xacro:property name="leg_offset_x" value="0.3285"/>
<xacro:property name="leg_offset_y" value="0.072"/>
<!-- <xacro:property name="trunk_offset_z" value="0.0"/> -->
<xacro:property name="hip_offset" value="0.11973"/>
<!-- leg offset from trunk center value -->
<xacro:property name="leg_offset_x" value="0.3285"/>
<xacro:property name="leg_offset_y" value="0.072"/>
<!-- <xacro:property name="trunk_offset_z" value="0.0"/> -->
<xacro:property name="hip_offset" value="0.11973"/>
<!-- offset of link and rotor locations (left front) -->
<!-- <xacro:property name="hip_offset_x" value="0.3285"/>
<xacro:property name="hip_offset_y" value="0.072"/>
<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_y" value="0.072"/>
<xacro:property name="hip_rotor_offset_z" value="0.0"/>
<!-- offset of link and rotor locations (left front) -->
<xacro:property name="hip_offset_x" value="0.3285"/>
<xacro:property name="hip_offset_y" value="0.072"/>
<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_y" value="0.072"/>
<xacro:property name="hip_rotor_offset_z" value="0.0"/>
<!-- <xacro:property name="thigh_offset_x" value="0"/>
<xacro:property name="thigh_offset_y" value="0.11973"/>
<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_y" value="0.00798"/>
<xacro:property name="thigh_rotor_offset_z" value="0.0"/>
<xacro:property name="thigh_offset_x" value="0"/>
<xacro:property name="thigh_offset_y" value="0.11973"/>
<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_y" value="0.00798"/>
<xacro:property name="thigh_rotor_offset_z" 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_z" value="-0.35"/> -->
<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_z" 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_z" value="-0.35"/>
<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_z" value="0.0"/>
<!-- joint limits -->
<xacro:property name="damping" value="0.1"/>
<xacro:property name="friction" value="0.1"/>
<xacro:property name="hip_position_max" value="0.87"/>
<xacro:property name="hip_position_min" value="-0.87"/>
<xacro:property name="hip_velocity_max" value="23"/>
<xacro:property name="hip_torque_max" value="200"/>
<xacro:property name="thigh_position_max" value="4.69"/>
<xacro:property name="thigh_position_min" value="-94"/>
<xacro:property name="thigh_velocity_max" value="23"/>
<xacro:property name="thigh_torque_max" value="200"/>
<xacro:property name="calf_position_max" value="-0.43"/>
<xacro:property name="calf_position_min" value="-2.82"/>
<xacro:property name="calf_velocity_max" value="14"/>
<xacro:property name="calf_torque_max" value="320"/>
<!-- joint limits -->
<xacro:property name="damping" value="0.1"/>
<xacro:property name="friction" value="0.1"/>
<xacro:property name="hip_position_max" value="0.87"/>
<xacro:property name="hip_position_min" value="-0.87"/>
<xacro:property name="hip_velocity_max" value="23"/>
<xacro:property name="hip_torque_max" value="200"/>
<xacro:property name="thigh_position_max" value="4.69"/>
<xacro:property name="thigh_position_min" value="-94"/>
<xacro:property name="thigh_velocity_max" value="23"/>
<xacro:property name="thigh_torque_max" value="200"/>
<xacro:property name="calf_position_max" value="-0.43"/>
<xacro:property name="calf_position_min" value="-2.82"/>
<xacro:property name="calf_velocity_max" value="14"/>
<xacro:property name="calf_torque_max" value="320"/>
<!-- dynamics inertial value -->
<!-- trunk -->
<xacro:property name="trunk_mass" value="35.606"/>
<xacro:property name="trunk_com_x" value="0.000458"/>
<xacro:property name="trunk_com_y" value="0.005261"/>
<xacro:property name="trunk_com_z" value="0.000665"/>
<xacro:property name="trunk_ixx" value="0.27466"/>
<xacro:property name="trunk_ixy" value="-0.000622"/>
<xacro:property name="trunk_ixz" value="-0.00315"/>
<xacro:property name="trunk_iyy" value="1.0618"/>
<xacro:property name="trunk_iyz" value="-0.00139"/>
<xacro:property name="trunk_izz" value="1.1825"/>
<!-- dynamics inertial value -->
<!-- trunk -->
<xacro:property name="trunk_mass" value="35.606"/>
<xacro:property name="trunk_com_x" value="0.000458"/>
<xacro:property name="trunk_com_y" value="0.005261"/>
<xacro:property name="trunk_com_z" value="0.000665"/>
<xacro:property name="trunk_ixx" value="0.27466"/>
<xacro:property name="trunk_ixy" value="-0.000622"/>
<xacro:property name="trunk_ixz" value="-0.00315"/>
<xacro:property name="trunk_iyy" value="1.0618"/>
<xacro:property name="trunk_iyz" value="-0.00139"/>
<xacro:property name="trunk_izz" value="1.1825"/>
<!-- hip (left front) -->
<xacro:property name="hip_mass" value="2.256"/>
<xacro:property name="hip_com_x" value="-0.009305"/>
<xacro:property name="hip_com_y" value="-0.010228"/>
<xacro:property name="hip_com_z" value="0.000264"/>
<xacro:property name="hip_ixx" value="0.0026431"/>
<xacro:property name="hip_ixy" value="0.00019234"/>
<xacro:property name="hip_ixz" value="-6.76E-06"/>
<xacro:property name="hip_iyy" value="0.0046728"/>
<xacro:property name="hip_iyz" value="-7.16E-06"/>
<xacro:property name="hip_izz" value="0.0034208"/>
<!-- hip (left front) -->
<xacro:property name="hip_mass" value="2.256"/>
<xacro:property name="hip_com_x" value="-0.009305"/>
<xacro:property name="hip_com_y" value="-0.010228"/>
<xacro:property name="hip_com_z" value="0.000264"/>
<xacro:property name="hip_ixx" value="0.0026431"/>
<xacro:property name="hip_ixy" value="0.00019234"/>
<xacro:property name="hip_ixz" value="-6.76E-06"/>
<xacro:property name="hip_iyy" value="0.0046728"/>
<xacro:property name="hip_iyz" value="-7.16E-06"/>
<xacro:property name="hip_izz" value="0.0034208"/>
<xacro:property name="hip_rotor_mass" value="0.2734"/>
<xacro:property name="hip_rotor_com_x" value="0.0"/>
<xacro:property name="hip_rotor_com_y" value="0.0"/>
<xacro:property name="hip_rotor_com_z" value="0.0"/>
<xacro:property name="hip_rotor_ixx" value="0.000144463"/>
<xacro:property name="hip_rotor_ixy" value="0.0"/>
<xacro:property name="hip_rotor_ixz" value="0.0"/>
<xacro:property name="hip_rotor_iyy" value="0.000144463"/>
<xacro:property name="hip_rotor_iyz" value="0.0"/>
<xacro:property name="hip_rotor_izz" value="0.000263053"/>
<xacro:property name="hip_rotor_mass" value="0.2734"/>
<xacro:property name="hip_rotor_com_x" value="0.0"/>
<xacro:property name="hip_rotor_com_y" value="0.0"/>
<xacro:property name="hip_rotor_com_z" value="0.0"/>
<xacro:property name="hip_rotor_ixx" value="0.000144463"/>
<xacro:property name="hip_rotor_ixy" value="0.0"/>
<xacro:property name="hip_rotor_ixz" value="0.0"/>
<xacro:property name="hip_rotor_iyy" value="0.000144463"/>
<xacro:property name="hip_rotor_iyz" value="0.0"/>
<xacro:property name="hip_rotor_izz" value="0.000263053"/>
<!-- thigh -->
<xacro:property name="thigh_mass" value="3.591"/>
<xacro:property name="thigh_com_x" value="-0.004346"/>
<xacro:property name="thigh_com_y" value="-0.035797"/>
<xacro:property name="thigh_com_z" value="-0.044921"/>
<xacro:property name="thigh_ixx" value="0.041718"/>
<xacro:property name="thigh_ixy" value="0.00055623"/>
<xacro:property name="thigh_ixz" value="-0.0022768"/>
<xacro:property name="thigh_iyy" value="0.041071"/>
<xacro:property name="thigh_iyz" value="0.005796"/>
<xacro:property name="thigh_izz" value="0.0065677"/>
<!-- thigh -->
<xacro:property name="thigh_mass" value="3.591"/>
<xacro:property name="thigh_com_x" value="-0.004346"/>
<xacro:property name="thigh_com_y" value="-0.035797"/>
<xacro:property name="thigh_com_z" value="-0.044921"/>
<xacro:property name="thigh_ixx" value="0.041718"/>
<xacro:property name="thigh_ixy" value="0.00055623"/>
<xacro:property name="thigh_ixz" value="-0.0022768"/>
<xacro:property name="thigh_iyy" value="0.041071"/>
<xacro:property name="thigh_iyz" value="0.005796"/>
<xacro:property name="thigh_izz" value="0.0065677"/>
<xacro:property name="thigh_rotor_mass" value="0.2734"/>
<xacro:property name="thigh_rotor_com_x" value="0.0"/>
<xacro:property name="thigh_rotor_com_y" value="0.0"/>
<xacro:property name="thigh_rotor_com_z" value="0.0"/>
<xacro:property name="thigh_rotor_ixx" value="0.000144463"/>
<xacro:property name="thigh_rotor_ixy" value="0.0"/>
<xacro:property name="thigh_rotor_ixz" value="0.0"/>
<xacro:property name="thigh_rotor_iyy" value="0.000144463"/>
<xacro:property name="thigh_rotor_iyz" value="0.0"/>
<xacro:property name="thigh_rotor_izz" value="0.000263053"/>
<xacro:property name="thigh_rotor_mass" value="0.2734"/>
<xacro:property name="thigh_rotor_com_x" value="0.0"/>
<xacro:property name="thigh_rotor_com_y" value="0.0"/>
<xacro:property name="thigh_rotor_com_z" value="0.0"/>
<xacro:property name="thigh_rotor_ixx" value="0.000144463"/>
<xacro:property name="thigh_rotor_ixy" value="0.0"/>
<xacro:property name="thigh_rotor_ixz" value="0.0"/>
<xacro:property name="thigh_rotor_iyy" value="0.000144463"/>
<xacro:property name="thigh_rotor_iyz" value="0.0"/>
<xacro:property name="thigh_rotor_izz" value="0.000263053"/>
<!-- calf -->
<xacro:property name="calf_mass" value="0.58094"/>
<xacro:property name="calf_com_x" value="0.012422"/>
<xacro:property name="calf_com_y" value="0.0"/>
<xacro:property name="calf_com_z" value="-0.12499"/>
<xacro:property name="calf_ixx" value="0.01143"/>
<xacro:property name="calf_ixy" value="0"/>
<xacro:property name="calf_ixz" value="0.000643"/>
<xacro:property name="calf_iyy" value="0.011534"/>
<xacro:property name="calf_iyz" value="0"/>
<xacro:property name="calf_izz" value="0.000331"/>
<!-- calf -->
<xacro:property name="calf_mass" value="0.58094"/>
<xacro:property name="calf_com_x" value="0.012422"/>
<xacro:property name="calf_com_y" value="0.0"/>
<xacro:property name="calf_com_z" value="-0.12499"/>
<xacro:property name="calf_ixx" value="0.01143"/>
<xacro:property name="calf_ixy" value="0"/>
<xacro:property name="calf_ixz" value="0.000643"/>
<xacro:property name="calf_iyy" value="0.011534"/>
<xacro:property name="calf_iyz" value="0"/>
<xacro:property name="calf_izz" value="0.000331"/>
<xacro:property name="calf_rotor_mass" value="0.2734"/>
<xacro:property name="calf_rotor_com_x" value="0.0"/>
<xacro:property name="calf_rotor_com_y" value="0.0"/>
<xacro:property name="calf_rotor_com_z" value="0.0"/>
<xacro:property name="calf_rotor_ixx" value="0.000144463"/>
<xacro:property name="calf_rotor_ixy" value="0.0"/>
<xacro:property name="calf_rotor_ixz" value="0.0"/>
<xacro:property name="calf_rotor_iyy" value="0.000144463"/>
<xacro:property name="calf_rotor_iyz" value="0.0"/>
<xacro:property name="calf_rotor_izz" value="0.000263053"/>
<xacro:property name="calf_rotor_mass" value="0.2734"/>
<xacro:property name="calf_rotor_com_x" value="0.0"/>
<xacro:property name="calf_rotor_com_y" value="0.0"/>
<xacro:property name="calf_rotor_com_z" value="0.0"/>
<xacro:property name="calf_rotor_ixx" value="0.000144463"/>
<xacro:property name="calf_rotor_ixy" value="0.0"/>
<xacro:property name="calf_rotor_ixz" value="0.0"/>
<xacro:property name="calf_rotor_iyy" value="0.000144463"/>
<xacro:property name="calf_rotor_iyz" value="0.0"/>
<xacro:property name="calf_rotor_izz" value="0.000263053"/>
<!-- foot -->
<xacro:property name="foot_mass" value="0.098183"/>
<!-- foot -->
<xacro:property name="foot_mass" value="0.098183"/>
</robot>

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

@ -1,188 +1,250 @@
<?xml version="1.0"?>
<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">
<hardware>
<plugin>gz_quadruped_hardware/GazeboSimSystem</plugin>
</hardware>
<ros2_control name="GazeboSystem" type="system">
<hardware>
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
</hardware>
<joint name="FR_hip_joint">
<command_interface name="position"/>
<command_interface name="velocity"/>
<command_interface name="effort"/>
<command_interface name="kp"/>
<command_interface name="kd"/>
<joint name="FR_hip_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FR_thigh_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FR_thigh_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FR_calf_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FR_calf_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_hip_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_hip_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_thigh_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_thigh_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_calf_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="FL_calf_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_hip_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_hip_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_thigh_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_thigh_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_calf_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RR_calf_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_hip_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_hip_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_thigh_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_thigh_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_calf_joint">
<command_interface name="effort" initial_value="0.0"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="RL_calf_joint">
<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="velocity"/>
<state_interface name="effort"/>
</joint>
<sensor name="imu_sensor">
<state_interface name="orientation.x"/>
<state_interface name="orientation.y"/>
<state_interface name="orientation.z"/>
<state_interface name="orientation.w"/>
<state_interface name="angular_velocity.x"/>
<state_interface name="angular_velocity.y"/>
<state_interface name="angular_velocity.z"/>
<state_interface name="linear_acceleration.x"/>
<state_interface name="linear_acceleration.y"/>
<state_interface name="linear_acceleration.z"/>
</sensor>
</ros2_control>
<sensor name="imu_sensor">
<state_interface name="orientation.x"/>
<state_interface name="orientation.y"/>
<state_interface name="orientation.z"/>
<state_interface name="orientation.w"/>
<state_interface name="angular_velocity.x"/>
<state_interface name="angular_velocity.y"/>
<state_interface name="angular_velocity.z"/>
<state_interface name="linear_acceleration.x"/>
<state_interface name="linear_acceleration.y"/>
<state_interface name="linear_acceleration.z"/>
</sensor>
<gazebo>
<plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
<parameters>$(find b2_description)/config/gazebo.yaml</parameters>
</plugin>
<plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu">
</plugin>
</gazebo>
<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>
<gazebo reference="trunk">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo>
<plugin filename="gz_quadruped_hardware-system" name="gz_quadruped_hardware::GazeboSimQuadrupedPlugin">
<parameters>$(find b2_description)/config/gazebo.yaml</parameters>
</plugin>
<plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu">
</plugin>
<plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"/>
</gazebo>
<gazebo reference="imu_link">
<sensor name="imu_sensor" type="imu">
<always_on>1</always_on>
<update_rate>500</update_rate>
<visualize>true</visualize>
<topic>imu</topic>
<imu>
<angular_velocity>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</z>
</angular_velocity>
<linear_acceleration>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</z>
</linear_acceleration>
</imu>
</sensor>
</gazebo>
<gazebo reference="imu_joint">
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
<gazebo reference="trunk">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="imu_link">
<sensor name="imu_sensor" type="imu">
<always_on>1</always_on>
<update_rate>500</update_rate>
<visualize>true</visualize>
<topic>imu</topic>
<imu>
<angular_velocity>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</z>
</angular_velocity>
<linear_acceleration>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</z>
</linear_acceleration>
</imu>
</sensor>
</gazebo>
<gazebo reference="imu_joint">
<disableFixedJointLumping>true</disableFixedJointLumping>
</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>

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

@ -90,7 +90,6 @@
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
<material name="green"/>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="${hip_rotor_com_x} ${hip_rotor_com_y} ${hip_rotor_com_z}"/>
@ -152,7 +151,6 @@
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
<material name="green"/>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="${thigh_rotor_com_x} ${thigh_rotor_com_y} ${thigh_rotor_com_z}"/>
@ -227,7 +225,6 @@
<geometry>
<cylinder length="0.02" radius="0.05"/>
</geometry>
<material name="green"/>
</visual>
<inertial>
<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:
ros__parameters:
update_rate: 200 # Hz
update_rate: 1000 # Hz
# Define the available controllers
joint_state_broadcaster:
@ -73,8 +73,8 @@ unitree_guide_controller:
ocs2_quadruped_controller:
ros__parameters:
update_rate: 100 # Hz
update_rate: 500 # Hz
robot_pkg: "go1_description"
joints:
- FL_hip_joint
- FL_thigh_joint

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

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

358
descriptions/unitree/go2_description/xacro/leg.xacro
View File

@ -2,202 +2,202 @@
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find go2_description)/xacro/transmission.xacro"/>
<xacro:include filename="$(find go2_description)/xacro/transmission.xacro"/>
<xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
<xacro:macro name="leg" params="name mirror mirror_dae front_hind front_hind_dae">
<joint name="${name}_hip_joint" type="revolute">
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if>
<parent link="trunk"/>
<child link="${name}_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<xacro:if value="${(mirror_dae == True)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}"
upper="${hip_position_max}"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}"
upper="${-hip_position_min}"/>
</xacro:if>
</joint>
<joint name="${name}_hip_joint" type="revolute">
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="${leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${-leg_offset_y} 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 0 0" xyz="${-leg_offset_x} ${leg_offset_y} 0"/>
</xacro:if>
<parent link="trunk"/>
<child link="${name}_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<xacro:if value="${(mirror_dae == True)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${hip_position_min}"
upper="${hip_position_max}"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False)}">
<limit effort="${hip_torque_max}" velocity="${hip_velocity_max}" lower="${-hip_position_max}"
upper="${-hip_position_min}"/>
</xacro:if>
</joint>
<link name="${name}_hip">
<visual>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="${PI} 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<geometry>
<mesh filename="file://$(find go2_description)/meshes/hip.dae" scale="1 1 1"/>
</geometry>
</visual>
<collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
<geometry>
<cylinder length="${hip_length}" radius="${hip_radius}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
<mass value="${hip_mass}"/>
<inertia
ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
izz="${hip_izz}"/>
</inertial>
</link>
<link name="${name}_hip">
<visual>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == True)}">
<origin rpy="0 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == True)}">
<origin rpy="${PI} 0 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == True) and (front_hind_dae == False)}">
<origin rpy="0 ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<xacro:if value="${(mirror_dae == False) and (front_hind_dae == False)}">
<origin rpy="${PI} ${PI} 0" xyz="0 0 0"/>
</xacro:if>
<geometry>
<mesh filename="file://$(find go2_description)/meshes/hip.dae" scale="1 1 1"/>
</geometry>
</visual>
<collision>
<origin rpy="${PI/2.0} 0 0" xyz="0 ${hip_offset*mirror} 0"/>
<geometry>
<cylinder length="${hip_length}" radius="${hip_radius}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${hip_com_x*front_hind} ${hip_com_y*mirror} ${hip_com_z}"/>
<mass value="${hip_mass}"/>
<inertia
ixx="${hip_ixx}" ixy="${hip_ixy*mirror*front_hind}" ixz="${hip_ixz*front_hind}"
iyy="${hip_iyy}" iyz="${hip_iyz*mirror}"
izz="${hip_izz}"/>
</inertial>
</link>
<joint name="${name}_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
<parent link="${name}_hip"/>
<child link="${name}_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}"
upper="${thigh_position_max}"/>
</joint>
<joint name="${name}_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 ${thigh_offset*mirror} 0"/>
<parent link="${name}_hip"/>
<child link="${name}_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${thigh_torque_max}" velocity="${thigh_velocity_max}" lower="${thigh_position_min}"
upper="${thigh_position_max}"/>
</joint>
<link name="${name}_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<xacro:if value="${mirror_dae == True}">
<mesh filename="file://$(find go2_description)/meshes/thigh.dae" scale="1 1 1"/>
</xacro:if>
<xacro:if value="${mirror_dae == False}">
<mesh filename="file://$(find go2_description)/meshes/thigh_mirror.dae" scale="1 1 1"/>
</xacro:if>
</geometry>
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
<geometry>
<box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
<mass value="${thigh_mass}"/>
<inertia
ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
izz="${thigh_izz}"/>
</inertial>
</link>
<link name="${name}_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<xacro:if value="${mirror_dae == True}">
<mesh filename="file://$(find go2_description)/meshes/thigh.dae" scale="1 1 1"/>
</xacro:if>
<xacro:if value="${mirror_dae == False}">
<mesh filename="file://$(find go2_description)/meshes/thigh_mirror.dae" scale="1 1 1"/>
</xacro:if>
</geometry>
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-thigh_length/2.0}"/>
<geometry>
<box size="${thigh_length} ${thigh_width} ${thigh_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${thigh_com_x} ${thigh_com_y*mirror} ${thigh_com_z}"/>
<mass value="${thigh_mass}"/>
<inertia
ixx="${thigh_ixx}" ixy="${thigh_ixy*mirror}" ixz="${thigh_ixz}"
iyy="${thigh_iyy}" iyz="${thigh_iyz*mirror}"
izz="${thigh_izz}"/>
</inertial>
</link>
<joint name="${name}_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
<parent link="${name}_thigh"/>
<child link="${name}_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}"
upper="${calf_position_max}"/>
</joint>
<joint name="${name}_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 ${-thigh_length}"/>
<parent link="${name}_thigh"/>
<child link="${name}_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damping}" friction="${friction}"/>
<limit effort="${calf_torque_max}" velocity="${calf_velocity_max}" lower="${calf_position_min}"
upper="${calf_position_max}"/>
</joint>
<link name="${name}_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://$(find go2_description)/meshes/calf.dae" scale="1 1 1"/>
</geometry>
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
<geometry>
<box size="${calf_length} ${calf_width} ${calf_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
<mass value="${calf_mass}"/>
<inertia
ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
iyy="${calf_iyy}" iyz="${calf_iyz}"
izz="${calf_izz}"/>
</inertial>
</link>
<link name="${name}_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://$(find go2_description)/meshes/calf.dae" scale="1 1 1"/>
</geometry>
</visual>
<collision>
<origin rpy="0 ${PI/2.0} 0" xyz="0 0 ${-calf_length/2.0}"/>
<geometry>
<box size="${calf_length} ${calf_width} ${calf_height}"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="${calf_com_x} ${calf_com_y} ${calf_com_z}"/>
<mass value="${calf_mass}"/>
<inertia
ixx="${calf_ixx}" ixy="${calf_ixy}" ixz="${calf_ixz}"
iyy="${calf_iyy}" iyz="${calf_iyz}"
izz="${calf_izz}"/>
</inertial>
</link>
<joint name="${name}_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
<parent link="${name}_calf"/>
<child link="${name}_foot"/>
</joint>
<joint name="${name}_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
<parent link="${name}_calf"/>
<child link="${name}_foot"/>
</joint>
<link name="${name}_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius-0.01}"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius}"/>
</geometry>
</collision>
<inertial>
<mass value="${foot_mass}"/>
<inertia
ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
</inertial>
</link>
<link name="${name}_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius-0.01}"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="${foot_radius}"/>
</geometry>
</collision>
<inertial>
<mass value="${foot_mass}"/>
<inertia
ixx="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" ixy="0.0" ixz="0.0"
iyy="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}" iyz="0.0"
izz="${(2*foot_mass)/5.0*(foot_radius*foot_radius)}"/>
</inertial>
</link>
<gazebo reference="${name}_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<gazebo reference="${name}_hip">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
</gazebo>
<gazebo reference="${name}_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>0</self_collide>
<kp value="1000000.0"/>
<kd value="100.0"/>
</gazebo>
<gazebo reference="${name}_thigh">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<self_collide>0</self_collide>
<kp value="1000000.0"/>
<kd value="100.0"/>
</gazebo>
<gazebo reference="${name}_calf">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="${name}_calf">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
</gazebo>
<gazebo reference="${name}_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="100.0"/>
</gazebo>
<gazebo reference="${name}_foot">
<mu1>0.6</mu1>
<mu2>0.6</mu2>
<self_collide>1</self_collide>
<kp value="1000000.0"/>
<kd value="100.0"/>
</gazebo>
<xacro:leg_transmission name="${name}"/>
</xacro:macro>
<xacro:leg_transmission name="${name}"/>
</xacro:macro>
</robot>

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

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