public void ImportJointData_SpecificLimit_Succeeds()
        {
            var joint = new Joint(
                name: "custom_joint", type: "planar", parent: "base", child: "link",
                limit: new Joint.Limit(4, 5, 6, 7),
                dynamics: new Joint.Dynamics(8, 9));

            GameObject baseObject = new GameObject("base");
            GameObject linkObject = new GameObject("link");

            linkObject.transform.parent = baseObject.transform;

            UrdfJoint.Create(baseObject, UrdfJoint.JointTypes.Fixed);
            TestUrdfJointPlanar urdfJoint        = linkObject.AddComponent <TestUrdfJointPlanar>();
            ArticulationBody    articulationBody = linkObject.GetComponent <ArticulationBody>();

            urdfJoint.TestImportJointData(joint);

            Assert.AreEqual(ArticulationDofLock.LockedMotion, articulationBody.linearLockX);
            Assert.AreEqual(ArticulationDofLock.LimitedMotion, articulationBody.linearLockY);
            Assert.AreEqual(ArticulationDofLock.LimitedMotion, articulationBody.linearLockZ);

            Assert.AreEqual(4, articulationBody.xDrive.lowerLimit);
            Assert.AreEqual(4, articulationBody.yDrive.lowerLimit);
            Assert.AreEqual(4, articulationBody.zDrive.lowerLimit);
            Assert.AreEqual(5, articulationBody.xDrive.upperLimit);
            Assert.AreEqual(5, articulationBody.yDrive.upperLimit);
            Assert.AreEqual(5, articulationBody.zDrive.upperLimit);
            Assert.AreEqual(6, articulationBody.xDrive.forceLimit);
            Assert.AreEqual(6, articulationBody.yDrive.forceLimit);
            Assert.AreEqual(6, articulationBody.zDrive.forceLimit);
            Assert.AreEqual(7, articulationBody.maxLinearVelocity);

            Object.DestroyImmediate(baseObject);
        }
        public void ExportSpecificJointData_Succeeds()
        {
            GameObject            linkObject       = new GameObject("link");
            TestUrdfJointRevolute urdfJoint        = linkObject.AddComponent <TestUrdfJointRevolute>();
            ArticulationBody      articulationBody = linkObject.GetComponent <ArticulationBody>();

            urdfJoint.SetAxisOfMotion(new Vector3(1.2345678f, 2.3456789f, 3.4567891f));
            urdfJoint.Dynamics(new Joint.Dynamics(4, 5));

            var joint = new Joint(
                name: "custom_joint", type: "continuous", parent: "base", child: "link");

            urdfJoint.TestExportSpecificJointData(joint);

            UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.234568f, (float)joint.axis.xyz[0]);
            UnityEngine.Assertions.Assert.AreApproximatelyEqual(2.345679f, (float)joint.axis.xyz[1]);
            UnityEngine.Assertions.Assert.AreApproximatelyEqual(3.456789f, (float)joint.axis.xyz[2]);
            Assert.AreEqual(4, joint.dynamics.damping);
            Assert.AreEqual(5, joint.dynamics.friction);
            Assert.AreEqual(articulationBody.xDrive.lowerLimit * Mathf.Deg2Rad, joint.limit.lower);
            Assert.AreEqual(articulationBody.xDrive.upperLimit * Mathf.Deg2Rad, joint.limit.upper);
            Assert.AreEqual(articulationBody.xDrive.forceLimit, joint.limit.effort);
            Assert.AreEqual(articulationBody.maxAngularVelocity, joint.limit.velocity);

            Object.DestroyImmediate(linkObject);
        }
コード例 #3
0
        public void ExportDefaultJointData_DefaultJoint_Succeeds()
        {
            Vector3    position = new Vector3(1, 2, 3);
            Quaternion rotation = Quaternion.Euler(4, 5, 6);

            GameObject baseObject = new GameObject("base");
            GameObject linkObject = new GameObject("link");

            linkObject.transform.parent   = baseObject.transform;
            linkObject.transform.position = position;
            linkObject.transform.rotation = rotation;

            Joint joint = UrdfJoint.ExportDefaultJoint(linkObject.transform);

            Assert.AreEqual("base_link_joint", joint.name);
            Assert.AreEqual("fixed", joint.type);
            Assert.AreEqual(baseObject.name, joint.parent);
            Assert.AreEqual(linkObject.name, joint.child);

            Assert.AreEqual(new double[] { position[2], -position[0], position[1] }, joint.origin.Xyz);
            UnityEngine.Assertions.Assert.AreApproximatelyEqual(-rotation.eulerAngles[2] * Mathf.Deg2Rad, (float)joint.origin.Rpy[0]);
            UnityEngine.Assertions.Assert.AreApproximatelyEqual(rotation.eulerAngles[0] * Mathf.Deg2Rad, (float)joint.origin.Rpy[1]);
            UnityEngine.Assertions.Assert.AreApproximatelyEqual(-rotation.eulerAngles[1] * Mathf.Deg2Rad, (float)joint.origin.Rpy[2]);

            Object.DestroyImmediate(baseObject);
            Object.DestroyImmediate(linkObject);
        }
        public void AreLimitsCorrect_Fails()
        {
            var joint = new Joint(
                name: "custom_joint", type: "planar", parent: "base", child: "link",
                limit: new Joint.Limit(5, 4, 6, 7),
                dynamics: new Joint.Dynamics(8, 9));
            GameObject linkObject = new GameObject("link");
            UrdfJoint  urdfJoint  = UrdfJoint.Create(linkObject, UrdfJoint.JointTypes.Revolute, joint);

            Assert.IsFalse(urdfJoint.AreLimitsCorrect());

            Object.DestroyImmediate(linkObject);
        }
        public void AreLimitsCorrect_Succeeds()
        {
            var joint = new Joint(
                name: "custom_joint", type: "planar", parent: "base", child: "link",
                limit: new Joint.Limit(4, 5, 6, 7),
                dynamics: new Joint.Dynamics(8, 9));
            GameObject linkObject = new GameObject("link");
            UrdfJoint  urdfJoint  = UrdfJoint.Create(linkObject, UrdfJoint.JointTypes.Prismatic, joint);

            Assert.IsTrue(urdfJoint.AreLimitsCorrect());

            Object.DestroyImmediate(linkObject);
        }
コード例 #6
0
        public void Create_WithOtherTypeOfJointData_FixedArticulationBody()
        {
            Joint joint = new Joint(
                name: "reference", type: "prismatic", parent: null, child: null);
            GameObject       linkObject       = new GameObject("link");
            UrdfJoint        urdfJoint        = UrdfJoint.Create(linkObject, UrdfJoint.JointTypes.Fixed, joint);
            ArticulationBody articulationBody = linkObject.GetComponent <ArticulationBody>();

            Assert.IsNotNull(urdfJoint);
            Assert.IsNotNull(articulationBody);
            Assert.AreEqual(UrdfJoint.JointTypes.Fixed, urdfJoint.JointType);
            Assert.AreEqual(ArticulationJointType.FixedJoint, articulationBody.jointType);
        }
コード例 #7
0
        public void Create_WithJointData_Succeeds()
        {
            GameObject baseObject = new GameObject("base");
            GameObject linkObject = new GameObject("link");

            linkObject.transform.parent = baseObject.transform;

            var       joint     = new Joint("custom_name", "revolute", "base", "link");
            UrdfJoint urdfJoint = UrdfJoint.Create(linkObject, UrdfJoint.JointTypes.Prismatic, joint);

            Assert.AreEqual("custom_name", urdfJoint.jointName);

            Object.DestroyImmediate(baseObject);
            Object.DestroyImmediate(linkObject);
        }
        public void ExportSpecificJointData_Succeeds()
        {
            GameObject          linkObject = new GameObject("link");
            TestUrdfJointPlanar urdfJoint  = linkObject.AddComponent <TestUrdfJointPlanar>();

            urdfJoint.SetAxisOfMotion(new Vector3(1.2345678f, 2.3456789f, 3.4567891f));
            urdfJoint.Dynamics(new Joint.Dynamics(4, 5));

            var joint = new Joint(
                name: "custom_joint", type: "planar", parent: "base", child: "link");

            urdfJoint.TestExportSpecificJointData(joint);

            UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.234568f, (float)joint.axis.xyz[0]);
            UnityEngine.Assertions.Assert.AreApproximatelyEqual(2.345679f, (float)joint.axis.xyz[1]);
            UnityEngine.Assertions.Assert.AreApproximatelyEqual(3.456789f, (float)joint.axis.xyz[2]);
            Assert.AreEqual(4, joint.dynamics.damping);
            Assert.AreEqual(5, joint.dynamics.friction);

            Object.DestroyImmediate(linkObject);
        }
        public void ImportJointData_SpecificAixs_Succeeds()
        {
            var joint = new Joint(
                name: "custom_joint", type: "prismatic", parent: "base", child: "link",
                axis: new Joint.Axis(new double[] { 1, 2, 3 }),
                limit: new Joint.Limit(4, 5, 6, 7),
                dynamics: new Joint.Dynamics(8, 9));

            GameObject baseObject = new GameObject("base");
            GameObject linkObject = new GameObject("link");

            linkObject.transform.parent = baseObject.transform;

            UrdfJoint.Create(baseObject, UrdfJoint.JointTypes.Fixed);
            TestUrdfJointRevolute urdfJoint        = linkObject.AddComponent <TestUrdfJointRevolute>();
            ArticulationBody      articulationBody = linkObject.GetComponent <ArticulationBody>();

            urdfJoint.TestImportJointData(joint);

            Assert.AreEqual(ArticulationDofLock.LimitedMotion, articulationBody.linearLockX);
            Assert.AreEqual(ArticulationDofLock.LockedMotion, articulationBody.linearLockY);
            Assert.AreEqual(ArticulationDofLock.LockedMotion, articulationBody.linearLockZ);
            Assert.AreEqual(ArticulationDofLock.LimitedMotion, articulationBody.twistLock);

            Quaternion expectedAnchorRotation = new Quaternion();

            expectedAnchorRotation.SetFromToRotation(new Vector3(1, 0, 0), -new Vector3(-2, 3, 1));
            Assert.AreEqual(expectedAnchorRotation, articulationBody.anchorRotation);

            Assert.AreEqual(4 * Mathf.Rad2Deg, articulationBody.xDrive.lowerLimit);
            Assert.AreEqual(5 * Mathf.Rad2Deg, articulationBody.xDrive.upperLimit);
            Assert.AreEqual(6, articulationBody.xDrive.forceLimit);
            Assert.AreEqual(7, articulationBody.maxAngularVelocity);
            Assert.AreEqual(8, articulationBody.linearDamping);
            Assert.AreEqual(8, articulationBody.angularDamping);
            Assert.AreEqual(9, articulationBody.jointFriction);

            Object.DestroyImmediate(baseObject);
        }
コード例 #10
0
        public void ImportJointData_DefaultAxis_Succeeds(Joint.Axis axis, Quaternion expectedAnchorRotation)
        {
            var joint = new Joint(
                name: "custom_joint", type: "continuous", parent: "base", child: "link",
                axis: axis);

            GameObject baseObject = new GameObject("base");
            GameObject linkObject = new GameObject("link");

            linkObject.transform.parent = baseObject.transform;

            UrdfJoint.Create(baseObject, UrdfJoint.JointTypes.Fixed);
            TestUrdfJointPlanar urdfJoint        = linkObject.AddComponent <TestUrdfJointPlanar>();
            ArticulationBody    articulationBody = linkObject.GetComponent <ArticulationBody>();

            urdfJoint.TestImportJointData(joint);

            UnityEngine.Assertions.Assert.AreApproximatelyEqual(expectedAnchorRotation.w, articulationBody.anchorRotation.w);
            UnityEngine.Assertions.Assert.AreApproximatelyEqual(expectedAnchorRotation.x, articulationBody.anchorRotation.x);
            UnityEngine.Assertions.Assert.AreApproximatelyEqual(expectedAnchorRotation.y, articulationBody.anchorRotation.y);
            UnityEngine.Assertions.Assert.AreApproximatelyEqual(expectedAnchorRotation.z, articulationBody.anchorRotation.z);

            Object.DestroyImmediate(baseObject);
        }
 public Joint TestExportSpecificJointData(Joint joint)
 {
     unityJoint = gameObject.GetComponent <ArticulationBody>();
     return(ExportSpecificJointData(joint));
 }
 public void TestImportJointData(Joint joint)
 {
     unityJoint = gameObject.GetComponent <ArticulationBody>();
     ImportJointData(joint);
 }