【AI解析】一个简单的 C 库，用于控制工业机器人源码注释（二）
·
PathPlanner.h PathPlanner.c
PathPlanner.h 文件
#ifndefPATH_PLANNER_H // 如果宏 PATH_PLANNER_H 未被定义
#definePATH_PLANNER_H// 则定义宏 PATH_PLANNER_H，这是标准的头文件保护宏，防止头文件被重复包含
#include"RobControl.h"// 包含机器人控制相关的头文件，可能定义了数据结构如 Robot_Parameter_JointLimits_Type
#include"trig.h"// 包含三角函数相关的头文件
#include"Misc.h"// 包含杂项功能和数据类型的头文件，如 Path_Type
#include"constants.h"// 包含项目自定义的常量定义头文件
#include<math.h>// 包含标准数学库头文件
#defineEDGE_END0// 定义一个宏 EDGE_END，值为0，表示路径段的末端过渡
#defineEDGE_START1// 定义一个宏 EDGE_START，值为1，表示路径段的始端过渡
/* Declaration of common functions used for path planning purposes: path lengths, geometric calculations, vector calculus... */// 注释块：声明用于路径规划的常用函数：路径长度、几何计算、向量演算等
doubleLineLength(double P1[6],double P2[6],int Size);// 声明函数：计算从点P1到点P2的n维（由Size决定）直线距离
doubleLineLengthCart(double P1[6],double P2[6]);// 声明函数：计算从点P1到点P2的笛卡尔空间（仅XYZ）直线距离
doubleLineLengthAng(double P1[6],double P2[6],int Size);// 声明函数：计算从姿态P1到姿态P2的角度变化距离（基于四元数或欧拉角）
doubleMinPathTime(double P1[6],double P2[6],int Size,structRobot_Parameter_JointLimits_Type Limit[6]);// 声明函数：根据各轴速度限制，计算完成P1到P2线性路径所需的最短时间
doubleVectorLength(double A[3]);// 声明函数：计算一个三维向量A的模长（欧几里得范数）
unsignedshortCrossProduct(double U[3],double V[3],double N[3]);// 声明函数：计算两个三维向量U和V的叉积，结果存入N
doubleDotProduct(double U[3],double V[3]);// 声明函数：计算两个三维向量U和V的点积
doubleAngleBetweenVectors(double U[6],double V[6]);// 声明函数：计算两个六维向量U和V之间的夹角（单位：度）
unsignedshortPointsToVector(double P1[6],double P2[6],double V[3]);// 声明函数：根据点P1和P2计算向量V（V = P2 - P1）
unsignedshortNormalize(double V[3]);// 声明函数：将一个三维向量V单位化
unsignedshortEvalCircle(Path_Type *Circle);// 声明函数：计算一个圆弧路径的所有几何参数（如半径、圆心、法向量等）
unsignedshortMaxBlockSpeed(double d,double a,double j,double v_end,double*v_max);// 声明函数：计算在给定距离、加速度、加加速度和末速度下，一个运动块能达到的最大（初始）速度
unsignedshortMaxMovementDynamics(double d,double a,double j,double v,double v_start,double v_end,double a_start,double*v_max,double*a_up_max,double*a_down_max,double*d_linear);// 声明函数：计算在给定运动参数下，能达到的最大速度、最大加/减速度以及匀速段长度
unsignedshortEvaluateBezier(Frame_Type P[5],double u, Frame_Type *Q,int Size,int Order);// 声明函数：使用de Casteljau算法计算贝塞尔曲线上参数u对应的点Q
doubleBezierLength(Frame_Type P[5],int Size,int Order);// 声明函数：估算一条贝塞尔曲线的长度
doubleBezierLengthHalf1(Frame_Type P[5],int Size,int Order);// 声明函数：估算一条贝塞尔曲线前半段的长度
doubleBezierLengthHalf2(Frame_Type P[5],int Size,int Order);// 声明函数：估算一条贝塞尔曲线后半段的长度
unsignedshortStoppingDistance(double v_max,double a_max,double j_max,double v_act,double a_act,double*stopping_distance);// 声明函数：计算在给定动力学约束和当前状态下，安全停止所需的距离
unsignedshortDynamicLimitsViolated(double P1[6],double P2[6],int Size,structRobot_Parameter_JointLimits_Type Limit[6],double CycleTime,double*redFactor);// 声明函数：检查在当前周期内从P1到P2的运动是否会违反关节的动力学限制
unsignedshortLineCrossBox(double L1[6],double L2[6],double B1[6],double B2[6]);// 声明函数：检查线段L1-L2是否与由B1和B2定义的包围盒相交
unsignedshortPointInBox(double P[6],double B1[6],double B2[6]);// 声明函数：检查点P是否在由B1和B2定义的包围盒内部
unsignedshortWorkspaceMonitor(unsignedchar MovementType, Path_Type* Path,double Tool[6], Robot_Parameter_Workspace_Type Workspace[MAX_ZONE],unsignedshort AxesNum, Mech_Type* Mechanics);// 声明函数：监控规划的路径是否违反了定义的工作空间限制
unsignedshortRoundEdgePoints(MotionPackage_Type* Movement, MotionPackage_Type* MovementPrev,unsignedshort AxesNum, Mech_Type* Mechanics);// 声明函数：计算两个运动块之间的过渡（圆角）控制点
doublePTPLength(Path_Type* Path,unsignedshort AxesNum, Mech_Type* Mechanics);// 声明函数：估算PTP运动在笛卡尔空间下的路径长度
#endif// 结束 #ifndef PATH_PLANNER_H 的宏定义条件块PathPlanner.c 文件
#include"PathPlanner.h"// 包含此源文件对应的头文件 "PathPlanner.h"
#include"RobControl.h"// 包含机器人控制相关的头文件
#include"trig.h"// 包含三角函数相关的头文件
#include"Misc.h"// 包含杂项功能和数据类型的头文件
#include"constants.h"// 包含项目自定义的常量定义头文件
#include"Transformations.h"// 包含坐标变换相关的头文件
#include<math.h>// 包含标准数学库头文件
#definesign(a)((a<0)?-1:1)// 定义一个宏函数 sign，返回一个数的符号（-1, 0, 或 1）
#definemax(a,b)((a>=b)?a:b )// 定义一个宏函数 max，返回两个数中的较大者
#definemin(a,b)((a<b)?a:b )// 定义一个宏函数 min，返回两个数中的较小者
staticdoublecrt(double x)// 定义一个静态函数 crt，用于计算立方根
{//cubic root // 函数体开始，注释：立方根
if(x<0)// 如果输入 x 是负数
{// if 块开始
return-pow(-x,1.0/3.0);// 返回其绝对值的立方根的相反数
}// if 块结束
else// 否则（x为正数或零）
{// else 块开始
returnpow(x,1.0/3.0);// 直接返回 x 的立方根
}// else 块结束
}
doubleLineLength(double P1[6],double P2[6],int Size)// 定义函数：计算从点P1到点P2的n维直线距离
{//calculate length of line from point P1 to point P2 // 函数体开始，注释：计算从点P1到点P2的线段长度
int k;// 声明一个整型变量 k，用作循环计数器
double sum;// 声明一个双精度浮点数变量 sum，用于累加平方差
sum =0;// 初始化 sum 为0
for(k=0;k<Size;k++)// 遍历每个维度（从0到Size-1）
sum +=pow(P2[k]-P1[k],2.0);// 计算两点在当前维度上坐标差的平方，并累加到sum
returnsqrt(sum);// 返回平方和的平方根，即欧几里得距离
}
doubleLineLengthCart(double P1[6],double P2[6])// 定义函数：计算P1到P2的笛卡尔空间距离
{//calculate length of line from point P1 to point P2 using only the three cartesian dimensions // 函数体开始，注释：仅使用三个笛卡尔维度计算线段长度
returnsqrt(pow(P2[0]-P1[0],2.0)+pow(P2[1]-P1[1],2.0)+pow(P2[2]-P1[2],2.0));// 直接计算并返回XYZ三维空间中的欧几里得距离
}
doubleLineLengthAng(double P1[6],double P2[6],int Size)// 定义函数：计算P1到P2的角度变化距离
{//calculate length of angular movement from point P1 to point P2 using quaternions // 函数体开始，注释：使用四元数计算从点P1到点P2的角度运动长度
returnsqrt(pow(P2[3]-P1[3],2.0)+pow(P2[4]-P1[4],2.0)+pow(P2[5]-P1[5],2.0));// 计算姿态分量（A,B,C或四元数分量）的欧几里得距离
}
doubleMinPathTime(double P1[6],double P2[6],int Size,structRobot_Parameter_JointLimits_Type Limit[6])// 定义函数：计算完成路径所需的最短时间
{//calculate minimum time to complete linear path between P1 and P2 given each axis max speed // 函数体开始，注释：在给定各轴最大速度下，计算完成P1和P2之间线性路径的最短时间
double t;// 声明一个双精度浮点数变量 t，用于存储单个轴所需的时间
double t_min =0.0;// 声明一个双精度浮点数变量 t_min，用于存储所有轴所需时间中的最大值，初始化为0.0
int k;// 声明一个整型变量 k，用作循环计数器
for(k=0;k<Size;k++)// 遍历每个轴
{// for 循环体开始
if(P2[k]>=P1[k]&& Limit[k].VelocityPos !=0){// 如果是正向运动且正向速度限制不为0
t =(P2[k]-P1[k])/Limit[k].VelocityPos;// 计算所需时间 = 距离 / 速度
}elseif(P2[k]<P1[k]&& Limit[k].VelocityNeg !=0){// 如果是反向运动且反向速度限制不为0
t =(P1[k]-P2[k])/Limit[k].VelocityNeg;// 计算所需时间 = 距离 / 速度
}// else if 块结束
if(t > t_min)// 如果当前轴所需时间 t 大于已记录的最大时间 t_min
{// this axis is slower // 注释：这个轴更慢
t_min = t;// 更新最大时间为 t
}// if 块结束
}// for 循环体结束
return t_min;// 返回所有轴中最慢的那个所需的时间，即路径总的最短时间
}
doubleVectorLength(double A[3])// 定义函数：计算三维向量A的模长
{//calculate length of vector A // 函数体开始，注释：计算向量A的长度
return(sqrt(A[0]*A[0]+ A[1]*A[1]+ A[2]*A[2]));// 计算并返回向量各分量平方和的平方根
}
unsignedshortCrossProduct(double U[3],double V[3],double N[3])// 定义函数：计算向量U和V的叉积
{// calculate scalar components of cross-product UxV // 函数体开始，注释：计算叉积UxV的标量分量
N[0]= U[1]*V[2]-U[2]*V[1];// 根据叉积公式计算结果向量N的x分量
N[1]= U[2]*V[0]-U[0]*V[2];// 根据叉积公式计算结果向量N的y分量
N[2]= U[0]*V[1]-U[1]*V[0];// 根据叉积公式计算结果向量N的z分量
return0;// 函数执行成功，返回0
}
doubleDotProduct(double U[3],double V[3])// 定义函数：计算向量U和V的点积
{// 函数体开始
return(U[0]*V[0]+ U[1]*V[1]+ U[2]*V[2]);// 根据点积公式计算并返回结果
}
doubleAngleBetweenVectors(double U[6],double V[6])// 定义函数：计算两个六维向量之间的夹角
{// calculate angle between 6-dimensional vectors in degrees // 函数体开始，注释：计算六维向量之间的夹角（度）
//calculate vectors lengths // 注释：计算向量长度
double U_Length =sqrt(U[0]*U[0]+ U[1]*U[1]+ U[2]*U[2]+ U[3]*U[3]+ U[4]*U[4]+ U[5]*U[5]);// 计算向量U的模长
double V_Length =sqrt(V[0]*V[0]+ V[1]*V[1]+ V[2]*V[2]+ V[3]*V[3]+ V[4]*V[4]+ V[5]*V[5]);// 计算向量V的模长
//check that input is valid (vector length not zero) // 注释：检查输入是否有效（向量长度不为零）
if((U_Length < TRF_EPSILON)||(V_Length < TRF_EPSILON))// 如果任一向量的模长接近于零
{// if 块开始
return-1;// 返回-1表示错误或无效输入
}// if 块结束
//calculate dot product between U and V // 注释：计算U和V之间的点积
double UV_DotProduct = U[0]*V[0]+ U[1]*V[1]+ U[2]*V[2]+ U[3]*V[3]+ U[4]*V[4]+ U[5]*V[5];// 计算点积
//extract cosine of angle between U and V // 注释：提取U和V之间夹角的余弦值
double UV_CosAngle = UV_DotProduct/(U_Length*V_Length);// 根据点积公式计算夹角的余弦
//limit it do +/- 1 range (in case of numerical errors) // 注释：将其限制在+/-1范围内（以防数值误差）
if(UV_CosAngle >1)// 如果计算结果略大于1
UV_CosAngle =1;// 钳位到1
elseif(UV_CosAngle <-1)// 如果计算结果略小于-1
UV_CosAngle =-1;// 钳位到-1
//return angle between U and V (in 0..180deg range) // 注释：返回U和V之间的夹角（在0..180度范围内）
returnacosd(UV_CosAngle);// 使用acosd（返回角度值的反余弦函数）计算并返回夹角
}
unsignedshortPointsToVector(double P1[6],double P2[6],double V[3])// 定义函数：根据点P1和P2计算向量V
{//make vector V from point P1 to point P2, i.e. V = P2-P1 // 函数体开始，注释：从点P1到点P2生成向量V，即 V = P2-P1
V[0]= P2[0]-P1[0];// 计算向量的x分量
V[1]= P2[1]-P1[1];// 计算向量的y分量
V[2]= P2[2]-P1[2];// 计算向量的z分量
return0;// 函数执行成功，返回0
}
unsignedshortNormalize(double V[3])// 定义函数：单位化向量V
{// 函数体开始
double norm =VectorLength(V);// 首先计算向量V的模长
if(norm !=0)// 如果模长不为零
{// if 块开始
V[0]/= norm;// 将x分量除以模长
V[1]/= norm;// 将y分量除以模长
V[2]/= norm;// 将z分量除以模长
}// if 块结束
return0;// 函数执行成功，返回0
}
unsignedshortEvalCircle(Path_Type *Circle)// 定义函数：计算圆弧的所有参数
{//calculate all circle parameters // 函数体开始，注释：计算所有圆弧参数
double a,b,c,s;// 声明一些双精度浮点数变量用于中间计算（边长，面积相关）
double A[3],B[3],C[3];// 声明三个三维向量，代表由三个点构成的三角形的边
double alpha, beta, gamma;// 声明三个双精度浮点数变量，用于计算圆心的重心坐标系数
/*** calculate radius and center of cicle using cross and dot product properties ***/// 注释块：使用叉积和点积属性计算圆的半径和圆心
a =LineLengthCart(Circle->MiddlePointPath,Circle->TargetPointPath);// 计算中间点到目标点的距离 a
b =LineLengthCart(Circle->StartPointPath,Circle->TargetPointPath);// 计算起点到目标点的距离 b
c =LineLengthCart(Circle->StartPointPath,Circle->MiddlePointPath);// 计算起点到中间点的距离 c
PointsToVector(Circle->MiddlePointPath,Circle->TargetPointPath,A);// 计算从中间点到目标点的向量A
PointsToVector(Circle->StartPointPath,Circle->TargetPointPath,B);// 计算从起点到目标点的向量B
PointsToVector(Circle->StartPointPath,Circle->MiddlePointPath,C);// 计算从起点到中间点的向量C
CrossProduct(C,A,Circle->Normal);// 计算向量C和A的叉积，得到三角形法向量，存入Circle->Normal
s =VectorLength(Circle->Normal);// 计算法向量的模长，它等于三角形面积的两倍
/* check that the three points are not collinear */// 注释块：检查三个点是否共线
if(s<TRF_EPSILON)// 如果法向量模长接近于零
{// if 块开始
return ERR_PP_CIRCLEPOINTS;// 返回错误，表示三点共线，无法定义圆
}// if 块结束
Circle->Radius = a*b*c /(2.0*s);//radius // 根据外接圆半径公式 R = abc / 4K (K是面积, s=2K) 计算半径
Normalize(Circle->Normal);//normal versor // 将法向量单位化
alpha = a*a *DotProduct(B,C)/(2.0*s*s);// 计算圆心关于起点的重心坐标系数 alpha
beta = b*b *-DotProduct(A,C)/(2.0*s*s);// 计算圆心关于中间点的重心坐标系数 beta
gamma = c*c *DotProduct(B,A)/(2.0*s*s);// 计算圆心关于终点的重心坐标系数 gamma
/* circle center point coordinates */// 注释块：圆心坐标
Circle->Center[0]= alpha * Circle->StartPointPath[0]+ beta * Circle->MiddlePointPath[0]+ gamma * Circle->TargetPointPath[0];// 计算圆心的X坐标
Circle->Center[1]= alpha * Circle->StartPointPath[1]+ beta * Circle->MiddlePointPath[1]+ gamma * Circle->TargetPointPath[1];// 计算圆心的Y坐标
Circle->Center[2]= alpha * Circle->StartPointPath[2]+ beta * Circle->MiddlePointPath[2]+ gamma * Circle->TargetPointPath[2];// 计算圆心的Z坐标
/* calculate vectors from center to start, middle and target point */// 注释块：计算从圆心到起点、中间点和目标点的向量
PointsToVector(Circle->Center,Circle->StartPointPath,Circle->StartVersor);// 计算从圆心到起点的向量
Normalize(Circle->StartVersor);// 单位化该向量
PointsToVector(Circle->Center,Circle->MiddlePointPath,Circle->MiddleVersor);// 计算从圆心到中间点的向量
Normalize(Circle->MiddleVersor);// 单位化该向量
PointsToVector(Circle->Center,Circle->TargetPointPath,Circle->EndVersor);// 计算从圆心到终点的向量
Normalize(Circle->EndVersor);// 单位化该向量
/* calculate cross versor */// 注释块：计算叉积向量（用于定义圆弧平面上的正交基）
CrossProduct(Circle->Normal,Circle->StartVersor,Circle->CrossVersor);// 计算法向量和起始向量的叉积
Normalize(Circle->CrossVersor);// 单位化该向量
/* calculate arc length to end point */// 注释块：计算到终点的弧长
double Test[3];// 声明一个临时三维向量
CrossProduct(Circle->StartVersor,Circle->EndVersor,Test);// 计算起始向量和终点向量的叉积，用于判断旋转方向
double tmpDotProd =DotProduct(Circle->StartVersor,Circle->EndVersor);// 计算起始和终点向量的点积
//argument of acos must be between +/- 1 (avoid numerical errors) // 注释：acos的参数必须在+/-1之间（避免数值误差）
if(tmpDotProd >1)// 如果点积结果略大于1
tmpDotProd =1;// 钳位到1
elseif(tmpDotProd <-1)// 如果点积结果略小于-1
tmpDotProd =-1;// 钳位到-1
Circle->Length = Circle->Radius *acos(tmpDotProd);// 计算弧长 = 半径 * 夹角（弧度）
if(sign(Circle->Normal[2])*sign(Test[2])<0)// 通过比较法向量和Test向量的Z分量符号来判断是优弧还是劣弧
{// if 块开始
Circle->Length = Circle->Radius *2.0* PI - Circle->Length;// 如果是优弧，则用整圆周长减去劣弧长度
}// if 块结束
/* calculate arc length to middle point */// 注释块：计算到中间点的弧长
CrossProduct(Circle->StartVersor,Circle->MiddleVersor,Test);// 同样的方法计算到中间点的弧长
tmpDotProd =DotProduct(Circle->StartVersor,Circle->MiddleVersor);// 计算点积
//argument of acos must be between +/- 1 (avoid numerical errors) // 注释：acos的参数必须在+/-1之间（避免数值误差）
if(tmpDotProd >1)// 钳位
tmpDotProd =1;// ...
elseif(tmpDotProd <-1)// 钳位
tmpDotProd =-1;// ...
Circle->MiddleLength = Circle->Radius *acos(tmpDotProd);// 计算弧长
if(sign(Circle->Normal[2])*sign(Test[2])<0)// 判断优劣弧
{// if 块开始
Circle->MiddleLength = Circle->Radius *2.0* PI - Circle->MiddleLength;// 调整弧长
}// if 块结束
if((Circle->Length <=0)||(Circle->MiddleLength <=0))// 如果计算出的弧长小于等于0
return ERR_PP_CIRCLE_LENGTH;// 返回圆弧长度错误
return0;// 函数执行成功，返回0
}
unsignedshortMaxBlockSpeed(double d,double a,double j,double v_end,double*v_max)// 定义函数：计算运动块的最大速度
{//find maximum (initial) speed of a block with length d, acceleration a, jerk j, and final speed v_end // 函数体开始，注释：寻找长度d，加速度a，加加速度j，末速度v_end的块的最大（初始）速度
if((d<0)||(v_end<0)||(a<=0)||(j<=0))// 检查输入参数的有效性
{// incorrect input -> error // 注释：不正确的输入 -> 错误
return255;// 返回错误码
}
if(d==0)// 如果运动距离为0
{//zero length -> no movement possible // 注释：零长度 -> 不可能运动
*v_max = v_end;// 最大速度就是末速度
return0;// 返回成功
}
double Delta =2.0*a*v_end/j + a*a*a/j/j;// 计算一个判据 Delta
if(d<=Delta)// 如果距离d小于等于Delta
{// a cannot be reached -> reduce it. There is no linear acceleration section, it's all jerk limited movement // 注释：无法达到加速度a -> 减小它。没有匀加速段，全是加加速度受限的运动
double p3 =2.0*v_end*j/3.0;// 计算三次方程的系数
double q2 =-d*j*j/2.0;// 计算三次方程的系数
double Det = p3*p3*p3+q2*q2;// 计算三次方程的判别式
double alpha =crt(-q2+sqrt(Det));// 求解三次方程
double betha =crt(-q2-sqrt(Det));// 求解三次方程
double a_max = alpha + betha;// 得到能达到的最大加速度
*v_max =(v_end + a_max*a_max/j);// 计算最大速度
return0;// 返回成功
}
else// 如果距离d大于Delta
{// a can be reached, there is a linear acceleration section // 注释：可以达到加速度a，存在匀加速段
double Det = a*a/j/j/4.0+2.0*d/a + v_end*v_end/a/a - v_end/j;// 计算一个判据
*v_max = a*(sqrt(Det)- a/j/2.0);// 计算最大速度
return0;// 返回成功
}
}
unsignedshortMaxMovementDynamics(double d,double a,double j,double v,double v_start,double v_end,double a_start,double*v_max,double*a_up_max,double*a_down_max,double*d_linear)// 定义函数：计算运动的最大动力学参数
{//find maximum speed and acceleration of a movement with length d, speed v, acceleration a, jerk j, initial and final speed v_start and v_end, initial acceleration a_start // 函数体开始，注释：寻找运动的...
//also returns size of linear speed interval // 注释：同样返回匀速区间的长度
double d_start, d_end;// 声明变量：加速段距离，减速段距离
double t_start, t_end;// 声明变量：加速段时间，减速段时间
double a_max, a_up, a_down;// 声明变量：最大加速度，上升加速度，下降加速度
double dv_start, dv_end;// 声明变量：速度变化量
double step, Delta, v_high, v_low;// 声明变量：用于二分法查找的步长、判据、速度上下界
if((d<0)||(v_end<0)||(v_end>v)||(v_start<0)||(a<=0)||(j<=0)||(v<=0)||(fabs(a_start)>a+TRF_EPSILON))// 检查输入参数的有效性
//note that programmed speed for path planner cannot be negative // 注释：路径规划器的编程速度不能为负
//note that starting acceleration cannot be higher than max acc // 注释：起始加速度不能高于最大加速度
{// incorrect input -> error // 注释：不正确的输入 -> 错误
return255;// 返回错误码
}
if(d==0)// 如果距离为0
{//zero length -> no movement possible // 注释：零长度 -> 不可能运动
if(v_start!=v_end)// 如果初末速度不同
{// if 块开始
return250;// 返回错误码
}// if 块结束
else// 如果初末速度相同
{// else 块开始
*v_max = v_start;// 最大速度就是初速度
*a_up_max =0;// 上升加速度为0
*a_down_max =0;// 下降加速度为0
*d_linear =0;// 匀速段长度为0
return0;// 返回成功
}// else 块结束
}// if 块结束
/*** check if v_end can be reached ***/// 注释块：检查是否能达到v_end
dv_start = v_end - v_start;// 计算从初速度到末速度的速度变化量
a_max =sqrt(j*fabs(dv_start)+0.5*a_start*a_start);//max acc (no linear segment) - normally limited by "a" (limit) // 计算不含匀加速段时能达到的最大加速度
/* REMOVED because v_end is not max speed and it can be exceeded. Think for example of a case when v_end=v_start.
if (fabs(a_start)>a_max+TRF_EPSILON) //a_start too high, max speed will be exceeded!
return 251;
*/// 注释块：已移除，因为v_end不是最大速度，可以被超过。例如v_end=v_start的情况。
if(a_max <fabs(a_start)) a_max =fabs(a_start);// a_max不能小于初始加速度的绝对值
if(a_max > a) a_max = a;// a_max不能超过系统设定的最大加速度a
if(a_max !=0)// 如果最大加速度不为0
{// if 块开始
double t0 =(a_max-fabs(a_start))/j;// 计算第一段加加速时间
if(t0<0) t0=0;// 时间不能为负
double t1 =1/a_max *(fabs(dv_start)-0.5*a_max*a_max/j -fabs(a_start)*(a_max-fabs(a_start))/j -0.5*(a_max-fabs(a_start))*(a_max-fabs(a_start))/j);// 计算匀加速段时间
if(t1<0) t1=0;// 时间不能为负
double t2 = a_max/j;// 计算减加速时间
t_start = t0 + t1 + t2;// 总的加速时间
double v1 = v_start +(fabs(a_start)* t0 +0.5*j *t0*t0)*sign(dv_start);//speed at end of t0 // 计算t0结束时的速度
double v2 = v1 +(a_max*t1)*sign(dv_start);//speed at end of t1 // 计算t1结束时的速度
d_start =(v_start * t0 +(j * t0*t0*t0 /6.0+0.5*fabs(a_start)* t0*t0)*sign(dv_start));// 计算t0段的距离
d_start +=(v1 * t1 +(0.5* a_max * t1*t1)*sign(dv_start));// 累加t1段的距离
d_start +=(v2 * t2 +(0.5* a_max * t2*t2 - j * t2*t2*t2 /6.0)*sign(dv_start));// 累加t2段的距离
}// if 块结束
else// 如果最大加速度为0
{// else 块开始
t_start =0;// 加速时间为0
d_start =0;// 加速距离为0
}// else 块结束
if(d_start > d+TRF_EPSILON)// 如果仅加速到v_end所需的距离就超过了总距离d
{// if 块开始
//v_end is too high (or too low) and cannot be reached with current dynamic limits -> movement will be interrupted while running with a!=0 // 注释：v_end太高（或太低），在当前动力学限制下无法达到 -> 运动将在a!=0时被中断
*v_max = v_end;// 最大速度就是v_end
*a_up_max = a_max*sign(dv_start);// 上升加速度
*a_down_max =0;// 下降加速度为0
*d_linear =0;// 匀速段为0
return0;// 返回成功
}// if 块结束
/* assume that v can be reached */// 注释块：假设可以达到速度v
// interval from v_start to v // 注释：从v_start到v的区间
dv_start = v - v_start;// 计算速度变化
a_up =sqrt(j*fabs(dv_start)+0.5*a_start*a_start);//max acc (no linear segment) - normally limited by a (limit) // 计算不含匀加速段时的最大加速度
if(fabs(a_start)>a_up+TRF_EPSILON)//a_start too high, max speed will be exceeded! // 注释：a_start太高，将超过最大速度！
return252;// 返回错误码
if(a_up > a) a_up = a;// 限制最大上升加速度
if(a_up !=0)// 如果上升加速度不为0
{// if 块开始
double t0 =(a_up-fabs(a_start))/j;// 计算时间
double t1 =1/a_up *(fabs(dv_start)-0.5*a_up*a_up/j -fabs(a_start)*(a_up-fabs(a_start))/j -0.5*(a_up-fabs(a_start))*(a_up-fabs(a_start))/j);// 计算时间
double t2 = a_up/j;// 计算时间
t_start = t0 + t1 + t2;// 总加速时间
double v1 = v_start +(fabs(a_start)* t0 +0.5*j *t0*t0)*sign(dv_start);//speed at end of t0 // 计算速度
double v2 = v1 +(a_up*t1)*sign(dv_start);//speed at end of t1 // 计算速度
d_start =(v_start * t0 +(j * t0*t0*t0 /6.0+0.5*fabs(a_start)* t0*t0)*sign(dv_start));// 计算距离
d_start +=(v1 * t1 +(0.5* a_up * t1*t1)*sign(dv_start));// 累加距离
d_start +=(v2 * t2 +(0.5* a_up * t2*t2 - j * t2*t2*t2 /6.0)*sign(dv_start));// 累加距离
}// if 块结束
else// 如果上升加速度为0
{// else 块开始
t_start =0;// 加速时间为0
d_start =0;// 加速距离为0
}// else 块结束
// interval from v to v_end // 注释：从v到v_end的区间
dv_end =fabs(v - v_end);// 计算速度变化
a_down =sqrt(j*(dv_end));// 计算下降加速度
if(a_down > a) a_down = a;// 限制最大下降加速度
if(a_down !=0)// 如果下降加速度不为0
{// if 块开始
t_end = dv_end/a_down + a_down/j;// 计算减速时间
d_end =0.5*(dv_end*dv_end/a_down + dv_end*a_down/j);// 计算减速距离
if(v<v_end)// 如果v小于v_end（实际是加速过程）
d_end += v * t_end;// 修正距离计算
else// 如果是减速过程
d_end += v_end * t_end;// 修正距离计算
}// if 块结束
else// 如果下降加速度为0
{// else 块开始
t_end =0;// 减速时间为0
d_end =0;// 减速距离为0
}// else 块结束
Delta = d - d_start - d_end;//constant speed section // 计算匀速段的距离
if(Delta >=0)// 如果匀速段距离大于等于0
{// v can be reached // 注释：可以达到v
*v_max = v;//if v<v_start then this v_max is not really maximum but speed of constant section // 最大速度就是v
*a_up_max = a_up *sign(dv_start);// 上升加速度
*a_down_max = a_down;// 下降加速度
*d_linear = Delta;// 匀速段距离
if(fabs(a_start)>a_up+TRF_EPSILON)//a_start too high, max speed will be exceeded! // 检查初始加速度是否过高
return253;// 返回错误码
return0;// 返回成功
}// if 块结束
/* removed - we already checked above for the case that v_end cannot be reached!
it works when you have many small blocks in acceleration but it does not work when you have one single block and need to
complete the movement by the end of the block.
else if (d >= d_start)
{// v can be reached (d>=d_start) but v_end cannot (Delta<0) -> start movement and let next blocks complete it (TODO what if there are no more blocks after it?!?!??!)
*v_max = v;
*a_up_max = a_up*sign(dv_start);
*a_down_max = a;
*d_linear = 0;
if (fabs(a_start)>a_up+TRF_EPSILON) //a_start too high, max speed will be exceeded!
return 248;
return 0;
}
*/// 注释块：已移除的逻辑
else// 如果匀速段距离为负
{// v cannot be reached, reduce v // 注释：无法达到v，需要减小v
if(v_start > v)// 如果初速度大于目标速度v
{// in this case v is not needed, just decelerate from v_start to v_end // 注释：这种情况下不需要v，只需从v_start减速到v_end
*v_max = v_end;//no linear speed interval // 最大速度是v_end，没有匀速段
//calculate a_max from d1+d2+d3=d // 注释：从d1+d2+d3=d计算a_max
double aaa =(v_start+v_end)/j+fabs(a_start)/j/j-fabs(a_start)*fabs(a_start)/j/j;// 复杂的系数计算
double bbb = d + v_start*fabs(a_start)/j -fabs(a_start)*fabs(a_start)*fabs(a_start)/3.0/j/j;// 复杂的系数计算
double ccc =(v_start-v_end+fabs(a_start)*fabs(a_start)/2.0/j)*(v_start+v_end+fabs(a_start)*fabs(a_start)/j);// 复杂的系数计算
double ddd = bbb*bbb-aaa*ccc;// 复杂的判据计算
if((ddd<0)||(aaa==0))// 如果无解
return254;//should never happen with current settings // 返回错误码
*a_up_max =-(bbb -sqrt(ddd))/ aaa;//acceleration is negative because movement is speeding down to v_end // 计算上升加速度（实际是减速度）
*a_down_max =0;//no ending interval // 没有下降段
*d_linear =0;// no linear speed interval // 没有匀速段
if(fabs(a_start)>fabs(*a_up_max)+TRF_EPSILON)//a_start too high, max speed will be exceeded! // 检查初始加速度
return255;// 返回错误码
return0;// 返回成功
}// if 块结束
step =0;// 初始化二分法步数
v_high = v;// 设置速度上界
v_low =max(v_start,v_end);// 设置速度下界
do// 开始二分法循环
{// do-while 循环体开始
step +=0.01;// 增加步数
if(Delta <0)// 如果匀速段距离为负
{// if 块开始
v_high = v;// 减小上界
v =(v_high + v_low)/2.0;// 更新v为中间值
}// if 块结束
elseif(Delta >0)// 如果匀速段距离为正
{// else if 块开始
v_low = v;// 增大小下界
v =(v_high + v_low)/2.0;// 更新v为中间值
}// else if 块结束
elseif(Delta ==0)// 如果恰好为0
{// else if 块开始
step =1;// 结束循环
}// else if 块结束
// interval from v_start to v // 注释：从v_start到v的区间（重新计算）
dv_start = v - v_start;// 速度变化
a_up =sqrt(j*fabs(dv_start)+0.5*a_start*a_start);//max acc (no linear segment) - normally limited by a (limit) // 上升加速度
if(a_up > a) a_up = a;// 限制
if(a_up !=0)// 如果不为0
{// if 块开始
double t0 =(a_up-fabs(a_start))/j;// 时间
double t1 =1/a_up *(fabs(dv_start)-0.5*a_up*a_up/j -fabs(a_start)*(a_up-fabs(a_start))/j -0.5*(a_up-fabs(a_start))*(a_up-fabs(a_start))/j);// 时间
double t2 = a_up/j;// 时间
t_start = t0 + t1 + t2;// 总时间
double v1 = v_start +(fabs(a_start)* t0 +0.5*j *t0*t0)*sign(dv_start);//speed at end of t0 // 速度
double v2 = v1 +(a_up*t1)*sign(dv_start);//speed at end of t1 // 速度
d_start =(v_start * t0 +(j * t0*t0*t0 /6.0+0.5*fabs(a_start)* t0*t0)*sign(dv_start));// 距离
d_start +=(v1 * t1 +(0.5* a_up * t1*t1)*sign(dv_start));// 累加距离
d_start +=(v2 * t2 +(0.5* a_up * t2*t2 - j * t2*t2*t2 /6.0)*sign(dv_start));// 累加距离
}// if 块结束
else// 如果为0
{// else 块开始
t_start =0;// 时间为0
d_start =0;// 距离为0
}// else 块结束
// interval from v to v_end // 注释：从v到v_end的区间（重新计算）
dv_end = v - v_end;// 速度变化
a_down =sqrt(j*(dv_end));// 下降加速度
if(a_down > a) a_down = a;// 限制
if(a_down !=0)// 如果不为0
{// if 块开始
t_end = dv_end/a_down + a_down/j;// 时间
d_end =0.5*(dv_end*dv_end/a_down + dv_end*a_down/j);// 距离
if(v<v_end)// 修正
d_end += v * t_end;// ...
else// 修正
d_end += v_end * t_end;// ...
}// if 块结束
else// 如果为0
{// else 块开始
t_end =0;// 时间为0
d_end =0;// 距离为0
}// else 块结束
Delta = d - d_start - d_end;// 重新计算匀速段距离
}while(step <1);// 循环直到找到解
*v_max = v;// 最大速度
*a_up_max = a_up;// 上升加速度
*a_down_max = a_down;// 下降加速度
*d_linear =0;// no linear speed interval // 匀速段为0
if(fabs(a_start)>a_up+TRF_EPSILON)//a_start too high, max speed will be exceeded! // 检查初始加速度
{// if 块开始
return249;// 返回错误码
}// if 块结束
return0;// 返回成功
}// else 块结束
}
unsignedshortEvaluateBezier(Frame_Type P[5],double u, Frame_Type *Q,int Size,int Order)// 定义函数：计算贝塞尔曲线上的点
{// input are Bezier control points P0..P4, time u and size of coordinate system (number of robot's axes); output is point Q // 函数体开始，注释：输入...
unsignedchar k,i,j;// 声明循环变量
int n = Order;// n=4 for quartic Bezier curves (used for round edges), n=3 for cubic Bezier curves (used for MS movement) // n是贝塞尔曲线的阶数
if((u<0)||(u>1))// 如果参数u不在0到1之间
return255;// 返回错误码
Frame_Type Point[5];// 声明一个临时数组用于de Casteljau算法
for(i=0;i<=n;i++) Point[i]=P[i];// 将控制点复制到临时数组
for(k=1; k<=n; k++)// de Casteljau算法的外层循环
{// for 循环体开始
for(i=0; i<=n-k; i++)// 内层循环
{// for 循环体开始
for(j=0; j<Size; j++)// 对每个坐标轴进行线性插值
Point[i].Axes[j]=(1.0-u)*Point[i].Axes[j]+ u*Point[i+1].Axes[j];
}// for 循环体结束
}// for 循环体结束
for(j=0; j<Size; j++)// 将最终计算出的点
Q->Axes[j]= Point[0].Axes[j];// 赋值给输出参数Q
return0;// 返回成功
}
doubleBezierLength(Frame_Type P[5],int Size,int Order)// 定义函数：估算贝塞尔曲线长度
{// input are Bezier control points P0..P4 and size of coordinate system (number of robot's axes) // 函数体开始，注释：输入...
Frame_Type Point[2];// 声明一个数组，用于存储曲线上的两个连续点
int i;// 声明循环变量
int k =10;//increase number of points for more accuracy // 细分点数，越多越精确
double Length =0;// 初始化长度为0
Point[0]= P[0];// 起始点为第一个控制点
//evaluate the BezierCurve at different points and then add the distances between them // 注释：在贝塞尔曲线上取不同的点然后将它们之间的距离相加
for(i=1;i<=k;i++)// 循环k次
{// for 循环体开始
EvaluateBezier(P,(double)(i)/(double)(k),&(Point[1]),Size,Order);// 计算下一个细分点
Length +=LineLength(Point[0].Axes,Point[1].Axes,Size);// 累加两点间的直线距离
Point[0]= Point[1];// 更新当前点
}// for 循环体结束
return Length;// 返回总长度
}
doubleBezierLengthHalf1(Frame_Type P[5],int Size,int Order)// 定义函数：估算贝塞尔曲线前半段长度
{// input are Bezier control points P0..P4 and size of coordinate system (number of robot's axes) // 函数体开始，注释：...
// calculates only length of first half P0..P2 // 注释：只计算前半段P0..P2的长度
Frame_Type Point[2];// 存储连续点的数组
int i;// 循环变量
int k =10;//increase number of points for more accuracy // 细分点数
double Length =0;// 初始化长度为0
Point[0]= P[0];// 起点
//evaluate the BezierCurve at different points and then add the distances between them // 注释：...
for(i=1;i<=k/2;i++)// 只循环到一半
{// for 循环体开始
EvaluateBezier(P,(double)(i)/(double)(k),&(Point[1]),Size,Order);// 计算细分点
Length +=LineLength(Point[0].Axes,Point[1].Axes,Size);// 累加距离
Point[0]= Point[1];// 更新当前点
}// for 循环体结束
return Length;// 返回前半段长度
}
doubleBezierLengthHalf2(Frame_Type P[5],int Size,int Order)// 定义函数：估算贝塞尔曲线后半段长度
{// input are Bezier control points P0..P4 and size of coordinate system (number of robot's axes) // 函数体开始，注释：...
// calculates only length of second half P2..P4 // 注释：只计算后半段P2..P4的长度
Frame_Type Point[2];// 存储连续点的数组
int i;// 循环变量
int k =10;//increase number of points for more accuracy // 细分点数
double Length =0;// 初始化长度为0
EvaluateBezier(P,0.5,&(Point[0]),Size,Order);// 计算曲线中点作为起始点
//evaluate the BezierCurve at different points and then add the distances between them // 注释：...
for(i=k/2+1;i<=k;i++)// 从中点后一个细分点开始循环
{// for 循环体开始
EvaluateBezier(P,(double)(i)/(double)(k),&(Point[1]),Size,Order);// 计算细分点
Length +=LineLength(Point[0].Axes,Point[1].Axes,Size);// 累加距离
Point[0]= Point[1];// 更新当前点
}// for 循环体结束
return Length;// 返回后半段长度
}
unsignedshortStoppingDistance(double v_max,double a_max,double j_max,double v_act,double a_act,double* stopping_distance)// 定义函数：计算停止距离
{//returns the stopping distance of a movement running at v_act and a_act given the // 函数体开始，注释：...
//v_max, a_max and j_max costraints // 注释：...
//NOTE: assumes positive direction of motion!!! // 注释：注意：假设正向运动！！！
/* check input parameters */// 注释块：检查输入参数
if((v_max<=0)||(a_max<=0)||(j_max<=0)||(v_act<=0))// 如果输入参数无效
{// if 块开始
*stopping_distance =0;// 停止距离为0
return255;// 返回错误码
}// if 块结束
double dt[4],ds[4];// 声明数组存储各阶段的时间和距离
double v_top,a_top;// 声明变量存储最高速度和最高加速度
if(a_act > TRF_EPSILON)// 如果当前正在加速
{// movement is accelerating -> need to bring acceleration to zero first // 注释：运动正在加速 -> 需要先将加速度降到零
dt[0]= a_act/j_max;// 计算将加速度降到零所需时间
v_top = v_act +0.5*a_act*a_act/j_max;//speed reached at the end of the first section // 计算该阶段结束时的速度
dt[2]= v_top/a_max - a_max/j_max;// 计算匀减速段时间
if(dt[2]<=0)//no linear section // 如果没有匀减速段
a_max =sqrt(v_top*j_max);// 重新计算能达到的最大减速度
dt[1]= a_max/j_max;// 减加速时间
dt[2]= v_top/a_max - a_max/j_max;// 匀减速时间
dt[3]= a_max/j_max;// 减减速时间
ds[0]= v_act*dt[0]+0.5*a_act*dt[0]*dt[0]- j_max*dt[0]*dt[0]*dt[0]/6.0;// 计算各阶段距离
ds[1]= v_top*dt[1]-(j_max * dt[1]*dt[1]*dt[1]/6.0);// ...
ds[2]= v_top*dt[2]-(0.5* a_max *dt[2]*dt[2]+0.5* a_max*a_max *dt[2]*dt[2]/ j_max);// ...
ds[3]= v_top*dt[3]-(- j_max*dt[3]*dt[3]*dt[3]/6.0+0.5* a_max *dt[3]*dt[3]+(v_top-0.5*a_max*a_max/j_max)*dt[3]);// ...
}// if 块结束
elseif(a_act <-TRF_EPSILON)// 如果当前正在减速
{// movement is decelerating already -> find out what deceleration is needed to reach zero // 注释：运动已在减速 -> 找出到达零速需要何种减速度
dt[0]=0;// 第一阶段时间为0
a_act =fabs(a_act);// 取加速度绝对值
v_top = v_act;// 最高速度就是当前速度
a_top =sqrt(j_max*v_act+0.5*a_act*a_act);// 计算能达到的最大减速度
if(a_top < a_act)// 限制
a_top = a_act;// ...
if(a_top > a_max)// 限制
a_top = a_max;// ...
dt[1]=(a_top-a_act)/j_max;// 计算各阶段时间
dt[2]=1/a_top *(v_act -0.5*a_top*a_top/j_max -a_act*(a_top-a_act)/j_max -0.5*(a_top-a_act)*(a_top-a_act)/j_max);// ...
dt[3]= a_top/j_max;// ...
double v1 = v_top -(a_act*dt[1]+0.5*j_max * dt[1]*dt[1]);// 计算中间速度
double v2 = v1 - a_top*dt[2];// ...
ds[0]=0;// 计算各阶段距离
ds[1]= v_top*dt[1]-(j_max * dt[1]*dt[1]*dt[1]/6.0)-0.5* a_act * dt[1]*dt[1];// ...
ds[2]= v1 *dt[2]-(0.5* a_top *dt[2]*dt[2]);// ...
ds[3]= v2 * dt[3]-(-j_max *dt[3]*dt[3]*dt[3]/6.0+0.5* a_top* dt[3]*dt[3]);// ...
}// else if 块结束
else// 如果当前是匀速
{// movement is running at constant speed -> decelerate with default acceleration // 注释：运动在匀速运行 -> 以默认加速度减速
dt[0]=0;// 第一阶段时间为0
v_top = v_act;// 最高速度是当前速度
dt[2]= v_top/a_max - a_max/j_max;// 计算匀减速时间
if(dt[2]<=0)//no linear section // 如果没有匀减速段
a_max =sqrt(v_top*j_max);// 重新计算最大减速度
dt[1]= a_max/j_max;// 减加速时间
dt[2]= v_top/a_max - a_max/j_max;// 匀减速时间
dt[3]= a_max/j_max;// 减减速时间
ds[0]=0;// 计算各阶段距离
ds[1]= v_top*dt[1]-(j_max * dt[1]*dt[1]*dt[1]/6.0);// ...
ds[2]= v_top*dt[2]-(0.5* a_max *dt[2]*dt[2]+0.5* a_max*a_max *dt[2]*dt[2]/ j_max);// ...
ds[3]= v_top*dt[3]-(- j_max*dt[3]*dt[3]*dt[3]/6.0+0.5* a_max *dt[3]*dt[3]+(v_top-0.5*a_max*a_max/j_max)*dt[3]);// ...
}// else 块结束
*stopping_distance = ds[0]+ds[1]+ds[2]+ds[3];// 总停止距离是各阶段距离之和
return0;// 返回成功
}
unsignedshortDynamicLimitsViolated(double P1[6],double P2[6],int Size,structRobot_Parameter_JointLimits_Type Limit[6],double CycleTime,double*redFactor)// 定义函数：检查是否违反动力学限制
{//checks if the dynamic limits of the joints are being violated during the current cycle time // 函数体开始，注释：...
//returns indexes(+1) of axes where largest violation occurs - e.g. 01010010 means violation on axes 1,4,6 // 注释：返回...
int k;// 循环变量
double tmpRedFactor =1.0;// 临时减速因子，初始化为1.0
unsignedshort BadAxes =0;// 存储违反限制的轴的位掩码，初始化为0
for(k=0;k<Size;k++)// 遍历所有轴
{// for 循环体开始
double JointSpeed =(P2[k]-P1[k])/ CycleTime;// 计算当前周期的关节速度
if(JointSpeed !=0)// 如果速度不为0
{// if 块开始
if((JointSpeed >0)&&(Limit[k].VelocityPos / JointSpeed < tmpRedFactor))// 如果是正向速度且超速
{// if 块开始
tmpRedFactor = Limit[k].VelocityPos / JointSpeed;// 更新减速因子
BadAxes |=1<<(k+1);// 在位掩码中标记该轴
}// if 块结束
if((JointSpeed <0)&&(-Limit[k].VelocityNeg / JointSpeed < tmpRedFactor))// 如果是反向速度且超速
{// if 块开始
tmpRedFactor =-Limit[k].VelocityNeg / JointSpeed;// 更新减速因子
BadAxes |=1<<(k+1);// 在位掩码中标记该轴
}// if 块结束
}// if 块结束
}// for 循环体结束
*redFactor = tmpRedFactor;// 将计算出的减速因子赋值给输出参数
return BadAxes;// 返回违反限制的轴的位掩码
}
unsignedshortLineCrossBox(double L1[6],double L2[6],double B1[6],double B2[6])// 定义函数：检查线段是否与包围盒相交
{//checks if the segment from L1 to L2 crosses the box defined by B1 and B2 // 函数体开始，注释：...
//note that line (as defined by L1-L2) might still cross box but outside L1-L2 segment // 注释：注意，L1-L2定义的直线可能穿过盒子，但在线段之外
//in that case the function returns false // 注释：那种情况下函数返回false
//https://tavianator.com/fast-branchless-raybounding-box-intersections/ // 注释：参考链接
double t[6];// 存储射线与包围盒各平面交点的参数t
double D[3];//line direction // 线段的方向向量
PointsToVector(L1,L2,D);// 计算方向向量
double LARGE_NUMBER =1e+10;// 定义一个很大的数
int i;// 循环变量
for(i=0;i<3;i++)// 遍历XYZ三个维度
{// for 循环体开始
if(D[i]!=0)// 如果在该维度上有移动
{// if 块开始
t[2*i+0]=(B1[i]- L1[i])/D[i];// 计算与第一个平面的交点参数t
t[2*i+1]=(B2[i]- L1[i])/D[i];// 计算与第二个平面的交点参数t
}// if 块结束
elseif(L1[i]>=min(B1[i],B2[i])&& L1[i]<=max(B1[i],B2[i]))// 如果在该维度上没有移动，但线段在该维度上位于盒子内
{// else if 块开始
t[2*i+0]= LARGE_NUMBER;// 设置为大数
t[2*i+1]=-LARGE_NUMBER;// 设置为负大数
}// else if 块结束
else// 如果在该维度上没有移动且位于盒子外
{// else 块开始
t[2*i+0]= LARGE_NUMBER;// 设置为大数
t[2*i+1]= LARGE_NUMBER;// 设置为大数
}// else 块结束
}// for 循环体结束
double tmin =max(max(min(t[0], t[1]),min(t[2], t[3])),min(t[4], t[5]));// 计算所有维度交集区间的起始参数t
double tmax =min(min(max(t[0], t[1]),max(t[2], t[3])),max(t[4], t[5]));// 计算所有维度交集区间的结束参数t
if(tmax<0|| tmin>1|| tmin>tmax)// 如果交集在射线负方向，或在线段之外，或交集为空
return0;// 不相交
else// 否则
return1;// 相交
}
unsignedshortPointInBox(double P[6],double B1[6],double B2[6])// 定义函数：检查点是否在包围盒内
{//checks if the point P is inside the box (returns 1) or outside (returns 0) // 函数体开始，注释：...
// XYZ says what axes are in and out of the box limits // 注释：XYZ表示哪些轴在盒子限制内外
int i;// 循环变量
unsignedshort tmpRet =1;// 临时返回值，初始化为1（在盒子内）
for(i=0;i<3;i++)// 遍历XYZ三轴
tmpRet *=(unsignedshort)(P[i]<=max(B1[i],B2[i])&& P[i]>=min(B1[i],B2[i]));// 对每轴检查是否在范围内，并将结果与tmpRet相乘
return tmpRet;// 返回最终结果，如果所有轴都在范围内，则为1，否则为0
}
unsignedshortWorkspaceMonitor(unsignedchar MovementType, Path_Type* Path,double Tool[6], Robot_Parameter_Workspace_Type Workspace[MAX_ZONE],unsignedshort AxesNum, Mech_Type* Mechanics)// 定义函数：工作空间监控
{// check if planned movement violates the defined workspace // 函数体开始，注释：...
// returns index of violated zone (1..MAX_ZONE), 0 otherwise // 注释：...
//path workspace monitoring // 注释：路径工作空间监控
short calculatePoints =0;// 标记是否已计算细分点，避免重复计算
double subInc,tmpRotAngle;// 细分增量，临时旋转角度
int k,j;// 循环变量
short sub;// 细分点索引
for(k=0;k<MAX_ZONE;k++)// 遍历所有定义的工作空间区域
{// for 循环体开始
//check for allowed and forbidden zones // 注释：检查允许和禁止区域
if(!Workspace[k].Type)// 如果区域类型未定义
continue;// 跳过此区域
Frame_Type subPoints[WS_SUBPOINTS];// 存储路径细分点的数组
memcpy(subPoints[0].Axes,Path->StartPointPath,sizeof(subPoints[0].Axes));// 将路径起点作为第一个细分点
//calculate inner subpoints in path only if not done yet // 注释：仅在尚未完成时计算路径上的内部子点
//do not calculate at all if no zones are defined // 注释：如果未定义区域，则根本不计算
//avoid repeating calculations for following zones // 注释：避免为后续区域重复计算
if(!calculatePoints)// 如果尚未计算细分点
{// if 块开始
calculatePoints =1;// 标记为已计算
subInc =1.0/(WS_SUBPOINTS-1);// 计算细分增量
if(MovementType == MOVE_CIRCLE)// 如果是圆弧运动
{// if 块开始
tmpRotAngle = Path->Length / Path->Radius;// 计算总旋转角度
if(tmpRotAngle >2*PI)// 如果超过一整圈
tmpRotAngle =2*PI;//consider only one full rotation at maximum // 最多只考虑一整圈
}// if 块结束
for(sub=1;sub<WS_SUBPOINTS;sub++)// 循环计算所有细分点
{// for 循环体开始
Frame_Type tmpTargetJ, tmpTargetX;// 临时关节和笛卡尔坐标目标
//interpolate movement at BlockLength/(WS_SUBPOINTS-1)*sub // 注释：在...位置插值运动
double u = sub * subInc;// 计算当前细分点的参数u
switch(MovementType)// 根据运动类型进行插值
{// switch 块开始
case MOVE_PTP:// PTP运动
for(j=0;j<AxesNum;j++)// 关节空间线性插值
{// for 循环体开始
tmpTargetJ.Axes[j]=(1-u)* Path->StartPointJoint[j]+ u * Path->TargetPointJoint[j];
}// for 循环体结束
//calculate corresponding TCP with direct TRF // 注释：用正运动学计算对应的TCP
Transformations(Mechanics,TRF_DIRECT,tmpTargetJ.Axes,Path->StartPointPath,tmpTargetX.Axes);// 正运动学变换
break;// case 结束
case MOVE_CIRCLE:// 圆弧运动
for(j=0;j<3;j++)// 笛卡尔空间圆弧插值
{//X,Y,Z on circle // 注释：圆上的X,Y,Z
//P = C + R cos(t) U + R sin(t) V // 注释：圆的参数方程
tmpTargetX.Axes[j]= Path->Center[j]+ Path->Radius *cos(tmpRotAngle * u)* Path->StartVersor[j]+
Path->Radius *sin(tmpRotAngle * u)* Path->CrossVersor[j];// 计算插值点坐标
}// for 循环体结束
break;// case 结束
case MOVE_SPLINE:// 样条运动
EvaluateBezier(Path->Spline.CtrlPoint,u,&tmpTargetX,BEZIER_XYZ,BEZIER_CUBIC);// 使用贝塞尔曲线插值
break;// case 结束
}// switch 块结束
//add tool // 注释：加上工具
if(AxesNum <5)// 如果是2D或3D机器人
{// if 块开始
SubFrame2D(Tool,tmpTargetX.Axes,subPoints[sub].Axes);// 应用2D工具变换
}// if 块结束
else// 如果是6轴机器人
{// else 块开始
SubFrame3D(Tool,tmpTargetX.Axes,0,0,0,subPoints[sub].Axes);// 应用3D工具变换
}// else 块结束
}// for 循环体结束
}// if 块结束
//check that lines between subpoints do not violate workspace // 注释：检查子点之间的线段是否违反工作空间
for(sub=1;sub<WS_SUBPOINTS;sub++)// 遍历所有细分线段
{// for 循环体开始
unsignedshort tmpInsideL1 =PointInBox(subPoints[sub-1].Axes,Workspace[k].PositionMin,Workspace[k].PositionMax);// 检查线段起点是否在盒子内
unsignedshort tmpInsideL2 =PointInBox(subPoints[sub].Axes,Workspace[k].PositionMin,Workspace[k].PositionMax);// 检查线段终点是否在盒子内
unsignedshort tmpHitBox =LineCrossBox(subPoints[sub-1].Axes,subPoints[sub].Axes,Workspace[k].PositionMin,Workspace[k].PositionMax);// 检查线段是否与盒子相交
if(((!tmpInsideL1 ||!tmpInsideL2)&& Workspace[k].Type==ZONE_SAFE)||(tmpHitBox && Workspace[k].Type==ZONE_FORBIDDEN))// 如果是安全区但点在区外，或者，是禁区且线段与之相交
{// if 块开始
return(k+1);// 返回违反的区域索引（+1）
}// if 块结束
}// for 循环体结束
}// for 循环体结束
return0;//no violation occurred if execution arrives here // 如果执行到这里，说明没有发生违规，返回0
}
//local help function // 注释：局部辅助函数
unsignedshortControlPoints(unsignedchar MovementType,Path_Type* Path,double BlockLength, Frame_Type CtrlPoints[2],unsignedchar Edge,unsignedshort AxesNum, Mech_Type* Mechanics)// 定义函数：计算过渡的控制点
{//calculate two control points of a round edge // 函数体开始，注释：计算圆角过渡的两个控制点
int j;// 循环变量
double DistA, DistB;// 距离参数
switch(MovementType)// 根据运动类型
{// switch 块开始
case MOVE_LINE:// 直线运动
//linear interpolation // 注释：线性插值
if(Edge == EDGE_END)// 如果是末端过渡
{// if 块开始
DistA =1- Path->EndEdge.Radius / BlockLength;// 计算参数
DistB =1- Path->EndEdge.Radius / BlockLength /2.0;// 计算参数
}// if 块结束
else//EDGE_START // 否则是始端过渡
{// else 块开始
DistA = Path->StartEdge.Radius / BlockLength /2.0;// 计算参数
DistB = Path->StartEdge.Radius / BlockLength;// 计算参数
}// else 块结束
for(j=0;j<BEZIER_XYZ;j++)//note that only cartesian coordinates are included // 遍历XYZ坐标
{// for 循环体开始
CtrlPoints[0].Axes[j]=(1-DistA)* Path->StartPointPath[j]+ DistA * Path->TargetPointPath[j];// 线性插值计算控制点
CtrlPoints[1].Axes[j]=(1-DistB)* Path->StartPointPath[j]+ DistB * Path->TargetPointPath[j];// ...
}// for 循环体结束
break;// case 结束
case MOVE_CIRCLE:// 圆弧运动
if(Edge == EDGE_END)// 末端过渡
{// if 块开始
DistA =1- Path->EndEdge.Radius / BlockLength;// 计算参数
//find control point #0 // 注释：寻找控制点 #0
for(j=0;j<BEZIER_XYZ;j++)//note that only cartesian coordinates are included // 遍历XYZ坐标
{// for 循环体开始
//P = C + R cos(t) U + R sin(t) V // 注释：圆的参数方程
CtrlPoints[0].Axes[j]= Path->Center[j]+ Path->Radius *cos(DistA*BlockLength/Path->Radius)* Path->StartVersor[j]+ Path->Radius *sin(DistA*BlockLength/Path->Radius)* Path->CrossVersor[j];// 在圆弧上找到点
}// for 循环体结束
//find control point #1 along tangent at distance Path->EndEdge.Radius/2 // 注释：沿切线方向距离...处找到控制点#1
double tmpCrossVersor[3];// 临时向量
PointsToVector(Path->Center,CtrlPoints[0].Axes,tmpCrossVersor);// ...
double tmpEndVersor[3];// 临时向量
CrossProduct(Path->Normal,tmpCrossVersor,tmpEndVersor);// 计算切线方向
Normalize(tmpEndVersor);// 单位化
for(j=0;j<BEZIER_XYZ;j++)// 遍历XYZ坐标
{// for 循环体开始
CtrlPoints[1].Axes[j]= CtrlPoints[0].Axes[j]+ tmpEndVersor[j]* Path->EndEdge.Radius/2;// 沿切线方向移动得到另一个控制点
}// for 循环体结束
}// if 块结束
else//EDGE_START // 始端过渡
{// else 块开始
DistA = Path->StartEdge.Radius / BlockLength;// 计算参数
//find control point #4 // 注释：寻找控制点 #4
for(j=0;j<BEZIER_XYZ;j++)//note that only cartesian coordinates are included // 遍历XYZ坐标
{// for 循环体开始
//P = C + R cos(t) U + R sin(t) V // 注释：圆的参数方程
CtrlPoints[1].Axes[j]= Path->Center[j]+ Path->Radius *cos(DistA*BlockLength/Path->Radius)* Path->StartVersor[j]+ Path->Radius *sin(DistA*BlockLength/Path->Radius)* Path->CrossVersor[j];// 在圆弧上找到点
}// for 循环体结束
//find control point #3 along tangent at distance Path->EndEdge.Radius/2 // 注释：沿切线方向距离...处找到控制点#3
double tmpCrossVersor[3];// 临时向量
PointsToVector(Path->Center,CtrlPoints[1].Axes,tmpCrossVersor);// ...
double tmpEndVersor[3];// 临时向量
CrossProduct(Path->Normal,tmpCrossVersor,tmpEndVersor);// 计算切线方向
Normalize(tmpEndVersor);// 单位化
for(j=0;j<BEZIER_XYZ;j++)// 遍历XYZ坐标
{//negative because point #3 is closer to beginning of movement // 注释：负号因为#3点更靠近运动的开始
CtrlPoints[0].Axes[j]= CtrlPoints[1].Axes[j]- tmpEndVersor[j]* Path->StartEdge.Radius/2;// 沿切线反方向移动得到另一个控制点
}// for 循环体结束
}// else 块结束
break;// case 结束
case MOVE_PTP:// PTP运动
//first interpolate linearly in joints coords, then transform results into path coords // 注释：首先在关节坐标系中线性插值，然后将结果转换到路径坐标系
//note that for PTP movements some control points should also hold the orientation (#0 for end edge and #4 for start edge) // 注释：注意PTP运动的某些控制点也应保持姿态
if(Edge == EDGE_END)// 末端过渡
{// if 块开始
DistA =1- Path->EndEdge.Radius / BlockLength;// 计算参数
//find control point #5 (is #0 in joint world) // 注释：寻找控制点#5（在关节世界里是#0）
for(j=0;j<AxesNum;j++)// 关节空间线性插值
{// for 循环体开始
CtrlPoints[5].Axes[j]=(1-DistA)* Path->StartPointJoint[j]+ DistA * Path->TargetPointJoint[j];
}// for 循环体结束
//find control point #0 // 注释：寻找控制点#0
Transformations(Mechanics,TRF_DIRECT,CtrlPoints[5].Axes,Path->TargetPointPath,CtrlPoints[0].Axes);// 正运动学
//find control point #1 along tangent at distance Path->EndEdge.Radius/2 // 注释：沿切线方向...
DistA +=(1.0/Path->EndEdge.Radius/10.0);// 微调参数以求切线
if(DistA >1)// 限制
DistA =1;// ...
double tmpAxesValues[6];// 临时关节值
for(j=0;j<AxesNum;j++)// 关节空间插值
{// for 循环体开始
tmpAxesValues[j]=(1-DistA)* Path->StartPointJoint[j]+(DistA)* Path->TargetPointJoint[j];
}// for 循环体结束
double tmpPathValues[6];// 临时路径值
Transformations(Mechanics,TRF_DIRECT,tmpAxesValues,Path->TargetPointPath,tmpPathValues);// 正运动学
double tmpTangent[3];// 临时切线向量
for(j=0;j<BEZIER_XYZ;j++)// 计算切线
{// for 循环体开始
tmpTangent[j]= tmpPathValues[j]- CtrlPoints[0].Axes[j];
}// for 循环体结束
Normalize(tmpTangent);// 单位化切线
for(j=0;j<BEZIER_XYZ;j++)// 计算控制点#1
{// for 循环体开始
CtrlPoints[1].Axes[j]= CtrlPoints[0].Axes[j]+ tmpTangent[j]* Path->EndEdge.Radius/2;
}// for 循环体结束
}// if 块结束
else//EDGE_START // 始端过渡
{// else 块开始
DistA = Path->StartEdge.Radius / BlockLength;// 计算参数
//find control point #6 (is #4 in joint world) // 注释：寻找控制点#6（在关节世界里是#4）
for(j=0;j<AxesNum;j++)// 关节空间插值
{// for 循环体开始
CtrlPoints[3].Axes[j]=(1-DistA)* Path->StartPointJoint[j]+ DistA * Path->TargetPointJoint[j];
}// for 循环体结束
//find control point #4 // 注释：寻找控制点#4
Transformations(Mechanics,TRF_DIRECT,CtrlPoints[3].Axes,Path->StartPointPath,CtrlPoints[1].Axes);// 正运动学
//find control point #3 along tangent at distance Path->StartEdge.Radius/2 // 注释：沿切线方向...
DistA -=(1.0/Path->StartEdge.Radius/10.0);// 微调参数
if(DistA <0)// 限制
DistA =0;// ...
double tmpAxesValues[6];// 临时关节值
for(j=0;j<AxesNum;j++)// 关节空间插值
{// for 循环体开始
tmpAxesValues[j]=(1-DistA)* Path->StartPointJoint[j]+ DistA * Path->TargetPointJoint[j];
}// for 循环体结束
double tmpPathValues[6];// 临时路径值
Transformations(Mechanics,TRF_DIRECT,tmpAxesValues,Path->StartPointPath,tmpPathValues);// 正运动学
double tmpTangent[3];// 临时切线向量
for(j=0;j<BEZIER_XYZ;j++)// 计算切线
{// for 循环体开始
tmpTangent[j]= tmpPathValues[j]- CtrlPoints[1].Axes[j];
}// for 循环体结束
Normalize(tmpTangent);// 单位化
for(j=0;j<BEZIER_XYZ;j++)// 计算控制点#3
{// for 循环体开始
CtrlPoints[0].Axes[j]= CtrlPoints[1].Axes[j]+ tmpTangent[j]* Path->StartEdge.Radius/2;
}// for 循环体结束
}// else 块结束
break;// case 结束
}// switch 块结束
return0;// 返回成功
}
unsignedshortRoundEdgePoints(MotionPackage_Type* Movement, MotionPackage_Type* MovementPrev,unsignedshort AxesNum, Mech_Type* Mechanics)// 定义函数：计算圆角过渡点
{// 函数体开始
int k;// 循环变量
//limit radius size to half of the block length // 注释：将半径大小限制为块长度的一半
Movement->Path.StartEdge.Radius =min(MovementPrev->Round,Movement->BlockLengthIdeal/2.0);// 当前运动的起始过渡半径
MovementPrev->Path.EndEdge.Radius =min(MovementPrev->Round,MovementPrev->BlockLengthIdeal/2.0);// 前一运动的末端过渡半径
//in case of circle also limit radius to half circumference (because BlockLengthIdeal could be many revolutions) // 注释：对于圆弧，还要将半径限制为半周长
if(Movement->MovementType == MOVE_CIRCLE)// 如果是圆弧
{// if 块开始
Movement->Path.StartEdge.Radius =min(Movement->Path.StartEdge.Radius,Movement->Path.Radius*PI);// 限制半径
}// if 块结束
if(MovementPrev->MovementType == MOVE_CIRCLE)// 如果前一个是圆弧
{// if 块开始
MovementPrev->Path.EndEdge.Radius =min(MovementPrev->Path.EndEdge.Radius,MovementPrev->Path.Radius*PI);// 限制半径
}// if 块结束
if(Movement->BlockLengthIdeal !=0&& MovementPrev->BlockLengthIdeal !=0&& MovementPrev->MovementType != MOVE_SPLINE)// 如果两个块长度都不为0且前一个不是样条
{//both blocks have non-zero length -> can apply round edge // 注释：两个块都有非零长度 -> 可以应用圆角过渡
//control points 0 and 1 (first half) // 注释：控制点0和1（前半部分）
ControlPoints(MovementPrev->MovementType,&MovementPrev->Path,MovementPrev->BlockLengthIdeal,&Movement->Path.StartEdge.CtrlPoint[0],EDGE_END,AxesNum,Mechanics);// 计算
//control point 2 (middle point) // 注释：控制点2（中点）
memcpy(Movement->Path.StartEdge.CtrlPoint[2].Axes, Movement->Path.StartPointPath,sizeof(Movement->Path.StartEdge.CtrlPoint[2].Axes));// 中点就是两段运动的连接点
//control points 3 and 4 (second half) // 注释：控制点3和4（后半部分）
ControlPoints(Movement->MovementType,&Movement->Path,Movement->BlockLengthIdeal,&Movement->Path.StartEdge.CtrlPoint[3],EDGE_START,AxesNum,Mechanics);// 计算
//calculate start edge length (only part included in current block) and previous block's end edge length (only part included in previous block) // 注释：计算起始过渡长度和前一运动的末端过渡长度
Movement->Path.StartEdge.Length =BezierLengthHalf2(Movement->Path.StartEdge.CtrlPoint,BEZIER_XYZ,BEZIER_QUARTIC);// 计算当前运动的起始过渡弧长
MovementPrev->Path.EndEdge.Length =BezierLengthHalf1(Movement->Path.StartEdge.CtrlPoint,BEZIER_XYZ,BEZIER_QUARTIC);// 计算前一运动的末端过渡弧长
// adjust length of previous and current block considering start edge // 注释：考虑起始过渡调整前一和当前块的长度
//NOTE: Edge Radius and Length are in mm, but blocklength could be in deg (e.g. PTP or ML/MC with FA programmed) // 注释：注意，过渡半径和长度是毫米，但块长度可能是度
//solution: use percentage change over full block length // 注释：解决方案：使用基于总块长度的百分比变化
Movement->BlockLength +=((Movement->Path.StartEdge.Length - Movement->Path.StartEdge.Radius)/Movement->BlockLengthIdeal * Movement->BlockLength);// 调整当前块长度
MovementPrev->BlockLength +=((MovementPrev->Path.EndEdge.Length - MovementPrev->Path.EndEdge.Radius)/MovementPrev->BlockLengthIdeal * MovementPrev->BlockLength);// 调整前一块长度
}// if 块结束
else//there is no round edge -> all points are coincident at transition // 注释：没有圆角过渡 -> 所有点在过渡处重合
{
for(k=0;k<5;k++)// 循环
{// for 循环体开始
memcpy(Movement->Path.StartEdge.CtrlPoint[k].Axes,Movement->Path.StartPointPath,sizeof(Movement->Path.StartEdge.CtrlPoint[k].Axes));// 将所有控制点设为连接点
}// for 循环体结束
for(k=5;k<7;k++)// 循环
{// for 循环体开始
memcpy(Movement->Path.StartEdge.CtrlPoint[k].Axes,Movement->Path.StartPointJoint,sizeof(Movement->Path.StartEdge.CtrlPoint[k].Axes));// ...
}// for 循环体结束
Movement->Path.StartEdge.Length =0;// 长度为0
Movement->Path.StartEdge.Radius =0;// 半径为0
MovementPrev->Path.EndEdge.Length =0;// 长度为0
MovementPrev->Path.EndEdge.Radius =0;// 半径为0
}// else 块结束
//copy points to previous block // 注释：将点复制到前一个块
memcpy(MovementPrev->Path.EndEdge.CtrlPoint,Movement->Path.StartEdge.CtrlPoint,sizeof(Frame_Type)*7);// 确保前一运动的末端过渡和当前运动的起始过渡信息一致
return0;// 返回成功
}
doublePTPLength(Path_Type* Path,unsignedshort AxesNum, Mech_Type* Mechanics)// 定义函数：计算PTP运动的近似笛卡尔长度
{//calculates approx. cartesian length of PTP movement (required when adding round edge to PTP) // 函数体开始，注释：...
Frame_Type Point[2];// 存储连续点的数组
int i,j;// 循环变量
int k =10;//increase number of points for more accuracy // 细分点数
double Length =0;// 初始化长度为0
memcpy(Point[0].Axes,Path->StartPointPath,sizeof(Point[0].Axes));// 起点
//evaluate the BezierCurve at different points and then add the distances between them // 注释：...
for(i=1;i<=k;i++)// 循环细分
{// for 循环体开始
double tmpAxesValues[6];// 临时关节值
//interpolate PTP to find next point // 注释：插值PTP以找到下一个点
for(j=0;j<AxesNum;j++)// 关节空间线性插值
{// for 循环体开始
tmpAxesValues[j]=(1-(double)i/(double)k)* Path->StartPointJoint[j]+(double)i/(double)k * Path->TargetPointJoint[j];
}// for 循环体结束
//transform in path coords. (assuming direct TRF never fail...) // 注释：转换到路径坐标系（假设正解永远不会失败...）
Transformations(Mechanics,TRF_DIRECT,tmpAxesValues,Point[0].Axes,Point[1].Axes);// 正运动学
//calculate segment length // 注释：计算段长度
Length +=LineLength(Point[0].Axes,Point[1].Axes,BEZIER_XYZ);// 累加笛卡尔距离
Point[0]= Point[1];// 更新当前点
}// for 循环体结束
return Length;// 返回总长度
}Robots.h Robots.c
Robots.h 文件
#ifndefROBOTS_H // 如果宏 ROBOTS_H 未被定义
#defineROBOTS_H// 则定义宏 ROBOTS_H，这是标准的头文件保护宏，防止头文件被重复包含
#include<math.h>// 包含标准数学库头文件
#include"RobControl.h"// 包含机器人控制相关的头文件，定义了如 Link_Type 等数据结构
#include"trig.h"// 包含三角函数相关的头文件
#include"Frame.h"// 包含坐标系相关的头文件
#include"Misc.h"// 包含杂项功能和数据类型的头文件
#include"constants.h"// 包含项目自定义的常量定义头文件
/* Declaration of direct and inverse transformations for all kinds of supported robots */// 注释块：声明所有支持的机器人类型的正解和逆解变换函数
//6-Axis Robot // 注释：6轴机器人
unsignedshortArmDirect(Link_Type Links[6],double JointAxes[6],double PathAxes[6],double Axes[6]);// 声明6轴机器人正运动学函数，输入连杆参数和关节角，输出笛卡尔坐标
unsignedshortArmInverse(Link_Type Links[6],double PathAxes[6],double JointAxes[6],double Axes[6]);// 声明6轴机器人逆运动学函数，输入连杆参数和笛卡尔坐标，输出关节角
//5-Axis RTCP // 注释：5轴 RTCP（远程刀具中心点）机器人
unsignedshortRTCP_Direct(Link_Type Links[6],double JointAxes[6],double PathAxes[6],double Axes[6]);// 声明5轴RTCP机器人正运动学函数
unsignedshortRTCP_Inverse(Link_Type Links[6],double PathAxes[6],double JointAxes[6],double Axes[6]);// 声明5轴RTCP机器人逆运动学函数
//Scara Robot // 注释：Scara机器人
unsignedshortScaraDirect(Link_Type Links[6],double JointAxes[6],double PathAxes[6],double Axes[6]);// 声明Scara机器人正运动学函数
unsignedshortScaraInverse(Link_Type Links[6],double PathAxes[6],double JointAxes[6],double Axes[6]);// 声明Scara机器人逆运动学函数
//Delta Robot // 注释：Delta（并联）机器人
unsignedshortDeltaDirect(Link_Type Links[6],double JointAxes[6],double PathAxes[6],double Axes[6]);// 声明Delta机器人正运动学函数
unsignedshortDeltaInverse(Link_Type Links[6],double PathAxes[6],double JointAxes[6],double Axes[6]);// 声明Delta机器人逆运动学函数
//Pallet Robot // 注释：码垛机器人
unsignedshortPalletDirect(Link_Type Links[6],double JointAxes[6],double PathAxes[6],double Axes[6]);// 声明码垛机器人正运动学函数
unsignedshortPalletInverse(Link_Type Links[6],double PathAxes[6],double JointAxes[6],double Axes[6]);// 声明码垛机器人逆运动学函数
#endif// 结束 #ifndef ROBOTS_H 的宏定义条件块Robots.c 文件
#include"Robots.h"// 包含此源文件对应的头文件 "Robots.h"
#include<math.h>// 包含标准数学库头文件
#include"trig.h"// 包含三角函数相关的头文件，可能定义了以角度为单位的三角函数
#include"RobControl.h"// 包含机器人控制相关的头文件
#include"Frame.h"// 包含坐标系相关的头文件
#include"Misc.h"// 包含杂项功能和数据类型的头文件
#include"constants.h"// 包含项目自定义的常量定义头文件
unsignedshortArmDirect(Link_Type Links[6],double JointAxes[6],double PathAxes[6],double Axes[6])// 定义6轴机器人正运动学函数
{//direct transformations for 6ax robot // 函数体开始，注释：6轴机器人的正解变换
//simplify notation // 注释：简化符号
double Q1 = JointAxes[0];// 将关节角数组中的值赋给局部变量以方便使用
double Q2 = JointAxes[1];// 将关节角数组中的值赋给局部变量
double Q3 = JointAxes[2];// 将关节角数组中的值赋给局部变量
double Q4 = JointAxes[3];// 将关节角数组中的值赋给局部变量
double Q5 = JointAxes[4];// 将关节角数组中的值赋给局部变量
double Q6 = JointAxes[5];// 将关节角数组中的值赋给局部变量
double a1x = Links[1].Offset.X;// 获取连杆1的X方向偏移量（DH参数 a_i）
//double a1y = Links[1].Offset.Y; // (被注释的代码) 获取连杆1的Y方向偏移量
double a1z = Links[1].Offset.Z;// 获取连杆1的Z方向偏移量（DH参数 d_i）
//double a2x = Links[2].Offset.X; // (被注释的代码) 获取连杆2的X方向偏移量
//double a2y = Links[2].Offset.Y; // (被注释的代码) 获取连杆2的Y方向偏移量
double a2z = Links[2].Offset.Z;// 获取连杆2的Z方向偏移量
double a3x = Links[3].Offset.X;// 获取连杆3的X方向偏移量
//double a3y = Links[3].Offset.Y; // (被注释的代码) 获取连杆3的Y方向偏移量
double a3z = Links[3].Offset.Z;// 获取连杆3的Z方向偏移量
double a4x = Links[4].Offset.X;// 获取连杆4的X方向偏移量
double a5x = Links[5].Offset.X;// 获取连杆5的X方向偏移量
/* base offset from world frame */// 注释块：从世界坐标系到基坐标系的偏移
double ZeroFrame[6];// 声明一个数组用于存储基坐标系的位姿
ZeroFrame[0]= Links[0].Offset.X;// 获取基坐标系的X平移
ZeroFrame[1]= Links[0].Offset.Y;// 获取基坐标系的Y平移
ZeroFrame[2]= Links[0].Offset.Z;// 获取基坐标系的Z平移
ZeroFrame[3]= Links[0].Rotation.X;// 获取基坐标系的X旋转（欧拉角A）
ZeroFrame[4]= Links[0].Rotation.Y;// 获取基坐标系的Y旋转（欧拉角B）
ZeroFrame[5]= Links[0].Rotation.Z;// 获取基坐标系的Z旋转（欧拉角C）
//temporary axes values // 注释：临时轴坐标值
double tmpAxes[6];// 声明一个临时数组用于存放计算结果
/* check mechanical parameters consistency */// 注释块：检查机械参数的一致性
if((a1z <0)||(a2z <=0)||(a3z <0)||((a3x+a4x)<=0)||(a5x <=0)){// 检查连杆参数是否符合物理约束（例如长度为正）
return ERR_TRF_MECH;// 如果不符合，返回机械结构参数错误
}
double aaa =cosd(Q3)*(a3x+a4x+cosd(Q5)*a5x)+sind(Q3)*(a3z-cosd(Q4)*sind(Q5)*a5x);// 正运动学公式中的中间变量计算
double bbb =-sind(Q3)*(a3x+a4x+cosd(Q5)*a5x)+cosd(Q3)*(a3z-cosd(Q4)*sind(Q5)*a5x);// 正运动学公式中的中间变量计算
double ddd = a1x +cosd(Q2)*aaa +sind(Q2)*(bbb+a2z);// 正运动学公式中的中间变量计算
double fff = a1z -sind(Q2)*aaa +cosd(Q2)*(bbb+a2z);// 正运动学公式中的中间变量计算
/* X axis */// 注释：X轴坐标
tmpAxes[0]=cosd(Q1)*ddd -sind(Q1)*sind(Q4)*sind(Q5)*a5x;// 根据正运动学公式计算TCP的X坐标
/* Y axis */// 注释：Y轴坐标
tmpAxes[1]=sind(Q1)*ddd +cosd(Q1)*sind(Q4)*sind(Q5)*a5x;// 根据正运动学公式计算TCP的Y坐标
/* Z axis */// 注释：Z轴坐标
tmpAxes[2]= fff;// 根据正运动学公式计算TCP的Z坐标
/* compose R_total from all joints rotations */// 注释块：根据所有关节旋转构建总旋转矩阵 R_total
double R_total[3][3];// 声明一个3x3矩阵用于存放总旋转矩阵
double temp01 =-sind(Q1)*cosd(Q4)+cosd(Q1)*sind(Q2+Q3)*sind(Q4);// 旋转矩阵计算的中间变量
double temp02 =cosd(Q1)*cosd(Q2+Q3)*sind(Q5)+cosd(Q5)*sind(Q1)*sind(Q4)+cosd(Q5)*cosd(Q1)*sind(Q2+Q3)*cosd(Q4);// 旋转矩阵计算的中间变量
double temp11 =cosd(Q1)*cosd(Q4)+sind(Q1)*sind(Q2+Q3)*sind(Q4);// 旋转矩阵计算的中间变量
double temp12 =sind(Q1)*cosd(Q2+Q3)*sind(Q5)-cosd(Q1)*sind(Q4)*cosd(Q5)+cosd(Q5)*sind(Q1)*sind(Q2+Q3)*cosd(Q4);// 旋转矩阵计算的中间变量
double temp21 =cosd(Q2+Q3)*sind(Q4);// 旋转矩阵计算的中间变量
double temp22 =-sind(Q2+Q3)*sind(Q5)+cosd(Q2+Q3)*cosd(Q4)*cosd(Q5);// 旋转矩阵计算的中间变量
R_total[0][0]=cosd(Q1)*cosd(Q2+Q3)*cosd(Q5)-sind(Q1)*sind(Q4)*sind(Q5)-cosd(Q1)*sind(Q2+Q3)*cosd(Q4)*sind(Q5);// 计算总旋转矩阵的元素
R_total[0][1]=cosd(Q6)*temp01 +sind(Q6)*temp02;// 计算总旋转矩阵的元素
R_total[0][2]=-sind(Q6)*temp01 +cosd(Q6)*temp02;// 计算总旋转矩阵的元素
R_total[1][0]=sind(Q1)*cosd(Q2+Q3)*cosd(Q5)+cosd(Q1)*sind(Q4)*sind(Q5)-sind(Q1)*sind(Q2+Q3)*cosd(Q4)*sind(Q5);// 计算总旋转矩阵的元素
R_total[1][1]=cosd(Q6)*temp11 +sind(Q6)*temp12;// 计算总旋转矩阵的元素
R_total[1][2]=-sind(Q6)*temp11 +cosd(Q6)*temp12;// 计算总旋转矩阵的元素
R_total[2][0]=-sind(Q2+Q3)*cosd(Q5)-cosd(Q2+Q3)*cosd(Q4)*sind(Q5);// 计算总旋转矩阵的元素
R_total[2][1]=cosd(Q6)*temp21 +sind(Q6)*temp22;// 计算总旋转矩阵的元素
R_total[2][2]=-sind(Q6)*temp21 +cosd(Q6)*temp22;// 计算总旋转矩阵的元素
/* A,B,C axis */// 注释：A,B,C轴（欧拉角）
double A,B,C;// 声明三个变量用于存放分解出的欧拉角
DecomposeMatrix(R_total,PathAxes[3],PathAxes[4],PathAxes[5],&A,&B,&C);// 调用函数将总旋转矩阵分解为欧拉角
tmpAxes[3]= A;// 将计算出的A角存入临时数组
tmpAxes[4]= B;// 将计算出的B角存入临时数组
tmpAxes[5]= C;// 将计算出的C角存入临时数组
/* consider zero offset (position and orientation of base point with respect to world origin) */// 注释块：考虑零点偏移（基点相对于世界原点的位置和方向）
SubFrame3D(tmpAxes,ZeroFrame,PathAxes[3],PathAxes[4],PathAxes[5],Axes);// 调用函数，将机器人基坐标系下的位姿变换到世界坐标系下
int i=0;// 声明循环变量
for(i=0;i<6;i++)// 遍历所有轴
{// for循环开始
Axes[i]=RoundToEpsilon(Axes[i]);// 对每个坐标值进行精度处理，避免极小的浮点误差
}// for循环结束
return STATUS_OK;// 函数执行成功，返回OK状态
}
unsignedshortArmInverse(Link_Type Links[6],double PathAxes[6],double JointAxes[6],double Axes[6])// 定义6轴机器人逆运动学函数
{//inverse transformations for 6ax robot // 函数体开始，注释：6轴机器人的逆解变换
/* base offset from world frame */// 注释块：从世界坐标系到基坐标系的偏移
double ZeroFrame[6];// 声明一个数组用于存储基坐标系的位姿
ZeroFrame[0]= Links[0].Offset.X;// 获取基坐标系的X平移
ZeroFrame[1]= Links[0].Offset.Y;// 获取基坐标系的Y平移
ZeroFrame[2]= Links[0].Offset.Z;// 获取基坐标系的Z平移
ZeroFrame[3]= Links[0].Rotation.X;// 获取基坐标系的X旋转（欧拉角A）
ZeroFrame[4]= Links[0].Rotation.Y;// 获取基坐标系的Y旋转（欧拉角B）
ZeroFrame[5]= Links[0].Rotation.Z;// 获取基坐标系的Z旋转（欧拉角C）
//temporary axes values // 注释：临时轴坐标值
double tmpAxes[6], WP[6];// 声明临时数组，tmpAxes用于变换后的位姿，WP用于手腕点坐标
/* consider zero offset (position and orientation of base point with respect to world origin) */// 注释块：考虑零点偏移
AddFrame3D(PathAxes,ZeroFrame,PathAxes[3],PathAxes[4],PathAxes[5],tmpAxes);// 调用函数，将世界坐标系下的位姿变换到机器人基坐标系下
//simplify notation // 注释：简化符号
double X = tmpAxes[0];// 将变换后的笛卡尔坐标赋给局部变量
double Y = tmpAxes[1];// 将变换后的笛卡尔坐标赋给局部变量
double Z = tmpAxes[2];// 将变换后的笛卡尔坐标赋给局部变量
double A = tmpAxes[3];// 将变换后的欧拉角赋给局部变量
double B = tmpAxes[4];// 将变换后的欧拉角赋给局部变量
double C = tmpAxes[5];// 将变换后的欧拉角赋给局部变量
double a1x = Links[1].Offset.X;// 获取连杆参数
//double a1y = Links[1].Offset.Y; // (被注释的代码)
double a1z = Links[1].Offset.Z;// 获取连杆参数
//double a2x = Links[2].Offset.X; // (被注释的代码)
//double a2y = Links[2].Offset.Y; // (被注释的代码)
double a2z = Links[2].Offset.Z;// 获取连杆参数
double a3x = Links[3].Offset.X;// 获取连杆参数
//double a3y = Links[3].Offset.Y; // (被注释的代码)
double a3z = Links[3].Offset.Z;// 获取连杆参数
double a4x = Links[4].Offset.X;// 获取连杆参数
double a5x = Links[5].Offset.X;// 获取连杆参数
//keep same pose as current joints configuration // 注释：保持与当前关节配置相同的姿态
short Pose =0;// 声明一个短整型变量Pose，用于存储机器人的姿态配置（如肘部向上/向下）
if(JointAxes[2]<-90){// 根据当前Q3关节角判断肘部姿态
Pose |= TRF_POSE_CONCAVE;// 设置为凹形姿态（肘部向下）
}else{// 否则
Pose |= TRF_POSE_CONVEX;// 设置为凸形姿态（肘部向上）
}
if(JointAxes[4]<0){// 根据当前Q5关节角判断手腕姿态
Pose |= TRF_POSE_NEGATIVE;// 设置为负向姿态
}else{// 否则
Pose |= TRF_POSE_POSITIVE;// 设置为正向姿态
}
/* check mechanical parameters consistency */// 注释块：检查机械参数的一致性
if((a1z <0)||(a2z <=0)||(a3z <0)||((a3x+a4x)<=0)||(a5x <=0)){// 检查连杆参数是否符合物理约束
return ERR_TRF_MECH;// 如果不符合，返回机械结构参数错误
}
/* compose rotation matrix */// 注释块：构建旋转矩阵
double R_total[3][3];// 声明一个3x3矩阵用于存放目标姿态的旋转矩阵
ComposeMatrix(R_total,A,B,C);// 根据目标欧拉角构建旋转矩阵
/* calculate wrist point WP from mounting point MP */// 注释块：从法兰盘中心点MP计算手腕中心点WP
WP[0]= X - a5x * R_total[0][0];// WP = MP - a5 * R_total * [1,0,0]^T (这里只用了第一列)
WP[1]= Y - a5x * R_total[1][0];// 计算手腕点的X, Y, Z坐标
WP[2]= Z - a5x * R_total[2][0];// ...
/* calculate Q1 */// 注释块：计算Q1
/* check for singularity */// 注释块：检查奇异点
if((fabs(WP[0])< TRF_EPSILON)&&(fabs(WP[1])< TRF_EPSILON))// 如果手腕点在Z轴上（奇异点）
{// if块开始
Axes[0]= JointAxes[0];// 保持当前Q1不变
}// if块结束
else// 否则
{// else块开始
Axes[0]=atan2d(WP[1],WP[0]);// 使用atan2计算Q1
}// else块结束
/* adjust positions of Q1 with +-PI to bring it closer to desired values */// 注释块：调整Q1的位置（+/-180度）使其更接近期望值
Axes[0]=ModuloPI(Axes[0],JointAxes[0]);// 调用函数将Q1调整到以当前Q1为中心的180度范围内
/* consider axis 3 shoulder */// 注释块：考虑轴3的肩部
double b3x =sqrt((a3x+a4x)*(a3x+a4x)+a3z*a3z);// 计算等效的连杆长度
/* calculate length and height of triangle formed by a2z and b3x */// 注释块：计算由a2z和b3x构成的三角形的长度和高度
double height = WP[2]- a1z;// 计算三角形的高度
double length =sqrt(WP[0]*WP[0]+ WP[1]*WP[1]);// 计算三角形的底边长度
//flip sign of length if Q1 was chosen 180 deg away from atan(Y,X) // 注释：如果Q1被选为与atan(Y,X)相差180度的解，则翻转长度的符号
if(fabs(Modulo2PI(Axes[0]-atan2d(WP[1],WP[0]),0))>90)// 检查Q1解的选择
{// if块开始
length =-length;// 翻转符号
}// if块结束
//add a1x correction // 注释：添加a1x修正
length -= a1x;// 修正底边长度
double rho =sqrt(length*length + height*height);// 计算从关节2到手腕点的距离rho
//check for workspace violations // 注释：检查工作空间违规
if((rho >(a2z + b3x + TRF_EPSILON))||(rho <(fabs(a2z-b3x)- TRF_EPSILON)))// 如果rho超出了连杆a2z和b3x能构成的范围
{// if块开始
return ERR_TRF_WORKSPACE;// 返回工作空间错误
}// if块结束
elseif(rho >(a2z + b3x))//adjust impreciseness // 如果因数值误差略微超出
{// else if块开始
rho =(a2z + b3x);// 钳位到最大值
}// else if块结束
elseif(rho <fabs(a2z-b3x))//adjust impreciseness // 如果因数值误差略微小于
{// else if块开始
rho =fabs(a2z-b3x);// 钳位到最小值
}// else if块结束
double alpha =atan2d(height,length);// 使用反切函数计算角度alpha
double cos_beta =(rho*rho + a2z*a2z - b3x*b3x)/(2.0*a2z*rho);// 使用余弦定理计算beta角的余弦值
double beta =atan2d(sqrt(1-cos_beta*cos_beta),cos_beta);// 计算beta角
double cos_gamma =(a2z*a2z + b3x*b3x - rho*rho)/(2.0*a2z*b3x);// 使用余弦定理计算gamma角的余弦值
double gamma =180.0-atan2d(sqrt(1-cos_gamma*cos_gamma),cos_gamma);// 计算gamma角
if(Pose & TRF_POSE_CONCAVE)// 如果是凹形（肘部向下）姿态
{// if块开始
Axes[1]=90.0- alpha + beta;// 计算Q2
Axes[2]=- gamma -atan2d(a3x+a4x,a3z);// 计算Q3
}// if块结束
else// 否则（凸形姿态）
{// else块开始
Axes[1]=90.0- alpha - beta;// 计算Q2
Axes[2]= gamma -atan2d(a3x+a4x,a3z);// 计算Q3
}// else块结束
int i=0;// 声明循环变量
for(i=0;i<3;i++)// 遍历前三个关节
{// for循环开始
Axes[i]=RoundToEpsilon(Axes[i]);// 对计算出的关节角进行精度处理
}// for循环结束
/* compute wrist rotation matrix */// 注释块：计算手腕旋转矩阵
/* R_wrist = (R_arm)^T * R_total*/// 注释：手腕旋转矩阵 = (手臂旋转矩阵的转置) * (总旋转矩阵)
double R_arm[3][3];// 声明手臂旋转矩阵
double R_wrist[3][3];// 声明手腕旋转矩阵
//R_arm = Rz(Q1) * Ry(Q2+Q3) // 注释：手臂旋转矩阵由Q1和Q2+Q3决定
double Qy = Axes[1]+ Axes[2];// 计算Q2+Q3
double Qz = Axes[0];// Q1
R_arm[0][0]=cosd(Qz)*cosd(Qy);// 计算手臂旋转矩阵的元素
R_arm[0][1]=-sind(Qz);// ...
R_arm[0][2]=sind(Qy)*cosd(Qz);// ...
R_arm[1][0]=cosd(Qy)*sind(Qz);// ...
R_arm[1][1]=cosd(Qz);// ...
R_arm[1][2]=sind(Qy)*sind(Qz);// ...
R_arm[2][0]=-sind(Qy);// ...
R_arm[2][1]=0;// ...
R_arm[2][2]=cosd(Qy);// ...
//transpose R_arm // 注释：转置手臂旋转矩阵
double tmpR;// 临时变量用于交换
tmpR = R_arm[0][1];// 交换元素
R_arm[0][1]= R_arm[1][0];// ...
R_arm[1][0]= tmpR;// ...
tmpR = R_arm[0][2];// 交换元素
R_arm[0][2]= R_arm[2][0];// ...
R_arm[2][0]= tmpR;// ...
tmpR = R_arm[1][2];// 交换元素
R_arm[1][2]= R_arm[2][1];// ...
R_arm[2][1]= tmpR;// ...
MatMult(R_arm,R_total,R_wrist);// 矩阵乘法，计算出手腕旋转矩阵
/* extract Q4,Q5,Q6 from wrist rotation matrix as XYX Euler angles */// 注释块：从手腕旋转矩阵中提取Q4,Q5,Q6作为XYX欧拉角
/* note that this angle type is not the same as the one used in the decompose matrix function in the frame.h file */// 注释：注意此角度类型与frame.h文件中的分解矩阵函数使用的不同
double A_temp[2],B_temp[2],C_temp[2],ABC_dist[2];// 声明数组用于存放两组可能的欧拉角解及其距离
double A_actual = JointAxes[3];// 获取当前Q4作为参考
double B_actual = JointAxes[4];// 获取当前Q5作为参考
double C_actual = JointAxes[5];// 获取当前Q6作为参考
B_temp[0]=atan2d(sqrt(1-R_wrist[0][0]*R_wrist[0][0]), R_wrist[0][0]);// 计算Q5的第一个可能解
B_temp[1]=atan2d(-sqrt(1-R_wrist[0][0]*R_wrist[0][0]), R_wrist[0][0]);// 计算Q5的第二个可能解
if(fabs(B_temp[0])>TRF_EPSILON)// 检查是否处于奇异点
{// if块开始
C_temp[0]=atan2d(R_wrist[0][1],R_wrist[0][2]);// 计算Q6的第一个解
C_temp[1]=atan2d(-R_wrist[0][1],-R_wrist[0][2]);// 计算Q6的第二个解
A_temp[0]=atan2d(R_wrist[1][0],-R_wrist[2][0]);// 计算Q4的第一个解
A_temp[1]=atan2d(-R_wrist[1][0],R_wrist[2][0]);// 计算Q4的第二个解
}// if块结束
else// 否则（奇异点）
{//singularity - choose A=currentQ4 // 注释：奇异点 - 选择A=当前Q4
A_temp[0]= A_temp[1]= A_actual;// 将Q4固定为当前值
C_temp[0]= C_temp[1]=atan2d(-R_wrist[1][2],R_wrist[2][2])- A_actual;// 计算Q6
}// else块结束
//A, C modulo +-2PI to bring them closer to current values // 注释：对A和C进行模2PI运算，使其更接近当前值
A_temp[0]=Modulo2PI(A_temp[0],JointAxes[3]);// 调整Q4的解
A_temp[1]=Modulo2PI(A_temp[1],JointAxes[3]);// 调整Q4的解
C_temp[0]=Modulo2PI(C_temp[0],JointAxes[5]);// 调整Q6的解
C_temp[1]=Modulo2PI(C_temp[1],JointAxes[5]);// 调整Q6的解
//calculate distance of the two solutions from actual values // 注释：计算两组解与当前值的距离
ABC_dist[0]=fabs(A_temp[0]-A_actual)+fabs(B_temp[0]-B_actual)+fabs(C_temp[0]-C_actual);// 计算第一组解的距离
ABC_dist[1]=fabs(A_temp[1]-A_actual)+fabs(B_temp[1]-B_actual)+fabs(C_temp[1]-C_actual);// 计算第二组解的距离
//keep same pose for wrist // 注释：保持手腕姿态一致
if(Pose & TRF_POSE_NEGATIVE)// 如果要求负向Q5姿态
{//use solution with negative Q5 // 注释：使用Q5为负的解
if((B_temp[0]<0)&&(B_temp[1]>=0))// 如果解0的Q5为负，解1为正
{//use B_temp[0] // 注释：使用B_temp[0]
Axes[3]= A_temp[0];// 选择第一组解
Axes[4]= B_temp[0];// ...
Axes[5]= C_temp[0];// ...
}
elseif((B_temp[1]<0)&&(B_temp[0]>=0))// 如果解1的Q5为负，解0为正
{//use B_temp[1] // 注释：使用B_temp[1]
Axes[3]= A_temp[1];// 选择第二组解
Axes[4]= B_temp[1];// ...
Axes[5]= C_temp[1];// ...
}
else// 否则
{//use closest solution to current values // 注释：使用离当前值最近的解
if(ABC_dist[0]<= ABC_dist[1])// 比较距离
{// if块开始
Axes[3]= A_temp[0];// 选择距离更近的解
Axes[4]= B_temp[0];// ...
Axes[5]= C_temp[0];// ...
}// if块结束
else// 否则
{// else块开始
Axes[3]= A_temp[1];// 选择距离更近的解
Axes[4]= B_temp[1];// ...
Axes[5]= C_temp[1];// ...
}// else块结束
}
}
else// 否则（要求正向Q5姿态）
{//use solution with positive Q5 // 注释：使用Q5为正的解
if((B_temp[0]>=0)&&(B_temp[1]<0))// 如果解0的Q5为正，解1为负
{//use B_temp[0] // 注释：使用B_temp[0]
Axes[3]= A_temp[0];// 选择第一组解
Axes[4]= B_temp[0];// ...
Axes[5]= C_temp[0];// ...
}
elseif((B_temp[1]>=0)&&(B_temp[0]<0))// 如果解1的Q5为正，解0为负
{//use B_temp[1] // 注释：使用B_temp[1]
Axes[3]= A_temp[1];// 选择第二组解
Axes[4]= B_temp[1];// ...
Axes[5]= C_temp[1];// ...
}
else// 否则
{//use closest solution to current values // 注释：使用离当前值最近的解
if(ABC_dist[0]<= ABC_dist[1])// 比较距离
{// if块开始
Axes[3]= A_temp[0];// 选择距离更近的解
Axes[4]= B_temp[0];// ...
Axes[5]= C_temp[0];// ...
}// if块结束
else// 否则
{// else块开始
Axes[3]= A_temp[1];// 选择距离更近的解
Axes[4]= B_temp[1];// ...
Axes[5]= C_temp[1];// ...
}// else块结束
}
}
//adjust positions of Q6 with +-2PI to bring it closer to desired value // 注释：调整Q6的位置（+/-360度）使其更接近期望值
Axes[3]=Modulo2PI(Axes[3],JointAxes[3]);// 调整Q4
Axes[5]=Modulo2PI(Axes[5],JointAxes[5]);// 调整Q6
for(i=3;i<6;i++)// 遍历后三个关节
{// for循环开始
Axes[i]=RoundToEpsilon(Axes[i]);// 对计算出的关节角进行精度处理
}// for循环结束
return STATUS_OK;// 函数执行成功，返回OK状态
}
unsignedshortRTCP_Direct(Link_Type Links[6],double JointAxes[6],double PathAxes[6],double Axes[6])// 定义5轴RTCP机器人正运动学函数
{// 函数体开始
//simplify notation // 注释：简化符号
double Q1 = JointAxes[0];// 获取关节角
double Q2 = JointAxes[1];// ...
double Q3 = JointAxes[2];// ...
double Q4 = JointAxes[3];// ...
double Q5 = JointAxes[4];// ...
double a1x = Links[1].Offset.X;// 获取连杆参数
double a1y = Links[1].Offset.Y;// ...
double a1z = Links[1].Offset.Z;// ...
double a2x = Links[2].Offset.X;// ...
double a2y = Links[2].Offset.Y;// ...
double a2z = Links[2].Offset.Z;// ...
double a3x = Links[3].Offset.X;// ...
double a3y = Links[3].Offset.Y;// ...
double a3z = Links[3].Offset.Z;// ...
double r2x = Links[2].Rotation.X;// 获取旋转轴定义
double r2y = Links[2].Rotation.Y;// ...
double r2z = Links[2].Rotation.Z;// ...
double r3x = Links[3].Rotation.X;// ...
double r3y = Links[3].Rotation.Y;// ...
double r3z = Links[3].Rotation.Z;// ...
/* base offset from world frame */// 注释块：从世界坐标系到基坐标系的偏移
double ZeroFrame[6];// 声明基坐标系位姿数组
ZeroFrame[0]= Links[0].Offset.X;// 获取
ZeroFrame[1]= Links[0].Offset.Y;// ...
ZeroFrame[2]= Links[0].Offset.Z;// ...
ZeroFrame[3]= Links[0].Rotation.X;// ...
ZeroFrame[4]= Links[0].Rotation.Y;// ...
ZeroFrame[5]= Links[0].Rotation.Z;// ...
//temporary axes values // 注释：临时轴坐标值
double tmpAxes[6];// 声明临时数组
int i,j;// 声明循环变量
/* check mechanical parameters consistency */// 注释块：检查机械参数的一致性
if(r2x+r2y+r2z !=1|| r3x+r3y+r3z !=1)// 检查旋转轴定义是否为单位向量
{// if块开始
return ERR_TRF_ROT;// 返回旋转错误
}// if块结束
double A1,B1,C1;// 声明旋转角
if(r2x ==1){// 根据旋转轴定义分配关节角
A1 = Q4; B1 =0; C1 =0;
}elseif(r2y ==1){
A1 =0; B1 = Q4; C1 =0;
}elseif(r2z ==1){
A1 =0; B1 =0; C1 = Q4;
}
double A2,B2,C2;// 声明旋转角
if(r3x ==1){// 根据旋转轴定义分配关节角
A2 = Q5; B2 =0; C2 =0;
}elseif(r3y ==1){
A2 =0; B2 = Q5; C2 =0;
}elseif(r3z ==1){
A2 =0; B2 =0; C2 = Q5;
}
/* build homogeneous matrices */// 注释块：构建齐次变换矩阵
double P2[4];// 点P2的齐次坐标
double H1[4][4];// 变换矩阵H1
double H2[4][4];// 变换矩阵H2
P2[0]= a3x;// P2的X坐标
P2[1]= a3y;// P2的Y坐标
P2[2]= a3z;// P2的Z坐标
P2[3]=1;// 齐次坐标分量
H2[0][3]= a2x;// H2的平移部分
H2[1][3]= a2y;// ...
H2[2][3]= a2z;// ...
H2[3][3]=1;// ...
H1[0][3]= a1x + Q1;// H1的平移部分
H1[1][3]= a1y + Q2;// ...
H1[2][3]= a1z + Q3;// ...
H1[3][3]=1;// ...
for(i=0;i<3;i++){// 初始化齐次矩阵的最后一行
H1[3][i]=0;
H2[3][i]=0;
}
double R[3][3];// 临时旋转矩阵
ComposeMatrix(R, A1, B1, C1);// 构建旋转矩阵
for(i=0;i<3;i++){// 填充到H1
for(j=0;j<3;j++){
H1[i][j]= R[i][j];
}
}
ComposeMatrix(R, A2, B2, C2);// 构建旋转矩阵
for(i=0;i<3;i++){// 填充到H2
for(j=0;j<3;j++){
H2[i][j]= R[i][j];
}
}
double P1[4];// 点P1的齐次坐标
/* P1 = H2xP2 */// 注释：计算P1
for(i=0;i<4;i++){// 矩阵向量乘法
P1[i]=0;
for(j=0;j<4;j++){
P1[i]+= H2[i][j]* P2[j];
}
}
double P0[4];// 点P0的齐次坐标
/* P0 = H1xP1 */// 注释：计算P0
for(i=0;i<4;i++){// 矩阵向量乘法
P0[i]=0;
for(j=0;j<4;j++){
P0[i]+= H1[i][j]* P1[j];
}
}
/* X axis */// 注释：X轴坐标
tmpAxes[0]= P0[0];// 提取X坐标
/* Y axis */// 注释：Y轴坐标
tmpAxes[1]= P0[1];// 提取Y坐标
/* Z axis */// 注释：Z轴坐标
tmpAxes[2]= P0[2];// 提取Z坐标
/* A,B,C axis */// 注释：A,B,C轴
tmpAxes[3]= A1+A2;// 欧拉角相加
tmpAxes[4]= B1+B2;// ...
tmpAxes[5]= C1+C2;// ...
/* consider zero offset (position and orientation of base point with respect to world origin) */// 注释块：考虑零点偏移
SubFrame3D(tmpAxes,ZeroFrame,PathAxes[3],PathAxes[4],PathAxes[5],Axes);// 变换到世界坐标系
for(i=0;i<6;i++){// 遍历
Axes[i]=RoundToEpsilon(Axes[i]);// 精度处理
}
return STATUS_OK;// 返回成功
}
unsignedshortRTCP_Inverse(Link_Type Links[6],double PathAxes[6],double JointAxes[6],double Axes[6])// 定义5轴RTCP机器人逆运动学函数
{// 函数体开始
/* base offset from world frame */// 注释块：从世界坐标系到基坐标系的偏移
double ZeroFrame[6];// 声明基坐标系位姿数组
ZeroFrame[0]= Links[0].Offset.X;// 获取
ZeroFrame[1]= Links[0].Offset.Y;// ...
ZeroFrame[2]= Links[0].Offset.Z;// ...
ZeroFrame[3]= Links[0].Rotation.X;// ...
ZeroFrame[4]= Links[0].Rotation.Y;// ...
ZeroFrame[5]= Links[0].Rotation.Z;// ...
//temporary axes values // 注释：临时轴坐标值
double tmpAxes[6];// 声明临时数组
/* consider zero offset (position and orientation of base point with respect to world origin) */// 注释块：考虑零点偏移
AddFrame3D(PathAxes,ZeroFrame,PathAxes[3],PathAxes[4],PathAxes[5],tmpAxes);// 变换到机器人基坐标系
//simplify notation // 注释：简化符号
double X = tmpAxes[0];// 获取坐标
double Y = tmpAxes[1];// ...
double Z = tmpAxes[2];// ...
double A = tmpAxes[3];// ...
double B = tmpAxes[4];// ...
double C = tmpAxes[5];// ...
double a1x = Links[1].Offset.X;// 获取连杆参数
double a1y = Links[1].Offset.Y;// ...
double a1z = Links[1].Offset.Z;// ...
double a2x = Links[2].Offset.X;// ...
double a2y = Links[2].Offset.Y;// ...
double a2z = Links[2].Offset.Z;// ...
double a3x = Links[3].Offset.X;// ...
double a3y = Links[3].Offset.Y;// ...
double a3z = Links[3].Offset.Z;// ...
double r2x = Links[2].Rotation.X;// 获取旋转轴定义
double r2y = Links[2].Rotation.Y;// ...
double r2z = Links[2].Rotation.Z;// ...
double r3x = Links[3].Rotation.X;// ...
double r3y = Links[3].Rotation.Y;// ...
double r3z = Links[3].Rotation.Z;// ...
int i,j;// 声明循环变量
/* check mechanical parameters consistency */// 注释块：检查机械参数的一致性
if(r2x+r2y+r2z !=1|| r3x+r3y+r3z !=1)// 检查旋转轴定义是否为单位向量
{// if块开始
return ERR_TRF_ROT;// 返回旋转错误
}// if块结束
double A1,B1,C1;// 声明旋转角
if(r2x ==1){// 根据旋转轴定义分配欧拉角
A1 = A; B1 =0; C1 =0;
}elseif(r2y ==1){
A1 =0; B1 = B; C1 =0;
}elseif(r2z ==1){
A1 =0; B1 =0; C1 = C;
}
double A2,B2,C2;// 声明旋转角
if(r3x ==1){// 根据旋转轴定义分配欧拉角
A2 = A; B2 =0; C2 =0;
}elseif(r3y ==1){
A2 =0; B2 = B; C2 =0;
}elseif(r3z ==1){
A2 =0; B2 =0; C2 = C;
}
/* compose rotation matrices */// 注释块：构建旋转矩阵
double R1[3][3];// 旋转矩阵R1
ComposeMatrix(R1, A1, B1, C1);// 构建
double R2[3][3];// 旋转矩阵R2
ComposeMatrix(R2, A2, B2, C2);// 构建
double R12[3][3];// 旋转矩阵R12
MatMult(R1,R2,R12);// R12 = R1 * R2
/* calculate X,Y,Z from mounting point MP */// 注释块：从法兰盘中心点MP计算X,Y,Z
// XYZ = TCP - a3xyz R12(t) - a2xyz R1(t) - a1xyz // 注释：公式
// XYZ = TCP - D2 - D1 - a1xyz // 注释：公式
double P1[3];//a2xyz // 向量P1
P1[0]= a2x;// ...
P1[1]= a2y;// ...
P1[2]= a2z;// ...
double P2[3];//a3xyz // 向量P2
P2[0]= a3x;// ...
P2[1]= a3y;// ...
P2[2]= a3z;// ...
/* D2 = R12(t) x P2 */// 注释：计算D2
double D2[3];// 向量D2
for(i=0;i<3;i++){// 矩阵向量乘法
D2[i]=0;
for(j=0;j<3;j++){
D2[i]+= R12[j][i]* P2[j];//note that R12 is inverted (transposed)! // 注意这里是转置相乘
}
}
/* D1 = R1(t) x P1 */// 注释：计算D1
double D1[3];// 向量D1
for(i=0;i<3;i++){// 矩阵向量乘法
D1[i]=0;
for(j=0;j<3;j++){
D1[i]+= R1[j][i]* P1[j];//note that R1 is inverted (transposed)! // 注意这里是转置相乘
}
}
/* Q1, Q2, Q3 */// 注释：计算Q1, Q2, Q3
Axes[0]= X - D2[0]- D1[0]- a1x;// 计算Q1
Axes[1]= Y - D2[1]- D1[1]- a1y;// 计算Q2
Axes[2]= Z - D2[2]- D1[2]- a1z;// 计算Q3
/* Q4 */// 注释：计算Q4
if(r2x ==1){// 根据旋转轴定义
Axes[3]= A;
}elseif(r2y ==1){
Axes[3]= B;
}elseif(r2z ==1){
Axes[3]= C;
}
/* Q5 */// 注释：计算Q5
if(r3x ==1){// 根据旋转轴定义
Axes[4]= A;
}elseif(r3y ==1){
Axes[4]= B;
}elseif(r3z ==1){
Axes[4]= C;
}
/* Q6 not used */// 注释：Q6未使用
Axes[5]=0;// 设为0
for(i=0;i<5;i++)// 遍历前5个轴
{// for循环开始
Axes[i]=RoundToEpsilon(Axes[i]);// 精度处理
}// for循环结束
return STATUS_OK;// 返回成功
}
unsignedshortScaraDirect(Link_Type Links[6],double JointAxes[6],double PathAxes[6],double Axes[6])// 定义Scara机器人正运动学函数
{//direct transformations for Scara robot // 函数体开始，注释：Scara机器人的正解变换
//simplify notation // 注释：简化符号
double Q1 = JointAxes[0];// 获取关节角
double Q2 = JointAxes[1];// ...
double Q3 = JointAxes[2];// ...
double Q4 = JointAxes[3];// ...
double a1x = Links[1].Offset.X;// 获取连杆参数
double a1z = Links[1].Offset.Z;// ...
double a2x = Links[2].Offset.X;// ...
double a2z = Links[2].Offset.Z;// ...
double a4z = Links[4].Offset.Z;// ...
/* base offset from world frame */// 注释块：从世界坐标系到基坐标系的偏移
double ZeroFrame[6];// 声明基坐标系位姿数组
ZeroFrame[0]= Links[0].Offset.X;// 获取
ZeroFrame[1]= Links[0].Offset.Y;// ...
ZeroFrame[2]= Links[0].Offset.Z;// ...
ZeroFrame[3]= Links[0].Rotation.Z;// ...
//temporary axes values // 注释：临时轴坐标值
double tmpAxes[6];// 声明临时数组
/* check mechanical parameters consistency */// 注释块：检查机械参数的一致性
if((a1x <=0)||(a2x <=0))// 检查连杆长度是否为正
{// if块开始
return ERR_TRF_MECH;// 返回机械结构错误
}// if块结束
/* X axis */// 注释：X轴坐标
tmpAxes[0]= a1x *cosd(Q1)+ a2x *cosd(Q1+Q2);// 根据正运动学公式计算X
/* Y axis */// 注释：Y轴坐标
tmpAxes[1]= a1x *sind(Q1)+ a2x *sind(Q1+Q2);// 根据正运动学公式计算Y
/* Z axis */// 注释：Z轴坐标
tmpAxes[2]= a1z + a2z + Q3 +(a4z * Q4 /360.0);// 根据正运动学公式计算Z
/* C axis */// 注释：C轴（姿态）
tmpAxes[3]= Q1 + Q2 + Q4;// 根据正运动学公式计算C
/* adjust positions of axis C with +-2PI to bring it closer to the desired value */// 注释块：调整C轴的位置（+/-360度）使其更接近期望值
tmpAxes[3]=Modulo2PI(tmpAxes[3],PathAxes[3]);// 调用函数将C角调整到以当前C角为中心的360度范围内
/* consider zero offset (position and orientation of base point with respect to world origin) */// 注释块：考虑零点偏移
SubFrame2D(tmpAxes,ZeroFrame,Axes);// 调用2D坐标变换函数，将机器人基坐标系下的位姿变换到世界坐标系下
return0;//STATUS_OK; // 函数执行成功，返回0
}
unsignedshortScaraInverse(Link_Type Links[6],double PathAxes[6],double JointAxes[6],double Axes[6])// 定义Scara机器人逆运动学函数
{//inverse transformations for Scara robot // 函数体开始，注释：Scara机器人的逆解变换
/* base offset from world frame */// 注释块：从世界坐标系到基坐标系的偏移
double ZeroFrame[6];// 声明基坐标系位姿数组
ZeroFrame[0]= Links[0].Offset.X;// 获取
ZeroFrame[1]= Links[0].Offset.Y;// ...
ZeroFrame[2]= Links[0].Offset.Z;// ...
ZeroFrame[3]= Links[0].Rotation.Z;// ...
//temporary axes values // 注释：临时轴坐标值
double tmpAxes[6];// 声明临时数组
/* consider zero offset (position and orientation of base point with respect to world origin) */// 注释块：考虑零点偏移
AddFrame2D(PathAxes,ZeroFrame,tmpAxes);// 调用2D坐标变换函数，将世界坐标系下的位姿变换到机器人基坐标系下
//simplify notation // 注释：简化符号
double X = tmpAxes[0];// 获取坐标
double Y = tmpAxes[1];// ...
double Z = tmpAxes[2];// ...
double C = tmpAxes[3];// ...
double a1x = Links[1].Offset.X;// 获取连杆参数
double a1z = Links[1].Offset.Z;// ...
double a2x = Links[2].Offset.X;// ...
double a2z = Links[2].Offset.Z;// ...
double a4z = Links[4].Offset.Z;// ...
//keep same pose as current joints configuration // 注释：保持与当前关节配置相同的姿态
short Pose;// 姿态配置变量
if(JointAxes[1]<0)// 根据当前Q2判断肘部姿态
{// if块开始
Pose = TRF_POSE_LEFT;// 左手姿态
}// if块结束
else// 否则
{// else块开始
Pose = TRF_POSE_RIGHT;// 右手姿态
}// else块结束
/* check mechanical parameters consistency */// 注释块：检查机械参数的一致性
if((a1x <=0)||(a2x <=0))// 检查连杆长度是否为正
{// if块开始
return ERR_TRF_MECH;// 返回机械结构错误
}// if块结束
double D =(X*X + Y*Y - a1x*a1x - a2x*a2x)/(2* a1x * a2x);// 根据余弦定理计算cos(Q2)
if((D*D)>1+ TRF_EPSILON)// 如果D的平方大于1（考虑到浮点误差）
{// if块开始
return ERR_TRF_WORKSPACE;// 返回工作空间错误，表示目标点不可达
}// if块结束
elseif((D*D)>1)//round numerical impreciseness back to 1 // 如果因数值误差略大于1
{// else if块开始
D =1;// 钳位到1
}// else if块结束
if((X <= TRF_EPSILON)&&(Y <= TRF_EPSILON))//TCP in origin // 如果TCP在原点（奇异点）
{// if块开始
return ERR_TRF_WORKSPACE;// 返回工作空间错误
}// if块结束
/* Q1 and Q2 axis */// 注释块：Q1和Q2轴
if(Pose == TRF_POSE_RIGHT)// 如果是右手姿态
{// if块开始
Axes[0]=atan2d(Y,X)-atan2d(a2x*sqrt(1-D*D),a1x + a2x*D);// 计算Q1
Axes[1]=atan2d(sqrt(1-D*D),D);// 计算Q2
}// if块结束
elseif(Pose == TRF_POSE_LEFT)// 如果是左手姿态
{// else if块开始
Axes[0]=atan2d(Y,X)-atan2d(-a2x*sqrt(1-D*D), a1x + a2x*D);// 计算Q1
Axes[1]=atan2d(-sqrt(1-D*D),D);// 计算Q2
}// else if块结束
else// 否则
{// else块开始
return(ERR_TRF_POSE);// 返回姿态错误
}// else块结束
/* adjust positions of Q1 and Q2 with +-2PI to bring them closer to desired values */// 注释块：调整Q1和Q2的位置（+/-360度）使其更接近期望值
Axes[0]=Modulo2PI(Axes[0],JointAxes[0]);// 调整Q1
Axes[1]=Modulo2PI(Axes[1],JointAxes[1]);// 调整Q2
double Q1 = Axes[0];// 将调整后的值赋给局部变量
double Q2 = Axes[1];// ...
/* Q4 axis */// 注释：Q4轴
Axes[3]= C - Q1 - Q2;// 计算Q4
/* adjust positions of Q3 with +-2PI to bring it closer to desired values */// 注释块：调整Q3的位置...（注释有误，应为Q4）
Axes[3]=Modulo2PI(Axes[3],JointAxes[3]);// 调整Q4
double Q4 = Axes[3];// 将调整后的值赋给局部变量
/* Q3 axis */// 注释：Q3轴
Axes[2]= Z - a1z - a2z -(a4z * Q4 /360.0);// 计算Q3
return STATUS_OK;// 返回成功
}
unsignedshortPalletDirect(Link_Type Links[6],double JointAxes[6],double PathAxes[6],double Axes[6])// 定义码垛机器人正运动学函数
{//direct transformations for Palletizer robot // 函数体开始，注释：码垛机器人的正解变换
//simplify notation // 注释：简化符号
double Q1 = JointAxes[0];// 获取关节角
double Q2 = JointAxes[1];// ...
// double Q3 = JointAxes[2] - JointAxes[1]; // mechanical coupling // (被注释的代码) 机械耦合
double Q3 = JointAxes[2];// ...
double Q4 = JointAxes[3];// ...
double a1x = Links[1].Offset.X;// 获取连杆参数
double a1z = Links[1].Offset.Z;// ...
double a2z = Links[2].Offset.Z;// ...
double a3x = Links[3].Offset.X;// ...
double a4x = Links[4].Offset.X;// ...
double a4z = Links[4].Offset.Z;// ...
/* base offset from world frame */// 注释块：从世界坐标系到基坐标系的偏移
double ZeroFrame[6];// 声明基坐标系位姿数组
ZeroFrame[0]= Links[0].Offset.X;// 获取
ZeroFrame[1]= Links[0].Offset.Y;// ...
ZeroFrame[2]= Links[0].Offset.Z;// ...
ZeroFrame[3]= Links[0].Rotation.Z;// ...
//temporary axes values // 注释：临时轴坐标值
double tmpAxes[6];// 声明临时数组
/* check mechanical parameters consistency */// 注释块：检查机械参数的一致性
if((a1z <0)||(a2z <=0)||(a3x <=0)||(a4x <0)||(a4z <0))// 检查连杆参数是否有效
{// if块开始
return ERR_TRF_MECH;// 返回机械结构错误
}// if块结束
double rho = a1x + a2z*sind(Q2)+ a3x*cosd(Q2+Q3)+ a4x;// 计算径向距离 rho
/* X axis */// 注释：X轴坐标
tmpAxes[0]= rho *cosd(Q1);// 根据正运动学公式计算X
/* Y axis */// 注释：Y轴坐标
tmpAxes[1]= rho *sind(Q1);// 根据正运动学公式计算Y
/* Z axis */// 注释：Z轴坐标
tmpAxes[2]= a1z + a2z*cosd(Q2)- a3x*sind(Q2+Q3)- a4z;// 根据正运动学公式计算Z
/* C axis */// 注释：C轴
tmpAxes[3]= Q1 + Q4;// 根据正运动学公式计算C
/* adjust positions of axis C with +-2PI to bring it closer to the desired value */// 注释块：调整C轴的位置...
tmpAxes[3]=Modulo2PI(tmpAxes[3],PathAxes[3]);// 调整C角
/* consider zero offset (position and orientation of base point with respect to world origin) */// 注释块：考虑零点偏移
SubFrame2D(tmpAxes,ZeroFrame,Axes);// 变换到世界坐标系
return STATUS_OK;// 返回成功
}
unsignedshortPalletInverse(Link_Type Links[6],double PathAxes[6],double JointAxes[6],double Axes[6])// 定义码垛机器人逆运动学函数
{//inverse transformations for Palletizer robot // 函数体开始，注释：码垛机器人的逆解变换
/* base offset from world frame */// 注释块：从世界坐标系到基坐标系的偏移
double ZeroFrame[6];// 声明基坐标系位姿数组
ZeroFrame[0]= Links[0].Offset.X;// 获取
ZeroFrame[1]= Links[0].Offset.Y;// ...
ZeroFrame[2]= Links[0].Offset.Z;// ...
ZeroFrame[3]= Links[0].Rotation.Z;// ...
//temporary axes values // 注释：临时轴坐标值
double tmpAxes[6];// 声明临时数组
/* consider zero offset (position and orientation of base point with respect to world origin) */// 注释块：考虑零点偏移
AddFrame2D(PathAxes,ZeroFrame,tmpAxes);// 变换到机器人基坐标系
//simplify notation // 注释：简化符号
double X = tmpAxes[0];// 获取坐标
double Y = tmpAxes[1];// ...
double Z = tmpAxes[2];// ...
double C = tmpAxes[3];// ...
double a1x = Links[1].Offset.X;// 获取连杆参数
double a1z = Links[1].Offset.Z;// ...
double a2z = Links[2].Offset.Z;// ...
double a3x = Links[3].Offset.X;// ...
double a4x = Links[4].Offset.X;// ...
double a4z = Links[4].Offset.Z;// ...
/* check mechanical parameters consistency */// 注释块：检查机械参数的一致性
if((a1z <0)||(a2z <=0)||(a3x <=0)||(a4x <0)||(a4z <0))// 检查连杆参数是否有效
{// if块开始
return ERR_TRF_MECH;// 返回机械结构错误
}// if块结束
/* calculate Q1 */// 注释块：计算Q1
/* check for singularity */// 注释块：检查奇异点
if((fabs(X)< TRF_EPSILON)&&(fabs(Y)< TRF_EPSILON))// 如果在Z轴上
{// if块开始
Axes[0]= JointAxes[0];// 保持当前Q1
}// if块结束
else// 否则
{// else块开始
Axes[0]=atan2d(Y,X);// 使用atan2计算Q1
}// else块结束
/* adjust positions of Q1 with +-PI to bring it closer to desired values */// 注释块：调整Q1的位置...
Axes[0]=ModuloPI(Axes[0],JointAxes[0]);// 调整Q1
double Q1 = Axes[0];// 将调整后的值赋给局部变量
/* calculate length and height of triangle formed by a2z and a3x */// 注释块：计算由a2z和a3x构成的三角形的长度和高度
double height = Z - a1z + a4z;// 计算高度
double length =sqrt(X*X + Y*Y);// 计算长度
//flip sign of length if Q1 was chosen 180 deg away from atan(Y,X) // 注释：如果Q1被选为与atan(Y,X)相差180度的解，则翻转长度的符号
if(fabs(Modulo2PI(Axes[0]-atan2d(Y,X),0))>90)// 检查Q1解的选择
{// if块开始
length =-length;// 翻转符号
}// if块结束
//add a1x correction // 注释：添加a1x修正
length -=(a1x+a4x);// 修正长度
double rho =sqrt(length*length + height*height);// 计算从关节2到手腕点的距离rho
//check for workspace violations // 注释：检查工作空间违规
if((rho >(a2z + a3x + TRF_EPSILON))||(rho <(fabs(a2z-a3x)- TRF_EPSILON)))// 如果rho超出了连杆能构成的范围
{// if块开始
return ERR_TRF_WORKSPACE;// 返回工作空间错误
}// if块结束
elseif(rho >(a2z + a3x))//adjust impreciseness // 如果因数值误差略微超出
{// else if块开始
rho =(a2z + a3x);// 钳位到最大值
}// else if块结束
elseif(rho <fabs(a2z-a3x))//adjust impreciseness // 如果因数值误差略微小于
{// else if块开始
rho =fabs(a2z-a3x);// 钳位到最小值
}// else if块结束
double alpha =atan2d(height,length);// 计算角度alpha
double cos_beta =(rho*rho + a2z*a2z - a3x*a3x)/(2.0*a2z*rho);// 使用余弦定理计算beta角的余弦值
double beta =atan2d(sqrt(1-cos_beta*cos_beta),cos_beta);// 计算beta角
double cos_gamma =(a2z*a2z + a3x*a3x - rho*rho)/(2.0*a2z*a3x);// 使用余弦定理计算gamma角的余弦值
double gamma =180.0-atan2d(sqrt(1-cos_gamma*cos_gamma),cos_gamma);// 计算gamma角
/* Q2 axis */// 注释：Q2轴
Axes[1]=90.0- alpha - beta;// 计算Q2
/* Q3 axis */// 注释：Q3轴
Axes[2]= gamma -90;// 计算Q3
/* Q4 axis */// 注释：Q4轴
Axes[3]= C - Q1;// 计算Q4
//adjust positions of Q4 with +-2PI to bring it closer to desired values // 注释：调整Q4的位置...
Axes[3]=Modulo2PI(Axes[3],JointAxes[3]);// 调整Q4
return STATUS_OK;// 返回成功
}
unsignedshortDeltaDirect(Link_Type Links[6],double JointAxes[6],double PathAxes[6],double Axes[6])// 定义Delta机器人正运动学函数
{//direct transformations for Delta robot // 函数体开始，注释：Delta机器人的正解变换
//simplify notation // 注释：简化符号
double Q1 = JointAxes[0];// 获取关节角
double Q2 = JointAxes[1];// ...
double Q3 = JointAxes[2];// ...
double Q4 = JointAxes[3];// ...
double a1x = Links[1].Offset.X;//radius top // 上平台半径
double a1z = Links[1].Offset.Z;//arm top // 上臂长度
double a2x = Links[2].Offset.X;//radius bottom // 下平台半径
double a2z = Links[2].Offset.Z;//arm bottom // 下臂长度
double a3z = Links[3].Offset.Z;//bottom vertical offset // 底部垂直偏移
/* base offset from world frame */// 注释块：从世界坐标系到基坐标系的偏移
double ZeroFrame[6];// 声明基坐标系位姿数组
ZeroFrame[0]= Links[0].Offset.X;// 获取
ZeroFrame[1]= Links[0].Offset.Y;// ...
ZeroFrame[2]= Links[0].Offset.Z;// ...
ZeroFrame[3]= Links[0].Rotation.Z;// ...
//temporary axes values // 注释：临时轴坐标值
double tmpAxes[6];// 声明临时数组
/* check mechanical parameters consistency */// 注释块：检查机械参数的一致性
if((a1x <=0)||(a1z <=0)||(a2x <=0)||(a2z <=0)||(a3z <0))// 检查几何参数是否有效
{// if块开始
return ERR_TRF_MECH;// 返回机械结构错误
}// if块结束
double a1y = a1x *2.0* sqrt3;//base side top // 上平台边长相关计算
double a2y = a2x *2.0* sqrt3;//base side bottom // 下平台边长相关计算
double t =(a1y - a2y)* tan30 /2.0;// 中间变量计算
double y1 =-(t + a1z *cosd(Q1));// 第一个臂的端点y,z坐标
double z1 =-a1z *sind(Q1);// ...
double y2 =(t + a1z *cosd(Q2))* sin30;// 第二个臂的端点x,y,z坐标
double x2 = y2 * tan60;// ...
double z2 =-a1z *sind(Q2);// ...
double y3 =(t + a1z *cosd(Q3))* sin30;// 第三个臂的端点x,y,z坐标
double x3 =-y3 * tan60;// ...
double z3 =-a1z *sind(Q3);// ...
double dnm =(y2-y1)* x3 -(y3-y1)* x2;// 分母项计算
if(fabs(dnm)< TRF_EPSILON)// 如果分母接近零（奇异）
{// if块开始
return ERR_TRF_WORKSPACE;// 返回工作空间错误
}// if块结束
double w1 = y1*y1 + z1*z1;// 中间变量
double w2 = x2*x2 + y2*y2 + z2*z2;// ...
double w3 = x3*x3 + y3*y3 + z3*z3;// ...
// x = (a1*z + b1)/dnm // 注释：x的表达式
double a1 =(z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);// 系数a1
double b1 =-((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;// 系数b1
// y = (a2*z + b2)/dnm; // 注释：y的表达式
double a2 =-(z2-z1)*x3+(z3-y1)*x2;// 系数a2
double b2 =((w2-w1)*x3 -(w3-w1)*x2)/2.0;// 系数b2
// a*z^2 + b*z + c = 0 // 注释：关于z的二次方程
double a = a1*a1 + a2*a2 + dnm*dnm;// 系数a
double b =2*(a1*b1 + a2*(b2-y1*dnm)- z1*dnm*dnm);// 系数b
double c =(b2-y1*dnm)*(b2-y1*dnm)+ b1*b1 + dnm*dnm*(z1*z1 - a2z*a2z);// 系数c
// discriminant // 注释：判别式
double d = b*b -(double)4.0*a*c;// 计算二次方程的判别式
if(d < TRF_EPSILON)// 如果判别式小于零（无实数解）
{// if块开始
return ERR_TRF_WORKSPACE;// 返回工作空间错误
}// if块结束
else// 否则
{// else块开始
/* Z axis */// 注释：Z轴坐标
tmpAxes[2]=-0.5*(b+sqrt(d))/a -a3z;// 解出Z坐标（选择其中一个解）并应用偏移
double Z = Axes[2];// 将值赋给局部变量
/* X axis */// 注释：X轴坐标
tmpAxes[0]=(a1 * Z + b1)/dnm;// 计算X坐标
/* Y axis */// 注释：Y轴坐标
tmpAxes[1]=(a2 * Z + b2)/dnm;// 计算Y坐标
/* C axis */// 注释：C轴
tmpAxes[3]= Q4;// C轴等于Q4
/* adjust positions of axis C with +-2PI to bring it closer to the desired value */// 注释块：调整C轴的位置...
tmpAxes[3]=Modulo2PI(tmpAxes[3],PathAxes[3]);// 调整C角
/* consider zero offset (position and orientation of base point with respect to world origin) */// 注释块：考虑零点偏移
SubFrame2D(tmpAxes,ZeroFrame,Axes);// 变换到世界坐标系
return STATUS_OK;// 返回成功
}
}
staticintdelta_calcAngleYZ(double a1y,double a1z,double a2y,double a2z,double X,double Y,double Z,double*theta)// 定义一个静态辅助函数，用于Delta机器人逆解计算
{// 函数体开始
double y1 =-a1y /2.0/ sqrt3;// 计算y1
Y -=(a2y /2.0/ sqrt3);// shift center to edge // 注释：将中心移到边缘
// z = a + b*y // 注释：直线方程
double a =(X*X + Y*Y + Z*Z + a1z*a1z - a2z*a2z - y1*y1)/(2.0*Z);// 计算系数a
double b =(y1-Y)/Z;// 计算系数b
// discriminant // 注释：判别式
double d =-(a+b*y1)*(a+b*y1)+a1z*(b*b*a1z+a1z);// 计算二次方程的判别式
if(d < TRF_EPSILON)// 如果小于零
{// if块开始
return ERR_TRF_WORKSPACE;// 返回工作空间错误
}// if块结束
else// 否则
{// else块开始
double yj =(y1 - a*b -sqrt(d))/(b*b +1);// 解出yj
double zj = a + b*yj;// 解出zj
*theta =atan2d(-zj,(y1 - yj));// 计算关节角theta
return STATUS_OK;// 返回成功
}// else块结束
}
unsignedshortDeltaInverse(Link_Type Links[6],double PathAxes[6],double JointAxes[6],double Axes[6])// 定义Delta机器人逆运动学函数
{//inverse transformations for Delta robot // 函数体开始，注释：Delta机器人的逆解变换
/* base offset from world frame */// 注释块：从世界坐标系到基坐标系的偏移
double ZeroFrame[6];// 声明基坐标系位姿数组
ZeroFrame[0]= Links[0].Offset.X;// 获取
ZeroFrame[1]= Links[0].Offset.Y;// ...
ZeroFrame[2]= Links[0].Offset.Z;// ...
ZeroFrame[3]= Links[0].Rotation.Z;// ...
//temporary axes values // 注释：临时轴坐标值
double tmpAxes[6];// 声明临时数组
/* consider zero offset (position and orientation of base point with respect to world origin) */// 注释块：考虑零点偏移
AddFrame2D(PathAxes,ZeroFrame,tmpAxes);// 变换到机器人基坐标系
//simplify notation // 注释：简化符号
double X = tmpAxes[0];// 获取坐标
double Y = tmpAxes[1];// ...
double Z = tmpAxes[2];// ...
double C = tmpAxes[3];// ...
double a1x = Links[1].Offset.X;//radius top // 上平台半径
double a1z = Links[1].Offset.Z;//arm top // 上臂长度
double a2x = Links[2].Offset.X;//radius bottom // 下平台半径
double a2z = Links[2].Offset.Z;//arm bottom // 下臂长度
double a3z = Links[3].Offset.Z;//bottom vertical offset // 底部垂直偏移
/* check mechanical parameters consistency */// 注释块：检查机械参数的一致性
if((a1x <=0)||(a1z <=0)||(a2x <=0)||(a2z <=0)||(a3z <0))// 检查几何参数是否有效
{// if块开始
return ERR_TRF_MECH;// 返回机械结构错误
}// if块结束
double a1y = a1x *2.0* sqrt3;//base side top // 上平台边长相关计算
double a2y = a2x *2.0* sqrt3;//base side bottom // 下平台边长相关计算
/* remove bottom vertical offset */// 注释块：移除底部垂直偏移
Z += a3z;// 修正Z坐标
if(Z >=0)// 如果Z坐标在基平面上方
{// if块开始
return ERR_TRF_WORKSPACE;// 返回工作空间错误
}// if块结束
double Q1, Q2, Q3;// 声明关节角变量
int status;// 状态变量
/* Q1 axis */// 注释：Q1轴
status =delta_calcAngleYZ(a1y, a1z, a2y, a2z, X, Y, Z,&Q1);// 调用辅助函数计算Q1
if(status == STATUS_OK)// 如果成功
{// if块开始
Axes[0]= Q1;// 赋值
}// if块结束
else// 否则
{// else块开始
return ERR_TRF_WORKSPACE;// 返回工作空间错误
}// else块结束
/* Q2 axis */// 注释：Q2轴
status =delta_calcAngleYZ(a1y, a1z, a2y, a2z, X*cos120 + Y*sin120, Y*cos120-X*sin120, Z,&Q2);// 调用辅助函数，旋转坐标系120度后计算Q2
if(status == STATUS_OK)// 如果成功
{// if块开始
Axes[1]= Q2;// 赋值
}// if块结束
else// 否则
{// else块开始
return ERR_TRF_WORKSPACE;// 返回工作空间错误
}// else块结束
/* Q3 axis */// 注释：Q3轴
status =delta_calcAngleYZ(a1y, a1z, a2y, a2z, X*cos120 - Y*sin120, Y*cos120+X*sin120, Z,&Q3);// 调用辅助函数，将坐标系旋转-120度后计算Q3
if(status == STATUS_OK)// 如果计算成功
{// if块开始
Axes[2]= Q3;// 将计算出的Q3值赋给结果数组的第三个元素
}// if块结束
else// 否则
{// else块开始
return ERR_TRF_WORKSPACE;// 返回工作空间错误
}// else块结束
/* Q4 axis */// 注释：Q4轴
Axes[3]= C;// Q4轴（旋转轴）直接等于输入的C（姿态角）
/* adjust positions of axis Q4 with +-2PI to bring it closer to the desired value */// 注释块：调整Q4轴的位置（+/-360度）使其更接近期望值
Axes[3]=Modulo2PI(Axes[3],JointAxes[3]);// 调用函数将Q4角调整到以当前Q4为中心的360度范围内
return STATUS_OK;// 函数执行成功，返回OK状态
}// DeltaInverse 函数体结束更多推荐
8
0- 0
已为社区贡献6条内容
所有评论(0)