public void FactoryTest() { // Create and safe file name and file path, to be usabel after the using clause. var amlxFileName = "IRB_120.amlx"; var amlxFilePath = Path.Combine(Environment.CurrentDirectory, amlxFileName); using ( var doc = AmlxDocument.Create()) { var project = new ProjectViewModel(doc); // DH ABB IRB 120 // theta, d, a, alpha // 0 165 0 0 // 0 125 0 -90 // -90 0 270 0 // 0 0 70 -90 // 0 302 0 90 // 0 0 0 -90 var j1 = new KinematicJointValue(doc) { Name = "J1", Minimum = -165, Maximum = 165, DefaultValue = 0 }; var j2 = new KinematicJointValue(doc) { Name = "J2", Minimum = -110, Maximum = 110, DefaultValue = 0 }; var j3 = new KinematicJointValue(doc) { Name = "J3", Minimum = -110, Maximum = 70, DefaultValue = 0 }; var j4 = new KinematicJointValue(doc) { Name = "J4", Minimum = -160, Maximum = 160, DefaultValue = 0 }; var j5 = new KinematicJointValue(doc) { Name = "J5", Minimum = -120, Maximum = 120, DefaultValue = 30 }; var j6 = new KinematicJointValue(doc) { Name = "J6", Minimum = -400, Maximum = 400, DefaultValue = 0 }; var cadDir = Path.Combine(Environment.CurrentDirectory, @"..\..\..\Resources\ABB_IRB_120"); var link0 = new KinematicLink(doc) { Name = "Base Link" }; var baseFlange = new Flange(doc) { Name = "Base", Type = FlangeType.Base }; baseFlange.Frame = new FrameProperty(doc) { Name = "BaseFrame", X = 0, Y = 0, Z = 0, RX = 0, RY = 0, RZ = 0 }; link0.Flanges.Add(baseFlange); var part0 = new PartViewModel(doc); var file0 = doc.Files.Add(Path.Combine(cadDir, "ABB_IRB_120_Base.STEP")); var geometry0 = new GeometryDataConnectorViewModel(doc) { Location = file0.Location, MimeType = XMLMimeTypeMapper.GetMimeType(file0.Location) }; part0.DataConnectors.Add(geometry0); link0.Parts.Add(part0); var link1 = new KinematicLink(doc) { Name = "Link 1" }; var part1 = new PartViewModel(doc); var file1 = doc.Files.Add(Path.Combine(cadDir, "ABB_IRB_120_Link1.STEP")); var geometry1 = new GeometryDataConnectorViewModel(doc) { Location = file1.Location, MimeType = XMLMimeTypeMapper.GetMimeType(file1.Location) }; part1.DataConnectors.Add(geometry1); link1.Parts.Add(part1); var link2 = new KinematicLink(doc) { Name = "Link 2" }; var part2 = new PartViewModel(doc); var file2 = doc.Files.Add(Path.Combine(cadDir, "ABB_IRB_120_Link2.STEP")); var geometry2 = new GeometryDataConnectorViewModel(doc) { Location = file2.Location, MimeType = XMLMimeTypeMapper.GetMimeType(file2.Location) }; part2.DataConnectors.Add(geometry2); link2.Parts.Add(part2); var link3 = new KinematicLink(doc) { Name = "Link 3" }; var part3 = new PartViewModel(doc); var file3 = doc.Files.Add(Path.Combine(cadDir, "ABB_IRB_120_Link3.STEP")); var geometry3 = new GeometryDataConnectorViewModel(doc) { Location = file3.Location, MimeType = XMLMimeTypeMapper.GetMimeType(file3.Location) }; part3.DataConnectors.Add(geometry3); link3.Parts.Add(part3); var link4 = new KinematicLink(doc) { Name = "Link 4" }; var part4 = new PartViewModel(doc); part4.Frame = new FrameProperty(doc) { Name = "Link4_Frame", X = 0, Y = 0, Z = 134, RX = 0, RY = 0, RZ = 0 }; var file4 = doc.Files.Add(Path.Combine(cadDir, "ABB_IRB_120_Link4.STEP")); var geometry4 = new GeometryDataConnectorViewModel(doc) { Location = file4.Location, MimeType = XMLMimeTypeMapper.GetMimeType(file4.Location) }; part4.DataConnectors.Add(geometry4); link4.Parts.Add(part4); var link5 = new KinematicLink(doc) { Name = "Link 5" }; var part5 = new PartViewModel(doc); var file5 = doc.Files.Add(Path.Combine(cadDir, "ABB_IRB_120_Link5.STEP")); var geometry5 = new GeometryDataConnectorViewModel(doc) { Location = file5.Location, MimeType = XMLMimeTypeMapper.GetMimeType(file5.Location) }; part5.DataConnectors.Add(geometry5); link5.Parts.Add(part5); var link6 = new KinematicLink(doc) { Name = "Link 6" }; var tcpFlange = new Flange(doc) { Name = "TcpFlange", Type = FlangeType.Tcp }; tcpFlange.Frame = new FrameProperty(doc) { Name = "TcpFrame", X = 0, Y = 0, Z = 72, RX = 0, RY = 0, RZ = 0 }; link6.Flanges.Add(tcpFlange); var part6 = new PartViewModel(doc); part6.Frame = new FrameProperty(doc) { Name = "Link6_Frame", X = 0, Y = 0, Z = 72, RX = 0, RY = 0, RZ = 0 }; var file6 = doc.Files.Add(Path.Combine(cadDir, "ABB_IRB_120_Link6.STEP")); var geometry6 = new GeometryDataConnectorViewModel(doc) { Location = file6.Location, MimeType = XMLMimeTypeMapper.GetMimeType(file6.Location) }; part6.DataConnectors.Add(geometry6); link6.Parts.Add(part6); var joint1 = new KinematicJoint(doc, 0, 165, 0, 0) { Name = "Joint1", Base = link0.Id, Axis = link1.Id, JointType = KinematicAxisType.Revolution, AxisValue = $"{j1.Id}", Coupling = new CouplingPropertyViewModel(doc) { Active = true } }; var joint2 = new KinematicJoint(doc, 0, 125, 0, -90) { Name = "Joint2", Base = link1.Id, Axis = link2.Id, JointType = KinematicAxisType.Revolution, AxisValue = $"{j2.Id}", Coupling = new CouplingPropertyViewModel(doc) { Active = false, Parent = joint1.Id, Ratio = -0.5 } }; var joint3 = new KinematicJoint(doc, -90, 0, 270, 0) { Name = "Joint3", Base = link2.Id, Axis = link3.Id, JointType = KinematicAxisType.Revolution, AxisValue = $"{j3.Id}" }; var joint4 = new KinematicJoint(doc, 0, 0, 70, -90) { Name = "Joint4", Base = link3.Id, Axis = link4.Id, JointType = KinematicAxisType.Revolution, AxisValue = $"{j4.Id}" }; var joint5 = new KinematicJoint(doc, 0, 302, 0, 90) { Name = "Joint5", Base = link4.Id, Axis = link5.Id, JointType = KinematicAxisType.Revolution, AxisValue = $"{j5.Id}" }; var joint6 = new KinematicJoint(doc, 0, 0, 0, -90) { Name = "Joint6", Base = link5.Id, Axis = link6.Id, JointType = KinematicAxisType.Revolution, AxisValue = $"{j6.Id}" }; var robot = new Kinematic(doc) { Name = "IRB 120" }; robot.Links.Add(link0); robot.Links.Add(link1); robot.Links.Add(link2); robot.Links.Add(link3); robot.Links.Add(link4); robot.Links.Add(link5); robot.Links.Add(link6); robot.Joints.Add(joint1); robot.Joints.Add(joint2); robot.Joints.Add(joint3); robot.Joints.Add(joint4); robot.Joints.Add(joint5); robot.Joints.Add(joint6); robot.JointValues.Add(j1); robot.JointValues.Add(j2); robot.JointValues.Add(j3); robot.JointValues.Add(j4); robot.JointValues.Add(j5); robot.JointValues.Add(j6); project.Parts.Add(robot); var instanceHierarchy = project.CaexObject as InstanceHierarchyType; var project2 = new ProjectViewModel(instanceHierarchy, doc); var robot2 = project2.Parts.OfType <Kinematic>().Single(); Assert.Equal(robot, robot2); } }
private GameObject CreateFromData(URDFData data) { Transform actor = new GameObject(data.Name).transform; actor.position = new Vector3(0f, 0f, 0f); actor.rotation = Quaternion.identity; List <Transform> Links = new List <Transform>(); List <Transform> Joints = new List <Transform>(); //Create Link Transforms for (int i = 0; i < data.Links.Count; i++) { Transform link = CreateGeometry(data.Links[i].Geometry).transform; link.name = data.Links[i].Name; link.SetParent(actor, false); Links.Add(link); } //Create Joint Transforms for (int i = 0; i < data.Joints.Count; i++) { Transform joint = new GameObject().transform; joint.name = data.Joints[i].Name; joint.SetParent(actor); Joints.Add(joint); } //Apply Parent-Child Relations for (int i = 0; i < Joints.Count; i++) { Transform joint = Joints[i]; Transform parent = FindTransformByName(Links, data.GetJointData(joint.name).Parent); Transform child = FindTransformByName(Links, data.GetJointData(joint.name).Child); Transform parentJoint = actor; string parentName = data.GetLinkData(parent.name).Name; for (int j = 0; j < Joints.Count; j++) { if (data.GetJointData(Joints[j].name).Child == parentName) { parentJoint = Joints[j]; break; } } joint.SetParent(parentJoint); child.SetParent(joint); } Links = GetOrderedTransforms(actor.root, Links, new List <Transform>()); Joints = GetOrderedTransforms(actor.root, Joints, new List <Transform>()); for (int i = 0; i < Joints.Count; i++) { Transform joint = Joints[i]; Vector3 angles = -Mathf.Rad2Deg * ROSToUnity(data.GetJointData(joint.name).OriginRPY); Quaternion rotation = Quaternion.Euler(angles); joint.position = joint.parent.position + joint.parent.rotation * ROSToUnity(data.GetJointData(joint.name).OriginXYZ); joint.rotation = joint.parent.rotation * rotation; } for (int i = 0; i < Links.Count; i++) { Transform link = Links[i]; Vector3 angles = -Mathf.Rad2Deg * ROSToUnity(data.GetLinkData(link.name).RPY); Quaternion rotation = Quaternion.Euler(angles); link.localPosition += ROSToUnity(data.GetLinkData(link.name).XYZ); link.localRotation = rotation * link.localRotation; } //Initialize Links for (int i = 0; i < Links.Count; i++) { //Nothing to do. } //Initialize Joints for (int i = 0; i < Joints.Count; i++) { KinematicJoint joint = Joints[i].gameObject.AddComponent <KinematicJoint>(); URDFData.JointData jointData = data.GetJointData(joint.name); if (jointData.Type == "fixed") { //Nothing to do } else { switch (jointData.Type) { case "prismatic": joint.Type = JointType.Prismatic; break; case "revolute": joint.Type = JointType.Revolute; break; case "continuous": joint.Type = JointType.Continuous; break; } joint.Connection = Vector3.zero; Vector3 axis = ROSToUnity(jointData.Axis); if (joint.Type != JointType.Prismatic) { axis = -axis; } JointMotion motion = joint.XMotion; joint.AxisOrientation = Quaternion.FromToRotation(Vector3.right, axis).eulerAngles; motion.State = JointState.Free; motion.SetMaximumVelocity(jointData.Velocity); motion.SetMaximumAcceleration(jointData.Velocity); motion.SetLowerLimit(Mathf.Min(0f, jointData.LowerLimit)); motion.SetUpperLimit(Mathf.Max(0f, jointData.UpperLimit)); } } if (Errors == 0) { Debug.Log("Successfully imported '" + actor.name + "'."); } else { Debug.Log(Errors + " errors or warnings during importing '" + actor.name + "'.\n" + Output); } Output = string.Empty; Errors = 0; return(actor.gameObject); }