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

11.11 version update

Browse Source
This commit is contained in:
NuoDaJia02
2025-12-18 09:49:23 +08:00
parent 60c98e7492
commit 7d892a6c98
9 changed files with 230 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.vscode/settings.json vendored
View File

@@ -77,6 +77,7 @@
"typeindex": "cpp",
"typeinfo": "cpp",
"variant": "cpp",
"dense": "cpp"
"dense": "cpp",
"filesystem": "cpp"
}
}

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

@@ -100,16 +100,16 @@ namespace robot_control
// 初始化关节索引
wheel_joint_indices_ = {0, 1};
waist_joint_indices_ = {0, 1};
waist_joint_indices_ = {0};
leg_joint_indices_ = {2, 3, 4, 5};
total_joints_ = 2;
total_joints_ = 1;
// 初始化关节方向
wheel_joint_directions_ = {1, -1};
waist_joint_directions_ = {1, 1};
waist_joint_directions_ = {1};
leg_joint_directions_ = {1, -1, -1, 1};
@@ -185,8 +185,8 @@ namespace robot_control
}
waist_zero_positions_ = {
181.0,
52.66
181.0
// 52.66
};
leg_zero_positions_ = {
@@ -201,8 +201,8 @@ namespace robot_control
};
waist_home_positions_ = {
181.0,
52.66
181.0
// 52.66
};

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

@@ -35,6 +35,11 @@ namespace robot_control
bool Stop(double dt);
bool GoHome(double dt);
void JogAxis(size_t axis, int dir);
void SetJogWheel(bool value);
bool GetJogWheel();
void WheelReset(){isWheelHomed_ = false;};
void ImuReset(){isGyroInited_ = false;}
// 检查参数是否合理
bool SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle);
@@ -71,6 +76,8 @@ namespace robot_control
bool isLegHomed_;
bool isWheelHomed_;
bool is_wheel_jog_;
// 控制器
LegControl* legController_;
WheelControl* wheelController_;

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

@@ -101,6 +101,8 @@ namespace robot_control
size_t jogIndex_;
double jogValue_;
double lastSpeed_;
void Publish_joint_trajectory();
void JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg);

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

@@ -30,6 +30,8 @@ namespace robot_control
bool SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle);
bool SetMoveWheelParametersInternalJog(double moveWheelDistance, double moveWheelAngle);
bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
bool MoveWheel(std::vector<double>& joint_positions);

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

@@ -25,8 +25,10 @@ void RobotControlManager::init()
gyroVelocities_.resize(3, 0.0);
gyroAccelerations_.resize(3, 0.0);
is_wheel_jog_ = false;
// Initialize the wheel commands
for (size_t i = 0; i < motionParams_.waist_joint_indices_.size(); i++)
for (size_t i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
{
wheelCommands_.data.push_back(0.0);
}
@@ -82,8 +84,23 @@ RobotControlManager::~RobotControlManager()
delete wheelController_;
}
void RobotControlManager::SetJogWheel(bool value)
{
is_wheel_jog_ = value;
}
bool RobotControlManager::GetJogWheel()
{
return is_wheel_jog_;
}
bool RobotControlManager::SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle)
{
if (is_wheel_jog_)
{
return wheelController_->SetMoveWheelParametersInternalJog(moveWheelDistance, moveWheelAngle);
}
return wheelController_->SetMoveWheelParametersInternal(moveWheelDistance, moveWheelAngle);
}

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

@@ -17,6 +17,7 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
isJogMode_ = false;
jogDirection_ = 0;
jogIndex_ = 0;
lastSpeed_ = 51;
// 初始化数据文件（设置路径，确保目录存在）
#if 0
@@ -381,6 +382,13 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_wheel_goal(
return rclcpp_action::GoalResponse::REJECT;
}
if (goal->move_distance > 2.1 || goal->move_distance < -1.0 || goal->move_angle < -10 || goal->move_angle > 10 )
{
RCLCPP_ERROR(this->get_logger(), "exceed limit");
return rclcpp_action::GoalResponse::REJECT;
}
// if (!robotControlManager_.SetMoveWheelParameters(goal->move_distance, goal->move_angle))
// {
// RCLCPP_ERROR(this->get_logger(), "Invalid move wheel request");
@@ -418,9 +426,25 @@ 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 tempValue = robotControlManager_.GetImuDifference()[2];
double wheelAngle = 0;
double wheelAngle = abs(tempValue) > 40.0 ? 0.0 : tempValue;
if (abs(goal->move_angle) > 0)
{
robotControlManager_.SetJogWheel(true);
wheelAngle = goal->move_angle;
}
if (abs(goal->move_angle) == 0 && abs(goal->move_distance) == 0)
{
robotControlManager_.SetJogWheel(false);
}
if (!robotControlManager_.GetJogWheel())
{
double tempValue = robotControlManager_.GetImuDifference()[2];
wheelAngle = abs(tempValue) > 40.0 ? 0.0 : tempValue;
}
// double wheelAngle = 1.5;
@@ -430,10 +454,20 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
auto request = std::make_shared<MotorParam::Request>();
if(goal->move_distance > 0.0)
if((goal->move_distance > 0.1 ) && !robotControlManager_.GetJogWheel()) // || goal->move_distance < -0.5
{
// if (lastSpeed_ < 51 && goal->move_distance < -0.5)
// {
// ratio = 1.02;
// }
request->motor_id = 1;
request->max_speed = static_cast<uint16_t>(round((ratio) * 48));
request->max_speed = static_cast<uint16_t>(round((ratio) * 51));
// if (goal->move_distance < -0.5)
// {
// request->add_acc = 4;
// }
request->add_acc = 8;
request->dec_acc = 8;
@@ -441,7 +475,7 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
std::cout << "set motor 1 max speed : " << request->max_speed << std::endl;
rclcpp::sleep_for(std::chrono::milliseconds(200));
rclcpp::sleep_for(std::chrono::milliseconds(50));
}
robotControlManager_.MoveWheel();
@@ -475,7 +509,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] < 7.0 && joint_feedback.data[1] - wheel_commands.data[1] < 7.0)
if (abs(joint_feedback.data[0] - wheel_commands.data[0]) < 20.0 && abs(joint_feedback.data[1] - wheel_commands.data[1]) < 20.0)
{
move_wheel_executing_ = false;
}
@@ -490,10 +524,10 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
loop_rate.sleep();
}
if(goal->move_distance > 0.0)
if ((goal->move_distance > 0.0) && !robotControlManager_.GetJogWheel()) //|| goal->move_distance < -0.5
{
request->motor_id = 1;
request->max_speed = 48;
request->max_speed = 51;
request->add_acc = 8;
request->dec_acc = 8;
@@ -534,7 +568,10 @@ void RobotControlNode::ControlLoop() {
{
if(robotControlManager_.GoHome(dt_sec))
{
robotControlManager_.SetJogWheel(false);
move_home_executing_ = false;
robotControlManager_.WheelReset();
robotControlManager_.ImuReset();
}
}

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

@@ -109,7 +109,7 @@ bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double movey
tempPosition.resize(total_joints_, 0.0);
tempPosition[0] = joint_commands_[0] - joint_home_positions_[0] + movepitch;
tempPosition[1] = joint_commands_[1] - joint_home_positions_[1] + moveyaw;
// tempPosition[1] = joint_commands_[1] - joint_home_positions_[1] + moveyaw;
if (!checkJointLimits(tempPosition))
{
@@ -122,7 +122,8 @@ bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double movey
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;
// std::cout << "Waist Target Joint Position: " << joint_position_desired_[0] << ", " << joint_position_desired_[1] << std::endl;
std::cout << "Waist Target Joint Position: " << joint_position_desired_[0] << std::endl;
is_target_set_ = true;

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

@@ -39,79 +39,80 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
double original1 = joint_position_desired_[0];
double original2 = joint_position_desired_[1];
std::vector<double> tempHomePositions;
std::vector<double> tempDesiredPositions;
tempHomePositions.resize(total_joints_, 0.0);
tempDesiredPositions.resize(total_joints_, 0.0);
for (size_t i = 0; i < total_joints_; i++)
{
tempHomePositions[i] = joint_home_positions_[i];
tempDesiredPositions[i] = joint_position_desired_[i];
}
// 设置移动轮参数
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;
double beta = moveWheelAngle * 0.8 / 360.0 * M_PI * 2 * lengthParameters_[1] / lengthParameters_[0] * 180.0 / M_PI;
if(moveWheelDistance > lastMoveDistance && (moveWheelDistance - lastMoveDistance > 1.0) && beta != 0)
if(moveWheelDistance > lastMoveDistance && (moveWheelDistance - lastMoveDistance > 1.5) && beta != 0)
{
tempAdjustCount ++;
if (tempAdjustCount > 0)
if (beta > 0)
{
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;
}
tempHomePositions[0] = joint_home_positions_[0] - beta - 8;
}
else
{
tempHomePositions[0] = joint_home_positions_[0] - beta + 8;
}
}
if(moveWheelDistance < lastMoveDistance && (moveWheelDistance - lastMoveDistance < -1.5) && beta != 0)
{
tempAdjustCount ++;
if (beta > 0)
{
tempHomePositions[0] = joint_home_positions_[0] - beta * 2.0;
}
else
{
tempHomePositions[0] = joint_home_positions_[0] - beta;
}
if (tempAdjustCount == 1)
{
tempHomePositions[0] - 40;
tempAdjustCount = 0;
}
// if (tempAdjustCount == 2)
// {
// tempAdjustCount = 0;
// }
}
for (size_t i = 0; i < joint_directions_.size(); i++)
{
if (joint_directions_[i] == 1)
{
joint_position_desired_[i] = joint_home_positions_[i] + theta;
tempDesiredPositions[i] = tempHomePositions[i] + theta;
}
else
{
joint_position_desired_[i] = joint_home_positions_[i] - theta; // * 0.98 * 1.02;
tempDesiredPositions[i] = tempHomePositions[i] - theta; // * 0.98 * 1.02;
}
}
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;
double a1 = abs(tempDesiredPositions[0] - original1);
double a2 = abs(tempDesiredPositions[1] - original2);
if (a2 > 0.0 && a1 > 0.0)
{
moveRatio_ = ((a1 / a2) - 1.0) * 0.50;
moveRatio_ = ((a1 / a2) - 1.0) * 0.9;
}
else
{
@@ -120,7 +121,32 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
moveRatio_ = moveRatio_ + 1.0;
// moveRatio_ = 1.0;
if (moveRatio_ > 0.99 && moveRatio_ < 1.009)
{
for (size_t i = 0; i < joint_directions_.size(); i++)
{
if (joint_directions_[i] == 1)
{
joint_position_desired_[i] = joint_home_positions_[i] + theta;
}
else
{
joint_position_desired_[i] = joint_home_positions_[i] - theta; // * 0.98 * 1.02;
}
}
}
else
{
std::cout << "change distance " << std::endl;
for (size_t i = 0; i < joint_directions_.size(); i++)
{
joint_position_desired_[i] = tempDesiredPositions[i];
joint_home_positions_[i] = tempHomePositions[i];
}
}
std::cout << "motor 1 distance : " << joint_position_desired_[0] - original1 << std::endl;
std::cout << "motor 2 distance : " << joint_position_desired_[1] - original2 << std::endl;
std::cout << "Set move ratio : " << moveRatio_ << std::endl;
@@ -133,6 +159,72 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
return true;
}
bool WheelControl::SetMoveWheelParametersInternalJog(double moveWheelDistance, double moveWheelAngle)
{
double original1 = joint_position_desired_[0];
double original2 = joint_position_desired_[1];
std::vector<double> tempHomePositions;
std::vector<double> tempDesiredPositions;
tempHomePositions.resize(total_joints_, 0.0);
tempDesiredPositions.resize(total_joints_, 0.0);
for (size_t i = 0; i < total_joints_; i++)
{
tempHomePositions[i] = joint_home_positions_[i];
tempDesiredPositions[i] = joint_position_desired_[i];
}
// 设置移动轮参数
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;
for (size_t i = 0; i < joint_directions_.size(); i++)
{
tempHomePositions[0] = joint_home_positions_[0] + beta;
tempHomePositions[1] = joint_home_positions_[1] + beta;
}
for (size_t i = 0; i < joint_directions_.size(); i++)
{
if (joint_directions_[i] == 1)
{
tempHomePositions[i] += theta;
tempDesiredPositions[i] = tempHomePositions[i] ;
}
else
{
tempHomePositions[i] -= theta;
tempDesiredPositions[i] = tempHomePositions[i]; // * 0.98 * 1.02;
}
}
moveRatio_ = 1;
for (size_t i = 0; i < joint_directions_.size(); i++)
{
joint_position_desired_[i] = tempDesiredPositions[i];
joint_home_positions_[i] = tempHomePositions[i];
}
std::cout << "motor 1 distance : " << joint_position_desired_[0] - original1 << std::endl;
std::cout << "motor 2 distance : " << joint_position_desired_[1] - original2 << std::endl;
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;
is_target_set_ = true;
return true;
}
bool WheelControl::MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double dt)
{