コード例 #1
0
ファイル: UrdfImporter.cs プロジェクト: johnfarrell/rr-godot
    /// <summary>
    /// <para>GenerateSpatial</para>
    /// <para>
    /// Generates a tree of Godot objects capable of
    /// being inserted into a Godot SceneTree.
    /// </para>
    /// <para>
    /// This tree accurately represents Urdf Joints and Links
    /// in Godot terms.
    /// </para>
    /// </summary>
    /// <param name="base_node">UrdfNode containing the root Urdf component.</param>
    /// <returns>A Godot.Spatial containing the root of the Godot tree.</returns>
    public StaticBody GenerateSpatial(UrdfNode base_node)
    {
        // Create the empty spatial node
        StaticBody rootSpat = new StaticBody();

        rootSpat.Name = base_node._name;

        // Add children recursively
        foreach (var child in base_node.GetChildren())
        {
            // Returns a joint connected to a rigid body
            Godot.Joint childJoint = GenerateSpatialRec(child);
            rootSpat.AddChild(childJoint);

            // Transform according to the child joint transformations
            // Godot's 3D scene has X forward, Y up, and Z right, while
            // Urdf uses X forward, Y right, Z up.
            // This is why the indices below aren't in order, it translates
            // the Urdf coordinates into Godot coordinates.
            childJoint.TranslateObjectLocal(new Vector3(
                                                (float)child._joint.origin.Xyz[0],
                                                (float)child._joint.origin.Xyz[2],
                                                (float)child._joint.origin.Xyz[1]
                                                ));
            childJoint.RotateX((float)child._joint.origin.Rpy[0]);
            childJoint.RotateY((float)child._joint.origin.Rpy[2]);
            childJoint.RotateZ(-1.0F * (float)child._joint.origin.Rpy[1]);
        }

        return(rootSpat);
    }
コード例 #2
0
ファイル: UrdfImporter.cs プロジェクト: johnfarrell/rr-godot
    /// <summary>
    /// <para>ConnectJoints</para>
    /// Iterates through a Spatial->Joint->Link tree setting the joint
    /// endpoints to the necessary links.
    /// <para>The tree is generated by <see cref="GenerateSpatial" /></para>
    /// </summary>
    /// <param name="root">Spatial node containing the root of the tree.</param>
    public void ConnectJoints(Spatial root)
    {
        Queue <Spatial> nodeQueue = new Queue <Spatial>();

        foreach (var child in root.GetChildren())
        {
            nodeQueue.Enqueue((Spatial)child);
        }

        Spatial curr = nodeQueue.Dequeue();

        while (curr != null)
        {
            if (!curr.GetType().Equals(typeof(Godot.PinJoint)) &&
                !curr.GetType().Equals(typeof(Godot.HingeJoint)) &&
                !curr.GetType().Equals(typeof(Generic6DOFJoint)))
            {
                // If the current object isn't a joint, its a link so you need
                // to add all of the children to the queue.
                // Some of this will be a meshinstance/collisionshape, but those
                // will get filtered out.
                foreach (var child in curr.GetChildren())
                {
                    nodeQueue.Enqueue((Spatial)child);
                }
                if (nodeQueue.Count == 0)
                {
                    break;
                }
                curr = nodeQueue.Dequeue();
                continue;
            }
            Godot.Joint tempJoint = (Godot.Joint)curr;

            // We have a joint, set the endpoints

            // A joints parent will always be a RigidBody.
            NodePath parentPath = curr.GetParent().GetPath();
            // A joint will always have only one child which is a RigidBody,
            // this is what we want the second endpoint to be.
            NodePath childPath = curr.GetChild(0).GetPath();

            tempJoint.SetOwner(curr.GetParent());

            tempJoint.SetNodeA(parentPath);
            tempJoint.SetNodeB(childPath);

            // tempJoint.Nodes__nodeA = parentPath;
            // tempJoint.Nodes__nodeB = childPath;

            nodeQueue.Enqueue((Spatial)curr.GetChild(0));

            if (nodeQueue.Count == 0)
            {
                break;
            }
            curr = nodeQueue.Dequeue();
        }
    }
コード例 #3
0
        public void UpdateList(Spatial rootObj)
        {
            Queue <Spatial> nodeQueue = new Queue <Spatial>();

            foreach (var child in rootObj.GetChildren())
            {
                nodeQueue.Enqueue((Spatial)child);
            }

            // Standard BFS traversal of the tree structure
            Spatial curr = nodeQueue.Dequeue();

            while (curr != null)
            {
                if (!curr.GetType().Equals(typeof(Godot.PinJoint)) &&
                    !curr.GetType().Equals(typeof(Godot.HingeJoint)) &&
                    !curr.GetType().Equals(typeof(Generic6DOFJoint)))
                {
                    // If the current object isn't a joint, its a link so you need
                    // to add all of the children to the queue.
                    // Some of this will be a meshinstance/collisionshape, but those
                    // will get filtered out.
                    foreach (var child in curr.GetChildren())
                    {
                        nodeQueue.Enqueue((Spatial)child);
                    }
                    if (nodeQueue.Count == 0)
                    {
                        break;
                    }
                    curr = nodeQueue.Dequeue();
                    continue;
                }
                Godot.Joint tempJoint = (Godot.Joint)curr;

                RegisterJoint(tempJoint);

                nodeQueue.Enqueue((Spatial)curr.GetChild(0));

                if (nodeQueue.Count == 0)
                {
                    break;
                }
                curr = nodeQueue.Dequeue();
            }
        }
コード例 #4
0
ファイル: UrdfImporter.cs プロジェクト: johnfarrell/rr-godot
    /// <summary>
    /// <para>GenerateSpatialRec</para>
    /// Recursive component to generate the Godot SceneTree
    /// structure of the URDF file, complete with joints and
    /// collision shapes.
    /// </summary>
    /// <param name="base_node">
    /// UrdfNode containing the node to generate off of.
    /// </param>
    /// <returns>
    /// A Godot.Generic6DOFJoint that represents the start of the Godot
    /// representation of the URDF tree structure.
    /// </returns>
    private Godot.Joint GenerateSpatialRec(UrdfNode base_node)
    {
        // Create the return joint
        Godot.Joint finJoint = ConfigureJoint(base_node._joint);
        finJoint.Name = base_node._joint.name;

        // Create the return RigidBody
        RigidBody tempLink = base_node.CreateLink();


        foreach (var child in base_node.GetChildren())
        {
            // This is the same as GenerateSpatial(), so look at that
            // function for the explanation.
            Godot.Joint childJoint = GenerateSpatialRec(child);
            tempLink.AddChild(childJoint);
            childJoint.SetOwner(tempLink);

            childJoint.TranslateObjectLocal(new Vector3(
                                                (float)child._joint.origin.Xyz[0],
                                                (float)child._joint.origin.Xyz[2],
                                                -1.0F * (float)child._joint.origin.Xyz[1]
                                                ));

            try
            {
                // childJoint.RotateX((float)child._joint.axis.xyz[0]);
                // childJoint.RotateY((float)child._joint.axis.xyz[2]);
                // childJoint.RotateZ(-1.0F * (float)child._joint.axis.xyz[1]);
            }
            catch
            {
                GD.Print("Axis not specified, continuing...");
            }


            childJoint.RotateX((float)child._joint.origin.Rpy[0]);
            childJoint.RotateY((float)child._joint.origin.Rpy[2]);
            childJoint.RotateZ(-1.0F * (float)child._joint.origin.Rpy[1]);

            GD.Print(childJoint.Transform.ToString());
        }
        finJoint.AddChild(tempLink);

        return(finJoint);
    }
コード例 #5
0
 // Adds and configures a Gen6DoFJointControllerDetail scene to the
 // detail list, replacing any current detail
 private void AddJoint(Godot.Generic6DOFJoint joint)
 {
     if (jointList.GetChildCount() == 1)
     {
         RemoveJoint(0);
     }
     try
     {
         Gen6DoFJointControllerDetail jointDetail = (Gen6DoFJointControllerDetail)GenDetailScene.Instance();
         jointDetail.Configure(joint);
         jointDetail.Name = joint.Name;
         jointList.AddChild(jointDetail);
         ActiveJoint = joint;
     }
     catch (Exception e)
     {
         GD.Print("Error adding joint...\n" + e.Message);
     }
 }
コード例 #6
0
 // Adds a HingeJointControllerDetail scene to the detail list,
 // replacing the current detail if there is one.
 private void AddJoint(Godot.HingeJoint joint)
 {
     if (jointList.GetChildCount() == 1)
     {
         RemoveJoint(0);
     }
     try
     {
         HingeJointControllerDetail jointDetail = (HingeJointControllerDetail)HingeDetailScene.Instance();
         jointDetail.Configure(joint);
         jointDetail.Name = joint.Name;
         jointList.AddChild(jointDetail);
         jointDetail.Connect("TargetVelChanged", this, "HingeJointVelChanged");
         ActiveJoint = joint;
     }
     catch (Exception e)
     {
         GD.Print("Error adding joint...\n" + e.Message);
     }
 }
コード例 #7
0
 public void RegisterJoint(Godot.Joint joint)
 {
     _jointList.Add(joint);
 }