/// <summary>
/// Adjusts roboy pose for all parts with the values from the simulation.
/// </summary>
/// <param name="msg">JSON msg containing the roboy pose.</param>
void adjustPose(string name, RoboyPoseMsg msg)
{
m_RoboyPoseMessage = msg;
Dictionary <string, float> xPositionsDictionary = m_RoboyPoseMessage.XDic;
Dictionary <string, float> yPositionsDictionary = m_RoboyPoseMessage.YDic;
Dictionary <string, float> zPositionsDictionary = m_RoboyPoseMessage.ZDic;
Dictionary <string, float> qxRotationsDictionary = m_RoboyPoseMessage.QxDic;
Dictionary <string, float> qyRotationsDictionary = m_RoboyPoseMessage.QyDic;
Dictionary <string, float> qzRotationsDictionary = m_RoboyPoseMessage.QzDic;
Dictionary <string, float> qwRotationsDictionary = m_RoboyPoseMessage.QwDic;
Debug.Log(name);
foreach (KeyValuePair <string, RoboyPart> roboyPart in m_RoboyPartsList[name])
{
Debug.Log(roboyPart.Key);
Debug.Log(roboyPart.Value);
string index = roboyPart.Key;
Vector3 originPosition = new Vector3(xPositionsDictionary[index], yPositionsDictionary[index], zPositionsDictionary[index]);
Quaternion originRotation = new Quaternion(qxRotationsDictionary[index], qyRotationsDictionary[index], qzRotationsDictionary[index], qwRotationsDictionary[index]);
roboyPart.Value.transform.localPosition = GazeboUtility.GazeboPositionToUnity(originPosition);
roboyPart.Value.transform.localRotation = GazeboUtility.GazeboRotationToUnity(originRotation);
}
}
/// <summary>
/// List of all points for this tendon
/// </summary>
/// <returns></returns>
public Vector3[] GetWirepoints()
{
Vector3[] result = new Vector3[_wirepoints.Count];
for (int i = 0; i < _wirepoints.Count; i++)
{
result[i] = GazeboUtility.GazeboPositionToUnity(_wirepoints[i]);
}
return(_wirepoints.ToArray());
}
/// <summary>
/// Creates prefabs for every model which were downloaded.
/// </summary>
public void CreatePrefab()
{
foreach (string modelName in m_ModelNames)
{
string pathToSDFFile = UpdaterUtility.ProjectFolder + @"/temp" + modelName + "SDFs.txt";
if (!File.Exists(pathToSDFFile))
{
Debug.LogWarning("Scan file not found! Check whether it exists or if python script is working!");
return;
}
// get file content of format title:url
string[] sdfContent = File.ReadAllLines(pathToSDFFile);
File.Delete(pathToSDFFile);
List <string[]> linkList = new List <string[]>();
foreach (string line in sdfContent)
{
string[] SDFline = line.Split(';');
linkList.Add(SDFline);
}
GameObject modelParent = null;
string absoluteModelPath = UpdaterUtility.ProjectFolder + @"/SimulationModels/" + modelName + "/OriginModels";
for (int i = 0; i < linkList.Count; i++)
//foreach (string[] line in linkList)
{
if (linkList[i][0] == "model_name")
{
//Create GameObject where everything will be attached
modelParent = new GameObject(linkList[i][1]);
linkList.Remove(linkList[i]);
//continue;
}
}
//List for all downloaded visuals
List <string> visMeshList = new List <string>();
if (absoluteModelPath != "")
{
visMeshList = UpdaterUtility.getFilePathsFBX(absoluteModelPath + @"/visual");
}
//List for all downloaded colliders
List <string> colMeshList = new List <string>();
if (absoluteModelPath != "")
{
colMeshList = UpdaterUtility.getFilePathsFBX(absoluteModelPath + @"/collision");
}
foreach (string name in visMeshList)
{
string[] currentLine = null;
foreach (string[] line in linkList)
{
for (int j = 0; j < line.Length; j++)
{
string name1 = name.Replace(".fbx", "");
Debug.Log(name1 + " : " + name);
if (line[j].Contains(name1))
{
currentLine = line;
Debug.Log(currentLine.Length);
}
}
}
string[] pose = null;
string[] VIS_scale = null;
//string[] COL_scale = null;
for (int i = 0; i < currentLine.Length; i++)
{
if (currentLine[i].Contains("link_pose"))
{
pose = currentLine[i + 1].Split(' ');
}
if (currentLine[i].Contains("VIS_mesh_scale"))
{
VIS_scale = currentLine[i + 1].Split(' ');
}
//if (currentLine[i].Contains("COL_mesh_scale"))
//{
// COL_scale = currentLine[i + 1].Split(' ');
//}
}
GameObject meshPrefab = null;
string relativeModelPath = "Assets/SimulationModels/" + modelName + "/OriginModels/";
// import Mesh
meshPrefab = (GameObject)AssetDatabase.LoadAssetAtPath(relativeModelPath + @"visual/" + name, typeof(UnityEngine.Object));
//StartCoroutine(importModelCoroutine(path, (result) => { meshPrefab = result; }));
if (meshPrefab == null)
{
Debug.Log("Could not import model!");
continue;
}
GameObject meshCopy = Instantiate(meshPrefab);
Debug.Log(pose.Length);
meshCopy.tag = "RoboyPart";
meshCopy.transform.position = GazeboUtility.GazeboPositionToUnity(new Vector3(float.Parse(pose[0], CultureInfo.InvariantCulture.NumberFormat),
float.Parse(pose[1], CultureInfo.InvariantCulture.NumberFormat),
float.Parse(pose[2], CultureInfo.InvariantCulture.NumberFormat)));
meshCopy.transform.eulerAngles = GazeboUtility.GazeboPositionToUnity(new Vector3(Mathf.Rad2Deg * float.Parse(pose[3], CultureInfo.InvariantCulture.NumberFormat),
Mathf.Rad2Deg * float.Parse(pose[4], CultureInfo.InvariantCulture.NumberFormat),
Mathf.Rad2Deg * float.Parse(pose[5], CultureInfo.InvariantCulture.NumberFormat)));
if (VIS_scale != null)
{
meshCopy.transform.localScale = GazeboUtility.GazeboPositionToUnity(new Vector3(100 * float.Parse(VIS_scale[0], CultureInfo.InvariantCulture.NumberFormat),
100 * float.Parse(VIS_scale[1], CultureInfo.InvariantCulture.NumberFormat),
100 * float.Parse(VIS_scale[2], CultureInfo.InvariantCulture.NumberFormat)));
}
UpdaterUtility.attachCollider(meshCopy, relativeModelPath, name);
var regex1 = new Regex(Regex.Escape("(Clone)"));
meshCopy.name = regex1.Replace(meshCopy.name, "", 1);
var regex2 = new Regex(Regex.Escape("VIS_"));
meshCopy.name = regex2.Replace(meshCopy.name, "", 1);
SelectableObject selectableObjectComponent = meshCopy.AddComponent <SelectableObject>();
selectableObjectComponent.TargetedMaterial = Resources.Load("RoboyMaterials/TargetedMaterial") as Material;
selectableObjectComponent.SelectedMaterial = Resources.Load("RoboyMaterials/SelectedMaterial") as Material;
meshCopy.AddComponent <RoboyPart>();
// Attach Model with mesh to parent GO
meshCopy.transform.parent = modelParent.transform;
}
modelParent.tag = "Roboy";
//Create Prefab of existing GO
UnityEngine.Object prefab = PrefabUtility.CreateEmptyPrefab("Assets/SimulationModels/" + modelName + "/" + modelName + ".prefab");
PrefabUtility.ReplacePrefab(modelParent, prefab, ReplacePrefabOptions.ConnectToPrefab);
//Destroy GO after prefab is created
DestroyImmediate(modelParent);
}
m_ModelNames.Clear();
}
/// <summary>
/// Downloads models given in the .world files and saves their pose in a struct.
/// </summary>
public void Magic()
{
List <KeyValuePair <string, bool> > tempURLList = WorldChoiceDictionary.Where(entry => entry.Value == true).ToList();
foreach (var urlEntry in tempURLList)
{
if (File.Exists(UpdaterUtility.ProjectFolder + @"/SimulationWorlds/" + urlEntry.Key + @"/" + urlEntry.Key + ".world"))
{
Debug.Log(".world file found!");
// read .world file
string[] argumentsSDFreader = { "python \"" + UpdaterUtility.PathToWorldReader + "\" \"" + UpdaterUtility.ProjectFolder + @"/SimulationWorlds/" + urlEntry.Key + @"/" + urlEntry.Key + ".world\"" };
CommandlineUtility.RunCommandLine(argumentsSDFreader);
}
else
{
Debug.LogWarning(".world file not found!");
}
string pathToWorldFile = UpdaterUtility.ProjectFolder + @"/temp" + urlEntry.Key + @"World.txt";
if (!File.Exists(pathToWorldFile))
{
Debug.LogWarning("Scan file not found! Check whether it exists or if python script is working!");
return;
}
//save content of temp file
string[] WorldContent = File.ReadAllLines(pathToWorldFile);
//delete temp file
File.Delete(pathToWorldFile);
List <string[]> modelList = new List <string[]>();
foreach (string line in WorldContent)
{
//each line contains of a model link with pose, scale, model_link
string[] SDFline = line.Split(';');
modelList.Add(SDFline);
}
string worldName = null;
for (int i = 0; i < modelList.Count; i++)
{
if (modelList[i][0] == "world_name")
{
//get world name
worldName = modelList[i][1];
modelList.Remove(modelList[i]);
}
}
foreach (string[] line in modelList)
{
//each model will be saved in a struct, so we can apply pose and scale later on
ModelTransformation testModel = new ModelTransformation();
testModel.worldname = worldName;
//check which attributes are set by the .world file and set them in the struct
for (int j = 0; j < line.Length; j++)
{
testModel.link = new LinkTransformation();
string[] pose = null;
if (line[j] == "model_pose")
{ //saving model_pose
pose = line[j + 1].Split(' ');
testModel.position = GazeboUtility.GazeboPositionToUnity(new Vector3(float.Parse(pose[0], CultureInfo.InvariantCulture.NumberFormat),
float.Parse(pose[1], CultureInfo.InvariantCulture.NumberFormat),
float.Parse(pose[2], CultureInfo.InvariantCulture.NumberFormat)));
testModel.rotation = GazeboUtility.GazeboPositionToUnity(new Vector3(Mathf.Rad2Deg * float.Parse(pose[3], CultureInfo.InvariantCulture.NumberFormat),
Mathf.Rad2Deg * float.Parse(pose[4], CultureInfo.InvariantCulture.NumberFormat),
Mathf.Rad2Deg * float.Parse(pose[5], CultureInfo.InvariantCulture.NumberFormat)));
}
if (line[j] == "model_link")
{ //saving model_link name
testModel.link.name = line[j + 1];
}
if (line[j] == "VIS_mesh_uri")
{ // saving VIS_mesh_link and model.name
testModel.link.VIS_meshName = line[j + 1];
string name1 = Regex.Replace(line[j + 1], ".*?//", "");
string name2 = Regex.Replace(name1, "/.*", "");
testModel.name = name2;
Debug.Log(testModel.link.VIS_meshName);
}
//saving COL_mesh_link
if (line[j] == "COL_mesh_uri")
{
testModel.link.COL_meshName = line[j + 1];
}
string[] VIS_Scale = null;
if (line[j] == "VIS_mesh_scale")
{
//saving model scale
VIS_Scale = line[j + 1].Split(' ');
}
if (VIS_Scale != null)
{ //converting model scale
testModel.link.VIS_scale = GazeboUtility.GazeboPositionToUnity(new Vector3(100 * float.Parse(VIS_Scale[0], CultureInfo.InvariantCulture.NumberFormat),
100 * float.Parse(VIS_Scale[1], CultureInfo.InvariantCulture.NumberFormat),
100 * float.Parse(VIS_Scale[2], CultureInfo.InvariantCulture.NumberFormat)));
}
string[] linkpose = null;
if (line[j] == "link_pose")
{
pose = line[j + 1].Split(' ');
}
if (linkpose != null)
{
testModel.position = GazeboUtility.GazeboPositionToUnity(new Vector3(float.Parse(linkpose[0], CultureInfo.InvariantCulture.NumberFormat),
float.Parse(linkpose[1], CultureInfo.InvariantCulture.NumberFormat),
float.Parse(linkpose[2], CultureInfo.InvariantCulture.NumberFormat)));
testModel.rotation = GazeboUtility.GazeboPositionToUnity(new Vector3(Mathf.Rad2Deg * float.Parse(linkpose[3], CultureInfo.InvariantCulture.NumberFormat),
Mathf.Rad2Deg * float.Parse(linkpose[4], CultureInfo.InvariantCulture.NumberFormat),
Mathf.Rad2Deg * float.Parse(linkpose[5], CultureInfo.InvariantCulture.NumberFormat)));
}
}
modellist.Add(testModel);
//}
}
WorldsCurrentState = UpdaterUtility.State.Downloaded;
}
List <string> names = new List <string>();
foreach (ModelTransformation model in modellist)
{
if (!names.Contains(model.name))
{
names.Add(model.name);
//Debug.Log(model.name);
}
}
foreach (string name in names)
{
//replace "tree" with "raw" in URL
var regex = new Regex(Regex.Escape("tree"));
m_PathToModelsFolder = regex.Replace(m_PathToModelsFolder, "raw", 1);
//start modeldownloader.py for visual
string[] updateArgumentsWorld = { "start \"\" \"" + UpdaterUtility.PathToBlender + "\" -P", UpdaterUtility.PathToDownloadScript, m_PathToModelsFolder + @"/" + name + @"/meshes/", UpdaterUtility.ProjectFolder + @"/SimulationWorlds/Models/" + name + "/meshes", "" };
CommandlineUtility.RunCommandLine(updateArgumentsWorld);
}
}