From 79fc0677f53932987ae2a849d59a1304f686d772 Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Tue, 17 Sep 2024 14:03:08 +0800 Subject: [PATCH] can stand but not stable --- .../unitree_guide_controller/FSM/FSMState.h | 4 +- .../control/Estimator.h | 56 +++++++++---------- .../robot/QuadrupedRobot.h | 2 +- .../unitree_guide_controller/robot/RobotLeg.h | 10 +++- .../src/FSM/StateBalanceTest.cpp | 54 ++++++++---------- .../src/FSM/StateFixedDown.cpp | 16 +++--- .../src/FSM/StateFixedStand.cpp | 16 +++--- .../src/FSM/StateFreeStand.cpp | 28 +++++----- .../src/FSM/StatePassive.cpp | 12 ++-- .../src/FSM/StateSwingTest.cpp | 54 +++++++++--------- .../src/control/BalanceCtrl.cpp | 6 +- .../src/control/Estimator.cpp | 39 ++++++------- .../src/robot/QuadrupedRobot.cpp | 4 +- .../src/robot/RobotLeg.cpp | 13 +++-- .../go2_description/config/robot_control.yaml | 2 +- .../go2_description/xacro/ros2_control.xacro | 2 +- .../src/HardwareUnitree.cpp | 8 +-- 17 files changed, 160 insertions(+), 166 deletions(-) diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h index dc21bd5..01d1c92 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h @@ -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 diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h index 5082a23..f2a058d 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h @@ -85,40 +85,38 @@ public: void update(const CtrlComponent &ctrlComp); private: - Eigen::Matrix _xhat; // The state of estimator, position(3)+velocity(3)+feet position(3x4) + Eigen::Matrix _xhat; // The state of estimator, position(3)+velocity(3)+feet position(3x4) - Eigen::Matrix _u; // The input of estimator + Eigen::Matrix _u; // The input of estimator + + Eigen::Matrix _y; // The measurement value of output y + Eigen::Matrix _yhat; // The prediction of output y + Eigen::Matrix A; // The transtion matrix of estimator + Eigen::Matrix B; // The input matrix + Eigen::Matrix C; // The output matrix - Eigen::Matrix _y; // The measurement value of output y - Eigen::Matrix _yhat; // The prediction of output y - Eigen::Matrix A; // The transtion matrix of estimator - Eigen::Matrix B; // The input matrix - Eigen::Matrix C; // The output matrix // Covariance Matrix - Eigen::Matrix P; // Prediction covariance - Eigen::Matrix Ppriori; // Priori prediction covariance - Eigen::Matrix Q; // Dynamic simulation covariance - Eigen::Matrix R; // Measurement covariance - Eigen::Matrix - QInit; // Initial value of Dynamic simulation covariance - Eigen::Matrix - RInit; // Initial value of Measurement covariance - Eigen::Matrix Qdig; // adjustable process noise covariance - Eigen::Matrix Cu; // The covariance of system input u + Eigen::Matrix P; // Prediction covariance + Eigen::Matrix Ppriori; // Priori prediction covariance + Eigen::Matrix Q; // Dynamic simulation covariance + Eigen::Matrix R; // Measurement covariance + Eigen::Matrix QInit_; // Initial value of Dynamic simulation covariance + Eigen::Matrix RInit_; // Initial value of Measurement covariance + Eigen::Matrix Qdig; // adjustable process noise covariance + Eigen::Matrix Cu; // The covariance of system input u + // Output Measurement - Eigen::Matrix - _feetPos2Body; // The feet positions to body, in the global coordinate - Eigen::Matrix - _feetVel2Body; // The feet velocity to body, in the global coordinate - Eigen::Matrix - _feetH; // The Height of each foot, in the global coordinate - Eigen::Matrix S; // _S = C*P*C.T + R + Eigen::Matrix _feetPos2Body; // The feet positions to body, in the global coordinate + Eigen::Matrix _feetVel2Body; // The feet velocity to body, in the global coordinate + Eigen::Matrix _feetH; // The Height of each foot, in the global coordinate + + Eigen::Matrix S; // _S = C*P*C.T + R Eigen::PartialPivLU > Slu; // _S.lu() - Eigen::Matrix Sy; // _Sy = _S.inv() * (y - yhat) - Eigen::Matrix Sc; // _Sc = _S.inv() * C - Eigen::Matrix SR; // _SR = _S.inv() * R - Eigen::Matrix STC; // _STC = (_S.transpose()).inv() * C - Eigen::Matrix IKC; // _IKC = I - KC + Eigen::Matrix Sy; // _Sy = _S.inv() * (y - yhat) + Eigen::Matrix Sc; // _Sc = _S.inv() * C + Eigen::Matrix SR; // _SR = _S.inv() * R + Eigen::Matrix STC; // _STC = (_S.transpose()).inv() * C + Eigen::Matrix IKC; // _IKC = I - KC KDL::Vector g_; double _dt; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h index 466b08f..611c621 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h @@ -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 diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/RobotLeg.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/RobotLeg.h index bbe0874..dad8626 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/RobotLeg.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/RobotLeg.h @@ -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 fk_pose_solver_; std::shared_ptr jac_solver_; std::shared_ptr ik_pose_solver_; - std::shared_ptr id_solver_; }; diff --git a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp index 98ca3d8..4d9010e 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp @@ -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 foot_force = ctrlComp_.balance_ctrl_.get().calF(_ddPcd, _dWbd, B2G_RotMat, - ctrlComp_.estimator_.get(). + const std::vector foot_force = ctrl_comp_.balance_ctrl_.get().calF(_ddPcd, _dWbd, B2G_Rotation, + ctrl_comp_.estimator_.get(). getFootPos2Body(), contact); - std::vector current_joints = ctrlComp_.robot_model_.get().current_joint_pos_; + std::vector 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< 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)); } } diff --git a/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp b/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp index 7520aba..b1be7c4 100644 --- a/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp +++ b/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp @@ -100,9 +100,9 @@ void BalanceCtrl::calConstraints(const std::vector &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 G, CE, CI; quadprogpp::Vector g0, ce0, ci0, x; diff --git a/controllers/unitree_guide_controller/src/control/Estimator.cpp b/controllers/unitree_guide_controller/src/control/Estimator.cpp index 2b74268..fc62055 100644 --- a/controllers/unitree_guide_controller/src/control/Estimator.cpp +++ b/controllers/unitree_guide_controller/src/control/Estimator.cpp @@ -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(foot_poses_[i].p.data); _feetVel2Body.segment(3 * i, 3) = Eigen::Map(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; diff --git a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp index 581a370..022e1d0 100644 --- a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp +++ b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp @@ -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 { diff --git a/controllers/unitree_guide_controller/src/robot/RobotLeg.cpp b/controllers/unitree_guide_controller/src/robot/RobotLeg.cpp index cfc4d87..d9f06e6 100644 --- a/controllers/unitree_guide_controller/src/robot/RobotLeg.cpp +++ b/controllers/unitree_guide_controller/src/robot/RobotLeg.cpp @@ -5,13 +5,14 @@ #include #include "unitree_guide_controller/robot/RobotLeg.h" +#include + RobotLeg::RobotLeg(const KDL::Chain &chain) { chain_ = chain; fk_pose_solver_ = std::make_shared(chain); ik_pose_solver_ = std::make_shared(chain); jac_solver_ = std::make_shared(chain); - id_solver_ = std::make_shared(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 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; } diff --git a/descriptions/go2_description/config/robot_control.yaml b/descriptions/go2_description/config/robot_control.yaml index 711b2aa..db8e7b3 100644 --- a/descriptions/go2_description/config/robot_control.yaml +++ b/descriptions/go2_description/config/robot_control.yaml @@ -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 diff --git a/descriptions/go2_description/xacro/ros2_control.xacro b/descriptions/go2_description/xacro/ros2_control.xacro index 1976767..ca6a705 100644 --- a/descriptions/go2_description/xacro/ros2_control.xacro +++ b/descriptions/go2_description/xacro/ros2_control.xacro @@ -152,10 +152,10 @@ + - diff --git a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp b/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp index 6caa434..6d94b26 100644 --- a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp +++ b/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp @@ -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];