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

parameters optimization

Browse Source
This commit is contained in:
NuoDaJia02
2025-11-08 17:06:09 +08:00
parent 55dd17e055
commit 60c98e7492
9 changed files with 115 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -6,7 +6,7 @@
#include <string>
#define CYCLE_TIME 4 // 插补周期（毫秒）
#define CYCLE_TIME 8 // 插补周期（毫秒）
#ifndef DEG2RAD
#define DEG2RAD(x) ((x)*0.017453293)
@@ -80,8 +80,8 @@ namespace robot_control
// 关节速度参数
max_joint_velocity_ = {
30,
30,
35,
100,
50,
50,
50,
@@ -89,8 +89,8 @@ namespace robot_control
};
max_joint_acceleration_ = {
100,
100,
80,
200,
100,
100,
100,
@@ -121,7 +121,7 @@ namespace robot_control
//TODO: 限位需要修改
joint_limits_ = {
JointLimit(0, 30.0, -30.0, LimitType::POSITION),
JointLimit(0, 40.0, -40.0, LimitType::POSITION),
JointLimit(1, 190.0, -190.0, LimitType::POSITION),
JointLimit(2, 80.0, 0.0, LimitType::POSITION),
// JointLimit(3, 5.0, -5.0, LimitType::POSITION),

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

@@ -18,7 +18,7 @@
#include "interfaces/msg/imu_msg.hpp"
using MotorPos = interfaces::msg::MotorPos;
using ImuMsg = interfaces::msg::ImuMsg;
using ImuMsg = sensor_msgs::msg::Imu;
namespace robot_control
{

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

@@ -22,7 +22,7 @@ namespace robot_control
~WaistControl() override;
void SetHomePositions(const std::vector<double>& home_positions) override;
// void SetHomePositions(const std::vector<double>& home_positions) override;
bool SetMoveWaistParametersInternal(double movepitch, double moveyaw);

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

@@ -158,6 +158,7 @@ void ControlBase::UpdateJointStates(
{
joint_commands_ = joint_position_;
is_joint_position_initialized_ = true;
std::cout << "joint commands initialized" << std::endl;
}
// 3. 更新关节速度
@@ -194,7 +195,7 @@ bool ControlBase::checkJointLimits(const std::vector<double>& target_joint)
{
if (target_joint[i] < minLimits_[i] || target_joint[i] > maxLimits_[i])
{
printf("Joint %zu target position %f is out of limits [%f, %f]", i, target_joint[i], minLimits_[i], maxLimits_[i]);
printf("Joint %zu target position %f is out of limits [%f, %f]", i + 1, target_joint[i], minLimits_[i], maxLimits_[i]);
return false;
}
}

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

@@ -196,7 +196,10 @@ std::vector<double> RobotControlManager::GetImuDifference()
}
//TODO: optimization
gyroInitValues_[2] = gyroInitValues_[2] - 0.1;
// gyroInitValues_[2] = gyroInitValues_[2] - 0.03;
std::cout << "get gyro init value : " << gyroInitValues_[2] << std::endl;
std::cout << "get gyro value : " << gyroValues_[2] << std::endl;
return result;
}
@@ -204,7 +207,7 @@ std::vector<double> RobotControlManager::GetImuDifference()
void RobotControlManager::UpdateImuMsg(const ImuMsg::SharedPtr msg)
{
imuMsg_ = *msg;
sensor_msgs::msg::Imu tempMsg = imuMsg_.imu;
ImuMsg tempMsg = imuMsg_;
if (tempMsg.orientation.w == 0 && tempMsg.orientation.x == 0 && tempMsg.orientation.y == 0 && tempMsg.orientation.z == 0)
{
@@ -351,7 +354,6 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
{
if (count > 50)
{
waistController_->SetHomePositions(jointPositions_);
jointCommands_.data = jointPositions_;
isJointInitValueSet_ = true;
count = 0;

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

@@ -143,7 +143,7 @@ void RobotControlNode::handle_move_home_accepted(const std::shared_ptr<GoalHandl
// 执行目标计算
void RobotControlNode::move_home_execute(const std::shared_ptr<GoalHandleMoveHome> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing home goal");
// RCLCPP_INFO(this->get_logger(), "Executing home goal");
rclcpp::Rate loop_rate(10); // 10Hz更新频率
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<MoveHome::Feedback>();
@@ -235,7 +235,7 @@ void RobotControlNode::handle_move_leg_accepted(const std::shared_ptr<GoalHandle
// 执行目标计算
void RobotControlNode::move_leg_execute(const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing leg goal");
// RCLCPP_INFO(this->get_logger(), "Executing leg goal");
rclcpp::Rate loop_rate(10); // 10Hz更新频率
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<MoveLeg::Feedback>();
@@ -326,7 +326,7 @@ void RobotControlNode::handle_move_waist_accepted(const std::shared_ptr<GoalHand
// 执行目标计算
void RobotControlNode::move_waist_execute(const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing waist goal");
// RCLCPP_INFO(this->get_logger(), "Executing waist goal");
rclcpp::Rate loop_rate(10); // 10Hz更新频率
const auto goal = goal_handle->get_goal(); // 获取目标
auto feedback = std::make_shared<MoveWaist::Feedback>();
@@ -422,23 +422,28 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
double wheelAngle = abs(tempValue) > 40.0 ? 0.0 : tempValue;
// double wheelAngle = 1.5;
robotControlManager_.SetMoveWheelParameters(goal->move_distance, wheelAngle);
double ratio = robotControlManager_.GetWheelRatio();
auto request = std::make_shared<MotorParam::Request>();
request->motor_id = 1;
request->max_speed = static_cast<uint16_t>(round(ratio * 40));
request->add_acc = 10;
request->dec_acc = 10;
if(goal->move_distance > 0.0)
{
request->motor_id = 1;
request->max_speed = static_cast<uint16_t>(round((ratio) * 48));
request->add_acc = 8;
request->dec_acc = 8;
motorParamClient_->async_send_request(request);
std::cout << "set motor 1 max speed : " << request->max_speed << std::endl;
rclcpp::sleep_for(std::chrono::milliseconds(200));
}
motorParamClient_->async_send_request(request);
std::cout << "request->max_speed : " << request->max_speed << std::endl;
rclcpp::sleep_for(std::chrono::milliseconds(1000));
robotControlManager_.MoveWheel();
std_msgs::msg::Float64MultiArray wheel_commands;
@@ -451,8 +456,8 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
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;
// 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);
@@ -485,14 +490,15 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
loop_rate.sleep();
}
request->motor_id = 1;
request->max_speed = 40;
request->add_acc = 10;
request->dec_acc = 10;
motorParamClient_->async_send_request(request);
std::cout << "request->max_speed : " << request->max_speed << std::endl;
if(goal->move_distance > 0.0)
{
request->motor_id = 1;
request->max_speed = 48;
request->add_acc = 8;
request->dec_acc = 8;
motorParamClient_->async_send_request(request);
}
// 检查目标是否仍在活跃状态
if (rclcpp::ok())

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

@@ -98,6 +98,7 @@ void TrapezoidalTrajectory::calculateTrajectoryParams()
std::vector<double> axis_decel_time(dof, 0.0); // 各轴理论减速时间
std::vector<double> axis_cruise_time(dof, 0.0); // 各轴理论匀速时间
std::vector<double> axis_cruise_vel(dof, 0.0); // 各轴理论巡航速度
total_time_ = 0.0;
// 步骤1：计算每个轴的理论最小运动参数（独立约束）
for (size_t i = 0; i < dof; ++i)
@@ -149,6 +150,11 @@ void TrapezoidalTrajectory::calculateTrajectoryParams()
acceleration_time_ = axis_accel_time[i];
deceleration_time_ = axis_decel_time[i];
cruise_time_ = axis_cruise_time[i];
// std::cout << "total time : " << total_time_ << std::endl;
// std::cout << "acceleration_time_ : " << acceleration_time_ << std::endl;
// std::cout << "deceleration_time_ : " << deceleration_time_ << std::endl;
// std::cout << "cruise_time_ : " << cruise_time_ << std::endl;
}
}

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

@@ -88,20 +88,20 @@ bool WaistControl::MoveWaist(std::vector<double>& joint_positions, double dt)
return false;
}
void WaistControl::SetHomePositions(const std::vector<double>& home_positions)
{
if (home_positions.size() != total_joints_)
{
throw std::invalid_argument("Home positions size does not match total joints!");
}
// void WaistControl::SetHomePositions(const std::vector<double>& home_positions)
// {
// if (home_positions.size() != total_joints_)
// {
// throw std::invalid_argument("Home positions size does not match total joints!");
// }
if (home_positions[1] < 0)
{
joint_home_positions_[1] = joint_home_positions_[1] - 360.0;
}
// // if (home_positions[1] < 0)
// // {
// // joint_home_positions_[1] = joint_home_positions_[1] - 360.0;
// // }
std::cout << "Home positions set to: " << joint_home_positions_[0] << ", " << joint_home_positions_[1] << std::endl;
}
// std::cout << "Home positions set to: " << joint_home_positions_[0] << ", " << joint_home_positions_[1] << std::endl;
// }
bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double moveyaw)
{

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

@@ -32,33 +32,63 @@ WheelControl::~WheelControl()
delete trapezoidalTrajectory_;
}
int tempAdjustCount = 0;
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 << "moveWheelDistance: " << moveWheelDistance << std::endl;
if(moveWheelDistance > lastMoveDistance && beta != 0)
if(moveWheelDistance > lastMoveDistance && (moveWheelDistance - lastMoveDistance > 1.0) && beta != 0)
{
std::cout << " beta: " << beta << std::endl;
tempAdjustCount ++;
for (size_t i = 0; i < joint_directions_.size(); i++)
if (tempAdjustCount > 0)
{
joint_home_positions_[i] = joint_home_positions_[i] - beta;
if (tempAdjustCount == 1)
{
for (size_t i = 0; i < joint_directions_.size(); i++)
{
joint_home_positions_[i] = joint_home_positions_[i] - beta;
}
if (beta > 0)
{
joint_home_positions_[1] = joint_home_positions_[1] + beta;
}
else
{
joint_home_positions_[0] = joint_home_positions_[0] + beta;
}
}
else if (tempAdjustCount == 2)
{
for (size_t i = 0; i < joint_directions_.size(); i++)
{
joint_home_positions_[i] = joint_home_positions_[i] - beta;
}
// joint_home_positions_[0] = joint_home_positions_[0] - beta;
// joint_home_positions_[1] = joint_home_positions_[1] + beta;
if (beta > 0)
{
joint_home_positions_[1] = joint_home_positions_[1] + beta;
}
else
{
joint_home_positions_[0] = joint_home_positions_[0] + beta;
}
tempAdjustCount = 0;
}
}
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++)
@@ -76,18 +106,25 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
double a1 = abs(joint_position_desired_[0] - original1);
double a2 = abs(joint_position_desired_[1] - original2);
std::cout << "motor 1 distance : " << a1 << std::endl;
std::cout << "motor 2 distance : " << a2 << std::endl;
if (a2 > 0.0 && a1 > 0.0)
{
moveRatio_ = a1 / a2;
moveRatio_ = ((a1 / a2) - 1.0) * 0.50;
}
else
{
moveRatio_ = 1.0;
moveRatio_ = 0.0;
}
moveRatio_ = moveRatio_ + 1.0;
// 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;
// std::cout << "Move Wheel joint_position_desired_: " << joint_position_desired_[0] << " , " << joint_position_desired_[1] << std::endl;
lastMoveDistance = moveWheelDistance;