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

waist action test success

Browse Source
This commit is contained in:
NuoDaJia
2025-10-19 17:25:01 +08:00
parent 4baed7b619
commit f4867f2702
11 changed files with 208 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/robot_control/CMakeLists.txt
View File

@@ -14,6 +14,7 @@ find_package(sensor_msgs REQUIRED)
find_package(trajectory_msgs REQUIRED)
find_package(control_msgs REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(action_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
# 定义Action文件
@@ -27,6 +28,7 @@ set(action_files
# 生成Action相关代码（添加依赖的消息包，根据你的Action实际使用的类型补充）
rosidl_generate_interfaces(${PROJECT_NAME}
${action_files}
DEPENDENCIES action_msgs
)
# 头文件目录

3
src/robot_control/include/control_base.hpp
View File

@@ -32,6 +32,7 @@ namespace robot_control
bool is_moving_; // 是否运动中
bool is_stopping_; // 是否停止中
bool is_target_set_; // 是否已设置目标点
public:
// 构造函数（子类需调用此构造函数初始化父类）
@@ -67,8 +68,6 @@ namespace robot_control
// 6. 判断是否运动中（通用逻辑，父类实现）
virtual bool IsMoving();
private:
// 父类私有辅助函数（子类不可见，如参数校验）
bool checkJointLimits(const std::vector<double>& target_joint);
};

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

@@ -3,6 +3,8 @@
#include <vector>
#include "sensor_msgs/msg/joint_state.hpp"
#define CYCLE_TIME 4 // 插补周期（毫秒）
namespace robot_control
{
enum class LimitType {
@@ -69,8 +71,6 @@ namespace robot_control
5,
5,
5,
5,
5,
}; // 10 度/秒
max_joint_acceleration_ = {
@@ -84,8 +84,6 @@ namespace robot_control
25,
25,
25,
25,
25,
};
// 初始化关节索引
@@ -93,21 +91,21 @@ namespace robot_control
waist_joint_indices_ = {0, 1};
leg_joint_indices_ = {2, 3, 4, 5, 6, 7, 8, 9, 10, 11};
leg_joint_indices_ = {2, 3, 4, 5, 6, 7, 8, 9};
total_joints_ = 11 ;
total_joints_ = 10;
// 初始化关节方向
wheel_joint_directions_ = {-1, 1};
waist_joint_directions_ = {1, 1};
leg_joint_directions_ = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1};
leg_joint_directions_ = {1, 1, 1, 1, 1, 1, 1, 1};
//TODO: check the ratio
joint_gear_ratio_ = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}; // 传动比 都一样
joint_gear_ratio_ = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}; // 传动比 都一样
joint_resolution_ = {524288, 524288 , 524288, 524288, 524288, 524288, 524288, 524288, 524288, 524288, 524288, 524288}; // 分辨率 都一样
joint_resolution_ = {524288, 131072 , 131072, 131072, 131072, 131072, 131072, 131072, 131072, 131072}; // 分辨率 都一样
joint_limits_ = {
JointLimit(0, 30.0, -30.0, LimitType::POSITION),
@@ -116,12 +114,12 @@ namespace robot_control
JointLimit(3, 30.0, -30.0, LimitType::POSITION),
JointLimit(4, 2.0, -2.0, LimitType::POSITION),
JointLimit(5, 30.0, -30.0, LimitType::POSITION),
JointLimit(6, 50.0, -50.0, LimitType::POSITION),
JointLimit(7, 50.0, -50.0, LimitType::POSITION),
JointLimit(8, 30.0, -30.0, LimitType::POSITION),
// JointLimit(6, 50.0, -50.0, LimitType::POSITION),
// JointLimit(7, 50.0, -50.0, LimitType::POSITION),
JointLimit(6, 30.0, -30.0, LimitType::POSITION),
JointLimit(7, 30.0, -30.0, LimitType::POSITION),
JointLimit(8, 2.0, -2.0, LimitType::POSITION),
JointLimit(9, 30.0, -30.0, LimitType::POSITION),
JointLimit(10, 2.0, -2.0, LimitType::POSITION),
JointLimit(11, 30.0, -30.0, LimitType::POSITION),
};
// 初始化限制参数
@@ -165,8 +163,6 @@ namespace robot_control
0.0,
0.0,
0.0,
0.0,
0.0,
0.0
};
@@ -174,18 +170,20 @@ namespace robot_control
radian_to_degree_ = 180.0 / M_PI;
degree_to_radian_ = M_PI / 180.0;
degree_to_pulse_ = 524288.0 / 360.0;
pulse_to_degree_ = 360.0 / 524288.0;
pulse_to_degree_.resize(joint_resolution_.size());
degree_to_pulse_.resize(joint_resolution_.size());
pulse_to_radian_ = 2.0 * M_PI / 524288.0;
radian_to_pulse_ = 524288.0 / (2.0 * M_PI);
for (size_t i = 0; i < joint_resolution_.size(); i++)
{
degree_to_pulse_[i] = joint_resolution_[i] / 360.0;
pulse_to_degree_[i] = 360.0 / joint_resolution_[i];
}
jog_step_size_ = 10 * pulse_to_degree_;
jog_step_size_ = 10.0/ (1000.0 / CYCLE_TIME) ; // 5度每秒
};
// 运动参数
size_t total_joints_; // 总关节数
// 关节索引
@@ -233,10 +231,9 @@ namespace robot_control
// 转换参数
double radian_to_degree_; // 弧度转角度的转换因子
double degree_to_radian_; // 角度转弧度的转换因子
double degree_to_pulse_; // 角度转脉冲的转换因子
double pulse_to_degree_; // 脉冲转角度的转换因子
double pulse_to_radian_; // 脉冲转弧度的转换因子
double radian_to_pulse_; // 弧度转脉冲的转换因子
std::vector<double> degree_to_pulse_; // 角度转脉冲的转换因子
std::vector<double> pulse_to_degree_; // 脉冲转角度的转换因子
double jog_step_size_; // Jog步长

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

@@ -35,7 +35,7 @@ namespace robot_control
// 检查参数是否合理
bool CheckMoveWheel(double moveWheelDistance, double moveWheelAngle){return wheelController_->CheckMoveWheel(moveWheelDistance, moveWheelAngle);};
bool CheckMoveLeg(double moveLegDistance){return legController_->CheckMoveLeg(moveLegDistance);};
bool CheckMoveWaist(double movePitchAngle, double moveYawAngle){return waistController_->CheckMoveWaist(movePitchAngle, moveYawAngle);};
bool SetMoveWaistParameters(double movePitchAngle, double moveYawAngle){return waistController_->SetMoveWaistParametersInternal(movePitchAngle, moveYawAngle);};
// 设置运动参数
void SetMoveLeg(double moveLegDistance){moveLegDistance_ = moveLegDistance;};
@@ -53,6 +53,8 @@ namespace robot_control
MotionParameters GetMotionParameters();
bool IsMoving(MovementPart part);
bool RobotInitFinished();
private:
void init();
@@ -82,11 +84,11 @@ namespace robot_control
std::vector<double> jointEfforts_; // 关节力矩（牛顿米）
std::vector<bool> jointInited_; // 机器人是否已经初始化
std::vector<bool> robotHomed_; // 关节是否已经复位
bool isJointInitValueSet_;
// 临时变量
std::vector<double> tempWaistCmd_;
std::vector<double> tempLegCmd;
std::vector<double> tempLegCmd_;
sensor_msgs::msg::JointState jointStates_;
std_msgs::msg::Float64MultiArray jointCommands_;

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

@@ -21,7 +21,7 @@ namespace robot_control
~WaistControl() override;
bool CheckMoveWaist(double movepitch, double moveyaw);
bool SetMoveWaistParametersInternal(double movepitch, double moveyaw);
bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;

18
src/robot_control/package.xml
View File

@@ -7,15 +7,31 @@
<maintainer email="Ray@email.com">Your Name</maintainer>
<license>Apache-2.0</license>
<!-- 编译时依赖 -->
<build_depend>ament_cmake</build_depend>
<build_depend>rosidl_default_generators</build_depend>
<build_depend>action_msgs</build_depend>
<build_depend>rclcpp</build_depend> <!-- 编译时也需依赖，避免链接错误 -->
<build_depend>rclcpp_action</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>trajectory_msgs</build_depend>
<build_depend>control_msgs</build_depend>
<build_depend>Eigen3</build_depend> <!-- 对应CMake中的Eigen3依赖 -->
<!-- 运行时依赖（必须完整，否则类型无法识别） -->
<exec_depend>rclcpp</exec_depend>
<exec_depend>rclcpp_action</exec_depend> <!-- 关键：Action通信核心库 -->
<exec_depend>action_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>trajectory_msgs</exec_depend>
<exec_depend>control_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

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

@@ -38,6 +38,8 @@ ControlBase::ControlBase(
is_moving_ = false;
is_stopping_ = false;
is_target_set_ = false;
stopDurationTime_ = 0.0;
movingDurationTime_ = 0.0;
}
@@ -160,4 +162,18 @@ bool ControlBase::IsMoving()
return is_moving_;
}
bool ControlBase::checkJointLimits(const std::vector<double>& target_joint)
{
for (size_t i = 0; i < target_joint.size(); i++)
{
if (target_joint[i] < minLimits_[i] || target_joint[i] > maxLimits_[i])
{
printf("Joint %zu target position %f is out of limits [%f, %f]", i, target_joint[i], minLimits_[i], maxLimits_[i]);
return false;
}
}
return true;
}
} // namespace robot_control

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

@@ -25,23 +25,6 @@ LegControl::LegControl(
joint_directions)
{
std::cout << "LegControl 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;
}
LegControl::~LegControl()

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

@@ -12,7 +12,8 @@ RobotControlManager::RobotControlManager()
void RobotControlManager::init()
{
jointInited_.resize(motionParams_.total_joints_, 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++)
@@ -51,7 +52,7 @@ void RobotControlManager::init()
motionParams_.wheel_joint_directions_
);
robotHomed_.resize(4, false);
isJointInitValueSet_ = false;
}
RobotControlManager::~RobotControlManager()
@@ -102,7 +103,7 @@ Float64MultiArray RobotControlManager::GetJointCommands()
double normalizedAngle = fmod(angle, 360.0);
// 计算原始脉冲值
double pulseValue = normalizedAngle * motionParams_.degree_to_pulse_;
double pulseValue = normalizedAngle * motionParams_.degree_to_pulse_[i];
tempValues.data[i] = pulseValue;
}
@@ -137,7 +138,6 @@ bool isAllTrueManual(const std::vector<bool>& vec) {
return true; // 所有元素都是 true
}
bool isValueSet = false;
void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg)
{
jointStates_ = *msg;
@@ -145,7 +145,8 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
jointVelocities_.resize(motionParams_.total_joints_);
jointEfforts_.resize(motionParams_.total_joints_);
for (size_t i = 0; i < motionParams_.total_joints_; i++)
// for (size_t i = 0; i < motionParams_.total_joints_; i++)
for (size_t i = 0; i < 3; i++)
{
std::string joint_name = jointStates_.name[i];
size_t index = 1e5;
@@ -156,7 +157,7 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
try
{
index = std::stoi(num_str) - 1;
index = std::stoi(num_str);
}
catch (const std::invalid_argument& e)
{
@@ -166,8 +167,8 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
if (index != 1e5 && index < motionParams_.total_joints_)
{
jointPositions_[index] = jointStates_.position[i] * motionParams_.pulse_to_degree_;
jointVelocities_[index] = jointStates_.velocity[i] * motionParams_.pulse_to_degree_;
jointPositions_[index] = jointStates_.position[i] * motionParams_.pulse_to_degree_[i];
jointVelocities_[index] = jointStates_.velocity[i] * motionParams_.pulse_to_degree_[i];
jointEfforts_[index] = 0.0;
}
@@ -176,24 +177,26 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
// TODO: This block can be deleted
for (size_t i = 0; i < motionParams_.total_joints_; i++)
// for (size_t i = 0; i < motionParams_.total_joints_; i++)
for (size_t i = 0; i < 3; i++)
{
if(!jointInited_[i])
{
if(jointPositions_[i] != 0)
{
jointInited_[i] = true;
std::cout << "get joint feedback, joint" << i + 1 << " " << jointPositions_[i] << std::endl;
}
}
}
static int count = 0;
if (isAllTrueManual(jointInited_) && !isValueSet)
if (isAllTrueManual(jointInited_) && !isJointInitValueSet_)
{
if (count > 5000)
if (count > 50)
{
jointCommands_.data = jointPositions_;
isValueSet = true;
isJointInitValueSet_ = true;
count = 0;
std::cout << "Joint commands set to joint positions" << std::endl;
std::cout << "All joints are initialized" << std::endl;
@@ -213,11 +216,11 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
waistController_->UpdateJointStates(waistPositions, waistVelocities, waistEfforts);
// Update the leg controller
size_t legJointSize = motionParams_.leg_joint_indices_.size();
std::vector<double> legPositions(jointPositions_.begin() + motionParams_.leg_joint_indices_[0], jointPositions_.begin() + motionParams_.leg_joint_indices_[0] + legJointSize);
std::vector<double> legVelocities(jointVelocities_.begin() + motionParams_.leg_joint_indices_[0], jointVelocities_.begin() + motionParams_.leg_joint_indices_[0] + legJointSize);
std::vector<double> legEfforts(jointEfforts_.begin() + motionParams_.leg_joint_indices_[0], jointEfforts_.begin() + motionParams_.leg_joint_indices_[0] + legJointSize);
legController_->UpdateJointStates(legPositions, legVelocities, legEfforts);
// size_t legJointSize = motionParams_.leg_joint_indices_.size();
// std::vector<double> legPositions(jointPositions_.begin() + motionParams_.leg_joint_indices_[0], jointPositions_.begin() + motionParams_.leg_joint_indices_[0] + legJointSize);
// std::vector<double> legVelocities(jointVelocities_.begin() + motionParams_.leg_joint_indices_[0], jointVelocities_.begin() + motionParams_.leg_joint_indices_[0] + legJointSize);
// std::vector<double> legEfforts(jointEfforts_.begin() + motionParams_.leg_joint_indices_[0], jointEfforts_.begin() + motionParams_.leg_joint_indices_[0] + legJointSize);
// legController_->UpdateJointStates(legPositions, legVelocities, legEfforts);
}
MotionParameters RobotControlManager::GetMotionParameters()
@@ -242,6 +245,11 @@ bool RobotControlManager::IsMoving(MovementPart part)
}
}
bool RobotControlManager::RobotInitFinished()
{
return isJointInitValueSet_;
}
void RobotControlManager::MoveForward()
{
wheelController_ -> setLinearAngularSpeed(motionParams_.max_linear_speed_x_, 0);
@@ -292,7 +300,7 @@ bool RobotControlManager::Stop(double dt)
AssignTempValues();
bool waistStopped = waistController_->Stop(tempWaistCmd_, dt);
bool legStopped = legController_->Stop(tempLegCmd, dt);
bool legStopped = legController_->Stop(tempLegCmd_, dt);
UpdateJointCommands();
@@ -303,7 +311,7 @@ bool RobotControlManager::MoveLeg(double dt)
{
AssignTempValues();
bool result = legController_ -> MoveUp(moveLegDistance_, dt,tempLegCmd);
bool result = legController_ -> MoveUp(moveLegDistance_, dt,tempLegCmd_);
UpdateJointCommands();
@@ -314,7 +322,7 @@ bool RobotControlManager::MoveWaist(double dt)
{
AssignTempValues();
bool result = waistController_ -> MoveWaist({moveWaistPitchAngle_,moveWaistYawAngle_}, dt,tempLegCmd);
bool result = waistController_ -> MoveWaist({moveWaistPitchAngle_,moveWaistYawAngle_}, dt, tempWaistCmd_);
UpdateJointCommands();
@@ -337,7 +345,7 @@ bool RobotControlManager::GoHome(double dt)
AssignTempValues();
bool waistHomed = waistController_->GoHome(tempWaistCmd_, dt);
bool legHomed = legController_->GoHome(tempLegCmd, dt);
bool legHomed = legController_->GoHome(tempLegCmd_, dt);
UpdateJointCommands();
@@ -363,11 +371,11 @@ 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]];
tempLegCmd_[i] = jointCommands_.data[motionParams_.leg_joint_indices_[i]];
}
}
@@ -380,6 +388,6 @@ void RobotControlManager::UpdateJointCommands()
for (size_t i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
{
jointCommands_.data[motionParams_.leg_joint_indices_[i]] = tempLegCmd[i];
jointCommands_.data[motionParams_.leg_joint_indices_[i]] = tempLegCmd_[i];
}
}

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

@@ -73,7 +73,7 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
ethercatSetPub_ = this->create_publisher<std_msgs::msg::String>("/ethercat/set", 10);
jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("/joint_states", 10,std::bind(&RobotControlNode::JointStatesCallback, this, std::placeholders::_1));
joyCommandSub_ = this->create_subscription<std_msgs::msg::String>("/gamepad_msg", 10,std::bind(&RobotControlNode::JoyCommandCallback, this, std::placeholders::_1));
controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(1),std::bind(&RobotControlNode::ControlLoop, this)); // 绑定到新的控制函数);
controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(CYCLE_TIME),std::bind(&RobotControlNode::ControlLoop, this)); // 绑定到新的控制函数);
lastTime_ = this->now(); // 初始化时间
std::cout << "RobotFsm Node is created finished!" << std::endl;
@@ -105,6 +105,12 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_home_goal(
RCLCPP_ERROR(this->get_logger(), "Another move home goal is executing");
return rclcpp_action::GoalResponse::REJECT;
}
if (isJogMode_)
{
RCLCPP_ERROR(this->get_logger(), "Jog mode is enabled");
return rclcpp_action::GoalResponse::REJECT;
}
(void)uuid;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
@@ -189,6 +195,12 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_leg_goal(
return rclcpp_action::GoalResponse::REJECT;
}
if (isJogMode_)
{
RCLCPP_ERROR(this->get_logger(), "Jog mode is enabled");
return rclcpp_action::GoalResponse::REJECT;
}
(void)uuid;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
@@ -258,8 +270,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_waist_goal(
{
RCLCPP_INFO(this->get_logger(), "Received move waist request: total time");
robotControlManager_.CheckMoveWaist(goal->move_pitch_degree, goal->move_yaw_degree);
if (robotControlManager_.IsMoving(MovementPart::WAIST))
{
RCLCPP_ERROR(this->get_logger(), "Robot is moving");
@@ -271,6 +281,20 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_waist_goal(
RCLCPP_ERROR(this->get_logger(), "Another move waist goal is executing");
return rclcpp_action::GoalResponse::REJECT;
}
if (isJogMode_)
{
RCLCPP_ERROR(this->get_logger(), "Jog mode is enabled");
return rclcpp_action::GoalResponse::REJECT;
}
if (!robotControlManager_.SetMoveWaistParameters(goal->move_pitch_degree, goal->move_yaw_degree))
{
RCLCPP_ERROR(this->get_logger(), "Invalid move waist request");
return rclcpp_action::GoalResponse::REJECT;
}
(void)uuid;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
@@ -443,20 +467,32 @@ void RobotControlNode::ControlLoop() {
if (move_leg_executing_)
{
robotControlManager_.MoveLeg(dt_sec);
if(robotControlManager_.MoveLeg(dt_sec))
{
move_leg_executing_ = false;
}
}
if (move_waist_executing_)
{
robotControlManager_.MoveWaist(dt_sec);
if(robotControlManager_.MoveWaist(dt_sec))
{
move_waist_executing_ = false;
}
}
if (move_wheel_executing_)
{
robotControlManager_.MoveWheel(dt_sec);
if(robotControlManager_.MoveWheel(dt_sec))
{
move_wheel_executing_ = false;
}
}
Publish_joint_trajectory();
if (robotControlManager_.RobotInitFinished())
{
Publish_joint_trajectory();
}
}
@@ -489,7 +525,7 @@ void RobotControlNode::Publish_joint_trajectory()
std::stringstream msg_stream;
size_t total_joints = robotControlManager_.GetMotionParameters().total_joints_;
for (size_t i = 0; i < total_joints; ++i)
for (size_t i = 0; i < 3; ++i)
{
double current_pos = cmd_msg.data[i];
@@ -501,6 +537,9 @@ void RobotControlNode::Publish_joint_trajectory()
}
}
positionMsg.data = msg_stream.str();
// std::cout << "publishing joint trajectory: " << positionMsg.data << std::endl;
ethercatSetPub_->publish(positionMsg);
}
@@ -518,27 +557,55 @@ void RobotControlNode::JoyCommandCallback(const std_msgs::msg::String::SharedPtr
else
return;
if (command == "LB,1") {
if (command == "RB,1") {
if (isJogMode_ && jogDirection_ == 0)
{
jogIndex_ ++;
std::cout << "switch to jog axis: " << jogIndex_ + 1 << std::endl;
if (jogIndex_ < robotControlManager_.GetMotionParameters().total_joints_ - 1)
{
jogIndex_ ++;
std::cout << "switch to jog axis: " << jogIndex_ + 1 << std::endl;
}
else
{
std::cout << "Reach the max joint, can't switch to next axis: " << jogIndex_ + 1 << std::endl;
}
}
}
else if (command == "RB,1") {
else if (command == "LB,1") {
if (isJogMode_ && jogDirection_ == 0)
{
jogIndex_ --;
std::cout << "switch to jog axis: " << jogIndex_ + 1 << std::endl;
if (jogIndex_ > 0)
{
jogIndex_ --;
std::cout << "switch to jog axis: " << jogIndex_ + 1 << std::endl;
}
else
{
std::cout << "Reach the min joint, can't switch to previous axis: " << jogIndex_ + 1 << std::endl;
}
}
}
else if (command == "A,1") {
isJogMode_ = 1;
std::cout << "jog mode on" << std::endl;
if (!robotControlManager_.IsMoving(MovementPart::ALL))
{
isJogMode_ = 1;
std::cout << "jog mode on" << std::endl;
}
else
{
std::cout << "robot is moving, can't switch jog mode" << std::endl;
}
}
else if (command == "B,1") {
isJogMode_ = 0;
std::cout << "jog mode OFF" << std::endl;
if (!robotControlManager_.IsMoving(MovementPart::ALL))
{
isJogMode_ = 0;
std::cout << "jog mode OFF" << std::endl;
}
else
{
std::cout << "robot is moving, can't switch jog mode" << std::endl;
}
}
else if (command == "方向垂直,-1.0") {
if (isJogMode_)
@@ -568,9 +635,6 @@ void RobotControlNode::JoyCommandCallback(const std_msgs::msg::String::SharedPtr
std::cout << "stop robot" << std::endl;
}
}
else {
RCLCPP_WARN(this->get_logger(), "未知的命令: %s", command.c_str());
}
}
void RobotControlNode::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg)

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

@@ -24,23 +24,6 @@ WaistControl::WaistControl(
joint_directions)
{
std::cout << "WaistControl 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(10.0, 100);
is_moving_ = false;
is_stopping_ = false;
stopDurationTime_ = 0.0;
movingDurationTime_ = 0.0;
}
WaistControl::~WaistControl()
@@ -71,32 +54,42 @@ bool WaistControl::MoveWaist(const std::vector<double> waistAngles, double dt, s
{
if (!is_moving_)
{
for (size_t i = 0; i < joint_position_.size(); i++)
if (!is_target_set_)
{
if (joint_directions_[i] == 1)
{
joint_position_desired_[i] = joint_position_[i] + 20;
}
else
{
joint_position_desired_[i] = joint_position_[i] - 20;
}
return false;
}
}
return MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
bool result = MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
if (result)
{
is_target_set_ = false;
}
return result;
}
bool WaistControl::CheckMoveWaist(double movepitch, double moveyaw)
bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double moveyaw)
{
if (movepitch == 0 && moveyaw == 0)
std::vector<double> tempPosition;
tempPosition.resize(total_joints_, 0.0);
tempPosition[0] = joint_position_[0] + movepitch;
tempPosition[1] = joint_position_[1] + moveyaw;
if (!checkJointLimits(tempPosition))
{
return false;
}
else
{
return true;
}
joint_position_desired_ = tempPosition;
std::cout << "Target Joint Position: " << joint_position_desired_[0] << ", " << joint_position_desired_[1] << std::endl;
is_target_set_ = true;
return true;
}
} // namespace robot_control