/// <summary>
/// Constructor
/// </summary>
/// <param name="swData">Storage model</param>
/// <param name="path">Path in the storage model</param>
/// <param name="ChildLink">Link that contains this attachment</param>
public JointedAttachment(StorageModel swData, string path, Link ChildLink)
: base(swData, path, ChildLink)
{
if (ParentLinkID != -1)
Joint = ChildLink.GetJointFromLink(ChildLink.robot.GetLink(ParentLinkID));
else if (ChildLink.UpperJoints.Length == 1)
SetJoint(ChildLink.GetJointFromLink(ChildLink.UpperJoints[0].Parent));
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="swData">Storage model</param>
/// <param name="path">Path in the storage model</param>
/// <param name="parentLink">Link that contains this attachment</param>
public Camera(StorageModel swData, string path, Link parentLink)
: base(swData, path, parentLink)
{
Icon = CommandManager.CameraPic;
Name = parentLink.Name + " " + GetName();
if (FOV == 0)
{
FOV = 90;
}
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="swData">Storage model</param>
/// <param name="path">Path in the storage model</param>
/// <param name="parentLink">Link that contains this attachment</param>
public InternalLimitSwitch(StorageModel swData, string path, Link parentLink)
: base(swData, path, parentLink)
{
Icon = CommandManager.IntLimitSwitchPic;
Name = parentLink.Name + " " + GetName();
if (ParentLinkID != -1)
Joint = ChildLink.GetJointFromLink(ChildLink.robot.GetLink(ParentLinkID));
else if (ChildLink.UpperJoints.Length == 1)
SetJoint(ChildLink.GetJointFromLink(ChildLink.UpperJoints[0].Parent));
}
/// <summary>
/// Creates a new GraphNode object
/// </summary>
/// <param name="l">The link this node will represent. If null this node is a temporary node</param>
public GraphNode(Link l)
{
StoredLink = l;
ParentNodes = new HashSet<GraphNode>();
ChildNodes = new HashSet<GraphNode>();
GraphLevel = -1;
LevelIndex = -1;
FlippedNodes = new List<GraphNode>();
MarkedNodes = new HashSet<GraphNode>();
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="swData">Storage model</param>
/// <param name="path">Path in the storage model</param>
/// <param name="parentLink">Link that contains this attachment</param>
public Rangefinder(StorageModel swData, string path, Link parentLink)
: base(swData, path, parentLink)
{
Icon = CommandManager.RangefinderPic;
Name = parentLink.Name + " " + GetName();
if (FOV == 0)
{
FOV = -.05;
}
}
/// <summary>
/// Exports the .stl model for one link
/// The model must already be switched to the correct configuration and all parts must be hidden before this method is called
/// </summary>
/// <param name="link">Link to be exported</param>
/// <param name="configuration">The configuration that this link should be exported from</param>
/// <param name="path">The path to save the .stl file</param>
/// <param name="log">The logger to write messages to</param>
public void ExportLink(Link link ,ModelConfiguration configuration, String path, ProgressLogger log)
{
log.WriteMessage("Exporting link " + link.Name + " as an STL. Configuration: " + ((ModelConfiguration.ModelConfigType)configuration.Type) + "; Path: " + path, false);
int errors = 0;
int warnings = 0;
ModelDoc2 ActiveDoc = (ModelDoc2)asm;
IsolateLink(link, configuration);
int saveOptions = (int)swSaveAsOptions_e.swSaveAsOptions_Silent;
ActiveDoc.Extension.SaveAs(path, (int)swSaveAsVersion_e.swSaveAsCurrentVersion, saveOptions, null, ref errors, ref warnings);
HideLink(link);
log.WriteMessage("Correcting STL header");
CorrectSTLMesh(path);
log.WriteMessage("Finished exporting STL.");
}
/// <summary>
/// Creates a new joint
/// </summary>
/// <param name="path">Path to this joint in the StorageModel</param>
/// <param name="owner">The Link that owns this joint (child Link in the joint)</param>
public Joint(string path, Link parent, Link child)
{
this.swApp = RobotInfo.SwApp;
this.asmDoc = RobotInfo.AssemDoc;
this.modelDoc = RobotInfo.ModelDoc;
this.swData = RobotInfo.SwData;
this.path = path;
this.robot = RobotInfo.Robot;
this.Parent = parent;
Parent.ChildJoints.Add(this);
this.Child = child;
this.Selected = false;
this.Type = JointFactory.DefaultJointType;
RobotInfo.WriteToLogFile("Getting Joint Specifics (Joint)");
jointSpecifics = JointFactory.GetSpecificJoint(Type,path,this);
RobotInfo.WriteToLogFile("Successfully created Joint Specifics (Joint)");
if (swData.GetDouble(path) == 0)
{
swData.SetDouble(path, 1);
}
}
/// <summary>
/// Generates a new attachment
/// </summary>
/// <param name="swData">The storage model to store the attachment in</param>
/// <param name="path">The path to the attachment in the storage model</param>
/// <param name="parentLink">The link that contains the attachment</param>
/// <returns>The newly generated attachment</returns>
public static Attachment GenerateAttachment(string path, Link parentLink)
{
StorageModel swData = RobotInfo.SwData;
int id = (int)swData.GetDouble(path);
if (parentLink.isBaseLink &&
(id == SimpleMotorID || id == QuadEncoderID || id == PotentiometerID || id == IntLimitSwitchID || id == PistonID))
{
string msg = "This attachment must be associated with a link that has at least one parent joint.";
MessageBox.Show(msg, "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
return null;
}
switch (id)
{
case SimpleMotorID:
return new SimpleMotor(swData, path, parentLink );
case QuadEncoderID:
return new QuadEncoder(swData, path, parentLink);
case PotentiometerID:
return new Potentiometer(swData, path, parentLink);
case GyroID:
return new Gyro(swData, path, parentLink);
case RangefinderID:
return new Rangefinder(swData, path, parentLink);
case CameraID:
return new Camera(swData, path, parentLink);
case ExtLimitSwitchID:
return new ExternalLimitSwitch(swData, path, parentLink);
case IntLimitSwitchID:
return new InternalLimitSwitch(swData, path, parentLink);
case PistonID:
return new Piston(swData, path, parentLink);
default:
return null;
}
}
/// <summary>
/// Loads existing modelconfiguration from the swdata,
/// or creats new one if not already existing
/// </summary>
/// <param name="path"></param>
/// <param name="type"></param>
public ModelConfiguration(String path, int type, Link owner)
{
//setup fields
this.modelDoc = RobotInfo.ModelDoc;
this.swData = RobotInfo.SwData;
this.path = path;
this.Type = type;
this.Owner = owner;
LinkComponents = new List<modelComponent>();
//If the link components data exists, load the components data
LinkComponentPIDs = new StringStorageArray(swData, path + "/components");
nextComponentNumber = 0;
modelComponent newComp;
if (LinkComponentPIDs.Count != 0)
{
foreach (String s in LinkComponentPIDs)
{
newComp = new modelComponent(path + "/component" + nextComponentNumber);
LinkComponents.Add(newComp);
nextComponentNumber++;
}
}
}
/// <summary>
/// Constructor for directional attachments. Directional attachments are attachments that have a location and direction, like cameras and rangefinders
/// </summary>
/// <param name="swData">The storage model that will be used for storage</param>
/// <param name="path">The storage path for this attachment's storage</param>
/// <param name="parentLink">The link that contains this attachment</param>
protected DirectionalAttachment(StorageModel swData, string path, Link parentLink)
: base(swData,path,parentLink)
{
}
/// <summary>
/// Writes a Link to the .URDF file
/// </summary>
/// <param name="link"> The Link to be written </param>
/// <param name="writer"> The writer to use to write the Link </param>
private void WriteLink(Link link, XmlWriter writer)
{
//Link data
writer.WriteStartElement("link");
writer.WriteAttributeString("name", link.Name);
log.WriteMessage("Writing link " + link.Name + " to URDF");
//Inertial data
writer.WriteStartElement("inertial");
//Origin data
writer.WriteStartElement("origin");
if (link.ParentConnection != null)
{
writer.WriteAttributeString("xyz", (link.ComX - link.ParentConnection.OriginX) + " " + (link.ComY - link.ParentConnection.OriginY) + " " + (link.ComZ - link.ParentConnection.OriginZ));
writer.WriteAttributeString("rpy", link.OriginR + " " + link.OriginP + " " + link.OriginW);
}
else
{
writer.WriteAttributeString("xyz", link.ComX + " " + link.ComY + " " + link.ComZ );
writer.WriteAttributeString("rpy", link.OriginR + " " + link.OriginP + " " + link.OriginW);
}
writer.WriteEndElement();
//Mass data
writer.WriteStartElement("mass");
writer.WriteAttributeString("value", link.Mass.ToString());
writer.WriteEndElement();
//Moment data
writer.WriteStartElement("inertia");
writer.WriteAttributeString("ixx", link.MomentIxx.ToString());
writer.WriteAttributeString("ixy", link.MomentIxy.ToString());
writer.WriteAttributeString("ixz", link.MomentIxz.ToString());
writer.WriteAttributeString("iyy", link.MomentIyy.ToString());
writer.WriteAttributeString("iyz", link.MomentIyz.ToString());
writer.WriteAttributeString("izz", link.MomentIzz.ToString());
writer.WriteEndElement();
writer.WriteEndElement();
//Visual Data
writer.WriteStartElement("visual");
//Visual Origin
writer.WriteStartElement("origin");
if (link.ParentConnection != null)//check for base link as the baselink will not have an offset
{
writer.WriteAttributeString("xyz", -link.ParentConnection.OriginX + " " + -link.ParentConnection.OriginY + " " + -link.ParentConnection.OriginZ);
writer.WriteAttributeString("rpy", link.OriginRvisual + " " + link.OriginPvisual + " " + link.OriginWvisual);
}
else
{
writer.WriteAttributeString("xyz", link.OriginXvisual + " " + link.OriginYvisual + " " + link.OriginZvisual);
writer.WriteAttributeString("rpy", link.OriginRvisual + " " + link.OriginPvisual + " " + link.OriginWvisual);
}
writer.WriteEndElement();
//Visual Mesh
writer.WriteStartElement("geometry");
writer.WriteStartElement("mesh");
writer.WriteAttributeString("filename", "package://" + robot.Name+"/visualSTL/" + link.Name + ".STL");
writer.WriteEndElement();
writer.WriteEndElement();
//Color
writer.WriteStartElement("material");
writer.WriteAttributeString("name", link.color.ToString("X6"));
writer.WriteStartElement("color");
writer.WriteAttributeString("rgba", (link.ColorRed/255.0).ToString() + " " + (link.ColorGreen/255.0).ToString() + " " + (link.ColorBlue/255.0).ToString() + " 1");
writer.WriteEndElement();
writer.WriteEndElement();
writer.WriteEndElement();
//Collision Data
writer.WriteStartElement("collision");
//Visual Mesh
writer.WriteStartElement("origin");
if (link.ParentConnection != null)
{
writer.WriteAttributeString("xyz", -link.ParentConnection.OriginX + " " + -link.ParentConnection.OriginY + " " + -link.ParentConnection.OriginZ);
writer.WriteAttributeString("rpy", link.OriginRcollision + " " + link.OriginPcollision + " " + link.OriginWcollision);
}
else
{
writer.WriteAttributeString("xyz", link.OriginXcollision + " " + link.OriginYcollision + " " + link.OriginZcollision);
writer.WriteAttributeString("rpy", link.OriginRcollision + " " + link.OriginPcollision + " " + link.OriginWcollision);
}
writer.WriteEndElement();
//Collision Mesh
writer.WriteStartElement("geometry");
writer.WriteStartElement("mesh");
writer.WriteAttributeString("filename", "package://" + robot.Name + "/collisionSTL/" + link.Name + ".STL");
writer.WriteEndElement();
writer.WriteEndElement();
writer.WriteEndElement();
writer.WriteEndElement();
//write Gazebo properties of link
if (link.Mu1 != 0 || link.Mu2 != 0 || link.Kp != 0 || link.Kd != 0)
{
writer.WriteStartElement("gazebo");
writer.WriteAttributeString("reference", link.Name);
if(link.Mu1 != 0 || link.Mu2 != 0)
{
writer.WriteStartElement("mu1");
writer.WriteString(link.Mu1.ToString());
writer.WriteEndElement();
writer.WriteStartElement("mu2");
writer.WriteString(link.Mu2.ToString());
writer.WriteEndElement();
}
if (link.Kp != 0)
{
writer.WriteStartElement("kp");
writer.WriteString(link.Kp.ToString());
writer.WriteEndElement();
}
if (link.Kd != 0)
{
writer.WriteStartElement("kd");
writer.WriteString(link.Kd.ToString());
writer.WriteEndElement();
}
writer.WriteEndElement();
}
/*writer.WriteStartElement("gazebo");
writer.WriteAttributeString("reference", link.name);
writer.WriteStartElement("material");
writer.WriteStartElement("ambient");
writer.WriteString((link.colorRed / 255.0).ToString() + " " + (link.colorGreen / 255.0).ToString() + " " + (link.colorBlue / 255.0).ToString() + " 1");
writer.WriteEndElement();
writer.WriteEndElement();
writer.WriteEndElement();
log.writeMessage("Finished writing link " + link.name + " to URDF");*/
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="swData">Storage model</param>
/// <param name="path">Path in the storage model</param>
/// <param name="parentLink">Link that contains this attachment</param>
public Gyro(StorageModel swData, string path, Link parentLink)
: base(swData, path, parentLink)
{
Icon = CommandManager.GyroPic;
Name = parentLink.Name + " " + GetName();
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="swData">Storage model</param>
/// <param name="path">Path in the storage model</param>
/// <param name="ChildLink">Link that contains this attachment</param>
public Potentiometer(StorageModel swData, string path, Link ChildLink)
: base(swData, path, ChildLink)
{
Icon = CommandManager.PotPic;
Name = ChildLink.Name + " " + GetName();
}
/// <summary>
/// update currentLink when new link selected
/// </summary>
/// <param name="link"></param>
public void OnLinkSelectionChanged(Link link)
{
if (currentLink != null)
SaveCurrentLinkSelection();
//RevertLinkColors();
currentLink = link;
LoadNewLinkSelection();
}
/// <summary>
/// Isolates the selceted link by hiding all other components
/// </summary>
/// <param name="l"> Link to be isolated</param>
private void IsolateLink(Link l, ModelConfiguration config)
{
List<Component2> comps = new List<Component2>();
foreach (modelComponent m in config.LinkComponents)
{
Component2 c = m.Component;
if (c == null )
continue;
if (((object[])c.GetChildren()).Length > 0)
{
GetSubComponents(c, comps);
}
else
{
comps.Add(c);
//System.Diagnostics.Debug.WriteLine(c.Name2);
}
}
DispatchWrapper[] dispComps = Array.ConvertAll(comps.ToArray(),element=>new DispatchWrapper(element));
SelectionMgr manager = ((ModelDoc2)asm).SelectionManager;
SelectData data = manager.CreateSelectData();
data.Mark = -1;
manager.SuspendSelectionList();
manager.AddSelectionListObjects(comps.ToArray(), data);
((ModelDoc2)asm).ShowComponent2();
manager.ResumeSelectionList();
}
/// <summary>
/// Hides all components in the link
/// </summary>
/// <param name="l">Link to be hidden</param>
private void HideLink(Link l)
{
((ModelDoc2)asm).Extension.SelectAll();
int temp = ((SelectionMgr)((ModelDoc2)asm).SelectionManager).GetSelectedObjectCount2(-1);//this fixes stuff. no idea why
((ModelDoc2)asm).HideComponent2();//since all componets are still selected from the isolate link method, they can simply be rehidden
((ModelDoc2)asm).ClearSelection2(true);
}
/// <summary>
/// Loads a robot from an assembly document, if a robot dosn't already
/// exist one will be created
/// </summary>
/// <param name="asm">Assembly document containing a robot model</param>
/// <param name="swApp">Interface for interacting with Solidworks</param>
public RobotModel(AssemblyDoc asm, SldWorks swApp)
{
RobotInfo.WriteToLogFile("Robot Created (Robot)");
var assembly = typeof(JointSpecifics).Assembly;
Type[] types = assembly.GetTypes().Where(
t => t.IsSubclassOf(typeof(JointSpecifics)) && !t.IsAbstract).ToArray();
foreach (Type t in types)
{
System.Runtime.CompilerServices.RuntimeHelpers.RunClassConstructor(t.TypeHandle);
}
RobotInfo.WriteToLogFile("Initialized JointTypes = " + String.Join(", ",JointFactory.GetTypesList()) + " (Robot)");
//Setup fields
this.swApp = swApp;
this.asmDoc = asm;
this.modelDoc = (ModelDoc2)asm;
swData = new StorageModel(modelDoc);
Selected = false;
RobotInfo.SetProperties(swApp, asmDoc, swData, this);
RobotInfo.WriteToLogFile("Setup Fields Setup");
//If the robot data dosn't exist yet, create it with default values
if (swData.GetDouble("robot") == 0)
{
swData.SetDouble("robot", 1);
/*PhysicalConfig = "Default";
VisualConfig = "Default";
CollisionConfig = "Default";*/
Name = ((ModelDoc2)asm).GetTitle();
RobotInfo.WriteToLogFile("Robot Data created with Default Values");
}
RobotInfo.WriteToLogFile("Robot Data created");
LinkNums = new DoubleStorageArray(swData, "robot/linkNums");
nextLinkNum = 0;
RobotInfo.WriteToLogFile("LinkNums Storage Array Created");
Links = new Dictionary<int,Link>();
//Load link structure
Link newLink;
if (LinkNums.Count == 0)
{
LinkNums.AddItem(0);
RobotInfo.WriteToLogFile("New Link added to LinkNums");
}
Configuration currentConfig = modelDoc.ConfigurationManager.ActiveConfiguration;
foreach (double d in LinkNums)
{
newLink = new Link("robot/link" + (int)d, (int)d);
RobotInfo.WriteToLogFile("New Link Created");
Links.Add((int)d,newLink);
if (d >= nextLinkNum)
nextLinkNum = (int)d + 1;
}
Links[0].isBaseLink = true;
foreach (Link l in Links.Values.ToArray())
{
l.InitializeJoints();
l.InitializeAttachments();
}
modelDoc.ShowConfiguration2(ConfigName);
CalcAxisVectors();
CalcOrigin();
modelDoc.ShowConfiguration2(currentConfig.Name);
}
public void DeleteLink(Link link)
{
Links.Remove(link.Id);
LinkNums.RemoveItem(link.Id);
RobotInfo.WriteToLogFile(link.Id + " has been deleted");
}
/// <summary>
/// Adds a link to the robot
/// </summary>
public Link AddLink()
{
RobotInfo.WriteToLogFile("Creating new link (Robot)");
Link tempLink = new Link("robot/link" + nextLinkNum, nextLinkNum);
tempLink.Name = "NewLink" + nextLinkNum.ToString();
RobotInfo.WriteToLogFile("Link created: " + tempLink.Name + " (Robot)");
Links.Add(nextLinkNum, tempLink);
LinkNums.AddItem(nextLinkNum);
nextLinkNum++;
return tempLink;
}
/// <summary>
/// creates a joint between the 2 specified links
/// </summary>
/// <param name="parent">The parent link of the joint</param>
/// <param name="child">The child link of the joint</param>
/// <returns>newly created joint</returns>
public Joint AddConnection(Link parent, Link child)
{
return child.AddParentJoint(parent);
}
/// <summary>
/// makes joint and connects two given links
/// </summary>
/// <param name="parent">parent link of new joint</param>
/// <param name="child">child link of new joint</param>
/// <returns>newly created joint</returns>
public Joint connectLinks(Link parent, Link child)
{
return robot.AddConnection(parent, child);
}
/// <summary>
/// Creates an new attachment of the specified type
/// </summary>
/// <param name="swData">The storage model to store the attachment in</param>
/// <param name="path">The path to the attachment in the storage model</param>
/// <param name="id">The type of the attachment</param>
/// <param name="parentLink">The link that contains the attachment</param>
/// <returns>The newly created attachment</returns>
public static Attachment CreateNewAttachment(string path, int id, Link parentLink)
{
RobotInfo.SwData.SetDouble(path, id);
return GenerateAttachment(path, parentLink);
}
/// <summary>
/// Adds to this link's list of parent joints and joint ids and incremenets nextJointNum
/// </summary>
/// <param name="parent">parent link of the new joint</param>
/// <returns>newly created joint</returns>
public Joint AddParentJoint(Link parent)
{
RobotInfo.WriteToLogFile("Creating new joint (Link)");
Joint newJoint = new Joint(path + "/joint/" + nextJointNum, parent, this);
RobotInfo.WriteToLogFile("Adding joint to parent list (Link)");
ParentJoints.Add(newJoint);
parentJointNums.AddItem(nextJointNum);
nextJointNum++;
return newJoint;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="swData">Storage model</param>
/// <param name="path">Path in the storage model</param>
/// <param name="ChildLink">Link that contains this attachment</param>
public SimpleMotor(StorageModel swData, string path, Link ChildLink)
: base(swData, path, ChildLink)
{
Icon = CommandManager.MotorPic;
Name = ChildLink.Name + " "+ GetName();
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="swData">The storage model to store values in</param>
/// <param name="path">The location of this attachment in the storage model</param>
/// <param name="parentLink">The link that contains this attachment</param>
protected Attachment(StorageModel swData, string path, Link parentLink)
{
this.SwData = swData;
this.Path = path;
this.ChildLink = parentLink;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="swData">Storage model</param>
/// <param name="path">Path in the storage model</param>
/// <param name="parentLink">Link that contains this attachment</param>
public ExternalLimitSwitch(StorageModel swData, string path, Link parentLink)
: base(swData, path, parentLink)
{
Icon = CommandManager.ExtLimitSwitchPic;
Name = parentLink.Name + " " + GetName();
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="swData">Storage model</param>
/// <param name="path">Path in the storage model</param>
/// <param name="ChildLink">Link that contains this attachment</param>
public QuadEncoder(StorageModel swData, string path, Link ChildLink)
: base(swData, path, ChildLink)
{
Icon = CommandManager.EncoderPic;
Name = ChildLink.Name + " " + GetName();
}
/// <summary>
/// Writes an attachment to the .URDF file
/// </summary>
/// <param name="att"> Attachment to be written </param>
/// <param name="parentLink"> The Link that contains this attachment </param>
/// <param name="writer"> The writer that will be used to write the URDF</param>
private void WriteAttachment(Attachment att, Link parentLink, XmlWriter writer)
{
att.WriteElements(log, writer);
log.WriteMessage("Finished writing attachment " + att.Name + " to URDF.");
}
/// <summary>
/// Sets the current link of this page
/// </summary>
/// <param name="l">link to be used</param>
public void setLink(Link l)
{
currentLink = l;
nameTextBox.Text = currentLink.Name;
changeColor.BackColor = Color.FromArgb(currentLink.ColorRed, currentLink.ColorGreen, currentLink.ColorBlue);
Mu1Textbox.Text = currentLink.Mu1.ToString();
Mu2Textbox.Text = currentLink.Mu2.ToString();
KpTextbox.Text = currentLink.Kp != 0? currentLink.Kp.ToString() : "";
KdTextbox.Text = currentLink.Kd != 0? currentLink.Kd.ToString() : "";
LinearDampingTextbox.Text = currentLink.LinearDamping != 0 ? currentLink.LinearDamping.ToString() : "";
AngularDampingTextbox.Text = currentLink.AngularDamping != 0 ? currentLink.AngularDamping.ToString() : "";
ToggleCollisionValues();
selfCollideCheckbox.Checked = currentLink.SelfCollide;
CustomMassValCheckbox.Checked = currentLink.UseCustomInertial;
toggleCOMTextboxes();
InertiaValuesUpdateTextbox();
PhysicalButton.Enabled = !CustomMassValCheckbox.Checked;
RemoveButton.Enabled = !currentLink.isBaseLink;
}
/// <summary>
/// Gets the joint that connects this link to the specified link
/// </summary>
/// <param name="l">Link that this joint is connected to</param>
/// <returns>The joint that connects this link and inputted link</returns>
public Joint GetJointFromLink(Link l)
{
foreach (Joint j in ParentJoints)
{
if (j.Parent.Equals(l))
{
return j;
}
}
foreach (Joint j in ChildJoints)
{
if (j.Child.Equals(l))
{
return j;
}
}
return null;
}
