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

fix waist structure modification

Browse Source
This commit is contained in:
NuoDaJia02
2025-12-26 13:08:09 +08:00
parent b0612c3de8
commit 31aa8dc179
3 changed files with 8 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -83,8 +83,8 @@ namespace robot_control
// 关节速度参数
max_joint_velocity_ = {
35,
100,
30,
30,
50,
50,
50,
@@ -95,7 +95,7 @@ namespace robot_control
max_joint_acceleration_ = {
80,
200,
80,
100,
100,
100,
@@ -215,9 +215,9 @@ namespace robot_control
leg_home_positions_ = {
217.52 - 65.0,
120.84 + 41.0,
108.7 + 35.5, //217479
108.7 + 40.63, //217479
221.95 - 41.0,
234.14 - 35.5, //298023
234.14 - 29.504, //298023
125.39 + 65.0
};

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

@@ -236,7 +236,6 @@ 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::Rate loop_rate(10); // 10Hz更新频率
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<MoveLeg::Feedback>();
@@ -250,7 +249,6 @@ void RobotControlNode::move_leg_execute(const std::shared_ptr<GoalHandleMoveLeg>
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "move leg canceled");
move_leg_executing_ = false;
//TODO: ADD STOP LOGIC
return;
}
@@ -387,14 +385,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_wheel_goal(
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");
// return rclcpp_action::GoalResponse::REJECT;
// }
(void)uuid;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
@@ -444,9 +434,6 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
double tempValue = robotControlManager_.GetImuDifference()[2];
wheelAngle = abs(tempValue) > 40.0 ? 0.0 : tempValue;
}
// double wheelAngle = 1.5;
robotControlManager_.SetMoveWheelParameters(goal->move_distance, wheelAngle);

6
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,8 +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] << 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;