示例#1
0
            double[] GetClosestSolution(Transform transform, double[] prevJoints, out Target.RobotConfigurations configuration, out List <string> errors, out double difference)
            {
                var solutions       = new double[8][];
                var solutionsErrors = new List <List <string> >(8);

                for (int i = 0; i < 8; i++)
                {
                    solutions[i] = InverseKinematics(transform, (Target.RobotConfigurations)i, out var solutionErrors);
                    solutions[i] = JointTarget.GetAbsoluteJoints(solutions[i], prevJoints);
                    solutionsErrors.Add(solutionErrors);
                }

                int    closestSolutionIndex = 0;
                double closestDifference    = double.MaxValue;

                for (int i = 0; i < 8; i++)
                {
                    double currentDifference = prevJoints.Zip(solutions[i], (x, y) => SquaredDifference(x, y)).Sum();

                    if (currentDifference < closestDifference)
                    {
                        closestSolutionIndex = i;
                        closestDifference    = currentDifference;
                    }
                }

                difference    = closestDifference;
                configuration = (Target.RobotConfigurations)closestSolutionIndex;
                errors        = solutionsErrors[closestSolutionIndex];
                return(solutions[closestSolutionIndex]);
            }
示例#2
0
        public JointTarget GetPosition()
        {
            if (!Connected)
            {
                Connect();            // return Target.Default as JointTarget;
            }
            var joints = _controller.MotionSystem.ActiveMechanicalUnit.GetPosition();
            var values = new double[] { joints.RobAx.Rax_1, joints.RobAx.Rax_2, joints.RobAx.Rax_3, joints.RobAx.Rax_4, joints.RobAx.Rax_5, joints.RobAx.Rax_6 };

            for (int i = 0; i < 6; i++)
            {
                values[i] = RobotAbb.ABBDegreeToRadian(values[i], i);
            }

            var target = new JointTarget(values);

            return(target);
        }
示例#3
0
 protected override void SetJoints(Target target, double[] prevJoints)
 {
     for (int i = 0; i < mechanism.Joints.Length; i++)
     {
         int externalNum = mechanism.Joints[i].Number - 6;
         if (target.External.Length - 1 < externalNum)
         {
             Errors.Add($"Positioner external axis not configured on this target.");
         }
         else
         {
             Joints[i] = target.External[externalNum];
         }
     }
     if (prevJoints != null)
     {
         Joints = JointTarget.GetAbsoluteJoints(Joints, prevJoints);
     }
 }
示例#4
0
            protected override void SetJoints(Target target, double[] prevJoints)
            {
                if (target is JointTarget)
                {
                    Joints = (target as JointTarget).Joints;
                }
                else if (target is CartesianTarget)
                {
                    double[] joints          = null;
                    var      cartesianTarget = target as CartesianTarget;
                    Plane    tcp             = target.Tool.Tcp;
                    tcp.Rotate(PI, Vector3d.ZAxis, Point3d.Origin);

                    Plane targetPlane = cartesianTarget.Plane;
                    targetPlane.Transform(target.Frame.Plane.ToTransform());

                    var transform = Transform.PlaneToPlane(Planes[0], Plane.WorldXY) * Transform.PlaneToPlane(tcp, targetPlane);

                    List <string> errors;

                    if (cartesianTarget.Configuration != null || prevJoints == null)
                    {
                        Configuration = cartesianTarget.Configuration ?? Target.RobotConfigurations.None;
                        joints        = InverseKinematics(transform, Configuration, out errors);
                    }
                    else
                    {
                        joints        = GetClosestSolution(transform, prevJoints, out var configuration, out errors, out var difference);
                        Configuration = configuration;
                    }

                    if (prevJoints != null)
                    {
                        Joints = JointTarget.GetAbsoluteJoints(joints, prevJoints);
                    }
                    else
                    {
                        Joints = joints;
                    }

                    Errors.AddRange(errors);
                }
            }
示例#5
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);
        }
示例#6
0
        public Target Lerp(ProgramTarget prevTarget, RobotSystem robot, double t, double start, double end)
        {
            double[] allJoints = JointTarget.Lerp(prevTarget.Kinematics.Joints, Kinematics.Joints, t, start, end);
            var      external  = allJoints.RangeSubset(6, Target.External.Length);

            if (IsJointMotion)
            {
                var joints = allJoints.RangeSubset(0, 6);
                return(new JointTarget(joints, Target, external));
            }
            else
            {
                Plane prevPlane = GetPrevPlane(prevTarget);
                Plane plane     = robot.CartesianLerp(prevPlane, Plane, t, start, end);
                //   Plane plane = CartesianTarget.Lerp(prevTarget.WorldPlane, this.WorldPlane, t, start, end);
                //  Target.RobotConfigurations? configuration = (Abs(prevTarget.cellTarget.TotalTime - t) < TimeTol) ? prevTarget.Kinematics.Configuration : this.Kinematics.Configuration;

                var target = new CartesianTarget(plane, Target, prevTarget.Kinematics.Configuration, Motions.Linear, external);
                // target.Frame = Frame.Default;
                return(target);
            }
        }
示例#7
0
 static Target()
 {
     Default = new JointTarget(new double[] { 0, PI / 2, 0, 0, 0, 0 });
 }
示例#8
0
 static Target()
 {
     Default = new JointTarget(new double[] { 0, PI / 2, 0, 0, 0, 0 }, Tool.Default, Speed.Default, Zone.Default, Command.Default, Frame.Default, null);
 }