//Adds a segment node into the OFKT data structure private void AddNode(Transform segment) { if (FindNode(segment) == null) { KinematicJoint joint = segment.GetComponent <KinematicJoint>(); Objective objective = segment.GetComponent <Objective>(); Node node = new Node(this, FindNode(segment.parent), segment, joint, objective); if (joint != null) { if (joint.GetDoF() == 0) { joint = null; } else { if (joint.GetXMotion().IsEnabled()) { MotionPtr motionPtr = new MotionPtr(joint.GetXMotion(), node, MotionPtrs.Length); System.Array.Resize(ref MotionPtrs, MotionPtrs.Length + 1); MotionPtrs[MotionPtrs.Length - 1] = motionPtr; node.XEnabled = true; node.XIndex = motionPtr.Index; } if (joint.GetYMotion().IsEnabled()) { MotionPtr motionPtr = new MotionPtr(joint.GetYMotion(), node, MotionPtrs.Length); System.Array.Resize(ref MotionPtrs, MotionPtrs.Length + 1); MotionPtrs[MotionPtrs.Length - 1] = motionPtr; node.YEnabled = true; node.YIndex = motionPtr.Index; } if (joint.GetZMotion().IsEnabled()) { MotionPtr motionPtr = new MotionPtr(joint.GetZMotion(), node, MotionPtrs.Length); System.Array.Resize(ref MotionPtrs, MotionPtrs.Length + 1); MotionPtrs[MotionPtrs.Length - 1] = motionPtr; node.ZEnabled = true; node.ZIndex = motionPtr.Index; } } } if (objective != null) { System.Array.Resize(ref ObjectivePtrs, ObjectivePtrs.Length + 1); ObjectivePtrs[ObjectivePtrs.Length - 1] = new ObjectivePtr(segment.GetComponent <Objective>(), node); } System.Array.Resize(ref Nodes, Nodes.Length + 1); Nodes[Nodes.Length - 1] = node; } }
private void DrawJoint(KinematicJoint joint) { Vector3 connection = joint.GetAnchorInWorldSpace(); //DrawSphere(connection, JointSize, JointColor); DrawCube(connection, joint.transform.rotation * Quaternion.Euler(joint.GetOrientation()), JointSize, JointColor); DrawLine(joint.transform.position, joint.GetAnchorInWorldSpace(), JointColor); //GUIStyle style = new GUIStyle(); //style.normal.textColor = Color.black; //Handles.Label(connection, joint.name, style); if (joint.GetXMotion().IsEnabled()) { Handles.color = Color.red; Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.GetXMotion().Axis), ArrowSize); } if (joint.GetYMotion().IsEnabled()) { Handles.color = Color.green; Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.GetYMotion().Axis), ArrowSize); } if (joint.GetZMotion().IsEnabled()) { Handles.color = Color.blue; Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.GetZMotion().Axis), ArrowSize); } }
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); * } */ }
//Recursively updates the transform information for the current frame public void UpdateTransformations() { //Update Root if (Chain.Joints.Length > 0) { Vector3 root = Chain.Joints[0].GetAnchorInWorldSpace(); RootX = root.x; RootY = root.y; RootZ = root.z; } else { RootX = Model.OPX; RootY = Model.OPY; RootZ = Model.OPZ; } //Local if (Joint == null) { Vector3 lp = Segment.localPosition; Quaternion lr = Segment.localRotation; LPX = lp.x; LPY = lp.y; LPZ = lp.z; LRX = lr.x; LRY = lr.y; LRZ = lr.z; LRW = lr.w; } else { XValue = Joint.GetXMotion().GetTargetValue(); YValue = Joint.GetYMotion().GetTargetValue(); ZValue = Joint.GetZMotion().GetTargetValue(); Joint.ComputeLocalTransformation(XValue, YValue, ZValue, out LPX, out LPY, out LPZ, out LRX, out LRY, out LRZ, out LRW); } Vector3 ws = Segment.lossyScale; WSX = ws.x; WSY = ws.y; WSZ = ws.z; //World ComputeWorldTransformation(); //Feed Forward foreach (Node child in Childs) { child.UpdateTransformations(); } }
private void ResetPosture(Transform t) { KinematicJoint joint = t.GetComponent <KinematicJoint>(); if (joint != null) { joint.GetXMotion().SetTargetValue(0f); joint.GetYMotion().SetTargetValue(0f); joint.GetZMotion().SetTargetValue(0f); } for (int i = 0; i < t.childCount; i++) { ResetPosture(t.GetChild(i)); } }
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); }