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,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> 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> singleton(unsigned int i) {
|
||||
std::set<unsigned int> singleton(unsigned int i) {
|
||||
std::set<unsigned int> tmp;
|
||||
tmp.insert(i);
|
||||
|
||||
return tmp;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace quadprogpp
|
||||
|
|
|
|||
|
|
@ -17,55 +17,56 @@ 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,
|
||||
|
||||
// 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);
|
||||
|
||||
void update_z(Vector<double> &z, const Matrix<double> &J,
|
||||
void update_z(Vector<double> &z, const Matrix<double> &J,
|
||||
const Vector<double> &d, int iq);
|
||||
|
||||
void update_r(const Matrix<double> &R, Vector<double> &r,
|
||||
void update_r(const Matrix<double> &R, Vector<double> &r,
|
||||
const Vector<double> &d, int iq);
|
||||
|
||||
bool add_constraint(Matrix<double> &R, Matrix<double> &J, Vector<double> &d,
|
||||
bool add_constraint(Matrix<double> &R, Matrix<double> &J, Vector<double> &d,
|
||||
unsigned int &iq, double &rnorm);
|
||||
|
||||
void delete_constraint(Matrix<double> &R, Matrix<double> &J, Vector<int> &A,
|
||||
void delete_constraint(Matrix<double> &R, Matrix<double> &J, Vector<int> &A,
|
||||
Vector<double> &u, unsigned int n, int p,
|
||||
unsigned int &iq, int l);
|
||||
|
||||
// Utility functions for computing the Cholesky decomposition and solving
|
||||
// linear systems
|
||||
void cholesky_decomposition(Matrix<double> &A);
|
||||
// Utility functions for computing the Cholesky decomposition and solving
|
||||
// linear systems
|
||||
void cholesky_decomposition(Matrix<double> &A);
|
||||
|
||||
void cholesky_solve(const Matrix<double> &L, Vector<double> &x,
|
||||
void cholesky_solve(const Matrix<double> &L, Vector<double> &x,
|
||||
const Vector<double> &b);
|
||||
|
||||
void forward_elimination(const Matrix<double> &L, Vector<double> &y,
|
||||
void forward_elimination(const Matrix<double> &L, Vector<double> &y,
|
||||
const Vector<double> &b);
|
||||
|
||||
void backward_elimination(const Matrix<double> &U, Vector<double> &x,
|
||||
void backward_elimination(const Matrix<double> &U, Vector<double> &x,
|
||||
const Vector<double> &y);
|
||||
|
||||
// Utility functions for computing the scalar product and the euclidean
|
||||
// distance between two numbers
|
||||
double scalar_product(const Vector<double> &x, const Vector<double> &y);
|
||||
// Utility functions for computing the scalar product and the euclidean
|
||||
// distance between two numbers
|
||||
double scalar_product(const Vector<double> &x, const Vector<double> &y);
|
||||
|
||||
double distance(double a, double b);
|
||||
double distance(double a, double b);
|
||||
|
||||
// Utility functions for printing vectors and matrices
|
||||
void print_matrix(const char *name, const Matrix<double> &A, int n = -1,
|
||||
// Utility functions for printing vectors and matrices
|
||||
void print_matrix(const char *name, const Matrix<double> &A, int n = -1,
|
||||
int m = -1);
|
||||
|
||||
template<typename T>
|
||||
void print_vector(const char *name, const Vector<T> &v, int n = -1);
|
||||
template <typename T>
|
||||
void print_vector(const char *name, const Vector<T> &v, int n = -1);
|
||||
|
||||
// The Solving function, implementing the Goldfarb-Idnani method
|
||||
double solve_quadprog(Matrix<double> &G, Vector<double> &g0,
|
||||
// The Solving function, implementing the Goldfarb-Idnani method
|
||||
double solve_quadprog(Matrix<double> &G, Vector<double> &g0,
|
||||
const Matrix<double> &CE, const Vector<double> &ce0,
|
||||
const Matrix<double> &CI, const Vector<double> &ci0,
|
||||
Vector<double> &x) {
|
||||
|
|
@ -219,7 +220,7 @@ namespace quadprogpp {
|
|||
/* set iai = K \ A */
|
||||
for (i = 0; i < m; i++) iai[i] = i;
|
||||
|
||||
l1:
|
||||
l1:
|
||||
iter++;
|
||||
#ifdef TRACE_SOLVER
|
||||
print_vector("x", x);
|
||||
|
|
@ -260,7 +261,7 @@ namespace quadprogpp {
|
|||
/* and for x */
|
||||
for (i = 0; i < n; i++) x_old[i] = x[i];
|
||||
|
||||
l2: /* Step 2: check for feasibility and determine a new S-pair */
|
||||
l2: /* Step 2: check for feasibility and determine a new S-pair */
|
||||
for (i = 0; i < m; i++) {
|
||||
if (s[i] < ss && iai[i] != -1 && iaexcl[i]) {
|
||||
ss = s[i];
|
||||
|
|
@ -283,7 +284,7 @@ namespace quadprogpp {
|
|||
print_vector("np", np);
|
||||
#endif
|
||||
|
||||
l2a: /* Step 2a: determine step direction */
|
||||
l2a: /* Step 2a: determine step direction */
|
||||
/* compute z = H np: the step direction in the primal space (through J, see
|
||||
* the paper) */
|
||||
compute_d(d, J, np);
|
||||
|
|
@ -429,9 +430,9 @@ namespace quadprogpp {
|
|||
print_vector("s", s, m);
|
||||
#endif
|
||||
goto l2a;
|
||||
}
|
||||
}
|
||||
|
||||
inline void compute_d(Vector<double> &d, const Matrix<double> &J,
|
||||
inline void compute_d(Vector<double> &d, const Matrix<double> &J,
|
||||
const Vector<double> &np) {
|
||||
int i, j, n = d.size();
|
||||
double sum;
|
||||
|
|
@ -442,9 +443,9 @@ namespace quadprogpp {
|
|||
for (j = 0; j < n; j++) sum += J[j][i] * np[j];
|
||||
d[i] = sum;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline void update_z(Vector<double> &z, const Matrix<double> &J,
|
||||
inline void update_z(Vector<double> &z, const Matrix<double> &J,
|
||||
const Vector<double> &d, int iq) {
|
||||
int i, j, n = z.size();
|
||||
|
||||
|
|
@ -453,9 +454,9 @@ namespace quadprogpp {
|
|||
z[i] = 0.0;
|
||||
for (j = iq; j < n; j++) z[i] += J[i][j] * d[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline void update_r(const Matrix<double> &R, Vector<double> &r,
|
||||
inline void update_r(const Matrix<double> &R, Vector<double> &r,
|
||||
const Vector<double> &d, int iq) {
|
||||
int i, j;
|
||||
double sum;
|
||||
|
|
@ -466,9 +467,9 @@ namespace quadprogpp {
|
|||
for (j = i + 1; j < iq; j++) sum += R[i][j] * r[j];
|
||||
r[i] = (d[i] - sum) / R[i][i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool add_constraint(Matrix<double> &R, Matrix<double> &J, Vector<double> &d,
|
||||
bool add_constraint(Matrix<double> &R, Matrix<double> &J, Vector<double> &d,
|
||||
unsigned int &iq, double &R_norm) {
|
||||
unsigned int n = d.size();
|
||||
#ifdef TRACE_SOLVER
|
||||
|
|
@ -531,9 +532,9 @@ namespace quadprogpp {
|
|||
}
|
||||
R_norm = std::max<double>(R_norm, fabs(d[iq - 1]));
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
void delete_constraint(Matrix<double> &R, Matrix<double> &J, Vector<int> &A,
|
||||
void delete_constraint(Matrix<double> &R, Matrix<double> &J, Vector<int> &A,
|
||||
Vector<double> &u, unsigned int n, int p,
|
||||
unsigned int &iq, int l) {
|
||||
#ifdef TRACE_SOLVER
|
||||
|
|
@ -607,9 +608,9 @@ namespace quadprogpp {
|
|||
J[k][j + 1] = xny * (J[k][j] + t1) - t2;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline double distance(double a, double b) {
|
||||
inline double distance(double a, double b) {
|
||||
double a1, b1, t;
|
||||
a1 = fabs(a);
|
||||
b1 = fabs(b);
|
||||
|
|
@ -621,18 +622,18 @@ namespace quadprogpp {
|
|||
return b1 * sqrt(1.0 + t * t);
|
||||
}
|
||||
return a1 * sqrt(2.0);
|
||||
}
|
||||
}
|
||||
|
||||
inline double scalar_product(const Vector<double> &x, const Vector<double> &y) {
|
||||
inline double scalar_product(const Vector<double> &x, const Vector<double> &y) {
|
||||
int i, n = x.size();
|
||||
double sum;
|
||||
|
||||
sum = 0.0;
|
||||
for (i = 0; i < n; i++) sum += x[i] * y[i];
|
||||
return sum;
|
||||
}
|
||||
}
|
||||
|
||||
void cholesky_decomposition(Matrix<double> &A) {
|
||||
void cholesky_decomposition(Matrix<double> &A) {
|
||||
int i, j, k, n = A.nrows();
|
||||
double sum;
|
||||
|
||||
|
|
@ -655,9 +656,9 @@ namespace quadprogpp {
|
|||
}
|
||||
for (k = i + 1; k < n; k++) A[i][k] = A[k][i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void cholesky_solve(const Matrix<double> &L, Vector<double> &x,
|
||||
void cholesky_solve(const Matrix<double> &L, Vector<double> &x,
|
||||
const Vector<double> &b) {
|
||||
int n = L.nrows();
|
||||
Vector<double> y(n);
|
||||
|
|
@ -666,9 +667,9 @@ namespace quadprogpp {
|
|||
forward_elimination(L, y, b);
|
||||
/* Solve L^T * x = y */
|
||||
backward_elimination(L, x, y);
|
||||
}
|
||||
}
|
||||
|
||||
inline void forward_elimination(const Matrix<double> &L, Vector<double> &y,
|
||||
inline void forward_elimination(const Matrix<double> &L, Vector<double> &y,
|
||||
const Vector<double> &b) {
|
||||
int i, j, n = L.nrows();
|
||||
|
||||
|
|
@ -678,9 +679,9 @@ namespace quadprogpp {
|
|||
for (j = 0; j < i; j++) y[i] -= L[i][j] * y[j];
|
||||
y[i] = y[i] / L[i][i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline void backward_elimination(const Matrix<double> &U, Vector<double> &x,
|
||||
inline void backward_elimination(const Matrix<double> &U, Vector<double> &x,
|
||||
const Vector<double> &y) {
|
||||
int i, j, n = U.nrows();
|
||||
|
||||
|
|
@ -690,9 +691,9 @@ namespace quadprogpp {
|
|||
for (j = i + 1; j < n; j++) x[i] -= U[i][j] * x[j];
|
||||
x[i] = x[i] / U[i][i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void print_matrix(const char *name, const Matrix<double> &A, int n, int m) {
|
||||
void print_matrix(const char *name, const Matrix<double> &A, int n, int m) {
|
||||
std::ostringstream s;
|
||||
std::string t;
|
||||
if (n == -1) n = A.nrows();
|
||||
|
|
@ -709,10 +710,10 @@ namespace quadprogpp {
|
|||
0, t.size() - 3); // To remove the trailing space, comma and newline
|
||||
|
||||
std::cout << t << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void print_vector(const char *name, const Vector<T> &v, int n) {
|
||||
template <typename T>
|
||||
void print_vector(const char *name, const Vector<T> &v, int n) {
|
||||
std::ostringstream s;
|
||||
std::string t;
|
||||
if (n == -1) n = v.size();
|
||||
|
|
@ -725,5 +726,6 @@ namespace quadprogpp {
|
|||
t = t.substr(0, t.size() - 2); // To remove the trailing space and comma
|
||||
|
||||
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