//机器人的引导运动
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 Quaternion getQua(float[] point)
{
return(IKA.SolutionOfKinematics_posture(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;
}
}
}
