Esempio n. 1
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);
    }