3342 lines
106 KiB
C++
3342 lines
106 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2007 *
|
|
* Human Rezaijafari <H.Rezai@web.de> *
|
|
* This file is part of the FreeCAD CAM development system. *
|
|
* *
|
|
* This library is free software; you can redistribute it and/or *
|
|
* modify it under the terms of the GNU Library General Public *
|
|
* License as published by the Free Software Foundation; either *
|
|
* version 2 of the License, or (at your option) any later version. *
|
|
* *
|
|
* This library is distributed in the hope that it will be useful, *
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
|
* GNU Library General Public License for more details. *
|
|
* *
|
|
* You should have received a copy of the GNU Library General Public *
|
|
* License along with this library; see the file COPYING.LIB. If not, *
|
|
* write to the Free Software Foundation, Inc., 59 Temple Place, *
|
|
* Suite 330, Boston, MA 02111-1307, USA *
|
|
* *
|
|
***************************************************************************/
|
|
|
|
#include "PreCompiled.h"
|
|
#include "path_simulate.h"
|
|
|
|
#include <GeomAPI_ProjectPointOnCurve.hxx>
|
|
|
|
#include <TColgp_Array1OfPnt.hxx>
|
|
#include <Base/Exception.h>
|
|
#include <Base/Builder3D.h>
|
|
#include <GCPnts_AbscissaPoint.hxx>
|
|
#include <Geom_BSplineCurve.hxx>
|
|
|
|
#define curvTOL 30.0 // gibt maximalen Krümmungsradius an ab welchem eine Unterteilung der Kurve erfolgt
|
|
#define TolDist 1.0 // entspricht der Samplingschrittweite der Kurvenpunkte für den Roboter-Output
|
|
|
|
/* Konstruktor mit zwei Bahnfolgen (master tool & supporting die) als Input */
|
|
path_simulate::path_simulate(const std::vector<Handle_Geom_BSplineCurve> &BSplineTop,
|
|
const std::vector<Handle_Geom_BSplineCurve> &BSplineBottom,
|
|
struct CuttingToolsSettings& set)
|
|
:m_BSplineTop(BSplineTop),m_BSplineBottom(BSplineBottom),m_set(set),
|
|
m_t0(0.0),m_step(1e-3),m_clip(90000)
|
|
{
|
|
|
|
m_pretension = m_set.spring_pretension;
|
|
m_blech = m_set.sheet_thickness;
|
|
m_amax = m_set.max_Acc;
|
|
m_vmax = m_set.max_Vel;
|
|
|
|
m_single = false;
|
|
|
|
if(m_pretension > 0) beam = true; // flag für "write_output_***" generierung
|
|
else beam = false;
|
|
|
|
//Initialize the Iterators
|
|
m_it1 = m_BSplineTop.begin();
|
|
m_it2 = m_BSplineBottom.begin();
|
|
|
|
//Initialise some vars
|
|
gp_Pnt p(0,0,0);
|
|
gp_Pnt q(0,0,0);
|
|
|
|
/*------------------------------Generate the first movement of the Master------------*/
|
|
|
|
/* Fill p with the starting point of the first Master curve*/
|
|
(*m_it1)->D0((*m_it1)->FirstParameter(),p);
|
|
|
|
/* Set q to the initial-Z-level in the Simulation: 2mm + Master-Radius over the sheet which is located at Z=0 */
|
|
q.SetZ(2.0 + set.master_radius);
|
|
|
|
//First we clear the vectors
|
|
m_StartPnts1.clear();
|
|
m_StartPnts2.clear();
|
|
|
|
//Now we insert the start points for the Master movement
|
|
m_StartPnts1.push_back(q);
|
|
m_StartPnts1.push_back(p);
|
|
|
|
//Inserts the start parameter
|
|
m_StartParam.push_back((*m_it1)->FirstParameter());
|
|
|
|
/*Master finished here*/
|
|
|
|
/*-----------------------------Generate the first movement for the Slave--------------*/
|
|
|
|
/* Fill p with the starting point of the first Slave curve*/
|
|
(*m_it2)->D0((*m_it2)->FirstParameter(),p);
|
|
|
|
/* Set q to the initial-Z-level in the Simulation: -5mm - Slave-Radius-Spring-Pretensionbelow the sheet upper level which is located at Z=0 */
|
|
q.SetZ(-5.0 - set.slave_radius - m_pretension);
|
|
|
|
/*Now we insert the start points for the Slave movement*/
|
|
m_StartPnts2.push_back(q);
|
|
m_StartPnts2.push_back(p);
|
|
|
|
//Inserts the start parameter
|
|
m_StartParam.push_back((*m_it2)->FirstParameter());
|
|
|
|
/*Slave finished here*/
|
|
}
|
|
|
|
path_simulate::~path_simulate()
|
|
{
|
|
}
|
|
|
|
/*
|
|
double path_simulate::GetLength(GeomAdaptor_Curve& curve, const Standard_Real startParameter,const Standard_Real endParameter)
|
|
{
|
|
Standard_Real firstParameter = curve.FirstParameter();
|
|
Standard_Real lastParameter = curve.LastParameter();
|
|
|
|
|
|
Standard_Real sParam = Min(startParameter,endParameter);
|
|
Standard_Real eParam = Max(startParameter,endParameter);
|
|
|
|
//Standard_Real length = 0.0;
|
|
|
|
if ( eParam > lastParameter )
|
|
{
|
|
//get the first part of the length
|
|
Standard_Real l1 = GetLength(curve,firstParameter,eParam-lastParameter);
|
|
Standard_Real l2 = GetLength(curve,sParam,lastParameter);
|
|
return l1 + l2;
|
|
}
|
|
if ( sParam < firstParameter )
|
|
{
|
|
Standard_Real l1 = GetLength(curve,lastParameter-fabs(sParam),lastParameter);
|
|
Standard_Real l2 = GetLength(curve,firstParameter,eParam);
|
|
return l1 + l2;
|
|
}
|
|
|
|
|
|
return GCPnts_AbscissaPoint::Length(curve,sParam,eParam); // genauigkeitssteuerung über parameter TOL nach eParam
|
|
}
|
|
*/
|
|
|
|
/*
|
|
double path_simulate::FindParamAt(GeomAdaptor_Curve& curve, double dist, double startParam)
|
|
{
|
|
double param;
|
|
|
|
//compute the parameter of the next point
|
|
GCPnts_AbscissaPoint absc(curve, dist, startParam);
|
|
if ( absc.IsDone() )
|
|
{
|
|
//the parameter is computed
|
|
param = absc.Parameter();
|
|
}
|
|
|
|
return param;
|
|
}
|
|
*/
|
|
|
|
/* Hier wird die absolute Geschwindigkeitsfunktion definiert und liefert die Geschwindigkeit zur Zeiteingabe <t>.
|
|
Die Funktion gliedert sich in drei Abschnitte mit den Parametergrenzen <m_t0>, <m_t1>, <m_t2>, <m_T> welche z.B.
|
|
mittels path_simulate::ParameterCalculation() ermittelt werden kann.
|
|
Die Start- und Endgeschwindigkeit müssen vorher in <m_v[0]> und <m_v[2]> bestimmt werden.
|
|
Die maximale Geschwindigkeit welche zwischen <m_t1> und <m_t2> erreicht werden soll, entspricht hier <m_v[1]> */
|
|
double path_simulate::GetVelocity(double t)
|
|
{
|
|
double vel;
|
|
double c[2];
|
|
|
|
c[0] = m_a/2.0;
|
|
|
|
if (t>=m_t0 && t<=m_t1)
|
|
{
|
|
if (t==m_t0 || (m_v[1] - m_v[0]) == 0.0)
|
|
{
|
|
vel = m_v[0];
|
|
}
|
|
else
|
|
{
|
|
c[1] = PI*m_a / (m_v[1] - m_v[0]);
|
|
vel = c[0]*(sin(c[1]*(t-m_t0) - PI)/c[1] + (t-m_t0)) + m_v[0];
|
|
}
|
|
}
|
|
else if (t>m_t1 && t<=m_t2)
|
|
{
|
|
vel = m_v[1];
|
|
}
|
|
else if (t>m_t2 && t<=m_T)
|
|
{
|
|
if (t==m_T)
|
|
{
|
|
vel = m_v[1] - c[0]*(t-m_t2);
|
|
}
|
|
else
|
|
{
|
|
c[1] = PI*m_a / (m_v[1] - m_v[2]);
|
|
vel = m_v[1] - c[0]*(sin(c[1]*(t-m_t2) - PI)/c[1] + (t-m_t2));
|
|
}
|
|
}
|
|
else
|
|
throw Base::RuntimeError("time input not inside [t0, T]");
|
|
|
|
return vel;
|
|
}
|
|
|
|
/* Diese Funktion liefert den zurückgelegten Weg zur Zeiteingabe <t> und entspricht
|
|
dem Integral der Funktion GetVelocity(t)*/
|
|
double path_simulate::GetDistance(double t)
|
|
{
|
|
double d;
|
|
double c[2];
|
|
|
|
c[0] = m_a/2.0;
|
|
|
|
if (t>=m_t0 && t<m_t1)
|
|
{
|
|
c[1] = PI*m_a / (m_v[1] - m_v[0]);
|
|
|
|
if(t==m_t0){d = 0.0;}
|
|
else{ d = -(c[0]/pow(c[1],2.0))*cos(c[1]*(t-m_t0) - PI) +
|
|
c[0]*pow((t-m_t0),2.0)/2 - c[0]/(c[1]*c[1]) +
|
|
m_v[0]*(t-m_t0);}
|
|
}
|
|
else if (t>=m_t1 && t<=m_t2)
|
|
{
|
|
d = c[0]*pow((m_t1-m_t0),2.0)/2 +
|
|
m_v[0]*(m_t1-m_t0) +
|
|
m_v[1]*(t-m_t1);
|
|
}
|
|
else if (t>m_t2 && t<=m_T)
|
|
{
|
|
c[1] = PI*m_a / (m_v[1] - m_v[2]);
|
|
|
|
if (t==m_T)
|
|
{
|
|
d = m_v[0]*(m_t1-m_t0) + c[0]*pow((m_t1-m_t0),2.0)/2 +
|
|
m_v[1]*(m_t2-m_t1) - c[0]*pow((t-m_t2),2.0)/2 +
|
|
m_v[1]*(t-m_t2);
|
|
}
|
|
else
|
|
{
|
|
d = m_v[0]*(m_t1-m_t0) + c[0]*pow((m_t1-m_t0),2.0)/2 +
|
|
m_v[1]*(m_t2-m_t1) + (c[0]/pow(c[1],2.0))*cos(c[1]*(t-m_t2) - PI) -
|
|
c[0]*pow((t-m_t2),2.0)/2 +
|
|
c[0]/pow(c[1],2.0) +
|
|
m_v[1]*(t-m_t2);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
throw Base::RuntimeError("time input not inside [t0,T]");
|
|
}
|
|
|
|
return d;
|
|
}
|
|
|
|
/*
|
|
double path_simulate::GetWireLength(TopoDS_Wire &aWire)
|
|
{
|
|
GProp_GProps lProps;
|
|
BRepGProp::LinearProperties(aWire,lProps);
|
|
double length = lProps.Mass();
|
|
return length;
|
|
}
|
|
*/
|
|
|
|
/*Parameterberechnung der Geschwindigkeitsfunktion für eine gerade Strecke der Länge <S1>*/
|
|
bool path_simulate::ParameterCalculation_Line(double S1)
|
|
{
|
|
if (S1 == 0.0) // hier gibts nichts zu tun
|
|
{
|
|
m_T = m_t0;
|
|
return true;
|
|
}
|
|
|
|
m_a = m_amax;
|
|
|
|
m_v[0] = 0.0; // Startgeschwindigkeit wird auf Null gesetzt
|
|
m_v[1] = sqrt(m_a*S1/2.0); // Geschwindigkeit die notwendig ist damit der Weg <S1> zur Zeit <m_T> erreicht wird
|
|
m_v[2] = 0.0; // Endgeschwindigkeit wird auf Null gesetzt
|
|
|
|
while(m_v[1] > m_vmax) // maximale Geschwindigkeit darf nicht überschritten werden
|
|
{
|
|
m_a /= 2; // Versuchs erneut mit halber Beschleunigung
|
|
m_v[1] = sqrt(m_a*S1/2.0); // Geschwindigkeit die notwendig ist damit der Weg <S1> zur Zeit <m_T> erreicht wird
|
|
}
|
|
|
|
// Jetzt lassen sich die Zeitgrenzen berechnen
|
|
m_t1 = 2*m_v[1]/m_a + m_t0;
|
|
m_t2 = m_t1;
|
|
m_T = 2*m_t1 - m_t0;
|
|
|
|
return true;
|
|
}
|
|
|
|
/*Parameterberechnung der Geschwindigkeitsfunktion (definiert in path_simulate::GetVelocity())
|
|
für einen Kurvenabschnitt der Länge <S1>. Aufruf muss stets vor der Funktion path_simulate::GetVelocity() erfolgen*/
|
|
bool path_simulate::ParameterCalculation_Curve(double S1)
|
|
{
|
|
// Berechnung der Zeitgrenzen
|
|
m_t1 = m_t0 + 2*(abs(m_v[1]-m_v[0]))/m_a;
|
|
m_t2 = m_t1;
|
|
m_T = m_t1 + 2*(abs(m_v[1]-m_v[2]))/m_a;
|
|
|
|
double tmp, v_tmp;
|
|
tmp = GetDistance(m_T); // liefert den Weg zurück, der unter den gegebenen Parametereinstellungen,
|
|
// zum hoch- und runterbeschleunigen, midestens notwendig ist
|
|
|
|
if (tmp <= S1) // d.h. der Weg reicht aus
|
|
{
|
|
m_t2 = m_t1 + (S1 - tmp)/m_v[1]; // zwischen <m_t1> und <m_t2> wird die Kurve mit der konstanten
|
|
// Geschwindigkeit <m_v[1]> durchlaufen
|
|
}
|
|
else // Weg reicht nicht aus -> Parameterkorrektur
|
|
{
|
|
// Berechne Geschwindigkeit die mindestenns notwendig ist damit der Weg <S1> zur Zeit <m_T> erreicht wird
|
|
m_v[1] = sqrt((m_a*S1 + m_v[0]*m_v[0] + m_v[2]*m_v[2])/2.0);
|
|
m_t1 = m_t0 + 2*(abs(m_v[1]-m_v[0]))/m_a;
|
|
m_t2 = m_t1;
|
|
|
|
// hier wird evtl. eine Korrektur notwendig
|
|
if(m_v[1] > m_vmax)
|
|
{
|
|
m_v[1] = m_vmax;
|
|
|
|
// ab hier wieder analog zu oben
|
|
m_t1 = m_t0 + 2*(abs(m_v[1]-m_v[0]))/m_a;
|
|
m_t2 = m_t1;
|
|
m_T = m_t1 + 2*(abs(m_v[1]-m_v[2]))/m_a;
|
|
|
|
tmp = GetDistance(m_T);
|
|
|
|
if (tmp <= S1)
|
|
{
|
|
m_t2 = m_t1 + (S1 - tmp)/m_v[1];
|
|
}
|
|
else
|
|
{
|
|
v_tmp = (m_vmax - std::min(m_v[1],m_v[2]))/2.0;
|
|
|
|
while(tmp > S1) // hier wird die Geschwindigkeit <m_v[1]> solange in Richtung <m_v[2]> vekleinert bis
|
|
// der Weg schließlich ausreicht
|
|
{
|
|
m_v[1] = std::min(m_v[1],m_v[2]) + v_tmp;
|
|
|
|
m_t1 = m_t0 + 2*(abs(m_v[1]-m_v[0]))/m_a;
|
|
m_t2 = m_t1;
|
|
m_T = m_t1 + 2*(abs(m_v[1]-m_v[2]))/m_a;
|
|
|
|
tmp = GetDistance(m_T);
|
|
|
|
v_tmp /= 2.0;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
m_T = m_t2 + 2*(abs(m_v[1]-m_v[2]))/m_a; // Endzeit lässt sich jetzt berechnen
|
|
|
|
return true;
|
|
}
|
|
|
|
/* setzt die Outputvektoren und den Beschleunigungsparameter <m_a> zurück. Die Startzeit <m_t0> wird aktualisiert*/
|
|
bool path_simulate::UpdateParam()
|
|
{
|
|
m_Output.clear();
|
|
m_Output2.clear();
|
|
m_Output_time.clear();
|
|
m_Output_time2.clear();
|
|
|
|
m_t0 = m_T; // Endzeit des letzten Durchlaufs wird zur neuen Startzeit
|
|
m_a = m_amax;
|
|
|
|
return true;
|
|
}
|
|
|
|
/* Hilfsfunktion für die Zustellung. Rückgabewert legt fest ob zuerst in z- oder in xy-Richtung zugestellt wird*/
|
|
bool path_simulate::CheckConnect()
|
|
{
|
|
gp_Pnt tmp;
|
|
|
|
// ab dem 2. lauf
|
|
if (m_it1 != m_BSplineTop.begin() || m_it2 != m_BSplineBottom.begin())
|
|
{
|
|
m_StartPnts1.clear();
|
|
m_StartPnts2.clear();
|
|
|
|
// Berechne neue Verbindungspunkte für die Zustellung - MASTER -
|
|
m_it1--;
|
|
|
|
(*m_it1)->D0((*m_it1)->LastParameter(),tmp); // Speichert Endpunkt der vorigen Master-Kurve in <tmp>
|
|
m_StartPnts1.push_back(tmp); // Pushe Endpunkt der vorigen Master-Kurve
|
|
|
|
m_it1++;
|
|
|
|
(*m_it1)->D0((*m_it1)->FirstParameter(),tmp); // Speichert Startpunkt der vorigen Master-Kurve in <tmp>
|
|
m_StartPnts1.push_back(tmp); // Pushe Startpunkt der aktuellen Master-Kurve
|
|
|
|
if (m_single == false) // Falls beidseitig gefahren wird, mache dasselbe, wie für den Slave (s.o.)
|
|
{
|
|
// Berechne neue Verbindungspunkte für die Zustellung - SLAVE -
|
|
m_it2--;
|
|
|
|
(*m_it2)->D0((*m_it2)->LastParameter(),tmp); // Speichert Startpunkt der vorigen Slave-Kurve in <tmp>
|
|
m_StartPnts2.push_back(tmp); // Pushe Endpunkt der vorigen Slave-Kurve
|
|
|
|
m_it2++;
|
|
|
|
(*m_it2)->D0((*m_it2)->FirstParameter(),tmp); // Speichert Startpunkt der aktuellen Slave-Kurve in <tmp>
|
|
m_StartPnts2.push_back(tmp); // Pushe Startpunkt der aktuellen Slave-Kurve
|
|
}
|
|
}
|
|
else
|
|
{
|
|
return true; // Erste zustellung immer gleich (in negativer z-Richtung)
|
|
}
|
|
|
|
if (m_StartPnts1[0].Z() - m_StartPnts1[1].Z() >= 0.0) return true; // Zustellung in negativer z-Richtung
|
|
else return false; // Zustellung in positiver z-Richtung
|
|
}
|
|
|
|
/* Hilfsfunktion für die Zustellung. Rückgabewert legt fest ob zuerst in z- oder in xy-Richtung zugestellt wird*/
|
|
bool path_simulate::CheckConnect(bool tool)
|
|
{
|
|
gp_Pnt tmp;
|
|
|
|
// ab dem 2. lauf
|
|
if (m_it1 != m_BSplineTop.begin() || m_it2 != m_BSplineBottom.begin())
|
|
{
|
|
if (m_Feat == true) // Für den Feature-Basierten Fall werden die Zustellungen von Master und Slave seperat behandelt
|
|
{
|
|
if (!tool)
|
|
{
|
|
m_StartPnts1.clear();
|
|
|
|
m_it1--;
|
|
|
|
(*m_it1)->D0((*m_it1)->LastParameter(),tmp);
|
|
m_StartPnts1.push_back(tmp); // Startpunkt
|
|
|
|
m_it1++;
|
|
|
|
(*m_it1)->D0((*m_it1)->FirstParameter(),tmp);
|
|
m_StartPnts1.push_back(tmp); // Zielpunkt
|
|
|
|
if(m_StartPnts1[0].Z() - m_StartPnts1[1].Z() >= 0.0) return true;
|
|
else return false;
|
|
}
|
|
else
|
|
{
|
|
m_StartPnts2.clear();
|
|
|
|
m_it2--;
|
|
(*m_it2)->D0((*m_it2)->LastParameter(),tmp);
|
|
m_it2++;
|
|
m_StartPnts2.push_back(tmp); // Startpunkt
|
|
|
|
(*m_it2)->D0((*m_it2)->FirstParameter(),tmp);
|
|
m_StartPnts2.push_back(tmp); // Zielpunkt
|
|
|
|
if(m_StartPnts2[0].Z() - m_StartPnts2[1].Z() >= 0.0) return true;
|
|
else return false;
|
|
}
|
|
}
|
|
|
|
// Ab hier: Berechnung der Zustellungsvektoren für die synchrone Zustellung von Master und Slave
|
|
m_StartPnts1.clear();
|
|
m_StartPnts2.clear();
|
|
|
|
// Berechne neue Verbindungspunkte für die Zustellung - MASTER
|
|
m_it1--;
|
|
|
|
(*m_it1)->D0((*m_it1)->LastParameter(),tmp);
|
|
m_StartPnts1.push_back(tmp); // Startpunkt
|
|
|
|
m_it1++;
|
|
|
|
(*m_it1)->D0((*m_it1)->FirstParameter(),tmp);
|
|
m_StartPnts1.push_back(tmp); // Zielpunkt
|
|
|
|
if (m_single == false)
|
|
{
|
|
// Berechne neue Verbindungspunkte für die Zustellung - SLAVE
|
|
m_it2--;
|
|
|
|
(*m_it2)->D0((*m_it2)->LastParameter(),tmp);
|
|
m_StartPnts2.push_back(tmp); // Startpunkt
|
|
|
|
m_it2++;
|
|
|
|
(*m_it2)->D0((*m_it2)->FirstParameter(),tmp);
|
|
m_StartPnts2.push_back(tmp); // Zielpunkt
|
|
}
|
|
}
|
|
else
|
|
{
|
|
return true; // erste Zustellung immer gleich
|
|
}
|
|
|
|
if (m_StartPnts1[0].Z() - m_StartPnts1[1].Z() >= 0.0) return true;
|
|
else return false;
|
|
}
|
|
|
|
/* Füllt die Outputvektoren für die erste Zustellung. Der Eingabeparameter <brob> legt den Ausgabetyp fest*/
|
|
bool path_simulate::ConnectPaths_xy(bool brob)
|
|
{
|
|
int N;
|
|
double t = m_t0;
|
|
|
|
std::vector<Base::Vector3d> tmp2;
|
|
std::vector<double> d;
|
|
Base::Vector3d tmp;
|
|
|
|
gp_Pnt tmpPnt, pnt1, pnt2, p;
|
|
gp_Vec vec_t(m_StartPnts1[0], m_StartPnts1[1]);
|
|
|
|
if( 1e-3 > vec_t.Magnitude()) // keine Zustellung erforderlich
|
|
return true;
|
|
|
|
if (m_single == false)
|
|
{
|
|
gp_Vec vec_1(m_StartPnts1[0], m_StartPnts1[1]);
|
|
gp_Vec vec_2(m_StartPnts2[0], m_StartPnts2[1]);
|
|
|
|
gp_Vec2d vec_11, // Speichert Master-Zustellung in XY-Richtung
|
|
vec_21; // Speichert Master-Zustellung in Z-Richtung
|
|
|
|
vec_11.SetX(vec_1.X());
|
|
vec_11.SetY(vec_1.Y());
|
|
|
|
if (m_it1 == m_BSplineTop.begin() && m_it2 == m_BSplineBottom.begin()) // erster lauf -> xy zustellung (Slave)
|
|
{
|
|
vec_21.SetX(vec_2.X());
|
|
vec_21.SetY(vec_2.Y());
|
|
}
|
|
else
|
|
{
|
|
vec_21.SetX(0.0);
|
|
vec_21.SetY(vec_2.Z()); // slave zustellung in z-Richtung (ab 2.Lauf)
|
|
}
|
|
|
|
// Simulationsoutput
|
|
if (brob == false)
|
|
{
|
|
// ***** MASTER ******
|
|
|
|
ParameterCalculation_Line(vec_11.Magnitude());
|
|
|
|
if (vec_11.Magnitude() != 0)
|
|
vec_11.Normalize();
|
|
|
|
N = std::max(2, int(ceil((m_T - m_t0)/m_step))); // Anzahl der zu erzeugenden Outputwerte
|
|
m_del_t = (m_T - m_t0)/N; // Zeitschrittweite
|
|
|
|
for (int i=0; i<N; ++i)
|
|
{
|
|
// Erzeuge XY-Outputvektor
|
|
tmp.x = vec_11.X()*GetVelocity(t);
|
|
tmp.y = vec_11.Y()*GetVelocity(t);
|
|
tmp.z = 0.0;
|
|
|
|
tmp2.push_back(tmp);
|
|
|
|
m_Output.push_back(tmp2);
|
|
m_Output_time.push_back(t);
|
|
|
|
t += m_del_t;
|
|
tmp2.clear();
|
|
}
|
|
|
|
m_Output_time.push_back(m_T);
|
|
|
|
// ***** SLAVE ******
|
|
t = m_t0;
|
|
ParameterCalculation_Line(vec_21.Magnitude());
|
|
|
|
if (vec_21.Magnitude() != 0)
|
|
vec_21.Normalize();
|
|
|
|
N = std::max(2, int(ceil((m_T - m_t0)/m_step))); // Anzahl der zu erzeugenden Outputwerte
|
|
m_del_t = (m_T - m_t0)/N; // Zeitschrittweite
|
|
|
|
for (int i=0; i<N; ++i)
|
|
{
|
|
if (m_it1 == m_BSplineTop.begin() && m_it2 == m_BSplineBottom.begin())
|
|
{
|
|
// Erzeuge XY-Outputvektor
|
|
tmp.x = vec_21.X()*GetVelocity(t);
|
|
tmp.y = vec_21.Y()*GetVelocity(t);
|
|
tmp.z = 0.0;
|
|
}
|
|
else
|
|
{
|
|
// Erzeuge Z-Outputvektor
|
|
tmp.x = 0.0;
|
|
tmp.y = 0.0;
|
|
tmp.z = vec_21.Y()*GetVelocity(t);
|
|
}
|
|
|
|
tmp2.push_back(tmp);
|
|
m_Output_time2.push_back(t);
|
|
m_Output2.push_back(tmp2);
|
|
|
|
|
|
t += m_del_t;
|
|
tmp2.clear();
|
|
}
|
|
|
|
m_Output_time2.push_back(m_T);
|
|
|
|
// Nullvektor
|
|
tmp.x = 0.0;
|
|
tmp.y = 0.0;
|
|
tmp.z = 0.0;
|
|
|
|
tmp2.push_back(tmp);
|
|
|
|
m_Output.push_back(tmp2);
|
|
m_Output2.push_back(tmp2);
|
|
}
|
|
else // Roboter Output für den beidseitigen Fall
|
|
{
|
|
bool con = false;
|
|
|
|
// hier wird die Anzahl <N> der Punkte für die Diskretisierung der Zustellungslinie berechnet
|
|
if (vec_11.Magnitude() > vec_21.Magnitude()) N = std::max(2, int(ceil(vec_21.Magnitude()/TolDist)));
|
|
else N = std::max(2, int(ceil(vec_11.Magnitude()/TolDist)));
|
|
|
|
if (vec_11.Magnitude() == 0.0 && vec_21.Magnitude() == 0.0) N=0;
|
|
if (!m_conn) con = true;
|
|
|
|
// Erster Punkt wird stets weggelassen
|
|
if (m_it1 == m_BSplineTop.begin() && m_it2 == m_BSplineBottom.begin())
|
|
{
|
|
// Startpunkt Master
|
|
tmp.x = m_StartPnts1[0].X();
|
|
tmp.y = m_StartPnts1[0].Y();
|
|
tmp.z = m_StartPnts1[0].Z();
|
|
|
|
m_Output_robo1.push_back(tmp);
|
|
RoboFlag_Master.push_back(0);
|
|
|
|
// Startpunkt Slave
|
|
tmp.x = m_StartPnts2[0].X();
|
|
tmp.y = m_StartPnts2[0].Y();
|
|
tmp.z = m_StartPnts2[0].Z();
|
|
|
|
m_Output_robo2.push_back(tmp);
|
|
RoboFlag_Slave.push_back(0);
|
|
}
|
|
|
|
// Erzeuge Output - MASTER
|
|
for (int i=1; i<N; ++i)
|
|
{
|
|
tmp.x = m_StartPnts1[0].X() + (double(i)*vec_11.X())/(double(N)-1.0);
|
|
tmp.y = m_StartPnts1[0].Y() + (double(i)*vec_11.Y())/(double(N)-1.0);
|
|
tmp.z = m_StartPnts1[con].Z();
|
|
|
|
m_Output_robo1.push_back(tmp);
|
|
RoboFlag_Master.push_back(0);
|
|
}
|
|
|
|
// // Erzeuge Output - SLAVE
|
|
for (int i=1; i<N; ++i)
|
|
{
|
|
if (m_it1 == m_BSplineTop.begin() && m_it2 == m_BSplineBottom.begin())
|
|
{
|
|
tmp.x = m_StartPnts2[0].X() + (double(i)*vec_21.X())/(double(N)-1.0);
|
|
tmp.y = m_StartPnts2[0].Y() + (double(i)*vec_21.Y())/(double(N)-1.0);
|
|
tmp.z = m_StartPnts2[0].Z();
|
|
}
|
|
else
|
|
{
|
|
tmp.x = m_StartPnts2[con].X();
|
|
tmp.y = m_StartPnts2[con].Y();
|
|
tmp.z = m_StartPnts2[0].Z() + (double(i)*vec_21.Y())/(double(N)-1.0);
|
|
}
|
|
|
|
m_Output_robo2.push_back(tmp);
|
|
RoboFlag_Slave.push_back(0);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
gp_Vec2d vec_11,vec_12;
|
|
gp_Vec vec_1(m_StartPnts1[0], m_StartPnts1[1]);
|
|
|
|
vec_11.SetX(vec_1.X());
|
|
vec_11.SetY(vec_1.Y());
|
|
|
|
if(brob == false)
|
|
{
|
|
ParameterCalculation_Line(vec_11.Magnitude());
|
|
|
|
if(vec_11.Magnitude() != 0.0)
|
|
vec_11.Normalize();
|
|
|
|
N = std::max(2, (int)((m_T-m_t0)/m_step));
|
|
m_del_t = (m_T-m_t0)/double(N);
|
|
|
|
for(int i=0; i<N; ++i)
|
|
{
|
|
// Erzeuge XY-Outputvektor
|
|
tmp.x = vec_11.X()*GetVelocity(t);
|
|
tmp.y = vec_11.Y()*GetVelocity(t);
|
|
tmp.z = 0.0;
|
|
|
|
tmp2.push_back(tmp);
|
|
|
|
m_Output_time.push_back(t);
|
|
m_Output.push_back(tmp2);
|
|
|
|
t += m_del_t;
|
|
tmp2.clear();
|
|
}
|
|
|
|
m_Output_time.push_back(m_T);
|
|
|
|
// Nullvektor
|
|
tmp.x = 0.0;
|
|
tmp.y = 0.0;
|
|
tmp.z = 0.0;
|
|
|
|
tmp2.push_back(tmp);
|
|
m_Output.push_back(tmp2);
|
|
tmp2.clear();
|
|
}
|
|
else
|
|
{
|
|
N = (int) vec_11.Magnitude(); // Anzahl der zu erzeugenden Outputwerte
|
|
|
|
for (int i=0; i<N; ++i)
|
|
{
|
|
tmp.x = m_StartPnts1[0].X() + (i*vec_11.X())/N;
|
|
tmp.y = m_StartPnts1[0].Y() + (i*vec_11.Y())/N;
|
|
tmp.z = m_StartPnts1[0].Z();
|
|
|
|
m_Output_robo1.push_back(tmp);
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/* Füllt die Outputvektoren für die zweite Zustellung. Der Eingabeparameter <brob> legt den Ausgabetyp fest*/
|
|
bool path_simulate::ConnectPaths_z(bool brob)
|
|
{
|
|
int N;
|
|
double t = m_t0;
|
|
|
|
Base::Vector3d tmp;
|
|
std::vector<double> d;
|
|
std::vector<Base::Vector3d> tmp2;
|
|
|
|
gp_Vec vec_t(m_StartPnts1[0], m_StartPnts1[1]);
|
|
if( 1e-3 > vec_t.Magnitude())
|
|
return true;
|
|
|
|
if (m_single == false)
|
|
{
|
|
gp_Vec vec_1(m_StartPnts1[0], m_StartPnts1[1]);
|
|
gp_Vec vec_2(m_StartPnts2[0], m_StartPnts2[1]);
|
|
|
|
gp_Vec2d vec_11,vec_12;
|
|
|
|
vec_11.SetX(0.0);
|
|
vec_11.SetY(vec_1.Z());
|
|
|
|
if (m_it1 == m_BSplineTop.begin() && m_it2 == m_BSplineBottom.begin())
|
|
{
|
|
vec_12.SetX(0.0);
|
|
vec_12.SetY(vec_2.Z());
|
|
}
|
|
else
|
|
{
|
|
vec_12.SetX(vec_2.X());
|
|
vec_12.SetY(vec_2.Y());
|
|
}
|
|
|
|
if (brob == false)
|
|
{
|
|
// ***** MASTER ******
|
|
ParameterCalculation_Line(vec_11.Magnitude());
|
|
|
|
if (vec_11.Magnitude() != 0)
|
|
vec_11.Normalize();
|
|
|
|
N = std::max(2, int(ceil((m_T - m_t0)/m_step))); // Anzahl der zu erzeugenden Outputwerte
|
|
m_del_t = (m_T - m_t0)/N; // Zeitschrittweite
|
|
|
|
for (int i=0; i<N; ++i)
|
|
{
|
|
// Erzeuge Z-Outputvektor
|
|
tmp.x = 0.0;
|
|
tmp.y = 0.0;
|
|
tmp.z = vec_11.Y()*GetVelocity(t);
|
|
|
|
tmp2.push_back(tmp);
|
|
|
|
m_Output.push_back(tmp2);
|
|
m_Output_time.push_back(t);
|
|
|
|
t += m_del_t;
|
|
tmp2.clear();
|
|
}
|
|
|
|
m_Output_time.push_back(m_T);
|
|
|
|
// ***** SLAVE ******
|
|
t = m_t0;
|
|
ParameterCalculation_Line(vec_12.Magnitude());
|
|
|
|
if (vec_12.Magnitude() != 0)
|
|
vec_12.Normalize();
|
|
|
|
N = std::max(2, int(ceil((m_T - m_t0)/m_step))); // Anzahl der zu erzeugenden Outputwerte
|
|
m_del_t = (m_T - m_t0)/N; // Zeitschrittweite
|
|
|
|
for (int i=0; i<N; ++i)
|
|
{
|
|
if (m_it1 == m_BSplineTop.begin() && m_it2 == m_BSplineBottom.begin())
|
|
{
|
|
// Erzeuge Z-Outputvektor im allerersten Schritt
|
|
tmp.x = 0.0;
|
|
tmp.y = 0.0;
|
|
tmp.z = vec_12.Y()*GetVelocity(t);
|
|
}
|
|
else // Slave Zustellung in XY-Richtung
|
|
{
|
|
// Erzeuge XY-Outputvektor
|
|
tmp.x = vec_12.X()*GetVelocity(t);
|
|
tmp.y = vec_12.Y()*GetVelocity(t);
|
|
tmp.z = 0.0;
|
|
}
|
|
|
|
tmp2.push_back(tmp);
|
|
m_Output_time2.push_back(t);
|
|
m_Output2.push_back(tmp2);
|
|
|
|
|
|
t += m_del_t;
|
|
tmp2.clear();
|
|
}
|
|
|
|
m_Output_time2.push_back(m_T);
|
|
|
|
tmp.x = 0.0;
|
|
tmp.y = 0.0;
|
|
tmp.z = 0.0;
|
|
|
|
tmp2.push_back(tmp);
|
|
|
|
m_Output.push_back(tmp2);
|
|
m_Output2.push_back(tmp2);
|
|
}
|
|
else // Roboteroutput
|
|
{
|
|
if (vec_11.Magnitude() > vec_12.Magnitude()) std::max(2, N = int(ceil(vec_12.Magnitude()/TolDist))); // Anzahl der zu erzeugenden Outputwerte
|
|
else std::max(2, N = int(ceil(vec_11.Magnitude()/TolDist))); // Anzahl der zu erzeugenden Outputwerte
|
|
|
|
if (vec_11.Magnitude() == 0.0 && vec_12.Magnitude() == 0.0) N=1;
|
|
|
|
for (int i=1; i<N; ++i)
|
|
{
|
|
/*MASTER*/
|
|
|
|
// Erzeuge Outputvektor für die Zustellung in Z-Richtung
|
|
tmp.x = m_StartPnts1[m_conn].X();
|
|
tmp.y = m_StartPnts1[m_conn].Y();
|
|
tmp.z = m_StartPnts1[0].Z() + (double(i)*vec_11.Y())/double(N-1);
|
|
|
|
m_Output_robo1.push_back(tmp);
|
|
RoboFlag_Master.push_back(0);
|
|
|
|
|
|
|
|
/*SLAVE*/
|
|
|
|
if (m_it1 == m_BSplineTop.begin() && m_it2 == m_BSplineBottom.begin())
|
|
{
|
|
// Erzeuge Outputvektor für die Zustellung in Z-Richtung (nur im allerersten Schritt)
|
|
tmp.x = m_StartPnts2[1].X();
|
|
tmp.y = m_StartPnts2[1].Y();
|
|
tmp.z = m_StartPnts2[0].Z() + (double(i)*vec_12.Y())/double(N-1);
|
|
}
|
|
else
|
|
{
|
|
// Erzeuge Outputvektor für die Zustellung in XY-Richtung
|
|
tmp.x = m_StartPnts2[0].X() + (double(i)*vec_12.X())/double(N-1);
|
|
tmp.y = m_StartPnts2[0].Y() + (double(i)*vec_12.Y())/double(N-1);
|
|
tmp.z = m_StartPnts2[m_conn].Z();
|
|
}
|
|
|
|
m_Output_robo2.push_back(tmp);
|
|
RoboFlag_Slave.push_back(0);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
gp_Vec vec_1(m_StartPnts1[0], m_StartPnts1[1]);
|
|
gp_Vec2d vec_12;
|
|
|
|
vec_12.SetX(0.0);
|
|
vec_12.SetY(vec_1.Z());
|
|
|
|
if (brob == false)
|
|
{
|
|
ParameterCalculation_Line(vec_12.Magnitude());
|
|
|
|
if (vec_12.Magnitude() != 0)
|
|
vec_12.Normalize();
|
|
|
|
N = std::max(2, int(ceil((m_T - m_t0)/m_step))); // Anzahl der zu erzeugenden Outputwerte
|
|
m_del_t = (m_T - m_t0)/N; // Zeitschrittweite
|
|
|
|
for (int i=0; i<N; ++i)
|
|
{
|
|
m_Output_time.push_back(t);
|
|
|
|
// Erzeuge Z-Outputvektor
|
|
tmp.x = 0.0;
|
|
tmp.y = 0.0;
|
|
tmp.z = vec_12.Y()*GetVelocity(t);
|
|
|
|
tmp2.push_back(tmp);
|
|
m_Output.push_back(tmp2);
|
|
|
|
t += m_del_t;
|
|
tmp2.clear();
|
|
}
|
|
|
|
t = m_T;
|
|
|
|
m_Output_time.push_back(t);
|
|
|
|
// Nullvektor
|
|
tmp.x = 0.0;
|
|
tmp.y = 0.0;
|
|
tmp.z = 0.0;
|
|
|
|
tmp2.push_back(tmp);
|
|
m_Output.push_back(tmp2);
|
|
tmp2.clear();
|
|
}
|
|
else // Roboteroutput
|
|
{
|
|
N =(int) vec_12.Magnitude(); // Anzahl der zu erzeugenden Outputwerte
|
|
|
|
for (int i=0; i<N; ++i)
|
|
{
|
|
// Erzeuge Outputvektor für die Zustellung in XY-Richtung
|
|
tmp.x = m_StartPnts1[0].X() + (i*vec_12.X())/N;
|
|
tmp.y = m_StartPnts1[0].Y() + (i*vec_12.Y())/N;
|
|
tmp.z = m_StartPnts1[0].Z();
|
|
|
|
m_Output_robo1.push_back(tmp);
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/* Füllt die Outputvektoren für die Zustellung für den Feature-Basierten Fall.
|
|
Der Eingabeparameter <brob> legt den Ausgabetyp fest*/
|
|
bool path_simulate::ConnectPaths_Feat(bool tool, // Tool (Master, Slave)
|
|
bool brob, // Ausgabetyp (Roboter, Simulation)
|
|
bool c_typ) // Zustellungsart (in zwei bzw. drei Schritten)
|
|
{
|
|
int N, ind;
|
|
double rad, t;
|
|
bool dir;
|
|
|
|
std::vector<double> Times;
|
|
std::vector<gp_Pnt> ConnPnts;
|
|
std::vector< std::vector<Base::Vector3d> > Out;
|
|
gp_Vec vec[3], vec_tmp[3];
|
|
|
|
Base::Vector3d tmp;
|
|
std::vector<double> d;
|
|
std::vector<Base::Vector3d> tmp2;
|
|
double vel;
|
|
|
|
dir = CheckConnect(tool); // setze Starpunkte neu
|
|
|
|
if (!tool)
|
|
{
|
|
ConnPnts = m_StartPnts1;
|
|
rad = m_set.master_radius;
|
|
}
|
|
else
|
|
{
|
|
ConnPnts = m_StartPnts2;
|
|
rad = m_set.slave_radius;
|
|
}
|
|
|
|
if (c_typ) // Zustellung in 2 Schritten
|
|
{
|
|
ind = 2;
|
|
vec_tmp[0].SetCoord(0.0, 0.0, abs(ConnPnts[1].Z()-ConnPnts[0].Z()));
|
|
vec_tmp[1].SetCoord(ConnPnts[1].X()-ConnPnts[0].X(), ConnPnts[1].Y()-ConnPnts[0].Y(), 0.0);
|
|
|
|
if (dir) // tool (Master/Slave) muss runter fahren
|
|
{
|
|
if (!tool)
|
|
{
|
|
vec[0] = vec_tmp[1]; // tool = Master
|
|
vec[1] = -vec_tmp[0];
|
|
}
|
|
else
|
|
{
|
|
vec[0] = -vec_tmp[0]; // tool = Slave
|
|
vec[1] = vec_tmp[1];
|
|
}
|
|
}
|
|
else // tool (Master/Slave) muss hoch fahren
|
|
{
|
|
if (!tool)
|
|
{
|
|
vec[0] = vec_tmp[0]; // tool = Master
|
|
vec[1] = vec_tmp[1];
|
|
}
|
|
else
|
|
{
|
|
vec[0] = vec_tmp[1]; // tool = Slave
|
|
vec[1] = vec_tmp[0];
|
|
}
|
|
}
|
|
}
|
|
else // Zustellung in 3 Schritten
|
|
{
|
|
ind = 3;
|
|
|
|
vec_tmp[0].SetCoord(0.0, 0.0, rad);
|
|
vec_tmp[1].SetCoord(ConnPnts[1].X()-ConnPnts[0].X(), ConnPnts[1].Y()-ConnPnts[0].Y(), 0.0);
|
|
vec_tmp[2].SetCoord(0.0, 0.0, abs(ConnPnts[1].Z()-ConnPnts[0].Z()) + rad);
|
|
|
|
if (dir) // tool (Master/Slave) muss runter fahren
|
|
{
|
|
if (!tool)
|
|
{
|
|
vec[0] = vec_tmp[0]; // tool = Master
|
|
vec[1] = vec_tmp[1];
|
|
vec[2] = -vec_tmp[2];
|
|
}
|
|
else
|
|
{
|
|
vec[0] = -vec_tmp[2]; // tool = Slave
|
|
vec[1] = vec_tmp[1];
|
|
vec[2] = vec_tmp[0];
|
|
}
|
|
}
|
|
else // tool (Master/Slave) muss hoch fahren
|
|
{
|
|
if (!tool)
|
|
{
|
|
vec[0] = vec_tmp[2]; // tool = Master
|
|
vec[1] = vec_tmp[1];
|
|
vec[2] = -vec_tmp[0];
|
|
}
|
|
else
|
|
{
|
|
vec[0] = -vec_tmp[0]; // tool = Slave
|
|
vec[1] = vec_tmp[1];
|
|
vec[2] = vec_tmp[2];
|
|
}
|
|
}
|
|
}
|
|
|
|
if (brob) // Roboteroutput
|
|
{
|
|
for (int i=0; i<ind; ++i)
|
|
{
|
|
N = std::max(2, int(ceil(vec[i].Magnitude()/TolDist))); // Anzahl der zu erzeugenden Outputwerte
|
|
|
|
for(int j=1; j<N; ++j)
|
|
{
|
|
tmp.x = ConnPnts[0].X() + (double(j)*vec[i].X())/(double(N)-1.0);
|
|
tmp.y = ConnPnts[0].Y() + (double(j)*vec[i].Y())/(double(N)-1.0);
|
|
tmp.z = ConnPnts[0].Z() + (double(j)*vec[i].Z())/(double(N)-1.0);;
|
|
|
|
if(!tool) m_Output_robo1.push_back(tmp); // Master
|
|
else m_Output_robo2.push_back(tmp); // Slave
|
|
}
|
|
|
|
ConnPnts[0].SetCoord(tmp.x , tmp.y, tmp.z); // Setze Startpunkt = Endpunkt für die nächste Iteration
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
if(ConnPnts[0].Distance(ConnPnts[1]) < 1e-3) return true; // keine Zustellung bei Spiralbahnen !!!
|
|
|
|
for (int i=0; i<ind; ++i)
|
|
{
|
|
t = m_t0;
|
|
ParameterCalculation_Line(vec[i].Magnitude());
|
|
if (vec[i].Magnitude() != 0.0) vec[i].Normalize();
|
|
else continue;
|
|
|
|
N = std::max(2,(int) ceil((m_T - m_t0)/m_step)); // Anzahl der zu erzeugenden Outputwerte
|
|
m_del_t = (m_T - m_t0)/N; // Zeitschrittweite
|
|
|
|
for (int j=0; j<N; ++j)
|
|
{
|
|
Times.push_back(t);
|
|
vel = GetVelocity(t);
|
|
|
|
tmp.x = vec[i].X()*vel;
|
|
tmp.y = vec[i].Y()*vel;
|
|
tmp.z = vec[i].Z()*vel;
|
|
|
|
tmp2.push_back(tmp);
|
|
Out.push_back(tmp2); // Fülle temporären Output-Vektor (wird weiter unten zugewiesen)
|
|
|
|
t += m_del_t;
|
|
tmp2.clear();
|
|
}
|
|
|
|
m_t0 = m_T; // Endzeit des letzten Durchlaufs wird zur neuen Startzeit
|
|
}
|
|
|
|
Times.push_back(m_T);
|
|
|
|
tmp.x = 0.0;
|
|
tmp.y = 0.0;
|
|
tmp.z = 0.0;
|
|
|
|
tmp2.clear();
|
|
tmp2.push_back(tmp);
|
|
Out.push_back(tmp2);
|
|
|
|
if(!tool)
|
|
{
|
|
m_Output = Out;
|
|
m_Output_time = Times;
|
|
}
|
|
else
|
|
{
|
|
m_Output2 = Out;
|
|
m_Output_time2 = Times;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/* Hier wird unter Berücksichtigung der Krümmungstoleranz <curvTOL> eine Unterteilung der Kurve vorgenommen.
|
|
Die Bereichsgrenzen werden im Rückgabevektor zurückgeliefert. Der Ausgabevektor ist leer wenn die maximale Krümmung
|
|
den Toleranzwert nicht überschreitet und somit auch keine Unterteilung notwendig ist */
|
|
std::vector<std::vector<double> > path_simulate::CompBounds(bool tool,std::vector<double> knots)
|
|
{
|
|
m_curMax = 0.0; // setze maximale Krümmung initial auf Null
|
|
double cr_bound = 1/curvTOL;
|
|
double cr_last;
|
|
gp_Vec dtmp1, dtmp2;
|
|
gp_Pnt dtmp0;
|
|
GeomAdaptor_Curve curve;
|
|
std::vector<double> single_bound;
|
|
std::vector<double> bounds;
|
|
std::vector<std::vector<double> > CriticalBounds;
|
|
|
|
// lade aktuelle Kurve
|
|
if (!tool) curve.Load(*m_it1);
|
|
else curve.Load(*m_it2);
|
|
|
|
double fParam = curve.FirstParameter(), // Erster Kurvenparameter
|
|
lParam = curve.LastParameter(), // Letzter Kurvenparameter
|
|
period = lParam - fParam; // Länge des Parameterbereichs
|
|
|
|
|
|
int n = knots.size(); // Länge des Knotenvektors
|
|
bool b = false;
|
|
|
|
// Hier erfolgt die Berechnung der maximalen Krümmung <m_curMax>
|
|
// Die Parameter der Bereichsgrenzen an denen die Kurvenkrümmung dem Toleranzwert <cr_bound>
|
|
// entspricht werden in den Vektor <bounds> für die weitere Nachbearbeitung gefüllt
|
|
for (int i=0; i<n; ++i)
|
|
{
|
|
curve.D2(knots[i], dtmp0, dtmp1, dtmp2); // Da es sich um eine kubische B-Spline Kurve handelt,
|
|
// kann die maximale Krümmung nur an den Knotenpunkten angenomen werden
|
|
if(dtmp2.Magnitude() >= cr_bound && !b)
|
|
{
|
|
if(knots[i] >= m_boundTol && knots[i] < lParam - m_boundTol)
|
|
{
|
|
if(cr_last < cr_bound)
|
|
{
|
|
bounds.push_back(knots[i-1] + (knots[i] - knots[i-1])*
|
|
(cr_last - cr_bound)/
|
|
(cr_last - dtmp2.Magnitude()));
|
|
}
|
|
else
|
|
{
|
|
bounds.push_back(fParam + m_boundTol);
|
|
}
|
|
b = true;
|
|
}
|
|
}
|
|
|
|
if(dtmp2.Magnitude() < cr_bound && b)
|
|
{
|
|
if(knots[i] <= lParam - m_boundTol)
|
|
{
|
|
bounds.push_back(knots[i-1] + (knots[i] - knots[i-1])*
|
|
(cr_bound - cr_last)/
|
|
(dtmp2.Magnitude() - cr_last));
|
|
}
|
|
else
|
|
{
|
|
bounds.push_back(lParam - m_boundTol);
|
|
|
|
}
|
|
b = false;
|
|
}
|
|
|
|
cr_last = dtmp2.Magnitude(); // Krümmung entspricht hier dem Betrag der zweiten Ableitung
|
|
|
|
if(m_curMax < cr_last) // Speichert maximale Krümmung
|
|
m_curMax = cr_last;
|
|
}
|
|
|
|
if(period < 2*m_boundTol || bounds.size() == 0)
|
|
return CriticalBounds;
|
|
|
|
if(b) bounds.push_back(lParam - m_boundTol); // Hier muss evtl. noch die letzte Grenze eingefügt werden
|
|
|
|
n = (int) bounds.size()/2; // <bounds> hat stets gerade Länge
|
|
for(int i=0; i<n; i++) // Fasse Bereiche welche einen geringeren Abstand als <m_boundTol> haben zusammen
|
|
{
|
|
single_bound.push_back(bounds[2*i]);
|
|
|
|
while(i<n-1 && bounds[2*i+2] - bounds[2*i+1] < m_boundTol)
|
|
i++;
|
|
|
|
single_bound.push_back(bounds[2*i+1]);
|
|
CriticalBounds.push_back(single_bound);
|
|
single_bound.clear();
|
|
}
|
|
|
|
return CriticalBounds;
|
|
}
|
|
|
|
/* Hier wird die Vorarbeit für die Funktion Gen_Path() geleistet.
|
|
Die Vektoren <m_length>, <m_velocity>, <m_accel> werden für die aktuelle Kurve gefüllt.*/
|
|
bool path_simulate::CompPath(bool tool) // tool = 0 -> Master
|
|
// tool = 1 -> Slave
|
|
{
|
|
m_boundTol = pow(m_vmax, 2.0)/m_amax; // Toleranzbereich vor kritischen Bereichen (notwendig zum Hochbeschleunigen)
|
|
|
|
double cur = 1.0/curvTOL, // Krümmungstoleranz für die Kurvenunterteilung
|
|
pos = 0.0,
|
|
cur_tmp = 0.0;
|
|
|
|
int nb_knots;
|
|
|
|
std::vector<std::vector<double> > v_vec;
|
|
std::vector<double> v(3), l_vec, a_vec;
|
|
|
|
std::vector<Base::Vector3d> Pnt1Vec;
|
|
|
|
GeomAdaptor_Curve curve;
|
|
|
|
gp_Pnt pnt0;
|
|
gp_Vec pnt, pnt1, pnt2, vec;
|
|
Base::Vector3d Pnt1;
|
|
|
|
double start,
|
|
fParam,
|
|
lParam,
|
|
period;
|
|
|
|
double d2, velo, tetha,
|
|
len, len_1;
|
|
|
|
double t0 = m_t0; // Übergibt aktuelle Startzeit
|
|
|
|
int num = Detect_FeatCurve(tool); // Liefert Anzahl der zu fahrenden Kurven (i.d.R. num = 1)
|
|
|
|
for(int a=0; a<num; a++) // Schleife über die zusammenhängenden Kurven
|
|
{
|
|
// lade aktuelle kurven
|
|
if (!tool) curve.Load(*m_it1);
|
|
else curve.Load(*m_it2);
|
|
|
|
// setze Parameter neu
|
|
fParam = curve.FirstParameter();
|
|
lParam = curve.LastParameter();
|
|
period = lParam - fParam;
|
|
|
|
// übergibt Starparameter der aktuellen Kurve
|
|
m_StartParam[tool] = fParam;
|
|
start = m_StartParam[tool];
|
|
|
|
// *** Berechnung des Knotenvektors ***
|
|
if (!tool)
|
|
{
|
|
nb_knots = (*m_it1)->NbKnots();
|
|
m_Knots = new TColStd_Array1OfReal(0,nb_knots-1);
|
|
(*m_it1)->Knots(*m_Knots);
|
|
}
|
|
else
|
|
{
|
|
nb_knots = (*m_it2)->NbKnots();
|
|
m_Knots = new TColStd_Array1OfReal(0,nb_knots-1);
|
|
(*m_it2)->Knots(*m_Knots);
|
|
}
|
|
|
|
std::vector<double> knot_vec(m_Knots->Length());
|
|
for(int i=0; i<m_Knots->Length(); i++)
|
|
knot_vec[i] = m_Knots->Value(i);
|
|
|
|
// *** Ende der Berechnung des Knotenvektors ***
|
|
|
|
|
|
std::vector<std::vector<double> > CriticalBounds =
|
|
CompBounds(tool, knot_vec); // Berechnet auf Basis der aktuellen Kurve,
|
|
// die kritischen Bereiche im Parameterraum und
|
|
// berechnet gleichzeitig die maximale Kurvenkrümmung <m_curMax>
|
|
|
|
m_vmid = std::min(m_vmax,sqrt(m_amax/m_curMax)); // Legt Geschwindigkeit fest mit der allen kritischen Bereiche
|
|
// abgefahren werden
|
|
|
|
Newtry: // Falls die generierten Weglängen nicht ausreichen, dann wird ein neuer Versuch mit halbem <m_vmid> gestartet
|
|
|
|
v[0] = 0.0; // starte jede Kurve mit v = 0
|
|
int m = 0;
|
|
|
|
for (unsigned int i=0; i<CriticalBounds.size(); ++i) // Schleife über einzelne Unterteilungsbereiche
|
|
{
|
|
d2 = 0.0;
|
|
pos = m_Knots->Value(m);
|
|
|
|
while(pos < m_StartParam[tool])
|
|
{
|
|
m++;
|
|
pos = m_Knots->Value(m);
|
|
}
|
|
|
|
/*------------------- Gerader Bereich ---------------------*/
|
|
|
|
while(pos < CriticalBounds[i][0]) // Berechnung der maximalen Krümmung dieses geraden Bereichs
|
|
{
|
|
curve.D2(pos, pnt0, pnt1, pnt2);
|
|
cur_tmp = pnt2.Magnitude(); // Krümmung am aktuellen Punkt
|
|
|
|
if(d2 < cur_tmp)
|
|
d2 = cur_tmp; // Speichert maximale Krümmung dieses geraden Bereichs
|
|
|
|
m++;
|
|
pos = m_Knots->Value(m);
|
|
}
|
|
|
|
tetha = 0.6 + 0.25*sqrt(d2/m_curMax); // Setzt tetha-parameter -> (0.6 < tetha < 0.85)
|
|
velo = std::min(m_vmax, tetha*(sqrt(m_amax/d2))); // Setzt maximale Geschwindigkeit <velo>
|
|
m_a = m_amax - d2*velo*velo; // wenn <velo> zu groß gewählt wurde, kann <m_a> evtl. negativ werden
|
|
|
|
// Korrektur
|
|
while(m_a <= 0.0)
|
|
{
|
|
velo = velo/2.0;
|
|
m_a = m_amax - d2*velo*velo;
|
|
}
|
|
|
|
if(velo < m_vmid)
|
|
m_vmid = velo;
|
|
|
|
v[1] = velo;
|
|
|
|
/*-------------- Korrektur der Geschwindigkeiten (falls der Weg zu lang) -------------*/
|
|
|
|
len = CriticalBounds[i][0] - m_StartParam[tool]; // Länge des i. geraden Abschnitts
|
|
len_1 = (pow(v[1] - v[0],2.0) + pow(v[1] - m_vmid,2.0))/m_a; // notwendige Länge
|
|
|
|
if(len < pow(v[0] - m_vmid,2.0)/m_a) // für diesen Fall ist keine Korrektur möglich
|
|
{
|
|
l_vec.clear();
|
|
v_vec.clear();
|
|
a_vec.clear();
|
|
|
|
m_StartParam[tool] = start; // setze Startparameter zurück
|
|
m_vmid = m_vmid/2; // halbiere kritische Durchlaufgeschwindigkeit
|
|
goto Newtry;
|
|
}
|
|
|
|
while(len < len_1)
|
|
{
|
|
v[1] = v[0] + (v[1] - v[0])/2.0;
|
|
len_1 = (pow(v[1] - v[0],2.0) + pow(v[1] - m_vmid,2.0))/m_a;
|
|
}
|
|
|
|
v[2] = m_vmid;
|
|
|
|
/*---Korrekturende---*/
|
|
|
|
// Fülle Vektoren
|
|
l_vec.push_back(len); // Länge
|
|
v_vec.push_back(v); // Geschwindigkeiten
|
|
a_vec.push_back(m_a); // Beschleunigung
|
|
|
|
m_StartParam[tool] += len; // setzt Startparameter neu
|
|
|
|
/*------------------------ gekrümmter Bereich ------------------------*/
|
|
|
|
len = CriticalBounds[i][1] - CriticalBounds[i][0]; // Bogenlänge des gekrümmten Bereichs
|
|
m_StartParam[tool] += len; // setzt Startparameter neu
|
|
v[0] = v[2]; // Endgeschwindigkeit wird zur Startgeschwindigkeit
|
|
|
|
l_vec.push_back(len);
|
|
}
|
|
|
|
d2 = 0;
|
|
|
|
// Korrigiere aktuellen Knotenparameter
|
|
while(pos < m_StartParam[tool])
|
|
{
|
|
m++;
|
|
pos = m_Knots->Value(m);
|
|
}
|
|
|
|
pos = m_Knots->Value(m);
|
|
|
|
// Berechnung der maximalen Krümmung für den letzten Abschnitt
|
|
while(pos < lParam)
|
|
{
|
|
curve.D2(pos, pnt0, pnt1, pnt2);
|
|
|
|
Pnt1.x = pnt2.X();
|
|
Pnt1.y = pnt2.Y();
|
|
Pnt1.z = pnt2.Z();
|
|
|
|
if(d2 < Pnt1.Length()) d2 = Pnt1.Length();
|
|
|
|
m++;
|
|
pos = m_Knots->Value(m);
|
|
}
|
|
|
|
tetha = 0.6 + 0.25*sqrt(d2/m_curMax); // 0.6 < tetha < 0.85
|
|
velo = std::min(m_vmax, tetha*(sqrt(m_amax/d2))); // vgl. oben
|
|
|
|
v[1] = velo;
|
|
v[2] = 0.0;
|
|
m_a = m_amax - d2*velo*velo; // wenn velo zu groß gewählt wurde, kann m_a negativ werden
|
|
|
|
// Korrektur
|
|
while(m_a <= 0.0)
|
|
{
|
|
velo = velo/2.0;
|
|
v[1] = velo;
|
|
m_a = m_amax - cur*velo*velo;
|
|
}
|
|
|
|
d2 = 0;
|
|
|
|
len = lParam - m_StartParam[tool] + start; // Länge des letzten geraden Abschnitts
|
|
len_1 = (pow(v[1] - v[0],2.0) + pow(v[1],2.0))/m_a; // notwendige Länge
|
|
|
|
if(len < pow(v[0],2.0)/m_a) // hier keine Korrektur möglich
|
|
{
|
|
l_vec.clear();
|
|
v_vec.clear();
|
|
a_vec.clear();
|
|
|
|
m_StartParam[tool] = start; // setze Startparameter zurück
|
|
m_vmid = m_vmid/2; // halbiere kritische Durchlaufgeschwindigkeit und versuchs erneut
|
|
goto Newtry;
|
|
}
|
|
|
|
while(len < len_1)
|
|
{
|
|
v[1] = v[0] + (v[1] - v[0])/2;
|
|
len_1 = (pow(v[1] - v[0],2.0) + pow(v[1],2.0))/m_a;
|
|
}
|
|
|
|
// Fülle Vektoren
|
|
l_vec.push_back(len); // Länge
|
|
v_vec.push_back(v); // Geschwindigkeiten
|
|
a_vec.push_back(m_a); // Beschleunignung
|
|
|
|
// Fülle hier erst die Ausgabevektoren (einmal pro Kurve)
|
|
if(tool)
|
|
{
|
|
m_length_sl.push_back(l_vec);
|
|
m_velocity_sl.push_back(v_vec);
|
|
m_accel_sl.push_back(a_vec);
|
|
}
|
|
else
|
|
{
|
|
m_length_ma.push_back(l_vec);
|
|
m_velocity_ma.push_back(v_vec);
|
|
m_accel_ma.push_back(a_vec);
|
|
}
|
|
|
|
// lade aktuelle kurven
|
|
if (!tool) m_it1++;
|
|
else m_it2++;
|
|
|
|
m_StartParam[tool] = start;
|
|
|
|
l_vec.clear();
|
|
v_vec.clear();
|
|
a_vec.clear();
|
|
|
|
curve.Delete();
|
|
CriticalBounds.clear();
|
|
}
|
|
|
|
// Setze Iterator und Startparameter wieder zurück
|
|
if (!tool)
|
|
{
|
|
for(int i=0; i<num; i++){m_it1--;}
|
|
m_StartParam[tool] = (*m_it1)->FirstParameter();
|
|
|
|
}
|
|
else
|
|
{
|
|
for(int i=0; i<num; i++){m_it2--;}
|
|
m_StartParam[tool] = (*m_it2)->FirstParameter();
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/* Integriert erst nach der Trapezregel die Outputwerte nach der Zeit und liefert den Fehlervektor als Verbindung von Start- und Endpunkt.
|
|
Abschließend erfolgt die Korrektur der Outputvektoren um eben diesen Fehlervektor */
|
|
bool path_simulate::Correction(bool tool)
|
|
{
|
|
int N;
|
|
gp_Vec vec;
|
|
gp_Pnt pnt;
|
|
Base::Vector3d tmp;
|
|
std::vector<Base::Vector3d> tmp2;
|
|
|
|
double Sum[3];
|
|
|
|
Sum[0] = 0.0;
|
|
Sum[1] = 0.0;
|
|
Sum[2] = 0.0;
|
|
|
|
// Hier erfolgt die numerische Integration des Outputvektors nach der Trapezregel und der
|
|
// entstehende Fehlervektor wird in <vec> abgelegt
|
|
if (tool==false) // Master
|
|
{
|
|
N = m_Output.size();
|
|
for (int i=1; i<N; ++i)
|
|
{
|
|
Sum[0] += (m_Output[i][0].x + m_Output[i-1][0].x)*(m_Output_time[i] - m_Output_time[i-1]) / 2.0;
|
|
Sum[1] += (m_Output[i][0].y + m_Output[i-1][0].y)*(m_Output_time[i] - m_Output_time[i-1]) / 2.0;
|
|
Sum[2] += (m_Output[i][0].z + m_Output[i-1][0].z)*(m_Output_time[i] - m_Output_time[i-1]) / 2.0;
|
|
}
|
|
|
|
if(m_StartPnts1[0].Distance(m_StartPnts1[1]) > 1e-3) // falls Kurve NICHT geschlossen
|
|
{
|
|
vec.SetCoord(Sum[0],Sum[1],Sum[2]);
|
|
vec.SetCoord(m_StartPnts1[1].X() - m_StartPnts1[0].X() - vec.X(),
|
|
m_StartPnts1[1].Y() - m_StartPnts1[0].Y() - vec.Y(),
|
|
m_StartPnts1[1].Z() - m_StartPnts1[0].Z() - vec.Z());
|
|
}
|
|
else
|
|
vec.SetCoord(-Sum[0],-Sum[1],-Sum[2]);
|
|
}
|
|
else // Slave
|
|
{
|
|
N = m_Output2.size();
|
|
for (int i=1; i<N; ++i)
|
|
{
|
|
Sum[0] += (m_Output2[i][0].x + m_Output2[i-1][0].x)*(m_Output_time2[i] - m_Output_time2[i-1]) / 2.0;
|
|
Sum[1] += (m_Output2[i][0].y + m_Output2[i-1][0].y)*(m_Output_time2[i] - m_Output_time2[i-1]) / 2.0;
|
|
Sum[2] += (m_Output2[i][0].z + m_Output2[i-1][0].z)*(m_Output_time2[i] - m_Output_time2[i-1]) / 2.0;
|
|
}
|
|
|
|
if(m_StartPnts2[0].Distance(m_StartPnts2[1]) > 1e-3) // falls Kurve NICHT geschlossen
|
|
{
|
|
vec.SetCoord(Sum[0],Sum[1],Sum[2]);
|
|
vec.SetCoord(m_StartPnts2[1].X() - m_StartPnts2[0].X() - vec.X(),
|
|
m_StartPnts2[1].Y() - m_StartPnts2[0].Y() - vec.Y(),
|
|
m_StartPnts2[1].Z() - m_StartPnts2[0].Z() - vec.Z());
|
|
}
|
|
else
|
|
vec.SetCoord(-Sum[0],-Sum[1],-Sum[2]);
|
|
}
|
|
|
|
ParameterCalculation_Line(vec.Magnitude()); // Berechnung der Zeitgrenzen
|
|
|
|
N = (int) ceil((m_T - m_t0)/m_step); // Anzahl der zu erzeugenden Outputwerte
|
|
m_del_t = (m_T - m_t0)/N; // Zeitschrittweite
|
|
|
|
if (N==1) // Nur ein Outputvektor
|
|
{
|
|
m_T = m_t0 + 2e-3;
|
|
|
|
if (tool==false) // Master
|
|
{
|
|
m_Output_time.push_back(m_t0);
|
|
m_Output_time.push_back(m_t0 + 1e-3);
|
|
}
|
|
else // Slave
|
|
{
|
|
m_Output_time2.push_back(m_t0);
|
|
m_Output_time2.push_back(m_t0 + 1e-3);
|
|
}
|
|
|
|
tmp.x = 0.0;
|
|
tmp.y = 0.0;
|
|
tmp.z = 0.0;
|
|
|
|
tmp2.push_back(tmp);
|
|
|
|
if(tool==false) m_Output.push_back(tmp2); // füllt Master-Output
|
|
else m_Output2.push_back(tmp2); // füllt Slave-Output
|
|
|
|
tmp2.clear();
|
|
|
|
tmp.x = (vec.X()/vec.Magnitude())*(vec.X()/1e-3);
|
|
tmp.y = (vec.Y()/vec.Magnitude())*(vec.Y()/1e-3);
|
|
tmp.z = (vec.Z()/vec.Magnitude())*(vec.Z()/1e-3);
|
|
|
|
tmp2.push_back(tmp);
|
|
|
|
if (tool==false) m_Output.push_back(tmp2);
|
|
else m_Output2.push_back(tmp2);
|
|
|
|
tmp2.clear();
|
|
}
|
|
else
|
|
{
|
|
double vel,t = m_t0;
|
|
if (vec.Magnitude() != 0) vec.Normalize();
|
|
|
|
for (int i=0; i<N; ++i)
|
|
{
|
|
if (tool==false) m_Output_time.push_back(t);
|
|
else m_Output_time2.push_back(t);
|
|
|
|
// MASTER
|
|
vel = GetVelocity(t);
|
|
|
|
// Ausgabevektor zum Zeitpunkt <t>
|
|
tmp.x = vec.X()*vel;
|
|
tmp.y = vec.Y()*vel;
|
|
tmp.z = vec.Z()*vel;
|
|
|
|
tmp2.push_back(tmp);
|
|
|
|
if (tool==false) m_Output.push_back(tmp2);
|
|
else m_Output2.push_back(tmp2);
|
|
|
|
t += m_del_t;
|
|
tmp2.clear();
|
|
}
|
|
}
|
|
|
|
// Nullvektor
|
|
tmp.x = 0.0;
|
|
tmp.y = 0.0;
|
|
tmp.z = 0.0;
|
|
|
|
tmp2.push_back(tmp);
|
|
|
|
// Übergebe Nullvektor zur Endzeit <m_T>
|
|
if (tool==false)
|
|
{
|
|
m_Output.push_back(tmp2);
|
|
m_Output_time.push_back(m_T);
|
|
}
|
|
else
|
|
{
|
|
m_Output2.push_back(tmp2);
|
|
m_Output_time2.push_back(m_T);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
// Hier wird der eigentliche Output für den Simulationsprozess generiert unter der Berücksichtigung
|
|
// der in CompPath() berechneten Vektoren m_velocity, m_accel, m_length
|
|
bool path_simulate::Gen_Path()
|
|
{
|
|
int n,m;
|
|
double length, lam, t_start, t_ma, t_sl, t_tmp;
|
|
|
|
t_start = m_t0; // setze Startzeit
|
|
m_StartPnts1[0] = (*m_it1)->StartPoint(); // setze Startparameter
|
|
|
|
if(m_single == false)
|
|
{
|
|
m_StartPnts2[0] = (*m_it2)->StartPoint();
|
|
|
|
/*------------------------------ Berechne Durchlaufzeit für das Master-Tool --------------------------------*/
|
|
|
|
n = m_velocity_ma.size();
|
|
for(int i=0; i<n; i++) // Schleife über die (zusammenhängenden) Kurven
|
|
{
|
|
m = m_velocity_ma[i].size();
|
|
for(int j=0; j<m; j++) // Schleife über die einzelnen Unterteilungsgebiete
|
|
{
|
|
m_v[0] = m_velocity_ma[i][j][0]; // Startgeschwindigkeit des i-ten Abschnitts
|
|
m_v[1] = m_velocity_ma[i][j][1]; // maximale Geschwindigkeit des i-ten Abschnitts
|
|
m_v[2] = m_velocity_ma[i][j][2]; // Endgeschwindigkeit des i-ten Abschnitts
|
|
|
|
m_a = m_accel_ma[i][j];
|
|
|
|
length = m_length_ma[i][2*j]; // Länge des unkritischen Abschnitts
|
|
// Bem: Jede Kurve beginnt und endet in einem unkritischen Abschnitt
|
|
ParameterCalculation_Curve(length); // Hier wird die Endzeit m_T berechnet
|
|
|
|
if(j != m-1)
|
|
{
|
|
length = m_length_ma[i][2*j+1];// Länge des kritischen Abschnitts
|
|
m_T = m_T + length/m_v[2]; // Berechnet neue Endzeit (kritische Abschnitte werden mit konstanter
|
|
// Geschwindigkeit m_v[2] durchlaufen)
|
|
}
|
|
|
|
m_t0 = m_T; // Endzeit wird zur Startzeit
|
|
}
|
|
}
|
|
|
|
t_ma = m_T - t_start; // Entspricht Durchlaufzeit für den Master
|
|
m_t0 = t_start; // Setzt Startzeit zurück
|
|
|
|
/*-------------------------- Berechnung der Durchlaufzeit für das Slave-Tool -------------------------------*/
|
|
|
|
n = m_velocity_sl.size();
|
|
for(int i=0; i<n; i++) // Schleife über die (zusammenhängenden) Kurven
|
|
{
|
|
m = m_velocity_sl[i].size();
|
|
for(int j=0; j<m; j++) // Schleife über die einzelnen Unterteilungsgebiete
|
|
{
|
|
m_v[0] = m_velocity_sl[i][j][0]; // Startgeschwindigkeit des i-ten Abschnitts
|
|
m_v[1] = m_velocity_sl[i][j][1]; // maximale Geschwindigkeit des i-ten Abschnitts
|
|
m_v[2] = m_velocity_sl[i][j][2]; // Endgeschwindigkeit des i-ten Abschnitts
|
|
|
|
m_a = m_accel_sl[i][j];
|
|
|
|
length = m_length_sl[i][2*j]; // Länge des unkritischen Abschnitts
|
|
// Bem: Jede Kurve beginnt und endet in einem unkritischen Abschnitt
|
|
ParameterCalculation_Curve(length);// Hier wird u.a. die Endzeit <m_T> berechnet
|
|
|
|
if(j != m-1)
|
|
{
|
|
length = m_length_sl[i][2*j+1];// Länge des kritischen Abschnitts
|
|
m_T = m_T + length/m_v[2]; // Berechnet neue Endzeit (kritische Abschnitte werden mit der konstanten
|
|
// Geschwindigkeit <m_v[2]> durchlaufen)
|
|
}
|
|
|
|
m_t0 = m_T; // Endzeit wird zur Startzeit
|
|
}
|
|
}
|
|
|
|
t_sl = m_T - t_start; // Entspricht Durchlaufzeit für den Slave
|
|
m_t0 = t_start; // Setzt Startzeit zurück
|
|
|
|
/*----------------------- Synchronisierung von Master und Slave -----------------------------*/
|
|
|
|
// Idee: Skaliere die Geschwindigkeiten und Beschleunigungen derjenigen Bahn mit der kürzeren Durchlaufzeit,
|
|
// so dass die Durchlaufzeiten für Master und Slave übereinstimmen
|
|
|
|
|
|
if(t_ma <= t_sl)
|
|
{
|
|
// Ab hier: Korrektur für den Master
|
|
|
|
lam = t_ma/t_sl; // Skalierungsfaktor 0 < lam <= 1
|
|
|
|
// Zunächst werden alle Geschwindigkeiten runterskaliert
|
|
n = m_velocity_ma.size();
|
|
for(int i=0; i<n; i++)
|
|
{
|
|
m = m_velocity_ma[i].size();
|
|
for(int j=0; j<m; j++)
|
|
{
|
|
// linearen Beziehung zwischen den Geschwindigkeiten und der Durchlaufzeit
|
|
m_velocity_ma[i][j][0] = lam*m_velocity_ma[i][j][0];
|
|
m_velocity_ma[i][j][1] = lam*m_velocity_ma[i][j][1];
|
|
m_velocity_ma[i][j][2] = lam*m_velocity_ma[i][j][2];
|
|
}
|
|
}
|
|
|
|
// Skalierung der Beschleunigungen
|
|
n = m_accel_ma.size();
|
|
for(int i=0; i<n; i++)
|
|
{
|
|
m = m_accel_ma[i].size();
|
|
for(int j=0; j<m; j++)
|
|
m_accel_ma[i][j] = lam*lam*m_accel_ma[i][j]; // Die Beschleunigungen müssen mit dem Quadrat des Skalierungsfaktors
|
|
} // multipliziert werden (quadratische Abhängigkeit zur Durchlaufzeit)
|
|
}
|
|
else
|
|
{
|
|
// Ab hier: Korrektur für den Slave
|
|
|
|
lam = t_sl/t_ma; // Skalierungsfaktor 0 < lam <= 1
|
|
|
|
// Zunächst werden alle Geschwindigkeiten runterskaliert
|
|
n = m_velocity_sl.size();
|
|
for(int i=0; i<n; i++)
|
|
{
|
|
m = m_velocity_sl[i].size();
|
|
for(int j=0; j<m; j++)
|
|
{
|
|
// vgl. Master
|
|
m_velocity_sl[i][j][0] = lam*m_velocity_sl[i][j][0];
|
|
m_velocity_sl[i][j][1] = lam*m_velocity_sl[i][j][1];
|
|
m_velocity_sl[i][j][2] = lam*m_velocity_sl[i][j][2];
|
|
}
|
|
}
|
|
|
|
// Skalierung der Beschleunigungen
|
|
n = m_accel_sl.size();
|
|
for(int i=0; i<n; i++)
|
|
{
|
|
m = m_accel_sl[i].size();
|
|
for(int j=0; j<m; j++)
|
|
m_accel_sl[i][j] = lam*lam*m_accel_sl[i][j]; // Die Beschleunigungen müssen mit dem Quadrat des Skalierungsfaktors
|
|
} // multipliziert werden
|
|
}
|
|
}
|
|
|
|
/*------------------------------- Generierung der Outputvektoren ------------------------------------*/
|
|
|
|
/*MASTER*/
|
|
|
|
bool l;
|
|
int q,p;
|
|
|
|
n = m_length_ma.size();
|
|
for(int i=0; i<n; i++) // Schleife geht über die Anzahl der zu fahrenden Kurven, vgl. path_simulate::CompPath()
|
|
{
|
|
if(i!=0)
|
|
m_it1++;
|
|
|
|
l = false; // Legt fest ob wir uns gerade in einem kritischen Abschnitt befinden
|
|
q = 0; // Zu Beginn jeder Kurve auf Null
|
|
p = 0; // Zu Beginn jeder Kurve auf Null
|
|
|
|
m_StartParam[0] = (*m_it1)->FirstParameter(); // Setzt Startparameter der aktuellen Kurve
|
|
|
|
m = m_length_ma[i].size();
|
|
for(int j=0; j<m; j++)
|
|
{
|
|
if(l==false) // wird in jedem zweiten Schritt aufgerufen
|
|
{
|
|
m_a = m_accel_ma[i][p];
|
|
|
|
m_v[0] = m_velocity_ma[i][q][0];
|
|
m_v[1] = m_velocity_ma[i][q][1];
|
|
m_v[2] = m_velocity_ma[i][q][2];
|
|
|
|
p++;
|
|
q++;
|
|
}
|
|
|
|
MakePathSingle(0,m_length_ma[i][j],l,0); // Hier werden die Outputvektoren erzeugt
|
|
|
|
// Update der Variablen
|
|
m_StartParam[0] += m_length_ma[i][j];
|
|
m_t0 = m_T;
|
|
l = !l;
|
|
}
|
|
}
|
|
|
|
m_t0 = t_start;
|
|
|
|
/*ENDE MASTER*/
|
|
|
|
|
|
/*SLAVE*/
|
|
|
|
// Analog zum Master (s.o.)
|
|
if(m_single == false)
|
|
{
|
|
n = m_length_sl.size();
|
|
for(int i=0; i<n; i++)
|
|
{
|
|
if(i!=0)
|
|
m_it2++;
|
|
|
|
l = false;
|
|
q = 0;
|
|
p = 0;
|
|
m_StartParam[1] = (*m_it2)->FirstParameter();
|
|
m = m_length_sl[i].size();
|
|
|
|
// vgl. Master
|
|
for(int j=0; j<m; j++)
|
|
{
|
|
if(l==false)
|
|
{
|
|
m_a = m_accel_sl[i][p];
|
|
|
|
m_v[0] = m_velocity_sl[i][q][0];
|
|
m_v[1] = m_velocity_sl[i][q][1];
|
|
m_v[2] = m_velocity_sl[i][q][2];
|
|
|
|
p++;
|
|
q++;
|
|
}
|
|
|
|
MakePathSingle(0,m_length_sl[i][j],l,1); // hier wird der output für den slave generiert
|
|
m_StartParam[1] += m_length_sl[i][j];
|
|
m_t0 = m_T;
|
|
l = !l;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*ENDE SLAVE*/
|
|
|
|
|
|
/*WEG-KORREKTUR*/
|
|
|
|
t_tmp = m_t0;
|
|
|
|
m_StartPnts1[1] = (*m_it1)->EndPoint(); // Setzt Endpunkt, notwendig für die folgende Wegkorrektur
|
|
Correction(0); // Wegkorrektur für den Master
|
|
|
|
if(m_single == false)
|
|
{
|
|
m_t0 = t_tmp;
|
|
m_StartPnts2[1] = (*m_it2)->EndPoint(); // Setzt Endpunkt, notwendig für die folgende Wegkorrektur
|
|
Correction(1); // Wegkorrektur für den Slave
|
|
}
|
|
|
|
m_t0 = t_start;
|
|
|
|
/*ENDE WEG-KORREKTUR*/
|
|
|
|
|
|
m_velocity_ma.clear();
|
|
m_velocity_sl.clear();
|
|
m_length_ma.clear();
|
|
m_length_sl.clear();
|
|
m_accel_ma.clear();
|
|
m_accel_sl.clear();
|
|
|
|
return true;
|
|
}
|
|
|
|
/* Hier werden die Simulations-Outputvektoren für die jeweiligen Kurvenabschnitte erzeugt */
|
|
bool path_simulate::MakePathSingle(bool brob, // Beschreibt Ausgabeart (Roboter, Simulation)
|
|
double length, // Bogenlänge des betrachteten Abschnitts
|
|
bool part, // Gibt an ob wir einen kritischen Abschnitt betrachten
|
|
bool tool) // Tool (Master, Slave)
|
|
{
|
|
GeomAdaptor_Curve anAdaptorCurve;
|
|
double firstParam, lastParam, param, period, d, velo;
|
|
int N;
|
|
|
|
if(tool == false) anAdaptorCurve.Load(*m_it1); // Master
|
|
else anAdaptorCurve.Load(*m_it2); // Slave
|
|
|
|
firstParam = anAdaptorCurve.FirstParameter();
|
|
lastParam = anAdaptorCurve.LastParameter();
|
|
period = lastParam - firstParam;
|
|
|
|
gp_Vec dtmp1, dtmp2;
|
|
gp_Pnt tmp;
|
|
Base::Vector3d tmp2;
|
|
std::vector<Base::Vector3d> tmp3;
|
|
|
|
// Base::Vector3f pnt1,pnt2;
|
|
|
|
if(brob)
|
|
{
|
|
N = std::max(2, (int) ceil(period / double(TolDist)));
|
|
|
|
for (int i=1; i<N; ++i) // niemals den ersten punkt mitnehmen
|
|
{
|
|
if (m_StartParam[tool] + double(i)*period/(double(N)-1.0) > lastParam)
|
|
{
|
|
anAdaptorCurve.D0(m_StartParam[tool] + double(i)*period/(double(N)-1.0) - period, tmp);
|
|
}
|
|
else if (m_StartParam[tool] + double(i)*period/(double(N)-1.0) < firstParam )
|
|
{
|
|
anAdaptorCurve.D0(m_StartParam[tool] + double(i)*period/(double(N)-1.0) + period, tmp);
|
|
}
|
|
else
|
|
{
|
|
anAdaptorCurve.D0(m_StartParam[tool] + double(i)*period/(double(N)-1.0), tmp);
|
|
}
|
|
|
|
tmp2.x = tmp.X();
|
|
tmp2.y = tmp.Y();
|
|
tmp2.z = tmp.Z();
|
|
|
|
if (!tool)
|
|
{
|
|
m_Output_robo1.push_back(tmp2);
|
|
|
|
if (i==1)
|
|
{
|
|
RoboFlag_Master.pop_back();
|
|
RoboFlag_Master.push_back(1);
|
|
}
|
|
|
|
RoboFlag_Master.push_back(0);
|
|
}
|
|
else
|
|
{
|
|
m_Output_robo2.push_back(tmp2);
|
|
|
|
if (i==1)
|
|
{
|
|
RoboFlag_Slave.pop_back();
|
|
RoboFlag_Slave.push_back(1);
|
|
}
|
|
|
|
RoboFlag_Slave.push_back(0);
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
if (part==true) m_T = m_t0 + length/m_v[2]; // kritische Bereiche werden mit konstanter Geschwindigkeit durchlaufen
|
|
else ParameterCalculation_Curve(length);
|
|
|
|
N = std::max(2, (int)((m_T-m_t0)/m_step));
|
|
|
|
if (N>=100000)
|
|
N = 99999; // maximale Anzahl möglicher Outputwerte
|
|
|
|
m_del_t = (m_T-m_t0)/double(N);
|
|
|
|
std::vector< std::vector<Base::Vector3d> > D0;
|
|
std::vector< std::vector<Base::Vector3d> > D1;
|
|
|
|
double t = m_t0;
|
|
|
|
//anAdaptorCurve.D0(m_StartParam[tool],tmp);
|
|
//pnt2.Set(tmp.X(),tmp.Y(),tmp.Z());
|
|
|
|
if (part == true) // kritischer Abschnitt
|
|
{
|
|
for (int i=0; i<N; ++i) // Hauptschleife
|
|
{
|
|
if (!tool) m_Output_time.push_back(t);
|
|
else m_Output_time2.push_back(t);
|
|
|
|
d = m_v[2]*(t-m_t0);
|
|
param = m_StartParam[tool] + d; // Kurvenparameter entspricht zurückgelegtem Weg
|
|
// für den Fall einer Bogenlängenparametrisierung.
|
|
// Bei beliebiger Parametrisierung:
|
|
// param = FindParamAt(anAdaptorCurve, d, m_StartParam[tool]);
|
|
|
|
// Berechnet alles bis zur zweiten Ableitung
|
|
if ( param > lastParam ){ anAdaptorCurve.D2(param - period, tmp, dtmp1, dtmp2);}
|
|
else if ( param < firstParam ){ anAdaptorCurve.D2(param + period, tmp, dtmp1, dtmp2);}
|
|
else { anAdaptorCurve.D2(param, tmp, dtmp1, dtmp2);}
|
|
|
|
//m_times_tmp.push_back(t);
|
|
//m_velo_tmp.push_back(m_v[2]);
|
|
|
|
// Ausgabevektor zum Zeitpunkt <t>
|
|
tmp2.x = dtmp1.X()*m_v[2];
|
|
tmp2.y = dtmp1.Y()*m_v[2];
|
|
tmp2.z = dtmp1.Z()*m_v[2];
|
|
|
|
tmp3.push_back(tmp2);
|
|
|
|
if (!tool) m_Output.push_back(tmp3);
|
|
else m_Output2.push_back(tmp3);
|
|
|
|
tmp3.clear();
|
|
|
|
/*
|
|
if (tool == false) // zeichnet den tatsächlichen Weg nach
|
|
{
|
|
pnt1 = pnt2;
|
|
|
|
pnt2.x = pnt2.x + dtmp1.X()*m_v[2]*m_del_t;
|
|
pnt2.y = pnt2.y + dtmp1.Y()*m_v[2]*m_del_t;
|
|
pnt2.z = pnt2.z + dtmp1.Z()*m_v[2]*m_del_t;
|
|
|
|
if(i==0)
|
|
m_log.addSingleArrow(pnt1,pnt2,2,1,1,1);
|
|
else
|
|
m_log.addSingleArrow(pnt1,pnt2,2,1,0,0);
|
|
}
|
|
*/
|
|
|
|
t += m_del_t;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
// unkritischer Abschnitt
|
|
for (int i=0; i<N; ++i)
|
|
{
|
|
if (!tool) m_Output_time.push_back(t);
|
|
else m_Output_time2.push_back(t);
|
|
|
|
d = GetDistance(t);
|
|
param = m_StartParam[tool] + d; // Kurvenparameter entspricht zurückgelegtem Weg
|
|
// für den Fall einer Bogenlängenparametrisierung.
|
|
// Bei beliebiger Parametrisierung:
|
|
// param = FindParamAt(anAdaptorCurve, d, m_StartParam[tool]);
|
|
|
|
// Berechnet alles bis zur zweiten Ableitung
|
|
if ( param > lastParam ){ anAdaptorCurve.D2(param - period, tmp, dtmp1, dtmp2);}
|
|
else if ( param < firstParam ){ anAdaptorCurve.D2(param + period, tmp, dtmp1, dtmp2);}
|
|
else { anAdaptorCurve.D2(param, tmp, dtmp1, dtmp2);}
|
|
|
|
velo = GetVelocity(t); // Berechnet die Geschwindigkeit <velo> des Tools zum Zeitpunkt <t>
|
|
// bzgl. den Parametern m_t0, m_t1, m_t2, m_T, m_a, m_v[i], i =1,2,3
|
|
|
|
// Ausgabevektor zum Zeitpunkt <t>
|
|
tmp2.x = dtmp1.X()*velo;
|
|
tmp2.y = dtmp1.Y()*velo;
|
|
tmp2.z = dtmp1.Z()*velo;
|
|
|
|
tmp3.push_back(tmp2);
|
|
|
|
if (!tool) m_Output.push_back(tmp3);
|
|
else m_Output2.push_back(tmp3);
|
|
|
|
tmp3.clear();
|
|
|
|
/*
|
|
if (tool == false) // zeichnet den tatsächlichen Weg nach
|
|
{
|
|
pnt1 = pnt2;
|
|
|
|
pnt2.x = pnt2.x + dtmp1.X()*velo*m_del_t;
|
|
pnt2.y = pnt2.y + dtmp1.Y()*velo*m_del_t;
|
|
pnt2.z = pnt2.z + dtmp1.Z()*velo*m_del_t;
|
|
|
|
if(i==0)
|
|
m_log.addSingleArrow(pnt1,pnt2,2,1,1,1);
|
|
else
|
|
m_log.addSingleArrow(pnt1,pnt2,2,1,0,0);
|
|
}
|
|
*/
|
|
|
|
//m_times_tmp.push_back(t);
|
|
//m_velo_tmp.push_back(velo);
|
|
|
|
t += m_del_t;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/* Hauptroutine zur Generierung des Roboteroutputs für den normalen beidseitigen Fall */
|
|
bool path_simulate::MakePathRobot()
|
|
{
|
|
ofstream anOutputFile;
|
|
ofstream anOutputFile2;
|
|
|
|
anOutputFile.open("output_master.k");
|
|
anOutputFile.precision(7);
|
|
|
|
anOutputFile << "none" << std::endl;
|
|
|
|
if (m_single == false)
|
|
{
|
|
anOutputFile2.open("output_slave.k");
|
|
anOutputFile2.precision(7);
|
|
|
|
anOutputFile2 << "none" << std::endl;
|
|
}
|
|
|
|
// Outputgenerierung über alle Kurven
|
|
for (m_it1 = m_BSplineTop.begin(); m_it1 != m_BSplineTop.end(); ++m_it1)
|
|
{
|
|
m_StartParam[0] = ((*m_it1)->FirstParameter());
|
|
if (m_single == false) m_StartParam[1] = ((*m_it2)->FirstParameter());
|
|
|
|
/*------ 1.ZUSTELLUNG ------*/
|
|
|
|
m_conn = CheckConnect();
|
|
|
|
if (m_conn) ConnectPaths_xy(1);
|
|
else ConnectPaths_z(1);
|
|
|
|
UpdateParam();
|
|
|
|
|
|
/*------ 2.ZUSTELLUNG ------*/
|
|
|
|
if (m_conn) ConnectPaths_z(1);
|
|
else ConnectPaths_xy(1);
|
|
|
|
UpdateParam();
|
|
|
|
|
|
/*------ KURVE ------*/
|
|
|
|
MakePathSingle(1,0,0,0); // Master
|
|
MakePathSingle(1,0,0,1); // Slave
|
|
UpdateParam();
|
|
|
|
if (m_single==false && (m_it1 != (m_BSplineTop.end()-1)))
|
|
++m_it2;
|
|
|
|
}
|
|
|
|
int c = 1;
|
|
|
|
/*--- Schreibe Roboter-Output ---*/
|
|
|
|
if (m_single==false)
|
|
{
|
|
WriteOutputDouble(anOutputFile,anOutputFile2,c,c,1,beam);
|
|
anOutputFile2 << "*END" << endl;
|
|
anOutputFile2.close();
|
|
}
|
|
else
|
|
{
|
|
WriteOutputSingle(anOutputFile,c,1,0,beam);
|
|
}
|
|
|
|
anOutputFile << "*END" << endl;
|
|
anOutputFile.close();
|
|
|
|
return true;
|
|
}
|
|
|
|
/* Wird nur zu Beginn der Ausschreibung in path_simulate::WriteOurputDouble() aufgerufen und passt die Ausgabevektoren
|
|
<m_Output_time>, <m_Output_time2> so aneinander an, dass die Endzeiten übereinstimmen*/
|
|
bool path_simulate::TimeCorrection()
|
|
{
|
|
int N;
|
|
Base::Vector3d vec(0.0,0.0,0.0);
|
|
std::vector<Base::Vector3d> vecc;
|
|
|
|
vecc.push_back(vec);
|
|
|
|
if (m_single == false) // Eine Zeitkorrektur macht nur für diesen Fall auch Sinn
|
|
{
|
|
if(m_Output_time.size() == 0 || m_Output_time2.size() == 0) // Sonderbehandlung für diesen Fall
|
|
{
|
|
if(m_Output_time.size() > m_Output_time2.size())
|
|
{
|
|
m_Output_time2 = m_Output_time;
|
|
N = m_Output_time2.size();
|
|
m_Output2.resize(N);
|
|
|
|
for (int i=0; i<N; ++i) // Füllt leeren Output mit Nullvektoren (Werkzeug hat zu warten)
|
|
{
|
|
m_Output2[i] = vecc;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
else if(m_Output_time2.size() > m_Output_time.size())
|
|
{
|
|
m_Output_time = m_Output_time2;
|
|
N = m_Output_time.size();
|
|
m_Output.resize(N);
|
|
|
|
for (int i=0; i<N; ++i) // Füllt leeren Output mit Nullvektoren (-> Werkzeug hat zu warten)
|
|
{
|
|
m_Output[i] = vecc;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
return true; //gibt true zurück wenn beide Outputvektoren <m_Output_time> und <m_Output_time2> leer sein sollten
|
|
}
|
|
|
|
if (m_Output_time[m_Output_time.size()-1] < m_Output_time2[m_Output_time2.size()-1])
|
|
{
|
|
m_T = m_Output_time2[m_Output_time2.size()-1];
|
|
|
|
N = std::max(1,int(ceil((m_Output_time2[m_Output_time2.size()-1] - m_Output_time[m_Output_time.size()-1])/ m_step)));
|
|
m_del_t = (m_Output_time2[m_Output_time2.size()-1] - m_Output_time[m_Output_time.size()-1])/double(N);
|
|
|
|
int ind = m_Output_time.size()-1;
|
|
double time = 0.0;
|
|
|
|
for (int i=1; i<N; ++i)
|
|
{
|
|
time += m_del_t;
|
|
m_Output_time.push_back(m_Output_time[ind] + time);
|
|
m_Output.push_back(vecc);
|
|
}
|
|
|
|
m_Output_time.push_back(m_Output_time2[m_Output_time2.size()-1]);
|
|
m_Output.push_back(vecc);
|
|
}
|
|
else if (m_Output_time[m_Output_time.size()-1] > m_Output_time2[m_Output_time2.size()-1])
|
|
{
|
|
m_T = m_Output_time[m_Output_time.size()-1];
|
|
|
|
N = std::max(1,int(ceil((m_Output_time[m_Output_time.size()-1] - m_Output_time2[m_Output_time2.size()-1])/ m_step)));
|
|
m_del_t = (m_Output_time[m_Output_time.size()-1] - m_Output_time2[m_Output_time2.size()-1])/double(N);
|
|
|
|
int ind = m_Output_time2.size()-1;
|
|
double time = 0.0;
|
|
|
|
for (int i=1; i<N; ++i)
|
|
{
|
|
time += m_del_t;
|
|
m_Output_time2.push_back(m_Output_time2[ind] + time);
|
|
m_Output2.push_back(vecc);
|
|
}
|
|
|
|
m_Output_time2.push_back(m_Output_time[m_Output_time.size()-1]);
|
|
m_Output2.push_back(vecc);
|
|
}
|
|
}
|
|
else // falls <m_single> = true (d.h nur Master -> keine Zeitkorrektur erforderlich!!!)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/* Hauptroutine zur Generierung des Simulationsoutputs für den feature-basierten und spiral-basierten beidseitigen Fall */
|
|
bool path_simulate::MakePathSimulate_Feat(const std::vector<float> &flatAreas, bool spiral)
|
|
{
|
|
m_Feat = true;
|
|
|
|
double rad[2];
|
|
bool tool;
|
|
int c[2];
|
|
|
|
gp_Pnt pnt0, pnt1;
|
|
|
|
std::vector<Handle_Geom_BSplineCurve>::iterator *it_1,*it_2;
|
|
std::vector<Handle_Geom_BSplineCurve> *curves_1, *curves_2;
|
|
|
|
ofstream anOutputFile[2];
|
|
|
|
anOutputFile[0].open("output_master.k");
|
|
anOutputFile[1].open("output_slave.k");
|
|
anOutputFile[0].precision(7);
|
|
anOutputFile[1].precision(7);
|
|
|
|
m_it1 = m_BSplineTop.begin();
|
|
m_it2 = m_BSplineBottom.begin();
|
|
|
|
rad[0] = m_set.master_radius;
|
|
rad[1] = - m_set.slave_radius - m_set.sheet_thickness;
|
|
|
|
c[0] = 1; // Start Index der Master Kurven
|
|
c[1] = 2001; // Start Index der Slave Kurven
|
|
|
|
int i = 0;
|
|
|
|
while (m_it1 != m_BSplineTop.end() && m_it2 != m_BSplineBottom.end())
|
|
{
|
|
tool = StartingTool(); // bestimmt welches tool warten muss:
|
|
// tool == true : Roboter = Slave & NC = Master
|
|
// tool == fasle : Roboter = Master & NC = Slave
|
|
if (!tool)
|
|
{
|
|
it_1 = &m_it1;
|
|
it_2 = &m_it2;
|
|
curves_1 = &m_BSplineTop;
|
|
curves_2 = &m_BSplineBottom;
|
|
}
|
|
else
|
|
{
|
|
it_1 = &m_it2;
|
|
it_2 = &m_it1;
|
|
curves_1 = &m_BSplineBottom;
|
|
curves_2 = &m_BSplineTop;
|
|
}
|
|
|
|
m_StartParam[0] = ((*m_it1)->FirstParameter()); // setze neue Startparameter (in unserem Fall immer 0)
|
|
|
|
if (m_single == false)
|
|
m_StartParam[1] = ((*m_it2)->FirstParameter());
|
|
|
|
// die erste Zustellung vor dem Kontakt mit dem Blech wird hier seperat gehandelt
|
|
if (i==0)
|
|
{
|
|
/*------ ZUSTELLUNG 1 ------*/
|
|
ConnectPaths_xy(0);
|
|
WriteOutputDouble(anOutputFile[0],anOutputFile[1],c[0],c[1],0,beam);
|
|
UpdateParam();
|
|
|
|
/*------ ZUSTELLUNG 2 ------*/
|
|
ConnectPaths_z(0);
|
|
WriteOutputDouble(anOutputFile[0],anOutputFile[1],c[0],c[1],0,beam);
|
|
UpdateParam();
|
|
}
|
|
|
|
while (true)
|
|
{
|
|
if (*it_1 != (*curves_1).end()-1)
|
|
{
|
|
/* ------ Kurve ------*/
|
|
CompPath(0); // Berechne Parameter für den Master
|
|
CompPath(1); // Berechne Parameter für den Slave
|
|
Gen_Path(); // Erzeuge Output für aktuelle Kurve
|
|
WriteOutputDouble(anOutputFile[0],anOutputFile[1],c[0],c[1],0,beam); // Schreibe Output
|
|
UpdateParam();
|
|
|
|
/*------ Zustellung ------*/
|
|
(*it_1)++; // Gehe zur nächsten Kurve
|
|
ConnectPaths_Feat(tool, 0, 1); // Erzeuge Output für Zustellung
|
|
WriteOutputDouble(anOutputFile[0],anOutputFile[1],c[0],c[1],0,beam); // Schreibe Output
|
|
UpdateParam();
|
|
|
|
if (*it_1 == (*curves_1).end()-1 && *it_2 == (*curves_2).end()-1) // Letzter Schritt
|
|
{
|
|
/* ------ Kurve ------*/
|
|
CompPath(0); // Berechne Parameter für den Master
|
|
CompPath(1); // Berechne Parameter für den Slave
|
|
Gen_Path(); // Erzeuge Output für aktuelle Kurve
|
|
WriteOutputDouble(anOutputFile[0],anOutputFile[1],c[0],c[1],0,beam); // Schreibe Output
|
|
UpdateParam();
|
|
break;
|
|
}
|
|
|
|
(**it_1)->D0((**it_1)->FirstParameter(),pnt0); // übergebe aktuellen Startpunkt
|
|
|
|
|
|
if ((pnt0.Z() > (flatAreas[i+1] + rad[tool] - 1e-1)) && // Erreicht der MASTER den nächsten ebenen
|
|
(pnt0.Z() < (flatAreas[i+1] + rad[tool] + 1e-1))) // Bereich, muss auf den SLAVE gewartet werden
|
|
{
|
|
if(!spiral) // Bei Spiralbahnen erfolgt die Zustellung sofort
|
|
{
|
|
/* ------ Kurve ------*/
|
|
CompPath(0); // Berechne Parameter für den Master
|
|
CompPath(1); // Berechne Parameter für den Slave
|
|
Gen_Path(); // Erzeuge Output für aktuelle Kurve
|
|
WriteOutputDouble(anOutputFile[0],anOutputFile[1],c[0],c[1],0,beam); // Schreibe Output
|
|
UpdateParam();
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (*it_2 == (*curves_2).end()-1) // letzter Schritt
|
|
{
|
|
/* ------ Kurve ------*/
|
|
CompPath(0); // Berechne Parameter für den Master
|
|
CompPath(1); // Berechne Parameter für den Slave
|
|
Gen_Path(); // Erzeuge Output für aktuelle Kurve
|
|
WriteOutputDouble(anOutputFile[0],anOutputFile[1],c[0],c[1],0,beam); // Schreibe Output
|
|
UpdateParam();
|
|
break;
|
|
}
|
|
else break;
|
|
}
|
|
}
|
|
|
|
if (m_it1 == m_BSplineTop.end()-1 && m_it2 == m_BSplineBottom.end()-1) // Fertig !!!
|
|
break;
|
|
|
|
/* ------ Zustellung ------*/
|
|
if(!spiral)
|
|
{
|
|
(*it_1)++; // gehe zur nächsten Kurve
|
|
ConnectPaths_Feat(tool, 0, 1); // Erzeuge Output für die Master-Zustellung
|
|
}
|
|
|
|
(*it_2)++; // gehe zur nächsten Kurve
|
|
ConnectPaths_Feat(!tool, 0, 0); // Erzeuge Output für die Slave-Zustellung
|
|
WriteOutputDouble(anOutputFile[0],anOutputFile[1],c[0],c[1],0,beam); // Schreibe Output
|
|
UpdateParam();
|
|
|
|
++i;
|
|
}
|
|
|
|
anOutputFile[0] << "*END" << endl;
|
|
anOutputFile[1] << "*END" << endl;
|
|
anOutputFile[0].close();
|
|
anOutputFile[1].close();
|
|
|
|
//WriteTimes();
|
|
|
|
return true;
|
|
}
|
|
|
|
/* Hauptroutine zur Generierung des Roboteroutputs für den feature-basierten und spiral-basierten beidseitigen Fall */
|
|
bool path_simulate::MakePathRobot_Feat(const std::vector<float> &flatAreas)
|
|
{
|
|
m_Feat = true;
|
|
int f = 0, run;
|
|
bool tool;
|
|
double rad[2];
|
|
gp_Pnt pnt0, pnt1;
|
|
|
|
std::vector<Handle_Geom_BSplineCurve>::iterator *it_1,*it_2;
|
|
std::vector<Handle_Geom_BSplineCurve> *curves_1, *curves_2;
|
|
|
|
ofstream anOutputFile[2];
|
|
|
|
std::stringstream master_file, slave_file;
|
|
|
|
m_it1 = m_BSplineTop.begin();
|
|
m_it2 = m_BSplineBottom.begin();
|
|
|
|
rad[0] = m_set.master_radius;
|
|
rad[1] = - m_set.slave_radius - m_set.sheet_thickness;
|
|
|
|
int i = 0;
|
|
|
|
while (m_it1 != m_BSplineTop.end() && m_it2 != m_BSplineBottom.end())
|
|
{
|
|
tool = StartingTool(); // Bestimmt welches tool warten muss
|
|
|
|
// Setze Startparameter (in unserem Fall unnötig da immer 0)
|
|
m_StartParam[0] = ((*m_it1)->FirstParameter());
|
|
if (m_single == false) m_StartParam[1] = ((*m_it2)->FirstParameter());
|
|
|
|
// Erste Zustellung wird seperat gehandelt
|
|
if (i==0)
|
|
{
|
|
ConnectPaths_xy(1); /* 1. ZUSTELLUNG */
|
|
ConnectPaths_z(1); /* 2. ZUSTELLUNG */
|
|
}
|
|
|
|
if (!tool)
|
|
{
|
|
it_1 = &m_it1;
|
|
it_2 = &m_it2;
|
|
curves_1 = &m_BSplineTop;
|
|
curves_2 = &m_BSplineBottom;
|
|
}
|
|
else
|
|
{
|
|
it_1 = &m_it2;
|
|
it_2 = &m_it1;
|
|
curves_1 = &m_BSplineBottom;
|
|
curves_2 = &m_BSplineTop;
|
|
}
|
|
|
|
// Hauptschleife
|
|
while (true)
|
|
{
|
|
MakePathSingle(1,0,0,tool); // Bahngenerierung des kontinuierlich fahrenden tools für aktuelle kurve
|
|
|
|
// MASTER
|
|
if (*it_1 != (*curves_1).end()-1)
|
|
{
|
|
(*it_1)++;
|
|
(**it_1)->D0((**it_1)->FirstParameter(),pnt0);
|
|
|
|
if (*it_1 == (*curves_1).end()-1 && *it_2 == (*curves_2).end()-1)
|
|
{
|
|
ConnectPaths_Feat(tool, 1, 1);
|
|
MakePathSingle(1,0,0,tool);
|
|
break;
|
|
}
|
|
|
|
if ((pnt0.Z() > (flatAreas[i+1] + rad[tool] - 1e-1)) &&
|
|
(pnt0.Z() < (flatAreas[i+1] + rad[tool] + 1e-1)))
|
|
{
|
|
|
|
run = Detect_FeatCurve(tool);
|
|
|
|
for (int i=0; i<run; ++i)
|
|
{
|
|
ConnectPaths_Feat(tool, 1, 1);
|
|
MakePathSingle(1,0,0,tool);
|
|
++(*it_1);
|
|
}
|
|
|
|
break;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (*it_2 == (*curves_2).end()-1)
|
|
{
|
|
ConnectPaths_Feat(tool, 1, 1);
|
|
MakePathSingle(1,0,0,tool);
|
|
break;
|
|
}
|
|
else break;
|
|
}
|
|
|
|
ConnectPaths_Feat(tool, 1, 1);
|
|
}
|
|
|
|
(**it_2)->D0((**it_2)->FirstParameter(),pnt0);
|
|
|
|
while ((pnt0.Z() > (flatAreas[i] + rad[!tool] - 1e-1)) &&
|
|
(pnt0.Z() < (flatAreas[i] + rad[!tool] + 1e-1)))
|
|
{
|
|
MakePathSingle(1,0,0,!tool);
|
|
|
|
if (*it_2 != (*curves_2).end()-1)
|
|
(*it_2)++;
|
|
else
|
|
break;
|
|
|
|
(**it_2)->D0((**it_2)->FirstParameter(),pnt0);
|
|
}
|
|
|
|
master_file << "output_master" << f << ".k";
|
|
slave_file << "output_slave" << f << ".k";
|
|
++f;
|
|
|
|
std::cout << master_file.str() << " , " << slave_file << std::endl;
|
|
|
|
anOutputFile[0].open((master_file.str()).c_str());
|
|
anOutputFile[1].open((slave_file.str()).c_str());
|
|
|
|
if (!tool)
|
|
{
|
|
anOutputFile[0] << "none" << std::endl;
|
|
anOutputFile[1] << "continuous" << std::endl;
|
|
}
|
|
else
|
|
{
|
|
anOutputFile[0] << "continuous" << std::endl;
|
|
anOutputFile[1] << "none" << std::endl;
|
|
}
|
|
|
|
WriteOutput_Feat(anOutputFile[0], anOutputFile[1],f,1);
|
|
|
|
master_file.str("");
|
|
master_file.clear();
|
|
slave_file.str("");
|
|
slave_file.clear();
|
|
|
|
m_Output_robo1.clear();
|
|
m_Output_robo2.clear();
|
|
|
|
if (*it_1 == (*curves_1).end()-1 && *it_2 == (*curves_2).end()-1)
|
|
break;
|
|
|
|
ConnectPaths_Feat(0, 1, 1);
|
|
ConnectPaths_Feat(1, 1, 0);
|
|
++i;
|
|
}
|
|
|
|
anOutputFile[0] << "*END" << endl;
|
|
anOutputFile[1] << "*END" << endl;
|
|
anOutputFile[0].close();
|
|
anOutputFile[1].close();
|
|
|
|
return true;
|
|
}
|
|
|
|
// Hilfsfunktion für den Featurebasierten Teil:
|
|
// Bestimmt welches Tool wartet, während das andere läuft
|
|
bool path_simulate::StartingTool()
|
|
{
|
|
double z0,z1;
|
|
gp_Pnt pnt0,pnt1;
|
|
|
|
if(m_it1 != m_BSplineTop.end()-1)
|
|
{
|
|
// z-Abstand zur nächsten Bahn - MASTER
|
|
(*m_it1)->D0((*m_it1)->FirstParameter(),pnt0); m_it1++;
|
|
(*m_it1)->D0((*m_it1)->FirstParameter(),pnt1); m_it1--;
|
|
|
|
z0 = abs(pnt0.Z() - pnt1.Z());
|
|
}
|
|
|
|
else
|
|
z0 = 1e+3;
|
|
|
|
if(m_it2 != m_BSplineBottom.end()-1)
|
|
{
|
|
// z-Abstand zur nächsten Bahn - SLAVE
|
|
(*m_it2)->D0((*m_it2)->FirstParameter(),pnt0); m_it2++;
|
|
(*m_it2)->D0((*m_it2)->FirstParameter(),pnt1); m_it2--;
|
|
|
|
z1 = abs(pnt0.Z() - pnt1.Z());
|
|
}
|
|
else z1 = 1e+3;
|
|
|
|
if(z0<z1) return false; // Slave muss warten
|
|
else return true; // Master muss warten
|
|
}
|
|
|
|
/* Hier wird geschaut, ob es sich um eine geschlossene Kurve handelt bzw. wieviele Kurven abgefahren werden müssen
|
|
um wieder am Anfangspunkt der Kurve anzukommen. Die Anzahl der zu fahrenden Kurven wird als integer zurückgegeben */
|
|
int path_simulate::Detect_FeatCurve(bool tool)
|
|
{
|
|
gp_Pnt pt0,pt1;
|
|
int num = 1;
|
|
|
|
if(!tool)
|
|
{
|
|
pt0 = (*m_it1)->StartPoint(); // Startpunkt der aktuellen Master-Kurve
|
|
pt1 = (*m_it1)->EndPoint(); // Endpunkt der aktuellen Master-Kurve
|
|
|
|
while(pt0.Distance(pt1) > 1e-3) // Mache weiter solange Start und Endpunkt nicht passen
|
|
{
|
|
if(m_it1 == m_BSplineTop.end()-1) // Stoppe wenn bei der letzten Kurve angekommen
|
|
{
|
|
for(int i=1; i<num; i++)
|
|
m_it1--;
|
|
|
|
num = 1;
|
|
pt1 = pt0; // Damit die äußere while-Schleife verlassen wird
|
|
}
|
|
else
|
|
{
|
|
num++;
|
|
m_it1++;
|
|
pt1 = (*m_it1)->EndPoint();
|
|
}
|
|
}
|
|
|
|
// Zurück zum Status quo
|
|
for(int i=1; i<num; i++)
|
|
m_it1--;
|
|
}
|
|
else
|
|
{
|
|
pt0 = (*m_it2)->StartPoint(); // Startpunkt der aktuellen Slave-Kurve
|
|
pt1 = (*m_it2)->EndPoint(); // Endpunkt der aktuellen Slave-Kurve
|
|
|
|
while(pt0.Distance(pt1) > 1e-3) // Mache weiter solange Start und Endpunkt nicht passen
|
|
{
|
|
if(m_it2 == m_BSplineBottom.end()-1) // Stoppe wenn bei der letzten Kurve angekommen
|
|
{
|
|
for(int i=1; i<num; i++)
|
|
m_it2--;
|
|
|
|
num = 1;
|
|
pt1 = pt0; // Damit die while-Schleife verlassen wird
|
|
}
|
|
else
|
|
{
|
|
num++;
|
|
m_it2++;
|
|
pt1 = (*m_it2)->EndPoint();
|
|
}
|
|
}
|
|
|
|
// Zurück zum Status quo
|
|
for(int i=1; i<num; i++)
|
|
m_it2--;
|
|
}
|
|
|
|
return num;
|
|
}
|
|
|
|
/* Hauptroutine zur Generierung des Simulationsoutputs für den normalen beidseitigen Fall */
|
|
bool path_simulate::MakePathSimulate()
|
|
{
|
|
int c1 = 1,
|
|
c2 = 2001;
|
|
|
|
ofstream anOutputFile, anOutputFile2;
|
|
|
|
anOutputFile.open("output_master.k");
|
|
anOutputFile.precision(7);
|
|
|
|
if (m_single == false)
|
|
{
|
|
anOutputFile2.open("output_slave.k");
|
|
anOutputFile2.precision(7);
|
|
}
|
|
|
|
for (m_it1 = m_BSplineTop.begin(); m_it1 < m_BSplineTop.end(); ++m_it1) // Schleife über alle Kurven
|
|
{
|
|
m_StartParam[0] = ((*m_it1)->FirstParameter()); // speichert Startparameterwert der aktuellen Master-Kurve
|
|
|
|
if (m_single == false)
|
|
m_StartParam[1] = ((*m_it2)->FirstParameter()); // speichert Startparameterwert der aktuellen Slave-Kurve
|
|
|
|
|
|
/*Zustellung Start*/
|
|
|
|
m_conn = CheckConnect(); // Rückgabewert = 1 bei negativer z-Richtungszustellung
|
|
// Rückgabewert = 0 bei positiver z-Richtungszustellung
|
|
|
|
// negative z-Richtung: 1. XY --> 2. Z
|
|
// positive z-Richtung: 1. Z --> 2. XY
|
|
|
|
// *** 1. ***
|
|
if (m_conn) ConnectPaths_xy(0);
|
|
else ConnectPaths_z(0);
|
|
|
|
if (m_single == false) WriteOutputDouble(anOutputFile,anOutputFile2,c1,c2,0,beam);
|
|
else WriteOutputSingle(anOutputFile,c1,0,0,beam);
|
|
|
|
UpdateParam();
|
|
|
|
// *** 2. ***
|
|
if (m_conn) ConnectPaths_z(0);
|
|
else ConnectPaths_xy(0);
|
|
|
|
// Schreibe Output
|
|
if (m_single == false) WriteOutputDouble(anOutputFile,anOutputFile2,c1,c2,0,beam);
|
|
else WriteOutputSingle(anOutputFile,c1,0,0,beam);
|
|
|
|
UpdateParam();
|
|
|
|
/*Zustellung Ende*
|
|
|
|
|
|
/*Kurve Start*/
|
|
|
|
CompPath(0); // Berechnung der Parameter für den Master
|
|
|
|
if (m_single == false)
|
|
CompPath(1); // Berechnung der Parameter für den Slave
|
|
|
|
Gen_Path(); // Erzeugung der Outputvektoren
|
|
|
|
// Schreibe Outputvektoren
|
|
if (m_single == false) WriteOutputDouble(anOutputFile,anOutputFile2,c1,c2,0,beam);
|
|
else WriteOutputSingle(anOutputFile,c1,0,0,beam);
|
|
|
|
UpdateParam();
|
|
|
|
if (m_single==false && (m_it1 != (m_BSplineTop.end()-1)))
|
|
++m_it2;
|
|
|
|
/*Kurve Ende*/
|
|
}
|
|
|
|
//m_log.saveToFile("c:/Master-Path.iv");
|
|
|
|
/*
|
|
ofstream anOutputvelocity;
|
|
anOutputvelocity.open("output_velocity.k");
|
|
anOutputvelocity.precision(7);
|
|
|
|
for(int i=0; i<(int)m_times_tmp.size(); ++i) // Schreibe absolute Geschwindigkeitswerte aus
|
|
anOutputvelocity << m_times_tmp[i] << ", " << m_velo_tmp[i] << endl;
|
|
|
|
anOutputvelocity.close();
|
|
*/
|
|
|
|
anOutputFile << "*END" << endl;
|
|
anOutputFile.close();
|
|
|
|
|
|
if (m_single == false)
|
|
{
|
|
anOutputFile2 << "*END" << endl;
|
|
anOutputFile2.close();
|
|
}
|
|
|
|
// WriteTimes();
|
|
return true;
|
|
}
|
|
|
|
/* Schreibt den Output für den feature- und spiral-basierten beidseitigen Fall */
|
|
bool path_simulate::WriteOutput_Feat(ofstream &anOutputFile, ofstream &anOutputFile2, int &c, bool brob)
|
|
{
|
|
if (m_single == false)
|
|
{
|
|
if (brob == false)
|
|
{
|
|
int n = m_Output.size();
|
|
|
|
anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile << "2,1,0," << c << ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
|
|
anOutputFile << "*DEFINE_CURVE" << std::endl << c << std::endl;
|
|
|
|
for (int i=0; i<n; ++i)
|
|
{
|
|
anOutputFile << m_Output_time[i] - m_Output_time[0]<<"," << m_Output[i][0].x << std::endl;
|
|
}
|
|
|
|
anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][0].x<< std::endl;
|
|
|
|
anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile << "3,1,0," << c << ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
|
|
anOutputFile << "*DEFINE_CURVE" << std::endl << c+1 << std::endl;
|
|
|
|
for (int i=0; i<n; ++i)
|
|
{
|
|
anOutputFile << m_Output_time[i] - m_Output_time[0]<<"," << m_Output[i][1].x << std::endl;
|
|
}
|
|
|
|
anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][1].x<< std::endl;
|
|
|
|
anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile << "2,2,0," << c+1 << ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
|
|
anOutputFile << "*DEFINE_CURVE" << std::endl << c+2 << std::endl;
|
|
|
|
for (int i=0; i<n; ++i)
|
|
{
|
|
anOutputFile << m_Output_time[i] - m_Output_time[0]<< "," << m_Output[i][0].y<< std::endl;
|
|
}
|
|
|
|
anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][0].y<< std::endl;
|
|
|
|
|
|
anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile << "3,2,0," << c+1 << ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
|
|
anOutputFile << "*DEFINE_CURVE" << std::endl << c+3 << std::endl;
|
|
|
|
for (int i=0; i<n; ++i)
|
|
{
|
|
anOutputFile << m_Output_time[i] - m_Output_time[0]<< "," << m_Output[i][1].y<< std::endl;
|
|
}
|
|
|
|
anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][1].y<< std::endl;
|
|
|
|
|
|
anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile << "2,3,0," << c+2 << ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
|
|
anOutputFile << "*DEFINE_CURVE" << std::endl << c+4 << std::endl;
|
|
|
|
for (int i=0; i<n; ++i)
|
|
{
|
|
anOutputFile << m_Output_time[i] - m_Output_time[0]<<"," << m_Output[i][0].z<< std::endl;
|
|
}
|
|
|
|
anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][0].z<< std::endl;
|
|
|
|
|
|
anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile << "3,3,0," << c+2 << ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
|
|
anOutputFile << "*DEFINE_CURVE" << std::endl << c+5 << std::endl;
|
|
|
|
for (int i=0; i<n; ++i)
|
|
{
|
|
anOutputFile << m_Output_time[i] - m_Output_time[0]<<"," << m_Output[i][1].z<< std::endl;
|
|
}
|
|
|
|
anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][1].z<< std::endl;
|
|
|
|
c = c+6;
|
|
}
|
|
else
|
|
{
|
|
int n1 = m_Output_robo1.size();
|
|
for (int i=0; i<n1; ++i)
|
|
anOutputFile << m_Output_robo1[i].x + m_set.x_offset_robot << "," << m_Output_robo1[i].y + m_set.y_offset_robot << "," << m_Output_robo1[i].z << endl;
|
|
|
|
anOutputFile.close();
|
|
|
|
int n2 = m_Output_robo2.size();
|
|
for (int i=0; i<n2; ++i)
|
|
anOutputFile2 << m_Output_robo2[i].x + m_set.x_offset_robot << "," << m_Output_robo2[i].y + m_set.y_offset_robot << "," << m_Output_robo2[i].z << endl;
|
|
|
|
anOutputFile2.close();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (brob == false)
|
|
{
|
|
|
|
int n = m_Output.size();
|
|
|
|
anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile << "2,1,0," << c << ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
|
|
anOutputFile << "*DEFINE_CURVE" << std::endl << c << std::endl;
|
|
|
|
for (int i=0; i<n; ++i)
|
|
{
|
|
anOutputFile << m_Output_time[i] - m_Output_time[0]<<"," << m_Output[i][0].x << std::endl;
|
|
}
|
|
|
|
anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][0].x<< std::endl;
|
|
|
|
anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile << "2,2,0," << c+1 << ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
|
|
anOutputFile << "*DEFINE_CURVE" << std::endl << c+1 << std::endl;
|
|
|
|
for (int i=0; i<n; ++i)
|
|
{
|
|
anOutputFile << m_Output_time[i] - m_Output_time[0]<< "," << m_Output[i][0].y<< std::endl;
|
|
}
|
|
|
|
anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][0].y<< std::endl;
|
|
|
|
anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile << "2,3,0," << c+2 << ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
|
|
anOutputFile << "*DEFINE_CURVE" << std::endl << c+4 << std::endl;
|
|
|
|
for (int i=0; i<n; ++i)
|
|
{
|
|
anOutputFile << m_Output_time[i] - m_Output_time[0]<<"," << m_Output[i][0].z<< std::endl;
|
|
}
|
|
|
|
anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][0].z<< std::endl;
|
|
|
|
c = c+3;
|
|
}
|
|
else
|
|
{
|
|
int n = m_Output_robo1.size();
|
|
|
|
for (int i=0; i<n; ++i)
|
|
anOutputFile << m_Output_robo1[i].x << "," << m_Output_robo1[i].y << "," << m_Output_robo1[i].z << ",";
|
|
|
|
anOutputFile << std::endl;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
// Schreibt alle Zeitwerte der Ausgabevektoren aus
|
|
bool path_simulate::WriteTimes()
|
|
{
|
|
ofstream anOutputFile;
|
|
|
|
anOutputFile.open("CurveTimes.k");
|
|
anOutputFile.precision(7);
|
|
|
|
for (unsigned int i=0; i< m_PathTimes_Master.size(); ++i)
|
|
{
|
|
anOutputFile << m_PathTimes_Master[i].first << " " << m_PathTimes_Master[i].second << std::endl;
|
|
}
|
|
|
|
for (unsigned int i=0; i< m_PathTimes_Slave.size(); ++i)
|
|
{
|
|
anOutputFile << m_PathTimes_Slave[i].first << " " << m_PathTimes_Slave[i].second << std::endl;
|
|
}
|
|
|
|
anOutputFile.close();
|
|
|
|
return true;
|
|
}
|
|
*/
|
|
|
|
/* Schreibt Output für den normalen einseitigen Fall */
|
|
bool path_simulate::WriteOutputSingle(ofstream &anOutputFile, int &c, bool brob, bool tool, bool beamfl)
|
|
{
|
|
std::vector< std::vector<Base::Vector3d> > Out_val;
|
|
std::vector<double> Out_time;
|
|
std::pair<float,float> times;
|
|
int n;
|
|
int ind;
|
|
|
|
int pid;
|
|
|
|
if (brob == true) // Schreibe Roboter-Output
|
|
{
|
|
std::vector<Base::Vector3d> Out_rob;
|
|
|
|
if (!tool) Out_rob = m_Output_robo1;
|
|
else Out_rob = m_Output_robo2;
|
|
|
|
n = Out_rob.size();
|
|
|
|
for (int i=0; i<n; ++i)
|
|
anOutputFile << Out_rob[i].x + m_set.x_offset_robot << "," << Out_rob[i].y + m_set.y_offset_robot << "," << Out_rob[i].z << "," << std::endl;
|
|
|
|
anOutputFile << std::endl;
|
|
return true;
|
|
}
|
|
|
|
if (!tool)
|
|
{
|
|
Out_val = m_Output;
|
|
Out_time = m_Output_time;
|
|
ind = 2;
|
|
}
|
|
else
|
|
{
|
|
Out_val = m_Output2;
|
|
Out_time = m_Output_time2;
|
|
ind = 3;
|
|
}
|
|
|
|
if (beamfl && tool) pid = ind+1; // pid: 2 - Master
|
|
else pid = ind; // 3 - Slave
|
|
// 4 - Plate (x-,y-movement)
|
|
|
|
n = Out_val.size();
|
|
|
|
if (n != Out_time.size())
|
|
throw Base::RuntimeError("Outputlängen passen nicht zusammen");
|
|
|
|
if (n>1)
|
|
{
|
|
anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile << ind << ",1,0," << c << ",1.000000, ," << Out_time[n-1] << "," << Out_time[0] << std::endl;
|
|
|
|
|
|
if (beamfl && tool)
|
|
{
|
|
anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile << pid << ",1,0," << c << ",1.000000, ," << Out_time[n-1] << "," << Out_time[0] << std::endl;
|
|
}
|
|
|
|
anOutputFile << "*DEFINE_CURVE" << std::endl << c << std::endl;
|
|
|
|
for (int i=0; i<n; ++i)
|
|
{
|
|
anOutputFile << Out_time[i] - Out_time[0]<<"," << Out_val[i][0].x << std::endl;
|
|
}
|
|
|
|
anOutputFile << Out_time[n-1] - Out_time[0] + 0.1 << "," << Out_val[n-1][0].x << std::endl;
|
|
|
|
times.first = (float) Out_time[0];
|
|
times.second = (float) Out_time[n-1];
|
|
|
|
if (!tool) m_PathTimes_Master.push_back(times);
|
|
else m_PathTimes_Slave.push_back(times);
|
|
|
|
anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile << ind << ",2,0," << c+1 << ",1.000000, ," << Out_time[n-1] << "," << Out_time[0] << std::endl;
|
|
|
|
|
|
if (beamfl && tool)
|
|
{
|
|
anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile << pid << ",2,0," << c+1 << ",1.000000, ," << Out_time[n-1] << "," << Out_time[0] << std::endl;
|
|
}
|
|
|
|
anOutputFile << "*DEFINE_CURVE" << std::endl << c+1 << std::endl;
|
|
|
|
for (int i=0; i<n; ++i)
|
|
{
|
|
anOutputFile << Out_time[i] - Out_time[0]<< "," << Out_val[i][0].y << std::endl;
|
|
}
|
|
|
|
anOutputFile << Out_time[n-1] - Out_time[0] + 0.1 << "," << Out_val[n-1][0].y << std::endl;
|
|
|
|
times.first = (float) Out_time[0];
|
|
times.second = (float) Out_time[n-1];
|
|
|
|
if (!tool) m_PathTimes_Master.push_back(times);
|
|
else m_PathTimes_Slave.push_back(times);
|
|
|
|
if (beamfl && tool)
|
|
{
|
|
anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile << pid << ",3,0," << c+2 << ",1.000000, ," << Out_time[n-1] << "," << Out_time[0] << std::endl;
|
|
}
|
|
else
|
|
{
|
|
anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile << "$# ind dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile << ind << ",3,0," << c+2 << ",1.000000, ," << Out_time[n-1] << "," << Out_time[0] << std::endl;
|
|
}
|
|
|
|
anOutputFile << "*DEFINE_CURVE" << std::endl << c+2 << std::endl;
|
|
|
|
|
|
for (int i=0; i<n; ++i)
|
|
{
|
|
anOutputFile << Out_time[i] - Out_time[0]<<"," << Out_val[i][0].z<< std::endl;
|
|
}
|
|
|
|
anOutputFile << Out_time[n-1] - Out_time[0] + 0.1 << "," << Out_val[n-1][0].z<< std::endl;
|
|
|
|
times.first = (float) Out_time[0];
|
|
times.second = (float) Out_time[n-1];
|
|
|
|
if (!tool) m_PathTimes_Master.push_back(times);
|
|
else m_PathTimes_Slave.push_back(times);
|
|
|
|
c = c+3;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/* Schreibt Output für den normalen beidseitigen Fall */
|
|
bool path_simulate::WriteOutputDouble(ofstream &anOutputFile, ofstream &anOutputFile2, int &c1, int &c2, bool brob, bool beamfl)
|
|
{
|
|
std::pair<float,float> times;
|
|
int pid1 = 2; // Master
|
|
int pid2 = 3; // Slave
|
|
int pid3 = 4; // Platte
|
|
|
|
if (brob == false) // Simulations-Output (brob == true -> roboter-output)
|
|
{
|
|
TimeCorrection();
|
|
|
|
int n = m_Output.size(); // Master
|
|
int n2 = m_Output2.size(); // Slave
|
|
|
|
if (n>1)
|
|
{
|
|
|
|
// MASTER-X
|
|
|
|
anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile << pid1 << ",1,0," << c1 << ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
|
|
anOutputFile << "*DEFINE_CURVE" << std::endl << c1 << std::endl;
|
|
|
|
for (int i=0; i<n; ++i)
|
|
{
|
|
anOutputFile << m_Output_time[i] - m_Output_time[0]<<"," << m_Output[i][0].x << std::endl;
|
|
}
|
|
|
|
anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][0].x << std::endl;
|
|
|
|
times.first = (float) m_Output_time[0];
|
|
times.second = (float) m_Output_time[n-1];
|
|
m_PathTimes_Master.push_back(times); // fülle vektor für curve-times
|
|
|
|
|
|
|
|
|
|
// SLAVE-X
|
|
|
|
anOutputFile2 << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile2 << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile2 << pid2 << ",1,0," << c2 << ",1.000000, ," << m_Output_time2[n2-1] << "," << m_Output_time2[0] << std::endl;
|
|
|
|
if (beamfl) // wenn auf true, dann füge neuen part mit ein
|
|
{
|
|
anOutputFile2 << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile2 << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile2 << pid3 << ",1,0," << c2<< ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
|
|
}
|
|
|
|
anOutputFile2 << "*DEFINE_CURVE" << std::endl << c2 << std::endl;
|
|
|
|
for (int i=0; i<n2; ++i)
|
|
{
|
|
anOutputFile2 << m_Output_time2[i] - m_Output_time2[0]<<"," << m_Output2[i][0].x << std::endl;
|
|
}
|
|
|
|
anOutputFile2 << m_Output_time2[n2-1] - m_Output_time2[0] + 0.1 << "," << m_Output2[n2-1][0].x << std::endl;
|
|
|
|
times.first = (float) m_Output_time2[0];
|
|
times.second = (float) m_Output_time2[n2-1];
|
|
m_PathTimes_Slave.push_back(times);
|
|
|
|
|
|
|
|
|
|
|
|
// MASTER-Y
|
|
|
|
anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile << pid1 << ",2,0," << c1+1 << ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
|
|
anOutputFile << "*DEFINE_CURVE" << std::endl << c1+1 << std::endl;
|
|
|
|
for (int i=0; i<n; ++i)
|
|
{
|
|
anOutputFile << m_Output_time[i] - m_Output_time[0]<< "," << m_Output[i][0].y << std::endl;
|
|
}
|
|
|
|
anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][0].y << std::endl;
|
|
|
|
times.first = (float) m_Output_time[0];
|
|
times.second = (float) m_Output_time[n-1];
|
|
m_PathTimes_Master.push_back(times);
|
|
|
|
|
|
|
|
|
|
// SLAVE-Y
|
|
|
|
anOutputFile2 << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile2 << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile2 << pid2 << ",2,0," << c2+1 << ",1.000000, ," << m_Output_time2[n2-1] << "," << m_Output_time2[0] << std::endl;
|
|
|
|
if (beamfl) // wenn auf true, dann füge neuen part mit ein
|
|
{
|
|
anOutputFile2 << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile2 << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile2 << pid3 << ",2,0," << c2+1 << ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
|
|
}
|
|
|
|
anOutputFile2 << "*DEFINE_CURVE" << std::endl << c2+1 << std::endl;
|
|
|
|
for (int i=0; i<n2; ++i)
|
|
{
|
|
anOutputFile2 << m_Output_time2[i] - m_Output_time2[0]<<"," << m_Output2[i][0].y << std::endl;
|
|
}
|
|
|
|
anOutputFile2 << m_Output_time2[n2-1] - m_Output_time2[0] + 0.1 << "," << m_Output2[n2-1][0].y << std::endl;
|
|
|
|
times.first = (float) m_Output_time2[0];
|
|
times.second = (float) m_Output_time2[n2-1];
|
|
m_PathTimes_Slave.push_back(times);
|
|
|
|
|
|
|
|
|
|
|
|
// MASTER-Z
|
|
anOutputFile << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile << pid1 << ",3,0," << c1+2 << ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
|
|
anOutputFile << "*DEFINE_CURVE" << std::endl << c1+2 << std::endl;
|
|
|
|
|
|
for (int i=0; i<n; ++i)
|
|
{
|
|
anOutputFile << m_Output_time[i] - m_Output_time[0]<<"," << m_Output[i][0].z<< std::endl;
|
|
}
|
|
|
|
anOutputFile << m_Output_time[n-1] - m_Output_time[0] + 0.1 << "," << m_Output[n-1][0].z<< std::endl;
|
|
|
|
times.first = (float) m_Output_time[0];
|
|
times.second = (float) m_Output_time[n-1];
|
|
m_PathTimes_Master.push_back(times);
|
|
|
|
|
|
|
|
// SLAVE-Z
|
|
|
|
if (beamfl)
|
|
{
|
|
anOutputFile2 << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile2 << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile2 << pid3 << ",3,0," << c2+2 << ",1.000000, ," << m_Output_time[n-1] << "," << m_Output_time[0] << std::endl;
|
|
|
|
}
|
|
else
|
|
{
|
|
anOutputFile2 << "*BOUNDARY_PRESCRIBED_MOTION_RIGID" << std::endl;
|
|
anOutputFile2 << "$# pid dof vad lcid sf vid death birth" << std::endl;
|
|
anOutputFile2 << pid2 << ",3,0," << c2+2 << ",1.000000, ," << m_Output_time2[n2-1] << "," << m_Output_time2[0] << std::endl;
|
|
}
|
|
|
|
anOutputFile2 << "*DEFINE_CURVE" << std::endl << c2+2 << std::endl;
|
|
|
|
for (int i=0; i<n2; ++i)
|
|
{
|
|
anOutputFile2 << m_Output_time2[i] - m_Output_time2[0]<<"," << m_Output2[i][0].z << std::endl;
|
|
}
|
|
|
|
anOutputFile2 << m_Output_time2[n2-1] - m_Output_time2[0] + 0.1 << "," << m_Output2[n2-1][0].z << std::endl;
|
|
|
|
times.first = (float) m_Output_time2[0];
|
|
times.second = (float) m_Output_time2[n2-1];
|
|
m_PathTimes_Slave.push_back(times);
|
|
|
|
c1 += 3;
|
|
c2 += 3;
|
|
}
|
|
}
|
|
else // Schreibe Roboter-Output
|
|
{
|
|
int n1 = m_Output_robo1.size();
|
|
for (int i=0; i<n1; ++i)
|
|
anOutputFile << m_Output_robo1[i].x + m_set.x_offset_robot << "," << m_Output_robo1[i].y + m_set.y_offset_robot << "," << m_Output_robo1[i].z << "," << RoboFlag_Master[i] << endl;
|
|
|
|
int n2 = m_Output_robo2.size();
|
|
for (int i=0; i<n2; ++i)
|
|
anOutputFile2 << m_Output_robo2[i].x + m_set.x_offset_robot << "," << m_Output_robo2[i].y + m_set.y_offset_robot << "," << m_Output_robo2[i].z << "," << RoboFlag_Slave[i] << endl;
|
|
}
|
|
|
|
return true;
|
|
}
|