From 1d04ce9ea03c46f85cbbac98714996e497bbdd73 Mon Sep 17 00:00:00 2001 From: wmayer Date: Tue, 13 Oct 2015 20:03:47 +0200 Subject: [PATCH 01/20] + port SurfaceFit to Eigen3 and add unit tests --- src/Mod/Mesh/App/AppMeshPy.cpp | 97 +++++++++-- src/Mod/Mesh/App/CMakeLists.txt | 4 +- src/Mod/Mesh/App/Core/Approximation.cpp | 204 ++++++++++++++++-------- src/Mod/Mesh/App/Core/Approximation.h | 17 +- src/Mod/Mesh/App/MeshTestsApp.py | 61 ++++++- 5 files changed, 288 insertions(+), 95 deletions(-) diff --git a/src/Mod/Mesh/App/AppMeshPy.cpp b/src/Mod/Mesh/App/AppMeshPy.cpp index 0170f751f..83694278c 100644 --- a/src/Mod/Mesh/App/AppMeshPy.cpp +++ b/src/Mod/Mesh/App/AppMeshPy.cpp @@ -40,6 +40,7 @@ #include "Core/MeshIO.h" #include "Core/Evaluation.h" #include "Core/Iterator.h" +#include "Core/Approximation.h" #include "MeshPy.h" #include "Mesh.h" @@ -387,7 +388,7 @@ createBox(PyObject *self, PyObject *args) } PY_CATCH; } -static PyObject * +static PyObject * calculateEigenTransform(PyObject *self, PyObject *args) { PyObject *input; @@ -395,8 +396,8 @@ calculateEigenTransform(PyObject *self, PyObject *args) if (!PyArg_ParseTuple(args, "O",&input)) return NULL; - if(! PySequence_Check(input) ){ - PyErr_SetString(Base::BaseExceptionFreeCADError, "Input have to be a sequence of Base.Vector()"); + if (!PySequence_Check(input)) { + PyErr_SetString(Base::BaseExceptionFreeCADError, "Input has to be a sequence of Base.Vector()"); return NULL; } @@ -416,25 +417,84 @@ calculateEigenTransform(PyObject *self, PyObject *args) Base::Vector3d* val = pcObject->getVectorPtr(); - current_node.Set(float(val->x),float(val->y),float(val->z)); - vertices.push_back(current_node); + current_node.Set(float(val->x),float(val->y),float(val->z)); + vertices.push_back(current_node); } - } + } - MeshCore::MeshFacet aFacet; - aFacet._aulPoints[0] = 0;aFacet._aulPoints[1] = 1;aFacet._aulPoints[2] = 2; - faces.push_back(aFacet); - //Fill the Kernel with the temp smesh structure and delete the current containers - aMesh.Adopt(vertices,faces); - MeshCore::MeshEigensystem pca(aMesh); - pca.Evaluate(); - Base::Matrix4D Trafo = pca.Transform(); + MeshCore::MeshFacet aFacet; + aFacet._aulPoints[0] = 0;aFacet._aulPoints[1] = 1;aFacet._aulPoints[2] = 2; + faces.push_back(aFacet); + //Fill the Kernel with the temp mesh structure and delete the current containers + aMesh.Adopt(vertices,faces); + MeshCore::MeshEigensystem pca(aMesh); + pca.Evaluate(); + Base::Matrix4D Trafo = pca.Transform(); return new Base::PlacementPy(new Base::Placement(Trafo) ); - } PY_CATCH; + } PY_CATCH; - Py_Return; + Py_Return; +} + +static PyObject * +polynomialFit(PyObject *self, PyObject *args) +{ + PyObject *input; + + if (!PyArg_ParseTuple(args, "O",&input)) + return NULL; + + if (!PySequence_Check(input)) { + PyErr_SetString(Base::BaseExceptionFreeCADError, "Input has to be a sequence of Base.Vector()"); + return NULL; + } + + PY_TRY { + MeshCore::SurfaceFit polyFit; + + Base::Vector3f point; + Py::Sequence list(input); + for (Py::Sequence::iterator it = list.begin(); it != list.end(); ++it) { + PyObject* value = (*it).ptr(); + if (PyObject_TypeCheck(value, &(Base::VectorPy::Type))) { + Base::VectorPy *pcObject = static_cast(value); + Base::Vector3d* val = pcObject->getVectorPtr(); + point.Set(float(val->x),float(val->y),float(val->z)); + polyFit.AddPoint(point); + } + } + + // fit quality + float fit = polyFit.Fit(); + Py::Dict dict; + dict.setItem(Py::String("Sigma"), Py::Float(fit)); + + // coefficients + double a,b,c,d,e,f; + polyFit.GetCoefficients(a,b,c,d,e,f); + Py::Tuple p(6); + p.setItem(0, Py::Float(a)); + p.setItem(1, Py::Float(b)); + p.setItem(2, Py::Float(c)); + p.setItem(3, Py::Float(d)); + p.setItem(4, Py::Float(e)); + p.setItem(5, Py::Float(f)); + dict.setItem(Py::String("Coefficients"), p); + + // residuals + std::vector local = polyFit.GetLocalPoints(); + Py::Tuple r(local.size()); + for (std::vector::iterator it = local.begin(); it != local.end(); ++it) { + double z = polyFit.Value(it->x, it->y); + double d = it->z - z; + r.setItem(it-local.begin(), Py::Float(d)); + } + dict.setItem(Py::String("Residuals"), r); + + return Py::new_reference_to(dict); + } PY_CATCH; } @@ -455,6 +515,10 @@ PyDoc_STRVAR(calculateEigenTransform_doc, "The local coordinate system is right-handed.\n" ); +PyDoc_STRVAR(polynomialFit_doc, +"polynomialFit(seq(Base.Vector)) -- Calculates a polynomial fit.\n" +); + /* List of functions defined in the module */ struct PyMethodDef Mesh_Import_methods[] = { @@ -471,5 +535,6 @@ struct PyMethodDef Mesh_Import_methods[] = { {"createCone",createCone, Py_NEWARGS, "Create a tessellated cone"}, {"createTorus",createTorus, Py_NEWARGS, "Create a tessellated torus"}, {"calculateEigenTransform",calculateEigenTransform, METH_VARARGS, calculateEigenTransform_doc}, + {"polynomialFit",polynomialFit, METH_VARARGS, polynomialFit_doc}, {NULL, NULL} /* sentinel */ }; diff --git a/src/Mod/Mesh/App/CMakeLists.txt b/src/Mod/Mesh/App/CMakeLists.txt index 3ee8aabfa..8934480b0 100644 --- a/src/Mod/Mesh/App/CMakeLists.txt +++ b/src/Mod/Mesh/App/CMakeLists.txt @@ -1,7 +1,5 @@ if(WIN32) - add_definitions(-DFCAppMesh -DWM4_FOUNDATION_DLL_EXPORT -DEIGEN2_SUPPORT) -else (Win32) - add_definitions(-DEIGEN2_SUPPORT) + add_definitions(-DFCAppMesh -DWM4_FOUNDATION_DLL_EXPORT) endif(WIN32) include_directories( diff --git a/src/Mod/Mesh/App/Core/Approximation.cpp b/src/Mod/Mesh/App/Core/Approximation.cpp index dd53f38ff..9f1e56233 100644 --- a/src/Mod/Mesh/App/Core/Approximation.cpp +++ b/src/Mod/Mesh/App/Core/Approximation.cpp @@ -38,28 +38,22 @@ #include //#define FC_USE_EIGEN -//#define FC_USE_BOOST -#if defined(FC_USE_BOOST) -#include -#include -#include - -#define BOOST_NUMERIC_BINDINGS_USE_CLAPACK -#include -#include -#include - -namespace ublas = boost::numeric::ublas; -extern "C" void LAPACK_DGESV (int const* n, int const* nrhs, - double* a, int const* lda, int* ipiv, - double* b, int const* ldb, int* info); - +#include +#ifdef FC_USE_EIGEN +#include #endif -# include - using namespace MeshCore; +Approximation::Approximation() + : _bIsFitted(false), _fLastResult(FLOAT_MAX) +{ +} + +Approximation::~Approximation() +{ + Clear(); +} void Approximation::Convert( const Wm4::Vector3& Wm4, Base::Vector3f& pt) { @@ -142,6 +136,18 @@ bool Approximation::Done() const // ------------------------------------------------------------------------------- +PlaneFit::PlaneFit() + : _vBase(0,0,0) + , _vDirU(1,0,0) + , _vDirV(0,1,0) + , _vDirW(0,0,1) +{ +} + +PlaneFit::~PlaneFit() +{ +} + float PlaneFit::Fit() { _bIsFitted = true; @@ -166,21 +172,7 @@ float PlaneFit::Fit() syz = syz - my*mz/((double)nSize); szz = szz - mz*mz/((double)nSize); -#if defined(FC_USE_BOOST) - ublas::matrix A(3,3); - A(0,0) = sxx; - A(1,1) = syy; - A(2,2) = szz; - A(0,1) = sxy; A(1,0) = sxy; - A(0,2) = sxz; A(2,0) = sxz; - A(1,2) = syz; A(2,1) = syz; - namespace lapack= boost::numeric::bindings::lapack; - ublas::vector eigenval(3); - int r = lapack::syev('V','U',A,eigenval,lapack::optimal_workspace()); - if (r) { - } - float sigma = 0; -#elif defined(FC_USE_EIGEN) +#if defined(FC_USE_EIGEN) Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero(); covMat(0,0) = sxx; covMat(1,1) = syy; @@ -398,6 +390,26 @@ void PlaneFit::Dimension(float& length, float& width) const width = bbox.MaxY - bbox.MinY; } +std::vector PlaneFit::GetLocalPoints() const +{ + std::vector localPoints; + if (_bIsFitted && _fLastResult < FLOAT_MAX) { + Base::Vector3d bs(this->_vBase.x,this->_vBase.y,this->_vBase.z); + Base::Vector3d ex(this->_vDirU.x,this->_vDirU.y,this->_vDirU.z); + Base::Vector3d ey(this->_vDirV.x,this->_vDirV.y,this->_vDirV.z); + Base::Vector3d ez(this->_vDirW.x,this->_vDirW.y,this->_vDirW.z); + + localPoints.insert(localPoints.begin(), _vPoints.begin(), _vPoints.end()); + for (std::vector::iterator it = localPoints.begin(); it != localPoints.end(); ++it) { + Base::Vector3d clPoint(it->x,it->y,it->z); + clPoint.TransformToCoordinateSystem(bs, ex, ey); + it->Set(static_cast(clPoint.x), static_cast(clPoint.y), static_cast(clPoint.z)); + } + } + + return localPoints; +} + // ------------------------------------------------------------------------------- bool QuadraticFit::GetCurvatureInfo(double x, double y, double z, @@ -608,24 +620,34 @@ double SurfaceFit::PolynomFit() Base::Vector3d ey(this->_vDirV.x,this->_vDirV.y,this->_vDirV.z); Base::Vector3d ez(this->_vDirW.x,this->_vDirW.y,this->_vDirW.z); -#if defined(FC_USE_BOOST) - ublas::matrix A(6,6); - ublas::vector b(6); - for (int i=0; i<6; i++) { - for (int j=0; j<6; j++) { - A(i,j) = 0.0; - } - b(i) = 0.0; - } -#else + // A*x = b + // See also www.cs.jhu.edu/~misha/Fall05/10.23.05.pdf + // z = f(x,y) = a*x^2 + b*y^2 + c*x*y + d*x + e*y + f + // z = P * Vi with Vi=(xi^2,yi^2,xiyi,xi,yi,1) and P=(a,b,c,d,e,f) + // To get the best-fit values the sum needs to be minimized: + // S = sum[(z-zi)^2} -> min with zi=z coordinates of the given points + // <=> S = sum[z^2 - 2*z*zi + zi^2] -> min + // <=> S(P) = sum[(P*Vi)^2 - 2*(P*Vi)*zi + zi^2] -> min + // To get the optimum the gradient of the expression must be the null vector + // Note: grad F(P) = (P*Vi)^2 = 2*(P*Vi)*Vi + // grad G(P) = -2*(P*Vi)*zi = -2*Vi*zi + // grad H(P) = zi^2 = 0 + // => grad S(P) = sum[2*(P*Vi)*Vi - 2*Vi*zi] = 0 + // <=> sum[(P*Vi)*Vi] = sum[Vi*zi] + // <=> sum[(Vi*Vi^t)*P] = sum[Vi*zi] where (Vi*Vi^t) is a symmetric matrix + // <=> sum[(Vi*Vi^t)]*P = sum[Vi*zi] Eigen::Matrix A = Eigen::Matrix::Zero(); Eigen::Matrix b = Eigen::Matrix::Zero(); Eigen::Matrix x = Eigen::Matrix::Zero(); -#endif + std::vector transform; + transform.reserve(_vPoints.size()); + + double dW2 = 0; for (std::list::const_iterator it = _vPoints.begin(); it != _vPoints.end(); ++it) { Base::Vector3d clPoint(it->x,it->y,it->z); clPoint.TransformToCoordinateSystem(bs, ex, ey); + transform.push_back(clPoint); double dU = clPoint.x; double dV = clPoint.y; double dW = clPoint.z; @@ -634,6 +656,8 @@ double SurfaceFit::PolynomFit() double dV2 = dV*dV; double dUV = dU*dV; + dW2 += dW*dW; + A(0,0) = A(0,0) + dU2*dU2; A(0,1) = A(0,1) + dU2*dV2; A(0,2) = A(0,2) + dU2*dUV; @@ -690,44 +714,84 @@ double SurfaceFit::PolynomFit() A(5,4) = A(4,5); -#if defined(FC_USE_BOOST) - namespace lapack= boost::numeric::bindings::lapack; - //std::clog << A << std::endl; - //std::clog << b << std::endl; - //lapack::gesv(A,b); - ublas::vector x(6); - x = b; - //std::clog << b << std::endl; -#else - // A.llt().solve(b,&x); // not sure if always positive definite - A.qr().solve(b,&x); -#endif + Eigen::HouseholderQR< Eigen::Matrix > qr(A); + x = qr.solve(b); - _fCoeff[0] = (float)(-x(5)); - _fCoeff[1] = (float)(-x(3)); - _fCoeff[2] = (float)(-x(4)); - _fCoeff[3] = 1.0f; - _fCoeff[4] = (float)(-x(0)); - _fCoeff[5] = (float)(-x(1)); - _fCoeff[6] = 0.0f; - _fCoeff[7] = (float)(-x(2)); - _fCoeff[8] = 0.0f; - _fCoeff[9] = 0.0f; + // FunctionContainer gets an implicit function F(x,y,z) = 0 of this form + // _fCoeff[0] + + // _fCoeff[1]*x + _fCoeff[2]*y + _fCoeff[3]*z + + // _fCoeff[4]*x^2 + _fCoeff[5]*y^2 + _fCoeff[6]*z^2 + + // _fCoeff[7]*x*y + _fCoeff[8]*x*z + _fCoeff[9]*y*z + // + // The bivariate polynomial surface we get here is of the form + // z = f(x,y) = a*x^2 + b*y^2 + c*x*y + d*x + e*y + f + // Writing it as implicit surface F(x,y,z) = 0 gives this form + // F(x,y,z) = f(x,y) - z = a*x^2 + b*y^2 + c*x*y + d*x + e*y - z + f + // Thus: + // _fCoeff[0] = f + // _fCoeff[1] = d + // _fCoeff[2] = e + // _fCoeff[3] = -1 + // _fCoeff[4] = a + // _fCoeff[5] = b + // _fCoeff[6] = 0 + // _fCoeff[7] = c + // _fCoeff[8] = 0 + // _fCoeff[9] = 0 - return 0.0f; + _fCoeff[0] = x(5); + _fCoeff[1] = x(3); + _fCoeff[2] = x(4); + _fCoeff[3] = -1.0; + _fCoeff[4] = x(0); + _fCoeff[5] = x(1); + _fCoeff[6] = 0.0; + _fCoeff[7] = x(2); + _fCoeff[8] = 0.0; + _fCoeff[9] = 0.0; + + // Get S(P) = sum[(P*Vi)^2 - 2*(P*Vi)*zi + zi^2] + double sigma = 0; + FunctionContainer clFuncCont(_fCoeff); + for (std::vector::const_iterator it = transform.begin(); it != transform.end(); ++it) { + double u = it->x; + double v = it->y; + double z = clFuncCont.F(u, v, 0.0); + sigma += z*z; + } + + sigma += dW2 - 2 * x.dot(b); + // This must be caused by some round-off errors. Theoretically it's impossible + // that 'sigma' becomes negative. + if (sigma < 0) + sigma = 0; + sigma = sqrt(sigma/_vPoints.size()); + + _fLastResult = static_cast(sigma); + return _fLastResult; } double SurfaceFit::Value(double x, double y) const { - float z = 0.0f; + double z = 0.0; if (_bIsFitted) { FunctionContainer clFuncCont(_fCoeff); - z = (float) clFuncCont.F(x, y, 0.0f); + z = clFuncCont.F(x, y, 0.0); } return z; } +void SurfaceFit::GetCoefficients(double& a,double& b,double& c,double& d,double& e,double& f) const +{ + a = _fCoeff[4]; + b = _fCoeff[5]; + c = _fCoeff[7]; + d = _fCoeff[1]; + e = _fCoeff[2]; + f = _fCoeff[0]; +} + // ----------------------------------------------------------------------------- PolynomialFit::PolynomialFit() @@ -773,7 +837,7 @@ float PolynomialFit::Value(float x, float y) const _fCoeff[3] * y + _fCoeff[4] * x * y + _fCoeff[5] * x * x * y + - _fCoeff[6] * y * y + + _fCoeff[6] * y * y + _fCoeff[7] * x * y * y + _fCoeff[8] * x * x * y * y; return fValue; diff --git a/src/Mod/Mesh/App/Core/Approximation.h b/src/Mod/Mesh/App/Core/Approximation.h index e0ff07fa9..c48d3b317 100644 --- a/src/Mod/Mesh/App/Core/Approximation.h +++ b/src/Mod/Mesh/App/Core/Approximation.h @@ -103,11 +103,11 @@ public: /** * Construction */ - Approximation() : _bIsFitted(false) { } + Approximation(); /** * Destroys the object and frees any allocated resources. */ - virtual ~Approximation(){ Clear(); } + virtual ~Approximation(); /** * Add point for the fit algorithm. */ @@ -188,11 +188,11 @@ public: /** * Construction */ - PlaneFit(){}; + PlaneFit(); /** * Destruction */ - virtual ~PlaneFit(){}; + virtual ~PlaneFit(); Base::Vector3f GetBase() const; Base::Vector3f GetDirU() const; Base::Vector3f GetDirV() const; @@ -229,6 +229,12 @@ public: * Get the dimension of the fitted plane. */ void Dimension(float& length, float& width) const; + /** + * Returns an array of the transformed points relative to the coordinate system + * of the plane. If this method is called before the plane is computed an empty + * array is returned. + */ + std::vector GetLocalPoints() const; protected: Base::Vector3f _vBase; /**< Base vector of the plane. */ @@ -319,7 +325,7 @@ protected: * to get a parametrisation of the points afterwards. The coordinates of the points with respect to the local * coordinate system of the plane are determined and then a quadratic polynomial function of the form: * w = f(u,v) = a*u^2 + b*v^2 + c*u*v + d*u + e*v + f - * is deermined. + * is determined. * This approach was developed as an alternative for the 3D approach with quadrics because * the latter suffers from strange artifacts in planar areas. */ @@ -340,6 +346,7 @@ public: bool GetCurvatureInfo(double x, double y, double z, double &rfCurv0, double &rfcurv1); float Fit(); double Value(double x, double y) const; + void GetCoefficients(double& a,double& b,double& c,double& d,double& e,double& f) const; protected: double PolynomFit(); diff --git a/src/Mod/Mesh/App/MeshTestsApp.py b/src/Mod/Mesh/App/MeshTestsApp.py index e798c111e..d4834d27f 100644 --- a/src/Mod/Mesh/App/MeshTestsApp.py +++ b/src/Mod/Mesh/App/MeshTestsApp.py @@ -1,7 +1,7 @@ # (c) Juergen Riegel (juergen.riegel@web.de) 2007 LGPL import FreeCAD, os, sys, unittest, Mesh -import thread, time, tempfile +import thread, time, tempfile, math #--------------------------------------------------------------------------- @@ -161,3 +161,62 @@ class LoadMeshInThreadsCases(unittest.TestCase): def tearDown(self): pass + + +class PolynomialFitCases(unittest.TestCase): + def setUp(self): + pass + + def testFitGood(self): + # symmetric + v=[] + v.append(FreeCAD.Vector(0,0,0.0)) + v.append(FreeCAD.Vector(1,0,0.5)) + v.append(FreeCAD.Vector(2,0,0.0)) + v.append(FreeCAD.Vector(0,1,0.5)) + v.append(FreeCAD.Vector(1,1,1.0)) + v.append(FreeCAD.Vector(2,1,0.5)) + v.append(FreeCAD.Vector(0,2,0.0)) + v.append(FreeCAD.Vector(1,2,0.5)) + v.append(FreeCAD.Vector(2,2,0.0)) + d = Mesh.polynomialFit(v) + c = d["Coefficients"] + print ("Polynomial: f(x,y)=%f*x^2%+f*y^2%+f*x*y%+f*x%+f*y%+f" % (c[0],c[1],c[2],c[3],c[4],c[5])) + for i in d["Residuals"]: + self.failUnless(math.fabs(i) < 0.0001, "Too high residual %f" % math.fabs(i)) + + def testFitExact(self): + # symmetric + v=[] + v.append(FreeCAD.Vector(0,0,0.0)) + v.append(FreeCAD.Vector(1,0,0.0)) + v.append(FreeCAD.Vector(2,0,0.0)) + v.append(FreeCAD.Vector(0,1,0.0)) + v.append(FreeCAD.Vector(1,1,1.0)) + v.append(FreeCAD.Vector(2,1,0.0)) + d = Mesh.polynomialFit(v) + c = d["Coefficients"] + print ("Polynomial: f(x,y)=%f*x^2%+f*y^2%+f*x*y%+f*x%+f*y%+f" % (c[0],c[1],c[2],c[3],c[4],c[5])) + for i in d["Residuals"]: + self.failUnless(math.fabs(i) < 0.0001, "Too high residual %f" % math.fabs(i)) + + def testFitBad(self): + # symmetric + v=[] + v.append(FreeCAD.Vector(0,0,0.0)) + v.append(FreeCAD.Vector(1,0,0.0)) + v.append(FreeCAD.Vector(2,0,0.0)) + v.append(FreeCAD.Vector(0,1,0.0)) + v.append(FreeCAD.Vector(1,1,1.0)) + v.append(FreeCAD.Vector(2,1,0.0)) + v.append(FreeCAD.Vector(0,2,0.0)) + v.append(FreeCAD.Vector(1,2,0.0)) + v.append(FreeCAD.Vector(2,2,0.0)) + d = Mesh.polynomialFit(v) + c = d["Coefficients"] + print ("Polynomial: f(x,y)=%f*x^2%+f*y^2%+f*x*y%+f*x%+f*y%+f" % (c[0],c[1],c[2],c[3],c[4],c[5])) + for i in d["Residuals"]: + self.failIf(math.fabs(i) < 0.0001, "Residual %f must be higher" % math.fabs(i)) + + def tearDown(self): + pass From dc224265fa0c0fed4e88eb881b1e7d4968ca6fa9 Mon Sep 17 00:00:00 2001 From: Abdullah Tahiri Date: Tue, 13 Oct 2015 14:13:21 +0200 Subject: [PATCH 02/20] Sketcher: Bug fix: Constraints dissapear after a sketch loses support ===================================================================== Issue: http://freecadweb.org/tracker/view.php?id=2292 Constraints dissapear after a sketch loses support Steps To Reproduce: 1. Make a sketch on a face. 2. Constrain it 3. Select "reorient sketch" 4. Do you want to lose support? Yes 5. Click cancel on the reorientation dialog. 6. Enter edit mode: No constraints... Why? GeoUndef not checked when deleting all external geometry (on dettaching from the support). Solution: Check for GeoUndef. --- src/Mod/Sketcher/App/SketchObject.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/Mod/Sketcher/App/SketchObject.cpp b/src/Mod/Sketcher/App/SketchObject.cpp index c5fc2703a..646321f54 100644 --- a/src/Mod/Sketcher/App/SketchObject.cpp +++ b/src/Mod/Sketcher/App/SketchObject.cpp @@ -1,5 +1,5 @@ /*************************************************************************** - * Copyright (c) Jürgen Riegel (juergen.riegel@web.de) 2008 * + * Copyright (c) Jürgen Riegel (juergen.riegel@web.de) 2008 * * * * This file is part of the FreeCAD CAx development system. * * * @@ -2754,7 +2754,9 @@ int SketchObject::delAllExternal() std::vector< Constraint * > newConstraints(0); for (std::vector::const_iterator it = constraints.begin(); it != constraints.end(); ++it) { - if ((*it)->First > -3 && (*it)->Second > -3 && (*it)->Third > -3) { + if ((*it)->First > -3 && + ((*it)->Second > -3 || (*it)->Second == Constraint::GeoUndef ) && + ((*it)->Third > -3 || (*it)->Third == Constraint::GeoUndef) ) { Constraint *copiedConstr = (*it)->clone(); newConstraints.push_back(copiedConstr); From 60ac7e4cb80241834ca159e9aaf17208e79ec919 Mon Sep 17 00:00:00 2001 From: Yorik van Havre Date: Tue, 13 Oct 2015 16:38:35 -0300 Subject: [PATCH 03/20] Sketcher's GCS solver fixed for eigen3.3 --- src/Mod/Sketcher/App/planegcs/GCS.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/Mod/Sketcher/App/planegcs/GCS.cpp b/src/Mod/Sketcher/App/planegcs/GCS.cpp index 5df8e7406..7de4aeda2 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.cpp +++ b/src/Mod/Sketcher/App/planegcs/GCS.cpp @@ -30,14 +30,17 @@ // NOTE: In CMakeList.txt -DEIGEN_NO_DEBUG is set (it does not work with a define here), to solve this: // this is needed to fix this SparseQR crash http://forum.freecadweb.org/viewtopic.php?f=10&t=11341&p=92146#p92146, // until Eigen library fixes its own problem with the assertion (definitely not solved in 3.2.0 branch) +// NOTE2: solved in eigen3.3 #define EIGEN_VERSION (EIGEN_WORLD_VERSION * 10000 \ + EIGEN_MAJOR_VERSION * 100 \ + EIGEN_MINOR_VERSION) #if EIGEN_VERSION >= 30202 +#if EIGEN_VERSION < 30290 // this is eigen3.3. Bad numbering? This should be safe anyway. #define EIGEN_SPARSEQR_COMPATIBLE #endif +#endif //#undef EIGEN_SPARSEQR_COMPATIBLE From f1f67b229801bca1127f1354dccc333d5ab460d2 Mon Sep 17 00:00:00 2001 From: Przemo Firszt Date: Sat, 10 Oct 2015 14:28:42 +0100 Subject: [PATCH 04/20] FEM: Add FemCommand class and use it in _CommandFrequencyAnalysis FemCommand class will gather all common functions/propertied od FEM gui commands. That should allow to reduce some common code. Signed-off-by: Przemo Firszt --- src/Mod/Fem/App/CMakeLists.txt | 1 + src/Mod/Fem/CMakeLists.txt | 1 + src/Mod/Fem/FemCommands.py | 55 ++++++++++++++++++++++++ src/Mod/Fem/Gui/AppFemGui.cpp | 2 + src/Mod/Fem/_CommandFrequencyAnalysis.py | 45 ++++++++++++++----- 5 files changed, 94 insertions(+), 10 deletions(-) create mode 100644 src/Mod/Fem/FemCommands.py diff --git a/src/Mod/Fem/App/CMakeLists.txt b/src/Mod/Fem/App/CMakeLists.txt index 6d64ae9c6..309c1b29c 100755 --- a/src/Mod/Fem/App/CMakeLists.txt +++ b/src/Mod/Fem/App/CMakeLists.txt @@ -82,6 +82,7 @@ SET(FemScripts_SRCS MechanicalMaterial.ui MechanicalMaterial.py ShowDisplacement.ui + FemCommands.py _ResultControlTaskPanel.py _JobControlTaskPanel.py _ViewProviderFemAnalysis.py diff --git a/src/Mod/Fem/CMakeLists.txt b/src/Mod/Fem/CMakeLists.txt index 607b0d180..73de859b0 100755 --- a/src/Mod/Fem/CMakeLists.txt +++ b/src/Mod/Fem/CMakeLists.txt @@ -24,6 +24,7 @@ INSTALL( MechanicalMaterial.ui MechanicalAnalysis.ui ShowDisplacement.ui + FemCommands.py _ResultControlTaskPanel.py _JobControlTaskPanel.py _ViewProviderFemAnalysis.py diff --git a/src/Mod/Fem/FemCommands.py b/src/Mod/Fem/FemCommands.py new file mode 100644 index 000000000..a3a7e6c80 --- /dev/null +++ b/src/Mod/Fem/FemCommands.py @@ -0,0 +1,55 @@ +#*************************************************************************** +#* * +#* Copyright (c) 2015 - FreeCAD Developers * +#* Author (c) 2015 - Przemo Fiszt < przemo@firszt.eu> * +#* * +#* This program is free software; you can redistribute it and/or modify * +#* it under the terms of the GNU Lesser General Public License (LGPL) * +#* as published by the Free Software Foundation; either version 2 of * +#* the License, or (at your option) any later version. * +#* for detail see the LICENCE text file. * +#* * +#* This program is distributed in the hope that it will be useful, * +#* but WITHOUT ANY WARRANTY; without even the implied warranty of * +#* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +#* GNU Library General Public License for more details. * +#* * +#* You should have received a copy of the GNU Library General Public * +#* License along with this program; if not, write to the Free Software * +#* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 * +#* USA * +#* * +#*************************************************************************** + +__title__ = "Fem Commands" +__author__ = "Przemo Firszt" +__url__ = "http://www.freecadweb.org" + +import FreeCAD + +if FreeCAD.GuiUp: + import FreeCADGui + import FemGui + from PySide import QtCore + + +class FemCommands(object): + def __init__(self): + self.resources = {'Pixmap': 'fem-frequency-analysis', + 'MenuText': QtCore.QT_TRANSLATE_NOOP("Fem_Command", "Default Fem Command MenuText"), + 'Accel': "", + 'ToolTip': QtCore.QT_TRANSLATE_NOOP("Fem_Command", "Default Fem Command ToolTip")} + #FIXME add option description + self.is_active = None + + def GetResources(self): + return self.resources + + def IsActive(self): + if not self.is_active: + active = False + elif self.is_active == 'with_document': + active = FreeCADGui.ActiveDocument is not None + elif self.is_active == 'with_analysis': + active = FreeCADGui.ActiveDocument is not None and FemGui.getActiveAnalysis() is not None + return active diff --git a/src/Mod/Fem/Gui/AppFemGui.cpp b/src/Mod/Fem/Gui/AppFemGui.cpp index dc427f658..c8e621f3d 100644 --- a/src/Mod/Fem/Gui/AppFemGui.cpp +++ b/src/Mod/Fem/Gui/AppFemGui.cpp @@ -102,6 +102,8 @@ void FemGuiExport initFemGui() FemGui::ViewProviderResult ::init(); FemGui::ViewProviderResultPython ::init(); + Base::Interpreter().loadModule("FemCommands"); + Base::Interpreter().loadModule("_CommandMechanicalShowResult"); Base::Interpreter().loadModule("_CommandFrequencyAnalysis"); Base::Interpreter().loadModule("_CommandQuickAnalysis"); diff --git a/src/Mod/Fem/_CommandFrequencyAnalysis.py b/src/Mod/Fem/_CommandFrequencyAnalysis.py index aa3a29165..0f2ebb88c 100644 --- a/src/Mod/Fem/_CommandFrequencyAnalysis.py +++ b/src/Mod/Fem/_CommandFrequencyAnalysis.py @@ -1,18 +1,46 @@ +#*************************************************************************** +#* * +#* Copyright (c) 2013-2015 - Juergen Riegel * +#* * +#* This program is free software; you can redistribute it and/or modify * +#* it under the terms of the GNU Lesser General Public License (LGPL) * +#* as published by the Free Software Foundation; either version 2 of * +#* the License, or (at your option) any later version. * +#* for detail see the LICENCE text file. * +#* * +#* This program is distributed in the hope that it will be useful, * +#* but WITHOUT ANY WARRANTY; without even the implied warranty of * +#* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * +#* GNU Library General Public License for more details. * +#* * +#* You should have received a copy of the GNU Library General Public * +#* License along with this program; if not, write to the Free Software * +#* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 * +#* USA * +#* * +#*************************************************************************** + +__title__ = "Command Frequency Analysis" +__author__ = "Juergen Riegel" +__url__ = "http://www.freecadweb.org" + import FreeCAD +from FemCommands import FemCommands from FemTools import FemTools if FreeCAD.GuiUp: import FreeCADGui - import FemGui from PySide import QtCore, QtGui -class _CommandFrequencyAnalysis: - def GetResources(self): - return {'Pixmap': 'fem-frequency-analysis', - 'MenuText': QtCore.QT_TRANSLATE_NOOP("Fem_Frequency_Analysis", "Run frequency analysis with CalculiX ccx"), - 'Accel': "R, F", - 'ToolTip': QtCore.QT_TRANSLATE_NOOP("Fem_Frequency_Analysis", "Write .inp file and run frequency analysis with CalculiX ccx")} +class _CommandFrequencyAnalysis(FemCommands): + def __init__(self): + super(_CommandFrequencyAnalysis, self).__init__() + self.resources = {'Pixmap': 'fem-frequency-analysis', + 'MenuText': QtCore.QT_TRANSLATE_NOOP("Fem_Frequency_Analysis", "Run frequency analysis with CalculiX ccx"), + 'Accel': "R, F", + 'ToolTip': QtCore.QT_TRANSLATE_NOOP("Fem_Frequency_Analysis", "Write .inp file and run frequency analysis with CalculiX ccx")} + self.is_active = 'with_analysis' def Activated(self): def load_results(ret_code): @@ -31,9 +59,6 @@ class _CommandFrequencyAnalysis: self.fea.finished.connect(load_results) QtCore.QThreadPool.globalInstance().start(self.fea) - def IsActive(self): - return FreeCADGui.ActiveDocument is not None and FemGui.getActiveAnalysis() is not None - if FreeCAD.GuiUp: FreeCADGui.addCommand('Fem_Frequency_Analysis', _CommandFrequencyAnalysis()) From 044c16d218faf897401c2b463b34ac56e6c9df01 Mon Sep 17 00:00:00 2001 From: Przemo Firszt Date: Mon, 12 Oct 2015 21:48:01 +0100 Subject: [PATCH 05/20] FEM: Migrate _CommandNewMechanicalAnalysis to FemCommands Signed-off-by: Przemo Firszt --- src/Mod/Fem/_CommandNewMechanicalAnalysis.py | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/src/Mod/Fem/_CommandNewMechanicalAnalysis.py b/src/Mod/Fem/_CommandNewMechanicalAnalysis.py index 82317cdf6..3d41f404f 100644 --- a/src/Mod/Fem/_CommandNewMechanicalAnalysis.py +++ b/src/Mod/Fem/_CommandNewMechanicalAnalysis.py @@ -25,20 +25,22 @@ __author__ = "Juergen Riegel" __url__ = "http://www.freecadweb.org" import FreeCAD +from FemCommands import FemCommands if FreeCAD.GuiUp: import FreeCADGui - import FemGui from PySide import QtCore -class _CommandNewMechanicalAnalysis: +class _CommandNewMechanicalAnalysis(FemCommands): "the Fem Analysis command definition" - def GetResources(self): - return {'Pixmap': 'fem-analysis', - 'MenuText': QtCore.QT_TRANSLATE_NOOP("Fem_Analysis", "New mechanical analysis"), - 'Accel': "N, A", - 'ToolTip': QtCore.QT_TRANSLATE_NOOP("Fem_Analysis", "Create a new mechanical analysis")} + def __init__(self): + super(_CommandNewMechanicalAnalysis, self).__init__() + self.resources = {'Pixmap': 'fem-analysis', + 'MenuText': QtCore.QT_TRANSLATE_NOOP("Fem_Analysis", "New mechanical analysis"), + 'Accel': "N, A", + 'ToolTip': QtCore.QT_TRANSLATE_NOOP("Fem_Analysis", "Create a new mechanical analysis")} + self.is_active = 'with_document' def Activated(self): FreeCAD.ActiveDocument.openTransaction("Create Analysis") @@ -63,9 +65,5 @@ class _CommandNewMechanicalAnalysis: #FreeCAD.ActiveDocument.commitTransaction() FreeCADGui.Selection.clearSelection() - def IsActive(self): - return FreeCADGui.ActiveDocument is not None and FemGui.getActiveAnalysis() is None - - if FreeCAD.GuiUp: FreeCADGui.addCommand('Fem_NewMechanicalAnalysis', _CommandNewMechanicalAnalysis()) From 217103affb66cb37f8fb9d9a3987b65999fa88c3 Mon Sep 17 00:00:00 2001 From: Przemo Firszt Date: Tue, 13 Oct 2015 10:41:38 +0100 Subject: [PATCH 06/20] FEM: Migrate _CommandPurgeFemResults to FemCommands and add new is_active type Signed-off-by: Przemo Firszt --- src/Mod/Fem/FemCommands.py | 10 +++++++++ src/Mod/Fem/_CommandPurgeFemResults.py | 28 +++++++++----------------- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/src/Mod/Fem/FemCommands.py b/src/Mod/Fem/FemCommands.py index a3a7e6c80..2c959d12f 100644 --- a/src/Mod/Fem/FemCommands.py +++ b/src/Mod/Fem/FemCommands.py @@ -52,4 +52,14 @@ class FemCommands(object): active = FreeCADGui.ActiveDocument is not None elif self.is_active == 'with_analysis': active = FreeCADGui.ActiveDocument is not None and FemGui.getActiveAnalysis() is not None + elif self.is_active == 'with_results': + active = FreeCADGui.ActiveDocument is not None and FemGui.getActiveAnalysis() is not None and self.results_present() return active + + def results_present(self): + results = False + analysis_members = FemGui.getActiveAnalysis().Member + for o in analysis_members: + if o.isDerivedFrom('Fem::FemResultObject'): + results = True + return results diff --git a/src/Mod/Fem/_CommandPurgeFemResults.py b/src/Mod/Fem/_CommandPurgeFemResults.py index 3e6993f87..16331923d 100644 --- a/src/Mod/Fem/_CommandPurgeFemResults.py +++ b/src/Mod/Fem/_CommandPurgeFemResults.py @@ -25,6 +25,7 @@ __author__ = "Juergen Riegel" __url__ = "http://www.freecadweb.org" import FreeCAD +from FemCommands import FemCommands from FemTools import FemTools if FreeCAD.GuiUp: @@ -32,30 +33,19 @@ if FreeCAD.GuiUp: from PySide import QtCore -class _CommandPurgeFemResults: - def GetResources(self): - return {'Pixmap': 'fem-purge-results', - 'MenuText': QtCore.QT_TRANSLATE_NOOP("Fem_PurgeResults", "Purge results"), - 'Accel': "S, S", - 'ToolTip': QtCore.QT_TRANSLATE_NOOP("Fem_PurgeResults", "Purge results from an analysis")} +class _CommandPurgeFemResults(FemCommands): + def __init__(self): + super(_CommandPurgeFemResults, self).__init__() + self.resources = {'Pixmap': 'fem-purge-results', + 'MenuText': QtCore.QT_TRANSLATE_NOOP("Fem_PurgeResults", "Purge results"), + 'Accel': "S, S", + 'ToolTip': QtCore.QT_TRANSLATE_NOOP("Fem_PurgeResults", "Purge results from an analysis")} + self.is_active = 'with_results' def Activated(self): fea = FemTools() fea.reset_all() - def IsActive(self): - return FreeCADGui.ActiveDocument is not None and results_present() - - -#Code duplication to be removed after migration to FemTools -def results_present(): - import FemGui - results = False - analysis_members = FemGui.getActiveAnalysis().Member - for o in analysis_members: - if o.isDerivedFrom('Fem::FemResultObject'): - results = True - return results if FreeCAD.GuiUp: FreeCADGui.addCommand('Fem_PurgeResults', _CommandPurgeFemResults()) From 1ad481b43531d7119fc69829f71b2852f0120476 Mon Sep 17 00:00:00 2001 From: Przemo Firszt Date: Tue, 13 Oct 2015 14:11:48 +0100 Subject: [PATCH 07/20] FEM: Migrate _CommandFemFromShape to FemCommands and add new is_active type Signed-off-by: Przemo Firszt --- src/Mod/Fem/FemCommands.py | 9 +++++++++ src/Mod/Fem/_CommandFemFromShape.py | 18 ++++++++---------- 2 files changed, 17 insertions(+), 10 deletions(-) diff --git a/src/Mod/Fem/FemCommands.py b/src/Mod/Fem/FemCommands.py index 2c959d12f..2fdf417a9 100644 --- a/src/Mod/Fem/FemCommands.py +++ b/src/Mod/Fem/FemCommands.py @@ -54,6 +54,8 @@ class FemCommands(object): active = FreeCADGui.ActiveDocument is not None and FemGui.getActiveAnalysis() is not None elif self.is_active == 'with_results': active = FreeCADGui.ActiveDocument is not None and FemGui.getActiveAnalysis() is not None and self.results_present() + elif self.is_active == 'with_part_feature': + active = FreeCADGui.ActiveDocument is not None and FemGui.getActiveAnalysis() is not None and self.part_feature_selected() return active def results_present(self): @@ -63,3 +65,10 @@ class FemCommands(object): if o.isDerivedFrom('Fem::FemResultObject'): results = True return results + + def part_feature_selected(self): + sel = FreeCADGui.Selection.getSelection() + if len(sel) == 1 and sel[0].isDerivedFrom("Part::Feature"): + return True + else: + return False diff --git a/src/Mod/Fem/_CommandFemFromShape.py b/src/Mod/Fem/_CommandFemFromShape.py index d1c3f5977..99e8bc21a 100644 --- a/src/Mod/Fem/_CommandFemFromShape.py +++ b/src/Mod/Fem/_CommandFemFromShape.py @@ -25,17 +25,20 @@ __author__ = "Juergen Riegel" __url__ = "http://www.freecadweb.org" import FreeCAD +from FemCommands import FemCommands if FreeCAD.GuiUp: import FreeCADGui from PySide import QtCore -class _CommandFemFromShape: - def GetResources(self): - return {'Pixmap': 'fem-fem-mesh-from-shape', - 'MenuText': QtCore.QT_TRANSLATE_NOOP("Fem_CreateFromShape", "Create FEM mesh"), - 'ToolTip': QtCore.QT_TRANSLATE_NOOP("Fem_CreateFromShape", "Create FEM mesh from shape")} +class _CommandFemFromShape(FemCommands): + def __init__(self): + super(_CommandFemFromShape, self).__init__() + self.resources = {'Pixmap': 'fem-fem-mesh-from-shape', + 'MenuText': QtCore.QT_TRANSLATE_NOOP("Fem_CreateFromShape", "Create FEM mesh"), + 'ToolTip': QtCore.QT_TRANSLATE_NOOP("Fem_CreateFromShape", "Create FEM mesh from shape")} + self.is_active = 'with_part_feature' def Activated(self): FreeCAD.ActiveDocument.openTransaction("Create FEM mesh") @@ -50,11 +53,6 @@ class _CommandFemFromShape: FreeCADGui.Selection.clearSelection() - def IsActive(self): - sel = FreeCADGui.Selection.getSelection() - if len(sel) == 1: - return sel[0].isDerivedFrom("Part::Feature") - return False if FreeCAD.GuiUp: FreeCADGui.addCommand('Fem_CreateFromShape', _CommandFemFromShape()) From 6725c540a6efeaac2e231204b50043bd38d547e0 Mon Sep 17 00:00:00 2001 From: Przemo Firszt Date: Tue, 13 Oct 2015 15:25:57 +0100 Subject: [PATCH 08/20] FEM: Migrate _CommandMechanicalJobControl to FemCommands Signed-off-by: Przemo Firszt --- src/Mod/Fem/_CommandMechanicalJobControl.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/Mod/Fem/_CommandMechanicalJobControl.py b/src/Mod/Fem/_CommandMechanicalJobControl.py index aeac10499..2f4182470 100644 --- a/src/Mod/Fem/_CommandMechanicalJobControl.py +++ b/src/Mod/Fem/_CommandMechanicalJobControl.py @@ -25,6 +25,7 @@ __author__ = "Juergen Riegel" __url__ = "http://www.freecadweb.org" import FreeCAD +from FemCommands import FemCommands if FreeCAD.GuiUp: import FreeCADGui @@ -32,13 +33,15 @@ if FreeCAD.GuiUp: from PySide import QtCore -class _CommandMechanicalJobControl: +class _CommandMechanicalJobControl(FemCommands): "the Fem JobControl command definition" - def GetResources(self): - return {'Pixmap': 'fem-new-analysis', - 'MenuText': QtCore.QT_TRANSLATE_NOOP("Fem_JobControl", "Start calculation"), - 'Accel': "S, C", - 'ToolTip': QtCore.QT_TRANSLATE_NOOP("Fem_JobControl", "Dialog to start the calculation of the mechanical anlysis")} + def __init__(self): + super(_CommandMechanicalJobControl, self).__init__() + self.resources = {'Pixmap': 'fem-new-analysis', + 'MenuText': QtCore.QT_TRANSLATE_NOOP("Fem_JobControl", "Start calculation"), + 'Accel': "S, C", + 'ToolTip': QtCore.QT_TRANSLATE_NOOP("Fem_JobControl", "Dialog to start the calculation of the mechanical anlysis")} + self.is_active = 'with_analysis' def Activated(self): import _JobControlTaskPanel @@ -47,9 +50,6 @@ class _CommandMechanicalJobControl: taskd.update() FreeCADGui.Control.showDialog(taskd) - def IsActive(self): - return FreeCADGui.ActiveDocument is not None and FemGui.getActiveAnalysis() is not None - if FreeCAD.GuiUp: FreeCADGui.addCommand('Fem_MechanicalJobControl', _CommandMechanicalJobControl()) From b122cb5866d609d1e55a5210c2b055a068f62c79 Mon Sep 17 00:00:00 2001 From: Przemo Firszt Date: Tue, 13 Oct 2015 15:26:20 +0100 Subject: [PATCH 09/20] FEM: Migrate _CommandMechanicalShowResult to FemCommands Signed-off-by: Przemo Firszt --- src/Mod/Fem/_CommandMechanicalShowResult.py | 29 +++++++-------------- 1 file changed, 9 insertions(+), 20 deletions(-) diff --git a/src/Mod/Fem/_CommandMechanicalShowResult.py b/src/Mod/Fem/_CommandMechanicalShowResult.py index 039576523..9796da791 100644 --- a/src/Mod/Fem/_CommandMechanicalShowResult.py +++ b/src/Mod/Fem/_CommandMechanicalShowResult.py @@ -25,19 +25,22 @@ __author__ = "Juergen Riegel" __url__ = "http://www.freecadweb.org" import FreeCAD +from FemCommands import FemCommands if FreeCAD.GuiUp: import FreeCADGui from PySide import QtCore, QtGui -class _CommandMechanicalShowResult: +class _CommandMechanicalShowResult(FemCommands): "the Fem JobControl command definition" - def GetResources(self): - return {'Pixmap': 'fem-result', - 'MenuText': QtCore.QT_TRANSLATE_NOOP("Fem_Result", "Show result"), - 'Accel': "S, R", - 'ToolTip': QtCore.QT_TRANSLATE_NOOP("Fem_Result", "Show result information of an analysis")} + def __init__(self): + super(_CommandMechanicalShowResult, self).__init__() + self.resources = {'Pixmap': 'fem-result', + 'MenuText': QtCore.QT_TRANSLATE_NOOP("Fem_Result", "Show result"), + 'Accel': "S, R", + 'ToolTip': QtCore.QT_TRANSLATE_NOOP("Fem_Result", "Show result information of an analysis")} + self.is_active = 'with_results' def Activated(self): self.result_object = get_results_object(FreeCADGui.Selection.getSelection()) @@ -50,20 +53,6 @@ class _CommandMechanicalShowResult: taskd = _ResultControlTaskPanel._ResultControlTaskPanel() FreeCADGui.Control.showDialog(taskd) - def IsActive(self): - return FreeCADGui.ActiveDocument is not None and results_present() - - -#Code duplidation - to be removed after migration to FemTools -def results_present(): - import FemGui - results = False - analysis_members = FemGui.getActiveAnalysis().Member - for o in analysis_members: - if o.isDerivedFrom('Fem::FemResultObject'): - results = True - return results - #Code duplidation - to be removed after migration to FemTools def get_results_object(sel): From cddb6aa390ce5c29727c9c9da168d750de810896 Mon Sep 17 00:00:00 2001 From: Przemo Firszt Date: Tue, 13 Oct 2015 15:26:37 +0100 Subject: [PATCH 10/20] FEM: Migrate _CommandQuickAnalysis to FemCommands Signed-off-by: Przemo Firszt --- src/Mod/Fem/_CommandQuickAnalysis.py | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/src/Mod/Fem/_CommandQuickAnalysis.py b/src/Mod/Fem/_CommandQuickAnalysis.py index b401d50ba..0c574d5db 100644 --- a/src/Mod/Fem/_CommandQuickAnalysis.py +++ b/src/Mod/Fem/_CommandQuickAnalysis.py @@ -26,19 +26,21 @@ __url__ = "http://www.freecadweb.org" import FreeCAD from FemTools import FemTools +from FemCommands import FemCommands if FreeCAD.GuiUp: import FreeCADGui - import FemGui from PySide import QtCore, QtGui -class _CommandQuickAnalysis: - def GetResources(self): - return {'Pixmap': 'fem-quick-analysis', - 'MenuText': QtCore.QT_TRANSLATE_NOOP("Fem_Quick_Analysis", "Run CalculiX ccx"), - 'Accel': "R, C", - 'ToolTip': QtCore.QT_TRANSLATE_NOOP("Fem_Quick_Analysis", "Write .inp file and run CalculiX ccx")} +class _CommandQuickAnalysis(FemCommands): + def __init__(self): + super(_CommandQuickAnalysis, self).__init__() + self.resources = {'Pixmap': 'fem-quick-analysis', + 'MenuText': QtCore.QT_TRANSLATE_NOOP("Fem_Quick_Analysis", "Run CalculiX ccx"), + 'Accel': "R, C", + 'ToolTip': QtCore.QT_TRANSLATE_NOOP("Fem_Quick_Analysis", "Write .inp file and run CalculiX ccx")} + self.is_active = 'with_analysis' def Activated(self): def load_results(ret_code): @@ -64,9 +66,6 @@ class _CommandQuickAnalysis: tp = _ResultControlTaskPanel._ResultControlTaskPanel() tp.restore_result_dialog() - def IsActive(self): - return FreeCADGui.ActiveDocument is not None and FemGui.getActiveAnalysis() is not None - if FreeCAD.GuiUp: FreeCADGui.addCommand('Fem_Quick_Analysis', _CommandQuickAnalysis()) From e4ac34ecb0cf7c69d6fbe7c032a4ec2f975c367c Mon Sep 17 00:00:00 2001 From: Yorik van Havre Date: Tue, 13 Oct 2015 16:44:09 -0300 Subject: [PATCH 11/20] Path: Removed KDL/Robot dependency --- CMakeLists.txt | 2 +- src/Mod/Path/App/CMakeLists.txt | 41 ++++++++++++++++----------------- src/Mod/Path/App/Path.cpp | 9 ++++---- src/Mod/Path/App/PreCompiled.h | 4 ++-- 4 files changed, 28 insertions(+), 28 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8e804b454..9b9787393 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -292,7 +292,7 @@ endif(BUILD_ROBOT) #path module dependencies. if(BUILD_PATH) set(BUILD_PART ON) - set(BUILD_ROBOT ON) + #set(BUILD_ROBOT ON) reenable this when using KDL endif(BUILD_PATH) #inferred from cmakelists.txt. appears to be working. diff --git a/src/Mod/Path/App/CMakeLists.txt b/src/Mod/Path/App/CMakeLists.txt index 11b508009..4d2387839 100644 --- a/src/Mod/Path/App/CMakeLists.txt +++ b/src/Mod/Path/App/CMakeLists.txt @@ -73,27 +73,26 @@ SET(Path_SRCS SOURCE_GROUP("Python" FILES ${Python_SRCS}) SOURCE_GROUP("Module" FILES ${Mod_SRCS}) -if (WIN32) - add_definitions(-DEIGEN2_SUPPORT) - FILE( GLOB KDL_SRCS ${CMAKE_SOURCE_DIR}/src/Mod/Robot/App/kdl_cp/[^.]*.cpp ) - FILE( GLOB KDL_HPPS ${CMAKE_SOURCE_DIR}/src/Mod/Robot/App/kdl_cp/[^.]*.hpp - ${CMAKE_SOURCE_DIR}/src/Mod/Robot/App/kdl_cp/[^.]*.inl) - - FILE( GLOB UTIL_SRCS ${CMAKE_SOURCE_DIR}/src/Mod/Robot/App/kdl_cp/utilities/[^.]*.cpp - ${CMAKE_SOURCE_DIR}/src/Mod/Robot/App/kdl_cp/utilities/[^.]*.cxx) - FILE( GLOB UTIL_HPPS ${CMAKE_SOURCE_DIR}/src/Mod/Robot/App/kdl_cp/utilities/[^.]*.h - ${CMAKE_SOURCE_DIR}/src/Mod/Robot/App/kdl_cp/utilities/[^.]*.hpp) - - SET(Path_SRCS - ${Path_SRCS} - ${KDL_SRCS} - ${KDL_HPPS} - ${UTIL_SRCS} - ${UTIL_HPPS} - ) - - SOURCE_GROUP("KDL" FILES ${KDL_SRCS} ${KDL_HPPS} ${UTIL_SRCS} ${UTIL_HPPS} ) -endif(WIN32) +#if (WIN32) uncomment to use KDL +# FILE( GLOB KDL_SRCS ${CMAKE_SOURCE_DIR}/src/Mod/Robot/App/kdl_cp/[^.]*.cpp ) +# FILE( GLOB KDL_HPPS ${CMAKE_SOURCE_DIR}/src/Mod/Robot/App/kdl_cp/[^.]*.hpp +# ${CMAKE_SOURCE_DIR}/src/Mod/Robot/App/kdl_cp/[^.]*.inl) +# +# FILE( GLOB UTIL_SRCS ${CMAKE_SOURCE_DIR}/src/Mod/Robot/App/kdl_cp/utilities/[^.]*.cpp +# ${CMAKE_SOURCE_DIR}/src/Mod/Robot/App/kdl_cp/utilities/[^.]*.cxx) +# FILE( GLOB UTIL_HPPS ${CMAKE_SOURCE_DIR}/src/Mod/Robot/App/kdl_cp/utilities/[^.]*.h +# ${CMAKE_SOURCE_DIR}/src/Mod/Robot/App/kdl_cp/utilities/[^.]*.hpp) +# +# SET(Path_SRCS +# ${Path_SRCS} +# ${KDL_SRCS} +# ${KDL_HPPS} +# ${UTIL_SRCS} +# ${UTIL_HPPS} +# ) +# +# SOURCE_GROUP("KDL" FILES ${KDL_SRCS} ${KDL_HPPS} ${UTIL_SRCS} ${UTIL_HPPS} ) +#endif(WIN32) SET(Path_Scripts Init.py diff --git a/src/Mod/Path/App/Path.cpp b/src/Mod/Path/App/Path.cpp index aafe99aef..4f609b9a6 100644 --- a/src/Mod/Path/App/Path.cpp +++ b/src/Mod/Path/App/Path.cpp @@ -34,10 +34,11 @@ #include #include -#include "Mod/Robot/App/kdl_cp/path_line.hpp" -#include "Mod/Robot/App/kdl_cp/path_circle.hpp" -#include "Mod/Robot/App/kdl_cp/rotational_interpolation_sa.hpp" -#include "Mod/Robot/App/kdl_cp/utilities/error.h" +// KDL stuff - at the moment, not used +//#include "Mod/Robot/App/kdl_cp/path_line.hpp" +//#include "Mod/Robot/App/kdl_cp/path_circle.hpp" +//#include "Mod/Robot/App/kdl_cp/rotational_interpolation_sa.hpp" +//#include "Mod/Robot/App/kdl_cp/utilities/error.h" #include "Path.h" diff --git a/src/Mod/Path/App/PreCompiled.h b/src/Mod/Path/App/PreCompiled.h index 07a2af006..dbac84df9 100644 --- a/src/Mod/Path/App/PreCompiled.h +++ b/src/Mod/Path/App/PreCompiled.h @@ -29,12 +29,12 @@ // Exporting of App classes #ifdef FC_OS_WIN32 # define PathExport __declspec(dllexport) -# define RobotExport __declspec(dllexport) +//# define RobotExport __declspec(dllexport) uncomment this to use KDL # define PartExport __declspec(dllexport) # define BaseExport __declspec(dllimport) #else // for Linux # define PathExport -# define RobotExport +//# define RobotExport uncomment this to use KDL # define PartExport # define BaseExport #endif From e380001762a160e6b9557b5470933eb7e688b2ff Mon Sep 17 00:00:00 2001 From: Yorik van Havre Date: Tue, 13 Oct 2015 17:22:05 -0300 Subject: [PATCH 12/20] Fixed Path's cmake file --- src/Mod/Path/App/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Mod/Path/App/CMakeLists.txt b/src/Mod/Path/App/CMakeLists.txt index 4d2387839..333782332 100644 --- a/src/Mod/Path/App/CMakeLists.txt +++ b/src/Mod/Path/App/CMakeLists.txt @@ -21,7 +21,7 @@ include_directories( link_directories(${OCC_LIBRARY_DIR}) set(Path_LIBS - Robot +# Robot Part ${QT_QTCORE_LIBRARY} FreeCADApp From 409ee710874f6a6d79b0c27a4c3fbb53bf28b9bf Mon Sep 17 00:00:00 2001 From: Yorik van Havre Date: Wed, 14 Oct 2015 11:18:10 -0300 Subject: [PATCH 13/20] Path: removing leftover KDL stuff --- src/Mod/Path/App/Path.h | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/Mod/Path/App/Path.h b/src/Mod/Path/App/Path.h index aa38c26e1..db7643f16 100644 --- a/src/Mod/Path/App/Path.h +++ b/src/Mod/Path/App/Path.h @@ -25,8 +25,8 @@ #define PATH_Path_H #include "Command.h" -#include "Mod/Robot/App/kdl_cp/path_composite.hpp" -#include "Mod/Robot/App/kdl_cp/frames_io.hpp" +//#include "Mod/Robot/App/kdl_cp/path_composite.hpp" +//#include "Mod/Robot/App/kdl_cp/frames_io.hpp" #include #include @@ -70,8 +70,9 @@ namespace Path protected: std::vector vpcCommands; - KDL::Path_Composite *pcPath; + //KDL::Path_Composite *pcPath; + /* inline KDL::Frame toFrame(const Base::Placement &To){ return KDL::Frame(KDL::Rotation::Quaternion(To.getRotation()[0], To.getRotation()[1], @@ -85,7 +86,7 @@ namespace Path 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 Path From 85994d17b8daf30bccdd11b0dde452f70af69fe9 Mon Sep 17 00:00:00 2001 From: wmayer Date: Wed, 14 Oct 2015 18:45:13 +0200 Subject: [PATCH 14/20] + replace || operator in BoundBox2D with real function names --- src/Base/Tools2D.cpp | 8 ++++---- src/Base/Tools2D.h | 6 +++--- src/Mod/Mesh/App/Core/Algorithm.cpp | 2 +- src/Mod/Mesh/App/Core/Trim.cpp | 2 +- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/Base/Tools2D.cpp b/src/Base/Tools2D.cpp index c67d715ad..e65871756 100644 --- a/src/Base/Tools2D.cpp +++ b/src/Base/Tools2D.cpp @@ -67,7 +67,7 @@ void Vector2D::ProjToLine (const Vector2D &rclPt, const Vector2D &rclLine) /********************************************************/ /** BOUNDBOX2D ********************************************/ -bool BoundBox2D::operator|| (const Line2D &rclLine) const +bool BoundBox2D::Intersect(const Line2D &rclLine) const { Line2D clThisLine; Vector2D clVct; @@ -104,7 +104,7 @@ bool BoundBox2D::operator|| (const Line2D &rclLine) const return false; } -bool BoundBox2D::operator|| (const BoundBox2D &rclBB) const +bool BoundBox2D::Intersect(const BoundBox2D &rclBB) const { //// compare bb2-points to this //if (Contains (Vector2D (rclBB.fMinX, rclBB.fMinY))) return TRUE; @@ -127,7 +127,7 @@ bool BoundBox2D::operator|| (const BoundBox2D &rclBB) const return false; } -bool BoundBox2D::operator|| (const Polygon2D &rclPoly) const +bool BoundBox2D::Intersect(const Polygon2D &rclPoly) const { unsigned long i; Line2D clLine; @@ -158,7 +158,7 @@ bool BoundBox2D::operator|| (const Polygon2D &rclPoly) const clLine.clV1 = rclPoly[i]; clLine.clV2 = rclPoly[i + 1]; } - if (*this || clLine) + if (Intersect(clLine)) return true; /***** RETURN INTERSECTION *********/ } // no intersection diff --git a/src/Base/Tools2D.h b/src/Base/Tools2D.h index 7f56d2dfc..062e7a58f 100644 --- a/src/Base/Tools2D.h +++ b/src/Base/Tools2D.h @@ -89,9 +89,9 @@ public: // operators inline BoundBox2D& operator= (const BoundBox2D& rclBB); inline bool operator== (const BoundBox2D& rclBB) const; - bool operator|| (const Line2D &rclLine) const; - bool operator|| (const BoundBox2D &rclBB) const; - bool operator|| (const Polygon2D &rclPoly) const; + bool Intersect(const Line2D &rclLine) const; + bool Intersect(const BoundBox2D &rclBB) const; + bool Intersect(const Polygon2D &rclPoly) const; inline void Add(const Vector2D &rclVct); void SetVoid (void) { fMinX = fMinY = DOUBLE_MAX; fMaxX = fMaxY = -DOUBLE_MAX; } diff --git a/src/Mod/Mesh/App/Core/Algorithm.cpp b/src/Mod/Mesh/App/Core/Algorithm.cpp index 35d3710e2..f822dccfe 100644 --- a/src/Mod/Mesh/App/Core/Algorithm.cpp +++ b/src/Mod/Mesh/App/Core/Algorithm.cpp @@ -1103,7 +1103,7 @@ void MeshAlgorithm::CheckFacets(const MeshFacetGrid& rclGrid, const Base::ViewPr { clBBox3d = clGridIter.GetBoundBox(); clViewBBox = clBBox3d.ProjectBox(pclProj); - if (clViewBBox || clPolyBBox) + if (clViewBBox.Intersect(clPolyBBox)) { // alle Elemente in AllElements sammeln clGridIter.GetElements(aulAllElements); diff --git a/src/Mod/Mesh/App/Core/Trim.cpp b/src/Mod/Mesh/App/Core/Trim.cpp index 88e049472..3d22d2e5d 100644 --- a/src/Mod/Mesh/App/Core/Trim.cpp +++ b/src/Mod/Mesh/App/Core/Trim.cpp @@ -71,7 +71,7 @@ void MeshTrimming::CheckFacets(const MeshFacetGrid& rclGrid, std::vector Date: Mon, 12 Oct 2015 19:35:53 -0300 Subject: [PATCH 15/20] Upgraded KDL to latest version --- src/Mod/Robot/App/kdl_cp/CMakeLists.txt | 151 +++++- .../App/kdl_cp/articulatedbodyinertia.cpp | 14 +- .../App/kdl_cp/articulatedbodyinertia.hpp | 54 +- src/Mod/Robot/App/kdl_cp/chain.cpp | 14 +- src/Mod/Robot/App/kdl_cp/chain.hpp | 4 +- src/Mod/Robot/App/kdl_cp/chaindynparam.cpp | 134 ++--- src/Mod/Robot/App/kdl_cp/chaindynparam.hpp | 4 +- src/Mod/Robot/App/kdl_cp/chainfksolver.hpp | 12 +- .../App/kdl_cp/chainfksolverpos_recursive.cpp | 14 +- .../App/kdl_cp/chainfksolvervel_recursive.cpp | 14 +- src/Mod/Robot/App/kdl_cp/chainidsolver.hpp | 10 +- .../chainidsolver_recursive_newton_euler.cpp | 14 +- .../App/kdl_cp/chainidsolver_vereshchagin.cpp | 472 ++++++++++++++++++ .../App/kdl_cp/chainidsolver_vereshchagin.hpp | 187 +++++++ src/Mod/Robot/App/kdl_cp/chainiksolver.hpp | 7 +- .../Robot/App/kdl_cp/chainiksolverpos_lma.cpp | 286 +++++++++++ .../Robot/App/kdl_cp/chainiksolverpos_lma.hpp | 247 +++++++++ .../Robot/App/kdl_cp/chainiksolverpos_nr.cpp | 19 +- .../Robot/App/kdl_cp/chainiksolverpos_nr.hpp | 16 + .../App/kdl_cp/chainiksolverpos_nr_jl.cpp | 20 +- .../App/kdl_cp/chainiksolverpos_nr_jl.hpp | 7 +- .../App/kdl_cp/chainiksolvervel_pinv.cpp | 44 +- .../App/kdl_cp/chainiksolvervel_pinv.hpp | 40 +- .../kdl_cp/chainiksolvervel_pinv_givens.cpp | 20 +- .../kdl_cp/chainiksolvervel_pinv_givens.hpp | 10 +- .../App/kdl_cp/chainiksolvervel_pinv_nso.cpp | 165 +++--- .../App/kdl_cp/chainiksolvervel_pinv_nso.hpp | 35 +- .../App/kdl_cp/chainiksolvervel_wdls.cpp | 101 +++- .../App/kdl_cp/chainiksolvervel_wdls.hpp | 80 ++- .../Robot/App/kdl_cp/chainjnttojacsolver.cpp | 29 +- .../Robot/App/kdl_cp/chainjnttojacsolver.hpp | 17 +- src/Mod/Robot/App/kdl_cp/config.h.in | 37 ++ src/Mod/Robot/App/kdl_cp/frameacc.hpp | 19 +- src/Mod/Robot/App/kdl_cp/frameacc.inl | 3 +- src/Mod/Robot/App/kdl_cp/frames.cpp | 102 ++-- src/Mod/Robot/App/kdl_cp/frames.hpp | 271 ++++++++-- src/Mod/Robot/App/kdl_cp/frames.inl | 58 +-- src/Mod/Robot/App/kdl_cp/framevel.hpp | 20 +- src/Mod/Robot/App/kdl_cp/framevel.inl | 3 - src/Mod/Robot/App/kdl_cp/jacobian.cpp | 10 +- src/Mod/Robot/App/kdl_cp/jacobian.hpp | 9 +- src/Mod/Robot/App/kdl_cp/jntarray.cpp | 4 +- src/Mod/Robot/App/kdl_cp/jntarray.hpp | 14 +- src/Mod/Robot/App/kdl_cp/jntarrayacc.hpp | 22 +- src/Mod/Robot/App/kdl_cp/jntarrayvel.hpp | 18 +- .../App/kdl_cp/jntspaceinertiamatrix.cpp | 6 +- .../App/kdl_cp/jntspaceinertiamatrix.hpp | 153 +++--- src/Mod/Robot/App/kdl_cp/joint.cpp | 48 +- src/Mod/Robot/App/kdl_cp/joint.hpp | 36 +- src/Mod/Robot/App/kdl_cp/kdl.hpp | 103 +++- src/Mod/Robot/App/kdl_cp/kdl.pc.in | 11 + src/Mod/Robot/App/kdl_cp/kinfam.hpp | 3 +- src/Mod/Robot/App/kdl_cp/kinfam_io.cpp | 13 +- src/Mod/Robot/App/kdl_cp/motion.hpp | 1 - src/Mod/Robot/App/kdl_cp/path.hpp | 14 +- src/Mod/Robot/App/kdl_cp/path_circle.cpp | 6 +- src/Mod/Robot/App/kdl_cp/path_circle.hpp | 8 + src/Mod/Robot/App/kdl_cp/path_composite.cpp | 29 +- src/Mod/Robot/App/kdl_cp/path_composite.hpp | 47 ++ .../Robot/App/kdl_cp/path_cyclic_closed.hpp | 6 + src/Mod/Robot/App/kdl_cp/path_line.hpp | 7 + src/Mod/Robot/App/kdl_cp/path_point.hpp | 7 + .../App/kdl_cp/path_roundedcomposite.cpp | 144 ++++-- .../App/kdl_cp/path_roundedcomposite.hpp | 69 ++- src/Mod/Robot/App/kdl_cp/rigidbodyinertia.cpp | 16 +- src/Mod/Robot/App/kdl_cp/rigidbodyinertia.hpp | 55 +- .../App/kdl_cp/rotational_interpolation.hpp | 2 +- .../kdl_cp/rotational_interpolation_sa.hpp | 4 +- .../Robot/App/kdl_cp/rotationalinertia.cpp | 6 +- .../Robot/App/kdl_cp/rotationalinertia.hpp | 5 +- src/Mod/Robot/App/kdl_cp/segment.hpp | 15 +- src/Mod/Robot/App/kdl_cp/solveri.hpp | 129 +++++ src/Mod/Robot/App/kdl_cp/trajectory.hpp | 6 - .../Robot/App/kdl_cp/trajectory_composite.cpp | 16 +- .../Robot/App/kdl_cp/trajectory_composite.hpp | 6 - .../Robot/App/kdl_cp/trajectory_segment.cpp | 17 +- .../Robot/App/kdl_cp/trajectory_segment.hpp | 11 +- .../App/kdl_cp/trajectory_stationary.hpp | 4 + src/Mod/Robot/App/kdl_cp/tree.cpp | 33 +- src/Mod/Robot/App/kdl_cp/tree.hpp | 73 ++- src/Mod/Robot/App/kdl_cp/treefksolver.hpp | 1 - .../App/kdl_cp/treefksolverpos_recursive.cpp | 12 +- src/Mod/Robot/App/kdl_cp/treeiksolver.hpp | 1 + .../App/kdl_cp/treeiksolverpos_nr_jl.cpp | 11 +- .../App/kdl_cp/treeiksolverpos_online.cpp | 175 +++++++ .../App/kdl_cp/treeiksolverpos_online.hpp | 108 ++++ .../Robot/App/kdl_cp/treeiksolvervel_wdls.cpp | 23 +- .../Robot/App/kdl_cp/treeiksolvervel_wdls.hpp | 4 +- .../Robot/App/kdl_cp/treejnttojacsolver.cpp | 12 +- .../Robot/App/kdl_cp/treejnttojacsolver.hpp | 2 +- src/Mod/Robot/App/kdl_cp/utilities/error.h | 10 +- .../App/kdl_cp/utilities/error_stack.cxx | 2 +- src/Mod/Robot/App/kdl_cp/utilities/rallNd.h | 9 +- .../App/kdl_cp/utilities/svd_eigen_HH.cpp | 25 +- .../App/kdl_cp/utilities/svd_eigen_HH.hpp | 3 +- .../App/kdl_cp/utilities/svd_eigen_Macie.hpp | 44 +- .../Robot/App/kdl_cp/utilities/utility.cxx | 2 +- .../App/kdl_cp/velocityprofile_spline.cpp | 194 +++++++ .../App/kdl_cp/velocityprofile_spline.hpp | 67 +++ .../Robot/App/kdl_cp/velocityprofile_trap.cpp | 19 + .../Robot/App/kdl_cp/velocityprofile_trap.hpp | 12 + .../App/kdl_cp/velocityprofile_traphalf.cpp | 20 +- .../App/kdl_cp/velocityprofile_traphalf.hpp | 34 +- src/Mod/Robot/Gui/CMakeLists.txt | 4 +- 104 files changed, 3812 insertions(+), 922 deletions(-) create mode 100644 src/Mod/Robot/App/kdl_cp/chainidsolver_vereshchagin.cpp create mode 100644 src/Mod/Robot/App/kdl_cp/chainidsolver_vereshchagin.hpp create mode 100644 src/Mod/Robot/App/kdl_cp/chainiksolverpos_lma.cpp create mode 100644 src/Mod/Robot/App/kdl_cp/chainiksolverpos_lma.hpp create mode 100644 src/Mod/Robot/App/kdl_cp/config.h.in create mode 100644 src/Mod/Robot/App/kdl_cp/kdl.pc.in create mode 100644 src/Mod/Robot/App/kdl_cp/solveri.hpp create mode 100644 src/Mod/Robot/App/kdl_cp/treeiksolverpos_online.cpp create mode 100644 src/Mod/Robot/App/kdl_cp/treeiksolverpos_online.hpp create mode 100644 src/Mod/Robot/App/kdl_cp/velocityprofile_spline.cpp create mode 100644 src/Mod/Robot/App/kdl_cp/velocityprofile_spline.hpp diff --git a/src/Mod/Robot/App/kdl_cp/CMakeLists.txt b/src/Mod/Robot/App/kdl_cp/CMakeLists.txt index 2322c723a..4acd4665d 100644 --- a/src/Mod/Robot/App/kdl_cp/CMakeLists.txt +++ b/src/Mod/Robot/App/kdl_cp/CMakeLists.txt @@ -1,29 +1,146 @@ -FILE( GLOB KDL_SRCS [^.]*.cpp ) +FILE( GLOB_RECURSE KDL_SRCS [^.]*.cpp [^.]*.cxx) FILE( GLOB KDL_HPPS [^.]*.hpp [^.]*.inl) -FILE( GLOB UTIL_SRCS utilities/[^.]*.cpp utilities/[^.]*.cxx) FILE( GLOB UTIL_HPPS utilities/[^.]*.h utilities/[^.]*.hpp) -ADD_LIBRARY(orocos-kdl SHARED ${KDL_SRCS} ${UTIL_SRCS}) +INCLUDE(CheckCXXSourceCompiles) +SET(CMAKE_REQUIRED_FLAGS) +CHECK_CXX_SOURCE_COMPILES(" + #include + #include + #include + + class TreeElement; + typedef std::map SegmentMap; + + class TreeElement + { + TreeElement(const std::string& name): number(0) {} + + public: + int number; + SegmentMap::const_iterator parent; + std::vector children; + + static TreeElement Root(std::string& name) + { + return TreeElement(name); + } + }; + + int main() + { + return 0; + } + " + HAVE_STL_CONTAINER_INCOMPLETE_TYPES) + +if(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) + SET(KDL_USE_NEW_TREE_INTERFACE_DEFAULT Off) +ELSE(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) + SET(KDL_USE_NEW_TREE_INTERFACE_DEFAULT On) +ENDIF(HAVE_STL_CONTAINER_INCOMPLETE_TYPES) + +SET(KDL_USE_NEW_TREE_INTERFACE ${KDL_USE_NEW_TREE_INTERFACE_DEFAULT} CACHE BOOL "Use the new KDL Tree interface") + +#Sanity check, inform the user +IF(NOT HAVE_STL_CONTAINER_INCOMPLETE_TYPES AND NOT KDL_USE_NEW_TREE_INTERFACE) + MESSAGE(WARNING "You have chosen to use the current Tree Interface, but your platform doesn't support containers of " + "incomplete types, this configuration is likely invalid") +ENDIF() + +#In Windows (Visual Studio) it is necessary to specify the postfix +#of the debug library name and no symbols are exported by kdl, +#so it is necessary to compile it as a static library +IF(MSVC) + SET(CMAKE_DEBUG_POSTFIX "d") + SET(LIB_TYPE STATIC) +ELSE(MSVC) + SET(LIB_TYPE SHARED) +ENDIF(MSVC) + +CONFIGURE_FILE(config.h.in config.h @ONLY) + +#### Settings for rpath +IF(${CMAKE_MINIMUM_REQUIRED_VERSION} VERSION_GREATER "2.8.12") + MESSAGE(AUTHOR_WARNING "CMAKE_MINIMUM_REQUIRED_VERSION is now ${CMAKE_MINIMUM_REQUIRED_VERSION}. This check can be removed.") +ENDIF() +IF(NOT (CMAKE_VERSION VERSION_LESS 2.8.12)) + IF(NOT MSVC) + #add the option to disable RPATH + OPTION(OROCOSKDL_ENABLE_RPATH "Enable RPATH during installation" FALSE) + MARK_AS_ADVANCED(OROCOSKDL_ENABLE_RPATH) + ENDIF(NOT MSVC) + + IF(OROCOSKDL_ENABLE_RPATH) + #Configure RPATH + SET(CMAKE_MACOSX_RPATH TRUE) #enable RPATH on OSX. This also suppress warnings on CMake >= 3.0 + # when building, don't use the install RPATH already + # (but later on when installing) + SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) + #build directory by default is built with RPATH + SET(CMAKE_SKIP_BUILD_RPATH FALSE) + + #This is relative RPATH for libraries built in the same project + #I assume that the directory is + # - install_dir/something for binaries + # - install_dir/lib for libraries + LIST(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/lib" isSystemDir) + IF("${isSystemDir}" STREQUAL "-1") + FILE(RELATIVE_PATH _rel_path "${CMAKE_INSTALL_PREFIX}/bin" "${CMAKE_INSTALL_PREFIX}/lib") + IF (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + SET(CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}") + ELSE() + SET(CMAKE_INSTALL_RPATH "\$ORIGIN/${_rel_path}") + ENDIF() + ENDIF("${isSystemDir}" STREQUAL "-1") + # add the automatically determined parts of the RPATH + # which point to directories outside the build tree to the install RPATH + SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) #very important! + ENDIF() +ENDIF() +#####end RPATH + +ADD_LIBRARY(orocos-kdl ${LIB_TYPE} ${KDL_SRCS} config.h) + SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES SOVERSION "${KDL_VERSION_MAJOR}.${KDL_VERSION_MINOR}" VERSION "${KDL_VERSION}" - COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS}") + COMPILE_FLAGS "${CMAKE_CXX_FLAGS_ADD} ${KDL_CFLAGS}" + PUBLIC_HEADER "${KDL_HPPS};${CMAKE_CURRENT_BINARY_DIR}/config.h" + ) -INSTALL_TARGETS( /lib orocos-kdl) +#### Settings for rpath disabled (back-compatibility) +IF(${CMAKE_MINIMUM_REQUIRED_VERSION} VERSION_GREATER "2.8.12") + MESSAGE(AUTHOR_WARNING "CMAKE_MINIMUM_REQUIRED_VERSION is now ${CMAKE_MINIMUM_REQUIRED_VERSION}. This check can be removed.") +ENDIF() +IF(CMAKE_VERSION VERSION_LESS 2.8.12) + SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES + INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib${LIB_SUFFIX}") +ELSE() + IF(NOT OROCOSKDL_ENABLE_RPATH) + SET_TARGET_PROPERTIES( orocos-kdl PROPERTIES + INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib${LIB_SUFFIX}") + ENDIF() +ENDIF() +#####end RPATH -# CMake 2.2: -INSTALL_FILES( /include/kdl FILES ${KDL_HPPS}) -INSTALL_FILES( /include/kdl/utilities FILES ${UTIL_HPPS}) +# Needed so that the generated config.h can be used +INCLUDE_DIRECTORIES(${CMAKE_CURRENT_BINARY_DIR}) +TARGET_LINK_LIBRARIES(orocos-kdl ${Boost_LIBRARIES}) + +INSTALL(TARGETS orocos-kdl + EXPORT OrocosKDLTargets + ARCHIVE DESTINATION lib${LIB_SUFFIX} + LIBRARY DESTINATION lib${LIB_SUFFIX} + PUBLIC_HEADER DESTINATION include/kdl +) + +INSTALL(FILES ${UTIL_HPPS} DESTINATION include/kdl/utilities) # Orocos convention: -CONFIGURE_FILE( kdl.pc.in src/orocos-kdl.pc @ONLY) -INSTALL_FILES( /lib/pkgconfig FILES orocos-kdl.pc) +CONFIGURE_FILE( kdl.pc.in ${CMAKE_CURRENT_BINARY_DIR}/orocos-kdl.pc @ONLY) +CONFIGURE_FILE( kdl.pc.in ${CMAKE_CURRENT_BINARY_DIR}/orocos_kdl.pc @ONLY) -IF( OROCOS_PLUGIN ) - ADD_SUBDIRECTORY( bindings/rtt ) -ENDIF( OROCOS_PLUGIN ) - -IF( PYTHON_BINDINGS ) - ADD_SUBDIRECTORY( bindings/python ) -ENDIF( PYTHON_BINDINGS ) +INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/orocos-kdl.pc DESTINATION lib${LIB_SUFFIX}/pkgconfig) +INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/orocos_kdl.pc DESTINATION lib${LIB_SUFFIX}/pkgconfig) diff --git a/src/Mod/Robot/App/kdl_cp/articulatedbodyinertia.cpp b/src/Mod/Robot/App/kdl_cp/articulatedbodyinertia.cpp index d7d9b5f96..e4cdcb937 100644 --- a/src/Mod/Robot/App/kdl_cp/articulatedbodyinertia.cpp +++ b/src/Mod/Robot/App/kdl_cp/articulatedbodyinertia.cpp @@ -27,10 +27,10 @@ using namespace Eigen; namespace KDL{ - ArticulatedBodyInertia::ArticulatedBodyInertia(const RigidBodyInertia& rbi) + ArticulatedBodyInertia::ArticulatedBodyInertia(const RigidBodyInertia& rbi) { - this->M.part()=Matrix3d::Identity()*rbi.m; - this->I.part()=Map(rbi.I.data); + this->M=Matrix3d::Identity()*rbi.m; + this->I=Map(rbi.I.data); this->H << 0,-rbi.h[2],rbi.h[1], rbi.h[2],0,-rbi.h[0], -rbi.h[1],rbi.h[0],0; @@ -41,10 +41,10 @@ namespace KDL{ *this = RigidBodyInertia(m,c,Ic); } - ArticulatedBodyInertia::ArticulatedBodyInertia(const Eigen::Matrix3d& M, const Eigen::Matrix3d& H, const Eigen::Matrix3d& I) + ArticulatedBodyInertia::ArticulatedBodyInertia(const Matrix3d& M, const Matrix3d& H, const Matrix3d& I) { - this->M.part()=M; - this->I.part()=I; + this->M=M; + this->I=I; this->H=H; } @@ -90,7 +90,7 @@ namespace KDL{ } ArticulatedBodyInertia operator*(const Rotation& M,const ArticulatedBodyInertia& I){ - Map E(M.data); + Map E(M.data); return ArticulatedBodyInertia(E.transpose()*I.M*E,E.transpose()*I.H*E,E.transpose()*I.I*E); } diff --git a/src/Mod/Robot/App/kdl_cp/articulatedbodyinertia.hpp b/src/Mod/Robot/App/kdl_cp/articulatedbodyinertia.hpp index bdcd09d17..eaca55a65 100644 --- a/src/Mod/Robot/App/kdl_cp/articulatedbodyinertia.hpp +++ b/src/Mod/Robot/App/kdl_cp/articulatedbodyinertia.hpp @@ -57,7 +57,7 @@ namespace KDL { * This constructor creates a cartesian space inertia matrix, * the arguments are the mass, the vector from the reference point to cog and the rotational inertia in the cog. */ - ArticulatedBodyInertia(double m, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero()); + explicit ArticulatedBodyInertia(double m, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero()); /** * Creates an inertia with zero mass, and zero RotationalInertia @@ -69,34 +69,13 @@ namespace KDL { ~ArticulatedBodyInertia(){}; - /** - * Scalar product: I_new = double * I_old - */ friend ArticulatedBodyInertia operator*(double a,const ArticulatedBodyInertia& I); - /** - * addition I: I_new = I_old1 + I_old2, make sure that I_old1 - * and I_old2 are expressed in the same reference frame/point, - * otherwise the result is worth nothing - */ friend ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib); friend ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib); friend ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib); friend ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib); - - /** - * calculate spatial momentum: h = I*v - * make sure that the twist v and the inertia are expressed in the same reference frame/point - */ friend Wrench operator*(const ArticulatedBodyInertia& I,const Twist& t); - - /** - * Coordinate system transform Ia = T_a_b*Ib with T_a_b the frame from a to b. - */ friend ArticulatedBodyInertia operator*(const Frame& T,const ArticulatedBodyInertia& I); - /** - * Reference frame orientation change Ia = R_a_b*Ib with R_a_b - * the rotation of b expressed in a - */ friend ArticulatedBodyInertia operator*(const Rotation& R,const ArticulatedBodyInertia& I); /** @@ -111,7 +90,36 @@ namespace KDL { Eigen::Matrix3d H; Eigen::Matrix3d I; }; -}//namespace + /** + * Scalar product: I_new = double * I_old + */ + ArticulatedBodyInertia operator*(double a,const ArticulatedBodyInertia& I); + /** + * addition I: I_new = I_old1 + I_old2, make sure that I_old1 + * and I_old2 are expressed in the same reference frame/point, + * otherwise the result is worth nothing + */ + ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib); + ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib); + ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const ArticulatedBodyInertia& Ib); + ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia,const RigidBodyInertia& Ib); + /** + * calculate spatial momentum: h = I*v + * make sure that the twist v and the inertia are expressed in the same reference frame/point + */ + Wrench operator*(const ArticulatedBodyInertia& I,const Twist& t); + + /** + * Coordinate system transform Ia = T_a_b*Ib with T_a_b the frame from a to b. + */ + ArticulatedBodyInertia operator*(const Frame& T,const ArticulatedBodyInertia& I); + /** + * Reference frame orientation change Ia = R_a_b*Ib with R_a_b + * the rotation of b expressed in a + */ + ArticulatedBodyInertia operator*(const Rotation& R,const ArticulatedBodyInertia& I); + +} #endif diff --git a/src/Mod/Robot/App/kdl_cp/chain.cpp b/src/Mod/Robot/App/kdl_cp/chain.cpp index 90c9294f1..1f5594349 100644 --- a/src/Mod/Robot/App/kdl_cp/chain.cpp +++ b/src/Mod/Robot/App/kdl_cp/chain.cpp @@ -25,14 +25,16 @@ namespace KDL { using namespace std; Chain::Chain(): - nrOfJoints(0), - nrOfSegments(0), - segments(0) + nrOfJoints(0), + nrOfSegments(0), + segments(0) { } - Chain::Chain(const Chain& in):nrOfJoints(0), - nrOfSegments(0) + Chain::Chain(const Chain& in): + nrOfJoints(0), + nrOfSegments(0), + segments(0) { for(unsigned int i=0;iaddSegment(in.getSegment(i)); @@ -43,7 +45,7 @@ namespace KDL { nrOfJoints=0; nrOfSegments=0; segments.resize(0); - for(int i=0;i segments; /** diff --git a/src/Mod/Robot/App/kdl_cp/chaindynparam.cpp b/src/Mod/Robot/App/kdl_cp/chaindynparam.cpp index 30bd614ba..b18346af1 100644 --- a/src/Mod/Robot/App/kdl_cp/chaindynparam.cpp +++ b/src/Mod/Robot/App/kdl_cp/chaindynparam.cpp @@ -26,83 +26,84 @@ namespace KDL { ChainDynParam::ChainDynParam(const Chain& _chain, Vector _grav): - chain(_chain), - nj(chain.getNrOfJoints()), - ns(chain.getNrOfSegments()), - grav(_grav), - jntarraynull(nj), - chainidsolver_coriolis( chain, Vector::Zero()), - chainidsolver_gravity( chain, grav), - wrenchnull(ns,Wrench::Zero()), - X(ns), - S(ns), - Ic(ns) + chain(_chain), + nr(0), + nj(chain.getNrOfJoints()), + ns(chain.getNrOfSegments()), + grav(_grav), + jntarraynull(nj), + chainidsolver_coriolis( chain, Vector::Zero()), + chainidsolver_gravity( chain, grav), + wrenchnull(ns,Wrench::Zero()), + X(ns), + S(ns), + Ic(ns) { - ag=-Twist(grav,Vector::Zero()); + ag=-Twist(grav,Vector::Zero()); } //calculate inertia matrix H int ChainDynParam::JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H) { - //Check sizes when in debug mode + //Check sizes when in debug mode if(q.rows()!=nj || H.rows()!=nj || H.columns()!=nj ) return -1; unsigned int k=0; - double q_; - - //Sweep from root to leaf + double q_; + + //Sweep from root to leaf for(unsigned int i=0;i=0;i--) - { + { + + if(i!=0) + { + //assumption that previous segment is parent + Ic[i-1]=Ic[i-1]+X[i]*Ic[i]; + } - if(i!=0) - { - //assumption that previous segment is parent - Ic[i-1]=Ic[i-1]+X[i]*Ic[i]; - } + F=Ic[i]*S[i]; + if(chain.getSegment(i).getJoint().getType()!=Joint::None) + { + H(k,k)=dot(S[i],F); + j=k; //countervariable for the joints + l=i; //countervariable for the segments + while(l!=0) //go from leaf to root starting at i + { + //assumption that previous segment is parent + F=X[l]*F; //calculate the unit force (cfr S) for every segment: F[l-1]=X[l]*F[l] + l--; //go down a segment + + if(chain.getSegment(l).getJoint().getType()!=Joint::None) //if the joint connected to segment is not a fixed joint + { + j--; + H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment + H(j,k)=H(k,j); + } + } + k--; //this if-loop should be repeated nj times (k=nj-1 to k=0) + } - F=Ic[i]*S[i]; - if(chain.getSegment(i).getJoint().getType()!=Joint::None) - { - H(k,k)=dot(S[i],F); - j=k; //countervariable for the joints - l=i; //countervariable for the segments - while(l!=0) //go from leaf to root starting at i - { - //assumption that previous segment is parent - F=X[l]*F; //calculate the unit force (cfr S) for every segment: F[l-1]=X[l]*F[l] - l--; //go down a segment - - if(chain.getSegment(l).getJoint().getType()!=Joint::None) //if the joint connected to segment is not a fixed joint - { - j--; - H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment - H(j,k)=H(k,j); - } - } - k--; //this if-loop should be repeated nj times (k=nj-1 to k=0) - } - - } // for - return 0; + } + return 0; } //calculate coriolis matrix C @@ -111,11 +112,11 @@ namespace KDL { //make a null matrix with the size of q_dotdot and a null wrench SetToZero(jntarraynull); - + //the calculation of coriolis matrix C - return chainidsolver_coriolis.CartToJnt(q, q_dot, jntarraynull, wrenchnull, coriolis); - - + chainidsolver_coriolis.CartToJnt(q, q_dot, jntarraynull, wrenchnull, coriolis); + + return 0; } //calculate gravity matrix G @@ -123,10 +124,11 @@ namespace KDL { { //make a null matrix with the size of q_dotdot and a null wrench - + SetToZero(jntarraynull); //the calculation of coriolis matrix C - return chainidsolver_gravity.CartToJnt(q, jntarraynull, jntarraynull, wrenchnull, gravity); + chainidsolver_gravity.CartToJnt(q, jntarraynull, jntarraynull, wrenchnull, gravity); + return 0; } ChainDynParam::~ChainDynParam() diff --git a/src/Mod/Robot/App/kdl_cp/chaindynparam.hpp b/src/Mod/Robot/App/kdl_cp/chaindynparam.hpp index 7a2e1d70a..287d08cf5 100644 --- a/src/Mod/Robot/App/kdl_cp/chaindynparam.hpp +++ b/src/Mod/Robot/App/kdl_cp/chaindynparam.hpp @@ -47,7 +47,7 @@ namespace KDL { { public: ChainDynParam(const Chain& chain, Vector _grav); - ~ChainDynParam(); + virtual ~ChainDynParam(); virtual int JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis); virtual int JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H); @@ -57,7 +57,7 @@ namespace KDL { const Chain chain; int nr; unsigned int nj; - unsigned int ns; + unsigned int ns; Vector grav; Vector vectornull; JntArray jntarraynull; diff --git a/src/Mod/Robot/App/kdl_cp/chainfksolver.hpp b/src/Mod/Robot/App/kdl_cp/chainfksolver.hpp index 3e535c020..ab52b3f80 100644 --- a/src/Mod/Robot/App/kdl_cp/chainfksolver.hpp +++ b/src/Mod/Robot/App/kdl_cp/chainfksolver.hpp @@ -28,6 +28,7 @@ #include "jntarray.hpp" #include "jntarrayvel.hpp" #include "jntarrayacc.hpp" +#include "solveri.hpp" namespace KDL { @@ -39,7 +40,7 @@ namespace KDL { */ //Forward definition - class ChainFkSolverPos { + class ChainFkSolverPos : public KDL::SolverI { public: /** * Calculate forward position kinematics for a KDL::Chain, @@ -47,7 +48,6 @@ namespace KDL { * * @param q_in input joint coordinates * @param p_out reference to output cartesian pose - * @param segmentNr default to -1 * * @return if < 0 something went wrong */ @@ -61,7 +61,7 @@ namespace KDL { * * @ingroup KinematicFamily */ - class ChainFkSolverVel { + class ChainFkSolverVel : public KDL::SolverI { public: /** * Calculate forward position and velocity kinematics, from @@ -69,7 +69,6 @@ namespace KDL { * * @param q_in input joint coordinates (position and velocity) * @param out output cartesian coordinates (position and velocity) - * @param segmentNr default to -1 * * @return if < 0 something went wrong */ @@ -85,7 +84,7 @@ namespace KDL { * @ingroup KinematicFamily */ - class ChainFkSolverAcc { + class ChainFkSolverAcc : public KDL::SolverI { public: /** * Calculate forward position, velocity and accelaration @@ -93,9 +92,8 @@ namespace KDL { * * @param q_in input joint coordinates (position, velocity and * acceleration - * @param out output cartesian coordinates (position, velocity + @param out output cartesian coordinates (position, velocity * and acceleration - * @param segmentNr default to -1 * * @return if < 0 something went wrong */ diff --git a/src/Mod/Robot/App/kdl_cp/chainfksolverpos_recursive.cpp b/src/Mod/Robot/App/kdl_cp/chainfksolverpos_recursive.cpp index 6ce347d47..41200e2d5 100644 --- a/src/Mod/Robot/App/kdl_cp/chainfksolverpos_recursive.cpp +++ b/src/Mod/Robot/App/kdl_cp/chainfksolverpos_recursive.cpp @@ -30,20 +30,22 @@ namespace KDL { { } - int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, int segmentNr) - { - if(segmentNr<0) - segmentNr=chain.getNrOfSegments(); + int ChainFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, int seg_nr) { + unsigned int segmentNr; + if(seg_nr<0) + segmentNr=chain.getNrOfSegments(); + else + segmentNr = seg_nr; p_out = Frame::Identity(); if(q_in.rows()!=chain.getNrOfJoints()) return -1; - else if(segmentNr>static_cast(chain.getNrOfSegments())) + else if(segmentNr>chain.getNrOfSegments()) return -1; else{ int j=0; - for(int i=0;istatic_cast(chain.getNrOfSegments())) + else if(segmentNr>chain.getNrOfSegments()) return -1; else{ int j=0; - for (int i=0;iinitial_upwards_sweep(q, q_dot, q_dotdot, f_ext); + //do an inward recursion for inertia, forces and constraints + this->downwards_sweep(alfa, torques); + //Solve for the constraint forces + this->constraint_calculation(beta); + //do an upward recursion to propagate the result + this->final_upwards_sweep(q_dotdot, torques); + return 0; +} + +void ChainIdSolver_Vereshchagin::initial_upwards_sweep(const JntArray &q, const JntArray &qdot, const JntArray &qdotdot, const Wrenches& f_ext) +{ + //if (q.rows() != nj || qdot.rows() != nj || qdotdot.rows() != nj || f_ext.size() != ns) + // return -1; + + unsigned int j = 0; + F_total = Frame::Identity(); + for (unsigned int i = 0; i < ns; i++) + { + //Express everything in the segments reference frame (body coordinates) + //which is at the segments tip, i.e. where the next joint is attached. + + //Calculate segment properties: X,S,vj,cj + const Segment& segment = chain.getSegment(i); + segment_info& s = results[i + 1]; + //The pose between the joint root and the segment tip (tip expressed in joint root coordinates) + s.F = segment.pose(q(j)); //X pose of each link in link coord system + + F_total = F_total * s.F; //X pose of the each link in root coord system + s.F_base = F_total; //X pose of the each link in root coord system for getter functions + + //The velocity due to the joint motion of the segment expressed in the segments reference frame (tip) + Twist vj = s.F.M.Inverse(segment.twist(q(j), qdot(j))); //XDot of each link + //Twist aj = s.F.M.Inverse(segment.twist(q(j), qdotdot(j))); //XDotDot of each link + + //The unit velocity due to the joint motion of the segment expressed in the segments reference frame (tip) + s.Z = s.F.M.Inverse(segment.twist(q(j), 1.0)); + //Put Z in the joint root reference frame: + s.Z = s.F * s.Z; + + //The total velocity of the segment expressed in the the segments reference frame (tip) + if (i != 0) + { + s.v = s.F.Inverse(results[i].v) + vj; // recursive velocity of each link in segment frame + //s.A=s.F.Inverse(results[i].A)+aj; + s.A = s.F.M.Inverse(results[i].A); + } + else + { + s.v = vj; + s.A = s.F.M.Inverse(acc_root); + } + //c[i] = cj + v[i]xvj (remark: cj=0, since our S is not time dependent in local coordinates) + //The velocity product acceleration + //std::cout << i << " Initial upward" << s.v << std::endl; + s.C = s.v*vj; //This is a cross product: cartesian space BIAS acceleration in local link coord. + //Put C in the joint root reference frame + s.C = s.F * s.C; //+F_total.M.Inverse(acc_root)); + //The rigid body inertia of the segment, expressed in the segments reference frame (tip) + s.H = segment.getInertia(); + + //wrench of the rigid body bias forces and the external forces on the segment (in body coordinates, tip) + //external forces are taken into account through s.U. + Wrench FextLocal = F_total.M.Inverse() * f_ext[i]; + s.U = s.v * (s.H * s.v) - FextLocal; //f_ext[i]; + if (segment.getJoint().getType() != Joint::None) + j++; + } + +} + +void ChainIdSolver_Vereshchagin::downwards_sweep(const Jacobian& alfa, const JntArray &torques) +{ + int j = nj - 1; + for (int i = ns; i >= 0; i--) + { + //Get a handle for the segment we are working on. + segment_info& s = results[i]; + //For segment N, + //tilde is in the segment refframe (tip, not joint root) + //without tilde is at the joint root (the childs tip!!!) + //P_tilde is the articulated body inertia + //R_tilde is the sum of external and coriolis/centrifugal forces + //M is the (unit) acceleration energy already generated at link i + //G is the (unit) magnitude of the constraint forces at link i + //E are the (unit) constraint forces due to the constraints + if (i == (int)ns) + { + s.P_tilde = s.H; + s.R_tilde = s.U; + s.M.setZero(); + s.G.setZero(); + //changeBase(alfa_N,F_total.M.Inverse(),alfa_N2); + for (unsigned int r = 0; r < 3; r++) + for (unsigned int c = 0; c < nc; c++) + { + //copy alfa constrain force matrix in E~ + s.E_tilde(r, c) = alfa(r + 3, c); + s.E_tilde(r + 3, c) = alfa(r, c); + } + //Change the reference frame of alfa to the segmentN tip frame + //F_Total holds end effector frame, if done per segment bases then constraints could be extended to all segments + Rotation base_to_end = F_total.M.Inverse(); + for (unsigned int c = 0; c < nc; c++) + { + Wrench col(Vector(s.E_tilde(3, c), s.E_tilde(4, c), s.E_tilde(5, c)), + Vector(s.E_tilde(0, c), s.E_tilde(1, c), s.E_tilde(2, c))); + col = base_to_end*col; + s.E_tilde.col(c) << Vector3d::Map(col.torque.data), Vector3d::Map(col.force.data); + } + } + else + { + //For all others: + //Everything should expressed in the body coordinates of segment i + segment_info& child = results[i + 1]; + //Copy PZ into a vector so we can do matrix manipulations, put torques above forces + Vector6d vPZ; + vPZ << Vector3d::Map(child.PZ.torque.data), Vector3d::Map(child.PZ.force.data); + Matrix6d PZDPZt; + PZDPZt.noalias() = vPZ * vPZ.transpose(); + PZDPZt /= child.D; + + //equation a) (see Vereshchagin89) PZDPZt=[I,H;H',M] + //Azamat:articulated body inertia as in Featherstone (7.19) + s.P_tilde = s.H + child.P - ArticulatedBodyInertia(PZDPZt.bottomRightCorner<3,3>(), PZDPZt.topRightCorner<3,3>(), PZDPZt.topLeftCorner<3,3>()); + //equation b) (see Vereshchagin89) + //Azamat: bias force as in Featherstone (7.20) + s.R_tilde = s.U + child.R + child.PC + (child.PZ / child.D) * child.u; + //equation c) (see Vereshchagin89) + s.E_tilde = child.E; + + //Azamat: equation (c) right side term + s.E_tilde.noalias() -= (vPZ * child.EZ.transpose()) / child.D; + + //equation d) (see Vereshchagin89) + s.M = child.M; + //Azamat: equation (d) right side term + s.M.noalias() -= (child.EZ * child.EZ.transpose()) / child.D; + + //equation e) (see Vereshchagin89) + s.G = child.G; + Twist CiZDu = child.C + (child.Z / child.D) * child.u; + Vector6d vCiZDu; + vCiZDu << Vector3d::Map(CiZDu.rot.data), Vector3d::Map(CiZDu.vel.data); + s.G.noalias() += child.E.transpose() * vCiZDu; + } + if (i != 0) + { + //Transform all results to joint root coordinates of segment i (== body coordinates segment i-1) + //equation a) + s.P = s.F * s.P_tilde; + //equation b) + s.R = s.F * s.R_tilde; + //equation c), in matrix: torques above forces, so switch and switch back + for (unsigned int c = 0; c < nc; c++) + { + Wrench col(Vector(s.E_tilde(3, c), s.E_tilde(4, c), s.E_tilde(5, c)), + Vector(s.E_tilde(0, c), s.E_tilde(1, c), s.E_tilde(2, c))); + col = s.F*col; + s.E.col(c) << Vector3d::Map(col.torque.data), Vector3d::Map(col.force.data); + } + + //needed for next recursion + s.PZ = s.P * s.Z; + s.D = dot(s.Z, s.PZ); + s.PC = s.P * s.C; + + //u=(Q-Z(R+PC)=sum of external forces along the joint axes, + //R are the forces comming from the children, + //Q is taken zero (do we need to take the previous calculated torques? + + //projection of coriolis and centrepital forces into joint subspace (0 0 Z) + s.totalBias = -dot(s.Z, s.R + s.PC); + s.u = torques(j) + s.totalBias; + + //Matrix form of Z, put rotations above translations + Vector6d vZ; + vZ << Vector3d::Map(s.Z.rot.data), Vector3d::Map(s.Z.vel.data); + s.EZ.noalias() = s.E.transpose() * vZ; + + if (chain.getSegment(i - 1).getJoint().getType() != Joint::None) + j--; + } + } +} + +void ChainIdSolver_Vereshchagin::constraint_calculation(const JntArray& beta) +{ + //equation f) nu = M_0_inverse*(beta_N - E0_tilde`*acc0 - G0) + //M_0_inverse, always nc*nc symmetric matrix + //std::cout<<"M0: "< axis torque/force + double constraint_torque = -dot(s.Z, constraint_force); + //The result should be the torque at this joint + + torques(j) = constraint_torque; + //s.constAccComp = torques(j) / s.D; + s.constAccComp = constraint_torque / s.D; + s.nullspaceAccComp = s.u / s.D; + //total joint space acceleration resulting from accelerations of parent joints, constraint forces and + // nullspace forces. + q_dotdot(j) = (s.nullspaceAccComp + parentAccComp + s.constAccComp); + s.acc = s.F.Inverse(a_p + s.Z * q_dotdot(j) + s.C);//returns acceleration in link distal tip coordinates. For use needs to be transformed + if (chain.getSegment(i - 1).getJoint().getType() != Joint::None) + j++; + } +} + +/* +void ChainIdSolver_Vereshchagin::getLinkCartesianPose(Frames& x_base) +{ + for (int i = 0; i < ns; i++) + { + x_base[i] = results[i + 1].F_base; + } + return; +} + +void ChainIdSolver_Vereshchagin::getLinkCartesianVelocity(Twists& xDot_base) +{ + + for (int i = 0; i < ns; i++) + { + xDot_base[i] = results[i + 1].F_base.M * results[i + 1].v; + } + return; +} + +void ChainIdSolver_Vereshchagin::getLinkCartesianAcceleration(Twists& xDotDot_base) +{ + + for (int i = 0; i < ns; i++) + { + xDotDot_base[i] = results[i + 1].F_base.M * results[i + 1].acc; + //std::cout << "XDotDot_base[i] " << xDotDot_base[i] << std::endl; + } + return; +} + +void ChainIdSolver_Vereshchagin::getLinkPose(Frames& x_local) +{ + for (int i = 0; i < ns; i++) + { + x_local[i] = results[i + 1].F; + } + return; +} + +void ChainIdSolver_Vereshchagin::getLinkVelocity(Twists& xDot_local) +{ + for (int i = 0; i < ns; i++) + { + xDot_local[i] = results[i + 1].v; + } + return; + +} + +void ChainIdSolver_Vereshchagin::getLinkAcceleration(Twists& xDotdot_local) +{ + for (int i = 0; i < ns; i++) + { + xDotdot_local[i] = results[i + 1].acc; + } + return; + +} + +void ChainIdSolver_Vereshchagin::getJointBiasAcceleration(JntArray& bias_q_dotdot) +{ + for (int i = 0; i < ns; i++) + { + //this is only force + double tmp = results[i + 1].totalBias; + //this is accelleration + bias_q_dotdot(i) = tmp / results[i + 1].D; + + //s.totalBias = - dot(s.Z, s.R + s.PC); + //std::cout << "totalBiasAccComponent" << i << ": " << bias_q_dotdot(i) << std::endl; + //bias_q_dotdot(i) = s.totalBias/s.D + + } + return; + +} + +void ChainIdSolver_Vereshchagin::getJointConstraintAcceleration(JntArray& constraint_q_dotdot) +{ + for (int i = 0; i < ns; i++) + { + constraint_q_dotdot(i) = results[i + 1].constAccComp; + //double tmp = results[i + 1].u; + //s.u = torques(j) + s.totalBias; + // std::cout << "s.constraintAccComp" << i << ": " << results[i+1].constAccComp << std::endl; + //nullspace_q_dotdot(i) = s.u/s.D + + } + return; + + +} + +//Check the name it does not seem to be appropriate + +void ChainIdSolver_Vereshchagin::getJointNullSpaceAcceleration(JntArray& nullspace_q_dotdot) +{ + for (int i = 0; i < ns; i++) + { + nullspace_q_dotdot(i) = results[i + 1].nullspaceAccComp; + //double tmp = results[i + 1].u; + //s.u = torques(j) + s.totalBias; + //std::cout << "s.nullSpaceAccComp" << i << ": " << results[i+1].nullspaceAccComp << std::endl; + //nullspace_q_dotdot(i) = s.u/s.D + + } + return; + + +} + +//This is not only a bias force energy but also includes generalized forces +//change type of parameter G +//this method should retur array of G's + +void ChainIdSolver_Vereshchagin::getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G) +{ + for (int i = 0; i < ns; i++) + { + G = results[i + 1].G; + //double tmp = results[i + 1].u; + //s.u = torques(j) + s.totalBias; + //std::cout << "s.G " << i << ": " << results[i+1].G << std::endl; + //nullspace_q_dotdot(i) = s.u/s.D + + } + return; + +} + +//this method should retur array of R's + +void ChainIdSolver_Vereshchagin::getLinkBiasForceMatrix(Wrenches& R_tilde) +{ + for (int i = 0; i < ns; i++) + { + R_tilde[i] = results[i + 1].R_tilde; + //Azamat: bias force as in Featherstone (7.20) + //s.R_tilde = s.U + child.R + child.PC + child.PZ / child.D * child.u; + std::cout << "s.R_tilde " << i << ": " << results[i + 1].R_tilde << std::endl; + } + return; +} + +*/ + +}//namespace diff --git a/src/Mod/Robot/App/kdl_cp/chainidsolver_vereshchagin.hpp b/src/Mod/Robot/App/kdl_cp/chainidsolver_vereshchagin.hpp new file mode 100644 index 000000000..fcf69fedd --- /dev/null +++ b/src/Mod/Robot/App/kdl_cp/chainidsolver_vereshchagin.hpp @@ -0,0 +1,187 @@ +// Copyright (C) 2009, 2011 + +// Version: 1.0 +// Author: Ruben Smits, Herman Bruyninckx, Azamat Shakhimardanov +// Maintainer: Ruben Smits, Azamat Shakhimardanov +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP +#define KDL_CHAINIDSOLVER_VERESHCHAGIN_HPP + +#include "chainidsolver.hpp" +#include "frames.hpp" +#include "articulatedbodyinertia.hpp" + +namespace KDL +{ +/** + * \brief Dynamics calculations by constraints based on Vereshchagin 1989. + * for a chain. This class creates instance of hybrid dynamics solver. + * The solver calculates total joint space accelerations in a chain when a constraint force(s) is applied + * to the chain's end-effector (task space/cartesian space). + */ + +class ChainIdSolver_Vereshchagin +{ + typedef std::vector Twists; + typedef std::vector Frames; + typedef Eigen::Matrix Vector6d; + typedef Eigen::Matrix Matrix6d; + typedef Eigen::Matrix Matrix6Xd; + +public: + /** + * Constructor for the solver, it will allocate all the necessary memory + * \param chain The kinematic chain to calculate the inverse dynamics for, an internal copy will be made. + * \param root_acc The acceleration vector of the root to use during the calculation.(most likely contains gravity) + * + */ + ChainIdSolver_Vereshchagin(const Chain& chain, Twist root_acc, unsigned int nc); + + ~ChainIdSolver_Vereshchagin() + { + }; + + /** + * This method calculates joint space constraint torques and total joint space acceleration. + * It returns 0 when it succeeds, otherwise -1 or -2 for unmatching matrix and array sizes. + * Input parameters; + * \param q The current joint positions + * \param q_dot The current joint velocities + * \param f_ext The external forces (no gravity, it is given in root acceleration) on the segments. + * Output parameters: + * \param q_dotdot The joint accelerations + * \param torques the resulting constraint torques for the joints + */ + int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques); + + /* + //Returns cartesian positions of links in base coordinates + void getLinkCartesianPose(Frames& x_base); + //Returns cartesian velocities of links in base coordinates + void getLinkCartesianVelocity(Twists& xDot_base); + //Returns cartesian acceleration of links in base coordinates + void getLinkCartesianAcceleration(Twists& xDotDot_base); + //Returns cartesian postions of links in link tip coordinates + void getLinkPose(Frames& x_local); + //Returns cartesian velocities of links in link tip coordinates + void getLinkVelocity(Twists& xDot_local); + //Returns cartesian acceleration of links in link tip coordinates + void getLinkAcceleration(Twists& xDotdot_local); + //Acceleration energy due to unit constraint forces at the end-effector + void getLinkUnitForceAccelerationEnergy(Eigen::MatrixXd& M); + //Acceleration energy due to arm configuration: bias force plus input joint torques + void getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G); + + void getLinkUnitForceMatrix(Matrix6Xd& E_tilde); + + void getLinkBiasForceMatrix(Wrenches& R_tilde); + + void getJointBiasAcceleration(JntArray &bias_q_dotdot); + */ +private: + /** + * This method calculates all cartesian space poses, twists, bias accelerations. + * External forces are also taken into account in this outward sweep. + */ + void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext); + /** + * This method is a force balance sweep. It calculates articulated body inertias and bias forces. + * Additionally, acceleration energies generated by bias forces and unit forces are calculated here. + */ + void downwards_sweep(const Jacobian& alfa, const JntArray& torques); + /** + * This method calculates constraint force magnitudes. + * + */ + void constraint_calculation(const JntArray& beta); + /** + * This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations) together. + * + */ + void final_upwards_sweep(JntArray &q_dotdot, JntArray &torques); + +private: + Chain chain; + unsigned int nj; + unsigned int ns; + unsigned int nc; + Twist acc_root; + Jacobian alfa_N; + Jacobian alfa_N2; + Eigen::MatrixXd M_0_inverse; + Eigen::MatrixXd Um; + Eigen::MatrixXd Vm; + JntArray beta_N; + Eigen::VectorXd nu; + Eigen::VectorXd nu_sum; + Eigen::VectorXd Sm; + Eigen::VectorXd tmpm; + Wrench qdotdot_sum; + Frame F_total; + + struct segment_info + { + Frame F; //local pose with respect to previous link in segments coordinates + Frame F_base; // pose of a segment in root coordinates + Twist Z; //Unit twist + Twist v; //twist + Twist acc; //acceleration twist + Wrench U; //wrench p of the bias forces (in cartesian space) + Wrench R; //wrench p of the bias forces + Wrench R_tilde; //vector of wrench p of the bias forces (new) in matrix form + Twist C; //constraint + Twist A; //constraint + ArticulatedBodyInertia H; //I (expressed in 6*6 matrix) + ArticulatedBodyInertia P; //I (expressed in 6*6 matrix) + ArticulatedBodyInertia P_tilde; //I (expressed in 6*6 matrix) + Wrench PZ; //vector U[i] = I_A[i]*S[i] + Wrench PC; //vector E[i] = I_A[i]*c[i] + double D; //vector D[i] = S[i]^T*U[i] + Matrix6Xd E; //matrix with virtual unit constraint force due to acceleration constraints + Matrix6Xd E_tilde; + Eigen::MatrixXd M; //acceleration energy already generated at link i + Eigen::VectorXd G; //magnitude of the constraint forces already generated at link i + Eigen::VectorXd EZ; //K[i] = Etiltde'*Z + double nullspaceAccComp; //Azamat: constribution of joint space u[i] forces to joint space acceleration + double constAccComp; //Azamat: constribution of joint space constraint forces to joint space acceleration + double biasAccComp; //Azamat: constribution of joint space bias forces to joint space acceleration + double totalBias; //Azamat: R+PC (centrepital+coriolis) in joint subspace + double u; //vector u[i] = torques(i) - S[i]^T*(p_A[i] + I_A[i]*C[i]) in joint subspace. Azamat: In code u[i] = torques(i) - s[i].totalBias + + segment_info(unsigned int nc): + D(0),nullspaceAccComp(0),constAccComp(0),biasAccComp(0),totalBias(0),u(0) + { + E.resize(6, nc); + E_tilde.resize(6, nc); + G.resize(nc); + M.resize(nc, nc); + EZ.resize(nc); + E.setZero(); + E_tilde.setZero(); + M.setZero(); + G.setZero(); + EZ.setZero(); + }; + }; + + std::vector results; + +}; +} + +#endif diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolver.hpp b/src/Mod/Robot/App/kdl_cp/chainiksolver.hpp index 4daed842c..f7f85b060 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolver.hpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolver.hpp @@ -29,6 +29,7 @@ #include "jntarray.hpp" #include "jntarrayvel.hpp" #include "jntarrayacc.hpp" +#include "solveri.hpp" namespace KDL { @@ -38,7 +39,7 @@ namespace KDL { * * @ingroup KinematicFamily */ - class ChainIkSolverPos { + class ChainIkSolverPos : public KDL::SolverI { public: /** * Calculate inverse position kinematics, from cartesian @@ -61,7 +62,7 @@ namespace KDL { * * @ingroup KinematicFamily */ - class ChainIkSolverVel { + class ChainIkSolverVel : public KDL::SolverI { public: /** * Calculate inverse velocity kinematics, from joint positions @@ -97,7 +98,7 @@ namespace KDL { * @ingroup KinematicFamily */ - class ChainIkSolverAcc { + class ChainIkSolverAcc : public KDL::SolverI { public: /** * Calculate inverse acceleration kinematics from joint diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolverpos_lma.cpp b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_lma.cpp new file mode 100644 index 000000000..c74ea8d52 --- /dev/null +++ b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_lma.cpp @@ -0,0 +1,286 @@ +/** + \file chainiksolverpos_lma.cpp + \brief computing inverse position kinematics using Levenberg-Marquardt. +*/ + +/************************************************************************** + begin : May 2012 + copyright : (C) 2012 Erwin Aertbelien + email : firstname.lastname@mech.kuleuven.ac.be + + History (only major changes)( AUTHOR-Description ) : + + *************************************************************************** + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public * + * License as published by the Free Software Foundation; either * + * version 2.1 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with this library; if not, write to the Free Software * + * Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307 USA * + * * + ***************************************************************************/ + +#include "chainiksolverpos_lma.hpp" +#include + +namespace KDL { + + + + +template +inline void Twist_to_Eigen(const KDL::Twist& t,Eigen::MatrixBase& e) { + e(0)=t.vel.data[0]; + e(1)=t.vel.data[1]; + e(2)=t.vel.data[2]; + e(3)=t.rot.data[0]; + e(4)=t.rot.data[1]; + e(5)=t.rot.data[2]; +} + + +ChainIkSolverPos_LMA::ChainIkSolverPos_LMA( + const KDL::Chain& _chain, + const Eigen::Matrix& _L, + double _eps, + int _maxiter, + double _eps_joints +) : + lastNrOfIter(0), + lastSV(_chain.getNrOfJoints()), + jac(6, _chain.getNrOfJoints()), + grad(_chain.getNrOfJoints()), + display_information(false), + maxiter(_maxiter), + eps(_eps), + eps_joints(_eps_joints), + L(_L.cast()), + chain(_chain), + T_base_jointroot(_chain.getNrOfJoints()), + T_base_jointtip(_chain.getNrOfJoints()), + q(_chain.getNrOfJoints()), + A(_chain.getNrOfJoints(), _chain.getNrOfJoints()), + tmp(_chain.getNrOfJoints()), + ldlt(_chain.getNrOfJoints()), + svd(6, _chain.getNrOfJoints(),Eigen::ComputeThinU | Eigen::ComputeThinV), + diffq(_chain.getNrOfJoints()), + q_new(_chain.getNrOfJoints()), + original_Aii(_chain.getNrOfJoints()) +{} + +ChainIkSolverPos_LMA::ChainIkSolverPos_LMA( + const KDL::Chain& _chain, + double _eps, + int _maxiter, + double _eps_joints +) : + lastNrOfIter(0), + lastSV(_chain.getNrOfJoints()>6?6:_chain.getNrOfJoints()), + jac(6, _chain.getNrOfJoints()), + grad(_chain.getNrOfJoints()), + display_information(false), + maxiter(_maxiter), + eps(_eps), + eps_joints(_eps_joints), + chain(_chain), + T_base_jointroot(_chain.getNrOfJoints()), + T_base_jointtip(_chain.getNrOfJoints()), + q(_chain.getNrOfJoints()), + A(_chain.getNrOfJoints(), _chain.getNrOfJoints()), + ldlt(_chain.getNrOfJoints()), + svd(6, _chain.getNrOfJoints(),Eigen::ComputeThinU | Eigen::ComputeThinV), + diffq(_chain.getNrOfJoints()), + q_new(_chain.getNrOfJoints()), + original_Aii(_chain.getNrOfJoints()) +{ + L(0)=1; + L(1)=1; + L(2)=1; + L(3)=0.01; + L(4)=0.01; + L(5)=0.01; +} + +ChainIkSolverPos_LMA::~ChainIkSolverPos_LMA() {} + +void ChainIkSolverPos_LMA::compute_fwdpos(const VectorXq& q) { + using namespace KDL; + unsigned int jointndx=0; + T_base_head = Frame::Identity(); // frame w.r.t. base of head + for (unsigned int i=0;i(); + compute_fwdpos(q); + compute_jacobian(q); + svd.compute(jac); + std::cout << "Singular values : " << svd.singularValues().transpose()<<"\n"; +} + + +int ChainIkSolverPos_LMA::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out) { + using namespace KDL; + double v = 2; + double tau = 10; + double rho; + double lambda; + Twist t; + double delta_pos_norm; + Eigen::Matrix delta_pos; + Eigen::Matrix delta_pos_new; + + + q=q_init.data.cast(); + compute_fwdpos(q); + Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos ); + delta_pos=L.asDiagonal()*delta_pos; + delta_pos_norm = delta_pos.norm(); + if (delta_pos_norm(); + return 0; + } + compute_jacobian(q); + jac = L.asDiagonal()*jac; + + lambda = tau; + double dnorm = 1; + for (unsigned int i=0;i(); + if (dnorm < eps_joints) { + lastDifference = delta_pos_norm; + lastNrOfIter = i; + lastSV = svd.singularValues(); + q_out.data = q.cast(); + compute_fwdpos(q); + Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos ); + lastTransDiff = delta_pos.topRows(3).norm(); + lastRotDiff = delta_pos.bottomRows(3).norm(); + return -2; + } + + + if (grad.transpose()*grad < eps_joints*eps_joints ) { + compute_fwdpos(q); + Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos ); + lastDifference = delta_pos_norm; + lastTransDiff = delta_pos.topRows(3).norm(); + lastRotDiff = delta_pos.bottomRows(3).norm(); + lastSV = svd.singularValues(); + lastNrOfIter = i; + q_out.data = q.cast(); + return -1; + } + + q_new = q+diffq; + compute_fwdpos(q_new); + Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos_new ); + delta_pos_new = L.asDiagonal()*delta_pos_new; + double delta_pos_new_norm = delta_pos_new.norm(); + rho = delta_pos_norm*delta_pos_norm - delta_pos_new_norm*delta_pos_new_norm; + rho /= diffq.transpose()*(lambda*diffq + grad); + if (rho > 0) { + q = q_new; + delta_pos = delta_pos_new; + delta_pos_norm = delta_pos_new_norm; + if (delta_pos_norm(); + return 0; + } + compute_jacobian(q_new); + jac = L.asDiagonal()*jac; + double tmp=2*rho-1; + lambda = lambda*max(1/3.0, 1-tmp*tmp*tmp); + v = 2; + } else { + lambda = lambda*v; + v = 2*v; + } + } + lastDifference = delta_pos_norm; + lastTransDiff = delta_pos.topRows(3).norm(); + lastRotDiff = delta_pos.bottomRows(3).norm(); + lastSV = svd.singularValues(); + lastNrOfIter = maxiter; + q_out.data = q.cast(); + return -3; + +} + + + +};//namespace KDL diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolverpos_lma.hpp b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_lma.hpp new file mode 100644 index 000000000..842a9c0a6 --- /dev/null +++ b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_lma.hpp @@ -0,0 +1,247 @@ +#ifndef KDL_CHAINIKSOLVERPOS_GN_HPP +#define KDL_CHAINIKSOLVERPOS_GN_HPP +/** + \file chainiksolverpos_lma.hpp + \brief computing inverse position kinematics using Levenberg-Marquardt. +*/ + +/************************************************************************** + begin : May 2012 + copyright : (C) 2012 Erwin Aertbelien + email : firstname.lastname@mech.kuleuven.ac.be + + History (only major changes)( AUTHOR-Description ) : + + *************************************************************************** + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public * + * License as published by the Free Software Foundation; either * + * version 2.1 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with this library; if not, write to the Free Software * + * Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307 USA * + * * + ***************************************************************************/ + + +#include "chainiksolver.hpp" +#include "chain.hpp" +#include + +namespace KDL +{ + +/** + * \brief Solver for the inverse position kinematics that uses Levenberg-Marquardt. + * + * The robustness and speed of this solver is improved in several ways: + * - by using a Levenberg-Marquardt method that automatically adapts the damping when + * computing the inverse damped least squares inverse velocity kinematics. + * - by using an internal implementation of forward position kinematics and the + * Jacobian kinematics. This implementation is more numerically robust, + * is able to cache previous computations, and implements an \f$ \mathcal{O}(N) \f$ + * algorithm for the computation of the Jacobian (with \f$N\f$, the number of joints, and for + * a fixed size task space). + * - by providing a way to specify the weights in task space, you can weigh rotations wrt translations. + * This is important e.g. to specify that rotations do not matter for the problem at hand, or to + * specify how important you judge rotations w.r.t. translations, typically in S.I.-units, ([m],[rad]), + * the rotations are over-specified, this can be avoided using the weight matrix. Weights also + * make the solver more robust . + * - only the constructors call memory allocation. + * + * De general principles behind the optimisation is inspired on: + * Jorge Nocedal, Stephen J. Wright, Numerical Optimization,Springer-Verlag New York, 1999. + + * \ingroup KinematicFamily + */ +class ChainIkSolverPos_LMA : public KDL::ChainIkSolverPos +{ +private: + typedef double ScalarType; + typedef Eigen::Matrix MatrixXq; + typedef Eigen::Matrix VectorXq; +public: + + /** + * \brief constructs an ChainIkSolverPos_LMA solver. + * + * The default parameters are choosen to be applicable to industrial-size robots + * (e.g. 0.5 to 3 meters range in task space), with an accuracy that is more then + * sufficient for typical industrial applications. + * + * Weights are applied in task space, i.e. the kinematic solver minimizes: + * \f$ E = \Delta \mathbf{x}^T \mathbf{L} \mathbf{L}^T \Delta \mathbf{x} \f$, with \f$\mathbf{L}\f$ a diagonal matrix. + * + * \param _chain specifies the kinematic chain. + * \param _L specifies the "square root" of the weight (diagonal) matrix in task space. This diagonal matrix is specified as a vector. + * \param _eps specifies the desired accuracy in task space; after weighing with + * the weight matrix, it is applied on \f$E\f$. + * \param _maxiter specifies the maximum number of iterations. + * \param _eps_joints specifies that the algorithm has to stop when the computed joint angle increments are + * smaller then _eps_joints. This is to avoid unnecessary computations up to _maxiter when the joint angle + * increments are so small that they effectively (in floating point) do not change the joint angles any more. The default + * is a few digits above numerical accuracy. + */ + ChainIkSolverPos_LMA( + const KDL::Chain& _chain, + const Eigen::Matrix& _L, + double _eps=1E-5, + int _maxiter=500, + double _eps_joints=1E-15 + ); + + /** + * \brief identical the full constructor for ChainIkSolverPos_LMA, but provides for a default weight matrix. + * + * \f$\mathbf{L} = \mathrm{diag}\left( \begin{bmatrix} 1 & 1 & 1 & 0.01 & 0.01 & 0.01 \end{bmatrix} \right) \f$. + */ + ChainIkSolverPos_LMA( + const KDL::Chain& _chain, + double _eps=1E-5, + int _maxiter=500, + double _eps_joints=1E-15 + ); + + /** + * \brief computes the inverse position kinematics. + * + * \param q_init initial joint position. + * \param T_base_goal goal position expressed with respect to the robot base. + * \param q_out joint position that achieves the specified goal position (if successful). + * \return 0 if successful, + * -1 the gradient of \f$ E \f$ towards the joints is to small, + * -2 if joint position increments are to small, + * -3 if number of iterations is exceeded. + */ + virtual int CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out); + + /** + * \brief destructor. + */ + virtual ~ChainIkSolverPos_LMA(); + + /** + * \brief for internal use only. + * + * Only exposed for test and diagnostic purposes. + */ + void compute_fwdpos(const VectorXq& q); + + /** + * \brief for internal use only. + * Only exposed for test and diagnostic purposes. + * compute_fwdpos(q) should always have been called before. + */ + void compute_jacobian(const VectorXq& q); + + /** + * \brief for internal use only. + * Only exposed for test and diagnostic purposes. + */ + void display_jac(const KDL::JntArray& jval); + + + + +public: + + + /** + * \brief contains the last number of iterations for an execution of CartToJnt. + */ + int lastNrOfIter; + + /** + * \brief contains the last value for \f$ E \f$ after an execution of CartToJnt. + */ + double lastDifference; + + /** + * \brief contains the last value for the (unweighted) translational difference after an execution of CartToJnt. + */ + double lastTransDiff; + + /** + * \brief contains the last value for the (unweighted) rotational difference after an execution of CartToJnt. + */ + double lastRotDiff; + + /** + * \brief contains the last values for the singular values of the weighted Jacobian after an execution of CartToJnt. + */ + VectorXq lastSV; + + /** + * \brief for internal use only. + * + * contains the last value for the Jacobian after an execution of compute_jacobian. + */ + MatrixXq jac; + + /** + * \brief for internal use only. + * + * contains the gradient of the error criterion after an execution of CartToJnt. + */ + VectorXq grad; + /** + * \brief for internal use only. + * + * contains the last value for the position of the tip of the robot (head) with respect to the base, after an execution of compute_jacobian. + */ + KDL::Frame T_base_head; + + /** + * \brief display information on each iteration step to the console. + */ + bool display_information; +private: + // additional specification of the inverse position kinematics problem: + + + unsigned int maxiter; + double eps; + double eps_joints; + Eigen::Matrix L; + const KDL::Chain& chain; + + + // state of compute_fwdpos and compute_jacobian: + std::vector T_base_jointroot; + std::vector T_base_jointtip; + // need 2 vectors because of the somewhat strange definition of segment.hpp + // you could also recompute jointtip out of jointroot, + // but then you'll need more expensive cos/sin functions. + + + // the following are state of CartToJnt that is pre-allocated: + + VectorXq q; + MatrixXq A; + VectorXq tmp; + Eigen::LDLT ldlt; + Eigen::JacobiSVD svd; + VectorXq diffq; + VectorXq q_new; + VectorXq original_Aii; +}; + + + + + +}; // namespace KDL + + + + + + +#endif diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr.cpp b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr.cpp index 0d5f84a0d..b10dff5ec 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr.cpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr.cpp @@ -38,20 +38,27 @@ namespace KDL for(i=0;i rc) + return (error = E_IKSOLVER_FAILED); + // we chose to continue if the child solver returned a positive + // "error", which may simply indicate a degraded solution Add(q_out,delta_q,q_out); if(Equal(delta_twist,Twist::Zero(),eps)) - break; + // converged, but possibly with a degraded solution + return (rc > E_NOERROR ? E_DEGRADED : E_NOERROR); } - if(i!=maxiter) - return 0; - else - return -3; + return (error = E_NO_CONVERGE); // failed to converge } ChainIkSolverPos_NR::~ChainIkSolverPos_NR() { } + const char* ChainIkSolverPos_NR::strError(const int error) const + { + if (E_IKSOLVER_FAILED == error) return "Child IK solver failed"; + else return SolverI::strError(error); + } } diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr.hpp b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr.hpp index f49ff9a35..1024c7486 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr.hpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr.hpp @@ -38,6 +38,8 @@ namespace KDL { class ChainIkSolverPos_NR : public ChainIkSolverPos { public: + static const int E_IKSOLVER_FAILED = -100; //! Child IK solver failed + /** * Constructor of the solver, it needs the chain, a forward * position kinematics solver and an inverse velocity @@ -57,8 +59,22 @@ namespace KDL { unsigned int maxiter=100,double eps=1e-6); ~ChainIkSolverPos_NR(); + /** + * Find an output joint pose \a q_out, given a starting joint pose + * \a q_init and a desired cartesian pose \a p_in + * + * @return: + * E_NOERROR=solution converged to q_max(j)) - //q_out(j) = q_max(j); - q_out(j) = q_out(j) - M_PI *2; + q_out(j) = q_max(j); } } diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.hpp b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.hpp index 9d42945cb..703f9151c 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.hpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.hpp @@ -46,8 +46,8 @@ namespace KDL { * kinematics solver for that chain. * * @param chain the chain to calculate the inverse position for - * @param q_max the maximum joint positions * @param q_min the minimum joint positions + * @param q_max the maximum joint positions * @param fksolver a forward position kinematics solver * @param iksolver an inverse velocity kinematics solver * @param maxiter the maximum Newton-Raphson iterations, @@ -69,11 +69,12 @@ namespace KDL { ChainIkSolverVel& iksolver; ChainFkSolverPos& fksolver; JntArray delta_q; + unsigned int maxiter; + double eps; + Frame f; Twist delta_twist; - unsigned int maxiter; - double eps; }; } diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv.cpp b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv.cpp index 3f3c64373..979d11b30 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv.cpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv.cpp @@ -33,7 +33,9 @@ namespace KDL V(chain.getNrOfJoints(),JntArray(chain.getNrOfJoints())), tmp(chain.getNrOfJoints()), eps(_eps), - maxiter(_maxiter) + maxiter(_maxiter), + nrZeroSigmas(0), + svdResult(0) { } @@ -48,13 +50,21 @@ namespace KDL //the current joint positions "q_in" jnt2jac.JntToJac(q_in,jac); + double sum; + unsigned int i,j; + + // Initialize near zero singular value counter + nrZeroSigmas = 0 ; + //Do a singular value decomposition of "jac" with maximum //iterations "maxiter", put the results in "U", "S" and "V" //jac = U*S*Vt - int ret = svd.calculate(jac,U,S,V,maxiter); - - double sum; - unsigned int i,j; + svdResult = svd.calculate(jac,U,S,V,maxiter); + if (0 != svdResult) + { + qdot_out.data.setZero(); + return (error = E_SVD_FAILED); + } // We have to calculate qdot_out = jac_pinv*v_in // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut): @@ -68,7 +78,14 @@ namespace KDL } //If the singular value is too small ( (jac.columns()-jac.rows()) ) { + return (error = E_CONVERGE_PINV_SINGULAR); // converged but pinv singular + } else { + return (error = E_NOERROR); // have converged + } } + const char* ChainIkSolverVel_pinv::strError(const int error) const + { + if (E_SVD_FAILED == error) return "SVD failed"; + else return SolverI::strError(error); + } } diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv.hpp b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv.hpp index fe1806b39..9e8abbd02 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv.hpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv.hpp @@ -40,6 +40,10 @@ namespace KDL class ChainIkSolverVel_pinv : public ChainIkSolverVel { public: + static const int E_SVD_FAILED = -100; //! Child SVD failed + /// solution converged but (pseudo)inverse is singular + static const int E_CONVERGE_PINV_SINGULAR = +100; + /** * Constructor of the solver * @@ -51,15 +55,47 @@ namespace KDL * default: 150 * */ - ChainIkSolverVel_pinv(const Chain& chain,double eps=0.00001,int maxiter=150); + explicit ChainIkSolverVel_pinv(const Chain& chain,double eps=0.00001,int maxiter=150); ~ChainIkSolverVel_pinv(); + /** + * Find an output joint velocity \a qdot_out, given a starting joint pose + * \a q_init and a desired cartesian velocity \a v_in + * + * @return + * E_NOERROR=solution converged to jac.col()-jac.row(), + * then the jacobian pseudoinverse is singular + */ + unsigned int getNrZeroSigmas()const {return nrZeroSigmas;}; + + /** + * Retrieve the latest return code from the SVD algorithm + * @return 0 if CartToJnt() not yet called, otherwise latest SVD result code. + */ + int getSVDResult()const {return svdResult;}; + + /// @copydoc KDL::SolverI::strError() + virtual const char* strError(const int error) const; + private: const Chain chain; ChainJntToJacSolver jnt2jac; @@ -71,6 +107,8 @@ namespace KDL JntArray tmp; double eps; int maxiter; + unsigned int nrZeroSigmas; + int svdResult; }; } diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.cpp b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.cpp index 0849110e2..682ca94d5 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.cpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.cpp @@ -29,8 +29,8 @@ namespace KDL jnt2jac(chain), jac(chain.getNrOfJoints()), transpose(chain.getNrOfJoints()>6),toggle(true), - m((int)max(6,chain.getNrOfJoints())), - n((int)min(6,chain.getNrOfJoints())), + m(max(6,chain.getNrOfJoints())), + n(min(6,chain.getNrOfJoints())), jac_eigen(m,n), U(MatrixXd::Identity(m,m)), V(MatrixXd::Identity(n,n)), @@ -52,13 +52,15 @@ namespace KDL int ChainIkSolverVel_pinv_givens::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out) { + toggle=!toggle; + jnt2jac.JntToJac(q_in,jac); for(unsigned int i=0;i<6;i++) v_in_eigen(i)=v_in(i); - for(int i=0;i -USING_PART_OF_NAMESPACE_EIGEN; +using namespace Eigen; namespace KDL { @@ -29,9 +29,13 @@ namespace KDL * * @param chain the chain to calculate the inverse velocity * kinematics for + * @param eps if a singular value is below this value, its + * inverse is set to zero, default: 0.00001 + * @param maxiter maximum iterations for the svd calculation, + * default: 150 * */ - ChainIkSolverVel_pinv_givens(const Chain& chain); + explicit ChainIkSolverVel_pinv_givens(const Chain& chain); ~ChainIkSolverVel_pinv_givens(); virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); @@ -45,7 +49,7 @@ namespace KDL ChainJntToJacSolver jnt2jac; Jacobian jac; bool transpose,toggle; - int m,n; + unsigned int m,n; MatrixXd jac_eigen,U,V,B; VectorXd S,tempi,tempj,UY,SUY,qdot_eigen,v_in_eigen; }; diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.cpp b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.cpp index 3243872b1..f5faab76a 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.cpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.cpp @@ -20,19 +20,21 @@ // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA #include "chainiksolvervel_pinv_nso.hpp" +#include "utilities/svd_eigen_HH.hpp" namespace KDL { - ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, JntArray _opt_pos, JntArray _weights, double _eps, int _maxiter, int _alpha): + ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, JntArray _opt_pos, JntArray _weights, double _eps, int _maxiter, double _alpha): chain(_chain), jnt2jac(chain), - jac(chain.getNrOfJoints()), - svd(jac), - U(6,JntArray(chain.getNrOfJoints())), - S(chain.getNrOfJoints()), - V(chain.getNrOfJoints(),JntArray(chain.getNrOfJoints())), - tmp(chain.getNrOfJoints()), - tmp2(chain.getNrOfJoints()-6), + nj(chain.getNrOfJoints()), + jac(nj), + U(MatrixXd::Zero(6,nj)), + S(VectorXd::Zero(nj)), + Sinv(VectorXd::Zero(nj)), + V(MatrixXd::Zero(nj,nj)), + tmp(VectorXd::Zero(nj)), + tmp2(VectorXd::Zero(nj)), eps(_eps), maxiter(_maxiter), alpha(_alpha), @@ -41,16 +43,17 @@ namespace KDL { } - ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, double _eps, int _maxiter, int _alpha): + ChainIkSolverVel_pinv_nso::ChainIkSolverVel_pinv_nso(const Chain& _chain, double _eps, int _maxiter, double _alpha): chain(_chain), jnt2jac(chain), - jac(chain.getNrOfJoints()), - svd(jac), - U(6,JntArray(chain.getNrOfJoints())), - S(chain.getNrOfJoints()), - V(chain.getNrOfJoints(),JntArray(chain.getNrOfJoints())), - tmp(chain.getNrOfJoints()), - tmp2(chain.getNrOfJoints()-6), + nj(chain.getNrOfJoints()), + jac(nj), + U(MatrixXd::Zero(6,nj)), + S(VectorXd::Zero(nj)), + Sinv(VectorXd::Zero(nj)), + V(MatrixXd::Zero(nj,nj)), + tmp(VectorXd::Zero(nj)), + tmp2(VectorXd::Zero(nj)), eps(_eps), maxiter(_maxiter), alpha(_alpha) @@ -71,76 +74,90 @@ namespace KDL //Do a singular value decomposition of "jac" with maximum //iterations "maxiter", put the results in "U", "S" and "V" //jac = U*S*Vt - int ret = svd.calculate(jac,U,S,V,maxiter); + int svdResult = svd_eigen_HH(jac.data,U,S,V,tmp,maxiter); + if (0 != svdResult) + { + qdot_out.data.setZero() ; + return svdResult; + } - double sum; - unsigned int i,j; + unsigned int i; // We have to calculate qdot_out = jac_pinv*v_in // Using the svd decomposition this becomes(jac_pinv=V*S_pinv*Ut): // qdot_out = V*S_pinv*Ut*v_in - //first we calculate Ut*v_in - for (i=0;i 1e-9) { + // Calculate inverse Jacobian Jc^-1 + for (i = 0; i < nj; ++i) { + tmp(i) = weights(i)*(q_in(i) - opt_pos(i)) / A; + } + + // Calcualte J^-1 * J * Jc^-1 = V*S^-1*U' * U*S*V' * tmp + tmp2 = V * Sinv.asDiagonal() * U.transpose() * U * S.asDiagonal() * V.transpose() * tmp; + + for (i = 0; i < nj; ++i) { + //std::cerr << i <<": "<< qdot_out(i) <<", "<< -2*alpha*g * (tmp(i) - tmp2(i)) << std::endl; + qdot_out(i) += -2*alpha*g * (tmp(i) - tmp2(i)); + } } - //return the return value of the svd decomposition - return ret; + return svdResult; + } + + int ChainIkSolverVel_pinv_nso::setWeights(const JntArray & _weights) + { + weights = _weights; + return 0; + } + + int ChainIkSolverVel_pinv_nso::setOptPos(const JntArray & _opt_pos) + { + opt_pos = _opt_pos; + return 0; + } + + int ChainIkSolverVel_pinv_nso::setAlpha(const double _alpha) + { + alpha = _alpha; + return 0; } - - int ChainIkSolverVel_pinv_nso::setWeights(const JntArray & _weights) - { - weights = _weights; - return 0; - } - int ChainIkSolverVel_pinv_nso::setOptPos(const JntArray & _opt_pos) - { - opt_pos = _opt_pos; - return 0; - } - int ChainIkSolverVel_pinv_nso::setAlpha(const int _alpha) - { - alpha = _alpha; - return 0; - } } diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.hpp b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.hpp index d5f188da7..20760f859 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.hpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.hpp @@ -19,19 +19,15 @@ // License along with this library; if not, write to the Free Software // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA -#ifndef KDL_CHAIN_IKSOLVERVEL_PINV_HPP -#define KDL_CHAIN_IKSOLVERVEL_PINV_HPP +#ifndef KDL_CHAIN_IKSOLVERVEL_PINV_NSO_HPP +#define KDL_CHAIN_IKSOLVERVEL_PINV_NSO_HPP #include "chainiksolver.hpp" #include "chainjnttojacsolver.hpp" -#include "utilities/svd_HH.hpp" +#include namespace KDL { - - // FIXME: seems this class is unused/unmaintained/unfinished for several years - // it supposed to be either fixer or removed - /** * Implementation of a inverse velocity kinematics algorithm based * on the generalize pseudo inverse to calculate the velocity @@ -64,10 +60,8 @@ namespace KDL * @param alpha the null-space velocity gain * */ - - // FIXME: alpha is int but is initialized with a float value. - ChainIkSolverVel_pinv_nso(const Chain& chain, JntArray opt_pos, JntArray weights, double eps=0.00001,int maxiter=150, int alpha = 0.25); - ChainIkSolverVel_pinv_nso(const Chain& chain, double eps=0.00001,int maxiter=150, int alpha = 0.25); + ChainIkSolverVel_pinv_nso(const Chain& chain, JntArray opt_pos, JntArray weights, double eps=0.00001,int maxiter=150, double alpha = 0.25); + explicit ChainIkSolverVel_pinv_nso(const Chain& chain, double eps=0.00001,int maxiter=150, double alpha = 0.25); ~ChainIkSolverVel_pinv_nso(); virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); @@ -77,7 +71,6 @@ namespace KDL */ virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;}; - /** *Set joint weights for optimization criterion * @@ -100,25 +93,25 @@ namespace KDL *@param alpha NUllspace velocity cgain * */ - virtual int setAlpha(const int alpha); + virtual int setAlpha(const double alpha); private: const Chain chain; ChainJntToJacSolver jnt2jac; + unsigned int nj; Jacobian jac; - SVD_HH svd; - std::vector U; - JntArray S; - std::vector V; - JntArray tmp; - JntArray tmp2; + Eigen::MatrixXd U; + Eigen::VectorXd S; + Eigen::VectorXd Sinv; + Eigen::MatrixXd V; + Eigen::VectorXd tmp; + Eigen::VectorXd tmp2; double eps; int maxiter; - int alpha; + double alpha; JntArray weights; JntArray opt_pos; - }; } #endif diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.cpp b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.cpp index b7323b1b7..3a24e9974 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.cpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.cpp @@ -42,7 +42,11 @@ namespace KDL tmp_js(MatrixXd::Zero(chain.getNrOfJoints(),chain.getNrOfJoints())), weight_ts(MatrixXd::Identity(6,6)), weight_js(MatrixXd::Identity(chain.getNrOfJoints(),chain.getNrOfJoints())), - lambda(0.0) + lambda(0.0), + lambda_scaled(0.0), + nrZeroSigmas(0), + svdResult(0), + sigmaMin(0) { } @@ -50,19 +54,29 @@ namespace KDL { } - void ChainIkSolverVel_wdls::setWeightJS(const Eigen::MatrixXd& Mq){ + void ChainIkSolverVel_wdls::setWeightJS(const MatrixXd& Mq){ weight_js = Mq; } - void ChainIkSolverVel_wdls::setWeightTS(const Eigen::MatrixXd& Mx){ + void ChainIkSolverVel_wdls::setWeightTS(const MatrixXd& Mx){ weight_ts = Mx; } - void ChainIkSolverVel_wdls::setLambda(const double& lambda_in) + void ChainIkSolverVel_wdls::setLambda(const double lambda_in) { lambda=lambda_in; } - + + void ChainIkSolverVel_wdls::setEps(const double eps_in) + { + eps=eps_in; + } + + void ChainIkSolverVel_wdls::setMaxIter(const int maxiter_in) + { + maxiter=maxiter_in; + } + int ChainIkSolverVel_wdls::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out) { jnt2jac.JntToJac(q_in,jac); @@ -70,6 +84,11 @@ namespace KDL double sum; unsigned int i,j; + // Initialize (internal) return values + nrZeroSigmas = 0 ; + sigmaMin = 0.; + lambda_scaled = 0.; + /* for (i=0;i=6 and 0 for <6 + if ( jac.columns() >= 6 ) { + sigmaMin = S(5); + } + else { + sigmaMin = 0.; + } + + // tmp = (Si*U'*Ly*y), for (i=0;i eps, then wdls is not active and lambda_scaled = 0 (default value) + // If sigmaMin < eps, then wdls is active and lambda_scaled is scaled from 0 to lambda + // Note: singular values are always positive so sigmaMin >=0 + if ( sigmaMin < eps ) + { + lambda_scaled = sqrt(1.0-(sigmaMin/eps)*(sigmaMin/eps))*lambda ; + } + if(fabs(S(i))=6 due to cols>rows + } + // Count number of singular values near zero + ++nrZeroSigmas ; + } + else { tmp(i) = sum/S(i); + } } + /* // x = Lx^-1*V*tmp + x for (i=0;i (jac.columns()-jac.rows()) ) { + return (error = E_CONVERGE_PINV_SINGULAR); // converged but pinv singular + } else { + return (error = E_NOERROR); // have converged + } } - + + const char* ChainIkSolverVel_wdls::strError(const int error) const + { + if (E_SVD_FAILED == error) return "SVD failed"; + else return SolverI::strError(error); + } + } diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.hpp b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.hpp index 2fbf99693..d1c171be4 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.hpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.hpp @@ -63,6 +63,10 @@ namespace KDL class ChainIkSolverVel_wdls : public ChainIkSolverVel { public: + static const int E_SVD_FAILED = -100; //! SVD solver failed + /// solution converged but (pseudo)inverse is singular + static const int E_CONVERGE_PINV_SINGULAR = +100; + /** * Constructor of the solver * @@ -75,10 +79,25 @@ namespace KDL * */ - ChainIkSolverVel_wdls(const Chain& chain,double eps=0.00001,int maxiter=150); + explicit ChainIkSolverVel_wdls(const Chain& chain,double eps=0.00001,int maxiter=150); //=ublas::identity_matrix ~ChainIkSolverVel_wdls(); + /** + * Find an output joint velocity \a qdot_out, given a starting joint pose + * \a q_init and a desired cartesian velocity \a v_in + * + * @return + * E_NOERROR=svd solution converged in maxiter + * E_SVD_FAILED=svd solution failed + * E_CONVERGE_PINV_SINGULAR=svd solution converged but (pseudo)inverse singular + * + * @note if E_CONVERGE_PINV_SINGULAR returned then converged and can + * continue motion, but have degraded solution + * + * @note If E_SVD_FAILED returned, then getSvdResult() returns the error + * code from the SVD algorithm. + */ virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out); /** * not (yet) implemented. @@ -89,9 +108,8 @@ namespace KDL /** * Set the joint space weighting matrix * - * weight_js joint space weighting symetric matrix, - * default : identity. - * @param Mq : This matrix being used as a + * @param weight_js joint space weighting symetric matrix, + * default : identity. M_q : This matrix being used as a * weight for the norm of the joint space speed it HAS TO BE * symmetric and positive definite. We can actually deal with * matrices containing a symmetric and positive definite block @@ -114,9 +132,8 @@ namespace KDL /** * Set the task space weighting matrix * - * weight_ts task space weighting symetric matrix, - * default: identity - * @param Mx : This matrix being used as a weight + * @param weight_ts task space weighting symetric matrix, + * default: identity M_x : This matrix being used as a weight * for the norm of the error (in terms of task space speed) it * HAS TO BE symmetric and positive definite. We can actually * deal with matrices containing a symmetric and positive @@ -137,7 +154,50 @@ namespace KDL */ void setWeightTS(const Eigen::MatrixXd& Mx); - void setLambda(const double& lambda); + /** + * Set lambda + */ + void setLambda(const double lambda); + /** + * Set eps + */ + void setEps(const double eps_in); + /** + * Set maxIter + */ + void setMaxIter(const int maxiter_in); + + /** + * Request the number of singular values of the jacobian that are < eps; + * if the number of near zero singular values is > jac.col()-jac.row(), + * then the jacobian pseudoinverse is singular + */ + unsigned int getNrZeroSigmas()const {return nrZeroSigmas;}; + + /** + * Request the minimum of the first six singular values + */ + double getSigmaMin()const {return sigmaMin;}; + + /** + * Request the value of lambda for the minimum + */ + double getLambda()const {return lambda;}; + + /** + * Request the scaled value of lambda for the minimum + * singular value 1-6 + */ + double getLambdaScaled()const {return lambda_scaled;}; + + /** + * Retrieve the latest return code from the SVD algorithm + * @return 0 if CartToJnt() not yet called, otherwise latest SVD result code. + */ + int getSVDResult()const {return svdResult;}; + + /// @copydoc KDL::SolverI::strError() + virtual const char* strError(const int error) const; private: const Chain chain; @@ -157,6 +217,10 @@ namespace KDL Eigen::MatrixXd weight_ts; Eigen::MatrixXd weight_js; double lambda; + double lambda_scaled; + unsigned int nrZeroSigmas ; + int svdResult; + double sigmaMin; }; } #endif diff --git a/src/Mod/Robot/App/kdl_cp/chainjnttojacsolver.cpp b/src/Mod/Robot/App/kdl_cp/chainjnttojacsolver.cpp index fd193fd0f..30a0d40e0 100644 --- a/src/Mod/Robot/App/kdl_cp/chainjnttojacsolver.cpp +++ b/src/Mod/Robot/App/kdl_cp/chainjnttojacsolver.cpp @@ -43,19 +43,32 @@ namespace KDL if(!locked_joints_[i]) nr_of_unlocked_joints_++; } + return 0; } - int ChainJntToJacSolver::JntToJac(const JntArray& q_in,Jacobian& jac) + int ChainJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, int seg_nr) { - if(q_in.rows()!=chain.getNrOfJoints()||nr_of_unlocked_joints_!=static_cast(jac.columns())) - return -1; + unsigned int segmentNr; + if(seg_nr<0) + segmentNr=chain.getNrOfSegments(); + else + segmentNr = seg_nr; + + //Initialize Jacobian to zero since only segmentNr colunns are computed + SetToZero(jac) ; + + if(q_in.rows()!=chain.getNrOfJoints()||nr_of_unlocked_joints_!=jac.columns()) + return (error = E_JAC_FAILED); + else if(segmentNr>chain.getNrOfSegments()) + return (error = E_JAC_FAILED); + T_tmp = Frame::Identity(); SetToZero(t_tmp); int j=0; int k=0; Frame total; - for (unsigned int i=0;i locked_joints); + + /// @copydoc KDL::SolverI::strError() + virtual const char* strError(const int error) const; + private: const Chain chain; Twist t_tmp; Frame T_tmp; std::vector locked_joints_; - int nr_of_unlocked_joints_; + unsigned int nr_of_unlocked_joints_; }; } #endif diff --git a/src/Mod/Robot/App/kdl_cp/config.h.in b/src/Mod/Robot/App/kdl_cp/config.h.in new file mode 100644 index 000000000..a63d12a47 --- /dev/null +++ b/src/Mod/Robot/App/kdl_cp/config.h.in @@ -0,0 +1,37 @@ +// Copyright (C) 2014 Ruben Smits + +// Version: 1.0 +// Author: Brian Jensen +// Maintainer: Ruben Smits +// URL: http://www.orocos.org/kdl + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_CONFIG_H +#define KDL_CONFIG_H + +#define KDL_VERSION_MAJOR @KDL_VERSION_MAJOR@ +#define KDL_VERSION_MINOR @KDL_VERSION_MINOR@ +#define KDL_VERSION_PATCH @KDL_VERSION_PATCH@ + +#define KDL_VERSION (KDL_VERSION_MAJOR << 16) | (KDL_VERSION_MINOR << 8) | KDL_VERSION_PATCH + +#define KDL_VERSION_STRING "@KDL_VERSION@" + +//Set which version of the Tree Interface to use +#cmakedefine HAVE_STL_CONTAINER_INCOMPLETE_TYPES +#cmakedefine KDL_USE_NEW_TREE_INTERFACE + +#endif //#define KDL_CONFIG_H diff --git a/src/Mod/Robot/App/kdl_cp/frameacc.hpp b/src/Mod/Robot/App/kdl_cp/frameacc.hpp index 03e32e3eb..6521d0cf2 100644 --- a/src/Mod/Robot/App/kdl_cp/frameacc.hpp +++ b/src/Mod/Robot/App/kdl_cp/frameacc.hpp @@ -40,6 +40,23 @@ namespace KDL { class TwistAcc; typedef Rall2d doubleAcc; +// Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4) +class FrameAcc; +class RotationAcc; +class VectorAcc; + +IMETHOD bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps=epsilon); +IMETHOD bool Equal(const Frame& r1,const FrameAcc& r2,double eps=epsilon); +IMETHOD bool Equal(const FrameAcc& r1,const Frame& r2,double eps=epsilon); +IMETHOD bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps=epsilon); +IMETHOD bool Equal(const Rotation& r1,const RotationAcc& r2,double eps=epsilon); +IMETHOD bool Equal(const RotationAcc& r1,const Rotation& r2,double eps=epsilon); +IMETHOD bool Equal(const TwistAcc& a,const TwistAcc& b,double eps=epsilon); +IMETHOD bool Equal(const Twist& a,const TwistAcc& b,double eps=epsilon); +IMETHOD bool Equal(const TwistAcc& a,const Twist& b,double eps=epsilon); +IMETHOD bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps=epsilon); +IMETHOD bool Equal(const Vector& r1,const VectorAcc& r2,double eps=epsilon); +IMETHOD bool Equal(const VectorAcc& r1,const Vector& r2,double eps=epsilon); class VectorAcc { @@ -244,13 +261,13 @@ public: -} #ifdef KDL_INLINE #include "frameacc.inl" #endif +} diff --git a/src/Mod/Robot/App/kdl_cp/frameacc.inl b/src/Mod/Robot/App/kdl_cp/frameacc.inl index 7bbeb5d77..dfbdc620e 100644 --- a/src/Mod/Robot/App/kdl_cp/frameacc.inl +++ b/src/Mod/Robot/App/kdl_cp/frameacc.inl @@ -17,7 +17,6 @@ ****************************************************************************/ -namespace KDL { /////////////////// VectorAcc ///////////////////////////////////// @@ -431,6 +430,7 @@ Twist FrameAcc::GetAccTwist() const { + TwistAcc TwistAcc::Zero() { return TwistAcc(VectorAcc::Zero(),VectorAcc::Zero()); @@ -596,4 +596,3 @@ bool Equal(const TwistAcc& a,const Twist& b,double eps) { Equal(a.vel,b.vel,eps) ); } -} diff --git a/src/Mod/Robot/App/kdl_cp/frames.cpp b/src/Mod/Robot/App/kdl_cp/frames.cpp index 935a14793..4b9ec2de5 100644 --- a/src/Mod/Robot/App/kdl_cp/frames.cpp +++ b/src/Mod/Robot/App/kdl_cp/frames.cpp @@ -27,6 +27,9 @@ #include "frames.hpp" +#define _USE_MATH_DEFINES // For MSVC +#include + namespace KDL { #ifndef KDL_INLINE @@ -40,7 +43,7 @@ namespace KDL { for (i=0;i<3;i++) { for (j=0;j<3;j++) d[i*4+j]=M(i,j); - d[i*4+3] = p(i)/1000; + d[i*4+3] = p(i); } for (j=0;j<3;j++) d[12+j] = 0.; @@ -194,32 +197,33 @@ namespace KDL { */ void Rotation::GetQuaternion(double& x,double& y,double& z, double& w) const { - double trace = (*this)(0,0) + (*this)(1,1) + (*this)(2,2) + 1.0f; + double trace = (*this)(0,0) + (*this)(1,1) + (*this)(2,2); + double epsilon=1E-12; if( trace > epsilon ){ - double s = 0.5f / sqrt(trace); - w = 0.25f / s; + double s = 0.5 / sqrt(trace + 1.0); + w = 0.25 / s; x = ( (*this)(2,1) - (*this)(1,2) ) * s; y = ( (*this)(0,2) - (*this)(2,0) ) * s; z = ( (*this)(1,0) - (*this)(0,1) ) * s; }else{ if ( (*this)(0,0) > (*this)(1,1) && (*this)(0,0) > (*this)(2,2) ){ - float s = 2.0f * sqrtf( 1.0f + (*this)(0,0) - (*this)(1,1) - (*this)(2,2)); + double s = 2.0 * sqrt( 1.0 + (*this)(0,0) - (*this)(1,1) - (*this)(2,2)); w = ((*this)(2,1) - (*this)(1,2) ) / s; - x = 0.25f * s; + x = 0.25 * s; y = ((*this)(0,1) + (*this)(1,0) ) / s; z = ((*this)(0,2) + (*this)(2,0) ) / s; } else if ((*this)(1,1) > (*this)(2,2)) { - float s = 2.0f * sqrtf( 1.0f + (*this)(1,1) - (*this)(0,0) - (*this)(2,2)); + double s = 2.0 * sqrt( 1.0 + (*this)(1,1) - (*this)(0,0) - (*this)(2,2)); w = ((*this)(0,2) - (*this)(2,0) ) / s; x = ((*this)(0,1) + (*this)(1,0) ) / s; - y = 0.25f * s; + y = 0.25 * s; z = ((*this)(1,2) + (*this)(2,1) ) / s; }else { - float s = 2.0f * sqrtf( 1.0f + (*this)(2,2) - (*this)(0,0) - (*this)(1,1) ); + double s = 2.0 * sqrt( 1.0 + (*this)(2,2) - (*this)(0,0) - (*this)(1,1) ); w = ((*this)(1,0) - (*this)(0,1) ) / s; x = ((*this)(0,2) + (*this)(2,0) ) / s; y = ((*this)(1,2) + (*this)(2,1) ) / s; - z = 0.25f * s; + z = 0.25 * s; } } } @@ -238,13 +242,13 @@ Rotation Rotation::RPY(double roll,double pitch,double yaw) // Gives back a rotation matrix specified with RPY convention void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const { - if (fabs(data[6]) > 1.0 - epsilon ) { - roll = -sign(data[6]) * atan2(data[1], data[4]); - pitch= -sign(data[6]) * PI / 2; - yaw = 0.0 ; + double epsilon=1E-12; + pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) ) ); + if ( fabs(pitch) > (M_PI/2.0-epsilon) ) { + yaw = atan2( -data[1], data[4]); + roll = 0.0 ; } else { roll = atan2(data[7], data[8]); - pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) ) ); yaw = atan2(data[3], data[0]); } } @@ -262,18 +266,19 @@ Rotation Rotation::EulerZYZ(double Alfa,double Beta,double Gamma) { } -void Rotation::GetEulerZYZ(double& alfa,double& beta,double& gamma) const { - if (fabs(data[6]) < epsilon ) { - alfa=0.0; +void Rotation::GetEulerZYZ(double& alpha,double& beta,double& gamma) const { + double epsilon = 1E-12; + if (fabs(data[8]) > 1-epsilon ) { + gamma=0.0; if (data[8]>0) { beta = 0.0; - gamma= atan2(-data[1],data[0]); + alpha= atan2(data[3],data[0]); } else { beta = PI; - gamma= atan2(data[1],-data[0]); + alpha= atan2(-data[3],-data[0]); } } else { - alfa=atan2(data[5], data[2]); + alpha=atan2(data[5], data[2]); beta=atan2(sqrt( sqr(data[6]) +sqr(data[7]) ),data[8]); gamma=atan2(data[7], -data[6]); } @@ -326,18 +331,10 @@ Vector Rotation::GetRot() const // Returns a vector with the direction of the equiv. axis // and its norm is angle { - Vector axis = Vector((data[7]-data[5]), - (data[2]-data[6]), - (data[3]-data[1]) )/2; - - double sa = axis.Norm(); - double ca = (data[0]+data[4]+data[8]-1)/2.0; - double alfa; - if (sa > epsilon) - alfa = ::atan2(sa,ca)/sa; - else - alfa = 1; - return axis * alfa; + Vector axis; + double angle; + angle = Rotation::GetRotAngle(axis,epsilon); + return axis * angle; } @@ -345,7 +342,7 @@ Vector Rotation::GetRot() const /** Returns the rotation angle around the equiv. axis * @param axis the rotation axis is returned in this variable * @param eps : in the case of angle == 0 : rot axis is undefined and choosen - * to be +/- Z-axis + * to be the Z-axis * in the case of angle == PI : 2 solutions, positive Z-component * of the axis is choosen. * @result returns the rotation angle (between [0..PI] ) @@ -354,30 +351,43 @@ Vector Rotation::GetRot() const */ double Rotation::GetRotAngle(Vector& axis,double eps) const { double ca = (data[0]+data[4]+data[8]-1)/2.0; - if (ca>1-eps) { + double t= eps*eps/2.0; + if (ca>1-t) { // undefined choose the Z-axis, and angle 0 axis = Vector(0,0,1); return 0; } - if (ca < -1+eps) { + if (ca < -1+t) { + // The case of angles consisting of multiples of M_PI: // two solutions, choose a positive Z-component of the axis - double z = sqrt( (data[8]+1)/2 ); - double x = (data[2])/2/z; - double y = (data[5])/2/z; + double x = sqrt( (data[0]+1.0)/2); + double y = sqrt( (data[4]+1.0)/2); + double z = sqrt( (data[8]+1.0)/2); + if ( data[2] < 0) x=-x; + if ( data[7] < 0) y=-y; + if ( x*y*data[1] < 0) x=-x; // this last line can be necessary when z is 0 + // z always >= 0 + // if z equal to zero axis = Vector( x,y,z ); return PI; } - double angle = acos(ca); - double sa = sin(angle); - axis = Vector((data[7]-data[5])/2/sa, - (data[2]-data[6])/2/sa, - (data[3]-data[1])/2/sa ); - return angle; + double angle; + double mod_axis; + double axisx, axisy, axisz; + axisx = data[7]-data[5]; + axisy = data[2]-data[6]; + axisz = data[3]-data[1]; + mod_axis = sqrt(axisx*axisx+axisy*axisy+axisz*axisz); + axis = Vector(axisx/mod_axis, + axisy/mod_axis, + axisz/mod_axis ); + angle = atan2(mod_axis/2,ca); + return angle; } bool operator==(const Rotation& a,const Rotation& b) { #ifdef KDL_USE_EQUAL - return Equal(a,b,epsilon); + return Equal(a,b); #else return ( a.data[0]==b.data[0] && a.data[1]==b.data[1] && diff --git a/src/Mod/Robot/App/kdl_cp/frames.hpp b/src/Mod/Robot/App/kdl_cp/frames.hpp index 8c769d5b4..c724358c7 100644 --- a/src/Mod/Robot/App/kdl_cp/frames.hpp +++ b/src/Mod/Robot/App/kdl_cp/frames.hpp @@ -144,6 +144,15 @@ class Rotation2; class Frame2; +// Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4) +inline bool Equal(const Vector& a,const Vector& b,double eps=epsilon); +inline bool Equal(const Frame& a,const Frame& b,double eps=epsilon); +inline bool Equal(const Twist& a,const Twist& b,double eps=epsilon); +inline bool Equal(const Wrench& a,const Wrench& b,double eps=epsilon); +inline bool Equal(const Vector2& a,const Vector2& b,double eps=epsilon); +inline bool Equal(const Rotation2& a,const Rotation2& b,double eps=epsilon); +inline bool Equal(const Frame2& a,const Frame2& b,double eps=epsilon); + /** * \brief A concrete implementation of a 3 dimensional vector class @@ -378,24 +387,34 @@ public: double GetRotAngle(Vector& axis,double eps=epsilon) const; - //! Gives back a rotation matrix specified with EulerZYZ convention : - //! First rotate around Z with alfa, - //! then around the new Y with beta, then around - //! new Z with gamma. +/** Gives back a rotation matrix specified with EulerZYZ convention : + * - First rotate around Z with alfa, + * - then around the new Y with beta, + * - then around new Z with gamma. + * Invariants: + * - EulerZYX(alpha,beta,gamma) == EulerZYX(alpha +/- PHI, -beta, gamma +/- PI) + * - (angle + 2*k*PI) + **/ static Rotation EulerZYZ(double Alfa,double Beta,double Gamma); - //! Gives back the EulerZYZ convention description of the rotation matrix : - //! First rotate around Z with alfa, - //! then around the new Y with beta, then around - //! new Z with gamma. - //! - //! Variables are bound by - //! (-PI <= alfa <= PI), - //! (0 <= beta <= PI), - //! (-PI <= alfa <= PI) - void GetEulerZYZ(double& alfa,double& beta,double& gamma) const; + /** Gives back the EulerZYZ convention description of the rotation matrix : + First rotate around Z with alpha, + then around the new Y with beta, then around + new Z with gamma. - //! Sets the value of this object to a rotation specified with Quaternion convention + Variables are bound by: + - (-PI < alpha <= PI), + - (0 <= beta <= PI), + - (-PI < gamma <= PI) + + if beta==0 or beta==PI, then alpha and gamma are not unique, in this case gamma is chosen to be zero. + Invariants: + - EulerZYX(alpha,beta,gamma) == EulerZYX(alpha +/- PI, -beta, gamma +/- PI) + - angle + 2*k*PI + */ + void GetEulerZYZ(double& alpha,double& beta,double& gamma) const; + + //! Gives back a rotation matrix specified with Quaternion convention //! the norm of (x,y,z,w) should be equal to 1 static Rotation Quaternion(double x,double y,double z, double w); @@ -403,42 +422,74 @@ public: //! \post the norm of (x,y,z,w) is 1 void GetQuaternion(double& x,double& y,double& z, double& w) const; - //! Sets the value of this object to a rotation specified with RPY convention: - //! first rotate around X with roll, then around the - //! old Y with pitch, then around old Z with alfa + /** + * + * Gives back a rotation matrix specified with RPY convention: + * first rotate around X with roll, then around the + * old Y with pitch, then around old Z with yaw + * + * Invariants: + * - RPY(roll,pitch,yaw) == RPY( roll +/- PI, PI-pitch, yaw +/- PI ) + * - angles + 2*k*PI + */ static Rotation RPY(double roll,double pitch,double yaw); - //! Gives back a vector in RPY coordinates, variables are bound by - //! -PI <= roll <= PI - //! -PI <= Yaw <= PI - //! -PI/2 <= PITCH <= PI/2 - //! - //! convention : first rotate around X with roll, then around the - //! old Y with pitch, then around old Z with alfa +/** Gives back a vector in RPY coordinates, variables are bound by + - -PI <= roll <= PI + - -PI <= Yaw <= PI + - -PI/2 <= PITCH <= PI/2 + + convention : + - first rotate around X with roll, + - then around the old Y with pitch, + - then around old Z with yaw + + if pitch == PI/2 or pitch == -PI/2, multiple solutions for gamma and alpha exist. The solution where roll==0 + is chosen. + + Invariants: + - RPY(roll,pitch,yaw) == RPY( roll +/- PI, PI-pitch, yaw +/- PI ) + - angles + 2*k*PI + +**/ void GetRPY(double& roll,double& pitch,double& yaw) const; - //! Gives back a rotation matrix specified with EulerZYX convention : - //! First rotate around Z with alfa, - //! then around the new Y with beta, then around - //! new X with gamma. - //! - //! closely related to RPY-convention + /** EulerZYX constructs a Rotation from the Euler ZYX parameters: + * - First rotate around Z with alfa, + * - then around the new Y with beta, + * - then around new X with gamma. + * + * Closely related to RPY-convention. + * + * Invariants: + * - EulerZYX(alpha,beta,gamma) == EulerZYX(alpha +/- PI, PI-beta, gamma +/- PI) + * - (angle + 2*k*PI) + **/ inline static Rotation EulerZYX(double Alfa,double Beta,double Gamma) { return RPY(Gamma,Beta,Alfa); } - //! GetEulerZYX gets the euler ZYX parameters of a rotation : - //! First rotate around Z with alfa, - //! then around the new Y with beta, then around - //! new X with gamma. - //! - //! Range of the results of GetEulerZYX : - //! -PI <= alfa <= PI - //! -PI <= gamma <= PI - //! -PI/2 <= beta <= PI/2 - //! - //! Closely related to RPY-convention. + /** GetEulerZYX gets the euler ZYX parameters of a rotation : + * First rotate around Z with alfa, + * then around the new Y with beta, then around + * new X with gamma. + * + * Range of the results of GetEulerZYX : + * - -PI <= alfa <= PI + * - -PI <= gamma <= PI + * - -PI/2 <= beta <= PI/2 + * + * if beta == PI/2 or beta == -PI/2, multiple solutions for gamma and alpha exist. The solution where gamma==0 + * is chosen. + * + * + * Invariants: + * - EulerZYX(alpha,beta,gamma) == EulerZYX(alpha +/- PI, PI-beta, gamma +/- PI) + * - and also (angle + 2*k*PI) + * + * Closely related to RPY-convention. + **/ inline void GetEulerZYX(double& Alfa,double& Beta,double& Gamma) const { GetRPY(Gamma,Beta,Alfa); } @@ -503,6 +554,7 @@ public: friend class Frame; }; bool operator==(const Rotation& a,const Rotation& b); + bool Equal(const Rotation& a,const Rotation& b,double eps=epsilon); @@ -587,9 +639,9 @@ public: //! @return the identity transformation Frame(Rotation::Identity(),Vector::Zero()). inline static Frame Identity(); - //! The twist \ is expressed wrt the current + //! The twist is expressed wrt the current //! frame. This frame is integrated into an updated frame with - //! \. Very simple first order integration rule. + //! . Very simple first order integration rule. inline void Integrate(const Twist& t_this,double frequency); /* @@ -1078,18 +1130,124 @@ public: inline friend bool Equal(const Frame2& a,const Frame2& b,double eps); }; -IMETHOD Vector diff(const Vector& a,const Vector& b,double dt=1); -IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt=1); -IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt=1); -IMETHOD Twist diff(const Twist& a,const Twist& b,double dt=1); -IMETHOD Wrench diff(const Wrench& W_a_p1,const Wrench& W_a_p2,double dt=1); -IMETHOD Vector addDelta(const Vector& a,const Vector&da,double dt=1); -IMETHOD Rotation addDelta(const Rotation& a,const Vector&da,double dt=1); -IMETHOD Frame addDelta(const Frame& a,const Twist& da,double dt=1); -IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1); -IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1); +/** + * determines the difference of vector b with vector a. + * + * see diff for Rotation matrices for further background information. + * + * \param p_w_a start vector a expressed to some frame w + * \param p_w_b end vector b expressed to some frame w . + * \param dt [optional][obsolete] time interval over which the numerical differentiation takes place. + * \return the difference (b-a) expressed to the frame w. + */ +IMETHOD Vector diff(const Vector& p_w_a,const Vector& p_w_b,double dt=1); -} // namespace KDL + +/** + * determines the (scaled) rotation axis necessary to rotate from b1 to b2. + * + * This rotation axis is expressed w.r.t. frame a. The rotation axis is scaled + * by the necessary rotation angle. The rotation angle is always in the + * (inclusive) interval \f$ [0 , \pi] \f$. + * + * This definition is chosen in this way to facilitate numerical differentiation. + * With this definition diff(a,b) == -diff(b,a). + * + * The diff() function is overloaded for all classes in frames.hpp and framesvel.hpp, such that + * numerical differentiation, equality checks with tolerances, etc. can be performed + * without caring exactly on which type the operation is performed. + * + * \param R_a_b1: The rotation matrix \f$ _a^{b1} R \f$ of b1 with respect to frame a. + * \param R_a_b2: The Rotation matrix \f$ _a^{b2} R \f$ of b2 with respect to frame a. + * \param dt [optional][obsolete] time interval over which the numerical differentiation takes place. By default this is set to 1.0. + * \return rotation axis to rotate from b1 to b2, scaled by the rotation angle, expressed in frame a. + * \warning - The result is not a rotational vector, i.e. it is not a mathematical vector. + * (no communitative addition). + * + * \warning - When used in the context of numerical differentiation, with the frames b1 and b2 very + * close to each other, the semantics correspond to the twist, scaled by the time. + * + * \warning - For angles equal to \f$ \pi \f$, The negative of the + * return value is equally valid. + */ +IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt=1); + +/** + * determines the rotation axis necessary to rotate the frame b1 to the same orientation as frame b2 and the vector + * necessary to translate the origin of b1 to the origin of b2, and stores the result in a Twist datastructure. + * \param F_a_b1 frame b1 expressed with respect to some frame a. + * \param F_a_b2 frame b2 expressed with respect to some frame a. + * \warning The result is not a Twist! + * see diff() for Rotation and Vector arguments for further detail on the semantics. + */ +IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt=1); + +/** + * determines the difference between two twists i.e. the difference between + * the underlying velocity vectors and rotational velocity vectors. + */ +IMETHOD Twist diff(const Twist& a,const Twist& b,double dt=1); + +/** + * determines the difference between two wrenches i.e. the difference between + * the underlying torque vectors and force vectors. + */ +IMETHOD Wrench diff(const Wrench& W_a_p1,const Wrench& W_a_p2,double dt=1); + +/** + * \brief adds vector da to vector a. + * see also the corresponding diff() routine. + * \param p_w_a vector a expressed to some frame w. + * \param p_w_da vector da expressed to some frame w. + * \returns the vector resulting from the displacement of vector a by vector da, expressed in the frame w. + */ +IMETHOD Vector addDelta(const Vector& p_w_a,const Vector& p_w_da,double dt=1); + +/** + * returns the rotation matrix resulting from the rotation of frame a by the axis and angle + * specified with da_w. + * + * see also the corresponding diff() routine. + * + * \param R_w_a Rotation matrix of frame a expressed to some frame w. + * \param da_w axis and angle of the rotation expressed to some frame w. + * \returns the rotation matrix resulting from the rotation of frame a by the axis and angle + * specified with da. The resulting rotation matrix is expressed with respect to + * frame w. + */ +IMETHOD Rotation addDelta(const Rotation& R_w_a,const Vector& da_w,double dt=1); + +/** + * returns the frame resulting from the rotation of frame a by the axis and angle + * specified in da_w and the translation of the origin (also specified in da_w). + * + * see also the corresponding diff() routine. + * \param R_w_a Rotation matrix of frame a expressed to some frame w. + * \param da_w axis and angle of the rotation (da_w.rot), together with a displacement vector for the origin (da_w.vel), expressed to some frame w. + * \returns the frame resulting from the rotation of frame a by the axis and angle + * specified with da.rot, and the translation of the origin da_w.vel . The resulting frame is expressed with respect to frame w. + */ +IMETHOD Frame addDelta(const Frame& F_w_a,const Twist& da_w,double dt=1); + +/** + * \brief adds the twist da to the twist a. + * see also the corresponding diff() routine. + * \param a a twist wrt some frame + * \param da a twist difference wrt some frame + * \returns The twist (a+da) wrt the corresponding frame. + */ +IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1); + + +/** + * \brief adds the wrench da to the wrench w. + * see also the corresponding diff() routine. + * see also the corresponding diff() routine. + * \param a a wrench wrt some frame + * \param da a wrench difference wrt some frame + * \returns the wrench (a+da) wrt the corresponding frame. + */ +IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1); #ifdef KDL_INLINE // #include "vector.inl" @@ -1105,4 +1263,7 @@ IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1); +} + + #endif diff --git a/src/Mod/Robot/App/kdl_cp/frames.inl b/src/Mod/Robot/App/kdl_cp/frames.inl index e80d2b706..156acf192 100644 --- a/src/Mod/Robot/App/kdl_cp/frames.inl +++ b/src/Mod/Robot/App/kdl_cp/frames.inl @@ -25,7 +25,12 @@ * * ***************************************************************************/ -namespace KDL { +/** + * \file frames.inl + * Inlined member functions and global functions that relate to the classes in frames.cpp + * + */ + IMETHOD Vector::Vector(const Vector & arg) @@ -1114,47 +1119,15 @@ IMETHOD Rotation Rot(const Vector& axis_a_b) { ct + vt*rotvec(2)*rotvec(2) ); } - IMETHOD Vector diff(const Vector& a,const Vector& b,double dt) { return (b-a)/dt; } -/** - * \brief diff operator for displacement rotational velocity. - * - * The Vector arguments here represent a displacement rotational velocity. i.e. a rotation - * around a fixed axis for a certain angle. For this representation you cannot use diff() but - * have to use diff_displ(). - * - * \todo represent a displacement twist and displacement rotational velocity - * with another class, instead of Vector and Twist. - * \warning do not confuse displacement rotational velocities and velocities - * \warning do not confuse displacement twist and twist. - * -IMETHOD Vector diff_displ(const Vector& a,const Vector& b,double dt) { - return diff(Rot(a),Rot(b),dt); -}*/ - -/** - * \brief diff operator for displacement twist. - * - * The Twist arguments here represent a displacement twist. i.e. a rotation - * around a fixed axis for a certain angle. For this representation you cannot use diff() but - * have to use diff_displ(). - * - * \warning do not confuse displacement rotational velocities and velocities - * \warning do not confuse displacement twist and twist. - * - -IMETHOD Twist diff_displ(const Twist& a,const Twist& b,double dt) { - return Twist(diff(a.vel,b.vel,dt),diff(Rot(a.rot),Rot(b.rot),dt)); -} -*/ - IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt) { Rotation R_b1_b2(R_a_b1.Inverse()*R_a_b2); return R_a_b1 * R_b1_b2.GetRot() / dt; } + IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt) { return Twist( diff(F_a_b1.p,F_a_b2.p,dt), @@ -1194,7 +1167,7 @@ IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt) { } -/* Commented code section +/** * \brief addDelta operator for displacement rotational velocity. * * The Vector arguments here represent a displacement rotational velocity. i.e. a rotation @@ -1212,7 +1185,7 @@ IMETHOD Vector addDelta_displ(const Vector& a,const Vector&da,double dt) { return getRot(addDelta(Rot(a),da,dt)); }*/ -/* Commented code section +/** * \brief addDelta operator for displacement twist. * * The Vector arguments here represent a displacement rotational velocity. i.e. a rotation @@ -1294,7 +1267,7 @@ IMETHOD void posrandom(Frame& F) { IMETHOD bool operator==(const Frame& a,const Frame& b ) { #ifdef KDL_USE_EQUAL - return Equal(a,b,epsilon); + return Equal(a,b); #else return (a.p == b.p && a.M == b.M ); @@ -1307,7 +1280,7 @@ IMETHOD bool operator!=(const Frame& a,const Frame& b) { IMETHOD bool operator==(const Vector& a,const Vector& b) { #ifdef KDL_USE_EQUAL - return Equal(a,b,epsilon); + return Equal(a,b); #else return (a.data[0]==b.data[0]&& a.data[1]==b.data[1]&& @@ -1321,7 +1294,7 @@ IMETHOD bool operator!=(const Vector& a,const Vector& b) { IMETHOD bool operator==(const Twist& a,const Twist& b) { #ifdef KDL_USE_EQUAL - return Equal(a,b,epsilon); + return Equal(a,b); #else return (a.rot==b.rot && a.vel==b.vel ); @@ -1334,7 +1307,7 @@ IMETHOD bool operator!=(const Twist& a,const Twist& b) { IMETHOD bool operator==(const Wrench& a,const Wrench& b ) { #ifdef KDL_USE_EQUAL - return Equal(a,b,epsilon); + return Equal(a,b); #else return (a.force==b.force && a.torque==b.torque ); @@ -1350,7 +1323,7 @@ IMETHOD bool operator!=(const Rotation& a,const Rotation& b) { IMETHOD bool operator==(const Vector2& a,const Vector2& b) { #ifdef KDL_USE_EQUAL - return Equal(a,b,epsilon); + return Equal(a,b); #else return (a.data[0]==b.data[0]&& a.data[1]==b.data[1] ); @@ -1361,6 +1334,3 @@ IMETHOD bool operator!=(const Vector2& a,const Vector2& b) { return !operator==(a,b); } -} // namespace KDL - - diff --git a/src/Mod/Robot/App/kdl_cp/framevel.hpp b/src/Mod/Robot/App/kdl_cp/framevel.hpp index ee8616738..8573d33f8 100644 --- a/src/Mod/Robot/App/kdl_cp/framevel.hpp +++ b/src/Mod/Robot/App/kdl_cp/framevel.hpp @@ -52,7 +52,7 @@ IMETHOD void posrandom(doubleVel& F) { posrandom(F.grad); } -} //namespace KDL +} template <> struct Traits { @@ -67,6 +67,20 @@ class VectorVel; class FrameVel; class RotationVel; +// Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4) +IMETHOD bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon); +IMETHOD bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon); +IMETHOD bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon); +IMETHOD bool Equal(const RotationVel& r1,const RotationVel& r2,double eps=epsilon); +IMETHOD bool Equal(const Rotation& r1,const RotationVel& r2,double eps=epsilon); +IMETHOD bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon); +IMETHOD bool Equal(const FrameVel& r1,const FrameVel& r2,double eps=epsilon); +IMETHOD bool Equal(const Frame& r1,const FrameVel& r2,double eps=epsilon); +IMETHOD bool Equal(const FrameVel& r1,const Frame& r2,double eps=epsilon); +IMETHOD bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon); +IMETHOD bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon); +IMETHOD bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon); + class VectorVel // = TITLE // An VectorVel is a Vector and its first derivative @@ -369,12 +383,12 @@ IMETHOD void posrandom(FrameVel& F) { posrandom(F.p); } -} // namespace KDL - #ifdef KDL_INLINE #include "framevel.inl" #endif +} // namespace + #endif diff --git a/src/Mod/Robot/App/kdl_cp/framevel.inl b/src/Mod/Robot/App/kdl_cp/framevel.inl index 1def47fd1..61a43fdc2 100644 --- a/src/Mod/Robot/App/kdl_cp/framevel.inl +++ b/src/Mod/Robot/App/kdl_cp/framevel.inl @@ -16,7 +16,6 @@ * $Name: $ ****************************************************************************/ -namespace KDL { // Methods and operators related to FrameVelVel // They all delegate most of the work to RotationVelVel and VectorVelVel @@ -533,5 +532,3 @@ Twist TwistVel::GetTwist() const { Twist TwistVel::GetTwistDot() const { return Twist(vel.v,rot.v); } - -} // namespace KDL diff --git a/src/Mod/Robot/App/kdl_cp/jacobian.cpp b/src/Mod/Robot/App/kdl_cp/jacobian.cpp index 854501713..40018d6cb 100644 --- a/src/Mod/Robot/App/kdl_cp/jacobian.cpp +++ b/src/Mod/Robot/App/kdl_cp/jacobian.cpp @@ -23,7 +23,7 @@ namespace KDL { - USING_PART_OF_NAMESPACE_EIGEN + using namespace Eigen; Jacobian::Jacobian() { @@ -126,12 +126,12 @@ namespace KDL bool Jacobian::operator ==(const Jacobian& arg)const { - return Equal((*this),arg,epsilon); + return Equal((*this),arg); } bool Jacobian::operator!=(const Jacobian& arg)const { - return !Equal((*this),arg,epsilon); + return !Equal((*this),arg); } bool Equal(const Jacobian& a,const Jacobian& b,double eps) @@ -147,8 +147,8 @@ namespace KDL } void Jacobian::setColumn(unsigned int i,const Twist& t){ - data.col(i).start<3>()=Eigen::Map(t.vel.data); - data.col(i).end<3>()=Eigen::Map(t.rot.data); + data.col(i).head<3>()=Eigen::Map(t.vel.data); + data.col(i).tail<3>()=Eigen::Map(t.rot.data); } } diff --git a/src/Mod/Robot/App/kdl_cp/jacobian.hpp b/src/Mod/Robot/App/kdl_cp/jacobian.hpp index 5627365f0..35b491b08 100644 --- a/src/Mod/Robot/App/kdl_cp/jacobian.hpp +++ b/src/Mod/Robot/App/kdl_cp/jacobian.hpp @@ -27,12 +27,19 @@ namespace KDL { + // Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4) + class Jacobian; + bool Equal(const Jacobian& a,const Jacobian& b,double eps=epsilon); + + class Jacobian { public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix data; Jacobian(); - Jacobian(unsigned int nr_of_columns); + explicit Jacobian(unsigned int nr_of_columns); Jacobian(const Jacobian& arg); ///Allocates memory for new size (can break realtime behavior) diff --git a/src/Mod/Robot/App/kdl_cp/jntarray.cpp b/src/Mod/Robot/App/kdl_cp/jntarray.cpp index 179472586..02358113d 100644 --- a/src/Mod/Robot/App/kdl_cp/jntarray.cpp +++ b/src/Mod/Robot/App/kdl_cp/jntarray.cpp @@ -23,7 +23,7 @@ namespace KDL { - USING_PART_OF_NAMESPACE_EIGEN + using namespace Eigen; JntArray::JntArray() { @@ -101,7 +101,7 @@ namespace KDL void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest) { - Eigen::Matrix t=(jac.data*src.data).lazy(); + Eigen::Matrix t=jac.data.lazyProduct(src.data); dest=Twist(Vector(t(0),t(1),t(2)),Vector(t(3),t(4),t(5))); } diff --git a/src/Mod/Robot/App/kdl_cp/jntarray.hpp b/src/Mod/Robot/App/kdl_cp/jntarray.hpp index bb51cc2e0..e0ff388ca 100644 --- a/src/Mod/Robot/App/kdl_cp/jntarray.hpp +++ b/src/Mod/Robot/App/kdl_cp/jntarray.hpp @@ -89,7 +89,7 @@ class MyTask : public RTT::TaskContext * @post 0 < rows() * @post all elements in data have 0 value */ - JntArray(unsigned int size); + explicit JntArray(unsigned int size); /** Copy constructor * @note Will correctly copy an empty object */ @@ -139,18 +139,6 @@ class MyTask : public RTT::TaskContext friend void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest); friend void Multiply(const JntArray& src,const double& factor,JntArray& dest); friend void Divide(const JntArray& src,const double& factor,JntArray& dest); - /** - * Function to multiply a KDL::Jacobian with a KDL::JntArray - * to get a KDL::Twist, it should not be used to calculate the - * forward velocity kinematics, the solver classes are built - * for this purpose. - * J*q = t - * - * @param jac J - * @param src q - * @param dest t - * @post dest == (KDL::Twist::)Zero() if 0 == src.rows() (ie src is empty) - */ friend void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest); friend void SetToZero(JntArray& array); friend bool Equal(const JntArray& src1,const JntArray& src2,double eps); diff --git a/src/Mod/Robot/App/kdl_cp/jntarrayacc.hpp b/src/Mod/Robot/App/kdl_cp/jntarrayacc.hpp index dceb02187..141d1534e 100644 --- a/src/Mod/Robot/App/kdl_cp/jntarrayacc.hpp +++ b/src/Mod/Robot/App/kdl_cp/jntarrayacc.hpp @@ -29,6 +29,23 @@ namespace KDL { + // Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4) + class JntArrayAcc; + bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps=epsilon); + void Add(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest); + void Add(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest); + void Add(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest); + void Subtract(const JntArrayAcc& src1,const JntArrayAcc& src2,JntArrayAcc& dest); + void Subtract(const JntArrayAcc& src1,const JntArrayVel& src2,JntArrayAcc& dest); + void Subtract(const JntArrayAcc& src1,const JntArray& src2,JntArrayAcc& dest); + void Multiply(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest); + void Multiply(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest); + void Multiply(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest); + void Divide(const JntArrayAcc& src,const double& factor,JntArrayAcc& dest); + void Divide(const JntArrayAcc& src,const doubleVel& factor,JntArrayAcc& dest); + void Divide(const JntArrayAcc& src,const doubleAcc& factor,JntArrayAcc& dest); + void SetToZero(JntArrayAcc& array); + class JntArrayAcc { public: @@ -37,10 +54,10 @@ namespace KDL JntArray qdotdot; public: JntArrayAcc(){}; - JntArrayAcc(unsigned int size); + explicit JntArrayAcc(unsigned int size); JntArrayAcc(const JntArray& q,const JntArray& qdot,const JntArray& qdotdot); JntArrayAcc(const JntArray& q,const JntArray& qdot); - JntArrayAcc(const JntArray& q); + explicit JntArrayAcc(const JntArray& q); void resize(unsigned int newSize); @@ -64,6 +81,7 @@ namespace KDL friend bool Equal(const JntArrayAcc& src1,const JntArrayAcc& src2,double eps); }; + } #endif diff --git a/src/Mod/Robot/App/kdl_cp/jntarrayvel.hpp b/src/Mod/Robot/App/kdl_cp/jntarrayvel.hpp index cb06482a7..cfc27a683 100644 --- a/src/Mod/Robot/App/kdl_cp/jntarrayvel.hpp +++ b/src/Mod/Robot/App/kdl_cp/jntarrayvel.hpp @@ -28,6 +28,19 @@ namespace KDL { + // Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4) + class JntArrayVel; + bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps=epsilon); + void Add(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest); + void Add(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest); + void Subtract(const JntArrayVel& src1,const JntArrayVel& src2,JntArrayVel& dest); + void Subtract(const JntArrayVel& src1,const JntArray& src2,JntArrayVel& dest); + void Multiply(const JntArrayVel& src,const double& factor,JntArrayVel& dest); + void Multiply(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest); + void Divide(const JntArrayVel& src,const double& factor,JntArrayVel& dest); + void Divide(const JntArrayVel& src,const doubleVel& factor,JntArrayVel& dest); + void SetToZero(JntArrayVel& array); + class JntArrayVel { @@ -36,9 +49,9 @@ namespace KDL JntArray qdot; public: JntArrayVel(){}; - JntArrayVel(unsigned int size); + explicit JntArrayVel(unsigned int size); JntArrayVel(const JntArray& q,const JntArray& qdot); - JntArrayVel(const JntArray& q); + explicit JntArrayVel(const JntArray& q); void resize(unsigned int newSize); @@ -57,6 +70,7 @@ namespace KDL friend bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps); }; + } #endif diff --git a/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.cpp b/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.cpp index 3fb8a0f92..dea5cb646 100644 --- a/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.cpp +++ b/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.cpp @@ -23,7 +23,7 @@ namespace KDL { - USING_PART_OF_NAMESPACE_EIGEN + using namespace Eigen; JntSpaceInertiaMatrix::JntSpaceInertiaMatrix() { @@ -100,7 +100,7 @@ namespace KDL void Multiply(const JntSpaceInertiaMatrix& src, const JntArray& vec, JntArray& dest) { - dest.data=(src.data*vec.data).lazy(); + dest.data=src.data.lazyProduct(vec.data); } void SetToZero(JntSpaceInertiaMatrix& mat) @@ -115,7 +115,7 @@ namespace KDL return src1.data.isApprox(src2.data,eps); } - bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2,epsilon);}; + bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);}; //bool operator!=(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);}; } diff --git a/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.hpp b/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.hpp index 49a2d725c..ae4f01298 100644 --- a/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.hpp +++ b/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.hpp @@ -63,9 +63,9 @@ class MyTask : public RTT::TaskContext ** use j here } }; -\endcode +/endcode - */ + */ class JntSpaceInertiaMatrix { @@ -90,7 +90,7 @@ class MyTask : public RTT::TaskContext * @post 0 < rows() * @post all elements in data have 0 value */ - JntSpaceInertiaMatrix(int size); + explicit JntSpaceInertiaMatrix(int size); /** Copy constructor * @note Will correctly copy an empty object */ @@ -134,84 +134,99 @@ class MyTask : public RTT::TaskContext */ unsigned int columns()const; - /** - * Function to add two joint matrix, all the arguments must - * have the same size: A + B = C. This function is - * aliasing-safe, A or B can be the same array as C. - * - * @param src1 A - * @param src2 B - * @param dest C - */ friend void Add(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest); - /** - * Function to subtract two joint matrix, all the arguments must - * have the same size: A - B = C. This function is - * aliasing-safe, A or B can be the same array as C. - * - * @param src1 A - * @param src2 B - * @param dest C - */ friend void Subtract(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest); - /** - * Function to multiply all the array values with a scalar - * factor: A*b=C. This function is aliasing-safe, A can be the - * same array as C. - * - * @param src A - * @param factor b - * @param dest C - */ friend void Multiply(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest); - /** - * Function to divide all the array values with a scalar - * factor: A/b=C. This function is aliasing-safe, A can be the - * same array as C. - * - * @param src A - * @param factor b - * @param dest C - */ friend void Divide(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest); - /** - * Function to multiply a KDL::Jacobian with a KDL::JntSpaceInertiaMatrix - * to get a KDL::Twist, it should not be used to calculate the - * forward velocity kinematics, the solver classes are built - * for this purpose. - * J*q = t - * - * @param src J: Inertia Matrix (6x6) - * @param vec q: Jacobian (6x1) - * @param dest t: Twist (6x1) - * @post dest==Twist::Zero() if 0==src.rows() (ie src is empty) - */ friend void Multiply(const JntSpaceInertiaMatrix& src, const JntArray& vec, JntArray& dest); - /** - * Function to set all the values of the array to 0 - * - * @param mat - */ - friend void SetToZero(JntSpaceInertiaMatrix& mat); - /** - * Function to check if two matrices are the same with a - *precision of eps - * - * @param src1 - * @param src2 - * @param eps default: epsilon - * @return true if each element of src1 is within eps of the same - * element in src2, or if both src1 and src2 have no data (ie 0==rows()) - */ + friend void SetToZero(JntSpaceInertiaMatrix& matrix); friend bool Equal(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,double eps); - friend bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2); //friend bool operator!=(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2); - }; + }; bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2); //bool operator!=(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2); + /** + * Function to add two joint matrix, all the arguments must + * have the same size: A + B = C. This function is + * aliasing-safe, A or B can be the same array as C. + * + * @param src1 A + * @param src2 B + * @param dest C + */ + void Add(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest); + + /** + * Function to subtract two joint matrix, all the arguments must + * have the same size: A - B = C. This function is + * aliasing-safe, A or B can be the same array as C. + * + * @param src1 A + * @param src2 B + * @param dest C + */ + void Subtract(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,JntSpaceInertiaMatrix& dest); + + /** + * Function to multiply all the array values with a scalar + * factor: A*b=C. This function is aliasing-safe, A can be the + * same array as C. + * + * @param src A + * @param factor b + * @param dest C + */ + void Multiply(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest); + + /** + * Function to divide all the array values with a scalar + * factor: A/b=C. This function is aliasing-safe, A can be the + * same array as C. + * + * @param src A + * @param factor b + * @param dest C + */ + void Divide(const JntSpaceInertiaMatrix& src,const double& factor,JntSpaceInertiaMatrix& dest); + + /** + * Function to multiply a KDL::Jacobian with a KDL::JntSpaceInertiaMatrix + * to get a KDL::Twist, it should not be used to calculate the + * forward velocity kinematics, the solver classes are built + * for this purpose. + * J*q = t + * + * @param jac J + * @param src q + * @param dest t + * @post dest==Twist::Zero() if 0==src.rows() (ie src is empty) + */ + void Multiply(const JntSpaceInertiaMatrix& src, const JntArray& vec, JntArray& dest); + + /** + * Function to set all the values of the array to 0 + * + * @param array + */ + void SetToZero(JntSpaceInertiaMatrix& matrix); + + /** + * Function to check if two matrices are the same with a + *precision of eps + * + * @param src1 + * @param src2 + * @param eps default: epsilon + * @return true if each element of src1 is within eps of the same + * element in src2, or if both src1 and src2 have no data (ie 0==rows()) + */ + bool Equal(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,double eps=epsilon); + + bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2); + } #endif diff --git a/src/Mod/Robot/App/kdl_cp/joint.cpp b/src/Mod/Robot/App/kdl_cp/joint.cpp index 758fa605c..e487334b8 100644 --- a/src/Mod/Robot/App/kdl_cp/joint.cpp +++ b/src/Mod/Robot/App/kdl_cp/joint.cpp @@ -29,6 +29,7 @@ namespace KDL { name(_name),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness) { if (type == RotAxis || type == TransAxis) throw joint_type_ex; + q_previous = 0; } // constructor for joint along x,y or z axis, at origin of reference frame @@ -37,12 +38,14 @@ namespace KDL { name("NoName"),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness) { if (type == RotAxis || type == TransAxis) throw joint_type_ex; + q_previous = 0; } // constructor for joint along arbitrary axis, at arbitrary origin Joint::Joint(const std::string& _name, const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness): - name(_name), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness), axis(_axis / _axis.Norm()), origin(_origin) + name(_name), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness) + , axis(_axis / _axis.Norm()), origin(_origin) { if (type != RotAxis && type != TransAxis) throw joint_type_ex; @@ -55,7 +58,8 @@ namespace KDL { // constructor for joint along arbitrary axis, at arbitrary origin Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness): - name("NoName"), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness), axis(_axis / _axis.Norm()), origin(_origin) + name("NoName"), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness), + axis(_axis / _axis.Norm()),origin(_origin) { if (type != RotAxis && type != TransAxis) throw joint_type_ex; @@ -71,42 +75,32 @@ namespace KDL { Frame Joint::pose(const double& q)const { - switch(type){ - case RotAxis:{ + case RotAxis: // calculate the rotation matrix around the vector "axis" if (q != q_previous){ q_previous = q; joint_pose.M = Rotation::Rot2(axis, scale*q+offset); } return joint_pose; - break;} case RotX: return Frame(Rotation::RotX(scale*q+offset)); - break; case RotY: return Frame(Rotation::RotY(scale*q+offset)); - break; case RotZ: return Frame(Rotation::RotZ(scale*q+offset)); - break; case TransAxis: return Frame(origin + (axis * (scale*q+offset))); - break; case TransX: return Frame(Vector(scale*q+offset,0.0,0.0)); - break; case TransY: return Frame(Vector(0.0,scale*q+offset,0.0)); - break; case TransZ: return Frame(Vector(0.0,0.0,scale*q+offset)); - break; case None: - default: return Frame::Identity(); - break; } + return Frame::Identity(); } Twist Joint::twist(const double& qdot)const @@ -114,33 +108,24 @@ namespace KDL { switch(type){ case RotAxis: return Twist(Vector(0,0,0), axis * ( scale * qdot)); - break; case RotX: return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0)); - break; case RotY: return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0)); - break; case RotZ: return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot)); - break; - case TransAxis: - return Twist(axis * (scale * qdot), Vector(0,0,0)); - break; + case TransAxis: + return Twist(axis * (scale * qdot), Vector(0,0,0)); case TransX: return Twist(Vector(scale*qdot,0.0,0.0),Vector(0.0,0.0,0.0)); - break; case TransY: return Twist(Vector(0.0,scale*qdot,0.0),Vector(0.0,0.0,0.0)); - break; case TransZ: return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0)); - break; case None: - default: return Twist::Zero(); - break; } + return Twist::Zero(); } Vector Joint::JointAxis() const @@ -149,33 +134,24 @@ namespace KDL { { case RotAxis: return axis; - break; case RotX: return Vector(1.,0.,0.); - break; case RotY: return Vector(0.,1.,0.); - break; case RotZ: return Vector(0.,0.,1.); - break; case TransAxis: return axis; - break; case TransX: return Vector(1.,0.,0.); - break; case TransY: return Vector(0.,1.,0.); - break; case TransZ: return Vector(0.,0.,1.); - break; case None: - default: return Vector::Zero(); - break; } + return Vector::Zero(); } Vector Joint::JointOrigin() const diff --git a/src/Mod/Robot/App/kdl_cp/joint.hpp b/src/Mod/Robot/App/kdl_cp/joint.hpp index ab6cfbb65..a188aff95 100644 --- a/src/Mod/Robot/App/kdl_cp/joint.hpp +++ b/src/Mod/Robot/App/kdl_cp/joint.hpp @@ -59,7 +59,7 @@ namespace KDL { * @param stiffness 1D stiffness along the joint axis, * default: 0 */ - Joint(const std::string& name, const JointType& type=None,const double& scale=1,const double& offset=0, + explicit Joint(const std::string& name, const JointType& type=None,const double& scale=1,const double& offset=0, const double& inertia=0,const double& damping=0,const double& stiffness=0); /** * Constructor of a joint. @@ -74,22 +74,21 @@ namespace KDL { * @param stiffness 1D stiffness along the joint axis, * default: 0 */ - Joint(const JointType& type=None,const double& scale=1,const double& offset=0, + explicit Joint(const JointType& type=None,const double& scale=1,const double& offset=0, const double& inertia=0,const double& damping=0,const double& stiffness=0); /** * Constructor of a joint. * * @param name of the joint - * @param _origin the origin of the joint - * @param _axis the axis of the joint - * @param type type of the joint - * @param _scale scale between joint input and actual geometric + * @param origin the origin of the joint + * @param axis the axis of the joint + * @param scale scale between joint input and actual geometric * movement, default: 1 - * @param _offset offset between joint input and actual + * @param offset offset between joint input and actual * geometric input, default: 0 - * @param _inertia 1D inertia along the joint axis, default: 0 - * @param _damping 1D damping along the joint axis, default: 0 - * @param _stiffness 1D stiffness along the joint axis, + * @param inertia 1D inertia along the joint axis, default: 0 + * @param damping 1D damping along the joint axis, default: 0 + * @param stiffness 1D stiffness along the joint axis, * default: 0 */ Joint(const std::string& name, const Vector& _origin, const Vector& _axis, const JointType& type, const double& _scale=1, const double& _offset=0, @@ -97,16 +96,15 @@ namespace KDL { /** * Constructor of a joint. * - * @param _origin the origin of the joint - * @param _axis the axis of the joint - * @param type type of the joint - * @param _scale scale between joint input and actual geometric + * @param origin the origin of the joint + * @param axis the axis of the joint + * @param scale scale between joint input and actual geometric * movement, default: 1 - * @param _offset offset between joint input and actual + * @param offset offset between joint input and actual * geometric input, default: 0 - * @param _inertia 1D inertia along the joint axis, default: 0 - * @param _damping 1D damping along the joint axis, default: 0 - * @param _stiffness 1D stiffness along the joint axis, + * @param inertia 1D inertia along the joint axis, default: 0 + * @param damping 1D damping along the joint axis, default: 0 + * @param stiffness 1D stiffness along the joint axis, * default: 0 */ Joint(const Vector& _origin, const Vector& _axis, const JointType& type, const double& _scale=1, const double& _offset=0, @@ -205,7 +203,7 @@ namespace KDL { double damping; double stiffness; - // varibles for RotAxis joint + // variables for RotAxis joint Vector axis, origin; mutable Frame joint_pose; mutable double q_previous; diff --git a/src/Mod/Robot/App/kdl_cp/kdl.hpp b/src/Mod/Robot/App/kdl_cp/kdl.hpp index b7656c868..c74ede6d7 100644 --- a/src/Mod/Robot/App/kdl_cp/kdl.hpp +++ b/src/Mod/Robot/App/kdl_cp/kdl.hpp @@ -1,4 +1,5 @@ // Copyright (C) 2007 Ruben Smits +// Copyright (C) 2011 Erwin Aertbelien // Version: 1.0 // Author: Ruben Smits @@ -20,25 +21,109 @@ // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA /** - * \defgroup KDL Kinematics and Dynamics Library - * \ingroup ROBOT + * \mainpage KDL * * This is the API reference of the - * Kinematics and Dynamics Library (KDL), a sub-project of Orocos, but that can be used independently of Orocos. KDL offers different kinds of functionality: + * Kinematics and Dynamics + * Library (KDL), a sub-project of Orocos, but that can be used + * independently of Orocos. KDL offers different kinds of + * functionality, grouped in the following Modules: + * - \subpage geomprim + * - \ref KinematicFamily : functionality to build kinematic chains and access their kinematic and dynamic properties, such as e.g. Forward and Inverse kinematics and dynamics. + * - \ref Motion : functionality to specify motion trajectories of frames and kinematic chains, such as e.g. Trapezoidal Velocity profiles. + * - \ref KDLTK : the interface code to integrate KDL into the Orocos Real-Time Toolkit (RTT). * - * Geometric primitives and their transformations: + * +**/ +/** + * \page geomprim Geometric Primitives + * \section Introduction + * Geometric primitives are represented by the following classes. * - KDL::Vector * - KDL::Rotation * - KDL::Frame * - KDL::Twist * - KDL::Wrench * - * Other modules: - * - \ref KinFam : functionality to build kinematic chains and access their kinematic and dynamic properties, such as e.g. Forward and Inverse kinematics and dynamics. - * - \ref Motion : functionality to specify motion trajectories of frames and kinematic chains, such as e.g. Trapezoidal Velocity profiles. + * \par Twist and Wrench transformations + * 3 different types of transformations do exist for the twists + * and wrenches. + * + * \verbatim + * 1) Frame * Twist or Frame * Wrench : + * this transforms both the velocity/force reference point + * and the basis to which the twist/wrench are expressed. + * 2) Rotation * Twist or Rotation * Wrench : + * this transforms the basis to which the twist/wrench are + * expressed, but leaves the reference point intact. + * 3) Twist.RefPoint(v_base_AB) or Wrench.RefPoint(v_base_AB) + * this transforms only the reference point. v is expressed + * in the same base as the twist/wrench and points from the + * old reference point to the new reference point. + * \endverbatim + * + *\warning + * Efficienty can be improved by writing p2 = A*(B*(C*p1))) instead of + * p2=A*B*C*p1 + * + * \par PROPOSED NAMING CONVENTION FOR FRAME-like OBJECTS + * + * \verbatim + * A naming convention of objects of the type defined in this file : + * (1) Frame : F... + * Rotation : R ... + * (2) Twist : T ... + * Wrench : W ... + * Vector : V ... + * This prefix is followed by : + * for category (1) : + * F_A_B : w.r.t. frame A, frame B expressed + * ( each column of F_A_B corresponds to an axis of B, + * expressed w.r.t. frame A ) + * in mathematical convention : + * A + * F_A_B == F + * B + * + * for category (2) : + * V_B : a vector expressed w.r.t. frame B + * + * This can also be prepended by a name : + * e.g. : temporaryV_B + * + * With this convention one can write : + * + * F_A_B = F_B_A.Inverse(); + * F_A_C = F_A_B * F_B_C; + * V_B = F_B_C * V_C; // both translation and rotation + * V_B = R_B_C * V_C; // only rotation + * \endverbatim + * + * \par CONVENTIONS FOR WHEN USED WITH ROBOTS : + * + * \verbatim + * world : represents the frame ([1 0 0,0 1 0,0 0 1],[0 0 0]') + * mp : represents mounting plate of a robot + * (i.e. everything before MP is constructed by robot manufacturer + * everything after MP is tool ) + * tf : represents task frame of a robot + * (i.e. frame in which motion and force control is expressed) + * sf : represents sensor frame of a robot + * (i.e. frame at which the forces measured by the force sensor + * are expressed ) + * + * Frame F_world_mp=...; + * Frame F_mp_sf(..) + * Frame F_mp_tf(,.) + * + * Wrench are measured in sensor frame SF, so one could write : + * Wrench_tf = F_mp_tf.Inverse()* ( F_mp_sf * Wrench_sf ); + * \endverbatim + * + * \par CONVENTIONS REGARDING UNITS : + * Typically we use the standard S.I. units: N, m, sec. * */ -/* This code doesn't seems to be integrated with freecad -- \ref KDLTK : the interface code to integrate KDL into the Orocos Real-Time Toolkit (RTT). */ diff --git a/src/Mod/Robot/App/kdl_cp/kdl.pc.in b/src/Mod/Robot/App/kdl_cp/kdl.pc.in new file mode 100644 index 000000000..49e2ab971 --- /dev/null +++ b/src/Mod/Robot/App/kdl_cp/kdl.pc.in @@ -0,0 +1,11 @@ +prefix=@CMAKE_INSTALL_PREFIX@ +libdir=${prefix}/lib +includedir=${prefix}/include + +Name: orocos-kdl +Description: The Orocos Kinematics and Dynamics Library +Requires: +Version: @KDL_VERSION@ +Libs: -L${libdir} -lorocos-kdl +Cflags: -I${includedir} @KDL_CFLAGS@ + diff --git a/src/Mod/Robot/App/kdl_cp/kinfam.hpp b/src/Mod/Robot/App/kdl_cp/kinfam.hpp index 665b57dc2..e3eb12f9a 100644 --- a/src/Mod/Robot/App/kdl_cp/kinfam.hpp +++ b/src/Mod/Robot/App/kdl_cp/kinfam.hpp @@ -20,8 +20,7 @@ // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA /** - * @defgroup KinFam Kinematic Families - * \ingroup KDL + * @defgroup KinematicFamily Kinematic Families * @brief All classes to support kinematic families. * * The Kinematic Families classes range from the basic building blocks diff --git a/src/Mod/Robot/App/kdl_cp/kinfam_io.cpp b/src/Mod/Robot/App/kdl_cp/kinfam_io.cpp index 32bb2cbf7..590484572 100644 --- a/src/Mod/Robot/App/kdl_cp/kinfam_io.cpp +++ b/src/Mod/Robot/App/kdl_cp/kinfam_io.cpp @@ -24,7 +24,8 @@ namespace KDL { std::ostream& operator <<(std::ostream& os, const Joint& joint) { - return os << joint.getTypeName(); + return os << joint.getName()<<":["<>(std::istream& is, Joint& joint) { @@ -32,7 +33,7 @@ std::istream& operator >>(std::istream& is, Joint& joint) { } std::ostream& operator <<(std::ostream& os, const Segment& segment) { - os << "[" << segment.getJoint() << ",\n" << segment.getFrameToTip() << "]"; + os << segment.getName()<<":[" << segment.getJoint() << ",\n tip: \n" << segment.getFrameToTip() << "]"; return os; } @@ -53,15 +54,15 @@ std::istream& operator >>(std::istream& is, Chain& chain) { } std::ostream& operator <<(std::ostream& os, const Tree& tree) { - SegmentMap::const_iterator root = tree.getSegment("root"); + SegmentMap::const_iterator root = tree.getRootSegment(); return os << root; } std::ostream& operator <<(std::ostream& os, SegmentMap::const_iterator root) { //os<first<<": "<second.segment<<"\n"; - os << root->first<<"(q_nr: "<second.q_nr<<")"<<"\n \t"; - for (unsigned int i = 0; i < root->second.children.size(); i++) { - os <<(root->second.children[i])<<"\t"; + os << root->first<<"(q_nr: "<< GetTreeElementQNr(root->second) << ")" << "\n \t"; + for (unsigned int i = 0; i < GetTreeElementChildren(root->second).size(); i++) { + os << ( GetTreeElementChildren(root->second)[i] ) << "\t"; } return os << "\n"; } diff --git a/src/Mod/Robot/App/kdl_cp/motion.hpp b/src/Mod/Robot/App/kdl_cp/motion.hpp index 38bfde7fd..7d427f72d 100644 --- a/src/Mod/Robot/App/kdl_cp/motion.hpp +++ b/src/Mod/Robot/App/kdl_cp/motion.hpp @@ -21,7 +21,6 @@ /** * @defgroup Motion Motion - * \ingroup KDL * @brief All classes related to the non-instantaneous motion of rigid * bodies and kinematic structures, e.g., path and trajecory definitions * and their building blocks. diff --git a/src/Mod/Robot/App/kdl_cp/path.hpp b/src/Mod/Robot/App/kdl_cp/path.hpp index 06e084a1e..6cffae4c1 100644 --- a/src/Mod/Robot/App/kdl_cp/path.hpp +++ b/src/Mod/Robot/App/kdl_cp/path.hpp @@ -58,7 +58,14 @@ namespace KDL { class Path { public: - + enum IdentifierType { + ID_LINE=1, + ID_CIRCLE=2, + ID_COMPOSITE=3, + ID_ROUNDED_COMPOSITE=4, + ID_POINT=5, + ID_CYCLIC_CLOSED=6 + }; /** * LengthToS() converts a physical length along the trajectory * to the parameter s used in Pos, Vel and Acc. This is used because @@ -115,6 +122,11 @@ class Path */ virtual Path* Clone() = 0; + /** + * gets an identifier indicating the type of this Path object + */ + virtual IdentifierType getIdentifier() const=0; + virtual ~Path() {} }; diff --git a/src/Mod/Robot/App/kdl_cp/path_circle.cpp b/src/Mod/Robot/App/kdl_cp/path_circle.cpp index 02c5d8341..5efd22bc8 100644 --- a/src/Mod/Robot/App/kdl_cp/path_circle.cpp +++ b/src/Mod/Robot/App/kdl_cp/path_circle.cpp @@ -65,14 +65,12 @@ Path_Circle::Path_Circle(const Frame& F_base_start, Vector x(F_base_start.p - F_base_center.p); radius = x.Normalize(); - if (radius < epsilon) - throw Error_MotionPlanning_Circle_ToSmall(); + if (radius < epsilon) throw Error_MotionPlanning_Circle_ToSmall(); Vector tmpv(V_base_p-F_base_center.p); tmpv.Normalize(); Vector z( x * tmpv); double n = z.Normalize(); - if (n < epsilon) - throw Error_MotionPlanning_Circle_No_Plane(); + if (n < epsilon) throw Error_MotionPlanning_Circle_No_Plane(); F_base_center.M = Rotation(x,z*x,z); double dist = alpha*radius; // See what has the slowest eq. motion, and adapt diff --git a/src/Mod/Robot/App/kdl_cp/path_circle.hpp b/src/Mod/Robot/App/kdl_cp/path_circle.hpp index fa3fa7150..36166cf99 100644 --- a/src/Mod/Robot/App/kdl_cp/path_circle.hpp +++ b/src/Mod/Robot/App/kdl_cp/path_circle.hpp @@ -100,6 +100,14 @@ class Path_Circle : public Path virtual Twist Acc(double s,double sd,double sdd) const; virtual Path* Clone(); virtual void Write(std::ostream& os); + + /** + * gets an identifier indicating the type of this Path object + */ + virtual IdentifierType getIdentifier() const { + return ID_CIRCLE; + } + virtual ~Path_Circle(); }; diff --git a/src/Mod/Robot/App/kdl_cp/path_composite.cpp b/src/Mod/Robot/App/kdl_cp/path_composite.cpp index 47f8d2458..62e099d51 100644 --- a/src/Mod/Robot/App/kdl_cp/path_composite.cpp +++ b/src/Mod/Robot/App/kdl_cp/path_composite.cpp @@ -53,7 +53,8 @@ namespace KDL { // you propably want to use the cached_index variable double Path_Composite::Lookup(double s) const { - + assert(s>=-1e-12); + assert(s<=pathlength+1e-12); if ( (cached_starts <=s) && ( s <= cached_ends) ) { return s - cached_starts; } @@ -92,6 +93,7 @@ double Path_Composite::PathLength() { return pathlength; } + Frame Path_Composite::Pos(double s) const { s = Lookup(s); return gv[cached_index].first->Pos(s); @@ -124,6 +126,29 @@ void Path_Composite::Write(std::ostream& os) { os << "]" << std::endl; } +int Path_Composite::GetNrOfSegments() { + return dv.size(); +} + +Path* Path_Composite::GetSegment(int i) { + assert(i>=0); + assert(i=0); + assert(iAdd( + new Path_Line(F_base_start, F_base_via, orient->Clone(), + eqradius)); + F_base_start = F_base_via; + F_base_via = F_base_point; + } else { + double d = radius / tan((PI - alpha) / 2); // tan. is guaranteed not to return zero. if (d >= abdist) - throw Error_MotionPlanning_Not_Feasible(); + throw Error_MotionPlanning_Not_Feasible(5); + if (d >= bcdist) - throw Error_MotionPlanning_Not_Feasible(); - std::auto_ptr line1 ( - new Path_Line(F_base_start,F_base_via,orient->Clone(),eqradius) - ); - std::auto_ptr line2 ( - new Path_Line(F_base_via,F_base_point,orient->Clone(),eqradius) - ); - Frame F_base_circlestart = line1->Pos(line1->LengthToS(abdist-d)); - Frame F_base_circleend = line2->Pos(line2->LengthToS(d)); - // end of circle segment, beginning of next line - Vector V_base_t = ab*(ab*bc); + throw Error_MotionPlanning_Not_Feasible(6); + + std::auto_ptr < Path + > line1( + new Path_Line(F_base_start, F_base_via, + orient->Clone(), eqradius)); + std::auto_ptr < Path + > line2( + new Path_Line(F_base_via, F_base_point, + orient->Clone(), eqradius)); + Frame F_base_circlestart = line1->Pos(line1->LengthToS(abdist - d)); + Frame F_base_circleend = line2->Pos(line2->LengthToS(d)); + // end of circle segment, beginning of next line + Vector V_base_t = ab * (ab * bc); V_base_t.Normalize(); - comp->Add(new Path_Line( - F_base_start,F_base_circlestart,orient->Clone(),eqradius - ) - ); - comp->Add(new Path_Circle( - F_base_circlestart, - F_base_circlestart.p - V_base_t * radius, - F_base_circleend.p, - F_base_circleend.M, - PI-alpha,orient->Clone(),eqradius - ) - ); - // shift for next line - F_base_start = F_base_circleend; // end of the circle segment - F_base_via = F_base_point; + comp->Add( + new Path_Line(F_base_start, F_base_circlestart, + orient->Clone(), eqradius)); + comp->Add( + new Path_Circle(F_base_circlestart, + F_base_circlestart.p - V_base_t * radius, + F_base_circleend.p, F_base_circleend.M, alpha, + orient->Clone(), eqradius)); + // shift for next line + F_base_start = F_base_circleend; // end of the circle segment + F_base_via = F_base_point; + } } + nrofpoints++; } void Path_RoundedComposite::Finish() { - if (nrofpoints>=1) { - comp->Add(new Path_Line(F_base_start,F_base_via,orient->Clone(),eqradius)); + if (nrofpoints >= 1) { + comp->Add( + new Path_Line(F_base_start, F_base_via, orient->Clone(), + eqradius)); } } -double Path_RoundedComposite::LengthToS(double length) { +double Path_RoundedComposite::LengthToS(double length) { return comp->LengthToS(length); } + double Path_RoundedComposite::PathLength() { return comp->PathLength(); } @@ -122,18 +155,37 @@ Frame Path_RoundedComposite::Pos(double s) const { return comp->Pos(s); } -Twist Path_RoundedComposite::Vel(double s,double sd) const { - return comp->Vel(s,sd); +Twist Path_RoundedComposite::Vel(double s, double sd) const { + return comp->Vel(s, sd); } -Twist Path_RoundedComposite::Acc(double s,double sd,double sdd) const { - return comp->Acc(s,sd,sdd); +Twist Path_RoundedComposite::Acc(double s, double sd, double sdd) const { + return comp->Acc(s, sd, sdd); } -void Path_RoundedComposite::Write(std::ostream& os) { +void Path_RoundedComposite::Write(std::ostream& os) { comp->Write(os); } +int Path_RoundedComposite::GetNrOfSegments() { + return comp->GetNrOfSegments(); +} + +Path* Path_RoundedComposite::GetSegment(int i) { + return comp->GetSegment(i); +} + +double Path_RoundedComposite::GetLengthToEndOfSegment(int i) { + return comp->GetLengthToEndOfSegment(i); +} + +void Path_RoundedComposite::GetCurrentSegmentLocation(double s, + int& segment_number, double& inner_s) { + comp->GetCurrentSegmentLocation(s,segment_number,inner_s); +} + + + Path_RoundedComposite::~Path_RoundedComposite() { if (aggregate) delete orient; @@ -142,7 +194,7 @@ Path_RoundedComposite::~Path_RoundedComposite() { Path* Path_RoundedComposite::Clone() { - return comp->Clone(); + return new Path_RoundedComposite(static_cast(comp->Clone()),radius,eqradius,orient->Clone(), true, nrofpoints); } } diff --git a/src/Mod/Robot/App/kdl_cp/path_roundedcomposite.hpp b/src/Mod/Robot/App/kdl_cp/path_roundedcomposite.hpp index 49e94eb51..79c2657f5 100644 --- a/src/Mod/Robot/App/kdl_cp/path_roundedcomposite.hpp +++ b/src/Mod/Robot/App/kdl_cp/path_roundedcomposite.hpp @@ -51,8 +51,8 @@ namespace KDL { /** - * The specification of a path, composed of - * way-points with rounded corners. + * The specification of a path, composed of way-points with rounded corners. + * * @ingroup Motion */ class Path_RoundedComposite : public Path @@ -73,21 +73,38 @@ class Path_RoundedComposite : public Path int nrofpoints; bool aggregate; + + Path_RoundedComposite(Path_Composite* comp,double radius,double eqradius,RotationalInterpolation* orient, bool aggregate, int nrofpoints); + public: /** * @param radius : radius of the rounding circles - * @param _eqradius : equivalent radius to compare rotations/velocities - * @param _orient : method of rotational_interpolation interpolation - * @param _aggregate : default True + * @param eqradius : equivalent radius to compare rotations/velocities + * @param orient : method of rotational_interpolation interpolation + * @param aggregate : if true, this object will own the _orient pointer, i.e. it will delete the _orient pointer + * when the destructor of this object is called. */ - Path_RoundedComposite(double radius,double _eqradius,RotationalInterpolation* _orient, bool _aggregate=true); + Path_RoundedComposite(double radius,double eqradius,RotationalInterpolation* orient, bool aggregate=true); /** * Adds a point to this rounded composite, between to adjecent points * a Path_Line will be created, between two lines there will be * rounding with the given radius with a Path_Circle - * Can throw Error_MotionPlanning_Not_Feasible object + * + * The Error_MotionPlanning_Not_Feasible has a type (obtained by GetType) of: + * - 3101 if the eq. radius <= 0 + * - 3102 if the first segment in a rounding has zero length. + * - 3103 if the second segment in a rounding has zero length. + * - 3104 if the angle between the first and the second segment is close to M_PI. + * (meaning that the segments are on top of each other) + * - 3105 if the distance needed for the rounding is larger then the first segment. + * - 3106 if the distance needed for the rounding is larger then the second segment. + * + * @param F_base_point the pose of a new via point. + * @warning Can throw Error_MotionPlanning_Not_Feasible object + * @TODO handle the case of error type 3105 and 3106 by skipping segments, such that the class could be applied + * with points that are very close to each other. */ void Add(const Frame& F_base_point); @@ -108,6 +125,7 @@ class Path_RoundedComposite : public Path */ virtual double PathLength(); + /** * Returns the Frame at the current path length s */ @@ -137,6 +155,43 @@ class Path_RoundedComposite : public Path */ virtual void Write(std::ostream& os); + /** + * returns the number of underlying segments. + */ + virtual int GetNrOfSegments(); + + /** + * returns a pointer to the underlying Path of the given segment number i. + * \param i segment number + * \return pointer to the underlying Path + * \warning The pointer is still owned by this class and is lifetime depends on the lifetime + * of this class. + */ + virtual Path* GetSegment(int i); + + /** + * gets the length to the end of the given segment. + * \param i segment number + * \return length to the end of the segment, i.e. the value for s corresponding to the end of + * this segment. + */ + virtual double GetLengthToEndOfSegment(int i); + + /** + * \param s [INPUT] path length variable for the composite. + * \param segment_number [OUTPUT] segments that corresponds to the path length variable s. + * \param inner_s [OUTPUT] path length to use within the segment. + */ + virtual void GetCurrentSegmentLocation(double s, int &segment_number, double& inner_s); + + /** + * gets an identifier indicating the type of this Path object + */ + virtual IdentifierType getIdentifier() const { + return ID_ROUNDED_COMPOSITE; + } + + virtual ~Path_RoundedComposite(); }; diff --git a/src/Mod/Robot/App/kdl_cp/rigidbodyinertia.cpp b/src/Mod/Robot/App/kdl_cp/rigidbodyinertia.cpp index 904b6de7e..2313fce56 100644 --- a/src/Mod/Robot/App/kdl_cp/rigidbodyinertia.cpp +++ b/src/Mod/Robot/App/kdl_cp/rigidbodyinertia.cpp @@ -30,15 +30,15 @@ namespace KDL{ const static bool mhi=true; RigidBodyInertia::RigidBodyInertia(double m_,const Vector& h_,const RotationalInertia& I_,bool mhi): - m(m_),I(I_),h(h_) + m(m_),h(h_),I(I_) { } RigidBodyInertia::RigidBodyInertia(double m_, const Vector& c_, const RotationalInertia& Ic): m(m_),h(m*c_){ //I=Ic-c x c x - Vector3d c_eig=Map(c_.data); - Map(I.data)=Map(Ic.data)-m_*(c_eig*c_eig.transpose()-c_eig.dot(c_eig)*Matrix3d::Identity()); + Vector3d c_eig=Map(c_.data); + Map(I.data)=Map(Ic.data)-m_*(c_eig*c_eig.transpose()-c_eig.dot(c_eig)*Matrix3d::Identity()); } RigidBodyInertia operator*(double a,const RigidBodyInertia& I){ @@ -60,13 +60,13 @@ namespace KDL{ //Ib = R(Ia+r x h x + (h-m*r) x r x)R' Vector hmr = (I.h-I.m*X.p); Vector3d r_eig = Map(X.p.data); - Vector3d h_eig = Map(I.h.data); + Vector3d h_eig = Map(I.h.data); Vector3d hmr_eig = Map(hmr.data); Matrix3d rcrosshcross = h_eig *r_eig.transpose()-r_eig.dot(h_eig)*Matrix3d::Identity(); Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Matrix3d::Identity(); Matrix3d R = Map(X.M.data); RotationalInertia Ib; - Map(Ib.data) = R*((Map(I.I.data)+rcrosshcross+hmrcrossrcross)*R.transpose()); + Map(Ib.data) = R*((Map(I.I.data)+rcrosshcross+hmrcrossrcross)*R.transpose()); return RigidBodyInertia(I.m,T.M*hmr,Ib,mhi); } @@ -75,9 +75,9 @@ namespace KDL{ //mb=ma //hb=R*h //Ib = R(Ia)R' with r=0 - Matrix3d R = Map(M.data); + Matrix3d R = Map(M.data); RotationalInertia Ib; - Map(Ib.data) = R.transpose()*(Map(I.I.data)*R); + Map(Ib.data) = R.transpose()*(Map(I.I.data)*R); return RigidBodyInertia(I.m,M*I.h,Ib,mhi); } @@ -87,7 +87,7 @@ namespace KDL{ //hb=(h-m*r) //Ib = (Ia+r x h x + (h-m*r) x r x) Vector hmr = (this->h-this->m*p); - Vector3d r_eig = Map(p.data); + Vector3d r_eig = Map(p.data); Vector3d h_eig = Map(this->h.data); Vector3d hmr_eig = Map(hmr.data); Matrix3d rcrosshcross = h_eig * r_eig.transpose()-r_eig.dot(h_eig)*Matrix3d::Identity(); diff --git a/src/Mod/Robot/App/kdl_cp/rigidbodyinertia.hpp b/src/Mod/Robot/App/kdl_cp/rigidbodyinertia.hpp index 427e55239..8992c4f9a 100644 --- a/src/Mod/Robot/App/kdl_cp/rigidbodyinertia.hpp +++ b/src/Mod/Robot/App/kdl_cp/rigidbodyinertia.hpp @@ -41,7 +41,7 @@ namespace KDL { * This constructor creates a cartesian space inertia matrix, * the arguments are the mass, the vector from the reference point to cog and the rotational inertia in the cog. */ - RigidBodyInertia(double m=0, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero()); + explicit RigidBodyInertia(double m=0, const Vector& oc=Vector::Zero(), const RotationalInertia& Ic=RotationalInertia::Zero()); /** * Creates an inertia with zero mass, and zero RotationalInertia @@ -53,31 +53,10 @@ namespace KDL { ~RigidBodyInertia(){}; - /** - * Scalar product: I_new = double * I_old - */ friend RigidBodyInertia operator*(double a,const RigidBodyInertia& I); - /** - * addition I: I_new = I_old1 + I_old2, make sure that I_old1 - * and I_old2 are expressed in the same reference frame/point, - * otherwise the result is worth nothing - */ friend RigidBodyInertia operator+(const RigidBodyInertia& Ia,const RigidBodyInertia& Ib); - - /** - * calculate spatial momentum: h = I*v - * make sure that the twist v and the inertia are expressed in the same reference frame/point - */ friend Wrench operator*(const RigidBodyInertia& I,const Twist& t); - - /** - * Coordinate system transform Ia = T_a_b*Ib with T_a_b the frame from a to b. - */ friend RigidBodyInertia operator*(const Frame& T,const RigidBodyInertia& I); - /** - * Reference frame orientation change Ia = R_a_b*Ib with R_a_b - * the rotation of b expressed in a - */ friend RigidBodyInertia operator*(const Rotation& R,const RigidBodyInertia& I); /** @@ -111,12 +90,42 @@ namespace KDL { private: RigidBodyInertia(double m,const Vector& h,const RotationalInertia& I,bool mhi); double m; - RotationalInertia I; Vector h; + RotationalInertia I; friend class ArticulatedBodyInertia; }; + + /** + * Scalar product: I_new = double * I_old + */ + RigidBodyInertia operator*(double a,const RigidBodyInertia& I); + /** + * addition I: I_new = I_old1 + I_old2, make sure that I_old1 + * and I_old2 are expressed in the same reference frame/point, + * otherwise the result is worth nothing + */ + RigidBodyInertia operator+(const RigidBodyInertia& Ia,const RigidBodyInertia& Ib); + + /** + * calculate spatial momentum: h = I*v + * make sure that the twist v and the inertia are expressed in the same reference frame/point + */ + Wrench operator*(const RigidBodyInertia& I,const Twist& t); + + /** + * Coordinate system transform Ia = T_a_b*Ib with T_a_b the frame from a to b. + */ + RigidBodyInertia operator*(const Frame& T,const RigidBodyInertia& I); + /** + * Reference frame orientation change Ia = R_a_b*Ib with R_a_b + * the rotation of b expressed in a + */ + RigidBodyInertia operator*(const Rotation& R,const RigidBodyInertia& I); + + + }//namespace diff --git a/src/Mod/Robot/App/kdl_cp/rotational_interpolation.hpp b/src/Mod/Robot/App/kdl_cp/rotational_interpolation.hpp index 29a6d1ec5..ced4914bb 100644 --- a/src/Mod/Robot/App/kdl_cp/rotational_interpolation.hpp +++ b/src/Mod/Robot/App/kdl_cp/rotational_interpolation.hpp @@ -105,7 +105,7 @@ class RotationalInterpolation static RotationalInterpolation* Read(std::istream& is); /** - * virtual constructor, construction by copying . + * virtual constructor, construction by copying .. */ virtual RotationalInterpolation* Clone() const = 0; diff --git a/src/Mod/Robot/App/kdl_cp/rotational_interpolation_sa.hpp b/src/Mod/Robot/App/kdl_cp/rotational_interpolation_sa.hpp index be18df147..b8a2840dc 100644 --- a/src/Mod/Robot/App/kdl_cp/rotational_interpolation_sa.hpp +++ b/src/Mod/Robot/App/kdl_cp/rotational_interpolation_sa.hpp @@ -56,8 +56,8 @@ namespace KDL { * An interpolation algorithm which rotates a frame over the existing * single rotation axis * formed by start and end rotation. If more than one rotational axis - * exist, an arbitrary one will be choosen, therefore it is better to - * not try to interpolate a 180 degrees rotation (but it will 'work'). + * exist, an arbitrary one will be choosen, therefore it is not recommended + * to try to interpolate a 180 degrees rotation. * @ingroup Motion */ class RotationalInterpolation_SingleAxis: public RotationalInterpolation diff --git a/src/Mod/Robot/App/kdl_cp/rotationalinertia.cpp b/src/Mod/Robot/App/kdl_cp/rotationalinertia.cpp index 780d64693..5210d6c72 100644 --- a/src/Mod/Robot/App/kdl_cp/rotationalinertia.cpp +++ b/src/Mod/Robot/App/kdl_cp/rotationalinertia.cpp @@ -43,19 +43,19 @@ namespace KDL Vector RotationalInertia::operator*(const Vector& omega) const { // Complexity : 9M+6A Vector result; - Map(result.data)=Map(this->data)*Map(omega.data); + Map(result.data)=Map(this->data)*Map(omega.data); return result; } RotationalInertia operator*(double a, const RotationalInertia& I){ RotationalInertia result; - Map(result.data)=a*Map(I.data); + Map(result.data)=a*Map(I.data); return result; } RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib){ RotationalInertia result; - Map(result.data)=Map(Ia.data)+Map(Ib.data); + Map(result.data)=Map(Ia.data)+Map(Ib.data); return result; } } diff --git a/src/Mod/Robot/App/kdl_cp/rotationalinertia.hpp b/src/Mod/Robot/App/kdl_cp/rotationalinertia.hpp index 5a3193379..c3c21c48d 100644 --- a/src/Mod/Robot/App/kdl_cp/rotationalinertia.hpp +++ b/src/Mod/Robot/App/kdl_cp/rotationalinertia.hpp @@ -34,7 +34,7 @@ namespace KDL class RotationalInertia{ public: - RotationalInertia(double Ixx=0,double Iyy=0,double Izz=0,double Ixy=0,double Ixz=0,double Iyz=0); + explicit RotationalInertia(double Ixx=0,double Iyy=0,double Izz=0,double Ixy=0,double Ixz=0,double Iyz=0); static inline RotationalInertia Zero(){ return RotationalInertia(0,0,0,0,0,0); @@ -65,6 +65,9 @@ namespace KDL double data[9]; }; + RotationalInertia operator*(double a, const RotationalInertia& I); + RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib); + } #endif diff --git a/src/Mod/Robot/App/kdl_cp/segment.hpp b/src/Mod/Robot/App/kdl_cp/segment.hpp index 0532c13d4..2ea46664a 100644 --- a/src/Mod/Robot/App/kdl_cp/segment.hpp +++ b/src/Mod/Robot/App/kdl_cp/segment.hpp @@ -60,9 +60,9 @@ namespace KDL { * Joint(Joint::None) * @param f_tip frame from the end of the joint to the tip of * the segment, default: Frame::Identity() - * @param I rigid body inertia of the segment, default: Inertia::Zero() + * @param M rigid body inertia of the segment, default: Inertia::Zero() */ - Segment(const std::string& name, const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); + explicit Segment(const std::string& name, const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); /** * Constructor of the segment * @@ -70,9 +70,9 @@ namespace KDL { * Joint(Joint::None) * @param f_tip frame from the end of the joint to the tip of * the segment, default: Frame::Identity() - * @param I rigid body inertia of the segment, default: Inertia::Zero() + * @param M rigid body inertia of the segment, default: Inertia::Zero() */ - Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); + explicit Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); Segment(const Segment& in); Segment& operator=(const Segment& arg); @@ -144,11 +144,12 @@ namespace KDL { * Request the pose from the joint end to the tip of the *segment. * - * @return const reference to the joint end - segment tip pose. + * @return the original parent end - segment end pose. */ - const Frame& getFrameToTip()const + Frame getFrameToTip()const { - return f_tip; + + return joint.pose(0)*f_tip; } }; diff --git a/src/Mod/Robot/App/kdl_cp/solveri.hpp b/src/Mod/Robot/App/kdl_cp/solveri.hpp new file mode 100644 index 000000000..46948c635 --- /dev/null +++ b/src/Mod/Robot/App/kdl_cp/solveri.hpp @@ -0,0 +1,129 @@ +// Copyright (C) 2013 Stephen Roderick + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef __SOLVERI_HPP +#define __SOLVERI_HPP + +namespace KDL { + +/** + * Solver interface supporting storage and description of the latest error. + * + * Error codes: Zero (0) indicates no error, positive error codes indicate more + * of a warning (e.g. a degraded solution, but motion can continue), and + * negative error codes indicate failure (e.g. a singularity, and motion + * can not continue). + * + * Error codes between -99 and +99 (inclusive) are reserved for system-wide + * error codes. Derived classes should use values > +100, and < -100. + * + * Example use + * + * \code + * class MySolver : public SolverI + * { + * public: + * static const int E_CHILDFAILD = xxx; + * + * MySolver(SomeOtherSolver& other); + * virtual ~MySolver(); + * int CartToJnt(...); + * virtual const char* strError(const int error) const; + * protected: + * SomeOtherSolver& child; + * }; + * + * ... + * + * int MySolver::CartToJnt(...) + * { + * error = child->SomeCall(); + * if (E_NOERROR != error) { + * error = E_CHILDFAILED; + * } else { + * ... + * } + * return error; + * } + * + * const char* MySolver::strError(const int error) const + * { + * if (E_CHILDFAILED == error) return "Child solver failed"; + * else return SolverI::strError(error); + * } + * + * void someFunc() + * { + * SomeOtherSolver child = new SomeOtherSolver(...); + * MySolver parent = new MySolver(child); + * ... + * int rc = parent->CartToJnt(...); + * if (E_NOERROR != rc) { + * if (MySolver::E_CHILDFAILED == rc) { + * rc = child->getError(); + * // cope with child failure 'rc' + * } + * } + * ... + * } + * \endcode + */ +class SolverI +{ +public: + enum { + /// Converged but degraded solution (e.g. WDLS with psuedo-inverse singular) + E_DEGRADED = +1, + //! No error + E_NOERROR = 0, + //! Failed to converge + E_NO_CONVERGE = -1, + //! Undefined value (e.g. computed a NAN, or tan(90 degrees) ) + E_UNDEFINED = -2 + }; + + /// Initialize latest error to E_NOERROR + SolverI() : + error(E_NOERROR) + {} + + virtual ~SolverI() + {} + + /// Return the latest error + virtual int getError() const { return error; } + + /** Return a description of the latest error + \return if \a error is known then a description of \a error, otherwise + "UNKNOWN ERROR" + */ + virtual const char* strError(const int error) const + { + if (E_NOERROR == error) return "No error"; + else if (E_NO_CONVERGE == error) return "Failed to converge"; + else if (E_UNDEFINED == error) return "Undefined value"; + else if (E_DEGRADED == error) return "Converged but degraded solution"; + else return "UNKNOWN ERROR"; + } + +protected: + /// Latest error, initialized to E_NOERROR in constructor + int error; +}; + +} // namespaces + +#endif diff --git a/src/Mod/Robot/App/kdl_cp/trajectory.hpp b/src/Mod/Robot/App/kdl_cp/trajectory.hpp index 714e1a13d..f633a0ce3 100644 --- a/src/Mod/Robot/App/kdl_cp/trajectory.hpp +++ b/src/Mod/Robot/App/kdl_cp/trajectory.hpp @@ -78,12 +78,6 @@ namespace KDL { class Trajectory { public: - virtual Path* GetPath() = 0; - // The underlying Path - - virtual VelocityProfile* GetProfile() = 0; - // The velocity profile - virtual double Duration() const = 0; // The duration of the trajectory diff --git a/src/Mod/Robot/App/kdl_cp/trajectory_composite.cpp b/src/Mod/Robot/App/kdl_cp/trajectory_composite.cpp index a4fca3de4..82f4851b6 100644 --- a/src/Mod/Robot/App/kdl_cp/trajectory_composite.cpp +++ b/src/Mod/Robot/App/kdl_cp/trajectory_composite.cpp @@ -23,7 +23,6 @@ namespace KDL { Trajectory_Composite::Trajectory_Composite():duration(0.0) { - path = new Path_Composite(); } double Trajectory_Composite::Duration() const{ @@ -72,7 +71,7 @@ namespace KDL { Twist Trajectory_Composite::Acc(double time) const { // not optimal, could be done in log(#elem) - unsigned int i; + unsigned int i; Trajectory* traj; double previoustime; if (time < 0) { @@ -93,7 +92,6 @@ namespace KDL { vt.insert(vt.end(),elem); duration += elem->Duration(); vd.insert(vd.end(),duration); - path->Add(elem->GetPath(),false); } void Trajectory_Composite::Destroy() { @@ -103,8 +101,6 @@ namespace KDL { } vt.erase(vt.begin(),vt.end()); vd.erase(vd.begin(),vd.end()); - - delete path; } Trajectory_Composite::~Trajectory_Composite() { @@ -129,16 +125,6 @@ namespace KDL { return comp; } - Path* Trajectory_Composite::GetPath() - { - return path; - } - - VelocityProfile* Trajectory_Composite::GetProfile() - { - return 0; - } - } diff --git a/src/Mod/Robot/App/kdl_cp/trajectory_composite.hpp b/src/Mod/Robot/App/kdl_cp/trajectory_composite.hpp index 6f27cc1a1..a110e390f 100644 --- a/src/Mod/Robot/App/kdl_cp/trajectory_composite.hpp +++ b/src/Mod/Robot/App/kdl_cp/trajectory_composite.hpp @@ -37,7 +37,6 @@ class Trajectory_Composite: public Trajectory VectorDouble vd; // contains end time for each Trajectory double duration; // total duration of the composed // Trajectory - Path_Composite* path; public: Trajectory_Composite(); @@ -54,11 +53,6 @@ class Trajectory_Composite: public Trajectory virtual void Destroy(); virtual void Write(std::ostream& os) const; virtual Trajectory* Clone() const; - virtual Path* GetPath(); - virtual VelocityProfile* GetProfile(); - - // access the single members - Trajectory *Get(unsigned int n){return vt[n];} virtual ~Trajectory_Composite(); }; diff --git a/src/Mod/Robot/App/kdl_cp/trajectory_segment.cpp b/src/Mod/Robot/App/kdl_cp/trajectory_segment.cpp index b0a59398d..f0090e89d 100644 --- a/src/Mod/Robot/App/kdl_cp/trajectory_segment.cpp +++ b/src/Mod/Robot/App/kdl_cp/trajectory_segment.cpp @@ -59,15 +59,6 @@ Trajectory_Segment::Trajectory_Segment(Path* _geom, VelocityProfile* _motprof, d motprof->SetProfileDuration(0, geom->PathLength(), _duration); } -Path* Trajectory_Segment::GetPath() -{ - return geom; -} - -VelocityProfile* Trajectory_Segment::GetProfile() -{ - return motprof; -} double Trajectory_Segment::Duration() const { @@ -106,5 +97,13 @@ Trajectory_Segment::~Trajectory_Segment() delete geom; } } +Path* Trajectory_Segment::GetPath() { + return geom; +} + +VelocityProfile* Trajectory_Segment::GetProfile() { + return motprof; +} + } diff --git a/src/Mod/Robot/App/kdl_cp/trajectory_segment.hpp b/src/Mod/Robot/App/kdl_cp/trajectory_segment.hpp index 2f7590fe4..8d7250306 100644 --- a/src/Mod/Robot/App/kdl_cp/trajectory_segment.hpp +++ b/src/Mod/Robot/App/kdl_cp/trajectory_segment.hpp @@ -66,18 +66,16 @@ namespace KDL { bool aggregate; public: /** - * This constructor assumes that \a geom and \<_motprof\> are initialised correctly. + * This constructor assumes that \a geom and <_motprof> are initialised correctly. */ Trajectory_Segment(Path* _geom, VelocityProfile* _motprof, bool _aggregate=true); /** - * This constructor assumes that \a geom is initialised and \<_motprof\> needs to be + * This constructor assumes that \a geom is initialised and <_motprof> needs to be * set according to \a duration. */ Trajectory_Segment(Path* _geom, VelocityProfile* _motprof, double duration, bool _aggregate=true); - virtual Path* GetPath(); - virtual VelocityProfile* GetProfile(); virtual double Duration() const; // The duration of the trajectory @@ -98,6 +96,11 @@ namespace KDL { virtual void Write(std::ostream& os) const; + virtual Path* GetPath(); + + virtual VelocityProfile* GetProfile(); + + virtual ~Trajectory_Segment(); }; diff --git a/src/Mod/Robot/App/kdl_cp/trajectory_stationary.hpp b/src/Mod/Robot/App/kdl_cp/trajectory_stationary.hpp index 013242758..162fa4e48 100644 --- a/src/Mod/Robot/App/kdl_cp/trajectory_stationary.hpp +++ b/src/Mod/Robot/App/kdl_cp/trajectory_stationary.hpp @@ -45,6 +45,10 @@ namespace KDL { return Twist::Zero(); } virtual void Write(std::ostream& os) const; + + virtual Trajectory* Clone() const { + return new Trajectory_Stationary(duration,pos); + } virtual ~Trajectory_Stationary() {} }; diff --git a/src/Mod/Robot/App/kdl_cp/tree.cpp b/src/Mod/Robot/App/kdl_cp/tree.cpp index 3e8edbb49..1ca751489 100644 --- a/src/Mod/Robot/App/kdl_cp/tree.cpp +++ b/src/Mod/Robot/App/kdl_cp/tree.cpp @@ -26,7 +26,8 @@ namespace KDL { using namespace std; Tree::Tree(const std::string& _root_name) : - nrOfJoints(0),nrOfSegments(0),root_name(_root_name) { + nrOfJoints(0), nrOfSegments(0), root_name(_root_name) +{ segments.insert(make_pair(root_name, TreeElement::Root(root_name))); } @@ -37,7 +38,7 @@ Tree::Tree(const Tree& in) { root_name = in.root_name; segments.insert(make_pair(root_name, TreeElement::Root(root_name))); - this->addTree(in, root_name); + addTree(in, root_name); } Tree& Tree::operator=(const Tree& in) { @@ -59,12 +60,18 @@ bool Tree::addSegment(const Segment& segment, const std::string& hook_name) { pair retval; //insert new element unsigned int q_nr = segment.getJoint().getType() != Joint::None ? nrOfJoints : 0; - retval = segments.insert(make_pair(segment.getName(), TreeElement(segment, parent, q_nr))); + +#ifdef KDL_USE_NEW_TREE_INTERFACE + retval = segments.insert(make_pair(segment.getName(), TreeElementType( new TreeElement(segment, parent, q_nr)))); +#else //#ifdef KDL_USE_NEW_TREE_INTERFACE + retval = segments.insert(make_pair(segment.getName(), TreeElementType(segment, parent, q_nr))); +#endif //#ifdef KDL_USE_NEW_TREE_INTERFACE + //check if insertion succeeded if (!retval.second) return false; //add iterator to new element in parents children list - parent->second.children.push_back(retval.first); + GetTreeElementChildren(parent->second).push_back(retval.first); //increase number of segments nrOfSegments++; //increase number of joints @@ -92,10 +99,10 @@ bool Tree::addTreeRecursive(SegmentMap::const_iterator root, const std::string& //get iterator for root-segment SegmentMap::const_iterator child; //try to add all of root's children - for (unsigned int i = 0; i < root->second.children.size(); i++) { - child = root->second.children[i]; + for (unsigned int i = 0; i < GetTreeElementChildren(root->second).size(); i++) { + child = GetTreeElementChildren(root->second)[i]; //Try to add the child - if (this->addSegment(child->second.segment, hook_name)) { + if (this->addSegment(GetTreeElementSegment(child->second), hook_name)) { //if child is added, add all the child's children if (!(this->addTreeRecursive(child, child->first))) //if it didn't work, return false @@ -114,12 +121,12 @@ bool Tree::addTreeRecursive(SegmentMap::const_iterator root, const std::string& // walk down from chain_root and chain_tip to the root of the tree vector parents_chain_root, parents_chain_tip; - for (SegmentMap::const_iterator s=getSegment(chain_root); s!=segments.end(); s=s->second.parent){ + for (SegmentMap::const_iterator s=getSegment(chain_root); s!=segments.end(); s = GetTreeElementParent(s->second)){ parents_chain_root.push_back(s->first); if (s->first == root_name) break; } if (parents_chain_root.empty() || parents_chain_root.back() != root_name) return false; - for (SegmentMap::const_iterator s=getSegment(chain_tip); s!=segments.end(); s=s->second.parent){ + for (SegmentMap::const_iterator s=getSegment(chain_tip); s!=segments.end(); s = GetTreeElementParent(s->second)){ parents_chain_tip.push_back(s->first); if (s->first == root_name) break; } @@ -138,20 +145,20 @@ bool Tree::addTreeRecursive(SegmentMap::const_iterator root, const std::string& // add the segments from the root to the common frame for (unsigned int s=0; ssecond.segment; + Segment seg = GetTreeElementSegment(getSegment(parents_chain_root[s])->second); Frame f_tip = seg.pose(0.0).Inverse(); Joint jnt = seg.getJoint(); if (jnt.getType() == Joint::RotX || jnt.getType() == Joint::RotY || jnt.getType() == Joint::RotZ || jnt.getType() == Joint::RotAxis) jnt = Joint(jnt.getName(), f_tip*jnt.JointOrigin(), f_tip.M*(-jnt.JointAxis()), Joint::RotAxis); else if (jnt.getType() == Joint::TransX || jnt.getType() == Joint::TransY || jnt.getType() == Joint::TransZ || jnt.getType() == Joint::TransAxis) jnt = Joint(jnt.getName(),f_tip*jnt.JointOrigin(), f_tip.M*(-jnt.JointAxis()), Joint::TransAxis); - chain.addSegment(Segment(getSegment(parents_chain_root[s+1])->second.segment.getName(), - jnt, f_tip, getSegment(parents_chain_root[s+1])->second.segment.getInertia())); + chain.addSegment(Segment(GetTreeElementSegment(getSegment(parents_chain_root[s+1])->second).getName(), + jnt, f_tip, GetTreeElementSegment(getSegment(parents_chain_root[s+1])->second).getInertia())); } // add the segments from the common frame to the tip frame for (int s=parents_chain_tip.size()-1; s>-1; s--){ - chain.addSegment(getSegment(parents_chain_tip[s])->second.segment); + chain.addSegment(GetTreeElementSegment(getSegment(parents_chain_tip[s])->second)); } return true; } diff --git a/src/Mod/Robot/App/kdl_cp/tree.hpp b/src/Mod/Robot/App/kdl_cp/tree.hpp index 6a4d8bd47..13ff496c0 100644 --- a/src/Mod/Robot/App/kdl_cp/tree.hpp +++ b/src/Mod/Robot/App/kdl_cp/tree.hpp @@ -22,38 +22,72 @@ #ifndef KDL_TREE_HPP #define KDL_TREE_HPP +#include "config.h" + #include "segment.hpp" #include "chain.hpp" #include #include +#ifdef KDL_USE_NEW_TREE_INTERFACE +#include +#endif //#ifdef KDL_USE_NEW_TREE_INTERFACE + namespace KDL { - //Forward declaration class TreeElement; + +#ifdef KDL_USE_NEW_TREE_INTERFACE + //We use smart pointers for managing tree nodes for now becuase + //c++11 and unique_ptr support is not ubiquitous + typedef boost::shared_ptr TreeElementPtr; + typedef boost::shared_ptr TreeElementConstPtr; + typedef std::map SegmentMap; + typedef TreeElementPtr TreeElementType; + +#define GetTreeElementChildren(tree_element) (tree_element)->children +#define GetTreeElementParent(tree_element) (tree_element)->parent +#define GetTreeElementQNr(tree_element) (tree_element)->q_nr +#define GetTreeElementSegment(tree_element) (tree_element)->segment + +#else //#ifdef KDL_USE_NEW_TREE_INTERFACE + //Forward declaration typedef std::map SegmentMap; + typedef TreeElement TreeElementType; + +#define GetTreeElementChildren(tree_element) (tree_element).children +#define GetTreeElementParent(tree_element) (tree_element).parent +#define GetTreeElementQNr(tree_element) (tree_element).q_nr +#define GetTreeElementSegment(tree_element) (tree_element).segment + +#endif //#ifdef KDL_USE_NEW_TREE_INTERFACE class TreeElement { - private: - TreeElement(const std::string& name):segment(name), q_nr(0) - {}; public: + TreeElement(const Segment& segment_in,const SegmentMap::const_iterator& parent_in,unsigned int q_nr_in): + segment(segment_in), + q_nr(q_nr_in), + parent(parent_in) + {} + + static TreeElementType Root(const std::string& root_name) + { +#ifdef KDL_USE_NEW_TREE_INTERFACE + return TreeElementType(new TreeElement(root_name)); +#else //#define KDL_USE_NEW_TREE_INTERFACE + return TreeElementType(root_name); +#endif + } + Segment segment; unsigned int q_nr; SegmentMap::const_iterator parent; std::vector children; - TreeElement(const Segment& segment_in,const SegmentMap::const_iterator& parent_in,unsigned int q_nr_in) - { - q_nr=q_nr_in; - segment=segment_in; - parent=parent_in; - }; - static TreeElement Root(const std::string& root_name) - { - return TreeElement(root_name); - }; + + private: + TreeElement(const std::string& name):segment(name), q_nr(0) {} }; /** @@ -66,8 +100,8 @@ namespace KDL { private: SegmentMap segments; - int nrOfJoints; - int nrOfSegments; + unsigned int nrOfJoints; + unsigned int nrOfSegments; std::string root_name; @@ -77,7 +111,7 @@ namespace KDL /** * The constructor of a tree, a new tree is always empty */ - Tree(const std::string& root_name="root"); + explicit Tree(const std::string& root_name="root"); Tree(const Tree& in); Tree& operator= (const Tree& arg); @@ -97,7 +131,6 @@ namespace KDL * Adds a complete chain to the end of the segment with * hook_name as segment_name. * - * @param chain the chain. * @param hook_name name of the segment to connect the chain with. * * @return false if hook_name could not be found. @@ -156,7 +189,9 @@ namespace KDL }; /** - * Request the chain of the tree between chain_root and chain_tip. The chain_root must be an ancester from chain_tip + * Request the chain of the tree between chain_root and chain_tip. The chain_root + * and chain_tip can be in different branches of the tree, the chain_root can be + * an ancestor of chain_tip, and chain_tip can be an ancestor of chain_root. * * @param chain_root the name of the root segment of the chain * @param chain_tip the name of the tip segment of the chain diff --git a/src/Mod/Robot/App/kdl_cp/treefksolver.hpp b/src/Mod/Robot/App/kdl_cp/treefksolver.hpp index 92c775d6a..3d521d340 100644 --- a/src/Mod/Robot/App/kdl_cp/treefksolver.hpp +++ b/src/Mod/Robot/App/kdl_cp/treefksolver.hpp @@ -50,7 +50,6 @@ namespace KDL { * * @param q_in input joint coordinates * @param p_out reference to output cartesian pose - * @param segmentName * * @return if < 0 something went wrong */ diff --git a/src/Mod/Robot/App/kdl_cp/treefksolverpos_recursive.cpp b/src/Mod/Robot/App/kdl_cp/treefksolverpos_recursive.cpp index 0685d2844..b919b9546 100644 --- a/src/Mod/Robot/App/kdl_cp/treefksolverpos_recursive.cpp +++ b/src/Mod/Robot/App/kdl_cp/treefksolverpos_recursive.cpp @@ -40,7 +40,7 @@ namespace KDL { else if(it == tree.getSegments().end()) //if the segment name is not found return -2; else{ - p_out = recursiveFk(q_in, it); + p_out = recursiveFk(q_in, it); return 0; } } @@ -48,15 +48,15 @@ namespace KDL { Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it) { //gets the frame for the current element (segment) - const TreeElement& currentElement = it->second; - Frame currentFrame = currentElement.segment.pose(q_in(currentElement.q_nr)); - - SegmentMap::const_iterator rootIterator = tree.getSegment("root"); + const TreeElementType& currentElement = it->second; + Frame currentFrame = GetTreeElementSegment(currentElement).pose(q_in(GetTreeElementQNr(currentElement))); + + SegmentMap::const_iterator rootIterator = tree.getRootSegment(); if(it == rootIterator){ return currentFrame; } else{ - SegmentMap::const_iterator parentIt = currentElement.parent; + SegmentMap::const_iterator parentIt = GetTreeElementParent(currentElement); return recursiveFk(q_in, parentIt) * currentFrame; } } diff --git a/src/Mod/Robot/App/kdl_cp/treeiksolver.hpp b/src/Mod/Robot/App/kdl_cp/treeiksolver.hpp index e6a520068..4067772da 100644 --- a/src/Mod/Robot/App/kdl_cp/treeiksolver.hpp +++ b/src/Mod/Robot/App/kdl_cp/treeiksolver.hpp @@ -11,6 +11,7 @@ #include "tree.hpp" #include "jntarray.hpp" #include "frames.hpp" +#include namespace KDL { diff --git a/src/Mod/Robot/App/kdl_cp/treeiksolverpos_nr_jl.cpp b/src/Mod/Robot/App/kdl_cp/treeiksolverpos_nr_jl.cpp index 6d341be92..2b0b1a3c8 100644 --- a/src/Mod/Robot/App/kdl_cp/treeiksolverpos_nr_jl.cpp +++ b/src/Mod/Robot/App/kdl_cp/treeiksolverpos_nr_jl.cpp @@ -29,13 +29,10 @@ namespace KDL { const JntArray& _q_min, const JntArray& _q_max, TreeFkSolverPos& _fksolver, TreeIkSolverVel& _iksolver, unsigned int _maxiter, double _eps) : - tree(_tree), q_min(tree.getNrOfJoints()), q_max(tree.getNrOfJoints()), - iksolver(_iksolver), fksolver(_fksolver), delta_q(tree.getNrOfJoints()), - endpoints(_endpoints), maxiter(_maxiter), eps(_eps) { - - q_min = _q_min; - q_max = _q_max; - + tree(_tree), q_min(_q_min), q_max(_q_max), iksolver(_iksolver), + fksolver(_fksolver), delta_q(tree.getNrOfJoints()), + endpoints(_endpoints), maxiter(_maxiter), eps(_eps) + { for (size_t i = 0; i < endpoints.size(); i++) { frames.insert(Frames::value_type(endpoints[i], Frame::Identity())); delta_twists.insert(Twists::value_type(endpoints[i], Twist::Zero())); diff --git a/src/Mod/Robot/App/kdl_cp/treeiksolverpos_online.cpp b/src/Mod/Robot/App/kdl_cp/treeiksolverpos_online.cpp new file mode 100644 index 000000000..3e3d1318d --- /dev/null +++ b/src/Mod/Robot/App/kdl_cp/treeiksolverpos_online.cpp @@ -0,0 +1,175 @@ +// Copyright (C) 2011 PAL Robotics S.L. All rights reserved. +// Copyright (C) 2007-2008 Ruben Smits +// Copyright (C) 2008 Mikael Mayer +// Copyright (C) 2008 Julia Jesse + +// Version: 1.0 +// Author: Marcus Liebhardt +// This class has been derived from the KDL::TreeIkSolverPos_NR_JL class +// by Julia Jesse, Mikael Mayer and Ruben Smits + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "treeiksolverpos_online.hpp" + +namespace KDL { + +TreeIkSolverPos_Online::TreeIkSolverPos_Online(const double& nr_of_jnts, + const std::vector& endpoints, + const JntArray& q_min, + const JntArray& q_max, + const JntArray& q_dot_max, + const double x_dot_trans_max, + const double x_dot_rot_max, + TreeFkSolverPos& fksolver, + TreeIkSolverVel& iksolver) : + q_min_(nr_of_jnts), + q_max_(nr_of_jnts), + q_dot_max_(nr_of_jnts), + fksolver_(fksolver), + iksolver_(iksolver), + q_dot_(nr_of_jnts) +{ + q_min_ = q_min; + q_max_ = q_max; + q_dot_max_ = q_dot_max; + x_dot_trans_max_ = x_dot_trans_max; + x_dot_rot_max_ = x_dot_rot_max; + + for (size_t i = 0; i < endpoints.size(); i++) + { + frames_.insert(Frames::value_type(endpoints[i], Frame::Identity())); + delta_twists_.insert(Twists::value_type(endpoints[i], Twist::Zero())); + } +} + + +TreeIkSolverPos_Online::~TreeIkSolverPos_Online() +{} + + +double TreeIkSolverPos_Online::CartToJnt(const JntArray& q_in, const Frames& p_in, JntArray& q_out) +{ + q_out = q_in; + + // First check, if all elements in p_in are available + for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it) + if(frames_.find(f_des_it->first)==frames_.end()) + return -2; + + for (Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it) + { + // Get all iterators for this endpoint + Frames::iterator f_it = frames_.find(f_des_it->first); + Twists::iterator delta_twists_it = delta_twists_.find(f_des_it->first); + + fksolver_.JntToCart(q_out, f_it->second, f_it->first); + twist_ = diff(f_it->second, f_des_it->second); + + // Checks, if the twist (twist_) exceeds the maximum translational and/or rotational velocity + // And scales them, if necessary + enforceCartVelLimits(); + + delta_twists_it->second = twist_; + } + + double res = iksolver_.CartToJnt(q_out, delta_twists_, q_dot_); + + if(res<0) + return res; + //If we got here q_out is definitely of the right size + if(q_out.rows()!=q_min_.rows() || q_out.rows()!=q_max_.rows() || q_out.rows()!= q_dot_max_.rows()) + return -1; + + // Checks, if joint velocities (q_dot_) exceed their maximum and scales them, if necessary + enforceJointVelLimits(); + + // Integrate + Add(q_out, q_dot_, q_out); + + // Limit joint positions + for (unsigned int j = 0; j < q_min_.rows(); j++) + { + if (q_out(j) < q_min_(j)) + q_out(j) = q_min_(j); + else if (q_out(j) > q_max_(j)) + q_out(j) = q_max_(j); + } + + return res; +} + + +void TreeIkSolverPos_Online::enforceJointVelLimits() +{ + // check, if one (or more) joint velocities exceed the maximum value + // and if so, safe the biggest overshoot for scaling q_dot_ properly + // to keep the direction of the velocity vector the same + double rel_os, rel_os_max = 0.0; // relative overshoot and the biggest relative overshoot + bool max_exceeded = false; + + for (unsigned int i = 0; i < q_dot_.rows(); i++) + { + if ( q_dot_(i) > q_dot_max_(i) ) + { + max_exceeded = true; + rel_os = (q_dot_(i) - q_dot_max_(i)) / q_dot_max_(i); + if ( rel_os > rel_os_max ) + { + rel_os_max = rel_os; + } + } + else if ( q_dot_(i) < -q_dot_max_(i) ) + { + max_exceeded = true; + rel_os = (-q_dot_(i) - q_dot_max_(i)) / q_dot_max_(i); + if ( rel_os > rel_os_max) + { + rel_os_max = rel_os; + } + } + } + + // scales q_out, if one joint exceeds the maximum value + if ( max_exceeded == true ) + { + Multiply(q_dot_, ( 1.0 / ( 1.0 + rel_os_max ) ), q_dot_); + } +} + + +void TreeIkSolverPos_Online::enforceCartVelLimits() +{ + double x_dot_trans, x_dot_rot; // vector lengths + x_dot_trans = sqrt( pow(twist_.vel.x(), 2) + pow(twist_.vel.y(), 2) + pow(twist_.vel.z(), 2)); + x_dot_rot = sqrt( pow(twist_.rot.x(), 2) + pow(twist_.rot.y(), 2) + pow(twist_.rot.z(), 2)); + + if ( x_dot_trans > x_dot_trans_max_ || x_dot_rot > x_dot_rot_max_ ) + { + if ( x_dot_trans > x_dot_rot ) + { + twist_.vel = twist_.vel * ( x_dot_trans_max_ / x_dot_trans ); + twist_.rot = twist_.rot * ( x_dot_trans_max_ / x_dot_trans ); + } + else if ( x_dot_rot > x_dot_trans ) + { + twist_.vel = twist_.vel * ( x_dot_rot_max_ / x_dot_rot ); + twist_.rot = twist_.rot * ( x_dot_rot_max_ / x_dot_rot ); + } + } +} + +} // namespace + diff --git a/src/Mod/Robot/App/kdl_cp/treeiksolverpos_online.hpp b/src/Mod/Robot/App/kdl_cp/treeiksolverpos_online.hpp new file mode 100644 index 000000000..7826eb4b3 --- /dev/null +++ b/src/Mod/Robot/App/kdl_cp/treeiksolverpos_online.hpp @@ -0,0 +1,108 @@ +// Copyright (C) 2011 PAL Robotics S.L. All rights reserved. +// Copyright (C) 2007-2008 Ruben Smits +// Copyright (C) 2008 Mikael Mayer +// Copyright (C) 2008 Julia Jesse + +// Version: 1.0 +// Author: Marcus Liebhardt +// This class has been derived from the KDL::TreeIkSolverPos_NR_JL class +// by Julia Jesse, Mikael Mayer and Ruben Smits + +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. + +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// Lesser General Public License for more details. + +// You should have received a copy of the GNU Lesser General Public +// License along with this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDLTREEIKSOLVERPOS_ONLINE_HPP +#define KDLTREEIKSOLVERPOS_ONLINE_HPP + +#include +#include +#include "treeiksolver.hpp" +#include "treefksolver.hpp" + +namespace KDL { + +/** + * Implementation of a general inverse position kinematics algorithm to calculate the position transformation from + * Cartesian to joint space of a general KDL::Tree. This class has been derived from the TreeIkSolverPos_NR_JL class, + * but was modified for online solving for use in realtime systems. Thus, the calculation is only done once, + * meaning that no iteration is done, because this solver is intended to run at a high frequency. + * It enforces velocity limits in task as well as in joint space. It also takes joint limits into account. + * + * @ingroup KinematicFamily + */ +class TreeIkSolverPos_Online: public TreeIkSolverPos { +public: + /** + * Constructor of the solver, it needs the number of joints of the tree, a list of the endpoints + * you are interested in, the maximum and minimum values you want to enforce and a forward position kinematics + * solver as well as an inverse velocity kinematics solver for the calculations + * + * @param nr_of_jnts number of joints of the tree to calculate the joint positions for + * @param endpoints the list of endpoints you are interested in + * @param q_min the minimum joint positions + * @param q_max the maximum joint positions + * @param q_dot_max the maximum joint velocities + * @param x_dot_trans_max the maximum translational velocity of your endpoints + * @param x_dot_rot_max the maximum rotational velocity of your endpoints + * @param fksolver a forward position kinematics solver + * @param iksolver an inverse velocity kinematics solver + * + * @return + */ + + TreeIkSolverPos_Online(const double& nr_of_jnts, + const std::vector& endpoints, + const JntArray& q_min, + const JntArray& q_max, + const JntArray& q_dot_max, + const double x_dot_trans_max, + const double x_dot_rot_max, + TreeFkSolverPos& fksolver, + TreeIkSolverVel& iksolver); + + ~TreeIkSolverPos_Online(); + + virtual double CartToJnt(const JntArray& q_in, const Frames& p_in, JntArray& q_out); + +private: + /** + * Scales the class member KDL::JntArray q_dot_, if one (or more) joint velocity exceeds the maximum value. + * Scaling is done propotional to the biggest overshoot among all joint velocities. + */ + void enforceJointVelLimits(); + + /** + * Scales translational and rotational velocity vectors of the class member KDL::Twist twist_, + * if at least one of both exceeds the maximum value/length. + * Scaling is done propotional to the biggest overshoot among both velocities. + */ + void enforceCartVelLimits(); + + JntArray q_min_; + JntArray q_max_; + JntArray q_dot_max_; + double x_dot_trans_max_; + double x_dot_rot_max_; + TreeFkSolverPos& fksolver_; + TreeIkSolverVel& iksolver_; + JntArray q_dot_; + Twist twist_; + Frames frames_; + Twists delta_twists_; +}; + +} // namespace + +#endif /* KDLTREEIKSOLVERPOS_ONLINE_HPP */ + diff --git a/src/Mod/Robot/App/kdl_cp/treeiksolvervel_wdls.cpp b/src/Mod/Robot/App/kdl_cp/treeiksolvervel_wdls.cpp index b40abc97f..924566ab8 100644 --- a/src/Mod/Robot/App/kdl_cp/treeiksolvervel_wdls.cpp +++ b/src/Mod/Robot/App/kdl_cp/treeiksolvervel_wdls.cpp @@ -22,7 +22,8 @@ namespace KDL { Wy_U(J.rows(),J.rows()),Wq_V(J.cols(),J.cols()), t(VectorXd::Zero(J.rows())), Wy_t(VectorXd::Zero(J.rows())), qdot(VectorXd::Zero(J.cols())), - tmp(VectorXd::Zero(J.cols())),S(VectorXd::Zero(J.cols())) + tmp(VectorXd::Zero(J.cols())),S(VectorXd::Zero(J.cols())), + lambda(0) { for (size_t i = 0; i < endpoints.size(); ++i) { @@ -68,30 +69,30 @@ namespace KDL { //lets put the jacobian in the big matrix and put the twist in the big t: J.block(6*k,0, 6,tree.getNrOfJoints()) = jac_it->second.data; const Twist& twist=v_in.find(jac_it->first)->second; - t.segment(6*k,3) = Eigen::Map(twist.vel.data); - t.segment(6*k+3,3) = Eigen::Map(twist.rot.data); + t.segment(6*k,3) = Eigen::Map(twist.vel.data); + t.segment(6*k+3,3) = Eigen::Map(twist.rot.data); } ++k; } //Lets use the wdls algorithm to find the qdot: // Create the Weighted jacobian - J_Wq = (J * Wq).lazy(); - Wy_J_Wq = (Wy * J_Wq).lazy(); + J_Wq.noalias() = J * Wq; + Wy_J_Wq.noalias() = Wy * J_Wq; // Compute the SVD of the weighted jacobian int ret = svd_eigen_HH(Wy_J_Wq, U, S, V, tmp); - (void)ret; - + if (ret < 0 ) + return E_SVD_FAILED; //Pre-multiply U and V by the task space and joint space weighting matrix respectively - Wy_t = (Wy * t).lazy(); - Wq_V = (Wq * V).lazy(); + Wy_t.noalias() = Wy * t; + Wq_V.noalias() = Wq * V; // tmp = (Si*Wy*U'*y), for (unsigned int i = 0; i < J.cols(); i++) { double sum = 0.0; for (unsigned int j = 0; j < J.rows(); j++) { - if (i < Wy_t.size()) + if (i < Wy_t.rows()) sum += U(j, i) * Wy_t(j); else sum += 0.0; @@ -100,7 +101,7 @@ namespace KDL { } // x = Lx^-1*V*tmp + x - qdot_out.data = (Wq_V * tmp).lazy(); + qdot_out.data.noalias() = Wq_V * tmp; return Wy_t.norm(); } diff --git a/src/Mod/Robot/App/kdl_cp/treeiksolvervel_wdls.hpp b/src/Mod/Robot/App/kdl_cp/treeiksolvervel_wdls.hpp index 51afc6643..8cf799829 100644 --- a/src/Mod/Robot/App/kdl_cp/treeiksolvervel_wdls.hpp +++ b/src/Mod/Robot/App/kdl_cp/treeiksolvervel_wdls.hpp @@ -14,10 +14,12 @@ namespace KDL { - USING_PART_OF_NAMESPACE_EIGEN; + using namespace Eigen; class TreeIkSolverVel_wdls: public TreeIkSolverVel { public: + static const int E_SVD_FAILED = -100; //! Child SVD failed + TreeIkSolverVel_wdls(const Tree& tree, const std::vector& endpoints); virtual ~TreeIkSolverVel_wdls(); diff --git a/src/Mod/Robot/App/kdl_cp/treejnttojacsolver.cpp b/src/Mod/Robot/App/kdl_cp/treejnttojacsolver.cpp index 761a7905e..9b5ff856a 100644 --- a/src/Mod/Robot/App/kdl_cp/treejnttojacsolver.cpp +++ b/src/Mod/Robot/App/kdl_cp/treejnttojacsolver.cpp @@ -33,22 +33,22 @@ int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, const std: //Let's make the jacobian zero: SetToZero(jac); - SegmentMap::const_iterator root = tree.getSegments().find("root"); + SegmentMap::const_iterator root = tree.getRootSegment(); Frame T_total = Frame::Identity(); //Lets recursively iterate until we are in the root segment while (it != root) { //get the corresponding q_nr for this TreeElement: - unsigned int q_nr = it->second.q_nr; + unsigned int q_nr = GetTreeElementQNr(it->second); //get the pose of the segment: - Frame T_local = it->second.segment.pose(q_in(q_nr)); + Frame T_local = GetTreeElementSegment(it->second).pose(q_in(q_nr)); //calculate new T_end: T_total = T_local * T_total; //get the twist of the segment: - if (it->second.segment.getJoint().getType() != Joint::None) { - Twist t_local = it->second.segment.twist(q_in(q_nr), 1.0); + if (GetTreeElementSegment(it->second).getJoint().getType() != Joint::None) { + Twist t_local = GetTreeElementSegment(it->second).twist(q_in(q_nr), 1.0); //transform the endpoint of the local twist to the global endpoint: t_local = t_local.RefPoint(T_total.p - T_local.p); //transform the base of the twist to the endpoint @@ -57,7 +57,7 @@ int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, const std: jac.setColumn(q_nr,t_local); }//endif //goto the parent - it = it->second.parent; + it = GetTreeElementParent(it->second); }//endwhile //Change the base of the complete jacobian from the endpoint to the base changeBase(jac, T_total.M, jac); diff --git a/src/Mod/Robot/App/kdl_cp/treejnttojacsolver.hpp b/src/Mod/Robot/App/kdl_cp/treejnttojacsolver.hpp index 40977dcd5..84f92ea6d 100644 --- a/src/Mod/Robot/App/kdl_cp/treejnttojacsolver.hpp +++ b/src/Mod/Robot/App/kdl_cp/treejnttojacsolver.hpp @@ -16,7 +16,7 @@ namespace KDL { class TreeJntToJacSolver { public: - TreeJntToJacSolver(const Tree& tree); + explicit TreeJntToJacSolver(const Tree& tree); virtual ~TreeJntToJacSolver(); diff --git a/src/Mod/Robot/App/kdl_cp/utilities/error.h b/src/Mod/Robot/App/kdl_cp/utilities/error.h index 87f594834..693c3dc25 100644 --- a/src/Mod/Robot/App/kdl_cp/utilities/error.h +++ b/src/Mod/Robot/App/kdl_cp/utilities/error.h @@ -46,8 +46,8 @@ #define ERROR_H_84822 #include "utility.h" +#include #include - namespace KDL { /** @@ -180,9 +180,13 @@ public: }; class Error_MotionPlanning_Not_Feasible: public Error_MotionPlanning { + int reason; public: - virtual const char* Description() const { return "Motion Profile with requested parameters is not feasible";} - virtual int GetType() const {return 3004;} + Error_MotionPlanning_Not_Feasible(int _reason):reason(_reason) {} + virtual const char* Description() const { + return "Motion Profile with requested parameters is not feasible"; + } + virtual int GetType() const {return 3100+reason;} }; class Error_MotionPlanning_Not_Applicable: public Error_MotionPlanning { diff --git a/src/Mod/Robot/App/kdl_cp/utilities/error_stack.cxx b/src/Mod/Robot/App/kdl_cp/utilities/error_stack.cxx index d493dc20c..939421042 100644 --- a/src/Mod/Robot/App/kdl_cp/utilities/error_stack.cxx +++ b/src/Mod/Robot/App/kdl_cp/utilities/error_stack.cxx @@ -26,8 +26,8 @@ namespace KDL { // interprete error messages from I/O typedef std::stack ErrorStack; -ErrorStack errorstack; // should be in Thread Local Storage if this gets multithreaded one day... +static ErrorStack errorstack; void IOTrace(const std::string& description) { diff --git a/src/Mod/Robot/App/kdl_cp/utilities/rallNd.h b/src/Mod/Robot/App/kdl_cp/utilities/rallNd.h index c96ff7506..63596ebc8 100644 --- a/src/Mod/Robot/App/kdl_cp/utilities/rallNd.h +++ b/src/Mod/Robot/App/kdl_cp/utilities/rallNd.h @@ -10,8 +10,7 @@ * generalized to the Nth derivative ! * The efficiency is not very good for high derivatives. * This could be improved by also using Rall2d - */ -/* + * template class RallNd : public Rall1d< RallNd, RallNd, double > @@ -46,8 +45,8 @@ public: * Als je tot 3de orde een efficiente berekening kan doen, * dan kan je tot een willekeurige orde alles efficient berekenen * 0 1 2 3 - * ==\> 1 2 3 4 - * ==\> 3 4 5 6 + * ==> 1 2 3 4 + * ==> 3 4 5 6 * 4 5 6 7 * * de aangeduide berekeningen zijn niet noodzakelijk, en er is dan niets @@ -76,7 +75,7 @@ public: template <> class RallNd<2> : public Rall2d { public: - RallNd() {} /* (dwz. met evenveel numerieke operaties als een */ + RallNd() {}* (dwz. met evenveel numerieke operaties als een RallNd(const Rall2d& arg) : Rall2d(arg) {} RallNd(double value,double d[]) { diff --git a/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.cpp b/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.cpp index c0dbb4cb7..36da50914 100644 --- a/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.cpp +++ b/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.cpp @@ -30,7 +30,7 @@ namespace KDL{ const int cols = A.cols(); U.setZero(); - U.corner(Eigen::TopLeft,rows,cols)=A; + U.topLeftCorner(rows,cols)=A; int i(-1),its(-1),j(-1),jj(-1),k(-1),nm=0; int ppi(0); @@ -53,14 +53,14 @@ namespace KDL{ s += U(k,i)*U(k,i); } f=U(i,i); // f is the diag elem - assert(s>=0); + if (!(s>=0)) return -3; g = -SIGN(sqrt(s),f); h=f*g-s; U(i,i)=f-g; for (j=ppi;j=0); + if (!(s>=0)) return -5; g = -SIGN(sqrt(s),f); h=f*g-s; U(i,ppi)=f-g; - assert(h!=0); + if (!(h!=0)) return -6; for (k=ppi;k=0;i--) { if (iepsilon) { - assert(U(i,ppi)!=0); + if (!(U(i,ppi)!=0)) return -7; for (j=ppi;jepsilon) { z=1.0/z; c=f*z; diff --git a/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.hpp b/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.hpp index 4445fad22..2859648b0 100644 --- a/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.hpp +++ b/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.hpp @@ -28,7 +28,7 @@ #include #include -USING_PART_OF_NAMESPACE_EIGEN; +using namespace Eigen; namespace KDL { @@ -63,7 +63,6 @@ namespace KDL * @param V matrix(nxn) * @param tmp vector n * @param maxiter defaults to 150 - * @param epsilon defaults to 1e-300 * * @return -2 if maxiter exceeded, 0 otherwise */ diff --git a/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_Macie.hpp b/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_Macie.hpp index f3767b318..0b8240790 100644 --- a/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_Macie.hpp +++ b/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_Macie.hpp @@ -22,14 +22,40 @@ //implementation of svd according to (Maciejewski and Klein,1989) //and (Braun, Ulrey, Maciejewski and Siegel,2002) + +/** + * \file svd_eigen_Macie.hpp + * provides Maciejewski's implementation for SVD. + */ + #ifndef SVD_BOOST_MACIE #define SVD_BOOST_MACIE #include -USING_PART_OF_NAMESPACE_EIGEN +using namespace Eigen; + namespace KDL { + + /** + * svd_eigen_Macie provides Maciejewski implementation for SVD. + * + * computes the singular value decomposition of a matrix A, such that + * A=U*Sm*V + * + * (Maciejewski and Klein,1989) and (Braun, Ulrey, Maciejewski and Siegel,2002) + * + * \param A [INPUT] is an \f$m \times n\f$-matrix, where \f$ m \geq n \f$. + * \param S [OUTPUT] is an \f$n\f$-vector, representing the diagonal elements of the diagonal matrix Sm. + * \param U [INPUT/OUTPUT] is an \f$m \times m\f$ orthonormal matrix. + * \param V [INPUT/OUTPUT] is an \f$n \times n\f$ orthonormal matrix. + * \param B [TEMPORARY] is an \f$m \times n\f$ matrix used for temporary storage. + * \param tempi [TEMPORARY] is an \f$m\f$ vector used for temporary storage. + * \param thresshold [INPUT] Thresshold to determine orthogonality. + * \param toggle [INPUT] toggle this boolean variable on each call of this routine. + * \return number of sweeps. + */ int svd_eigen_Macie(const MatrixXd& A,MatrixXd& U,VectorXd& S, MatrixXd& V, MatrixXd& B, VectorXd& tempi, double treshold,bool toggle) @@ -39,7 +65,7 @@ namespace KDL unsigned int rotations=0; if(toggle){ //Calculate B from new A and previous V - B=(A*V).lazy(); + B=A.lazyProduct(V); while(rotate){ rotate=false; rotations=0; @@ -76,9 +102,9 @@ namespace KDL B.col(i) = tempi; //Apply plane rotation to columns of V - tempi.start(V.rows()) = cos*V.col(i) + sin*V.col(j); + tempi.head(V.rows()) = cos*V.col(i) + sin*V.col(j); V.col(j) = - sin*V.col(i) + cos*V.col(j); - V.col(i) = tempi.start(V.rows()); + V.col(i) = tempi.head(V.rows()); rotate=true; } @@ -103,7 +129,7 @@ namespace KDL return sweeps; }else{ //Calculate B from new A and previous U' - B =(U.transpose() * A).lazy(); + B = U.transpose().lazyProduct(A); while(rotate){ rotate=false; rotations=0; @@ -137,15 +163,15 @@ namespace KDL } //Apply plane rotation to rows of B - tempi.start(B.cols()) = cos*B.row(i) + sin*B.row(j); + tempi.head(B.cols()) = cos*B.row(i) + sin*B.row(j); B.row(j) = - sin*B.row(i) + cos*B.row(j); - B.row(i) = tempi.start(B.cols()); + B.row(i) = tempi.head(B.cols()); //Apply plane rotation to rows of U - tempi.start(U.rows()) = cos*U.col(i) + sin*U.col(j); + tempi.head(U.rows()) = cos*U.col(i) + sin*U.col(j); U.col(j) = - sin*U.col(i) + cos*U.col(j); - U.col(i) = tempi.start(U.rows()); + U.col(i) = tempi.head(U.rows()); rotate=true; } diff --git a/src/Mod/Robot/App/kdl_cp/utilities/utility.cxx b/src/Mod/Robot/App/kdl_cp/utilities/utility.cxx index 713bdac6e..c79816728 100644 --- a/src/Mod/Robot/App/kdl_cp/utilities/utility.cxx +++ b/src/Mod/Robot/App/kdl_cp/utilities/utility.cxx @@ -1,4 +1,4 @@ -/** @file utility.cxx +/** @file utility.cpp * @author Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * @version * ORO_Geometry V0.2 diff --git a/src/Mod/Robot/App/kdl_cp/velocityprofile_spline.cpp b/src/Mod/Robot/App/kdl_cp/velocityprofile_spline.cpp new file mode 100644 index 000000000..ec651ed9b --- /dev/null +++ b/src/Mod/Robot/App/kdl_cp/velocityprofile_spline.cpp @@ -0,0 +1,194 @@ + +#include + +#include "velocityprofile_spline.hpp" + +namespace KDL { + +static inline void generatePowers(int n, double x, double* powers) +{ + powers[0] = 1.0; + for (int i=1; i<=n; i++) + { + powers[i] = powers[i-1]*x; + } + return; +} + +VelocityProfile_Spline::VelocityProfile_Spline() +{ + duration_ = 0.0; + + coeff_[0] = 0.0; + coeff_[1] = 0.0; + coeff_[2] = 0.0; + coeff_[3] = 0.0; + coeff_[4] = 0.0; + coeff_[5] = 0.0; + + return; +} + +VelocityProfile_Spline::VelocityProfile_Spline(const VelocityProfile_Spline &p) +{ + duration_ = p.duration_; + + coeff_[0] = p.coeff_[0]; + coeff_[1] = p.coeff_[1]; + coeff_[2] = p.coeff_[2]; + coeff_[3] = p.coeff_[3]; + coeff_[4] = p.coeff_[4]; + coeff_[5] = p.coeff_[5]; + + return; +} + +VelocityProfile_Spline::~VelocityProfile_Spline() +{ + return; +} + +void VelocityProfile_Spline::SetProfile(double pos1, double pos2) +{ + return; +} + +void VelocityProfile_Spline::SetProfileDuration(double pos1, double pos2, double duration) +{ + duration_ = duration; + if (duration <= std::numeric_limits::epsilon() ) + { + coeff_[0] = pos1; + coeff_[1] = 0.0; + coeff_[2] = 0.0; + coeff_[3] = 0.0; + coeff_[4] = 0.0; + coeff_[5] = 0.0; + } else + { + coeff_[0] = pos1; + coeff_[1] = (pos2 - pos1) / duration; + coeff_[2] = 0.0; + coeff_[3] = 0.0; + coeff_[4] = 0.0; + coeff_[5] = 0.0; + } + + return; +} + +void VelocityProfile_Spline::SetProfileDuration(double pos1, double vel1, double pos2, double vel2, double duration) +{ + double T[4]; + duration_ = duration; + generatePowers(3, duration, T); + + if (duration <= std::numeric_limits::epsilon() ) + { + coeff_[0] = pos2; + coeff_[1] = vel2; + coeff_[2] = 0.0; + coeff_[3] = 0.0; + coeff_[4] = 0.0; + coeff_[5] = 0.0; + } else + { + coeff_[0] = pos1; + coeff_[1] = vel1; + coeff_[2] = (-3.0*pos1 + 3.0*pos2 - 2.0*vel1*T[1] - vel2*T[1]) / T[2]; + coeff_[3] = (2.0*pos1 - 2.0*pos2 + vel1*T[1] + vel2*T[1]) / T[3]; + coeff_[4] = 0.0; + coeff_[5] = 0.0; + } + + return; +} + +void VelocityProfile_Spline::SetProfileDuration(double pos1, double vel1, double acc1, double pos2, double vel2, double acc2, double duration) +{ + double T[6]; + duration_ = duration; + generatePowers(5, duration, T); + + if (duration <= std::numeric_limits::epsilon() ) + { + coeff_[0] = pos2; + coeff_[1] = vel2; + coeff_[2] = 0.5 * acc2; + coeff_[3] = 0.0; + coeff_[4] = 0.0; + coeff_[5] = 0.0; + } else + { + coeff_[0] = pos1; + coeff_[1] = vel1; + coeff_[2] = 0.5*acc1; + coeff_[3] = (-20.0*pos1 + 20.0*pos2 - 3.0*acc1*T[2] + acc2*T[2] - + 12.0*vel1*T[1] - 8.0*vel2*T[1]) / (2.0*T[3]); + coeff_[4] = (30.0*pos1 - 30.0*pos2 + 3.0*acc1*T[2] - 2.0*acc2*T[2] + + 16.0*vel1*T[1] + 14.0*vel2*T[1]) / (2.0*T[4]); + coeff_[5] = (-12.0*pos1 + 12.0*pos2 - acc1*T[2] + acc2*T[2] - + 6.0*vel1*T[1] - 6.0*vel2*T[1]) / (2.0*T[5]); + } + + return; +} + +double VelocityProfile_Spline::Duration() const +{ + return duration_; +} + +double VelocityProfile_Spline::Pos(double time) const +{ + double t[6]; + double position; + generatePowers(5, time, t); + + position = t[0]*coeff_[0] + + t[1]*coeff_[1] + + t[2]*coeff_[2] + + t[3]*coeff_[3] + + t[4]*coeff_[4] + + t[5]*coeff_[5]; + return position; +} + +double VelocityProfile_Spline::Vel(double time) const +{ + double t[5]; + double velocity; + generatePowers(4, time, t); + + velocity = t[0]*coeff_[1] + + 2.0*t[1]*coeff_[2] + + 3.0*t[2]*coeff_[3] + + 4.0*t[3]*coeff_[4] + + 5.0*t[4]*coeff_[5]; + return velocity; +} + +double VelocityProfile_Spline::Acc(double time) const +{ + double t[4]; + double acceleration; + generatePowers(3, time, t); + + acceleration = 2.0*t[0]*coeff_[2] + + 6.0*t[1]*coeff_[3] + + 12.0*t[2]*coeff_[4] + + 20.0*t[3]*coeff_[5]; + return acceleration; +} + +void VelocityProfile_Spline::Write(std::ostream& os) const +{ + os << "coefficients : [ " << coeff_[0] << " " << coeff_[1] << " " << coeff_[2] << " " << coeff_[3] << " " << coeff_[4] << " " << coeff_[5] << " ]"; + return; +} + +VelocityProfile* VelocityProfile_Spline::Clone() const +{ + return new VelocityProfile_Spline(*this); +} +} diff --git a/src/Mod/Robot/App/kdl_cp/velocityprofile_spline.hpp b/src/Mod/Robot/App/kdl_cp/velocityprofile_spline.hpp new file mode 100644 index 000000000..0ce9ea896 --- /dev/null +++ b/src/Mod/Robot/App/kdl_cp/velocityprofile_spline.hpp @@ -0,0 +1,67 @@ +#ifndef VELOCITYPROFILE_SPLINE_H +#define VELOCITYPROFILE_SPLINE_H + +#include "velocityprofile.hpp" + +namespace KDL +{ + /** + * \brief A spline VelocityProfile trajectory interpolation. + * @ingroup Motion + */ +class VelocityProfile_Spline : public VelocityProfile +{ +public: + VelocityProfile_Spline(); + VelocityProfile_Spline(const VelocityProfile_Spline &p); + + virtual ~VelocityProfile_Spline(); + + virtual void SetProfile(double pos1, double pos2); + /** + * Generate linear interpolation coeffcients. + * + * @param pos1 begin position. + * @param pos2 end position. + * @param duration duration of the profile. + */ + virtual void SetProfileDuration( + double pos1, double pos2, double duration); + + /** + * Generate cubic spline interpolation coeffcients. + * + * @param pos1 begin position. + * @param vel1 begin velocity. + * @param pos2 end position. + * @param vel2 end velocity. + * @param duration duration of the profile. + */ + virtual void SetProfileDuration( + double pos1, double vel1, double pos2, double vel2, double duration); + + /** + * Generate quintic spline interpolation coeffcients. + * + * @param pos1 begin position. + * @param vel1 begin velocity. + * @param acc1 begin acceleration + * @param pos2 end position. + * @param vel2 end velocity. + * @param acc2 end acceleration. + * @param duration duration of the profile. + */ + virtual void SetProfileDuration(double pos1, double vel1, double acc1, double pos2, double vel2, double acc2, double duration); + virtual double Duration() const; + virtual double Pos(double time) const; + virtual double Vel(double time) const; + virtual double Acc(double time) const; + virtual void Write(std::ostream& os) const; + virtual VelocityProfile* Clone() const; +private: + + double coeff_[6]; + double duration_; +}; +} +#endif // VELOCITYPROFILE_CUBICSPLINE_H diff --git a/src/Mod/Robot/App/kdl_cp/velocityprofile_trap.cpp b/src/Mod/Robot/App/kdl_cp/velocityprofile_trap.cpp index 679b3adaf..798caa917 100644 --- a/src/Mod/Robot/App/kdl_cp/velocityprofile_trap.cpp +++ b/src/Mod/Robot/App/kdl_cp/velocityprofile_trap.cpp @@ -108,6 +108,25 @@ void VelocityProfile_Trap::SetProfileDuration( t2 /= factor; } +void VelocityProfile_Trap::SetProfileVelocity( + double pos1,double pos2,double newvelocity) { + // Max velocity + SetProfile(pos1,pos2); + // Must be Slower : + double factor = newvelocity; // valid = [KDL::epsilon, 1.0] + if (1.0 < factor) factor = 1.0; + if (KDL::epsilon > factor) factor = KDL::epsilon; + a2*=factor; + a3*=factor*factor; + b2*=factor; + b3*=factor*factor; + c2*=factor; + c3*=factor*factor; + duration = duration / factor; + t1 /= factor; + t2 /= factor; +} + void VelocityProfile_Trap::SetMax(double _maxvel,double _maxacc) { maxvel = _maxvel; maxacc = _maxacc; diff --git a/src/Mod/Robot/App/kdl_cp/velocityprofile_trap.hpp b/src/Mod/Robot/App/kdl_cp/velocityprofile_trap.hpp index a8e1ba0ab..80efeb411 100644 --- a/src/Mod/Robot/App/kdl_cp/velocityprofile_trap.hpp +++ b/src/Mod/Robot/App/kdl_cp/velocityprofile_trap.hpp @@ -83,6 +83,18 @@ class VelocityProfile_Trap : public VelocityProfile double pos1,double pos2,double newduration ); + /** Compute trapezoidal profile at a given fraction of max velocity + @param pos1 Position to start from + @param pos2 Position to end at + @param newvelocity Fraction of max velocity to use during the + non-ramp, flat-velocity part of the profile. + @param KDL::epsilon <= newvelocity <= 1.0 (forcibly clamped to + this range internally) + */ + virtual void SetProfileVelocity( + double pos1,double pos2,double newvelocity + ); + virtual void SetMax(double _maxvel,double _maxacc); virtual double Duration() const; virtual double Pos(double time) const; diff --git a/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.cpp b/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.cpp index 090cec854..1631b4bd9 100644 --- a/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.cpp +++ b/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.cpp @@ -58,9 +58,9 @@ void VelocityProfile_TrapHalf::PlanProfile1(double v,double a) { a3 = 0; a2 = 0; a1 = startpos; - b3 = a/2; + b3 = a/2.0; b2 = -a*t1; - b1 = startpos + a2*t1*t1/2; + b1 = startpos + a*t1*t1/2.0; c3 = 0; c2 = v; c1 = endpos - v*duration; @@ -70,9 +70,9 @@ void VelocityProfile_TrapHalf::PlanProfile2(double v,double a) { a3 = 0; a2 = v; a1 = startpos; - b3 = -a/2; + b3 = -a/2.0; b2 = a*t2; - b1 = endpos - a*t2*t2/2; + b1 = endpos - a*t2*t2/2.0; c3 = 0; c2 = 0; c1 = endpos; @@ -82,15 +82,17 @@ void VelocityProfile_TrapHalf::SetProfile(double pos1,double pos2) { startpos = pos1; endpos = pos2; double s = sign(endpos-startpos); - duration = s*(endpos-startpos)/maxvel+maxvel/maxacc/2.0; + // check that the maxvel is obtainable + double vel = std::min(maxvel, sqrt(2.0*s*(endpos-startpos)*maxacc)); + duration = s*(endpos-startpos)/vel+vel/maxacc/2.0; if (starting) { t1 = 0; - t2 = maxvel/maxacc; - PlanProfile1(maxvel*s,maxacc*s); + t2 = vel/maxacc; + PlanProfile1(vel*s,maxacc*s); } else { - t1 = duration-maxvel/maxacc; + t1 = duration-vel/maxacc; t2 = duration; - PlanProfile2(s*maxvel,s*maxacc); + PlanProfile2(s*vel,s*maxacc); } } diff --git a/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.hpp b/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.hpp index 8ed6a4dfc..1537791db 100644 --- a/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.hpp +++ b/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.hpp @@ -81,21 +81,39 @@ class VelocityProfile_TrapHalf : public VelocityProfile void PlanProfile2(double v,double a); public: - /** - * \param _maxvel maximal velocity of the motion profile (positive) - * \param _maxacc maximal acceleration of the motion profile (positive) - * \param _starting this value is true when initial velocity is zero - * and ending velocity is maxvel, is false for the reverse - */ - VelocityProfile_TrapHalf(double _maxvel=0,double _maxacc=0,bool _starting=true); + /** + * \param maxvel maximal velocity of the motion profile (positive) + * \param maxacc maximal acceleration of the motion profile (positive) + * \param starting this value is true when initial velocity is zero + * and ending velocity is maxvel, is false for the reverse + */ + VelocityProfile_TrapHalf(double _maxvel=0,double _maxacc=0,bool _starting=true); - void SetMax(double _maxvel,double _maxacc, bool _starting ); + void SetMax(double _maxvel,double _maxacc,bool _starting); /** + * Plans a 'Half' Trapezoidal VelocityProfile between pos1 and pos2. + * If the distance is too short betweeen pos1 and pos2, + * only the acceleration phase is set and the max velocity is not reached. + * + * \param pos1 Starting position + * \param pos2 Ending position + * * Can throw a Error_MotionPlanning_Not_Feasible */ virtual void SetProfile(double pos1,double pos2); + /** + * Can be used to prolong the profile, there are two possible outcomes: in a first + * phase the acceleration is lowered as such that the end position and maximum velocity + * are reached at the given duration (newduration). In this case there is an acceleration part and a constant velocity part, + * when this reaches a minimum acceleration value at which the constant part disappears, the motion is stalled, + * in this case their is a non-motion part and an acceleration part. + * + *\param pos1 starting position + *\param pos2 ending position + *\param newduration the desired duration, if it is lower than the minimum duration, the minimum duration will be used instead of the given duration. + */ virtual void SetProfileDuration( double pos1,double pos2,double newduration ); diff --git a/src/Mod/Robot/Gui/CMakeLists.txt b/src/Mod/Robot/Gui/CMakeLists.txt index 1982509f4..6c17aa893 100644 --- a/src/Mod/Robot/Gui/CMakeLists.txt +++ b/src/Mod/Robot/Gui/CMakeLists.txt @@ -1,7 +1,7 @@ if(MSVC) - add_definitions(-DFCGuiRobot -DHAVE_ACOSH -DHAVE_ASINH -DHAVE_ATANH -DEIGEN2_SUPPORT) + add_definitions(-DFCGuiRobot -DHAVE_ACOSH -DHAVE_ASINH -DHAVE_ATANH) else(MSVC) - add_definitions(-DHAVE_LIMITS_H -DHAVE_CONFIG_H -DEIGEN2_SUPPORT -DEIGEN_NO_EIGEN2_DEPRECATED_WARNING) + add_definitions(-DHAVE_LIMITS_H -DHAVE_CONFIG_H) endif(MSVC) From 1908e852f06114cfcb5c4221ad56d6f3efee997d Mon Sep 17 00:00:00 2001 From: Yorik van Havre Date: Wed, 14 Oct 2015 12:39:29 -0300 Subject: [PATCH 16/20] Added doxygen fixes from svn commit 3590 to KDL --- .../App/kdl_cp/articulatedbodyinertia.cpp | 2 +- src/Mod/Robot/App/kdl_cp/chainfksolver.hpp | 5 +++- src/Mod/Robot/App/kdl_cp/chainidsolver.hpp | 7 +++-- .../kdl_cp/chainiksolvervel_pinv_givens.hpp | 4 --- .../App/kdl_cp/chainiksolvervel_wdls.cpp | 4 +-- .../App/kdl_cp/chainiksolvervel_wdls.hpp | 10 ++++--- src/Mod/Robot/App/kdl_cp/frameacc.hpp | 4 +-- src/Mod/Robot/App/kdl_cp/frameacc.inl | 3 +- src/Mod/Robot/App/kdl_cp/frames.hpp | 9 +++--- src/Mod/Robot/App/kdl_cp/frames.inl | 3 +- src/Mod/Robot/App/kdl_cp/framevel.hpp | 6 ++-- src/Mod/Robot/App/kdl_cp/framevel.inl | 3 ++ src/Mod/Robot/App/kdl_cp/jntarray.hpp | 12 ++++++++ .../App/kdl_cp/jntspaceinertiamatrix.hpp | 12 ++++---- src/Mod/Robot/App/kdl_cp/joint.hpp | 30 ++++++++++--------- src/Mod/Robot/App/kdl_cp/kdl.hpp | 6 ++-- src/Mod/Robot/App/kdl_cp/kinfam.hpp | 1 + src/Mod/Robot/App/kdl_cp/motion.hpp | 1 + .../App/kdl_cp/rotational_interpolation.hpp | 2 +- src/Mod/Robot/App/kdl_cp/segment.hpp | 4 +-- .../Robot/App/kdl_cp/trajectory_segment.hpp | 4 +-- src/Mod/Robot/App/kdl_cp/treefksolver.hpp | 2 +- src/Mod/Robot/App/kdl_cp/utilities/rallNd.h | 9 +++--- .../App/kdl_cp/utilities/svd_eigen_HH.hpp | 2 +- .../Robot/App/kdl_cp/utilities/utility.cxx | 2 +- .../App/kdl_cp/velocityprofile_traphalf.hpp | 6 ++-- 26 files changed, 89 insertions(+), 64 deletions(-) diff --git a/src/Mod/Robot/App/kdl_cp/articulatedbodyinertia.cpp b/src/Mod/Robot/App/kdl_cp/articulatedbodyinertia.cpp index e4cdcb937..484814385 100644 --- a/src/Mod/Robot/App/kdl_cp/articulatedbodyinertia.cpp +++ b/src/Mod/Robot/App/kdl_cp/articulatedbodyinertia.cpp @@ -41,7 +41,7 @@ namespace KDL{ *this = RigidBodyInertia(m,c,Ic); } - ArticulatedBodyInertia::ArticulatedBodyInertia(const Matrix3d& M, const Matrix3d& H, const Matrix3d& I) + ArticulatedBodyInertia::ArticulatedBodyInertia(const Eigen::Matrix3d& M, const Eigen::Matrix3d& H, const Eigen::Matrix3d& I) { this->M=M; this->I=I; diff --git a/src/Mod/Robot/App/kdl_cp/chainfksolver.hpp b/src/Mod/Robot/App/kdl_cp/chainfksolver.hpp index ab52b3f80..ea2f333d2 100644 --- a/src/Mod/Robot/App/kdl_cp/chainfksolver.hpp +++ b/src/Mod/Robot/App/kdl_cp/chainfksolver.hpp @@ -48,6 +48,7 @@ namespace KDL { * * @param q_in input joint coordinates * @param p_out reference to output cartesian pose + * @param segmentNr default to -1 * * @return if < 0 something went wrong */ @@ -69,6 +70,7 @@ namespace KDL { * * @param q_in input joint coordinates (position and velocity) * @param out output cartesian coordinates (position and velocity) + * @param segmentNr default to -1 * * @return if < 0 something went wrong */ @@ -92,8 +94,9 @@ namespace KDL { * * @param q_in input joint coordinates (position, velocity and * acceleration - @param out output cartesian coordinates (position, velocity + * @param out output cartesian coordinates (position, velocity * and acceleration + * @param segmentNr default to -1 * * @return if < 0 something went wrong */ diff --git a/src/Mod/Robot/App/kdl_cp/chainidsolver.hpp b/src/Mod/Robot/App/kdl_cp/chainidsolver.hpp index fe489185c..8a407def3 100644 --- a/src/Mod/Robot/App/kdl_cp/chainidsolver.hpp +++ b/src/Mod/Robot/App/kdl_cp/chainidsolver.hpp @@ -41,14 +41,15 @@ namespace KDL { public: /** - * Calculate inverse dynamics, from joint positions, velocity, acceleration, external forces + * Calculate inverse dynamics, from joint positions, + * velocity, acceleration, external forces * to joint torques/forces. * * @param q input joint positions * @param q_dot input joint velocities * @param q_dotdot input joint accelerations - * - * @param torque output joint torques + * @param f_ext external forces + * @param torques output joint torques * * @return if < 0 something went wrong */ diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.hpp b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.hpp index b78e3d855..0371d362f 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.hpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.hpp @@ -29,10 +29,6 @@ namespace KDL * * @param chain the chain to calculate the inverse velocity * kinematics for - * @param eps if a singular value is below this value, its - * inverse is set to zero, default: 0.00001 - * @param maxiter maximum iterations for the svd calculation, - * default: 150 * */ explicit ChainIkSolverVel_pinv_givens(const Chain& chain); diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.cpp b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.cpp index 3a24e9974..80bc45022 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.cpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.cpp @@ -54,11 +54,11 @@ namespace KDL { } - void ChainIkSolverVel_wdls::setWeightJS(const MatrixXd& Mq){ + void ChainIkSolverVel_wdls::setWeightJS(const Eigen::MatrixXd& Mq){ weight_js = Mq; } - void ChainIkSolverVel_wdls::setWeightTS(const MatrixXd& Mx){ + void ChainIkSolverVel_wdls::setWeightTS(const Eigen::MatrixXd& Mx){ weight_ts = Mx; } diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.hpp b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.hpp index d1c171be4..e1068c380 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.hpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_wdls.hpp @@ -108,8 +108,9 @@ namespace KDL /** * Set the joint space weighting matrix * - * @param weight_js joint space weighting symetric matrix, - * default : identity. M_q : This matrix being used as a + * weight_js joint space weighting symetric matrix, + * default : identity. + * @param Mq : This matrix being used as a * weight for the norm of the joint space speed it HAS TO BE * symmetric and positive definite. We can actually deal with * matrices containing a symmetric and positive definite block @@ -132,8 +133,9 @@ namespace KDL /** * Set the task space weighting matrix * - * @param weight_ts task space weighting symetric matrix, - * default: identity M_x : This matrix being used as a weight + * weight_ts task space weighting symetric matrix, + * default: identity + * @param Mx : This matrix being used as a weight * for the norm of the error (in terms of task space speed) it * HAS TO BE symmetric and positive definite. We can actually * deal with matrices containing a symmetric and positive diff --git a/src/Mod/Robot/App/kdl_cp/frameacc.hpp b/src/Mod/Robot/App/kdl_cp/frameacc.hpp index 6521d0cf2..eb54cfdac 100644 --- a/src/Mod/Robot/App/kdl_cp/frameacc.hpp +++ b/src/Mod/Robot/App/kdl_cp/frameacc.hpp @@ -260,14 +260,14 @@ public: - +} #ifdef KDL_INLINE #include "frameacc.inl" #endif -} + diff --git a/src/Mod/Robot/App/kdl_cp/frameacc.inl b/src/Mod/Robot/App/kdl_cp/frameacc.inl index dfbdc620e..599cf416d 100644 --- a/src/Mod/Robot/App/kdl_cp/frameacc.inl +++ b/src/Mod/Robot/App/kdl_cp/frameacc.inl @@ -17,7 +17,7 @@ ****************************************************************************/ - +namespace KDL { /////////////////// VectorAcc ///////////////////////////////////// @@ -596,3 +596,4 @@ bool Equal(const TwistAcc& a,const Twist& b,double eps) { Equal(a.vel,b.vel,eps) ); } +} diff --git a/src/Mod/Robot/App/kdl_cp/frames.hpp b/src/Mod/Robot/App/kdl_cp/frames.hpp index c724358c7..9604b4084 100644 --- a/src/Mod/Robot/App/kdl_cp/frames.hpp +++ b/src/Mod/Robot/App/kdl_cp/frames.hpp @@ -639,9 +639,9 @@ public: //! @return the identity transformation Frame(Rotation::Identity(),Vector::Zero()). inline static Frame Identity(); - //! The twist is expressed wrt the current + //! The twist \ is expressed wrt the current //! frame. This frame is integrated into an updated frame with - //! . Very simple first order integration rule. + //! \. Very simple first order integration rule. inline void Integrate(const Twist& t_this,double frequency); /* @@ -1249,6 +1249,8 @@ IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1); */ IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1); +} // namespace KDL + #ifdef KDL_INLINE // #include "vector.inl" // #include "wrench.inl" @@ -1263,7 +1265,4 @@ IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1); -} - - #endif diff --git a/src/Mod/Robot/App/kdl_cp/frames.inl b/src/Mod/Robot/App/kdl_cp/frames.inl index 156acf192..d85a25935 100644 --- a/src/Mod/Robot/App/kdl_cp/frames.inl +++ b/src/Mod/Robot/App/kdl_cp/frames.inl @@ -31,7 +31,7 @@ * */ - +namespace KDL { IMETHOD Vector::Vector(const Vector & arg) { @@ -1334,3 +1334,4 @@ IMETHOD bool operator!=(const Vector2& a,const Vector2& b) { return !operator==(a,b); } +} // namespace KDL diff --git a/src/Mod/Robot/App/kdl_cp/framevel.hpp b/src/Mod/Robot/App/kdl_cp/framevel.hpp index 8573d33f8..ed9c87c0a 100644 --- a/src/Mod/Robot/App/kdl_cp/framevel.hpp +++ b/src/Mod/Robot/App/kdl_cp/framevel.hpp @@ -52,7 +52,7 @@ IMETHOD void posrandom(doubleVel& F) { posrandom(F.grad); } -} +} //namespace KDL template <> struct Traits { @@ -383,12 +383,12 @@ IMETHOD void posrandom(FrameVel& F) { posrandom(F.p); } +} // namespace KDL + #ifdef KDL_INLINE #include "framevel.inl" #endif -} // namespace - #endif diff --git a/src/Mod/Robot/App/kdl_cp/framevel.inl b/src/Mod/Robot/App/kdl_cp/framevel.inl index 61a43fdc2..1def47fd1 100644 --- a/src/Mod/Robot/App/kdl_cp/framevel.inl +++ b/src/Mod/Robot/App/kdl_cp/framevel.inl @@ -16,6 +16,7 @@ * $Name: $ ****************************************************************************/ +namespace KDL { // Methods and operators related to FrameVelVel // They all delegate most of the work to RotationVelVel and VectorVelVel @@ -532,3 +533,5 @@ Twist TwistVel::GetTwist() const { Twist TwistVel::GetTwistDot() const { return Twist(vel.v,rot.v); } + +} // namespace KDL diff --git a/src/Mod/Robot/App/kdl_cp/jntarray.hpp b/src/Mod/Robot/App/kdl_cp/jntarray.hpp index e0ff388ca..a4b1bf835 100644 --- a/src/Mod/Robot/App/kdl_cp/jntarray.hpp +++ b/src/Mod/Robot/App/kdl_cp/jntarray.hpp @@ -139,6 +139,18 @@ class MyTask : public RTT::TaskContext friend void Subtract(const JntArray& src1,const JntArray& src2,JntArray& dest); friend void Multiply(const JntArray& src,const double& factor,JntArray& dest); friend void Divide(const JntArray& src,const double& factor,JntArray& dest); + /** + * Function to multiply a KDL::Jacobian with a KDL::JntArray + * to get a KDL::Twist, it should not be used to calculate the + * forward velocity kinematics, the solver classes are built + * for this purpose. + * J*q = t + * + * @param jac J + * @param src q + * @param dest t + * @post dest == (KDL::Twist::)Zero() if 0 == src.rows() (ie src is empty) + */ friend void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest); friend void SetToZero(JntArray& array); friend bool Equal(const JntArray& src1,const JntArray& src2,double eps); diff --git a/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.hpp b/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.hpp index ae4f01298..987be53f3 100644 --- a/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.hpp +++ b/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.hpp @@ -63,7 +63,7 @@ class MyTask : public RTT::TaskContext ** use j here } }; -/endcode +\endcode */ @@ -199,9 +199,9 @@ class MyTask : public RTT::TaskContext * for this purpose. * J*q = t * - * @param jac J - * @param src q - * @param dest t + * @param src J: Inertia Matrix (6x6) + * @param vec q: Jacobian (6x1) + * @param dest t: Twist (6x1) * @post dest==Twist::Zero() if 0==src.rows() (ie src is empty) */ void Multiply(const JntSpaceInertiaMatrix& src, const JntArray& vec, JntArray& dest); @@ -209,9 +209,9 @@ class MyTask : public RTT::TaskContext /** * Function to set all the values of the array to 0 * - * @param array + * @param mat */ - void SetToZero(JntSpaceInertiaMatrix& matrix); + void SetToZero(JntSpaceInertiaMatrix& mat); /** * Function to check if two matrices are the same with a diff --git a/src/Mod/Robot/App/kdl_cp/joint.hpp b/src/Mod/Robot/App/kdl_cp/joint.hpp index a188aff95..798ce9077 100644 --- a/src/Mod/Robot/App/kdl_cp/joint.hpp +++ b/src/Mod/Robot/App/kdl_cp/joint.hpp @@ -80,15 +80,16 @@ namespace KDL { * Constructor of a joint. * * @param name of the joint - * @param origin the origin of the joint - * @param axis the axis of the joint - * @param scale scale between joint input and actual geometric + * @param _origin the origin of the joint + * @param _axis the axis of the joint + * @param type type of the joint + * @param _scale scale between joint input and actual geometric * movement, default: 1 - * @param offset offset between joint input and actual + * @param _offset offset between joint input and actual * geometric input, default: 0 - * @param inertia 1D inertia along the joint axis, default: 0 - * @param damping 1D damping along the joint axis, default: 0 - * @param stiffness 1D stiffness along the joint axis, + * @param _inertia 1D inertia along the joint axis, default: 0 + * @param _damping 1D damping along the joint axis, default: 0 + * @param _stiffness 1D stiffness along the joint axis, * default: 0 */ Joint(const std::string& name, const Vector& _origin, const Vector& _axis, const JointType& type, const double& _scale=1, const double& _offset=0, @@ -96,15 +97,16 @@ namespace KDL { /** * Constructor of a joint. * - * @param origin the origin of the joint - * @param axis the axis of the joint - * @param scale scale between joint input and actual geometric + * @param _origin the origin of the joint + * @param _axis the axis of the joint + * @param type type of the joint + * @param _scale scale between joint input and actual geometric * movement, default: 1 - * @param offset offset between joint input and actual + * @param _offset offset between joint input and actual * geometric input, default: 0 - * @param inertia 1D inertia along the joint axis, default: 0 - * @param damping 1D damping along the joint axis, default: 0 - * @param stiffness 1D stiffness along the joint axis, + * @param _inertia 1D inertia along the joint axis, default: 0 + * @param _damping 1D damping along the joint axis, default: 0 + * @param _stiffness 1D stiffness along the joint axis, * default: 0 */ Joint(const Vector& _origin, const Vector& _axis, const JointType& type, const double& _scale=1, const double& _offset=0, diff --git a/src/Mod/Robot/App/kdl_cp/kdl.hpp b/src/Mod/Robot/App/kdl_cp/kdl.hpp index c74ede6d7..b02e776aa 100644 --- a/src/Mod/Robot/App/kdl_cp/kdl.hpp +++ b/src/Mod/Robot/App/kdl_cp/kdl.hpp @@ -21,7 +21,8 @@ // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA /** - * \mainpage KDL + * \defgroup KDL Kinematics and Dynamics Library + * \ingroup ROBOT * * This is the API reference of the * Kinematics and Dynamics @@ -32,7 +33,6 @@ * - \subpage geomprim * - \ref KinematicFamily : functionality to build kinematic chains and access their kinematic and dynamic properties, such as e.g. Forward and Inverse kinematics and dynamics. * - \ref Motion : functionality to specify motion trajectories of frames and kinematic chains, such as e.g. Trapezoidal Velocity profiles. - * - \ref KDLTK : the interface code to integrate KDL into the Orocos Real-Time Toolkit (RTT). * * **/ @@ -125,5 +125,7 @@ * Typically we use the standard S.I. units: N, m, sec. * */ + +/* This code doesn't seems to be integrated with freecad - \ref KDLTK : the interface code to integrate KDL into the Orocos Real-Time Toolkit (RTT). */ diff --git a/src/Mod/Robot/App/kdl_cp/kinfam.hpp b/src/Mod/Robot/App/kdl_cp/kinfam.hpp index e3eb12f9a..a9ce651a3 100644 --- a/src/Mod/Robot/App/kdl_cp/kinfam.hpp +++ b/src/Mod/Robot/App/kdl_cp/kinfam.hpp @@ -21,6 +21,7 @@ /** * @defgroup KinematicFamily Kinematic Families + * \ingroup KDL * @brief All classes to support kinematic families. * * The Kinematic Families classes range from the basic building blocks diff --git a/src/Mod/Robot/App/kdl_cp/motion.hpp b/src/Mod/Robot/App/kdl_cp/motion.hpp index 7d427f72d..38bfde7fd 100644 --- a/src/Mod/Robot/App/kdl_cp/motion.hpp +++ b/src/Mod/Robot/App/kdl_cp/motion.hpp @@ -21,6 +21,7 @@ /** * @defgroup Motion Motion + * \ingroup KDL * @brief All classes related to the non-instantaneous motion of rigid * bodies and kinematic structures, e.g., path and trajecory definitions * and their building blocks. diff --git a/src/Mod/Robot/App/kdl_cp/rotational_interpolation.hpp b/src/Mod/Robot/App/kdl_cp/rotational_interpolation.hpp index ced4914bb..2736c93db 100644 --- a/src/Mod/Robot/App/kdl_cp/rotational_interpolation.hpp +++ b/src/Mod/Robot/App/kdl_cp/rotational_interpolation.hpp @@ -105,7 +105,7 @@ class RotationalInterpolation static RotationalInterpolation* Read(std::istream& is); /** - * virtual constructor, construction by copying .. + * virtual constructor, construction by copying. */ virtual RotationalInterpolation* Clone() const = 0; diff --git a/src/Mod/Robot/App/kdl_cp/segment.hpp b/src/Mod/Robot/App/kdl_cp/segment.hpp index 2ea46664a..53e6285ed 100644 --- a/src/Mod/Robot/App/kdl_cp/segment.hpp +++ b/src/Mod/Robot/App/kdl_cp/segment.hpp @@ -60,7 +60,7 @@ namespace KDL { * Joint(Joint::None) * @param f_tip frame from the end of the joint to the tip of * the segment, default: Frame::Identity() - * @param M rigid body inertia of the segment, default: Inertia::Zero() + * @param I rigid body inertia of the segment, default: Inertia::Zero() */ explicit Segment(const std::string& name, const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); /** @@ -70,7 +70,7 @@ namespace KDL { * Joint(Joint::None) * @param f_tip frame from the end of the joint to the tip of * the segment, default: Frame::Identity() - * @param M rigid body inertia of the segment, default: Inertia::Zero() + * @param I rigid body inertia of the segment, default: Inertia::Zero() */ explicit Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero()); Segment(const Segment& in); diff --git a/src/Mod/Robot/App/kdl_cp/trajectory_segment.hpp b/src/Mod/Robot/App/kdl_cp/trajectory_segment.hpp index 8d7250306..968b81f13 100644 --- a/src/Mod/Robot/App/kdl_cp/trajectory_segment.hpp +++ b/src/Mod/Robot/App/kdl_cp/trajectory_segment.hpp @@ -66,12 +66,12 @@ namespace KDL { bool aggregate; public: /** - * This constructor assumes that \a geom and <_motprof> are initialised correctly. + * This constructor assumes that \a geom and \<_motprof\> are initialised correctly. */ Trajectory_Segment(Path* _geom, VelocityProfile* _motprof, bool _aggregate=true); /** - * This constructor assumes that \a geom is initialised and <_motprof> needs to be + * This constructor assumes that \a geom is initialised and \<_motprof\> needs to be * set according to \a duration. */ Trajectory_Segment(Path* _geom, VelocityProfile* _motprof, double duration, bool _aggregate=true); diff --git a/src/Mod/Robot/App/kdl_cp/treefksolver.hpp b/src/Mod/Robot/App/kdl_cp/treefksolver.hpp index 3d521d340..bbbed6968 100644 --- a/src/Mod/Robot/App/kdl_cp/treefksolver.hpp +++ b/src/Mod/Robot/App/kdl_cp/treefksolver.hpp @@ -50,7 +50,7 @@ namespace KDL { * * @param q_in input joint coordinates * @param p_out reference to output cartesian pose - * + * @param segmentName * @return if < 0 something went wrong */ virtual int JntToCart(const JntArray& q_in, Frame& p_out, std::string segmentName)=0; diff --git a/src/Mod/Robot/App/kdl_cp/utilities/rallNd.h b/src/Mod/Robot/App/kdl_cp/utilities/rallNd.h index 63596ebc8..81b5ecba5 100644 --- a/src/Mod/Robot/App/kdl_cp/utilities/rallNd.h +++ b/src/Mod/Robot/App/kdl_cp/utilities/rallNd.h @@ -10,7 +10,8 @@ * generalized to the Nth derivative ! * The efficiency is not very good for high derivatives. * This could be improved by also using Rall2d - * + */ +/* template class RallNd : public Rall1d< RallNd, RallNd, double > @@ -45,15 +46,15 @@ public: * Als je tot 3de orde een efficiente berekening kan doen, * dan kan je tot een willekeurige orde alles efficient berekenen * 0 1 2 3 - * ==> 1 2 3 4 - * ==> 3 4 5 6 + * ==\> 1 2 3 4 + * ==\> 3 4 5 6 * 4 5 6 7 * * de aangeduide berekeningen zijn niet noodzakelijk, en er is dan niets * verniet berekend in de recursieve implementatie. * of met 2de orde over 1ste order : kan ook efficient : * 0 1 - * ==>1 2 + * ==\>1 2 * 2 3 */ // N>2: diff --git a/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.hpp b/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.hpp index 2859648b0..e6618c6d2 100644 --- a/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.hpp +++ b/src/Mod/Robot/App/kdl_cp/utilities/svd_eigen_HH.hpp @@ -63,7 +63,7 @@ namespace KDL * @param V matrix(nxn) * @param tmp vector n * @param maxiter defaults to 150 - * + * @param epsilon defaults to 1e-300 * @return -2 if maxiter exceeded, 0 otherwise */ int svd_eigen_HH(const MatrixXd& A,MatrixXd& U,VectorXd& S,MatrixXd& V,VectorXd& tmp,int maxiter=150,double epsilon=1e-300); diff --git a/src/Mod/Robot/App/kdl_cp/utilities/utility.cxx b/src/Mod/Robot/App/kdl_cp/utilities/utility.cxx index c79816728..713bdac6e 100644 --- a/src/Mod/Robot/App/kdl_cp/utilities/utility.cxx +++ b/src/Mod/Robot/App/kdl_cp/utilities/utility.cxx @@ -1,4 +1,4 @@ -/** @file utility.cpp +/** @file utility.cxx * @author Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven * @version * ORO_Geometry V0.2 diff --git a/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.hpp b/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.hpp index 1537791db..344ae7f59 100644 --- a/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.hpp +++ b/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.hpp @@ -82,9 +82,9 @@ class VelocityProfile_TrapHalf : public VelocityProfile public: /** - * \param maxvel maximal velocity of the motion profile (positive) - * \param maxacc maximal acceleration of the motion profile (positive) - * \param starting this value is true when initial velocity is zero + * \param _maxvel maximal velocity of the motion profile (positive) + * \param _maxacc maximal acceleration of the motion profile (positive) + * \param _starting this value is true when initial velocity is zero * and ending velocity is maxvel, is false for the reverse */ VelocityProfile_TrapHalf(double _maxvel=0,double _maxacc=0,bool _starting=true); From 9304f7a78a47e59be860cc77de58ea5ada8c33f4 Mon Sep 17 00:00:00 2001 From: Yorik van Havre Date: Wed, 14 Oct 2015 16:51:50 -0300 Subject: [PATCH 17/20] Added Juergen's original changes to KDL --- .../Robot/App/kdl_cp/chainiksolverpos_nr_jl.cpp | 15 +++++++++++++-- src/Mod/Robot/App/kdl_cp/trajectory.hpp | 6 ++++++ src/Mod/Robot/App/kdl_cp/trajectory_composite.cpp | 14 ++++++++++++++ src/Mod/Robot/App/kdl_cp/trajectory_composite.hpp | 6 ++++++ .../Robot/App/kdl_cp/trajectory_stationary.hpp | 14 ++++++++++++++ src/Mod/Robot/App/kdl_cp/utilities/rallNd.h | 2 +- 6 files changed, 54 insertions(+), 3 deletions(-) diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.cpp b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.cpp index 5620e20b2..1ed3c4d72 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.cpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.cpp @@ -23,6 +23,15 @@ #include "chainiksolverpos_nr_jl.hpp" +// FreeCAD change +#ifndef M_PI + #define M_PI 3.14159265358979323846 /* pi */ +#endif + +#ifndef M_PI_2 + #define M_PI_2 1.57079632679489661923 /* pi/2 */ +#endif + namespace KDL { ChainIkSolverPos_NR_JL::ChainIkSolverPos_NR_JL(const Chain& _chain, const JntArray& _q_min, const JntArray& _q_max, ChainFkSolverPos& _fksolver,ChainIkSolverVel& _iksolver, @@ -50,13 +59,15 @@ namespace KDL for(unsigned int j=0; j q_max(j)) - q_out(j) = q_max(j); + //q_out(j) = q_max(j); // FreeCAD change + q_out(j) = q_out(j) - M_PI *2; } } diff --git a/src/Mod/Robot/App/kdl_cp/trajectory.hpp b/src/Mod/Robot/App/kdl_cp/trajectory.hpp index f633a0ce3..b6a67fc8a 100644 --- a/src/Mod/Robot/App/kdl_cp/trajectory.hpp +++ b/src/Mod/Robot/App/kdl_cp/trajectory.hpp @@ -78,6 +78,12 @@ namespace KDL { class Trajectory { public: + virtual Path* GetPath() = 0; + // The underlying Path - FreeCAD change + + virtual VelocityProfile* GetProfile() = 0; + // The velocity profile - FreeCAD change + virtual double Duration() const = 0; // The duration of the trajectory diff --git a/src/Mod/Robot/App/kdl_cp/trajectory_composite.cpp b/src/Mod/Robot/App/kdl_cp/trajectory_composite.cpp index 82f4851b6..73bec462a 100644 --- a/src/Mod/Robot/App/kdl_cp/trajectory_composite.cpp +++ b/src/Mod/Robot/App/kdl_cp/trajectory_composite.cpp @@ -23,6 +23,7 @@ namespace KDL { Trajectory_Composite::Trajectory_Composite():duration(0.0) { + path = new Path_Composite(); // FreeCAD change } double Trajectory_Composite::Duration() const{ @@ -92,6 +93,7 @@ namespace KDL { vt.insert(vt.end(),elem); duration += elem->Duration(); vd.insert(vd.end(),duration); + path->Add(elem->GetPath(),false); // FreeCAD change } void Trajectory_Composite::Destroy() { @@ -101,6 +103,8 @@ namespace KDL { } vt.erase(vt.begin(),vt.end()); vd.erase(vd.begin(),vd.end()); + + delete path; // FreeCAD change } Trajectory_Composite::~Trajectory_Composite() { @@ -124,7 +128,17 @@ namespace KDL { } return comp; } + + // FreeCAD change + Path* Trajectory_Composite::GetPath() + { + return path; + } + VelocityProfile* Trajectory_Composite::GetProfile() + { + return 0; + } } diff --git a/src/Mod/Robot/App/kdl_cp/trajectory_composite.hpp b/src/Mod/Robot/App/kdl_cp/trajectory_composite.hpp index a110e390f..14523af11 100644 --- a/src/Mod/Robot/App/kdl_cp/trajectory_composite.hpp +++ b/src/Mod/Robot/App/kdl_cp/trajectory_composite.hpp @@ -37,6 +37,7 @@ class Trajectory_Composite: public Trajectory VectorDouble vd; // contains end time for each Trajectory double duration; // total duration of the composed // Trajectory + Path_Composite* path; // FreeCAD change public: Trajectory_Composite(); @@ -53,6 +54,11 @@ class Trajectory_Composite: public Trajectory virtual void Destroy(); virtual void Write(std::ostream& os) const; virtual Trajectory* Clone() const; + virtual Path* GetPath(); // FreeCAD change + virtual VelocityProfile* GetProfile(); // FreeCAD change + + // access the single members + Trajectory *Get(unsigned int n){return vt[n];} // FreeCAD change virtual ~Trajectory_Composite(); }; diff --git a/src/Mod/Robot/App/kdl_cp/trajectory_stationary.hpp b/src/Mod/Robot/App/kdl_cp/trajectory_stationary.hpp index 162fa4e48..1d5343b8e 100644 --- a/src/Mod/Robot/App/kdl_cp/trajectory_stationary.hpp +++ b/src/Mod/Robot/App/kdl_cp/trajectory_stationary.hpp @@ -29,9 +29,22 @@ namespace KDL { { double duration; Frame pos; + VelocityProfile* prof; // FreeCAD change + Path* path; // FreeCAD change public: Trajectory_Stationary(double _duration,const Frame& _pos): duration(_duration),pos(_pos) {} + + // FreeCAD change + virtual Path* GetPath() { + return path; + } + + // FreeCAD change + virtual VelocityProfile* GetProfile() { + return prof; + } + virtual double Duration() const { return duration; } @@ -49,6 +62,7 @@ namespace KDL { virtual Trajectory* Clone() const { return new Trajectory_Stationary(duration,pos); } + virtual ~Trajectory_Stationary() {} }; diff --git a/src/Mod/Robot/App/kdl_cp/utilities/rallNd.h b/src/Mod/Robot/App/kdl_cp/utilities/rallNd.h index 81b5ecba5..53a3ad3d2 100644 --- a/src/Mod/Robot/App/kdl_cp/utilities/rallNd.h +++ b/src/Mod/Robot/App/kdl_cp/utilities/rallNd.h @@ -76,7 +76,7 @@ public: template <> class RallNd<2> : public Rall2d { public: - RallNd() {}* (dwz. met evenveel numerieke operaties als een + RallNd() {} /* (dwz. met evenveel numerieke operaties als een */ RallNd(const Rall2d& arg) : Rall2d(arg) {} RallNd(double value,double d[]) { From ba2e003b92c563c9e6aa1a3c1fb4d9f26caca635 Mon Sep 17 00:00:00 2001 From: Yorik van Havre Date: Wed, 14 Oct 2015 17:54:59 -0300 Subject: [PATCH 18/20] Added changes to KDL brought after the SVN merge, where applicable Taken from commits 470880e, df4c99f, cd0ae20, efb0823, c519989 --- src/Mod/Robot/App/kdl_cp/chaindynparam.hpp | 2 +- .../App/kdl_cp/chainiksolvervel_pinv_nso.hpp | 6 +++++ src/Mod/Robot/App/kdl_cp/frameacc.hpp | 24 +++++++++---------- src/Mod/Robot/App/kdl_cp/frames.cpp | 2 +- src/Mod/Robot/App/kdl_cp/frames.inl | 10 ++++---- src/Mod/Robot/App/kdl_cp/jacobian.cpp | 4 ++-- src/Mod/Robot/App/kdl_cp/jacobian.hpp | 2 +- .../App/kdl_cp/jntspaceinertiamatrix.cpp | 2 +- src/Mod/Robot/App/kdl_cp/path_composite.cpp | 4 ++-- src/Mod/Robot/App/kdl_cp/tree.cpp | 2 +- 10 files changed, 32 insertions(+), 26 deletions(-) diff --git a/src/Mod/Robot/App/kdl_cp/chaindynparam.hpp b/src/Mod/Robot/App/kdl_cp/chaindynparam.hpp index 287d08cf5..2592de318 100644 --- a/src/Mod/Robot/App/kdl_cp/chaindynparam.hpp +++ b/src/Mod/Robot/App/kdl_cp/chaindynparam.hpp @@ -57,7 +57,7 @@ namespace KDL { const Chain chain; int nr; unsigned int nj; - unsigned int ns; + unsigned int ns; Vector grav; Vector vectornull; JntArray jntarraynull; diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.hpp b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.hpp index 20760f859..b9f217513 100644 --- a/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.hpp +++ b/src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.hpp @@ -28,6 +28,10 @@ namespace KDL { + + // FIXME: seems this class is unused/unmaintained/unfinished for several years + // it supposed to be either fixer or removed + /** * Implementation of a inverse velocity kinematics algorithm based * on the generalize pseudo inverse to calculate the velocity @@ -60,6 +64,8 @@ namespace KDL * @param alpha the null-space velocity gain * */ + + // FIXME: alpha is int but is initialized with a float value. ChainIkSolverVel_pinv_nso(const Chain& chain, JntArray opt_pos, JntArray weights, double eps=0.00001,int maxiter=150, double alpha = 0.25); explicit ChainIkSolverVel_pinv_nso(const Chain& chain, double eps=0.00001,int maxiter=150, double alpha = 0.25); ~ChainIkSolverVel_pinv_nso(); diff --git a/src/Mod/Robot/App/kdl_cp/frameacc.hpp b/src/Mod/Robot/App/kdl_cp/frameacc.hpp index eb54cfdac..4bda4cd3f 100644 --- a/src/Mod/Robot/App/kdl_cp/frameacc.hpp +++ b/src/Mod/Robot/App/kdl_cp/frameacc.hpp @@ -45,18 +45,18 @@ class FrameAcc; class RotationAcc; class VectorAcc; -IMETHOD bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps=epsilon); -IMETHOD bool Equal(const Frame& r1,const FrameAcc& r2,double eps=epsilon); -IMETHOD bool Equal(const FrameAcc& r1,const Frame& r2,double eps=epsilon); -IMETHOD bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps=epsilon); -IMETHOD bool Equal(const Rotation& r1,const RotationAcc& r2,double eps=epsilon); -IMETHOD bool Equal(const RotationAcc& r1,const Rotation& r2,double eps=epsilon); -IMETHOD bool Equal(const TwistAcc& a,const TwistAcc& b,double eps=epsilon); -IMETHOD bool Equal(const Twist& a,const TwistAcc& b,double eps=epsilon); -IMETHOD bool Equal(const TwistAcc& a,const Twist& b,double eps=epsilon); -IMETHOD bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps=epsilon); -IMETHOD bool Equal(const Vector& r1,const VectorAcc& r2,double eps=epsilon); -IMETHOD bool Equal(const VectorAcc& r1,const Vector& r2,double eps=epsilon); +IMETHOD bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps); +IMETHOD bool Equal(const Frame& r1,const FrameAcc& r2,double eps); +IMETHOD bool Equal(const FrameAcc& r1,const Frame& r2,double eps); +IMETHOD bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps); +IMETHOD bool Equal(const Rotation& r1,const RotationAcc& r2,double eps); +IMETHOD bool Equal(const RotationAcc& r1,const Rotation& r2,double eps); +IMETHOD bool Equal(const TwistAcc& a,const TwistAcc& b,double eps); +IMETHOD bool Equal(const Twist& a,const TwistAcc& b,double eps); +IMETHOD bool Equal(const TwistAcc& a,const Twist& b,double eps); +IMETHOD bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps); +IMETHOD bool Equal(const Vector& r1,const VectorAcc& r2,double eps); +IMETHOD bool Equal(const VectorAcc& r1,const Vector& r2,double eps); class VectorAcc { diff --git a/src/Mod/Robot/App/kdl_cp/frames.cpp b/src/Mod/Robot/App/kdl_cp/frames.cpp index 4b9ec2de5..0b42a30d0 100644 --- a/src/Mod/Robot/App/kdl_cp/frames.cpp +++ b/src/Mod/Robot/App/kdl_cp/frames.cpp @@ -387,7 +387,7 @@ double Rotation::GetRotAngle(Vector& axis,double eps) const { bool operator==(const Rotation& a,const Rotation& b) { #ifdef KDL_USE_EQUAL - return Equal(a,b); + return Equal(a,b,epsilon); #else return ( a.data[0]==b.data[0] && a.data[1]==b.data[1] && diff --git a/src/Mod/Robot/App/kdl_cp/frames.inl b/src/Mod/Robot/App/kdl_cp/frames.inl index d85a25935..e9eccc3cb 100644 --- a/src/Mod/Robot/App/kdl_cp/frames.inl +++ b/src/Mod/Robot/App/kdl_cp/frames.inl @@ -1267,7 +1267,7 @@ IMETHOD void posrandom(Frame& F) { IMETHOD bool operator==(const Frame& a,const Frame& b ) { #ifdef KDL_USE_EQUAL - return Equal(a,b); + return Equal(a,b,epsilon); #else return (a.p == b.p && a.M == b.M ); @@ -1280,7 +1280,7 @@ IMETHOD bool operator!=(const Frame& a,const Frame& b) { IMETHOD bool operator==(const Vector& a,const Vector& b) { #ifdef KDL_USE_EQUAL - return Equal(a,b); + return Equal(a,b,epsilon); #else return (a.data[0]==b.data[0]&& a.data[1]==b.data[1]&& @@ -1294,7 +1294,7 @@ IMETHOD bool operator!=(const Vector& a,const Vector& b) { IMETHOD bool operator==(const Twist& a,const Twist& b) { #ifdef KDL_USE_EQUAL - return Equal(a,b); + return Equal(a,b,epsilon); #else return (a.rot==b.rot && a.vel==b.vel ); @@ -1307,7 +1307,7 @@ IMETHOD bool operator!=(const Twist& a,const Twist& b) { IMETHOD bool operator==(const Wrench& a,const Wrench& b ) { #ifdef KDL_USE_EQUAL - return Equal(a,b); + return Equal(a,b,epsilon); #else return (a.force==b.force && a.torque==b.torque ); @@ -1323,7 +1323,7 @@ IMETHOD bool operator!=(const Rotation& a,const Rotation& b) { IMETHOD bool operator==(const Vector2& a,const Vector2& b) { #ifdef KDL_USE_EQUAL - return Equal(a,b); + return Equal(a,b,epsilon); #else return (a.data[0]==b.data[0]&& a.data[1]==b.data[1] ); diff --git a/src/Mod/Robot/App/kdl_cp/jacobian.cpp b/src/Mod/Robot/App/kdl_cp/jacobian.cpp index 40018d6cb..7c49c18c0 100644 --- a/src/Mod/Robot/App/kdl_cp/jacobian.cpp +++ b/src/Mod/Robot/App/kdl_cp/jacobian.cpp @@ -126,12 +126,12 @@ namespace KDL bool Jacobian::operator ==(const Jacobian& arg)const { - return Equal((*this),arg); + return Equal((*this),arg,epsilon); } bool Jacobian::operator!=(const Jacobian& arg)const { - return !Equal((*this),arg); + return !Equal((*this),arg,epsilon); } bool Equal(const Jacobian& a,const Jacobian& b,double eps) diff --git a/src/Mod/Robot/App/kdl_cp/jacobian.hpp b/src/Mod/Robot/App/kdl_cp/jacobian.hpp index 35b491b08..4c34e8c3b 100644 --- a/src/Mod/Robot/App/kdl_cp/jacobian.hpp +++ b/src/Mod/Robot/App/kdl_cp/jacobian.hpp @@ -29,7 +29,7 @@ namespace KDL { // Equal is friend function, but default arguments for friends are forbidden (§8.3.6.4) class Jacobian; - bool Equal(const Jacobian& a,const Jacobian& b,double eps=epsilon); + bool Equal(const Jacobian& a,const Jacobian& b,double eps); class Jacobian diff --git a/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.cpp b/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.cpp index dea5cb646..b1bc10528 100644 --- a/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.cpp +++ b/src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.cpp @@ -115,7 +115,7 @@ namespace KDL return src1.data.isApprox(src2.data,eps); } - bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);}; + bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2,epsilon);}; //bool operator!=(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2){return Equal(src1,src2);}; } diff --git a/src/Mod/Robot/App/kdl_cp/path_composite.cpp b/src/Mod/Robot/App/kdl_cp/path_composite.cpp index 62e099d51..6bae1de3e 100644 --- a/src/Mod/Robot/App/kdl_cp/path_composite.cpp +++ b/src/Mod/Robot/App/kdl_cp/path_composite.cpp @@ -132,13 +132,13 @@ int Path_Composite::GetNrOfSegments() { Path* Path_Composite::GetSegment(int i) { assert(i>=0); - assert(i(dv.size())); return gv[i].first; } double Path_Composite::GetLengthToEndOfSegment(int i) { assert(i>=0); - assert(i(dv.size())); return dv[i]; } diff --git a/src/Mod/Robot/App/kdl_cp/tree.cpp b/src/Mod/Robot/App/kdl_cp/tree.cpp index 1ca751489..50425e298 100644 --- a/src/Mod/Robot/App/kdl_cp/tree.cpp +++ b/src/Mod/Robot/App/kdl_cp/tree.cpp @@ -33,8 +33,8 @@ Tree::Tree(const std::string& _root_name) : Tree::Tree(const Tree& in) { segments.clear(); - nrOfSegments = 0; nrOfJoints = 0; + nrOfSegments = 0; root_name = in.root_name; segments.insert(make_pair(root_name, TreeElement::Root(root_name))); From 52d5b8ab7f527324149defe6336ff51e1cef5955 Mon Sep 17 00:00:00 2001 From: Yorik van Havre Date: Thu, 15 Oct 2015 11:55:52 -0300 Subject: [PATCH 19/20] Robot: Added missing header in KDL --- src/Mod/Robot/App/CMakeLists.txt | 4 ++-- src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.cpp | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/Mod/Robot/App/CMakeLists.txt b/src/Mod/Robot/App/CMakeLists.txt index bae201dbe..837482984 100644 --- a/src/Mod/Robot/App/CMakeLists.txt +++ b/src/Mod/Robot/App/CMakeLists.txt @@ -1,7 +1,7 @@ if(MSVC) - add_definitions(-DFCAppRobot -DHAVE_ACOSH -DHAVE_ASINH -DHAVE_ATANH -DEIGEN2_SUPPORT) + add_definitions(-DFCAppRobot -DHAVE_ACOSH -DHAVE_ASINH -DHAVE_ATANH) else(MSVC) - add_definitions(-DHAVE_LIMITS_H -DHAVE_CONFIG_H -DEIGEN2_SUPPORT -DEIGEN_NO_EIGEN2_DEPRECATED_WARNING) + add_definitions(-DHAVE_LIMITS_H -DHAVE_CONFIG_H) endif(MSVC) diff --git a/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.cpp b/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.cpp index 1631b4bd9..c95288449 100644 --- a/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.cpp +++ b/src/Mod/Robot/App/kdl_cp/velocityprofile_traphalf.cpp @@ -41,6 +41,7 @@ //#include "error.h" +#include #include "velocityprofile_traphalf.hpp" namespace KDL { From 2e6c94fc8e2641e26611632d95e6a28a1618aa89 Mon Sep 17 00:00:00 2001 From: wmayer Date: Thu, 15 Oct 2015 19:18:07 +0200 Subject: [PATCH 20/20] + suppress kdl related warnings with msvc --- src/Mod/Robot/App/kdl_cp/utilities/utility.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/Mod/Robot/App/kdl_cp/utilities/utility.h b/src/Mod/Robot/App/kdl_cp/utilities/utility.h index 198f3ffb3..a25295e80 100644 --- a/src/Mod/Robot/App/kdl_cp/utilities/utility.h +++ b/src/Mod/Robot/App/kdl_cp/utilities/utility.h @@ -81,6 +81,10 @@ namespace KDL { #ifndef __GNUC__ //only real solution : get Rall1d and varia out of namespaces. #pragma warning (disable:4786) + #pragma warning (disable:4244) + #pragma warning (disable:4267) + #pragma warning (disable:4800) + #pragma warning (disable:4996) inline double sin(double a) { return ::sin(a);