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)
: state_name(stateName),
state_name_string(std::move(stateNameString)),
ctrlComp_(std::move(ctrlComp)) {
ctrl_comp_(std::move(ctrlComp)) {
}
virtual void enter() = 0;
@ -33,7 +33,7 @@ public:
std::string state_name_string;
protected:
CtrlComponent ctrlComp_;
CtrlComponent ctrl_comp_;
};
#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);
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
Eigen::Matrix<double, 18, 18> P; // Prediction covariance
Eigen::Matrix<double, 18, 18> Ppriori; // Priori prediction covariance
Eigen::Matrix<double, 18, 18> Q; // Dynamic simulation covariance
Eigen::Matrix<double, 28, 28> R; // Measurement covariance
Eigen::Matrix<double, 18, 18>
QInit; // Initial value of Dynamic simulation covariance
Eigen::Matrix<double, 28, 28>
RInit; // Initial value of Measurement covariance
Eigen::Matrix<double, 18, 1> Qdig; // adjustable process noise covariance
Eigen::Matrix<double, 3, 3> Cu; // The covariance of system input u
Eigen::Matrix<double, 18, 18> P; // Prediction covariance
Eigen::Matrix<double, 18, 18> Ppriori; // Priori prediction covariance
Eigen::Matrix<double, 18, 18> Q; // Dynamic simulation covariance
Eigen::Matrix<double, 28, 28> R; // Measurement covariance
Eigen::Matrix<double, 18, 18> QInit_; // Initial value of Dynamic simulation covariance
Eigen::Matrix<double, 28, 28> RInit_; // Initial value of Measurement covariance
Eigen::Matrix<double, 18, 1> Qdig; // adjustable process noise covariance
Eigen::Matrix<double, 3, 3> Cu; // The covariance of system input u
// Output Measurement
Eigen::Matrix<double, 12, 1>
_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, 4, 1>
_feetH; // The Height of each foot, in the global coordinate
Eigen::Matrix<double, 28, 28> S; // _S = C*P*C.T + R
Eigen::Matrix<double, 12, 1> _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, 4, 1> _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::Matrix<double, 28, 1> Sy; // _Sy = _S.inv() * (y - yhat)
Eigen::Matrix<double, 28, 18> Sc; // _Sc = _S.inv() * C
Eigen::Matrix<double, 28, 28> SR; // _SR = _S.inv() * R
Eigen::Matrix<double, 28, 18> STC; // _STC = (_S.transpose()).inv() * C
Eigen::Matrix<double, 18, 18> IKC; // _IKC = I - KC
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, 28> SR; // _SR = _S.inv() * R
Eigen::Matrix<double, 28, 18> STC; // _STC = (_S.transpose()).inv() * C
Eigen::Matrix<double, 18, 18> IKC; // _IKC = I - KC
KDL::Vector g_;
double _dt;

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

@ -55,7 +55,7 @@ public:
* @return torque
*/
[[nodiscard]] KDL::JntArray getTorque(
const KDL::Wrenches &force, int index) const;
const KDL::Vector &force, int index) const;
/**
* 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::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:
KDL::Chain chain_;
std::shared_ptr<KDL::ChainFkSolverPos_recursive> fk_pose_solver_;
std::shared_ptr<KDL::ChainJntToJacSolver> jac_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() {
pcd_ = ctrlComp_.estimator_.get().getPosition();
pcd_ = ctrl_comp_.estimator_.get().getPosition();
pcdInit_ = pcd_;
RdInit_ = ctrlComp_.estimator_.get().getRotation();
RdInit_ = ctrl_comp_.estimator_.get().getRotation();
}
void StateBalanceTest::run() {
pcd_(0) = pcdInit_(0) + invNormalize(ctrlComp_.control_inputs_.get().ly, _xMin, _xMax);
pcd_(1) = pcdInit_(1) - invNormalize(ctrlComp_.control_inputs_.get().lx, _yMin, _yMax);
pcd_(2) = pcdInit_(2) + invNormalize(ctrlComp_.control_inputs_.get().ry, _zMin, _zMax);
const float yaw = invNormalize(ctrlComp_.control_inputs_.get().rx, _yawMin, _yawMax);
pcd_(0) = pcdInit_(0) + invNormalize(ctrl_comp_.control_inputs_.get().ly, _xMin, _xMax);
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_;
pose_body_ = ctrlComp_.estimator_.get().getPosition();
vel_body_ = ctrlComp_.estimator_.get().getVelocity();
calcTorque();
pose_body_ = ctrl_comp_.estimator_.get().getPosition();
vel_body_ = ctrl_comp_.estimator_.get().getVelocity();
for (int i = 0; i < 12; i++) {
ctrlComp_.joint_kp_command_interface_[i].get().set_value(0.8);
ctrlComp_.joint_kd_command_interface_[i].get().set_value(0.8);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(0.8);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(0.8);
}
calcTorque();
}
void StateBalanceTest::exit() {
}
FSMStateName StateBalanceTest::checkChange() {
switch (ctrlComp_.control_inputs_.get().command) {
switch (ctrl_comp_.control_inputs_.get().command) {
case 1:
return FSMStateName::FIXEDDOWN;
case 2:
@ -67,31 +67,25 @@ void StateBalanceTest::calcTorque() {
_ddPcd = Kp_p_ * Vec3((pcd_ - pose_body_).data) + Kd_p_ * Vec3(
(KDL::Vector(0, 0, 0) - vel_body_).data);
std::cout << "ddPcd: " << Vec3(vel_body_.data) << std::endl;
// expected body angular acceleration
const KDL::Rotation B2G_RotMat = ctrlComp_.estimator_.get().getRotation();
const KDL::Rotation G2B_RotMat = B2G_RotMat.Inverse();
_dWbd = kp_w_ * rotationToExp(Rd_ * G2B_RotMat) +
Kd_w_ * (Vec3(0, 0, 0) - Vec3(ctrlComp_.estimator_.get().getGlobalGyro().data));
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));
// calculate foot force
const std::vector contact(4, 1);
const std::vector<KDL::Vector> foot_force = ctrlComp_.balance_ctrl_.get().calF(_ddPcd, _dWbd, B2G_RotMat,
ctrlComp_.estimator_.get().
const std::vector<KDL::Vector> foot_force = ctrl_comp_.balance_ctrl_.get().calF(_ddPcd, _dWbd, B2G_Rotation,
ctrl_comp_.estimator_.get().
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++) {
const auto &foot = foot_force[i];
std::cout << "foot_force: " << foot.x() << "," << foot.y() << "," << foot.z() << std::endl;
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);
std::cout<<Vec3(foot_force[i].data).transpose()<<std::endl;
KDL::JntArray torque = ctrl_comp_.robot_model_.get().getTorque(B2G_Rotation*(-foot_force[i]), i);
for (int j = 0; j < 3; j++) {
ctrlComp_.joint_effort_command_interface_[i * 3 + j].get().set_value(torque0(j));
ctrlComp_.joint_position_command_interface_[i * 3 + j].get().set_value(current_joints[i](j));
ctrl_comp_.joint_effort_command_interface_[i * 3 + j].get().set_value(torque(j));
ctrl_comp_.joint_position_command_interface_[i * 3 + j].get().set_value(current_joints[i](j));
}
}
}

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

@ -8,12 +8,12 @@
StateFixedDown::StateFixedDown(CtrlComponent ctrlComp): FSMState(
FSMStateName::FIXEDDOWN, "fixed down", std::move(ctrlComp)) {
duration_ = ctrlComp_.frequency_ * 1.2;
duration_ = ctrl_comp_.frequency_ * 1.2;
}
void StateFixedDown::enter() {
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_;
phase = std::tanh(percent_);
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]);
ctrlComp_.joint_velocity_command_interface_[i].get().set_value(0);
ctrlComp_.joint_effort_command_interface_[i].get().set_value(0);
ctrlComp_.joint_kp_command_interface_[i].get().set_value(50.0);
ctrlComp_.joint_kd_command_interface_[i].get().set_value(3.5);
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_effort_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(50.0);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(3.5);
}
}
@ -38,7 +38,7 @@ FSMStateName StateFixedDown::checkChange() {
if (percent_ < 1.5) {
return FSMStateName::FIXEDDOWN;
}
switch (ctrlComp_.control_inputs_.get().command) {
switch (ctrl_comp_.control_inputs_.get().command) {
case 1:
return FSMStateName::PASSIVE;
case 2:

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

@ -8,12 +8,12 @@
StateFixedStand::StateFixedStand(CtrlComponent ctrlComp): FSMState(
FSMStateName::FIXEDSTAND, "fixed stand", std::move(ctrlComp)) {
duration_ = ctrlComp_.frequency_ * 1.2;
duration_ = ctrl_comp_.frequency_ * 1.2;
}
void StateFixedStand::enter() {
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_;
phase = std::tanh(percent_);
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]);
ctrlComp_.joint_velocity_command_interface_[i].get().set_value(0);
ctrlComp_.joint_effort_command_interface_[i].get().set_value(0);
ctrlComp_.joint_kp_command_interface_[i].get().set_value(
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_effort_command_interface_[i].get().set_value(0);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(
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) {
return FSMStateName::FIXEDSTAND;
}
switch (ctrlComp_.control_inputs_.get().command) {
switch (ctrl_comp_.control_inputs_.get().command) {
case 1:
return FSMStateName::FIXEDDOWN;
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() {
for (int i = 0; i < 12; i++) {
ctrlComp_.joint_kp_command_interface_[i].get().set_value(180);
ctrlComp_.joint_kd_command_interface_[i].get().set_value(5);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(180);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(5);
}
init_joint_pos_ = ctrlComp_.robot_model_.get().current_joint_pos_;
init_foot_pos_ = ctrlComp_.robot_model_.get().getFeet2BPositions();
init_joint_pos_ = ctrl_comp_.robot_model_.get().current_joint_pos_;
init_foot_pos_ = ctrl_comp_.robot_model_.get().getFeet2BPositions();
fr_init_pos_ = init_foot_pos_[0];
@ -35,18 +35,18 @@ void StateFreeStand::enter() {
}
void StateFreeStand::run() {
ctrlComp_.robot_model_.get().update(ctrlComp_);
calc_body_target(invNormalize(ctrlComp_.control_inputs_.get().lx, row_min_, row_max_),
invNormalize(ctrlComp_.control_inputs_.get().ly, pitch_min_, pitch_max_),
invNormalize(ctrlComp_.control_inputs_.get().rx, yaw_min_, yaw_max_),
invNormalize(ctrlComp_.control_inputs_.get().ry, height_min_, height_max_));
ctrl_comp_.robot_model_.get().update(ctrl_comp_);
calc_body_target(invNormalize(ctrl_comp_.control_inputs_.get().lx, row_min_, row_max_),
invNormalize(ctrl_comp_.control_inputs_.get().ly, pitch_min_, pitch_max_),
invNormalize(ctrl_comp_.control_inputs_.get().rx, yaw_min_, yaw_max_),
invNormalize(ctrl_comp_.control_inputs_.get().ry, height_min_, height_max_));
}
void StateFreeStand::exit() {
}
FSMStateName StateFreeStand::checkChange() {
switch (ctrlComp_.control_inputs_.get().command) {
switch (ctrl_comp_.control_inputs_.get().command) {
case 1:
return FSMStateName::FIXEDDOWN;
case 2:
@ -68,11 +68,11 @@ void StateFreeStand::calc_body_target(const float row, const float pitch,
for (int i = 0; i < 4; 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++) {
ctrlComp_.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));
ctrlComp_.joint_position_command_interface_[i * 3 + 2].get().set_value(target_joint_pos_[i](2));
ctrl_comp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0));
ctrl_comp_.joint_position_command_interface_[i * 3 + 1].get().set_value(target_joint_pos_[i](1));
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() {
for (auto i: ctrlComp_.joint_effort_command_interface_) {
for (auto i: ctrl_comp_.joint_effort_command_interface_) {
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);
}
for (auto i: ctrlComp_.joint_velocity_command_interface_) {
for (auto i: ctrl_comp_.joint_velocity_command_interface_) {
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);
}
for (auto i: ctrlComp_.joint_kd_command_interface_) {
for (auto i: ctrl_comp_.joint_kd_command_interface_) {
i.get().set_value(1);
}
}
@ -36,7 +36,7 @@ void StatePassive::exit() {
}
FSMStateName StatePassive::checkChange() {
if (ctrlComp_.control_inputs_.get().command == 2) {
if (ctrl_comp_.control_inputs_.get().command == 2) {
return FSMStateName::FIXEDDOWN;
}
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() {
for (int i = 0; i < 3; i++) {
ctrlComp_.joint_kp_command_interface_[i].get().set_value(3);
ctrlComp_.joint_kd_command_interface_[i].get().set_value(2);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(3);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(2);
}
for (int i = 3; i < 12; i++) {
ctrlComp_.joint_kp_command_interface_[i].get().set_value(180);
ctrlComp_.joint_kd_command_interface_[i].get().set_value(5);
ctrl_comp_.joint_kp_command_interface_[i].get().set_value(180);
ctrl_comp_.joint_kd_command_interface_[i].get().set_value(5);
}
Kp = KDL::Vector(20, 20, 50);
Kd = KDL::Vector(5, 5, 20);
init_joint_pos_ = ctrlComp_.robot_model_.get().current_joint_pos_;
init_foot_pos_ = ctrlComp_.robot_model_.get().getFeet2BPositions();
init_joint_pos_ = ctrl_comp_.robot_model_.get().current_joint_pos_;
init_foot_pos_ = ctrl_comp_.robot_model_.get().getFeet2BPositions();
target_foot_pos_ = init_foot_pos_;
fr_init_pos_ = init_foot_pos_[0];
@ -38,29 +38,29 @@ void StateSwingTest::enter() {
}
void StateSwingTest::run() {
if (ctrlComp_.control_inputs_.get().ly > 0) {
fr_goal_pos_.p.x(invNormalize(ctrlComp_.control_inputs_.get().ly, fr_init_pos_.p.x(),
if (ctrl_comp_.control_inputs_.get().ly > 0) {
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));
} 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));
}
if (ctrlComp_.control_inputs_.get().lx > 0) {
fr_goal_pos_.p.y(invNormalize(ctrlComp_.control_inputs_.get().lx, fr_init_pos_.p.y(),
if (ctrl_comp_.control_inputs_.get().lx > 0) {
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));
} 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));
}
if (ctrlComp_.control_inputs_.get().ry > 0) {
fr_goal_pos_.p.z(invNormalize(ctrlComp_.control_inputs_.get().ry, fr_init_pos_.p.z(),
if (ctrl_comp_.control_inputs_.get().ry > 0) {
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));
} 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));
}
ctrlComp_.robot_model_.get().update(ctrlComp_);
ctrl_comp_.robot_model_.get().update(ctrl_comp_);
positionCtrl();
torqueCtrl();
}
@ -69,7 +69,7 @@ void StateSwingTest::exit() {
}
FSMStateName StateSwingTest::checkChange() {
switch (ctrlComp_.control_inputs_.get().command) {
switch (ctrl_comp_.control_inputs_.get().command) {
case 1:
return FSMStateName::FIXEDDOWN;
case 2:
@ -81,27 +81,25 @@ FSMStateName StateSwingTest::checkChange() {
void StateSwingTest::positionCtrl() {
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++) {
ctrlComp_.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));
ctrlComp_.joint_position_command_interface_[i * 3 + 2].get().set_value(target_joint_pos_[i](2));
ctrl_comp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0));
ctrl_comp_.joint_position_command_interface_[i * 3 + 1].get().set_value(target_joint_pos_[i](1));
ctrl_comp_.joint_position_command_interface_[i * 3 + 2].get().set_value(target_joint_pos_[i](2));
}
}
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 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::Wrench wrench0(force0, KDL::Vector::Zero()); // 假设力矩为零
const KDL::Wrenches wrenches(1, wrench0); // 创建一个包含 wrench0 的容器
KDL::JntArray torque0 = ctrlComp_.robot_model_.get().getTorque(wrenches, 0);
const KDL::Vector force0 = Kp * (pos_goal - pos0) + Kd * -vel0;
KDL::JntArray torque0 = ctrl_comp_.robot_model_.get().getTorque(force0, 0);
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() {
const int n = _F.size();
const int m = _ce0.size();
const int p = _ci0.size();
const long n = _F.size();
const long m = _ce0.size();
const long p = _ci0.size();
quadprogpp::Matrix<double> G, CE, CI;
quadprogpp::Vector<double> g0, ce0, ci0, x;

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

@ -18,16 +18,22 @@ Estimator::Estimator() {
g_ = KDL::Vector(0, 0, -9.81);
_dt = 0.002;
_largeVariance = 100;
for (int i(0); i < Qdig.rows(); ++i) {
Qdig(i) = i < 6 ? 0.0003 : 0.01;
}
_xhat.setZero();
_u.setZero();
A.setZero();
A.block(0, 0, 3, 3) = I3;
A.block(0, 3, 3, 3) = I3 * _dt;
A.block(3, 3, 3, 3) = I3;
A.block(6, 6, 12, 12) = I12;
B.setZero();
B.block(3, 0, 3, 3) = I3 * _dt;
C.setZero();
C.block(0, 0, 3, 3) = -I3;
C.block(3, 0, 3, 3) = -I3;
@ -42,10 +48,11 @@ Estimator::Estimator() {
C(25, 11) = 1;
C(26, 14) = 1;
C(27, 17) = 1;
P.setIdentity();
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.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,
@ -129,8 +136,8 @@ Estimator::Estimator() {
Cu << 268.573, -43.819, -147.211, -43.819, 92.949, 58.082, -147.211, 58.082,
302.120;
QInit = Qdig.asDiagonal();
QInit += B * Cu * B.transpose();
QInit_ = Qdig.asDiagonal();
QInit_ += B * Cu * B.transpose();
low_pass_filters_.resize(3);
@ -142,8 +149,8 @@ Estimator::Estimator() {
void Estimator::update(const CtrlComponent &ctrlComp) {
if (ctrlComp.robot_model_.get().mass_ == 0) return;
Q = QInit;
R = RInit;
Q = QInit_;
R = RInit_;
foot_poses_ = ctrlComp.robot_model_.get().getFeet2BPositions();
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);
Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) =
(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) =
(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) =
(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);
_feetVel2Body.segment(3 * i, 3) = Eigen::Map<Eigen::Vector3d>(foot_vels_[i].data);
}
// Acceleration from imu as system input
rotation_ = KDL::Rotation::Quaternion(ctrlComp.imu_state_interface_[0].get().get_value(),
ctrlComp.imu_state_interface_[1].get().get_value(),
rotation_ = KDL::Rotation::Quaternion(ctrlComp.imu_state_interface_[1].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(),
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());
_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;
_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(
const KDL::Wrenches &force, const int index) const {
return robot_legs_[index]->calcTorque(current_joint_pos_[index], current_joint_vel_[index], force);
const KDL::Vector &force, const int index) const {
return robot_legs_[index]->calcTorque(current_joint_pos_[index], force);
}
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 "unitree_guide_controller/robot/RobotLeg.h"
#include <unitree_guide_controller/common/mathTypes.h>
RobotLeg::RobotLeg(const KDL::Chain &chain) {
chain_ = chain;
fk_pose_solver_ = std::make_shared<KDL::ChainFkSolverPos_recursive>(chain);
ik_pose_solver_ = std::make_shared<KDL::ChainIkSolverPos_LMA>(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 {
@ -32,10 +33,12 @@ KDL::Jacobian RobotLeg::calcJaco(const KDL::JntArray &joint_positions) const {
return jacobian;
}
KDL::JntArray RobotLeg::calcTorque(const KDL::JntArray &joint_positions, const KDL::JntArray &joint_velocities,
const KDL::Wrenches &force) const {
KDL::JntArray RobotLeg::calcTorque(const KDL::JntArray &joint_positions, const KDL::Vector &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());
id_solver_->CartToJnt(joint_positions, joint_velocities, KDL::JntArray(chain_.getNrOfJoints()), force,
torque);
for (unsigned int i = 0; i < chain_.getNrOfJoints(); ++i) {
torque(i) = torque_eigen(i);
}
return torque;
}

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

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

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

@ -152,10 +152,10 @@
</joint>
<sensor name="imu_sensor">
<state_interface name="orientation.w"/>
<state_interface name="orientation.x"/>
<state_interface name="orientation.y"/>
<state_interface name="orientation.z"/>
<state_interface name="orientation.w"/>
<state_interface name="angular_velocity.x"/>
<state_interface name="angular_velocity.y"/>
<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_[0] = _lowState.imu_state().quaternion()[0];
imu_states_[1] = _lowState.imu_state().quaternion()[1];
imu_states_[2] = _lowState.imu_state().quaternion()[2];
imu_states_[3] = _lowState.imu_state().quaternion()[3];
imu_states_[0] = _lowState.imu_state().quaternion()[0]; // w
imu_states_[1] = _lowState.imu_state().quaternion()[1]; // x
imu_states_[2] = _lowState.imu_state().quaternion()[2]; // y
imu_states_[3] = _lowState.imu_state().quaternion()[3]; // z
imu_states_[4] = _lowState.imu_state().gyroscope()[0];
imu_states_[5] = _lowState.imu_state().gyroscope()[1];
imu_states_[6] = _lowState.imu_state().gyroscope()[2];