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

add new changes

Browse Source
This commit is contained in:
NuoDaJia02
2025-11-01 22:02:57 +08:00
parent 753b72268b
commit 9520d275a8
10 changed files with 212 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -6,6 +6,7 @@
#include "motion_parameters.hpp"
#define POSITION_TOLERANCE 1.0
#define TIME_OUT_COUNT 10000
namespace robot_control
{
@@ -27,7 +28,9 @@ namespace robot_control
std::vector<double> joint_zero_positions_; // 零点位置
std::vector<int> joint_directions_; // 关节运动方向
std::vector<double> joint_position_; // 当前关节位置
std::vector<double> joint_commands_; // 当前关节指令
std::vector<double> joint_velocity_; // 当前关节速度
std::vector<double> joint_velocity_commands_; // 当前关节速度指令
std::vector<double> joint_acceleration_; // 当前关节加速度
std::vector<double> joint_torque_; // 当前关节力矩
std::vector<double> minLimits_; // 关节位置下限
@@ -38,7 +41,9 @@ namespace robot_control
bool is_stopping_; // 是否停止中
bool is_target_set_; // 是否已设置目标点
bool is_cmd_send_finished_;
bool is_joint_position_initialized_; // 是否已初始化关节位置
int time_out_count_; // 超时时间
public:
// 构造函数（子类需调用此构造函数初始化父类）
ControlBase(

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

@@ -56,11 +56,11 @@ namespace robot_control
// 初始化结构参数 unit m
//TODO: 修改为实际参数
wheel_radius_ = 0.09;
wheel_separation_ = 0.26;
wheel_separation_ = 0.55;
wheel_length_ = {
wheel_radius_,
wheel_radius_
wheel_separation_
};
// 腿长参数 unit m
@@ -80,8 +80,8 @@ namespace robot_control
// 关节速度参数
max_joint_velocity_ = {
5,
25,
30,
30,
50,
50,
50,

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

@@ -46,6 +46,7 @@ namespace robot_control
std_msgs::msg::Float64MultiArray GetJointFeedback();
std_msgs::msg::Float64MultiArray GetWheelCommands();
std_msgs::msg::Float64MultiArray GetWheelFeedback();
double GetWheelRatio();
// 机器人状态
void CheckJointLimits(std_msgs::msg::Float64MultiArray& jointCommands);
@@ -55,6 +56,8 @@ namespace robot_control
MotionParameters GetMotionParameters();
std::vector<double> GetImuDifference();
bool IsMoving(MovementPart part);
bool RobotInitFinished();
@@ -91,7 +94,9 @@ namespace robot_control
// 陀螺仪状态
ImuMsg imuMsg_;
bool isGyroInited_;
std::vector<double> gyroValues_; // 陀螺仪数据（弧度/秒）
std::vector<double> gyroInitValues_; // 陀螺仪初始位置
std::vector<double> gyroVelocities_; // 陀螺仪速度（弧度/秒）
std::vector<double> gyroAccelerations_; // 陀螺仪加速度（弧度/秒^2）

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

@@ -16,6 +16,7 @@
#include "interfaces/action/move_waist.hpp"
#include "interfaces/action/move_wheel.hpp"
#include "interfaces/msg/motor_cmd.hpp"
#include "interfaces/srv/motor_param.hpp"
using MoveHome = interfaces::action::MoveHome;
using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;
@@ -31,6 +32,8 @@ using GoalHandleMoveWheel = rclcpp_action::ServerGoalHandle<MoveWheel>;
using MotorCmd = interfaces::msg::MotorCmd;
using MotorParam = interfaces::srv::MotorParam;
namespace robot_control
{
class RobotControlNode : public rclcpp::Node
@@ -81,6 +84,7 @@ namespace robot_control
rclcpp::Publisher<MotorCmd>::SharedPtr motorCmdPub_;
rclcpp::Subscription<MotorPos>::SharedPtr wheelStatesSub_;
rclcpp::Subscription<ImuMsg>::SharedPtr imuMsgSub_;
rclcpp::Client<MotorParam>::SharedPtr motorParamClient_;
RobotControlManager robotControlManager_;

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

@@ -22,6 +22,12 @@ namespace robot_control
~WheelControl() override;
double moveRatio_= 1.0;
double lastMoveDistance = 0.0;
double GetWheelRatioInternal(){ return moveRatio_ ;};
bool SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle);
bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;

14
src/robot_control/src/control_base.cpp
View File

@@ -35,6 +35,8 @@ ControlBase::ControlBase(
joint_acceleration_.resize(total_joints_, 0.0);
joint_torque_.resize(total_joints_, 0.0);
joint_position_desired_.resize(total_joints_, 0.0);
joint_commands_.resize(total_joints_, 0.0);
joint_velocity_commands_.resize(total_joints_, 0.0);
// 初始化梯形轨迹规划器
trapezoidalTrajectory_ = new TrapezoidalTrajectory(maxSpeed, maxAcc);
@@ -43,9 +45,12 @@ ControlBase::ControlBase(
is_stopping_ = false;
is_target_set_ = false;
is_cmd_send_finished_ = false;
is_joint_position_initialized_ = false;
stopDurationTime_ = 0.0;
movingDurationTime_ = 0.0;
time_out_count_ = 0;
}
ControlBase::~ControlBase()
@@ -56,6 +61,7 @@ ControlBase::~ControlBase()
void ControlBase::SetHomePositions(const std::vector<double>& home_positions)
{
joint_home_positions_ = home_positions;
joint_position_desired_ = home_positions;
}
bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const std::vector<double>& target_joint, double dt)
@@ -88,12 +94,14 @@ bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const
if (trapezoidalTrajectory_->isFinished(movingDurationTime_))
{
joint_positions = target_joint;
joint_commands_ = target_joint;
is_moving_ = false;
movingDurationTime_ = 0.0;
return true;
}
joint_commands_ = desired_pos;
joint_positions = desired_pos;
joint_velocity_ = trapezoidalTrajectory_->getVelocity();
joint_acceleration_ = trapezoidalTrajectory_->getAcceleration();
@@ -146,6 +154,12 @@ void ControlBase::UpdateJointStates(
// 2. 更新关节位置（同时同步关节角度，假设位置与角度单位一致）
joint_position_ = joint_positions;
if( !is_joint_position_initialized_)
{
joint_commands_ = joint_position_;
is_joint_position_initialized_ = true;
}
// 3. 更新关节速度
joint_velocity_ = joint_velocities;

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

@@ -72,6 +72,7 @@ void RobotControlManager::init()
isWheelHomed_ = false;
isJointInitValueSet_ = false;
isGyroInited_ = false;
}
RobotControlManager::~RobotControlManager()
@@ -129,6 +130,11 @@ Float64MultiArray RobotControlManager::GetWheelFeedback()
return tempValues;
}
double RobotControlManager::GetWheelRatio()
{
return wheelController_->GetWheelRatioInternal();
}
Float64MultiArray RobotControlManager::GetJointCommands()
{
Float64MultiArray tempValues;
@@ -177,6 +183,21 @@ bool isAllTrueManual(const std::vector<bool>& vec) {
return true; // 所有元素都是 true
}
std::vector<double> RobotControlManager::GetImuDifference()
{
std::vector<double> result;
result.resize(gyroValues_.size(), 0.0);
for (size_t i = 0; i < gyroValues_.size(); i++)
{
result[i] = gyroValues_[i] - gyroInitValues_[i];
}
return result;
}
void RobotControlManager::UpdateImuMsg(const ImuMsg::SharedPtr msg)
{
imuMsg_ = *msg;
@@ -202,6 +223,15 @@ void RobotControlManager::UpdateImuMsg(const ImuMsg::SharedPtr msg)
gyroValues_[2]
);
if (!isGyroInited_)
{
gyroInitValues_ = gyroValues_;
isGyroInited_ = true;
std::cout << "IMU values : " << gyroInitValues_[0] << " " << gyroInitValues_[1] << " " << gyroInitValues_[2] << std::endl;
}
gyroVelocities_[0] = tempMsg.angular_velocity.x;
gyroVelocities_[1] = tempMsg.angular_velocity.y;
gyroVelocities_[2] = tempMsg.angular_velocity.z;
@@ -210,7 +240,7 @@ void RobotControlManager::UpdateImuMsg(const ImuMsg::SharedPtr msg)
gyroAccelerations_[1] = tempMsg.linear_acceleration.y;
gyroAccelerations_[2] = tempMsg.linear_acceleration.z;
std::cout << "gyroValues_ : " << gyroValues_[0] << " " << gyroValues_[1] << " " << gyroValues_[2] << std::endl;
// std::cout << "gyroValues_ : " << gyroValues_[0] << " " << gyroValues_[1] << " " << gyroValues_[2] << std::endl;
}

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

@@ -80,6 +80,7 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
wheelStatesSub_ = this->create_subscription<MotorPos>("/motor_pos", 10,std::bind(&RobotControlNode::WheelStatesCallback, this, std::placeholders::_1));
joyCommandSub_ = this->create_subscription<std_msgs::msg::String>("/gamepad_msg", 10,std::bind(&RobotControlNode::JoyCommandCallback, this, std::placeholders::_1));
controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(CYCLE_TIME),std::bind(&RobotControlNode::ControlLoop, this)); // 绑定到新的控制函数);
motorParamClient_ = this->create_client<MotorParam>("/motor_param");
lastTime_ = this->now(); // 初始化时间
std::cout << "RobotFsm Node is created finished!" << std::endl;
@@ -392,11 +393,11 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_wheel_goal(
return rclcpp_action::GoalResponse::REJECT;
}
if (!robotControlManager_.SetMoveWheelParameters(goal->move_distance, goal->move_angle))
{
RCLCPP_ERROR(this->get_logger(), "Invalid move wheel request");
return rclcpp_action::GoalResponse::REJECT;
}
// if (!robotControlManager_.SetMoveWheelParameters(goal->move_distance, goal->move_angle))
// {
// RCLCPP_ERROR(this->get_logger(), "Invalid move wheel request");
// return rclcpp_action::GoalResponse::REJECT;
// }
(void)uuid;
@@ -430,6 +431,26 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
auto feedback = std::make_shared<MoveWheel::Feedback>();
auto result = std::make_shared<MoveWheel::Result>();
double wheelAngle = abs(robotControlManager_.GetImuDifference()[2]) > 5.0 ? 0.0 : robotControlManager_.GetImuDifference()[2];
robotControlManager_.SetMoveWheelParameters(goal->move_distance, wheelAngle);
if (goal->move_distance > 0)
// if (true)
{
double ratio = robotControlManager_.GetWheelRatio();
auto request = std::make_shared<MotorParam::Request>();
request->motor_id = 1;
request->max_speed = (uint16_t)(ratio * 20);
request->add_acc = 5;
request->dec_acc = 5;
motorParamClient_->async_send_request(request);
rclcpp::sleep_for(std::chrono::milliseconds(1000));
}
robotControlManager_.MoveWheel();
std_msgs::msg::Float64MultiArray wheel_commands;
@@ -475,6 +496,71 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
loop_rate.sleep();
}
// if (goal->move_distance > 0)
// // if (true)
// {
// robotControlManager_.SetMoveWheelParameters(goal->move_distance, robotControlManager_.GetImuDifference()[2] * 2.0 );
// std::cout << " IMU different : " << robotControlManager_.GetImuDifference()[2] << std::endl;
// robotControlManager_.MoveWheel();
// wheel_commands = robotControlManager_.GetWheelCommands();
// wheel_commands_msg.target = "rs485";
// wheel_commands_msg.type = "bm";
// wheel_commands_msg.position = "";
// wheel_commands_msg.motor_id = {1,2};
// wheel_commands_msg.motor_angle = {(float)(wheel_commands.data[0]),(float)(wheel_commands.data[1])};
// std::cout << "wheel_commands.data[0] " << wheel_commands.data[0] << std::endl;
// std::cout << "wheel_commands.data[1] " << wheel_commands.data[1] << std::endl;
// motorCmdPub_->publish(wheel_commands_msg);
// while (move_wheel_executing_ && rclcpp::ok())
// {
// if (goal_handle->is_canceling())
// {
// result->success = false;
// goal_handle->canceled(result);
// RCLCPP_INFO(this->get_logger(), "move wheel canceled");
// move_wheel_executing_ = false;
// //TODO: ADD STOP LOGIC
// return;
// }
// auto joint_feedback = robotControlManager_.GetWheelFeedback();
// if (joint_feedback.data[0] - wheel_commands.data[0] < 7.0 && joint_feedback.data[1] - wheel_commands.data[1] < 7.0)
// {
// move_wheel_executing_ = false;
// }
// feedback->current_pos = joint_feedback.data[0];
// //TODO: get the angle from lidar.
// feedback->current_angle = 0.0;
// goal_handle->publish_feedback(feedback);
// loop_rate.sleep();
// }
// }
if (goal->move_distance > 0)
// if (true)
{
auto request = std::make_shared<MotorParam::Request>();
request->motor_id = 1;
request->max_speed = 20;
request->add_acc = 5;
request->dec_acc = 5;
motorParamClient_->async_send_request(request);
}
// 检查目标是否仍在活跃状态
if (rclcpp::ok())
{

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

@@ -69,15 +69,17 @@ bool WaistControl::MoveWaist(std::vector<double>& joint_positions, double dt)
}
else
{
if (IsReached(joint_position_desired_))
if (IsReached(joint_position_desired_) || (time_out_count_ > TIME_OUT_COUNT))
{
std::cout << "Waist reached target position!" << std::endl;
is_target_set_ = false;
is_cmd_send_finished_ = false;
time_out_count_ = 0;
return true;
}
else
{
time_out_count_ ++;
// std::cout << "Waist not reached target position!" << std::endl;
return false;
}
@@ -106,8 +108,8 @@ bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double movey
std::vector<double> tempPosition;
tempPosition.resize(total_joints_, 0.0);
tempPosition[0] = joint_position_[0] - joint_home_positions_[0] + movepitch;
tempPosition[1] = joint_position_[1] - joint_home_positions_[1] + moveyaw;
tempPosition[0] = joint_commands_[0] - joint_home_positions_[0] + movepitch;
tempPosition[1] = joint_commands_[1] - joint_home_positions_[1] + moveyaw;
if (!checkJointLimits(tempPosition))
{

65
src/robot_control/src/wheel_control.cpp
View File

@@ -34,40 +34,69 @@ WheelControl::~WheelControl()
bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle)
{
double original1 = joint_position_desired_[0];
double original2 = joint_position_desired_[1];
std::cout << "Original joint_position_desired_: " << original1 << " , " << original2 << std::endl;
// 设置移动轮参数
double theta = moveWheelDistance / lengthParameters_[0] * 180.0 / M_PI;
double beta = moveWheelAngle / 360.0 * M_PI * 2 * lengthParameters_[1] / lengthParameters_[0] * 180.0 / M_PI;
std::cout << "Move Wheel theta: " << theta << " beta: " << beta << std::endl;
if(moveWheelDistance > lastMoveDistance && beta != 0)
{
for (size_t i = 0; i < joint_directions_.size(); i++)
{
joint_home_positions_[i] = joint_home_positions_[i] - beta;
}
double additionDistance = 0.001 / lengthParameters_[0] * 180.0 / M_PI;
joint_home_positions_[0] = joint_home_positions_[0] + additionDistance;
joint_home_positions_[1] = joint_home_positions_[1] - additionDistance;
}
for (size_t i = 0; i < joint_directions_.size(); i++)
{
if (joint_directions_[i] == 1)
{
if (theta < 0.0)
{
joint_position_desired_[i] = joint_home_positions_[i] + theta;
}
else
{
joint_position_desired_[i] = joint_home_positions_[i] + theta; // * 1.02;
}
joint_position_desired_[i] = joint_home_positions_[i] + theta;
}
else
{
if (theta < 0.0)
{
joint_position_desired_[i] = joint_home_positions_[i] - theta;
}
else
{
joint_position_desired_[i] = joint_home_positions_[i] - theta; // * 0.98 * 1.02;
}
joint_position_desired_[i] = joint_home_positions_[i] - theta; // * 0.98 * 1.02;
}
}
//TODO: 设置移动轮角度
(void) moveWheelAngle;
std::cout << "Move Wheel joint_position_desired_: " << joint_position_desired_[0] << " , " << joint_position_desired_[1] << std::endl;
double a1 = abs(joint_position_desired_[0] - original1);
double a2 = abs(joint_position_desired_[1] - original2);
if (a2 > 0.0 && a1 > 0.0)
{
moveRatio_ = a1 / a2;
}
else
{
moveRatio_ = 1.0;
}
if (moveRatio_ > 1.3 || moveRatio_ < 0.7)
{
moveRatio_ = 1.0;
}
std::cout << "Set move ratio : " << moveRatio_ << std::endl;
std::cout << "Move Wheel joint_position_desired_: " << joint_position_desired_[0] << " , " << joint_position_desired_[1] << std::endl;
lastMoveDistance = moveWheelDistance;
is_target_set_ = true;
return true;