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

tried to add balance test

This commit is contained in:
Huang Zhenbiao 2024-09-16 21:15:34 +08:00
parent 028b7b52b1
commit 5eaf8e1e17
19 changed files with 732 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
controllers/unitree_guide_controller/CMakeLists.txt
View File

@ -5,6 +5,7 @@ if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif ()
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
set(CONTROLLER_INCLUDE_DEPENDS
@ -32,12 +33,14 @@ add_library(${PROJECT_NAME} SHARED
src/FSM/StateFixedStand.cpp
src/FSM/StateSwingTest.cpp
src/FSM/StateFreeStand.cpp
src/FSM/StateBalanceTest.cpp
src/robot/QuadrupedRobot.cpp
src/robot/RobotLeg.cpp
src/control/Estimator.cpp
src/control/LowPassFilter.cpp
src/control/BalanceCtrl.cpp
src/quadProgpp/Array.cc
src/quadProgpp/QuadProg++.cc

43
controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h Normal file
View File

@ -0,0 +1,43 @@
//
// Created by tlab-uav on 24-9-16.
//
#ifndef STATEBALANCETEST_H
#define STATEBALANCETEST_H
#include "FSMState.h"
class StateBalanceTest final : public FSMState {
public:
explicit StateBalanceTest(CtrlComponent ctrlComp);
void enter() override;
void run() override;
void exit() override;
FSMStateName checkChange() override;
private:
void calcTorque();
KDL::Vector pcd_;
KDL::Vector pcdInit_;
KDL::Rotation Rd_;
KDL::Rotation RdInit_;
KDL::Vector pose_body_, vel_body_;
double kp_w_;
Mat3 Kp_p_, Kd_p_, Kd_w_;
Vec3 _ddPcd, _dWbd;
float _xMax, _xMin;
float _yMax, _yMin;
float _zMax, _zMin;
float _yawMax, _yawMin;
};
#endif //STATEBALANCETEST_H

2
controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h
View File

@ -10,6 +10,7 @@
#include <unitree_guide_controller/FSM/FSMState.h>
#include <unitree_guide_controller/common/enumClass.h>
#include "FSM/StateBalanceTest.h"
#include "FSM/StateFixedDown.h"
#include "FSM/StateFixedStand.h"
#include "FSM/StateFreeStand.h"
@ -25,6 +26,7 @@ namespace unitree_guide_controller {
std::shared_ptr<StateFreeStand> freeStand;
std::shared_ptr<StateSwingTest> swingTest;
std::shared_ptr<StateBalanceTest> balanceTest;
};
class UnitreeGuideController final : public controller_interface::ControllerInterface {

32
controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h
View File

@ -33,5 +33,37 @@ T windowFunc(const T x, const T windowRatio, const T xRange = 1.0,
return yRange;
}
inline Eigen::Matrix3d skew(const KDL::Vector &vec) {
Eigen::Matrix3d skewMat;
skewMat << 0, -vec.z(), vec.y(),
vec.z(), 0, -vec.x(),
-vec.y(), vec.x(), 0;
return skewMat;
}
inline Vec3 rotationToExp(const KDL::Rotation &rotation) {
auto rm = Eigen::Matrix3d(rotation.data);
double cosValue = rm.trace() / 2.0 - 1 / 2.0;
if (cosValue > 1.0f) {
cosValue = 1.0f;
} else if (cosValue < -1.0f) {
cosValue = -1.0f;
}
double angle = acos(cosValue);
Vec3 exp;
if (fabs(angle) < 1e-5) {
exp = Vec3(0, 0, 0);
} else if (fabs(angle - M_PI) < 1e-5) {
exp = angle * Vec3(rm(0, 0) + 1, rm(0, 1), rm(0, 2)) /
sqrt(2 * (1 + rm(0, 0)));
} else {
exp = angle / (2.0f * sin(angle)) *
Vec3(rm(2, 1) - rm(1, 2), rm(0, 2) - rm(2, 0), rm(1, 0) - rm(0, 1));
}
return exp;
}
#endif //MATHTOOLS_H

34
controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTypes.h
View File

@ -10,58 +10,58 @@
/******** Vector ********/
/************************/
// 2x1 Vector
using Vec2 = typename Eigen::Matrix<double, 2, 1>;
using Vec2 = Eigen::Matrix<double, 2, 1>;
// 3x1 Vector
using Vec3 = typename Eigen::Matrix<double, 3, 1>;
using Vec3 = Eigen::Matrix<double, 3, 1>;
// 4x1 Vector
using Vec4 = typename Eigen::Matrix<double, 4, 1>;
using Vec4 = Eigen::Matrix<double, 4, 1>;
// 6x1 Vector
using Vec6 = typename Eigen::Matrix<double, 6, 1>;
using Vec6 = Eigen::Matrix<double, 6, 1>;
// Quaternion
using Quat = typename Eigen::Matrix<double, 4, 1>;
using Quat = Eigen::Matrix<double, 4, 1>;
// 4x1 Integer Vector
using VecInt4 = typename Eigen::Matrix<int, 4, 1>;
using VecInt4 = Eigen::Matrix<int, 4, 1>;
// 12x1 Vector
using Vec12 = typename Eigen::Matrix<double, 12, 1>;
using Vec12 = Eigen::Matrix<double, 12, 1>;
// 18x1 Vector
using Vec18 = typename Eigen::Matrix<double, 18, 1>;
using Vec18 = Eigen::Matrix<double, 18, 1>;
// Dynamic Length Vector
using VecX = typename Eigen::Matrix<double, Eigen::Dynamic, 1>;
using VecX = Eigen::Matrix<double, Eigen::Dynamic, 1>;
/************************/
/******** Matrix ********/
/************************/
// Rotation Matrix
using RotMat = typename Eigen::Matrix<double, 3, 3>;
using RotMat = Eigen::Matrix<double, 3, 3>;
// Homogenous Matrix
using HomoMat = typename Eigen::Matrix<double, 4, 4>;
using HomoMat = Eigen::Matrix<double, 4, 4>;
// 2x2 Matrix
using Mat2 = typename Eigen::Matrix<double, 2, 2>;
using Mat2 = Eigen::Matrix<double, 2, 2>;
// 3x3 Matrix
using Mat3 = typename Eigen::Matrix<double, 3, 3>;
using Mat3 = Eigen::Matrix<double, 3, 3>;
// 3x3 Identity Matrix
#define I3 Eigen::MatrixXd::Identity(3, 3)
// 3x4 Matrix, each column is a 3x1 vector
using Vec34 = typename Eigen::Matrix<double, 3, 4>;
using Vec34 = Eigen::Matrix<double, 3, 4>;
// 6x6 Matrix
using Mat6 = typename Eigen::Matrix<double, 6, 6>;
using Mat6 = Eigen::Matrix<double, 6, 6>;
// 12x12 Matrix
using Mat12 = typename Eigen::Matrix<double, 12, 12>;
using Mat12 = Eigen::Matrix<double, 12, 12>;
// 12x12 Identity Matrix
#define I12 Eigen::MatrixXd::Identity(12, 12)
@ -70,7 +70,7 @@ using Mat12 = typename Eigen::Matrix<double, 12, 12>;
#define I18 Eigen::MatrixXd::Identity(18, 18)
// Dynamic Size Matrix
using MatX = typename Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
using MatX = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
/************************/
/****** Functions *******/

48
controllers/unitree_guide_controller/include/unitree_guide_controller/control/BalanceCtrl.h Normal file
View File

@ -0,0 +1,48 @@
//
// Created by tlab-uav on 24-9-16.
//
#ifndef BALANCECTRL_H
#define BALANCECTRL_H
#include <kdl/frames.hpp>
#include "unitree_guide_controller/common/mathTypes.h"
class QuadrupedRobot;
class BalanceCtrl {
public:
explicit BalanceCtrl();
~BalanceCtrl() = default;
void init(const QuadrupedRobot &robot);
std::vector<KDL::Vector> calF(const Vec3 &ddPcd, const Vec3 &dWbd, const KDL::Rotation &rotM,
const std::vector<KDL::Vector> &feetPos2B, const std::vector<int> &contact);
private:
void calMatrixA(const std::vector<KDL::Vector> &feetPos2B, const KDL::Rotation &rotM);
void calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const KDL::Rotation &rotM);
void calConstraints(const std::vector<int> &contact);
void solveQP();
Mat12 _G, _W, _U;
Mat6 _S;
Mat3 _Ib;
Vec6 _bd;
KDL::Vector _pcb;
Vec3 _g;
Vec12 _F, _Fprev, _g0T;
double _mass, _alpha, _beta, _fricRatio;
Eigen::MatrixXd _CE, _CI;
Eigen::VectorXd _ce0, _ci0;
Eigen::Matrix<double, 6, 12> _A;
Eigen::Matrix<double, 5, 3> _fricMat;
};
#endif //BALANCECTRL_H

12
controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h
View File

@ -11,6 +11,9 @@
#include <control_input_msgs/msg/inputs.hpp>
#include <unitree_guide_controller/robot/QuadrupedRobot.h>
#include "BalanceCtrl.h"
#include "Estimator.h"
struct CtrlComponent {
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
joint_effort_command_interface_;
@ -41,7 +44,14 @@ struct CtrlComponent {
QuadrupedRobot default_robot_model_;
std::reference_wrapper<QuadrupedRobot> robot_model_;
CtrlComponent() : control_inputs_(default_inputs_), robot_model_(default_robot_model_) {
Estimator default_estimator_;
std::reference_wrapper<Estimator> estimator_;
BalanceCtrl default_balance_ctrl_;
std::reference_wrapper<BalanceCtrl> balance_ctrl_;
CtrlComponent() : control_inputs_(default_inputs_), robot_model_(default_robot_model_),
estimator_(default_estimator_), balance_ctrl_(default_balance_ctrl_) {
}
void clear() {

116
controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h
View File

@ -4,9 +4,12 @@
#ifndef ESTIMATOR_H
#define ESTIMATOR_H
#include <memory>
#include <eigen3/Eigen/Dense>
#include <kdl/frames.hpp>
#include "LowPassFilter.h"
struct CtrlComponent;
class Estimator {
@ -19,49 +22,89 @@ public:
* Get the estimated robot central position
* @return robot central position
*/
Eigen::Matrix<double, 3, 1> getPosition() {
return _xhat.segment(0, 3);
KDL::Vector getPosition() {
return {_xhat(0), _xhat(1), _xhat(2)};
}
/**
* Get the estimated robot central velocity
* @return robot central velocity
*/
Eigen::Matrix<double, 3, 1> getVelocity() {
return _xhat.segment(3, 3);
KDL::Vector getVelocity() {
return {_xhat(3), _xhat(4), _xhat(5)};
}
void run(
const CtrlComponent &ctrlComp,
std::vector<KDL::Frame> feet_poses_,
std::vector<KDL::Vector> feet_vels_,
const std::vector<int> &contact,
const std::vector<double> &phase
);
/**
* Get the estimated foot position in world frame
* @param index leg index
* @return foot position in world frame
*/
KDL::Vector getFootPos(const int index) {
return getPosition() + rotation_ * foot_poses_[index].p;
}
/**
* Get all estimated foot positions in world frame
* @return all foot positions in world frame
*/
std::vector<KDL::Vector> getFootPos() {
std::vector<KDL::Vector> foot_pos;
foot_pos.resize(4);
for (int i = 0; i < 4; i++) {
foot_pos[i] = getFootPos(i);
}
return foot_pos;
}
/**
* Get the estimated foot position in body frame
* @return
*/
std::vector<KDL::Vector> getFootPos2Body() {
std::vector<KDL::Vector> foot_pos;
foot_pos.resize(4);
const KDL::Vector body_pos = getPosition();
for (int i = 0; i < 4; i++) {
foot_pos[i] = getFootPos(i) - body_pos;
}
return foot_pos;
}
KDL::Rotation getRotation() {
return rotation_;
}
KDL::Vector getGyro() {
return gyro_;
}
[[nodiscard]] KDL::Vector getGlobalGyro() const {
return rotation_ * gyro_;
}
void update(const CtrlComponent &ctrlComp);
private:
void init();
Eigen::Matrix<double, 18, 1> _xhat; // The state of estimator, position(3)+velocity(3)+feet position(3x4)
Eigen::Matrix<double, 3, 1> _u; // The input of estimator
Eigen::Matrix<double, 28, 1> _y; // The measurement value of output y
Eigen::Matrix<double, 28, 1> _yhat; // The prediction of output y
Eigen::Matrix<double, 18, 18> _A; // The transtion matrix of estimator
Eigen::Matrix<double, 18, 3> _B; // The input matrix
Eigen::Matrix<double, 28, 18> _C; // The output matrix
Eigen::Matrix<double, 18, 18> A; // The transtion matrix of estimator
Eigen::Matrix<double, 18, 3> B; // The input matrix
Eigen::Matrix<double, 28, 18> C; // The output matrix
// Covariance Matrix
Eigen::Matrix<double, 18, 18> _P; // Prediction covariance
Eigen::Matrix<double, 18, 18> _Ppriori; // Priori prediction covariance
Eigen::Matrix<double, 18, 18> _Q; // Dynamic simulation covariance
Eigen::Matrix<double, 28, 28> _R; // Measurement covariance
Eigen::Matrix<double, 18, 18> P; // Prediction covariance
Eigen::Matrix<double, 18, 18> Ppriori; // Priori prediction covariance
Eigen::Matrix<double, 18, 18> Q; // Dynamic simulation covariance
Eigen::Matrix<double, 28, 28> R; // Measurement covariance
Eigen::Matrix<double, 18, 18>
_QInit; // Initial value of Dynamic simulation covariance
QInit; // Initial value of Dynamic simulation covariance
Eigen::Matrix<double, 28, 28>
_RInit; // Initial value of Measurement covariance
Eigen::Matrix<double, 18, 1> _Qdig; // adjustable process noise covariance
Eigen::Matrix<double, 3, 3> _Cu; // The covariance of system input u
RInit; // Initial value of Measurement covariance
Eigen::Matrix<double, 18, 1> Qdig; // adjustable process noise covariance
Eigen::Matrix<double, 3, 3> Cu; // The covariance of system input u
// Output Measurement
Eigen::Matrix<double, 12, 1>
_feetPos2Body; // The feet positions to body, in the global coordinate
@ -69,20 +112,25 @@ private:
_feetVel2Body; // The feet velocity to body, in the global coordinate
Eigen::Matrix<double, 4, 1>
_feetH; // The Height of each foot, in the global coordinate
Eigen::Matrix<double, 28, 28> _S; // _S = C*P*C.T + R
Eigen::PartialPivLU<Eigen::Matrix<double, 28, 28> > _Slu; // _S.lu()
Eigen::Matrix<double, 28, 1> _Sy; // _Sy = _S.inv() * (y - yhat)
Eigen::Matrix<double, 28, 18> _Sc; // _Sc = _S.inv() * C
Eigen::Matrix<double, 28, 28> _SR; // _SR = _S.inv() * R
Eigen::Matrix<double, 28, 18> _STC; // _STC = (_S.transpose()).inv() * C
Eigen::Matrix<double, 18, 18> _IKC; // _IKC = I - KC
Eigen::Matrix<double, 28, 28> S; // _S = C*P*C.T + R
Eigen::PartialPivLU<Eigen::Matrix<double, 28, 28> > Slu; // _S.lu()
Eigen::Matrix<double, 28, 1> Sy; // _Sy = _S.inv() * (y - yhat)
Eigen::Matrix<double, 28, 18> Sc; // _Sc = _S.inv() * C
Eigen::Matrix<double, 28, 28> SR; // _SR = _S.inv() * R
Eigen::Matrix<double, 28, 18> STC; // _STC = (_S.transpose()).inv() * C
Eigen::Matrix<double, 18, 18> IKC; // _IKC = I - KC
KDL::Vector g_;
double _dt;
std::vector<KDL::Frame> feet_poses_;
std::vector<KDL::Vector> feet_vels_;
KDL::Rotation rotation_;
KDL::Vector acceleration_;
KDL::Vector gyro_;
std::vector<KDL::Frame> foot_poses_;
std::vector<KDL::Vector> foot_vels_;
std::vector<std::shared_ptr<LowPassFilter> > low_pass_filters_;
double _trust;
double _largeVariance;
};

10
controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h
View File

@ -30,10 +30,9 @@ public:
/**
* Calculate the foot end position based on joint positions
* @param joint_positions vector of joint positions
* @return vector of foot-end position
*/
[[nodiscard]] std::vector<KDL::Frame> getFeet2BPositions(const std::vector<KDL::JntArray> &joint_positions) const;
[[nodiscard]] std::vector<KDL::Frame> getFeet2BPositions() const;
/**
* Calculate the foot end position based on joint positions
@ -63,22 +62,21 @@ public:
* @param index leg index
* @return velocity vector
*/
[[nodiscard]] KDL::Vector getFeet2BVelocities (int index) const;
[[nodiscard]] KDL::Vector getFeet2BVelocities(int index) const;
/**
* Calculate all foot end velocity
* @return list of foot end velocity
*/
[[nodiscard]] std::vector<KDL::Vector> getFeet2BVelocities () const;
[[nodiscard]] std::vector<KDL::Vector> getFeet2BVelocities() const;
double mass_ = 0;
std::vector<KDL::JntArray> current_joint_pos_;
std::vector<KDL::JntArray> current_joint_vel_;
void update(const CtrlComponent &ctrlComp);
private:
double mass_ = 0;
std::vector<std::shared_ptr<RobotLeg> > robot_legs_;
KDL::Chain fr_chain_;

2
controllers/unitree_guide_controller/readme.md
View File

@ -1,6 +1,6 @@
# Unitree Guide Controller
This is a ros2-control controller based on unitree guide.
This is a ros2-control controller based on unitree guide. The original unitree guide project could be found [here](https://github.com/unitreerobotics/unitree_guide)
```bash
cd ~/ros2_ws

97
controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp Normal file
View File

@ -0,0 +1,97 @@
//
// Created by tlab-uav on 24-9-16.
//
#include "unitree_guide_controller/FSM/StateBalanceTest.h"
#include <unitree_guide_controller/common/mathTools.h>
StateBalanceTest::StateBalanceTest(CtrlComponent ctrlComp) : FSMState(FSMStateName::BALANCETEST, "balance test",
std::move(ctrlComp)) {
_xMax = 0.05;
_xMin = -_xMax;
_yMax = 0.05;
_yMin = -_yMax;
_zMax = 0.04;
_zMin = -_zMax;
_yawMax = 20 * M_PI / 180;
_yawMin = -_yawMax;
Kp_p_ = Vec3(150, 150, 150).asDiagonal();
Kd_p_ = Vec3(25, 25, 25).asDiagonal();
kp_w_ = 200;
Kd_w_ = Vec3(30, 30, 30).asDiagonal();
}
void StateBalanceTest::enter() {
pcd_ = ctrlComp_.estimator_.get().getPosition();
pcdInit_ = pcd_;
RdInit_ = ctrlComp_.estimator_.get().getRotation();
}
void StateBalanceTest::run() {
pcd_(0) = pcdInit_(0) + invNormalize(ctrlComp_.control_inputs_.get().ly, _xMin, _xMax);
pcd_(1) = pcdInit_(1) - invNormalize(ctrlComp_.control_inputs_.get().lx, _yMin, _yMax);
pcd_(2) = pcdInit_(2) + invNormalize(ctrlComp_.control_inputs_.get().ry, _zMin, _zMax);
const float yaw = invNormalize(ctrlComp_.control_inputs_.get().rx, _yawMin, _yawMax);
Rd_ = KDL::Rotation::RPY(0, 0, yaw) * RdInit_;
pose_body_ = ctrlComp_.estimator_.get().getPosition();
vel_body_ = ctrlComp_.estimator_.get().getVelocity();
calcTorque();
for (int i = 0; i < 12; i++) {
ctrlComp_.joint_kp_command_interface_[i].get().set_value(0.8);
ctrlComp_.joint_kd_command_interface_[i].get().set_value(0.8);
}
}
void StateBalanceTest::exit() {
}
FSMStateName StateBalanceTest::checkChange() {
switch (ctrlComp_.control_inputs_.get().command) {
case 1:
return FSMStateName::FIXEDDOWN;
case 2:
return FSMStateName::FIXEDSTAND;
default:
return FSMStateName::BALANCETEST;
}
}
void StateBalanceTest::calcTorque() {
// expected body acceleration
_ddPcd = Kp_p_ * Vec3((pcd_ - pose_body_).data) + Kd_p_ * Vec3(
(KDL::Vector(0, 0, 0) - vel_body_).data);
std::cout << "ddPcd: " << Vec3(vel_body_.data) << std::endl;
// expected body angular acceleration
const KDL::Rotation B2G_RotMat = ctrlComp_.estimator_.get().getRotation();
const KDL::Rotation G2B_RotMat = B2G_RotMat.Inverse();
_dWbd = kp_w_ * rotationToExp(Rd_ * G2B_RotMat) +
Kd_w_ * (Vec3(0, 0, 0) - Vec3(ctrlComp_.estimator_.get().getGlobalGyro().data));
// calculate foot force
const std::vector contact(4, 1);
const std::vector<KDL::Vector> foot_force = ctrlComp_.balance_ctrl_.get().calF(_ddPcd, _dWbd, B2G_RotMat,
ctrlComp_.estimator_.get().
getFootPos2Body(), contact);
std::vector<KDL::JntArray> current_joints = ctrlComp_.robot_model_.get().current_joint_pos_;
for (int i = 0; i < 4; i++) {
const auto &foot = foot_force[i];
std::cout << "foot_force: " << foot.x() << "," << foot.y() << "," << foot.z() << std::endl;
const KDL::Wrench wrench0(G2B_RotMat * foot, KDL::Vector::Zero());
const KDL::Wrenches wrenches(1, wrench0); // 创建一个包含 wrench0 的容器
KDL::JntArray torque0 = ctrlComp_.robot_model_.get().getTorque(wrenches, i);
for (int j = 0; j < 3; j++) {
ctrlComp_.joint_effort_command_interface_[i * 3 + j].get().set_value(torque0(j));
ctrlComp_.joint_position_command_interface_[i * 3 + j].get().set_value(current_joints[i](j));
}
}
}

2
controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp
View File

@ -46,6 +46,8 @@ FSMStateName StateFixedStand::checkChange() {
return FSMStateName::FREESTAND;
case 9:
return FSMStateName::SWINGTEST;
case 10:
return FSMStateName::BALANCETEST;
default:
return FSMStateName::FIXEDSTAND;
}

3
controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp
View File

@ -22,10 +22,9 @@ void StateFreeStand::enter() {
ctrlComp_.joint_kp_command_interface_[i].get().set_value(180);
ctrlComp_.joint_kd_command_interface_[i].get().set_value(5);
}
ctrlComp_.robot_model_.get().update(ctrlComp_);
init_joint_pos_ = ctrlComp_.robot_model_.get().current_joint_pos_;
init_foot_pos_ = ctrlComp_.robot_model_.get().getFeet2BPositions(init_joint_pos_);
init_foot_pos_ = ctrlComp_.robot_model_.get().getFeet2BPositions();
fr_init_pos_ = init_foot_pos_[0];

3
controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp
View File

@ -29,10 +29,9 @@ void StateSwingTest::enter() {
Kp = KDL::Vector(20, 20, 50);
Kd = KDL::Vector(5, 5, 20);
ctrlComp_.robot_model_.get().update(ctrlComp_);
init_joint_pos_ = ctrlComp_.robot_model_.get().current_joint_pos_;
init_foot_pos_ = ctrlComp_.robot_model_.get().getFeet2BPositions();
init_foot_pos_ = ctrlComp_.robot_model_.get().getFeet2BPositions(init_joint_pos_);
target_foot_pos_ = init_foot_pos_;
fr_init_pos_ = init_foot_pos_[0];
fr_goal_pos_ = fr_init_pos_;

21
controllers/unitree_guide_controller/src/UnitreeGuideController.cpp
View File

@ -41,6 +41,19 @@ namespace unitree_guide_controller {
controller_interface::return_type UnitreeGuideController::
update(const rclcpp::Time &time, const rclcpp::Duration &period) {
// auto now = std::chrono::steady_clock::now();
// std::chrono::duration<double> time_diff = now - last_update_time_;
// last_update_time_ = now;
//
// // Calculate the frequency
// update_frequency_ = 1.0 / time_diff.count();
// RCLCPP_INFO(get_node()->get_logger(), "Update frequency: %f Hz", update_frequency_);
ctrl_comp_.robot_model_.get().update(ctrl_comp_);
ctrl_comp_.estimator_.get().update(ctrl_comp_);
if (mode_ == FSMMode::NORMAL) {
current_state_->run();
next_state_name_ = current_state_->checkChange();
@ -72,7 +85,6 @@ namespace unitree_guide_controller {
// 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_);
} catch (const std::exception &e) {
fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
return controller_interface::CallbackReturn::ERROR;
@ -96,8 +108,8 @@ namespace unitree_guide_controller {
robot_description_subscription_ = get_node()->create_subscription<std_msgs::msg::String>(
"~/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(),
[this](const std_msgs::msg::String::SharedPtr msg) {
// Handle message
ctrl_comp_.robot_model_.get().init(msg->data);
ctrl_comp_.balance_ctrl_.get().init(ctrl_comp_.robot_model_.get());
});
get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
@ -130,6 +142,7 @@ namespace unitree_guide_controller {
state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_comp_);
state_list_.swingTest = std::make_shared<StateSwingTest>(ctrl_comp_);
state_list_.freeStand = std::make_shared<StateFreeStand>(ctrl_comp_);
state_list_.balanceTest = std::make_shared<StateBalanceTest>(ctrl_comp_);
// Initialize FSM
current_state_ = state_list_.passive;
@ -178,8 +191,8 @@ namespace unitree_guide_controller {
// return state_list_.trotting;
case FSMStateName::SWINGTEST:
return state_list_.swingTest;
// case FSMStateName::BALANCETEST:
// return state_list_.balanceTest;
case FSMStateName::BALANCETEST:
return state_list_.balanceTest;
default:
return state_list_.invalid;
}

153
controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp Normal file
View File

@ -0,0 +1,153 @@
//
// Created by tlab-uav on 24-9-16.
//
#include "unitree_guide_controller/control/BalanceCtrl.h"
#include <unitree_guide_controller/common/mathTools.h>
#include <unitree_guide_controller/quadProgpp/QuadProg++.hh>
#include <unitree_guide_controller/robot/QuadrupedRobot.h>
BalanceCtrl::BalanceCtrl() {
_mass = 15;
_alpha = 0.001;
_beta = 0.1;
_fricRatio = 0.4;
_g << 0, 0, -9.81;
}
void BalanceCtrl::init(const QuadrupedRobot &robot) {
_mass = robot.mass_;
_pcb = KDL::Vector(0.0, 0.0, 0.0);
_Ib = Vec3(0.0792, 0.2085, 0.2265).asDiagonal();
Vec6 s;
Vec12 w, u;
w << 10, 10, 4, 10, 10, 4, 10, 10, 4, 10, 10, 4;
u << 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3;
s << 20, 20, 50, 450, 450, 450;
_S = s.asDiagonal();
_W = w.asDiagonal();
_U = u.asDiagonal();
_Fprev.setZero();
_fricMat << 1, 0, _fricRatio, -1, 0, _fricRatio, 0, 1, _fricRatio, 0, -1,
_fricRatio, 0, 0, 1;
}
std::vector<KDL::Vector> BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const KDL::Rotation &rotM,
const std::vector<KDL::Vector> &feetPos2B, const std::vector<int> &contact) {
calMatrixA(feetPos2B, rotM);
calVectorBd(ddPcd, dWbd, rotM);
calConstraints(contact);
_G = _A.transpose() * _S * _A + _alpha * _W + _beta * _U;
_g0T = -_bd.transpose() * _S * _A - _beta * _Fprev.transpose() * _U;
solveQP();
_Fprev = _F;
std::vector<KDL::Vector> res;
res.resize(4);
for (int i = 0; i < 4; ++i) {
res[i] = KDL::Vector(_F(i * 3), _F(i * 3 + 1), _F(i * 3 + 2));
}
return res;
}
void BalanceCtrl::calMatrixA(const std::vector<KDL::Vector> &feetPos2B, const KDL::Rotation &rotM) {
for (int i = 0; i < 4; ++i) {
_A.block(0, 3 * i, 3, 3) = I3;
KDL::Vector tempVec = feetPos2B[i] - rotM * _pcb;
_A.block(3, 3 * i, 3, 3) = skew(tempVec);
}
}
void BalanceCtrl::calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const KDL::Rotation &rotM) {
_bd.head(3) = _mass * (ddPcd - _g);
_bd.tail(3) = Eigen::Matrix3d(rotM.data) * _Ib * Eigen::Matrix3d(rotM.data).transpose() *
dWbd;
}
void BalanceCtrl::calConstraints(const std::vector<int> &contact) {
int contactLegNum = 0;
for (int i(0); i < 4; ++i) {
if (contact[i] == 1) {
contactLegNum += 1;
}
}
_CI.resize(5 * contactLegNum, 12);
_ci0.resize(5 * contactLegNum);
_CE.resize(3 * (4 - contactLegNum), 12);
_ce0.resize(3 * (4 - contactLegNum));
_CI.setZero();
_ci0.setZero();
_CE.setZero();
_ce0.setZero();
int ceID = 0;
int ciID = 0;
for (int i(0); i < 4; ++i) {
if (contact[i] == 1) {
_CI.block(5 * ciID, 3 * i, 5, 3) = _fricMat;
++ciID;
} else {
_CE.block(3 * ceID, 3 * i, 3, 3) = I3;
++ceID;
}
}
}
void BalanceCtrl::solveQP() {
const int n = _F.size();
const int m = _ce0.size();
const int p = _ci0.size();
quadprogpp::Matrix<double> G, CE, CI;
quadprogpp::Vector<double> g0, ce0, ci0, x;
G.resize(n, n);
CE.resize(n, m);
CI.resize(n, p);
g0.resize(n);
ce0.resize(m);
ci0.resize(p);
x.resize(n);
for (int i = 0; i < n; ++i) {
for (int j = 0; j < n; ++j) {
G[i][j] = _G(i, j);
}
}
for (int i = 0; i < n; ++i) {
for (int j = 0; j < m; ++j) {
CE[i][j] = (_CE.transpose())(i, j);
}
}
for (int i = 0; i < n; ++i) {
for (int j = 0; j < p; ++j) {
CI[i][j] = (_CI.transpose())(i, j);
}
}
for (int i = 0; i < n; ++i) {
g0[i] = _g0T[i];
}
for (int i = 0; i < m; ++i) {
ce0[i] = _ce0[i];
}
for (int i = 0; i < p; ++i) {
ci0[i] = _ci0[i];
}
solve_quadprog(G, g0, CE, ce0, CI, ci0, x);
for (int i = 0; i < n; ++i) {
_F[i] = x[i];
}
}

240
controllers/unitree_guide_controller/src/control/Estimator.cpp
View File

@ -4,48 +4,228 @@
#include "unitree_guide_controller/control/Estimator.h"
#include "unitree_guide_controller/control/CtrlComponent.h"
#include <unitree_guide_controller/common/mathTools.h>
#include "unitree_guide_controller/common/mathTools.h"
#define I3 Eigen::MatrixXd::Identity(3, 3)
// 12x12 Identity Matrix
#define I12 Eigen::MatrixXd::Identity(12, 12)
// 18x18 Identity Matrix
#define I18 Eigen::MatrixXd::Identity(18, 18)
Estimator::Estimator() {
g_ = KDL::Vector(0, 0, -9.81);
_dt = 0.002;
_largeVariance = 100;
_xhat.setZero();
_u.setZero();
A.setZero();
A.block(0, 0, 3, 3) = I3;
A.block(0, 3, 3, 3) = I3 * _dt;
A.block(3, 3, 3, 3) = I3;
A.block(6, 6, 12, 12) = I12;
B.setZero();
B.block(3, 0, 3, 3) = I3 * _dt;
C.setZero();
C.block(0, 0, 3, 3) = -I3;
C.block(3, 0, 3, 3) = -I3;
C.block(6, 0, 3, 3) = -I3;
C.block(9, 0, 3, 3) = -I3;
C.block(12, 3, 3, 3) = -I3;
C.block(15, 3, 3, 3) = -I3;
C.block(18, 3, 3, 3) = -I3;
C.block(21, 3, 3, 3) = -I3;
C.block(0, 6, 12, 12) = I12;
C(24, 8) = 1;
C(25, 11) = 1;
C(26, 14) = 1;
C(27, 17) = 1;
P.setIdentity();
P = _largeVariance * P;
RInit << 0.008, 0.012, -0.000, -0.009, 0.012, 0.000, 0.009, -0.009, -0.000,
-0.009, -0.009, 0.000, -0.000, 0.000, -0.000, 0.000, -0.000, -0.001,
-0.002, 0.000, -0.000, -0.003, -0.000, -0.001, 0.000, 0.000, 0.000, 0.000,
0.012, 0.019, -0.001, -0.014, 0.018, -0.000, 0.014, -0.013, -0.000,
-0.014, -0.014, 0.001, -0.001, 0.001, -0.001, 0.000, 0.000, -0.001,
-0.003, 0.000, -0.001, -0.004, -0.000, -0.001, 0.000, 0.000, 0.000, 0.000,
-0.000, -0.001, 0.001, 0.001, -0.001, 0.000, -0.000, 0.000, -0.000, 0.001,
0.000, -0.000, 0.000, -0.000, 0.000, 0.000, -0.000, -0.000, 0.000, -0.000,
-0.000, -0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, -0.009, -0.014,
0.001, 0.010, -0.013, 0.000, -0.010, 0.010, 0.000, 0.010, 0.010, -0.000,
0.001, 0.000, 0.000, 0.001, -0.000, 0.001, 0.002, -0.000, 0.000, 0.003,
0.000, 0.001, 0.000, 0.000, 0.000, 0.000, 0.012, 0.018, -0.001, -0.013,
0.018, -0.000, 0.013, -0.013, -0.000, -0.013, -0.013, 0.001, -0.001,
0.000, -0.001, 0.000, 0.001, -0.001, -0.003, 0.000, -0.001, -0.004,
-0.000, -0.001, 0.000, 0.000, 0.000, 0.000, 0.000, -0.000, 0.000, 0.000,
-0.000, 0.001, 0.000, 0.000, -0.000, 0.000, 0.000, -0.000, -0.000, 0.000,
-0.000, 0.000, 0.000, 0.000, -0.000, -0.000, -0.000, -0.000, 0.000, 0.000,
0.000, 0.000, 0.000, 0.000, 0.009, 0.014, -0.000, -0.010, 0.013, 0.000,
0.010, -0.010, -0.000, -0.010, -0.010, 0.000, -0.001, 0.000, -0.001,
0.000, -0.000, -0.001, -0.001, 0.000, -0.000, -0.003, -0.000, -0.001,
0.000, 0.000, 0.000, 0.000, -0.009, -0.013, 0.000, 0.010, -0.013, 0.000,
-0.010, 0.009, 0.000, 0.010, 0.010, -0.000, 0.001, -0.000, 0.000, -0.000,
0.000, 0.001, 0.002, 0.000, 0.000, 0.003, 0.000, 0.001, 0.000, 0.000,
0.000, 0.000, -0.000, -0.000, -0.000, 0.000, -0.000, -0.000, -0.000,
0.000, 0.001, 0.000, 0.000, 0.000, 0.000, -0.000, 0.000, -0.000, 0.000,
-0.000, 0.000, -0.000, 0.000, 0.000, -0.000, -0.000, 0.000, 0.000, 0.000,
0.000, -0.009, -0.014, 0.001, 0.010, -0.013, 0.000, -0.010, 0.010, 0.000,
0.010, 0.010, -0.000, 0.001, 0.000, 0.000, -0.000, -0.000, 0.001, 0.002,
-0.000, 0.000, 0.003, 0.000, 0.001, 0.000, 0.000, 0.000, 0.000, -0.009,
-0.014, 0.000, 0.010, -0.013, 0.000, -0.010, 0.010, 0.000, 0.010, 0.010,
-0.000, 0.001, -0.000, 0.000, -0.000, 0.000, 0.001, 0.002, -0.000, 0.000,
0.003, 0.001, 0.001, 0.000, 0.000, 0.000, 0.000, 0.000, 0.001, -0.000,
-0.000, 0.001, -0.000, 0.000, -0.000, 0.000, -0.000, -0.000, 0.001, 0.000,
-0.000, -0.000, -0.000, 0.000, 0.000, -0.000, 0.000, 0.000, 0.000, 0.000,
0.000, 0.000, 0.000, 0.000, 0.000, -0.000, -0.001, 0.000, 0.001, -0.001,
-0.000, -0.001, 0.001, 0.000, 0.001, 0.001, 0.000, 1.708, 0.048, 0.784,
0.062, 0.042, 0.053, 0.077, 0.001, -0.061, 0.046, -0.019, -0.029, 0.000,
0.000, 0.000, 0.000, 0.000, 0.001, -0.000, 0.000, 0.000, 0.000, 0.000,
-0.000, -0.000, 0.000, -0.000, -0.000, 0.048, 5.001, -1.631, -0.036,
0.144, 0.040, 0.036, 0.016, -0.051, -0.067, -0.024, -0.005, 0.000, 0.000,
0.000, 0.000, -0.000, -0.001, 0.000, 0.000, -0.001, -0.000, -0.001, 0.000,
0.000, 0.000, 0.000, -0.000, 0.784, -1.631, 1.242, 0.057, -0.037, 0.018,
0.034, -0.017, -0.015, 0.058, -0.021, -0.029, 0.000, 0.000, 0.000, 0.000,
0.000, 0.000, 0.000, 0.001, 0.000, 0.000, 0.000, -0.000, -0.000, -0.000,
-0.000, -0.000, 0.062, -0.036, 0.057, 6.228, -0.014, 0.932, 0.059, 0.053,
-0.069, 0.148, 0.015, -0.031, 0.000, 0.000, 0.000, 0.000, -0.000, 0.000,
-0.000, -0.000, 0.001, 0.000, -0.000, 0.000, 0.000, -0.000, 0.000, 0.000,
0.042, 0.144, -0.037, -0.014, 3.011, 0.986, 0.076, 0.030, -0.052, -0.027,
0.057, 0.051, 0.000, 0.000, 0.000, 0.000, -0.001, -0.001, -0.000, 0.001,
-0.001, 0.000, -0.001, 0.001, -0.000, 0.001, 0.001, 0.000, 0.053, 0.040,
0.018, 0.932, 0.986, 0.885, 0.090, 0.044, -0.055, 0.057, 0.051, -0.003,
0.000, 0.000, 0.000, 0.000, -0.002, -0.003, 0.000, 0.002, -0.003, -0.000,
-0.001, 0.002, 0.000, 0.002, 0.002, -0.000, 0.077, 0.036, 0.034, 0.059,
0.076, 0.090, 6.230, 0.139, 0.763, 0.013, -0.019, -0.024, 0.000, 0.000,
0.000, 0.000, 0.000, 0.000, -0.000, -0.000, 0.000, -0.000, 0.000, 0.000,
-0.000, -0.000, -0.000, 0.000, 0.001, 0.016, -0.017, 0.053, 0.030, 0.044,
0.139, 3.130, -1.128, -0.010, 0.131, 0.018, 0.000, 0.000, 0.000, 0.000,
-0.000, -0.001, -0.000, 0.000, -0.001, -0.000, -0.000, 0.000, 0.000,
0.000, 0.000, 0.000, -0.061, -0.051, -0.015, -0.069, -0.052, -0.055,
0.763, -1.128, 0.866, -0.022, -0.053, 0.007, 0.000, 0.000, 0.000, 0.000,
-0.003, -0.004, -0.000, 0.003, -0.004, -0.000, -0.003, 0.003, 0.000,
0.003, 0.003, 0.000, 0.046, -0.067, 0.058, 0.148, -0.027, 0.057, 0.013,
-0.010, -0.022, 2.437, -0.102, 0.938, 0.000, 0.000, 0.000, 0.000, -0.000,
-0.000, 0.000, 0.000, -0.000, 0.000, -0.000, 0.000, -0.000, 0.000, 0.001,
0.000, -0.019, -0.024, -0.021, 0.015, 0.057, 0.051, -0.019, 0.131, -0.053,
-0.102, 4.944, 1.724, 0.000, 0.000, 0.000, 0.000, -0.001, -0.001, 0.000,
0.001, -0.001, 0.000, -0.001, 0.001, -0.000, 0.001, 0.001, 0.000, -0.029,
-0.005, -0.029, -0.031, 0.051, -0.003, -0.024, 0.018, 0.007, 0.938, 1.724,
1.569, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000,
0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000,
0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 1.0, 0.000,
0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000,
0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000,
0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 1.0, 0.000, 0.000, 0.000,
0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000,
0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000,
0.000, 0.000, 0.000, 0.000, 0.000, 1.0, 0.000, 0.000, 0.000, 0.000, 0.000,
0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000,
0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000,
0.000, 0.000, 0.000, 1.0;
Cu << 268.573, -43.819, -147.211, -43.819, 92.949, 58.082, -147.211, 58.082,
302.120;
QInit = Qdig.asDiagonal();
QInit += B * Cu * B.transpose();
low_pass_filters_.resize(3);
low_pass_filters_[0] = std::make_shared<LowPassFilter>(0.02, 3.0);
low_pass_filters_[1] = std::make_shared<LowPassFilter>(0.02, 3.0);
low_pass_filters_[2] = std::make_shared<LowPassFilter>(0.02, 3.0);
}
void Estimator::run(const CtrlComponent &ctrlComp, std::vector<KDL::Frame> feet_poses_,
std::vector<KDL::Vector> feet_vels_,
const std::vector<int> &contact, const std::vector<double> &phase) {
_Q = _QInit;
_R = _RInit;
void Estimator::update(const CtrlComponent &ctrlComp) {
if (ctrlComp.robot_model_.get().mass_ == 0) return;
Q = QInit;
R = RInit;
foot_poses_ = ctrlComp.robot_model_.get().getFeet2BPositions();
foot_vels_ = ctrlComp.robot_model_.get().getFeet2BVelocities();
_feetH.setZero();
const std::vector contact(4, 1);
const std::vector phase(4, 0.5);
// Adjust the covariance based on foot contact and phase.
for (int i(0); i < 4; ++i) {
if (contact[i] == 0) {
_Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3);
_R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3);
_R(24 + i, 24 + i) = _largeVariance;
// foot not contact
Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3);
R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3);
R(24 + i, 24 + i) = _largeVariance;
} else {
_trust = windowFunc(phase[i], 0.2);
_Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) =
(1 + (1 - _trust) * _largeVariance) *
_QInit.block(6 + 3 * i, 6 + 3 * i, 3, 3);
_R.block(12 + 3 * i, 12 + 3 * i, 3, 3) =
(1 + (1 - _trust) * _largeVariance) *
_RInit.block(12 + 3 * i, 12 + 3 * i, 3, 3);
_R(24 + i, 24 + i) =
(1 + (1 - _trust) * _largeVariance) * _RInit(24 + i, 24 + i);
// foot contact
const double trust = windowFunc(phase[i], 0.2);
Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) =
(1 + (1 - trust) * _largeVariance) *
QInit.block(6 + 3 * i, 6 + 3 * i, 3, 3);
R.block(12 + 3 * i, 12 + 3 * i, 3, 3) =
(1 + (1 - trust) * _largeVariance) *
RInit.block(12 + 3 * i, 12 + 3 * i, 3, 3);
R(24 + i, 24 + i) =
(1 + (1 - trust) * _largeVariance) * RInit(24 + i, 24 + i);
}
_feetPos2Body.segment(3 * i, 3) = Eigen::Map<Eigen::Vector3d>(feet_poses_[i].p.data);
_feetVel2Body.segment(3 * i, 3) = Eigen::Map<Eigen::Vector3d>(feet_vels_[i].data);
_feetPos2Body.segment(3 * i, 3) = Eigen::Map<Eigen::Vector3d>(foot_poses_[i].p.data);
_feetVel2Body.segment(3 * i, 3) = Eigen::Map<Eigen::Vector3d>(foot_vels_[i].data);
}
// Acceleration from imu as system input
const KDL::Rotation rotation = KDL::Rotation::Quaternion(ctrlComp.imu_state_interface_[7].get().get_value(),
ctrlComp.imu_state_interface_[8].get().get_value(),
ctrlComp.imu_state_interface_[9].get().get_value(),
ctrlComp.imu_state_interface_[6].get().get_value());
const KDL::Vector acc(ctrlComp.imu_state_interface_[3].get().get_value(),
ctrlComp.imu_state_interface_[4].get().get_value(),
ctrlComp.imu_state_interface_[5].get().get_value());
_u = Eigen::Map<Eigen::Vector3d>((rotation * acc + g_).data);
_xhat = _A * _xhat + _B * _u;
_yhat = _C * _xhat;
rotation_ = KDL::Rotation::Quaternion(ctrlComp.imu_state_interface_[0].get().get_value(),
ctrlComp.imu_state_interface_[1].get().get_value(),
ctrlComp.imu_state_interface_[2].get().get_value(),
ctrlComp.imu_state_interface_[3].get().get_value());
gyro_ = KDL::Vector(ctrlComp.imu_state_interface_[4].get().get_value(),
ctrlComp.imu_state_interface_[5].get().get_value(),
ctrlComp.imu_state_interface_[6].get().get_value());
acceleration_ = KDL::Vector(ctrlComp.imu_state_interface_[7].get().get_value(),
ctrlComp.imu_state_interface_[8].get().get_value(),
ctrlComp.imu_state_interface_[9].get().get_value());
_u = Vec3((rotation_ * acceleration_ + g_).data);
const double *rot_mat = rotation_.data;
std::cout << "Rotation Matrix: " << std::endl;
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
std::cout << rot_mat[i * 3 + j] << " ";
}
std::cout << std::endl;
}
_xhat = A * _xhat + B * _u;
_yhat = C * _xhat;
// Update the measurement value
_y << _feetPos2Body, _feetVel2Body, _feetH;
// Update the covariance matrix
Ppriori = A * P * A.transpose() + Q;
S = R + C * Ppriori * C.transpose();
Slu = S.lu();
Sy = Slu.solve(_y - _yhat);
Sc = Slu.solve(C);
SR = Slu.solve(R);
STC = S.transpose().lu().solve(C);
IKC = Eigen::MatrixXd::Identity(18, 18) - Ppriori * C.transpose() * Sc;
// Update the state and covariance matrix
_xhat += Ppriori * C.transpose() * Sy;
P = IKC * Ppriori * IKC.transpose() +
Ppriori * C.transpose() * SR * STC * Ppriori.transpose();
// Using low pass filter to smooth the velocity
low_pass_filters_[0]->addValue(_xhat(3));
low_pass_filters_[1]->addValue(_xhat(4));
low_pass_filters_[2]->addValue(_xhat(5));
_xhat(3) = low_pass_filters_[0]->getValue();
_xhat(4) = low_pass_filters_[1]->getValue();
_xhat(5) = low_pass_filters_[2]->getValue();
}

5
controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp
View File

@ -42,11 +42,11 @@ std::vector<KDL::JntArray> QuadrupedRobot::getQ(const std::vector<KDL::Frame> &p
return result;
}
std::vector<KDL::Frame> QuadrupedRobot::getFeet2BPositions(const std::vector<KDL::JntArray> &joint_positions) const {
std::vector<KDL::Frame> QuadrupedRobot::getFeet2BPositions() const {
std::vector<KDL::Frame> result;
result.resize(4);
for (int i = 0; i < 4; i++) {
result[i] = robot_legs_[i]->calcPEe2B(joint_positions[i]);
result[i] = robot_legs_[i]->calcPEe2B(current_joint_pos_[i]);
}
return result;
}
@ -80,6 +80,7 @@ std::vector<KDL::Vector> QuadrupedRobot::getFeet2BVelocities() const {
}
void QuadrupedRobot::update(const CtrlComponent &ctrlComp) {
if (mass_ == 0) return;
for (int i = 0; i < 4; i++) {
KDL::JntArray pos_array(3);
pos_array(0) = ctrlComp.joint_position_state_interface_[i * 3].get().get_value();

8
descriptions/go2_description/config/robot_control.yaml
View File

@ -50,13 +50,13 @@ unitree_guide_controller:
imu_name: "imu_sensor"
imu_interfaces:
- orientation.x
- orientation.y
- orientation.z
- orientation.w
- angular_velocity.x
- angular_velocity.y
- angular_velocity.z
- linear_acceleration.x
- linear_acceleration.y
- linear_acceleration.z
- orientation.w
- orientation.x
- orientation.y
- orientation.z