// 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));
}
}
// Travel down preloaded assets hierarchy to find the prefab mesh.
private GeomController FindGeomPrefabInPrefabSet(Transform prefab_set, string part_name)
{
string[] split_name = part_name.Split(new char[] { ':' }, 2);
if (split_name.Length == 2)
{
Transform prefab_sub_set = SceneUtils.Find <Transform>(prefab_set, split_name[0], null);
if (prefab_sub_set != null)
{
return(FindGeomPrefabInPrefabSet(prefab_sub_set, split_name[1]));
}
else
{
Logger.Warning(
"RobotLoader::FindAssemblyPartPrefabInPrefabSet::Cannot find: {0} subset, trying default.",
split_name[0]);
part_name = split_name[1];
}
}
GeomController geom_prefab = SceneUtils.Find <GeomController>(prefab_set, part_name, null);
if (geom_prefab != null)
{
return(geom_prefab);
}
Logger.Warning("RobotLoader::FindAssemblyPartPrefabInPrefabSet::Cannot find assembly part: {0}", part_name);
return(null);
}
public static GeomController CreateCapsule(KineticHierarchyController parent, string name, Vector3 position,
Quaternion rotation, float half_length, float radius)
{
name = ResolveName("capsule", ref capsule_count_, name);
GeomController capsule_geom = SceneUtils.InstantiateWithController <GeomController>(name);
capsule_geom.InitializeCapsule(parent, name, position, rotation, half_length, radius);
return(capsule_geom);
}
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);
}
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 void UpdateGeomCategory(GeomController geom, XmlNode geom_xml)
{
string geom_category = GeomController.GetGeomCategoryFromXml(geom_xml);
if (geom_category != null)
{
// the background has category id = 0, hence the +1
geom.category_id_ = geom_categories_.IndexOf(geom_category) + 1;
geom.SetCategoryRendererProperties();
}
}
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)));
}
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 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)));
}
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 void PrepareGeomCategories(XmlNode node)
{
List <XmlNode> geom_nodes = XmlUtils.GetChildNodes(node, "geom");
foreach (XmlNode geom_node in geom_nodes)
{
string geom_category = GeomController.GetGeomCategoryFromXml(geom_node);
if (geom_category != null && !geom_categories_.Contains(geom_category))
{
geom_categories_.Add(geom_category);
}
}
List <XmlNode> body_nodes = XmlUtils.GetChildNodes(node, "body");
foreach (XmlNode body_node in body_nodes)
{
PrepareGeomCategories(body_node);
}
}
