//机器人的引导运动 public void GuidMove(Vector3 Direction) { const float rate1 = 0.01f; Vector3 StartPoint = IKA.SolutionOfKinematics(CurrentAngle_All()); Quaternion Posture = IKA.SolutionOfKinematics_posture(CurrentAngle_All()); Vector3 EndPoint = StartPoint + Direction * rate1; float[] movl_A_inter = IKA.AcceptInterPointPosture(EndPoint, Posture, CurrentAngle_All()); if (!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])) { StartCoroutine(ScreenScript.WarningYellow("运动极限")); } GSKDATA.VList.Add(J5.transform.position);//记录点 }
public Vector3 getPos(float[] point) { return(IKA.SolutionOfKinematics(point)); }
//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; } } }