EmbodiedAI
/
quadruped_ros2_control
mirror of https://github.com/legubiao/quadruped_ros2_control.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

can stand but not stable

This commit is contained in:
Huang Zhenbiao 2024-09-17 14:03:08 +08:00
parent 5eaf8e1e17
commit 79fc0677f5
17 changed files with 160 additions and 166 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h
View File

@ -18,7 +18,7 @@ public:
FSMState(const FSMStateName &stateName, std::string stateNameString, CtrlComponent ctrlComp) FSMState(const FSMStateName &stateName, std::string stateNameString, CtrlComponent ctrlComp)
: state_name(stateName), : state_name(stateName),
state_name_string(std::move(stateNameString)), state_name_string(std::move(stateNameString)),
ctrlComp_(std::move(ctrlComp)) { ctrl_comp_(std::move(ctrlComp)) {
} }
virtual void enter() = 0; virtual void enter() = 0;
@ -33,7 +33,7 @@ public:
std::string state_name_string; std::string state_name_string;
protected: protected:
CtrlComponent ctrlComp_; CtrlComponent ctrl_comp_;
}; };
#endif //FSMSTATE_H #endif //FSMSTATE_H

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

@ -85,40 +85,38 @@ public:
void update(const CtrlComponent &ctrlComp); void update(const CtrlComponent &ctrlComp);
private: private:
Eigen::Matrix<double, 18, 1> _xhat; // The state of estimator, position(3)+velocity(3)+feet position(3x4) Eigen::Matrix<double, 18, 1> _xhat; // The state of estimator, position(3)+velocity(3)+feet position(3x4)
Eigen::Matrix<double, 3, 1> _u; // The input of estimator Eigen::Matrix<double, 3, 1> _u; // The input of estimator
Eigen::Matrix<double, 28, 1> _y; // The measurement value of output y
Eigen::Matrix<double, 28, 1> _yhat; // The prediction of output y
Eigen::Matrix<double, 18, 18> A; // The transtion matrix of estimator
Eigen::Matrix<double, 18, 3> B; // The input matrix
Eigen::Matrix<double, 28, 18> C; // The output matrix
Eigen::Matrix<double, 28, 1> _y; // The measurement value of output y
Eigen::Matrix<double, 28, 1> _yhat; // The prediction of output y
Eigen::Matrix<double, 18, 18> A; // The transtion matrix of estimator
Eigen::Matrix<double, 18, 3> B; // The input matrix
Eigen::Matrix<double, 28, 18> C; // The output matrix
// Covariance Matrix // Covariance Matrix
Eigen::Matrix<double, 18, 18> P; // Prediction covariance Eigen::Matrix<double, 18, 18> P; // Prediction covariance
Eigen::Matrix<double, 18, 18> Ppriori; // Priori prediction covariance Eigen::Matrix<double, 18, 18> Ppriori; // Priori prediction covariance
Eigen::Matrix<double, 18, 18> Q; // Dynamic simulation covariance Eigen::Matrix<double, 18, 18> Q; // Dynamic simulation covariance
Eigen::Matrix<double, 28, 28> R; // Measurement covariance Eigen::Matrix<double, 28, 28> R; // Measurement covariance
Eigen::Matrix<double, 18, 18> Eigen::Matrix<double, 18, 18> QInit_; // Initial value of Dynamic simulation covariance
QInit; // Initial value of Dynamic simulation covariance Eigen::Matrix<double, 28, 28> RInit_; // Initial value of Measurement covariance
Eigen::Matrix<double, 28, 28> Eigen::Matrix<double, 18, 1> Qdig; // adjustable process noise covariance
RInit; // Initial value of Measurement covariance Eigen::Matrix<double, 3, 3> Cu; // The covariance of system input u
Eigen::Matrix<double, 18, 1> Qdig; // adjustable process noise covariance
Eigen::Matrix<double, 3, 3> Cu; // The covariance of system input u
// Output Measurement // Output Measurement
Eigen::Matrix<double, 12, 1> Eigen::Matrix<double, 12, 1> _feetPos2Body; // The feet positions to body, in the global coordinate
_feetPos2Body; // The feet positions to body, in the global coordinate Eigen::Matrix<double, 12, 1> _feetVel2Body; // The feet velocity to body, in the global coordinate
Eigen::Matrix<double, 12, 1> Eigen::Matrix<double, 4, 1> _feetH; // The Height of each foot, in the global coordinate
_feetVel2Body; // The feet velocity to body, in the global coordinate
Eigen::Matrix<double, 4, 1> Eigen::Matrix<double, 28, 28> S; // _S = C*P*C.T + R
_feetH; // The Height of each foot, in the global coordinate
Eigen::Matrix<double, 28, 28> S; // _S = C*P*C.T + R
Eigen::PartialPivLU<Eigen::Matrix<double, 28, 28> > Slu; // _S.lu() Eigen::PartialPivLU<Eigen::Matrix<double, 28, 28> > Slu; // _S.lu()
Eigen::Matrix<double, 28, 1> Sy; // _Sy = _S.inv() * (y - yhat) Eigen::Matrix<double, 28, 1> Sy; // _Sy = _S.inv() * (y - yhat)
Eigen::Matrix<double, 28, 18> Sc; // _Sc = _S.inv() * C Eigen::Matrix<double, 28, 18> Sc; // _Sc = _S.inv() * C
Eigen::Matrix<double, 28, 28> SR; // _SR = _S.inv() * R Eigen::Matrix<double, 28, 28> SR; // _SR = _S.inv() * R
Eigen::Matrix<double, 28, 18> STC; // _STC = (_S.transpose()).inv() * C Eigen::Matrix<double, 28, 18> STC; // _STC = (_S.transpose()).inv() * C
Eigen::Matrix<double, 18, 18> IKC; // _IKC = I - KC Eigen::Matrix<double, 18, 18> IKC; // _IKC = I - KC
KDL::Vector g_; KDL::Vector g_;
double _dt; double _dt;

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

@ -55,7 +55,7 @@ public:
* @return torque * @return torque
*/ */
[[nodiscard]] KDL::JntArray getTorque( [[nodiscard]] KDL::JntArray getTorque(
const KDL::Wrenches &force, int index) const; const KDL::Vector &force, int index) const;
/** /**
* Calculate the foot end velocity * Calculate the foot end velocity

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

@ -40,15 +40,19 @@ public:
*/ */
[[nodiscard]] KDL::Jacobian calcJaco(const KDL::JntArray &joint_positions) const; [[nodiscard]] KDL::Jacobian calcJaco(const KDL::JntArray &joint_positions) const;
[[nodiscard]] KDL::JntArray calcTorque(const KDL::JntArray &joint_positions, const KDL::JntArray &joint_velocities, /**
const KDL::Wrenches& force) const; * Calculate the torque based on joint positions and end force
* @param joint_positions current joint positions
* @param force foot end force
* @return joint torque
*/
[[nodiscard]] KDL::JntArray calcTorque(const KDL::JntArray &joint_positions, const KDL::Vector &force) const;
protected: protected:
KDL::Chain chain_; KDL::Chain chain_;
std::shared_ptr<KDL::ChainFkSolverPos_recursive> fk_pose_solver_; std::shared_ptr<KDL::ChainFkSolverPos_recursive> fk_pose_solver_;
std::shared_ptr<KDL::ChainJntToJacSolver> jac_solver_; std::shared_ptr<KDL::ChainJntToJacSolver> jac_solver_;
std::shared_ptr<KDL::ChainIkSolverPos_LMA> ik_pose_solver_; std::shared_ptr<KDL::ChainIkSolverPos_LMA> ik_pose_solver_;
std::shared_ptr<KDL::ChainIdSolver_RNE> id_solver_;
}; };

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

@ -25,34 +25,34 @@ StateBalanceTest::StateBalanceTest(CtrlComponent ctrlComp) : FSMState(FSMStateNa
} }
void StateBalanceTest::enter() { void StateBalanceTest::enter() {
pcd_ = ctrlComp_.estimator_.get().getPosition(); pcd_ = ctrl_comp_.estimator_.get().getPosition();
pcdInit_ = pcd_; pcdInit_ = pcd_;
RdInit_ = ctrlComp_.estimator_.get().getRotation(); RdInit_ = ctrl_comp_.estimator_.get().getRotation();
} }
void StateBalanceTest::run() { void StateBalanceTest::run() {
pcd_(0) = pcdInit_(0) + invNormalize(ctrlComp_.control_inputs_.get().ly, _xMin, _xMax); pcd_(0) = pcdInit_(0) + invNormalize(ctrl_comp_.control_inputs_.get().ly, _xMin, _xMax);
pcd_(1) = pcdInit_(1) - invNormalize(ctrlComp_.control_inputs_.get().lx, _yMin, _yMax); pcd_(1) = pcdInit_(1) - invNormalize(ctrl_comp_.control_inputs_.get().lx, _yMin, _yMax);
pcd_(2) = pcdInit_(2) + invNormalize(ctrlComp_.control_inputs_.get().ry, _zMin, _zMax); pcd_(2) = pcdInit_(2) + invNormalize(ctrl_comp_.control_inputs_.get().ry, _zMin, _zMax);
const float yaw = invNormalize(ctrlComp_.control_inputs_.get().rx, _yawMin, _yawMax); const float yaw = invNormalize(ctrl_comp_.control_inputs_.get().rx, _yawMin, _yawMax);
Rd_ = KDL::Rotation::RPY(0, 0, yaw) * RdInit_; Rd_ = KDL::Rotation::RPY(0, 0, yaw) * RdInit_;
pose_body_ = ctrlComp_.estimator_.get().getPosition(); pose_body_ = ctrl_comp_.estimator_.get().getPosition();
vel_body_ = ctrlComp_.estimator_.get().getVelocity(); vel_body_ = ctrl_comp_.estimator_.get().getVelocity();
calcTorque();
for (int i = 0; i < 12; i++) { for (int i = 0; i < 12; i++) {
ctrlComp_.joint_kp_command_interface_[i].get().set_value(0.8); ctrl_comp_.joint_kp_command_interface_[i].get().set_value(0.8);
ctrlComp_.joint_kd_command_interface_[i].get().set_value(0.8); ctrl_comp_.joint_kd_command_interface_[i].get().set_value(0.8);
} }
calcTorque();
} }
void StateBalanceTest::exit() { void StateBalanceTest::exit() {
} }
FSMStateName StateBalanceTest::checkChange() { FSMStateName StateBalanceTest::checkChange() {
switch (ctrlComp_.control_inputs_.get().command) { switch (ctrl_comp_.control_inputs_.get().command) {
case 1: case 1:
return FSMStateName::FIXEDDOWN; return FSMStateName::FIXEDDOWN;
case 2: case 2:
@ -67,31 +67,25 @@ void StateBalanceTest::calcTorque() {
_ddPcd = Kp_p_ * Vec3((pcd_ - pose_body_).data) + Kd_p_ * Vec3( _ddPcd = Kp_p_ * Vec3((pcd_ - pose_body_).data) + Kd_p_ * Vec3(
(KDL::Vector(0, 0, 0) - vel_body_).data); (KDL::Vector(0, 0, 0) - vel_body_).data);
std::cout << "ddPcd: " << Vec3(vel_body_.data) << std::endl;
// expected body angular acceleration // expected body angular acceleration
const KDL::Rotation B2G_RotMat = ctrlComp_.estimator_.get().getRotation(); const KDL::Rotation B2G_Rotation = ctrl_comp_.estimator_.get().getRotation();
const KDL::Rotation G2B_RotMat = B2G_RotMat.Inverse(); const KDL::Rotation G2B_Rotation = B2G_Rotation.Inverse();
_dWbd = kp_w_ * rotationToExp(Rd_ * G2B_RotMat) + _dWbd = kp_w_ * rotationToExp(Rd_ * G2B_Rotation) +
Kd_w_ * (Vec3(0, 0, 0) - Vec3(ctrlComp_.estimator_.get().getGlobalGyro().data)); Kd_w_ * (Vec3(0, 0, 0) - Vec3(ctrl_comp_.estimator_.get().getGlobalGyro().data));
// calculate foot force // calculate foot force
const std::vector contact(4, 1); const std::vector contact(4, 1);
const std::vector<KDL::Vector> foot_force = ctrlComp_.balance_ctrl_.get().calF(_ddPcd, _dWbd, B2G_RotMat, const std::vector<KDL::Vector> foot_force = ctrl_comp_.balance_ctrl_.get().calF(_ddPcd, _dWbd, B2G_Rotation,
ctrlComp_.estimator_.get(). ctrl_comp_.estimator_.get().
getFootPos2Body(), contact); getFootPos2Body(), contact);
std::vector<KDL::JntArray> current_joints = ctrlComp_.robot_model_.get().current_joint_pos_; std::vector<KDL::JntArray> current_joints = ctrl_comp_.robot_model_.get().current_joint_pos_;
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
const auto &foot = foot_force[i]; std::cout<<Vec3(foot_force[i].data).transpose()<<std::endl;
std::cout << "foot_force: " << foot.x() << "," << foot.y() << "," << foot.z() << std::endl; KDL::JntArray torque = ctrl_comp_.robot_model_.get().getTorque(B2G_Rotation*(-foot_force[i]), i);
const KDL::Wrench wrench0(G2B_RotMat * foot, KDL::Vector::Zero());
const KDL::Wrenches wrenches(1, wrench0); // 创建一个包含 wrench0 的容器
KDL::JntArray torque0 = ctrlComp_.robot_model_.get().getTorque(wrenches, i);
for (int j = 0; j < 3; j++) { for (int j = 0; j < 3; j++) {
ctrlComp_.joint_effort_command_interface_[i * 3 + j].get().set_value(torque0(j)); ctrl_comp_.joint_effort_command_interface_[i * 3 + j].get().set_value(torque(j));
ctrlComp_.joint_position_command_interface_[i * 3 + j].get().set_value(current_joints[i](j)); ctrl_comp_.joint_position_command_interface_[i * 3 + j].get().set_value(current_joints[i](j));
} }
} }
} }

16
controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp
View File

@ -8,12 +8,12 @@
StateFixedDown::StateFixedDown(CtrlComponent ctrlComp): FSMState( StateFixedDown::StateFixedDown(CtrlComponent ctrlComp): FSMState(
FSMStateName::FIXEDDOWN, "fixed down", std::move(ctrlComp)) { FSMStateName::FIXEDDOWN, "fixed down", std::move(ctrlComp)) {
duration_ = ctrlComp_.frequency_ * 1.2; duration_ = ctrl_comp_.frequency_ * 1.2;
} }
void StateFixedDown::enter() { void StateFixedDown::enter() {
for (int i = 0; i < 12; i++) { for (int i = 0; i < 12; i++) {
start_pos_[i] = ctrlComp_.joint_position_state_interface_[i].get().get_value(); start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
} }
} }
@ -21,12 +21,12 @@ void StateFixedDown::run() {
percent_ += 1 / duration_; percent_ += 1 / duration_;
phase = std::tanh(percent_); phase = std::tanh(percent_);
for (int i = 0; i < 12; i++) { for (int i = 0; i < 12; i++) {
ctrlComp_.joint_position_command_interface_[i].get().set_value( ctrl_comp_.joint_position_command_interface_[i].get().set_value(
phase * target_pos_[i] + (1 - phase) * start_pos_[i]); phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
ctrlComp_.joint_velocity_command_interface_[i].get().set_value(0); ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
ctrlComp_.joint_effort_command_interface_[i].get().set_value(0); ctrl_comp_.joint_effort_command_interface_[i].get().set_value(0);
ctrlComp_.joint_kp_command_interface_[i].get().set_value(50.0); ctrl_comp_.joint_kp_command_interface_[i].get().set_value(50.0);
ctrlComp_.joint_kd_command_interface_[i].get().set_value(3.5); ctrl_comp_.joint_kd_command_interface_[i].get().set_value(3.5);
} }
} }
@ -38,7 +38,7 @@ FSMStateName StateFixedDown::checkChange() {
if (percent_ < 1.5) { if (percent_ < 1.5) {
return FSMStateName::FIXEDDOWN; return FSMStateName::FIXEDDOWN;
} }
switch (ctrlComp_.control_inputs_.get().command) { switch (ctrl_comp_.control_inputs_.get().command) {
case 1: case 1:
return FSMStateName::PASSIVE; return FSMStateName::PASSIVE;
case 2: case 2:

16
controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp
View File

@ -8,12 +8,12 @@
StateFixedStand::StateFixedStand(CtrlComponent ctrlComp): FSMState( StateFixedStand::StateFixedStand(CtrlComponent ctrlComp): FSMState(
FSMStateName::FIXEDSTAND, "fixed stand", std::move(ctrlComp)) { FSMStateName::FIXEDSTAND, "fixed stand", std::move(ctrlComp)) {
duration_ = ctrlComp_.frequency_ * 1.2; duration_ = ctrl_comp_.frequency_ * 1.2;
} }
void StateFixedStand::enter() { void StateFixedStand::enter() {
for (int i = 0; i < 12; i++) { for (int i = 0; i < 12; i++) {
start_pos_[i] = ctrlComp_.joint_position_state_interface_[i].get().get_value(); start_pos_[i] = ctrl_comp_.joint_position_state_interface_[i].get().get_value();
} }
} }
@ -21,13 +21,13 @@ void StateFixedStand::run() {
percent_ += 1 / duration_; percent_ += 1 / duration_;
phase = std::tanh(percent_); phase = std::tanh(percent_);
for (int i = 0; i < 12; i++) { for (int i = 0; i < 12; i++) {
ctrlComp_.joint_position_command_interface_[i].get().set_value( ctrl_comp_.joint_position_command_interface_[i].get().set_value(
phase * target_pos_[i] + (1 - phase) * start_pos_[i]); phase * target_pos_[i] + (1 - phase) * start_pos_[i]);
ctrlComp_.joint_velocity_command_interface_[i].get().set_value(0); ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
ctrlComp_.joint_effort_command_interface_[i].get().set_value(0); ctrl_comp_.joint_effort_command_interface_[i].get().set_value(0);
ctrlComp_.joint_kp_command_interface_[i].get().set_value( ctrl_comp_.joint_kp_command_interface_[i].get().set_value(
phase * 60.0 + (1 - phase) * 20.0); phase * 60.0 + (1 - phase) * 20.0);
ctrlComp_.joint_kd_command_interface_[i].get().set_value(3.5); ctrl_comp_.joint_kd_command_interface_[i].get().set_value(3.5);
} }
} }
@ -39,7 +39,7 @@ FSMStateName StateFixedStand::checkChange() {
if (percent_ < 1.5) { if (percent_ < 1.5) {
return FSMStateName::FIXEDSTAND; return FSMStateName::FIXEDSTAND;
} }
switch (ctrlComp_.control_inputs_.get().command) { switch (ctrl_comp_.control_inputs_.get().command) {
case 1: case 1:
return FSMStateName::FIXEDDOWN; return FSMStateName::FIXEDDOWN;
case 3: case 3:

28
controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp
View File

@ -19,12 +19,12 @@ StateFreeStand::StateFreeStand(CtrlComponent ctrl_component) : FSMState(FSMState
void StateFreeStand::enter() { void StateFreeStand::enter() {
for (int i = 0; i < 12; i++) { for (int i = 0; i < 12; i++) {
ctrlComp_.joint_kp_command_interface_[i].get().set_value(180); ctrl_comp_.joint_kp_command_interface_[i].get().set_value(180);
ctrlComp_.joint_kd_command_interface_[i].get().set_value(5); ctrl_comp_.joint_kd_command_interface_[i].get().set_value(5);
} }
init_joint_pos_ = ctrlComp_.robot_model_.get().current_joint_pos_; init_joint_pos_ = ctrl_comp_.robot_model_.get().current_joint_pos_;
init_foot_pos_ = ctrlComp_.robot_model_.get().getFeet2BPositions(); init_foot_pos_ = ctrl_comp_.robot_model_.get().getFeet2BPositions();
fr_init_pos_ = init_foot_pos_[0]; fr_init_pos_ = init_foot_pos_[0];
@ -35,18 +35,18 @@ void StateFreeStand::enter() {
} }
void StateFreeStand::run() { void StateFreeStand::run() {
ctrlComp_.robot_model_.get().update(ctrlComp_); ctrl_comp_.robot_model_.get().update(ctrl_comp_);
calc_body_target(invNormalize(ctrlComp_.control_inputs_.get().lx, row_min_, row_max_), calc_body_target(invNormalize(ctrl_comp_.control_inputs_.get().lx, row_min_, row_max_),
invNormalize(ctrlComp_.control_inputs_.get().ly, pitch_min_, pitch_max_), invNormalize(ctrl_comp_.control_inputs_.get().ly, pitch_min_, pitch_max_),
invNormalize(ctrlComp_.control_inputs_.get().rx, yaw_min_, yaw_max_), invNormalize(ctrl_comp_.control_inputs_.get().rx, yaw_min_, yaw_max_),
invNormalize(ctrlComp_.control_inputs_.get().ry, height_min_, height_max_)); invNormalize(ctrl_comp_.control_inputs_.get().ry, height_min_, height_max_));
} }
void StateFreeStand::exit() { void StateFreeStand::exit() {
} }
FSMStateName StateFreeStand::checkChange() { FSMStateName StateFreeStand::checkChange() {
switch (ctrlComp_.control_inputs_.get().command) { switch (ctrl_comp_.control_inputs_.get().command) {
case 1: case 1:
return FSMStateName::FIXEDDOWN; return FSMStateName::FIXEDDOWN;
case 2: case 2:
@ -68,11 +68,11 @@ void StateFreeStand::calc_body_target(const float row, const float pitch,
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
goal_pos[i] = body_2_fr_pos * init_foot_pos_[i]; goal_pos[i] = body_2_fr_pos * init_foot_pos_[i];
} }
target_joint_pos_ = ctrlComp_.robot_model_.get().getQ(goal_pos); target_joint_pos_ = ctrl_comp_.robot_model_.get().getQ(goal_pos);
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
ctrlComp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0)); ctrl_comp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0));
ctrlComp_.joint_position_command_interface_[i * 3 + 1].get().set_value(target_joint_pos_[i](1)); ctrl_comp_.joint_position_command_interface_[i * 3 + 1].get().set_value(target_joint_pos_[i](1));
ctrlComp_.joint_position_command_interface_[i * 3 + 2].get().set_value(target_joint_pos_[i](2)); ctrl_comp_.joint_position_command_interface_[i * 3 + 2].get().set_value(target_joint_pos_[i](2));
} }
} }

12
controllers/unitree_guide_controller/src/FSM/StatePassive.cpp
View File

@ -12,19 +12,19 @@ StatePassive::StatePassive(CtrlComponent ctrlComp) : FSMState(
} }
void StatePassive::enter() { void StatePassive::enter() {
for (auto i: ctrlComp_.joint_effort_command_interface_) { for (auto i: ctrl_comp_.joint_effort_command_interface_) {
i.get().set_value(0); i.get().set_value(0);
} }
for (auto i: ctrlComp_.joint_position_command_interface_) { for (auto i: ctrl_comp_.joint_position_command_interface_) {
i.get().set_value(0); i.get().set_value(0);
} }
for (auto i: ctrlComp_.joint_velocity_command_interface_) { for (auto i: ctrl_comp_.joint_velocity_command_interface_) {
i.get().set_value(0); i.get().set_value(0);
} }
for (auto i: ctrlComp_.joint_kp_command_interface_) { for (auto i: ctrl_comp_.joint_kp_command_interface_) {
i.get().set_value(0); i.get().set_value(0);
} }
for (auto i: ctrlComp_.joint_kd_command_interface_) { for (auto i: ctrl_comp_.joint_kd_command_interface_) {
i.get().set_value(1); i.get().set_value(1);
} }
} }
@ -36,7 +36,7 @@ void StatePassive::exit() {
} }
FSMStateName StatePassive::checkChange() { FSMStateName StatePassive::checkChange() {
if (ctrlComp_.control_inputs_.get().command == 2) { if (ctrl_comp_.control_inputs_.get().command == 2) {
return FSMStateName::FIXEDDOWN; return FSMStateName::FIXEDDOWN;
} }
return FSMStateName::PASSIVE; return FSMStateName::PASSIVE;

54
controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp
View File

@ -18,19 +18,19 @@ StateSwingTest::StateSwingTest(CtrlComponent ctrlComp): FSMState(
void StateSwingTest::enter() { void StateSwingTest::enter() {
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
ctrlComp_.joint_kp_command_interface_[i].get().set_value(3); ctrl_comp_.joint_kp_command_interface_[i].get().set_value(3);
ctrlComp_.joint_kd_command_interface_[i].get().set_value(2); ctrl_comp_.joint_kd_command_interface_[i].get().set_value(2);
} }
for (int i = 3; i < 12; i++) { for (int i = 3; i < 12; i++) {
ctrlComp_.joint_kp_command_interface_[i].get().set_value(180); ctrl_comp_.joint_kp_command_interface_[i].get().set_value(180);
ctrlComp_.joint_kd_command_interface_[i].get().set_value(5); ctrl_comp_.joint_kd_command_interface_[i].get().set_value(5);
} }
Kp = KDL::Vector(20, 20, 50); Kp = KDL::Vector(20, 20, 50);
Kd = KDL::Vector(5, 5, 20); Kd = KDL::Vector(5, 5, 20);
init_joint_pos_ = ctrlComp_.robot_model_.get().current_joint_pos_; init_joint_pos_ = ctrl_comp_.robot_model_.get().current_joint_pos_;
init_foot_pos_ = ctrlComp_.robot_model_.get().getFeet2BPositions(); init_foot_pos_ = ctrl_comp_.robot_model_.get().getFeet2BPositions();
target_foot_pos_ = init_foot_pos_; target_foot_pos_ = init_foot_pos_;
fr_init_pos_ = init_foot_pos_[0]; fr_init_pos_ = init_foot_pos_[0];
@ -38,29 +38,29 @@ void StateSwingTest::enter() {
} }
void StateSwingTest::run() { void StateSwingTest::run() {
if (ctrlComp_.control_inputs_.get().ly > 0) { if (ctrl_comp_.control_inputs_.get().ly > 0) {
fr_goal_pos_.p.x(invNormalize(ctrlComp_.control_inputs_.get().ly, fr_init_pos_.p.x(), fr_goal_pos_.p.x(invNormalize(ctrl_comp_.control_inputs_.get().ly, fr_init_pos_.p.x(),
fr_init_pos_.p.x() + _xMax, 0, 1)); fr_init_pos_.p.x() + _xMax, 0, 1));
} else { } else {
fr_goal_pos_.p.x(invNormalize(ctrlComp_.control_inputs_.get().ly, fr_init_pos_.p.x() + _xMin, fr_goal_pos_.p.x(invNormalize(ctrl_comp_.control_inputs_.get().ly, fr_init_pos_.p.x() + _xMin,
fr_init_pos_.p.x(), -1, 0)); fr_init_pos_.p.x(), -1, 0));
} }
if (ctrlComp_.control_inputs_.get().lx > 0) { if (ctrl_comp_.control_inputs_.get().lx > 0) {
fr_goal_pos_.p.y(invNormalize(ctrlComp_.control_inputs_.get().lx, fr_init_pos_.p.y(), fr_goal_pos_.p.y(invNormalize(ctrl_comp_.control_inputs_.get().lx, fr_init_pos_.p.y(),
fr_init_pos_.p.y() + _yMax, 0, 1)); fr_init_pos_.p.y() + _yMax, 0, 1));
} else { } else {
fr_goal_pos_.p.y(invNormalize(ctrlComp_.control_inputs_.get().lx, fr_init_pos_.p.y() + _yMin, fr_goal_pos_.p.y(invNormalize(ctrl_comp_.control_inputs_.get().lx, fr_init_pos_.p.y() + _yMin,
fr_init_pos_.p.y(), -1, 0)); fr_init_pos_.p.y(), -1, 0));
} }
if (ctrlComp_.control_inputs_.get().ry > 0) { if (ctrl_comp_.control_inputs_.get().ry > 0) {
fr_goal_pos_.p.z(invNormalize(ctrlComp_.control_inputs_.get().ry, fr_init_pos_.p.z(), fr_goal_pos_.p.z(invNormalize(ctrl_comp_.control_inputs_.get().ry, fr_init_pos_.p.z(),
fr_init_pos_.p.z() + _zMax, 0, 1)); fr_init_pos_.p.z() + _zMax, 0, 1));
} else { } else {
fr_goal_pos_.p.z(invNormalize(ctrlComp_.control_inputs_.get().ry, fr_init_pos_.p.z() + _zMin, fr_goal_pos_.p.z(invNormalize(ctrl_comp_.control_inputs_.get().ry, fr_init_pos_.p.z() + _zMin,
fr_init_pos_.p.z(), -1, 0)); fr_init_pos_.p.z(), -1, 0));
} }
ctrlComp_.robot_model_.get().update(ctrlComp_); ctrl_comp_.robot_model_.get().update(ctrl_comp_);
positionCtrl(); positionCtrl();
torqueCtrl(); torqueCtrl();
} }
@ -69,7 +69,7 @@ void StateSwingTest::exit() {
} }
FSMStateName StateSwingTest::checkChange() { FSMStateName StateSwingTest::checkChange() {
switch (ctrlComp_.control_inputs_.get().command) { switch (ctrl_comp_.control_inputs_.get().command) {
case 1: case 1:
return FSMStateName::FIXEDDOWN; return FSMStateName::FIXEDDOWN;
case 2: case 2:
@ -81,27 +81,25 @@ FSMStateName StateSwingTest::checkChange() {
void StateSwingTest::positionCtrl() { void StateSwingTest::positionCtrl() {
target_foot_pos_[0] = fr_goal_pos_; target_foot_pos_[0] = fr_goal_pos_;
target_joint_pos_ = ctrlComp_.robot_model_.get().getQ(target_foot_pos_); target_joint_pos_ = ctrl_comp_.robot_model_.get().getQ(target_foot_pos_);
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
ctrlComp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0)); ctrl_comp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0));
ctrlComp_.joint_position_command_interface_[i * 3 + 1].get().set_value(target_joint_pos_[i](1)); ctrl_comp_.joint_position_command_interface_[i * 3 + 1].get().set_value(target_joint_pos_[i](1));
ctrlComp_.joint_position_command_interface_[i * 3 + 2].get().set_value(target_joint_pos_[i](2)); ctrl_comp_.joint_position_command_interface_[i * 3 + 2].get().set_value(target_joint_pos_[i](2));
} }
} }
void StateSwingTest::torqueCtrl() const { void StateSwingTest::torqueCtrl() const {
const KDL::Frame fr_current_pos = ctrlComp_.robot_model_.get().getFeet2BPositions(0); const KDL::Frame fr_current_pos = ctrl_comp_.robot_model_.get().getFeet2BPositions(0);
const KDL::Vector pos_goal = fr_goal_pos_.p; const KDL::Vector pos_goal = fr_goal_pos_.p;
const KDL::Vector pos0 = fr_current_pos.p; const KDL::Vector pos0 = fr_current_pos.p;
const KDL::Vector vel0 = ctrlComp_.robot_model_.get().getFeet2BVelocities(0); const KDL::Vector vel0 = ctrl_comp_.robot_model_.get().getFeet2BVelocities(0);
const KDL::Vector force0 = Kp * (pos_goal - pos0) + Kd * (-vel0); const KDL::Vector force0 = Kp * (pos_goal - pos0) + Kd * -vel0;
const KDL::Wrench wrench0(force0, KDL::Vector::Zero()); // 假设力矩为零 KDL::JntArray torque0 = ctrl_comp_.robot_model_.get().getTorque(force0, 0);
const KDL::Wrenches wrenches(1, wrench0); // 创建一个包含 wrench0 的容器
KDL::JntArray torque0 = ctrlComp_.robot_model_.get().getTorque(wrenches, 0);
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
ctrlComp_.joint_effort_command_interface_[i].get().set_value(torque0(i)); ctrl_comp_.joint_effort_command_interface_[i].get().set_value(torque0(i));
} }
} }

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

@ -100,9 +100,9 @@ void BalanceCtrl::calConstraints(const std::vector<int> &contact) {
} }
void BalanceCtrl::solveQP() { void BalanceCtrl::solveQP() {
const int n = _F.size(); const long n = _F.size();
const int m = _ce0.size(); const long m = _ce0.size();
const int p = _ci0.size(); const long p = _ci0.size();
quadprogpp::Matrix<double> G, CE, CI; quadprogpp::Matrix<double> G, CE, CI;
quadprogpp::Vector<double> g0, ce0, ci0, x; quadprogpp::Vector<double> g0, ce0, ci0, x;

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

@ -18,16 +18,22 @@ Estimator::Estimator() {
g_ = KDL::Vector(0, 0, -9.81); g_ = KDL::Vector(0, 0, -9.81);
_dt = 0.002; _dt = 0.002;
_largeVariance = 100; _largeVariance = 100;
for (int i(0); i < Qdig.rows(); ++i) {
Qdig(i) = i < 6 ? 0.0003 : 0.01;
}
_xhat.setZero(); _xhat.setZero();
_u.setZero(); _u.setZero();
A.setZero(); A.setZero();
A.block(0, 0, 3, 3) = I3; A.block(0, 0, 3, 3) = I3;
A.block(0, 3, 3, 3) = I3 * _dt; A.block(0, 3, 3, 3) = I3 * _dt;
A.block(3, 3, 3, 3) = I3; A.block(3, 3, 3, 3) = I3;
A.block(6, 6, 12, 12) = I12; A.block(6, 6, 12, 12) = I12;
B.setZero(); B.setZero();
B.block(3, 0, 3, 3) = I3 * _dt; B.block(3, 0, 3, 3) = I3 * _dt;
C.setZero(); C.setZero();
C.block(0, 0, 3, 3) = -I3; C.block(0, 0, 3, 3) = -I3;
C.block(3, 0, 3, 3) = -I3; C.block(3, 0, 3, 3) = -I3;
@ -42,10 +48,11 @@ Estimator::Estimator() {
C(25, 11) = 1; C(25, 11) = 1;
C(26, 14) = 1; C(26, 14) = 1;
C(27, 17) = 1; C(27, 17) = 1;
P.setIdentity(); P.setIdentity();
P = _largeVariance * P; P = _largeVariance * P;
RInit << 0.008, 0.012, -0.000, -0.009, 0.012, 0.000, 0.009, -0.009, -0.000, RInit_ << 0.008, 0.012, -0.000, -0.009, 0.012, 0.000, 0.009, -0.009, -0.000,
-0.009, -0.009, 0.000, -0.000, 0.000, -0.000, 0.000, -0.000, -0.001, -0.009, -0.009, 0.000, -0.000, 0.000, -0.000, 0.000, -0.000, -0.001,
-0.002, 0.000, -0.000, -0.003, -0.000, -0.001, 0.000, 0.000, 0.000, 0.000, -0.002, 0.000, -0.000, -0.003, -0.000, -0.001, 0.000, 0.000, 0.000, 0.000,
0.012, 0.019, -0.001, -0.014, 0.018, -0.000, 0.014, -0.013, -0.000, 0.012, 0.019, -0.001, -0.014, 0.018, -0.000, 0.014, -0.013, -0.000,
@ -129,8 +136,8 @@ Estimator::Estimator() {
Cu << 268.573, -43.819, -147.211, -43.819, 92.949, 58.082, -147.211, 58.082, Cu << 268.573, -43.819, -147.211, -43.819, 92.949, 58.082, -147.211, 58.082,
302.120; 302.120;
QInit = Qdig.asDiagonal(); QInit_ = Qdig.asDiagonal();
QInit += B * Cu * B.transpose(); QInit_ += B * Cu * B.transpose();
low_pass_filters_.resize(3); low_pass_filters_.resize(3);
@ -142,8 +149,8 @@ Estimator::Estimator() {
void Estimator::update(const CtrlComponent &ctrlComp) { void Estimator::update(const CtrlComponent &ctrlComp) {
if (ctrlComp.robot_model_.get().mass_ == 0) return; if (ctrlComp.robot_model_.get().mass_ == 0) return;
Q = QInit; Q = QInit_;
R = RInit; R = RInit_;
foot_poses_ = ctrlComp.robot_model_.get().getFeet2BPositions(); foot_poses_ = ctrlComp.robot_model_.get().getFeet2BPositions();
foot_vels_ = ctrlComp.robot_model_.get().getFeet2BVelocities(); foot_vels_ = ctrlComp.robot_model_.get().getFeet2BVelocities();
@ -164,22 +171,22 @@ void Estimator::update(const CtrlComponent &ctrlComp) {
const double trust = windowFunc(phase[i], 0.2); const double trust = windowFunc(phase[i], 0.2);
Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) =
(1 + (1 - trust) * _largeVariance) * (1 + (1 - trust) * _largeVariance) *
QInit.block(6 + 3 * i, 6 + 3 * i, 3, 3); QInit_.block(6 + 3 * i, 6 + 3 * i, 3, 3);
R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = R.block(12 + 3 * i, 12 + 3 * i, 3, 3) =
(1 + (1 - trust) * _largeVariance) * (1 + (1 - trust) * _largeVariance) *
RInit.block(12 + 3 * i, 12 + 3 * i, 3, 3); RInit_.block(12 + 3 * i, 12 + 3 * i, 3, 3);
R(24 + i, 24 + i) = R(24 + i, 24 + i) =
(1 + (1 - trust) * _largeVariance) * RInit(24 + i, 24 + i); (1 + (1 - trust) * _largeVariance) * RInit_(24 + i, 24 + i);
} }
_feetPos2Body.segment(3 * i, 3) = Eigen::Map<Eigen::Vector3d>(foot_poses_[i].p.data); _feetPos2Body.segment(3 * i, 3) = Eigen::Map<Eigen::Vector3d>(foot_poses_[i].p.data);
_feetVel2Body.segment(3 * i, 3) = Eigen::Map<Eigen::Vector3d>(foot_vels_[i].data); _feetVel2Body.segment(3 * i, 3) = Eigen::Map<Eigen::Vector3d>(foot_vels_[i].data);
} }
// Acceleration from imu as system input // Acceleration from imu as system input
rotation_ = KDL::Rotation::Quaternion(ctrlComp.imu_state_interface_[0].get().get_value(), rotation_ = KDL::Rotation::Quaternion(ctrlComp.imu_state_interface_[1].get().get_value(),
ctrlComp.imu_state_interface_[1].get().get_value(),
ctrlComp.imu_state_interface_[2].get().get_value(), ctrlComp.imu_state_interface_[2].get().get_value(),
ctrlComp.imu_state_interface_[3].get().get_value()); ctrlComp.imu_state_interface_[3].get().get_value(),
ctrlComp.imu_state_interface_[0].get().get_value());
gyro_ = KDL::Vector(ctrlComp.imu_state_interface_[4].get().get_value(), gyro_ = KDL::Vector(ctrlComp.imu_state_interface_[4].get().get_value(),
ctrlComp.imu_state_interface_[5].get().get_value(), ctrlComp.imu_state_interface_[5].get().get_value(),
@ -190,16 +197,6 @@ void Estimator::update(const CtrlComponent &ctrlComp) {
ctrlComp.imu_state_interface_[9].get().get_value()); ctrlComp.imu_state_interface_[9].get().get_value());
_u = Vec3((rotation_ * acceleration_ + g_).data); _u = Vec3((rotation_ * acceleration_ + g_).data);
const double *rot_mat = rotation_.data;
std::cout << "Rotation Matrix: " << std::endl;
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
std::cout << rot_mat[i * 3 + j] << " ";
}
std::cout << std::endl;
}
_xhat = A * _xhat + B * _u; _xhat = A * _xhat + B * _u;
_yhat = C * _xhat; _yhat = C * _xhat;

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

@ -60,8 +60,8 @@ KDL::Jacobian QuadrupedRobot::getJacobian(const int index) const {
} }
KDL::JntArray QuadrupedRobot::getTorque( KDL::JntArray QuadrupedRobot::getTorque(
const KDL::Wrenches &force, const int index) const { const KDL::Vector &force, const int index) const {
return robot_legs_[index]->calcTorque(current_joint_pos_[index], current_joint_vel_[index], force); return robot_legs_[index]->calcTorque(current_joint_pos_[index], force);
} }
KDL::Vector QuadrupedRobot::getFeet2BVelocities(const int index) const { KDL::Vector QuadrupedRobot::getFeet2BVelocities(const int index) const {

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

@ -5,13 +5,14 @@
#include <memory> #include <memory>
#include "unitree_guide_controller/robot/RobotLeg.h" #include "unitree_guide_controller/robot/RobotLeg.h"
#include <unitree_guide_controller/common/mathTypes.h>
RobotLeg::RobotLeg(const KDL::Chain &chain) { RobotLeg::RobotLeg(const KDL::Chain &chain) {
chain_ = chain; chain_ = chain;
fk_pose_solver_ = std::make_shared<KDL::ChainFkSolverPos_recursive>(chain); fk_pose_solver_ = std::make_shared<KDL::ChainFkSolverPos_recursive>(chain);
ik_pose_solver_ = std::make_shared<KDL::ChainIkSolverPos_LMA>(chain); ik_pose_solver_ = std::make_shared<KDL::ChainIkSolverPos_LMA>(chain);
jac_solver_ = std::make_shared<KDL::ChainJntToJacSolver>(chain); jac_solver_ = std::make_shared<KDL::ChainJntToJacSolver>(chain);
id_solver_ = std::make_shared<KDL::ChainIdSolver_RNE>(chain, KDL::Vector(0, 0, -9.81));
} }
KDL::Frame RobotLeg::calcPEe2B(const KDL::JntArray &joint_positions) const { KDL::Frame RobotLeg::calcPEe2B(const KDL::JntArray &joint_positions) const {
@ -32,10 +33,12 @@ KDL::Jacobian RobotLeg::calcJaco(const KDL::JntArray &joint_positions) const {
return jacobian; return jacobian;
} }
KDL::JntArray RobotLeg::calcTorque(const KDL::JntArray &joint_positions, const KDL::JntArray &joint_velocities, KDL::JntArray RobotLeg::calcTorque(const KDL::JntArray &joint_positions, const KDL::Vector &force) const {
const KDL::Wrenches &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);
KDL::JntArray torque(chain_.getNrOfJoints()); KDL::JntArray torque(chain_.getNrOfJoints());
id_solver_->CartToJnt(joint_positions, joint_velocities, KDL::JntArray(chain_.getNrOfJoints()), force, for (unsigned int i = 0; i < chain_.getNrOfJoints(); ++i) {
torque); torque(i) = torque_eigen(i);
}
return torque; return torque;
} }

2
descriptions/go2_description/config/robot_control.yaml
View File

@ -50,10 +50,10 @@ unitree_guide_controller:
imu_name: "imu_sensor" imu_name: "imu_sensor"
imu_interfaces: imu_interfaces:
- orientation.w
- orientation.x - orientation.x
- orientation.y - orientation.y
- orientation.z - orientation.z
- orientation.w
- angular_velocity.x - angular_velocity.x
- angular_velocity.y - angular_velocity.y
- angular_velocity.z - angular_velocity.z

2
descriptions/go2_description/xacro/ros2_control.xacro
View File

@ -152,10 +152,10 @@
</joint> </joint>
<sensor name="imu_sensor"> <sensor name="imu_sensor">
<state_interface name="orientation.w"/>
<state_interface name="orientation.x"/> <state_interface name="orientation.x"/>
<state_interface name="orientation.y"/> <state_interface name="orientation.y"/>
<state_interface name="orientation.z"/> <state_interface name="orientation.z"/>
<state_interface name="orientation.w"/>
<state_interface name="angular_velocity.x"/> <state_interface name="angular_velocity.x"/>
<state_interface name="angular_velocity.y"/> <state_interface name="angular_velocity.y"/>
<state_interface name="angular_velocity.z"/> <state_interface name="angular_velocity.z"/>

8
hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp
View File

@ -118,10 +118,10 @@ return_type HardwareUnitree::read(const rclcpp::Time &time, const rclcpp::Durati
} }
// imu states // imu states
imu_states_[0] = _lowState.imu_state().quaternion()[0]; imu_states_[0] = _lowState.imu_state().quaternion()[0]; // w
imu_states_[1] = _lowState.imu_state().quaternion()[1]; imu_states_[1] = _lowState.imu_state().quaternion()[1]; // x
imu_states_[2] = _lowState.imu_state().quaternion()[2]; imu_states_[2] = _lowState.imu_state().quaternion()[2]; // y
imu_states_[3] = _lowState.imu_state().quaternion()[3]; imu_states_[3] = _lowState.imu_state().quaternion()[3]; // z
imu_states_[4] = _lowState.imu_state().gyroscope()[0]; imu_states_[4] = _lowState.imu_state().gyroscope()[0];
imu_states_[5] = _lowState.imu_state().gyroscope()[1]; imu_states_[5] = _lowState.imu_state().gyroscope()[1];
imu_states_[6] = _lowState.imu_state().gyroscope()[2]; imu_states_[6] = _lowState.imu_state().gyroscope()[2];