Exemple #1
0
 protected override void SetJoints(Target target, double[] prevJoints)
 {
     int externalNum = mechanism.Joints[0].Number - 6;
     if (target.External.Length < externalNum)
         Errors.Add($"Track external axis not configured on this target.");
     else
         Joints[0] = target.External[externalNum];
 }
Exemple #2
0
            protected override void SetPlanes(Target target)
            {
                int jointCount = mechanism.Joints.Length;

                for (int i = 0; i < jointCount; i++)
                {
                    Planes[i + 1] = mechanism.Joints[i].Plane;
                    for (int j = i; j >= 0; j--)
                        Planes[i + 1].Rotate(Joints[j], mechanism.Joints[j].Plane.Normal, mechanism.Joints[j].Plane.Origin);
                }
            }
Exemple #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);

            }
Exemple #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);
                }
            }
Exemple #5
0
            protected override void SetPlanes(Target target)
            {
                var jointTransforms = ForwardKinematics(Joints);

                if (target is JointTarget)
                {
                    var closest = GetClosestSolution(jointTransforms[jointTransforms.Length - 1], Joints, out var configuration, out var errors, out var difference);
                    this.Configuration = difference < AngleTol ? configuration : Target.RobotConfigurations.Undefined;
                }

                for (int i = 0; i < 6; i++)
                {
                    var plane = jointTransforms[i].ToPlane();
                    plane.Rotate(PI, plane.ZAxis);
                    Planes[i + 1] = plane;
                }

                /* 
                 {
                     Planes[7] = target.Tool.Tcp;
                     Planes[7].Transform(Planes[6].ToTransform());
                 }
                */
            }
Exemple #6
0
        internal ProgramTarget(Target target, int group)
        {
            this.Target = target;
            this.Group = group;

            this.Commands = new Commands.Group();

            if (target.Command != null)
            {
                if (target.Command is Commands.Group)
                    Commands.AddRange((target.Command as Commands.Group).Flatten());
                else
                    Commands.Add(target.Command);
            }
        }
Exemple #7
0
 public OffsetWristKinematics(RobotArm robot, Target target, double[] prevJoints, bool displayMeshes, Plane? basePlane) : base(robot, target, prevJoints, displayMeshes, basePlane) { }
Exemple #8
0
 protected RobotKinematics(RobotArm robot, Target target, double[] prevJoints = null, bool displayMeshes = false, Plane? basePlane = null) : base(robot, target, prevJoints, displayMeshes, basePlane) { }
Exemple #9
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];
            }
Exemple #10
0
 public JointTarget(double[] joints, Target target, IEnumerable<double> external = null) : this(joints, target.Tool, target.Speed, target.Zone, target.Command, target.Frame, external ?? target.External) { }
Exemple #11
0
        /*
        public static void WriteMeshes()
        {
            Rhino.FileIO.File3dm robotsGeometry = Rhino.FileIO.File3dm.Read($@"{ResourcesFolder}\robotsGeometry.3dm");
            var jointmeshes = new JointMeshes();

            foreach (var layer in robotsGeometry.Layers)
            {
                if (layer.Name == "Default" || layer.ParentLayerId != Guid.Empty) continue;
                jointmeshes.Names.Add(layer.Name);
                var meshes = new List<Mesh>();
                meshes.Add(robotsGeometry.Objects.First(x => x.Attributes.LayerIndex == layer.LayerIndex).Geometry as Mesh);

                int i = 0;
                while (true)
                {
                    string name = $"{i++ + 1}";
                    var jointLayer = robotsGeometry.Layers.FirstOrDefault(x => (x.Name == name) && (x.ParentLayerId == layer.Id));
                    if (jointLayer == null) break;
                    meshes.Add(robotsGeometry.Objects.First(x => x.Attributes.LayerIndex == jointLayer.LayerIndex).Geometry as Mesh);
                }
                jointmeshes.Meshes.Add(meshes);
            }

            using (var stream = new MemoryStream())
            {
                var formatter = new BinaryFormatter();
                formatter.Serialize(stream, jointmeshes);
                File.WriteAllBytes($@"{ResourcesFolder}\Meshes.rob", stream.ToArray());
            }
        }
        */

        public abstract KinematicSolution Kinematics(Target target, double[] prevJoints = null, bool displayMeshes = false, Plane? basePlane = null);
Exemple #12
0
 protected override void SetPlanes(Target target)
 {
     Planes[1] = mechanism.Joints[0].Plane;
     Planes[1].Translate(Planes[1].YAxis * Joints[0]);
 }
Exemple #13
0
 internal TrackKinematics(Track track, Target target, bool displayMeshes, Plane? basePlane) : base(track, target, null, displayMeshes, basePlane) { }
Exemple #14
0
 public override KinematicSolution Kinematics(Target target, double[] prevJoints = null, bool displayMeshes = false, Plane? basePlane = null) => new TrackKinematics(this, target, displayMeshes, basePlane);
Exemple #15
0
 static Target()
 {
     Default = new JointTarget(new double[] { 0, PI / 2, 0, 0, 0, 0 }, Tool.Default, Speed.Default, Zone.Default, null, Frame.Default, null);
 }
Exemple #16
0
 public CartesianTarget(Plane plane, Target target, RobotConfigurations? configuration = null, Motions motion = Motions.Joint, IEnumerable<double> external = null) : this(plane, configuration, motion, target.Tool, target.Speed, target.Zone, target.Command, target.Frame, external ?? target.External) { }
Exemple #17
0
            internal MechanismKinematics(Mechanism mechanism, Target target, double[] prevJoints, bool displayMeshes, Plane? basePlane)
            {
                this.mechanism = mechanism;
                int jointCount = mechanism.Joints.Length;

                // Init properties
                Joints = new double[jointCount];
                Planes = new Plane[jointCount + 1];
                if (displayMeshes)
                    Meshes = new Mesh[jointCount + 1];
                else
                    Meshes = new Mesh[0];

                // Base plane
                Planes[0] = mechanism.BasePlane;

                if (basePlane != null)
                {

                    Planes[0].Transform(Transform.PlaneToPlane(Plane.WorldXY,(Plane)basePlane));
                }

                SetJoints(target, prevJoints);
                JointsOutOfRange();

                SetPlanes(target);

                // Move planes to base
                var transform = Planes[0].ToTransform();
                for (int i = 1; i < jointCount + 1; i++)
                    Planes[i].Transform(transform);

                // Meshes
                if (displayMeshes)
                    SetMeshes(target.Tool);
            }
Exemple #18
0
 public override KinematicSolution Kinematics(Target target, double[] prevJoints, bool displayMeshes = true, Plane? basePlane = null) => new OffsetWristKinematics(this, target, prevJoints, displayMeshes, basePlane);
Exemple #19
0
 protected abstract void SetJoints(Target target, double[] prevJoints);
Exemple #20
0
            /// <summary>
            /// Inverse kinematics for a spherical wrist 6 axis robot.
            /// Code adapted from https://github.com/whitegreen/KinematikJava
            /// </summary>
            /// <param name="target">Cartesian target</param>
            /// <returns>Returns the 6 rotation values in radians.</returns>
            override protected double[] InverseKinematics(Transform transform, Target.RobotConfigurations configuration, out List<string> errors)
            {
                errors = new List<string>();

                bool shoulder = configuration.HasFlag(Target.RobotConfigurations.Shoulder);
                bool elbow = configuration.HasFlag(Target.RobotConfigurations.Elbow);
                if (shoulder) elbow = !elbow;
                bool wrist = !configuration.HasFlag(Target.RobotConfigurations.Wrist);

                bool isUnreachable = false;

                double[] a = mechanism.Joints.Select(joint => joint.A).ToArray();
                double[] d = mechanism.Joints.Select(joint => joint.D).ToArray();

                Plane flange = Plane.WorldXY;
                flange.Transform(transform);

                double[] joints = new double[6];

                double l2 = Sqrt(a[2] * a[2] + d[3] * d[3]);
                double ad2 = Atan2(a[2], d[3]);
                Point3d center = flange.Origin - flange.Normal * d[5];
                joints[0] = Atan2(center.Y, center.X);
                double ll = Sqrt(center.X * center.X + center.Y * center.Y);
                Point3d p1 = new Point3d(a[0] * center.X / ll, a[0] * center.Y / ll, d[0]);

                if (shoulder)
                {
                    joints[0] += PI;
                    var rotate = Transform.Rotation(PI, new Point3d(0, 0, 0));
                    center.Transform(rotate);
                }

                double l3 = (center - p1).Length;
                double l1 = a[1];
                double beta = Acos((l1 * l1 + l3 * l3 - l2 * l2) / (2 * l1 * l3));
                if (double.IsNaN(beta))
                {
                    beta = 0;
                    isUnreachable = true;
                }
                if (elbow)
                    beta *= -1;

                double ttl = new Vector3d(center.X - p1.X, center.Y - p1.Y, 0).Length;
                // if (p1.X * (center.X - p1.X) < 0)
                if (shoulder)
                    ttl = -ttl;
                double al = Atan2(center.Z - p1.Z, ttl);

                joints[1] = beta + al;

                double gama = Acos((l1 * l1 + l2 * l2 - l3 * l3) / (2 * l1 * l2));
                if (double.IsNaN(gama))
                {
                    gama = PI;
                    isUnreachable = true;
                }
                if (elbow)
                    gama *= -1;

                joints[2] = gama - ad2 - PI / 2;

                double[] c = new double[3];
                double[] s = new double[3];
                for (int i = 0; i < 3; i++)
                {
                    c[i] = Cos(joints[i]);
                    s[i] = Sin(joints[i]);
                }

                var arr = new Transform();
                arr[0, 0] = c[0] * (c[1] * c[2] - s[1] * s[2]); arr[0, 1] = s[0]; arr[0, 2] = c[0] * (c[1] * s[2] + s[1] * c[2]); arr[0, 3] = c[0] * (a[2] * (c[1] * c[2] - s[1] * s[2]) + a[1] * c[1]) + a[0] * c[0];
                arr[1, 0] = s[0] * (c[1] * c[2] - s[1] * s[2]); arr[1, 1] = -c[0]; arr[1, 2] = s[0] * (c[1] * s[2] + s[1] * c[2]); arr[1, 3] = s[0] * (a[2] * (c[1] * c[2] - s[1] * s[2]) + a[1] * c[1]) + a[0] * s[0];
                arr[2, 0] = s[1] * c[2] + c[1] * s[2]; arr[2, 1] = 0; arr[2, 2] = s[1] * s[2] - c[1] * c[2]; arr[2, 3] = a[2] * (s[1] * c[2] + c[1] * s[2]) + a[1] * s[1] + d[0];
                arr[3, 0] = 0; arr[3, 1] = 0; arr[3, 2] = 0; arr[3, 3] = 1;

                arr.TryGetInverse(out var in123);

                var mr = Transform.Multiply(in123, transform);
                joints[3] = Atan2(mr[1, 2], mr[0, 2]);
                joints[4] = Acos(mr[2, 2]);
                joints[5] = Atan2(mr[2, 1], -mr[2, 0]);

                if (wrist)
                {
                    joints[3] += PI;
                    joints[4] *= -1;
                    joints[5] -= PI;
                }

                for (int i = 0; i < 6; i++)
                {
                    if (joints[i] > PI) joints[i] -= 2 * PI;
                    if (joints[i] < -PI) joints[i] += 2 * PI;
                }

                if (isUnreachable)
                    errors.Add($"Target out of reach");

                if (Abs(1 - mr[2, 2]) < 0.0001)
                    errors.Add($"Near wrist singularity");

                if (new Vector3d(center.X, center.Y, 0).Length < a[0] + SingularityTol)
                    errors.Add($"Near overhead singularity");


                for (int i = 0; i < 6; i++)
                {
                    if (double.IsNaN(joints[i])) joints[i] = 0;
                }
                return joints;
            }
Exemple #21
0
 protected abstract void SetPlanes(Target target);
Exemple #22
0
 public KinematicSolution Kinematics(Target target, double[] prevJoints = null, Plane? coupledPlane = null, bool displayMeshes = false, Plane? basePlane = null) => new MechanicalGroupKinematics(this, target, prevJoints, coupledPlane, displayMeshes, basePlane);
Exemple #23
0
        public double[] RadiansToDegreesExternal(Target target)
        {
            double[] values = new double[target.External.Length];

            foreach (var mechanism in this.Externals)
            {
                foreach (var joint in mechanism.Joints)
                {
                    values[joint.Number - 6] = mechanism.RadianToDegree(target.External[joint.Number - 6], joint.Index);
                }
            }

            return values;
        }
Exemple #24
0
            /// <summary>
            /// Inverse kinematics for a offset wrist 6 axis robot.
            /// Code adapted from https://github.com/ros-industrial/universal_robot/blob/indigo-devel/ur_kinematics/src/ur_kin.cpp
            /// </summary>
            /// <param name="target">Cartesian target</param>
            /// <returns>Returns the 6 rotation values in radians.</returns>
            override protected double[] InverseKinematics(Transform transform, Target.RobotConfigurations configuration, out List<string> errors)
            {
                errors = new List<string>();

                bool shoulder = configuration.HasFlag(Target.RobotConfigurations.Shoulder);
                bool elbow = configuration.HasFlag(Target.RobotConfigurations.Elbow);
                if (shoulder) elbow = !elbow;
                bool wrist = !configuration.HasFlag(Target.RobotConfigurations.Wrist);
                if (shoulder) wrist = !wrist;

                double[] joints = new double[6];
                bool isUnreachable = false;

                transform *= Transform.Rotation(PI / 2, Point3d.Origin);

                double[] a = mechanism.Joints.Select(joint => joint.A).ToArray();
                double[] d = mechanism.Joints.Select(joint => joint.D).ToArray();

                // shoulder
                {
                    double A = d[5] * transform[1, 2] - transform[1, 3];
                    double B = d[5] * transform[0, 2] - transform[0, 3];
                    double R = A * A + B * B;

                    double arccos = Acos(d[3] / Sqrt(R));
                    if (double.IsNaN(arccos))
                    {
                        errors.Add($"Overhead singularity.");
                        arccos = 0;
                    }

                    double arctan = Atan2(-B, A);

                    if (!shoulder)
                        joints[0] = arccos + arctan;
                    else
                        joints[0] = -arccos + arctan;
                }

                // wrist 2
                {
                    double numer = (transform[0, 3] * Sin(joints[0]) - transform[1, 3] * Cos(joints[0]) - d[3]);
                    double div = numer / d[5];

                    double arccos = Acos(div);
                    if (double.IsNaN(arccos))
                    {
                        errors.Add($"Overhead singularity 2.");
                        arccos = PI;
                        isUnreachable = true;
                    }

                    if (!wrist)
                        joints[4] = arccos;
                    else
                        joints[4] = 2.0 * PI - arccos;
                }

                // rest
                {
                    double c1 = Cos(joints[0]);
                    double s1 = Sin(joints[0]);
                    double c5 = Cos(joints[4]);
                    double s5 = Sin(joints[4]);

                    joints[5] = Atan2(Sign(s5) * -(transform[0, 1] * s1 - transform[1, 1] * c1), Sign(s5) * (transform[0, 0] * s1 - transform[1, 0] * c1));

                    double c6 = Cos(joints[5]), s6 = Sin(joints[5]);
                    double x04x = -s5 * (transform[0, 2] * c1 + transform[1, 2] * s1) - c5 * (s6 * (transform[0, 1] * c1 + transform[1, 1] * s1) - c6 * (transform[0, 0] * c1 + transform[1, 0] * s1));
                    double x04y = c5 * (transform[2, 0] * c6 - transform[2, 1] * s6) - transform[2, 2] * s5;
                    double p13x = d[4] * (s6 * (transform[0, 0] * c1 + transform[1, 0] * s1) + c6 * (transform[0, 1] * c1 + transform[1, 1] * s1)) - d[5] * (transform[0, 2] * c1 + transform[1, 2] * s1) + transform[0, 3] * c1 + transform[1, 3] * s1;
                    double p13y = transform[2, 3] - d[0] - d[5] * transform[2, 2] + d[4] * (transform[2, 1] * c6 + transform[2, 0] * s6);
                    double c3 = (p13x * p13x + p13y * p13y - a[1] * a[1] - a[2] * a[2]) / (2.0 * a[1] * a[2]);

                    double arccos = Acos(c3);
                    if (double.IsNaN(arccos))
                    {
                        arccos = 0;
                        isUnreachable = true;
                    }

                    if (!elbow)
                        joints[2] = arccos;
                    else
                        joints[2] = 2.0 * PI - arccos;

                    double denom = a[1] * a[1] + a[2] * a[2] + 2 * a[1] * a[2] * c3;
                    double s3 = Sin(arccos);
                    double A = (a[1] + a[2] * c3);
                    double B = a[2] * s3;

                    if (!elbow)
                        joints[1] = Atan2((A * p13y - B * p13x) / denom, (A * p13x + B * p13y) / denom);
                    else
                        joints[1] = Atan2((A * p13y + B * p13x) / denom, (A * p13x - B * p13y) / denom);

                    double c23_0 = Cos(joints[1] + joints[2]);
                    double s23_0 = Sin(joints[1] + joints[2]);

                    joints[3] = Atan2(c23_0 * x04y - s23_0 * x04x, x04x * c23_0 + x04y * s23_0);
                }

                if (isUnreachable)
                    errors.Add($"Target out of reach.");

                //   joints[5] += PI / 2;

                for (int i = 0; i < 6; i++)
                {
                    if (joints[i] > PI) joints[i] -= 2.0 * PI;
                    if (joints[i] < -PI) joints[i] += 2.0 * PI;
                }

                return joints;
            }
Exemple #25
0
            internal MechanicalGroupKinematics(MechanicalGroup group, Target target, double[] prevJoints, Plane? coupledPlane, bool displayMeshes, Plane? basePlane)
            {
                var jointCount = group.Joints.Count;
                Joints = new double[jointCount];
                var planes = new List<Plane>();
                List<Mesh> meshes = (displayMeshes) ? new List<Mesh>() : null;
                var errors = new List<string>();

                Plane? robotBase = basePlane;

                target = target.ShallowClone();
                Mechanism coupledMech = null;

                if (target.Frame.CoupledMechanism != -1 && target.Frame.CoupledMechanicalGroup == group.Index)
                {
                    coupledMech = group.Externals[target.Frame.CoupledMechanism];
                }


                // Externals
                foreach (var external in group.Externals)
                {
                    var externalPrevJoints = prevJoints == null ? null : prevJoints.Subset(external.Joints.Select(x => x.Number).ToArray());
                    var externalKinematics = external.Kinematics(target, externalPrevJoints, displayMeshes, basePlane);

                    for (int i = 0; i < external.Joints.Length; i++)
                        Joints[external.Joints[i].Number] = externalKinematics.Joints[i];

                    planes.AddRange(externalKinematics.Planes);
                    if (displayMeshes) meshes.AddRange(externalKinematics.Meshes);
                    errors.AddRange(externalKinematics.Errors);

                    if (external == coupledMech)
                        coupledPlane = externalKinematics.Planes[externalKinematics.Planes.Length - 1];

                    if (external.MovesRobot)
                        robotBase = externalKinematics.Planes[externalKinematics.Planes.Length - 1];
                }

                // Coupling
                if (coupledPlane != null)
                {
                    var coupledFrame = target.Frame.ShallowClone();
                    var plane = coupledFrame.Plane;
                    plane.Transform(Transform.PlaneToPlane(Plane.WorldXY, (Plane)coupledPlane));
                    coupledFrame.Plane = plane;
                    target.Frame = coupledFrame;
                }

                // Robot
                var robot = group.Robot;

                if (robot != null)
                {
                    var robotPrevJoints = prevJoints == null ? null : prevJoints.Subset(robot.Joints.Select(x => x.Number).ToArray());
                    var robotKinematics = robot.Kinematics(target, robotPrevJoints, displayMeshes, robotBase);

                    for (int j = 0; j < robot.Joints.Length; j++)
                        Joints[robot.Joints[j].Number] = robotKinematics.Joints[j];

                    planes.AddRange(robotKinematics.Planes);
                    if (displayMeshes) meshes.AddRange(robotKinematics.Meshes);
                    Configuration = robotKinematics.Configuration;
                    if (robotKinematics.Errors.Count > 0)
                    {
                        errors.AddRange(robotKinematics.Errors);
                    }
                }

                // Tool
                    Plane toolPlane = target.Tool.Tcp;
                    toolPlane.Transform(planes[planes.Count - 1].ToTransform());
                    planes.Add(toolPlane);


                if (displayMeshes)
                {
                    if (target.Tool.Mesh != null)
                    {
                        Mesh toolMesh = target.Tool.Mesh.DuplicateMesh();
                        toolMesh.Transform(Transform.PlaneToPlane(target.Tool.Tcp, planes[planes.Count - 1]));
                        meshes.Add(toolMesh);
                    }
                    else
                        meshes.Add(null);
                }

                Planes = planes.ToArray();
                if (displayMeshes) Meshes = meshes.ToArray();

                if (errors.Count > 0)
                {
                    Errors.AddRange(errors);
                }
            }
Exemple #26
0
 protected abstract double[] InverseKinematics(Transform transform, Target.RobotConfigurations configuration, out List<string> errors);
Exemple #27
0
 internal PositionerKinematics(Positioner positioner, Target target, double[] prevJoints, bool displayMeshes, Plane? basePlane) : base(positioner, target, prevJoints, displayMeshes, basePlane) { }