/*************************************************************************** * Copyright (c) 2007 * * Human Rezaijafari * * 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 #include #include #include #include #include #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 &BSplineTop, const std::vector &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 . Die Funktion gliedert sich in drei Abschnitte mit den Parametergrenzen , , , welche z.B. mittels path_simulate::ParameterCalculation() ermittelt werden kann. Die Start- und Endgeschwindigkeit müssen vorher in und bestimmt werden. Die maximale Geschwindigkeit welche zwischen und erreicht werden soll, entspricht hier */ 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 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 && 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 */ 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 zur Zeit 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 zur Zeit 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 . 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 und wird die Kurve mit der konstanten // Geschwindigkeit durchlaufen } else // Weg reicht nicht aus -> Parameterkorrektur { // Berechne Geschwindigkeit die mindestenns notwendig ist damit der Weg zur Zeit 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 solange in Richtung 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 zurück. Die Startzeit 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 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 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 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 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 legt den Ausgabetyp fest*/ bool path_simulate::ConnectPaths_xy(bool brob) { int N; double t = m_t0; std::vector tmp2; std::vector 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 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 legt den Ausgabetyp fest*/ bool path_simulate::ConnectPaths_z(bool brob) { int N; double t = m_t0; Base::Vector3d tmp; std::vector d; std::vector 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 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 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 Times; std::vector ConnPnts; std::vector< std::vector > Out; gp_Vec vec[3], vec_tmp[3]; Base::Vector3d tmp; std::vector d; std::vector 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 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 > path_simulate::CompBounds(bool tool,std::vector 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 single_bound; std::vector bounds; std::vector > 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 // Die Parameter der Bereichsgrenzen an denen die Kurvenkrümmung dem Toleranzwert // entspricht werden in den Vektor für die weitere Nachbearbeitung gefüllt for (int i=0; i= 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; // hat stets gerade Länge for(int i=0; i haben zusammen { single_bound.push_back(bounds[2*i]); while(i, , 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 > v_vec; std::vector v(3), l_vec, a_vec; std::vector 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; aNbKnots(); 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 knot_vec(m_Knots->Length()); for(int i=0; iLength(); i++) knot_vec[i] = m_Knots->Value(i); // *** Ende der Berechnung des Knotenvektors *** std::vector > CriticalBounds = CompBounds(tool, knot_vec); // Berechnet auf Basis der aktuellen Kurve, // die kritischen Bereiche im Parameterraum und // berechnet gleichzeitig die maximale Kurvenkrümmung 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 gestartet v[0] = 0.0; // starte jede Kurve mit v = 0 int m = 0; for (unsigned int i=0; iValue(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 m_a = m_amax - d2*velo*velo; // wenn zu groß gewählt wurde, kann 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; iFirstParameter(); } else { for(int i=0; iFirstParameter(); } 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 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 abgelegt if (tool==false) // Master { N = m_Output.size(); for (int i=1; i 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 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 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 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 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 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; iFirstParameter(); // Setzt Startparameter der aktuellen Kurve m = m_length_ma[i].size(); for(int j=0; jFirstParameter(); m = m_length_sl[i].size(); // vgl. Master for(int j=0; jEndPoint(); // 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 tmp3; // Base::Vector3f pnt1,pnt2; if(brob) { N = std::max(2, (int) ceil(period / double(TolDist))); for (int i=1; i 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 > D0; std::vector< std::vector > 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 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 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 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 des Tools zum Zeitpunkt // bzgl. den Parametern m_t0, m_t1, m_t2, m_T, m_a, m_v[i], i =1,2,3 // Ausgabevektor zum Zeitpunkt 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 , so aneinander an, dass die Endzeiten übereinstimmen*/ bool path_simulate::TimeCorrection() { int N; Base::Vector3d vec(0.0,0.0,0.0); std::vector 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 m_Output_time.size()) { m_Output_time = m_Output_time2; N = m_Output_time.size(); m_Output.resize(N); for (int i=0; i Werkzeug hat zu warten) { m_Output[i] = vecc; } return true; } return true; //gibt true zurück wenn beide Outputvektoren und 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 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 = 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 &flatAreas, bool spiral) { m_Feat = true; double rad[2]; bool tool; int c[2]; gp_Pnt pnt0, pnt1; std::vector::iterator *it_1,*it_2; std::vector *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 &flatAreas) { m_Feat = true; int f = 0, run; bool tool; double rad[2]; gp_Pnt pnt0, pnt1; std::vector::iterator *it_1,*it_2; std::vector *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; iD0((**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(z0StartPoint(); // 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; iEndPoint(); } } // Zurück zum Status quo for(int i=1; iStartPoint(); // 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; iEndPoint(); } } // Zurück zum Status quo for(int i=1; iFirstParameter()); // 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 > Out_val; std::vector Out_time; std::pair times; int n; int ind; int pid; if (brob == true) // Schreibe Roboter-Output { std::vector Out_rob; if (!tool) Out_rob = m_Output_robo1; else Out_rob = m_Output_robo2; n = Out_rob.size(); for (int i=0; i1) { 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 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