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++)
{
KinematicJoint joint = Joints[i].gameObject.AddComponent <KinematicJoint>();
URDFData.JointData jointData = data.GetJointData(joint.name);
if (jointData.Type == "fixed")
{
//Nothing to do
}
else
{
switch (jointData.Type)
{
case "prismatic":
joint.Type = JointType.Prismatic;
break;
case "revolute":
joint.Type = JointType.Revolute;
break;
case "continuous":
joint.Type = JointType.Continuous;
break;
}
joint.Connection = Vector3.zero;
Vector3 axis = ROSToUnity(jointData.Axis);
if (joint.Type != JointType.Prismatic)
{
axis = -axis;
}
JointMotion motion = joint.XMotion;
joint.AxisOrientation = Quaternion.FromToRotation(Vector3.right, axis).eulerAngles;
motion.State = JointState.Free;
motion.SetMaximumVelocity(jointData.Velocity);
motion.SetMaximumAcceleration(jointData.Velocity);
motion.SetLowerLimit(Mathf.Min(0f, jointData.LowerLimit));
motion.SetUpperLimit(Mathf.Max(0f, jointData.UpperLimit));
}
}
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);
}
public Animation(AssetData asset)
{
using (Stream s = asset.InputStream)
{
short version = ReadInt16(s);
short subversion = ReadInt16(s);
bool isOldVersion = false;
if (version == 0 && subversion == 1)
{
isOldVersion = true;
}
else if (version != KEYFRAME_MOTION_VERSION || subversion != KEYFRAME_MOTION_SUBVERSION)
{
throw new NotAnAnimationFormatException();
}
int basePrio = ReadInt32(s);
if (basePrio < (int)JointPriority.UseMotionPriority)
{
throw new NotAnAnimationFormatException();
}
if (basePrio > (int)JointPriority.AdditivePriority)
{
BasePriority = JointPriority.AdditivePriority - 1;
}
else
{
BasePriority = (JointPriority)basePrio;
}
Duration = ReadFloat(s);
EmoteName = ReadString(s);
LoopInPoint = ReadFloat(s);
LoopOutPoint = ReadFloat(s);
Loop = ReadInt32(s);
EaseInDuration = ReadFloat(s);
EaseOutDuration = ReadFloat(s);
uint handpose = ReadUInt32(s);
if (!Enum.IsDefined(typeof(HandmotionType), handpose))
{
throw new NotAnAnimationFormatException();
}
Handpose = (HandmotionType)handpose;
uint num_motions = ReadUInt32(s);
if (num_motions == 0 || num_motions > CHARACTER_MAX_ANIMATED_JOINTS)
{
throw new NotAnAnimationFormatException();
}
for (uint motionidx = 0; motionidx < num_motions; ++motionidx)
{
JointMotion motion = new JointMotion();
Motions.Add(ReadString(s), motion);
int jointPriority = ReadInt32(s);
if (jointPriority < (int)JointPriority.UseMotionPriority)
{
throw new NotAnAnimationFormatException();
}
motion.Priority = (JointPriority)jointPriority;
int num_rot_keys = ReadInt32(s);
if (num_rot_keys < 0)
{
throw new NotAnAnimationFormatException();
}
while (num_rot_keys-- != 0)
{
JointMotion.RotationEntry r = new JointMotion.RotationEntry();
double x;
double y;
double z;
if (isOldVersion)
{
r.Timepoint = ReadFloat(s);
x = ReadFloat(s) * Math.PI / 180.0;
y = ReadFloat(s) * Math.PI / 180.0;
z = ReadFloat(s) * Math.PI / 180.0;
Quaternion qx = Quaternion.CreateFromAxisAngle(Vector3.UnitX, x);
Quaternion qy = Quaternion.CreateFromAxisAngle(Vector3.UnitY, y);
Quaternion qz = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, z);
r.Rotation = qz * qy * qx;
}
else
{
r.Timepoint = 0.0.Lerp(Duration, ReadUInt16(s) / 65535.0);
x = (-1.0).Lerp(1.0, ReadUInt16(s) / 65535.0);
y = (-1.0).Lerp(1.0, ReadUInt16(s) / 65535.0);
z = (-1.0).Lerp(1.0, ReadUInt16(s) / 65535.0);
r.Rotation = new Quaternion(x, y, z);
}
motion.Rotations.Add(r);
}
int num_pos_keys = ReadInt32(s);
if (num_pos_keys < 0)
{
throw new NotAnAnimationFormatException();
}
while (num_pos_keys-- != 0)
{
JointMotion.PositionEntry p = new JointMotion.PositionEntry();
if (isOldVersion)
{
p.Timepoint = ReadFloat(s);
p.Position = ReadVector(s);
}
else
{
p.Timepoint = 0.0.Lerp(Duration, ReadUInt16(s) / 65535.0);
p.Position.X = (-MAX_PELVIS_OFFSET).Lerp(MAX_PELVIS_OFFSET, ReadUInt16(s) / 65535.0);
p.Position.Y = (-MAX_PELVIS_OFFSET).Lerp(MAX_PELVIS_OFFSET, ReadUInt16(s) / 65535.0);
p.Position.Z = (-MAX_PELVIS_OFFSET).Lerp(MAX_PELVIS_OFFSET, ReadUInt16(s) / 65535.0);
}
motion.Positions.Add(p);
}
}
int numconstraints = ReadInt32(s);
if (numconstraints < 0)
{
throw new NotAnAnimationFormatException();
}
while (numconstraints-- != 0)
{
Constraints.Add(new Constraint
{
ChainLength = ReadByte(s),
ConstraintType = ReadByte(s),
SourceVolume = ReadBytes(s, 16),
TargetVolume = ReadBytes(s, 16),
TargetOffset = ReadVector(s),
TargetDirection = ReadVector(s),
EaseInStart = ReadFloat(s),
EaseInStop = ReadFloat(s),
EaseOutStart = ReadFloat(s),
EaseOutStop = ReadFloat(s)
});
}
}
}
