HiveCoreRD/hivecore_robot_motion
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add leg movement

Browse Source
This commit is contained in:
NuoDaJia
2025-10-19 21:38:40 +08:00
parent f4867f2702
commit fe42a1e25e
12 changed files with 137 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/robot_control/include/control_base.hpp
View File

@@ -3,6 +3,7 @@
#include <Eigen/Dense>
#include "rclcpp/rclcpp.hpp"
#include "trapezoidal_trajectory.hpp"
#include "motion_parameters.hpp"
namespace robot_control
{

4
src/robot_control/include/leg_control.hpp
View File

@@ -28,11 +28,11 @@ namespace robot_control
~LegControl() override;
bool CheckMoveLeg(double moveLegDistance);
bool SetMoveLegParametersInternal(double moveDistance);
bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
bool MoveUp(const double move_length, double dt, std::vector<double>& joint_positions);
bool MoveUp(double dt, std::vector<double>& joint_positions);
};
} // namespace robot_control

66
src/robot_control/include/motion_parameters.hpp
View File

@@ -5,6 +5,14 @@
#define CYCLE_TIME 4 // 插补周期（毫秒）
#ifndef DEG2RAD
#define DEG2RAD(x) ((x)*0.017453293)
#endif
#ifndef RAD2DEG
#define RAD2DEG(x) ((x)*57.29578)
#endif
namespace robot_control
{
enum class LimitType {
@@ -46,7 +54,8 @@ namespace robot_control
// 腿长参数 unit m
leg_length_ = {
0.38,
0.36
0.36,
0.66
};
waist_length_ = {
@@ -61,29 +70,29 @@ namespace robot_control
// 关节速度参数
max_joint_velocity_ = {
5,
5,
5,
5,
5,
5,
5,
5,
5,
5,
}; // 10 度/秒
25,
25,
25,
25,
25,
25,
25,
25,
25,
25
};
max_joint_acceleration_ = {
25,
25,
25,
25,
25,
25,
25,
25,
25,
25,
250,
250,
250,
250,
250,
250,
250,
250,
250,
250
};
// 初始化关节索引
@@ -107,6 +116,8 @@ namespace robot_control
joint_resolution_ = {524288, 131072 , 131072, 131072, 131072, 131072, 131072, 131072, 131072, 131072}; // 分辨率 都一样
//TODO: 限位需要修改
joint_limits_ = {
JointLimit(0, 30.0, -30.0, LimitType::POSITION),
JointLimit(1, 180.0, -180.0, LimitType::POSITION),
@@ -154,6 +165,9 @@ namespace robot_control
0.0
};
// 后腿零点为站直时候的状态, 然后 + 25度, 后腿长 0.66m, 25度为 0.6m
// 前腿零点为站直时候的状态, 然后 髋部 pitch + 49度, 大腿长 0.38m, 38度为 0.25m , 小腿长0.36 膝盖 pitch 再收回 35.5 度, 剩余 13.5度 长度为 0.35m
// 初始化初始位置
leg_home_positions_ = {
0.0,
@@ -166,10 +180,6 @@ namespace robot_control
0.0
};
// 初始化转换参数
radian_to_degree_ = 180.0 / M_PI;
degree_to_radian_ = M_PI / 180.0;
pulse_to_degree_.resize(joint_resolution_.size());
degree_to_pulse_.resize(joint_resolution_.size());
@@ -228,10 +238,6 @@ namespace robot_control
std::vector<double> joint_gear_ratio_;
std::vector<double> joint_resolution_;
// 转换参数
double radian_to_degree_; // 弧度转角度的转换因子
double degree_to_radian_; // 角度转弧度的转换因子
std::vector<double> degree_to_pulse_; // 角度转脉冲的转换因子
std::vector<double> pulse_to_degree_; // 脉冲转角度的转换因子

14
src/robot_control/include/robot_control_manager.hpp
View File

@@ -34,13 +34,8 @@ namespace robot_control
// 检查参数是否合理
bool CheckMoveWheel(double moveWheelDistance, double moveWheelAngle){return wheelController_->CheckMoveWheel(moveWheelDistance, moveWheelAngle);};
bool CheckMoveLeg(double moveLegDistance){return legController_->CheckMoveLeg(moveLegDistance);};
bool SetMoveLegParameters(double moveLegDistance){return legController_->SetMoveLegParametersInternal(moveLegDistance);};
bool SetMoveWaistParameters(double movePitchAngle, double moveYawAngle){return waistController_->SetMoveWaistParametersInternal(movePitchAngle, moveYawAngle);};
// 设置运动参数
void SetMoveLeg(double moveLegDistance){moveLegDistance_ = moveLegDistance;};
void SetMoveWaist(double movePitchAngle, double moveYawAngle){moveWaistPitchAngle_ = movePitchAngle; moveWaistYawAngle_ = moveYawAngle;};
void SetMoveWheel(double moveWheelDistance, double moveWheelAngle){moveWheelDistance_ = moveWheelDistance; moveWheelAngle_ = moveWheelAngle;};
// 机器人关节指令
std_msgs::msg::Float64MultiArray GetJointCommands();
@@ -65,13 +60,6 @@ namespace robot_control
bool isWheelMoving_;
bool isWaistMoving_;
// 运动控制参数
double moveLegDistance_;
double moveWaistPitchAngle_;
double moveWaistYawAngle_;
double moveWheelDistance_;
double moveWheelAngle_;
// 控制器
LegControl* legController_;
WheelControl* wheelController_;

2
src/robot_control/include/robot_control_node.hpp
View File

@@ -88,7 +88,7 @@ namespace robot_control
bool isJogMode_;
int jogDirection_;
int jogIndex_;
size_t jogIndex_;
double jogValue_;
void Publish_joint_trajectory();

6
src/robot_control/include/trapezoidal_trajectory.hpp
View File

@@ -39,7 +39,7 @@ public:
* @param target_pos 目标位置（向量，长度=轴数）
* @param duration 期望运动时间（若>0且>最小时间则强制使用该时间）
*/
void init(const std::vector<double>& start_pos, const std::vector<double>& target_pos, double duration = 0.0);
void init(const std::vector<double>& start_pos, const std::vector<double>& target_pos);
/**
* @brief 初始化单轴轨迹规划
@@ -47,7 +47,7 @@ public:
* @param target_pos 目标位置
* @param duration 期望运动时间（若>0且>最小时间则强制使用该时间）
*/
void init(double start_pos, double target_pos, double duration = 0.0);
void init(double start_pos, double target_pos);
/**
* @brief 更新多轴轨迹，计算当前时间的位置
@@ -150,6 +150,8 @@ private:
std::vector<double> max_velocity_; // 最大速度（各轴独立）
std::vector<double> max_acceleration_; // 最大加速度（各轴独立）
std::vector<double> cruise_velocity_; // 巡航速度（各轴独立）
std::vector<double> acceleration_; // 加速度（各轴独立）
std::vector<double> deceleration_; // 减速度（各轴独立）
// 统一时间参数（所有轴共用）
double total_time_; // 总运动时间

2
src/robot_control/include/waist_control.hpp
View File

@@ -25,7 +25,7 @@ namespace robot_control
bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
bool MoveWaist(const std::vector<double> waistAngles, double dt, std::vector<double>& joint_positions);
bool MoveWaist(double dt, std::vector<double>& joint_positions);
};
}

73
src/robot_control/src/leg_control.cpp
View File

@@ -50,36 +50,83 @@ bool LegControl::MoveToTargetPoint(std::vector<double>& joint_positions, const s
return MoveToTargetJoint(joint_positions, joint_target, dt);
}
bool LegControl::MoveUp(const double move_length, double dt, std::vector<double>& joint_positions)
bool LegControl::MoveUp(double dt, std::vector<double>& joint_positions)
{
if (!is_moving_)
{
for (size_t i = 0; i < joint_position_.size(); i++)
if (!is_target_set_)
{
if (joint_directions_[i] == 1)
{
joint_position_desired_[i] = joint_position_[i] + 20;
}
else
{
joint_position_desired_[i] = joint_position_[i] - 20;
std::cout << "Target position not set!" << std::endl;
return true;
}
}
}
return MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
bool result = MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
if (result)
{
is_target_set_ = false;
}
return result;
}
bool LegControl::CheckMoveLeg(double moveLegDistance)
bool LegControl::SetMoveLegParametersInternal(double moveDistance)
{
if (moveLegDistance == 0.0)
std::vector<double> tempPosition;
tempPosition.resize(total_joints_, 0.0);
double currentBackLegLength1 = lengthParameters_[3] * abs(std::cos(DEG2RAD(joint_position_[0] - joint_home_positions_[0])));
double currentFrountLegLength1 = lengthParameters_[0] * abs(std::cos(DEG2RAD(joint_position_[1] - joint_home_positions_[1])));
if (moveDistance > 0)
{
return false;
if (moveDistance > (lengthParameters_[0] - currentFrountLegLength1 - 5))
{
std::cout << "Move distance is too large!" << std::endl;
return false;
}
}
else
{
return true;
if (moveDistance < -currentFrountLegLength1 - 5)
{
std::cout << "Move distance is too large!" << std::endl;
return false;
}
}
double finalBackLegLength1 = currentBackLegLength1 + moveDistance;
double finalFrountLegLength1 = currentFrountLegLength1 + moveDistance;
double backFinalAngle = RAD2DEG(std::acos(finalBackLegLength1 / lengthParameters_[3])) + joint_home_positions_[0];
double frontFinalAngle = RAD2DEG(std::acos(finalFrountLegLength1 / lengthParameters_[0])) + joint_home_positions_[1];
double backAdjustAngle = backFinalAngle - joint_position_[0];
double frontAdjustAngle = frontFinalAngle - joint_position_[1];
tempPosition[0] = joint_position_[0] + backAdjustAngle;
tempPosition[1] = joint_position_[1] + frontAdjustAngle;
tempPosition[2] = joint_position_[2];
tempPosition[3] = joint_position_[3] - frontAdjustAngle; //TODO : check the direction
tempPosition[4] = joint_position_[4] + backAdjustAngle;
tempPosition[5] = joint_position_[5] + frontAdjustAngle;
tempPosition[6] = joint_position_[6];
tempPosition[7] = joint_position_[7] - frontAdjustAngle; //TODO : check the direction
if (!checkJointLimits(tempPosition))
{
return false;
}
joint_position_desired_ = tempPosition;
std::cout << "Target Joint Position: " << joint_position_desired_[0] << ", " << joint_position_desired_[1] << std::endl;
is_target_set_ = true;
return true;
}
} // namespace robot_control

4
src/robot_control/src/robot_control_manager.cpp
View File

@@ -311,7 +311,7 @@ bool RobotControlManager::MoveLeg(double dt)
{
AssignTempValues();
bool result = legController_ -> MoveUp(moveLegDistance_, dt,tempLegCmd_);
bool result = legController_ -> MoveUp(dt,tempLegCmd_);
UpdateJointCommands();
@@ -322,7 +322,7 @@ bool RobotControlManager::MoveWaist(double dt)
{
AssignTempValues();
bool result = waistController_ -> MoveWaist({moveWaistPitchAngle_,moveWaistYawAngle_}, dt, tempWaistCmd_);
bool result = waistController_ -> MoveWaist(dt, tempWaistCmd_);
UpdateJointCommands();

4
src/robot_control/src/robot_control_node.cpp
View File

@@ -181,7 +181,7 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_leg_goal(
{
RCLCPP_INFO(this->get_logger(), "Received move leg request: total time");
robotControlManager_.CheckMoveLeg(goal->move_up_distance);
robotControlManager_.SetMoveLegParameters(goal->move_up_distance);
if (robotControlManager_.IsMoving(MovementPart::LEG))
{
@@ -581,7 +581,7 @@ void RobotControlNode::JoyCommandCallback(const std_msgs::msg::String::SharedPtr
}
else
{
std::cout << "Reach the min joint, can't switch to previous axis: " << jogIndex_ + 1 << std::endl;
std::cout << "Reach the min joint, can't switch to previous axis: " << jogIndex_ - 1 << std::endl;
}
}
}

94
src/robot_control/src/trapezoidal_trajectory.cpp
View File

@@ -10,7 +10,7 @@ namespace robot_control
// 多轴构造函数
TrapezoidalTrajectory::TrapezoidalTrajectory(const std::vector<double>& max_velocities,
const std::vector<double>& max_accelerations)
: is_single_joint_(false), is_stopping_(false)
: is_stopping_(false), is_single_joint_(false)
{
if (max_velocities.empty() || max_accelerations.empty())
throw std::invalid_argument("Max velocities and accelerations cannot be empty");
@@ -24,14 +24,11 @@ TrapezoidalTrajectory::TrapezoidalTrajectory(const std::vector<double>& max_velo
max_velocity_ = max_velocities;
max_acceleration_ = max_accelerations;
is_single_joint_ = false;
is_stopping_ = false;
}
// 单轴构造函数
TrapezoidalTrajectory::TrapezoidalTrajectory(double max_velocity, double max_acceleration)
: is_single_joint_(true), is_stopping_(false)
: is_stopping_(false), is_single_joint_(true)
{
if (max_velocity <= 0)
throw std::invalid_argument("Max velocity must be positive");
@@ -40,15 +37,11 @@ TrapezoidalTrajectory::TrapezoidalTrajectory(double max_velocity, double max_acc
max_velocity_ = {max_velocity};
max_acceleration_ = {max_acceleration};
is_single_joint_ = false;
is_stopping_ = false;
}
// 多轴初始化
void TrapezoidalTrajectory::init(const std::vector<double>& start_pos,
const std::vector<double>& target_pos,
double duration)
const std::vector<double>& target_pos)
{
if (start_pos.size() != target_pos.size())
throw std::invalid_argument("Start and target positions must have same size");
@@ -67,37 +60,18 @@ void TrapezoidalTrajectory::init(const std::vector<double>& start_pos,
current_velocity_.resize(dof);
current_acceleration_.resize(dof);
cruise_velocity_.resize(dof);
acceleration_.resize(dof);
deceleration_.resize(dof);
// 计算各轴总位移
for (size_t i = 0; i < dof; ++i)
total_distance_[i] = std::fabs(target_pos[i] - start_pos[i]);
calculateTrajectoryParams();
// 若指定时间大于最小时间，强制调整总时间
if (duration > 0 && duration > total_time_)
{
total_time_ = duration;
cruise_time_ = total_time_ - acceleration_time_ - deceleration_time_;
// 重新计算各轴巡航速度以适配新时间
for (size_t i = 0; i < dof; ++i)
{
if (total_distance_[i] < 1e-6)
{
cruise_velocity_[i] = 0.0;
continue;
}
double accel_dist = 0.5 * max_acceleration_[i] * acceleration_time_ * acceleration_time_;
double decel_dist = cruise_velocity_[i] * deceleration_time_
- 0.5 * max_acceleration_[i] * deceleration_time_ * deceleration_time_;
cruise_velocity_[i] = (total_distance_[i] - accel_dist - decel_dist) / cruise_time_;
cruise_velocity_[i] = std::min(cruise_velocity_[i], max_velocity_[i]);
}
}
}
// 单轴初始化
void TrapezoidalTrajectory::init(double start_pos, double target_pos, double duration)
void TrapezoidalTrajectory::init(double start_pos, double target_pos)
{
is_single_joint_ = true;
is_stopping_ = false;
@@ -109,23 +83,10 @@ void TrapezoidalTrajectory::init(double start_pos, double target_pos, double dur
current_velocity_.resize(1);
current_acceleration_.resize(1);
cruise_velocity_.resize(1);
acceleration_.resize(1);
deceleration_.resize(1);
calculateTrajectoryParams();
// 强制调整时间（单轴版）
if (duration > 0 && duration > total_time_)
{
total_time_ = duration;
cruise_time_ = total_time_ - acceleration_time_ - deceleration_time_;
if (total_distance_[0] >= 1e-6)
{
double accel_dist = 0.5 * max_acceleration_[0] * acceleration_time_ * acceleration_time_;
double decel_dist = cruise_velocity_[0] * deceleration_time_
- 0.5 * max_acceleration_[0] * deceleration_time_ * deceleration_time_;
cruise_velocity_[0] = (total_distance_[0] - accel_dist - decel_dist) / cruise_time_;
cruise_velocity_[0] = std::min(cruise_velocity_[0], max_velocity_[0]);
}
}
}
// 核心：计算轨迹参数（统一时间+各轴巡航速度）
@@ -180,8 +141,17 @@ void TrapezoidalTrajectory::calculateTrajectoryParams()
}
}
// 步骤2：确定统一时间参数（以最长理论时间为基准）
total_time_ = *std::max_element(axis_total_time.begin(), axis_total_time.end());
for (size_t i = 0; i < axis_total_time.size(); i++)
{
if (total_time_ < axis_total_time[i])
{
total_time_ = axis_total_time[i];
acceleration_time_ = axis_accel_time[i];
deceleration_time_ = axis_decel_time[i];
cruise_time_ = axis_cruise_time[i];
}
}
if (total_time_ < 1e-6) // 所有轴位移为0
{
acceleration_time_ = 0.0;
@@ -191,11 +161,6 @@ void TrapezoidalTrajectory::calculateTrajectoryParams()
return;
}
// 统一加速/减速时间（取各轴理论加速/减速时间的最大值，确保所有轴能完成加速）
acceleration_time_ = *std::max_element(axis_accel_time.begin(), axis_accel_time.end());
deceleration_time_ = *std::max_element(axis_decel_time.begin(), axis_decel_time.end());
cruise_time_ = total_time_ - acceleration_time_ - deceleration_time_;
// 步骤3：计算各轴最终巡航速度（适配统一时间）
for (size_t i = 0; i < dof; ++i)
{
@@ -206,21 +171,10 @@ void TrapezoidalTrajectory::calculateTrajectoryParams()
continue;
}
double a = max_acceleration_[i];
// 加速阶段位移
double dist_accel = 0.5 * a * acceleration_time_ * acceleration_time_;
// 减速阶段位移（基于巡航速度和统一减速时间）
double v_cruise_i = std::min(axis_cruise_vel[i], max_velocity_[i]); // 初步限制
double dist_decel = v_cruise_i * deceleration_time_
- 0.5 * a * deceleration_time_ * deceleration_time_;
// 修正巡航速度以适配总位移
if (cruise_time_ > 1e-6)
v_cruise_i = (dist - dist_accel - dist_decel) / cruise_time_;
else
v_cruise_i = std::sqrt((dist * a) / 2); // 无匀速阶段时的速度
// 最终限制（不超过自身最大速度）
cruise_velocity_[i] = std::min(v_cruise_i, max_velocity_[i]);
//TODO: 最大加速度需要保持一致,否则这里的计算会有问题.
cruise_velocity_[i] = dist / (acceleration_time_ + cruise_time_);
acceleration_[i] = cruise_velocity_[i] / acceleration_time_;
deceleration_[i] = cruise_velocity_[i] / deceleration_time_;
}
}
@@ -247,7 +201,7 @@ std::vector<double> TrapezoidalTrajectory::update(double time)
}
double dir = (target_pos_[i] - start_pos_[i]) > 0 ? 1.0 : -1.0; // 方向
double a = max_acceleration_[i];
double a = acceleration_[i];
double v_cruise = cruise_velocity_[i];
double accel = 0.0;
double vel = 0.0;

5
src/robot_control/src/waist_control.cpp
View File

@@ -50,13 +50,14 @@ bool WaistControl::MoveToTargetPoint(std::vector<double>& joint_positions, const
return MoveToTargetJoint(joint_positions, joint_target, dt);
}
bool WaistControl::MoveWaist(const std::vector<double> waistAngles, double dt, std::vector<double>& joint_positions)
bool WaistControl::MoveWaist(double dt, std::vector<double>& joint_positions)
{
if (!is_moving_)
{
if (!is_target_set_)
{
return false;
std::cout << "Target position not set!" << std::endl;
return true;
}
}