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

replace new waist motor

Browse Source
This commit is contained in:
NuoDaJia02
2025-10-31 15:33:24 +08:00
parent fca06d7e73
commit 753b72268b
9 changed files with 47 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,7 +5,7 @@
#include "trapezoidal_trajectory.hpp"
#include "motion_parameters.hpp"
#define POSITION_TOLERANCE 0.01
#define POSITION_TOLERANCE 1.0
namespace robot_control
{
@@ -37,6 +37,7 @@ namespace robot_control
bool is_moving_; // 是否运动中
bool is_stopping_; // 是否停止中
bool is_target_set_; // 是否已设置目标点
bool is_cmd_send_finished_;
public:
// 构造函数（子类需调用此构造函数初始化父类）
@@ -75,6 +76,8 @@ namespace robot_control
virtual bool IsReached(const std::vector<double>& target_joint); // 判断是否到达目标点
virtual void SetHomePositions(const std::vector<double>& home_positions);
bool checkJointLimits(const std::vector<double>& target_joint);
};

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

@@ -80,8 +80,8 @@ namespace robot_control
// 关节速度参数
max_joint_velocity_ = {
50,
50,
5,
25,
50,
50,
50,
@@ -186,7 +186,7 @@ namespace robot_control
waist_zero_positions_ = {
181.0,
158.5
52.66
};
leg_zero_positions_ = {
@@ -202,7 +202,7 @@ namespace robot_control
waist_home_positions_ = {
181.0,
158.5
52.66
};

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

@@ -22,6 +22,8 @@ namespace robot_control
~WaistControl() override;
void SetHomePositions(const std::vector<double>& home_positions) override;
bool SetMoveWaistParametersInternal(double movepitch, double moveyaw);
bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;

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

@@ -22,8 +22,6 @@ namespace robot_control
~WheelControl() override;
void SetWheelHomePositions(const std::vector<double>& home_positions);
bool SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle);
bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;

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

@@ -42,6 +42,7 @@ ControlBase::ControlBase(
is_moving_ = false;
is_stopping_ = false;
is_target_set_ = false;
is_cmd_send_finished_ = false;
stopDurationTime_ = 0.0;
movingDurationTime_ = 0.0;
@@ -52,6 +53,11 @@ ControlBase::~ControlBase()
delete trapezoidalTrajectory_;
}
void ControlBase::SetHomePositions(const std::vector<double>& home_positions)
{
joint_home_positions_ = home_positions;
}
bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const std::vector<double>& target_joint, double dt)
{
if (joint_position_.size() != target_joint.size())

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

@@ -272,7 +272,7 @@ void RobotControlManager::UpdateWheelStates(const MotorPos::SharedPtr msg)
if (!isWheelHomed_)
{
wheelController_ -> SetWheelHomePositions(wheelPositions_);
wheelController_ -> SetHomePositions(wheelPositions_);
isWheelHomed_ = true;
}
@@ -318,6 +318,7 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
{
if (count > 50)
{
waistController_->SetHomePositions(jointPositions_);
jointCommands_.data = jointPositions_;
isJointInitValueSet_ = true;
count = 0;

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

@@ -461,7 +461,7 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
auto joint_feedback = robotControlManager_.GetWheelFeedback();
if (joint_feedback.data[0] - wheel_commands.data[0] < 5.0 && joint_feedback.data[1] - wheel_commands.data[1] < 5.0)
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;
}

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

@@ -62,24 +62,43 @@ bool WaistControl::MoveWaist(std::vector<double>& joint_positions, double dt)
return true;
}
}
bool result = MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
if (result)
if (!is_cmd_send_finished_)
{
is_cmd_send_finished_ = MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
}
else
{
if (IsReached(joint_position_desired_))
{
std::cout << "Waist reached target position!" << std::endl;
is_target_set_ = false;
is_cmd_send_finished_ = false;
return true;
}
else
{
std::cout << "Waist not reached target position!" << std::endl;
// std::cout << "Waist not reached target position!" << std::endl;
return false;
}
}
return result;
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!");
}
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;
}
bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double moveyaw)
@@ -87,8 +106,8 @@ bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double movey
std::vector<double> tempPosition;
tempPosition.resize(total_joints_, 0.0);
tempPosition[0] = joint_position_[0] - joint_zero_positions_[0] + movepitch;
tempPosition[1] = joint_position_[1] - joint_zero_positions_[1] + moveyaw;
tempPosition[0] = joint_position_[0] - joint_home_positions_[0] + movepitch;
tempPosition[1] = joint_position_[1] - joint_home_positions_[1] + moveyaw;
if (!checkJointLimits(tempPosition))
{
@@ -98,7 +117,7 @@ bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double movey
for (size_t i = 0; i < joint_position_desired_.size(); i++)
{
joint_position_desired_[i] = tempPosition[i] + joint_zero_positions_[i];
joint_position_desired_[i] = tempPosition[i] + joint_home_positions_[i];
}
std::cout << "Waist Target Joint Position: " << joint_position_desired_[0] << ", " << joint_position_desired_[1] << std::endl;

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

@@ -32,11 +32,6 @@ WheelControl::~WheelControl()
delete trapezoidalTrajectory_;
}
void WheelControl::SetWheelHomePositions(const std::vector<double>& home_positions)
{
joint_home_positions_ = home_positions;
}
bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle)
{
// 设置移动轮参数