From 4c470ecd111618b1775a8f32ff5b738e899eb12f Mon Sep 17 00:00:00 2001 From: wmayer Date: Mon, 11 Sep 2023 10:24:21 +0200 Subject: [PATCH] Robot: Apply clang format --- src/Mod/Robot/App/AppRobot.cpp | 47 ++- src/Mod/Robot/App/Edge2TracObject.cpp | 333 +++++++++--------- src/Mod/Robot/App/Edge2TracObject.h | 18 +- src/Mod/Robot/App/PreCompiled.cpp | 2 +- src/Mod/Robot/App/PreCompiled.h | 22 +- src/Mod/Robot/App/PropertyTrajectory.cpp | 49 ++- src/Mod/Robot/App/PropertyTrajectory.h | 22 +- src/Mod/Robot/App/Robot6Axis.cpp | 194 +++++----- src/Mod/Robot/App/Robot6Axis.h | 47 +-- src/Mod/Robot/App/Robot6AxisPy.xml | 16 +- src/Mod/Robot/App/Robot6AxisPyImp.cpp | 75 ++-- src/Mod/Robot/App/RobotAlgos.cpp | 59 ++-- src/Mod/Robot/App/RobotAlgos.h | 27 +- src/Mod/Robot/App/RobotObject.cpp | 138 +++++--- src/Mod/Robot/App/RobotObject.h | 44 +-- src/Mod/Robot/App/RobotObjectPy.xml | 17 +- src/Mod/Robot/App/RobotObjectPyImp.cpp | 12 +- src/Mod/Robot/App/Simulation.cpp | 34 +- src/Mod/Robot/App/Simulation.h | 42 ++- src/Mod/Robot/App/Trajectory.cpp | 263 ++++++++------ src/Mod/Robot/App/Trajectory.h | 55 +-- src/Mod/Robot/App/TrajectoryCompound.cpp | 35 +- src/Mod/Robot/App/TrajectoryCompound.h | 16 +- src/Mod/Robot/App/TrajectoryDressUpObject.cpp | 114 ++++-- src/Mod/Robot/App/TrajectoryDressUpObject.h | 29 +- src/Mod/Robot/App/TrajectoryObject.cpp | 21 +- src/Mod/Robot/App/TrajectoryObject.h | 21 +- src/Mod/Robot/App/TrajectoryPy.xml | 16 +- src/Mod/Robot/App/TrajectoryPyImp.cpp | 71 ++-- src/Mod/Robot/App/Waypoint.cpp | 127 +++---- src/Mod/Robot/App/Waypoint.h | 33 +- src/Mod/Robot/App/WaypointPy.xml | 16 +- src/Mod/Robot/App/WaypointPyImp.cpp | 167 +++++---- 33 files changed, 1215 insertions(+), 967 deletions(-) diff --git a/src/Mod/Robot/App/AppRobot.cpp b/src/Mod/Robot/App/AppRobot.cpp index 7da22192c3..390bd5940d 100644 --- a/src/Mod/Robot/App/AppRobot.cpp +++ b/src/Mod/Robot/App/AppRobot.cpp @@ -27,8 +27,8 @@ #include "Edge2TracObject.h" #include "PropertyTrajectory.h" -#include "Robot6AxisPy.h" #include "Robot6Axis.h" +#include "Robot6AxisPy.h" #include "RobotObject.h" #include "Simulation.h" #include "Trajectory.h" @@ -36,39 +36,48 @@ #include "TrajectoryDressUpObject.h" #include "TrajectoryObject.h" #include "TrajectoryPy.h" -#include "WaypointPy.h" #include "Waypoint.h" +#include "WaypointPy.h" -namespace Robot { -class Module : public Py::ExtensionModule +namespace Robot +{ +class Module: public Py::ExtensionModule { public: - Module() : Py::ExtensionModule("Robot") + Module() + : Py::ExtensionModule("Robot") { - add_varargs_method("simulateToFile",&Module::simulateToFile, - "simulateToFile(Robot,Trajectory,TickSize,FileName) - runs the simulation and write the result to a file." - ); - initialize("This module is the Robot module."); // register with Python + add_varargs_method("simulateToFile", + &Module::simulateToFile, + "simulateToFile(Robot,Trajectory,TickSize,FileName) - runs the " + "simulation and write the result to a file."); + initialize("This module is the Robot module.");// register with Python } private: Py::Object simulateToFile(const Py::Tuple& args) { - PyObject *pcRobObj; - PyObject *pcTracObj; + PyObject* pcRobObj; + PyObject* pcTracObj; float tick; char* FileName; - if (!PyArg_ParseTuple(args.ptr(), "O!O!fs", &(Robot6AxisPy::Type), &pcRobObj, - &(TrajectoryPy::Type), &pcTracObj, - &tick,&FileName)) + if (!PyArg_ParseTuple(args.ptr(), + "O!O!fs", + &(Robot6AxisPy::Type), + &pcRobObj, + &(TrajectoryPy::Type), + &pcTracObj, + &tick, + &FileName)) { throw Py::Exception(); + } try { - Robot::Trajectory &Trac = * static_cast(pcTracObj)->getTrajectoryPtr(); - Robot::Robot6Axis &Rob = * static_cast(pcRobObj)->getRobot6AxisPtr(); - Simulation Sim(Trac,Rob); + Robot::Trajectory& Trac = *static_cast(pcTracObj)->getTrajectoryPtr(); + Robot::Robot6Axis& Rob = *static_cast(pcRobObj)->getRobot6AxisPtr(); + Simulation Sim(Trac, Rob); } catch (const Base::Exception& e) { throw Py::RuntimeError(e.what()); @@ -83,12 +92,13 @@ PyObject* initModule() return Base::Interpreter().addModule(new Module); } -} // namespace Robot +}// namespace Robot /* Python entry */ PyMOD_INIT_FUNC(Robot) { + // clang-format off // load dependent module try { Base::Interpreter().runString("import Part"); @@ -122,4 +132,5 @@ PyMOD_INIT_FUNC(Robot) Robot::TrajectoryDressUpObject ::init(); PyMOD_Return(robotModule); + // clang-format on } diff --git a/src/Mod/Robot/App/Edge2TracObject.cpp b/src/Mod/Robot/App/Edge2TracObject.cpp index 051920197e..4c8543bbcf 100644 --- a/src/Mod/Robot/App/Edge2TracObject.cpp +++ b/src/Mod/Robot/App/Edge2TracObject.cpp @@ -22,15 +22,15 @@ #include "PreCompiled.h" #ifndef _PreComp_ -# include -# include -# include -# include +#include +#include +#include +#include #endif #include -#include #include +#include #include "Edge2TracObject.h" #include "Trajectory.h" @@ -46,221 +46,232 @@ PROPERTY_SOURCE(Robot::Edge2TracObject, Robot::TrajectoryObject) Edge2TracObject::Edge2TracObject() { - ADD_PROPERTY_TYPE( Source, (nullptr) , "Edge2Trac",Prop_None,"Edges to generate the Trajectory"); - ADD_PROPERTY_TYPE( SegValue, (0.5), "Edge2Trac",Prop_None,"Max deviation from original geometry"); - ADD_PROPERTY_TYPE( UseRotation, (0) , "Edge2Trac",Prop_None,"use orientation of the edge"); + ADD_PROPERTY_TYPE(Source, + (nullptr), + "Edge2Trac", + Prop_None, + "Edges to generate the Trajectory"); + ADD_PROPERTY_TYPE(SegValue, + (0.5), + "Edge2Trac", + Prop_None, + "Max deviation from original geometry"); + ADD_PROPERTY_TYPE(UseRotation, (0), "Edge2Trac", Prop_None, "use orientation of the edge"); NbrOfEdges = 0; NbrOfCluster = 0; } -App::DocumentObjectExecReturn *Edge2TracObject::execute() +App::DocumentObjectExecReturn* Edge2TracObject::execute() { App::DocumentObject* link = Source.getValue(); - if (!link) + if (!link) { return new App::DocumentObjectExecReturn("No object linked"); - if (!link->getTypeId().isDerivedFrom(Part::Feature::getClassTypeId())) + } + if (!link->getTypeId().isDerivedFrom(Part::Feature::getClassTypeId())) { return new App::DocumentObjectExecReturn("Linked object is not a Part object"); - Part::Feature *base = static_cast(Source.getValue()); + } + Part::Feature* base = static_cast(Source.getValue()); const Part::TopoShape& TopShape = base->Shape.getShape(); const std::vector& SubVals = Source.getSubValuesStartsWith("Edge"); - if (SubVals.empty()) + if (SubVals.empty()) { return new App::DocumentObjectExecReturn("No Edges specified"); + } - // container for all the edges + // container for all the edges std::vector edges; // run through the edge name and get the real objects from the TopoShape - for (const auto & SubVal : SubVals) { - TopoDS_Edge edge = TopoDS::Edge(TopShape.getSubShape(SubVal.c_str())); - edges.push_back(edge); + for (const auto& SubVal : SubVals) { + TopoDS_Edge edge = TopoDS::Edge(TopShape.getSubShape(SubVal.c_str())); + edges.push_back(edge); } - // instantiate an edge cluster sorter and get the result + // instantiate an edge cluster sorter and get the result Part::Edgecluster acluster(edges); Part::tEdgeClusterVector aclusteroutput = acluster.GetClusters(); - if(aclusteroutput.empty()) + if (aclusteroutput.empty()) { return new App::DocumentObjectExecReturn("No Edges specified"); + } - // set the number of cluster and edges + // set the number of cluster and edges NbrOfCluster = aclusteroutput.size(); NbrOfEdges = 0; - for(const auto & it : aclusteroutput) + for (const auto& it : aclusteroutput) { NbrOfEdges += it.size(); + } // trajectory to fill Robot::Trajectory trac; bool first = true; // cycle through the cluster - for(const auto & it : aclusteroutput) - { + for (const auto& it : aclusteroutput) { // cycle through the edges of the cluster - for(const auto& it2 : it) - { + for (const auto& it2 : it) { BRepAdaptor_Curve adapt(it2); - - switch(adapt.GetType()) - { - case GeomAbs_Line: - { - // get start and end point - gp_Pnt P1 = adapt.Value(adapt.FirstParameter()); - gp_Pnt P2 = adapt.Value(adapt.LastParameter()); - Base::Rotation R1; - Base::Rotation R2; + switch (adapt.GetType()) { + case GeomAbs_Line: { + // get start and end point + gp_Pnt P1 = adapt.Value(adapt.FirstParameter()); + gp_Pnt P2 = adapt.Value(adapt.LastParameter()); - // if orientation is used - if(UseRotation.getValue()) { - // here get the orientation of the start and end point... - //R1 = ; - //R2 = ; + Base::Rotation R1; + Base::Rotation R2; - } + // if orientation is used + if (UseRotation.getValue()) { + // here get the orientation of the start and end point... + // R1 = ; + // R2 = ; + } - // if reverse orintation, switch the points - if ( it2.Orientation() == TopAbs_REVERSED ) - { - //switch the points and orientation - gp_Pnt tmpP = P1; - Base::Rotation tmpR = R1; - P1 = P2; - R1 = R2; - R2 = tmpR; - P2 = tmpP; - } - if(first){ - Waypoint wp("Pt",Base::Placement(Base::Vector3d(P1.X(),P1.Y(),P1.Z()),R1)); + // if reverse orintation, switch the points + if (it2.Orientation() == TopAbs_REVERSED) { + // switch the points and orientation + gp_Pnt tmpP = P1; + Base::Rotation tmpR = R1; + P1 = P2; + R1 = R2; + R2 = tmpR; + P2 = tmpP; + } + if (first) { + Waypoint wp("Pt", + Base::Placement(Base::Vector3d(P1.X(), P1.Y(), P1.Z()), R1)); + trac.addWaypoint(wp); + first = false; + } + Waypoint wp("Pt", Base::Placement(Base::Vector3d(P2.X(), P2.Y(), P2.Z()), R2)); trac.addWaypoint(wp); - first = false; + break; } - Waypoint wp("Pt",Base::Placement(Base::Vector3d(P2.X(),P2.Y(),P2.Z()),R2)); - trac.addWaypoint(wp); - break; - } - case GeomAbs_BSplineCurve: - { - Standard_Real Length = CPnts_AbscissaPoint::Length(adapt); - Standard_Real ParLength = adapt.LastParameter()-adapt.FirstParameter(); - Standard_Real NbrSegments = Round(Length / SegValue.getValue()); + case GeomAbs_BSplineCurve: { + Standard_Real Length = CPnts_AbscissaPoint::Length(adapt); + Standard_Real ParLength = adapt.LastParameter() - adapt.FirstParameter(); + Standard_Real NbrSegments = Round(Length / SegValue.getValue()); - Standard_Real beg = adapt.FirstParameter(); - Standard_Real end = adapt.LastParameter(); - Standard_Real stp = ParLength / NbrSegments; - bool reversed = false; - if (it2.Orientation() == TopAbs_REVERSED) { - std::swap(beg, end); - stp = - stp; - reversed = true; - } + Standard_Real beg = adapt.FirstParameter(); + Standard_Real end = adapt.LastParameter(); + Standard_Real stp = ParLength / NbrSegments; + bool reversed = false; + if (it2.Orientation() == TopAbs_REVERSED) { + std::swap(beg, end); + stp = -stp; + reversed = true; + } - if (first) - first = false; - else - beg += stp; - Base::SequencerLauncher seq("Create waypoints", static_cast((end-beg)/stp)); - if(reversed) - { - for (;beg > end; beg += stp) { - gp_Pnt P = adapt.Value(beg); - Base::Rotation R1; - // if orientation is used - if(UseRotation.getValue()) { - // here get the orientation of the start and end point... - //R1 = ; + if (first) { + first = false; + } + else { + beg += stp; + } + Base::SequencerLauncher seq("Create waypoints", + static_cast((end - beg) / stp)); + if (reversed) { + for (; beg > end; beg += stp) { + gp_Pnt P = adapt.Value(beg); + Base::Rotation R1; + // if orientation is used + if (UseRotation.getValue()) { + // here get the orientation of the start and end point... + // R1 = ; + } + + Waypoint wp("Pt", + Base::Placement(Base::Vector3d(P.X(), P.Y(), P.Z()), R1)); + trac.addWaypoint(wp); + seq.next(); } - - Waypoint wp("Pt",Base::Placement(Base::Vector3d(P.X(),P.Y(),P.Z()),R1)); - trac.addWaypoint(wp); - seq.next(); - } - } - else - { - for (;beg < end; beg += stp) { - gp_Pnt P = adapt.Value(beg); - Base::Rotation R1; - // if orientation is used - if(UseRotation.getValue()) { - // here get the orientation of the start and end point... - //R1 = ; + } + else { + for (; beg < end; beg += stp) { + gp_Pnt P = adapt.Value(beg); + Base::Rotation R1; + // if orientation is used + if (UseRotation.getValue()) { + // here get the orientation of the start and end point... + // R1 = ; + } + Waypoint wp("Pt", + Base::Placement(Base::Vector3d(P.X(), P.Y(), P.Z()), R1)); + trac.addWaypoint(wp); + seq.next(); } - Waypoint wp("Pt",Base::Placement(Base::Vector3d(P.X(),P.Y(),P.Z()),R1)); - trac.addWaypoint(wp); - seq.next(); - } - } - + } + } break; - case GeomAbs_Circle: - { - Standard_Real Length = CPnts_AbscissaPoint::Length(adapt); - Standard_Real ParLength = adapt.LastParameter()-adapt.FirstParameter(); - Standard_Real NbrSegments = Round(Length / SegValue.getValue()); - Standard_Real SegLength = ParLength / NbrSegments; - - if ( it2.Orientation() == TopAbs_REVERSED ) - { - //Beginning and End switch - double i = adapt.LastParameter(); - if(first) - first=false; - else - i -= SegLength; - for (;i>adapt.FirstParameter();i-= SegLength){ - gp_Pnt P = adapt.Value(i); - Base::Rotation R1; - // if orientation is used - if(UseRotation.getValue()) { - // here get the orientation of the start and end point... - //R1 = ; + case GeomAbs_Circle: { + Standard_Real Length = CPnts_AbscissaPoint::Length(adapt); + Standard_Real ParLength = adapt.LastParameter() - adapt.FirstParameter(); + Standard_Real NbrSegments = Round(Length / SegValue.getValue()); + Standard_Real SegLength = ParLength / NbrSegments; + + if (it2.Orientation() == TopAbs_REVERSED) { + // Beginning and End switch + double i = adapt.LastParameter(); + if (first) { + first = false; } - Waypoint wp("Pt",Base::Placement(Base::Vector3d(P.X(),P.Y(),P.Z()),R1)); - trac.addWaypoint(wp); - } - } - else - { - double i = adapt.FirstParameter(); - if(first) - first=false; - else - i += SegLength; - for (;i adapt.FirstParameter(); i -= SegLength) { + gp_Pnt P = adapt.Value(i); + Base::Rotation R1; + // if orientation is used + if (UseRotation.getValue()) { + // here get the orientation of the start and end point... + // R1 = ; + } + Waypoint wp("Pt", + Base::Placement(Base::Vector3d(P.X(), P.Y(), P.Z()), R1)); + trac.addWaypoint(wp); + } + } + else { + double i = adapt.FirstParameter(); + if (first) { + first = false; + } + else { + i += SegLength; + } + for (; i < adapt.LastParameter(); i += SegLength) { + gp_Pnt P = adapt.Value(i); + Base::Rotation R1; + // if orientation is used + if (UseRotation.getValue()) { + // here get the orientation of the start and end point... + // R1 = ; + } + Waypoint wp("Pt", + Base::Placement(Base::Vector3d(P.X(), P.Y(), P.Z()), R1)); + trac.addWaypoint(wp); + } + } + break; } - default: - throw Base::TypeError("Unknown Edge type in Robot::Edge2TracObject::execute()"); + default: + throw Base::TypeError("Unknown Edge type in Robot::Edge2TracObject::execute()"); } } } Trajectory.setValue(trac); - + return App::DocumentObject::StdReturn; } -//short Edge2TracObject::mustExecute(void) const +// short Edge2TracObject::mustExecute(void) const //{ -// return 0; -//} +// return 0; +// } void Edge2TracObject::onChanged(const Property* prop) { diff --git a/src/Mod/Robot/App/Edge2TracObject.h b/src/Mod/Robot/App/Edge2TracObject.h index 9a42f14b4d..8ce2fb816b 100644 --- a/src/Mod/Robot/App/Edge2TracObject.h +++ b/src/Mod/Robot/App/Edge2TracObject.h @@ -31,7 +31,7 @@ namespace Robot { -class RobotExport Edge2TracObject : public TrajectoryObject +class RobotExport Edge2TracObject: public TrajectoryObject { PROPERTY_HEADER_WITH_OVERRIDE(Robot::TrajectoryObject); @@ -39,9 +39,9 @@ public: /// Constructor Edge2TracObject(); - App::PropertyLinkSub Source; + App::PropertyLinkSub Source; App::PropertyFloatConstraint SegValue; - App::PropertyBool UseRotation; + App::PropertyBool UseRotation; /// set by execute with the number of clusters found int NbrOfCluster; @@ -49,18 +49,18 @@ public: int NbrOfEdges; /// returns the type name of the ViewProvider - const char* getViewProviderName() const override { + const char* getViewProviderName() const override + { return "RobotGui::ViewProviderEdge2TracObject"; } - App::DocumentObjectExecReturn *execute() override; + App::DocumentObjectExecReturn* execute() override; protected: /// get called by the container when a property has changed - void onChanged (const App::Property* prop) override; - + void onChanged(const App::Property* prop) override; }; -} //namespace Robot +}// namespace Robot -#endif // ROBOT_ROBOTOBJECT_H +#endif// ROBOT_ROBOTOBJECT_H diff --git a/src/Mod/Robot/App/PreCompiled.cpp b/src/Mod/Robot/App/PreCompiled.cpp index 942976f8be..cde4369c5b 100644 --- a/src/Mod/Robot/App/PreCompiled.cpp +++ b/src/Mod/Robot/App/PreCompiled.cpp @@ -20,4 +20,4 @@ * * ***************************************************************************/ -#include "PreCompiled.h" +#include "PreCompiled.h" diff --git a/src/Mod/Robot/App/PreCompiled.h b/src/Mod/Robot/App/PreCompiled.h index 2885f0a1c9..6ef6597ab5 100644 --- a/src/Mod/Robot/App/PreCompiled.h +++ b/src/Mod/Robot/App/PreCompiled.h @@ -27,13 +27,13 @@ // Exporting of App classes #ifdef FC_OS_WIN32 -# define RobotExport __declspec(dllexport) -# define PartExport __declspec(dllimport) -# define MeshExport __declspec(dllimport) -#else // for Linux -# define RobotExport -# define PartExport -# define MeshExport +#define RobotExport __declspec(dllexport) +#define PartExport __declspec(dllimport) +#define MeshExport __declspec(dllimport) +#else// for Linux +#define RobotExport +#define PartExport +#define MeshExport #endif #ifdef _PreComp_ @@ -44,10 +44,10 @@ // kdl_cp #include "kdl_cp/chain.hpp" -#include "kdl_cp/chainiksolverpos_nr.hpp" -#include "kdl_cp/chainiksolvervel_pinv.hpp" -#include "kdl_cp/chainiksolverpos_nr_jl.hpp" #include "kdl_cp/chainfksolverpos_recursive.hpp" +#include "kdl_cp/chainiksolverpos_nr.hpp" +#include "kdl_cp/chainiksolverpos_nr_jl.hpp" +#include "kdl_cp/chainiksolvervel_pinv.hpp" #include "kdl_cp/frames_io.hpp" #include "kdl_cp/path_line.hpp" #include "kdl_cp/path_roundedcomposite.hpp" @@ -63,5 +63,5 @@ #include #include -#endif // _PreComp_ +#endif// _PreComp_ #endif diff --git a/src/Mod/Robot/App/PropertyTrajectory.cpp b/src/Mod/Robot/App/PropertyTrajectory.cpp index b7ebb1ecbe..1926fcaec5 100644 --- a/src/Mod/Robot/App/PropertyTrajectory.cpp +++ b/src/Mod/Robot/App/PropertyTrajectory.cpp @@ -22,7 +22,7 @@ #include "PreCompiled.h" #ifndef _PreComp_ -# include +#include #endif #include @@ -34,7 +34,7 @@ using namespace Robot; -TYPESYSTEM_SOURCE(Robot::PropertyTrajectory , App::Property) +TYPESYSTEM_SOURCE(Robot::PropertyTrajectory, App::Property) PropertyTrajectory::PropertyTrajectory() = default; @@ -48,7 +48,7 @@ void PropertyTrajectory::setValue(const Trajectory& sh) } -const Trajectory &PropertyTrajectory::getValue()const +const Trajectory& PropertyTrajectory::getValue() const { return _Trajectory; } @@ -57,15 +57,15 @@ const Trajectory &PropertyTrajectory::getValue()const Base::BoundBox3d PropertyTrajectory::getBoundingBox() const { Base::BoundBox3d box; - //if (_Trajectory._Trajectory.IsNull()) - // return box; - //try { - // // If the shape is empty an exception may be thrown - // Bnd_Box bounds; - // BRepBndLib::Add(_Trajectory._Trajectory, bounds); - // bounds.SetGap(0.0); - // Standard_Real xMin, yMin, zMin, xMax, yMax, zMax; - // bounds.Get(xMin, yMin, zMin, xMax, yMax, zMax); + // if (_Trajectory._Trajectory.IsNull()) + // return box; + // try { + // // If the shape is empty an exception may be thrown + // Bnd_Box bounds; + // BRepBndLib::Add(_Trajectory._Trajectory, bounds); + // bounds.SetGap(0.0); + // Standard_Real xMin, yMin, zMin, xMax, yMax, zMax; + // bounds.Get(xMin, yMin, zMin, xMax, yMax, zMax); // box.MinX = xMin; // box.MaxX = xMax; @@ -74,22 +74,22 @@ Base::BoundBox3d PropertyTrajectory::getBoundingBox() const // box.MinZ = zMin; // box.MaxZ = zMax; //} - //catch (Standard_Failure& e) { + // catch (Standard_Failure& e) { //} return box; } -PyObject *PropertyTrajectory::getPyObject() +PyObject* PropertyTrajectory::getPyObject() { return new TrajectoryPy(new Trajectory(_Trajectory)); } -void PropertyTrajectory::setPyObject(PyObject *value) +void PropertyTrajectory::setPyObject(PyObject* value) { if (PyObject_TypeCheck(value, &(TrajectoryPy::Type))) { - TrajectoryPy *pcObject = static_cast(value); + TrajectoryPy* pcObject = static_cast(value); setValue(*pcObject->getTrajectoryPtr()); } else { @@ -99,37 +99,34 @@ void PropertyTrajectory::setPyObject(PyObject *value) } } -App::Property *PropertyTrajectory::Copy() const +App::Property* PropertyTrajectory::Copy() const { - PropertyTrajectory *prop = new PropertyTrajectory(); + PropertyTrajectory* prop = new PropertyTrajectory(); prop->_Trajectory = this->_Trajectory; - + return prop; } -void PropertyTrajectory::Paste(const App::Property &from) +void PropertyTrajectory::Paste(const App::Property& from) { aboutToSetValue(); _Trajectory = dynamic_cast(from)._Trajectory; hasSetValue(); } -unsigned int PropertyTrajectory::getMemSize () const +unsigned int PropertyTrajectory::getMemSize() const { return _Trajectory.getMemSize(); } -void PropertyTrajectory::Save (Base::Writer &writer) const +void PropertyTrajectory::Save(Base::Writer& writer) const { _Trajectory.Save(writer); } -void PropertyTrajectory::Restore(Base::XMLReader &reader) +void PropertyTrajectory::Restore(Base::XMLReader& reader) { Robot::Trajectory temp; temp.Restore(reader); setValue(temp); } - - - diff --git a/src/Mod/Robot/App/PropertyTrajectory.h b/src/Mod/Robot/App/PropertyTrajectory.h index 8e5eba9cb0..58ccf92e66 100644 --- a/src/Mod/Robot/App/PropertyTrajectory.h +++ b/src/Mod/Robot/App/PropertyTrajectory.h @@ -35,7 +35,7 @@ namespace Robot /** The part shape property class. * @author Werner Mayer */ -class RobotExport PropertyTrajectory : public App::Property +class RobotExport PropertyTrajectory: public App::Property { TYPESYSTEM_HEADER_WITH_OVERRIDE(); @@ -48,10 +48,10 @@ public: /// set the part shape void setValue(const Trajectory&); /// get the part shape - const Trajectory &getValue() const; + const Trajectory& getValue() const; //@} - + /** @name Getting basic geometric entities */ //@{ /** Returns the bounding box around the underlying mesh kernel */ @@ -61,17 +61,17 @@ public: /** @name Python interface */ //@{ PyObject* getPyObject() override; - void setPyObject(PyObject *value) override; + void setPyObject(PyObject* value) override; //@} /** @name Save/restore */ //@{ - void Save (Base::Writer &writer) const override; - void Restore(Base::XMLReader &reader) override; + void Save(Base::Writer& writer) const override; + void Restore(Base::XMLReader& reader) override; - App::Property *Copy() const override; - void Paste(const App::Property &from) override; - unsigned int getMemSize () const override; + App::Property* Copy() const override; + void Paste(const App::Property& from) override; + unsigned int getMemSize() const override; //@} private: @@ -79,7 +79,7 @@ private: }; -} //namespace Robot +}// namespace Robot -#endif // PROPERTYTOPOSHAPE_H +#endif// PROPERTYTOPOSHAPE_H diff --git a/src/Mod/Robot/App/Robot6Axis.cpp b/src/Mod/Robot/App/Robot6Axis.cpp index fca75e0ab9..cff8c4abf8 100644 --- a/src/Mod/Robot/App/Robot6Axis.cpp +++ b/src/Mod/Robot/App/Robot6Axis.cpp @@ -22,9 +22,9 @@ #include "PreCompiled.h" #ifndef _PreComp_ -# include "kdl_cp/chainfksolverpos_recursive.hpp" -# include "kdl_cp/chainiksolvervel_pinv.hpp" -# include "kdl_cp/chainiksolverpos_nr_jl.hpp" +#include "kdl_cp/chainfksolverpos_recursive.hpp" +#include "kdl_cp/chainiksolverpos_nr_jl.hpp" +#include "kdl_cp/chainiksolvervel_pinv.hpp" #endif #include @@ -37,21 +37,21 @@ #ifndef M_PI - #define M_PI 3.14159265358979323846 /* pi */ +#define M_PI 3.14159265358979323846 /* pi */ #endif #ifndef M_PI_2 - #define M_PI_2 1.57079632679489661923 /* pi/2 */ +#define M_PI_2 1.57079632679489661923 /* pi/2 */ #endif using namespace Robot; using namespace Base; using namespace KDL; +// clang-format off // some default roboter - AxisDefinition KukaIR500[6] = { -// a ,alpha ,d ,theta ,rotDir ,maxAngle ,minAngle ,AxisVelocity +// a ,alpha ,d ,theta ,rotDir ,maxAngle ,minAngle ,AxisVelocity {500 ,-90 ,1045 ,0 , -1 ,+185 ,-185 ,156 }, // Axis 1 {1300 ,0 ,0 ,0 , 1 ,+35 ,-155 ,156 }, // Axis 2 {55 ,+90 ,0 ,-90 , 1 ,+154 ,-130 ,156 }, // Axis 3 @@ -59,9 +59,10 @@ AxisDefinition KukaIR500[6] = { {0 ,+90 ,0 ,0 , 1 ,+130 ,-130 ,330 }, // Axis 5 {0 ,+180 ,-300 ,0 , 1 ,+350 ,-350 ,615 } // Axis 6 }; +// clang-format on -TYPESYSTEM_SOURCE(Robot::Robot6Axis , Base::Persistence) +TYPESYSTEM_SOURCE(Robot::Robot6Axis, Base::Persistence) Robot6Axis::Robot6Axis() { @@ -81,11 +82,15 @@ void Robot6Axis::setKinematic(const AxisDefinition KinDef[6]) Chain temp; - for(int i=0 ; i<6 ;i++){ - temp.addSegment(Segment(Joint(Joint::RotZ),Frame::DH(KinDef[i].a ,KinDef[i].alpha * (M_PI/180) ,KinDef[i].d ,KinDef[i].theta * (M_PI/180) ))); - RotDir [i] = KinDef[i].rotDir; - Max(i) = KinDef[i].maxAngle * (M_PI/180); - Min(i) = KinDef[i].minAngle * (M_PI/180); + for (int i = 0; i < 6; i++) { + temp.addSegment(Segment(Joint(Joint::RotZ), + Frame::DH(KinDef[i].a, + KinDef[i].alpha * (M_PI / 180), + KinDef[i].d, + KinDef[i].theta * (M_PI / 180)))); + RotDir[i] = KinDef[i].rotDir; + Max(i) = KinDef[i].maxAngle * (M_PI / 180); + Min(i) = KinDef[i].minAngle * (M_PI / 180); Velocity[i] = KinDef[i].velocity; } @@ -98,56 +103,56 @@ void Robot6Axis::setKinematic(const AxisDefinition KinDef[6]) double Robot6Axis::getMaxAngle(int Axis) { - return Max(Axis) * (180.0/M_PI); + return Max(Axis) * (180.0 / M_PI); } double Robot6Axis::getMinAngle(int Axis) { - return Min(Axis) * (180.0/M_PI); + return Min(Axis) * (180.0 / M_PI); } void split(std::string const& string, const char delimiter, std::vector& destination) { - std::string::size_type last_position(0); - std::string::size_type position(0); + std::string::size_type last_position(0); + std::string::size_type position(0); - for (std::string::const_iterator it(string.begin()); it != string.end(); ++it, ++position) - { - if (*it == delimiter ) - { - destination.push_back(string.substr(last_position, position - last_position )); + for (std::string::const_iterator it(string.begin()); it != string.end(); ++it, ++position) { + if (*it == delimiter) { + destination.push_back(string.substr(last_position, position - last_position)); last_position = position + 1; } } - destination.push_back(string.substr(last_position, position - last_position )); + destination.push_back(string.substr(last_position, position - last_position)); } -void Robot6Axis::readKinematic(const char * FileName) +void Robot6Axis::readKinematic(const char* FileName) { char buf[120]; Base::FileInfo fi(FileName); Base::ifstream in(fi); - if (!in) + if (!in) { return; + } std::vector destination; AxisDefinition temp[6]; // over read the header - in.getline(buf,119,'\n'); + in.getline(buf, 119, '\n'); // read 6 Axis - for (auto & i : temp) { - in.getline(buf,79,'\n'); + for (auto& i : temp) { + in.getline(buf, 79, '\n'); destination.clear(); - split(std::string(buf),',',destination); - if (destination.size() < 8) + split(std::string(buf), ',', destination); + if (destination.size() < 8) { continue; + } // transfer the values in kinematic structure - i.a = atof(destination[0].c_str()); - i.alpha = atof(destination[1].c_str()); - i.d = atof(destination[2].c_str()); - i.theta = atof(destination[3].c_str()); - i.rotDir = atof(destination[4].c_str()); + i.a = atof(destination[0].c_str()); + i.alpha = atof(destination[1].c_str()); + i.d = atof(destination[2].c_str()); + i.theta = atof(destination[3].c_str()); + i.rotDir = atof(destination[4].c_str()); i.maxAngle = atof(destination[5].c_str()); i.minAngle = atof(destination[6].c_str()); i.velocity = atof(destination[7].c_str()); @@ -156,62 +161,65 @@ void Robot6Axis::readKinematic(const char * FileName) setKinematic(temp); } -unsigned int Robot6Axis::getMemSize () const +unsigned int Robot6Axis::getMemSize() const { return 0; } -void Robot6Axis::Save (Writer &writer) const +void Robot6Axis::Save(Writer& writer) const { - for(unsigned int i=0;i<6;i++){ + for (unsigned int i = 0; i < 6; i++) { Base::Placement Tip = toPlacement(Kinematic.getSegment(i).getFrameToTip()); writer.Stream() << writer.ind() << "" - << std::endl; + << "Px=\"" << Tip.getPosition().x << "\" " + << "Py=\"" << Tip.getPosition().y << "\" " + << "Pz=\"" << Tip.getPosition().z << "\" " + << "Q0=\"" << Tip.getRotation()[0] << "\" " + << "Q1=\"" << Tip.getRotation()[1] << "\" " + << "Q2=\"" << Tip.getRotation()[2] << "\" " + << "Q3=\"" << Tip.getRotation()[3] << "\" " + << "rotDir=\"" << RotDir[i] << "\" " + << "maxAngle=\"" << Max(i) * (180.0 / M_PI) << "\" " + << "minAngle=\"" << Min(i) * (180.0 / M_PI) << "\" " + << "AxisVelocity=\"" << Velocity[i] << "\" " + << "Pos=\"" << Actual(i) << "\"/>" << std::endl; } } -void Robot6Axis::Restore(XMLReader &reader) +void Robot6Axis::Restore(XMLReader& reader) { Chain Temp; Base::Placement Tip; - for(unsigned int i=0;i<6;i++){ + for (unsigned int i = 0; i < 6; i++) { // read my Element reader.readElement("Axis"); // get the value of the placement - Tip = Base::Placement(Base::Vector3d(reader.getAttributeAsFloat("Px"), - reader.getAttributeAsFloat("Py"), - reader.getAttributeAsFloat("Pz")), - Base::Rotation(reader.getAttributeAsFloat("Q0"), - reader.getAttributeAsFloat("Q1"), - reader.getAttributeAsFloat("Q2"), - reader.getAttributeAsFloat("Q3"))); - Temp.addSegment(Segment(Joint(Joint::RotZ),toFrame(Tip))); + Tip = Base::Placement(Base::Vector3d(reader.getAttributeAsFloat("Px"), + reader.getAttributeAsFloat("Py"), + reader.getAttributeAsFloat("Pz")), + Base::Rotation(reader.getAttributeAsFloat("Q0"), + reader.getAttributeAsFloat("Q1"), + reader.getAttributeAsFloat("Q2"), + reader.getAttributeAsFloat("Q3"))); + Temp.addSegment(Segment(Joint(Joint::RotZ), toFrame(Tip))); - if(reader.hasAttribute("rotDir")) + if (reader.hasAttribute("rotDir")) { Velocity[i] = reader.getAttributeAsFloat("rotDir"); - else + } + else { Velocity[i] = 1.0; + } // read the axis constraints - Min(i) = reader.getAttributeAsFloat("maxAngle")* (M_PI/180); - Max(i) = reader.getAttributeAsFloat("minAngle")* (M_PI/180); - if(reader.hasAttribute("AxisVelocity")) + Min(i) = reader.getAttributeAsFloat("maxAngle") * (M_PI / 180); + Max(i) = reader.getAttributeAsFloat("minAngle") * (M_PI / 180); + if (reader.hasAttribute("AxisVelocity")) { Velocity[i] = reader.getAttributeAsFloat("AxisVelocity"); - else + } + else { Velocity[i] = 156.0; + } Actual(i) = reader.getAttributeAsFloat("Pos"); } Kinematic = Temp; @@ -219,21 +227,32 @@ void Robot6Axis::Restore(XMLReader &reader) calcTcp(); } -bool Robot6Axis::setTo(const Placement &To) +bool Robot6Axis::setTo(const Placement& To) { - //Creation of the solvers: - ChainFkSolverPos_recursive fksolver1(Kinematic);//Forward position solver - ChainIkSolverVel_pinv iksolver1v(Kinematic);//Inverse velocity solver - ChainIkSolverPos_NR_JL iksolver1(Kinematic,Min,Max,fksolver1,iksolver1v,100,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6 + // Creation of the solvers: + ChainFkSolverPos_recursive fksolver1(Kinematic);// Forward position solver + ChainIkSolverVel_pinv iksolver1v(Kinematic); // Inverse velocity solver + ChainIkSolverPos_NR_JL iksolver1(Kinematic, + Min, + Max, + fksolver1, + iksolver1v, + 100, + 1e-6);// Maximum 100 iterations, stop at accuracy 1e-6 - //Creation of jntarrays: + // Creation of jntarrays: JntArray result(Kinematic.getNrOfJoints()); - //Set destination frame - Frame F_dest = Frame(KDL::Rotation::Quaternion(To.getRotation()[0],To.getRotation()[1],To.getRotation()[2],To.getRotation()[3]),KDL::Vector(To.getPosition()[0],To.getPosition()[1],To.getPosition()[2])); + // Set destination frame + Frame F_dest = + Frame(KDL::Rotation::Quaternion(To.getRotation()[0], + To.getRotation()[1], + To.getRotation()[2], + To.getRotation()[3]), + KDL::Vector(To.getPosition()[0], To.getPosition()[1], To.getPosition()[2])); // solve - if (iksolver1.CartToJnt(Actual,F_dest,result) < 0) { + if (iksolver1.CartToJnt(Actual, F_dest, result) < 0) { return false; } else { @@ -245,9 +264,10 @@ bool Robot6Axis::setTo(const Placement &To) Base::Placement Robot6Axis::getTcp() { - double x,y,z,w; - Tcp.M.GetQuaternion(x,y,z,w); - return Base::Placement(Base::Vector3d(Tcp.p[0],Tcp.p[1],Tcp.p[2]),Base::Rotation(x,y,z,w)); + double x, y, z, w; + Tcp.M.GetQuaternion(x, y, z, w); + return Base::Placement(Base::Vector3d(Tcp.p[0], Tcp.p[1], Tcp.p[2]), + Base::Rotation(x, y, z, w)); } bool Robot6Axis::calcTcp() @@ -255,13 +275,13 @@ bool Robot6Axis::calcTcp() // Create solver based on kinematic chain ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(Kinematic); - // Create the frame that will contain the results - KDL::Frame cartpos; - + // Create the frame that will contain the results + KDL::Frame cartpos; + // Calculate forward position kinematics int kinematics_status; - kinematics_status = fksolver.JntToCart(Actual,cartpos); - if (kinematics_status>=0) { + kinematics_status = fksolver.JntToCart(Actual, cartpos); + if (kinematics_status >= 0) { Tcp = cartpos; return true; } @@ -270,13 +290,13 @@ bool Robot6Axis::calcTcp() } } -bool Robot6Axis::setAxis(int Axis,double Value) +bool Robot6Axis::setAxis(int Axis, double Value) { - Actual(Axis) = RotDir[Axis] * Value * (M_PI/180); // degree to radiants + Actual(Axis) = RotDir[Axis] * Value * (M_PI / 180);// degree to radiants return calcTcp(); } double Robot6Axis::getAxis(int Axis) { - return RotDir[Axis] * (Actual(Axis)/(M_PI/180)); // radian to degree + return RotDir[Axis] * (Actual(Axis) / (M_PI / 180));// radian to degree } diff --git a/src/Mod/Robot/App/Robot6Axis.h b/src/Mod/Robot/App/Robot6Axis.h index b63544909d..66600f31a5 100644 --- a/src/Mod/Robot/App/Robot6Axis.h +++ b/src/Mod/Robot/App/Robot6Axis.h @@ -23,10 +23,10 @@ #ifndef ROBOT_ROBOT6AXLE_H #define ROBOT_ROBOT6AXLE_H -#include -#include #include "kdl_cp/chain.hpp" #include "kdl_cp/jntarray.hpp" +#include +#include #include @@ -34,21 +34,22 @@ namespace Robot { /// Definition of the Axis properties -struct AxisDefinition { - double a = 0.0; // a of the Denavit-Hartenberg parameters (mm) - double alpha = 0.0; // alpha of the Denavit-Hartenberg parameters (°) - double d = 0.0; // d of the Denavit-Hartenberg parameters (mm) - double theta = 0.0; // a of the Denavit-Hartenberg parameters (°) - double rotDir = 0.0; // rotational direction (1|-1) - double maxAngle = 0.0; // soft ends + in ° - double minAngle = 0.0; // soft ends - in ° - double velocity = 0.0; // max vlocity of the axle in °/s +struct AxisDefinition +{ + double a = 0.0; // a of the Denavit-Hartenberg parameters (mm) + double alpha = 0.0; // alpha of the Denavit-Hartenberg parameters (°) + double d = 0.0; // d of the Denavit-Hartenberg parameters (mm) + double theta = 0.0; // a of the Denavit-Hartenberg parameters (°) + double rotDir = 0.0; // rotational direction (1|-1) + double maxAngle = 0.0;// soft ends + in ° + double minAngle = 0.0;// soft ends - in ° + double velocity = 0.0;// max vlocity of the axle in °/s }; /** The representation for a 6-Axis industry grade robot */ -class RobotExport Robot6Axis : public Base::Persistence +class RobotExport Robot6Axis: public Base::Persistence { TYPESYSTEM_HEADER_WITH_OVERRIDE(); @@ -56,19 +57,19 @@ public: Robot6Axis(); // from base class - unsigned int getMemSize () const override; - void Save (Base::Writer &/*writer*/) const override; - void Restore(Base::XMLReader &/*reader*/) override; + unsigned int getMemSize() const override; + void Save(Base::Writer& /*writer*/) const override; + void Restore(Base::XMLReader& /*reader*/) override; // interface /// set the kinematic parameters of the robot void setKinematic(const AxisDefinition KinDef[6]); /// read the kinematic parameters of the robot from a file - void readKinematic(const char * FileName); - + void readKinematic(const char* FileName); + /// set the robot to that position, calculates the Axis - bool setTo(const Base::Placement &To); - bool setAxis(int Axis,double Value); + bool setTo(const Base::Placement& To); + bool setAxis(int Axis, double Value); double getAxis(int Axis); double getMaxAngle(int Axis); double getMinAngle(int Axis); @@ -76,7 +77,7 @@ public: bool calcTcp(); Base::Placement getTcp(); - //void setKinematik(const std::vector > &KinTable); + // void setKinematik(const std::vector > &KinTable); protected: @@ -87,10 +88,10 @@ protected: KDL::Frame Tcp; double Velocity[6]; - double RotDir [6]; + double RotDir[6]; }; -} //namespace Part +}// namespace Robot -#endif // PART_TOPOSHAPE_H +#endif// PART_TOPOSHAPE_H diff --git a/src/Mod/Robot/App/Robot6AxisPy.xml b/src/Mod/Robot/App/Robot6AxisPy.xml index a6f67fc9b7..fba8feebf0 100644 --- a/src/Mod/Robot/App/Robot6AxisPy.xml +++ b/src/Mod/Robot/App/Robot6AxisPy.xml @@ -1,13 +1,13 @@ - diff --git a/src/Mod/Robot/App/Robot6AxisPyImp.cpp b/src/Mod/Robot/App/Robot6AxisPyImp.cpp index 32c5477c3f..afd6ffb9f5 100644 --- a/src/Mod/Robot/App/Robot6AxisPyImp.cpp +++ b/src/Mod/Robot/App/Robot6AxisPyImp.cpp @@ -22,15 +22,17 @@ #include "PreCompiled.h" #ifndef _PreComp_ -# include +#include #endif #include #include +// clang-format off // inclusion of the generated files (generated out of Robot6AxisPy.xml) #include "Robot6AxisPy.h" #include "Robot6AxisPy.cpp" +// clang-format on using namespace Robot; @@ -38,28 +40,27 @@ using namespace Robot; // returns a string which represents the object e.g. when printed in python std::string Robot6AxisPy::representation() const { - std::stringstream str; + std::stringstream str; str.precision(5); - str << "getTcp().getPosition().x << "," - << getRobot6AxisPtr()->getTcp().getPosition().y << "," - << getRobot6AxisPtr()->getTcp().getPosition().z << ") " - << "Axis:(" - << "1:" << getRobot6AxisPtr()->getAxis(0) << " " - << "2:" << getRobot6AxisPtr()->getAxis(1) << " " - << "3:" << getRobot6AxisPtr()->getAxis(2) << " " - << "4:" << getRobot6AxisPtr()->getAxis(3) << " " - << "5:" << getRobot6AxisPtr()->getAxis(4) << " " - << "6:" << getRobot6AxisPtr()->getAxis(5) << ")"; + str << "getTcp().getPosition().x << "," + << getRobot6AxisPtr()->getTcp().getPosition().y << "," + << getRobot6AxisPtr()->getTcp().getPosition().z << ") " + << "Axis:(" + << "1:" << getRobot6AxisPtr()->getAxis(0) << " " + << "2:" << getRobot6AxisPtr()->getAxis(1) << " " + << "3:" << getRobot6AxisPtr()->getAxis(2) << " " + << "4:" << getRobot6AxisPtr()->getAxis(3) << " " + << "5:" << getRobot6AxisPtr()->getAxis(4) << " " + << "6:" << getRobot6AxisPtr()->getAxis(5) << ")"; - return str.str(); + return str.str(); } -PyObject *Robot6AxisPy::PyMake(struct _typeobject *, PyObject *, PyObject *) // Python wrapper +PyObject* Robot6AxisPy::PyMake(struct _typeobject*, PyObject*, PyObject*)// Python wrapper { - // create a new instance of Robot6AxisPy and the Twin object + // create a new instance of Robot6AxisPy and the Twin object return new Robot6AxisPy(new Robot6Axis); } @@ -70,14 +71,13 @@ int Robot6AxisPy::PyInit(PyObject* /*args*/, PyObject* /*kwd*/) } -PyObject* Robot6AxisPy::check(PyObject * /*args*/) +PyObject* Robot6AxisPy::check(PyObject* /*args*/) { PyErr_SetString(PyExc_NotImplementedError, "Not yet implemented"); return nullptr; } - Py::Float Robot6AxisPy::getAxis1() const { return Py::Float(getRobot6AxisPtr()->getAxis(0)); @@ -85,7 +85,7 @@ Py::Float Robot6AxisPy::getAxis1() const void Robot6AxisPy::setAxis1(Py::Float arg) { - getRobot6AxisPtr()->setAxis(0,(float)arg.operator double()); + getRobot6AxisPtr()->setAxis(0, (float)arg.operator double()); } Py::Float Robot6AxisPy::getAxis2() const @@ -95,7 +95,7 @@ Py::Float Robot6AxisPy::getAxis2() const void Robot6AxisPy::setAxis2(Py::Float arg) { - getRobot6AxisPtr()->setAxis(1,(float)arg.operator double()); + getRobot6AxisPtr()->setAxis(1, (float)arg.operator double()); } Py::Float Robot6AxisPy::getAxis3() const @@ -105,7 +105,7 @@ Py::Float Robot6AxisPy::getAxis3() const void Robot6AxisPy::setAxis3(Py::Float arg) { - getRobot6AxisPtr()->setAxis(2,(float)arg.operator double()); + getRobot6AxisPtr()->setAxis(2, (float)arg.operator double()); } Py::Float Robot6AxisPy::getAxis4() const @@ -115,7 +115,7 @@ Py::Float Robot6AxisPy::getAxis4() const void Robot6AxisPy::setAxis4(Py::Float arg) { - getRobot6AxisPtr()->setAxis(3,(float)arg.operator double()); + getRobot6AxisPtr()->setAxis(3, (float)arg.operator double()); } Py::Float Robot6AxisPy::getAxis5() const @@ -125,7 +125,7 @@ Py::Float Robot6AxisPy::getAxis5() const void Robot6AxisPy::setAxis5(Py::Float arg) { - getRobot6AxisPtr()->setAxis(4,(float)arg.operator double()); + getRobot6AxisPtr()->setAxis(4, (float)arg.operator double()); } Py::Float Robot6AxisPy::getAxis6() const @@ -135,33 +135,34 @@ Py::Float Robot6AxisPy::getAxis6() const void Robot6AxisPy::setAxis6(Py::Float arg) { - getRobot6AxisPtr()->setAxis(5,(float)arg.operator double()); + getRobot6AxisPtr()->setAxis(5, (float)arg.operator double()); } Py::Object Robot6AxisPy::getTcp() const { - return Py::asObject(new Base::PlacementPy(new Base::Placement(getRobot6AxisPtr()->getTcp()))); + return Py::asObject(new Base::PlacementPy(new Base::Placement(getRobot6AxisPtr()->getTcp()))); } void Robot6AxisPy::setTcp(Py::Object value) { - if (PyObject_TypeCheck(*value, &(Base::MatrixPy::Type))) { - Base::MatrixPy *pcObject = (Base::MatrixPy*)*value; + if (PyObject_TypeCheck(*value, &(Base::MatrixPy::Type))) { + Base::MatrixPy* pcObject = (Base::MatrixPy*)*value; Base::Matrix4D mat = pcObject->value(); Base::Placement p; p.fromMatrix(mat); - getRobot6AxisPtr()->setTo(p); + getRobot6AxisPtr()->setTo(p); } else if (PyObject_TypeCheck(*value, &(Base::PlacementPy::Type))) { - if(! getRobot6AxisPtr()->setTo(*static_cast(*value)->getPlacementPtr())) - throw Base::RuntimeError("Can not reach Point"); + if (!getRobot6AxisPtr()->setTo( + *static_cast(*value)->getPlacementPtr())) { + throw Base::RuntimeError("Can not reach Point"); + } } else { std::string error = std::string("type must be 'Matrix' or 'Placement', not "); error += (*value)->ob_type->tp_name; throw Py::TypeError(error); } - } Py::Object Robot6AxisPy::getBase() const @@ -170,18 +171,14 @@ Py::Object Robot6AxisPy::getBase() const } void Robot6AxisPy::setBase(Py::Object /*arg*/) -{ +{} -} - -PyObject *Robot6AxisPy::getCustomAttributes(const char* /*attr*/) const +PyObject* Robot6AxisPy::getCustomAttributes(const char* /*attr*/) const { return nullptr; } int Robot6AxisPy::setCustomAttributes(const char* /*attr*/, PyObject* /*obj*/) { - return 0; + return 0; } - - diff --git a/src/Mod/Robot/App/RobotAlgos.cpp b/src/Mod/Robot/App/RobotAlgos.cpp index f47be96b56..b0d6c842af 100644 --- a/src/Mod/Robot/App/RobotAlgos.cpp +++ b/src/Mod/Robot/App/RobotAlgos.cpp @@ -22,10 +22,10 @@ #include "PreCompiled.h" #ifndef _PreComp_ -# include "kdl_cp/chainiksolverpos_nr.hpp" -# include "kdl_cp/chainiksolvervel_pinv.hpp" -# include "kdl_cp/chainfksolverpos_recursive.hpp" -# include "kdl_cp/frames_io.hpp" +#include "kdl_cp/chainfksolverpos_recursive.hpp" +#include "kdl_cp/chainiksolverpos_nr.hpp" +#include "kdl_cp/chainiksolvervel_pinv.hpp" +#include "kdl_cp/frames_io.hpp" #endif #include "RobotAlgos.h" @@ -36,12 +36,12 @@ using namespace std; using namespace KDL; #ifndef M_PI - #define M_PI 3.14159265358979323846 - #define M_PI 3.14159265358979323846 /* pi */ +#define M_PI 3.14159265358979323846 +#define M_PI 3.14159265358979323846 /* pi */ #endif #ifndef M_PI_2 - #define M_PI_2 1.57079632679489661923 /* pi/2 */ +#define M_PI_2 1.57079632679489661923 /* pi/2 */ #endif //=========================================================================== @@ -62,14 +62,14 @@ void RobotAlgos::Test() chain.addSegment(Segment(Joint(Joint::RotZ))); chain.addSegment(Segment(Joint(Joint::RotX), Frame(Vector(0.0, 0.0, 0.120)))); chain.addSegment(Segment(Joint(Joint::RotZ))); - + // Create solver based on kinematic chain ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain); - + // Create joint array unsigned int nj = chain.getNrOfJoints(); KDL::JntArray jointpositions = JntArray(nj); - + // Assign some values to the joint positions for (unsigned int i = 0; i < nj; i++) { float myinput; @@ -78,13 +78,13 @@ void RobotAlgos::Test() (void)result; jointpositions(i) = (double)myinput; } - + // Create the frame that will contain the results - KDL::Frame cartpos; - + KDL::Frame cartpos; + // Calculate forward position kinematics int kinematics_status; - kinematics_status = fksolver.JntToCart(jointpositions,cartpos); + kinematics_status = fksolver.JntToCart(jointpositions, cartpos); if (kinematics_status >= 0) { std::cout << cartpos << std::endl; printf("%s \n", "Success, thanks KDL!"); @@ -93,22 +93,25 @@ void RobotAlgos::Test() printf("%s \n", "Error: could not calculate forward kinematics :("); } - // Creation of the solvers: + // Creation of the solvers: ChainFkSolverPos_recursive fksolver1(chain);// Forward position solver ChainIkSolverVel_pinv iksolver1v(chain); // Inverse velocity solver - ChainIkSolverPos_NR iksolver1(chain, fksolver1, iksolver1v, - 100, 1e-6);// Maximum 100 iterations, stop at accuracy 1e-6 - - // Creation of jntarrays: - JntArray result(chain.getNrOfJoints()); - JntArray q_init(chain.getNrOfJoints()); - - // Set destination frame - Frame F_dest=cartpos; - - iksolver1.CartToJnt(q_init,F_dest,result); + ChainIkSolverPos_NR iksolver1(chain, + fksolver1, + iksolver1v, + 100, + 1e-6);// Maximum 100 iterations, stop at accuracy 1e-6 - for (unsigned int i = 0; i < nj; i++) + // Creation of jntarrays: + JntArray result(chain.getNrOfJoints()); + JntArray q_init(chain.getNrOfJoints()); + + // Set destination frame + Frame F_dest = cartpos; + + iksolver1.CartToJnt(q_init, F_dest, result); + + for (unsigned int i = 0; i < nj; i++) { printf("Axle %i: %f \n", i, result(i)); - + } } diff --git a/src/Mod/Robot/App/RobotAlgos.h b/src/Mod/Robot/App/RobotAlgos.h index 173ea3c2b5..907ed52523 100644 --- a/src/Mod/Robot/App/RobotAlgos.h +++ b/src/Mod/Robot/App/RobotAlgos.h @@ -23,10 +23,10 @@ #ifndef _RobotAlgos_h_ #define _RobotAlgos_h_ +#include "kdl_cp/frames_io.hpp" #include #include #include -#include "kdl_cp/frames_io.hpp" namespace Robot @@ -38,24 +38,29 @@ class RobotExport RobotAlgos { public: - /// Constructor + /// Constructor RobotAlgos(); - virtual ~RobotAlgos(); + virtual ~RobotAlgos(); void Test(); }; -inline KDL::Frame toFrame(const Base::Placement &To){ - return KDL::Frame(KDL::Rotation::Quaternion(To.getRotation()[0],To.getRotation()[1],To.getRotation()[2],To.getRotation()[3]),KDL::Vector(To.getPosition()[0],To.getPosition()[1],To.getPosition()[2])); +inline KDL::Frame toFrame(const Base::Placement& To) +{ + return KDL::Frame(KDL::Rotation::Quaternion(To.getRotation()[0], + To.getRotation()[1], + To.getRotation()[2], + To.getRotation()[3]), + KDL::Vector(To.getPosition()[0], To.getPosition()[1], To.getPosition()[2])); } -inline Base::Placement toPlacement(const KDL::Frame &To){ - double x,y,z,w; - To.M.GetQuaternion(x,y,z,w); - return Base::Placement(Base::Vector3d(To.p[0],To.p[1],To.p[2]),Base::Rotation(x,y,z,w)); +inline Base::Placement toPlacement(const KDL::Frame& To) +{ + double x, y, z, w; + To.M.GetQuaternion(x, y, z, w); + return Base::Placement(Base::Vector3d(To.p[0], To.p[1], To.p[2]), Base::Rotation(x, y, z, w)); } -} //namespace Robot - +}// namespace Robot #endif diff --git a/src/Mod/Robot/App/RobotObject.cpp b/src/Mod/Robot/App/RobotObject.cpp index 4b332a3dc3..ed4b094ed3 100644 --- a/src/Mod/Robot/App/RobotObject.cpp +++ b/src/Mod/Robot/App/RobotObject.cpp @@ -38,25 +38,73 @@ PROPERTY_SOURCE(Robot::RobotObject, App::GeoFeature) RobotObject::RobotObject() { - ADD_PROPERTY_TYPE(RobotVrmlFile ,(nullptr),"Robot definition" ,Prop_None,"Included file with the VRML representation of the robot"); - ADD_PROPERTY_TYPE(RobotKinematicFile,(nullptr),"Robot definition",Prop_None,"Included file with kinematic definition of the robot Axis"); + ADD_PROPERTY_TYPE(RobotVrmlFile, + (nullptr), + "Robot definition", + Prop_None, + "Included file with the VRML representation of the robot"); + ADD_PROPERTY_TYPE(RobotKinematicFile, + (nullptr), + "Robot definition", + Prop_None, + "Included file with kinematic definition of the robot Axis"); - ADD_PROPERTY_TYPE(Axis1,(0.0),"Robot kinematic",Prop_None,"Axis 1 angle of the robot in degre"); - ADD_PROPERTY_TYPE(Axis2,(0.0),"Robot kinematic",Prop_None,"Axis 2 angle of the robot in degre"); - ADD_PROPERTY_TYPE(Axis3,(0.0),"Robot kinematic",Prop_None,"Axis 3 angle of the robot in degre"); - ADD_PROPERTY_TYPE(Axis4,(0.0),"Robot kinematic",Prop_None,"Axis 4 angle of the robot in degre"); - ADD_PROPERTY_TYPE(Axis5,(0.0),"Robot kinematic",Prop_None,"Axis 5 angle of the robot in degre"); - ADD_PROPERTY_TYPE(Axis6,(0.0),"Robot kinematic",Prop_None,"Axis 6 angle of the robot in degre"); - ADD_PROPERTY_TYPE(Error,("") ,"Robot kinematic",Prop_None,"Robot error while moving"); - - ADD_PROPERTY_TYPE(Tcp,(Base::Placement()),"Robot kinematic",Prop_None,"Tcp of the robot"); - ADD_PROPERTY_TYPE(Base,(Base::Placement()),"Robot kinematic",Prop_None,"Actual base frame of the robot"); - ADD_PROPERTY_TYPE(Tool,(Base::Placement()),"Robot kinematic",Prop_None,"Tool frame of the robot (Tool)"); - ADD_PROPERTY_TYPE(ToolShape,(nullptr),"Robot definition",Prop_None,"Link to the Shape is used as Tool"); - ADD_PROPERTY_TYPE(ToolBase ,(Base::Placement()),"Robot definition",Prop_None,"Defines where to connect the ToolShape"); - //ADD_PROPERTY_TYPE(Position,(Base::Placement()),"Robot definition",Prop_None,"Position of the robot in the simulation"); - ADD_PROPERTY_TYPE(Home ,(0),"Robot kinematic",Prop_None,"Axis position for home"); + ADD_PROPERTY_TYPE(Axis1, + (0.0), + "Robot kinematic", + Prop_None, + "Axis 1 angle of the robot in degre"); + ADD_PROPERTY_TYPE(Axis2, + (0.0), + "Robot kinematic", + Prop_None, + "Axis 2 angle of the robot in degre"); + ADD_PROPERTY_TYPE(Axis3, + (0.0), + "Robot kinematic", + Prop_None, + "Axis 3 angle of the robot in degre"); + ADD_PROPERTY_TYPE(Axis4, + (0.0), + "Robot kinematic", + Prop_None, + "Axis 4 angle of the robot in degre"); + ADD_PROPERTY_TYPE(Axis5, + (0.0), + "Robot kinematic", + Prop_None, + "Axis 5 angle of the robot in degre"); + ADD_PROPERTY_TYPE(Axis6, + (0.0), + "Robot kinematic", + Prop_None, + "Axis 6 angle of the robot in degre"); + ADD_PROPERTY_TYPE(Error, (""), "Robot kinematic", Prop_None, "Robot error while moving"); + ADD_PROPERTY_TYPE(Tcp, (Base::Placement()), "Robot kinematic", Prop_None, "Tcp of the robot"); + ADD_PROPERTY_TYPE(Base, + (Base::Placement()), + "Robot kinematic", + Prop_None, + "Actual base frame of the robot"); + ADD_PROPERTY_TYPE(Tool, + (Base::Placement()), + "Robot kinematic", + Prop_None, + "Tool frame of the robot (Tool)"); + ADD_PROPERTY_TYPE(ToolShape, + (nullptr), + "Robot definition", + Prop_None, + "Link to the Shape is used as Tool"); + ADD_PROPERTY_TYPE(ToolBase, + (Base::Placement()), + "Robot definition", + Prop_None, + "Defines where to connect the ToolShape"); + // ADD_PROPERTY_TYPE(Position,(Base::Placement()),"Robot definition",Prop_None,"Position of the + // robot in the simulation"); + ADD_PROPERTY_TYPE(Home, (0), "Robot kinematic", Prop_None, "Axis position for home"); } short RobotObject::mustExecute() const @@ -64,61 +112,61 @@ short RobotObject::mustExecute() const return 0; } -PyObject *RobotObject::getPyObject() +PyObject* RobotObject::getPyObject() { - if (PythonObject.is(Py::_None())){ + if (PythonObject.is(Py::_None())) { // ref counter is set to 1 - PythonObject = Py::Object(new DocumentObjectPy(this),true); + PythonObject = Py::Object(new DocumentObjectPy(this), true); } - return Py::new_reference_to(PythonObject); + return Py::new_reference_to(PythonObject); } void RobotObject::onChanged(const Property* prop) { - - if(prop == &RobotKinematicFile){ + + if (prop == &RobotKinematicFile) { // load the new kinematic robot.readKinematic(RobotKinematicFile.getValue()); } - if(prop == &Axis1 && !block){ - robot.setAxis(0,Axis1.getValue()); + if (prop == &Axis1 && !block) { + robot.setAxis(0, Axis1.getValue()); block = true; Tcp.setValue(robot.getTcp()); block = false; } - if(prop == &Axis2 && !block){ - robot.setAxis(1,Axis2.getValue()); + if (prop == &Axis2 && !block) { + robot.setAxis(1, Axis2.getValue()); block = true; Tcp.setValue(robot.getTcp()); block = false; } - if(prop == &Axis3 && !block){ - robot.setAxis(2,Axis3.getValue()); + if (prop == &Axis3 && !block) { + robot.setAxis(2, Axis3.getValue()); block = true; Tcp.setValue(robot.getTcp()); block = false; } - if(prop == &Axis4 && !block){ - robot.setAxis(3,Axis4.getValue()); + if (prop == &Axis4 && !block) { + robot.setAxis(3, Axis4.getValue()); block = true; Tcp.setValue(robot.getTcp()); block = false; } - if(prop == &Axis5 && !block){ - robot.setAxis(4,Axis5.getValue()); + if (prop == &Axis5 && !block) { + robot.setAxis(4, Axis5.getValue()); block = true; Tcp.setValue(robot.getTcp()); block = false; } - if(prop == &Axis6 && !block){ - robot.setAxis(5,Axis6.getValue()); + if (prop == &Axis6 && !block) { + robot.setAxis(5, Axis6.getValue()); block = true; Tcp.setValue(robot.getTcp()); block = false; } - if(prop == &Tcp && !block){ + if (prop == &Tcp && !block) { robot.setTo(Tcp.getValue()); block = true; Axis1.setValue((float)robot.getAxis(0)); @@ -132,25 +180,25 @@ void RobotObject::onChanged(const Property* prop) App::GeoFeature::onChanged(prop); } -void RobotObject::Save (Base::Writer &writer) const +void RobotObject::Save(Base::Writer& writer) const { App::GeoFeature::Save(writer); robot.Save(writer); } -void RobotObject::Restore(Base::XMLReader &reader) +void RobotObject::Restore(Base::XMLReader& reader) { block = true; App::GeoFeature::Restore(reader); robot.Restore(reader); - // set up the robot with the loaded axis position - robot.setAxis(0,Axis1.getValue()); - robot.setAxis(1,Axis2.getValue()); - robot.setAxis(2,Axis3.getValue()); - robot.setAxis(3,Axis4.getValue()); - robot.setAxis(4,Axis5.getValue()); - robot.setAxis(5,Axis6.getValue()); + // set up the robot with the loaded axis position + robot.setAxis(0, Axis1.getValue()); + robot.setAxis(1, Axis2.getValue()); + robot.setAxis(2, Axis3.getValue()); + robot.setAxis(3, Axis4.getValue()); + robot.setAxis(4, Axis5.getValue()); + robot.setAxis(5, Axis6.getValue()); robot.setTo(Tcp.getValue()); Tcp.setValue(robot.getTcp()); block = false; diff --git a/src/Mod/Robot/App/RobotObject.h b/src/Mod/Robot/App/RobotObject.h index 791eb7eb12..0f0f7244a9 100644 --- a/src/Mod/Robot/App/RobotObject.h +++ b/src/Mod/Robot/App/RobotObject.h @@ -32,7 +32,7 @@ namespace Robot { -class RobotExport RobotObject : public App::GeoFeature +class RobotExport RobotObject: public App::GeoFeature { PROPERTY_HEADER_WITH_OVERRIDE(Robot::RobotObject); @@ -41,46 +41,50 @@ public: RobotObject(); /// returns the type name of the ViewProvider - const char* getViewProviderName() const override { + const char* getViewProviderName() const override + { return "RobotGui::ViewProviderRobotObject"; } - App::DocumentObjectExecReturn *execute() override { + App::DocumentObjectExecReturn* execute() override + { return App::DocumentObject::StdReturn; } short mustExecute() const override; - PyObject *getPyObject() override; + PyObject* getPyObject() override; - void Save (Base::Writer &/*writer*/) const override; - void Restore(Base::XMLReader &/*reader*/) override; + void Save(Base::Writer& /*writer*/) const override; + void Restore(Base::XMLReader& /*reader*/) override; - Robot6Axis &getRobot(){return robot;} + Robot6Axis& getRobot() + { + return robot; + } App::PropertyFileIncluded RobotVrmlFile; App::PropertyFileIncluded RobotKinematicFile; - App::PropertyFloat Axis1,Axis2,Axis3,Axis4,Axis5,Axis6; + App::PropertyFloat Axis1, Axis2, Axis3, Axis4, Axis5, Axis6; - App::PropertyPlacement Base; - App::PropertyPlacement Tool; - App::PropertyLink ToolShape; - App::PropertyPlacement ToolBase; - App::PropertyPlacement Tcp; - //App::PropertyPlacement Position; + App::PropertyPlacement Base; + App::PropertyPlacement Tool; + App::PropertyLink ToolShape; + App::PropertyPlacement ToolBase; + App::PropertyPlacement Tcp; + // App::PropertyPlacement Position; - App::PropertyString Error; + App::PropertyString Error; App::PropertyFloatList Home; protected: /// get called by the container when a property has changed - void onChanged (const App::Property* prop) override; + void onChanged(const App::Property* prop) override; Robot6Axis robot; - bool block{false}; - + bool block {false}; }; -} //namespace Robot +}// namespace Robot -#endif // ROBOT_ROBOTOBJECT_H +#endif// ROBOT_ROBOTOBJECT_H diff --git a/src/Mod/Robot/App/RobotObjectPy.xml b/src/Mod/Robot/App/RobotObjectPy.xml index 390cc15c13..a514598461 100644 --- a/src/Mod/Robot/App/RobotObjectPy.xml +++ b/src/Mod/Robot/App/RobotObjectPy.xml @@ -1,13 +1,13 @@  - @@ -21,6 +21,5 @@ - diff --git a/src/Mod/Robot/App/RobotObjectPyImp.cpp b/src/Mod/Robot/App/RobotObjectPyImp.cpp index 086685286f..4bb779a232 100644 --- a/src/Mod/Robot/App/RobotObjectPyImp.cpp +++ b/src/Mod/Robot/App/RobotObjectPyImp.cpp @@ -22,9 +22,11 @@ #include "PreCompiled.h" +// clang-format off // inclusion of the generated files (generated out of RobotObjectPy.xml) #include "RobotObjectPy.h" #include "RobotObjectPy.cpp" +// clang-format on using namespace Robot; @@ -36,23 +38,19 @@ std::string RobotObjectPy::representation() const } - -PyObject* RobotObjectPy::getRobot(PyObject * /*args*/) +PyObject* RobotObjectPy::getRobot(PyObject* /*args*/) { PyErr_SetString(PyExc_NotImplementedError, "Not yet implemented"); return nullptr; } - -PyObject *RobotObjectPy::getCustomAttributes(const char* /*attr*/) const +PyObject* RobotObjectPy::getCustomAttributes(const char* /*attr*/) const { return nullptr; } int RobotObjectPy::setCustomAttributes(const char* /*attr*/, PyObject* /*obj*/) { - return 0; + return 0; } - - diff --git a/src/Mod/Robot/App/Simulation.cpp b/src/Mod/Robot/App/Simulation.cpp index 16ff7e3246..637a84bd58 100644 --- a/src/Mod/Robot/App/Simulation.cpp +++ b/src/Mod/Robot/App/Simulation.cpp @@ -33,14 +33,15 @@ using namespace KDL; // Simulation class //=========================================================================== -Simulation::Simulation(const Robot::Trajectory &Trac,Robot::Robot6Axis &Rob) -:Trac(Trac),Rob(Rob) +Simulation::Simulation(const Robot::Trajectory& Trac, Robot::Robot6Axis& Rob) + : Trac(Trac) + , Rob(Rob) { // simulate a trajectory with only one waypoint make no sense! assert(Trac.getSize() > 1); - - startAxis[0] = Rob.getAxis(0); + + startAxis[0] = Rob.getAxis(0); startAxis[1] = Rob.getAxis(1); startAxis[2] = Rob.getAxis(2); startAxis[3] = Rob.getAxis(3); @@ -48,26 +49,23 @@ Simulation::Simulation(const Robot::Trajectory &Trac,Robot::Robot6Axis &Rob) startAxis[5] = Rob.getAxis(5); setToTime(0); - } Simulation::~Simulation() = default; void Simulation::step(double tick) { - Pos += tick; + Pos += tick; } void Simulation::setToWaypoint(unsigned int) -{ - -} +{} void Simulation::setToTime(float t) { Pos = t; Base::Placement NeededPos = Trac.getPosition(Pos); - NeededPos = NeededPos *Tool.inverse(); + NeededPos = NeededPos * Tool.inverse(); Rob.setTo(NeededPos); Axis[0] = Rob.getAxis(0); Axis[1] = Rob.getAxis(1); @@ -75,20 +73,19 @@ void Simulation::setToTime(float t) Axis[3] = Rob.getAxis(3); Axis[4] = Rob.getAxis(4); Axis[5] = Rob.getAxis(5); - } void Simulation::reset() { - Rob.setAxis(0,startAxis[0]); - Rob.setAxis(1,startAxis[1]); - Rob.setAxis(2,startAxis[2]); - Rob.setAxis(3,startAxis[3]); - Rob.setAxis(4,startAxis[4]); - Rob.setAxis(5,startAxis[5]); + Rob.setAxis(0, startAxis[0]); + Rob.setAxis(1, startAxis[1]); + Rob.setAxis(2, startAxis[2]); + Rob.setAxis(3, startAxis[3]); + Rob.setAxis(4, startAxis[4]); + Rob.setAxis(5, startAxis[5]); Base::Placement NeededPos = Trac.getPosition(0.0); - NeededPos = NeededPos *Tool.inverse(); + NeededPos = NeededPos * Tool.inverse(); Rob.setTo(NeededPos); Axis[0] = Rob.getAxis(0); @@ -97,5 +94,4 @@ void Simulation::reset() Axis[3] = Rob.getAxis(3); Axis[4] = Rob.getAxis(4); Axis[5] = Rob.getAxis(5); - } diff --git a/src/Mod/Robot/App/Simulation.h b/src/Mod/Robot/App/Simulation.h index ac3ee59e8b..10ea2ab932 100644 --- a/src/Mod/Robot/App/Simulation.h +++ b/src/Mod/Robot/App/Simulation.h @@ -19,7 +19,7 @@ * Suite 330, Boston, MA 02111-1307, USA * * * ***************************************************************************/ - + #ifndef _Simulation_h_ #define _Simulation_h_ @@ -38,35 +38,45 @@ class RobotExport Simulation { public: - /// Constructor - Simulation(const Trajectory &Trac,Robot6Axis &Rob); - virtual ~Simulation(); + /// Constructor + Simulation(const Trajectory& Trac, Robot6Axis& Rob); + virtual ~Simulation(); - double getLength(){return Trac.getLength();} - double getDuration(){return Trac.getDuration();} + double getLength() + { + return Trac.getLength(); + } + double getDuration() + { + return Trac.getDuration(); + } - Base::Placement getPosition(){return Trac.getPosition(Pos);} - double getVelocity(){return Trac.getVelocity(Pos);} + Base::Placement getPosition() + { + return Trac.getPosition(Pos); + } + double getVelocity() + { + return Trac.getVelocity(Pos); + } - void step(double tick); + void step(double tick); void setToWaypoint(unsigned int n); void setToTime(float t); // apply the start axis angles and set to time 0. Restores the exact start position void reset(); - double Pos{0.0}; - double Axis[6]{}; - double startAxis[6]{}; + double Pos {0.0}; + double Axis[6] {}; + double startAxis[6] {}; Trajectory Trac; - Robot6Axis &Rob; + Robot6Axis& Rob; Base::Placement Tool; }; - -} //namespace Robot - +}// namespace Robot #endif diff --git a/src/Mod/Robot/App/Trajectory.cpp b/src/Mod/Robot/App/Trajectory.cpp index 6b48567373..2b11feb752 100644 --- a/src/Mod/Robot/App/Trajectory.cpp +++ b/src/Mod/Robot/App/Trajectory.cpp @@ -23,69 +23,76 @@ #include "PreCompiled.h" #ifndef _PreComp_ -# include +#include -# include "kdl_cp/path_line.hpp" -# include "kdl_cp/path_roundedcomposite.hpp" -# include "kdl_cp/rotational_interpolation_sa.hpp" -# include "kdl_cp/trajectory_composite.hpp" -# include "kdl_cp/trajectory_segment.hpp" -# include "kdl_cp/utilities/error.h" -# include "kdl_cp/velocityprofile_trap.hpp" +#include "kdl_cp/path_line.hpp" +#include "kdl_cp/path_roundedcomposite.hpp" +#include "kdl_cp/rotational_interpolation_sa.hpp" +#include "kdl_cp/trajectory_composite.hpp" +#include "kdl_cp/trajectory_segment.hpp" +#include "kdl_cp/utilities/error.h" +#include "kdl_cp/velocityprofile_trap.hpp" #endif #include #include #include -#include "Trajectory.h" #include "RobotAlgos.h" +#include "Trajectory.h" #ifndef M_PI - #define M_PI 3.14159265358979323846 - #define M_PI 3.14159265358979323846 /* pi */ +#define M_PI 3.14159265358979323846 +#define M_PI 3.14159265358979323846 /* pi */ #endif #ifndef M_PI_2 - #define M_PI_2 1.57079632679489661923 /* pi/2 */ +#define M_PI_2 1.57079632679489661923 /* pi/2 */ #endif using namespace Robot; using namespace Base; -//using namespace KDL; +// using namespace KDL; -TYPESYSTEM_SOURCE(Robot::Trajectory , Base::Persistence) +TYPESYSTEM_SOURCE(Robot::Trajectory, Base::Persistence) Trajectory::Trajectory() = default; Trajectory::Trajectory(const Trajectory& Trac) -:vpcWaypoints(Trac.vpcWaypoints.size()),pcTrajectory(nullptr) + : vpcWaypoints(Trac.vpcWaypoints.size()) + , pcTrajectory(nullptr) { operator=(Trac); } Trajectory::~Trajectory() { - for(auto it : vpcWaypoints) + for (auto it : vpcWaypoints) { delete it; + } delete pcTrajectory; } -Trajectory &Trajectory::operator=(const Trajectory& Trac) +Trajectory& Trajectory::operator=(const Trajectory& Trac) { - if (this == &Trac) + if (this == &Trac) { return *this; + } - for(auto it : vpcWaypoints) + for (auto it : vpcWaypoints) { delete it; + } vpcWaypoints.clear(); vpcWaypoints.resize(Trac.vpcWaypoints.size()); - int i=0; - for (std::vector::const_iterator it=Trac.vpcWaypoints.begin();it!=Trac.vpcWaypoints.end();++it,i++) + int i = 0; + for (std::vector::const_iterator it = Trac.vpcWaypoints.begin(); + it != Trac.vpcWaypoints.end(); + ++it, i++) { vpcWaypoints[i] = new Waypoint(**it); + } generateTrajectory(); return *this; @@ -93,60 +100,72 @@ Trajectory &Trajectory::operator=(const Trajectory& Trac) double Trajectory::getLength(int n) const { - if(pcTrajectory) - if(n<0) + if (pcTrajectory) { + if (n < 0) { return pcTrajectory->GetPath()->PathLength(); - else + } + else { return pcTrajectory->Get(n)->GetPath()->PathLength(); - else + } + } + else { return 0; + } } double Trajectory::getDuration(int n) const { - if(pcTrajectory) - if(n<0) + if (pcTrajectory) { + if (n < 0) { return pcTrajectory->Duration(); - else + } + else { return pcTrajectory->Get(n)->Duration(); - else + } + } + else { return 0; + } } -Placement Trajectory::getPosition(double time)const +Placement Trajectory::getPosition(double time) const { - if (pcTrajectory) + if (pcTrajectory) { return Placement(toPlacement(pcTrajectory->Pos(time))); + } return {}; } -double Trajectory::getVelocity(double time)const +double Trajectory::getVelocity(double time) const { - if(pcTrajectory){ + if (pcTrajectory) { KDL::Vector vec = pcTrajectory->Vel(time).vel; - Base::Vector3d vec2(vec[0],vec[1],vec[2]); + Base::Vector3d vec2(vec[0], vec[1], vec[2]); return vec2.Length(); - }else + } + else { return 0; + } } void Trajectory::deleteLast(unsigned int n) { - for(unsigned int i=0;i<=n;i++){ - delete(*vpcWaypoints.rbegin()); + for (unsigned int i = 0; i <= n; i++) { + delete (*vpcWaypoints.rbegin()); vpcWaypoints.pop_back(); } - } void Trajectory::generateTrajectory() { - if (vpcWaypoints.empty()) + if (vpcWaypoints.empty()) { return; + } // delete the old and create a new one - if (pcTrajectory) + if (pcTrajectory) { delete (pcTrajectory); + } pcTrajectory = new KDL::Trajectory_Composite(); // pointer to the pieces while iterating @@ -159,7 +178,9 @@ void Trajectory::generateTrajectory() // handle the first waypoint special bool first = true; - for (std::vector::const_iterator it = vpcWaypoints.begin(); it != vpcWaypoints.end(); ++it) { + for (std::vector::const_iterator it = vpcWaypoints.begin(); + it != vpcWaypoints.end(); + ++it) { if (first) { Last = toFrame((*it)->EndPos); first = false; @@ -167,59 +188,70 @@ void Trajectory::generateTrajectory() else { // destinct the type of movement switch ((*it)->Type) { - case Waypoint::LINE: - case Waypoint::PTP: { - KDL::Frame Next = toFrame((*it)->EndPos); - // continues the movement until no continuous waypoint or the end - bool Cont = (*it)->Cont && !(it == --vpcWaypoints.end()); - // start of a continue block - if (Cont && !pcRoundComp) { - pcRoundComp = std::make_unique(3, 3, - new KDL::RotationalInterpolation_SingleAxis()); - // the velocity of the first waypoint is used - pcVelPrf = std::make_unique((*it)->Velocity, (*it)->Acceleration); - pcRoundComp->Add(Last); - pcRoundComp->Add(Next); + case Waypoint::LINE: + case Waypoint::PTP: { + KDL::Frame Next = toFrame((*it)->EndPos); + // continues the movement until no continuous waypoint or the end + bool Cont = (*it)->Cont && !(it == --vpcWaypoints.end()); + // start of a continue block + if (Cont && !pcRoundComp) { + pcRoundComp = std::make_unique( + 3, + 3, + new KDL::RotationalInterpolation_SingleAxis()); + // the velocity of the first waypoint is used + pcVelPrf = + std::make_unique((*it)->Velocity, + (*it)->Acceleration); + pcRoundComp->Add(Last); + pcRoundComp->Add(Next); - // continue a continues block - } - else if (Cont && pcRoundComp) { - pcRoundComp->Add(Next); - // end a continuous block - } - else if (!Cont && pcRoundComp) { - // add the last one - pcRoundComp->Add(Next); - pcRoundComp->Finish(); - pcVelPrf->SetProfile(0, pcRoundComp->PathLength()); - pcTrak = std::make_unique(pcRoundComp.release(), pcVelPrf.release()); + // continue a continues block + } + else if (Cont && pcRoundComp) { + pcRoundComp->Add(Next); + // end a continuous block + } + else if (!Cont && pcRoundComp) { + // add the last one + pcRoundComp->Add(Next); + pcRoundComp->Finish(); + pcVelPrf->SetProfile(0, pcRoundComp->PathLength()); + pcTrak = + std::make_unique(pcRoundComp.release(), + pcVelPrf.release()); - // normal block - } - else if (!Cont && !pcRoundComp) { - KDL::Path* pcPath; - pcPath = new KDL::Path_Line(Last, - Next, - new KDL::RotationalInterpolation_SingleAxis(), - 1.0, - true - ); + // normal block + } + else if (!Cont && !pcRoundComp) { + KDL::Path* pcPath; + pcPath = + new KDL::Path_Line(Last, + Next, + new KDL::RotationalInterpolation_SingleAxis(), + 1.0, + true); - pcVelPrf = std::make_unique((*it)->Velocity, (*it)->Acceleration); - pcVelPrf->SetProfile(0, pcPath->PathLength()); - pcTrak = std::make_unique(pcPath, pcVelPrf.release()); + pcVelPrf = + std::make_unique((*it)->Velocity, + (*it)->Acceleration); + pcVelPrf->SetProfile(0, pcPath->PathLength()); + pcTrak = std::make_unique(pcPath, + pcVelPrf.release()); + } + Last = Next; + break; } - Last = Next; - break; } - case Waypoint::WAIT: - break; - default: - break; + case Waypoint::WAIT: + break; + default: + break; } // add the segment if no continuous block is running - if (!pcRoundComp && pcTrak) + if (!pcRoundComp && pcTrak) { pcTrajectory->Add(pcTrak.release()); + } } } } @@ -228,27 +260,33 @@ void Trajectory::generateTrajectory() } } -std::string Trajectory::getUniqueWaypointName(const char *Name) const +std::string Trajectory::getUniqueWaypointName(const char* Name) const { - if (!Name || *Name == '\0') + if (!Name || *Name == '\0') { return {}; + } // check for first character whether it's a digit std::string CleanName = Name; - if (!CleanName.empty() && CleanName[0] >= 48 && CleanName[0] <= 57) + if (!CleanName.empty() && CleanName[0] >= 48 && CleanName[0] <= 57) { CleanName[0] = '_'; + } // strip illegal chars - for (char & it : CleanName) { - if (!((it>=48 && it<=57) || // number - (it>=65 && it<=90) || // uppercase letter - (it>=97 && it<=122))) // lowercase letter - it = '_'; // it's neither number nor letter + for (char& it : CleanName) { + if (!((it >= 48 && it <= 57) || // number + (it >= 65 && it <= 90) || // uppercase letter + (it >= 97 && it <= 122))) {// lowercase letter + it = '_'; // it's neither number nor letter + } } // name in use? std::vector::const_iterator it; - for(it = vpcWaypoints.begin();it!=vpcWaypoints.end();++it) - if((*it)->Name == CleanName) break; + for (it = vpcWaypoints.begin(); it != vpcWaypoints.end(); ++it) { + if ((*it)->Name == CleanName) { + break; + } + } if (it == vpcWaypoints.end()) { // if not, name is OK @@ -257,14 +295,15 @@ std::string Trajectory::getUniqueWaypointName(const char *Name) const else { // find highest suffix int nSuff = 0; - for(it = vpcWaypoints.begin();it!=vpcWaypoints.end();++it) { - const std::string &ObjName = (*it)->Name; - if (ObjName.substr(0, CleanName.length()) == CleanName) { // same prefix + for (it = vpcWaypoints.begin(); it != vpcWaypoints.end(); ++it) { + const std::string& ObjName = (*it)->Name; + if (ObjName.substr(0, CleanName.length()) == CleanName) {// same prefix std::string clSuffix(ObjName.substr(CleanName.length())); if (!clSuffix.empty()) { std::string::size_type nPos = clSuffix.find_first_not_of("0123456789"); - if (nPos==std::string::npos) + if (nPos == std::string::npos) { nSuff = std::max(nSuff, std::atol(clSuffix.c_str())); + } } } } @@ -275,34 +314,32 @@ std::string Trajectory::getUniqueWaypointName(const char *Name) const } } -void Trajectory::addWaypoint(const Waypoint &WPnt) +void Trajectory::addWaypoint(const Waypoint& WPnt) { std::string UniqueName = getUniqueWaypointName(WPnt.Name.c_str()); - Waypoint *tmp = new Waypoint(WPnt); + Waypoint* tmp = new Waypoint(WPnt); tmp->Name = UniqueName; vpcWaypoints.push_back(tmp); } - - -unsigned int Trajectory::getMemSize () const +unsigned int Trajectory::getMemSize() const { - return 0; + return 0; } -void Trajectory::Save (Writer &writer) const +void Trajectory::Save(Writer& writer) const { - writer.Stream() << writer.ind() << "" << std::endl; + writer.Stream() << writer.ind() << "" << std::endl; writer.incInd(); - for(unsigned int i = 0;iSave(writer); + } writer.decInd(); - writer.Stream() << writer.ind() << "" << std::endl ; - + writer.Stream() << writer.ind() << "" << std::endl; } -void Trajectory::Restore(XMLReader &reader) +void Trajectory::Restore(XMLReader& reader) { vpcWaypoints.clear(); // read my element @@ -312,7 +349,7 @@ void Trajectory::Restore(XMLReader &reader) vpcWaypoints.resize(count); for (int i = 0; i < count; i++) { - Waypoint *tmp = new Waypoint(); + Waypoint* tmp = new Waypoint(); tmp->Restore(reader); vpcWaypoints[i] = tmp; } diff --git a/src/Mod/Robot/App/Trajectory.h b/src/Mod/Robot/App/Trajectory.h index 1e6c7d56cc..e5a8a1cd17 100644 --- a/src/Mod/Robot/App/Trajectory.h +++ b/src/Mod/Robot/App/Trajectory.h @@ -31,7 +31,10 @@ #include "Waypoint.h" -namespace KDL {class Trajectory_Composite;} +namespace KDL +{ +class Trajectory_Composite; +} namespace Robot { @@ -39,7 +42,7 @@ namespace Robot /** The representation of a Trajectory */ -class RobotExport Trajectory : public Base::Persistence +class RobotExport Trajectory: public Base::Persistence { TYPESYSTEM_HEADER_WITH_OVERRIDE(); @@ -48,39 +51,47 @@ public: Trajectory(const Trajectory&); ~Trajectory() override; - Trajectory &operator=(const Trajectory&); + Trajectory& operator=(const Trajectory&); - // from base class - unsigned int getMemSize () const override; - void Save (Base::Writer &/*writer*/) const override; - void Restore(Base::XMLReader &/*reader*/) override; + // from base class + unsigned int getMemSize() const override; + void Save(Base::Writer& /*writer*/) const override; + void Restore(Base::XMLReader& /*reader*/) override; - // interface + // interface void generateTrajectory(); - void addWaypoint(const Waypoint &WPnt); - unsigned int getSize() const{return vpcWaypoints.size();} - const Waypoint &getWaypoint(unsigned int pos)const {return *vpcWaypoints[pos];} - std::string getUniqueWaypointName(const char *Name) const; - const std::vector &getWaypoints()const{return vpcWaypoints;} + void addWaypoint(const Waypoint& WPnt); + unsigned int getSize() const + { + return vpcWaypoints.size(); + } + const Waypoint& getWaypoint(unsigned int pos) const + { + return *vpcWaypoints[pos]; + } + std::string getUniqueWaypointName(const char* Name) const; + const std::vector& getWaypoints() const + { + return vpcWaypoints; + } /// delete the last n waypoints - void deleteLast(unsigned int n=1); + void deleteLast(unsigned int n = 1); /// return the Length (mm) of the Trajectory if -1 or of the Waypoint with the given number - double getLength(int n=-1) const; + double getLength(int n = -1) const; /// return the duration (s) of the Trajectory if -1 or of the Waypoint with the given number - double getDuration (int n=-1) const; - Base::Placement getPosition(double time)const; - double getVelocity(double time)const; + double getDuration(int n = -1) const; + Base::Placement getPosition(double time) const; + double getVelocity(double time) const; protected: - std::vector vpcWaypoints; - KDL::Trajectory_Composite *pcTrajectory{nullptr}; + KDL::Trajectory_Composite* pcTrajectory {nullptr}; }; -} //namespace Part +}// namespace Robot -#endif // PART_TOPOSHAPE_H +#endif// PART_TOPOSHAPE_H diff --git a/src/Mod/Robot/App/TrajectoryCompound.cpp b/src/Mod/Robot/App/TrajectoryCompound.cpp index 8cb30a115a..7e4ce7b341 100644 --- a/src/Mod/Robot/App/TrajectoryCompound.cpp +++ b/src/Mod/Robot/App/TrajectoryCompound.cpp @@ -35,38 +35,41 @@ PROPERTY_SOURCE(Robot::TrajectoryCompound, Robot::TrajectoryObject) TrajectoryCompound::TrajectoryCompound() { - ADD_PROPERTY_TYPE( Source, (nullptr) , "Compound",Prop_None,"list of trajectories to combine"); - + ADD_PROPERTY_TYPE(Source, (nullptr), "Compound", Prop_None, "list of trajectories to combine"); } -App::DocumentObjectExecReturn *TrajectoryCompound::execute() +App::DocumentObjectExecReturn* TrajectoryCompound::execute() { - const std::vector &Tracs = Source.getValues(); + const std::vector& Tracs = Source.getValues(); Robot::Trajectory result; for (auto it : Tracs) { - if (it->getTypeId().isDerivedFrom(Robot::TrajectoryObject::getClassTypeId())){ - const std::vector &wps = static_cast(it)->Trajectory.getValue().getWaypoints(); + if (it->getTypeId().isDerivedFrom(Robot::TrajectoryObject::getClassTypeId())) { + const std::vector& wps = + static_cast(it)->Trajectory.getValue().getWaypoints(); for (auto wp : wps) { result.addWaypoint(*wp); } - }else - return new App::DocumentObjectExecReturn("Not all objects in compound are trajectories!"); + } + else { + return new App::DocumentObjectExecReturn( + "Not all objects in compound are trajectories!"); + } } Trajectory.setValue(result); - + return App::DocumentObject::StdReturn; } -//short TrajectoryCompound::mustExecute(void) const +// short TrajectoryCompound::mustExecute(void) const //{ -// return 0; -//} +// return 0; +// } -//void TrajectoryCompound::onChanged(const Property* prop) +// void TrajectoryCompound::onChanged(const Property* prop) //{ -// -// App::GeoFeature::onChanged(prop); -//} +// +// App::GeoFeature::onChanged(prop); +// } diff --git a/src/Mod/Robot/App/TrajectoryCompound.h b/src/Mod/Robot/App/TrajectoryCompound.h index 8c39fed289..0202179e8b 100644 --- a/src/Mod/Robot/App/TrajectoryCompound.h +++ b/src/Mod/Robot/App/TrajectoryCompound.h @@ -31,7 +31,7 @@ namespace Robot { -class RobotExport TrajectoryCompound : public TrajectoryObject +class RobotExport TrajectoryCompound: public TrajectoryObject { PROPERTY_HEADER_WITH_OVERRIDE(Robot::TrajectoryObject); @@ -39,21 +39,21 @@ public: /// Constructor TrajectoryCompound(); - App::PropertyLinkList Source; + App::PropertyLinkList Source; /// returns the type name of the ViewProvider - const char* getViewProviderName() const override { + const char* getViewProviderName() const override + { return "RobotGui::ViewProviderTrajectoryCompound"; } - App::DocumentObjectExecReturn *execute() override; + App::DocumentObjectExecReturn* execute() override; protected: /// get called by the container when a property has changed - //virtual void onChanged (const App::Property* prop); - + // virtual void onChanged (const App::Property* prop); }; -} //namespace Robot +}// namespace Robot -#endif // ROBOT_ROBOTOBJECT_H +#endif// ROBOT_ROBOTOBJECT_H diff --git a/src/Mod/Robot/App/TrajectoryDressUpObject.cpp b/src/Mod/Robot/App/TrajectoryDressUpObject.cpp index 2eaa55af32..d2c736ee9c 100644 --- a/src/Mod/Robot/App/TrajectoryDressUpObject.cpp +++ b/src/Mod/Robot/App/TrajectoryDressUpObject.cpp @@ -31,23 +31,46 @@ using namespace App; PROPERTY_SOURCE(Robot::TrajectoryDressUpObject, Robot::TrajectoryObject) -const char* TrajectoryDressUpObject::ContTypeEnums[]= {"DontChange","Continues","Discontinues",nullptr}; -const char* TrajectoryDressUpObject::AddTypeEnums[] = {"DontChange","UseOrientation","AddPosition","AddOrintation","AddPositionAndOrientation",nullptr}; +const char* TrajectoryDressUpObject::ContTypeEnums[] = {"DontChange", + "Continues", + "Discontinues", + nullptr}; +const char* TrajectoryDressUpObject::AddTypeEnums[] = {"DontChange", + "UseOrientation", + "AddPosition", + "AddOrintation", + "AddPositionAndOrientation", + nullptr}; TrajectoryDressUpObject::TrajectoryDressUpObject() { - ADD_PROPERTY_TYPE( Source, (nullptr) , "TrajectoryDressUp",Prop_None,"Trajectory to dress up"); - ADD_PROPERTY_TYPE( Speed, (1000) , "TrajectoryDressUp",Prop_None,"Speed to use"); - ADD_PROPERTY_TYPE( UseSpeed , (0) , "TrajectoryDressUp",Prop_None,"Switch the speed usage on"); - ADD_PROPERTY_TYPE( Acceleration, (1000) , "TrajectoryDressUp",Prop_None,"Acceleration to use"); - ADD_PROPERTY_TYPE( UseAcceleration, (0) , "TrajectoryDressUp",Prop_None,"Switch the acceleration usage on"); - ADD_PROPERTY_TYPE( ContType, ((long)0) , "TrajectoryDressUp",Prop_None,"Define the dress up of continuity"); + ADD_PROPERTY_TYPE(Source, (nullptr), "TrajectoryDressUp", Prop_None, "Trajectory to dress up"); + ADD_PROPERTY_TYPE(Speed, (1000), "TrajectoryDressUp", Prop_None, "Speed to use"); + ADD_PROPERTY_TYPE(UseSpeed, (0), "TrajectoryDressUp", Prop_None, "Switch the speed usage on"); + ADD_PROPERTY_TYPE(Acceleration, (1000), "TrajectoryDressUp", Prop_None, "Acceleration to use"); + ADD_PROPERTY_TYPE(UseAcceleration, + (0), + "TrajectoryDressUp", + Prop_None, + "Switch the acceleration usage on"); + ADD_PROPERTY_TYPE(ContType, + ((long)0), + "TrajectoryDressUp", + Prop_None, + "Define the dress up of continuity"); ContType.setEnums(ContTypeEnums); - ADD_PROPERTY_TYPE( PosAdd, (Base::Placement()) , "TrajectoryDressUp",Prop_None,"Position & Orientation to use"); - ADD_PROPERTY_TYPE( AddType, ((long)0) , "TrajectoryDressUp",Prop_None,"How to change the Position & Orientation"); + ADD_PROPERTY_TYPE(PosAdd, + (Base::Placement()), + "TrajectoryDressUp", + Prop_None, + "Position & Orientation to use"); + ADD_PROPERTY_TYPE(AddType, + ((long)0), + "TrajectoryDressUp", + Prop_None, + "How to change the Position & Orientation"); AddType.setEnums(AddTypeEnums); - } App::DocumentObjectExecReturn* TrajectoryDressUpObject::execute() @@ -55,44 +78,57 @@ App::DocumentObjectExecReturn* TrajectoryDressUpObject::execute() Robot::Trajectory result; App::DocumentObject* link = Source.getValue(); - if (!link) + if (!link) { return new App::DocumentObjectExecReturn("No object linked"); - if (!link->getTypeId().isDerivedFrom(Robot::TrajectoryObject::getClassTypeId())) + } + if (!link->getTypeId().isDerivedFrom(Robot::TrajectoryObject::getClassTypeId())) { return new App::DocumentObjectExecReturn("Linked object is not a Trajectory object"); + } - const std::vector& wps = static_cast(link)->Trajectory.getValue().getWaypoints(); + const std::vector& wps = + static_cast(link)->Trajectory.getValue().getWaypoints(); for (auto wp : wps) { Waypoint wpt = *wp; - if (UseSpeed.getValue()) + if (UseSpeed.getValue()) { wpt.Velocity = Speed.getValue(); - if (UseAcceleration.getValue()) + } + if (UseAcceleration.getValue()) { wpt.Acceleration = Acceleration.getValue(); + } switch (ContType.getValue()) { - case 0: break; - case 1: wpt.Cont = true; break; - case 2: wpt.Cont = false; break; - default: assert(0); // must not happen! + case 0: + break; + case 1: + wpt.Cont = true; + break; + case 2: + wpt.Cont = false; + break; + default: + assert(0);// must not happen! } switch (AddType.getValue()) { - // do nothing - case 0: break; - // use orientation - case 1: - wpt.EndPos.setRotation(PosAdd.getValue().getRotation()); - break; - // add position - case 2: - wpt.EndPos.setPosition(wpt.EndPos.getPosition() + PosAdd.getValue().getPosition()); - break; - // add orientation - case 3: - wpt.EndPos.setRotation(wpt.EndPos.getRotation() * PosAdd.getValue().getRotation()); - break; - // add orientation & position - case 4: - wpt.EndPos = wpt.EndPos * PosAdd.getValue(); - break; - default: assert(0); // must not happen! + // do nothing + case 0: + break; + // use orientation + case 1: + wpt.EndPos.setRotation(PosAdd.getValue().getRotation()); + break; + // add position + case 2: + wpt.EndPos.setPosition(wpt.EndPos.getPosition() + PosAdd.getValue().getPosition()); + break; + // add orientation + case 3: + wpt.EndPos.setRotation(wpt.EndPos.getRotation() * PosAdd.getValue().getRotation()); + break; + // add orientation & position + case 4: + wpt.EndPos = wpt.EndPos * PosAdd.getValue(); + break; + default: + assert(0);// must not happen! } result.addWaypoint(wpt); diff --git a/src/Mod/Robot/App/TrajectoryDressUpObject.h b/src/Mod/Robot/App/TrajectoryDressUpObject.h index 712fda73bb..dfad44e2fe 100644 --- a/src/Mod/Robot/App/TrajectoryDressUpObject.h +++ b/src/Mod/Robot/App/TrajectoryDressUpObject.h @@ -32,7 +32,7 @@ namespace Robot { -class RobotExport TrajectoryDressUpObject : public TrajectoryObject +class RobotExport TrajectoryDressUpObject: public TrajectoryObject { PROPERTY_HEADER_WITH_OVERRIDE(Robot::TrajectoryObject); @@ -40,33 +40,32 @@ public: /// Constructor TrajectoryDressUpObject(); - App::PropertyLink Source; - App::PropertySpeed Speed; - App::PropertyBool UseSpeed; + App::PropertyLink Source; + App::PropertySpeed Speed; + App::PropertyBool UseSpeed; App::PropertyAcceleration Acceleration; - App::PropertyBool UseAcceleration; - App::PropertyEnumeration ContType; - App::PropertyPlacement PosAdd; - App::PropertyEnumeration AddType; + App::PropertyBool UseAcceleration; + App::PropertyEnumeration ContType; + App::PropertyPlacement PosAdd; + App::PropertyEnumeration AddType; /// returns the type name of the ViewProvider - const char* getViewProviderName() const override { + const char* getViewProviderName() const override + { return "RobotGui::ViewProviderTrajectoryDressUp"; } - App::DocumentObjectExecReturn *execute() override; + App::DocumentObjectExecReturn* execute() override; static const char* ContTypeEnums[]; static const char* AddTypeEnums[]; protected: /// get called by the container when a property has changed - void onChanged (const App::Property* prop) override; - - + void onChanged(const App::Property* prop) override; }; -} //namespace Robot +}// namespace Robot -#endif // ROBOT_ROBOTOBJECT_H +#endif// ROBOT_ROBOTOBJECT_H diff --git a/src/Mod/Robot/App/TrajectoryObject.cpp b/src/Mod/Robot/App/TrajectoryObject.cpp index 8dd8447ef4..c627f34a0a 100644 --- a/src/Mod/Robot/App/TrajectoryObject.cpp +++ b/src/Mod/Robot/App/TrajectoryObject.cpp @@ -37,9 +37,16 @@ PROPERTY_SOURCE(Robot::TrajectoryObject, App::GeoFeature) TrajectoryObject::TrajectoryObject() { - ADD_PROPERTY_TYPE(Base, (Base::Placement()), "Trajectory", Prop_None, "Base frame of the trajectory"); - ADD_PROPERTY_TYPE(Trajectory, (Robot::Trajectory()), "Trajectory", Prop_None, "Trajectory object"); - + ADD_PROPERTY_TYPE(Base, + (Base::Placement()), + "Trajectory", + Prop_None, + "Base frame of the trajectory"); + ADD_PROPERTY_TYPE(Trajectory, + (Robot::Trajectory()), + "Trajectory", + Prop_None, + "Trajectory object"); } short TrajectoryObject::mustExecute() const @@ -47,13 +54,13 @@ short TrajectoryObject::mustExecute() const return 0; } -PyObject *TrajectoryObject::getPyObject() +PyObject* TrajectoryObject::getPyObject() { - if (PythonObject.is(Py::_None())){ + if (PythonObject.is(Py::_None())) { // ref counter is set to 1 - PythonObject = Py::Object(new DocumentObjectPy(this),true); + PythonObject = Py::Object(new DocumentObjectPy(this), true); } - return Py::new_reference_to(PythonObject); + return Py::new_reference_to(PythonObject); } void TrajectoryObject::onChanged(const Property* prop) diff --git a/src/Mod/Robot/App/TrajectoryObject.h b/src/Mod/Robot/App/TrajectoryObject.h index e0b6215b25..f347d2e45c 100644 --- a/src/Mod/Robot/App/TrajectoryObject.h +++ b/src/Mod/Robot/App/TrajectoryObject.h @@ -26,14 +26,14 @@ #include #include -#include "Trajectory.h" #include "PropertyTrajectory.h" +#include "Trajectory.h" namespace Robot { -class RobotExport TrajectoryObject : public App::GeoFeature +class RobotExport TrajectoryObject: public App::GeoFeature { PROPERTY_HEADER_WITH_OVERRIDE(Robot::TrajectoryObject); @@ -42,26 +42,27 @@ public: TrajectoryObject(); /// returns the type name of the ViewProvider - const char* getViewProviderName() const override { + const char* getViewProviderName() const override + { return "RobotGui::ViewProviderTrajectory"; } - App::DocumentObjectExecReturn *execute() override { + App::DocumentObjectExecReturn* execute() override + { return App::DocumentObject::StdReturn; } short mustExecute() const override; - PyObject *getPyObject() override; + PyObject* getPyObject() override; App::PropertyPlacement Base; - PropertyTrajectory Trajectory; + PropertyTrajectory Trajectory; protected: /// get called by the container when a property has changed - void onChanged (const App::Property* prop) override; - + void onChanged(const App::Property* prop) override; }; -} //namespace Robot +}// namespace Robot -#endif // ROBOT_ROBOTOBJECT_H +#endif// ROBOT_ROBOTOBJECT_H diff --git a/src/Mod/Robot/App/TrajectoryPy.xml b/src/Mod/Robot/App/TrajectoryPy.xml index 878b29a3ac..4d87d81721 100644 --- a/src/Mod/Robot/App/TrajectoryPy.xml +++ b/src/Mod/Robot/App/TrajectoryPy.xml @@ -1,13 +1,13 @@ - diff --git a/src/Mod/Robot/App/TrajectoryPyImp.cpp b/src/Mod/Robot/App/TrajectoryPyImp.cpp index 1dcd42193c..dce4e77f38 100644 --- a/src/Mod/Robot/App/TrajectoryPyImp.cpp +++ b/src/Mod/Robot/App/TrajectoryPyImp.cpp @@ -24,10 +24,12 @@ #include +// clang-format off // inclusion of the generated files (generated out of TrajectoryPy.xml) #include #include #include +// clang-format on using namespace Robot; @@ -46,24 +48,26 @@ std::string TrajectoryPy::representation() const return str.str(); } -PyObject *TrajectoryPy::PyMake(struct _typeobject *, PyObject *, PyObject *) // Python wrapper +PyObject* TrajectoryPy::PyMake(struct _typeobject*, PyObject*, PyObject*)// Python wrapper { - // create a new instance of TrajectoryPy and the Twin object + // create a new instance of TrajectoryPy and the Twin object return new TrajectoryPy(new Trajectory); } // constructor method int TrajectoryPy::PyInit(PyObject* args, PyObject* /*kwd*/) { - PyObject *pcObj=nullptr; - if (!PyArg_ParseTuple(args, "|O!", &(PyList_Type), &pcObj)) + PyObject* pcObj = nullptr; + if (!PyArg_ParseTuple(args, "|O!", &(PyList_Type), &pcObj)) { return -1; + } if (pcObj) { Py::List list(pcObj); for (Py::List::iterator it = list.begin(); it != list.end(); ++it) { if (PyObject_TypeCheck((*it).ptr(), &(Robot::WaypointPy::Type))) { - Robot::Waypoint &wp = *static_cast((*it).ptr())->getWaypointPtr(); + Robot::Waypoint& wp = + *static_cast((*it).ptr())->getWaypointPtr(); getTrajectoryPtr()->addWaypoint(wp); } } @@ -73,13 +77,13 @@ int TrajectoryPy::PyInit(PyObject* args, PyObject* /*kwd*/) } -PyObject* TrajectoryPy::insertWaypoints(PyObject * args) +PyObject* TrajectoryPy::insertWaypoints(PyObject* args) { PyObject* o; if (PyArg_ParseTuple(args, "O!", &(Base::PlacementPy::Type), &o)) { - Base::Placement *plm = static_cast(o)->getPlacementPtr(); - getTrajectoryPtr()->addWaypoint(Robot::Waypoint("Pt",*plm)); + Base::Placement* plm = static_cast(o)->getPlacementPtr(); + getTrajectoryPtr()->addWaypoint(Robot::Waypoint("Pt", *plm)); getTrajectoryPtr()->generateTrajectory(); return new TrajectoryPy(new Robot::Trajectory(*getTrajectoryPtr())); @@ -87,12 +91,12 @@ PyObject* TrajectoryPy::insertWaypoints(PyObject * args) PyErr_Clear(); if (PyArg_ParseTuple(args, "O!", &(Robot::WaypointPy::Type), &o)) { - Robot::Waypoint &wp = *static_cast(o)->getWaypointPtr(); + Robot::Waypoint& wp = *static_cast(o)->getWaypointPtr(); getTrajectoryPtr()->addWaypoint(wp); getTrajectoryPtr()->generateTrajectory(); - + return new TrajectoryPy(new Robot::Trajectory(*getTrajectoryPtr())); - //Py_Return; + // Py_Return; } PyErr_Clear(); @@ -100,47 +104,49 @@ PyObject* TrajectoryPy::insertWaypoints(PyObject * args) Py::List list(o); for (Py::List::iterator it = list.begin(); it != list.end(); ++it) { if (PyObject_TypeCheck((*it).ptr(), &(Robot::WaypointPy::Type))) { - Robot::Waypoint &wp = *static_cast((*it).ptr())->getWaypointPtr(); + Robot::Waypoint& wp = + *static_cast((*it).ptr())->getWaypointPtr(); getTrajectoryPtr()->addWaypoint(wp); } } getTrajectoryPtr()->generateTrajectory(); - + return new TrajectoryPy(new Robot::Trajectory(*getTrajectoryPtr())); } Py_Error(PyExc_TypeError, "Wrong parameters - waypoint or placement expected"); - } -PyObject* TrajectoryPy::position(PyObject * args) +PyObject* TrajectoryPy::position(PyObject* args) { double pos; - if (!PyArg_ParseTuple(args, "d", &pos)) + if (!PyArg_ParseTuple(args, "d", &pos)) { return nullptr; + } return (new Base::PlacementPy(new Base::Placement(getTrajectoryPtr()->getPosition(pos)))); } -PyObject* TrajectoryPy::velocity(PyObject * args) +PyObject* TrajectoryPy::velocity(PyObject* args) { double pos; - if (!PyArg_ParseTuple(args, "d", &pos)) + if (!PyArg_ParseTuple(args, "d", &pos)) { return nullptr; + } - // return velocity as float + // return velocity as float return Py::new_reference_to(Py::Float(getTrajectoryPtr()->getVelocity(pos))); } -PyObject* TrajectoryPy::deleteLast(PyObject *args) +PyObject* TrajectoryPy::deleteLast(PyObject* args) { - int n=1; - if (!PyArg_ParseTuple(args, "|i", &n)) + int n = 1; + if (!PyArg_ParseTuple(args, "|i", &n)) { return nullptr; + } getTrajectoryPtr()->deleteLast(n); return new TrajectoryPy(new Robot::Trajectory(*getTrajectoryPtr())); - } - +} Py::Float TrajectoryPy::getDuration() const @@ -151,8 +157,10 @@ Py::Float TrajectoryPy::getDuration() const Py::List TrajectoryPy::getWaypoints() const { Py::List list; - for(unsigned int i = 0; i < getTrajectoryPtr()->getSize(); i++) - list.append(Py::asObject(new Robot::WaypointPy(new Robot::Waypoint(getTrajectoryPtr()->getWaypoint(i))))); + for (unsigned int i = 0; i < getTrajectoryPtr()->getSize(); i++) { + list.append(Py::asObject( + new Robot::WaypointPy(new Robot::Waypoint(getTrajectoryPtr()->getWaypoint(i))))); + } return list; } @@ -163,20 +171,15 @@ Py::Float TrajectoryPy::getLength() const } - void TrajectoryPy::setWaypoints(Py::List) -{ - -} +{} -PyObject *TrajectoryPy::getCustomAttributes(const char* /*attr*/) const +PyObject* TrajectoryPy::getCustomAttributes(const char* /*attr*/) const { return nullptr; } int TrajectoryPy::setCustomAttributes(const char* /*attr*/, PyObject* /*obj*/) { - return 0; + return 0; } - - diff --git a/src/Mod/Robot/App/Waypoint.cpp b/src/Mod/Robot/App/Waypoint.cpp index c73fd8f556..85b95e155f 100644 --- a/src/Mod/Robot/App/Waypoint.cpp +++ b/src/Mod/Robot/App/Waypoint.cpp @@ -22,7 +22,7 @@ #include "PreCompiled.h" #ifndef _PreComp_ -# include "kdl_cp/chain.hpp" +#include "kdl_cp/chain.hpp" #endif #include @@ -32,12 +32,12 @@ #ifndef M_PI - #define M_PI 3.14159265358979323846 - #define M_PI 3.14159265358979323846 /* pi */ +#define M_PI 3.14159265358979323846 +#define M_PI 3.14159265358979323846 /* pi */ #endif #ifndef M_PI_2 - #define M_PI_2 1.57079632679489661923 /* pi/2 */ +#define M_PI_2 1.57079632679489661923 /* pi/2 */ #endif using namespace Robot; @@ -45,10 +45,10 @@ using namespace Base; using namespace KDL; -TYPESYSTEM_SOURCE(Robot::Waypoint , Base::Persistence) +TYPESYSTEM_SOURCE(Robot::Waypoint, Base::Persistence) Waypoint::Waypoint(const char* name, - const Base::Placement &endPos, + const Base::Placement& endPos, WaypointType type, float velocity, float acceleration, @@ -56,64 +56,67 @@ Waypoint::Waypoint(const char* name, unsigned int tool, unsigned int base) - : Name(name) - , Type(type) - , Velocity(velocity) - , Acceleration(acceleration) - , Cont(cont) - , Tool(tool) - , Base(base) - , EndPos(endPos) -{ -} + : Name(name) + , Type(type) + , Velocity(velocity) + , Acceleration(acceleration) + , Cont(cont) + , Tool(tool) + , Base(base) + , EndPos(endPos) +{} Waypoint::Waypoint() - : Type(UNDEF) - , Velocity(1000.0) - , Acceleration(100.0) - , Cont(false) - , Tool(0) - , Base(0) -{ -} + : Type(UNDEF) + , Velocity(1000.0) + , Acceleration(100.0) + , Cont(false) + , Tool(0) + , Base(0) +{} Waypoint::~Waypoint() = default; -unsigned int Waypoint::getMemSize () const +unsigned int Waypoint::getMemSize() const { - return 0; + return 0; } -void Waypoint::Save (Writer &writer) const +void Waypoint::Save(Writer& writer) const { - writer.Stream() << writer.ind() << " "; - else if(Type == Waypoint::LINE) + } + else if (Type == Waypoint::LINE) { writer.Stream() << " type=\"LIN\"/> "; - else if(Type == Waypoint::CIRC) + } + else if (Type == Waypoint::CIRC) { writer.Stream() << " type=\"CIRC\"/> "; - else if(Type == Waypoint::WAIT) + } + else if (Type == Waypoint::WAIT) { writer.Stream() << " type=\"WAIT\"/> "; - else if(Type == Waypoint::UNDEF) + } + else if (Type == Waypoint::UNDEF) { writer.Stream() << " type=\"UNDEF\"/> "; - writer.Stream()<< std::endl; + } + writer.Stream() << std::endl; } -void Waypoint::Restore(XMLReader &reader) +void Waypoint::Restore(XMLReader& reader) { // read my Element reader.readElement("Waypoint"); @@ -127,24 +130,26 @@ void Waypoint::Restore(XMLReader &reader) reader.getAttributeAsFloat("Q2"), reader.getAttributeAsFloat("Q3"))); - Velocity = (float) reader.getAttributeAsFloat("vel"); - Acceleration = (float) reader.getAttributeAsFloat("acc"); - Cont = (reader.getAttributeAsInteger("cont") != 0)?true:false; - Tool = reader.getAttributeAsInteger("tool"); - Base = reader.getAttributeAsInteger("base"); + Velocity = (float)reader.getAttributeAsFloat("vel"); + Acceleration = (float)reader.getAttributeAsFloat("acc"); + Cont = (reader.getAttributeAsInteger("cont") != 0) ? true : false; + Tool = reader.getAttributeAsInteger("tool"); + Base = reader.getAttributeAsInteger("base"); std::string type = reader.getAttribute("type"); - if(type=="PTP") + if (type == "PTP") { Type = Waypoint::PTP; - else if(type=="LIN") + } + else if (type == "LIN") { Type = Waypoint::LINE; - else if(type=="CIRC") + } + else if (type == "CIRC") { Type = Waypoint::CIRC; - else if(type=="WAIT") + } + else if (type == "WAIT") { Type = Waypoint::WAIT; - else + } + else { Type = Waypoint::UNDEF; - - + } } - diff --git a/src/Mod/Robot/App/Waypoint.h b/src/Mod/Robot/App/Waypoint.h index 7f0f4be145..ea540cd744 100644 --- a/src/Mod/Robot/App/Waypoint.h +++ b/src/Mod/Robot/App/Waypoint.h @@ -33,28 +33,30 @@ namespace Robot /** The representation of a waypoint in a trajectory */ -class RobotExport Waypoint : public Base::Persistence +class RobotExport Waypoint: public Base::Persistence { TYPESYSTEM_HEADER_WITH_OVERRIDE(); public: - enum WaypointType { + enum WaypointType + { UNDEF, PTP, LINE, CIRC, - WAIT }; + WAIT + }; Waypoint(); - /// full constructor + /// full constructor Waypoint(const char* name, - const Base::Placement& endPos, - WaypointType type = Waypoint::LINE, - float velocity = 2000.0, - float acceleration = 100.0, - bool cont = false, - unsigned int tool = 0, - unsigned int base = 0); + const Base::Placement& endPos, + WaypointType type = Waypoint::LINE, + float velocity = 2000.0, + float acceleration = 100.0, + bool cont = false, + unsigned int tool = 0, + unsigned int base = 0); ~Waypoint() override; @@ -63,18 +65,17 @@ public: void Save(Base::Writer& /*writer*/) const override; void Restore(Base::XMLReader& /*reader*/) override; - + std::string Name; WaypointType Type; float Velocity; float Acceleration; bool Cont; - unsigned int Tool,Base; + unsigned int Tool, Base; Base::Placement EndPos; - }; -} //namespace Part +}// namespace Robot -#endif // ROBOT_WAYPOINT_H +#endif// ROBOT_WAYPOINT_H diff --git a/src/Mod/Robot/App/WaypointPy.xml b/src/Mod/Robot/App/WaypointPy.xml index b14f124432..cf600b6950 100644 --- a/src/Mod/Robot/App/WaypointPy.xml +++ b/src/Mod/Robot/App/WaypointPy.xml @@ -1,13 +1,13 @@ - diff --git a/src/Mod/Robot/App/WaypointPyImp.cpp b/src/Mod/Robot/App/WaypointPyImp.cpp index 3379220d65..0e0da7ac3b 100644 --- a/src/Mod/Robot/App/WaypointPyImp.cpp +++ b/src/Mod/Robot/App/WaypointPyImp.cpp @@ -26,9 +26,11 @@ #include #include +// clang-format off // inclusion of the generated files (generated out of WaypointPy.xml) #include "WaypointPy.h" #include "WaypointPy.cpp" +// clang-format on using namespace Robot; @@ -37,40 +39,50 @@ using namespace Base; // returns a string which represents the object e.g. when printed in python std::string WaypointPy::representation() const { - double A,B,C; + double A, B, C; std::stringstream str; - getWaypointPtr()->EndPos.getRotation().getYawPitchRoll(A,B,C); + getWaypointPtr()->EndPos.getRotation().getYawPitchRoll(A, B, C); str.precision(5); str << "Waypoint ["; - if(getWaypointPtr()->Type == Waypoint::PTP) + if (getWaypointPtr()->Type == Waypoint::PTP) { str << "PTP "; - else if(getWaypointPtr()->Type == Waypoint::LINE) + } + else if (getWaypointPtr()->Type == Waypoint::LINE) { str << "LIN "; - else if(getWaypointPtr()->Type == Waypoint::CIRC) + } + else if (getWaypointPtr()->Type == Waypoint::CIRC) { str << "CIRC "; - else if(getWaypointPtr()->Type == Waypoint::WAIT) + } + else if (getWaypointPtr()->Type == Waypoint::WAIT) { str << "WAIT "; - else if(getWaypointPtr()->Type == Waypoint::UNDEF) + } + else if (getWaypointPtr()->Type == Waypoint::UNDEF) { str << "UNDEF "; + } str << getWaypointPtr()->Name; str << " ("; - str << getWaypointPtr()->EndPos.getPosition().x << ","<< getWaypointPtr()->EndPos.getPosition().y << "," << getWaypointPtr()->EndPos.getPosition().z; + str << getWaypointPtr()->EndPos.getPosition().x << "," + << getWaypointPtr()->EndPos.getPosition().y << "," + << getWaypointPtr()->EndPos.getPosition().z; str << ";" << A << "," << B << "," << C << ")"; str << "v=" << getWaypointPtr()->Velocity << " "; - if(getWaypointPtr()->Cont) + if (getWaypointPtr()->Cont) { str << "Cont "; - if(getWaypointPtr()->Tool != 0) + } + if (getWaypointPtr()->Tool != 0) { str << "Tool" << getWaypointPtr()->Tool << " "; - if(getWaypointPtr()->Base != 0) + } + if (getWaypointPtr()->Base != 0) { str << "Tool" << getWaypointPtr()->Base << " "; + } str << "]"; return str.str(); } -PyObject *WaypointPy::PyMake(struct _typeobject *, PyObject *, PyObject *) // Python wrapper +PyObject* WaypointPy::PyMake(struct _typeobject*, PyObject*, PyObject*)// Python wrapper { - // create a new instance of WaypointPy and the Twin object + // create a new instance of WaypointPy and the Twin object return new WaypointPy(new Waypoint); } @@ -86,12 +98,22 @@ int WaypointPy::PyInit(PyObject* args, PyObject* kwd) int tool = 0; int base = 0; - static const std::array kwlist{"Pos", "type", "name", "vel", "cont", "tool", "base", "acc", - nullptr}; + static const std::array + kwlist {"Pos", "type", "name", "vel", "cont", "tool", "base", "acc", nullptr}; - if (!Base::Wrapped_ParseTupleAndKeywords(args, kwd, "O!|ssOiiiO", kwlist, - &(Base::PlacementPy::Type), &pos, // the placement object - &type, &name, &vel, &cont, &tool, &base, &acc)) { + if (!Base::Wrapped_ParseTupleAndKeywords(args, + kwd, + "O!|ssOiiiO", + kwlist, + &(Base::PlacementPy::Type), + &pos,// the placement object + &type, + &name, + &vel, + &cont, + &tool, + &base, + &acc)) { return -1; } @@ -99,40 +121,49 @@ int WaypointPy::PyInit(PyObject* args, PyObject* kwd) getWaypointPtr()->EndPos = TempPos; getWaypointPtr()->Name = name; std::string typeStr(type); - if (typeStr == "PTP") + if (typeStr == "PTP") { getWaypointPtr()->Type = Waypoint::PTP; - else if (typeStr == "LIN") + } + else if (typeStr == "LIN") { getWaypointPtr()->Type = Waypoint::LINE; - else if (typeStr == "CIRC") + } + else if (typeStr == "CIRC") { getWaypointPtr()->Type = Waypoint::CIRC; - else if (typeStr == "WAIT") + } + else if (typeStr == "WAIT") { getWaypointPtr()->Type = Waypoint::WAIT; - else + } + else { getWaypointPtr()->Type = Waypoint::UNDEF; + } - if (!vel) + if (!vel) { switch (getWaypointPtr()->Type) { - case Waypoint::PTP: - getWaypointPtr()->Velocity = 100; - break; - case Waypoint::LINE: - getWaypointPtr()->Velocity = 2000; - break; - case Waypoint::CIRC: - getWaypointPtr()->Velocity = 2000; - break; - default: - getWaypointPtr()->Velocity = 0; + case Waypoint::PTP: + getWaypointPtr()->Velocity = 100; + break; + case Waypoint::LINE: + getWaypointPtr()->Velocity = 2000; + break; + case Waypoint::CIRC: + getWaypointPtr()->Velocity = 2000; + break; + default: + getWaypointPtr()->Velocity = 0; } - else + } + else { getWaypointPtr()->Velocity = Base::UnitsApi::toDouble(vel, Base::Unit::Velocity); + } getWaypointPtr()->Cont = cont ? true : false; getWaypointPtr()->Tool = tool; getWaypointPtr()->Base = base; - if (!acc) + if (!acc) { getWaypointPtr()->Acceleration = 100; - else + } + else { getWaypointPtr()->Acceleration = Base::UnitsApi::toDouble(acc, Base::Unit::Acceleration); + } return 0; } @@ -143,9 +174,9 @@ Py::Float WaypointPy::getVelocity() const return Py::Float(getWaypointPtr()->Velocity); } -void WaypointPy::setVelocity(Py::Float arg) +void WaypointPy::setVelocity(Py::Float arg) { - getWaypointPtr()->Velocity = (float) arg.operator double(); + getWaypointPtr()->Velocity = (float)arg.operator double(); } @@ -161,46 +192,58 @@ void WaypointPy::setName(Py::String arg) Py::String WaypointPy::getType() const { - if(getWaypointPtr()->Type == Waypoint::PTP) + if (getWaypointPtr()->Type == Waypoint::PTP) { return Py::String("PTP"); - else if(getWaypointPtr()->Type == Waypoint::LINE) + } + else if (getWaypointPtr()->Type == Waypoint::LINE) { return Py::String("LIN"); - else if(getWaypointPtr()->Type == Waypoint::CIRC) + } + else if (getWaypointPtr()->Type == Waypoint::CIRC) { return Py::String("CIRC"); - else if(getWaypointPtr()->Type == Waypoint::WAIT) + } + else if (getWaypointPtr()->Type == Waypoint::WAIT) { return Py::String("WAIT"); - else if(getWaypointPtr()->Type == Waypoint::UNDEF) + } + else if (getWaypointPtr()->Type == Waypoint::UNDEF) { return Py::String("UNDEF"); - else + } + else { throw Base::TypeError("Unknown waypoint type! Only: PTP,LIN,CIRC,WAIT are supported."); + } } void WaypointPy::setType(Py::String arg) { std::string typeStr(arg.as_std_string("ascii")); - if(typeStr=="PTP") + if (typeStr == "PTP") { getWaypointPtr()->Type = Waypoint::PTP; - else if(typeStr=="LIN") + } + else if (typeStr == "LIN") { getWaypointPtr()->Type = Waypoint::LINE; - else if(typeStr=="CIRC") + } + else if (typeStr == "CIRC") { getWaypointPtr()->Type = Waypoint::CIRC; - else if(typeStr=="WAIT") + } + else if (typeStr == "WAIT") { getWaypointPtr()->Type = Waypoint::WAIT; - else + } + else { throw Base::TypeError("Unknown waypoint type! Only: PTP,LIN,CIRC,WAIT are allowed."); + } } Py::Object WaypointPy::getPos() const { - return Py::Object(new PlacementPy(new Placement(getWaypointPtr()->EndPos)),true); + return Py::Object(new PlacementPy(new Placement(getWaypointPtr()->EndPos)), true); } void WaypointPy::setPos(Py::Object arg) { Py::Type PlacementType(Base::getTypeAsObject(&(Base::PlacementPy::Type))); - if(arg.isType(PlacementType)) + if (arg.isType(PlacementType)) { getWaypointPtr()->EndPos = *static_cast((*arg))->getPlacementPtr(); + } } Py::Boolean WaypointPy::getCont() const @@ -221,10 +264,12 @@ Py::Long WaypointPy::getTool() const void WaypointPy::setTool(Py::Long arg) { long value = static_cast(arg); - if (value >= 0) + if (value >= 0) { getWaypointPtr()->Tool = value; - else + } + else { throw Py::ValueError("negative tool not allowed!"); + } } Py::Long WaypointPy::getBase() const @@ -235,20 +280,20 @@ Py::Long WaypointPy::getBase() const void WaypointPy::setBase(Py::Long arg) { long value = static_cast(arg); - if (value >= 0) + if (value >= 0) { getWaypointPtr()->Base = value; - else + } + else { throw Py::ValueError("negative base not allowed!"); + } } -PyObject *WaypointPy::getCustomAttributes(const char* /*attr*/) const +PyObject* WaypointPy::getCustomAttributes(const char* /*attr*/) const { return nullptr; } int WaypointPy::setCustomAttributes(const char* /*attr*/, PyObject* /*obj*/) { - return 0; + return 0; } - -