balance control achieved
This commit is contained in:
Huang Zhenbiao
parent
79fc0677f5
commit
8cedc4f422
|
|
@ -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}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
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_;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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,12 +86,12 @@ 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, 28, 1> _y; // The measurement value of output y
|
||||
Eigen::Matrix<double, 28, 1> _yhat; // The prediction 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
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
||||
|
|
@ -48,6 +49,15 @@ public:
|
|||
*/
|
||||
[[nodiscard]] KDL::Jacobian getJacobian(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 Vec3 &force, int index) const;
|
||||
|
||||
/**
|
||||
* Calculate the torque based on joint positions, joint velocities and external force
|
||||
* @param force external force
|
||||
|
|
|
|||
|
|
@ -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_;
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
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));
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
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];
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,13 +5,14 @@
|
|||
// 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);
|
||||
|
|
@ -25,4 +26,5 @@ namespace quadprogpp {
|
|||
|
||||
return tmp;
|
||||
}
|
||||
|
||||
} // namespace quadprogpp
|
||||
|
|
|
|||
|
|
@ -17,10 +17,11 @@ File $Id: QuadProg++.cc 232 2007-06-21 12:29:00Z digasper $
|
|||
#include <limits>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
#include "unitree_guide_controller/quadProgpp/QuadProg++.hh"
|
||||
#include "QuadProg++.hh"
|
||||
// #define TRACE_SOLVER
|
||||
|
||||
namespace quadprogpp {
|
||||
|
||||
// Utility functions for updating some data needed by the solution method
|
||||
void compute_d(Vector<double> &d, const Matrix<double> &J,
|
||||
const Vector<double> &np);
|
||||
|
|
@ -726,4 +727,5 @@ namespace quadprogpp {
|
|||
|
||||
std::cout << t << std::endl;
|
||||
}
|
||||
|
||||
} // namespace quadprogpp
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue