Example #1
0
        public IEnumerator IKAuto(List <float> angles, List <LinkParam> linkParams, FKManager fk)
        {
            int culcCount = 0;

            double[,] q = angles.ToVertical();

            double[,] prev_e = new double[6, 1];

            while (true /*culcCount < 100000*/)
            {
                var HTMs = HomogeneourCoordinate.GetHTMs(linkParams, angles);

                var J   = fk.GetJacobian(HTMs).Matrix;
                var J_T = J.Transpose();

                var EndHTM = HTMs[HTMs.Count() - 1];
                var e_q    = Error(fk.GetEndPosition(EndHTM), EndHTM.RotationMatrix());

                for (int i = 0; i < 3; i++)
                {
                    W_e[i, i] = w_e_pos;
                }
                for (int i = 3; i < 6; i++)
                {
                    W_e[i, i] = w_e_rot;
                }

                var Jt_W_J_plus_Wn__inv = (J_T.Times(W_e).Times(J).Plus(W_n(e_q, W_e))).inverseMatrix();


                var delta_q = Jt_W_J_plus_Wn__inv.Times(J_T).Times(W_e).Times(e_q);

                q = q.Plus(delta_q);

                for (int i = 0; i < q.GetLength(0); i++)
                {
                    angles[i] = (float)q[i, 0];
                }


                prev_e = e_q;

                culcCount++;

                if (culcCount % 100 == 0)
                {
                    yield return(null);
                }
            }
            print("loop num: " + culcCount);
            yield break;
        }
Example #2
0
        IEnumerator TargetMove(Vector3 targetPos, List <List <float> > midsTargetAngles)
        {
            var startAngle = robot.GetAngles();

            var deltas = new List <float>();

            deltas.Add(GetMagnitude(startAngle, midsTargetAngles[0]));

            for (var i = 1; i < midsTargetAngles.Count(); ++i)
            {
                deltas.Add(GetMagnitude(midsTargetAngles[i - 1], midsTargetAngles[i]));
            }

            var allDelta = deltas.Sum();
            var phasies  = deltas.Select(d => d / allDelta).ToList();
            var moveTime = allDelta / aveSpeed;

            var currentTime = 0.0f;
            int moveloop    = 0;


            print("moving");

            int index = 0;

            FKManager.Foreach(linkParams, midsTargetAngles[index], fk);

            float offset   = 0;
            float progress = 0;

            while (!IsCloseEnough(robot.CurrentEndPosition(), targetPos) && moveloop < maxMoveLoop && currentTime < moveTime)
            {
                if (progress >= 1)
                {
                    offset += phasies[index];
                    ++index;
                    startAngle = robot.GetAngles();
                    FKManager.Foreach(linkParams, midsTargetAngles[index], fk);
                }
                robot.SetAngle(AnglesDeltaByPhase(robot.GetAngles(), startAngle, midsTargetAngles[index], offset, phasies[index], out progress, currentTime, moveTime));


                moveloop++;
                currentTime += Time.deltaTime;
                yield return(null);
            }
        }
Example #3
0
        IEnumerator AngleMove(List <float> targetAngle)
        {
            int moveloop = 0;
            var moveTime = GetMagnitude(robot.GetAngles(), targetAngle) / aveSpeed;

            var currentTime = 0.0f;

            FKManager.Foreach(linkParams, targetAngle, fk);
            print("moving");

            var startAngle = robot.GetAngles();

            while (robot.GetAngles().Where(a => a != 0).Any() && moveloop < maxMoveLoop && currentTime < moveTime)
            {
                robot.SetAngle(AnglesDelta(robot.GetAngles(), startAngle, targetAngle, currentTime, moveTime));
                moveloop++;
                currentTime += Time.deltaTime;
                yield return(null);
            }
        }
Example #4
0
        IEnumerator TargetMove(Vector3 targetPos, List <float> targetAngle)
        {
            var startAngle = robot.GetAngles();
            var moveTime   = GetMagnitude(robot.GetAngles(), targetAngle) / aveSpeed;

            var currentTime = 0.0f;
            int moveloop    = 0;

            FKManager.Foreach(linkParams, targetAngle, fk);

            print("moving");

            while (!IsCloseEnough(robot.CurrentEndPosition(), targetPos) && moveloop < maxMoveLoop && currentTime < moveTime)
            {
                robot.SetAngle(AnglesDelta(robot.GetAngles(), startAngle, targetAngle, currentTime, moveTime));
                moveloop++;
                currentTime += Time.deltaTime;
                yield return(null);
            }
        }
Example #5
0
 public static Vector3 GetEndPosition(List <LinkParam> linkParams, List <float> angles, FKManager fk)
 {
     return(HomogeneourCoordinate.GetHTMs(linkParams, angles)[~1] * fk.hE);
 }
Example #6
0
 public static void Foreach(List <LinkParam> linkParams, List <float> angles, FKManager fk)
 {
     fk.Foreach(HomogeneourCoordinate.GetHTMs(linkParams, angles));
 }
Example #7
0
        IEnumerator TaskSequence()
        {
            List <float> targetAngle = new List <float>();

            fk         = robot.GetFK();
            linkParams = robot.GetLinkParams();

            var taskCount = 0;

            int moveloop;

            foreach (var task in taskObjects)
            {
                robot.SetInControl(true);

                var midAngleList = new List <List <float> >();

                var taskAbove = task.position + new Vector3(0, 0.1f, 0);
                targetAngle = robot.GetAngles();

                if (taskCount == 0)
                {
                    yield return(StartCoroutine(robot.CulcIK(targetAngle, taskAbove, Quaternion.Euler(-90, -90, 0))));

                    yield return(StartCoroutine(TargetMove(taskAbove, targetAngle)));
                }
                else
                {
                    midAngleList = new List <List <float> >();
                    foreach (var p in midPoints)
                    {
                        yield return(StartCoroutine(robot.CulcIK(targetAngle, p.position, Quaternion.Euler(-90, 180, 0))));

                        midAngleList.Add(new List <float>(targetAngle));
                    }

                    midAngleList.Reverse();

                    yield return(StartCoroutine(robot.CulcIK(targetAngle, taskAbove, Quaternion.Euler(-90, -90, 0))));

                    midAngleList.Add(new List <float>(targetAngle));

                    yield return(StartCoroutine(TargetMove(taskAbove, midAngleList)));
                }



                yield return(StartCoroutine(robot.CulcIK(targetAngle, task.position + new Vector3(0, 0.01f, 0), Quaternion.Euler(-90, -90, 0))));

                yield return(StartCoroutine(TargetMove(task.position + new Vector3(0, 0.01f, 0), targetAngle)));


                moveloop = 0;
                print("gripping");
                float gripWidth = 0;
                while (gripWidth < 0.28f)
                {
                    gripWidth += pg;
                    grip.SetWidth(gripWidth);
                    moveloop++;
                    yield return(null);
                }


                yield return(StartCoroutine(robot.CulcIK(targetAngle, taskAbove, Quaternion.Euler(-90, -90, 0))));

                yield return(StartCoroutine(TargetMove(taskAbove, targetAngle)));


                midAngleList = new List <List <float> >();
                foreach (var p in midPoints)
                {
                    yield return(StartCoroutine(robot.CulcIK(targetAngle, p.position, Quaternion.Euler(-90, 180, 0))));

                    midAngleList.Add(new List <float>(targetAngle));
                }
                yield return(StartCoroutine(robot.CulcIK(targetAngle, goalArea.transform.position, Quaternion.Euler(-90, 90, 0))));

                midAngleList.Add(new List <float>(targetAngle));



                yield return(StartCoroutine(TargetMove(goalArea.position, midAngleList)));



                //FKManager.Foreach(linkParams, targetAngle, fk);

                //yield return StartCoroutine(TargetMove(goalArea.position, targetAngle));



                moveloop = 0;
                print("gripping");
                while (gripWidth >= 0)
                {
                    gripWidth -= pg;
                    grip.SetWidth(gripWidth);
                    moveloop++;
                    yield return(null);
                }
                ++taskCount;
            }

            targetAngle = targetAngle.Select(a => 0f).ToList();


            yield return(StartCoroutine(AngleMove(targetAngle)));

            robot.SetInControl(false);
        }