机械臂两点间的运动（满足速度曲线）

记录用。不作为其他舵机的使用。

 

 给两组机械臂的关节角度值：

速度满足梯形曲线，已知总时间，加速度，求最大速度

我这里有四个机械臂串联：

std::vector<double> angle_inital = angle_traj[i-1]; std::vector<double> angle_target = angle_traj[i]; int TotalTime = 50; double accelerated = 1; double duration[4] = { 0,0,0,0 }; double temp = 0; for (int i = 0; i < 4; i++) { //这里是针对我的舵机的参数计算的变量，这是有公式的，大致意思就是根据T计算可以达到的最大速度 duration[i] = 4 * 64 * 64 * accelerated * abs(angle_target[i] - angle_inital[i]); temp = accelerated * accelerated* TotalTime * TotalTime; if (duration[i] > temp) TotalTime = sqrt(duration[i] / (pow(accelerated, 2))); std::cout << 4 * 64 * 64 * abs(angle_target[i] - angle_inital[i]) << std::endl; } double velocity[4] = { 0,0,0,0 }; //计算完时间后根据速度和总时间T检测是否有匀速的段 for (int i = 0; i < 4; i++) { std::cout << abs(angle_target[i] - angle_inital[i]); velocity[i] = (accelerated * TotalTime + sqrt(pow(temp, 2) - duration[i])) / (2 * 64); if (abs(angle_target[i] - angle_inital[i]) >= (pow(velocity[i], 2) / accelerated)) { std::cout << i << " 存在匀速段" << std::endl; } else std::cout << i << " 不存在匀速段" << std::endl; }