示例#1
0
        public override Plane CartesianLerp(Plane a, Plane b, double t, double min, double max)
        {
            t = (t - min) / (max - min);
            if (double.IsNaN(t))
            {
                t = 0;
            }

            var anglesA = RobotCellKuka.PlaneToEuler(a);
            var anglesB = RobotCellKuka.PlaneToEuler(b);
            var result  = new double[anglesA.Length];

            for (int i = 0; i < anglesA.Length; i++)
            {
                if (i < 3)
                {
                    result[i] = anglesA[i] * (1.0 - t) + anglesB[i] * t;
                }
                else
                {
                    double angleA   = anglesA[i].ToRadians();
                    double angleB   = anglesB[i].ToRadians();
                    double delta    = angleB - angleA;
                    double absDelta = Abs(delta);
                    if (absDelta > PI)
                    {
                        delta = Sign(delta) * (absDelta - PI * 2);
                    }

                    angleB = angleA + delta;

                    double angle = angleA * (1.0 - t) + angleB * t;
                    // if (i == 4) angle = Abs(angle);
                    var results = JointTarget.GetAbsoluteJoints(new double[] { angle }, new double[] { angleB });
                    result[i] = results[0].ToDegrees();
                }
            }

            return(RobotCellKuka.EulerToPlane(result[0], result[1], result[2], result[3], result[4], result[5]));
            // return base.CartesianLerp(a, b, t, min, max);
        }
示例#2
0
            internal KRLPostProcessor(RobotCellKuka robotCell, Program program)
            {
                this.cell    = robotCell;
                this.program = program;
                this.Code    = new List <List <List <string> > >();

                for (int i = 0; i < cell.MechanicalGroups.Count; i++)
                {
                    var groupCode = new List <List <string> >();

                    groupCode.Add(MainFile(i));
                    groupCode.Add(DatFile(i));

                    for (int j = 0; j < program.MultiFileIndices.Count; j++)
                    {
                        groupCode.Add(SrcFile(j, i));
                    }

                    Code.Add(groupCode);
                }
            }
示例#3
0
            internal KRLPostProcessor(RobotCellKuka robotCell, Program program)
            {
                this.cell = robotCell;
                this.program = program;
                this.Code = new List<List<List<string>>>();

                for (int i = 0; i < cell.MechanicalGroups.Count; i++)
                {
                    var groupCode = new List<List<string>>();

                    groupCode.Add(MainFile(i));
                    groupCode.Add(DatFile(i));

                    for (int j = 0; j < program.MultiFileIndices.Count; j++)
                        groupCode.Add(SrcFile(j, i));

                    Code.Add(groupCode);
                }
            }