public void ConstructJointRequiredProperties()
{
Joint.Builder builder = new Joint.Builder(TEST_JOINT_NAME, TEST_JOINT_TYPE, TEST_PARENT_LINK, TEST_CHILD_LINK);
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.IsNotNull(joint.Origin);
Assert.AreEqual(0, joint.Origin.Xyz.X);
Assert.AreEqual(0, joint.Origin.Xyz.Y);
Assert.AreEqual(0, joint.Origin.Xyz.Z);
Assert.AreEqual(0, joint.Origin.Rpy.R);
Assert.AreEqual(0, joint.Origin.Rpy.P);
Assert.AreEqual(0, joint.Origin.Rpy.Y);
Assert.IsNotNull(joint.Axis);
Assert.AreEqual(1, joint.Axis.Xyz.X);
Assert.AreEqual(0, joint.Axis.Xyz.Z);
Assert.AreEqual(0, joint.Axis.Xyz.Z);
Assert.IsNull(joint.Calibration);
Assert.IsNull(joint.Dynamics);
Assert.IsNull(joint.Limit);
Assert.IsNull(joint.Mimic);
Assert.IsNull(joint.SafetyController);
}
/// <summary>
/// Parses a URDF <mimic> element from XML.
/// </summary>
/// <param name="node">The XML node of a <mimic> element</param>
/// <returns>An Mimic object parsed from the XML</returns>
public override Mimic Parse(XmlNode node)
{
ValidateXmlNode(node);
XmlAttribute jointAttribute = GetAttributeFromNode(node, UrdfSchema.JOINT_ATTRIBUTE_NAME);
XmlAttribute multiplierAttribute = GetAttributeFromNode(node, UrdfSchema.MULTIPLIER_ATTRIBUTE_NAME);
XmlAttribute offsetAttribute = GetAttributeFromNode(node, UrdfSchema.OFFSET_ATTRIBUTE_NAME);
Joint joint = DEFAULT_JOINT;
double multiplier = (multiplierAttribute != null) ? RegexUtils.MatchDouble(multiplierAttribute.Value, DEFAULT_MULTIPLIER) : DEFAULT_MULTIPLIER;
double offset = (offsetAttribute != null) ? RegexUtils.MatchDouble(offsetAttribute.Value, DEFAULT_OFFSET) : DEFAULT_OFFSET;
if (jointAttribute == null || String.IsNullOrEmpty(jointAttribute.Value))
{
LogMissingRequiredAttribute(UrdfSchema.JOINT_ATTRIBUTE_NAME);
}
else
{
if (!this.jointDictionary.ContainsKey(jointAttribute.Value))
{
//Logger.Info("Unknown joint name specified in <mimic>: {0}", jointAttribute.Value);
joint = new Joint.Builder(jointAttribute.Value, DEFAULT_JOINT.Type, DEFAULT_JOINT.Parent, DEFAULT_JOINT.Child).Build();
}
else
{
joint = this.jointDictionary[jointAttribute.Value];
}
}
return(new Mimic(joint, multiplier, offset));
}
public void ConstructRobot()
{
string name = "robot";
Dictionary <string, Link> links = new Dictionary <string, Link>();
Dictionary <string, Joint> joints = new Dictionary <string, Joint>();
Robot robot = new Robot(name, links, joints);
Link link1 = new Link.Builder("link1").Build();
Link link2 = new Link.Builder("link2").Build();
Link link3 = new Link.Builder("link3").Build();
Joint joint1 = new Joint.Builder("joint1", Joint.JointType.Continuous, link1, link2).Build();
Joint joint2 = new Joint.Builder("joint2", Joint.JointType.Continuous, link1, link3).Build();
links.Add(link1.Name, link1);
links.Add(link2.Name, link2);
links.Add(link3.Name, link3);
joints.Add(joint1.Name, joint1);
joints.Add(joint2.Name, joint2);
Assert.AreEqual(name, robot.Name);
Assert.AreEqual(links, robot.Links);
Assert.AreEqual(joints, robot.Joints);
foreach (Joint joint in robot.Joints.Values)
{
Assert.IsTrue(robot.Links.ContainsValue(joint.Parent));
Assert.IsTrue(robot.Links.ContainsValue(joint.Child));
}
}
public void EqualsAndHash()
{
Dictionary <string, Link> links = new Dictionary <string, Link>();
Dictionary <string, Link> differentLinks = new Dictionary <string, Link>();
Dictionary <string, Link> mostlySameLinks = new Dictionary <string, Link>();
Dictionary <string, Joint> joints = new Dictionary <string, Joint>();
Dictionary <string, Joint> mostlySameJoints = new Dictionary <string, Joint>();
Dictionary <string, Joint> differentJoints = new Dictionary <string, Joint>();
Link link1 = new Link.Builder("link1").Build();
Link link2 = new Link.Builder("link2").Build();
Link link3 = new Link.Builder("link3").Build();
Joint joint1 = new Joint.Builder("joint1", Joint.JointType.Continuous, link1, link2).Build();
Joint joint2 = new Joint.Builder("joint2", Joint.JointType.Continuous, link1, link3).Build();
Link differentLink1 = new Link.Builder("different_link1").Build();
Link differentLink2 = new Link.Builder("different_link2").Build();
Joint differentJoint = new Joint.Builder("differnt_joint", Joint.JointType.Continuous, differentLink1, differentLink2).Build();
links.Add(link1.Name, link1);
links.Add(link2.Name, link2);
links.Add(link3.Name, link3);
joints.Add(joint1.Name, joint1);
joints.Add(joint2.Name, joint2);
mostlySameLinks.Add(link1.Name, link1);
mostlySameLinks.Add(link2.Name, link2);
mostlySameJoints.Add(joint1.Name, joint1);
differentLinks.Add(differentLink1.Name, differentLink1);
differentLinks.Add(differentLink2.Name, differentLink2);
differentJoints.Add(differentJoint.Name, differentJoint);
Robot robot = new Robot("robot", links, joints);
Robot same1 = new Robot("robot", links, joints);
Robot same2 = new Robot("robot", new Dictionary <string, Link>(links), new Dictionary <string, Joint>(joints));
Robot diff1 = new Robot("different_robot", links, joints);
Robot diff2 = new Robot("robot", differentLinks, differentJoints);
Robot diff3 = new Robot("robot", links, mostlySameJoints);
Robot diff4 = new Robot("robot", mostlySameLinks, joints);
Assert.IsTrue(robot.Equals(robot));
Assert.IsFalse(robot.Equals(null));
Assert.IsTrue(robot.Equals(same1));
Assert.IsTrue(robot.Equals(same2));
Assert.IsTrue(same1.Equals(robot));
Assert.IsTrue(same2.Equals(robot));
Assert.IsFalse(robot.Equals(diff1));
Assert.IsFalse(robot.Equals(diff2));
Assert.IsFalse(robot.Equals(diff3));
Assert.IsFalse(robot.Equals(diff4));
Assert.AreEqual(robot.GetHashCode(), robot.GetHashCode());
Assert.AreEqual(robot.GetHashCode(), same1.GetHashCode());
Assert.AreEqual(robot.GetHashCode(), same2.GetHashCode());
Assert.AreNotEqual(robot.GetHashCode(), diff1.GetHashCode());
Assert.AreNotEqual(robot.GetHashCode(), diff2.GetHashCode());
Assert.AreNotEqual(robot.GetHashCode(), diff3.GetHashCode());
Assert.AreNotEqual(robot.GetHashCode(), diff4.GetHashCode());
}
public static void Initialize(TestContext context)
{
Joint joint1 = new Joint.Builder("joint1", Joint.JointType.Fixed, new Link.Builder("link1").Build(), new Link.Builder("link2").Build()).Build();
Joint joint2 = new Joint.Builder("joint2", Joint.JointType.Fixed, new Link.Builder("link3").Build(), new Link.Builder("link4").Build()).Build();
Joint joint3 = new Joint.Builder("joint3", Joint.JointType.Fixed, new Link.Builder("link5").Build(), new Link.Builder("link6").Build()).Build();
jointDictionary.Add(joint1.Name, joint1);
jointDictionary.Add(joint2.Name, joint2);
jointDictionary.Add(joint3.Name, joint3);
}
public static void Initialize(TestContext context)
{
Link parent = new Link.Builder(PARENT_JOINT_NAME).Build();
Link child = new Link.Builder(CHILD_JOINT_NAME).Build();
Joint mimic = new Joint.Builder(MIMIC_JOINT_NAME, Joint.JointType.Continuous, parent, child).Build();
linkDictionary.Add(parent.Name, parent);
linkDictionary.Add(child.Name, child);
jointDictionary.Add(mimic.Name, mimic);
}
/// <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 EqualsAndHash()
{
Joint joint = new Joint.Builder("joint", Joint.JointType.Continuous, new Link.Builder("parent").Build(), new Link.Builder("child").Build()).Build();
Mimic mimic = new Mimic(joint);
Mimic same = new Mimic(joint);
Mimic diff = new Mimic(joint, 1, 2);
Assert.IsTrue(mimic.Equals(mimic));
Assert.IsFalse(mimic.Equals(null));
Assert.IsTrue(mimic.Equals(same));
Assert.IsTrue(same.Equals(mimic));
Assert.IsFalse(mimic.Equals(diff));
Assert.AreEqual(mimic.GetHashCode(), same.GetHashCode());
Assert.AreNotEqual(mimic.GetHashCode(), diff.GetHashCode());
}
public void ToStringRobot()
{
Dictionary <string, Link> links = new Dictionary <string, Link>();
Dictionary <string, Joint> joints = new Dictionary <string, Joint>();
Robot robot = new Robot("robo", links, joints);
Link link1 = new Link.Builder("link1").Build();
Link link2 = new Link.Builder("link2").Build();
Joint joint1 = new Joint.Builder("joint1", Joint.JointType.Fixed, link1, link2).Build();
links.Add(link1.Name, link1);
links.Add(link2.Name, link2);
joints.Add(joint1.Name, joint1);
Assert.AreEqual("<robot name=\"robo\">\r\n<link name=\"link1\"/>\r\n<link name=\"link2\"/>\r\n<joint name=\"joint1\" type=\"fixed\">\r\n<parent link=\"link1\"/>\r\n<child link=\"link2\"/>\r\n</joint>\r\n</robot>",
robot.ToString().Replace(" ", ""));
}
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 EqualsAndHash()
{
Joint joint = new Joint.Builder("joint", Joint.JointType.Continuous, new Link.Builder("parent").Build(), new Link.Builder("child").Build()).Build();
Joint same = new Joint.Builder("joint", Joint.JointType.Continuous, new Link.Builder("parent").Build(), new Link.Builder("child").Build()).Build();
Joint diff1 = new Joint.Builder("different_joint", Joint.JointType.Continuous, new Link.Builder("parent").Build(), new Link.Builder("child").Build()).Build();
Joint diff2 = new Joint.Builder("joint", Joint.JointType.Fixed, new Link.Builder("parent").Build(), new Link.Builder("child").Build()).Build();
Joint diff3 = new Joint.Builder("joint", Joint.JointType.Continuous, new Link.Builder("differnt_parent").Build(), new Link.Builder("child").Build()).Build();
Joint diff4 = new Joint.Builder("joint", Joint.JointType.Continuous, new Link.Builder("parent").Build(), new Link.Builder("different_child").Build()).Build();
Assert.IsTrue(joint.Equals(joint));
Assert.IsFalse(joint.Equals(null));
Assert.IsTrue(joint.Equals(same));
Assert.IsTrue(same.Equals(joint));
Assert.IsFalse(joint.Equals(diff1));
Assert.IsFalse(joint.Equals(diff2));
Assert.IsFalse(joint.Equals(diff3));
Assert.IsFalse(joint.Equals(diff4));
Assert.AreEqual(joint.GetHashCode(), joint.GetHashCode());
Assert.AreEqual(joint.GetHashCode(), same.GetHashCode());
Assert.AreNotEqual(joint.GetHashCode(), diff1.GetHashCode());
Assert.AreNotEqual(joint.GetHashCode(), diff2.GetHashCode());
Assert.AreNotEqual(joint.GetHashCode(), diff3.GetHashCode());
Assert.AreNotEqual(joint.GetHashCode(), diff4.GetHashCode());
}
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 BuilderNullLimitRevoluteType()
{
Joint.Builder builder = new Joint.Builder(TEST_JOINT_NAME, Joint.JointType.Revolute, TEST_PARENT_LINK, TEST_CHILD_LINK);
builder.Build();
}
public void BuilderNullLimitPrismaticType()
{
Joint.Builder builder = new Joint.Builder(TEST_JOINT_NAME, Joint.JointType.Prismatic, TEST_PARENT_LINK, TEST_CHILD_LINK);
builder.Build();
}
public void ConstructBuilderNoChild()
{
Joint.Builder builder = new Joint.Builder(TEST_JOINT_NAME, TEST_JOINT_TYPE, TEST_PARENT_LINK, null);
}
public void ConstructBuilderNoParent()
{
Joint.Builder builder = new Joint.Builder(TEST_JOINT_NAME, TEST_JOINT_TYPE, null, TEST_CHILD_LINK);
}
public void ConstructBuilderNoName()
{
Joint.Builder builder = new Joint.Builder("", TEST_JOINT_TYPE, TEST_PARENT_LINK, TEST_CHILD_LINK);
}