/// <summary>
/// Parses a URDF <joint> element from XML.
/// </summary>
/// <param name="node">The XML node of a <joint> element</param>
/// <returns>A Joint object parsed from the XML</returns>
public override Joint Parse(XmlNode node)
{
ValidateXmlNode(node);
XmlAttribute nameAttribute = GetAttributeFromNode(node, UrdfSchema.NAME_ATTRIBUTE_NAME);
XmlAttribute typeAttribute = GetAttributeFromNode(node, UrdfSchema.JOINT_TYPE_ATTRIBUTE_NAME);
XmlElement originElement = GetElementFromNode(node, UrdfSchema.ORIGIN_ELEMENT_NAME);
XmlElement parentElement = GetElementFromNode(node, UrdfSchema.PARENT_ELEMENT_NAME);
XmlElement childElement = GetElementFromNode(node, UrdfSchema.CHILD_ELEMENT_NAME);
XmlElement axisElement = GetElementFromNode(node, UrdfSchema.AXIS_ELEMENT_NAME);
XmlElement calibrationElement = GetElementFromNode(node, UrdfSchema.CALIBRATION_ELEMENT_NAME);
XmlElement dynamicsElement = GetElementFromNode(node, UrdfSchema.DYNAMICS_ELEMENT_NAME);
XmlElement limitElement = GetElementFromNode(node, UrdfSchema.LIMIT_ELEMENT_NAME);
XmlElement mimicElement = GetElementFromNode(node, UrdfSchema.MIMIC_ELEMENT_NAME);
XmlElement safetyControllerElement = GetElementFromNode(node, UrdfSchema.SAFETY_CONTROLLER_ELEMENT_NAME);
Joint.Builder builder = ConstructBuilder(nameAttribute, typeAttribute, parentElement, childElement);
if (originElement != null)
{
builder.SetOrigin(this.originParser.Parse(originElement));
}
if (axisElement != null)
{
builder.SetAxis(this.axisParser.Parse(axisElement));
}
if (calibrationElement != null)
{
builder.SetCalibration(this.calibrationParser.Parse(calibrationElement));
}
if (dynamicsElement != null)
{
builder.SetDynamics(this.dynamicsParser.Parse(dynamicsElement));
}
if (limitElement != null)
{
builder.SetLimit(this.limitParser.Parse(limitElement));
}
if (mimicElement != null)
{
builder.SetMimic(this.mimicParser.Parse(mimicElement));
}
if (safetyControllerElement != null)
{
builder.SetSafetyController(this.safetyControllerParser.Parse(safetyControllerElement));
}
return(builder.Build());
}
public void ConstructJointAllProperties()
{
Axis axis = new Axis(new XyzAttribute(1, 2, 3));
Calibration calibration = new Calibration(1, 2);
Dynamics dynamics = new Dynamics(1, 2);
Limit limit = new Limit(1, 2);
Mimic mimic = new Mimic(new Joint.Builder("mimic", TEST_JOINT_TYPE, TEST_PARENT_LINK, TEST_CHILD_LINK).Build());
Origin origin = new Origin.Builder().SetXyz(new XyzAttribute(1, 2, 3)).SetRpy(new RpyAttribute(1, 2, 3)).Build();
SafetyController safetyController = new SafetyController(1);
Joint.Builder builder = new Joint.Builder(TEST_JOINT_NAME, TEST_JOINT_TYPE, TEST_PARENT_LINK, TEST_CHILD_LINK);
builder.SetAxis(axis);
builder.SetCalibration(calibration);
builder.SetDynamics(dynamics);
builder.SetLimit(limit);
builder.SetMimic(mimic);
builder.SetOrigin(origin);
builder.SetSafetyController(safetyController);
Joint joint = builder.Build();
Assert.AreEqual(TEST_JOINT_NAME, joint.Name);
Assert.AreEqual(TEST_JOINT_TYPE, joint.Type);
Assert.AreEqual(TEST_PARENT_LINK, joint.Parent);
Assert.AreEqual(TEST_CHILD_LINK, joint.Child);
Assert.AreEqual(axis, joint.Axis);
Assert.AreEqual(axis.Xyz.X, joint.Axis.Xyz.X);
Assert.AreEqual(axis.Xyz.Y, joint.Axis.Xyz.Y);
Assert.AreEqual(axis.Xyz.Z, joint.Axis.Xyz.Z);
Assert.AreEqual(calibration, joint.Calibration);
Assert.AreEqual(calibration.Rising, joint.Calibration.Rising);
Assert.AreEqual(calibration.Falling, joint.Calibration.Falling);
Assert.AreEqual(dynamics, joint.Dynamics);
Assert.AreEqual(dynamics.Damping, joint.Dynamics.Damping);
Assert.AreEqual(dynamics.Friction, joint.Dynamics.Friction);
Assert.AreEqual(limit, joint.Limit);
Assert.AreEqual(limit.Lower, joint.Limit.Lower);
Assert.AreEqual(limit.Upper, joint.Limit.Upper);
Assert.AreEqual(limit.Effort, joint.Limit.Effort);
Assert.AreEqual(limit.Velocity, joint.Limit.Velocity);
Assert.AreEqual(mimic, joint.Mimic);
Assert.AreEqual(mimic.Joint, joint.Mimic.Joint);
Assert.AreEqual(mimic.Multiplier, joint.Mimic.Multiplier);
Assert.AreEqual(mimic.Offset, joint.Mimic.Offset);
Assert.AreEqual(origin, joint.Origin);
Assert.AreEqual(origin.Xyz.X, joint.Origin.Xyz.X);
Assert.AreEqual(origin.Xyz.Y, joint.Origin.Xyz.Y);
Assert.AreEqual(origin.Xyz.Z, joint.Origin.Xyz.Z);
Assert.AreEqual(origin.Rpy.R, joint.Origin.Rpy.R);
Assert.AreEqual(origin.Rpy.P, joint.Origin.Rpy.P);
Assert.AreEqual(origin.Rpy.Y, joint.Origin.Rpy.Y);
Assert.AreEqual(safetyController, joint.SafetyController);
Assert.AreEqual(safetyController.SoftLowerLimit, joint.SafetyController.SoftLowerLimit);
Assert.AreEqual(safetyController.SoftUpperLimit, joint.SafetyController.SoftUpperLimit);
Assert.AreEqual(safetyController.KPosition, joint.SafetyController.KPosition);
Assert.AreEqual(safetyController.KVelocity, joint.SafetyController.KVelocity);
}
public void BuilderSetNullCalibration()
{
TEST_BUILDER.SetCalibration(null);
}
