/// <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);
}
/// <summary>
/// <para>CreateNodeTree</para>
/// Creates a tree representation
/// of the URDF file using custom
/// UrdfNode objects.
/// </summary>
/// <returns>
/// UrdfNode of the root node in the
/// tree.
/// </returns>
private UrdfNode CreateNodeTree(Robot bot)
{
// Create the root node
UrdfNode rootNode = new UrdfNode(null, bot.root, null);
rootNode._isRoot = true;
rootNode._name = bot.root.name;
PopulateChildren(rootNode);
return(rootNode);
}
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);
}
/// <summary>
/// <para>PopulateChildren</para>
/// Recursively builds a n-tree of UrdfNode objects
/// from the parsed Urdf file.
/// </summary>
/// <param name="base_node">
/// Base node to build the tree off.
/// </param>
private void PopulateChildren(UrdfNode base_node)
{
// Go through each joint and get the connected
// link, creating a fully specified UrdfNode
// object to add as a child to base_node
foreach (var joint in base_node._link.joints)
{
Link joint_link = joint.ChildLink;
UrdfNode temp = new UrdfNode(base_node, joint_link, joint);
temp._name = joint.child;
temp._offsetXyz = joint.origin.Xyz;
temp._offsetRpy = joint.origin.Rpy;
base_node.AddChild(temp);
PopulateChildren(temp);
}
}
/// <summary>
/// <para>Parse</para>
/// Parses a Urdf file into class RosSharp.Urdf.Robot member.
/// </summary>
/// <param name="file_name">Name of the Urdf file.</param>
/// <returns>
/// <para>True if the file was succesfully parsed.</para>
/// <para>False if there was an error while parsing.<para>
/// </returns>
public bool Parse(string file_name)
{
try
{
_robot = new Robot(file_name);
GD.Print("Robot link and joint count:");
GD.Print(_robot.links.Count);
GD.Print(_robot.joints.Count);
_robotRoot = CreateNodeTree(_robot);
}
catch
{
UrdfPrint("Error parsing Urdf file.\tRobot not parsed!");
return(false);
}
PrintRobotInfo(_robot);
return(true);
}
