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

add swing test mode

This commit is contained in:
Huang Zhenbiao 2024-09-12 20:04:20 +08:00
parent 073f18bff1
commit 7e9e1e346c
11 changed files with 222 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateSwingTest.h
View File

@ -21,9 +21,29 @@ public:
FSMStateName checkChange() override;
private:
void currentJointPos();
void positionCtrl();
void torqueCtrl();
float _xMin, _xMax;
float _yMin, _yMax;
float _zMin, _zMax;
KDL::Vector Kp, Kd;
std::vector<KDL::JntArray> init_joint_pos_;
std::vector<KDL::JntArray> target_joint_pos_;
std::vector<KDL::JntArray> current_joint_pos_;
std::vector<KDL::JntArray> current_joint_vel_;
std::vector<KDL::Frame> init_foot_pos_;
std::vector<KDL::Frame> target_foot_pos_;
KDL::Frame fr_init_pos_;
KDL::Frame fr_goal_pos_;
};

6
controllers/unitree_guide_controller/include/unitree_guide_controller/common/interface.h
View File

@ -34,9 +34,11 @@ struct CtrlComponent {
control_input_msgs::msg::Inputs default_inputs_;
std::reference_wrapper<control_input_msgs::msg::Inputs> control_inputs_;
int frequency_{};
std::shared_ptr<QuadrupedRobot> robot_model_;
CtrlComponent() : control_inputs_(default_inputs_) {
QuadrupedRobot default_robot_model_;
std::reference_wrapper<QuadrupedRobot> robot_model_;
CtrlComponent() : control_inputs_(default_inputs_), default_robot_model_(), robot_model_(default_robot_model_) {
}
void clear() {

14
controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h Normal file
View File

@ -0,0 +1,14 @@
//
// Created by tlab-uav on 24-9-12.
//
#ifndef MATHTOOLS_H
#define MATHTOOLS_H
template<typename T0, typename T1, typename T2>
T1 invNormalize(const T0 value, const T1 min, const T2 max,
const double minLim = -1, const double maxLim = 1) {
return (value - minLim) * (max - min) / (maxLim - minLim) + min;
}
#endif //MATHTOOLS_H

20
controllers/unitree_guide_controller/include/unitree_guide_controller/robotics/QuadrupedRobot.h
View File

@ -6,14 +6,18 @@
#ifndef QUADRUPEDROBOT_H
#define QUADRUPEDROBOT_H
#include <string>
#include <kdl_parser/kdl_parser/kdl_parser.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include "RobotLeg.h"
class QuadrupedRobot {
public:
explicit QuadrupedRobot(const std::string &robot_description);
explicit QuadrupedRobot() = default;
~QuadrupedRobot() = default;
void init(const std::string &robot_description);
/**
* Calculate the joint positions based on the foot end position
@ -31,10 +35,18 @@ public:
*/
[[nodiscard]] std::vector<KDL::Frame> getFeet2BPositions(const std::vector<KDL::JntArray> &joint_positions) const;
protected:
[[nodiscard]] KDL::Frame getFeet2BPositions(const KDL::JntArray &joint_positions, int index) const;
[[nodiscard]] KDL::Jacobian getJacobian(const KDL::JntArray &joint_positions, int index) const;
[[nodiscard]] KDL::JntArray getTorque(const KDL::JntArray &joint_positions,
const KDL::JntArray &joint_velocities,
const KDL::Wrenches &force, int index) const;
private:
double mass_;
std::vector<Robotleg> robot_legs_;
std::vector<std::shared_ptr<RobotLeg> > robot_legs_;
KDL::Chain fr_chain_;
KDL::Chain fl_chain_;

22
controllers/unitree_guide_controller/include/unitree_guide_controller/robotics/RobotLeg.h
View File

@ -8,13 +8,15 @@
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl_parser/kdl_parser/kdl_parser.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
class Robotleg {
class RobotLeg {
public:
explicit Robotleg(const KDL::Chain &chain);
explicit RobotLeg(const KDL::Chain &chain);
~Robotleg() = default;
~RobotLeg() = default;
/**
* Use forward kinematic to calculate the Pose of End effector to Body frame.
@ -31,10 +33,22 @@ public:
*/
[[nodiscard]] KDL::JntArray calcQ(const KDL::Frame &pEe, const KDL::JntArray &q_init) const;
/**
* Calculate the current jacobian matrix.
* @param joint_positions Leg joint positions
* @return jacobian matrix
*/
[[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;
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_;
};

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

@ -24,8 +24,7 @@ void StateFixedDown::run() {
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(
phase * 50.0 + (1 - phase) * 20.0);
ctrlComp_.joint_kp_command_interface_[i].get().set_value(50.0);
ctrlComp_.joint_kd_command_interface_[i].get().set_value(3.5);
}
}
@ -35,7 +34,7 @@ void StateFixedDown::exit() {
}
FSMStateName StateFixedDown::checkChange() {
if (percent_ < 1) {
if (percent_ < 2) {
return FSMStateName::FIXEDDOWN;
}
switch (ctrlComp_.control_inputs_.get().command) {

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

@ -25,8 +25,8 @@ void StateFixedStand::run() {
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(
phase * 50.0 + (1 - phase) * 20.0);
ctrlComp_.joint_kd_command_interface_[i].get().set_value(8);
phase * 60.0 + (1 - phase) * 20.0);
ctrlComp_.joint_kd_command_interface_[i].get().set_value(3.5);
}
}
@ -35,7 +35,7 @@ void StateFixedStand::exit() {
}
FSMStateName StateFixedStand::checkChange() {
if (percent_ < 1) {
if (percent_ < 2) {
return FSMStateName::FIXEDSTAND;
}
switch (ctrlComp_.control_inputs_.get().command) {

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

@ -2,7 +2,9 @@
// Created by biao on 24-9-12.
//
#include <iostream>
#include <unitree_guide_controller/FSM/StateSwingTest.h>
#include <unitree_guide_controller/common/mathTools.h>
StateSwingTest::StateSwingTest(CtrlComponent ctrlComp): FSMState(
FSMStateName::SWINGTEST, "swing test", std::move(ctrlComp)) {
@ -15,9 +17,54 @@ 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);
}
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);
}
Kp = KDL::Vector(20, 20, 50);
Kd = KDL::Vector(5, 5, 20);
current_joint_pos_.resize(4);
current_joint_vel_.resize(4);
currentJointPos();
init_joint_pos_ = current_joint_pos_;
init_foot_pos_ = ctrlComp_.robot_model_.get().getFeet2BPositions(init_joint_pos_);
target_foot_pos_ = init_foot_pos_;
fr_init_pos_ = init_foot_pos_[0];
fr_goal_pos_ = fr_init_pos_;
}
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(),
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_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(),
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_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(),
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_init_pos_.p.z(), -1, 0));
}
positionCtrl();
torqueCtrl();
}
void StateSwingTest::exit() {
@ -33,3 +80,51 @@ FSMStateName StateSwingTest::checkChange() {
return FSMStateName::SWINGTEST;
}
}
void StateSwingTest::currentJointPos() {
for (int i = 0; i < 4; i++) {
KDL::JntArray pos_array(3);
pos_array(0) = ctrlComp_.joint_position_state_interface_[i * 3].get().get_value();
pos_array(1) = ctrlComp_.joint_position_state_interface_[i * 3 + 1].get().get_value();
pos_array(2) = ctrlComp_.joint_position_state_interface_[i * 3 + 2].get().get_value();
current_joint_pos_[i] = pos_array;
KDL::JntArray vel_array(3);
vel_array(0) = ctrlComp_.joint_velocity_state_interface_[i * 3].get().get_value();
vel_array(1) = ctrlComp_.joint_velocity_state_interface_[i * 3 + 1].get().get_value();
vel_array(2) = ctrlComp_.joint_velocity_state_interface_[i * 3 + 2].get().get_value();
current_joint_vel_[i] = vel_array;
}
}
void StateSwingTest::positionCtrl() {
target_foot_pos_[0] = fr_goal_pos_;
target_joint_pos_ = ctrlComp_.robot_model_.get().getQ(target_foot_pos_, init_joint_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));
}
}
void StateSwingTest::torqueCtrl() {
const KDL::Frame fr_current_pos = ctrlComp_.robot_model_.get().getFeet2BPositions(current_joint_pos_[0], 0);
KDL::Jacobian fr_jaco = ctrlComp_.robot_model_.get().getJacobian(current_joint_pos_[0], 0);
const KDL::Vector pos_goal = fr_goal_pos_.p;
const KDL::Vector pos0 = fr_current_pos.p;
const Eigen::Matrix<double, 3, Eigen::Dynamic> linear_jacobian = fr_jaco.data.topRows(3);
Eigen::Product<Eigen::Matrix<double, 3, -1>, Eigen::Matrix<double, -1, 1> > vel_eigen =
linear_jacobian * current_joint_vel_[0].data;
const KDL::Vector vel0(vel_eigen(0), vel_eigen(1), vel_eigen(2));
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(current_joint_pos_[0], current_joint_vel_[0],
wrenches, 0);
for (int i = 0; i < 3; i++) {
ctrlComp_.joint_effort_command_interface_[i].get().set_value(torque0(i));
}
}

3
controllers/unitree_guide_controller/src/UnitreeGuideController.cpp
View File

@ -4,7 +4,6 @@
#include <unitree_guide_controller/UnitreeGuideController.h>
#include <unitree_guide_controller/FSM/StatePassive.h>
#include <kdl_parser/kdl_parser/kdl_parser.hpp>
#include <unitree_guide_controller/robotics/QuadrupedRobot.h>
namespace unitree_guide_controller {
@ -91,7 +90,7 @@ namespace unitree_guide_controller {
"~/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(),
[this](const std_msgs::msg::String::SharedPtr msg) {
// Handle message
ctrl_comp_.robot_model_ = std::make_shared<QuadrupedRobot>(msg->data);
ctrl_comp_.robot_model_.get().init(msg->data);
});
get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);

38
controllers/unitree_guide_controller/src/robotics/QuadrupedRobot.cpp
View File

@ -2,9 +2,10 @@
// Created by biao on 24-9-12.
//
#include <iostream>
#include <unitree_guide_controller/robotics/QuadrupedRobot.h>
QuadrupedRobot::QuadrupedRobot(const std::string &robot_description) {
void QuadrupedRobot::init(const std::string &robot_description) {
KDL::Tree robot_tree;
kdl_parser::treeFromString(robot_description, robot_tree);
@ -13,17 +14,19 @@ QuadrupedRobot::QuadrupedRobot(const std::string &robot_description) {
robot_tree.getChain("base", "RR_foot", rr_chain_);
robot_tree.getChain("base", "RL_foot", rl_chain_);
robot_legs_.emplace_back(fr_chain_);
robot_legs_.emplace_back(fl_chain_);
robot_legs_.emplace_back(rr_chain_);
robot_legs_.emplace_back(rl_chain_);
robot_legs_.resize(4);
robot_legs_[0] = std::make_shared<RobotLeg>(fr_chain_);
robot_legs_[1] = std::make_shared<RobotLeg>(fl_chain_);
robot_legs_[2] = std::make_shared<RobotLeg>(rr_chain_);
robot_legs_[3] = std::make_shared<RobotLeg>(rl_chain_);
std::cout << "robot_legs_.size(): " << robot_legs_.size() << std::endl;
// calculate total mass from urdf
double totoal_mass = 0.0;
mass_ = 0;
for (const auto &[fst, snd]: robot_tree.getSegments()) {
totoal_mass += snd.segment.getInertia().getMass();
mass_ += snd.segment.getInertia().getMass();
}
mass_ = totoal_mass;
}
std::vector<KDL::JntArray> QuadrupedRobot::getQ(const std::vector<KDL::Frame> &pEe_list,
@ -31,7 +34,7 @@ std::vector<KDL::JntArray> QuadrupedRobot::getQ(const std::vector<KDL::Frame> &p
std::vector<KDL::JntArray> result;
result.resize(4);
for (int i(0); i < 4; ++i) {
result.push_back(robot_legs_[i].calcQ(pEe_list[i], q_init[i]));
result[i] = robot_legs_[i]->calcQ(pEe_list[i], q_init[i]);
}
return result;
}
@ -39,8 +42,21 @@ std::vector<KDL::JntArray> QuadrupedRobot::getQ(const std::vector<KDL::Frame> &p
std::vector<KDL::Frame> QuadrupedRobot::getFeet2BPositions(const std::vector<KDL::JntArray> &joint_positions) const {
std::vector<KDL::Frame> result;
result.resize(4);
for (int i(0); i < 4; ++i) {
result.push_back(robot_legs_[i].calcPEe2B(joint_positions[i]));
for (int i = 0; i < 4; i++) {
result[i] = robot_legs_[i]->calcPEe2B(joint_positions[i]);
}
return result;
}
KDL::Frame QuadrupedRobot::getFeet2BPositions(const KDL::JntArray &joint_positions, const int index) const {
return robot_legs_[index]->calcPEe2B(joint_positions);
}
KDL::Jacobian QuadrupedRobot::getJacobian(const KDL::JntArray &joint_positions, const int index) const {
return robot_legs_[index]->calcJaco(joint_positions);
}
KDL::JntArray QuadrupedRobot::getTorque(const KDL::JntArray &joint_positions, const KDL::JntArray &joint_velocities,
const KDL::Wrenches &force, const int index) const {
return robot_legs_[index]->calcTorque(joint_positions, joint_velocities, force);
}

32
controllers/unitree_guide_controller/src/robotics/RobotLeg.cpp
View File

@ -2,28 +2,40 @@
// Created by biao on 24-9-12.
//
#include <iostream>
#include <memory>
#include <unitree_guide_controller/robotics/RobotLeg.h>
Robotleg::Robotleg(const KDL::Chain &chain) {
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 {
KDL::Frame RobotLeg::calcPEe2B(const KDL::JntArray &joint_positions) const {
KDL::Frame pEe;
if (fk_pose_solver_->JntToCart(joint_positions, pEe) < 0) {
std::cerr << "Failed to calculate forward kinematics" << std::endl;
}
fk_pose_solver_->JntToCart(joint_positions, pEe);
return pEe;
}
KDL::JntArray Robotleg::calcQ(const KDL::Frame &pEe, const KDL::JntArray &q_init) const {
KDL::JntArray RobotLeg::calcQ(const KDL::Frame &pEe, const KDL::JntArray &q_init) const {
KDL::JntArray q(chain_.getNrOfJoints());
if (ik_pose_solver_->CartToJnt(q_init, pEe, q) < 0) {
std::cerr << "Failed to calculate inverse kinematics" << std::endl;
}
ik_pose_solver_->CartToJnt(q_init, pEe, q);
return q;
}
KDL::Jacobian RobotLeg::calcJaco(const KDL::JntArray &joint_positions) const {
KDL::Jacobian jacobian(chain_.getNrOfJoints());
jac_solver_->JntToJac(joint_positions, jacobian);
return jacobian;
}
KDL::JntArray RobotLeg::calcTorque(const KDL::JntArray &joint_positions, const KDL::JntArray &joint_velocities,
const KDL::Wrenches &force) const {
KDL::JntArray torque(chain_.getNrOfJoints());
id_solver_->CartToJnt(joint_positions, joint_velocities, KDL::JntArray(chain_.getNrOfJoints()), force,
torque);
return torque;
}