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

运动学规划解耦合

Browse Source
This commit is contained in:
zj
2025-10-16 09:59:42 +08:00
parent 2f2d8e1490
commit 695c86384f
18 changed files with 556 additions and 656 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
HiveCoreR1/include/arm_define.h
View File

@@ -12,10 +12,14 @@
#define GET_FUNC_LINE_STR() \
(std::string("[") + __func__ + ":" + std::to_string(__LINE__) + "]")
#define LEFT_ARM_IP_ADDRESS "192.168.1.16"
#define LEFT_ARM_IP_ADDRESS "192.168.2.2"
#define RIGHT_ARM_IP_ADDRESS "192.168.2.18"
#define ARM_IP_PORT (8080)
#define TRAJECTORY_COMPLETE 1
#define TRAJECTORY_ONGOING 0
#define TRAJECTORY_ERROR -1
typedef enum {
ARM_COMMAND_TYPE_ANGLE_STEP_ON = 0,
ARM_COMMAND_TYPE_POSE_STEP_ON,

3
HiveCoreR1/src/rm_arm_control/CMakeLists.txt
View File

@@ -36,8 +36,7 @@ set(SOURCES
src/rm_arm_handler.cpp
src/rm_arm_node.cpp
simulator/rm_arm_simulator.cpp
simulator/kinematics.cpp
src/trapezoidal_trajectory.cpp
simulator/trapezoidal_trajectory_kinematics.cpp
src/goal_manager.cpp
driver/arm_driver.cpp
driver/rm_arm_driver.cpp

8
HiveCoreR1/src/rm_arm_control/driver/rm_arm_driver.cpp
View File

@@ -1,10 +1,7 @@
#include "rm_service.h"
#include "arm_driver.hpp"
#include "rm_arm_driver.hpp"
#include <cstring>
RM_Service *rmService_;
RmArmDriver::RmArmDriver()
{
rmService_ = nullptr;
@@ -23,6 +20,7 @@ RmArmDriver::RmArmStatusCallbackFun RmArmDriver::armStatusCallback_ = nullptr;
RmArmDriver::ApiLogFunc RmArmDriver::customApiLog_ = nullptr;
rm_robot_handle *RmArmDriver::leftRobotHandle_;
rm_robot_handle *RmArmDriver::rightRobotHandle_;
RM_Service *RmArmDriver::rmService_ = nullptr;
int RmArmDriver::ArmMove(const float points[USED_ARM_DOF], int armId)
{
@@ -149,14 +147,14 @@ int RmArmDriver::Init()
rmService_->rm_realtime_arm_state_call_back(CallbackRmRealtimeArmJointState);
int robot_port = ARM_IP_PORT;
rm_realtime_push_config_t config = {20, true, 8089, 0, "192.168.1.100"};
rm_realtime_push_config_t config = {20, true, 8089, 0, "192.168.2.3"};
result = InitHandle(&leftRobotHandle_, LEFT_ARM_IP_ADDRESS, robot_port, &config);
if (result != 0) {
rmService_->rm_destroy();
delete rmService_;
return UNKNOWN_ERR;
}
config = {20, true, 8089, 0, "192.168.2.100"};
config = {20, true, 8089, 0, "192.168.2.19"};
result = InitHandle(&rightRobotHandle_, RIGHT_ARM_IP_ADDRESS, robot_port, &config);
if (result != 0) {
rmService_->rm_destroy();

19
HiveCoreR1/src/rm_arm_control/include/kinematics.hpp
View File

@@ -1,19 +0,0 @@
#ifndef KINEMATICS_HPP
#define KINEMATICS_HPP
#include "rm_service.h"
class ArmKinematics
{
public:
ArmKinematics(RM_Service *rmService);
ArmKinematics();
~ArmKinematics();
int ForwardKinematics(float *jointAngles, float *endEffectorPose);
int InverseKinematics(const float *endEffectorPose, float *jointAngles);
private:
RM_Service *rmService_;
};
#endif // KINEMATICS_HPP

2
HiveCoreR1/src/rm_arm_control/include/rm_arm_driver.hpp
View File

@@ -28,6 +28,8 @@ public:
armStatusCallback_ = callback;
}
static RM_Service *rmService_;
private:
int initFlag_;
static ApiLogFunc customApiLog_;

14
HiveCoreR1/src/rm_arm_control/include/rm_arm_handler.hpp
View File

@@ -11,7 +11,6 @@
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "trapezoidal_trajectory.hpp"
#include "goal_manager.hpp"
#include "arm_define.h"
#include "arm_driver.hpp"
@@ -32,19 +31,14 @@ private:
sem_t *sem_;
GoalManager *goalManager_;
ArmSimulator *armSimulator_;
ArmDriver *driver_;
public:
TrapezoidalTrajectory *trapezoidalTrajectory_;
int armType_; // 0:left 1:right
void SetSimulator(ArmSimulator *simulator) {
armSimulator_ = simulator;
}
RmArmHandler(ArmDriver *driver, int armType, sem_t *sem);
RmArmHandler(ArmDriver *driver, int armType, sem_t *sem, KinematicsManager *trapezoidalTrajectory);
~RmArmHandler();
KinematicsManager *trapezoidalTrajectory_;
int armType_; // 0:left 1:right
void ArmDealCallBackInfo(ArmStatusCallbackData *data);
float nowJoints_[USED_ARM_DOF];

2
HiveCoreR1/src/rm_arm_control/include/rm_arm_node.hpp
View File

@@ -18,7 +18,7 @@ using GoalHandleArm = rclcpp_action::ServerGoalHandle<ArmAction>;
class RmArmNode: public rclcpp::Node
{
public:
RmArmNode(ArmDriver *armDriver);
RmArmNode(ArmDriver *armDriver, Kinematics *kinematics);
~RmArmNode();
static RmArmHandler *leftArmHandler_;
static RmArmHandler *rightArmHandler_;

18
HiveCoreR1/src/rm_arm_control/include/rm_arm_simulator.hpp
View File

@@ -6,9 +6,7 @@
#include <urdf_model/pose.h>
#include <std_msgs/msg/string.hpp>
#include "arm_define.h"
#include "trapezoidal_trajectory.hpp"
#include "kinematics.hpp"
#include "trajectory.hpp"
#include "gjk_interface.hpp"
#define ARM_COLLISION_CHECK_SIMPLE_CNT 2
@@ -55,13 +53,15 @@ enum {
RIGHT_ARM_COLLISION,
};
using namespace ArmKinematicsNs;
class ArmSimulator
{
public:
ArmSimulator(std_msgs::msg::String::SharedPtr urdf_string_ptr, float *left_now_joints, float *right_now_joints,
TrapezoidalTrajectory *leftTrapezoidalTrajectory, TrapezoidalTrajectory *rightTrapezoidalTrajectory);
KinematicsManager *leftTrajectory, KinematicsManager *rightTrajectory);
ArmSimulator(std::string urdf_string, float *left_now_joints, float *right_now_joints,
TrapezoidalTrajectory *leftTrapezoidalTrajectory, TrapezoidalTrajectory *rightTrapezoidalTrajectory);
KinematicsManager *leftTrajectory, KinematicsManager *rightTrajectory);
~ArmSimulator();
int CheckCollision();
@@ -77,11 +77,8 @@ public:
int GetFrameChange(float *pose, int arm, int64_t frameTime, JointInfo *jointInfo);
int ForwardKinematics(float *jointAngles, float *endEffectorPose, int armId);
int InverseKinematics(const float *endEffectorPose, float *jointAngles, int armId);
TrapezoidalTrajectory *leftTrapezoidalTrajectory_;
TrapezoidalTrajectory *rightTrapezoidalTrajectory_;
KinematicsManager *leftTrajectory_;
KinematicsManager *rightTrajectory_;
std::vector<CollisionStructure> collision_structures_;
std::map<int, JointInfo> jointMap_;
@@ -93,7 +90,6 @@ public:
float rightArmJoints_[ARM_COLLISION_CHECK_SIMPLE_CNT][USED_ARM_DOF];
private:
std::unordered_map<int, Polyhedron> addedPolyhedrons_;
ArmKinematics *armKinematics_;
};
#endif // RM_ARM_SIMULATOR_HPP

121
HiveCoreR1/src/rm_arm_control/include/trajectory.hpp Normal file
View File

@@ -0,0 +1,121 @@
#ifndef TRAJECTORY_HPP
#define TRAJECTORY_HPP
#include <vector>
#include "arm_define.h"
#include "trapezoidal_trajectory_kinematics.hpp"
namespace ArmKinematicsNs {
template <typename T, typename K>
class TrajectoryManager
{
public:
TrajectoryManager(K *armKinematics) : armKinematics_(armKinematics) {
nowTrajectoryIndex_ = -1;
trajectoryFullFlag_ = 0;
}
~TrajectoryManager() {}
int computeTrajectory(const float *start, const float *end, int type) {
T* newTrajectory = addTrajectory();
if (type == GOAL_TYPE_MOVING) {
armKinematics_->computeMovingTrajectory(newTrajectory, start, end);
} else if (type == GOAL_TYPE_POSE_STEPPING) {
armKinematics_->computePoseSteppingTrajectory(newTrajectory, start, end);
} else {
armKinematics_->computeSteppingTrajectory(newTrajectory, start, end);
}
return 0;
}
int updateTrajectory(int64_t startTime, float *aimAngles) {
if (nowTrajectoryIndex_ < 0) {
return OK;
}
if (trajectoryStartTime_[nowTrajectoryIndex_] == 0) {
trajectoryStartTime_[nowTrajectoryIndex_] = startTime;
}
return armKinematics_->updateTrajectory(&trajectoryHistory_[nowTrajectoryIndex_], aimAngles);
}
int getAngleFromTime(float *aimAngles, int64_t timeStamp) {
if (nowTrajectoryIndex_ < 0) {
return OK;
}
int timeStep = 0;
int aimIndex = -1;
int startIndex = nowTrajectoryIndex_ + 1;
int endIndex = trajectoryFullFlag_ == 1 ? startIndex : 0;
do {
startIndex = (startIndex + MAX_TRAJECTORY_HISTORY - 1) % MAX_TRAJECTORY_HISTORY;
if (trajectoryStartTime_[startIndex] < timeStamp) {
aimIndex = startIndex;
timeStep = static_cast<int>((timeStamp - trajectoryStartTime_[startIndex]) / ARM_FOLLOWING_PERIOD);
break;
}
} while (startIndex != endIndex);
if (aimIndex == -1) {
return UNKNOWN_ERR;
}
armKinematics_->getAngleForCheckingInner(&trajectoryHistory_[aimIndex], timeStep, aimAngles);
return 0;
}
void getAngleForChecking(float *aimAngles) {
if (nowTrajectoryIndex_ < 0) {
return;
}
T* nowTrajectory = &trajectoryHistory_[nowTrajectoryIndex_];
armKinematics_->getAngleForCheckingOffset(nowTrajectory, ARM_FOLLOWING_CHECKING_STEP * 2, aimAngles);
}
void getAngleForStarting(float *aimAngles) {
if (nowTrajectoryIndex_ < 0) {
return;
}
T* nowTrajectory = &trajectoryHistory_[nowTrajectoryIndex_];
armKinematics_->getAngleForCheckingOffset(nowTrajectory, ARM_FOLLOWING_CHECKING_STEP, aimAngles);
}
void stopTrajectory() {
armKinematics_->stopTrajectory(&trajectoryHistory_[nowTrajectoryIndex_]);
}
int continuePoseSteppingTrajectory(float *nowAngles, const float *direct) {
return armKinematics_->continuePoseSteppingTrajectory(&trajectoryHistory_[nowTrajectoryIndex_], nowAngles, direct);
}
T* getNowTrajectory() {
if (nowTrajectoryIndex_ < 0) {
return nullptr;
}
return &trajectoryHistory_[nowTrajectoryIndex_];
}
K *armKinematics_;
private:
T trajectoryHistory_[MAX_TRAJECTORY_HISTORY];
int64_t trajectoryStartTime_[MAX_TRAJECTORY_HISTORY];
int nowTrajectoryIndex_;
int trajectoryFullFlag_;
T* addTrajectory() {
nowTrajectoryIndex_++;
if (nowTrajectoryIndex_ >= MAX_TRAJECTORY_HISTORY) {
nowTrajectoryIndex_ = 0;
trajectoryFullFlag_ = 1;
}
trajectoryStartTime_[nowTrajectoryIndex_] = 0;
return &trajectoryHistory_[nowTrajectoryIndex_];
}
};
using KinematicsManager = TrajectoryManager<TrapezoidalTrajectory, TrapezoidalTrajectoryKinematics>;
using Trajectory = TrapezoidalTrajectory;
using Kinematics = TrapezoidalTrajectoryKinematics;
} // namespace ArmKinematicsNs
#endif // TRAJECTORY_HPP

68
HiveCoreR1/src/rm_arm_control/include/trapezoidal_trajectory.hpp
View File

@@ -1,68 +0,0 @@
#ifndef TRAPEZOIDAL_TRAJECTORY_HPP
#define TRAPEZOIDAL_TRAJECTORY_HPP
#include <vector>
#include "arm_define.h"
#define TRAJECTORY_COMPELETE 1
#define TRAJECTORY_ONGOING 0
#define TRAJECTORY_ERROR -1
#define STEPPING_UNIFORM_TIME_STAMP -1
struct Trajectory {
float startAngles[USED_ARM_DOF];
float endAngles[USED_ARM_DOF];
float aim_velocity[USED_ARM_DOF];
float now_acceleration[USED_ARM_DOF];
int64_t startTime;
int accelerate_time_step;
int uniform_time_step;
int goalType;
int current_time_step;
int64_t stopTime;
};
class TrapezoidalTrajectory
{
public:
TrapezoidalTrajectory();
~TrapezoidalTrajectory();
int computeTrajectory(const float *start, const float *end, int type);
int updateTrajectory(float *aimAngles);
int checkStartTrajectory(int64_t startTime);
int getAngleFromTime(float *aimAngles, int64_t timeStamp);
void getAngleForChecking(float *aimAngles);
void getAngleForStarting(float *aimAngles);
void stopTrajectory();
int updatePoseSteppingTrajectory(float *aimAngles);
int continuePoseSteppingTrajectory(float *nowAngles, float *aimAngles);
Trajectory* getNowTrajectory() {
if (nowTrajectoryIndex_ < 0) {
return nullptr;
}
return &trajectoryHistory_[nowTrajectoryIndex_];
}
private:
float max_velocity_;
float max_acceleration_;
int max_accelerate_time_;
float max_accelerate_angle_;
float min_angle_for_pose_stepping_max_speed_;
Trajectory trajectoryHistory_[MAX_TRAJECTORY_HISTORY];
int nowTrajectoryIndex_;
int trajectoryFullFlag_;
Trajectory* addTrajectory();
void computeMovingTrajectory(Trajectory *newTrajectory, const float *start, const float *end);
void computeSteppingTrajectory(Trajectory *newTrajectory, const float *start, const float *end);
void computePoseSteppingTrajectory(Trajectory *newTrajectory, const float *start, const float *end);
};
#endif // TRAPEZOIDAL_TRAJECTORY_HPP

51
HiveCoreR1/src/rm_arm_control/include/trapezoidal_trajectory_kinematics.hpp Normal file
View File

@@ -0,0 +1,51 @@
#ifndef TRAPEZOIDAL_TRAJECTORY_KINEMATICS_HPP
#define TRAPEZOIDAL_TRAJECTORY_KINEMATICS_HPP
#include "arm_define.h"
#include "rm_service.h"
#define STEPPING_UNIFORM_TIME_STAMP -1
struct TrapezoidalTrajectory {
float startAngles[USED_ARM_DOF];
float endAngles[USED_ARM_DOF];
float aim_velocity[USED_ARM_DOF];
float now_acceleration[USED_ARM_DOF];
int64_t startTime;
int accelerate_time_step;
int uniform_time_step;
int goalType;
int current_time_step;
int64_t stopTime;
};
class TrapezoidalTrajectoryKinematics
{
public:
TrapezoidalTrajectoryKinematics(RM_Service *rmService);
TrapezoidalTrajectoryKinematics();
~TrapezoidalTrajectoryKinematics();
void getAngleForCheckingInner(TrapezoidalTrajectory *trajectory, int timeStep, float *aimAngles);
void getAngleForCheckingOffset(TrapezoidalTrajectory *trajectory, int timeStepOffset, float *aimAngles);
void computeMovingTrajectory(TrapezoidalTrajectory *trajectory, const float *start, const float *end);
void computeSteppingTrajectory(TrapezoidalTrajectory *trajectory, const float *start, const float *speed);
void computePoseSteppingTrajectory(TrapezoidalTrajectory *trajectory, const float *start, const float *end);
int updateTrajectory(TrapezoidalTrajectory *trajectory, float *aimAngles);
void stopTrajectory(TrapezoidalTrajectory *trajectory);
int continuePoseSteppingTrajectory(TrapezoidalTrajectory *trajectory, float *nowAngles, const float *direct);
int ForwardKinematics(float *jointAngles, float *endEffectorPose);
int InverseKinematics(const float *endEffectorPose, float *jointAngles);
private:
RM_Service *rmService_;
float max_velocity_;
float max_acceleration_;
int max_accelerate_time_;
float max_accelerate_angle_;
float min_angle_for_pose_stepping_max_speed_;
};
#endif // TRAPEZOIDAL_TRAJECTORY_KINEMATICS_HPP

80
HiveCoreR1/src/rm_arm_control/simulator/kinematics.cpp
View File

@@ -1,80 +0,0 @@
#include "kinematics.hpp"
#include "arm_define.h"
#include <stdexcept>
#include <cstring>
ArmKinematics::ArmKinematics(RM_Service *rmService) :
rmService_(rmService)
{
rm_robot_arm_model_e Mode = RM_MODEL_RM_65_E;
rm_force_type_e Type = RM_MODEL_RM_B_E;
rm_algo_init_sys_data(Mode, Type);
}
ArmKinematics::ArmKinematics()
{
rmService_ = new RM_Service();
int result = rmService_->rm_init(RM_TRIPLE_MODE_E);
if (result != 0) {
throw std::runtime_error("Failed to init rm.");
return;
}
rm_robot_arm_model_e Mode = RM_MODEL_RM_65_E;
rm_force_type_e Type = RM_MODEL_RM_B_E;
rm_algo_init_sys_data(Mode, Type);
}
ArmKinematics::~ArmKinematics()
{
delete rmService_;
}
int ArmKinematics::ForwardKinematics(float *jointAngles, float *endEffectorPose)
{
if (!rmService_)
{
return UNKNOWN_ERR; // Error: RM_Service not initialized
}
// Call the RM_Service's ForwardKinematics method
rm_pose_t pose = rmService_->rm_algo_forward_kinematics(NULL, jointAngles);
endEffectorPose[POSE_POSITION_X] = pose.position.x;
endEffectorPose[POSE_POSITION_Y] = pose.position.y;
endEffectorPose[POSE_POSITION_Z] = pose.position.z;
endEffectorPose[POSE_QUATERNION_X] = pose.quaternion.x;
endEffectorPose[POSE_QUATERNION_Y] = pose.quaternion.y;
endEffectorPose[POSE_QUATERNION_Z] = pose.quaternion.z;
endEffectorPose[POSE_QUATERNION_W] = pose.quaternion.w;
return 0;
}
int ArmKinematics::InverseKinematics(const float *endEffectorPose, float *jointAngles)
{
if (!rmService_)
{
return UNKNOWN_ERR; // Error: RM_Service not initialized
}
float nowJoints_[ARM_DOF] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
for (int i = 0; i < USED_ARM_DOF; i++) {
nowJoints_[i] = jointAngles[i];
}
rm_inverse_kinematics_params_t inverse_params;
memcpy(inverse_params.q_in, nowJoints_, sizeof(nowJoints_));
inverse_params.q_pose.position.x = endEffectorPose[POSE_POSITION_X];
inverse_params.q_pose.position.y = endEffectorPose[POSE_POSITION_Y];
inverse_params.q_pose.position.z = endEffectorPose[POSE_POSITION_Z];
inverse_params.q_pose.quaternion.x = endEffectorPose[POSE_QUATERNION_X];
inverse_params.q_pose.quaternion.y = endEffectorPose[POSE_QUATERNION_Y];
inverse_params.q_pose.quaternion.z = endEffectorPose[POSE_QUATERNION_Z];
inverse_params.q_pose.quaternion.w = endEffectorPose[POSE_QUATERNION_W];
inverse_params.flag = 0;
int result = rmService_->rm_algo_inverse_kinematics(NULL, inverse_params, nowJoints_);
if (result != 0){
return ARM_AIM_CANNOT_REACH; // Inverse kinematics failed
}
memcpy(jointAngles, nowJoints_, sizeof(float) * USED_ARM_DOF);
return 0;
}

69
HiveCoreR1/src/rm_arm_control/simulator/rm_arm_simulator.cpp
View File

@@ -244,9 +244,9 @@ static int UpdateJointAngles(ArmSimulator *simulator)
{
if (simulator == NULL) return -1;
int simple = simulator->simpleUpdateIndex_;
simulator->leftTrapezoidalTrajectory_->getAngleForChecking
simulator->leftTrajectory_->getAngleForChecking
(simulator->leftArmJoints_[simple]);
simulator->rightTrapezoidalTrajectory_->getAngleForChecking
simulator->rightTrajectory_->getAngleForChecking
(simulator->rightArmJoints_[simple]);
if (simulator->CheckJointsAngleLimit(simulator->leftArmJoints_[simple]) != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleDirectMove joints limit error.");
@@ -691,11 +691,10 @@ static int InitArmPolyhedrons(ArmSimulator *simulator, urdf::Model &urdf_model)
}
ArmSimulator::ArmSimulator(std::string urdf_string, float *left_now_joints, float *right_now_joints,
TrapezoidalTrajectory *leftTrapezoidalTrajectory, TrapezoidalTrajectory *rightTrapezoidalTrajectory)
: leftTrapezoidalTrajectory_(leftTrapezoidalTrajectory), rightTrapezoidalTrajectory_(rightTrapezoidalTrajectory)
KinematicsManager *leftTrajectory, KinematicsManager *rightTrajectory)
: leftTrajectory_(leftTrajectory), rightTrajectory_(rightTrajectory)
{
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ArmSimulator: 初始化机械臂碰撞检测模块...");
armKinematics_ = new ArmKinematics();
urdf::Model urdf_model;
simpleUpdateIndex_ = 0;
if (!urdf_model.initString(urdf_string)) {
@@ -715,11 +714,10 @@ ArmSimulator::ArmSimulator(std::string urdf_string, float *left_now_joints, floa
}
ArmSimulator::ArmSimulator(std_msgs::msg::String::SharedPtr urdf_string_ptr, float *left_now_joints, float *right_now_joints,
TrapezoidalTrajectory *leftTrapezoidalTrajectory, TrapezoidalTrajectory *rightTrapezoidalTrajectory)
: leftTrapezoidalTrajectory_(leftTrapezoidalTrajectory), rightTrapezoidalTrajectory_(rightTrapezoidalTrajectory)
KinematicsManager *leftTrajectory, KinematicsManager *rightTrajectory)
: leftTrajectory_(leftTrajectory), rightTrajectory_(rightTrajectory)
{
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ArmSimulator: 初始化机械臂碰撞检测模块...");
armKinematics_ = new ArmKinematics();
std::string urdf_string = urdf_string_ptr->data;
for (int i = 0; i < ARM_COLLISION_CHECK_SIMPLE_CNT; i++) {
for (int j = 0; j < USED_ARM_DOF; j++) {
@@ -743,7 +741,6 @@ ArmSimulator::ArmSimulator(std_msgs::msg::String::SharedPtr urdf_string_ptr, flo
ArmSimulator::~ArmSimulator()
{
delete armKinematics_;
}
static int CheckCollisionInner(std::vector<OBB> *addedOBBs, std::vector<OBB> *addedOBBsNext_)
@@ -799,15 +796,15 @@ int ArmSimulator::UpdateCollisionForNewGoal(int arm)
float *jointList40;
if (arm == LEFT_ARM) {
leftTrapezoidalTrajectory_->getAngleForStarting(leftArmJoints_[simple20]);
leftTrapezoidalTrajectory_->getAngleForChecking(leftArmJoints_[simple40]);
leftTrajectory_->getAngleForStarting(leftArmJoints_[simple20]);
leftTrajectory_->getAngleForChecking(leftArmJoints_[simple40]);
startLinkIndex = LEFT;
endLinkIndex = RIGHT;
jointList20 = leftArmJoints_[simple20];
jointList40 = leftArmJoints_[simple40];
} else if (arm == RIGHT_ARM) {
rightTrapezoidalTrajectory_->getAngleForStarting(rightArmJoints_[simple20]);
rightTrapezoidalTrajectory_->getAngleForChecking(rightArmJoints_[simple40]);
rightTrajectory_->getAngleForStarting(rightArmJoints_[simple20]);
rightTrajectory_->getAngleForChecking(rightArmJoints_[simple40]);
startLinkIndex = RIGHT;
endLinkIndex = BASE;
jointList20 = rightArmJoints_[simple20];
@@ -875,7 +872,7 @@ int ArmSimulator::CheckCollision()
int nowIndex = (simpleUpdateIndex_ + ARM_COLLISION_CHECK_SIMPLE_CNT - 1) % ARM_COLLISION_CHECK_SIMPLE_CNT;
std::vector<OBB> *addedOBBsNext_ = &(addedOBBs_[simpleUpdateIndex_]);
std::vector<OBB> *addedOBBs = &(addedOBBs_[nowIndex]);
//result = CheckCollisionInner(addedOBBs, addedOBBsNext_);
result = CheckCollisionInner(addedOBBs, addedOBBsNext_);
if (result != SAFE) {
if (result & LEFT_ARM_COLLISION) {
@@ -949,10 +946,10 @@ int ArmSimulator::GetFrameChange(float *pose, int arm, int64_t frameTime, JointI
float jointList[USED_ARM_DOF] = {0};
if (arm == LEFT_ARM) {
parent = LEFT;
result = leftTrapezoidalTrajectory_->getAngleFromTime(jointList, frameTime);
result = leftTrajectory_->getAngleFromTime(jointList, frameTime);
} else {
parent = RIGHT;
result = rightTrapezoidalTrajectory_->getAngleFromTime(jointList, frameTime);
result = rightTrajectory_->getAngleFromTime(jointList, frameTime);
}
if (result != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ChangePoseToArmFrame: get joint fail");
@@ -1066,43 +1063,3 @@ int ArmSimulator::CheckJointsAngleLimit(float *joints)
}
return 0;
}
int ArmSimulator::ForwardKinematics(float *jointAngles, float *endEffectorPose, int armId)
{
int result = armKinematics_->ForwardKinematics(jointAngles, endEffectorPose);
if (result != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ForwardKinematics error, joints: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
jointAngles[0], jointAngles[1], jointAngles[2], jointAngles[3], jointAngles[4], jointAngles[5]);
return ARM_AIM_CANNOT_REACH;
}
result = ChangeArmBasePoseToOriFrame(endEffectorPose, armId);
if (result != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "frame change frame error.");
return UNKNOWN_ERR;
}
return 0;
}
int ArmSimulator::InverseKinematics(const float *endEffectorPose, float *jointAngles, int armId)
{
float points[POSE_DIMENSION] = {endEffectorPose[0], endEffectorPose[1], endEffectorPose[2],
endEffectorPose[3], endEffectorPose[4], endEffectorPose[5], endEffectorPose[6]};
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
points[0], points[1], points[2], points[3], points[4], points[5], points[6]);
int result = ChangeOriFrameToArmBasePose(points, armId);
if (result != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "frame change error.");
return UNKNOWN_ERR;
}
result = armKinematics_->InverseKinematics(points, jointAngles);
if (result != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "InverseKinematics error, pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
points[0], points[1], points[2], points[3], points[4], points[5], points[6]);
return ARM_AIM_CANNOT_REACH;
}
if (CheckJointsAngleLimit(jointAngles) != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleDirectMove joints limit error.");
return ARM_AIM_CANNOT_REACH;
}
return 0;
}

319
HiveCoreR1/src/rm_arm_control/simulator/trapezoidal_trajectory_kinematics.cpp Normal file
View File

@@ -0,0 +1,319 @@
#include <stdexcept>
#include "arm_define.h"
#include "trapezoidal_trajectory_kinematics.hpp"
#include <cstring>
#include <cmath>
TrapezoidalTrajectoryKinematics::TrapezoidalTrajectoryKinematics(RM_Service *rmService)
: max_velocity_(1.0), max_acceleration_(0.25), rmService_(rmService)
{
max_accelerate_time_ = 4;
max_accelerate_angle_ = max_velocity_ * max_accelerate_time_;
min_angle_for_pose_stepping_max_speed_ = ARM_FOLLOWING_CHECKING_STEP * max_velocity_ * 2;
rm_robot_arm_model_e Mode = RM_MODEL_RM_65_E;
rm_force_type_e Type = RM_MODEL_RM_B_E;
rm_algo_init_sys_data(Mode, Type);
}
TrapezoidalTrajectoryKinematics::TrapezoidalTrajectoryKinematics()
: max_velocity_(1.0), max_acceleration_(0.25), rmService_(nullptr)
{
max_accelerate_time_ = 4;
max_accelerate_angle_ = max_velocity_ * max_accelerate_time_;
min_angle_for_pose_stepping_max_speed_ = ARM_FOLLOWING_CHECKING_STEP * max_velocity_ * 2;
rmService_ = new RM_Service();
int result = rmService_->rm_init(RM_TRIPLE_MODE_E);
if (result != 0) {
throw std::runtime_error("Failed to init rm.");
return;
}
rm_robot_arm_model_e Mode = RM_MODEL_RM_65_E;
rm_force_type_e Type = RM_MODEL_RM_B_E;
rm_algo_init_sys_data(Mode, Type);
}
TrapezoidalTrajectoryKinematics::~TrapezoidalTrajectoryKinematics() {
delete rmService_;
}
void TrapezoidalTrajectoryKinematics::getAngleForCheckingInner(TrapezoidalTrajectory *traj, int timeStep, float *aimAngles)
{
if (timeStep >= traj->accelerate_time_step * 2 + traj->uniform_time_step
&& traj->uniform_time_step != STEPPING_UNIFORM_TIME_STAMP) { // 分支预判优化
for (int i = 0; i < USED_ARM_DOF; ++i) {
aimAngles[i] = traj->endAngles[i];
}
return;
}
for (int i = 0; i < USED_ARM_DOF; ++i) {
if (timeStep < traj->accelerate_time_step) {
aimAngles[i] = traj->startAngles[i] + 0.5 * traj->now_acceleration[i] * (timeStep + 1) * timeStep;
} else if (traj->uniform_time_step == STEPPING_UNIFORM_TIME_STAMP){
aimAngles[i] = traj->startAngles[i] + 0.5 * traj->now_acceleration[i] * (traj->accelerate_time_step + 1) * (traj->accelerate_time_step);
aimAngles[i] += traj->aim_velocity[i] * (timeStep - traj->accelerate_time_step);
}else if (timeStep < traj->accelerate_time_step + traj->uniform_time_step) {
aimAngles[i] = traj->endAngles[i] - 0.5 * traj->now_acceleration[i] * (traj->accelerate_time_step + 1) * (traj->accelerate_time_step);
aimAngles[i] -= traj->aim_velocity[i] * (traj->uniform_time_step - (timeStep - traj->accelerate_time_step));
} else if (timeStep <= 2 * traj->accelerate_time_step + traj->uniform_time_step) {
int decelerate_time_step = timeStep - traj->accelerate_time_step - traj->uniform_time_step;
aimAngles[i] = traj->endAngles[i] - 0.5 * traj->now_acceleration[i] * (decelerate_time_step + 1) * decelerate_time_step;
}
}
}
void TrapezoidalTrajectoryKinematics::getAngleForCheckingOffset(TrapezoidalTrajectory *traj, int timeStepOffset, float *aimAngles)
{
int timeStep = traj->current_time_step + timeStepOffset;
getAngleForCheckingInner(traj, timeStep, aimAngles);
}
int TrapezoidalTrajectoryKinematics::updateTrajectory(TrapezoidalTrajectory *trajectory, float *aimAngles)
{
trajectory->current_time_step++;
if ((trajectory->goalType == GOAL_TYPE_POSE_STEPPING && trajectory->current_time_step >= trajectory->accelerate_time_step + trajectory->uniform_time_step) ||
(trajectory->current_time_step >= trajectory->accelerate_time_step * 2 + trajectory->uniform_time_step
&& trajectory->uniform_time_step != STEPPING_UNIFORM_TIME_STAMP)) {
for (int i = 0; i < USED_ARM_DOF; ++i) {
aimAngles[i] = trajectory->endAngles[i];
}
return TRAJECTORY_COMPLETE;
}
getAngleForCheckingInner(trajectory, trajectory->current_time_step, aimAngles);
return TRAJECTORY_ONGOING;
}
void TrapezoidalTrajectoryKinematics::computeMovingTrajectory(TrapezoidalTrajectory *newTrajectory, const float *start, const float *end)
{
float max_angle = 0.0;
float angle_diff[USED_ARM_DOF];
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->startAngles[i] = start[i];
newTrajectory->endAngles[i] = end[i];
angle_diff[i] = end[i] - start[i];
float abs_diff = fabs(angle_diff[i]);
if (abs_diff > max_angle) {
max_angle = abs_diff;
}
}
if (max_angle < 0.01) {
newTrajectory->accelerate_time_step = 0;
newTrajectory->uniform_time_step = 0;
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->aim_velocity[i] = 0;
newTrajectory->now_acceleration[i] = 0;
newTrajectory->endAngles[i] = start[i];
}
return;
}
float aim_velocity = max_velocity_;
if (max_angle < max_accelerate_angle_) {
newTrajectory->accelerate_time_step = static_cast<int>(sqrt(max_angle / max_acceleration_)) + 1;
aim_velocity = max_angle / newTrajectory->accelerate_time_step;
newTrajectory->uniform_time_step = 0;
} else {
newTrajectory->accelerate_time_step = max_accelerate_time_;
newTrajectory->uniform_time_step = static_cast<int>((max_angle - max_accelerate_angle_) / max_velocity_);
}
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->aim_velocity[i] = aim_velocity * angle_diff[i] / max_angle;
newTrajectory->now_acceleration[i] = newTrajectory->aim_velocity[i] / newTrajectory->accelerate_time_step;
}
}
void TrapezoidalTrajectoryKinematics::computeSteppingTrajectory(TrapezoidalTrajectory *newTrajectory, const float *start, const float *speed)
{
float maxSpeed = 0.0;
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->startAngles[i] = start[i];
if (fabs(speed[i]) > max_velocity_) {
newTrajectory->aim_velocity[i] = (speed[i] > 0 ? 1 : -1) * max_velocity_;
} else {
newTrajectory->aim_velocity[i] = speed[i];
}
if (fabs(newTrajectory->aim_velocity[i]) > maxSpeed) {
maxSpeed = fabs(newTrajectory->aim_velocity[i]);
}
}
newTrajectory->uniform_time_step = -1;
newTrajectory->accelerate_time_step = static_cast<int>(maxSpeed / max_acceleration_) + 1;
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->now_acceleration[i] = newTrajectory->aim_velocity[i] / newTrajectory->accelerate_time_step;
newTrajectory->endAngles[i] = 0;
}
}
void TrapezoidalTrajectoryKinematics::computePoseSteppingTrajectory(TrapezoidalTrajectory *newTrajectory, const float *start, const float *end)
{
float max_angle = 0.0;
float angle_diff[USED_ARM_DOF];
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->startAngles[i] = start[i];
newTrajectory->endAngles[i] = end[i];
angle_diff[i] = end[i] - start[i];
float abs_diff = fabs(angle_diff[i]);
if (abs_diff > max_angle) {
max_angle = abs_diff;
}
}
if (max_angle < 0.01) {
newTrajectory->accelerate_time_step = 0;
newTrajectory->uniform_time_step = 0;
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->aim_velocity[i] = 0;
newTrajectory->now_acceleration[i] = 0;
newTrajectory->endAngles[i] = start[i];
}
return;
}
float aim_velocity = max_velocity_;
newTrajectory->accelerate_time_step = max_accelerate_time_;
if (max_angle < min_angle_for_pose_stepping_max_speed_) {
aim_velocity = max_angle / (ARM_FOLLOWING_CHECKING_STEP * 2);
newTrajectory->uniform_time_step = ARM_FOLLOWING_CHECKING_STEP * 2 - max_accelerate_time_;
} else {
newTrajectory->uniform_time_step = static_cast<int>((max_angle - (max_accelerate_angle_ - 1) * 0.5) / max_velocity_);
}
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->aim_velocity[i] = aim_velocity * angle_diff[i] / max_angle;
newTrajectory->now_acceleration[i] = newTrajectory->aim_velocity[i] / max_accelerate_time_;
}
}
void TrapezoidalTrajectoryKinematics::stopTrajectory(TrapezoidalTrajectory *traj)
{
int formerPoseSteppingUniformTimeStep = traj->uniform_time_step;
if (traj->current_time_step < traj->accelerate_time_step) {
traj->accelerate_time_step = traj->current_time_step;
traj->uniform_time_step = 0;
} else if (traj->uniform_time_step == STEPPING_UNIFORM_TIME_STAMP ||
traj->current_time_step < traj->accelerate_time_step + traj->uniform_time_step) {
traj->uniform_time_step = traj->current_time_step - traj->accelerate_time_step;
}
traj->stopTime = traj->startTime + ARM_FOLLOWING_PERIOD * (traj->accelerate_time_step * 2 + traj->uniform_time_step);
for (int i = 0; i < USED_ARM_DOF; ++i) {
if (traj->goalType == GOAL_TYPE_POSE_STEPPING) {
traj->now_acceleration[i] = traj->aim_velocity[i] / traj->accelerate_time_step;
traj->endAngles[i] = traj->endAngles[i]
- traj->aim_velocity[i] * (formerPoseSteppingUniformTimeStep - traj->uniform_time_step);
traj->endAngles[i] += 0.5 * traj->now_acceleration[i] * traj->accelerate_time_step * traj->accelerate_time_step;
} else if (traj->current_time_step <= traj->accelerate_time_step + traj->uniform_time_step) {
traj->endAngles[i] = traj->startAngles[i] + traj->aim_velocity[i] * (traj->uniform_time_step + traj->accelerate_time_step);
}
}
if (traj->goalType == GOAL_TYPE_POSE_STEPPING) {
traj->goalType = GOAL_TYPE_MOVING;
}
}
int TrapezoidalTrajectoryKinematics::continuePoseSteppingTrajectory(TrapezoidalTrajectory *newTrajectory, float *nowAngles, const float *direct)
{
float poseTemp[POSE_DIMENSION] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
int result = ForwardKinematics(nowAngles, poseTemp);
if (result != 0) {
return UNKNOWN_ERR;
}
for (int i = 0; i < POSE_DIMENSION; i++) {
poseTemp[i] = direct[i] + poseTemp[i];
if (fabs(poseTemp[i]) < 0.0001) {
poseTemp[i] = 0.0f;
}
}
float newJoints[ARM_DOF] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
for (int i = 0; i < USED_ARM_DOF; i++) {
newJoints[i] = nowAngles[i];
}
rm_inverse_kinematics_params_t inverse_params;
memcpy(inverse_params.q_in, newJoints, sizeof(newJoints));
inverse_params.q_pose.position.x = poseTemp[POSE_POSITION_X];
inverse_params.q_pose.position.y = poseTemp[POSE_POSITION_Y];
inverse_params.q_pose.position.z = poseTemp[POSE_POSITION_Z];
inverse_params.q_pose.quaternion.x = poseTemp[POSE_QUATERNION_X];
inverse_params.q_pose.quaternion.y = poseTemp[POSE_QUATERNION_Y];
inverse_params.q_pose.quaternion.z = poseTemp[POSE_QUATERNION_Z];
inverse_params.q_pose.quaternion.w = poseTemp[POSE_QUATERNION_W];
inverse_params.flag = 0;
result = rmService_->rm_algo_inverse_kinematics(NULL, inverse_params, newJoints);
if (result != 0){
return ARM_AIM_CANNOT_REACH; // Inverse kinematics failed
}
float angle_diff[USED_ARM_DOF];
float max_angle = 0.0;
float max_velocity = 0.0;
int fastestIndex = 0;
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->endAngles[i] = newJoints[i];
angle_diff[i] = newTrajectory->endAngles[i] - nowAngles[i];
float abs_diff = fabs(angle_diff[i]);
if (abs_diff > max_angle) {
max_angle = abs_diff;
fastestIndex = i;
}
}
max_velocity = fabs(newTrajectory->aim_velocity[fastestIndex]);
if (max_velocity * (ARM_FOLLOWING_CHECKING_STEP * 2 + (max_accelerate_time_ - 1) * 0.5) >= max_angle) {
max_velocity = max_angle / (ARM_FOLLOWING_CHECKING_STEP * 2 + (max_accelerate_time_ - 1) * 0.5);
}
for (int i = 0; i < USED_ARM_DOF; ++i) {
if (max_angle < 0.0001) {
newTrajectory->aim_velocity[i] = 0;
} else {
newTrajectory->aim_velocity[i] = max_velocity * angle_diff[i] / max_angle;
}
}
newTrajectory->uniform_time_step += max_angle / max_velocity;
return 0;
}
int TrapezoidalTrajectoryKinematics::ForwardKinematics(float *jointAngles, float *endEffectorPose)
{
if (!rmService_)
{
return UNKNOWN_ERR; // Error: RM_Service not initialized
}
// Call the RM_Service's ForwardKinematics method
rm_pose_t pose = rmService_->rm_algo_forward_kinematics(NULL, jointAngles);
endEffectorPose[POSE_POSITION_X] = pose.position.x;
endEffectorPose[POSE_POSITION_Y] = pose.position.y;
endEffectorPose[POSE_POSITION_Z] = pose.position.z;
endEffectorPose[POSE_QUATERNION_X] = pose.quaternion.x;
endEffectorPose[POSE_QUATERNION_Y] = pose.quaternion.y;
endEffectorPose[POSE_QUATERNION_Z] = pose.quaternion.z;
endEffectorPose[POSE_QUATERNION_W] = pose.quaternion.w;
return 0;
}
int TrapezoidalTrajectoryKinematics::InverseKinematics(const float *endEffectorPose, float *jointAngles)
{
if (!rmService_)
{
return UNKNOWN_ERR; // Error: RM_Service not initialized
}
float nowJoints_[ARM_DOF] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
for (int i = 0; i < USED_ARM_DOF; i++) {
nowJoints_[i] = jointAngles[i];
}
rm_inverse_kinematics_params_t inverse_params;
memcpy(inverse_params.q_in, nowJoints_, sizeof(nowJoints_));
inverse_params.q_pose.position.x = endEffectorPose[POSE_POSITION_X];
inverse_params.q_pose.position.y = endEffectorPose[POSE_POSITION_Y];
inverse_params.q_pose.position.z = endEffectorPose[POSE_POSITION_Z];
inverse_params.q_pose.quaternion.x = endEffectorPose[POSE_QUATERNION_X];
inverse_params.q_pose.quaternion.y = endEffectorPose[POSE_QUATERNION_Y];
inverse_params.q_pose.quaternion.z = endEffectorPose[POSE_QUATERNION_Z];
inverse_params.q_pose.quaternion.w = endEffectorPose[POSE_QUATERNION_W];
inverse_params.flag = 0;
int result = rmService_->rm_algo_inverse_kinematics(NULL, inverse_params, nowJoints_);
if (result != 0){
return ARM_AIM_CANNOT_REACH; // Inverse kinematics failed
}
memcpy(jointAngles, nowJoints_, sizeof(float) * USED_ARM_DOF);
return 0;
}

13
HiveCoreR1/src/rm_arm_control/src/main.cpp
View File

@@ -1,6 +1,7 @@
#include "arm_driver.hpp"
#include "rm_arm_driver.hpp"
#include "rm_arm_node.hpp"
#include "trajectory.hpp"
#include <thread>
std::thread publicThread_;
@@ -25,7 +26,7 @@ void custom_api_log(const char* message, va_list args)
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), " %s", buffer);
}
int InitSimulatorDriver(ArmDriver **armDriver)
int InitSimulatorDriver(ArmDriver **armDriver, Kinematics **armKinematics)
{
*armDriver = new ArmDriver();
(*armDriver)->RegArmStatusCallback(RmArmNode::CallbackRealtimeArmJointState);
@@ -34,12 +35,13 @@ int InitSimulatorDriver(ArmDriver **armDriver)
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to initialize arm driver. %d", initResult);
return UNKNOWN_ERR;
}
*armKinematics = new Kinematics();
std::thread(PublicThread, *armDriver).detach();
return 0;
}
int InitRmArmDriver(ArmDriver **armDriver)
int InitRmArmDriver(ArmDriver **armDriver, Kinematics **armKinematics)
{
auto rmArmDriver = new RmArmDriver();
rmArmDriver->SetCustomApiLog(custom_api_log);
@@ -58,6 +60,8 @@ int InitRmArmDriver(ArmDriver **armDriver)
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to initialize arm driver. %d", initResult);
return UNKNOWN_ERR;
}
*armKinematics = new Kinematics(rmArmDriver->rmService_);
return 0;
}
@@ -67,13 +71,14 @@ int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
ArmDriver *armDriver = NULL;
int initResult = InitSimulatorDriver(&armDriver);
Kinematics *armKinematics = NULL;
int initResult = InitSimulatorDriver(&armDriver, &armKinematics);
if (initResult != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to initialize rm arm driver. %d", initResult);
return UNKNOWN_ERR;
}
auto rm_arm_control = std::make_shared<RmArmNode>(armDriver);
auto rm_arm_control = std::make_shared<RmArmNode>(armDriver, armKinematics);
rclcpp::spin(rm_arm_control);

34
HiveCoreR1/src/rm_arm_control/src/rm_arm_handler.cpp
View File

@@ -2,15 +2,14 @@
const float maxJointSpeedList[USED_ARM_DOF] = {4.0, 4.0, 4.0, 4.0, 4.0, 4.0};
RmArmHandler::RmArmHandler(ArmDriver *driver, int armType, sem_t *sem):
armType_(armType), driver_(driver)
RmArmHandler::RmArmHandler(ArmDriver *driver, int armType, sem_t *sem, KinematicsManager *trapezoidalTrajectory):
armType_(armType), driver_(driver), trapezoidalTrajectory_(trapezoidalTrajectory)
{
goalManager_ = new GoalManager();
armStatus = ARM_STATUS_ERROR;
sem_ = sem;
memset(nowJoints_, 0, sizeof(nowJoints_));
memset(nextJoints_, 0, sizeof(nextJoints_));
trapezoidalTrajectory_ = new TrapezoidalTrajectory();
armStatus = ARM_STATUS_READY;
}
@@ -106,7 +105,7 @@ int RmArmHandler::AddNextPointFromPoseMoving(const float pose[POSE_DIMENSION], c
for (int i = 0; i < USED_ARM_DOF; i++) {
q_out[i] = nextJoints_[i];
}
int result = armSimulator_->InverseKinematics(pose, q_out, armType_);
int result = trapezoidalTrajectory_->armKinematics_->InverseKinematics(pose, q_out);
if (result == 0) {
std::unique_lock lock(rw_mutex_);
return goalManager_->AddGoal(q_out, index, GOAL_TYPE_MOVING);
@@ -117,7 +116,7 @@ int RmArmHandler::AddNextPointFromPoseMoving(const float pose[POSE_DIMENSION], c
int RmArmHandler::GetNextPointFromPoseStepping(const float pose[POSE_DIMENSION], float *joints)
{
float poseTemp[POSE_DIMENSION] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
int result = armSimulator_->ForwardKinematics(nextJoints_, poseTemp, armType_);
int result = trapezoidalTrajectory_->armKinematics_->ForwardKinematics(nextJoints_, poseTemp);
if (result != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "ForwardKinematics error.");
return UNKNOWN_ERR;
@@ -128,15 +127,19 @@ int RmArmHandler::GetNextPointFromPoseStepping(const float pose[POSE_DIMENSION],
poseTemp[i] = 0.0f;
}
}
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Current pose:%.6f %.6f %.6f %.6f %.6f %.6f %.6f",
poseTemp[0], poseTemp[1], poseTemp[2], poseTemp[3], poseTemp[4], poseTemp[5], poseTemp[6]);
float q_out[GOAL_DATA_LENGTH] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
for (int i = 0; i < USED_ARM_DOF; i++) {
q_out[i] = nextJoints_[i];
}
result = armSimulator_->InverseKinematics(poseTemp, q_out, armType_);
result = trapezoidalTrajectory_->armKinematics_->InverseKinematics(poseTemp, q_out);
if (result != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "InverseKinematics error.");
return ARM_AIM_CANNOT_REACH;
}
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Next joints:%.6f %.6f %.6f %.6f %.6f %.6f",
q_out[0], q_out[1], q_out[2], q_out[3], q_out[4], q_out[5]);
for (int i = 0; i < USED_ARM_DOF; i++) {
joints[i] = q_out[i];
}
@@ -179,33 +182,26 @@ void RmArmHandler::ArmRun(int64_t timeStamp)
std::shared_lock lock(rw_mutex_);
type = goalManager_->GetGoalType();
}
if (type == GOAL_TYPE_POSE_STEPPING) {
result = trapezoidalTrajectory_->updatePoseSteppingTrajectory(nextJoints_);
} else {
trapezoidalTrajectory_->checkStartTrajectory(timeStamp);
result = trapezoidalTrajectory_->updateTrajectory(nextJoints_);
}
result = trapezoidalTrajectory_->updateTrajectory(timeStamp, nextJoints_);
if (driver_->ArmMove(nextJoints_, armType_) != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Error moving to joint space! ");
return;
}
if (result == TRAJECTORY_COMPELETE) {
if (result == TRAJECTORY_COMPLETE) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Trajectory complete.");
if (type == GOAL_TYPE_POSE_STEPPING) {
float joints[USED_ARM_DOF] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
{
std::shared_lock lock(rw_mutex_);
const float* pose = goalManager_->GetGoal();
result = GetNextPointFromPoseStepping(pose, joints);
result = trapezoidalTrajectory_->continuePoseSteppingTrajectory(nextJoints_, pose);
}
if (result != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "GetNextPointFromPoseStepping failed.");
ArmStop(ARM_AIM_CANNOT_REACH);
return;
}
trapezoidalTrajectory_->continuePoseSteppingTrajectory(nextJoints_, joints);
} else {
int runOffFlag;
{
@@ -250,12 +246,6 @@ int RmArmHandler::StartNewTrajectory()
} else {
trapezoidalTrajectory_->computeTrajectory(start, goal, goalType);
}
Trajectory* nowTrajectory = trapezoidalTrajectory_->getNowTrajectory();
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "New trajectory: accelerate time step %d, uniform time step %d. aim_velocity: %.6f, %.6f, %.6f, %.6f, %.6f, %.6f "
"type: %d",
nowTrajectory->accelerate_time_step, nowTrajectory->uniform_time_step, nowTrajectory->aim_velocity[0],
nowTrajectory->aim_velocity[1], nowTrajectory->aim_velocity[2], nowTrajectory->aim_velocity[3],
nowTrajectory->aim_velocity[4], nowTrajectory->aim_velocity[5], goalType);
return OK;
}

17
HiveCoreR1/src/rm_arm_control/src/rm_arm_node.cpp
View File

@@ -98,19 +98,22 @@ static RmArmHandler *GetArmHandler(RmArmNode *node, int body_id)
}
// 机器人控制器构造函数
RmArmNode::RmArmNode(ArmDriver *armDriver) : Node("rm_arm_control"), armDriver_(armDriver)
RmArmNode::RmArmNode(ArmDriver *armDriver, Kinematics *kinematics) : Node("rm_arm_control"), armDriver_(armDriver)
{
localNodePtr = this;
sem_init(&sem_, 0, 0);
leftArmHandler_ = new RmArmHandler(armDriver_, LEFT_ARM, &sem_);
KinematicsManager *leftTrajectory = new KinematicsManager(kinematics);
KinematicsManager *rightTrajectory = new KinematicsManager(kinematics);
leftArmHandler_ = new RmArmHandler(armDriver_, LEFT_ARM, &sem_, leftTrajectory);
if (leftArmHandler_ != NULL && leftArmHandler_->armStatus == ARM_STATUS_READY) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Left arm handler initialization success.");
} else {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Left arm handler initialization failed.");
}
rightArmHandler_ = new RmArmHandler(armDriver_, RIGHT_ARM, &sem_);
rightArmHandler_ = new RmArmHandler(armDriver_, RIGHT_ARM, &sem_, rightTrajectory);
if (rightArmHandler_ != NULL && rightArmHandler_->armStatus == ARM_STATUS_READY) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Right arm handler initialization success.");
} else {
@@ -138,8 +141,6 @@ RmArmNode::RmArmNode(ArmDriver *armDriver) : Node("rm_arm_control"), armDriver_(
armSimulator_ = new ArmSimulator(urdf_string, leftArmHandler_->nowJoints_, rightArmHandler_->nowJoints_,
leftArmHandler_->trapezoidalTrajectory_, rightArmHandler_->trapezoidalTrajectory_);
robotDescriptionSub_ = NULL;
leftArmHandler_->SetSimulator(armSimulator_);
rightArmHandler_->SetSimulator(armSimulator_);
} else {
armSimulator_ = NULL;
robotDescriptionSub_ = this->create_subscription<std_msgs::msg::String>
@@ -170,8 +171,6 @@ void RmArmNode::CallbackGetRobotDescription(const std_msgs::msg::String::SharedP
armStates.header.frame_id = "base_link";
armSimulator_ = new ArmSimulator(msg, leftArmHandler_->nowJoints_, rightArmHandler_->nowJoints_,
leftArmHandler_->trapezoidalTrajectory_, rightArmHandler_->trapezoidalTrajectory_);
leftArmHandler_->SetSimulator(armSimulator_);
rightArmHandler_->SetSimulator(armSimulator_);
}
}
@@ -362,8 +361,8 @@ void RmArmNode::ArmActionCheck()
g_totalColTime = 0;
}
// if (coll != SAFE) {
if (coll == ARM_AIM_CANNOT_REACH) {
if (coll != SAFE) {
// if (coll == ARM_AIM_CANNOT_REACH) {
leftArmHandler_->ArmStop(ARM_COLLISION);
rightArmHandler_->ArmStop(ARM_COLLISION);
ArmActionFeedback(coll);

368
HiveCoreR1/src/rm_arm_control/src/trapezoidal_trajectory.cpp
View File

@@ -1,368 +0,0 @@
#include <cmath>
#include "trapezoidal_trajectory.hpp"
TrapezoidalTrajectory::TrapezoidalTrajectory()
: max_velocity_(1.0), max_acceleration_(0.25)
{
max_accelerate_time_ = 4;
max_accelerate_angle_ = max_velocity_ * max_accelerate_time_;
nowTrajectoryIndex_ = -1;
trajectoryFullFlag_ = 0;
trajectoryHistory_[0].current_time_step = 0;
min_angle_for_pose_stepping_max_speed_ = ARM_FOLLOWING_CHECKING_STEP * max_velocity_ * 2;
}
TrapezoidalTrajectory::~TrapezoidalTrajectory() {}
Trajectory* TrapezoidalTrajectory::addTrajectory()
{
nowTrajectoryIndex_++;
if (nowTrajectoryIndex_ >= MAX_TRAJECTORY_HISTORY) {
nowTrajectoryIndex_ = 0;
trajectoryFullFlag_ = 1;
}
return &trajectoryHistory_[nowTrajectoryIndex_];
}
void TrapezoidalTrajectory::computeMovingTrajectory(Trajectory *newTrajectory, const float *start, const float *end)
{
float max_angle = 0.0;
float angle_diff[USED_ARM_DOF];
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->startAngles[i] = start[i];
newTrajectory->endAngles[i] = end[i];
angle_diff[i] = end[i] - start[i];
float abs_diff = fabs(angle_diff[i]);
if (abs_diff > max_angle) {
max_angle = abs_diff;
}
}
if (max_angle < 0.01) {
newTrajectory->accelerate_time_step = 0;
newTrajectory->uniform_time_step = 0;
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->aim_velocity[i] = 0;
newTrajectory->now_acceleration[i] = 0;
newTrajectory->endAngles[i] = start[i];
}
return;
}
float aim_velocity = max_velocity_;
if (max_angle < max_accelerate_angle_) {
newTrajectory->accelerate_time_step = static_cast<int>(sqrt(max_angle / max_acceleration_)) + 1;
aim_velocity = max_angle / newTrajectory->accelerate_time_step;
newTrajectory->uniform_time_step = 0;
} else {
newTrajectory->accelerate_time_step = max_accelerate_time_;
newTrajectory->uniform_time_step = static_cast<int>((max_angle - max_accelerate_angle_) / max_velocity_);
}
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->aim_velocity[i] = aim_velocity * angle_diff[i] / max_angle;
newTrajectory->now_acceleration[i] = newTrajectory->aim_velocity[i] / newTrajectory->accelerate_time_step;
}
}
void TrapezoidalTrajectory::computeSteppingTrajectory(Trajectory *newTrajectory, const float *start, const float *speed)
{
float maxSpeed = 0.0;
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->startAngles[i] = start[i];
if (fabs(speed[i]) > max_velocity_) {
newTrajectory->aim_velocity[i] = (speed[i] > 0 ? 1 : -1) * max_velocity_;
} else {
newTrajectory->aim_velocity[i] = speed[i];
}
if (fabs(newTrajectory->aim_velocity[i]) > maxSpeed) {
maxSpeed = fabs(newTrajectory->aim_velocity[i]);
}
}
newTrajectory->uniform_time_step = -1;
newTrajectory->accelerate_time_step = static_cast<int>(maxSpeed / max_acceleration_) + 1;
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->now_acceleration[i] = newTrajectory->aim_velocity[i] / newTrajectory->accelerate_time_step;
newTrajectory->endAngles[i] = 0;
}
}
void TrapezoidalTrajectory::computePoseSteppingTrajectory(Trajectory *newTrajectory, const float *start, const float *end)
{
float max_angle = 0.0;
float angle_diff[USED_ARM_DOF];
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->startAngles[i] = start[i];
newTrajectory->endAngles[i] = end[i];
angle_diff[i] = end[i] - start[i];
float abs_diff = fabs(angle_diff[i]);
if (abs_diff > max_angle) {
max_angle = abs_diff;
}
}
if (max_angle < 0.01) {
newTrajectory->accelerate_time_step = 0;
newTrajectory->uniform_time_step = 0;
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->aim_velocity[i] = 0;
newTrajectory->now_acceleration[i] = 0;
newTrajectory->endAngles[i] = start[i];
}
return;
}
float aim_velocity = max_velocity_;
newTrajectory->accelerate_time_step = max_accelerate_time_;
if (max_angle < min_angle_for_pose_stepping_max_speed_ - 0.5 * max_accelerate_angle_) {
aim_velocity = max_angle / (ARM_FOLLOWING_CHECKING_STEP * 2 - (max_accelerate_time_ - 1) * 0.5);
newTrajectory->uniform_time_step = ARM_FOLLOWING_CHECKING_STEP * 2 - max_accelerate_time_;
} else {
newTrajectory->uniform_time_step = static_cast<int>((max_angle - (max_accelerate_angle_ - 1) * 0.5) / max_velocity_);
}
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->aim_velocity[i] = aim_velocity * angle_diff[i] / max_angle;
newTrajectory->now_acceleration[i] = newTrajectory->aim_velocity[i] / max_accelerate_time_;
}
}
int TrapezoidalTrajectory::computeTrajectory(const float *start, const float *end, int type)
{
Trajectory* newTrajectory = addTrajectory();
newTrajectory->goalType = type;
newTrajectory->startTime = 0;
newTrajectory->stopTime = 0;
newTrajectory->current_time_step = 0;
if (type == GOAL_TYPE_MOVING) {
computeMovingTrajectory(newTrajectory, start, end);
} else if (type == GOAL_TYPE_POSE_STEPPING) {
computePoseSteppingTrajectory(newTrajectory, start, end);
} else {
computeSteppingTrajectory(newTrajectory, start, end);
}
return 0;
}
int TrapezoidalTrajectory::checkStartTrajectory(int64_t startTime)
{
if (nowTrajectoryIndex_ < 0) {
return -1;
}
if (trajectoryHistory_[nowTrajectoryIndex_].startTime != 0) {
return 0;
}
trajectoryHistory_[nowTrajectoryIndex_].startTime = startTime;
trajectoryHistory_[nowTrajectoryIndex_].stopTime = startTime
+ (2 * trajectoryHistory_[nowTrajectoryIndex_].accelerate_time_step
+ trajectoryHistory_[nowTrajectoryIndex_].uniform_time_step) * ARM_FOLLOWING_PERIOD;
return 0;
}
int TrapezoidalTrajectory::updateTrajectory(float *aimAngles)
{
// Update the aimAngles based on the computed trajectory
Trajectory* newTrajectory = &trajectoryHistory_[nowTrajectoryIndex_];
newTrajectory->current_time_step++;
if (newTrajectory->current_time_step < newTrajectory->accelerate_time_step) {
for (int i = 0; i < USED_ARM_DOF; ++i) {
float v = newTrajectory->now_acceleration[i] * newTrajectory->current_time_step;
aimAngles[i] += v;
}
} else if (newTrajectory->uniform_time_step == STEPPING_UNIFORM_TIME_STAMP ||
newTrajectory->current_time_step < newTrajectory->accelerate_time_step + newTrajectory->uniform_time_step) {
for (int i = 0; i < USED_ARM_DOF; ++i) {
aimAngles[i] += newTrajectory->aim_velocity[i];
}
} else if (newTrajectory->current_time_step < 2 * newTrajectory->accelerate_time_step + newTrajectory->uniform_time_step) {
for (int i = 0; i < USED_ARM_DOF; ++i) {
int decelerate_time_step = newTrajectory->current_time_step - newTrajectory->accelerate_time_step
- newTrajectory->uniform_time_step;
float v = newTrajectory->aim_velocity[i] - newTrajectory->now_acceleration[i] * decelerate_time_step;
if (v < 0) v = 0;
aimAngles[i] += v;
}
} else {
for (int i = 0; i < USED_ARM_DOF; ++i) {
aimAngles[i] = newTrajectory->endAngles[i];
}
return TRAJECTORY_COMPELETE;
}
return TRAJECTORY_ONGOING;
}
int TrapezoidalTrajectory::getAngleFromTime(float *aimAngles, int64_t timeStamp)
{
if (nowTrajectoryIndex_ == -1) {
return OK;
}
int startIndex = nowTrajectoryIndex_ + 1;
int endIndex = trajectoryFullFlag_ == 1 ? startIndex : 0;
do {
startIndex = (startIndex + MAX_TRAJECTORY_HISTORY - 1) % MAX_TRAJECTORY_HISTORY;
Trajectory* traj = &trajectoryHistory_[startIndex];
if (traj -> stopTime == 0) {
if (startIndex == nowTrajectoryIndex_) {
return ARM_NOW_FORCE_MOVING;
}
return UNKNOWN_ERR;
}
if (timeStamp >= traj->stopTime) {
for (int i = 0; i < USED_ARM_DOF; ++i) {
aimAngles[i] = traj->endAngles[i];
}
return 0;
}
if (timeStamp >= traj->startTime) {
int64_t time_diff = timeStamp - traj->startTime;
int time_step = static_cast<int>(time_diff / ARM_FOLLOWING_PERIOD);
for (int i = 0; i < USED_ARM_DOF; ++i) {
float angle = traj->startAngles[i];
if (time_step < traj->accelerate_time_step) {
angle += 0.5 * traj->now_acceleration[i] * time_step * time_step;
} else if (traj->uniform_time_step == STEPPING_UNIFORM_TIME_STAMP
|| time_step < traj->accelerate_time_step + traj->uniform_time_step) {
angle += 0.5 * traj->now_acceleration[i] * traj->accelerate_time_step * (traj->accelerate_time_step);
angle += traj->aim_velocity[i] * (time_step - traj->accelerate_time_step);
} else if (time_step <= 2 * traj->accelerate_time_step + traj->uniform_time_step) {
int decelerate_time_step = time_step - traj->accelerate_time_step - traj->uniform_time_step;
angle += 0.5 * traj->now_acceleration[i] * (traj->accelerate_time_step) * (traj->accelerate_time_step);
angle += traj->aim_velocity[i] * (traj->uniform_time_step);
angle += traj->aim_velocity[i] * decelerate_time_step
- 0.5 * traj->now_acceleration[i] * decelerate_time_step * decelerate_time_step;
} else {
angle = traj->endAngles[i];
}
aimAngles[i] = angle;
}
return 0;
}
} while (startIndex != endIndex);
return -1;
}
static void getAngleForCheckingInner(Trajectory* traj, int timeStepOffset, float *aimAngles)
{
int time_step = traj->current_time_step + timeStepOffset;
if (time_step >= traj->accelerate_time_step * 2 + traj->uniform_time_step
&& traj->uniform_time_step != STEPPING_UNIFORM_TIME_STAMP) {
for (int i = 0; i < USED_ARM_DOF; ++i) {
aimAngles[i] = traj->endAngles[i];
}
return;
}
for (int i = 0; i < USED_ARM_DOF; ++i) {
float angle = traj->startAngles[i];
if (time_step < traj->accelerate_time_step) {
angle += 0.5 * traj->now_acceleration[i] * time_step * time_step;
} else if (time_step < traj->accelerate_time_step + traj->uniform_time_step ||
traj->uniform_time_step == STEPPING_UNIFORM_TIME_STAMP) {
angle += 0.5 * traj->now_acceleration[i] * (traj->accelerate_time_step) * (traj->accelerate_time_step);
angle += traj->aim_velocity[i] * (time_step - traj->accelerate_time_step);
} else if (time_step <= 2 * traj->accelerate_time_step + traj->uniform_time_step) {
int decelerate_time_step = time_step - traj->accelerate_time_step - traj->uniform_time_step;
angle += 0.5 * traj->now_acceleration[i] * (traj->accelerate_time_step) * (traj->accelerate_time_step);
angle += traj->aim_velocity[i] * (traj->uniform_time_step);
angle += traj->aim_velocity[i] * decelerate_time_step
- 0.5 * traj->now_acceleration[i] * decelerate_time_step * decelerate_time_step;
}
aimAngles[i] = angle;
}
}
void TrapezoidalTrajectory::getAngleForChecking(float *aimAngles)
{
if (nowTrajectoryIndex_ < 0) {
return;
}
Trajectory* traj = &trajectoryHistory_[nowTrajectoryIndex_];
getAngleForCheckingInner(traj, ARM_FOLLOWING_CHECKING_STEP * 2, aimAngles);
}
void TrapezoidalTrajectory::getAngleForStarting(float *aimAngles)
{
if (nowTrajectoryIndex_ < 0) {
return;
}
Trajectory* traj = &trajectoryHistory_[nowTrajectoryIndex_];
getAngleForCheckingInner(traj, ARM_FOLLOWING_CHECKING_STEP, aimAngles);
}
void TrapezoidalTrajectory::stopTrajectory()
{
if (nowTrajectoryIndex_ < 0) {
return;
}
Trajectory* traj = &trajectoryHistory_[nowTrajectoryIndex_];
int formerPoseSteppingUniformTimeStep = traj->uniform_time_step;
if (traj->current_time_step < traj->accelerate_time_step) {
traj->accelerate_time_step = traj->current_time_step;
traj->uniform_time_step = 0;
} else if (traj->uniform_time_step == STEPPING_UNIFORM_TIME_STAMP ||
traj->current_time_step < traj->accelerate_time_step + traj->uniform_time_step) {
traj->uniform_time_step = traj->current_time_step - traj->accelerate_time_step;
}
traj->stopTime = traj->startTime + ARM_FOLLOWING_PERIOD * (traj->accelerate_time_step * 2 + traj->uniform_time_step);
for (int i = 0; i < USED_ARM_DOF; ++i) {
if (traj->goalType == GOAL_TYPE_POSE_STEPPING) {
traj->now_acceleration[i] = traj->aim_velocity[i] / traj->accelerate_time_step;
traj->endAngles[i] = traj->endAngles[i]
- traj->aim_velocity[i] * (formerPoseSteppingUniformTimeStep - traj->uniform_time_step);
traj->endAngles[i] += 0.5 * traj->now_acceleration[i] * traj->accelerate_time_step * traj->accelerate_time_step;
} else {
traj->endAngles[i] = traj->startAngles[i] + traj->aim_velocity[i] * (traj->uniform_time_step + traj->accelerate_time_step);
}
}
if (traj->goalType == GOAL_TYPE_POSE_STEPPING) {
traj->goalType = GOAL_TYPE_MOVING;
}
}
int TrapezoidalTrajectory::continuePoseSteppingTrajectory(float *nowAngles, float *aimAngles)
{
// Update the aimAngles based on the computed trajectory
Trajectory* newTrajectory = &trajectoryHistory_[nowTrajectoryIndex_];
float angle_diff[USED_ARM_DOF];
float max_angle = 0.0;
float max_velocity = 0.0;
int fastestIndex = 0;
for (int i = 0; i < USED_ARM_DOF; ++i) {
newTrajectory->endAngles[i] = aimAngles[i];
angle_diff[i] = newTrajectory->endAngles[i] - nowAngles[i];
float abs_diff = fabs(angle_diff[i]);
if (abs_diff > max_angle) {
max_angle = abs_diff;
fastestIndex = i;
}
}
max_velocity = fabs(newTrajectory->aim_velocity[fastestIndex]);
if (max_velocity * ARM_FOLLOWING_CHECKING_STEP * 2 >= max_angle) {
max_velocity = max_angle / (ARM_FOLLOWING_CHECKING_STEP * 2);
}
for (int i = 0; i < USED_ARM_DOF; ++i) {
if (max_angle < 0.0001) {
newTrajectory->aim_velocity[i] = 0;
} else {
newTrajectory->aim_velocity[i] = max_velocity * angle_diff[i] / max_angle;
}
}
newTrajectory->uniform_time_step += max_angle / max_velocity;
return 0;
}
int TrapezoidalTrajectory::updatePoseSteppingTrajectory(float *aimAngles)
{
Trajectory* newTrajectory = &trajectoryHistory_[nowTrajectoryIndex_];
newTrajectory->current_time_step++;
if (newTrajectory->current_time_step < newTrajectory->accelerate_time_step) {
for (int i = 0; i < USED_ARM_DOF; ++i) {
float v = newTrajectory->now_acceleration[i] * newTrajectory->current_time_step;
aimAngles[i] += v;
}
} else if (newTrajectory->current_time_step < newTrajectory->accelerate_time_step + newTrajectory->uniform_time_step) {
for (int i = 0; i < USED_ARM_DOF; ++i) {
aimAngles[i] += newTrajectory->aim_velocity[i];
}
} else {
for (int i = 0; i < USED_ARM_DOF; ++i) {
aimAngles[i] += newTrajectory->aim_velocity[i];
}
return TRAJECTORY_COMPELETE;
}
return TRAJECTORY_ONGOING;
}