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;
}
