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

code optimization

Browse Source
This commit is contained in:
hivecore
2025-10-23 14:12:07 +08:00
parent 5cdb04dc9d
commit 01e0ab59d5
17 changed files with 480 additions and 348 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
src/robot_control/CMakeLists.txt
View File

@@ -16,20 +16,7 @@ find_package(control_msgs REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(action_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
# 定义Action文件
set(action_files
"action/MoveLeg.action"
"action/MoveWaist.action"
"action/MoveHome.action"
"action/MoveWheel.action"
)
# 生成Action相关代码（添加依赖的消息包，根据你的Action实际使用的类型补充）
rosidl_generate_interfaces(${PROJECT_NAME}
${action_files}
DEPENDENCIES action_msgs
)
find_package(interfaces REQUIRED)
# 头文件目录
include_directories(
@@ -66,13 +53,9 @@ ament_target_dependencies(robot_control_node
trajectory_msgs
control_msgs
rclcpp_action
interfaces
)
# 关键：将生成的Action接口代码链接到可执行文件（必须添加）
rosidl_target_interfaces(robot_control_node
${PROJECT_NAME}
"rosidl_typesupport_cpp"
)
# 安装可执行文件和launch目录
install(TARGETS

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

@@ -16,6 +16,7 @@ namespace robot_control
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions
);

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

@@ -21,7 +21,8 @@ namespace robot_control
TrapezoidalTrajectory* trapezoidalTrajectory_; // 轨迹规划器（子类直接用）
std::vector<double> joint_home_positions_; // 回零点位置
std::vector<double> joint_home_positions_; // 回home点位置
std::vector<double> joint_zero_positions_; // 零点位置
std::vector<int> joint_directions_; // 关节运动方向
std::vector<double> joint_position_; // 当前关节位置
std::vector<double> joint_velocity_; // 当前关节速度
@@ -45,6 +46,7 @@ namespace robot_control
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions
);

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

@@ -23,6 +23,7 @@ namespace robot_control
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions
);
@@ -32,7 +33,7 @@ namespace robot_control
bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
bool MoveUp(double dt, std::vector<double>& joint_positions);
bool MoveUp(std::vector<double>& joint_positions, double dt);
};
} // namespace robot_control

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

@@ -1,7 +1,10 @@
#pragma once
#include <vector>
#include "sensor_msgs/msg/joint_state.hpp"
#include <iostream>
#include <map>
#include <string>
#define CYCLE_TIME 4 // 插补周期（毫秒）
@@ -13,6 +16,10 @@
#define RAD2DEG(x) ((x)*57.29578)
#endif
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
namespace robot_control
{
enum class LimitType {
@@ -51,10 +58,14 @@ namespace robot_control
wheel_radius_ = 0.09;
wheel_separation_ = 0.26;
wheel_length_ = {
wheel_radius_,
wheel_radius_
};
// 腿长参数 unit m
leg_length_ = {
0.38,
0.36,
0.70,
0.66
};
@@ -63,17 +74,12 @@ namespace robot_control
};
// 轮子速度参数
// TODO: 如果转弯的话，需要采用速度模式，直接位置模式会有问题。
max_linear_speed_x_ = 100;
max_linear_speed_z_ = 10;
max_angular_speed_z_ = 50;
// 关节速度参数
max_joint_velocity_ = {
25,
25,
25,
25,
25,
25,
25,
@@ -83,10 +89,6 @@ namespace robot_control
};
max_joint_acceleration_ = {
250,
250,
250,
250,
250,
250,
250,
@@ -100,40 +102,62 @@ namespace robot_control
waist_joint_indices_ = {0, 1};
leg_joint_indices_ = {2, 3, 4, 5, 6, 7, 8, 9};
leg_joint_indices_ = {2, 3, 4, 5};
total_joints_ = 10;
total_joints_ = 6;
// 初始化关节方向
wheel_joint_directions_ = {-1, 1};
waist_joint_directions_ = {1, 1};
leg_joint_directions_ = {1, 1, 1, 1, 1, 1, 1, 1};
leg_joint_directions_ = {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}; // 传动比 都一样
joint_gear_ratio_ = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0}; // 传动比 都一样
joint_resolution_ = {131072, 131072 , 131072, 131072, 524288, 131072, 131072, 131072, 131072, 131072}; // 分辨率 都一样
joint_resolution_ = {524288, 131072, 131072 , 131072, 131072, 131072}; // 分辨率 都一样
//TODO: 限位需要修改
joint_limits_ = {
JointLimit(0, 30.0, -30.0, LimitType::POSITION),
JointLimit(1, 180.0, -180.0, LimitType::POSITION),
JointLimit(2, 30.0, -30.0, LimitType::POSITION),
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(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(2, 60.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(7, 5.0, -5.0, LimitType::POSITION),
// JointLimit(8, 50.0, -50.0, LimitType::POSITION),
JointLimit(5, 50.0, 0.0, LimitType::POSITION),
};
// 初始化限制参数
wheel_max_velocity_.resize(wheel_joint_indices_.size());
wheel_max_acceleration_.resize(wheel_joint_indices_.size());
wheel_max_limit_.resize(wheel_joint_indices_.size());
wheel_min_limit_.resize(wheel_joint_indices_.size());
wheel_max_velocity_ = {
25,
25
};
wheel_max_acceleration_ = {
250,
250
};
//There is no limit for wheel
wheel_max_limit_ = {
0.0,
0.0
};
wheel_min_limit_ = {
0.0,
0.0
};
waist_min_limit_.resize(waist_joint_indices_.size());
waist_max_limit_.resize(waist_joint_indices_.size());
waist_max_velocity_.resize(waist_joint_indices_.size());
@@ -159,10 +183,26 @@ namespace robot_control
leg_max_velocity_[i] = max_joint_velocity_[leg_joint_indices_[i]];
leg_max_acceleration_[i] = max_joint_acceleration_[leg_joint_indices_[i]];
}
waist_zero_positions_ = {
181.0,
158.5
};
leg_zero_positions_ = {
-18.36,
// 129.71,
// 45.02,
25.54,
-21.54,
// 167.96,
// 16.26,
-127.24
};
waist_home_positions_ = {
0.0,
0.0
181.0,
158.5
};
@@ -170,13 +210,24 @@ namespace robot_control
// 前腿零点为站直时候的状态, 然后 髋部 pitch + 49度, 大腿长 0.38m, 38度为 0.25m , 小腿长0.36 膝盖 pitch 再收回 35.5 度, 剩余 13.5度 长度为 0.35m
// 初始化初始位置
leg_home_positions_ = {
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
0.0,
30.64,
// 129.71,
// 9.52,
0.54,
-70.54,
// 167.96,
// 51.76,
-102.24
};
// 初始化零点位置
wheel_home_positions_ = {
0.0,
0.0
};
wheel_zero_positions_ = {
0.0,
0.0
};
@@ -200,6 +251,8 @@ namespace robot_control
std::vector<int> leg_joint_indices_; // 腿部关节索引
std::vector<int> wheel_joint_indices_; // 轮子关节索引
std::vector<int> waist_joint_indices_; // 腰部关节索引
std::vector<int> real_waist_joint_indices_; // 实际腰部关节索引
std::vector<int> real_leg_joint_indices_; // 实际腿部关节索引
std::vector<int> wheel_joint_directions_; // 轮子关节方向
std::vector<int> waist_joint_directions_; // 身体关节方向
@@ -207,6 +260,11 @@ namespace robot_control
std::vector<double> leg_home_positions_; // 左腿初始位置
std::vector<double> waist_home_positions_; // 身体初始位置
std::vector<double> wheel_home_positions_; // 轮子零点位置
std::vector<double> waist_zero_positions_; // 身体零点位置
std::vector<double> leg_zero_positions_; // 左腿零点位置
std::vector<double> wheel_zero_positions_; // 轮子零点位置
// 限制参数
std::vector<JointLimit> joint_limits_; // 关节限制
@@ -214,14 +272,18 @@ namespace robot_control
std::vector<double> waist_max_limit_;
std::vector<double> leg_min_limit_;
std::vector<double> leg_max_limit_;
std::vector<double> wheel_min_limit_;
std::vector<double> wheel_max_limit_;
std::vector<double> max_joint_velocity_;
std::vector<double> waist_max_velocity_;
std::vector<double> leg_max_velocity_;
std::vector<double> wheel_max_velocity_;
std::vector<double> max_joint_acceleration_;
std::vector<double> waist_max_acceleration_;
std::vector<double> leg_max_acceleration_;
std::vector<double> wheel_max_acceleration_;
// 轮子控制相关
double wheel_radius_; // 轮子半径（米）
@@ -233,6 +295,7 @@ namespace robot_control
// 尺寸相关
std::vector<double> leg_length_;
std::vector<double> waist_length_;
std::vector<double> wheel_length_;
//关节传动比和分辨率
std::vector<double> joint_gear_ratio_;

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

@@ -12,6 +12,10 @@
#include "common_enum.hpp"
#include "waist_control.hpp"
#include "interfaces/msg/motor_pos.hpp"
using MotorPos = interfaces::msg::MotorPos;
namespace robot_control
{
class RobotControlManager
@@ -21,29 +25,27 @@ namespace robot_control
~RobotControlManager();
// 控制机器人运动
void MoveForward();
void MoveBackward();
void TurnLeft();
void TurnRight();
bool MoveLeg(double dt);
bool MoveWaist(double dt);
bool MoveWheel(double dt);
bool MoveWheel();
bool Stop(double dt);
bool GoHome(double dt);
void JogAxis(size_t axis, int dir);
// 检查参数是否合理
bool CheckMoveWheel(double moveWheelDistance, double moveWheelAngle){return wheelController_->CheckMoveWheel(moveWheelDistance, moveWheelAngle);};
bool SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle){return wheelController_->SetMoveWheelParametersInternal(moveWheelDistance, moveWheelAngle);};
bool SetMoveLegParameters(double moveLegDistance){return legController_->SetMoveLegParametersInternal(moveLegDistance);};
bool SetMoveWaistParameters(double movePitchAngle, double moveYawAngle){return waistController_->SetMoveWaistParametersInternal(movePitchAngle, moveYawAngle);};
// 机器人关节指令
std_msgs::msg::Float64MultiArray GetJointCommands();
std_msgs::msg::Float64MultiArray GetJointFeedback();
std_msgs::msg::Float64MultiArray GetWheelState();
std_msgs::msg::Float64MultiArray GetWheelCommands();
std_msgs::msg::Float64MultiArray GetWheelFeedback();
void CheckJointLimits(std_msgs::msg::Float64MultiArray& jointCommands);
void UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg);
void UpdateWheelStates(const MotorPos::SharedPtr msg);
MotionParameters GetMotionParameters();
@@ -66,10 +68,17 @@ namespace robot_control
WaistControl* waistController_;
// 运动状态
std::vector<double> wheelPositions_;
std::vector<double> jointPositions_; // 关节位置（弧度）
std::vector<double> jointVelocities_; // 关节速度（弧度/秒）
std::vector<double> wheelVelocities_; // 关节速度（弧度/秒）
std::vector<double> jointAccelerations_; // 关节加速度（弧度/秒^2）
std::vector<double> wheelAccelerations_; // 关节加速度（弧度/秒^2）
std::vector<double> jointEfforts_; // 关节力矩（牛顿米）
std::vector<double> wheelEfforts_; // 关节力矩（牛顿米）
std::vector<bool> jointInited_; // 机器人是否已经初始化
bool isJointInitValueSet_;
@@ -77,13 +86,15 @@ namespace robot_control
// 临时变量
std::vector<double> tempWaistCmd_;
std::vector<double> tempLegCmd_;
std::vector<double> tempWheelCmd_;
MotorPos motorPos_;
sensor_msgs::msg::JointState jointStates_;
std_msgs::msg::Float64MultiArray jointCommands_;
std_msgs::msg::Float64MultiArray wheelCommands_;
void AssignTempValues();
void UpdateJointCommands();
};
} // namespace robot_control_manager

23
src/robot_control/include/robot_control_node.hpp
View File

@@ -11,23 +11,27 @@
#include "std_msgs/msg/string.hpp"
#include "robot_control_manager.hpp"
#include "robot_control/action/move_home.hpp"
#include "robot_control/action/move_leg.hpp"
#include "robot_control/action/move_waist.hpp"
#include "robot_control/action/move_wheel.hpp"
#include "interfaces/action/move_home.hpp"
#include "interfaces/action/move_leg.hpp"
#include "interfaces/action/move_waist.hpp"
#include "interfaces/action/move_wheel.hpp"
#include "interfaces/msg/motor_cmd.hpp"
using MoveHome = robot_control::action::MoveHome;
using MoveHome = interfaces::action::MoveHome;
using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;
using MoveLeg = robot_control::action::MoveLeg;
using MoveLeg = interfaces::action::MoveLeg;
using GoalHandleMoveLeg = rclcpp_action::ServerGoalHandle<MoveLeg>;
using MoveWaist = robot_control::action::MoveWaist;
using MoveWaist = interfaces::action::MoveWaist;
using GoalHandleMoveWaist = rclcpp_action::ServerGoalHandle<MoveWaist>;
using MoveWheel = robot_control::action::MoveWheel;
using MoveWheel = interfaces::action::MoveWheel;
using GoalHandleMoveWheel = rclcpp_action::ServerGoalHandle<MoveWheel>;
using MotorCmd = interfaces::msg::MotorCmd;
namespace robot_control
{
class RobotControlNode : public rclcpp::Node
@@ -75,6 +79,8 @@ namespace robot_control
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr ethercatSetPub_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr joyCommandSub_;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_;
rclcpp::Publisher<MotorCmd>::SharedPtr motorCmdPub_;
rclcpp::Subscription<MotorPos>::SharedPtr wheelStatesSub_;
RobotControlManager robotControlManager_;
@@ -95,6 +101,7 @@ namespace robot_control
void JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
void JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg);
void WheelStatesCallback(const MotorPos::SharedPtr cmd_msg);
};
} // namespace robot_control

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

@@ -16,6 +16,7 @@ namespace robot_control
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions
);
@@ -25,7 +26,7 @@ namespace robot_control
bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
bool MoveWaist(double dt, std::vector<double>& joint_positions);
bool MoveWaist(std::vector<double>& joint_positions, double dt);
};
}

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

@@ -1,53 +1,33 @@
#pragma once
#include "rclcpp/rclcpp.hpp"
#include <Eigen/Dense>
#include "trapezoidal_trajectory.hpp"
#include "control_base.hpp"
namespace robot_control
{
class WheelControl
class WheelControl : public ControlBase
{
public:
WheelControl(int totalJoints, double wheelRadius, double wheelSeparation, double linearSpeed, double angularSpeed, std::vector<int32_t> wheelDirection);
~WheelControl();
public:
WheelControl(
size_t total_joints,
const std::vector<double>& lengthParameters,
const std::vector<double>& maxSpeed,
const std::vector<double>& maxAcc,
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions
);
void ResetJoints();
~WheelControl() override;
void CalculateWheelSpeeds();
void setLinearAngularSpeed(double linearSpeed, double angularSpeed);
bool SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle);
bool ExecuteCommand();
bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
bool CheckMoveWheel(double moveWheelDistance, double moveWheelAngle);
bool MoveWheel(std::vector<double>& joint_positions);
double GetLinearSpeed() {return linearSpeed_;}
bool IsMoving() {return !isSendFinished_;}
private:
int totalJoints_; // 总关节数
TrapezoidalTrajectory wheel_trajectory_; // 轮子轨迹（速度模式）
// 轮子控制相关
double wheelRadius_; // 轮子半径（米）
double wheelSeparation_; // 轮距（米）
double linearSpeed_; // 线速度（m/s）
double angularSpeed_; // 角速度（rad/s）
double maxLinearSpeed_; // 最大线速度（m/s）
double maxAngularSpeed_; // 最大角速度（rad/s）
bool isSendFinished_; // 是否发送完成
std::vector<int32_t> wheelSpeed_; // 轮子速度（rpm）
std::vector<int32_t> previousWheelSpeed_; // 轮子速度（rpm）
std::vector<int32_t> wheelDirection_; // 关节位置（rad）
bool GoHome(std::vector<double>& joint_positions, double dt) override;
};
} // namespace robot_control

3
src/robot_control/package.xml
View File

@@ -18,6 +18,8 @@
<build_depend>trajectory_msgs</build_depend>
<build_depend>control_msgs</build_depend>
<build_depend>Eigen3</build_depend> <!-- 对应CMake中的Eigen3依赖 -->
<build_depend>interfaces</build_depend>
<!-- 运行时依赖（必须完整，否则类型无法识别） -->
<exec_depend>rclcpp</exec_depend>
@@ -28,6 +30,7 @@
<exec_depend>trajectory_msgs</exec_depend>
<exec_depend>control_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>interfaces</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

2
src/robot_control/src/arm_control.cpp
View File

@@ -13,6 +13,7 @@ ArmControl::ArmControl(
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions
): ControlBase(
total_joints,
@@ -21,6 +22,7 @@ ArmControl::ArmControl(
minLimits,
maxLimits,
home_positions,
zero_positions,
joint_directions)
{
std::cout << "ArmControl Constructor" << std::endl;

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

@@ -13,6 +13,7 @@ ControlBase::ControlBase(
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions
):
total_joints_(total_joints),
@@ -20,6 +21,7 @@ ControlBase::ControlBase(
maxSpeed_(maxSpeed),
maxAcc_(maxAcc),
joint_home_positions_(home_positions),
joint_zero_positions_(zero_positions),
joint_directions_(joint_directions),
minLimits_(minLimits),
maxLimits_(maxLimits)

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

@@ -13,6 +13,7 @@ LegControl::LegControl(
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions
) : ControlBase(
total_joints,
@@ -22,6 +23,7 @@ LegControl::LegControl(
minLimits,
maxLimits,
home_positions,
zero_positions,
joint_directions)
{
std::cout << "LegControl Constructor" << std::endl;
@@ -50,7 +52,7 @@ bool LegControl::MoveToTargetPoint(std::vector<double>& joint_positions, const s
return MoveToTargetJoint(joint_positions, joint_target, dt);
}
bool LegControl::MoveUp(double dt, std::vector<double>& joint_positions)
bool LegControl::MoveUp(std::vector<double>& joint_positions, double dt)
{
if (!is_moving_)
{
@@ -78,12 +80,14 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
std::vector<double> tempPosition;
tempPosition.resize(total_joints_, 0.0);
double currentBackLegLength1 = lengthParameters_[3] * abs(std::cos(DEG2RAD(joint_position_[0] - joint_home_positions_[0])));
double currentFrountLegLength1 = lengthParameters_[0] * abs(std::cos(DEG2RAD(joint_position_[1] - joint_home_positions_[1])));
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 > (lengthParameters_[0] - currentFrountLegLength1 - 5))
if ((moveDistance + currentFrountLegLength) > (lengthParameters_[0] - 5) || (moveDistance + currentBackLegLength) > (lengthParameters_[1] -5))
{
std::cout << "Move distance is too large!" << std::endl;
return false;
@@ -91,36 +95,33 @@ bool LegControl::SetMoveLegParametersInternal(double moveDistance)
}
else
{
if (moveDistance < -currentFrountLegLength1 - 5)
if (moveDistance < (-currentFrountLegLength + 5) || moveDistance < (-currentBackLegLength + 5))
{
std::cout << "Move distance is too large!" << std::endl;
return false;
}
}
double finalBackLegLength1 = currentBackLegLength1 + moveDistance;
double finalFrountLegLength1 = currentFrountLegLength1 + moveDistance;
double finalFrountLegLength = currentFrountLegLength + moveDistance;
double finalBackLegLength = currentBackLegLength + moveDistance;
double frontFinalAngle = RAD2DEG(std::acos(finalFrountLegLength / lengthParameters_[0]));
double backFinalAngle = RAD2DEG(std::acos(finalBackLegLength / lengthParameters_[1]));
double backFinalAngle = RAD2DEG(std::acos(finalBackLegLength1 / lengthParameters_[3])) + joint_home_positions_[0];
double frontFinalAngle = RAD2DEG(std::acos(finalFrountLegLength1 / lengthParameters_[0])) + joint_home_positions_[1];
double backAdjustAngle = backFinalAngle - joint_position_[0];
double frontAdjustAngle = frontFinalAngle - joint_position_[1];
tempPosition[0] = joint_position_[0] + backAdjustAngle;
tempPosition[1] = joint_position_[1] + frontAdjustAngle;
tempPosition[2] = joint_position_[2];
tempPosition[3] = joint_position_[3] - frontAdjustAngle; //TODO : check the direction
tempPosition[4] = joint_position_[4] + backAdjustAngle;
tempPosition[5] = joint_position_[5] + frontAdjustAngle;
tempPosition[6] = joint_position_[6];
tempPosition[7] = joint_position_[7] - frontAdjustAngle; //TODO : check the direction
tempPosition[0] = joint_directions_[0] * frontFinalAngle;
tempPosition[1] = joint_directions_[1] * backFinalAngle;
tempPosition[2] = joint_directions_[2] * frontFinalAngle;
tempPosition[3] = joint_directions_[3] * backFinalAngle;
if (!checkJointLimits(tempPosition))
{
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 << "Target Joint Position: " << joint_position_desired_[0] << ", " << joint_position_desired_[1] << std::endl;

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

@@ -19,6 +19,7 @@ void RobotControlManager::init()
for (size_t i = 0; i < motionParams_.total_joints_; i++)
{
jointCommands_.data.push_back(0.0);
wheelCommands_.data.push_back(0.0);
}
legController_ = new LegControl(
@@ -29,6 +30,7 @@ void RobotControlManager::init()
motionParams_.leg_min_limit_,
motionParams_.leg_max_limit_,
motionParams_.leg_home_positions_,
motionParams_.leg_zero_positions_,
motionParams_.leg_joint_directions_
);
@@ -40,15 +42,19 @@ void RobotControlManager::init()
motionParams_.waist_min_limit_,
motionParams_.waist_max_limit_,
motionParams_.waist_home_positions_,
motionParams_.waist_zero_positions_,
motionParams_.waist_joint_directions_
);
wheelController_ = new WheelControl(
motionParams_.wheel_joint_indices_.size(),
motionParams_.wheel_radius_,
motionParams_.wheel_separation_,
motionParams_.max_linear_speed_x_,
motionParams_.max_angular_speed_z_,
motionParams_.wheel_length_,
motionParams_.wheel_max_velocity_,
motionParams_.wheel_max_acceleration_,
motionParams_.wheel_min_limit_,
motionParams_.wheel_max_limit_,
motionParams_.wheel_home_positions_,
motionParams_.wheel_zero_positions_,
motionParams_.wheel_joint_directions_
);
@@ -62,14 +68,17 @@ RobotControlManager::~RobotControlManager()
delete wheelController_;
}
Float64MultiArray RobotControlManager::GetWheelState()
Float64MultiArray RobotControlManager::GetWheelCommands()
{
Float64MultiArray tempValues;
tempValues.data.resize(2);
//TODO： get the wheel state feedback
tempValues.data[0] = 0.0;
tempValues.data[1] = 0.0;
for (size_t i = 0; i < wheelCommands_.data.size(); i++)
{
double angle = wheelCommands_.data[i];
tempValues.data[i] = angle;
}
return tempValues;
}
@@ -79,7 +88,6 @@ Float64MultiArray RobotControlManager::GetJointFeedback()
Float64MultiArray tempValues;
tempValues.data.resize(motionParams_.total_joints_);
// Convert the joint commands to motor values and limit to encoder range
for (size_t i = 0; i < jointPositions_.size(); i++)
{
double angle = jointPositions_[i];
@@ -90,6 +98,21 @@ Float64MultiArray RobotControlManager::GetJointFeedback()
return tempValues;
}
Float64MultiArray RobotControlManager::GetWheelFeedback()
{
Float64MultiArray tempValues;
tempValues.data.resize(motionParams_.wheel_joint_indices_.size());
for (size_t i = 0; i < wheelPositions_.size(); i++)
{
double angle = wheelPositions_[i];
tempValues.data[i] = angle;
}
return tempValues;
}
Float64MultiArray RobotControlManager::GetJointCommands()
{
Float64MultiArray tempValues;
@@ -138,6 +161,27 @@ bool isAllTrueManual(const std::vector<bool>& vec) {
return true; // 所有元素都是 true
}
void RobotControlManager::UpdateWheelStates(const MotorPos::SharedPtr msg)
{
motorPos_ = *msg;
wheelPositions_.resize(motionParams_.wheel_joint_indices_.size());
if (motorPos_.motor_angle.size() != motionParams_.wheel_joint_indices_.size())
{
std::cout << "wheel states size is not equal to wheel numbles" << std::endl;
return;
}
for (size_t i = 0; i < motorPos_.motor_angle.size(); i++)
{
wheelPositions_[i] = motorPos_.motor_angle[i];
wheelVelocities_[i] = 0.0;
wheelEfforts_[i] = 0.0;
}
wheelController_->UpdateJointStates(wheelPositions_, wheelVelocities_, wheelEfforts_);
}
void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg)
{
jointStates_ = *msg;
@@ -145,37 +189,19 @@ 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 < 3; i++)
if (jointStates_.position.size() != motionParams_.total_joints_)
{
std::string joint_name = jointStates_.name[i];
size_t index = 1e5;
size_t underscore_pos = joint_name.find_last_of('_');
if (underscore_pos != std::string::npos && underscore_pos < joint_name.size() - 1)
{
std::string num_str = joint_name.substr(underscore_pos + 1);
try
{
index = std::stoi(num_str);
}
catch (const std::invalid_argument& e)
{
std::cerr << "关节名称 " << joint_name << " 格式错误：下划线后非数字" << std::endl;
}
}
if (index != 1e5 && index < motionParams_.total_joints_)
{
jointPositions_[index] = jointStates_.position[i] * motionParams_.pulse_to_degree_[i];
jointVelocities_[index] = jointStates_.velocity[i] * motionParams_.pulse_to_degree_[i];
jointEfforts_[index] = 0.0;
}
// else do not update.
std::cout << "Joint states size is not equal to total joints" << std::endl;
return;
}
for (size_t i = 0; i < jointStates_.position.size(); i++)
{
jointPositions_[i] = jointStates_.position[i] * motionParams_.pulse_to_degree_[i];
jointVelocities_[i] = jointStates_.velocity[i] * motionParams_.pulse_to_degree_[i];
jointEfforts_[i] = jointStates_.effort[i];
}
// TODO: This block can be deleted
for (size_t i = 0; i < motionParams_.total_joints_; i++)
{
@@ -208,17 +234,43 @@ void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::
}
// Update the waist controller
size_t waistStartIndex = motionParams_.waist_joint_indices_[0];
size_t waistJointSize = motionParams_.waist_joint_indices_.size();
std::vector<double> waistPositions(jointPositions_.begin() + motionParams_.waist_joint_indices_[0], jointPositions_.begin() + motionParams_.waist_joint_indices_[0] + waistJointSize);
std::vector<double> waistVelocities(jointVelocities_.begin() + motionParams_.waist_joint_indices_[0], jointVelocities_.begin() + motionParams_.waist_joint_indices_[0] + waistJointSize);
std::vector<double> waistEfforts(jointEfforts_.begin() + motionParams_.waist_joint_indices_[0], jointEfforts_.begin() + motionParams_.waist_joint_indices_[0] + waistJointSize);
std::vector<double> waistPositions(
jointPositions_.begin() + waistStartIndex,
jointPositions_.begin() + waistStartIndex + waistJointSize
);
std::vector<double> waistVelocities(
jointVelocities_.begin() + waistStartIndex,
jointVelocities_.begin() + waistStartIndex + waistJointSize
);
std::vector<double> waistEfforts(
jointEfforts_.begin() + waistStartIndex,
jointEfforts_.begin() + waistStartIndex + waistJointSize
);
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();
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);
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> legEfforts(
jointEfforts_.begin() + legStartIndex,
jointEfforts_.begin() + legStartIndex + legJointSize
);
legController_->UpdateJointStates(legPositions, legVelocities, legEfforts);
}
@@ -249,68 +301,24 @@ bool RobotControlManager::RobotInitFinished()
return isJointInitValueSet_;
}
void RobotControlManager::MoveForward()
{
wheelController_ -> setLinearAngularSpeed(motionParams_.max_linear_speed_x_, 0);
wheelController_ -> ExecuteCommand();
}
void RobotControlManager::MoveBackward()
{
wheelController_ -> setLinearAngularSpeed( - motionParams_.max_linear_speed_x_, 0.0);
wheelController_ -> ExecuteCommand();
}
void RobotControlManager::TurnLeft()
{
wheelController_ -> setLinearAngularSpeed(0.0, motionParams_.max_angular_speed_z_);
wheelController_ -> ExecuteCommand();
}
void RobotControlManager::TurnRight()
{
wheelController_ -> setLinearAngularSpeed(0.0, - motionParams_.max_angular_speed_z_);
wheelController_ -> ExecuteCommand();
}
bool executeFinished = true;
bool RobotControlManager::Stop(double dt)
{
double currentSpeed = wheelController_ -> GetLinearSpeed();
bool wheelStopped = abs(currentSpeed) > 0 ? false : true;
if(executeFinished)
{
double tempSpeed = 0.0;
if(currentSpeed < -1.0)
{
tempSpeed = currentSpeed + 1.0;
}
else if(currentSpeed > 5.0)
{
tempSpeed = currentSpeed - 1.0;
}
wheelController_ -> setLinearAngularSpeed(tempSpeed, 0.0);
}
executeFinished = wheelController_ -> ExecuteCommand();
AssignTempValues();
bool waistStopped = waistController_->Stop(tempWaistCmd_, dt);
bool legStopped = legController_->Stop(tempLegCmd_, dt);
bool wheelStopped = wheelController_->Stop(tempWheelCmd_, dt);
UpdateJointCommands();
return legStopped && waistStopped && wheelStopped && executeFinished;
return legStopped && waistStopped && wheelStopped;
}
bool RobotControlManager::MoveLeg(double dt)
{
AssignTempValues();
bool result = legController_ -> MoveUp(dt,tempLegCmd_);
bool result = legController_ -> MoveUp(tempLegCmd_, dt);
UpdateJointCommands();
@@ -321,21 +329,24 @@ bool RobotControlManager::MoveWaist(double dt)
{
AssignTempValues();
bool result = waistController_ -> MoveWaist(dt, tempWaistCmd_);
bool result = waistController_ -> MoveWaist(tempWaistCmd_, dt);
UpdateJointCommands();
return result;
}
bool RobotControlManager::MoveWheel(double dt)
bool RobotControlManager::MoveWheel()
{
(void)dt;
wheelController_ -> setLinearAngularSpeed(motionParams_.max_linear_speed_x_, 0.0);
wheelController_ -> ExecuteCommand();
bool result = wheelController_ -> MoveWheel(tempWaistCmd_);
return true;
for (size_t i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
{
wheelCommands_.data[motionParams_.wheel_joint_indices_[i]] = tempWheelCmd_[i];
}
return result;
}

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

@@ -6,6 +6,9 @@
using namespace robot_control;
using namespace std;
using namespace std_msgs::msg;
namespace fs = std::filesystem;
RobotControlNode::RobotControlNode() : Node("robot_control_node")
@@ -70,8 +73,10 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
RCLCPP_INFO(this->get_logger(), "MoveWheel action server is ready");
motorCmdPub_ = this->create_publisher<MotorCmd>("/motor_cmd", 10);
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));
jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("/ec_joint_states", 10,std::bind(&RobotControlNode::JointStatesCallback, this, std::placeholders::_1));
wheelStatesSub_ = this->create_subscription<MotorPos>("/motor_pos", 10,std::bind(&RobotControlNode::WheelStatesCallback, 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(CYCLE_TIME),std::bind(&RobotControlNode::ControlLoop, this)); // 绑定到新的控制函数);
@@ -150,7 +155,9 @@ void RobotControlNode::move_home_execute(const std::shared_ptr<GoalHandleMoveHom
{
result->success = false;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Goal canceled");
RCLCPP_INFO(this->get_logger(), "Move home canceled");
//TODO: ADD STOP LOGIC
return;
}
@@ -181,8 +188,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_leg_goal(
{
RCLCPP_INFO(this->get_logger(), "Received move leg request: total time");
robotControlManager_.SetMoveLegParameters(goal->move_up_distance);
if (robotControlManager_.IsMoving(MovementPart::LEG))
{
RCLCPP_ERROR(this->get_logger(), "Robot is moving");
@@ -201,6 +206,13 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_leg_goal(
return rclcpp_action::GoalResponse::REJECT;
}
if (!robotControlManager_.SetMoveLegParameters(goal->move_up_distance))
{
RCLCPP_ERROR(this->get_logger(), "Failed to set move leg parameters");
return rclcpp_action::GoalResponse::REJECT;
}
(void)uuid;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
@@ -239,7 +251,9 @@ void RobotControlNode::move_leg_execute(const std::shared_ptr<GoalHandleMoveLeg>
{
result->success = false;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Goal canceled");
RCLCPP_INFO(this->get_logger(), "move leg canceled");
//TODO: ADD STOP LOGIC
return;
}
@@ -332,7 +346,9 @@ void RobotControlNode::move_waist_execute(const std::shared_ptr<GoalHandleMoveWa
{
result->success = false;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Goal canceled");
RCLCPP_INFO(this->get_logger(), "move waist canceled");
//TODO: ADD STOP LOGIC
return;
}
@@ -363,8 +379,6 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_wheel_goal(
{
RCLCPP_INFO(this->get_logger(), "Received move wheel request");
robotControlManager_.CheckMoveWheel(goal->move_distance, goal->move_angle);
if (robotControlManager_.IsMoving(MovementPart::WHEEL))
{
RCLCPP_ERROR(this->get_logger(), "Robot is moving");
@@ -376,6 +390,14 @@ rclcpp_action::GoalResponse RobotControlNode::handle_move_wheel_goal(
RCLCPP_ERROR(this->get_logger(), "Another move wheel goal is executing");
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;
}
@@ -406,6 +428,21 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
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;
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])};
motorCmdPub_->publish(wheel_commands_msg);
}
while (move_wheel_executing_ && rclcpp::ok())
{
@@ -413,14 +450,23 @@ void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWh
{
result->success = false;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Goal canceled");
RCLCPP_INFO(this->get_logger(), "move wheel canceled");
//TODO: ADD STOP LOGIC
return;
}
auto joint_commands = robotControlManager_.GetWheelState();
auto joint_feedback = robotControlManager_.GetWheelFeedback();
feedback->current_pos = joint_commands.data[0];
feedback->current_angle = joint_commands.data[1];
if (joint_feedback.data[0] - wheel_commands.data[0] < 5)
{
move_wheel_executing_ = false;
}
feedback->current_pos = joint_feedback.data[0];
//TODO: get the angle from lidar.
feedback->current_angle = 0.0;
goal_handle->publish_feedback(feedback);
@@ -448,7 +494,6 @@ void RobotControlNode::ControlLoop() {
move_home_executing_ = false;
move_leg_executing_ = false;
move_waist_executing_ = false;
move_wheel_executing_ = false;
isJogMode_ = false;
if (robotControlManager_.Stop(dt_sec))
@@ -481,14 +526,6 @@ void RobotControlNode::ControlLoop() {
}
}
if (move_wheel_executing_)
{
if(robotControlManager_.MoveWheel(dt_sec))
{
move_wheel_executing_ = false;
}
}
if (robotControlManager_.RobotInitFinished())
{
Publish_joint_trajectory();
@@ -647,3 +684,13 @@ void RobotControlNode::JointStatesCallback(const sensor_msgs::msg::JointState::S
robotControlManager_.UpdateJointStates(msg);
}
void RobotControlNode::WheelStatesCallback(const MotorPos::SharedPtr cmd_msg)
{
if (!cmd_msg) { // 检查消息是否有效
std::cout << "get null wheel states!" << std::endl;
return;
}
robotControlManager_.UpdateWheelStates(cmd_msg);
}

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

@@ -13,6 +13,7 @@ WaistControl::WaistControl(
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions
): ControlBase(
total_joints,
@@ -21,6 +22,7 @@ WaistControl::WaistControl(
minLimits,
maxLimits,
home_positions,
zero_positions,
joint_directions)
{
std::cout << "WaistControl Constructor" << std::endl;
@@ -50,7 +52,7 @@ bool WaistControl::MoveToTargetPoint(std::vector<double>& joint_positions, const
return MoveToTargetJoint(joint_positions, joint_target, dt);
}
bool WaistControl::MoveWaist(double dt, std::vector<double>& joint_positions)
bool WaistControl::MoveWaist(std::vector<double>& joint_positions, double dt)
{
if (!is_moving_)
{
@@ -86,7 +88,7 @@ bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double movey
joint_position_desired_ = tempPosition;
std::cout << "Target Joint Position: " << joint_position_desired_[0] << ", " << joint_position_desired_[1] << std::endl;
std::cout << "Waist Target Joint Position: " << joint_position_desired_[0] << ", " << joint_position_desired_[1] << std::endl;
is_target_set_ = true;

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

@@ -1,108 +1,123 @@
#include "wheel_control.hpp"
#include <vector>
namespace robot_control
{
using namespace std::chrono_literals;
WheelControl::WheelControl(int totalJoints, double wheelRadius, double wheelSeparation, double maxLinearSpeed, double maxAngularSpeed, std::vector<int32_t> wheelDirection)
WheelControl::WheelControl(
size_t total_joints,
const std::vector<double>& lengthParameters,
const std::vector<double>& maxSpeed,
const std::vector<double>& maxAcc,
const std::vector<double>& minLimits,
const std::vector<double>& maxLimits,
const std::vector<double>& home_positions,
const std::vector<double>& zero_positions,
const std::vector<int>& joint_directions
): ControlBase(
total_joints,
lengthParameters,
maxSpeed, maxAcc,
minLimits,
maxLimits,
home_positions,
zero_positions,
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()
{
delete trapezoidalTrajectory_;
}
bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle)
{
// 设置移动轮参数
double theta = moveWheelDistance / lengthParameters_[0] * 180.0 / M_PI;
for (size_t i = 0; i < joint_directions_.size(); i++)
{
totalJoints_ = totalJoints; // 总关节数
wheelRadius_ = wheelRadius; // 轮子半径
wheelSeparation_ = wheelSeparation; // 轮子间距
maxLinearSpeed_ = maxLinearSpeed; // 线速度
maxAngularSpeed_ = maxAngularSpeed; // 角速度
wheelDirection_ = wheelDirection; // 轮子方向
// 初始化轮子目标角度
wheelSpeed_.resize(totalJoints_, 0);
previousWheelSpeed_.resize(totalJoints_, 0);
isSendFinished_ = true;
}
WheelControl::~WheelControl()
{
// 关闭节点时复位关节
ResetJoints();
}
// 关节复位
void WheelControl::ResetJoints()
{
wheelSpeed_.resize(totalJoints_, 0);
}
void WheelControl::CalculateWheelSpeeds()
{
// 计算左右轮目标线速度
double leftSpeed = linearSpeed_ + (angularSpeed_ * wheelSeparation_) / 2.0;
double rightSpeed = linearSpeed_ - (angularSpeed_ * wheelSeparation_) / 2.0;
// 计算轮子角速度（rad/s）
int32_t leftOmega = leftSpeed ;
int32_t rightOmega = rightSpeed;
wheelSpeed_[0] = wheelDirection_[0] * leftOmega; // left_wheel1
wheelSpeed_[1] = wheelDirection_[1] * rightOmega; // right_wheel
}
void WheelControl::setLinearAngularSpeed(double linearSpeed, double angularSpeed)
{
// 检查是否超限
if (linearSpeed > maxLinearSpeed_ )
if (joint_directions_[i] == 1)
{
linearSpeed = maxLinearSpeed_;
}
else if (linearSpeed < -maxLinearSpeed_)
{
linearSpeed = -maxLinearSpeed_;
}
if (angularSpeed > maxAngularSpeed_)
{
angularSpeed = maxAngularSpeed_;
}
else if (angularSpeed < -maxAngularSpeed_)
{
angularSpeed = -maxAngularSpeed_;
}
if (linearSpeed != linearSpeed_ || angularSpeed != angularSpeed_)
{
linearSpeed_ = linearSpeed;
angularSpeed_ = angularSpeed;
}
}
bool WheelControl::ExecuteCommand()
{
CalculateWheelSpeeds();
// if (wheelSpeed_ != previousWheelSpeed_)
// {
// isSendFinished_ = rs485_driver_.controlAllMotors(wheelSpeed_);
// if (isSendFinished_)
// {
// previousWheelSpeed_ = wheelSpeed_;
// }
// }
return isSendFinished_;
}
bool WheelControl::CheckMoveWheel(double moveWheelDistance, double moveWheelAngle)
{
if (moveWheelDistance == 0 && moveWheelAngle == 0)
{
return false;
joint_position_desired_[i] = joint_position_[i] + theta;
}
else
{
joint_position_desired_[i] = joint_position_[i] - theta;
}
}
//TODO: 设置移动轮角度
(void) moveWheelAngle;
is_target_set_ = true;
return true;
}
bool WheelControl::MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double dt)
{
// 注意：这里需要运动学逆解将笛卡尔空间目标位置转换为关节空间
// 为简化示例，这里假设已完成逆解，直接使用虚拟关节位置
std::vector<double> joint_target(total_joints_, 0.0);
// 实际应用中应在此处添加逆运动学求解
// joint_target = inverse_kinematics(target_pos);
for (size_t i = 0; i < target_pos.size(); i++)
{
std::cout << "Target Position: " << target_pos[i].transpose() << std::endl;
}
// 使用梯形轨迹规划移动到目标关节位置
return MoveToTargetJoint(joint_positions, joint_target, dt);
}
bool WheelControl::MoveWheel(std::vector<double>& joint_positions)
{
if (!is_moving_)
{
if (!is_target_set_)
{
std::cout << "Target position not set!" << std::endl;
return true;
}
}
joint_positions = joint_position_desired_;
is_target_set_ = false;
return true;
}
bool WheelControl::GoHome(std::vector<double>& joint_positions, double dt)
{
// do nothing
(void) joint_positions;
(void) dt;
return true;
}
}