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

use unitree rotation calculation

This commit is contained in:
Huang Zhenbiao 2024-09-18 21:48:14 +08:00
parent f0a5257852
commit af6599e904
12 changed files with 155 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h
View File

@ -29,9 +29,7 @@ private:
Vec3 pcd_, pcd_init_;
RotMat Rd_;
KDL::Rotation init_rotation_;
Vec3 pose_body_, vel_body_;
RotMat init_rotation_;
double kp_w_;
Mat3 Kp_p_, Kd_p_, Kd_w_;

19
controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h
View File

@ -42,34 +42,25 @@ private:
QuadrupedRobot &robot_model_;
BalanceCtrl &balance_ctrl_;
WaveGenerator &wave_generator_;
GaitGenerator gait_generator_;
// Robot State
Vec3 _posBody, _velBody;
double _yaw{}, _dYaw{};
Vec34 _posFeetGlobal, _velFeetGlobal;
Vec34 _posFeet2BGlobal;
Vec3 pos_body_, vel_body_;
RotMat B2G_RotMat, G2B_RotMat;
Vec12 _q;
// Robot command
Vec3 _pcd;
Vec3 _vCmdGlobal, _vCmdBody;
Vec3 pcd_;
Vec3 vel_target_, _vCmdBody;
double dt_;
double _yawCmd{}, _dYawCmd{};
double _dYawCmdPast{};
Vec3 _wCmdGlobal;
Vec34 _posFeetGlobalGoal, _velFeetGlobalGoal;
Vec34 _posFeet2BGoal, _velFeet2BGoal;
Vec34 pos_feet_global_goal_, vel_feet_global_goal_;
RotMat Rd;
Vec3 _ddPcd, _dWbd;
Vec34 force_feet_global_, force_feet_body_;
Vec12 _tau;
// Control Parameters
double _gaitHeight;
double gait_height_;
Vec3 pos_error_, vel_error_;
Mat3 Kpp, Kdp, Kdw;
double _kpw;

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

@ -81,5 +81,36 @@ T saturation(const T a, Vec2 limits) {
return a;
}
inline RotMat quatToRotMat(const Quat &q) {
double e0 = q(0);
double e1 = q(1);
double e2 = q(2);
double e3 = q(3);
RotMat R;
R << 1 - 2 * (e2 * e2 + e3 * e3), 2 * (e1 * e2 - e0 * e3),
2 * (e1 * e3 + e0 * e2), 2 * (e1 * e2 + e0 * e3),
1 - 2 * (e1 * e1 + e3 * e3), 2 * (e2 * e3 - e0 * e1),
2 * (e1 * e3 - e0 * e2), 2 * (e2 * e3 + e0 * e1),
1 - 2 * (e1 * e1 + e2 * e2);
return R;
}
inline Vec3 rotMatToRPY(const Mat3 &R) {
Vec3 rpy;
rpy(0) = atan2(R(2, 1), R(2, 2));
rpy(1) = asin(-R(2, 0));
rpy(2) = atan2(R(1, 0), R(0, 0));
return rpy;
}
inline RotMat rotz(const double &theta) {
const double s = std::sin(theta);
const double c = std::cos(theta);
RotMat R;
R << c, -s, 0, s, c, 0, 0, 0, 1;
return R;
}
#endif //MATHTOOLS_H

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

@ -7,6 +7,7 @@
#include <memory>
#include <kdl/frames.hpp>
#include <unitree_guide_controller/common/mathTypes.h>
#include <unitree_guide_controller/robot/QuadrupedRobot.h>
#include "LowPassFilter.h"
@ -41,7 +42,7 @@ public:
* @return foot position in world frame
*/
Vec3 getFootPos(const int index) {
return getPosition() + Vec3((rotation_ * foot_poses_[index].p).data);
return getPosition() + rotation_ * Vec3(foot_poses_[index].p.data);
}
/**
@ -49,11 +50,24 @@ public:
* @return feet velocity in world frame
*/
Vec34 getFeetPos() {
Vec34 feetPos;
Vec34 feet_pos;
for (int i(0); i < 4; ++i) {
feetPos.col(i) = getFootPos(i);
feet_pos.col(i) = getFootPos(i);
}
return feetPos;
return feet_pos;
}
/**
* Get the estimated feet velocity in world frame
* @return feet velocity in world frame
*/
Vec34 getFeetVel() {
const std::vector<KDL::Vector> feet_vel = robot_model_.getFeet2BVelocities();
Vec34 result;
for (int i(0); i < 4; ++i) {
result.col(i) = Vec3(feet_vel[i].data) + getVelocity();
}
return result;
}
/**
@ -69,23 +83,19 @@ public:
return foot_pos;
}
KDL::Rotation getRotation() {
RotMat getRotation() {
return rotation_;
}
KDL::Vector getGyro() {
Vec3 getGyro() {
return gyro_;
}
[[nodiscard]] Vec3 getGlobalGyro() const {
return Vec3((rotation_ * gyro_).data);
return rotation_ * gyro_;
}
[[nodiscard]] double getYaw() const {
double roll, pitch, yaw;
rotation_.GetRPY(roll, pitch, yaw);
return yaw;
}
[[nodiscard]] double getYaw() const;
[[nodiscard]] double getDYaw() const {
return getGlobalGyro()(2);
@ -130,12 +140,12 @@ private:
Eigen::Matrix<double, 28, 18> STC; // _STC = (_S.transpose()).inv() * C
Eigen::Matrix<double, 18, 18> IKC; // _IKC = I - KC
KDL::Vector g_;
Vec3 g_;
double _dt;
KDL::Rotation rotation_;
KDL::Vector acceleration_;
KDL::Vector gyro_;
RotMat rotation_;
Vec3 acceleration_;
Vec3 gyro_;
std::vector<KDL::Frame> foot_poses_;
std::vector<KDL::Vector> foot_vels_;

2
controllers/unitree_guide_controller/include/unitree_guide_controller/gait/GaitGenerator.h
View File

@ -22,7 +22,7 @@ public:
void setGait(Vec2 vxy_goal_global, double d_yaw_goal, double gait_height);
void run(Vec34 &feet_pos, Vec34 &feet_vel);
void generate(Vec34 &feet_pos, Vec34 &feet_vel);
void restart();

17
controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp
View File

@ -39,11 +39,9 @@ void StateBalanceTest::run() {
pcd_(0) = pcd_init_(0) + invNormalize(ctrl_comp_.control_inputs_.get().ly, _xMin, _xMax);
pcd_(1) = pcd_init_(1) - invNormalize(ctrl_comp_.control_inputs_.get().lx, _yMin, _yMax);
pcd_(2) = pcd_init_(2) + invNormalize(ctrl_comp_.control_inputs_.get().ry, _zMin, _zMax);
const float yaw = invNormalize(ctrl_comp_.control_inputs_.get().rx, _yawMin, _yawMax);
Rd_ = Mat3((KDL::Rotation::RPY(0, 0, yaw).Inverse() * init_rotation_).data);
pose_body_ = estimator_.getPosition();
vel_body_ = estimator_.getVelocity();
const float yaw = invNormalize(ctrl_comp_.control_inputs_.get().rx, _yawMin, _yawMax);
Rd_ = rotz(yaw) * init_rotation_;
for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(0.8);
@ -68,15 +66,18 @@ FSMStateName StateBalanceTest::checkChange() {
}
void StateBalanceTest::calcTorque() {
const auto B2G_Rotation = Eigen::Matrix3d(estimator_.getRotation().data);
const auto B2G_Rotation = estimator_.getRotation();
const RotMat G2B_Rotation = B2G_Rotation.transpose();
const Vec3 pose_body = estimator_.getPosition();
const Vec3 vel_body = estimator_.getVelocity();
// expected body acceleration
dd_pcd_ = Kp_p_ * (pcd_ - pose_body_) + Kd_p_ * -vel_body_;
dd_pcd_ = Kp_p_ * (pcd_ - pose_body) + Kd_p_ * (Vec3(0, 0, 0) - vel_body);
// expected body angular acceleration
d_wbd_ = -kp_w_ * rotMatToExp(Rd_ * G2B_Rotation) +
Kd_w_ * -estimator_.getGlobalGyro();
d_wbd_ = kp_w_ * rotMatToExp(Rd_ * G2B_Rotation) +
Kd_w_ * (Vec3(0, 0, 0) - estimator_.getGlobalGyro());
// calculate foot force
const Vec34 foot_force = G2B_Rotation * balance_ctrl_.calF(dd_pcd_, d_wbd_, B2G_Rotation,

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

@ -62,7 +62,7 @@ void StateFreeStand::calc_body_target(const float row, const float pitch,
KDL::Frame fr_2_body_pos;
fr_2_body_pos.p = -fr_init_pos_.p;
fr_2_body_pos.p.z(fr_2_body_pos.p.z() + height);
fr_2_body_pos.M = KDL::Rotation::RPY(row, pitch, yaw);
fr_2_body_pos.M = KDL::Rotation::RPY(row, pitch, -yaw);
const KDL::Frame body_2_fr_pos = fr_2_body_pos.Inverse();
std::vector goal_pos(4, KDL::Frame::Identity());

84
controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp
View File

@ -12,7 +12,7 @@ StateTrotting::StateTrotting(CtrlComponent &ctrlComp) : FSMState(FSMStateName::T
balance_ctrl_(ctrlComp.balance_ctrl_),
wave_generator_(ctrl_comp_.wave_generator_),
gait_generator_(ctrlComp) {
_gaitHeight = 0.08;
gait_height_ = 0.08;
Kpp = Vec3(70, 70, 70).asDiagonal();
Kdp = Vec3(10, 10, 10).asDiagonal();
_kpw = 780;
@ -27,7 +27,7 @@ StateTrotting::StateTrotting(CtrlComponent &ctrlComp) : FSMState(FSMStateName::T
}
void StateTrotting::enter() {
_pcd = estimator_.getPosition();
pcd_ = estimator_.getPosition();
_vCmdBody.setZero();
_yawCmd = estimator_.getYaw();
Rd = Mat3(KDL::Rotation::RPY(0, 0, _yawCmd).Inverse().data);
@ -38,18 +38,17 @@ void StateTrotting::enter() {
}
void StateTrotting::run() {
_posBody = estimator_.getPosition();
_velBody = estimator_.getVelocity();
_posFeet2BGlobal = estimator_.getFeetPos2Body();
_posFeetGlobal = estimator_.getFeetPos();
B2G_RotMat = Eigen::Matrix3d(estimator_.getRotation().data);
pos_body_ = estimator_.getPosition();
vel_body_ = estimator_.getVelocity();
B2G_RotMat = estimator_.getRotation();
G2B_RotMat = B2G_RotMat.transpose();
getUserCmd();
calcCmd();
gait_generator_.setGait(_vCmdGlobal.segment(0, 2), _wCmdGlobal(2), _gaitHeight);
gait_generator_.run(_posFeetGlobalGoal, _velFeetGlobalGoal);
gait_generator_.setGait(vel_target_.segment(0, 2), _wCmdGlobal(2), gait_height_);
gait_generator_.generate(pos_feet_global_goal_, vel_feet_global_goal_);
calcTau();
calcQQd();
@ -92,19 +91,19 @@ void StateTrotting::getUserCmd() {
void StateTrotting::calcCmd() {
/* Movement */
_vCmdGlobal = B2G_RotMat * _vCmdBody;
vel_target_ = B2G_RotMat * _vCmdBody;
_vCmdGlobal(0) =
saturation(_vCmdGlobal(0), Vec2(_velBody(0) - 0.2, _velBody(0) + 0.2));
_vCmdGlobal(1) =
saturation(_vCmdGlobal(1), Vec2(_velBody(1) - 0.2, _velBody(1) + 0.2));
vel_target_(0) =
saturation(vel_target_(0), Vec2(vel_body_(0) - 0.2, vel_body_(0) + 0.2));
vel_target_(1) =
saturation(vel_target_(1), Vec2(vel_body_(1) - 0.2, vel_body_(1) + 0.2));
_pcd(0) = saturation(_pcd(0) + _vCmdGlobal(0) * dt_,
Vec2(_posBody(0) - 0.05, _posBody(0) + 0.05));
_pcd(1) = saturation(_pcd(1) + _vCmdGlobal(1) * dt_,
Vec2(_posBody(1) - 0.05, _posBody(1) + 0.05));
pcd_(0) = saturation(pcd_(0) + vel_target_(0) * dt_,
Vec2(pos_body_(0) - 0.05, pos_body_(0) + 0.05));
pcd_(1) = saturation(pcd_(1) + vel_target_(1) * dt_,
Vec2(pos_body_(1) - 0.05, pos_body_(1) + 0.05));
_vCmdGlobal(2) = 0;
vel_target_(2) = 0;
/* Turning */
_yawCmd = _yawCmd + _dYawCmd * dt_;
@ -113,33 +112,37 @@ void StateTrotting::calcCmd() {
}
void StateTrotting::calcTau() {
pos_error_ = _pcd - _posBody;
vel_error_ = _vCmdGlobal - _velBody;
pos_error_ = pcd_ - pos_body_;
vel_error_ = vel_target_ - vel_body_;
_ddPcd = Kpp * pos_error_ + Kdp * vel_error_;
_dWbd = _kpw * rotMatToExp(Rd * G2B_RotMat) +
Vec3 dd_pcd = Kpp * pos_error_ + Kdp * vel_error_;
Vec3 d_wbd = _kpw * rotMatToExp(Rd * G2B_RotMat) +
Kdw * (_wCmdGlobal - estimator_.getGlobalGyro());
_ddPcd(0) = saturation(_ddPcd(0), Vec2(-3, 3));
_ddPcd(1) = saturation(_ddPcd(1), Vec2(-3, 3));
_ddPcd(2) = saturation(_ddPcd(2), Vec2(-5, 5));
dd_pcd(0) = saturation(dd_pcd(0), Vec2(-3, 3));
dd_pcd(1) = saturation(dd_pcd(1), Vec2(-3, 3));
dd_pcd(2) = saturation(dd_pcd(2), Vec2(-5, 5));
_dWbd(0) = saturation(_dWbd(0), Vec2(-40, 40));
_dWbd(1) = saturation(_dWbd(1), Vec2(-40, 40));
_dWbd(2) = saturation(_dWbd(2), Vec2(-10, 10));
d_wbd(0) = saturation(d_wbd(0), Vec2(-40, 40));
d_wbd(1) = saturation(d_wbd(1), Vec2(-40, 40));
d_wbd(2) = saturation(d_wbd(2), Vec2(-10, 10));
force_feet_global_ =
-balance_ctrl_.calF(_ddPcd, _dWbd, B2G_RotMat, _posFeet2BGlobal, wave_generator_.contact_);
const Vec34 pos_feet2_b_global = estimator_.getFeetPos2Body();
Vec34 force_feet_global =
-balance_ctrl_.calF(dd_pcd, d_wbd, B2G_RotMat, pos_feet2_b_global, wave_generator_.contact_);
Vec34 pos_feet_global = estimator_.getFeetPos();
Vec34 vel_feet_global = estimator_.getFeetVel();
for (int i(0); i < 4; ++i) {
if (wave_generator_.contact_(i) == 0) {
force_feet_global_.col(i) =
KpSwing * (_posFeetGlobalGoal.col(i) - _posFeetGlobal.col(i)) +
KdSwing * (_velFeetGlobalGoal.col(i) - _velFeetGlobal.col(i));
force_feet_global.col(i) =
KpSwing * (pos_feet_global_goal_.col(i) - pos_feet_global.col(i)) +
KdSwing * (vel_feet_global_goal_.col(i) - vel_feet_global.col(i));
}
}
force_feet_body_ = G2B_RotMat * force_feet_global_;
Vec34 force_feet_body_ = G2B_RotMat * force_feet_global;
std::vector<KDL::JntArray> current_joints = robot_model_.current_joint_pos_;
for (int i = 0; i < 4; i++) {
@ -151,18 +154,19 @@ void StateTrotting::calcTau() {
}
void StateTrotting::calcQQd() {
const std::vector<KDL::Frame> _posFeet2B = robot_model_.getFeet2BPositions();
const std::vector<KDL::Frame> pos_feet2_b = robot_model_.getFeet2BPositions();
Vec34 pos_feet2_b_goal, vel_feet2_b_goal;
for (int i(0); i < 4; ++i) {
_posFeet2BGoal.col(i) = G2B_RotMat * (_posFeetGlobalGoal.col(i) - _posBody);
_velFeet2BGoal.col(i) = G2B_RotMat * (_velFeetGlobalGoal.col(i) - _velBody);
pos_feet2_b_goal.col(i) = G2B_RotMat * (pos_feet_global_goal_.col(i) - pos_body_);
vel_feet2_b_goal.col(i) = G2B_RotMat * (vel_feet_global_goal_.col(i) - vel_body_);
// _velFeet2BGoal.col(i) = _G2B_RotMat * (_velFeetGlobalGoal.col(i) -
// _velBody - _B2G_RotMat * (skew(_lowState->getGyro()) * _posFeet2B.col(i))
// ); // c.f formula (6.12)
}
Vec12 q_goal = robot_model_.getQ(_posFeet2BGoal);
Vec12 qd_goal = robot_model_.getQd(_posFeet2B, _velFeet2BGoal);
Vec12 q_goal = robot_model_.getQ(pos_feet2_b_goal);
Vec12 qd_goal = robot_model_.getQd(pos_feet2_b, vel_feet2_b_goal);
for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_position_command_interface_[i].get().set_value(q_goal(i));
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(qd_goal(i));

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

@ -49,7 +49,6 @@ namespace unitree_guide_controller {
// update_frequency_ = 1.0 / time_diff.count();
// RCLCPP_INFO(get_node()->get_logger(), "Update frequency: %f Hz", update_frequency_);
ctrl_comp_.robot_model_.update();
ctrl_comp_.wave_generator_.update();
ctrl_comp_.estimator_.update();

3
controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp
View File

@ -47,8 +47,8 @@ Vec34 BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rot_m
g0T_ = -bd_.transpose() * S_ * A_ - beta_ * F_prev_.transpose() * U_;
solveQP();
F_prev_ = F_;
F_prev_ = F_;
return vec12ToVec34(F_);
}
@ -71,6 +71,7 @@ void BalanceCtrl::calConstraints(const VecInt4 &contact) {
contactLegNum += 1;
}
}
CI_.resize(5 * contactLegNum, 12);
ci0_.resize(5 * contactLegNum);
CE_.resize(3 * (4 - contactLegNum), 12);

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

@ -3,12 +3,14 @@
//
#include "unitree_guide_controller/control/Estimator.h"
#include <unitree_guide_controller/common/mathTools.h>
#include "unitree_guide_controller/control/CtrlComponent.h"
#include "unitree_guide_controller/common/mathTools.h"
Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_component),
robot_model_(ctrl_component.robot_model_) {
g_ = KDL::Vector(0, 0, -9.81);
g_ << 0, 0, -9.81;
_dt = 0.002;
_largeVariance = 100;
for (int i(0); i < Qdig.rows(); ++i) {
@ -139,6 +141,10 @@ Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_compo
low_pass_filters_[2] = std::make_shared<LowPassFilter>(0.02, 3.0);
}
double Estimator::getYaw() const {
return rotMatToRPY(rotation_)(2);
}
void Estimator::update() {
if (robot_model_.mass_ == 0) return;
@ -176,20 +182,34 @@ void Estimator::update() {
}
// Acceleration from imu as system input
rotation_ = KDL::Rotation::Quaternion(ctrl_component_.imu_state_interface_[1].get().get_value(),
// rotation_ = KDL::Rotation::Quaternion(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_value());
Quat quat;
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_value());
ctrl_component_.imu_state_interface_[3].get().get_value();
rotation_ = quatToRotMat(quat);
gyro_ = KDL::Vector(ctrl_component_.imu_state_interface_[4].get().get_value(),
gyro_ << 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_[6].get().get_value();
acceleration_ = KDL::Vector(ctrl_component_.imu_state_interface_[7].get().get_value(),
// gyro_ = KDL::Vector(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());
acceleration_ << 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_[9].get().get_value();
// acceleration_ = KDL::Vector(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());
_u = Vec3((rotation_ * acceleration_ + g_).data);
_u = rotation_ * acceleration_ + g_;
x_hat_ = A * x_hat_ + B * _u;
y_hat_ = C * x_hat_;

2
controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp
View File

@ -20,7 +20,7 @@ void GaitGenerator::setGait(Vec2 vxy_goal_global, const double d_yaw_goal, const
gait_height_ = gait_height;
}
void GaitGenerator::run(Vec34 &feet_pos, Vec34 &feet_vel) {
void GaitGenerator::generate(Vec34 &feet_pos, Vec34 &feet_vel) {
if (first_run_) {
start_p_ = estimator_.getFeetPos();
first_run_ = false;