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

fix motion action issues #2

Merged
hivecore merged 1 commits from simple_dev_Ray into main 2025-10-24 11:21:48 +08:00
Conversation 0 Commits 1 Files Changed 8 +103 -95
8 changed files with 103 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -80,21 +80,21 @@ namespace robot_control
// 关节速度参数
max_joint_velocity_ = {
25,
25,
25,
25,
25,
25
15,
15,
15,
15,
15,
15
};
max_joint_acceleration_ = {
250,
250,
250,
250,
250,
250
50,
50,
50,
50,
50,
50
};
// 初始化关节索引
@@ -107,7 +107,7 @@ namespace robot_control
total_joints_ = 6;
// 初始化关节方向
wheel_joint_directions_ = {-1, 1};
wheel_joint_directions_ = {1, -1};
waist_joint_directions_ = {1, 1};
@@ -123,14 +123,14 @@ namespace robot_control
joint_limits_ = {
JointLimit(0, 30.0, -30.0, LimitType::POSITION),
JointLimit(1, 180.0, -180.0, LimitType::POSITION),
JointLimit(2, 60.0, 0.0, LimitType::POSITION),
JointLimit(2, 80.0, 0.0, LimitType::POSITION),
// JointLimit(3, 5.0, -5.0, LimitType::POSITION),
// JointLimit(4, 50.0, -50.0, LimitType::POSITION),
JointLimit(3, 0.0, -50.0, LimitType::POSITION),
JointLimit(4, 0, -60.0, LimitType::POSITION),
JointLimit(3, 0.0, -60.0, LimitType::POSITION),
JointLimit(4, 0, -80.0, LimitType::POSITION),
// JointLimit(7, 5.0, -5.0, LimitType::POSITION),
// JointLimit(8, 50.0, -50.0, LimitType::POSITION),
JointLimit(5, 50.0, 0.0, LimitType::POSITION),
JointLimit(5, 60.0, 0.0, LimitType::POSITION),
};
// 初始化限制参数
@@ -140,12 +140,12 @@ namespace robot_control
wheel_min_limit_.resize(wheel_joint_indices_.size());
wheel_max_velocity_ = {
25,
25
5,
5
};
wheel_max_acceleration_ = {
250,
250
25,
25
};
//There is no limit for wheel

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

@@ -57,10 +57,9 @@ namespace robot_control
void init();
MotionParameters motionParams_;
bool isMoving_;
bool isLegMoving_;
bool isWheelMoving_;
bool isWaistMoving_;
bool isWaistHomed_;
bool isLegHomed_;
// 控制器
LegControl* legController_;

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

@@ -34,6 +34,7 @@ ControlBase::ControlBase(
joint_velocity_.resize(total_joints_, 0.0);
joint_acceleration_.resize(total_joints_, 0.0);
joint_torque_.resize(total_joints_, 0.0);
joint_position_desired_.resize(total_joints_, 0.0);
// 初始化梯形轨迹规划器
trapezoidalTrajectory_ = new TrapezoidalTrajectory(maxSpeed, maxAcc);

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

@@ -83,21 +83,26 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
double currentFrountLegLength = lengthParameters_[0] * abs(std::cos(DEG2RAD(joint_position_[0] - joint_zero_positions_[0])));
double currentBackLegLength = lengthParameters_[1] * abs(std::cos(DEG2RAD(joint_position_[1] - joint_zero_positions_[1])));
if (moveDistance > 0)
{
if ((moveDistance + currentFrountLegLength) > (lengthParameters_[0] - 5) || (moveDistance + currentBackLegLength) > (lengthParameters_[1] -5))
if ((moveDistance + currentFrountLegLength) > (lengthParameters_[0] - 0.02) || (moveDistance + currentBackLegLength) > (lengthParameters_[1] - 0.03))
{
std::cout << "Move distance is too large!" << std::endl;
std::cout << "Move distance is too large! " << std::endl;
std::cout << " Current Front Leg Length: " << currentFrountLegLength << std::endl;
std::cout << " Current Back Leg Length: " << currentBackLegLength << std::endl;
std::cout << " Move Distance: " << moveDistance << std::endl;
return false;
}
}
else
{
if (moveDistance < (-currentFrountLegLength + 5) || moveDistance < (-currentBackLegLength + 5))
if (moveDistance < (-currentFrountLegLength + 0.03) || moveDistance < (-currentBackLegLength + 0.03))
{
std::cout << "Move distance is too large!" << std::endl;
std::cout << " Current Front Leg Length: " << currentFrountLegLength << std::endl;
std::cout << " Current Back Leg Length: " << currentBackLegLength << std::endl;
std::cout << " Move Distance: " << moveDistance << std::endl;
return false;
}
}
@@ -115,6 +120,7 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
if (!checkJointLimits(tempPosition))
{
std::cout << "Joint limits exceeded!" << std::endl;
return false;
}
@@ -123,7 +129,10 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
joint_position_desired_[i] = tempPosition[i] + joint_zero_positions_[i];
}
std::cout << "Target Joint Position: " << joint_position_desired_[0] << ", " << joint_position_desired_[1] << std::endl;
std::cout << "Target Joint Position: "
<< joint_position_desired_[0]
<< ", " << joint_position_desired_[1] << ", "
<< joint_position_desired_[2] << ", " << joint_position_desired_[3] << std::endl;
is_target_set_ = true;

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

@@ -58,6 +58,9 @@ void RobotControlManager::init()
motionParams_.wheel_joint_directions_
);
isWaistHomed_ = false;
isLegHomed_ = false;
isJointInitValueSet_ = false;
}
@@ -70,17 +73,7 @@ RobotControlManager::~RobotControlManager()
Float64MultiArray RobotControlManager::GetWheelCommands()
{
Float64MultiArray tempValues;
tempValues.data.resize(2);
for (size_t i = 0; i < wheelCommands_.data.size(); i++)
{
double angle = wheelCommands_.data[i];
tempValues.data[i] = angle;
}
return tempValues;
return wheelCommands_;
}
Float64MultiArray RobotControlManager::GetJointFeedback()
@@ -90,9 +83,7 @@ Float64MultiArray RobotControlManager::GetJointFeedback()
for (size_t i = 0; i < jointPositions_.size(); i++)
{
double angle = jointPositions_[i];
tempValues.data[i] = angle;
tempValues.data[i] = jointPositions_[i];
}
return tempValues;
@@ -123,7 +114,7 @@ Float64MultiArray RobotControlManager::GetJointCommands()
{
double angle = jointCommands_.data[i];
double normalizedAngle = fmod(angle, 360.0);
double normalizedAngle = angle ; // TODO : check angle fmod(angle, 360.0);
// 计算原始脉冲值
double pulseValue = normalizedAngle * motionParams_.degree_to_pulse_[i];
@@ -165,6 +156,8 @@ void RobotControlManager::UpdateWheelStates(const MotorPos::SharedPtr msg)
{
motorPos_ = *msg;
wheelPositions_.resize(motionParams_.wheel_joint_indices_.size());
wheelVelocities_.resize(motionParams_.wheel_joint_indices_.size());
wheelEfforts_.resize(motionParams_.wheel_joint_indices_.size());
if (motorPos_.motor_angle.size() != motionParams_.wheel_joint_indices_.size())
{
@@ -338,14 +331,16 @@ bool RobotControlManager::MoveWaist(double dt)
bool RobotControlManager::MoveWheel()
{
tempWheelCmd_.resize(motionParams_.wheel_joint_indices_.size());
bool result = wheelController_ -> MoveWheel(tempWaistCmd_);
bool result = wheelController_ -> MoveWheel(tempWheelCmd_);
for (size_t i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
{
wheelCommands_.data[motionParams_.wheel_joint_indices_[i]] = tempWheelCmd_[i];
// std::cout << "Wheel commands: " << i << " " << wheelCommands_.data[motionParams_.wheel_joint_indices_[i]] << std::endl;
}
return result;
}
@@ -354,12 +349,28 @@ bool RobotControlManager::GoHome(double dt)
{
AssignTempValues();
bool waistHomed = waistController_->GoHome(tempWaistCmd_, dt);
bool legHomed = legController_->GoHome(tempLegCmd_, dt);
if (!isWaistHomed_)
{
isWaistHomed_ = waistController_->GoHome(tempWaistCmd_, dt);
}
if (!isLegHomed_)
{
isLegHomed_ = legController_->GoHome(tempLegCmd_, dt);
}
UpdateJointCommands();
return waistHomed && legHomed;
if (isWaistHomed_ && isLegHomed_)
{
isWaistHomed_ = false;
isLegHomed_ = false;
return true;
}
else
{
return false;
}
}
void RobotControlManager::JogAxis(size_t axis, int direction)

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

@@ -95,7 +95,7 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_home_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const MoveHome::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received move home request: total time");
RCLCPP_INFO(this->get_logger(), "Received move home request");
(void)goal;
@@ -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 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>();
@@ -156,7 +156,7 @@ void RobotControlNode::move_home_execute(const std::shared_ptr<GoalHandleMoveHom
result->success = false;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Move home canceled");
move_home_executing_ = false;
//TODO: ADD STOP LOGIC
return;
}
@@ -186,7 +186,7 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_leg_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const MoveLeg::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received move leg request: total time");
RCLCPP_INFO(this->get_logger(), "Received move leg request");
if (robotControlManager_.IsMoving(MovementPart::LEG))
{
@@ -239,7 +239,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 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>();
@@ -252,7 +252,7 @@ void RobotControlNode::move_leg_execute(const std::shared_ptr<GoalHandleMoveLeg>
result->success = false;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "move leg canceled");
move_leg_executing_ = false;
//TODO: ADD STOP LOGIC
return;
}
@@ -282,7 +282,7 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_waist_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const MoveWaist::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received move waist request: total time");
RCLCPP_INFO(this->get_logger(), "Received move waist request");
if (robotControlManager_.IsMoving(MovementPart::WAIST))
{
@@ -334,7 +334,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 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>();
@@ -347,7 +347,7 @@ void RobotControlNode::move_waist_execute(const std::shared_ptr<GoalHandleMoveWa
result->success = false;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "move waist canceled");
move_waist_executing_ = false;
//TODO: ADD STOP LOGIC
return;
}
@@ -423,26 +423,25 @@ void RobotControlNode::handle_move_wheel_accepted(const std::shared_ptr<GoalHand
// 执行目标计算
void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWheel> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing goal");
RCLCPP_INFO(this->get_logger(), "Executing wheel goal");
rclcpp::Rate loop_rate(10); // 10Hz更新频率
const auto goal = goal_handle->get_goal(); // 获取目标
auto feedback = std::make_shared<MoveWheel::Feedback>();
auto result = std::make_shared<MoveWheel::Result>();
Float64MultiArray wheel_commands;
if (robotControlManager_.MoveWheel())
{
wheel_commands = robotControlManager_.GetWheelCommands();
MotorCmd wheel_commands_msg;
robotControlManager_.MoveWheel();
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 = {static_cast<float>(wheel_commands.data[0]), static_cast<float>(wheel_commands.data[1])};
std_msgs::msg::Float64MultiArray wheel_commands;
wheel_commands = robotControlManager_.GetWheelCommands();
motorCmdPub_->publish(wheel_commands_msg);
}
MotorCmd wheel_commands_msg;
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])};
motorCmdPub_->publish(wheel_commands_msg);
while (move_wheel_executing_ && rclcpp::ok())
{
@@ -451,14 +450,14 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
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] < 5)
if (joint_feedback.data[0] - wheel_commands.data[0] < 10)
{
move_wheel_executing_ = false;
}

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

@@ -78,16 +78,20 @@ bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double movey
std::vector<double> tempPosition;
tempPosition.resize(total_joints_, 0.0);
tempPosition[0] = joint_position_[0] + movepitch;
tempPosition[1] = joint_position_[1] + moveyaw;
tempPosition[0] = joint_position_[0] - joint_zero_positions_[0] + movepitch;
tempPosition[1] = joint_position_[1] - joint_zero_positions_[1] + moveyaw;
if (!checkJointLimits(tempPosition))
{
std::cout << "Joint limits exceeded!" << std::endl;
return false;
}
joint_position_desired_ = tempPosition;
for (size_t i = 0; i < joint_position_desired_.size(); i++)
{
joint_position_desired_[i] = tempPosition[i] + joint_zero_positions_[i];
}
std::cout << "Waist Target Joint Position: " << joint_position_desired_[0] << ", " << joint_position_desired_[1] << std::endl;
is_target_set_ = true;

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

@@ -25,23 +25,6 @@ WheelControl::WheelControl(
joint_directions)
{
std::cout << "WheelControl Constructor" << std::endl;
if (total_joints_ <= 0)
throw std::invalid_argument("Total joints must be positive");
// 初始化关节状态向量
joint_position_.resize(total_joints_, 0.0);
joint_velocity_.resize(total_joints_, 0.0);
joint_acceleration_.resize(total_joints_, 0.0);
joint_torque_.resize(total_joints_, 0.0);
joint_position_desired_.resize(total_joints_, 0.0);
// 初始化梯形轨迹规划器
trapezoidalTrajectory_ = new TrapezoidalTrajectory(maxSpeed, maxAcc);
is_moving_ = false;
is_stopping_ = false;
stopDurationTime_ = 0.0;
movingDurationTime_ = 0.0;
}
WheelControl::~WheelControl()
@@ -69,6 +52,8 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
//TODO: 设置移动轮角度
(void) moveWheelAngle;
std::cout << "Move Wheel joint_position_desired_: " << joint_position_desired_[0] << " , " << joint_position_desired_[1] << std::endl;
is_target_set_ = true;
return true;