bool CreateBall = false; //是否创建小球 private void FixedUpdate() { if (Run) { if (Finsh == true) { Finsh = false; //从队列中获取一个元素 JPostion jp = (JPostion)PosList.Dequeue(); CreateBall = jp.CreateBall; mAxis1_3target = jp.Axis1_3; mAxis4_6target = jp.Axis4_6; } Vector3 Axis1_3 = new Vector3(Axis1.GetAngle(), Axis2.GetAngle(), Axis3.GetAngle()); Vector3 Axis4_6 = new Vector3(Axis4.GetAngle(), Axis5.GetAngle(), Axis6.GetAngle()); var temp1_3 = Vector3.MoveTowards(Axis1_3, mAxis1_3target, speed); var temp4_6 = Vector3.MoveTowards(Axis4_6, mAxis4_6target, speed); SetJPos(temp1_3.x, temp1_3.y, temp1_3.z, temp4_6.x, temp4_6.y, temp4_6.z); //创建一个小球 if (CreateBall) { GameObject obj1 = GameObject.CreatePrimitive(PrimitiveType.Sphere); obj1.GetComponent <MeshRenderer>().material = Ballmaterial; //设置物体的位置Vector3三个参数分别代表x,y,z的坐标数 obj1.transform.position = rbt_tool.transform.position; obj1.transform.localScale = new Vector3(0.01f, 0.01f, 0.01f); obj1.transform.parent = Ball.transform; } //判断当前小步是否走完 if ((Axis1_3 - mAxis1_3target).magnitude < 0.001f && (Axis4_6 - mAxis4_6target).magnitude < 0.001f ) { Finsh = true; if (PosList.Count == 0) { Run = false; Debug.Log("运动完成"); } } } }
//沿世界坐标系移动到目标位置 PtP移动 public void CMove(CPostion pose, bool CreateBall = false) { Vector3 Axis1_3target = new Vector3(); Vector3 Axis4_6target = new Vector3(); //运动学正解计算出8组解 var kk = Kinematics.InverseKinematics(pose.Postion, pose.Pose); for (int i = 0; i < 8; i++) { //判断角度是否有效 if (kk[i, 0] == kk[i, 0] && kk[i, 1] == kk[i, 1] && kk[i, 2] == kk[i, 2] && kk[i, 3] == kk[i, 3] && kk[i, 4] == kk[i, 4] && kk[i, 5] == kk[i, 5] ) { Axis1_3target = new Vector3((float)kk[i, 0], (float)kk[i, 1], (float)kk[i, 2]); Axis4_6target = new Vector3((float)kk[i, 3], (float)kk[i, 4], (float)kk[i, 5]); JPostion jp = new JPostion(Axis1_3target, Axis4_6target); jp.CreateBall = CreateBall; lock (PosList) { PosList.Enqueue(jp); } break; } } Axis1_3Visual.x = Axis1_3target.x; Axis1_3Visual.y = Axis1_3target.y; Axis1_3Visual.z = Axis1_3target.z; Axis4_6Visual.x = Axis4_6target.x; Axis4_6Visual.y = Axis4_6target.y; Axis4_6Visual.z = Axis4_6target.z; }
//沿世界坐标系移动到目标位置 直线移动 public void CLine(CPostion mpose, bool CreateBall = false) { Vector3 Position; Vector3 pose; Vector3 Axis1_3target = new Vector3(); Vector3 Axis4_6target = new Vector3(); var kk = Kinematics.InverseKinematics(mpose.Postion, mpose.Pose); //运动学正解计算出当前位置, Kinematics.forwardKinematics(Axis1_3Visual.x, Axis1_3Visual.y, Axis1_3Visual.z, Axis4_6Visual.x, Axis4_6Visual.y, Axis4_6Visual.z, out Position, out pose); //计算两者之间直线长度 double distance = mpose.GetDistance(new CPostion(Position, pose)); //计算插值数量 int count = (int)(distance / interLine); //计算单个轴的增量 //如果距离很小 if (count == 0) { var ikine_t = Kinematics.InverseKinematics(mpose.Postion, mpose.Pose); for (int i = 0; i < 8; i++) { //判断角度是否有效 if (ikine_t[i, 0] == ikine_t[i, 0] && ikine_t[i, 1] == ikine_t[i, 1] && ikine_t[i, 2] == ikine_t[i, 2] && ikine_t[i, 3] == ikine_t[i, 3] && ikine_t[i, 4] == ikine_t[i, 4] && ikine_t[i, 5] == ikine_t[i, 5] ) { Axis1_3target = new Vector3((float)ikine_t[i, 0], (float)ikine_t[i, 1], (float)ikine_t[i, 2]); Axis4_6target = new Vector3((float)ikine_t[i, 3], (float)ikine_t[i, 4], (float)ikine_t[i, 5]); JPostion jp = new JPostion(Axis1_3target, Axis4_6target); jp.CreateBall = CreateBall; lock (PosList) { PosList.Enqueue(jp); } break; } } } else { double dx = (mpose.x - Position.x) / count; double dy = (mpose.y - Position.y) / count; double dz = (mpose.z - Position.z) / count; double drx = (mpose.rx - pose.x) / count; double dry = (mpose.ry - pose.y) / count; double drz = (mpose.rz - pose.z) / count; //细分 for (int j = 0; j < count; j++) { Position.x += (float)dx; Position.y += (float)dy; Position.z += (float)dz; pose.x += (float)drx; pose.y += (float)dry; pose.z += (float)drz; //计算逆解 var ikine_t = Kinematics.InverseKinematics(Position, pose); for (int i = 0; i < 8; i++) { //判断角度是否有效 if (ikine_t[i, 0] == ikine_t[i, 0] && ikine_t[i, 1] == ikine_t[i, 1] && ikine_t[i, 2] == ikine_t[i, 2] && ikine_t[i, 3] == ikine_t[i, 3] && ikine_t[i, 4] == ikine_t[i, 4] && ikine_t[i, 5] == ikine_t[i, 5] ) { Axis1_3target = new Vector3((float)ikine_t[i, 0], (float)ikine_t[i, 1], (float)ikine_t[i, 2]); Axis4_6target = new Vector3((float)ikine_t[i, 3], (float)ikine_t[i, 4], (float)ikine_t[i, 5]); JPostion jp = new JPostion(Axis1_3target, Axis4_6target); jp.CreateBall = CreateBall; lock (PosList) { PosList.Enqueue(jp); } break; } } } } //更新当前虚拟坐标 Axis1_3Visual.x = Axis1_3target.x; Axis1_3Visual.y = Axis1_3target.y; Axis1_3Visual.z = Axis1_3target.z; Axis4_6Visual.x = Axis4_6target.x; Axis4_6Visual.y = Axis4_6target.y; Axis4_6Visual.z = Axis4_6target.z; }