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

set wheel home position added

Browse Source
This commit is contained in:
NuoDaJia02
2025-10-30 14:32:28 +08:00
parent 8b99d0ce85
commit c2f8758864
5 changed files with 21 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -66,6 +66,7 @@ namespace robot_control
bool isWaistHomed_;
bool isLegHomed_;
bool isWheelHomed_;
// 控制器
LegControl* legController_;

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

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

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

@@ -69,6 +69,7 @@ void RobotControlManager::init()
isWaistHomed_ = false;
isLegHomed_ = false;
isWheelHomed_ = false;
isJointInitValueSet_ = false;
}
@@ -269,6 +270,13 @@ void RobotControlManager::UpdateWheelStates(const MotorPos::SharedPtr msg)
wheelEfforts_[i] = 0.0;
}
if (!isWheelHomed_)
{
wheelController_ -> SetWheelHomePositions(wheelPositions_);
isWheelHomed_ = true;
}
wheelController_->UpdateJointStates(wheelPositions_, wheelVelocities_, wheelEfforts_);
}

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] < 10)
if (joint_feedback.data[0] - wheel_commands.data[0] < 5.0 && joint_feedback.data[1] - wheel_commands.data[1] < 5.0)
{
move_wheel_executing_ = false;
}

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

@@ -32,6 +32,11 @@ 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)
{
// 设置移动轮参数
@@ -43,25 +48,23 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
{
if (theta < 0.0)
{
joint_position_desired_[i] = joint_position_[i] + theta;
joint_position_desired_[i] = joint_home_positions_[i] + theta;
}
else
{
joint_position_desired_[i] = joint_position_[i] + theta * 1.02;
joint_position_desired_[i] = joint_home_positions_[i] + theta; // * 1.02;
}
}
else
{
if (theta < 0.0)
{
joint_position_desired_[i] = joint_position_[i] - theta;
joint_position_desired_[i] = joint_home_positions_[i] - theta;
}
else
{
joint_position_desired_[i] = joint_position_[i] - theta * 0.98 * 1.02;
joint_position_desired_[i] = joint_home_positions_[i] - theta; // * 0.98 * 1.02;
}
}
}