public override void OnInspectorGUI() { Undo.RecordObject(Target, Target.name); using (var scope = new EditorGUILayout.VerticalScope("Button")) { EditorGUILayout.HelpBox("Geometry", MessageType.None); Target.SetJointType((JointType)EditorGUILayout.EnumPopup("Joint Type", Target.GetJointType())); Target.SetAnchor(EditorGUILayout.Vector3Field("Anchor", Target.GetAnchor())); Target.SetOrientation(EditorGUILayout.Vector3Field("Orientation", Target.GetOrientation())); } using (var scope = new EditorGUILayout.VerticalScope("Button")) { EditorGUILayout.HelpBox("Motion", MessageType.None); Target.SetMotionType((MotionType)EditorGUILayout.EnumPopup("Motion Type", Target.GetMotionType())); Target.SetSmoothing(EditorGUILayout.Slider("Smoothing", Target.GetSmoothing(), 0f, 1f)); if (Target.GetMotionType() == MotionType.Realistic) { Target.SetMaximumVelocity(EditorGUILayout.FloatField("Max Velocity", Target.GetMaximumVelocity())); Target.SetMaximumAcceleration(EditorGUILayout.FloatField("Max Acceleration", Target.GetMaximumAcceleration())); if (Target.GetMaximumVelocity() == 0f || Target.GetMaximumAcceleration() == 0f) { EditorGUILayout.HelpBox("Velocity and Acceleration must be assigned, or nothing will move.", MessageType.Warning); } } DrawMotionInspector(Target.GetXMotion(), "X"); DrawMotionInspector(Target.GetYMotion(), "Y"); DrawMotionInspector(Target.GetZMotion(), "Z"); } /* * EditorGUILayout.HelpBox( * "Current Value: " + Target.GetCurrentValue().ToString("F3") + "\n" + * "Current Error: " + Target.GetCurrentError().ToString("F3") + "\n" + * "Current Velocity: " + Target.GetCurrentVelocity().ToString("F3") + "\n" + * "Current Acceleration: " + Target.GetCurrentAcceleration().ToString("F3"), MessageType.None); */ EditorUtility.SetDirty(Target); /* * using (var scope = new EditorGUILayout.VerticalScope ("Button")) { * EditorGUILayout.HelpBox("Debug", MessageType.None); * EditorGUILayout.Vector3Field("Anchor", Target.GetAnchor()); * EditorGUILayout.Vector3Field("World Anchor", Target.GetAnchorInWorldSpace()); * EditorGUILayout.Vector3Field("Orientation", Target.GetOrientation()); * EditorGUILayout.Vector3Field("X Axis", Target.GetXMotion().Axis); * EditorGUILayout.Vector3Field("Y Axis", Target.GetYMotion().Axis); * EditorGUILayout.Vector3Field("Z Axis", Target.GetZMotion().Axis); * EditorGUILayout.Vector3Field("Default Reference Position", Target.GetDefaultReferencePosition()); * EditorGUILayout.Vector3Field("Default Reference Rotation", Target.GetDefaultReferenceRotation().eulerAngles); * } */ }
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++) { BioIK.KinematicJoint joint = Joints[i].gameObject.AddComponent <BioIK.KinematicJoint>(); URDFData.JointData jointData = data.GetJointData(joint.name); if (jointData.Type == "fixed") { //Nothing to do } else { switch (jointData.Type) { case "prismatic": joint.SetJointType(BioIK.JointType.Prismatic); break; case "revolute": joint.SetJointType(BioIK.JointType.Revolute); break; case "continuous": joint.SetJointType(BioIK.JointType.Continuous); break; } joint.SetAnchor(Vector3.zero); Vector3 axis = ROSToUnity(jointData.Axis); if (joint.GetJointType() != BioIK.JointType.Prismatic) { axis = -axis; } joint.SetOrientation(Quaternion.FromToRotation(Vector3.right, axis).eulerAngles); //joint.SetMaximumVelocity(jointData.Velocity); //joint.SetMaximumAcceleration(jointData.Velocity); BioIK.Motion motion = joint.GetXMotion(); motion.SetEnabled(true); motion.SetLowerLimit(jointData.LowerLimit); motion.SetUpperLimit(jointData.UpperLimit); joint.Initialise(); } } 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); }