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

balance control achieved

This commit is contained in:
Huang Zhenbiao 2024-09-17 18:21:04 +08:00
parent 79fc0677f5
commit 8cedc4f422
16 changed files with 731 additions and 690 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
controllers/unitree_guide_controller/CMakeLists.txt
View File

@ -49,7 +49,9 @@ add_library(${PROJECT_NAME} SHARED
target_include_directories(${PROJECT_NAME}
PUBLIC
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
PRIVATE
src)
ament_target_dependencies(
${PROJECT_NAME} PUBLIC
${CONTROLLER_INCLUDE_DEPENDS}

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

@ -24,14 +24,14 @@ private:
KDL::Vector pcd_;
KDL::Vector pcdInit_;
KDL::Rotation Rd_;
KDL::Rotation RdInit_;
RotMat Rd_;
RotMat init_rotation_;
KDL::Vector pose_body_, vel_body_;
double kp_w_;
Mat3 Kp_p_, Kd_p_, Kd_w_;
Vec3 _ddPcd, _dWbd;
Vec3 dd_pcd_, d_wbd_;
float _xMax, _xMin;
float _yMax, _yMin;

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

@ -33,17 +33,13 @@ 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 Mat3 skew(const Vec3 &v) {
Mat3 m;
m << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0;
return m;
}
inline Vec3 rotationToExp(const KDL::Rotation &rotation) {
auto rm = Eigen::Matrix3d(rotation.data);
inline Vec3 rotMatToExp(const RotMat &rm) {
double cosValue = rm.trace() / 2.0 - 1 / 2.0;
if (cosValue > 1.0f) {
cosValue = 1.0f;
@ -51,7 +47,7 @@ inline Vec3 rotationToExp(const KDL::Rotation &rotation) {
cosValue = -1.0f;
}
double angle = acos(cosValue);
const double angle = acos(cosValue);
Vec3 exp;
if (fabs(angle) < 1e-5) {
exp = Vec3(0, 0, 0);
@ -65,5 +61,39 @@ inline Vec3 rotationToExp(const KDL::Rotation &rotation) {
return exp;
}
inline RotMat rotx(const double &theta) {
double s = std::sin(theta);
double c = std::cos(theta);
RotMat R;
R << 1, 0, 0, 0, c, -s, 0, s, c;
return R;
}
inline RotMat roty(const double &theta) {
double s = std::sin(theta);
double c = std::cos(theta);
RotMat R;
R << c, 0, s, 0, 1, 0, -s, 0, c;
return R;
}
inline RotMat rotz(const double &theta) {
double s = std::sin(theta);
double c = std::cos(theta);
RotMat R;
R << c, -s, 0, s, c, 0, 0, 0, 1;
return R;
}
inline RotMat rpyToRotMat(const double &row, const double &pitch,
const double &yaw) {
RotMat m = rotz(yaw) * roty(pitch) * rotx(row);
return m;
}
#endif //MATHTOOLS_H

31
controllers/unitree_guide_controller/include/unitree_guide_controller/control/BalanceCtrl.h
View File

@ -18,30 +18,29 @@ public:
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);
Vec34 calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &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 calMatrixA(const std::vector<KDL::Vector> &feetPos2B, const RotMat &rotM);
void calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const KDL::Rotation &rotM);
void calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &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;
Mat12 G_, W_, U_;
Mat6 S_;
Mat3 Ib_;
Vec6 bd_;
Vec3 _g, _pcb;
Vec12 F_, F_prev_, g0T_;
double mass_, alpha_, beta_, friction_ratio_;
Eigen::MatrixXd CE_, CI_;
Eigen::VectorXd ce0_, ci0_;
Eigen::Matrix<double, 6, 12> A_;
Eigen::Matrix<double, 5, 3> friction_mat_;
};

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

@ -4,6 +4,7 @@
#ifndef ESTIMATOR_H
#define ESTIMATOR_H
#include <iostream>
#include <memory>
#include <eigen3/Eigen/Dense>
#include <kdl/frames.hpp>
@ -23,7 +24,7 @@ public:
* @return robot central position
*/
KDL::Vector getPosition() {
return {_xhat(0), _xhat(1), _xhat(2)};
return {x_hat_(0), x_hat_(1), x_hat_(2)};
}
/**
@ -31,7 +32,7 @@ public:
* @return robot central velocity
*/
KDL::Vector getVelocity() {
return {_xhat(3), _xhat(4), _xhat(5)};
return {x_hat_(3), x_hat_(4), x_hat_(5)};
}
/**
@ -85,38 +86,38 @@ public:
void update(const CtrlComponent &ctrlComp);
private:
Eigen::Matrix<double, 18, 1> _xhat; // The state of estimator, position(3)+velocity(3)+feet position(3x4)
Eigen::Matrix<double, 18, 1> x_hat_; // The state of estimator, position(3)+velocity(3)+feet position(3x4)
Eigen::Matrix<double, 3, 1> _u; // The input of estimator
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, 28, 1> _y; // The measurement value of output y
Eigen::Matrix<double, 28, 1> y_hat_; // 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
// 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> 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
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
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
// Output Measurement
Eigen::Matrix<double, 12, 1> _feetPos2Body; // The feet positions to body, in the global coordinate
Eigen::Matrix<double, 12, 1> _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, 12, 1> _feetPos2Body; // The feet positions to body, in the global coordinate
Eigen::Matrix<double, 12, 1> _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::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, 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;

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

@ -7,6 +7,7 @@
#define QUADRUPEDROBOT_H
#include <string>
#include <kdl_parser/kdl_parser.hpp>
#include <unitree_guide_controller/common/mathTypes.h>
#include "RobotLeg.h"
@ -54,6 +55,15 @@ public:
* @param index leg index
* @return torque
*/
[[nodiscard]] KDL::JntArray getTorque(
const Vec3 &force, int index) const;
/**
* Calculate the torque based on joint positions, joint velocities and external force
* @param force external force
* @param index leg index
* @return torque
*/
[[nodiscard]] KDL::JntArray getTorque(
const KDL::Vector &force, int index) const;

3
controllers/unitree_guide_controller/include/unitree_guide_controller/robot/RobotLeg.h
View File

@ -11,6 +11,7 @@
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
#include <unitree_guide_controller/common/mathTypes.h>
class RobotLeg {
public:
@ -46,7 +47,7 @@ public:
* @param force foot end force
* @return joint torque
*/
[[nodiscard]] KDL::JntArray calcTorque(const KDL::JntArray &joint_positions, const KDL::Vector &force) const;
[[nodiscard]] KDL::JntArray calcTorque(const KDL::JntArray &joint_positions, const Vec3 &force) const;
protected:
KDL::Chain chain_;

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

@ -25,9 +25,9 @@ StateBalanceTest::StateBalanceTest(CtrlComponent ctrlComp) : FSMState(FSMStateNa
}
void StateBalanceTest::enter() {
pcd_ = ctrl_comp_.estimator_.get().getPosition();
pcdInit_ = pcd_;
RdInit_ = ctrl_comp_.estimator_.get().getRotation();
pcdInit_ = ctrl_comp_.estimator_.get().getPosition();
pcd_ = pcdInit_;
init_rotation_ = Eigen::Matrix3d(ctrl_comp_.estimator_.get().getRotation().data);
}
void StateBalanceTest::run() {
@ -35,7 +35,7 @@ void StateBalanceTest::run() {
pcd_(1) = pcdInit_(1) - invNormalize(ctrl_comp_.control_inputs_.get().lx, _yMin, _yMax);
pcd_(2) = pcdInit_(2) + invNormalize(ctrl_comp_.control_inputs_.get().ry, _zMin, _zMax);
const float yaw = invNormalize(ctrl_comp_.control_inputs_.get().rx, _yawMin, _yawMax);
Rd_ = KDL::Rotation::RPY(0, 0, yaw) * RdInit_;
Rd_ = rpyToRotMat(0, 0, yaw) * init_rotation_;
pose_body_ = ctrl_comp_.estimator_.get().getPosition();
vel_body_ = ctrl_comp_.estimator_.get().getVelocity();
@ -63,26 +63,25 @@ FSMStateName StateBalanceTest::checkChange() {
}
void StateBalanceTest::calcTorque() {
const auto B2G_Rotation = Eigen::Matrix3d(ctrl_comp_.estimator_.get().getRotation().data);
const RotMat G2B_Rotation = B2G_Rotation.transpose();
// expected body acceleration
_ddPcd = Kp_p_ * Vec3((pcd_ - pose_body_).data) + Kd_p_ * Vec3(
(KDL::Vector(0, 0, 0) - vel_body_).data);
dd_pcd_ = Kp_p_ * Vec3((pcd_ - pose_body_).data) + Kd_p_ * Vec3((-vel_body_).data);
// expected body angular acceleration
const KDL::Rotation B2G_Rotation = ctrl_comp_.estimator_.get().getRotation();
const KDL::Rotation G2B_Rotation = B2G_Rotation.Inverse();
_dWbd = kp_w_ * rotationToExp(Rd_ * G2B_Rotation) +
Kd_w_ * (Vec3(0, 0, 0) - Vec3(ctrl_comp_.estimator_.get().getGlobalGyro().data));
d_wbd_ = kp_w_ * rotMatToExp(Rd_ * G2B_Rotation) +
Kd_w_ * (Vec3(0,0,0) - Vec3((-ctrl_comp_.estimator_.get().getGlobalGyro()).data));
// calculate foot force
const std::vector contact(4, 1);
const std::vector<KDL::Vector> foot_force = ctrl_comp_.balance_ctrl_.get().calF(_ddPcd, _dWbd, B2G_Rotation,
ctrl_comp_.estimator_.get().
getFootPos2Body(), contact);
const Vec34 foot_force = G2B_Rotation * ctrl_comp_.balance_ctrl_.get().calF(dd_pcd_, -d_wbd_, B2G_Rotation,
ctrl_comp_.estimator_.get().
getFootPos2Body(), contact);
std::vector<KDL::JntArray> current_joints = ctrl_comp_.robot_model_.get().current_joint_pos_;
for (int i = 0; i < 4; i++) {
std::cout<<Vec3(foot_force[i].data).transpose()<<std::endl;
KDL::JntArray torque = ctrl_comp_.robot_model_.get().getTorque(B2G_Rotation*(-foot_force[i]), i);
KDL::JntArray torque = ctrl_comp_.robot_model_.get().getTorque(-foot_force.col(i), i);
for (int j = 0; j < 3; j++) {
ctrl_comp_.joint_effort_command_interface_[i * 3 + j].get().set_value(torque(j));
ctrl_comp_.joint_position_command_interface_[i * 3 + j].get().set_value(current_joints[i](j));

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

@ -5,21 +5,24 @@
#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>
#include "quadProgpp/QuadProg++.hh"
BalanceCtrl::BalanceCtrl() {
_mass = 15;
_alpha = 0.001;
_beta = 0.1;
_fricRatio = 0.4;
mass_ = 15;
alpha_ = 0.001;
beta_ = 0.1;
_g << 0, 0, -9.81;
friction_ratio_ = 0.4;
friction_mat_ << 1, 0, friction_ratio_, -1, 0, friction_ratio_, 0, 1, friction_ratio_, 0, -1,
friction_ratio_, 0, 0, 1;
}
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();
mass_ = robot.mass_;
_pcb = Vec3(0.0, 0.0, 0.0);
Ib_ = Vec3(0.0792, 0.2085, 0.2265).asDiagonal();
Vec6 s;
Vec12 w, u;
@ -27,46 +30,42 @@ void BalanceCtrl::init(const QuadrupedRobot &robot) {
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;
S_ = s.asDiagonal();
W_ = w.asDiagonal();
U_ = u.asDiagonal();
F_prev_.setZero();
}
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) {
Vec34 BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM,
const std::vector<KDL::Vector> &feetPos2B, const std::vector<int> &contact) {
std::cout << "ddPcd: " << ddPcd.transpose() << std::endl;
std::cout << "dWbd: " << dWbd.transpose() << std::endl;
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;
G_ = A_.transpose() * S_ * A_ + alpha_ * W_ + beta_ * U_;
g0T_ = -bd_.transpose() * S_ * A_ - beta_ * F_prev_.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;
F_prev_ = F_;
return vec12ToVec34(F_);
}
void BalanceCtrl::calMatrixA(const std::vector<KDL::Vector> &feetPos2B, const KDL::Rotation &rotM) {
void BalanceCtrl::calMatrixA(const std::vector<KDL::Vector> &feetPos2B, const RotMat &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);
A_.block(0, 3 * i, 3, 3) = I3;
A_.block(3, 3 * i, 3, 3) = skew(Vec3(feetPos2B[i].data) - rotM * _pcb);
}
}
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::calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM) {
bd_.head(3) = mass_ * (ddPcd - _g);
bd_.tail(3) = rotM * Ib_ * rotM.transpose() * dWbd;
}
void BalanceCtrl::calConstraints(const std::vector<int> &contact) {
@ -76,33 +75,33 @@ void BalanceCtrl::calConstraints(const std::vector<int> &contact) {
contactLegNum += 1;
}
}
_CI.resize(5 * contactLegNum, 12);
_ci0.resize(5 * contactLegNum);
_CE.resize(3 * (4 - contactLegNum), 12);
_ce0.resize(3 * (4 - contactLegNum));
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();
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;
CI_.block(5 * ciID, 3 * i, 5, 3) = friction_mat_;
++ciID;
} else {
_CE.block(3 * ceID, 3 * i, 3, 3) = I3;
CE_.block(3 * ceID, 3 * i, 3, 3) = I3;
++ceID;
}
}
}
void BalanceCtrl::solveQP() {
const long n = _F.size();
const long m = _ce0.size();
const long p = _ci0.size();
const long n = F_.size();
const long m = ce0_.size();
const long p = ci0_.size();
quadprogpp::Matrix<double> G, CE, CI;
quadprogpp::Vector<double> g0, ce0, ci0, x;
@ -117,37 +116,37 @@ void BalanceCtrl::solveQP() {
for (int i = 0; i < n; ++i) {
for (int j = 0; j < n; ++j) {
G[i][j] = _G(i, 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);
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);
CI[i][j] = CI_.transpose()(i, j);
}
}
for (int i = 0; i < n; ++i) {
g0[i] = _g0T[i];
g0[i] = g0T_[i];
}
for (int i = 0; i < m; ++i) {
ce0[i] = _ce0[i];
ce0[i] = ce0_[i];
}
for (int i = 0; i < p; ++i) {
ci0[i] = _ci0[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];
F_[i] = x[i];
}
}

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

@ -6,14 +6,6 @@
#include "unitree_guide_controller/control/CtrlComponent.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;
@ -22,7 +14,7 @@ Estimator::Estimator() {
Qdig(i) = i < 6 ? 0.0003 : 0.01;
}
_xhat.setZero();
x_hat_.setZero();
_u.setZero();
A.setZero();
@ -197,8 +189,8 @@ void Estimator::update(const CtrlComponent &ctrlComp) {
ctrlComp.imu_state_interface_[9].get().get_value());
_u = Vec3((rotation_ * acceleration_ + g_).data);
_xhat = A * _xhat + B * _u;
_yhat = C * _xhat;
x_hat_ = A * x_hat_ + B * _u;
y_hat_ = C * x_hat_;
// Update the measurement value
_y << _feetPos2Body, _feetVel2Body, _feetH;
@ -207,22 +199,22 @@ void Estimator::update(const CtrlComponent &ctrlComp) {
Ppriori = A * P * A.transpose() + Q;
S = R + C * Ppriori * C.transpose();
Slu = S.lu();
Sy = Slu.solve(_y - _yhat);
Sy = Slu.solve(_y - y_hat_);
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;
x_hat_ += 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();
low_pass_filters_[0]->addValue(x_hat_(3));
low_pass_filters_[1]->addValue(x_hat_(4));
low_pass_filters_[2]->addValue(x_hat_(5));
x_hat_(3) = low_pass_filters_[0]->getValue();
x_hat_(4) = low_pass_filters_[1]->getValue();
x_hat_(5) = low_pass_filters_[2]->getValue();
}

26
controllers/unitree_guide_controller/src/quadProgpp/Array.cc
View File

@ -5,24 +5,26 @@
// This software may be modified and distributed under the terms
// of the MIT license. See the LICENSE file for details.
#include "unitree_guide_controller/quadProgpp/Array.hh"
#include "Array.hh"
/**
Index utilities
*/
namespace quadprogpp {
std::set<unsigned int> seq(unsigned int s, unsigned int e) {
std::set<unsigned int> tmp;
for (unsigned int i = s; i <= e; i++) tmp.insert(i);
return tmp;
}
std::set<unsigned int> seq(unsigned int s, unsigned int e) {
std::set<unsigned int> tmp;
for (unsigned int i = s; i <= e; i++) tmp.insert(i);
std::set<unsigned int> singleton(unsigned int i) {
std::set<unsigned int> tmp;
tmp.insert(i);
return tmp;
}
return tmp;
}
} // namespace quadprogpp
std::set<unsigned int> singleton(unsigned int i) {
std::set<unsigned int> tmp;
tmp.insert(i);
return tmp;
}
} // namespace quadprogpp

0
controllers/unitree_guide_controller/include/unitree_guide_controller/quadProgpp/Array.hh → controllers/unitree_guide_controller/src/quadProgpp/Array.hh
View File

1064
controllers/unitree_guide_controller/src/quadProgpp/QuadProg++.cc
View File

File diff suppressed because it is too large Load Diff

0
controllers/unitree_guide_controller/include/unitree_guide_controller/quadProgpp/QuadProg++.hh → controllers/unitree_guide_controller/src/quadProgpp/QuadProg++.hh
View File

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

@ -60,10 +60,14 @@ KDL::Jacobian QuadrupedRobot::getJacobian(const int index) const {
}
KDL::JntArray QuadrupedRobot::getTorque(
const KDL::Vector &force, const int index) const {
const Vec3 &force, const int index) const {
return robot_legs_[index]->calcTorque(current_joint_pos_[index], force);
}
KDL::JntArray QuadrupedRobot::getTorque(const KDL::Vector &force, int index) const {
return robot_legs_[index]->calcTorque(current_joint_pos_[index], Vec3(force.data));
}
KDL::Vector QuadrupedRobot::getFeet2BVelocities(const int index) const {
const Eigen::Matrix<double, 3, Eigen::Dynamic> jacobian = getJacobian(index).data.topRows(3);
Eigen::VectorXd foot_velocity = jacobian * current_joint_vel_[index].data;

4
controllers/unitree_guide_controller/src/robot/RobotLeg.cpp
View File

@ -33,9 +33,9 @@ KDL::Jacobian RobotLeg::calcJaco(const KDL::JntArray &joint_positions) const {
return jacobian;
}
KDL::JntArray RobotLeg::calcTorque(const KDL::JntArray &joint_positions, const KDL::Vector &force) const {
KDL::JntArray RobotLeg::calcTorque(const KDL::JntArray &joint_positions, const Vec3 &force) const {
const Eigen::Matrix<double, 3, Eigen::Dynamic> jacobian = calcJaco(joint_positions).data.topRows(3);
Eigen::VectorXd torque_eigen = jacobian.transpose() * Vec3(force.data);
Eigen::VectorXd torque_eigen = jacobian.transpose() * force;
KDL::JntArray torque(chain_.getNrOfJoints());
for (unsigned int i = 0; i < chain_.getNrOfJoints(); ++i) {
torque(i) = torque_eigen(i);