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