/// <summary>
/// Los ejemplos del 1 al 4 son no alcanzables
/// Del 5 al 7 son alcanzables. Los poligonos se construyen comenzando por el vertice que menos coordenada
/// "Y" tenga (menor "X" en caso de empate) y siguiendo el orden inverso de las manesillas del reloj
/// </summary>
private void cargar_ejemplos()
{
this.sistema_obstaculos = new TMObstacleSystem(this.obstaculos);
C_Free = this.sistema_obstaculos.freeConfigurationSpace(universe, out this.grafo_busqueda);
//GraphPane pane = zgc.GraphPane;
zgc.AxisChange();
relena_Arbol(tv, this.grafo_busqueda.Root);
var end_efector = new TMPoint(5, 40);
grafo_trayectorias = new TREXT_TrajectoryGraph(this.grafo_busqueda, end_efector, this.objetivo, config.TR_Separacion_Obstaculos);
optimal_path = new List <List <TRNode> >();
optimizar_camino();
this.index_path = 0;
Publica_lo_basico();
}
public bool CS_Filter
(
out List <TMPoint> chain_result,
out int it,
out double distance,
out double w_best_particle,
int max_iter,
double epsilon,
TMObstacleSystem S,
List <TMPoint> arm,
TMPoint obj,
int samples,
double m_gaussian
)
{
var obstaculos = new List <Obstaculo>();
foreach (TMPoligon poligon in S.Obstacles.Poligons)
{
var xVector = new double[poligon.Points.Count];
var yVector = new double[poligon.Points.Count];
for (int i = 0; i < poligon.Points.Count; i++)
{
xVector[i] = poligon.Points[i].X;
yVector[i] = poligon.Points[i].Y;
}
obstaculos.Add(new Obstaculo(xVector, yVector));
}
var arm_ = arm.Select(tmPoint => new PointD(tmPoint.X, tmPoint.Y)).ToList();
var particleFilter = new CS_ParticleFilter.ParticleFilter();
FilterResult filterResult = particleFilter.PrtFltr_MkwskSum(max_iter, epsilon,
new SistemaObstaculos(obstaculos),
arm_,
new PointD(obj.X, obj.Y),
samples,
m_gaussian);
chain_result = filterResult.ChainResult_points.Select(point => new TMPoint(point.X, point.Y)).ToList();
it = filterResult.Iterations;
distance = filterResult.Distance;
w_best_particle = filterResult.ChainResult.W;
return(distance <= epsilon);
}
/**
* % Esta funcion es la que engloba los algoritmos de Filtro de Particulas
* % y Suma de Minkowski. Como parámetros se le pasa:
* % max_iter: Maximo de iteraciones que realizara el filtraje
* % epsilon: tolerancia de cercanía con el objetivo
* % S: el sistema de obstaculos
* % x_axis: posiciones en el eje x de la cadena en su posición "inicial"
* % y_axis: posiciones en el eje y de la cadena en su posición "inicial"
* % obj: objetivo intermedio
* % samples: cantidad de particulas que se generaran
* % media_gausiana: media para la generación de particulas (0.03)
* % S:
* % S = [O1 O2 ... Ok]
* % Ok.ptos: lista de puntos, ordenados tomando como principio el que menos
* % coordenada "y" tenga (menos "x") en caso de empate y siguiendo el orden
* % inverso a a las manesillas del reloj
**/
/// <summary>
/// Este metodo se encarga de encontrar la configuracion que debe adoptar el brazo (arm) para llegar al objetivo (obj)
/// </summary>
/// <param name="chain_result">(Salida) Posiciones de la cadena resultante</param>
/// <param name="it">(Salida) Numero de iteraciones realizadas</param>
/// <param name="distance">(Salida) Distancia a la que quedo del objetivo intermedio (obj)</param>
/// <param name="w_best_particle"></param>
/// <param name="max_iter">Maximo de iteraciones que realizara el filtro</param>
/// <param name="epsilon">Distancia a la que se quiere llegar del objetivo intermedio (obj)</param>
/// <param name="S">Sistemas de obstaculos</param>
/// <param name="arm">Posiciones de los nodos de la cadena inicialmente</param>
/// <param name="obj">Posicion del objetivo intermedio</param>
/// <param name="samples">Numero de particulas que se generaran en el proceso de filtrado</param>
/// <param name="m_gaussian">Media gaussiana</param>
/// <returns>Indica si el brazo pudo llegar al objetivo.</returns>
public bool MATLAB_Filter
(
out List <TMPoint> chain_result,
out int it,
out double distance,
out double w_best_particle,
int max_iter,
double epsilon,
TMObstacleSystem S,
List <TMPoint> arm,
TMPoint obj,
int samples,
double m_gaussian
)
{
//chain_result = null;
//it = 0;
//distance = 0;
//PF = new MATLAB_ParticleFilter.MatlabParticleFilter_v2();
//function [chainResult, chainResult_points, it, distance] = PrtFltr_MkwskSum(max_iter, epsilon, S, x_axis, y_axis, obj, samples, media_gausiana)
MWArray MAT_max_iter = new MWNumericArray(max_iter);
MWArray MAT_epsilon = new MWNumericArray(epsilon);
#region OBSTACULOS
if (this.MAT_obst_ranks == null || this.MAT_x_obst == null || this.MAT_y_obst == null)
{
int obs_count = S.Obstacles.Poligons.Count;
/*
* obst_ranks = [4];
* x_obst = [0.4 0.7 0.7 0.4];
* y_obst = [0.6 0.6 0.9 0.9];
*/
var rr = new int[obs_count];
int c_ptos = 0;
for (int i = 0; i < obs_count; i++)
{
c_ptos += S.Obstacles.Poligons[i].Count;
rr[i] = c_ptos;
}
this.MAT_obst_ranks = new MWNumericArray(1, obs_count, rr);
var x_rr = new double[c_ptos];
var y_rr = new double[c_ptos];
int index = 0;
foreach (TMPoligon pol in S.Obstacles.Poligons)
{
foreach (TMPoint p in pol.Points)
{
x_rr[index] = p.X;
y_rr[index] = p.Y;
index++;
}
}
//corrimiento_minkowski(rr, out x_rr, out y_rr);
this.MAT_x_obst = new MWNumericArray(1, c_ptos, x_rr);
this.MAT_y_obst = new MWNumericArray(1, c_ptos, y_rr);
}
#endregion
var a_x = new double[arm.Count];
var a_y = new double[arm.Count];
for (int i = 0; i < arm.Count; i++)
{
a_x[i] = arm[i].X;
a_y[i] = arm[i].Y;
}
MWArray MAT_X_axis = new MWNumericArray(1, arm.Count, a_x);
MWArray MAT_Y_axis = new MWNumericArray(1, arm.Count, a_y);
MWArray MAT_Obj = new MWNumericArray(1, 2, new[] { obj.X, obj.Y });
MWArray MAT_Samples = new MWNumericArray(samples);
MWArray MAT_MedGauss = new MWNumericArray(m_gaussian);
//MWArray t = new MWStructArray();
MWArray[] outs = PF.PrtFltr_MkwskSum(4, MAT_max_iter, MAT_epsilon, MAT_obst_ranks, MAT_x_obst, MAT_y_obst, MAT_X_axis, MAT_Y_axis, MAT_Obj, MAT_Samples, MAT_MedGauss);
w_best_particle = ((MWNumericArray)((MWStructArray)outs[0])["w"]).ToScalarDouble();
distance = ((MWNumericArray)outs[3]).ToScalarDouble();
it = ((MWNumericArray)outs[2]).ToScalarInteger();
int ptos_brazos = outs[1].Dimensions[0];
chain_result = new List <TMPoint>();
Array mat_chain_result = outs[1].ToArray();
for (int i = 0; i < ptos_brazos; i++)
{
var p = new TMPoint((double)mat_chain_result.GetValue(i, 0), (double)mat_chain_result.GetValue(i, 1));
chain_result.Add(p);
}
return(distance <= epsilon);
}
