public Robot1 CreateRobot(string modelName, Vector3 p, Quaternion qq)
{
/*
* GameObject prefab = null;
* for (int i = 0; i < PrimitivePrefabs.Length; i++)
* {
* if (PrimitivePrefabs[i].name == modelName)
* {
* prefab = PrimitivePrefabs[i];
* }
* }
* if (prefab == null)
* {
* Debug.LogError("Could not find a prefab named " + modelName);
* return null;
* }
*/
Robot1 bdy = new Robot1();
bdy.bodyIndex = pybullet.LoadUrdfAndGetBodyIdx(modelName + ".urdf", p, qq);
if (bdy.bodyIndex != -1)
{
/*
* bdy.unityProxy = GameObject.Instantiate<GameObject>(prefab).transform;
* bdy.unityProxy.position = p;
* bdy.unityProxy.rotation = qq;
*/
return(bdy);
}
else
{
Debug.LogError("Error creating bullet primitive for " + modelName);
}
return(null);
}
// Use this for initialization
void Start()
{
Debug.Log("start begin");
Debug.Log("bb");
bodies = new List <Robot1>();
pybullet = PhysicsClient.CreatePhysicsClientSharedMemory();
if (!pybullet.CanSubmitCommand())
{
pybullet = PhysicsClient.CreatePhysicsClientDirect();
}
pybullet.SetGravity(Physics.gravity);
Robot1 cube = CreateRobot("quadruped/minitaur", new Vector3(0, 15, 0), Quaternion.Euler(35, 0, 0));
//Robot1 plane = CreateRobot("plane", Vector3.zero, Quaternion.Euler(-90, 0, 0));
bodies.Add(cube);
//bodies.Add(plane);
}
public void StartSimulation()
{
TurnOnAnimation();
// CollisionCheck();
GetObstaclesInConfigurationSpace();
SavedRobotCopy = Robot1.Clone();
timer = new DispatcherTimer(DispatcherPriority.Render);
pathFrameNumber = Path.Count() - 1;
if (Path.Count > 1)
{
Status2 = "";
timer.Interval = TimeSpan.FromMilliseconds(1000 * SimulationTime / Path.Count);
timer.Tick += TimerOnTick;
timer.Start();
}
else
{
Status2 = " || No path found";
TurnOffAnimation();
}
// OnPropertyChanged(nameof(Status2));
}
void GetObstaclesInConfigurationSpace()
{
int startAlpha0;
int startAlpha1;
int endAlpha0;
int endAlpha1;
if (_showFirst)
{
startAlpha0 = wrapAngle((int)(Robot1.Alpha0 * 180 / Math.PI));
startAlpha1 = wrapAngle((int)(Robot1.Alpha1 * 180 / Math.PI));
}
else
{
startAlpha0 = wrapAngle((int)(Robot1.Alpha0bis * 180 / Math.PI));
startAlpha1 = wrapAngle((int)(Robot1.Alpha1bis * 180 / Math.PI));
}
if (_showFirstEnd)
{
endAlpha0 = wrapAngle((int)(Robot2.Alpha0 * 180 / Math.PI));
endAlpha1 = wrapAngle((int)(Robot2.Alpha1 * 180 / Math.PI));
}
else
{
endAlpha0 = wrapAngle((int)(Robot2.Alpha0bis * 180 / Math.PI));
endAlpha1 = wrapAngle((int)(Robot2.Alpha1bis * 180 / Math.PI));
}
ConfigurationSpaceArray = new int[360, 360];
bitmapHelper.SetColor(240, 248, 255);
Path.Clear();
if (double.IsNaN(Robot1.Alpha0) || double.IsNaN(Robot1.Alpha1) || double.IsNaN(Robot2.Alpha0) || double.IsNaN(Robot2.Alpha1) || CollisionCheck())
{
OnPropertyChanged(nameof(ConfigurationSpaceBitmap));
return;
}
// _configurationSpace.Clear();
Robot tempRobot = Robot1.Clone();
for (int i = 0; i < 360; i++)
{
for (int j = 0; j < 360; j++)
{
ConfigurationSpaceArray[i, j] = 0;
// ConfigurationSpaceArray[i+ j*360] = 0;
foreach (var item in ObstaclesCollection)
{
tempRobot.Alpha0 = Math.PI * i / 180;
tempRobot.Alpha1 = Math.PI * j / 180;
if (item.CollisionCheck(tempRobot.Point0, tempRobot.Point1) ||
item.CollisionCheck(tempRobot.Point1, tempRobot.Point2))
{
ConfigurationSpaceArray[i, j] = -1;
//ConfigurationSpaceBitmap.SetPixel(i, j, System.Drawing.Color.FromArgb(0, 0, 0));
//bitmapHelper.SetPixel(i,j,255,0,0);
}
}
}
}
var maxDistance = arrayFloodFill(ConfigurationSpaceArray, startAlpha0, startAlpha1, endAlpha0, endAlpha1);
if (maxDistance == 0)
{
return;
}
for (int i = 0; i < 360; i++)
{
for (int j = 0; j < 360; j++)
{
if (ConfigurationSpaceArray[i, j] != -1)
{
//bitmapHelper.SetPixel(i, j, 0, (byte)(ConfigurationSpaceArray[i, j] * 255 / 720), 0);
bitmapHelper.SetPixel(i, j, 0, (byte)(ConfigurationSpaceArray[i, j] * 255 / maxDistance), 0);
}
else
{
bitmapHelper.SetPixel(i, j, 255, 0, 0);
ConfigurationSpaceArray[i, j] = -1;
}
}
}
//double[] startAngles = Robot1.CalculateArmAnglesForPosition(StartPosition);
//double[] endAngles = Robot1.CalculateArmAnglesForPosition(EndPosition);
double[] endAngles;
if (_showFirstEnd)
{
endAngles = new double[] { Robot2.Alpha0, Robot2.Alpha1 };
}
else
{
endAngles = new double[] { Robot2.Alpha0bis, Robot2.Alpha1bis };
}
Path.Add(new int[3] {
(int)(endAngles[0] * 180 / Math.PI), (int)(endAngles[1] * 180 / Math.PI), ConfigurationSpaceArray[(int)(endAngles[0] * 180 / Math.PI), (int)(endAngles[1] * 180 / Math.PI)]
});
int value = ConfigurationSpaceArray[Path.Last()[0], Path.Last()[1]];
while (value > 1)
{
if (ConfigurationSpaceArray[wrapAngle(Path.Last()[0] + 1), wrapAngle(Path.Last()[1])] == (value - 1))
{
Path.Add(new int[3] {
wrapAngle(Path.Last()[0] + 1), wrapAngle(Path.Last()[1]), wrapAngle(ConfigurationSpaceArray[wrapAngle(Path.Last()[0] + 1), wrapAngle(Path.Last()[1])])
});
}
else if (ConfigurationSpaceArray[wrapAngle(Path.Last()[0] - 1), wrapAngle(Path.Last()[1])] == (value - 1))
{
{
Path.Add(new int[3] {
wrapAngle(Path.Last()[0] - 1), wrapAngle(Path.Last()[1]), wrapAngle(ConfigurationSpaceArray[wrapAngle(Path.Last()[0] - 1), wrapAngle(Path.Last()[1])])
});
}
}
else if (ConfigurationSpaceArray[wrapAngle(Path.Last()[0]), wrapAngle(Path.Last()[1] + 1)] == (value - 1))
{
{
Path.Add(new int[3] {
wrapAngle(Path.Last()[0]), wrapAngle(Path.Last()[1] + 1), wrapAngle(ConfigurationSpaceArray[wrapAngle(Path.Last()[0]), wrapAngle(Path.Last()[1] + 1)])
});
}
}
else if (ConfigurationSpaceArray[wrapAngle(Path.Last()[0]), wrapAngle(Path.Last()[1] - 1)] == (value - 1))
{
{
Path.Add(new int[3] {
wrapAngle(Path.Last()[0]), wrapAngle(Path.Last()[1] - 1), wrapAngle(ConfigurationSpaceArray[wrapAngle(Path.Last()[0]), wrapAngle(Path.Last()[1] - 1)])
});
}
}
else
{
MessageBox.Show("Path not found");
}
value = ConfigurationSpaceArray[wrapAngle(Path.Last()[0]), wrapAngle(Path.Last()[1])];
}
foreach (var item in Path)
{
bitmapHelper.SetPixel(item[0], item[1], 255, 255, 255);
}
OnPropertyChanged(nameof(ConfigurationSpaceBitmap));
}
IEnumerator StatusChaneAtertime(float time, Robot1 robot1)
{
yield return(new WaitForSeconds(time));
robot1.status = Status.isSearching;
}
