// A capsule is created from a cylinder and two spheres.
protected void InitializeCapsule(KineticHierarchyController parent, string capsule_name, Vector3 position,
Quaternion rotation, float half_length, float radius)
{
Initialize(parent, capsule_name, position, rotation);
GameObject top_sphere = GameObject.CreatePrimitive(PrimitiveType.Sphere);
Destroy(top_sphere.GetComponent <SphereCollider>());
top_sphere.name = string.Format("{0}(top_sphere)", name);
top_sphere.transform.parent = transform;
top_sphere.transform.localPosition = Vector3.forward * half_length;
top_sphere.transform.localScale = Vector3.one * radius;
top_sphere.transform.localRotation = Quaternion.identity;
GameObject bottom_sphere = GameObject.CreatePrimitive(PrimitiveType.Sphere);
Destroy(bottom_sphere.GetComponent <SphereCollider>());
bottom_sphere.name = string.Format("{0}(bottom_sphere)", name);
bottom_sphere.transform.parent = transform;
bottom_sphere.transform.localPosition = Vector3.back * half_length;
bottom_sphere.transform.localScale = Vector3.one * radius;
bottom_sphere.transform.localRotation = Quaternion.identity;
GameObject cylinder = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
Destroy(cylinder.GetComponent <MeshCollider>());
Destroy(cylinder.GetComponent <CapsuleCollider>());
cylinder.name = string.Format("{0}(cylinder)", name);
cylinder.transform.parent = transform;
cylinder.transform.localPosition = Vector3.zero;
cylinder.transform.localScale = new Vector3(radius, half_length, radius);
cylinder.transform.localRotation = Quaternion.identity * Quaternion.Euler(90.0f, 0.0f, 0.0f);
}
// Find the preloaded material and override with geom colors
// (if defined).
private void ResolveMaterial(XmlNode node_xml, XmlUtils.Defaults defaults, KineticHierarchyController mesh)
{
MeshRenderer[] renderers = mesh.GetComponentsInChildren <MeshRenderer>();
string material_name = XmlUtils.GetStringWithDefaults(node_xml, defaults, "material", "");
if (material_name != null)
{
if (materials_.ContainsKey(material_name))
{
foreach (MeshRenderer mesh_renderer in renderers)
{
mesh_renderer.material = materials_[material_name];
}
}
}
if (XmlUtils.HasAttributeWithDefaults(node_xml, defaults, "rgba"))
{
Color color = XmlUtils.GetColorWithDefaults(node_xml, defaults, "rgba", Color.white);
foreach (MeshRenderer mesh_renderer in renderers)
{
if (mesh_renderer.material.HasProperty("_Color"))
{
mesh_renderer.material.color = color;
}
}
}
}
// Create the hierarchy that handles local transformations
// and joints for a given body.
private KineticHierarchyController BuildRobotBodyAttachment(KineticHierarchyController parent, XmlNode part_xml,
XmlUtils.Defaults defaults)
{
string part_name = XmlUtils.GetString(part_xml, "name", string.Format("part_{0}", part_counter_));
part_counter_++;
// Build the body and set local position/rotation/scale relative to parent.
BodyController body = SceneUtils.InstantiateWithController <BodyController>(part_name);
body.Initialize(parent, part_name, XmlUtils.GetVector3(part_xml, "pos", Vector3.zero),
XmlUtils.GetRotation(part_xml, Quaternion.identity));
List <XmlNode> joint_nodes = XmlUtils.GetChildNodes(part_xml, "joint");
// Add all the joints in a hierarchy, one after another
// (XML order is important).
KineticHierarchyController last_game_object = body;
for (int i = 0; i < joint_nodes.Count; ++i)
{
last_game_object = BuildJoint(last_game_object, part_name, i, joint_nodes[i], defaults);
}
return(last_game_object);
}
// Handle creation of a MuJoCo XML body / worldbody entity.
private bool AddRobotBody(KineticHierarchyController parent, XmlNode part_xml, XmlUtils.Defaults defaults)
{
defaults = defaults.Resolve(XmlUtils.GetString(part_xml, "childclass", null));
KineticHierarchyController robot_part_attachment = BuildRobotBodyAttachment(parent, part_xml, defaults);
return(BuildRobotBodyChildren(robot_part_attachment, part_xml, defaults));
}
// A capsule can be defined with a from-to pair, which complicates
// the code a little.
private GeomController AddRobotCapsuleGeom(KineticHierarchyController parent, XmlNode geom_xml,
XmlUtils.Defaults defaults)
{
string capsule_name = XmlUtils.GetString(geom_xml, "name", null);
if (XmlUtils.HasAttribute(geom_xml, "fromto"))
{
string from_to = XmlUtils.GetStringWithDefaults(geom_xml, defaults, "fromto", "0 0 0 0 0 0");
string[] from_to_split = from_to.Split(new char[] { ' ' }, System.StringSplitOptions.RemoveEmptyEntries);
if (from_to_split.Length != 6)
{
Logger.Error("RobotLoader::AddRobotCapsuleGeom::Malformed fromto: {0}", from_to);
return(null);
}
Vector3 from_position = new Vector3(float.Parse(from_to_split[0]), float.Parse(from_to_split[1]),
float.Parse(from_to_split[2]));
Vector3 to_position = new Vector3(float.Parse(from_to_split[3]), float.Parse(from_to_split[4]),
float.Parse(from_to_split[5]));
Vector3 center = (from_position + to_position) / 2.0f;
float half_length = (to_position - from_position).magnitude / 2.0f;
Quaternion rotation = Quaternion.LookRotation(to_position - from_position);
return(GeomController.CreateCapsule(
parent, capsule_name, center, rotation, half_length, XmlUtils.GetFloat(geom_xml, "size", 1.0f)));
}
else
{
Vector2 mujoco_size = XmlUtils.GetVector2WithDefaults(geom_xml, defaults, "size", Vector2.one);
return(GeomController.CreateCapsule(
parent, capsule_name,
XmlUtils.GetVector3WithDefaults(geom_xml, defaults, "pos", Vector3.zero),
XmlUtils.GetRotationWithDefaults(geom_xml, defaults, Quaternion.identity),
mujoco_size.y,
mujoco_size.x));
}
}
private bool AddCamera(KineticHierarchyController parent, XmlNode camera_xml, XmlUtils.Defaults defaults)
{
string camera_name = XmlUtils.GetString(camera_xml, "name", null);
if (camera_name == null)
{
camera_name = string.Format("camera_{0}", camera_counter_++);
}
Camera camera_prototype = SceneUtils.Find <Camera>(assembly_parts_, camera_name,
SceneUtils.Find <Camera>(assembly_parts_, "__camera_template", null));
if (camera_prototype == null)
{
Logger.Error("RobotLoader::AddCamera::Cannot find camera prefab for: {0}", camera_name);
return(false);
}
Camera robot_camera = Instantiate(camera_prototype);
robot_camera.name = camera_name;
robot_camera.transform.parent = parent.transform;
robot_camera.transform.localRotation = XmlUtils.GetRotation(camera_xml, Quaternion.identity);
robot_camera.transform.localPosition = XmlUtils.GetVector3(camera_xml, "pos", Vector3.zero);
robot_camera.transform.LookAt(robot_camera.transform.position + robot_camera.transform.TransformDirection(Vector3.back),
robot_camera.transform.TransformDirection(Vector3.up));
robot_camera.fieldOfView = XmlUtils.GetFloat(camera_xml, "fovy", robot_camera.fieldOfView);
return(true);
}
public static SiteController CreateCapsule(KineticHierarchyController parent, string name, Vector3 position,
Quaternion rotation, float half_length, float radius)
{
name = ResolveName(name);
SiteController capsule_geom = SceneUtils.InstantiateWithController <SiteController>(name);
capsule_geom.InitializeCapsule(parent, name, position, rotation, half_length, radius);
return(capsule_geom);
}
public static SiteController CreatePlane(KineticHierarchyController parent, string name, Vector3 position,
Quaternion rotation, Vector3 plane_size)
{
name = ResolveName(name);
SiteController plane_site = SceneUtils.InstantiateWithController <SiteController>(name);
plane_site.InitializePlane(parent, name, position, rotation, plane_size);
return(plane_site);
}
public static GeomController CreatePlane(KineticHierarchyController parent, string name, Vector3 position,
Quaternion rotation, Vector3 plane_size)
{
name = ResolveName("plane", ref plane_count_, name);
GeomController plane_geom = SceneUtils.InstantiateWithController <GeomController>(name);
plane_geom.InitializePlane(parent, name, position, rotation, plane_size);
return(plane_geom);
}
// Helper factory methods used to create geometric primitives. Pass a
// hierarchy parent, a name (or use a generated one, and basic parametric
// geometry values.
public static GeomController CreateBox(KineticHierarchyController parent, string name, Vector3 position,
Quaternion rotation, Vector3 box_size)
{
name = ResolveName("box", ref box_count_, name);
GeomController box_geom = SceneUtils.InstantiateWithController <GeomController>(name);
box_geom.InitializeBox(parent, name, position, rotation, box_size);
return(box_geom);
}
public static SiteController CreateCylinder(KineticHierarchyController parent, string name, Vector3 position,
Quaternion rotation, Vector3 cylinder_size)
{
name = ResolveName(name);
SiteController cylinder_site = SceneUtils.InstantiateWithController <SiteController>(name);
cylinder_site.InitializeCylinder(parent, name, position, rotation, cylinder_size);
return(cylinder_site);
}
private GeomController AddRobotSphereGeom(KineticHierarchyController parent, XmlNode geom_xml,
XmlUtils.Defaults defaults)
{
return(GeomController.CreateSphere(
parent,
XmlUtils.GetString(geom_xml, "name", null),
XmlUtils.GetVector3WithDefaults(geom_xml, defaults, "pos", Vector3.zero),
XmlUtils.GetRotationWithDefaults(geom_xml, defaults, Quaternion.identity),
2 * XmlUtils.GetVector3WithDefaults(geom_xml, defaults, "size", Vector3.one)));
}
private SiteController AddRobotBoxSite(KineticHierarchyController parent, XmlNode site_xml,
XmlUtils.Defaults defaults)
{
return(SiteController.CreateBox(
parent,
XmlUtils.GetString(site_xml, "name", null),
XmlUtils.GetVector3WithDefaults(site_xml, defaults, "pos", Vector3.zero),
XmlUtils.GetRotationWithDefaults(site_xml, defaults, Quaternion.identity),
2 * XmlUtils.GetVector3WithDefaults(site_xml, defaults, "size", Vector3.one)));
}
public void Initialize(KineticHierarchyController parent, string name, Vector3 position, Quaternion rotation)
{
this.name = name;
if (parent != null)
{
transform.parent = parent.transform;
}
transform.localPosition = position;
transform.localRotation = rotation;
}
public static GeomController CreateCylinder(KineticHierarchyController parent, string name, Vector3 position,
Quaternion rotation, Vector3 cylinder_size)
{
if (name == null)
{
name = string.Format("cylinder_{0}", cylinder_count_++);
}
GeomController plane_geom = SceneUtils.InstantiateWithController <GeomController>(name);
plane_geom.InitializeCylinder(parent, name, position, rotation, cylinder_size);
return(plane_geom);
}
private GeomController AddRobotCylinderGeom(KineticHierarchyController parent, XmlNode geom_xml,
XmlUtils.Defaults defaults)
{
Vector2 mujoco_size = XmlUtils.GetVector2WithDefaults(geom_xml, defaults, "size", Vector2.one);
return(GeomController.CreateCylinder(
parent,
XmlUtils.GetString(geom_xml, "name", null),
XmlUtils.GetVector3WithDefaults(geom_xml, defaults, "pos", Vector3.zero),
XmlUtils.GetRotationWithDefaults(geom_xml, defaults, Quaternion.identity),
2 * new Vector3(mujoco_size.x, mujoco_size.y / 2.0f, mujoco_size.x)));
}
private SiteController AddRobotSphereSite(KineticHierarchyController parent, XmlNode site_xml,
XmlUtils.Defaults defaults)
{
Vector3 size = XmlUtils.GetVector3WithDefaults(site_xml, defaults, "size", Vector3.zero);
return(SiteController.CreateSphere(
parent,
XmlUtils.GetString(site_xml, "name", null),
XmlUtils.GetVector3WithDefaults(site_xml, defaults, "pos", Vector3.zero),
XmlUtils.GetRotationWithDefaults(site_xml, defaults, Quaternion.identity),
Vector3.one * 2.0f * size.x));
}
public void Initialize(KineticHierarchyController parent, string joint_name, Vector3 position, Quaternion rotation,
Vector3 axis, Vector2 range, string joint_type)
{
if (joint_name == null)
{
joint_name = string.Format("joint_{0}", joint_count_++);
}
Initialize(parent, joint_name, position, rotation);
this.axis_ = axis;
this.range_ = range;
this.joint_type_ = ParseType(joint_type);
}
private bool AddRobotGeom(KineticHierarchyController parent, XmlNode geom_xml, XmlUtils.Defaults defaults)
{
defaults = defaults.Resolve(XmlUtils.GetString(geom_xml, "class", null)).GetSubclass("geom");
GeomController geom = null;
string mesh_name = XmlUtils.GetStringWithDefaults(geom_xml, defaults, "mesh", null);
string type = XmlUtils.GetStringWithDefaults(geom_xml, defaults, "type", null);
if (mesh_name != null)
{
geom = AddRobotMeshGeom(parent, geom_xml, defaults, mesh_name);
}
else if ("box".Equals(type))
{
geom = AddRobotBoxGeom(parent, geom_xml, defaults);
}
else if ("plane".Equals(type))
{
geom = AddRobotPlaneGeom(parent, geom_xml, defaults);
}
else if (type == null || "sphere".Equals(type))
{
geom = AddRobotSphereGeom(parent, geom_xml, defaults);
}
else if ("cylinder".Equals(type))
{
geom = AddRobotCylinderGeom(parent, geom_xml, defaults);
}
else if ("capsule".Equals(type))
{
geom = AddRobotCapsuleGeom(parent, geom_xml, defaults);
}
else
{
Logger.Error("RobotLoader::AddRobotGeom::Unsupported geom type: {0} in {1}.", type, geom_xml.OuterXml);
return(false);
}
if (geom == null)
{
Logger.Error("RobotLoader::AddRobotGeom::Cannot instantiate geom.");
return(false);
}
// Set the geom category for semantic segmentation
UpdateGeomCategory(geom, geom_xml);
// Find the material in the preloaded assets.
ResolveMaterial(geom_xml, defaults, geom);
return(true);
}
private GeomController AddRobotPlaneGeom(KineticHierarchyController parent, XmlNode geom_xml,
XmlUtils.Defaults defaults)
{
Vector3 mujoco_size = XmlUtils.GetVector3WithDefaults(geom_xml, defaults, "size", Vector3.one);
return(GeomController.CreatePlane(
parent,
XmlUtils.GetString(geom_xml, "name", null),
XmlUtils.GetVector3WithDefaults(geom_xml, defaults, "pos", Vector3.zero),
XmlUtils.GetRotationWithDefaults(geom_xml, defaults, Quaternion.identity)
* Quaternion.Euler(90.0f, 0.0f, 0.0f),
2 * new Vector3(mujoco_size.x / 10.0f, 0.0f, mujoco_size.y / 10.0f)));
}
protected void InitializeBox(KineticHierarchyController parent, string box_name, Vector3 position,
Quaternion rotation, Vector3 box_size)
{
Initialize(parent, box_name, position, rotation);
GameObject the_box = GameObject.CreatePrimitive(PrimitiveType.Cube);
Destroy(the_box.GetComponent <BoxCollider>());
the_box.name = string.Format("{0}(primitive)", name);
the_box.transform.parent = transform;
the_box.transform.localPosition = Vector3.zero;
the_box.transform.localRotation = Quaternion.identity;
the_box.transform.localScale = box_size;
}
protected void InitializeCylinder(KineticHierarchyController parent, string cylinder_name, Vector3 position,
Quaternion rotation, Vector3 cylinder_size)
{
Initialize(parent, cylinder_name, position, rotation);
GameObject the_cylinder = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
Destroy(the_cylinder.GetComponent <MeshCollider>());
the_cylinder.name = string.Format("{0}(primitive)", name);
the_cylinder.transform.parent = transform;
the_cylinder.transform.localPosition = Vector3.zero;
the_cylinder.transform.localRotation = Quaternion.identity * Quaternion.Euler(90.0f, 0.0f, 0.0f);
the_cylinder.transform.localScale = cylinder_size;
}
protected void InitializePlane(KineticHierarchyController parent, string plane_name, Vector3 position,
Quaternion rotation, Vector3 plane_size)
{
Initialize(parent, plane_name, position, rotation);
GameObject the_plane = GameObject.CreatePrimitive(PrimitiveType.Plane);
Destroy(the_plane.GetComponent <MeshCollider>());
the_plane.name = string.Format("{0}(primitive)", name);
the_plane.transform.parent = transform;
the_plane.transform.localPosition = Vector3.zero;
the_plane.transform.localRotation = Quaternion.identity;
the_plane.transform.localScale = plane_size;
}
// Recursively add geoms, cameras, sites and other bodies.
private bool BuildRobotBodyChildren(KineticHierarchyController parent, XmlNode part_xml,
XmlUtils.Defaults defaults)
{
List <XmlNode> geom_nodes = XmlUtils.GetChildNodes(part_xml, "geom");
foreach (XmlNode geom_node in geom_nodes)
{
if (!AddRobotGeom(parent, geom_node, defaults))
{
Logger.Error("RobotLoader::BuildRobotBodyChildren::Cannot add robot geom.");
return(false);
}
}
List <XmlNode> camera_nodes = XmlUtils.GetChildNodes(part_xml, "camera");
foreach (XmlNode camera_node in camera_nodes)
{
if (!AddCamera(parent, camera_node, defaults))
{
Logger.Error("RobotLoader::BuildRobotBodyChildren::Cannot add robot camera.");
return(false);
}
}
List <XmlNode> site_nodes = XmlUtils.GetChildNodes(part_xml, "site");
foreach (XmlNode site_node in site_nodes)
{
if (!AddRobotSite(parent, site_node, defaults))
{
Logger.Error("RobotLoader::BuildRobotBodyChildren::Cannot add robot site.");
return(false);
}
}
List <XmlNode> child_nodes = XmlUtils.GetChildNodes(part_xml, "body");
foreach (XmlNode child_node in child_nodes)
{
if (!AddRobotBody(parent, child_node, defaults))
{
Logger.Error("RobotLoader::BuildRobotBodyChildren::Cannot add robot body.");
return(false);
}
}
return(true);
}
private GeomController AddRobotMeshGeom(KineticHierarchyController parent, XmlNode geom_xml,
XmlUtils.Defaults defaults, string mesh_name)
{
GeomController mesh_prefab = FindGeomPrefab(mesh_name);
if (mesh_prefab == null)
{
Logger.Error("RobotLoader::AddRobotMeshGeom::Cannot find mesh prefab for: {0}", mesh_name);
return(null);
}
GeomController mesh_geom = Instantiate <GeomController>(mesh_prefab, parent.transform);
mesh_geom.Initialize(parent, mesh_name,
XmlUtils.GetVector3WithDefaults(geom_xml, defaults, "pos", Vector3.zero),
XmlUtils.GetRotationWithDefaults(geom_xml, defaults, Quaternion.identity));
return(mesh_geom);
}
private KineticHierarchyController BuildJoint(KineticHierarchyController parent, string part_name, int id,
XmlNode joint_xml, XmlUtils.Defaults defaults)
{
string joint_name = XmlUtils.GetString(joint_xml, "name", string.Format("{0}_joint_{1}", part_name, id));
XmlUtils.Defaults joint_defaults = defaults.GetSubclass("joint");
JointController joint = SceneUtils.InstantiateWithController <JointController>(joint_name);
joint.Initialize(parent, joint_name,
XmlUtils.GetVector3WithDefaults(joint_xml, joint_defaults, "pos", Vector3.zero),
XmlUtils.GetRotationWithDefaults(joint_xml, joint_defaults, Quaternion.identity),
XmlUtils.GetVector3WithDefaults(joint_xml, joint_defaults, "axis", Vector3.up),
XmlUtils.GetVector2WithDefaults(joint_xml, joint_defaults, "range",
new Vector2(float.MinValue, float.MaxValue)),
XmlUtils.GetStringWithDefaults(joint_xml, joint_defaults, "type", null));
return(joint);
}
private bool AddRobotSite(KineticHierarchyController parent, XmlNode site_xml, XmlUtils.Defaults defaults)
{
defaults = defaults.Resolve(XmlUtils.GetString(site_xml, "class", null)).GetSubclass("site");
SiteController site = null;
string type = XmlUtils.GetStringWithDefaults(site_xml, defaults, "type", "sphere");
if ("box".Equals(type))
{
site = AddRobotBoxSite(parent, site_xml, defaults);
}
else if ("plane".Equals(type))
{
site = AddRobotPlaneSite(parent, site_xml, defaults);
}
else if (type == null || "sphere".Equals(type))
{
site = AddRobotSphereSite(parent, site_xml, defaults);
}
else if ("cylinder".Equals(type))
{
site = AddRobotCylinderSite(parent, site_xml, defaults);
}
else if ("capsule".Equals(type))
{
site = AddRobotCapsuleSite(parent, site_xml, defaults);
}
else
{
Logger.Error("RobotLoader::AddRobotSite::Unsupported site type: {0} in {1}.", type, site_xml.OuterXml);
return(false);
}
if (site == null)
{
Logger.Error("RobotLoader::AddRobotSite::Cannot instantiate site.");
return(false);
}
ResolveMaterial(site_xml, defaults, site);
return(true);
}
