Ejemplo n.º 1
0
        private void DrawJoint(KinematicJoint joint)
        {
            Vector3 connection = joint.GetAnchorInWorldSpace();

            //DrawSphere(connection, JointSize, JointColor);
            DrawCube(connection, joint.transform.rotation * Quaternion.Euler(joint.GetOrientation()), JointSize, JointColor);
            DrawLine(joint.transform.position, joint.GetAnchorInWorldSpace(), JointColor);

            //GUIStyle style = new GUIStyle();
            //style.normal.textColor = Color.black;
            //Handles.Label(connection, joint.name, style);

            if (joint.GetXMotion().IsEnabled())
            {
                Handles.color = Color.red;
                Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.GetXMotion().Axis), ArrowSize);
            }
            if (joint.GetYMotion().IsEnabled())
            {
                Handles.color = Color.green;
                Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.GetYMotion().Axis), ArrowSize);
            }
            if (joint.GetZMotion().IsEnabled())
            {
                Handles.color = Color.blue;
                Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.GetZMotion().Axis), ArrowSize);
            }
        }
Ejemplo n.º 2
0
            private void VisualizeKinematicJoint(KinematicJoint joint)
            {
                Vector3 connection = joint.ComputeConnectionInWorldSpace();

                Handles.color = Color.magenta;
                Handles.SphereCap(0, connection, Quaternion.identity, 1 / 100f);

                GUIStyle style = new GUIStyle();

                style.normal.textColor = Color.black;
                Handles.Label(connection, joint.name, style);

                if (joint.XMotion.State == JointState.Free)
                {
                    Handles.color = Color.red;
                    Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.ComputeXAxis()), 0.1f);
                }
                if (joint.YMotion.State == JointState.Free)
                {
                    Handles.color = Color.green;
                    Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.ComputeYAxis()), 0.1f);
                }
                if (joint.ZMotion.State == JointState.Free)
                {
                    Handles.color = Color.blue;
                    Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.ComputeZAxis()), 0.1f);
                }
            }
Ejemplo n.º 3
0
 void Awake()
 {
     Target = (KinematicJoint)target;
     Target.XMotion.Joint = Target;
     Target.YMotion.Joint = Target;
     Target.ZMotion.Joint = Target;
 }
Ejemplo n.º 4
0
        public Chain(Transform start, Transform end)
        {
            List <Transform>      segments = new List <Transform>();
            List <KinematicJoint> joints   = new List <KinematicJoint>();

            Length = 0f;
            DoF    = 0;

            Transform t = end;

            while (true)
            {
                segments.Add(t);
                KinematicJoint joint = t.GetComponent <KinematicJoint>();
                if (joint != null)
                {
                    if (joint.GetDOF() != 0)
                    {
                        joints.Add(joint);
                    }
                }
                if (t == start)
                {
                    break;
                }
                else
                {
                    t = t.parent;
                }
            }

            segments.Reverse();
            joints.Reverse();
            Segments = segments.ToArray();
            Joints   = joints.ToArray();

            if (Joints.Length == 0)
            {
                Length = 0f;
            }
            else
            {
                Vector3 reference = Joints[0].ComputeConnectionInWorldSpace();
                for (int i = 1; i < Joints.Length; i++)
                {
                    Length   += Vector3.Distance(reference, Joints[i].ComputeConnectionInWorldSpace());
                    reference = Joints[i].ComputeConnectionInWorldSpace();
                }
                Length += Vector3.Distance(reference, end.position);
            }

            for (int i = 0; i < Joints.Length; i++)
            {
                DoF += Joints[i].GetDOF();
            }
        }
Ejemplo n.º 5
0
        //Adds a segment node into the OFKT data structure
        private void AddNode(Transform segment)
        {
            if (FindNode(segment) == null)
            {
                KinematicJoint joint     = segment.GetComponent <KinematicJoint>();
                Objective      objective = segment.GetComponent <Objective>();

                Node node = new Node(this, FindNode(segment.parent), segment, joint, objective);

                if (joint != null)
                {
                    if (joint.GetDoF() == 0)
                    {
                        joint = null;
                    }
                    else
                    {
                        if (joint.GetXMotion().IsEnabled())
                        {
                            MotionPtr motionPtr = new MotionPtr(joint.GetXMotion(), node, MotionPtrs.Length);
                            System.Array.Resize(ref MotionPtrs, MotionPtrs.Length + 1);
                            MotionPtrs[MotionPtrs.Length - 1] = motionPtr;
                            node.XEnabled = true;
                            node.XIndex   = motionPtr.Index;
                        }
                        if (joint.GetYMotion().IsEnabled())
                        {
                            MotionPtr motionPtr = new MotionPtr(joint.GetYMotion(), node, MotionPtrs.Length);
                            System.Array.Resize(ref MotionPtrs, MotionPtrs.Length + 1);
                            MotionPtrs[MotionPtrs.Length - 1] = motionPtr;
                            node.YEnabled = true;
                            node.YIndex   = motionPtr.Index;
                        }
                        if (joint.GetZMotion().IsEnabled())
                        {
                            MotionPtr motionPtr = new MotionPtr(joint.GetZMotion(), node, MotionPtrs.Length);
                            System.Array.Resize(ref MotionPtrs, MotionPtrs.Length + 1);
                            MotionPtrs[MotionPtrs.Length - 1] = motionPtr;
                            node.ZEnabled = true;
                            node.ZIndex   = motionPtr.Index;
                        }
                    }
                }

                if (objective != null)
                {
                    System.Array.Resize(ref ObjectivePtrs, ObjectivePtrs.Length + 1);
                    ObjectivePtrs[ObjectivePtrs.Length - 1] = new ObjectivePtr(segment.GetComponent <Objective>(), node);
                }

                System.Array.Resize(ref Nodes, Nodes.Length + 1);
                Nodes[Nodes.Length - 1] = node;
            }
        }
Ejemplo n.º 6
0
        private void AddNode(Transform segment)
        {
            if (FindNode(segment) == null)
            {
                KinematicJoint joint   = segment.GetComponent <KinematicJoint>();
                MotionPtr[]    motions = new MotionPtr[3];

                Node node = new Node(this, FindNode(segment.parent), segment, joint, motions);

                if (joint != null)
                {
                    if (joint.GetDOF() == 0)
                    {
                        joint = null;
                    }
                    else
                    {
                        if (joint.XMotion.State != JointState.Fixed)
                        {
                            MotionPtr motionPtr = new MotionPtr(joint.XMotion, node, Motions.Length);
                            System.Array.Resize(ref Motions, Motions.Length + 1);
                            Motions[Motions.Length - 1] = motionPtr;
                            motions[0] = motionPtr;
                        }
                        if (joint.YMotion.State != JointState.Fixed)
                        {
                            MotionPtr motionPtr = new MotionPtr(joint.YMotion, node, Motions.Length);
                            System.Array.Resize(ref Motions, Motions.Length + 1);
                            Motions[Motions.Length - 1] = motionPtr;
                            motions[1] = motionPtr;
                        }
                        if (joint.ZMotion.State != JointState.Fixed)
                        {
                            MotionPtr motionPtr = new MotionPtr(joint.ZMotion, node, Motions.Length);
                            System.Array.Resize(ref Motions, Motions.Length + 1);
                            Motions[Motions.Length - 1] = motionPtr;
                            motions[2] = motionPtr;
                        }
                    }
                }

                IKTip tip = segment.GetComponent <IKTip>();
                if (tip != null)
                {
                    System.Array.Resize(ref Tips, Tips.Length + 1);
                    Tips[Tips.Length - 1] = new TipPtr(segment.GetComponent <IKTip>(), node, Tips.Length);
                }

                System.Array.Resize(ref Nodes, Nodes.Length + 1);
                Nodes[Nodes.Length - 1] = node;
            }
        }
Ejemplo n.º 7
0
            private double ZValue = 0.0;                                //

            //Setup for the node
            public Node(Model model, Node parent, Transform segment, KinematicJoint joint, Objective objective)
            {
                Model  = model;
                Parent = parent;
                if (Parent != null)
                {
                    Parent.AddChild(this);
                }
                Segment   = segment;
                Chain     = new Chain(model.Root, segment);
                Joint     = joint;
                Objective = objective;
            }
Ejemplo n.º 8
0
        private void ResetPosture(Transform t)
        {
            KinematicJoint joint = t.GetComponent <KinematicJoint>();

            if (joint != null)
            {
                joint.GetXMotion().SetTargetValue(0f);
                joint.GetYMotion().SetTargetValue(0f);
                joint.GetZMotion().SetTargetValue(0f);
            }
            for (int i = 0; i < t.childCount; i++)
            {
                ResetPosture(t.GetChild(i));
            }
        }
Ejemplo n.º 9
0
        public Chain(Transform start, Transform end)
        {
            List <Transform>      segments = new List <Transform>();
            List <KinematicJoint> joints   = new List <KinematicJoint>();

            Length = 0f;

            Transform t = end;

            while (true)
            {
                segments.Add(t);
                KinematicJoint joint = t.GetComponent <KinematicJoint>();
                if (joint != null)
                {
                    if (joint.GetDoF() != 0)
                    {
                        joints.Add(joint);
                    }
                }
                if (t == start)
                {
                    break;
                }
                else
                {
                    t = t.parent;
                }
            }

            segments.Reverse();
            joints.Reverse();
            Segments = segments.ToArray();
            Joints   = joints.ToArray();

            if (Joints.Length == 0)
            {
                Length = 0f;
            }
            else
            {
                for (int i = 1; i < Joints.Length; i++)
                {
                    Length += Vector3.Distance(Joints[i - 1].GetAnchorInWorldSpace(), Joints[i].GetAnchorInWorldSpace());
                }
                Length += Vector3.Distance(Joints[Joints.Length - 1].GetAnchorInWorldSpace(), end.position);
            }
        }
Ejemplo n.º 10
0
            public Node(Model model, Node parent, Transform segment, KinematicJoint joint, MotionPtr[] motions)
            {
                Model  = model;
                Parent = parent;
                if (Parent != null)
                {
                    Parent.AddChild(this);
                }
                Segment = segment;
                Chain   = new Chain(model.Body.transform, segment);
                Joint   = joint;
                Motions = motions;

                HeuristicInputs = 0f;
                HeuristicError  = 0f;
            }
Ejemplo n.º 11
0
            private void DrawKinematicChain(Chain chain)
            {
                //Visualize Joints and Kinematic Chain
                if (chain.Joints.Length > 0)
                {
                    KinematicJoint reference = chain.Joints[0];
                    VisualizeKinematicJoint(reference);
                    for (int k = 1; k < chain.Joints.Length; k++)
                    {
                        Handles.color = Color.cyan;
                        Handles.DrawLine(reference.ComputeConnectionInWorldSpace(), chain.Joints[k].ComputeConnectionInWorldSpace());
                        reference = chain.Joints[k];
                        VisualizeKinematicJoint(reference);
                    }

                    Handles.color = Color.cyan;
                    Handles.DrawLine(chain.Joints[chain.Joints.Length - 1].ComputeConnectionInWorldSpace(), chain.Segments[chain.Segments.Length - 1].transform.position);

                    Handles.color = new Color(0.25f, 0.25f, 0.25f, 1f);
                    Handles.SphereCap(0, chain.Segments[chain.Segments.Length - 1].position, chain.Segments[chain.Segments.Length - 1].rotation, 0.01f);
                }
            }
Ejemplo n.º 12
0
    private GameObject CreateFromData(URDFData data)
    {
        Transform actor = new GameObject(data.Name).transform;

        actor.position = new Vector3(0f, 0f, 0f);
        actor.rotation = Quaternion.identity;

        List <Transform> Links  = new List <Transform>();
        List <Transform> Joints = new List <Transform>();

        //Create Link Transforms
        for (int i = 0; i < data.Links.Count; i++)
        {
            Transform link = CreateGeometry(data.Links[i].Geometry).transform;
            link.name = data.Links[i].Name;
            link.SetParent(actor, false);
            Links.Add(link);
        }

        //Create Joint Transforms
        for (int i = 0; i < data.Joints.Count; i++)
        {
            Transform joint = new GameObject().transform;
            joint.name = data.Joints[i].Name;
            joint.SetParent(actor);
            Joints.Add(joint);
        }

        //Apply Parent-Child Relations
        for (int i = 0; i < Joints.Count; i++)
        {
            Transform joint  = Joints[i];
            Transform parent = FindTransformByName(Links, data.GetJointData(joint.name).Parent);
            Transform child  = FindTransformByName(Links, data.GetJointData(joint.name).Child);

            Transform parentJoint = actor;
            string    parentName  = data.GetLinkData(parent.name).Name;
            for (int j = 0; j < Joints.Count; j++)
            {
                if (data.GetJointData(Joints[j].name).Child == parentName)
                {
                    parentJoint = Joints[j];
                    break;
                }
            }

            joint.SetParent(parentJoint);
            child.SetParent(joint);
        }

        Links  = GetOrderedTransforms(actor.root, Links, new List <Transform>());
        Joints = GetOrderedTransforms(actor.root, Joints, new List <Transform>());

        for (int i = 0; i < Joints.Count; i++)
        {
            Transform joint = Joints[i];

            Vector3    angles   = -Mathf.Rad2Deg * ROSToUnity(data.GetJointData(joint.name).OriginRPY);
            Quaternion rotation = Quaternion.Euler(angles);
            joint.position = joint.parent.position + joint.parent.rotation * ROSToUnity(data.GetJointData(joint.name).OriginXYZ);
            joint.rotation = joint.parent.rotation * rotation;
        }

        for (int i = 0; i < Links.Count; i++)
        {
            Transform  link     = Links[i];
            Vector3    angles   = -Mathf.Rad2Deg * ROSToUnity(data.GetLinkData(link.name).RPY);
            Quaternion rotation = Quaternion.Euler(angles);
            link.localPosition += ROSToUnity(data.GetLinkData(link.name).XYZ);
            link.localRotation  = rotation * link.localRotation;
        }

        //Initialize Links
        for (int i = 0; i < Links.Count; i++)
        {
            //Nothing to do.
        }

        //Initialize Joints
        for (int i = 0; i < Joints.Count; i++)
        {
            BioIK.KinematicJoint joint     = Joints[i].gameObject.AddComponent <BioIK.KinematicJoint>();
            URDFData.JointData   jointData = data.GetJointData(joint.name);

            if (jointData.Type == "fixed")
            {
                //Nothing to do
            }
            else
            {
                switch (jointData.Type)
                {
                case "prismatic":
                    joint.SetJointType(BioIK.JointType.Prismatic);
                    break;

                case "revolute":
                    joint.SetJointType(BioIK.JointType.Revolute);
                    break;

                case "continuous":
                    joint.SetJointType(BioIK.JointType.Continuous);
                    break;
                }

                joint.SetAnchor(Vector3.zero);
                Vector3 axis = ROSToUnity(jointData.Axis);
                if (joint.GetJointType() != BioIK.JointType.Prismatic)
                {
                    axis = -axis;
                }

                joint.SetOrientation(Quaternion.FromToRotation(Vector3.right, axis).eulerAngles);
                //joint.SetMaximumVelocity(jointData.Velocity);
                //joint.SetMaximumAcceleration(jointData.Velocity);

                BioIK.Motion motion = joint.GetXMotion();
                motion.SetEnabled(true);
                motion.SetLowerLimit(jointData.LowerLimit);
                motion.SetUpperLimit(jointData.UpperLimit);

                joint.Initialise();
            }
        }

        if (Errors == 0)
        {
            Debug.Log("Successfully imported '" + actor.name + "'.");
        }
        else
        {
            Debug.Log(Errors + " errors or warnings during importing '" + actor.name + "'.\n" + Output);
        }
        Output = string.Empty;
        Errors = 0;

        return(actor.gameObject);
    }
Ejemplo n.º 13
0
 void Awake()
 {
     Target = (KinematicJoint)target;
 }