/// <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);
}
private void PrintTree(UrdfNode root, int level)
{
int count = 1;
foreach (var childNode in root.GetChildren())
{
string status = "(" + count + ") - Node " + childNode._name + ": " + childNode.GetNumChildren();
UrdfPrint(status, level);
PrintTree(childNode, level + 1);
count += 1;
}
}
/// <summary>
/// <para>PrintTree</para>
/// Prints the tree structure of
/// the Urdf file.
/// </summary>
/// <param name="root">
/// Root UrdfNode of the tree.
/// </param>
public void PrintTree(UrdfNode root)
{
int count = 1;
foreach (var childNode in root.GetChildren())
{
string status = "(" + count + ") - Node " + childNode._name + ": " + childNode.GetNumChildren();
UrdfPrint(status);
PrintTree(childNode, 1);
count += 1;
}
}
/// <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);
}