still struggle with trot

This commit is contained in:
Huang Zhenbiao 2024-09-19 14:00:43 +08:00
parent 842a62465b
commit 2ee53e2f51
13 changed files with 185 additions and 86 deletions

View File

@ -62,9 +62,9 @@ private:
// Control Parameters
double gait_height_;
Vec3 pos_error_, vel_error_;
Mat3 Kpp, Kdp, Kdw;
double _kpw;
Mat3 KpSwing, KdSwing;
Mat3 Kpp, Kdp, Kd_w_;
double kp_w_;
Mat3 Kp_swing_, Kd_swing_;
Vec2 _vxLim, _vyLim, _wyawLim;
};

View File

@ -11,6 +11,7 @@
#include "LowPassFilter.h"
class WaveGenerator;
class QuadrupedRobot;
struct CtrlComponent;
@ -106,10 +107,11 @@ public:
private:
CtrlComponent &ctrl_component_;
QuadrupedRobot &robot_model_;
WaveGenerator &wave_generator_;
Eigen::Matrix<double, 18, 1> x_hat_; // 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> y_hat_; // The prediction of output y

View File

@ -5,14 +5,14 @@
#ifndef WAVEGENERATOR_H
#define WAVEGENERATOR_H
#include <sys/time.h>
#include <chrono>
#include <unitree_guide_controller/common/enumClass.h>
#include <unitree_guide_controller/common/mathTypes.h>
inline long long getSystemTime() {
timeval t{};
gettimeofday(&t, nullptr);
return 1000000 * t.tv_sec + t.tv_usec;
const auto now = std::chrono::system_clock::now();
const auto duration = now.time_since_epoch();
return std::chrono::duration_cast<std::chrono::microseconds>(duration).count();
}
class WaveGenerator {

View File

@ -86,6 +86,7 @@ public:
[[nodiscard]] std::vector<KDL::Vector> getFeet2BVelocities() const;
double mass_ = 0;
Vec34 feet_pos_normal_stand_;
std::vector<KDL::JntArray> current_joint_pos_;
std::vector<KDL::JntArray> current_joint_vel_;

View File

@ -40,7 +40,7 @@ void StateBalanceTest::run() {
pcd_(1) = pcd_init_(1) - invNormalize(ctrl_comp_.control_inputs_.get().lx, _yMin, _yMax);
pcd_(2) = pcd_init_(2) + invNormalize(ctrl_comp_.control_inputs_.get().ry, _zMin, _zMax);
const float yaw = invNormalize(ctrl_comp_.control_inputs_.get().rx, _yawMin, _yawMax);
const float yaw = - invNormalize(ctrl_comp_.control_inputs_.get().rx, _yawMin, _yawMax);
Rd_ = rotz(yaw) * init_rotation_;
for (int i = 0; i < 12; i++) {

View File

@ -15,10 +15,10 @@ StateTrotting::StateTrotting(CtrlComponent &ctrlComp) : FSMState(FSMStateName::T
gait_height_ = 0.08;
Kpp = Vec3(70, 70, 70).asDiagonal();
Kdp = Vec3(10, 10, 10).asDiagonal();
_kpw = 780;
Kdw = Vec3(70, 70, 70).asDiagonal();
KpSwing = Vec3(400, 400, 400).asDiagonal();
KdSwing = Vec3(10, 10, 10).asDiagonal();
kp_w_ = 780;
Kd_w_ = Vec3(70, 70, 70).asDiagonal();
Kp_swing_ = Vec3(300,300,300).asDiagonal();
Kd_swing_ = Vec3(10, 10, 10).asDiagonal();
_vxLim << -0.4, 0.4;
_vyLim << -0.3, 0.3;
@ -28,9 +28,10 @@ StateTrotting::StateTrotting(CtrlComponent &ctrlComp) : FSMState(FSMStateName::T
void StateTrotting::enter() {
pcd_ = estimator_.getPosition();
pcd_(2) = -robot_model_.feet_pos_normal_stand_(2, 0);
_vCmdBody.setZero();
_yawCmd = estimator_.getYaw();
Rd = Mat3(KDL::Rotation::RPY(0, 0, _yawCmd).Inverse().data);
Rd = rotz(_yawCmd);
_wCmdGlobal.setZero();
ctrl_comp_.control_inputs_.get().command = 0;
@ -107,7 +108,7 @@ void StateTrotting::calcCmd() {
/* Turning */
_yawCmd = _yawCmd + _dYawCmd * dt_;
Rd = Mat3(KDL::Rotation::RPY(0, 0, _yawCmd).Inverse().data);
Rd = rotz(_yawCmd);
_wCmdGlobal(2) = _dYawCmd;
}
@ -116,8 +117,8 @@ void StateTrotting::calcTau() {
vel_error_ = vel_target_ - vel_body_;
Vec3 dd_pcd = Kpp * pos_error_ + Kdp * vel_error_;
Vec3 d_wbd = _kpw * rotMatToExp(Rd * G2B_RotMat) +
Kdw * (_wCmdGlobal - estimator_.getGlobalGyro());
Vec3 d_wbd = kp_w_ * rotMatToExp(Rd * G2B_RotMat) +
Kd_w_ * (_wCmdGlobal - estimator_.getGlobalGyro());
dd_pcd(0) = saturation(dd_pcd(0), Vec2(-3, 3));
dd_pcd(1) = saturation(dd_pcd(1), Vec2(-3, 3));
@ -127,9 +128,9 @@ void StateTrotting::calcTau() {
d_wbd(1) = saturation(d_wbd(1), Vec2(-40, 40));
d_wbd(2) = saturation(d_wbd(2), Vec2(-10, 10));
const Vec34 pos_feet2_b_global = estimator_.getFeetPos2Body();
const Vec34 pos_feet_body_global = estimator_.getFeetPos2Body();
Vec34 force_feet_global =
-balance_ctrl_.calF(dd_pcd, d_wbd, B2G_RotMat, pos_feet2_b_global, wave_generator_.contact_);
-balance_ctrl_.calF(dd_pcd, d_wbd, B2G_RotMat, pos_feet_body_global, wave_generator_.contact_);
Vec34 pos_feet_global = estimator_.getFeetPos();
@ -137,8 +138,8 @@ void StateTrotting::calcTau() {
for (int i(0); i < 4; ++i) {
if (wave_generator_.contact_(i) == 0) {
force_feet_global.col(i) =
KpSwing * (pos_feet_global_goal_.col(i) - pos_feet_global.col(i)) +
KdSwing * (vel_feet_global_goal_.col(i) - vel_feet_global.col(i));
Kp_swing_ * (pos_feet_global_goal_.col(i) - pos_feet_global.col(i)) +
Kd_swing_ * (vel_feet_global_goal_.col(i) - vel_feet_global.col(i));
}
}
@ -154,19 +155,19 @@ void StateTrotting::calcTau() {
}
void StateTrotting::calcQQd() {
const std::vector<KDL::Frame> pos_feet2_b = robot_model_.getFeet2BPositions();
const std::vector<KDL::Frame> pos_feet_body = robot_model_.getFeet2BPositions();
Vec34 pos_feet2_b_goal, vel_feet2_b_goal;
Vec34 pos_feet_target, vel_feet_target;
for (int i(0); i < 4; ++i) {
pos_feet2_b_goal.col(i) = G2B_RotMat * (pos_feet_global_goal_.col(i) - pos_body_);
vel_feet2_b_goal.col(i) = G2B_RotMat * (vel_feet_global_goal_.col(i) - vel_body_);
// _velFeet2BGoal.col(i) = _G2B_RotMat * (_velFeetGlobalGoal.col(i) -
// _velBody - _B2G_RotMat * (skew(_lowState->getGyro()) * _posFeet2B.col(i))
// ); // c.f formula (6.12)
pos_feet_target.col(i) = G2B_RotMat * (pos_feet_global_goal_.col(i) - pos_body_);
vel_feet_target.col(i) = G2B_RotMat * (vel_feet_global_goal_.col(i) - vel_body_);
// vel_feet2_target.col(i) = G2B_RotMat * (vel_feet_global_goal_.col(i) - vel_body_ - B2G_RotMat * skew(
// estimator_.getGyro()) * Vec3(pos_feet_body[i].p.data)
// ); // c.f formula (6.12)
}
Vec12 q_goal = robot_model_.getQ(pos_feet2_b_goal);
Vec12 qd_goal = robot_model_.getQd(pos_feet2_b, vel_feet2_b_goal);
Vec12 q_goal = robot_model_.getQ(pos_feet_target);
Vec12 qd_goal = robot_model_.getQd(pos_feet_body, vel_feet_target);
for (int i = 0; i < 12; i++) {
ctrl_comp_.joint_position_command_interface_[i].get().set_value(q_goal(i));
ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(qd_goal(i));
@ -176,11 +177,13 @@ void StateTrotting::calcQQd() {
void StateTrotting::calcGain() const {
for (int i(0); i < 4; ++i) {
if (wave_generator_.contact_(i) == 0) {
// swing gain
for (int j = 0; j < 3; j++) {
ctrl_comp_.joint_kp_command_interface_[i * 3 + j].get().set_value(3);
ctrl_comp_.joint_kd_command_interface_[i * 3 + j].get().set_value(2);
}
} else {
// stable gain
for (int j = 0; j < 3; j++) {
ctrl_comp_.joint_kp_command_interface_[i * 3 + j].get().set_value(0.8);
ctrl_comp_.joint_kd_command_interface_[i * 3 + j].get().set_value(0.8);

View File

@ -9,7 +9,8 @@
#include "unitree_guide_controller/control/CtrlComponent.h"
Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_component),
robot_model_(ctrl_component.robot_model_) {
robot_model_(ctrl_component.robot_model_),
wave_generator_(ctrl_component.wave_generator_) {
g_ << 0, 0, -9.81;
_dt = 0.002;
_largeVariance = 100;
@ -18,7 +19,7 @@ Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_compo
}
x_hat_.setZero();
_u.setZero();
u_.setZero();
A.setZero();
A.block(0, 0, 3, 3) = I3;
@ -155,19 +156,16 @@ void Estimator::update() {
foot_vels_ = robot_model_.getFeet2BVelocities();
_feetH.setZero();
const std::vector contact(4, 1);
const std::vector phase(4, 0.5);
// Adjust the covariance based on foot contact and phase.
for (int i(0); i < 4; ++i) {
if (contact[i] == 0) {
if (wave_generator_.contact_[i] == 0) {
// foot not contact
Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3);
R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3);
R(24 + i, 24 + i) = _largeVariance;
} else {
// foot contact
const double trust = windowFunc(phase[i], 0.2);
const double trust = windowFunc(wave_generator_.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);
@ -209,8 +207,8 @@ void Estimator::update() {
// ctrl_component_.imu_state_interface_[8].get().get_value(),
// ctrl_component_.imu_state_interface_[9].get().get_value());
_u = rotation_ * acceleration_ + g_;
x_hat_ = A * x_hat_ + B * _u;
u_ = rotation_ * acceleration_ + g_;
x_hat_ = A * x_hat_ + B * u_;
y_hat_ = C * x_hat_;
// Update the measurement value

View File

@ -18,7 +18,8 @@ void FeetEndCtrl::init() {
t_stance_ = ctrl_component_.wave_generator_.get_t_stance();
t_swing_ = ctrl_component_.wave_generator_.get_t_swing();
Vec34 feet_pos_body = estimator_.getFeetPos2Body();
// Vec34 feet_pos_body = estimator_.getFeetPos2Body();
Vec34 feet_pos_body = robot_model_.feet_pos_normal_stand_;
for (int i(0); i < 4; ++i) {
feet_radius_(i) =
sqrt(pow(feet_pos_body(0, i), 2) + pow(feet_pos_body(1, i), 2));

View File

@ -8,10 +8,11 @@
#include "unitree_guide_controller/control/CtrlComponent.h"
GaitGenerator::GaitGenerator(CtrlComponent &ctrl_component) : estimator_(ctrl_component.estimator_),
feet_end_ctrl_(ctrl_component),
wave_generator_(ctrl_component.wave_generator_) {
GaitGenerator::GaitGenerator(CtrlComponent &ctrl_component) : wave_generator_(ctrl_component.wave_generator_),
estimator_(ctrl_component.estimator_),
feet_end_ctrl_(ctrl_component) {
first_run_ = true;
feet_end_ctrl_.init();
}
void GaitGenerator::setGait(Vec2 vxy_goal_global, const double d_yaw_goal, const double gait_height) {
@ -28,8 +29,8 @@ void GaitGenerator::generate(Vec34 &feet_pos, Vec34 &feet_vel) {
for (int i = 0; i < 4; i++) {
if (wave_generator_.contact_(i) == 1) {
// foot contact the ground
if (wave_generator_.phase_(i) < 0.5) {
// foot contact the ground
start_p_.col(i) = estimator_.getFootPos(i);
}
feet_pos.col(i) = start_p_.col(i);
@ -46,9 +47,9 @@ void GaitGenerator::generate(Vec34 &feet_pos, Vec34 &feet_vel) {
void GaitGenerator::restart() {
first_run_ = true;
vxy_goal_.setZero();
feet_end_ctrl_.init();
}
Vec3 GaitGenerator::getFootPos(const int i) {
Vec3 foot_pos;

View File

@ -31,6 +31,8 @@ void QuadrupedRobot::init(const std::string &robot_description) {
for (const auto &[fst, snd]: robot_tree.getSegments()) {
mass_ += snd.segment.getInertia().getMass();
}
feet_pos_normal_stand_ << 0.1881, 0.1881, -0.1881, -0.1881, -0.1300, 0.1300,
-0.1300, 0.1300, -0.3200, -0.3200, -0.3200, -0.3200;
}
std::vector<KDL::JntArray> QuadrupedRobot::getQ(const std::vector<KDL::Frame> &pEe_list) const {

View File

@ -1,9 +1,62 @@
# Controller Manager configuration
controller_manager:
ros__parameters:
update_rate: 200 # Hz
use_sim_time: true # If running in simulation
update_rate: 500 # Hz
# Define the available controllers
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
type: joint_state_broadcaster/JointStateBroadcaster
imu_sensor_broadcaster:
type: imu_sensor_broadcaster/IMUSensorBroadcaster
unitree_guide_controller:
type: unitree_guide_controller/UnitreeGuideController
imu_sensor_broadcaster:
ros__parameters:
sensor_name: "imu_sensor"
frame_id: "imu_link"
unitree_guide_controller:
ros__parameters:
update_rate: 500 # Hz
joints:
- FR_hip_joint
- FR_thigh_joint
- FR_calf_joint
- FL_hip_joint
- FL_thigh_joint
- FL_calf_joint
- RR_hip_joint
- RR_thigh_joint
- RR_calf_joint
- RL_hip_joint
- RL_thigh_joint
- RL_calf_joint
command_interfaces:
- effort
- position
- velocity
- kp
- kd
state_interfaces:
- effort
- position
- velocity
imu_name: "imu_sensor"
imu_interfaces:
- orientation.w
- orientation.x
- orientation.y
- orientation.z
- angular_velocity.x
- angular_velocity.y
- angular_velocity.z
- linear_acceleration.x
- linear_acceleration.y
- linear_acceleration.z

View File

@ -3,8 +3,8 @@ import os
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
@ -29,43 +29,68 @@ def launch_setup(context, *args, **kwargs):
"robot_control.yaml",
]
)
return [
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[
{
'publish_frequency': 20.0,
'use_tf_static': True,
'robot_description': robot_description,
'ignore_timestamp': True
}
],
),
Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_controllers],
remappings=[
("~/robot_description", "/robot_description"),
],
output="both",
),
Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster",
"--controller-manager", "/controller_manager"],
),
# Node(
# package="controller_manager",
# executable="spawner",
# arguments=["imu_sensor_broadcaster",
# "--controller-manager", "/controller_manager"],
# ),
]
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[
{
'publish_frequency': 20.0,
'use_tf_static': True,
'robot_description': robot_description,
'ignore_timestamp': True
}
],
)
controller_manager = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_controllers],
remappings=[
("~/robot_description", "/robot_description"),
],
output="both",
)
joint_state_publisher = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster",
"--controller-manager", "/controller_manager"],
)
imu_sensor_broadcaster = Node(
package="controller_manager",
executable="spawner",
arguments=["imu_sensor_broadcaster",
"--controller-manager", "/controller_manager"],
)
unitree_guide_controller = Node(
package="controller_manager",
executable="spawner",
arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
)
return [
robot_state_publisher,
controller_manager,
joint_state_publisher,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_publisher,
on_exit=[imu_sensor_broadcaster],
)
),
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=imu_sensor_broadcaster,
on_exit=[unitree_guide_controller],
)
),
]
def generate_launch_description():
robot_type_arg = DeclareLaunchArgument(

View File

@ -151,6 +151,19 @@
<state_interface name="effort"/>
</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="angular_velocity.x"/>
<state_interface name="angular_velocity.y"/>
<state_interface name="angular_velocity.z"/>
<state_interface name="linear_acceleration.x"/>
<state_interface name="linear_acceleration.y"/>
<state_interface name="linear_acceleration.z"/>
</sensor>
</ros2_control>
</robot>