/// <summary>
/// Adds a new sensor to the Robot model, creating a joint and link object for the component being added.
/// </summary>
/// <param name="component">The component object being added. MUST NOT BE NULL</param>
/// <param name="parent">The name of the parent link that this component is linked to. MUST NOT BE NULL OR EMPTY</param>
/// <param name="xyz">The XYZ offset of this component from its parent link. MUST NOT BE NULL</param>
/// <param name="rpy">The RPY offset of this component from its parent link. MUST NOT BE NULL</param>
/// <returns><c>true</c> if the component was successfully added, otherwise <c>false</c></returns>
public string AddComponent(Component component, string parent, XyzAttribute xyz, RpyAttribute rpy)
{
Preconditions.IsNotNull(component, "Cannot add a null component to the Robot");
Preconditions.IsNotEmpty(parent, $"Cannot add component '{component.Name}' to the Robot model with missing parent name");
Preconditions.IsNotNull(xyz, $"Cannot add component '{component.Name}' to '{Name}' Robot model with null XYZ offset");
Preconditions.IsNotNull(rpy, $"Cannot add component '{component.Name}' to '{Name}' Robot model with null RPY offset");
if (!this.Links.ContainsKey(parent))
{
//LOGGER.Warn($"Adding component '{component.Name}' to '{Name}' Robot model failed because '{Name}' doesn't contain link called '{parent}'");
return(null);
}
string linkName = GenerateUniqueKey(component.Name, new List <string>(this.Links.Keys));
string jointName = GenerateUniqueKey($"{component.Name}_joint", new List <string>(this.Joints.Keys));
Geometry geometry = (component.Box != null) ? new Geometry(new Box(component.Box.Size))
: new Geometry(new Mesh.Builder(component.FileName).Build());
Visual visual = new Visual.Builder(geometry).Build();
Link link = new Link.Builder(linkName).SetVisual(visual).Build();
Joint joint = new Joint.Builder(jointName, Joint.JointType.Fixed, this.Links[parent], link).Build();
this.Links.Add(link.Name, link);
this.Joints.Add(joint.Name, joint);
return(linkName);
}
public void ParseLink()
{
string name = "name";
Geometry geometry1 = new Geometry(new Sphere(1));
Geometry geometry2 = new Geometry(new Box(new SizeAttribute(1, 2, 3)));
Visual visual1 = new Visual.Builder(geometry1).Build();
Visual visual2 = new Visual.Builder(geometry2).Build();
Collision collision = new Collision.Builder().SetGeometry(geometry1).Build();
Inertial inertial = new Inertial(new Mass(1), new Inertia(1, 2, 3, 4, 5, 6));
string format = @"<link name='{0}'>
<visual><geometry><sphere radius='{1}'/></geometry></visual>
<visual><geometry><box size='{2} {3} {4}'/></geometry></visual>
<collision><geometry><sphere radius='{5}'/></geometry></collision>
<inertial><mass value='{6}'/><inertia ixx='{7}' ixy='{8}' ixz='{9}' iyy='{10}' iyz='{11}' izz='{12}'/></inertial>
</link>";
string xml = String.Format(format, name, visual1.Geometry.Sphere.Radius,
visual2.Geometry.Box.Size.Length, visual2.Geometry.Box.Size.Width, visual2.Geometry.Box.Size.Height,
collision.Geometry.Sphere.Radius, inertial.Mass.Value, inertial.Inertia.Ixx, inertial.Inertia.Ixy,
inertial.Inertia.Ixz, inertial.Inertia.Iyy, inertial.Inertia.Iyz, inertial.Inertia.Izz);
this.xmlDoc.Load(XmlReader.Create(new StringReader(xml)));
Link link = this.parser.Parse(this.xmlDoc.DocumentElement);
Assert.AreEqual(name, link.Name);
Assert.AreEqual(2, link.Visual.Count);
Assert.AreEqual(visual1, link.Visual[0]);
Assert.AreEqual(visual2, link.Visual[1]);
Assert.AreEqual(1, link.Collision.Count);
Assert.AreEqual(collision, link.Collision[0]);
Assert.AreEqual(inertial, link.Inertial);
}
public void ConstructVisualOnlyGeometry()
{
Geometry geometry = new Geometry(new Sphere(1));
Visual visual = new Visual.Builder(geometry).Build();
Assert.AreEqual(geometry, visual.Geometry);
Assert.IsNull(visual.Material);
Assert.AreEqual(new Origin(), visual.Origin);
}
public void ConstructVisualWithMaterial()
{
Geometry geometry = new Geometry(new Sphere(1));
Material material = new Material("name");
Visual visual = new Visual.Builder(geometry).SetMaterial(material).Build();
Assert.AreEqual(new Origin(), visual.Origin);
Assert.AreEqual(geometry, visual.Geometry);
Assert.AreEqual(material, visual.Material);
}
public void ConstructVisualWithOrigin()
{
Origin origin = new Origin();
Geometry geometry = new Geometry(new Sphere(1));
Visual visual = new Visual.Builder(geometry).SetOrigin(origin).Build();
Assert.AreEqual(origin, visual.Origin);
Assert.AreEqual(geometry, visual.Geometry);
Assert.IsNull(visual.Material);
}
public void ConstructLinkChainBuilderSetters()
{
string name = "linkName";
Inertial inertial = new Inertial(new Mass(1), new Inertia(1, 1, 1, 1, 1, 1));
Visual visual = new Visual.Builder(new Geometry(new Sphere(1))).Build();
List<Collision> collisions = new List<Collision>();
Link link = new Link.Builder(name).SetInertial(inertial).SetVisual(visual).SetCollision(collisions).Build();
Assert.AreEqual(name, link.Name);
Assert.AreEqual(inertial, link.Inertial);
Assert.AreEqual(visual, link.Visual[0]);
Assert.AreEqual(collisions, link.Collision);
}
public void EqualsAndHash()
{
Visual visual = new Visual.Builder(new Geometry(new Sphere(1))).Build();
Visual same = new Visual.Builder(new Geometry(new Sphere(1))).Build();
Visual diff = new Visual.Builder(new Geometry(new Sphere(2))).Build();
Assert.IsTrue(visual.Equals(visual));
Assert.IsFalse(visual.Equals(null));
Assert.IsTrue(visual.Equals(same));
Assert.IsTrue(same.Equals(visual));
Assert.IsFalse(visual.Equals(diff));
Assert.AreEqual(visual.GetHashCode(), same.GetHashCode());
Assert.AreNotEqual(visual.GetHashCode(), diff.GetHashCode());
}
public void EqualsAndHash()
{
Inertial inertial = new Inertial(new Mass(1), new Inertia(1, 1, 1, 1, 1, 1));
Visual visual = new Visual.Builder(new Geometry(new Sphere(1))).Build();
List<Visual> visualList = new List<Visual>();
List<Visual> diffVisualList = new List<Visual>();
List<Collision> collisionList = new List<Collision>();
visualList.Add(visual);
diffVisualList.Add(visual);
diffVisualList.Add(new Visual.Builder(new Geometry(new Box(new SizeAttribute(1, 2, 3)))).Build());
Link link = new Link.Builder("link").SetInertial(inertial).SetVisual(visual).SetCollision(collisionList).Build();
Link same1 = new Link.Builder("link").SetInertial(inertial).SetVisual(visual).SetCollision(collisionList).Build();
Link same2 = new Link.Builder("link").SetInertial(inertial).SetVisual(visualList).SetCollision(collisionList).Build();
Link same3 = new Link.Builder("link").SetInertial(inertial).SetVisual(visualList).Build();
Link diff1 = new Link.Builder("different_link").SetInertial(inertial).SetVisual(visual).SetCollision(collisionList).Build();
Link diff2 = new Link.Builder("link").SetVisual(visual).SetCollision(collisionList).Build();
Link diff3 = new Link.Builder("link").SetInertial(inertial).SetCollision(collisionList).Build();
Link diff4 = new Link.Builder("link").SetInertial(inertial).SetVisual(diffVisualList).SetCollision(collisionList).Build();
Link diff5 = new Link.Builder("link").Build();
Assert.IsTrue(link.Equals(link));
Assert.IsFalse(link.Equals(null));
Assert.IsTrue(link.Equals(same1));
Assert.IsTrue(link.Equals(same2));
Assert.IsTrue(link.Equals(same3));
Assert.IsTrue(same1.Equals(link));
Assert.IsTrue(same2.Equals(link));
Assert.IsTrue(same3.Equals(link));
Assert.IsFalse(link.Equals(diff1));
Assert.IsFalse(link.Equals(diff2));
Assert.IsFalse(link.Equals(diff3));
Assert.IsFalse(link.Equals(diff4));
Assert.IsFalse(link.Equals(diff5));
Assert.AreEqual(link.GetHashCode(), link.GetHashCode());
Assert.AreEqual(link.GetHashCode(), same1.GetHashCode());
Assert.AreEqual(link.GetHashCode(), same2.GetHashCode());
Assert.AreEqual(link.GetHashCode(), same3.GetHashCode());
Assert.AreNotEqual(link.GetHashCode(), diff1.GetHashCode());
Assert.AreNotEqual(link.GetHashCode(), diff2.GetHashCode());
Assert.AreNotEqual(link.GetHashCode(), diff3.GetHashCode());
Assert.AreNotEqual(link.GetHashCode(), diff4.GetHashCode());
Assert.AreNotEqual(link.GetHashCode(), diff5.GetHashCode());
}
public void ParseLinkVisualOnly()
{
string name = "name";
Geometry geometry = new Geometry(new Sphere(1));
Visual visual = new Visual.Builder(geometry).Build();
string format = @"<link name='{0}'>
<visual><geometry><sphere radius='{1}'/></geometry></visual>
</link>";
string xml = String.Format(format, name, visual.Geometry.Sphere.Radius);
this.xmlDoc.Load(XmlReader.Create(new StringReader(xml)));
Link link = this.parser.Parse(this.xmlDoc.DocumentElement);
Assert.AreEqual(name, link.Name);
Assert.AreEqual(1, link.Visual.Count);
Assert.AreEqual(visual, link.Visual[0]);
Assert.AreEqual(0, link.Collision.Count);
Assert.IsNull(link.Inertial);
}
/// <summary>
/// Parses a URDF <visual> element from XML.
/// </summary>
/// <param name="node">The XML node of a <visual> element</param>
/// <returns>A Visual object parsed from the XML</returns>
public override Visual Parse(XmlNode node)
{
ValidateXmlNode(node);
XmlAttribute nameAttribute = GetAttributeFromNode(node, UrdfSchema.NAME_ATTRIBUTE_NAME);
XmlElement originElement = GetElementFromNode(node, UrdfSchema.ORIGIN_ELEMENT_NAME);
XmlElement geometryElement = GetElementFromNode(node, UrdfSchema.GEOMETRY_ELEMENT_NAME);
XmlElement materialElement = GetElementFromNode(node, UrdfSchema.MATERIAL_ELEMENT_NAME);
Visual.Builder builder;
if (geometryElement == null)
{
LogMissingRequiredElement(UrdfSchema.GEOMETRY_ELEMENT_NAME);
builder = new Visual.Builder(GeometryParser.DEFAULT_GEOMETRY);
}
else
{
builder = new Visual.Builder(this.geometryParser.Parse(geometryElement));
}
if (nameAttribute != null)
{
builder.SetName(nameAttribute.Value);
}
if (originElement != null)
{
builder.SetOrigin(this.originParser.Parse(originElement));
}
if (materialElement != null)
{
builder.SetMaterial(this.materialParser.Parse(materialElement));
}
return(builder.Build());
}
public void ConstructVisualNullOrigin()
{
Visual visual = new Visual.Builder(new Geometry(new Sphere(1))).SetOrigin(null).Build();
}
public void ConstructVisualNullGeometry()
{
Visual visual = new Visual.Builder(null).Build();
}
