/// <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>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();
}
}
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();
}
}
/// <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);
}
// 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);
}
}
// 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);
}
}
public void RegisterJoint(Godot.Joint joint)
{
_jointList.Add(joint);
}
