diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h index e1d2ef9..7595984 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h @@ -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; }; 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 3d51587..f8f206c 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 @@ -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 x_hat_; // 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 y_hat_; // The prediction of output y diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/WaveGenerator.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/WaveGenerator.h index 885f2aa..ac7d0f5 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/WaveGenerator.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/WaveGenerator.h @@ -5,14 +5,14 @@ #ifndef WAVEGENERATOR_H #define WAVEGENERATOR_H -#include +#include #include #include 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(duration).count(); } class WaveGenerator { 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 cc3d32c..76bdd18 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 @@ -86,6 +86,7 @@ public: [[nodiscard]] std::vector getFeet2BVelocities() const; double mass_ = 0; + Vec34 feet_pos_normal_stand_; std::vector current_joint_pos_; std::vector current_joint_vel_; diff --git a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp index 6885399..5bbcd46 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp @@ -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++) { diff --git a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp index 4a774d3..3e20dda 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp @@ -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 pos_feet2_b = robot_model_.getFeet2BPositions(); + const std::vector 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); diff --git a/controllers/unitree_guide_controller/src/control/Estimator.cpp b/controllers/unitree_guide_controller/src/control/Estimator.cpp index a3ed982..b86429d 100644 --- a/controllers/unitree_guide_controller/src/control/Estimator.cpp +++ b/controllers/unitree_guide_controller/src/control/Estimator.cpp @@ -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 diff --git a/controllers/unitree_guide_controller/src/gait/FeetEndCtrl.cpp b/controllers/unitree_guide_controller/src/gait/FeetEndCtrl.cpp index 661ad59..37f1686 100644 --- a/controllers/unitree_guide_controller/src/gait/FeetEndCtrl.cpp +++ b/controllers/unitree_guide_controller/src/gait/FeetEndCtrl.cpp @@ -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)); diff --git a/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp b/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp index 463f33b..ea508cb 100644 --- a/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp +++ b/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp @@ -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; diff --git a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp index a18336a..8d34482 100644 --- a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp +++ b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp @@ -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 QuadrupedRobot::getQ(const std::vector &pEe_list) const { diff --git a/descriptions/go1_description/config/robot_control.yaml b/descriptions/go1_description/config/robot_control.yaml index c352b35..db8e7b3 100644 --- a/descriptions/go1_description/config/robot_control.yaml +++ b/descriptions/go1_description/config/robot_control.yaml @@ -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 \ No newline at end of file + 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 \ No newline at end of file diff --git a/descriptions/go1_description/launch/hardware.launch.py b/descriptions/go1_description/launch/hardware.launch.py index ec158ac..615dbb0 100644 --- a/descriptions/go1_description/launch/hardware.launch.py +++ b/descriptions/go1_description/launch/hardware.launch.py @@ -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( diff --git a/descriptions/go1_description/xacro/ros2_control.xacro b/descriptions/go1_description/xacro/ros2_control.xacro index 80dba85..ca6a705 100644 --- a/descriptions/go1_description/xacro/ros2_control.xacro +++ b/descriptions/go1_description/xacro/ros2_control.xacro @@ -151,6 +151,19 @@ + + + + + + + + + + + + +