Exemple #1
0
    public void ActivateIK()
    {
        Debug.Log("activate Ik");
        robotStatus = RobotStatus.Idle;
        baseTargets = new GameObject("Base Targets").transform;
        baseTargets.SetParent(GameObject.Find("Vessel Offset").transform);
        baseTargets.localEulerAngles = Vector3.zero;
        baseTargets.SetParent(null);
        baseTargets.localPosition = Vector3.zero;

        directionTarget = new GameObject("Direction Target").transform;
        directionTarget.SetParent(GameObject.Find("Gimbal").transform);
        directionTarget.localEulerAngles = GameObject.Find("Mirror Vessel COM").transform.localEulerAngles;
        targetBaseHeight = directionTarget.position;
        foreach (var limb in limbs)
        {
            var baseTarget = Instantiate(GameObject.Find("Base Target")).transform;
            baseTarget.SetParent(baseTargets);
            limb.SetBaseTarget(baseTarget);

            limb.ActivateIK();
        }
        baseTargets.SetParent(directionTarget);
        IKactivaed.Invoke();
        DebugVector.DrawVector(directionTarget, DebugVector.Direction.z, 3, .1f, Color.red, Color.white, Color.green);
        DebugVector.DrawVector(memoryBridge.vesselControl.mirrorVessel.vesselOffset, DebugVector.Direction.z, 3, .1f, Color.red, Color.white, Color.blue);

        steeringPoint = new GameObject("steering point");
        DebugVector.DrawVector(steeringPoint.transform, DebugVector.Direction.all, 2, .1f, Color.red, Color.white, Color.blue);
        // Debug.LogError("");
    }
Exemple #2
0
 public void SetHomePoint(Vector3 position)
 {
     Debug.Log("Home point updated");
     homePoint          = new GameObject("Home Point").transform;
     homePoint.position = position;
     DebugVector.DrawVector(homePoint, Color.red, Color.green, Color.blue, Vector3.one);
 }
Exemple #3
0
    private void Start()
    {
        for (int i = 0; i < flightPoints.Count; i++)
        {
            DebugVector.DrawVector(flightPoints[i].transform, Color.red, Color.green, Color.blue, Vector3.one);

            //if (i != 0)
            //{
            //    flightPoints[i].LookAt(flightPoints[i -1], Vector3.up);

            //}
            //else
            //{
            //    flightPoints[i].LookAt(flightPoints[flightPoints.Count - 1], Vector3.up);
            //}
            //flightPoints[i].localEulerAngles += new Vector3(0, 180, 0);
        }
    }
    void CalculateIK(IKAxis IKAxis)
    {
        var servoGroup = IKAxis.servoGroup;

        if (IKAxis.fixedAngleServo)
        {
            var targetOffset = IKAxis.fixedAngleServo.servoBase.InverseTransformPoint(IKAxis.fixedAngleServo.servoBase.transform.position + new Vector3(0, -100, 0));
            var angle        = Math.Atan2(targetOffset.z, targetOffset.y);
            angle *= (180 / Math.PI);
            IKAxis.fixedAngleServo.SetServo((float)angle);
            //IKAxis.fixedAngleServo.LookDown();
        }

        if (IKAxis.servo1)
        {
            var targetOffset = IKAxis.servo1.servoBase.InverseTransformPoint(IKtargetTransform.position);
            var angle0       = Math.Atan2(targetOffset.z, targetOffset.y);
            angle0 *= (180 / Math.PI);

            if (firstFrame)
            {
                //var children = GetComponentsInChildren<Transform>();
                //foreach (var child in children)
                //{
                //    if (child.name == "Foot(Clone)")
                //    {
                //        trueLimbEnd = child;
                //        DebugVector.DrawVector(trueLimbEnd, DebugVector.Direction.all, 5f, .1f, Color.red, Color.white, Color.blue);
                //    }

                //}
                // Debug.Log("servo 1 offset " + IKAxis.servo1.targetOffset);
                firstFrame = false;
                var axis = IKAxisZ;
                if (axis.servo0)
                {
                    DebugVector.DrawVector(axis.servo0.transform, DebugVector.Direction.all, .5f, .1f, Color.red, Color.white, Color.blue);
                }
                if (axis.servo1)
                {
                    DebugVector.DrawVector(IKAxis.servo1.transform, DebugVector.Direction.all, .5f, .1f, Color.red, Color.white, Color.blue);
                }
                //DebugVector.DrawVector(IKAxis.servo1.transform, DebugVector.Direction.all, .5f, .1f, Color.red, Color.white, Color.blue);
            }
            servo1_0Dist = Vector3.Distance(IKAxis.servo1.transform.position, IKAxis.servo0.transform.position);

            var localPos = IKAxis.servo0.transform.InverseTransformPoint(trueLimbEnd.position);
            localPos.x = 0;
            var globalPos = IKAxis.servo0.transform.TransformPoint(localPos);
            servo0_GroundPointDist = Vector3.Distance(IKAxis.servo0.transform.position, globalPos);


            servo1_GroundPointDist = Vector3.Distance(IKAxis.servo1.transform.position, IKtargetTransform.position);

            float angle1 = LawOfCosines(servo1_0Dist, servo0_GroundPointDist, servo1_GroundPointDist);
            //var angle1 = LawOfCosines(servo1_0Dist, )

            if (IKAxis.servo1.invert)
            {
                angle1 -= (float)angle0;
            }
            else
            {
                angle1 += (float)angle0;
            }

            // var angle = (float)(angle1 + angle0);// - IKAxis.servo1.targetOffset);
            if (!float.IsNaN(angle1))
            {
                IKAxis.servo1.SetServo(angle1);
            }
            //   if (limbController.name == "ik Test")
            //    {
            //Debug.Log(Time.frameCount);
            //// Debug.Log(" limblength " + IKAxis.servo0.limbLength);
            //Debug.Log(" servo1_0Dist " + servo1_0Dist);
            //Debug.Log(" servo0_groundPoint " + servo0_GroundPointDist);
            //Debug.Log(" servo1_ground point " + servo1_GroundPointDist);
            //Debug.Log("Angle 1 " + angle0);
            //Debug.Log("Angle 2 " + angle1);
        }
        if (IKAxis.servo0)
        {
            if (IKAxis.fixedAngleServo)
            {
                // Debug.Log("run servo 0 fixed angle");
                // var yDif = IKAxis.servo0.groundPoint.position.y - IKtargetTransform.position.y;
                // Debug.Log(yDif);
                // var limbDist = Vector3.Distance(IKAxis.fixedAngleServo.transform.position, IKAxis.fixedAngleServo.groundPoint.position);
                var targetPos = IKtargetTransform.position; // + new Vector3(0, limbDist, 0);
                                                            //  if (testTarget)
                                                            //     testTarget.position = targetPos;

                var targetOffset = IKAxis.servo0.servoBase.InverseTransformPoint(targetPos);
                var angle        = Math.Atan2(targetOffset.z, targetOffset.y);
                angle *= (180 / Math.PI);

                // angle += IKAxis.fixedAngleServo.setAngle;
                // angle = -IKAxis.fixedAngleServo.setAngle;

                //   IKAxis.servo0.SetServo((float)angle);// + IKAxis.fixedAngleServo.setAngle);
            }
            else
            {
                var footOffset = IKAxis.servo0.transform.InverseTransformPoint(trueLimbEnd.position);
                xOffset = footOffset.z;
                Vector3 targetOffset;
                //if (IKAxis.servo0 == servoBase && Math.Abs(footOffset.z) > .1f)
                //    targetOffset = IKAxis.servo0.servoBase.InverseTransformPoint(IKtargetTransform.position - (IKAxis.servo0.transform.forward * footOffset.z));
                //else
                //{
                targetOffset = IKAxis.servo0.servoBase.InverseTransformPoint(IKtargetTransform.position);
                //  }


                var angle = Math.Atan2(targetOffset.z, targetOffset.y);
                angle *= (180 / Math.PI);

                // Debug.Log(" servo 0 " + IKAxis.servo0.name);

                // angle -= IKAxis.servo0.targetOffset;
                IKAxis.servo0.SetServo((float)angle);
            }
        }
    }
    void CalculateIK(IKAxis IKAxis)
    {
        var servoGroup = IKAxis.servoGroup;

        if (IKAxis.fixedAngleServo)
        {
            var targetOffset = IKAxis.fixedAngleServo.servoBase.InverseTransformPoint(IKAxis.fixedAngleServo.servoBase.transform.position + new Vector3(0, -100, 0));
            var angle        = Math.Atan2(targetOffset.z, targetOffset.y);
            angle *= (180 / Math.PI);
            IKAxis.fixedAngleServo.SetServo((float)angle);
            //IKAxis.fixedAngleServo.LookDown();
        }

        if (IKAxis.servo0)
        {
            if (IKAxis.fixedAngleServo)
            {
                Debug.Log("run servo 0 fixed angle");
                var yDif = IKAxis.servo0.groundPoint.position.y - targetTransform.position.y;
                Debug.Log(yDif);
                var limbDist  = Vector3.Distance(IKAxis.fixedAngleServo.transform.position, IKAxis.fixedAngleServo.groundPoint.position);
                var targetPos = targetTransform.position;// + new Vector3(0, limbDist, 0);
                if (testTarget)
                {
                    testTarget.position = targetPos;
                }

                var targetOffset = IKAxis.servo0.servoBase.InverseTransformPoint(targetPos);
                var angle        = Math.Atan2(targetOffset.z, targetOffset.y);
                angle *= (180 / Math.PI);

                // angle += IKAxis.fixedAngleServo.setAngle;
                // angle = -IKAxis.fixedAngleServo.setAngle;

                //   IKAxis.servo0.SetServo((float)angle);// + IKAxis.fixedAngleServo.setAngle);
            }
            else
            {
                //   Debug.Log("run servo 0");
                var targetOffset = IKAxis.servo0.servoBase.InverseTransformPoint(targetTransform.position);
                var angle        = Math.Atan2(targetOffset.z, targetOffset.y);
                angle *= (180 / Math.PI);

                // angle -= IKAxis.servo0.targetOffset;
                IKAxis.servo0.SetServo((float)angle);
            }
        }

        if (IKAxis.servo1)
        {
            // Debug.Log("run servo 2");
            var targetOffset = IKAxis.servo1.servoBase.InverseTransformPoint(targetTransform.position);
            var angle0       = Math.Atan2(targetOffset.z, targetOffset.y);
            angle0 *= (180 / Math.PI);
            // float angle1 = LawOfCosines(IKAxis.servo1.limbLength, IKAxis.servo0.limbLength, Vector3.Distance(IKAxis.servo1.transform.position, targetTransform.position));

            // if (limbDistCheckCount == 0)
            //  var dist = Vector3.Distance(IKAxis.servo0.groundPoint.position, targetTransform.position);
            //  if(dist > .5f)
            // {
            // var 8 = IKAxis.servo1.limbLength

            if (firstFrame)
            {
                var children = GetComponentsInChildren <Transform>();
                foreach (var child in children)
                {
                    if (child.name == "Foot(Clone)")
                    {
                        limbEndPoint = child;
                    }
                }
                // Debug.Log("servo 1 offset " + IKAxis.servo1.targetOffset);
                firstFrame = false;
                var axis = IKAxisZ;
                if (axis.servo0)
                {
                    DebugVector.DrawVector(axis.servo0.transform, DebugVector.Direction.all, .5f, .1f, Color.red, Color.white, Color.blue);
                }
                if (axis.servo1)
                {
                    DebugVector.DrawVector(IKAxis.servo1.transform, DebugVector.Direction.all, .5f, .1f, Color.red, Color.white, Color.blue);
                }
                //DebugVector.DrawVector(IKAxis.servo1.transform, DebugVector.Direction.all, .5f, .1f, Color.red, Color.white, Color.blue);
            }
            servo1_0Dist           = Vector3.Distance(IKAxis.servo1.transform.position, IKAxis.servo0.transform.position);
            servo0_GroundPointDist = Vector3.Distance(IKAxis.servo0.transform.position, limbEndPoint.position);
            servo1_GroundPointDist = Vector3.Distance(IKAxis.servo1.transform.position, targetTransform.position);
            // limbDistCheckCount = 10;
            //   }
            //  limbDistCheckCount--;
            float angle1 = LawOfCosines(servo1_0Dist, servo0_GroundPointDist, servo1_GroundPointDist);
            //float angle1 = LawOfCosines(IKAxis.servo1.limbLength, Vector3.Distance(IKAxis.servo0.transform.position,targetTransform.position), Vector3.Distance(IKAxis.servo1.transform.position, targetTransform.position));

            if (IKAxis.servo1.invert)
            {
                angle1 -= (float)angle0;
            }
            else
            {
                angle1 += (float)angle0;
            }

            // var angle = (float)(angle1 + angle0);// - IKAxis.servo1.targetOffset);
            if (!float.IsNaN(angle1))
            {
                IKAxis.servo1.SetServo(angle1);
            }


            //   if (limbController.name == "ik Test")
            //    {
            //Debug.Log(Time.frameCount);

            //// Debug.Log(" limblength " + IKAxis.servo0.limbLength);
            //Debug.Log(" servo1_0Dist " + servo1_0Dist);
            //Debug.Log(" servo0_groundPoint " + servo0_GroundPointDist);
            //Debug.Log(" servo1_ground point " + servo1_GroundPointDist);
            //Debug.Log("Angle 1 " + angle0);
            //Debug.Log("Angle 2 " + angle1);
            //  }//
        }



        // }

        // // for (int i = 0; i < servoGroup.Count; i++)
        ////  {
        //      var servo = servoGroup[i];
        //      if (i == 0)
        //      {
        //          if (servo.targetAngle != Vector3.down)
        //          {
        //              var targetOffset = servoGroup[0].servoBase.InverseTransformPoint(targetTransform.position);
        //              var angle = Math.Atan2(targetOffset.z, targetOffset.y);
        //              angle *= (180 / Math.PI);
        //              servoGroup[0].SetServo((float)angle);
        //          }
        //          else
        //          {
        //              var targetOffset = servo.servoBase.InverseTransformPoint(Vector3.down);
        //              var angle = Math.Atan2(targetOffset.z, targetOffset.y);
        //              angle *= (180 / Math.PI);
        //              servoGroup[0].SetServo(-(float)angle);
        //          }
        //      }

        //      if (i == 1 || i == 2)
        //      {
        //          Debug.Log("run servo 2");
        //          var targetOffset = servoGroup[i].servoBase.InverseTransformPoint(targetTransform.position);
        //          var angle0 = Math.Atan2(targetOffset.z, targetOffset.y);
        //          angle0 *= (180 / Math.PI);
        //          float angle1 = LawOfCosines(servoGroup[i].limbLength, servoGroup[i - 1].limbLength, Vector3.Distance(servoGroup[i].transform.position, targetTransform.position));
        //          //float angle1 = LawOfCosines(Vector3.Distance(servoGroup[i].transform.position, servoGroup[0].transform.position), servoGroup[0].limbLength, Vector3.Distance(servoGroup[i].transform.position, targetTransform.position));
        //          if (servo.invert)
        //          {
        //              angle1 -= (float)angle0;
        //          }
        //          else
        //          {
        //              angle1 += (float)angle0;
        //          }



        //          if (!float.IsNaN((float)angle1))
        //          {
        //              servoGroup[i].SetServo((float)angle1);
        //          }
        //          else
        //          {
        //              //  servoGroup[i].SetServo((float)angle0);
        //          }
        //      }
        // // }
    }
 // Use this for initialization
 void Start()
 {
     target = transform.Find("Target");
     DebugVector.DrawVector(target, Color.red, Color.green, Color.blue, Vector3.one);
 }