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); } }
private void VisualizeKinematicJoint(KinematicJoint joint) { Vector3 connection = joint.ComputeConnectionInWorldSpace(); Handles.color = Color.magenta; Handles.SphereCap(0, connection, Quaternion.identity, 1 / 100f); GUIStyle style = new GUIStyle(); style.normal.textColor = Color.black; Handles.Label(connection, joint.name, style); if (joint.XMotion.State == JointState.Free) { Handles.color = Color.red; Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.ComputeXAxis()), 0.1f); } if (joint.YMotion.State == JointState.Free) { Handles.color = Color.green; Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.ComputeYAxis()), 0.1f); } if (joint.ZMotion.State == JointState.Free) { Handles.color = Color.blue; Handles.ArrowCap(0, connection, joint.transform.rotation * Quaternion.LookRotation(joint.ComputeZAxis()), 0.1f); } }
void Awake() { Target = (KinematicJoint)target; Target.XMotion.Joint = Target; Target.YMotion.Joint = Target; Target.ZMotion.Joint = Target; }
public Chain(Transform start, Transform end) { List <Transform> segments = new List <Transform>(); List <KinematicJoint> joints = new List <KinematicJoint>(); Length = 0f; DoF = 0; Transform t = end; while (true) { segments.Add(t); KinematicJoint joint = t.GetComponent <KinematicJoint>(); if (joint != null) { if (joint.GetDOF() != 0) { joints.Add(joint); } } if (t == start) { break; } else { t = t.parent; } } segments.Reverse(); joints.Reverse(); Segments = segments.ToArray(); Joints = joints.ToArray(); if (Joints.Length == 0) { Length = 0f; } else { Vector3 reference = Joints[0].ComputeConnectionInWorldSpace(); for (int i = 1; i < Joints.Length; i++) { Length += Vector3.Distance(reference, Joints[i].ComputeConnectionInWorldSpace()); reference = Joints[i].ComputeConnectionInWorldSpace(); } Length += Vector3.Distance(reference, end.position); } for (int i = 0; i < Joints.Length; i++) { DoF += Joints[i].GetDOF(); } }
//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 AddNode(Transform segment) { if (FindNode(segment) == null) { KinematicJoint joint = segment.GetComponent <KinematicJoint>(); MotionPtr[] motions = new MotionPtr[3]; Node node = new Node(this, FindNode(segment.parent), segment, joint, motions); if (joint != null) { if (joint.GetDOF() == 0) { joint = null; } else { if (joint.XMotion.State != JointState.Fixed) { MotionPtr motionPtr = new MotionPtr(joint.XMotion, node, Motions.Length); System.Array.Resize(ref Motions, Motions.Length + 1); Motions[Motions.Length - 1] = motionPtr; motions[0] = motionPtr; } if (joint.YMotion.State != JointState.Fixed) { MotionPtr motionPtr = new MotionPtr(joint.YMotion, node, Motions.Length); System.Array.Resize(ref Motions, Motions.Length + 1); Motions[Motions.Length - 1] = motionPtr; motions[1] = motionPtr; } if (joint.ZMotion.State != JointState.Fixed) { MotionPtr motionPtr = new MotionPtr(joint.ZMotion, node, Motions.Length); System.Array.Resize(ref Motions, Motions.Length + 1); Motions[Motions.Length - 1] = motionPtr; motions[2] = motionPtr; } } } IKTip tip = segment.GetComponent <IKTip>(); if (tip != null) { System.Array.Resize(ref Tips, Tips.Length + 1); Tips[Tips.Length - 1] = new TipPtr(segment.GetComponent <IKTip>(), node, Tips.Length); } System.Array.Resize(ref Nodes, Nodes.Length + 1); Nodes[Nodes.Length - 1] = node; } }
private double ZValue = 0.0; // //Setup for the node public Node(Model model, Node parent, Transform segment, KinematicJoint joint, Objective objective) { Model = model; Parent = parent; if (Parent != null) { Parent.AddChild(this); } Segment = segment; Chain = new Chain(model.Root, segment); Joint = joint; Objective = objective; }
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)); } }
public Chain(Transform start, Transform end) { List <Transform> segments = new List <Transform>(); List <KinematicJoint> joints = new List <KinematicJoint>(); Length = 0f; Transform t = end; while (true) { segments.Add(t); KinematicJoint joint = t.GetComponent <KinematicJoint>(); if (joint != null) { if (joint.GetDoF() != 0) { joints.Add(joint); } } if (t == start) { break; } else { t = t.parent; } } segments.Reverse(); joints.Reverse(); Segments = segments.ToArray(); Joints = joints.ToArray(); if (Joints.Length == 0) { Length = 0f; } else { for (int i = 1; i < Joints.Length; i++) { Length += Vector3.Distance(Joints[i - 1].GetAnchorInWorldSpace(), Joints[i].GetAnchorInWorldSpace()); } Length += Vector3.Distance(Joints[Joints.Length - 1].GetAnchorInWorldSpace(), end.position); } }
public Node(Model model, Node parent, Transform segment, KinematicJoint joint, MotionPtr[] motions) { Model = model; Parent = parent; if (Parent != null) { Parent.AddChild(this); } Segment = segment; Chain = new Chain(model.Body.transform, segment); Joint = joint; Motions = motions; HeuristicInputs = 0f; HeuristicError = 0f; }
private void DrawKinematicChain(Chain chain) { //Visualize Joints and Kinematic Chain if (chain.Joints.Length > 0) { KinematicJoint reference = chain.Joints[0]; VisualizeKinematicJoint(reference); for (int k = 1; k < chain.Joints.Length; k++) { Handles.color = Color.cyan; Handles.DrawLine(reference.ComputeConnectionInWorldSpace(), chain.Joints[k].ComputeConnectionInWorldSpace()); reference = chain.Joints[k]; VisualizeKinematicJoint(reference); } Handles.color = Color.cyan; Handles.DrawLine(chain.Joints[chain.Joints.Length - 1].ComputeConnectionInWorldSpace(), chain.Segments[chain.Segments.Length - 1].transform.position); Handles.color = new Color(0.25f, 0.25f, 0.25f, 1f); Handles.SphereCap(0, chain.Segments[chain.Segments.Length - 1].position, chain.Segments[chain.Segments.Length - 1].rotation, 0.01f); } }
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); }
void Awake() { Target = (KinematicJoint)target; }