//movl private void movl() { Vector3 p1 = IKA.SolutionOfKinematics(movl_s); Vector3 p2 = IKA.SolutionOfKinematics(movl_e); Quaternion z1 = IKA.SolutionOfKinematics_posture(movl_s); Quaternion z2 = IKA.SolutionOfKinematics_posture(movl_e); float Tb = run_speed / GSKDATA.MOVL_a; //加速段的时间 float Lb = 0.5f * movl_a * Tb * Tb; float L = Vector3.Distance(p1, p2); //直线运动的总位移 float tf = 2 * Tb + (L - 2 * Lb) / run_speed; //直线运动的总时间 //归一化 if (tf != 0) { float L_b = Lb / L; float T_b = Tb / tf; float a_ = 2 * L_b / (T_b * T_b); movl_t = movl_t / tf; // float namta = 0f; if (movl_t >= 0 && movl_t <= T_b) { namta = 0.5f * a_ * movl_t * movl_t; } else if (movl_t > T_b && movl_t <= (1 - T_b)) { namta = 0.5f * a_ * T_b * T_b + a_ * T_b * (movl_t - T_b); } else if (movl_t > (1 - T_b) && movl_t <= 1) { namta = 0.5f * a_ * T_b * T_b + a_ * T_b * (movl_t - T_b) - 0.5f * a_ * (movl_t + T_b - 1) * (movl_t + T_b - 1); } else if (movl_t > 1) { namta = 1; } ///计算出插值点 //位置 Vector3 Inter_Point_P = Vector3.Lerp(p1, p2, namta); //姿态 Quaternion Inter_Point_Z = Quaternion.Slerp(z1, z2, namta); float[] movl_A_inter = IKA.AcceptInterPointPosture(Inter_Point_P, Inter_Point_Z, CurrentAngle_All()); for (int i = 0; i < 6; i++) { joints[i].RotateFixedAngle_FixedTime(movl_A_inter[i]); } if (namta == 1) { axis_running = false;//插补结束 movl_t = 0f; } } else { tf = CalculateTime(movl_s, movl_e); if (tf != 0) { for (int i = 0; i < 6; i++) { joints[i].JointInterpolation_3(movl_s[i], movl_e[i], tf, movl_t); } } else { axis_running = false;//插补结束 movl_t = 0f; } } }
/// <summary> /// 直线插补算法部分 /// </summary> /// <param name="t">已插补时长</param> public void MOVL(float t) { float Tb = GSKDATA.Speed / GSKDATA.MOVL_a; //加速段的时间 float Lb = 0.5f * GSKDATA.MOVL_a * Tb * Tb; float L = Vector3.Distance(GSKDATA.MOVL_P1, GSKDATA.MOVL_P2); //直线运动的总位移 float tf = 2 * Tb + (L - 2 * Lb) / GSKDATA.Speed; //直线运动的总时间 //归一化 if (tf != 0) { float L_b = Lb / L; float T_b = Tb / tf; float a_ = 2 * L_b / (T_b * T_b); t = t / tf; // float namta = 0f; if (t >= 0 && t <= T_b) { namta = 0.5f * a_ * t * t; } else if (t > T_b && t <= (1 - T_b)) { namta = 0.5f * a_ * T_b * T_b + a_ * T_b * (t - T_b); } else if (t > (1 - T_b) && t <= 1) { namta = 0.5f * a_ * T_b * T_b + a_ * T_b * (t - T_b) - 0.5f * a_ * (t + T_b - 1) * (t + T_b - 1); } else if (t > 1) { namta = 1; } ///计算出插值点 //位置 Vector3 Inter_Point_P = Vector3.Lerp(GSKDATA.MOVL_P1, GSKDATA.MOVL_P2, namta); //姿态 Quaternion Inter_Point_Z = Quaternion.Slerp(GSKDATA.MOVL_Z1, GSKDATA.MOVL_Z2, namta); float[] movl_A_inter = IKA.AcceptInterPointPosture(Inter_Point_P, Inter_Point_Z, CurrentAngle_All()); //Debug.Log("movl_A_inter[1]:" + movl_A_inter[1]); JMove_1.RotateFixedAngle_FixedTime(movl_A_inter[0]); JMove_2.RotateFixedAngle_FixedTime(movl_A_inter[1]); JMove_3.RotateFixedAngle_FixedTime(movl_A_inter[2]); JMove_4.RotateFixedAngle_FixedTime(movl_A_inter[3]); JMove_5.RotateFixedAngle_FixedTime(movl_A_inter[4]); JMove_6.RotateFixedAngle_FixedTime(movl_A_inter[5]); if (namta == 1) { GSKDATA.AxisRunning = false;//插补结束 GSKDATA.MOVL_t = 0f; } } else { tf = CalculateTime(GSKDATA.MOVL_P1, GSKDATA.MOVL_P2); if (tf != 0) { JMove_1.JointInterpolation_3(GSKDATA.MOVJ_A1[0], GSKDATA.MOVJ_A2[0], tf, t); JMove_2.JointInterpolation_3(GSKDATA.MOVJ_A1[1], GSKDATA.MOVJ_A2[1], tf, t); JMove_3.JointInterpolation_3(GSKDATA.MOVJ_A1[2], GSKDATA.MOVJ_A2[2], tf, t); JMove_4.JointInterpolation_3(GSKDATA.MOVJ_A1[3], GSKDATA.MOVJ_A2[3], tf, t); JMove_5.JointInterpolation_3(GSKDATA.MOVJ_A1[4], GSKDATA.MOVJ_A2[4], tf, t); JMove_6.JointInterpolation_3(GSKDATA.MOVJ_A1[5], GSKDATA.MOVJ_A2[5], tf, t); } else { GSKDATA.AxisRunning = false;//插补结束 GSKDATA.MOVL_t = 0f; } } }