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 free stand

This commit is contained in:
Huang Zhenbiao 2024-09-13 15:00:20 +08:00
parent 7e9e1e346c
commit 0561c115ba
11 changed files with 204 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
commands/keyboard_input/src/KeyboardInput.cpp
View File

@ -21,6 +21,12 @@ void KeyboardInput::timer_callback() {
char key = getchar();
check_command(key);
if (inputs_.command == 0) check_value(key);
else {
inputs_.lx = 0;
inputs_.ly = 0;
inputs_.rx = 0;
inputs_.ry = 0;
}
publisher_->publish(inputs_);
}
}
@ -110,8 +116,7 @@ bool KeyboardInput::kbhit() {
}
int main(int argc, char *argv[])
{
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
auto node = std::make_shared<KeyboardInput>();
spin(node);

1
controllers/unitree_guide_controller/CMakeLists.txt
View File

@ -29,6 +29,7 @@ add_library(${PROJECT_NAME} SHARED
src/FSM/StateFixedDown.cpp
src/FSM/StateFixedStand.cpp
src/FSM/StateSwingTest.cpp
src/FSM/StateFreeStand.cpp
src/robotics/QuadrupedRobot.cpp
src/robotics/RobotLeg.cpp
)

38
controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFreeStand.h Normal file
View File

@ -0,0 +1,38 @@
//
// Created by tlab-uav on 24-9-13.
//
#ifndef STATEFREESTAND_H
#define STATEFREESTAND_H
#include "FSMState.h"
class StateFreeStand final : public FSMState {
public:
explicit StateFreeStand(CtrlComponent ctrl_component);
void enter() override;
void run() override;
void exit() override;
FSMStateName checkChange() override;
private:
void calc_body_target(float row, float pitch, float yaw, float height);
float row_max_, row_min_;
float pitch_max_, pitch_min_;
float yaw_max_, yaw_min_;
float height_max_, height_min_;
std::vector<KDL::JntArray> init_joint_pos_;
std::vector<KDL::JntArray> target_joint_pos_;
KDL::Frame fr_init_pos_;
std::vector<KDL::Frame> init_foot_pos_; // 4 feet position in fr-foot frame
};
#endif //STATEFREESTAND_H

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

@ -22,10 +22,9 @@ public:
private:
void currentJointPos();
void positionCtrl();
void torqueCtrl();
void torqueCtrl() const;
float _xMin, _xMax;
float _yMin, _yMax;
@ -36,9 +35,6 @@ private:
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_;

2
controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h
View File

@ -12,6 +12,7 @@
#include "FSM/StateFixedDown.h"
#include "FSM/StateFixedStand.h"
#include "FSM/StateFreeStand.h"
#include "FSM/StatePassive.h"
#include "FSM/StateSwingTest.h"
@ -21,6 +22,7 @@ namespace unitree_guide_controller {
std::shared_ptr<StatePassive> passive;
std::shared_ptr<StateFixedDown> fixedDown;
std::shared_ptr<StateFixedStand> fixedStand;
std::shared_ptr<StateFreeStand> freeStand;
std::shared_ptr<StateSwingTest> swingTest;
};

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

@ -11,6 +11,8 @@
#include "RobotLeg.h"
struct CtrlComponent;
class QuadrupedRobot {
public:
explicit QuadrupedRobot() = default;
@ -22,29 +24,47 @@ public:
/**
* Calculate the joint positions based on the foot end position
* @param pEe_list vector of foot-end position
* @param q_init vector of current joint positions
* @return
*/
[[nodiscard]] std::vector<KDL::JntArray> getQ(const std::vector<KDL::Frame> &pEe_list,
const std::vector<KDL::JntArray> &q_init) const;
[[nodiscard]] std::vector<KDL::JntArray> getQ(const std::vector<KDL::Frame> &pEe_list) const;
/**
* Calculate the foot end position based on joint positions
* @param joint_positions vector of current joint positions
* @param joint_positions vector of joint positions
* @return vector of foot-end position
*/
[[nodiscard]] std::vector<KDL::Frame> getFeet2BPositions(const std::vector<KDL::JntArray> &joint_positions) const;
[[nodiscard]] KDL::Frame getFeet2BPositions(const KDL::JntArray &joint_positions, int index) const;
/**
* Calculate the foot end position based on joint positions
* @param index leg index
* @return foot-end position
*/
[[nodiscard]] KDL::Frame getFeet2BPositions(int index) const;
[[nodiscard]] KDL::Jacobian getJacobian(const KDL::JntArray &joint_positions, int index) const;
/**
* Calculate the Jacobian matrix based on joint positions
* @param index leg index
* @return Jacobian matrix
*/
[[nodiscard]] KDL::Jacobian getJacobian(int index) const;
[[nodiscard]] KDL::JntArray getTorque(const KDL::JntArray &joint_positions,
const KDL::JntArray &joint_velocities,
/**
* Calculate the torque based on joint positions, joint velocities and external force
* @param force external force
* @param index leg index
* @return torque
*/
[[nodiscard]] KDL::JntArray getTorque(
const KDL::Wrenches &force, int index) const;
std::vector<KDL::JntArray> current_joint_pos_;
std::vector<KDL::JntArray> current_joint_vel_;
void update(const CtrlComponent &ctrlComp);
private:
double mass_;
double mass_ = 0;
std::vector<std::shared_ptr<RobotLeg> > robot_legs_;

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

@ -41,7 +41,9 @@ FSMStateName StateFixedStand::checkChange() {
switch (ctrlComp_.control_inputs_.get().command) {
case 1:
return FSMStateName::FIXEDDOWN;
case 8:
case 3:
return FSMStateName::FREESTAND;
case 9:
return FSMStateName::SWINGTEST;
default:
return FSMStateName::FIXEDSTAND;

79
controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp Normal file
View File

@ -0,0 +1,79 @@
//
// Created by tlab-uav on 24-9-13.
//
#include <unitree_guide_controller/common/mathTools.h>
#include <unitree_guide_controller/FSM/StateFreeStand.h>
StateFreeStand::StateFreeStand(CtrlComponent ctrl_component) : FSMState(FSMStateName::FREESTAND, "free stand",
std::move(ctrl_component)) {
row_max_ = 20 * M_PI / 180;
row_min_ = -row_max_;
pitch_max_ = 15 * M_PI / 180;
pitch_min_ = -pitch_max_;
yaw_max_ = 20 * M_PI / 180;
yaw_min_ = -yaw_max_;
height_max_ = 0.1;
height_min_ = -height_max_;
}
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);
}
ctrlComp_.robot_model_.get().update(ctrlComp_);
init_joint_pos_ = ctrlComp_.robot_model_.get().current_joint_pos_;
init_foot_pos_ = ctrlComp_.robot_model_.get().getFeet2BPositions(init_joint_pos_);
fr_init_pos_ = init_foot_pos_[0];
for (auto &foot_pos: init_foot_pos_) {
foot_pos.p -= fr_init_pos_.p;
foot_pos.M = KDL::Rotation::RPY(0, 0, 0);
}
}
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_));
}
void StateFreeStand::exit() {
}
FSMStateName StateFreeStand::checkChange() {
switch (ctrlComp_.control_inputs_.get().command) {
case 1:
return FSMStateName::FIXEDDOWN;
case 2:
return FSMStateName::FIXEDSTAND;
default:
return FSMStateName::FREESTAND;
}
}
void StateFreeStand::calc_body_target(const float row, const float pitch,
const float yaw, const float height) {
KDL::Frame fr_2_body_pos;
fr_2_body_pos.p = -fr_init_pos_.p;
fr_2_body_pos.p.z(fr_2_body_pos.p.z() + height);
fr_2_body_pos.M = KDL::Rotation::RPY(row, pitch, yaw);
const KDL::Frame body_2_fr_pos = fr_2_body_pos.Inverse();
std::vector goal_pos(4, KDL::Frame::Identity());
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);
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));
}
}

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

@ -29,11 +29,8 @@ void StateSwingTest::enter() {
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_;
ctrlComp_.robot_model_.get().update(ctrlComp_);
init_joint_pos_ = ctrlComp_.robot_model_.get().current_joint_pos_;
init_foot_pos_ = ctrlComp_.robot_model_.get().getFeet2BPositions(init_joint_pos_);
target_foot_pos_ = init_foot_pos_;
@ -63,6 +60,8 @@ void StateSwingTest::run() {
fr_goal_pos_.p.z(invNormalize(ctrlComp_.control_inputs_.get().ry, fr_init_pos_.p.z() + _zMin,
fr_init_pos_.p.z(), -1, 0));
}
ctrlComp_.robot_model_.get().update(ctrlComp_);
positionCtrl();
torqueCtrl();
}
@ -81,25 +80,9 @@ FSMStateName StateSwingTest::checkChange() {
}
}
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_);
target_joint_pos_ = ctrlComp_.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));
@ -107,22 +90,21 @@ void StateSwingTest::positionCtrl() {
}
}
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);
void StateSwingTest::torqueCtrl() const {
const KDL::Frame fr_current_pos = ctrlComp_.robot_model_.get().getFeet2BPositions(0);
KDL::Jacobian fr_jaco = ctrlComp_.robot_model_.get().getJacobian(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;
linear_jacobian * ctrlComp_.robot_model_.get().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);
KDL::JntArray torque0 = ctrlComp_.robot_model_.get().getTorque(wrenches, 0);
for (int i = 0; i < 3; i++) {
ctrlComp_.joint_effort_command_interface_[i].get().set_value(torque0(i));

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

@ -118,6 +118,7 @@ namespace unitree_guide_controller {
state_list_.fixedDown = std::make_shared<StateFixedDown>(ctrl_comp_);
state_list_.fixedStand = std::make_shared<StateFixedStand>(ctrl_comp_);
state_list_.swingTest = std::make_shared<StateSwingTest>(ctrl_comp_);
state_list_.freeStand = std::make_shared<StateFreeStand>(ctrl_comp_);
// Initialize FSM
current_state_ = state_list_.passive;
@ -160,8 +161,8 @@ namespace unitree_guide_controller {
return state_list_.fixedDown;
case FSMStateName::FIXEDSTAND:
return state_list_.fixedStand;
// case FSMStateName::FREESTAND:
// return state_list_.freeStand;
case FSMStateName::FREESTAND:
return state_list_.freeStand;
// case FSMStateName::TROTTING:
// return state_list_.trotting;
case FSMStateName::SWINGTEST:

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

@ -3,6 +3,7 @@
//
#include <iostream>
#include <unitree_guide_controller/common/interface.h>
#include <unitree_guide_controller/robotics/QuadrupedRobot.h>
void QuadrupedRobot::init(const std::string &robot_description) {
@ -20,6 +21,9 @@ void QuadrupedRobot::init(const std::string &robot_description) {
robot_legs_[2] = std::make_shared<RobotLeg>(rr_chain_);
robot_legs_[3] = std::make_shared<RobotLeg>(rl_chain_);
current_joint_pos_.resize(4);
current_joint_vel_.resize(4);
std::cout << "robot_legs_.size(): " << robot_legs_.size() << std::endl;
// calculate total mass from urdf
@ -29,12 +33,11 @@ void QuadrupedRobot::init(const std::string &robot_description) {
}
}
std::vector<KDL::JntArray> QuadrupedRobot::getQ(const std::vector<KDL::Frame> &pEe_list,
const std::vector<KDL::JntArray> &q_init) const {
std::vector<KDL::JntArray> QuadrupedRobot::getQ(const std::vector<KDL::Frame> &pEe_list) const {
std::vector<KDL::JntArray> result;
result.resize(4);
for (int i(0); i < 4; ++i) {
result[i] = robot_legs_[i]->calcQ(pEe_list[i], q_init[i]);
result[i] = robot_legs_[i]->calcQ(pEe_list[i], current_joint_pos_[i]);
}
return result;
}
@ -48,15 +51,31 @@ std::vector<KDL::Frame> QuadrupedRobot::getFeet2BPositions(const std::vector<KDL
return result;
}
KDL::Frame QuadrupedRobot::getFeet2BPositions(const KDL::JntArray &joint_positions, const int index) const {
return robot_legs_[index]->calcPEe2B(joint_positions);
KDL::Frame QuadrupedRobot::getFeet2BPositions(const int index) const {
return robot_legs_[index]->calcPEe2B(current_joint_pos_[index]);
}
KDL::Jacobian QuadrupedRobot::getJacobian(const KDL::JntArray &joint_positions, const int index) const {
return robot_legs_[index]->calcJaco(joint_positions);
KDL::Jacobian QuadrupedRobot::getJacobian(const int index) const {
return robot_legs_[index]->calcJaco(current_joint_pos_[index]);
}
KDL::JntArray QuadrupedRobot::getTorque(const KDL::JntArray &joint_positions, const KDL::JntArray &joint_velocities,
KDL::JntArray QuadrupedRobot::getTorque(
const KDL::Wrenches &force, const int index) const {
return robot_legs_[index]->calcTorque(joint_positions, joint_velocities, force);
return robot_legs_[index]->calcTorque(current_joint_pos_[index], current_joint_vel_[index], force);
}
void QuadrupedRobot::update(const CtrlComponent &ctrlComp) {
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;
}
}