trot but not stable
This commit is contained in:
Huang Zhenbiao
parent
da5be83eda
commit
f0a5257852
|
|
@ -34,6 +34,7 @@ add_library(${PROJECT_NAME} SHARED
|
|||
src/FSM/StateSwingTest.cpp
|
||||
src/FSM/StateFreeStand.cpp
|
||||
src/FSM/StateBalanceTest.cpp
|
||||
src/FSM/StateTrotting.cpp
|
||||
|
||||
src/robot/QuadrupedRobot.cpp
|
||||
src/robot/RobotLeg.cpp
|
||||
|
|
@ -46,7 +47,7 @@ add_library(${PROJECT_NAME} SHARED
|
|||
src/quadProgpp/QuadProg++.cc
|
||||
|
||||
src/gait/WaveGenerator.cpp
|
||||
src/gait/FootEndCtrl.cpp
|
||||
src/gait/FeetEndCtrl.cpp
|
||||
src/gait/GaitGenerator.cpp
|
||||
|
||||
)
|
||||
|
|
|
|||
|
|
@ -22,9 +22,10 @@ public:
|
|||
private:
|
||||
void calcTorque();
|
||||
|
||||
Estimator& estimator_;
|
||||
QuadrupedRobot& robot_model_;
|
||||
BalanceCtrl& balance_ctrl_;
|
||||
Estimator &estimator_;
|
||||
QuadrupedRobot &robot_model_;
|
||||
BalanceCtrl &balance_ctrl_;
|
||||
WaveGenerator &wave_generator_;
|
||||
|
||||
Vec3 pcd_, pcd_init_;
|
||||
RotMat Rd_;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,81 @@
|
|||
//
|
||||
// Created by tlab-uav on 24-9-18.
|
||||
//
|
||||
|
||||
#ifndef STATETROTTING_H
|
||||
#define STATETROTTING_H
|
||||
#include <unitree_guide_controller/gait/GaitGenerator.h>
|
||||
|
||||
#include "FSMState.h"
|
||||
|
||||
|
||||
class StateTrotting final : public FSMState {
|
||||
public:
|
||||
explicit StateTrotting(CtrlComponent &ctrlComp);
|
||||
|
||||
void enter() override;
|
||||
|
||||
void run() override;
|
||||
|
||||
void exit() override;
|
||||
|
||||
FSMStateName checkChange() override;
|
||||
|
||||
private:
|
||||
void getUserCmd();
|
||||
|
||||
void calcCmd();
|
||||
|
||||
void calcTau();
|
||||
|
||||
void calcQQd();
|
||||
|
||||
void calcGain() const;
|
||||
|
||||
/**
|
||||
* Check whether the robot should take a step or not
|
||||
* @return
|
||||
*/
|
||||
bool checkStepOrNot();
|
||||
|
||||
Estimator &estimator_;
|
||||
QuadrupedRobot &robot_model_;
|
||||
BalanceCtrl &balance_ctrl_;
|
||||
WaveGenerator &wave_generator_;
|
||||
|
||||
GaitGenerator gait_generator_;
|
||||
|
||||
// Robot State
|
||||
Vec3 _posBody, _velBody;
|
||||
double _yaw{}, _dYaw{};
|
||||
Vec34 _posFeetGlobal, _velFeetGlobal;
|
||||
Vec34 _posFeet2BGlobal;
|
||||
RotMat B2G_RotMat, G2B_RotMat;
|
||||
Vec12 _q;
|
||||
|
||||
|
||||
// Robot command
|
||||
Vec3 _pcd;
|
||||
Vec3 _vCmdGlobal, _vCmdBody;
|
||||
double dt_;
|
||||
double _yawCmd{}, _dYawCmd{};
|
||||
double _dYawCmdPast{};
|
||||
Vec3 _wCmdGlobal;
|
||||
Vec34 _posFeetGlobalGoal, _velFeetGlobalGoal;
|
||||
Vec34 _posFeet2BGoal, _velFeet2BGoal;
|
||||
RotMat Rd;
|
||||
Vec3 _ddPcd, _dWbd;
|
||||
Vec34 force_feet_global_, force_feet_body_;
|
||||
Vec12 _tau;
|
||||
|
||||
// Control Parameters
|
||||
double _gaitHeight;
|
||||
Vec3 pos_error_, vel_error_;
|
||||
Mat3 Kpp, Kdp, Kdw;
|
||||
double _kpw;
|
||||
Mat3 KpSwing, KdSwing;
|
||||
Vec2 _vxLim, _vyLim, _wyawLim;
|
||||
};
|
||||
|
||||
|
||||
#endif //STATETROTTING_H
|
||||
|
|
@ -16,6 +16,7 @@
|
|||
#include "FSM/StateFreeStand.h"
|
||||
#include "FSM/StatePassive.h"
|
||||
#include "FSM/StateSwingTest.h"
|
||||
#include "FSM/StateTrotting.h"
|
||||
|
||||
namespace unitree_guide_controller {
|
||||
struct FSMStateList {
|
||||
|
|
@ -24,6 +25,7 @@ namespace unitree_guide_controller {
|
|||
std::shared_ptr<StateFixedDown> fixedDown;
|
||||
std::shared_ptr<StateFixedStand> fixedStand;
|
||||
std::shared_ptr<StateFreeStand> freeStand;
|
||||
std::shared_ptr<StateTrotting> trotting;
|
||||
|
||||
std::shared_ptr<StateSwingTest> swingTest;
|
||||
std::shared_ptr<StateBalanceTest> balanceTest;
|
||||
|
|
|
|||
|
|
@ -61,5 +61,25 @@ inline Vec3 rotMatToExp(const RotMat &rm) {
|
|||
return exp;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
T saturation(const T a, Vec2 limits) {
|
||||
T lowLim, highLim;
|
||||
if (limits(0) > limits(1)) {
|
||||
lowLim = limits(1);
|
||||
highLim = limits(0);
|
||||
} else {
|
||||
lowLim = limits(0);
|
||||
highLim = limits(1);
|
||||
}
|
||||
|
||||
if (a < lowLim) {
|
||||
return lowLim;
|
||||
}
|
||||
if (a > highLim) {
|
||||
return highLim;
|
||||
}
|
||||
return a;
|
||||
}
|
||||
|
||||
|
||||
#endif //MATHTOOLS_H
|
||||
|
|
|
|||
|
|
@ -26,14 +26,14 @@ public:
|
|||
* @return
|
||||
*/
|
||||
Vec34 calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rot_matrix,
|
||||
const Vec34 &feet_pos_2_body, const std::vector<int> &contact);
|
||||
const Vec34 &feet_pos_2_body, const VecInt4 &contact);
|
||||
|
||||
private:
|
||||
void calMatrixA(const Vec34 &feet_pos_2_body, const RotMat &rotM);
|
||||
|
||||
void calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM);
|
||||
|
||||
void calConstraints(const std::vector<int> &contact);
|
||||
void calConstraints(const VecInt4 &contact);
|
||||
|
||||
void solveQP();
|
||||
|
||||
|
|
|
|||
|
|
@ -44,11 +44,23 @@ public:
|
|||
return getPosition() + Vec3((rotation_ * foot_poses_[index].p).data);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the estimated feet velocity in world frame
|
||||
* @return feet velocity in world frame
|
||||
*/
|
||||
Vec34 getFeetPos() {
|
||||
Vec34 feetPos;
|
||||
for (int i(0); i < 4; ++i) {
|
||||
feetPos.col(i) = getFootPos(i);
|
||||
}
|
||||
return feetPos;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the estimated foot position in body frame
|
||||
* @return
|
||||
*/
|
||||
Vec34 getFootPos2Body() {
|
||||
Vec34 getFeetPos2Body() {
|
||||
Vec34 foot_pos;
|
||||
const Vec3 body_pos = getPosition();
|
||||
for (int i = 0; i < 4; i++) {
|
||||
|
|
@ -65,8 +77,18 @@ public:
|
|||
return gyro_;
|
||||
}
|
||||
|
||||
[[nodiscard]] KDL::Vector getGlobalGyro() const {
|
||||
return rotation_ * gyro_;
|
||||
[[nodiscard]] Vec3 getGlobalGyro() const {
|
||||
return Vec3((rotation_ * gyro_).data);
|
||||
}
|
||||
|
||||
[[nodiscard]] double getYaw() const {
|
||||
double roll, pitch, yaw;
|
||||
rotation_.GetRPY(roll, pitch, yaw);
|
||||
return yaw;
|
||||
}
|
||||
|
||||
[[nodiscard]] double getDYaw() const {
|
||||
return getGlobalGyro()(2);
|
||||
}
|
||||
|
||||
void update();
|
||||
|
|
|
|||
|
|
@ -0,0 +1,38 @@
|
|||
//
|
||||
// Created by biao on 24-9-18.
|
||||
//
|
||||
|
||||
|
||||
#ifndef FOOTENDCTRL_H
|
||||
#define FOOTENDCTRL_H
|
||||
#include <unitree_guide_controller/common/mathTypes.h>
|
||||
|
||||
|
||||
class Estimator;
|
||||
struct CtrlComponent;
|
||||
class QuadrupedRobot;
|
||||
|
||||
class FeetEndCtrl {
|
||||
public:
|
||||
explicit FeetEndCtrl(CtrlComponent &ctrl_component);
|
||||
|
||||
void init();
|
||||
|
||||
~FeetEndCtrl() = default;
|
||||
|
||||
Vec3 calcFootPos(int index, Vec2 vxy_goal_global, double d_yaw_global, double phase);
|
||||
|
||||
private:
|
||||
CtrlComponent &ctrl_component_;
|
||||
QuadrupedRobot &robot_model_;
|
||||
Estimator &estimator_;
|
||||
|
||||
Vec4 feet_radius_, feet_init_angle_;
|
||||
|
||||
double k_x_, k_y_, k_yaw_;
|
||||
double t_stance_{}, t_swing_{};
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif //FOOTENDCTRL_H
|
||||
|
|
@ -1,24 +0,0 @@
|
|||
//
|
||||
// Created by biao on 24-9-18.
|
||||
//
|
||||
|
||||
|
||||
#ifndef FOOTENDCTRL_H
|
||||
#define FOOTENDCTRL_H
|
||||
#include <unitree_guide_controller/common/mathTypes.h>
|
||||
|
||||
|
||||
class FootEndCtrl {
|
||||
public:
|
||||
FootEndCtrl();
|
||||
|
||||
~FootEndCtrl() = default;
|
||||
|
||||
Vec3 calcFootPos(int index, Vec2 vxy_goal_global, double d_yaw_global, double phase);
|
||||
|
||||
private:
|
||||
double k_x_, k_y_, k_yaw_;
|
||||
};
|
||||
|
||||
|
||||
#endif //FOOTENDCTRL_H
|
||||
|
|
@ -5,10 +5,76 @@
|
|||
|
||||
#ifndef GAITGENERATOR_H
|
||||
#define GAITGENERATOR_H
|
||||
#include <unitree_guide_controller/common/mathTypes.h>
|
||||
|
||||
#include "FeetEndCtrl.h"
|
||||
|
||||
|
||||
class Gaitgenerator {
|
||||
class Estimator;
|
||||
class WaveGenerator;
|
||||
struct CtrlComponent;
|
||||
|
||||
class GaitGenerator {
|
||||
public:
|
||||
explicit GaitGenerator(CtrlComponent &ctrl_component);
|
||||
|
||||
~GaitGenerator() = default;
|
||||
|
||||
void setGait(Vec2 vxy_goal_global, double d_yaw_goal, double gait_height);
|
||||
|
||||
void run(Vec34 &feet_pos, Vec34 &feet_vel);
|
||||
|
||||
void restart();
|
||||
|
||||
private:
|
||||
Vec3 getFootPos(int i);
|
||||
|
||||
Vec3 getFootVel(int i);
|
||||
|
||||
/**
|
||||
* Calculate the position of the foot in the XY plane
|
||||
* @param startXY
|
||||
* @param endXY
|
||||
* @param phase
|
||||
* @return
|
||||
*/
|
||||
static double cycloidXYPosition(double startXY, double endXY, double phase);
|
||||
|
||||
/**
|
||||
* Calculate the position of the foot in the Z direction
|
||||
* @param startZ
|
||||
* @param height
|
||||
* @param phase
|
||||
* @return
|
||||
*/
|
||||
static double cycloidZPosition(double startZ, double height, double phase);
|
||||
|
||||
/**
|
||||
* Calculate the velocity of the foot in the XY plane
|
||||
* @param startXY
|
||||
* @param endXY
|
||||
* @param phase
|
||||
* @return
|
||||
*/
|
||||
[[nodiscard]] double cycloidXYVelocity(double startXY, double endXY, double phase) const;
|
||||
|
||||
/**
|
||||
* Calculate the velocity of the foot in the Z direction
|
||||
* @param height
|
||||
* @param phase
|
||||
* @return
|
||||
*/
|
||||
[[nodiscard]] double cycloidZVelocity(double height, double phase) const;
|
||||
|
||||
WaveGenerator &wave_generator_;
|
||||
Estimator &estimator_;
|
||||
FeetEndCtrl feet_end_ctrl_;
|
||||
|
||||
double gait_height_{};
|
||||
Vec2 vxy_goal_;
|
||||
double d_yaw_goal_{};
|
||||
Vec34 start_p_, end_p_, ideal_p_, past_p_;
|
||||
bool first_run_;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -23,12 +23,16 @@ public:
|
|||
|
||||
void init(double period, double st_ratio, const Vec4 &bias);
|
||||
|
||||
void calculate(Vec4 &phase_result, VecInt4 &contact_result, WaveStatus status);
|
||||
void update();
|
||||
|
||||
[[nodiscard]] double get_t_stance() const { return period_ * st_ratio_; }
|
||||
[[nodiscard]] double get_t_swing() const { return period_ * (1 - st_ratio_); }
|
||||
[[nodiscard]] double get_t() const { return period_; }
|
||||
|
||||
Vec4 phase_;
|
||||
VecInt4 contact_;
|
||||
WaveStatus status_{};
|
||||
|
||||
private:
|
||||
/**
|
||||
* Update phase, contact and status based on current time.
|
||||
|
|
@ -43,8 +47,8 @@ private:
|
|||
Vec4 bias_;
|
||||
|
||||
Vec4 normal_t_; // normalize time [0,1)
|
||||
Vec4 phase_, phase_past_; // foot phase
|
||||
VecInt4 contact_, contact_past_; // foot contact
|
||||
Vec4 phase_past_; // foot phase
|
||||
VecInt4 contact_past_; // foot contact
|
||||
VecInt4 switch_status_;
|
||||
WaveStatus status_past_;
|
||||
|
||||
|
|
|
|||
|
|
@ -30,6 +30,10 @@ public:
|
|||
*/
|
||||
[[nodiscard]] std::vector<KDL::JntArray> getQ(const std::vector<KDL::Frame> &pEe_list) const;
|
||||
|
||||
[[nodiscard]] Vec12 getQ(const Vec34 &vecP) const;
|
||||
|
||||
Vec12 getQd(const std::vector<KDL::Frame> &pos, const Vec34 &vel);
|
||||
|
||||
/**
|
||||
* Calculate the foot end position based on joint positions
|
||||
* @return vector of foot-end position
|
||||
|
|
|
|||
|
|
@ -10,7 +10,8 @@ StateBalanceTest::StateBalanceTest(CtrlComponent &ctrlComp) : FSMState(FSMStateN
|
|||
ctrlComp),
|
||||
estimator_(ctrlComp.estimator_),
|
||||
robot_model_(ctrlComp.robot_model_),
|
||||
balance_ctrl_(ctrlComp.balance_ctrl_) {
|
||||
balance_ctrl_(ctrlComp.balance_ctrl_),
|
||||
wave_generator_(ctrl_comp_.wave_generator_) {
|
||||
_xMax = 0.05;
|
||||
_xMin = -_xMax;
|
||||
_yMax = 0.05;
|
||||
|
|
@ -31,6 +32,7 @@ void StateBalanceTest::enter() {
|
|||
pcd_init_ = estimator_.getPosition();
|
||||
pcd_ = pcd_init_;
|
||||
init_rotation_ = estimator_.getRotation();
|
||||
wave_generator_.status_ = WaveStatus::STANCE_ALL;
|
||||
}
|
||||
|
||||
void StateBalanceTest::run() {
|
||||
|
|
@ -74,12 +76,11 @@ void StateBalanceTest::calcTorque() {
|
|||
|
||||
// expected body angular acceleration
|
||||
d_wbd_ = -kp_w_ * rotMatToExp(Rd_ * G2B_Rotation) +
|
||||
Kd_w_ * Vec3((-estimator_.getGlobalGyro()).data);
|
||||
Kd_w_ * -estimator_.getGlobalGyro();
|
||||
|
||||
// calculate foot force
|
||||
const std::vector contact(4, 1);
|
||||
const Vec34 foot_force = G2B_Rotation * balance_ctrl_.calF(dd_pcd_, d_wbd_, B2G_Rotation,
|
||||
estimator_.getFootPos2Body(), contact);
|
||||
estimator_.getFeetPos2Body(), wave_generator_.contact_);
|
||||
|
||||
std::vector<KDL::JntArray> current_joints = robot_model_.current_joint_pos_;
|
||||
for (int i = 0; i < 4; i++) {
|
||||
|
|
|
|||
|
|
@ -15,6 +15,7 @@ void StateFixedDown::enter() {
|
|||
for (int i = 0; i < 12; i++) {
|
||||
start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
|
||||
}
|
||||
ctrl_comp_.control_inputs_.get().command = 0;
|
||||
}
|
||||
|
||||
void StateFixedDown::run() {
|
||||
|
|
|
|||
|
|
@ -15,6 +15,7 @@ void StateFixedStand::enter() {
|
|||
for (int i = 0; i < 12; i++) {
|
||||
start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
|
||||
}
|
||||
ctrl_comp_.control_inputs_.get().command = 0;
|
||||
}
|
||||
|
||||
void StateFixedStand::run() {
|
||||
|
|
@ -44,6 +45,8 @@ FSMStateName StateFixedStand::checkChange() {
|
|||
return FSMStateName::FIXEDDOWN;
|
||||
case 3:
|
||||
return FSMStateName::FREESTAND;
|
||||
case 4:
|
||||
return FSMStateName::TROTTING;
|
||||
case 9:
|
||||
return FSMStateName::SWINGTEST;
|
||||
case 10:
|
||||
|
|
|
|||
|
|
@ -33,6 +33,7 @@ void StateFreeStand::enter() {
|
|||
foot_pos.p -= fr_init_pos_.p;
|
||||
foot_pos.M = KDL::Rotation::RPY(0, 0, 0);
|
||||
}
|
||||
ctrl_comp_.control_inputs_.get().command = 0;
|
||||
}
|
||||
|
||||
void StateFreeStand::run() {
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@ void StatePassive::enter() {
|
|||
for (auto i: ctrl_comp_.joint_kd_command_interface_) {
|
||||
i.get().set_value(1);
|
||||
}
|
||||
ctrl_comp_.control_inputs_.get().command = 0;
|
||||
}
|
||||
|
||||
void StatePassive::run() {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,196 @@
|
|||
//
|
||||
// Created by tlab-uav on 24-9-18.
|
||||
//
|
||||
|
||||
#include "unitree_guide_controller/FSM/StateTrotting.h"
|
||||
|
||||
#include <unitree_guide_controller/common/mathTools.h>
|
||||
|
||||
StateTrotting::StateTrotting(CtrlComponent &ctrlComp) : FSMState(FSMStateName::TROTTING, "trotting", ctrlComp),
|
||||
estimator_(ctrlComp.estimator_),
|
||||
robot_model_(ctrlComp.robot_model_),
|
||||
balance_ctrl_(ctrlComp.balance_ctrl_),
|
||||
wave_generator_(ctrl_comp_.wave_generator_),
|
||||
gait_generator_(ctrlComp) {
|
||||
_gaitHeight = 0.08;
|
||||
Kpp = Vec3(70, 70, 70).asDiagonal();
|
||||
Kdp = Vec3(10, 10, 10).asDiagonal();
|
||||
_kpw = 780;
|
||||
Kdw = Vec3(70, 70, 70).asDiagonal();
|
||||
KpSwing = Vec3(400, 400, 400).asDiagonal();
|
||||
KdSwing = Vec3(10, 10, 10).asDiagonal();
|
||||
|
||||
_vxLim << -0.4, 0.4;
|
||||
_vyLim << -0.3, 0.3;
|
||||
_wyawLim << -0.5, 0.5;
|
||||
dt_ = 1.0 / ctrl_comp_.frequency_;
|
||||
}
|
||||
|
||||
void StateTrotting::enter() {
|
||||
_pcd = estimator_.getPosition();
|
||||
_vCmdBody.setZero();
|
||||
_yawCmd = estimator_.getYaw();
|
||||
Rd = Mat3(KDL::Rotation::RPY(0, 0, _yawCmd).Inverse().data);
|
||||
_wCmdGlobal.setZero();
|
||||
|
||||
ctrl_comp_.control_inputs_.get().command = 0;
|
||||
gait_generator_.restart();
|
||||
}
|
||||
|
||||
void StateTrotting::run() {
|
||||
_posBody = estimator_.getPosition();
|
||||
_velBody = estimator_.getVelocity();
|
||||
_posFeet2BGlobal = estimator_.getFeetPos2Body();
|
||||
_posFeetGlobal = estimator_.getFeetPos();
|
||||
B2G_RotMat = Eigen::Matrix3d(estimator_.getRotation().data);
|
||||
G2B_RotMat = B2G_RotMat.transpose();
|
||||
|
||||
getUserCmd();
|
||||
calcCmd();
|
||||
|
||||
gait_generator_.setGait(_vCmdGlobal.segment(0, 2), _wCmdGlobal(2), _gaitHeight);
|
||||
gait_generator_.run(_posFeetGlobalGoal, _velFeetGlobalGoal);
|
||||
|
||||
calcTau();
|
||||
calcQQd();
|
||||
|
||||
if (checkStepOrNot()) {
|
||||
wave_generator_.status_ = WaveStatus::WAVE_ALL;
|
||||
} else {
|
||||
wave_generator_.status_ = WaveStatus::STANCE_ALL;
|
||||
}
|
||||
|
||||
calcGain();
|
||||
}
|
||||
|
||||
void StateTrotting::exit() {
|
||||
wave_generator_.status_ = WaveStatus::SWING_ALL;
|
||||
}
|
||||
|
||||
FSMStateName StateTrotting::checkChange() {
|
||||
switch (ctrl_comp_.control_inputs_.get().command) {
|
||||
case 1:
|
||||
return FSMStateName::FIXEDDOWN;
|
||||
case 2:
|
||||
return FSMStateName::FIXEDSTAND;
|
||||
default:
|
||||
return FSMStateName::TROTTING;
|
||||
}
|
||||
}
|
||||
|
||||
void StateTrotting::getUserCmd() {
|
||||
/* Movement */
|
||||
_vCmdBody(0) = invNormalize(ctrl_comp_.control_inputs_.get().ly, _vxLim(0), _vxLim(1));
|
||||
_vCmdBody(1) = -invNormalize(ctrl_comp_.control_inputs_.get().lx, _vyLim(0), _vyLim(1));
|
||||
_vCmdBody(2) = 0;
|
||||
|
||||
/* Turning */
|
||||
_dYawCmd = -invNormalize(ctrl_comp_.control_inputs_.get().rx, _wyawLim(0), _wyawLim(1));
|
||||
_dYawCmd = 0.9 * _dYawCmdPast + (1 - 0.9) * _dYawCmd;
|
||||
_dYawCmdPast = _dYawCmd;
|
||||
}
|
||||
|
||||
void StateTrotting::calcCmd() {
|
||||
/* Movement */
|
||||
_vCmdGlobal = B2G_RotMat * _vCmdBody;
|
||||
|
||||
_vCmdGlobal(0) =
|
||||
saturation(_vCmdGlobal(0), Vec2(_velBody(0) - 0.2, _velBody(0) + 0.2));
|
||||
_vCmdGlobal(1) =
|
||||
saturation(_vCmdGlobal(1), Vec2(_velBody(1) - 0.2, _velBody(1) + 0.2));
|
||||
|
||||
_pcd(0) = saturation(_pcd(0) + _vCmdGlobal(0) * dt_,
|
||||
Vec2(_posBody(0) - 0.05, _posBody(0) + 0.05));
|
||||
_pcd(1) = saturation(_pcd(1) + _vCmdGlobal(1) * dt_,
|
||||
Vec2(_posBody(1) - 0.05, _posBody(1) + 0.05));
|
||||
|
||||
_vCmdGlobal(2) = 0;
|
||||
|
||||
/* Turning */
|
||||
_yawCmd = _yawCmd + _dYawCmd * dt_;
|
||||
Rd = Mat3(KDL::Rotation::RPY(0, 0, _yawCmd).Inverse().data);
|
||||
_wCmdGlobal(2) = _dYawCmd;
|
||||
}
|
||||
|
||||
void StateTrotting::calcTau() {
|
||||
pos_error_ = _pcd - _posBody;
|
||||
vel_error_ = _vCmdGlobal - _velBody;
|
||||
|
||||
_ddPcd = Kpp * pos_error_ + Kdp * vel_error_;
|
||||
_dWbd = _kpw * rotMatToExp(Rd * G2B_RotMat) +
|
||||
Kdw * (_wCmdGlobal - estimator_.getGlobalGyro());
|
||||
|
||||
_ddPcd(0) = saturation(_ddPcd(0), Vec2(-3, 3));
|
||||
_ddPcd(1) = saturation(_ddPcd(1), Vec2(-3, 3));
|
||||
_ddPcd(2) = saturation(_ddPcd(2), Vec2(-5, 5));
|
||||
|
||||
_dWbd(0) = saturation(_dWbd(0), Vec2(-40, 40));
|
||||
_dWbd(1) = saturation(_dWbd(1), Vec2(-40, 40));
|
||||
_dWbd(2) = saturation(_dWbd(2), Vec2(-10, 10));
|
||||
|
||||
force_feet_global_ =
|
||||
-balance_ctrl_.calF(_ddPcd, _dWbd, B2G_RotMat, _posFeet2BGlobal, wave_generator_.contact_);
|
||||
|
||||
for (int i(0); i < 4; ++i) {
|
||||
if (wave_generator_.contact_(i) == 0) {
|
||||
force_feet_global_.col(i) =
|
||||
KpSwing * (_posFeetGlobalGoal.col(i) - _posFeetGlobal.col(i)) +
|
||||
KdSwing * (_velFeetGlobalGoal.col(i) - _velFeetGlobal.col(i));
|
||||
}
|
||||
}
|
||||
|
||||
force_feet_body_ = G2B_RotMat * force_feet_global_;
|
||||
|
||||
std::vector<KDL::JntArray> current_joints = robot_model_.current_joint_pos_;
|
||||
for (int i = 0; i < 4; i++) {
|
||||
KDL::JntArray torque = robot_model_.getTorque(force_feet_body_.col(i), i);
|
||||
for (int j = 0; j < 3; j++) {
|
||||
ctrl_comp_.joint_effort_command_interface_[i * 3 + j].get().set_value(torque(j));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void StateTrotting::calcQQd() {
|
||||
const std::vector<KDL::Frame> _posFeet2B = robot_model_.getFeet2BPositions();
|
||||
|
||||
for (int i(0); i < 4; ++i) {
|
||||
_posFeet2BGoal.col(i) = G2B_RotMat * (_posFeetGlobalGoal.col(i) - _posBody);
|
||||
_velFeet2BGoal.col(i) = G2B_RotMat * (_velFeetGlobalGoal.col(i) - _velBody);
|
||||
// _velFeet2BGoal.col(i) = _G2B_RotMat * (_velFeetGlobalGoal.col(i) -
|
||||
// _velBody - _B2G_RotMat * (skew(_lowState->getGyro()) * _posFeet2B.col(i))
|
||||
// ); // c.f formula (6.12)
|
||||
}
|
||||
|
||||
Vec12 q_goal = robot_model_.getQ(_posFeet2BGoal);
|
||||
Vec12 qd_goal = robot_model_.getQd(_posFeet2B, _velFeet2BGoal);
|
||||
for (int i = 0; i < 12; i++) {
|
||||
ctrl_comp_.joint_position_command_interface_[i].get().set_value(q_goal(i));
|
||||
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(qd_goal(i));
|
||||
}
|
||||
}
|
||||
|
||||
void StateTrotting::calcGain() const {
|
||||
for (int i(0); i < 4; ++i) {
|
||||
if (wave_generator_.contact_(i) == 0) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
ctrl_comp_.joint_kp_command_interface_[i * 3 + j].get().set_value(3);
|
||||
ctrl_comp_.joint_kd_command_interface_[i * 3 + j].get().set_value(2);
|
||||
}
|
||||
} else {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
ctrl_comp_.joint_kp_command_interface_[i * 3 + j].get().set_value(0.8);
|
||||
ctrl_comp_.joint_kd_command_interface_[i * 3 + j].get().set_value(0.8);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool StateTrotting::checkStepOrNot() {
|
||||
if (fabs(_vCmdBody(0)) > 0.03 || fabs(_vCmdBody(1)) > 0.03 ||
|
||||
fabs(pos_error_(0)) > 0.08 || fabs(pos_error_(1)) > 0.08 ||
|
||||
fabs(vel_error_(0)) > 0.05 || fabs(vel_error_(1)) > 0.05 ||
|
||||
fabs(_dYawCmd) > 0.20) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
|
@ -51,6 +51,7 @@ namespace unitree_guide_controller {
|
|||
|
||||
|
||||
ctrl_comp_.robot_model_.update();
|
||||
ctrl_comp_.wave_generator_.update();
|
||||
ctrl_comp_.estimator_.update();
|
||||
|
||||
if (mode_ == FSMMode::NORMAL) {
|
||||
|
|
@ -114,6 +115,8 @@ namespace unitree_guide_controller {
|
|||
get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
|
||||
RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_);
|
||||
|
||||
ctrl_comp_.wave_generator_.init(0.45, 0.5, Vec4(0, 0.5, 0.5, 0));
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
|
|
@ -142,6 +145,7 @@ namespace unitree_guide_controller {
|
|||
state_list_.swingTest = std::make_shared<StateSwingTest>(ctrl_comp_);
|
||||
state_list_.freeStand = std::make_shared<StateFreeStand>(ctrl_comp_);
|
||||
state_list_.balanceTest = std::make_shared<StateBalanceTest>(ctrl_comp_);
|
||||
state_list_.trotting = std::make_shared<StateTrotting>(ctrl_comp_);
|
||||
|
||||
// Initialize FSM
|
||||
current_state_ = state_list_.passive;
|
||||
|
|
@ -186,8 +190,8 @@ namespace unitree_guide_controller {
|
|||
return state_list_.fixedStand;
|
||||
case FSMStateName::FREESTAND:
|
||||
return state_list_.freeStand;
|
||||
// case FSMStateName::TROTTING:
|
||||
// return state_list_.trotting;
|
||||
case FSMStateName::TROTTING:
|
||||
return state_list_.trotting;
|
||||
case FSMStateName::SWINGTEST:
|
||||
return state_list_.swingTest;
|
||||
case FSMStateName::BALANCETEST:
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@ void BalanceCtrl::init(const QuadrupedRobot &robot) {
|
|||
}
|
||||
|
||||
Vec34 BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rot_matrix,
|
||||
const Vec34 &feet_pos_2_body, const std::vector<int> &contact) {
|
||||
const Vec34 &feet_pos_2_body, const VecInt4 &contact) {
|
||||
calMatrixA(feet_pos_2_body, rot_matrix);
|
||||
calVectorBd(ddPcd, dWbd, rot_matrix);
|
||||
calConstraints(contact);
|
||||
|
|
@ -64,7 +64,7 @@ void BalanceCtrl::calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat
|
|||
bd_.tail(3) = rotM * Ib_ * rotM.transpose() * dWbd;
|
||||
}
|
||||
|
||||
void BalanceCtrl::calConstraints(const std::vector<int> &contact) {
|
||||
void BalanceCtrl::calConstraints(const VecInt4 &contact) {
|
||||
int contactLegNum = 0;
|
||||
for (int i(0); i < 4; ++i) {
|
||||
if (contact[i] == 1) {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,56 @@
|
|||
//
|
||||
// Created by biao on 24-9-18.
|
||||
//
|
||||
|
||||
#include "unitree_guide_controller/gait/FeetEndCtrl.h"
|
||||
|
||||
#include "unitree_guide_controller/control/CtrlComponent.h"
|
||||
|
||||
FeetEndCtrl::FeetEndCtrl(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_component),
|
||||
robot_model_(ctrl_component.robot_model_),
|
||||
estimator_(ctrl_component.estimator_) {
|
||||
k_x_ = 0.005;
|
||||
k_y_ = 0.005;
|
||||
k_yaw_ = 0.005;
|
||||
}
|
||||
|
||||
void FeetEndCtrl::init() {
|
||||
t_stance_ = ctrl_component_.wave_generator_.get_t_stance();
|
||||
t_swing_ = ctrl_component_.wave_generator_.get_t_swing();
|
||||
|
||||
Vec34 feet_pos_body = estimator_.getFeetPos2Body();
|
||||
for (int i(0); i < 4; ++i) {
|
||||
feet_radius_(i) =
|
||||
sqrt(pow(feet_pos_body(0, i), 2) + pow(feet_pos_body(1, i), 2));
|
||||
feet_init_angle_(i) = atan2(feet_pos_body(1, i), feet_pos_body(0, i));
|
||||
}
|
||||
}
|
||||
|
||||
Vec3 FeetEndCtrl::calcFootPos(const int index, Vec2 vxy_goal_global, const double d_yaw_global, const double phase) {
|
||||
Vec3 body_vel_global = estimator_.getVelocity();
|
||||
Vec3 next_step;
|
||||
|
||||
next_step(0) = body_vel_global(0) * (1 - phase) * t_swing_ +
|
||||
body_vel_global(0) * t_stance_ / 2 +
|
||||
k_x_ * (body_vel_global(0) - vxy_goal_global(0));
|
||||
next_step(1) = body_vel_global(1) * (1 - phase) * t_swing_ +
|
||||
body_vel_global(1) * t_stance_ / 2 +
|
||||
k_y_ * (body_vel_global(1) - vxy_goal_global(1));
|
||||
next_step(2) = 0;
|
||||
|
||||
const double yaw = estimator_.getYaw();
|
||||
const double d_yaw = estimator_.getDYaw();
|
||||
const double next_yaw = d_yaw * (1 - phase) * t_swing_ + d_yaw * t_stance_ / 2 +
|
||||
k_yaw_ * (d_yaw_global - d_yaw);
|
||||
|
||||
|
||||
next_step(0) +=
|
||||
feet_radius_(index) * cos(yaw + feet_init_angle_(index) + next_yaw);
|
||||
next_step(1) +=
|
||||
feet_radius_(index) * sin(yaw + feet_init_angle_(index) + next_yaw);
|
||||
|
||||
Vec3 foot_pos = estimator_.getPosition() + next_step;
|
||||
foot_pos(2) = 0.0;
|
||||
|
||||
return foot_pos;
|
||||
}
|
||||
|
|
@ -1,14 +0,0 @@
|
|||
//
|
||||
// Created by biao on 24-9-18.
|
||||
//
|
||||
|
||||
#include "unitree_guide_controller/gait/FootEndCtrl.h"
|
||||
|
||||
FootEndCtrl::FootEndCtrl() {
|
||||
k_x_ = 0.005;
|
||||
k_y_ = 0.005;
|
||||
k_yaw_ = 0.005;
|
||||
}
|
||||
|
||||
Vec3 FootEndCtrl::calcFootPos(int index, Vec2 vxy_goal_global, double d_yaw_global, double phase) {
|
||||
}
|
||||
|
|
@ -3,3 +3,92 @@
|
|||
//
|
||||
|
||||
#include "unitree_guide_controller/gait/GaitGenerator.h"
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include "unitree_guide_controller/control/CtrlComponent.h"
|
||||
|
||||
GaitGenerator::GaitGenerator(CtrlComponent &ctrl_component) : estimator_(ctrl_component.estimator_),
|
||||
feet_end_ctrl_(ctrl_component),
|
||||
wave_generator_(ctrl_component.wave_generator_) {
|
||||
first_run_ = true;
|
||||
}
|
||||
|
||||
void GaitGenerator::setGait(Vec2 vxy_goal_global, const double d_yaw_goal, const double gait_height) {
|
||||
vxy_goal_ = std::move(vxy_goal_global);
|
||||
d_yaw_goal_ = d_yaw_goal;
|
||||
gait_height_ = gait_height;
|
||||
}
|
||||
|
||||
void GaitGenerator::run(Vec34 &feet_pos, Vec34 &feet_vel) {
|
||||
if (first_run_) {
|
||||
start_p_ = estimator_.getFeetPos();
|
||||
first_run_ = false;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
if (wave_generator_.contact_(i) == 1) {
|
||||
// foot contact the ground
|
||||
if (wave_generator_.phase_(i) < 0.5) {
|
||||
start_p_.col(i) = estimator_.getFootPos(i);
|
||||
}
|
||||
feet_pos.col(i) = start_p_.col(i);
|
||||
feet_vel.col(i).setZero();
|
||||
} else {
|
||||
// foot not contact, swing
|
||||
end_p_.col(i) = feet_end_ctrl_.calcFootPos(i, vxy_goal_, d_yaw_goal_, wave_generator_.phase_(i));
|
||||
feet_pos.col(i) = getFootPos(i);
|
||||
feet_vel.col(i) = getFootVel(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void GaitGenerator::restart() {
|
||||
first_run_ = true;
|
||||
vxy_goal_.setZero();
|
||||
feet_end_ctrl_.init();
|
||||
}
|
||||
|
||||
Vec3 GaitGenerator::getFootPos(const int i) {
|
||||
Vec3 foot_pos;
|
||||
|
||||
foot_pos(0) =
|
||||
cycloidXYPosition(start_p_.col(i)(0), end_p_.col(i)(0), wave_generator_.phase_(i));
|
||||
foot_pos(1) =
|
||||
cycloidXYPosition(start_p_.col(i)(1), end_p_.col(i)(1), wave_generator_.phase_(i));
|
||||
foot_pos(2) = cycloidZPosition(start_p_.col(i)(2), gait_height_, wave_generator_.phase_(i));
|
||||
|
||||
return foot_pos;
|
||||
}
|
||||
|
||||
Vec3 GaitGenerator::getFootVel(const int i) {
|
||||
Vec3 foot_vel;
|
||||
|
||||
foot_vel(0) =
|
||||
cycloidXYVelocity(start_p_.col(i)(0), end_p_.col(i)(0), wave_generator_.phase_(i));
|
||||
foot_vel(1) =
|
||||
cycloidXYVelocity(start_p_.col(i)(1), end_p_.col(i)(1), wave_generator_.phase_(i));
|
||||
foot_vel(2) = cycloidZVelocity(gait_height_, wave_generator_.phase_(i));
|
||||
|
||||
return foot_vel;
|
||||
}
|
||||
|
||||
double GaitGenerator::cycloidXYPosition(const double startXY, const double endXY, const double phase) {
|
||||
const double phase_pi = 2 * M_PI * phase;
|
||||
return (endXY - startXY) * (phase_pi - sin(phase_pi)) / (2 * M_PI) + startXY;
|
||||
}
|
||||
|
||||
double GaitGenerator::cycloidZPosition(const double startZ, const double height, const double phase) {
|
||||
const double phase_pi = 2 * M_PI * phase;
|
||||
return height * (1 - cos(phase_pi)) / 2 + startZ;
|
||||
}
|
||||
|
||||
double GaitGenerator::cycloidXYVelocity(const double startXY, const double endXY, const double phase) const {
|
||||
const double phase_pi = 2 * M_PI * phase;
|
||||
return (endXY - startXY) * (1 - cos(phase_pi)) / wave_generator_.get_t_swing();
|
||||
}
|
||||
|
||||
double GaitGenerator::cycloidZVelocity(const double height, const double phase) const {
|
||||
const double phase_pi = 2 * M_PI * phase;
|
||||
return height * M_PI * sin(phase_pi) / wave_generator_.get_t_swing();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -12,6 +12,7 @@ WaveGenerator::WaveGenerator() {
|
|||
phase_past_ << 0.5, 0.5, 0.5, 0.5;
|
||||
contact_past_.setZero();
|
||||
status_past_ = WaveStatus::SWING_ALL;
|
||||
status_ = WaveStatus::SWING_ALL;
|
||||
}
|
||||
|
||||
void WaveGenerator::init(const double period, const double st_ratio, const Vec4 &bias) {
|
||||
|
|
@ -35,18 +36,18 @@ void WaveGenerator::init(const double period, const double st_ratio, const Vec4
|
|||
start_t_ = getSystemTime();
|
||||
}
|
||||
|
||||
auto WaveGenerator::calculate(Vec4 &phase_result, VecInt4 &contact_result, const WaveStatus status) -> void {
|
||||
calcWave(phase_, contact_, status);
|
||||
auto WaveGenerator::update() -> void {
|
||||
calcWave(phase_, contact_, status_);
|
||||
|
||||
if (status != status_past_) {
|
||||
if (status_ != status_past_) {
|
||||
if (switch_status_.sum() == 0) {
|
||||
switch_status_.setOnes();
|
||||
}
|
||||
calcWave(phase_past_, contact_past_, status_past_);
|
||||
|
||||
if (status == WaveStatus::STANCE_ALL && status_past_ == WaveStatus::SWING_ALL) {
|
||||
if (status_ == WaveStatus::STANCE_ALL && status_past_ == WaveStatus::SWING_ALL) {
|
||||
contact_past_.setOnes();
|
||||
} else if (status == WaveStatus::SWING_ALL && status_past_ == WaveStatus::STANCE_ALL) {
|
||||
} else if (status_ == WaveStatus::SWING_ALL && status_past_ == WaveStatus::STANCE_ALL) {
|
||||
contact_past_.setZero();
|
||||
}
|
||||
}
|
||||
|
|
@ -61,12 +62,9 @@ auto WaveGenerator::calculate(Vec4 &phase_result, VecInt4 &contact_result, const
|
|||
}
|
||||
}
|
||||
if (switch_status_.sum() == 0) {
|
||||
status_past_ = status;
|
||||
status_past_ = status_;
|
||||
}
|
||||
}
|
||||
|
||||
phase_result = phase_;
|
||||
contact_result = contact_;
|
||||
}
|
||||
|
||||
void WaveGenerator::calcWave(Vec4 &phase, VecInt4 &contact, const WaveStatus status) {
|
||||
|
|
|
|||
|
|
@ -42,6 +42,26 @@ std::vector<KDL::JntArray> QuadrupedRobot::getQ(const std::vector<KDL::Frame> &p
|
|||
return result;
|
||||
}
|
||||
|
||||
Vec12 QuadrupedRobot::getQ(const Vec34 &vecP) const {
|
||||
Vec12 q;
|
||||
for (int i(0); i < 4; ++i) {
|
||||
KDL::Frame frame;
|
||||
frame.p = KDL::Vector(vecP.col(i)[0], vecP.col(i)[1], vecP.col(i)[2]);
|
||||
q.segment(3 * i, 3) = robot_legs_[i]->calcQ(frame, current_joint_pos_[i]).data;
|
||||
}
|
||||
return q;
|
||||
}
|
||||
|
||||
Vec12 QuadrupedRobot::getQd(const std::vector<KDL::Frame> &pos, const Vec34 &vel) {
|
||||
Vec12 qd;
|
||||
std::vector<KDL::JntArray> q = getQ(pos);
|
||||
for (int i(0); i < 4; ++i) {
|
||||
Mat3 jacobian = robot_legs_[i]->calcJaco(q[i]).data.topRows(3);
|
||||
qd.segment(3 * i, 3) = jacobian.inverse() * vel.col(i);
|
||||
}
|
||||
return qd;
|
||||
}
|
||||
|
||||
std::vector<KDL::Frame> QuadrupedRobot::getFeet2BPositions() const {
|
||||
std::vector<KDL::Frame> result;
|
||||
result.resize(4);
|
||||
|
|
|
|||
Loading…
Reference in New Issue