public void ConstructOrigin()
{
XyzAttribute xyz = new XyzAttribute(1, 2, 3);
RpyAttribute rpy = new RpyAttribute(4, 5, 6);
Origin origin = new Origin.Builder().SetXyz(xyz).SetRpy(rpy).Build();
Assert.AreEqual(xyz, origin.Xyz);
Assert.AreEqual(rpy, origin.Rpy);
}
public void ConstructRpyOrigin()
{
RpyAttribute rpy = new RpyAttribute(4, 5, 6);
Origin origin = new Origin.Builder().SetRpy(rpy).Build();
Assert.AreEqual(rpy, origin.Rpy);
Assert.AreEqual(0, origin.Xyz.X);
Assert.AreEqual(0, origin.Xyz.Y);
Assert.AreEqual(0, origin.Xyz.Z);
}
public void ConstructXyzOrigin()
{
XyzAttribute xyz = new XyzAttribute(1, 2, 3);
Origin origin = new Origin.Builder().SetXyz(xyz).Build();
Assert.AreEqual(xyz, origin.Xyz);
Assert.AreEqual(0, origin.Rpy.R);
Assert.AreEqual(0, origin.Rpy.P);
Assert.AreEqual(0, origin.Rpy.Y);
}
public void ConstructDefaultOriginWithBuilder()
{
Origin origin = new Origin.Builder().Build();
Assert.AreEqual(0, origin.Xyz.X);
Assert.AreEqual(0, origin.Xyz.Y);
Assert.AreEqual(0, origin.Xyz.Z);
Assert.AreEqual(0, origin.Rpy.R);
Assert.AreEqual(0, origin.Rpy.P);
Assert.AreEqual(0, origin.Rpy.Y);
}
public void EqualsAndHash()
{
Origin origin = new Origin.Builder().SetXyz(new XyzAttribute(1, 2, 3)).Build();
Origin same = new Origin.Builder().SetXyz(new XyzAttribute(1, 2, 3)).SetRpy(new RpyAttribute()).Build();
Origin diff = new Origin();
Assert.IsTrue(origin.Equals(origin));
Assert.IsFalse(origin.Equals(null));
Assert.IsTrue(origin.Equals(same));
Assert.IsTrue(same.Equals(origin));
Assert.IsFalse(origin.Equals(diff));
Assert.AreEqual(origin.GetHashCode(), same.GetHashCode());
Assert.AreNotEqual(origin.GetHashCode(), diff.GetHashCode());
}
public void ConstructJointChainBuilderSetters()
{
Axis axis = new Axis(new XyzAttribute(1, 2, 3));
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).SetOrigin(origin).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(origin, joint.Origin);
Assert.AreEqual(safetyController, joint.SafetyController);
}
public void ParseJoint()
{
string name = "joint";
string type = "continuous";
Origin origin = new Origin.Builder().SetXyz(new XyzAttribute(1, 2, 3)).SetRpy(new RpyAttribute(1, 2, 3)).Build();
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(jointDictionary[MIMIC_JOINT_NAME], 2, 3);
SafetyController safetyController = new SafetyController(1);
StringBuilder xmlBuilder = new StringBuilder();
xmlBuilder.Append(String.Format("<joint name='{0}' type='{1}'>", name, type));
xmlBuilder.Append(String.Format("<origin rpy='{0} {1} {2}' xyz='{3} {4} {5}'/>", origin.Xyz.X, origin.Xyz.Y, origin.Xyz.Z, origin.Rpy.R, origin.Rpy.P, origin.Rpy.Y));
xmlBuilder.Append(String.Format("<parent link='{0}'/>", PARENT_JOINT_NAME));
xmlBuilder.Append(String.Format("<child link='{0}'/>", CHILD_JOINT_NAME));
xmlBuilder.Append(String.Format("<axis xyz='{0} {1} {2}'/>", axis.Xyz.X, axis.Xyz.Y, axis.Xyz.Z));
xmlBuilder.Append(String.Format("<calibration rising='{0}' falling='{1}'/>", calibration.Rising, calibration.Falling));
xmlBuilder.Append(String.Format("<dynamics damping='{0}' friction='{1}'/>", dynamics.Damping, dynamics.Friction));
xmlBuilder.Append(String.Format("<limit lower='{0}' upper='{1}' effort='{2}' velocity='{3}'/>", limit.Lower, limit.Upper, limit.Effort, limit.Velocity));
xmlBuilder.Append(String.Format("<mimic joint='{0}' multiplier='{1}' offset='{2}'/>", MIMIC_JOINT_NAME, mimic.Multiplier, mimic.Offset));
xmlBuilder.Append(String.Format("<safety_controller soft_lower_limit='{0}' soft_upper_limit='{1}' k_position='{2}' k_velocity='{3}'/>", safetyController.SoftLowerLimit, safetyController.SoftUpperLimit, safetyController.KPosition, safetyController.KVelocity));
xmlBuilder.Append("</joint>");
this.xmlDoc.Load(XmlReader.Create(new StringReader(xmlBuilder.ToString())));
Joint joint = this.parser.Parse(this.xmlDoc.DocumentElement);
Assert.AreEqual(name, joint.Name);
Assert.AreEqual(JointParser.GetJointTypeFromName(type), joint.Type);
Assert.AreEqual(origin, joint.Origin);
Assert.AreEqual(PARENT_JOINT_NAME, joint.Parent.Name);
Assert.AreEqual(CHILD_JOINT_NAME, joint.Child.Name);
Assert.AreEqual(axis, joint.Axis);
Assert.AreEqual(calibration, joint.Calibration);
Assert.AreEqual(dynamics, joint.Dynamics);
Assert.AreEqual(limit, joint.Limit);
Assert.AreEqual(mimic, joint.Mimic);
Assert.AreEqual(safetyController, joint.SafetyController);
}
/// <summary>
/// Parses a URDF <origin> element from XML.
/// </summary>
/// <param name="node">The XML node of a <origin> element. MUST NOT BE NULL</param>
/// <returns>An Origin object parsed from the XML</returns>
public override Origin Parse(XmlNode node)
{
ValidateXmlNode(node);
XmlAttribute xyzAttribute = GetAttributeFromNode(node, UrdfSchema.XYZ_ATTRIBUTE_NAME);
XmlAttribute rpyAttribute = GetAttributeFromNode(node, UrdfSchema.RPY_ATTRIBUTE_NAME);
Origin.Builder originBuilder = new Origin.Builder();
if (xyzAttribute != null)
{
if (!RegexUtils.IsMatchNDoubles(xyzAttribute.Value, 3))
{
LogMalformedAttribute(UrdfSchema.XYZ_ATTRIBUTE_NAME);
}
else
{
double[] values = RegexUtils.MatchDoubles(xyzAttribute.Value);
originBuilder.SetXyz(new XyzAttribute(values[0], values[1], values[2]));
}
}
if (rpyAttribute != null)
{
if (!RegexUtils.IsMatchNDoubles(rpyAttribute.Value, 3))
{
LogMalformedAttribute(UrdfSchema.RPY_ATTRIBUTE_NAME);
}
else
{
double[] values = RegexUtils.MatchDoubles(rpyAttribute.Value);
originBuilder.SetRpy(new RpyAttribute(values[0], values[1], values[2]));
}
}
return(originBuilder.Build());
}
public void ParseVisual()
{
string name = "name";
Geometry geometry = new Geometry(new Box(new SizeAttribute(1, 2, 3)));
Origin origin = new Origin.Builder().SetXyz(new XyzAttribute(1, 2, 3)).SetRpy(new RpyAttribute(4, 5, 6)).Build();
Material material = new Material("material", new Color(new RgbAttribute(255, 255, 255)));
string format = @"<visual name='{0}'>
<geometry><box size='{1} {2} {3}'/></geometry>
<origin xyz='{4} {5} {6}' rpy='{7} {8} {9}'/>
<material name='{10}'><color rgb='{11} {12} {13}'/></material>
</visual>";
string xml = String.Format(format, name, geometry.Box.Size.Length, geometry.Box.Size.Width,
geometry.Box.Size.Height,
origin.Xyz.X, origin.Xyz.Y, origin.Xyz.Z, origin.Rpy.R, origin.Rpy.P, origin.Rpy.Y,
material.Name, material.Color.Rgb.R, material.Color.Rgb.G, material.Color.Rgb.B);
this.xmlDoc.Load(XmlReader.Create(new StringReader(xml)));
Visual visual = this.parser.Parse(this.xmlDoc.DocumentElement);
Assert.AreEqual(name, visual.Name);
Assert.AreEqual(geometry, visual.Geometry);
Assert.AreEqual(origin, visual.Origin);
Assert.AreEqual(material, visual.Material);
}
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 ConstructDefaultOriginWithBuilderNullRpy()
{
Origin.Builder builder = new Origin.Builder().SetRpy(null);
}
