Пример #1
0
        public void ConstructDefault()
        {
            RpyAttribute rpy = new RpyAttribute();

            Assert.AreEqual(rpy.R, 0);
            Assert.AreEqual(rpy.P, 0);
            Assert.AreEqual(rpy.Y, 0);
        }
Пример #2
0
        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);
        }
Пример #3
0
        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);
        }
Пример #4
0
        public void ConstructRpy()
        {
            int          r   = 1;
            int          p   = 2;
            int          y   = 3;
            RpyAttribute rpy = new RpyAttribute(r, p, y);

            Assert.AreEqual(rpy.R, r);
            Assert.AreEqual(rpy.P, p);
            Assert.AreEqual(rpy.Y, y);
        }
Пример #5
0
        public void EqualsAndHash()
        {
            RpyAttribute rpy  = new RpyAttribute(1, 2, 3);
            RpyAttribute same = new RpyAttribute(1, 2, 3);
            RpyAttribute diff = new RpyAttribute(3, 2, 1);

            Assert.IsTrue(rpy.Equals(rpy));
            Assert.IsFalse(rpy.Equals(null));
            Assert.IsTrue(rpy.Equals(same));
            Assert.IsTrue(same.Equals(rpy));
            Assert.IsFalse(rpy.Equals(diff));
            Assert.AreEqual(rpy.GetHashCode(), same.GetHashCode());
            Assert.AreNotEqual(rpy.GetHashCode(), diff.GetHashCode());
        }
Пример #6
0
        /// <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);
        }
Пример #7
0
        /// <summary>
        /// Adds a new sensor to the Robot model, creating a joint object for the component being added.
        /// </summary>
        /// <param name="component">The Robot model object being added. MUST NOT BE NULL</param>
        /// <param name="parent">The name of the parent link on this Robot that this component is linked to. MUST NOT BE NULL OR EMPTY</param>
        /// <param name="child">The name of the child link on the component Robot that this component is linked by. 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>The name of the connected Link object if the component was successfully added, otherwise <c>null</c></returns>
        public string AddComponent(Robot component, string parent, string child, 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.IsNotEmpty(child, $"Cannot add component '{component.Name}' to the Robot model with missing child 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);
            }

            if (!component.Links.ContainsKey(child))
            {
                //LOGGER.Warn($"Adding component '{component.Name}' to '{Name}' Robot model failed because '{component.Name}' doesn't contain link called '{child}'");
                return(null);
            }

            string jointName = GenerateUniqueKey($"{component.Name}_joint", new List <string>(this.Joints.Keys));
            Joint  newJoint  = new Joint.Builder(jointName, Joint.JointType.Fixed, this.Links[parent], component.Links[child]).Build();

            this.Joints.Add(newJoint.Name, newJoint);

            foreach (Link link in component.Links.Values)
            {
                this.Links.Add(link.Name, link);
            }
            foreach (Joint joint in component.Joints.Values)
            {
                this.Joints.Add(joint.Name, joint);
            }

            return(child);
        }
Пример #8
0
 protected bool Equals(RpyAttribute other)
 {
     return(R.Equals(other.R) && P.Equals(other.P) && Y.Equals(other.Y));
 }
Пример #9
0
 /// <summary>
 /// Sets the Origin's RpyAttribute.
 /// </summary>
 /// <param name="rpy">The origin element's fixed axis roll, pitch and yaw. MUST NOT BE NULL</param>
 /// <returns>This Origin.Builder instance</returns>
 public Builder SetRpy(RpyAttribute rpy)
 {
     Preconditions.IsNotNull(rpy, "Origin rpy property cannot be set to null");
     this.rpy = rpy;
     return(this);
 }
Пример #10
0
 /// <summary>
 /// Creates a new instance of Origin with the specified xyz and rpy attribute values.
 /// An Origin.Builder must be used to instantiate an Origin with optional properties as specified.
 /// </summary>
 /// <param name="xyz">The origin element's x, y, z offset.</param>
 /// <param name="rpy">The origin element's fixed axis roll, pitch and yaw.</param>
 private Origin(XyzAttribute xyz, RpyAttribute rpy)
 {
     this.Xyz = xyz;
     this.Rpy = rpy;
 }
Пример #11
0
 /// <summary>
 /// Creates a new instance of Origin with default xyz and rpy attribute values.
 /// </summary>
 public Origin()
 {
     this.Xyz = new XyzAttribute();
     this.Rpy = new RpyAttribute();
 }