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

adjust to 2 motors

Browse Source
This commit is contained in:
NuoDaJia02
2025-10-28 10:58:39 +08:00
parent 2f69f94bbd
commit 08e5962444
4 changed files with 73 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -80,21 +80,21 @@ namespace robot_control
// 关节速度参数
max_joint_velocity_ = {
15,
15,
15,
15,
15,
15
};
max_joint_acceleration_ = {
50,
50,
50,
50,
50,
50
};
max_joint_acceleration_ = {
100,
100,
100,
100,
100,
100
};
// 初始化关节索引
@@ -104,7 +104,7 @@ namespace robot_control
leg_joint_indices_ = {2, 3, 4, 5};
total_joints_ = 6;
total_joints_ = 2;
// 初始化关节方向
wheel_joint_directions_ = {1, -1};
@@ -122,7 +122,7 @@ namespace robot_control
//TODO: 限位需要修改
joint_limits_ = {
JointLimit(0, 30.0, -30.0, LimitType::POSITION),
JointLimit(1, 180.0, -180.0, LimitType::POSITION),
JointLimit(1, 190.0, -190.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),

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

@@ -12,13 +12,18 @@ RobotControlManager::RobotControlManager()
void RobotControlManager::init()
{
// jointInited_.resize(motionParams_.total_joints_, false);
jointInited_.resize(3, false);
jointInited_.resize(motionParams_.total_joints_, false);
// jointInited_.resize(3, false);
// Initialize the joint commands
for (size_t i = 0; i < motionParams_.total_joints_; i++)
{
jointCommands_.data.push_back(0.0);
}
// Initialize the wheel commands
for (size_t i = 0; i < motionParams_.waist_joint_indices_.size(); i++)
{
wheelCommands_.data.push_back(0.0);
}
@@ -247,24 +252,24 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
waistController_->UpdateJointStates(waistPositions, waistVelocities, waistEfforts);
// Update the leg controller
size_t legStartIndex = motionParams_.leg_joint_indices_[0];
size_t legJointSize = motionParams_.leg_joint_indices_.size();
// size_t legStartIndex = motionParams_.leg_joint_indices_[0];
// size_t legJointSize = motionParams_.leg_joint_indices_.size();
std::vector<double> legPositions(
jointPositions_.begin() + legStartIndex,
jointPositions_.begin() + legStartIndex + legJointSize
);
// std::vector<double> legPositions(
// jointPositions_.begin() + legStartIndex,
// jointPositions_.begin() + legStartIndex + legJointSize
// );
std::vector<double> legVelocities(
jointVelocities_.begin() + legStartIndex,
jointVelocities_.begin() + legStartIndex + legJointSize
);
// std::vector<double> legVelocities(
// jointVelocities_.begin() + legStartIndex,
// jointVelocities_.begin() + legStartIndex + legJointSize
// );
std::vector<double> legEfforts(
jointEfforts_.begin() + legStartIndex,
jointEfforts_.begin() + legStartIndex + legJointSize
);
legController_->UpdateJointStates(legPositions, legVelocities, legEfforts);
// std::vector<double> legEfforts(
// jointEfforts_.begin() + legStartIndex,
// jointEfforts_.begin() + legStartIndex + legJointSize
// );
// legController_->UpdateJointStates(legPositions, legVelocities, legEfforts);
}
MotionParameters RobotControlManager::GetMotionParameters()
@@ -279,11 +284,11 @@ bool RobotControlManager::IsMoving(MovementPart part)
case MovementPart::WHEEL:
return wheelController_->IsMoving();
case MovementPart::LEG:
return legController_->IsMoving();
return true; // legController_->IsMoving();
case MovementPart::WAIST:
return waistController_->IsMoving();
case MovementPart::ALL:
return wheelController_->IsMoving() && legController_->IsMoving() && waistController_->IsMoving();
return wheelController_->IsMoving() && waistController_->IsMoving() ; //&& legController_->IsMoving();
default:
return false;
}
@@ -299,12 +304,12 @@ bool RobotControlManager::Stop(double dt)
AssignTempValues();
bool waistStopped = waistController_->Stop(tempWaistCmd_, dt);
bool legStopped = legController_->Stop(tempLegCmd_, dt);
// bool legStopped = legController_->Stop(tempLegCmd_, dt);
bool wheelStopped = wheelController_->Stop(tempWheelCmd_, dt);
UpdateJointCommands();
return legStopped && waistStopped && wheelStopped;
return waistStopped && wheelStopped; //legStopped &&
}
bool RobotControlManager::MoveLeg(double dt)
@@ -353,11 +358,13 @@ bool RobotControlManager::GoHome(double dt)
{
isWaistHomed_ = waistController_->GoHome(tempWaistCmd_, dt);
}
isLegHomed_ = true;
if (!isLegHomed_)
{
isLegHomed_ = legController_->GoHome(tempLegCmd_, dt);
}
// if (!isLegHomed_)
// {
// isLegHomed_ = legController_->GoHome(tempLegCmd_, dt);
// }
UpdateJointCommands();
@@ -392,12 +399,12 @@ void RobotControlManager::AssignTempValues()
tempWaistCmd_[i] = jointCommands_.data[motionParams_.waist_joint_indices_[i]];
}
tempLegCmd_.resize(motionParams_.leg_joint_indices_.size());
// tempLegCmd_.resize(motionParams_.leg_joint_indices_.size());
for (size_t i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
{
tempLegCmd_[i] = jointCommands_.data[motionParams_.leg_joint_indices_[i]];
}
// for (size_t i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
// {
// tempLegCmd_[i] = jointCommands_.data[motionParams_.leg_joint_indices_[i]];
// }
}
void RobotControlManager::UpdateJointCommands()
@@ -407,8 +414,8 @@ void RobotControlManager::UpdateJointCommands()
jointCommands_.data[motionParams_.waist_joint_indices_[i]] = tempWaistCmd_[i];
}
for (size_t i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.leg_joint_indices_[i]] = tempLegCmd_[i];
}
// for (size_t i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
// {
// jointCommands_.data[motionParams_.leg_joint_indices_[i]] = tempLegCmd_[i];
// }
}

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

@@ -441,6 +441,9 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
wheel_commands_msg.motor_id = {1,2};
wheel_commands_msg.motor_angle = {(float)(wheel_commands.data[0]),(float)(wheel_commands.data[1])};
std::cout << "wheel_commands.data[0] " << wheel_commands.data[0] << std::endl;
std::cout << "wheel_commands.data[1] " << wheel_commands.data[1] << std::endl;
motorCmdPub_->publish(wheel_commands_msg);
while (move_wheel_executing_ && rclcpp::ok())

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

@@ -41,11 +41,27 @@ bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, doub
{
if (joint_directions_[i] == 1)
{
joint_position_desired_[i] = joint_position_[i] + theta;
if (theta < 0.0)
{
joint_position_desired_[i] = joint_position_[i] + theta;
}
else
{
joint_position_desired_[i] = joint_position_[i] + theta * 1.02;
}
}
else
{
joint_position_desired_[i] = joint_position_[i] - theta;
if (theta < 0.0)
{
joint_position_desired_[i] = joint_position_[i] - theta;
}
else
{
joint_position_desired_[i] = joint_position_[i] - theta * 0.98 * 1.02;
}
}
}