/// <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 BuilderSetNullSafetyController() { TEST_BUILDER.SetSafetyController(null); }