diff --git a/controllers/unitree_guide_controller/CMakeLists.txt b/controllers/unitree_guide_controller/CMakeLists.txt index e308953..3b770ed 100644 --- a/controllers/unitree_guide_controller/CMakeLists.txt +++ b/controllers/unitree_guide_controller/CMakeLists.txt @@ -49,7 +49,9 @@ add_library(${PROJECT_NAME} SHARED target_include_directories(${PROJECT_NAME} PUBLIC "$" - "$") + "$" + PRIVATE + src) ament_target_dependencies( ${PROJECT_NAME} PUBLIC ${CONTROLLER_INCLUDE_DEPENDS} diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h index b2b786b..465f68c 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h @@ -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; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h index 5e630f2..a76bdb3 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h @@ -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 diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/BalanceCtrl.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/BalanceCtrl.h index 307e09e..820b6a8 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/BalanceCtrl.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/BalanceCtrl.h @@ -18,30 +18,29 @@ public: void init(const QuadrupedRobot &robot); - std::vector calF(const Vec3 &ddPcd, const Vec3 &dWbd, const KDL::Rotation &rotM, - const std::vector &feetPos2B, const std::vector &contact); + Vec34 calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM, + const std::vector &feetPos2B, const std::vector &contact); private: - void calMatrixA(const std::vector &feetPos2B, const KDL::Rotation &rotM); + void calMatrixA(const std::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 &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 _A; - Eigen::Matrix _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 A_; + Eigen::Matrix friction_mat_; }; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h index f2a058d..8771e8a 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h @@ -4,6 +4,7 @@ #ifndef ESTIMATOR_H #define ESTIMATOR_H +#include #include #include #include @@ -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 _xhat; // The state of estimator, position(3)+velocity(3)+feet position(3x4) + Eigen::Matrix x_hat_; // The state of estimator, position(3)+velocity(3)+feet position(3x4) - Eigen::Matrix _u; // The input of estimator + Eigen::Matrix _u; // The input of estimator - Eigen::Matrix _y; // The measurement value of output y - Eigen::Matrix _yhat; // The prediction of output y - Eigen::Matrix A; // The transtion matrix of estimator - Eigen::Matrix B; // The input matrix - Eigen::Matrix C; // The output matrix + Eigen::Matrix _y; // The measurement value of output y + Eigen::Matrix y_hat_; // The prediction of output y + Eigen::Matrix A; // The transtion matrix of estimator + Eigen::Matrix B; // The input matrix + Eigen::Matrix C; // The output matrix // Covariance Matrix - Eigen::Matrix P; // Prediction covariance - Eigen::Matrix Ppriori; // Priori prediction covariance - Eigen::Matrix Q; // Dynamic simulation covariance - Eigen::Matrix R; // Measurement covariance - Eigen::Matrix QInit_; // Initial value of Dynamic simulation covariance - Eigen::Matrix RInit_; // Initial value of Measurement covariance - Eigen::Matrix Qdig; // adjustable process noise covariance - Eigen::Matrix Cu; // The covariance of system input u + Eigen::Matrix P; // Prediction covariance + Eigen::Matrix Ppriori; // Priori prediction covariance + Eigen::Matrix Q; // Dynamic simulation covariance + Eigen::Matrix R; // Measurement covariance + Eigen::Matrix QInit_; // Initial value of Dynamic simulation covariance + Eigen::Matrix RInit_; // Initial value of Measurement covariance + Eigen::Matrix Qdig; // adjustable process noise covariance + Eigen::Matrix Cu; // The covariance of system input u // Output Measurement - Eigen::Matrix _feetPos2Body; // The feet positions to body, in the global coordinate - Eigen::Matrix _feetVel2Body; // The feet velocity to body, in the global coordinate - Eigen::Matrix _feetH; // The Height of each foot, in the global coordinate + Eigen::Matrix _feetPos2Body; // The feet positions to body, in the global coordinate + Eigen::Matrix _feetVel2Body; // The feet velocity to body, in the global coordinate + Eigen::Matrix _feetH; // The Height of each foot, in the global coordinate - Eigen::Matrix S; // _S = C*P*C.T + R + Eigen::Matrix S; // _S = C*P*C.T + R Eigen::PartialPivLU > Slu; // _S.lu() - Eigen::Matrix Sy; // _Sy = _S.inv() * (y - yhat) - Eigen::Matrix Sc; // _Sc = _S.inv() * C - Eigen::Matrix SR; // _SR = _S.inv() * R - Eigen::Matrix STC; // _STC = (_S.transpose()).inv() * C - Eigen::Matrix IKC; // _IKC = I - KC + Eigen::Matrix Sy; // _Sy = _S.inv() * (y - yhat) + Eigen::Matrix Sc; // _Sc = _S.inv() * C + Eigen::Matrix SR; // _SR = _S.inv() * R + Eigen::Matrix STC; // _STC = (_S.transpose()).inv() * C + Eigen::Matrix IKC; // _IKC = I - KC KDL::Vector g_; double _dt; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h index 611c621..b593547 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h @@ -7,6 +7,7 @@ #define QUADRUPEDROBOT_H #include #include +#include #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; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/RobotLeg.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/RobotLeg.h index dad8626..885dad7 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/RobotLeg.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/RobotLeg.h @@ -11,6 +11,7 @@ #include #include #include +#include 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_; diff --git a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp index 4d9010e..c0b4270 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp @@ -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 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 current_joints = ctrl_comp_.robot_model_.get().current_joint_pos_; for (int i = 0; i < 4; i++) { - std::cout< -#include #include +#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 BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const KDL::Rotation &rotM, - const std::vector &feetPos2B, const std::vector &contact) { +Vec34 BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM, + const std::vector &feetPos2B, const std::vector &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 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 &feetPos2B, const KDL::Rotation &rotM) { +void BalanceCtrl::calMatrixA(const std::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 &contact) { @@ -76,33 +75,33 @@ void BalanceCtrl::calConstraints(const std::vector &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 G, CE, CI; quadprogpp::Vector 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]; } } diff --git a/controllers/unitree_guide_controller/src/control/Estimator.cpp b/controllers/unitree_guide_controller/src/control/Estimator.cpp index fc62055..1c423cc 100644 --- a/controllers/unitree_guide_controller/src/control/Estimator.cpp +++ b/controllers/unitree_guide_controller/src/control/Estimator.cpp @@ -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(); } diff --git a/controllers/unitree_guide_controller/src/quadProgpp/Array.cc b/controllers/unitree_guide_controller/src/quadProgpp/Array.cc index 4045bee..d327c09 100755 --- a/controllers/unitree_guide_controller/src/quadProgpp/Array.cc +++ b/controllers/unitree_guide_controller/src/quadProgpp/Array.cc @@ -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 seq(unsigned int s, unsigned int e) { - std::set tmp; - for (unsigned int i = s; i <= e; i++) tmp.insert(i); - return tmp; - } +std::set seq(unsigned int s, unsigned int e) { + std::set tmp; + for (unsigned int i = s; i <= e; i++) tmp.insert(i); - std::set singleton(unsigned int i) { - std::set tmp; - tmp.insert(i); + return tmp; +} - return tmp; - } -} // namespace quadprogpp +std::set singleton(unsigned int i) { + std::set tmp; + tmp.insert(i); + + return tmp; +} + +} // namespace quadprogpp diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/quadProgpp/Array.hh b/controllers/unitree_guide_controller/src/quadProgpp/Array.hh similarity index 100% rename from controllers/unitree_guide_controller/include/unitree_guide_controller/quadProgpp/Array.hh rename to controllers/unitree_guide_controller/src/quadProgpp/Array.hh diff --git a/controllers/unitree_guide_controller/src/quadProgpp/QuadProg++.cc b/controllers/unitree_guide_controller/src/quadProgpp/QuadProg++.cc index d7fb6c3..692b8d0 100755 --- a/controllers/unitree_guide_controller/src/quadProgpp/QuadProg++.cc +++ b/controllers/unitree_guide_controller/src/quadProgpp/QuadProg++.cc @@ -17,105 +17,106 @@ File $Id: QuadProg++.cc 232 2007-06-21 12:29:00Z digasper $ #include #include #include -#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 &d, const Matrix &J, - const Vector &np); - void update_z(Vector &z, const Matrix &J, - const Vector &d, int iq); +// Utility functions for updating some data needed by the solution method +void compute_d(Vector &d, const Matrix &J, + const Vector &np); - void update_r(const Matrix &R, Vector &r, - const Vector &d, int iq); +void update_z(Vector &z, const Matrix &J, + const Vector &d, int iq); - bool add_constraint(Matrix &R, Matrix &J, Vector &d, - unsigned int &iq, double &rnorm); +void update_r(const Matrix &R, Vector &r, + const Vector &d, int iq); - void delete_constraint(Matrix &R, Matrix &J, Vector &A, - Vector &u, unsigned int n, int p, - unsigned int &iq, int l); +bool add_constraint(Matrix &R, Matrix &J, Vector &d, + unsigned int &iq, double &rnorm); - // Utility functions for computing the Cholesky decomposition and solving - // linear systems - void cholesky_decomposition(Matrix &A); +void delete_constraint(Matrix &R, Matrix &J, Vector &A, + Vector &u, unsigned int n, int p, + unsigned int &iq, int l); - void cholesky_solve(const Matrix &L, Vector &x, - const Vector &b); +// Utility functions for computing the Cholesky decomposition and solving +// linear systems +void cholesky_decomposition(Matrix &A); - void forward_elimination(const Matrix &L, Vector &y, - const Vector &b); +void cholesky_solve(const Matrix &L, Vector &x, + const Vector &b); - void backward_elimination(const Matrix &U, Vector &x, - const Vector &y); +void forward_elimination(const Matrix &L, Vector &y, + const Vector &b); - // Utility functions for computing the scalar product and the euclidean - // distance between two numbers - double scalar_product(const Vector &x, const Vector &y); +void backward_elimination(const Matrix &U, Vector &x, + const Vector &y); - double distance(double a, double b); +// Utility functions for computing the scalar product and the euclidean +// distance between two numbers +double scalar_product(const Vector &x, const Vector &y); - // Utility functions for printing vectors and matrices - void print_matrix(const char *name, const Matrix &A, int n = -1, - int m = -1); +double distance(double a, double b); - template - void print_vector(const char *name, const Vector &v, int n = -1); +// Utility functions for printing vectors and matrices +void print_matrix(const char *name, const Matrix &A, int n = -1, + int m = -1); - // The Solving function, implementing the Goldfarb-Idnani method - double solve_quadprog(Matrix &G, Vector &g0, - const Matrix &CE, const Vector &ce0, - const Matrix &CI, const Vector &ci0, - Vector &x) { - std::ostringstream msg; - unsigned int n = G.ncols(), p = CE.ncols(), m = CI.ncols(); - if (G.nrows() != n) { - msg << "The matrix G is not a squared matrix (" << G.nrows() << " x " - << G.ncols() << ")"; - throw std::logic_error(msg.str()); - } - if (CE.nrows() != n) { - msg << "The matrix CE is incompatible (incorrect number of rows " - << CE.nrows() << " , expecting " << n << ")"; - throw std::logic_error(msg.str()); - } - if (ce0.size() != p) { - msg << "The vector ce0 is incompatible (incorrect dimension " << ce0.size() - << ", expecting " << p << ")"; - throw std::logic_error(msg.str()); - } - if (CI.nrows() != n) { - msg << "The matrix CI is incompatible (incorrect number of rows " - << CI.nrows() << " , expecting " << n << ")"; - throw std::logic_error(msg.str()); - } - if (ci0.size() != m) { - msg << "The vector ci0 is incompatible (incorrect dimension " << ci0.size() - << ", expecting " << m << ")"; - throw std::logic_error(msg.str()); - } - x.resize(n); - unsigned int i, j, k, l; /* indices */ - int ip; // this is the index of the constraint to be added to the active set - Matrix R(n, n), J(n, n); - Vector s(m + p), z(n), r(m + p), d(n), np(n), u(m + p), x_old(n), - u_old(m + p); - double f_value, psi, c1, c2, sum, ss, R_norm; - double inf; - if (std::numeric_limits::has_infinity) - inf = std::numeric_limits::infinity(); - else - inf = 1.0E300; - double t, t1, t2; /* t is the step lenght, which is the minimum of the partial +template +void print_vector(const char *name, const Vector &v, int n = -1); + +// The Solving function, implementing the Goldfarb-Idnani method +double solve_quadprog(Matrix &G, Vector &g0, + const Matrix &CE, const Vector &ce0, + const Matrix &CI, const Vector &ci0, + Vector &x) { + std::ostringstream msg; + unsigned int n = G.ncols(), p = CE.ncols(), m = CI.ncols(); + if (G.nrows() != n) { + msg << "The matrix G is not a squared matrix (" << G.nrows() << " x " + << G.ncols() << ")"; + throw std::logic_error(msg.str()); + } + if (CE.nrows() != n) { + msg << "The matrix CE is incompatible (incorrect number of rows " + << CE.nrows() << " , expecting " << n << ")"; + throw std::logic_error(msg.str()); + } + if (ce0.size() != p) { + msg << "The vector ce0 is incompatible (incorrect dimension " << ce0.size() + << ", expecting " << p << ")"; + throw std::logic_error(msg.str()); + } + if (CI.nrows() != n) { + msg << "The matrix CI is incompatible (incorrect number of rows " + << CI.nrows() << " , expecting " << n << ")"; + throw std::logic_error(msg.str()); + } + if (ci0.size() != m) { + msg << "The vector ci0 is incompatible (incorrect dimension " << ci0.size() + << ", expecting " << m << ")"; + throw std::logic_error(msg.str()); + } + x.resize(n); + unsigned int i, j, k, l; /* indices */ + int ip; // this is the index of the constraint to be added to the active set + Matrix R(n, n), J(n, n); + Vector s(m + p), z(n), r(m + p), d(n), np(n), u(m + p), x_old(n), + u_old(m + p); + double f_value, psi, c1, c2, sum, ss, R_norm; + double inf; + if (std::numeric_limits::has_infinity) + inf = std::numeric_limits::infinity(); + else + inf = 1.0E300; + double t, t1, t2; /* t is the step lenght, which is the minimum of the partial * step length t1 and the full step length t2 */ - Vector A(m + p), A_old(m + p), iai(m + p); - unsigned int iq, iter = 0; - Vector iaexcl(m + p); + Vector A(m + p), A_old(m + p), iai(m + p); + unsigned int iq, iter = 0; + Vector iaexcl(m + p); - /* p is the number of equality constraints */ - /* m is the number of inequality constraints */ + /* p is the number of equality constraints */ + /* m is the number of inequality constraints */ #ifdef TRACE_SOLVER std::cout << std::endl << "Starting solve_quadprog" << std::endl; print_matrix("G", G); @@ -126,64 +127,64 @@ namespace quadprogpp { print_vector("ci0", ci0); #endif - /* - * Preprocessing phase - */ + /* + * Preprocessing phase + */ - /* compute the trace of the original matrix G */ - c1 = 0.0; - for (i = 0; i < n; i++) { - c1 += G[i][i]; - } - /* decompose the matrix G in the form L^T L */ - cholesky_decomposition(G); + /* compute the trace of the original matrix G */ + c1 = 0.0; + for (i = 0; i < n; i++) { + c1 += G[i][i]; + } + /* decompose the matrix G in the form L^T L */ + cholesky_decomposition(G); #ifdef TRACE_SOLVER print_matrix("G", G); #endif - /* initialize the matrix R */ - for (i = 0; i < n; i++) { - d[i] = 0.0; - for (j = 0; j < n; j++) R[i][j] = 0.0; - } - R_norm = 1.0; /* this variable will hold the norm of the matrix R */ + /* initialize the matrix R */ + for (i = 0; i < n; i++) { + d[i] = 0.0; + for (j = 0; j < n; j++) R[i][j] = 0.0; + } + R_norm = 1.0; /* this variable will hold the norm of the matrix R */ - /* compute the inverse of the factorized matrix G^-1, this is the initial - * value for H */ - c2 = 0.0; - for (i = 0; i < n; i++) { - d[i] = 1.0; - forward_elimination(G, z, d); - for (j = 0; j < n; j++) J[i][j] = z[j]; - c2 += z[i]; - d[i] = 0.0; - } + /* compute the inverse of the factorized matrix G^-1, this is the initial + * value for H */ + c2 = 0.0; + for (i = 0; i < n; i++) { + d[i] = 1.0; + forward_elimination(G, z, d); + for (j = 0; j < n; j++) J[i][j] = z[j]; + c2 += z[i]; + d[i] = 0.0; + } #ifdef TRACE_SOLVER print_matrix("J", J); #endif - /* c1 * c2 is an estimate for cond(G) */ + /* c1 * c2 is an estimate for cond(G) */ - /* - * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x - * this is a feasible point in the dual space - * x = G^-1 * g0 - */ - cholesky_solve(G, x, g0); - for (i = 0; i < n; i++) x[i] = -x[i]; - /* and compute the current solution value */ - f_value = 0.5 * scalar_product(g0, x); + /* + * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x + * this is a feasible point in the dual space + * x = G^-1 * g0 + */ + cholesky_solve(G, x, g0); + for (i = 0; i < n; i++) x[i] = -x[i]; + /* and compute the current solution value */ + f_value = 0.5 * scalar_product(g0, x); #ifdef TRACE_SOLVER std::cout << "Unconstrained solution: " << f_value << std::endl; print_vector("x", x); #endif - /* Add equality constraints to the working set A */ - iq = 0; - for (i = 0; i < p; i++) { - for (j = 0; j < n; j++) np[j] = CE[j][i]; - compute_d(d, J, np); - update_z(z, J, d, iq); - update_r(R, r, d, iq); + /* Add equality constraints to the working set A */ + iq = 0; + for (i = 0; i < p; i++) { + for (j = 0; j < n; j++) np[j] = CE[j][i]; + compute_d(d, J, np); + update_z(z, J, d, iq); + update_r(R, r, d, iq); #ifdef TRACE_SOLVER print_matrix("R", R, n, iq); print_vector("z", z); @@ -191,106 +192,106 @@ namespace quadprogpp { print_vector("d", d); #endif - /* compute full step length t2: i.e., the minimum step in primal space s.t. - the contraint becomes feasible */ - t2 = 0.0; - if (fabs(scalar_product(z, z)) > - std::numeric_limits::epsilon()) // i.e. z != 0 - t2 = (-scalar_product(np, x) - ce0[i]) / scalar_product(z, np); + /* compute full step length t2: i.e., the minimum step in primal space s.t. + the contraint becomes feasible */ + t2 = 0.0; + if (fabs(scalar_product(z, z)) > + std::numeric_limits::epsilon()) // i.e. z != 0 + t2 = (-scalar_product(np, x) - ce0[i]) / scalar_product(z, np); - /* set x = x + t2 * z */ - for (k = 0; k < n; k++) x[k] += t2 * z[k]; + /* set x = x + t2 * z */ + for (k = 0; k < n; k++) x[k] += t2 * z[k]; - /* set u = u+ */ - u[iq] = t2; - for (k = 0; k < iq; k++) u[k] -= t2 * r[k]; + /* set u = u+ */ + u[iq] = t2; + for (k = 0; k < iq; k++) u[k] -= t2 * r[k]; - /* compute the new solution value */ - f_value += 0.5 * (t2 * t2) * scalar_product(z, np); - A[i] = -i - 1; + /* compute the new solution value */ + f_value += 0.5 * (t2 * t2) * scalar_product(z, np); + A[i] = -i - 1; - if (!add_constraint(R, J, d, iq, R_norm)) { - // Equality constraints are linearly dependent - throw std::runtime_error("Constraints are linearly dependent"); - return f_value; - } - } + if (!add_constraint(R, J, d, iq, R_norm)) { + // Equality constraints are linearly dependent + throw std::runtime_error("Constraints are linearly dependent"); + return f_value; + } + } - /* set iai = K \ A */ - for (i = 0; i < m; i++) iai[i] = i; + /* set iai = K \ A */ + for (i = 0; i < m; i++) iai[i] = i; - l1: - iter++; +l1: + iter++; #ifdef TRACE_SOLVER print_vector("x", x); #endif - /* step 1: choose a violated constraint */ - for (i = p; i < iq; i++) { - ip = A[i]; - iai[ip] = -1; - } + /* step 1: choose a violated constraint */ + for (i = p; i < iq; i++) { + ip = A[i]; + iai[ip] = -1; + } - /* compute s[x] = ci^T * x + ci0 for all elements of K \ A */ - ss = 0.0; - psi = 0.0; /* this value will contain the sum of all infeasibilities */ - ip = 0; /* ip will be the index of the chosen violated constraint */ - for (i = 0; i < m; i++) { - iaexcl[i] = true; - sum = 0.0; - for (j = 0; j < n; j++) sum += CI[j][i] * x[j]; - sum += ci0[i]; - s[i] = sum; - psi += std::min(0.0, sum); - } + /* compute s[x] = ci^T * x + ci0 for all elements of K \ A */ + ss = 0.0; + psi = 0.0; /* this value will contain the sum of all infeasibilities */ + ip = 0; /* ip will be the index of the chosen violated constraint */ + for (i = 0; i < m; i++) { + iaexcl[i] = true; + sum = 0.0; + for (j = 0; j < n; j++) sum += CI[j][i] * x[j]; + sum += ci0[i]; + s[i] = sum; + psi += std::min(0.0, sum); + } #ifdef TRACE_SOLVER print_vector("s", s, m); #endif - if (fabs(psi) <= - m * std::numeric_limits::epsilon() * c1 * c2 * 100.0) { - /* numerically there are not infeasibilities anymore */ - return f_value; - } + if (fabs(psi) <= + m * std::numeric_limits::epsilon() * c1 * c2 * 100.0) { + /* numerically there are not infeasibilities anymore */ + return f_value; + } - /* save old values for u and A */ - for (i = 0; i < iq; i++) { - u_old[i] = u[i]; - A_old[i] = A[i]; - } - /* and for x */ - for (i = 0; i < n; i++) x_old[i] = x[i]; + /* save old values for u and A */ + for (i = 0; i < iq; i++) { + u_old[i] = u[i]; + A_old[i] = A[i]; + } + /* 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 */ - for (i = 0; i < m; i++) { - if (s[i] < ss && iai[i] != -1 && iaexcl[i]) { - ss = s[i]; - ip = i; - } - } - if (ss >= 0.0) { - return f_value; - } +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]; + ip = i; + } + } + if (ss >= 0.0) { + return f_value; + } - /* set np = n[ip] */ - for (i = 0; i < n; i++) np[i] = CI[i][ip]; - /* set u = [u 0]^T */ - u[iq] = 0.0; - /* add ip to the active set A */ - A[iq] = ip; + /* set np = n[ip] */ + for (i = 0; i < n; i++) np[i] = CI[i][ip]; + /* set u = [u 0]^T */ + u[iq] = 0.0; + /* add ip to the active set A */ + A[iq] = ip; #ifdef TRACE_SOLVER std::cout << "Trying with constraint " << ip << std::endl; print_vector("np", np); #endif - 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); - update_z(z, J, d, iq); - /* compute N* np (if q > 0): the negative of the step direction in the dual - * space */ - update_r(R, r, d, iq); +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); + update_z(z, J, d, iq); + /* compute N* np (if q > 0): the negative of the step direction in the dual + * space */ + update_r(R, r, d, iq); #ifdef TRACE_SOLVER std::cout << "Step direction z" << std::endl; print_vector("z", z); @@ -300,72 +301,72 @@ namespace quadprogpp { print_vector("A", A, iq + 1); #endif - /* Step 2b: compute step length */ - l = 0; - /* Compute t1: partial step length (maximum step in dual space without - * violating dual feasibility */ - t1 = inf; /* +inf */ - /* find the index l s.t. it reaches the minimum of u+[x] / r */ - for (k = p; k < iq; k++) { - if (r[k] > 0.0) { - if (u[k] / r[k] < t1) { - t1 = u[k] / r[k]; - l = A[k]; - } - } - } - /* Compute t2: full step length (minimum step in primal space such that the - * constraint ip becomes feasible */ - if (fabs(scalar_product(z, z)) > - std::numeric_limits::epsilon()) // i.e. z != 0 - { - t2 = -s[ip] / scalar_product(z, np); - if (t2 < 0) // patch suggested by Takano Akio for handling numerical - // inconsistencies - t2 = inf; - } else - t2 = inf; /* +inf */ + /* Step 2b: compute step length */ + l = 0; + /* Compute t1: partial step length (maximum step in dual space without + * violating dual feasibility */ + t1 = inf; /* +inf */ + /* find the index l s.t. it reaches the minimum of u+[x] / r */ + for (k = p; k < iq; k++) { + if (r[k] > 0.0) { + if (u[k] / r[k] < t1) { + t1 = u[k] / r[k]; + l = A[k]; + } + } + } + /* Compute t2: full step length (minimum step in primal space such that the + * constraint ip becomes feasible */ + if (fabs(scalar_product(z, z)) > + std::numeric_limits::epsilon()) // i.e. z != 0 + { + t2 = -s[ip] / scalar_product(z, np); + if (t2 < 0) // patch suggested by Takano Akio for handling numerical + // inconsistencies + t2 = inf; + } else + t2 = inf; /* +inf */ - /* the step is chosen as the minimum of t1 and t2 */ - t = std::min(t1, t2); + /* the step is chosen as the minimum of t1 and t2 */ + t = std::min(t1, t2); #ifdef TRACE_SOLVER std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") "; #endif - /* Step 2c: determine new S-pair and take step: */ + /* Step 2c: determine new S-pair and take step: */ - /* case (i): no step in primal or dual space */ - if (t >= inf) { - /* QPP is infeasible */ - // FIXME: unbounded to raise - return inf; - } - /* case (ii): step in dual space */ - if (t2 >= inf) { - /* set u = u + t * [-r 1] and drop constraint l from the active set A */ - for (k = 0; k < iq; k++) u[k] -= t * r[k]; - u[iq] += t; - iai[l] = l; - delete_constraint(R, J, A, u, n, p, iq, l); + /* case (i): no step in primal or dual space */ + if (t >= inf) { + /* QPP is infeasible */ + // FIXME: unbounded to raise + return inf; + } + /* case (ii): step in dual space */ + if (t2 >= inf) { + /* set u = u + t * [-r 1] and drop constraint l from the active set A */ + for (k = 0; k < iq; k++) u[k] -= t * r[k]; + u[iq] += t; + iai[l] = l; + delete_constraint(R, J, A, u, n, p, iq, l); #ifdef TRACE_SOLVER std::cout << " in dual space: " << f_value << std::endl; print_vector("x", x); print_vector("z", z); print_vector("A", A, iq + 1); #endif - goto l2a; - } + goto l2a; + } - /* case (iii): step in primal and dual space */ + /* case (iii): step in primal and dual space */ - /* set x = x + t * z */ - for (k = 0; k < n; k++) x[k] += t * z[k]; - /* update the solution value */ - f_value += t * scalar_product(z, np) * (0.5 * t + u[iq]); - /* u = u + t * [-r 1] */ - for (k = 0; k < iq; k++) u[k] -= t * r[k]; - u[iq] += t; + /* set x = x + t * z */ + for (k = 0; k < n; k++) x[k] += t * z[k]; + /* update the solution value */ + f_value += t * scalar_product(z, np) * (0.5 * t + u[iq]); + /* u = u + t * [-r 1] */ + for (k = 0; k < iq; k++) u[k] -= t * r[k]; + u[iq] += t; #ifdef TRACE_SOLVER std::cout << " in both spaces: " << f_value << std::endl; print_vector("x", x); @@ -374,150 +375,150 @@ namespace quadprogpp { print_vector("A", A, iq + 1); #endif - if (fabs(t - t2) < std::numeric_limits::epsilon()) { + if (fabs(t - t2) < std::numeric_limits::epsilon()) { #ifdef TRACE_SOLVER std::cout << "Full step has taken " << t << std::endl; print_vector("x", x); #endif - /* full step has taken */ - /* add constraint ip to the active set*/ - if (!add_constraint(R, J, d, iq, R_norm)) { - iaexcl[ip] = false; - delete_constraint(R, J, A, u, n, p, iq, ip); + /* full step has taken */ + /* add constraint ip to the active set*/ + if (!add_constraint(R, J, d, iq, R_norm)) { + iaexcl[ip] = false; + delete_constraint(R, J, A, u, n, p, iq, ip); #ifdef TRACE_SOLVER print_matrix("R", R); print_vector("A", A, iq); print_vector("iai", iai); #endif - for (i = 0; i < m; i++) iai[i] = i; - for (i = p; i < iq; i++) { - A[i] = A_old[i]; - u[i] = u_old[i]; - iai[A[i]] = -1; - } - for (i = 0; i < n; i++) x[i] = x_old[i]; - goto l2; /* go to step 2 */ - } else - iai[ip] = -1; + for (i = 0; i < m; i++) iai[i] = i; + for (i = p; i < iq; i++) { + A[i] = A_old[i]; + u[i] = u_old[i]; + iai[A[i]] = -1; + } + for (i = 0; i < n; i++) x[i] = x_old[i]; + goto l2; /* go to step 2 */ + } else + iai[ip] = -1; #ifdef TRACE_SOLVER print_matrix("R", R); print_vector("A", A, iq); print_vector("iai", iai); #endif - goto l1; - } + goto l1; + } - /* a patial step has taken */ + /* a patial step has taken */ #ifdef TRACE_SOLVER std::cout << "Partial step has taken " << t << std::endl; print_vector("x", x); #endif - /* drop constraint l */ - iai[l] = l; - delete_constraint(R, J, A, u, n, p, iq, l); + /* drop constraint l */ + iai[l] = l; + delete_constraint(R, J, A, u, n, p, iq, l); #ifdef TRACE_SOLVER print_matrix("R", R); print_vector("A", A, iq); #endif - /* update s[ip] = CI * x + ci0 */ - sum = 0.0; - for (k = 0; k < n; k++) sum += CI[k][ip] * x[k]; - s[ip] = sum + ci0[ip]; + /* update s[ip] = CI * x + ci0 */ + sum = 0.0; + for (k = 0; k < n; k++) sum += CI[k][ip] * x[k]; + s[ip] = sum + ci0[ip]; #ifdef TRACE_SOLVER print_vector("s", s, m); #endif - goto l2a; - } + goto l2a; +} - inline void compute_d(Vector &d, const Matrix &J, - const Vector &np) { - int i, j, n = d.size(); - double sum; +inline void compute_d(Vector &d, const Matrix &J, + const Vector &np) { + int i, j, n = d.size(); + double sum; - /* compute d = H^T * np */ - for (i = 0; i < n; i++) { - sum = 0.0; - for (j = 0; j < n; j++) sum += J[j][i] * np[j]; - d[i] = sum; - } - } + /* compute d = H^T * np */ + for (i = 0; i < n; i++) { + sum = 0.0; + for (j = 0; j < n; j++) sum += J[j][i] * np[j]; + d[i] = sum; + } +} - inline void update_z(Vector &z, const Matrix &J, - const Vector &d, int iq) { - int i, j, n = z.size(); +inline void update_z(Vector &z, const Matrix &J, + const Vector &d, int iq) { + int i, j, n = z.size(); - /* setting of z = H * d */ - for (i = 0; i < n; i++) { - z[i] = 0.0; - for (j = iq; j < n; j++) z[i] += J[i][j] * d[j]; - } - } + /* setting of z = H * d */ + for (i = 0; i < n; i++) { + z[i] = 0.0; + for (j = iq; j < n; j++) z[i] += J[i][j] * d[j]; + } +} - inline void update_r(const Matrix &R, Vector &r, - const Vector &d, int iq) { - int i, j; - double sum; +inline void update_r(const Matrix &R, Vector &r, + const Vector &d, int iq) { + int i, j; + double sum; - /* setting of r = R^-1 d */ - for (i = iq - 1; i >= 0; i--) { - sum = 0.0; - for (j = i + 1; j < iq; j++) sum += R[i][j] * r[j]; - r[i] = (d[i] - sum) / R[i][i]; - } - } + /* setting of r = R^-1 d */ + for (i = iq - 1; i >= 0; i--) { + sum = 0.0; + 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 &R, Matrix &J, Vector &d, - unsigned int &iq, double &R_norm) { - unsigned int n = d.size(); +bool add_constraint(Matrix &R, Matrix &J, Vector &d, + unsigned int &iq, double &R_norm) { + unsigned int n = d.size(); #ifdef TRACE_SOLVER std::cout << "Add constraint " << iq << '/'; #endif - unsigned int i, j, k; - double cc, ss, h, t1, t2, xny; + unsigned int i, j, k; + double cc, ss, h, t1, t2, xny; - /* we have to find the Givens rotation which will reduce the element - d[j] to zero. - if it is already zero we don't have to do anything, except of - decreasing j */ - for (j = n - 1; j >= iq + 1; j--) { - /* The Givens rotation is done with the matrix (cc cs, cs -cc). - If cc is one, then element (j) of d is zero compared with element - (j - 1). Hence we don't have to do anything. - If cc is zero, then we just have to switch column (j) and column (j - 1) - of J. Since we only switch columns in J, we have to be careful how we - update d depending on the sign of gs. - Otherwise we have to apply the Givens rotation to these columns. - The i - 1 element of d has to be updated to h. */ - cc = d[j - 1]; - ss = d[j]; - h = distance(cc, ss); - if (fabs(h) < std::numeric_limits::epsilon()) // h == 0 - continue; - d[j] = 0.0; - ss = ss / h; - cc = cc / h; - if (cc < 0.0) { - cc = -cc; - ss = -ss; - d[j - 1] = -h; - } else - d[j - 1] = h; - xny = ss / (1.0 + cc); - for (k = 0; k < n; k++) { - t1 = J[k][j - 1]; - t2 = J[k][j]; - J[k][j - 1] = t1 * cc + t2 * ss; - J[k][j] = xny * (t1 + J[k][j - 1]) - t2; - } - } - /* update the number of constraints added*/ - iq++; - /* To update R we have to put the iq components of the d vector - into column iq - 1 of R - */ - for (i = 0; i < iq; i++) R[i][iq - 1] = d[i]; + /* we have to find the Givens rotation which will reduce the element + d[j] to zero. + if it is already zero we don't have to do anything, except of + decreasing j */ + for (j = n - 1; j >= iq + 1; j--) { + /* The Givens rotation is done with the matrix (cc cs, cs -cc). + If cc is one, then element (j) of d is zero compared with element + (j - 1). Hence we don't have to do anything. + If cc is zero, then we just have to switch column (j) and column (j - 1) + of J. Since we only switch columns in J, we have to be careful how we + update d depending on the sign of gs. + Otherwise we have to apply the Givens rotation to these columns. + The i - 1 element of d has to be updated to h. */ + cc = d[j - 1]; + ss = d[j]; + h = distance(cc, ss); + if (fabs(h) < std::numeric_limits::epsilon()) // h == 0 + continue; + d[j] = 0.0; + ss = ss / h; + cc = cc / h; + if (cc < 0.0) { + cc = -cc; + ss = -ss; + d[j - 1] = -h; + } else + d[j - 1] = h; + xny = ss / (1.0 + cc); + for (k = 0; k < n; k++) { + t1 = J[k][j - 1]; + t2 = J[k][j]; + J[k][j - 1] = t1 * cc + t2 * ss; + J[k][j] = xny * (t1 + J[k][j - 1]) - t2; + } + } + /* update the number of constraints added*/ + iq++; + /* To update R we have to put the iq components of the d vector + into column iq - 1 of R + */ + for (i = 0; i < iq; i++) R[i][iq - 1] = d[i]; #ifdef TRACE_SOLVER std::cout << iq << std::endl; print_matrix("R", R, iq, iq); @@ -525,205 +526,206 @@ namespace quadprogpp { print_vector("d", d, iq); #endif - if (fabs(d[iq - 1]) <= std::numeric_limits::epsilon() * R_norm) { - // problem degenerate - return false; - } - R_norm = std::max(R_norm, fabs(d[iq - 1])); - return true; - } + if (fabs(d[iq - 1]) <= std::numeric_limits::epsilon() * R_norm) { + // problem degenerate + return false; + } + R_norm = std::max(R_norm, fabs(d[iq - 1])); + return true; +} - void delete_constraint(Matrix &R, Matrix &J, Vector &A, - Vector &u, unsigned int n, int p, - unsigned int &iq, int l) { +void delete_constraint(Matrix &R, Matrix &J, Vector &A, + Vector &u, unsigned int n, int p, + unsigned int &iq, int l) { #ifdef TRACE_SOLVER std::cout << "Delete constraint " << l << ' ' << iq; #endif - unsigned int i, j, k, - qq = 0; // just to prevent warnings from smart compilers - double cc, ss, h, xny, t1, t2; + unsigned int i, j, k, + qq = 0; // just to prevent warnings from smart compilers + double cc, ss, h, xny, t1, t2; - bool found = false; - /* Find the index qq for active constraint l to be removed */ - for (i = p; i < iq; i++) - if (A[i] == l) { - qq = i; - found = true; - break; - } + bool found = false; + /* Find the index qq for active constraint l to be removed */ + for (i = p; i < iq; i++) + if (A[i] == l) { + qq = i; + found = true; + break; + } - if (!found) { - std::ostringstream os; - os << "Attempt to delete non existing constraint, constraint: " << l; - throw std::invalid_argument(os.str()); - } - /* remove the constraint from the active set and the duals */ - for (i = qq; i < iq - 1; i++) { - A[i] = A[i + 1]; - u[i] = u[i + 1]; - for (j = 0; j < n; j++) R[j][i] = R[j][i + 1]; - } + if (!found) { + std::ostringstream os; + os << "Attempt to delete non existing constraint, constraint: " << l; + throw std::invalid_argument(os.str()); + } + /* remove the constraint from the active set and the duals */ + for (i = qq; i < iq - 1; i++) { + A[i] = A[i + 1]; + u[i] = u[i + 1]; + for (j = 0; j < n; j++) R[j][i] = R[j][i + 1]; + } - A[iq - 1] = A[iq]; - u[iq - 1] = u[iq]; - A[iq] = 0; - u[iq] = 0.0; - for (j = 0; j < iq; j++) R[j][iq - 1] = 0.0; - /* constraint has been fully removed */ - iq--; + A[iq - 1] = A[iq]; + u[iq - 1] = u[iq]; + A[iq] = 0; + u[iq] = 0.0; + for (j = 0; j < iq; j++) R[j][iq - 1] = 0.0; + /* constraint has been fully removed */ + iq--; #ifdef TRACE_SOLVER std::cout << '/' << iq << std::endl; #endif - if (iq == 0) return; + if (iq == 0) return; - for (j = qq; j < iq; j++) { - cc = R[j][j]; - ss = R[j + 1][j]; - h = distance(cc, ss); - if (fabs(h) < std::numeric_limits::epsilon()) // h == 0 - continue; - cc = cc / h; - ss = ss / h; - R[j + 1][j] = 0.0; - if (cc < 0.0) { - R[j][j] = -h; - cc = -cc; - ss = -ss; - } else - R[j][j] = h; + for (j = qq; j < iq; j++) { + cc = R[j][j]; + ss = R[j + 1][j]; + h = distance(cc, ss); + if (fabs(h) < std::numeric_limits::epsilon()) // h == 0 + continue; + cc = cc / h; + ss = ss / h; + R[j + 1][j] = 0.0; + if (cc < 0.0) { + R[j][j] = -h; + cc = -cc; + ss = -ss; + } else + R[j][j] = h; - xny = ss / (1.0 + cc); - for (k = j + 1; k < iq; k++) { - t1 = R[j][k]; - t2 = R[j + 1][k]; - R[j][k] = t1 * cc + t2 * ss; - R[j + 1][k] = xny * (t1 + R[j][k]) - t2; - } - for (k = 0; k < n; k++) { - t1 = J[k][j]; - t2 = J[k][j + 1]; - J[k][j] = t1 * cc + t2 * ss; - J[k][j + 1] = xny * (J[k][j] + t1) - t2; - } + xny = ss / (1.0 + cc); + for (k = j + 1; k < iq; k++) { + t1 = R[j][k]; + t2 = R[j + 1][k]; + R[j][k] = t1 * cc + t2 * ss; + R[j + 1][k] = xny * (t1 + R[j][k]) - t2; + } + for (k = 0; k < n; k++) { + t1 = J[k][j]; + t2 = J[k][j + 1]; + J[k][j] = t1 * cc + t2 * ss; + J[k][j + 1] = xny * (J[k][j] + t1) - t2; + } + } +} + +inline double distance(double a, double b) { + double a1, b1, t; + a1 = fabs(a); + b1 = fabs(b); + if (a1 > b1) { + t = (b1 / a1); + return a1 * sqrt(1.0 + t * t); + } else if (b1 > a1) { + t = (a1 / b1); + return b1 * sqrt(1.0 + t * t); + } + return a1 * sqrt(2.0); +} + +inline double scalar_product(const Vector &x, const Vector &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 &A) { + int i, j, k, n = A.nrows(); + double sum; + + for (i = 0; i < n; i++) { + for (j = i; j < n; j++) { + sum = A[i][j]; + for (k = i - 1; k >= 0; k--) sum -= A[i][k] * A[j][k]; + if (i == j) { + if (sum <= 0.0) { + std::ostringstream os; + // raise error + print_matrix("A", A); + os << "Error in cholesky decomposition, sum: " << sum; + throw std::logic_error(os.str()); + exit(-1); } + A[i][i] = sqrt(sum); + } else + A[j][i] = sum / A[i][i]; } + for (k = i + 1; k < n; k++) A[i][k] = A[k][i]; + } +} - inline double distance(double a, double b) { - double a1, b1, t; - a1 = fabs(a); - b1 = fabs(b); - if (a1 > b1) { - t = (b1 / a1); - return a1 * sqrt(1.0 + t * t); - } else if (b1 > a1) { - t = (a1 / b1); - return b1 * sqrt(1.0 + t * t); - } - return a1 * sqrt(2.0); - } +void cholesky_solve(const Matrix &L, Vector &x, + const Vector &b) { + int n = L.nrows(); + Vector y(n); - inline double scalar_product(const Vector &x, const Vector &y) { - int i, n = x.size(); - double sum; + /* Solve L * y = b */ + forward_elimination(L, y, b); + /* Solve L^T * x = y */ + backward_elimination(L, x, y); +} - sum = 0.0; - for (i = 0; i < n; i++) sum += x[i] * y[i]; - return sum; - } +inline void forward_elimination(const Matrix &L, Vector &y, + const Vector &b) { + int i, j, n = L.nrows(); - void cholesky_decomposition(Matrix &A) { - int i, j, k, n = A.nrows(); - double sum; + y[0] = b[0] / L[0][0]; + for (i = 1; i < n; i++) { + y[i] = b[i]; + for (j = 0; j < i; j++) y[i] -= L[i][j] * y[j]; + y[i] = y[i] / L[i][i]; + } +} - for (i = 0; i < n; i++) { - for (j = i; j < n; j++) { - sum = A[i][j]; - for (k = i - 1; k >= 0; k--) sum -= A[i][k] * A[j][k]; - if (i == j) { - if (sum <= 0.0) { - std::ostringstream os; - // raise error - print_matrix("A", A); - os << "Error in cholesky decomposition, sum: " << sum; - throw std::logic_error(os.str()); - exit(-1); - } - A[i][i] = sqrt(sum); - } else - A[j][i] = sum / A[i][i]; - } - for (k = i + 1; k < n; k++) A[i][k] = A[k][i]; - } - } +inline void backward_elimination(const Matrix &U, Vector &x, + const Vector &y) { + int i, j, n = U.nrows(); - void cholesky_solve(const Matrix &L, Vector &x, - const Vector &b) { - int n = L.nrows(); - Vector y(n); + x[n - 1] = y[n - 1] / U[n - 1][n - 1]; + for (i = n - 2; i >= 0; i--) { + x[i] = y[i]; + for (j = i + 1; j < n; j++) x[i] -= U[i][j] * x[j]; + x[i] = x[i] / U[i][i]; + } +} - /* Solve L * y = b */ - forward_elimination(L, y, b); - /* Solve L^T * x = y */ - backward_elimination(L, x, y); - } +void print_matrix(const char *name, const Matrix &A, int n, int m) { + std::ostringstream s; + std::string t; + if (n == -1) n = A.nrows(); + if (m == -1) m = A.ncols(); - inline void forward_elimination(const Matrix &L, Vector &y, - const Vector &b) { - int i, j, n = L.nrows(); + s << name << ": " << std::endl; + for (int i = 0; i < n; i++) { + s << " "; + for (int j = 0; j < m; j++) s << A[i][j] << ", "; + s << std::endl; + } + t = s.str(); + t = t.substr( + 0, t.size() - 3); // To remove the trailing space, comma and newline - y[0] = b[0] / L[0][0]; - for (i = 1; i < n; i++) { - y[i] = b[i]; - for (j = 0; j < i; j++) y[i] -= L[i][j] * y[j]; - y[i] = y[i] / L[i][i]; - } - } + std::cout << t << std::endl; +} - inline void backward_elimination(const Matrix &U, Vector &x, - const Vector &y) { - int i, j, n = U.nrows(); +template +void print_vector(const char *name, const Vector &v, int n) { + std::ostringstream s; + std::string t; + if (n == -1) n = v.size(); - x[n - 1] = y[n - 1] / U[n - 1][n - 1]; - for (i = n - 2; i >= 0; i--) { - x[i] = y[i]; - for (j = i + 1; j < n; j++) x[i] -= U[i][j] * x[j]; - x[i] = x[i] / U[i][i]; - } - } + s << name << ": " << std::endl << " "; + for (int i = 0; i < n; i++) { + s << v[i] << ", "; + } + t = s.str(); + t = t.substr(0, t.size() - 2); // To remove the trailing space and comma - void print_matrix(const char *name, const Matrix &A, int n, int m) { - std::ostringstream s; - std::string t; - if (n == -1) n = A.nrows(); - if (m == -1) m = A.ncols(); + std::cout << t << std::endl; +} - s << name << ": " << std::endl; - for (int i = 0; i < n; i++) { - s << " "; - for (int j = 0; j < m; j++) s << A[i][j] << ", "; - s << std::endl; - } - t = s.str(); - t = t.substr( - 0, t.size() - 3); // To remove the trailing space, comma and newline - - std::cout << t << std::endl; - } - - template - void print_vector(const char *name, const Vector &v, int n) { - std::ostringstream s; - std::string t; - if (n == -1) n = v.size(); - - s << name << ": " << std::endl << " "; - for (int i = 0; i < n; i++) { - s << v[i] << ", "; - } - t = s.str(); - t = t.substr(0, t.size() - 2); // To remove the trailing space and comma - - std::cout << t << std::endl; - } -} // namespace quadprogpp +} // namespace quadprogpp diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/quadProgpp/QuadProg++.hh b/controllers/unitree_guide_controller/src/quadProgpp/QuadProg++.hh similarity index 100% rename from controllers/unitree_guide_controller/include/unitree_guide_controller/quadProgpp/QuadProg++.hh rename to controllers/unitree_guide_controller/src/quadProgpp/QuadProg++.hh diff --git a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp index 022e1d0..5b9e87c 100644 --- a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp +++ b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp @@ -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 jacobian = getJacobian(index).data.topRows(3); Eigen::VectorXd foot_velocity = jacobian * current_joint_vel_[index].data; diff --git a/controllers/unitree_guide_controller/src/robot/RobotLeg.cpp b/controllers/unitree_guide_controller/src/robot/RobotLeg.cpp index d9f06e6..331d97f 100644 --- a/controllers/unitree_guide_controller/src/robot/RobotLeg.cpp +++ b/controllers/unitree_guide_controller/src/robot/RobotLeg.cpp @@ -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 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);