コード例 #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>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);
    }