/// <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(); } }
/// <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); }