kindred/create
1
1
Fork 0
Code Issues 37 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity

Robot: Apply clang format

Browse Source
This commit is contained in:
wmayer
2023-09-11 10:24:21 +02:00
committed by wwmayer
parent e1aa8197d3
commit 4c470ecd11
33 changed files with 1215 additions and 967 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
src/Mod/Robot/App/AppRobot.cpp
View File

@@ -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<Module>
namespace Robot
{
class Module: public Py::ExtensionModule<Module>
{
public:
Module() : Py::ExtensionModule<Module>("Robot")
Module()
: Py::ExtensionModule<Module>("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<TrajectoryPy*>(pcTracObj)->getTrajectoryPtr();
Robot::Robot6Axis &Rob = * static_cast<Robot6AxisPy*>(pcRobObj)->getRobot6AxisPtr();
Simulation Sim(Trac,Rob);
Robot::Trajectory& Trac = *static_cast<TrajectoryPy*>(pcTracObj)->getTrajectoryPtr();
Robot::Robot6Axis& Rob = *static_cast<Robot6AxisPy*>(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
}

333
src/Mod/Robot/App/Edge2TracObject.cpp
View File

@@ -22,15 +22,15 @@
#include "PreCompiled.h"
#ifndef _PreComp_
# include <BRepAdaptor_Curve.hxx>
# include <CPnts_AbscissaPoint.hxx>
# include <TopoDS.hxx>
# include <TopoDS_Edge.hxx>
#include <BRepAdaptor_Curve.hxx>
#include <CPnts_AbscissaPoint.hxx>
#include <TopoDS.hxx>
#include <TopoDS_Edge.hxx>
#endif
#include <Base/Sequencer.h>
#include <Mod/Part/App/edgecluster.h>
#include <Mod/Part/App/PartFeature.h>
#include <Mod/Part/App/edgecluster.h>
#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<Part::Feature*>(Source.getValue());
}
Part::Feature* base = static_cast<Part::Feature*>(Source.getValue());
const Part::TopoShape& TopShape = base->Shape.getShape();
const std::vector<std::string>& 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<TopoDS_Edge> 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<size_t>((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<size_t>((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.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 = ;
else {
i -= SegLength;
}
Waypoint wp("Pt",Base::Placement(Base::Vector3d(P.X(),P.Y(),P.Z()),R1));
trac.addWaypoint(wp);
}
}
break;
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)
{

18
src/Mod/Robot/App/Edge2TracObject.h
View File

@@ -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

2
src/Mod/Robot/App/PreCompiled.cpp
View File

@@ -20,4 +20,4 @@
* *
***************************************************************************/
#include "PreCompiled.h"
#include "PreCompiled.h"

22
src/Mod/Robot/App/PreCompiled.h
View File

@@ -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 <TopoDS.hxx>
#include <TopoDS_Edge.hxx>
#endif // _PreComp_
#endif// _PreComp_
#endif

49
src/Mod/Robot/App/PropertyTrajectory.cpp
View File

@@ -22,7 +22,7 @@
#include "PreCompiled.h"
#ifndef _PreComp_
# include <sstream>
#include <sstream>
#endif
#include <Base/Reader.h>
@@ -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<TrajectoryPy*>(value);
TrajectoryPy* pcObject = static_cast<TrajectoryPy*>(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<const PropertyTrajectory&>(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);
}

22
src/Mod/Robot/App/PropertyTrajectory.h
View File

@@ -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

194
src/Mod/Robot/App/Robot6Axis.cpp
View File

@@ -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 <Base/FileInfo.h>
@@ -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<std::string>& 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<std::string> 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() << "<Axis "
<< "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;
<< "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
}

47
src/Mod/Robot/App/Robot6Axis.h
View File

@@ -23,10 +23,10 @@
#ifndef ROBOT_ROBOT6AXLE_H
#define ROBOT_ROBOT6AXLE_H
#include <Base/Persistence.h>
#include <Base/Placement.h>
#include "kdl_cp/chain.hpp"
#include "kdl_cp/jntarray.hpp"
#include <Base/Persistence.h>
#include <Base/Placement.h>
#include <Mod/Robot/RobotGlobal.h>
@@ -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<std::vector<float> > &KinTable);
// void setKinematik(const std::vector<std::vector<float> > &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

16
src/Mod/Robot/App/Robot6AxisPy.xml
View File

@@ -1,13 +1,13 @@
<?xml version="1.0" encoding="UTF-8"?>
<GenerateModel xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="generateMetaModel_Module.xsd">
<PythonExport
Father="PersistencePy"
Name="Robot6AxisPy"
Twin="Robot6Axis"
TwinPointer="Robot6Axis"
Include="Mod/Robot/App/Robot6Axis.h"
Namespace="Robot"
FatherInclude="Base/PersistencePy.h"
<PythonExport
Father="PersistencePy"
Name="Robot6AxisPy"
Twin="Robot6Axis"
TwinPointer="Robot6Axis"
Include="Mod/Robot/App/Robot6Axis.h"
Namespace="Robot"
FatherInclude="Base/PersistencePy.h"
FatherNamespace="Base"
Constructor="true"
Delete="true">

75
src/Mod/Robot/App/Robot6AxisPyImp.cpp
View File

@@ -22,15 +22,17 @@
#include "PreCompiled.h"
#ifndef _PreComp_
# include <sstream>
#include <sstream>
#endif
#include <Base/MatrixPy.h>
#include <Base/PlacementPy.h>
// 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 << "<Robot6Axis "
<< "Tcp:("
<< getRobot6AxisPtr()->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 << "<Robot6Axis "
<< "Tcp:(" << getRobot6AxisPtr()->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<Base::PlacementPy*>(*value)->getPlacementPtr()))
throw Base::RuntimeError("Can not reach Point");
if (!getRobot6AxisPtr()->setTo(
*static_cast<Base::PlacementPy*>(*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;
}

59
src/Mod/Robot/App/RobotAlgos.cpp
View File

@@ -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));
}
}

27
src/Mod/Robot/App/RobotAlgos.h
View File

@@ -23,10 +23,10 @@
#ifndef _RobotAlgos_h_
#define _RobotAlgos_h_
#include "kdl_cp/frames_io.hpp"
#include <Base/Placement.h>
#include <Base/Vector3D.h>
#include <Mod/Robot/RobotGlobal.h>
#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

138
src/Mod/Robot/App/RobotObject.cpp
View File

@@ -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;

44
src/Mod/Robot/App/RobotObject.h
View File

@@ -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

17
src/Mod/Robot/App/RobotObjectPy.xml
View File

@@ -1,13 +1,13 @@
<?xml version="1.0" encoding="UTF-8"?>
<GenerateModel xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="generateMetaModel_Module.xsd">
<PythonExport
Father="DocumentObjectPy"
Name="RobotObjectPy"
Twin="RobotObject"
TwinPointer="RobotObject"
Include="Mod/Robot/App/RobotObject.h"
Namespace="Robot"
FatherInclude="App/DocumentObjectPy.h"
<PythonExport
Father="DocumentObjectPy"
Name="RobotObjectPy"
Twin="RobotObject"
TwinPointer="RobotObject"
Include="Mod/Robot/App/RobotObject.h"
Namespace="Robot"
FatherInclude="App/DocumentObjectPy.h"
FatherNamespace="App">
<Documentation>
<Author Licence="LGPL" Name="Juergen Riegel" EMail="FreeCAD@juergen-riegel.net" />
@@ -21,6 +21,5 @@
</UserDocu>
</Documentation>
</Methode>
</PythonExport>
</GenerateModel>

12
src/Mod/Robot/App/RobotObjectPyImp.cpp
View File

@@ -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;
}

34
src/Mod/Robot/App/Simulation.cpp
View File

@@ -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);
}

42
src/Mod/Robot/App/Simulation.h
View File

@@ -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

263
src/Mod/Robot/App/Trajectory.cpp
View File

@@ -23,69 +23,76 @@
#include "PreCompiled.h"
#ifndef _PreComp_
# include <memory>
#include <memory>
# 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 <Base/Exception.h>
#include <Base/Reader.h>
#include <Base/Writer.h>
#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<Waypoint*>::const_iterator it=Trac.vpcWaypoints.begin();it!=Trac.vpcWaypoints.end();++it,i++)
int i = 0;
for (std::vector<Waypoint*>::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<Waypoint*>::const_iterator it = vpcWaypoints.begin(); it != vpcWaypoints.end(); ++it) {
for (std::vector<Waypoint*>::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<KDL::Path_RoundedComposite>(3, 3,
new KDL::RotationalInterpolation_SingleAxis());
// the velocity of the first waypoint is used
pcVelPrf = std::make_unique<KDL::VelocityProfile_Trap>((*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<KDL::Path_RoundedComposite>(
3,
3,
new KDL::RotationalInterpolation_SingleAxis());
// the velocity of the first waypoint is used
pcVelPrf =
std::make_unique<KDL::VelocityProfile_Trap>((*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<KDL::Trajectory_Segment>(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<KDL::Trajectory_Segment>(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<KDL::VelocityProfile_Trap>((*it)->Velocity, (*it)->Acceleration);
pcVelPrf->SetProfile(0, pcPath->PathLength());
pcTrak = std::make_unique<KDL::Trajectory_Segment>(pcPath, pcVelPrf.release());
pcVelPrf =
std::make_unique<KDL::VelocityProfile_Trap>((*it)->Velocity,
(*it)->Acceleration);
pcVelPrf->SetProfile(0, pcPath->PathLength());
pcTrak = std::make_unique<KDL::Trajectory_Segment>(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<Robot::Waypoint*>::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<int>(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() << "<Trajectory count=\"" << getSize() <<"\">" << std::endl;
writer.Stream() << writer.ind() << "<Trajectory count=\"" << getSize() << "\">" << std::endl;
writer.incInd();
for(unsigned int i = 0;i<getSize(); i++)
for (unsigned int i = 0; i < getSize(); i++) {
vpcWaypoints[i]->Save(writer);
}
writer.decInd();
writer.Stream() << writer.ind() << "</Trajectory>" << std::endl ;
writer.Stream() << writer.ind() << "</Trajectory>" << 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;
}

55
src/Mod/Robot/App/Trajectory.h
View File

@@ -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<Waypoint*> &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<Waypoint*>& 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<Waypoint*> vpcWaypoints;
KDL::Trajectory_Composite *pcTrajectory{nullptr};
KDL::Trajectory_Composite* pcTrajectory {nullptr};
};
} //namespace Part
}// namespace Robot
#endif // PART_TOPOSHAPE_H
#endif// PART_TOPOSHAPE_H

35
src/Mod/Robot/App/TrajectoryCompound.cpp
View File

@@ -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<DocumentObject*> &Tracs = Source.getValues();
const std::vector<DocumentObject*>& Tracs = Source.getValues();
Robot::Trajectory result;
for (auto it : Tracs) {
if (it->getTypeId().isDerivedFrom(Robot::TrajectoryObject::getClassTypeId())){
const std::vector<Waypoint*> &wps = static_cast<Robot::TrajectoryObject*>(it)->Trajectory.getValue().getWaypoints();
if (it->getTypeId().isDerivedFrom(Robot::TrajectoryObject::getClassTypeId())) {
const std::vector<Waypoint*>& wps =
static_cast<Robot::TrajectoryObject*>(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);
// }

16
src/Mod/Robot/App/TrajectoryCompound.h
View File

@@ -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

114
src/Mod/Robot/App/TrajectoryDressUpObject.cpp
View File

@@ -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<Waypoint*>& wps = static_cast<Robot::TrajectoryObject*>(link)->Trajectory.getValue().getWaypoints();
const std::vector<Waypoint*>& wps =
static_cast<Robot::TrajectoryObject*>(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);

29
src/Mod/Robot/App/TrajectoryDressUpObject.h
View File

@@ -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

21
src/Mod/Robot/App/TrajectoryObject.cpp
View File

@@ -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)

21
src/Mod/Robot/App/TrajectoryObject.h
View File

@@ -26,14 +26,14 @@
#include <App/GeoFeature.h>
#include <App/PropertyGeo.h>
#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

16
src/Mod/Robot/App/TrajectoryPy.xml
View File

@@ -1,13 +1,13 @@
<?xml version="1.0" encoding="UTF-8"?>
<GenerateModel xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="generateMetaModel_Module.xsd">
<PythonExport
Father="PersistencePy"
Name="TrajectoryPy"
Twin="Trajectory"
TwinPointer="Trajectory"
Include="Mod/Robot/App/Trajectory.h"
Namespace="Robot"
FatherInclude="Base/PersistencePy.h"
<PythonExport
Father="PersistencePy"
Name="TrajectoryPy"
Twin="Trajectory"
TwinPointer="Trajectory"
Include="Mod/Robot/App/Trajectory.h"
Namespace="Robot"
FatherInclude="Base/PersistencePy.h"
FatherNamespace="Base"
Constructor="true"
Delete="true">

71
src/Mod/Robot/App/TrajectoryPyImp.cpp
View File

@@ -24,10 +24,12 @@
#include <Base/PlacementPy.h>
// clang-format off
// inclusion of the generated files (generated out of TrajectoryPy.xml)
#include <Mod/Robot/App/TrajectoryPy.h>
#include <Mod/Robot/App/TrajectoryPy.cpp>
#include <Mod/Robot/App/WaypointPy.h>
// 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<Robot::WaypointPy*>((*it).ptr())->getWaypointPtr();
Robot::Waypoint& wp =
*static_cast<Robot::WaypointPy*>((*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<Base::PlacementPy*>(o)->getPlacementPtr();
getTrajectoryPtr()->addWaypoint(Robot::Waypoint("Pt",*plm));
Base::Placement* plm = static_cast<Base::PlacementPy*>(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<Robot::WaypointPy*>(o)->getWaypointPtr();
Robot::Waypoint& wp = *static_cast<Robot::WaypointPy*>(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<Robot::WaypointPy*>((*it).ptr())->getWaypointPtr();
Robot::Waypoint& wp =
*static_cast<Robot::WaypointPy*>((*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;
}

127
src/Mod/Robot/App/Waypoint.cpp
View File

@@ -22,7 +22,7 @@
#include "PreCompiled.h"
#ifndef _PreComp_
# include "kdl_cp/chain.hpp"
#include "kdl_cp/chain.hpp"
#endif
#include <Base/Reader.h>
@@ -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() << "<Waypoint "
<< "name=\"" << Name << "\" "
<< "Px=\"" << EndPos.getPosition().x << "\" "
<< "Py=\"" << EndPos.getPosition().y << "\" "
<< "Pz=\"" << EndPos.getPosition().z << "\" "
<< "Q0=\"" << EndPos.getRotation()[0] << "\" "
<< "Q1=\"" << EndPos.getRotation()[1] << "\" "
<< "Q2=\"" << EndPos.getRotation()[2] << "\" "
<< "Q3=\"" << EndPos.getRotation()[3] << "\" "
<< "vel=\"" << Velocity << "\" "
<< "acc=\"" << Acceleration << "\" "
<< "cont=\"" << int((Cont)?1:0) << "\" "
<< "tool=\"" << Tool << "\" "
<< "base=\"" << Base << "\" ";
if(Type == Waypoint::PTP)
writer.Stream() << writer.ind() << "<Waypoint "
<< "name=\"" << Name << "\" "
<< "Px=\"" << EndPos.getPosition().x << "\" "
<< "Py=\"" << EndPos.getPosition().y << "\" "
<< "Pz=\"" << EndPos.getPosition().z << "\" "
<< "Q0=\"" << EndPos.getRotation()[0] << "\" "
<< "Q1=\"" << EndPos.getRotation()[1] << "\" "
<< "Q2=\"" << EndPos.getRotation()[2] << "\" "
<< "Q3=\"" << EndPos.getRotation()[3] << "\" "
<< "vel=\"" << Velocity << "\" "
<< "acc=\"" << Acceleration << "\" "
<< "cont=\"" << int((Cont) ? 1 : 0) << "\" "
<< "tool=\"" << Tool << "\" "
<< "base=\"" << Base << "\" ";
if (Type == Waypoint::PTP) {
writer.Stream() << " type=\"PTP\"/> ";
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;
}
}

33
src/Mod/Robot/App/Waypoint.h
View File

@@ -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

16
src/Mod/Robot/App/WaypointPy.xml
View File

@@ -1,13 +1,13 @@
<?xml version="1.0" encoding="UTF-8"?>
<GenerateModel xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="generateMetaModel_Module.xsd">
<PythonExport
Father="PersistencePy"
Name="WaypointPy"
Twin="Waypoint"
TwinPointer="Waypoint"
Include="Mod/Robot/App/Waypoint.h"
Namespace="Robot"
FatherInclude="Base/PersistencePy.h"
<PythonExport
Father="PersistencePy"
Name="WaypointPy"
Twin="Waypoint"
TwinPointer="Waypoint"
Include="Mod/Robot/App/Waypoint.h"
Namespace="Robot"
FatherInclude="Base/PersistencePy.h"
FatherNamespace="Base"
Constructor="true"
Delete="true">

167
src/Mod/Robot/App/WaypointPyImp.cpp
View File

@@ -26,9 +26,11 @@
#include <Base/PyWrapParseTupleAndKeywords.h>
#include <Base/UnitsApi.h>
// 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<const char *, 9> kwlist{"Pos", "type", "name", "vel", "cont", "tool", "base", "acc",
nullptr};
static const std::array<const char*, 9>
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<Base::PlacementPy*>((*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<long>(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<long>(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;
}