Merge remote-tracking branch 'upstream/master'

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.

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

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

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

src/Mod/Fem/CMakeLists.txt

@@ -24,6 +24,7 @@ INSTALL(
         MechanicalMaterial.ui
         MechanicalAnalysis.ui
         ShowDisplacement.ui
+        FemCommands.py
         _ResultControlTaskPanel.py
         _JobControlTaskPanel.py
         _ViewProviderFemAnalysis.py

src/Mod/Fem/FemCommands.py

@@ -0,0 +1,74 @@
+#***************************************************************************
+#*                                                                         *
+#*   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
+            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):
+            results = False
+            analysis_members = FemGui.getActiveAnalysis().Member
+            for o in analysis_members:
+                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

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");

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

src/Mod/Fem/_CommandFrequencyAnalysis.py

@@ -1,18 +1,46 @@
+#***************************************************************************
+#*                                                                         *
+#*   Copyright (c) 2013-2015 - Juergen Riegel <FreeCAD@juergen-riegel.net> *
+#*                                                                         *
+#*   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())

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

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

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

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

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

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<Base::VectorPy*>(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<Base::Vector3f> local = polyFit.GetLocalPoints();
+        Py::Tuple r(local.size());
+        for (std::vector<Base::Vector3f>::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 */
 };

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(

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

src/Mod/Mesh/App/Core/Approximation.cpp

@@ -38,28 +38,22 @@
 #include <Mod/Mesh/App/WildMagic4/Wm4ApprPolyFit3.h>
 
 //#define FC_USE_EIGEN
-//#define FC_USE_BOOST
-#if defined(FC_USE_BOOST)
-#include <boost/numeric/ublas/io.hpp>
-#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
-#include <boost/numeric/bindings/traits/ublas_vector.hpp>
-
-#define BOOST_NUMERIC_BINDINGS_USE_CLAPACK
-#include <boost/numeric/bindings/lapack/syev.hpp>
-#include <boost/numeric/bindings/lapack/heev.hpp>
-#include <boost/numeric/bindings/lapack/gesv.hpp> 
-
-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 <Eigen/QR>
+#ifdef FC_USE_EIGEN
+#include <Eigen/Eigenvalues>
 #endif
 
-# include <Eigen/LeastSquares>
-
 using namespace MeshCore;
 
+Approximation::Approximation()
+  : _bIsFitted(false), _fLastResult(FLOAT_MAX)
+{
+}
+
+Approximation::~Approximation()
+{
+    Clear();
+}
 
 void Approximation::Convert( const Wm4::Vector3<double>& 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<double> 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<double> 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<Base::Vector3f> PlaneFit::GetLocalPoints() const
+{
+    std::vector<Base::Vector3f> 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<Base::Vector3f>::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<float>(clPoint.x), static_cast<float>(clPoint.y), static_cast<float>(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<double> A(6,6);
-    ublas::vector<double> 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<double,6,6> A = Eigen::Matrix<double,6,6>::Zero();
     Eigen::Matrix<double,6,1> b = Eigen::Matrix<double,6,1>::Zero();
     Eigen::Matrix<double,6,1> x = Eigen::Matrix<double,6,1>::Zero();
-#endif
 
+    std::vector<Base::Vector3d> transform;
+    transform.reserve(_vPoints.size());
+
+    double dW2 = 0;
     for (std::list<Base::Vector3f>::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<double> 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<double,6,6> > 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<Base::Vector3d>::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<float>(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;

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<Base::Vector3f> 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();

src/Mod/Mesh/App/Core/Trim.cpp

@@ -71,7 +71,7 @@ void MeshTrimming::CheckFacets(const MeshFacetGrid& rclGrid, std::vector<unsigne
         for (clGridIter.Init(); clGridIter.More(); clGridIter.Next()) {
             clBBox3d = clGridIter.GetBoundBox();
             clViewBBox = clBBox3d.ProjectBox(myProj);
-            if (clViewBBox || clPolyBBox) {
+            if (clViewBBox.Intersect(clPolyBBox)) {
                 // save all elements in AllElements 
                 clGridIter.GetElements(aulAllElements);
             }

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

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

src/Mod/Path/App/Path.cpp

@@ -34,10 +34,11 @@
 #include <Base/Stream.h>
 #include <Base/Exception.h>
 
-#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"
 

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 <Base/Persistence.h>
 #include <Base/Vector3D.h>
 
@@ -70,8 +70,9 @@ namespace Path
         
         protected:
             std::vector<Command*> 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

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

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)
 
 

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 <string>
+    #include <map>
+    #include <vector.hpp>
+
+    class TreeElement;
+    typedef std::map<std::string, TreeElement> SegmentMap;
+
+    class TreeElement
+    {
+        TreeElement(const std::string& name): number(0) {}
+
+    public:
+        int number;
+        SegmentMap::const_iterator parent;
+        std::vector<SegmentMap::const_iterator> 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)

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<SelfAdjoint>()=Matrix3d::Identity()*rbi.m;
-        this->I.part<SelfAdjoint>()=Map<Matrix3d>(rbi.I.data);
+        this->M=Matrix3d::Identity()*rbi.m;
+        this->I=Map<const Matrix3d>(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 Eigen::Matrix3d& M, const Eigen::Matrix3d& H, const Eigen::Matrix3d& I)
     {
-        this->M.part<SelfAdjoint>()=M;
-        this->I.part<SelfAdjoint>()=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<Matrix3d> E(M.data);
+        Map<const Matrix3d> E(M.data);
         return ArticulatedBodyInertia(E.transpose()*I.M*E,E.transpose()*I.H*E,E.transpose()*I.I*E);
     }
 

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

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;i<in.getNrOfSegments();i++)
             this->addSegment(in.getSegment(i));
@@ -43,7 +45,7 @@ namespace KDL {
         nrOfJoints=0;
         nrOfSegments=0;
         segments.resize(0);
-        for(int i=0;i<arg.nrOfSegments;i++)
+        for(unsigned int i=0;i<arg.nrOfSegments;i++)
             addSegment(arg.getSegment(i));
         return *this;
 

src/Mod/Robot/App/kdl_cp/chain.hpp

@@ -34,8 +34,8 @@ namespace KDL {
      */
     class Chain {
     private:
-        int nrOfJoints;
-        int nrOfSegments;
+        unsigned int nrOfJoints;
+        unsigned int nrOfSegments;
     public:
         std::vector<Segment> segments;
         /**

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<ns;i++)
-        {
-          //Collect RigidBodyInertia
+	{
+	  //Collect RigidBodyInertia
           Ic[i]=chain.getSegment(i).getInertia();
           if(chain.getSegment(i).getJoint().getType()!=Joint::None)
-          {
-              q_=q(k);
-              k++;
-          }
-          else
-          {
-            q_=0.0;
-          }
-          X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the frame for transformations from the parent to the current coord frame
-          S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));
+	  {
+	      q_=q(k);
+	      k++;
+	  }
+	  else
+	  {
+	    q_=0.0;
+	  }
+	  X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the frame for transformations from the parent to the current coord frame
+	  S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));  
         }
-        //Sweep from leaf to root
+	//Sweep from leaf to root
         int j,l;
-        k=nj-1; //reset k
+	k=nj-1; //reset k
         for(int i=ns-1;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()

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

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,
@@ -61,7 +62,7 @@ namespace KDL {
      *
      * @ingroup KinematicFamily
      */
-    class ChainFkSolverVel {
+    class ChainFkSolverVel : public KDL::SolverI {
     public:
         /**
          * Calculate forward position and velocity kinematics, from
@@ -85,7 +86,7 @@ namespace KDL {
      * @ingroup KinematicFamily
      */
 
-    class ChainFkSolverAcc {
+    class ChainFkSolverAcc : public KDL::SolverI {
     public:
         /**
          * Calculate forward position, velocity and accelaration

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<int>(chain.getNrOfSegments()))
+        else if(segmentNr>chain.getNrOfSegments())
             return -1;
         else{
             int j=0;
-            for(int i=0;i<segmentNr;i++){
+            for(unsigned int i=0;i<segmentNr;i++){
                 if(chain.getSegment(i).getJoint().getType()!=Joint::None){
                     p_out = p_out*chain.getSegment(i).pose(q_in(j));
                     j++;

src/Mod/Robot/App/kdl_cp/chainfksolvervel_recursive.cpp

@@ -33,21 +33,23 @@ namespace KDL
     {
     }
 
-    int ChainFkSolverVel_recursive::JntToCart(const JntArrayVel& in,FrameVel& out,int segmentNr)
+    int ChainFkSolverVel_recursive::JntToCart(const JntArrayVel& in,FrameVel& out,int seg_nr)
     {
-
-        if(segmentNr<0)
-             segmentNr=chain.getNrOfSegments();
+        unsigned int segmentNr;
+        if(seg_nr<0)
+            segmentNr=chain.getNrOfSegments();
+        else
+            segmentNr = seg_nr;
 
         out=FrameVel::Identity();
 
         if(!(in.q.rows()==chain.getNrOfJoints()&&in.qdot.rows()==chain.getNrOfJoints()))
             return -1;
-        else if(segmentNr>static_cast<int>(chain.getNrOfSegments()))
+        else if(segmentNr>chain.getNrOfSegments())
             return -1;
         else{
             int j=0;
-            for (int i=0;i<segmentNr;i++) {
+            for (unsigned int i=0;i<segmentNr;i++) {
                 //Calculate new Frame_base_ee
                 if(chain.getSegment(i).getJoint().getType()!=Joint::None){
                     out=out*FrameVel(chain.getSegment(i).pose(in.q(j)),

src/Mod/Robot/App/kdl_cp/chainidsolver.hpp

@@ -25,6 +25,7 @@
 #include "chain.hpp"
 #include "frames.hpp"
 #include "jntarray.hpp"
+#include "solveri.hpp"
 
 namespace KDL
 {
@@ -36,12 +37,12 @@ namespace KDL
 	 * dynamics solver for a KDL::Chain.
 	 *
 	 */
-	class ChainIdSolver
+	class ChainIdSolver : public KDL::SolverI
 	{
 		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

src/Mod/Robot/App/kdl_cp/chainidsolver_recursive_newton_euler.cpp

@@ -23,7 +23,7 @@
 #include "frames_io.hpp"
 
 namespace KDL{
-
+    
     ChainIdSolver_RNE::ChainIdSolver_RNE(const Chain& chain_,Vector grav):
         chain(chain_),nj(chain.getNrOfJoints()),ns(chain.getNrOfSegments()),
         X(ns),S(ns),v(ns),a(ns),f(ns)
@@ -35,7 +35,7 @@ namespace KDL{
     {
         //Check sizes when in debug mode
         if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns)
-            return -1;
+            return (error = -1);
         unsigned int j=0;
 
         //Sweep from root to leaf
@@ -48,10 +48,10 @@ namespace KDL{
                 j++;
             }else
                 q_=qdot_=qdotdot_=0.0;
-
+            
             //Calculate segment properties: X,S,vj,cj
-            X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the
-                                                //frame for transformations from
+            X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the 
+                                                //frame for transformations from 
                                                 //the parent to the current coord frame
             //Transform velocity and unit velocity to segment frame
             Twist vj=X[i].M.Inverse(chain.getSegment(i).twist(q_,qdot_));
@@ -69,7 +69,7 @@ namespace KDL{
             //Collect RigidBodyInertia and external forces
             RigidBodyInertia Ii=chain.getSegment(i).getInertia();
             f[i]=Ii*a[i]+v[i]*(Ii*v[i])-f_ext[i];
-            //std::cout << "a[i]=" << a[i] << "\n f[i]=" << f[i] << "\n S[i]" << S[i] << std::endl;
+	    //std::cout << "a[i]=" << a[i] << "\n f[i]=" << f[i] << "\n S[i]" << S[i] << std::endl;
         }
         //Sweep from leaf to root
         j=nj-1;
@@ -79,6 +79,6 @@ namespace KDL{
             if(i!=0)
                 f[i-1]=f[i-1]+X[i]*f[i];
         }
-        return 0;
+	return (error = E_NOERROR);
     }
 }//namespace

src/Mod/Robot/App/kdl_cp/chainidsolver_vereshchagin.cpp

@@ -0,0 +1,472 @@
+// 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
+
+#include "chainidsolver_vereshchagin.hpp"
+#include "frames_io.hpp"
+#include "utilities/svd_eigen_HH.hpp"
+
+
+namespace KDL
+{
+using namespace Eigen;
+
+ChainIdSolver_Vereshchagin::ChainIdSolver_Vereshchagin(const Chain& chain_, Twist root_acc, unsigned int _nc) :
+    chain(chain_), nj(chain.getNrOfJoints()), ns(chain.getNrOfSegments()), nc(_nc),
+    results(ns + 1, segment_info(nc))
+{
+    acc_root = root_acc;
+
+    //Provide the necessary memory for computing the inverse of M0
+    nu_sum.resize(nc);
+    M_0_inverse.resize(nc, nc);
+    Um = MatrixXd::Identity(nc, nc);
+    Vm = MatrixXd::Identity(nc, nc);
+    Sm = VectorXd::Ones(nc);
+    tmpm = VectorXd::Ones(nc);
+}
+
+int ChainIdSolver_Vereshchagin::CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques)
+{
+    //Check sizes always
+    if (q.rows() != nj || q_dot.rows() != nj || q_dotdot.rows() != nj || torques.rows() != nj || f_ext.size() != ns)
+        return -1;
+    if (alfa.columns() != nc || beta.rows() != nc)
+        return -2;
+    //do an upward recursion for position and velocities
+    this->initial_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: "<<results[0].M<<std::endl;
+    //results[0].M-=MatrixXd::Identity(nc,nc);
+    //std::cout<<"augmented M0: "<<results[0].M<<std::endl;
+
+
+    //ToDo: Need to check ill conditions
+
+    //M_0_inverse=results[0].M.inverse();
+    svd_eigen_HH(results[0].M, Um, Sm, Vm, tmpm);
+    //truncated svd, what would sdls, dls physically mean?
+    for (unsigned int i = 0; i < nc; i++)
+        if (Sm(i) < 1e-14)
+            Sm(i) = 0.0;
+        else
+            Sm(i) = 1 / Sm(i);
+
+    results[0].M.noalias() = Vm * Sm.asDiagonal();
+    M_0_inverse.noalias() = results[0].M * Um.transpose();
+    //results[0].M.ldlt().solve(MatrixXd::Identity(nc,nc),&M_0_inverse);
+    //results[0].M.computeInverse(&M_0_inverse);
+    Vector6d acc;
+    acc << Vector3d::Map(acc_root.rot.data), Vector3d::Map(acc_root.vel.data);
+    nu_sum.noalias() = -(results[0].E_tilde.transpose() * acc);
+    //nu_sum.setZero();
+    nu_sum += beta.data;
+    nu_sum -= results[0].G;
+
+    //equation f) nu = M_0_inverse*(beta_N - E0_tilde`*acc0 - G0)
+    nu.noalias() = M_0_inverse * nu_sum;
+}
+
+void ChainIdSolver_Vereshchagin::final_upwards_sweep(JntArray &q_dotdot, JntArray &torques)
+{
+    unsigned int j = 0;
+
+    for (unsigned int i = 1; i <= ns; i++)
+    {
+        segment_info& s = results[i];
+        //Calculation of joint and segment accelerations
+        //equation g) qdotdot[i] = D^-1*(Q - Z'(R + P(C + acc[i-1]) + E*nu))
+        // = D^-1(u - Z'(P*acc[i-1] + E*nu)
+        Twist a_g;
+        Twist a_p;
+        if (i == 1)
+        {
+            a_p = acc_root;
+        }
+        else
+        {
+            a_p = results[i - 1].acc;
+        }
+
+        //The contribution of the constraint forces at segment i
+        Vector6d tmp = s.E*nu;
+        Wrench constraint_force = Wrench(Vector(tmp(3), tmp(4), tmp(5)),
+                                         Vector(tmp(0), tmp(1), tmp(2)));
+
+        //acceleration components are also computed
+        //Contribution of the acceleration of the parent (i-1)
+        Wrench parent_force = s.P*a_p;
+        double parent_forceProjection = -dot(s.Z, parent_force);
+        double parentAccComp = parent_forceProjection / s.D;
+
+        //The constraint force and acceleration force projected on the joint axes -> 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

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<Twist> Twists;
+    typedef std::vector<Frame> Frames;
+    typedef Eigen::Matrix<double, 6, 1 > Vector6d;
+    typedef Eigen::Matrix<double, 6, 6 > Matrix6d;
+    typedef Eigen::Matrix<double, 6, Eigen::Dynamic> 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<segment_info> results;
+
+};
+}
+
+#endif

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

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 <iostream>
+
+namespace KDL {
+
+
+
+
+template <typename Derived>
+inline void Twist_to_Eigen(const KDL::Twist& t,Eigen::MatrixBase<Derived>& 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<double,6,1>& _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<ScalarType>()),
+	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<chain.getNrOfSegments();i++) {
+		const Segment& segment = chain.getSegment(i);
+		if (segment.getJoint().getType()!=Joint::None) {
+			T_base_jointroot[jointndx] = T_base_head;
+			T_base_head = T_base_head * segment.pose(q(jointndx));
+			T_base_jointtip[jointndx] = T_base_head;
+			jointndx++;
+		} else {
+			T_base_head = T_base_head * segment.pose(0.0);
+		}
+	}
+}
+
+void ChainIkSolverPos_LMA::compute_jacobian(const VectorXq& q) {
+	using namespace KDL;
+	unsigned int jointndx=0;
+	for (unsigned int i=0;i<chain.getNrOfSegments();i++) {
+		const Segment& segment = chain.getSegment(i);
+		if (segment.getJoint().getType()!=Joint::None) {
+			// compute twist of the end effector motion caused by joint [jointndx]; expressed in base frame, with vel. ref. point equal to the end effector
+			KDL::Twist t = ( T_base_jointroot[jointndx].M * segment.twist(q(jointndx),1.0) ).RefPoint( T_base_head.p - T_base_jointtip[jointndx].p);
+			jac(0,jointndx)=t[0];
+			jac(1,jointndx)=t[1];
+			jac(2,jointndx)=t[2];
+			jac(3,jointndx)=t[3];
+			jac(4,jointndx)=t[4];
+			jac(5,jointndx)=t[5];
+			jointndx++;
+		}
+	}
+}
+
+void ChainIkSolverPos_LMA::display_jac(const KDL::JntArray& jval) {
+	VectorXq q;
+	q = jval.data.cast<ScalarType>();
+	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<ScalarType,6,1> delta_pos;
+	Eigen::Matrix<ScalarType,6,1> delta_pos_new;
+
+
+	q=q_init.data.cast<ScalarType>();
+	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<eps) {
+		lastNrOfIter    =0 ;
+		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();
+		svd.compute(jac);
+		original_Aii    = svd.singularValues();
+		lastSV          = svd.singularValues();
+		q_out.data      = q.cast<double>();
+		return 0;
+	}
+	compute_jacobian(q);
+	jac = L.asDiagonal()*jac;
+
+	lambda = tau;
+	double dnorm = 1;
+	for (unsigned int i=0;i<maxiter;++i) {
+
+		svd.compute(jac);
+		original_Aii = svd.singularValues();
+		for (unsigned int j=0;j<original_Aii.rows();++j) {
+			original_Aii(j) = original_Aii(j)/( original_Aii(j)*original_Aii(j)+lambda);
+
+		}
+		tmp = svd.matrixU().transpose()*delta_pos;
+		tmp = original_Aii.cwiseProduct(tmp);
+		diffq = svd.matrixV()*tmp;
+		grad = jac.transpose()*delta_pos;
+		if (display_information) {
+			std::cout << "------- iteration " << i << " ----------------\n"
+					  << "  q              = " << q.transpose() << "\n"
+					  << "  weighted jac   = \n" << jac << "\n"
+					  << "  lambda         = " << lambda << "\n"
+					  << "  eigenvalues    = " << svd.singularValues().transpose() << "\n"
+					  << "  difference     = "   << delta_pos.transpose() << "\n"
+					  << "  difference norm= "   << delta_pos_norm << "\n"
+					  << "  proj. on grad. = "   << grad << "\n";
+			std::cout << std::endl;
+		}
+		dnorm = diffq.lpNorm<Eigen::Infinity>();
+		if (dnorm < eps_joints) {
+				lastDifference = delta_pos_norm;
+				lastNrOfIter   = i;
+				lastSV         = svd.singularValues();
+				q_out.data     = q.cast<double>();
+				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<double>();
+			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<eps) {
+				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<double>();
+				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<double>();
+	return -3;
+
+}
+
+
+
+};//namespace KDL

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 <Eigen/Dense>
+
+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. <B>Weights also
+ *     make the solver more robust </B>.
+ *   - only the constructors call <B>memory allocation</B>.
+ *
+ * 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<ScalarType,Eigen::Dynamic,Eigen::Dynamic> MatrixXq;
+    typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,1> 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; <B>after</B> 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<double,6,1>& _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<ScalarType,6,1> L;
+    const KDL::Chain& chain;
+
+
+    // state of compute_fwdpos and compute_jacobian:
+    std::vector<KDL::Frame> T_base_jointroot;
+    std::vector<KDL::Frame> 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<MatrixXq> ldlt;
+    Eigen::JacobiSVD<MatrixXq> svd;
+    VectorXq diffq;
+    VectorXq q_new;
+    VectorXq original_Aii;
+};
+
+
+
+
+
+}; // namespace KDL
+
+
+
+
+
+
+#endif

src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr.cpp

@@ -38,20 +38,27 @@ namespace KDL
             for(i=0;i<maxiter;i++){
                 fksolver.JntToCart(q_out,f);
                 delta_twist = diff(f,p_in);
-                iksolver.CartToJnt(q_out,delta_twist,delta_q);
+                const int rc = iksolver.CartToJnt(q_out,delta_twist,delta_q);
+                if (E_NOERROR > 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);
+    }
 }
 

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 <eps in maxiter
+         *  E_DEGRADED=solution converged to <eps in maxiter, but solution is
+         *  degraded in quality (e.g. pseudo-inverse in iksolver is singular)
+         *  E_IKSOLVER_FAILED=velocity solver failed
+         *  E_NO_CONVERGE=solution did not converge (e.g. large displacement, low iterations)
+         */
         virtual int CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out);
 
+        /// @copydoc KDL::SolverI::strError()
+        virtual const char* strError(const int error) const;
+
     private:
         const Chain chain;
         ChainIkSolverVel& iksolver;

src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.cpp

@@ -23,24 +23,23 @@
 
 #include "chainiksolverpos_nr_jl.hpp"
 
+// FreeCAD change
+#ifndef M_PI
+    #define M_PI    3.14159265358979323846 /* pi */
+#endif
 
-#ifndef M_PI
-    #define M_PI    3.14159265358979323846 /* pi */
-#endif
-
-#ifndef M_PI_2
-    #define M_PI_2  1.57079632679489661923 /* pi/2 */
-#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,
                                              unsigned int _maxiter, double _eps):
-        chain(_chain), q_min(chain.getNrOfJoints()), q_max(chain.getNrOfJoints()), iksolver(_iksolver), fksolver(_fksolver),delta_q(_chain.getNrOfJoints()),
+        chain(_chain), q_min(_q_min), q_max(_q_max), iksolver(_iksolver), fksolver(_fksolver), delta_q(_chain.getNrOfJoints()),
         maxiter(_maxiter),eps(_eps)
     {
-    	q_min = _q_min;
-    	q_max = _q_max;
+
     }
 
     int ChainIkSolverPos_NR_JL::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
@@ -60,14 +59,14 @@ namespace KDL
 
                 for(unsigned int j=0; j<q_min.rows(); j++) {
                   if(q_out(j) < q_min(j))
-                    //q_out(j) = q_min(j);
+                    //q_out(j) = q_min(j); // FreeCAD change
                     q_out(j) = q_out(j) + M_PI *2;
                 }
 
 
                 for(unsigned int j=0; j<q_max.rows(); j++) {
                     if(q_out(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;
                 }
             }

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

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 (<eps), don't invert it but
             //set the inverted singular value to zero (truncated svd)
-            tmp(i) = sum*(fabs(S(i))<eps?0.0:1.0/S(i));
+            if ( fabs(S(i))<eps ) {
+                tmp(i) = 0.0 ;
+                //  Count number of singular values near zero
+                ++nrZeroSigmas ;
+            }
+            else {
+                tmp(i) = sum/S(i) ;
+            }
         }
         //tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
         //it with V to get qdot_out
@@ -80,8 +97,19 @@ namespace KDL
             //Put the result in qdot_out
             qdot_out(i)=sum;
         }
-        //return the return value of the svd decomposition
-        return ret;
+
+        // Note if the solution is singular, i.e. if number of near zero
+        // singular values is greater than the full rank of jac
+        if ( nrZeroSigmas > (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);
+    }
 }

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 <eps in maxiter
+         *  E_SVD_FAILED=SVD computation failed
+         *  E_CONVERGE_PINV_SINGULAR=solution converged but (pseudo)inverse is 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.
          *
          */
         virtual int CartToJnt(const JntArray& q_init, const FrameVel& v_in, JntArrayVel& q_out){return -1;};
+
+        /**
+         * Retrieve 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;};
+
+        /**
+         * 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;
 
     };
 }

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<m;i++){
-            for(int j=0;j<n;j++)
+        for(unsigned int i=0;i<m;i++){
+            for(unsigned int j=0;j<n;j++)
                 if(transpose)
                     jac_eigen(i,j)=jac(j,i);
                 else
@@ -68,11 +70,11 @@ namespace KDL
         //std::cout<<"# sweeps: "<<ret<<std::endl;
 
         if(transpose)
-            UY = (V.transpose() * v_in_eigen).lazy();
+            UY.noalias() = V.transpose() * v_in_eigen;
         else
-            UY = (U.transpose() * v_in_eigen).lazy();
+            UY.noalias() = U.transpose() * v_in_eigen;
 
-        for (int i = 0; i < n; i++){
+        for (unsigned int i = 0; i < n; i++){
             double wi = UY(i);
             double alpha = S(i);
             
@@ -83,9 +85,9 @@ namespace KDL
             SUY(i)= alpha * wi;
         }
         if(transpose)
-            qdot_eigen = (U * SUY).lazy();
+            qdot_eigen.noalias() = U * SUY;
         else
-            qdot_eigen = (V * SUY).lazy();
+            qdot_eigen.noalias() = V * SUY;
 
         for (unsigned int j=0;j<chain.getNrOfJoints();j++)
             qdot_out(j)=qdot_eigen(j);

src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_givens.hpp

@@ -8,7 +8,7 @@
 
 #include <Eigen/Core>
 
-USING_PART_OF_NAMESPACE_EIGEN;
+using namespace Eigen;
 
 namespace KDL
 {
@@ -31,7 +31,7 @@ namespace KDL
          * kinematics for
          *
          */
-        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 +45,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;
     };

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<jac.columns();i++) {
-            sum = 0.0;
-            for (j=0;j<jac.rows();j++) {
-                sum+= U[j](i)*v_in(j);
-            }
-            //If the singular value is too small (<eps), don't invert it but
-            //set the inverted singular value to zero (truncated svd)
-            tmp(i) = sum*(fabs(S(i))<eps?0.0:1.0/S(i));
+        // S^-1
+        for (i = 0; i < nj; ++i) {
+            Sinv(i) = fabs(S(i))<eps ? 0.0 : 1.0/S(i);
         }
-        //tmp is now: tmp=S_pinv*Ut*v_in, we still have to premultiply
-        //it with V to get qdot_out
-        for (i=0;i<jac.columns();i++) {
-            sum = 0.0;
-            for (j=0;j<jac.columns();j++) {
-                sum+=V[i](j)*tmp(j);
-            }
-            //Put the result in qdot_out
-            qdot_out(i)=sum;
+        for (i = 0; i < 6; ++i) {
+            tmp(i) = v_in(i);
         }
-        
-        //Now onto NULL space
-        
-        for(i = 0; i < jac.columns(); i++)
-            tmp(i) = weights(i)*(opt_pos(i) - q_in(i));
-        
-        //Vtn*tmp
-        for (i=jac.rows()+1;i<jac.columns();i++) {
-            tmp2(i-(jac.rows()+1)) = 0.0;
-            for (j=0;j<jac.columns();j++) {
-                tmp2(i-(jac.rows()+1)) +=V[j](i)*tmp(j);
-            }
+
+        qdot_out.data = V * Sinv.asDiagonal() * U.transpose() * tmp.head(6);
+
+        // Now onto NULL space
+        // Given the cost function g, and the current joints q, desired joints qd, and weights w:
+        // t = g(q) = 1/2 * Sum( w_i * (q_i - qd_i)^2 )
+        //
+        // The jacobian Jc is:
+        //  t_dot = Jc(q) * q_dot
+        //  Jc = dt/dq = w_j * (q_i - qd_i) [1 x nj vector]
+        //
+        // The pseudo inverse (^-1) is
+        // Jc^-1 = w_j * (q_i - qd_i) / A [nj x 1 vector]
+        // A = Sum( w_i^2 * (q_i - qd_i)^2 )
+        //
+        // We can set the change as the step needed to remove the error times a value alpha:
+        // t_dot = -2 * alpha * t
+        //
+        // When we put it together and project into the nullspace, the final result is
+        // q'_out += (I_n - J^-1 * J) * Jc^-1 * (-2 * alpha * g(q))
+
+        double g = 0; // g(q)
+        double A = 0; // normalizing term
+        for (i = 0; i < nj; ++i) {
+            double qd = q_in(i) - opt_pos(i);
+            g += 0.5 * qd*qd * weights(i);
+            A += qd*qd * weights(i)*weights(i);
         }
-        
-        for (i=0;i<jac.columns();i++) {
-            sum = 0.0;
-            for (j=jac.rows()+1;j<jac.columns();j++) {
-                sum +=V[i](j)*tmp2(j);
-            }
-            qdot_out(i) += alpha*sum;
+
+        if (A > 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;
-	}
 
 
 }

src/Mod/Robot/App/kdl_cp/chainiksolvervel_pinv_nso.hpp

@@ -19,16 +19,16 @@
 // 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 <Eigen/Core>
 
 namespace KDL
 {
-
+    
     // FIXME: seems this class is unused/unmaintained/unfinished for several years
     //        it supposed to be either fixer or removed
 
@@ -64,10 +64,10 @@ 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 +77,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 +99,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<JntArray> U;
-        JntArray S;
-        std::vector<JntArray> 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

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)
     {
     }
     
@@ -58,11 +62,21 @@ namespace KDL
         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<jac.rows();i++) {
             for (j=0;j<jac.columns();j++)
@@ -78,17 +97,30 @@ namespace KDL
         */
         
         // Create the Weighted jacobian
-        tmp_jac_weight1 = (jac.data*weight_js).lazy();
-        tmp_jac_weight2 = (weight_ts*tmp_jac_weight1).lazy();
+        tmp_jac_weight1 = jac.data.lazyProduct(weight_js);
+        tmp_jac_weight2 = weight_ts.lazyProduct(tmp_jac_weight1);
    
         // Compute the SVD of the weighted jacobian
-        int ret = svd_eigen_HH(tmp_jac_weight2,U,S,V,tmp,maxiter);
-                
+        svdResult = svd_eigen_HH(tmp_jac_weight2,U,S,V,tmp,maxiter);
+        if (0 != svdResult)
+        {
+            qdot_out.data.setZero() ;
+            return (error = E_SVD_FAILED);
+        }
+
         //Pre-multiply U and V by the task space and joint space weighting matrix respectively
-        tmp_ts = (weight_ts*U.corner(Eigen::TopLeft,6,6)).lazy();
-        tmp_js = (weight_js*V).lazy(); 
-        
-        // tmp = (Si*U'*Ly*y), 
+        tmp_ts = weight_ts.lazyProduct(U.topLeftCorner(6,6));
+        tmp_js = weight_js.lazyProduct(V);
+
+        // Minimum of six largest singular values of J is S(5) if number of joints >=6 and 0 for <6
+        if ( jac.columns() >= 6 ) {
+            sigmaMin = S(5);
+        }
+        else {
+            sigmaMin = 0.;
+        }
+
+        // tmp = (Si*U'*Ly*y),
         for (i=0;i<jac.columns();i++) {
             sum = 0.0;
             for (j=0;j<jac.rows();j++) {
@@ -97,11 +129,29 @@ namespace KDL
                 else
                     sum+=0.0;
             }
-            if(S(i)==0||S(i)<eps)
-                tmp(i) = sum*((S(i)/(S(i)*S(i)+lambda*lambda)));   
-            else
+            // If sigmaMin > 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))<eps) {
+				if (i<6) {
+					// Scale lambda to size of singular value sigmaMin
+					tmp(i) = sum*((S(i)/(S(i)*S(i)+lambda_scaled*lambda_scaled)));
+				}
+				else {
+					tmp(i)=0.0;  // S(i)=0 for 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();i++) {
@@ -112,8 +162,21 @@ namespace KDL
             qdot_out(i)=sum;
         }
         */
-        qdot_out.data=(tmp_js*tmp).lazy();
-        return ret;
+        qdot_out.data=tmp_js.lazyProduct(tmp);
+
+        // If number of near zero singular values is greater than the full rank
+        // of jac, then wdls is active
+        if ( nrZeroSigmas > (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);
+    }
+
 }

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<double>
         ~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.
@@ -90,7 +109,7 @@ namespace KDL
          * Set the joint space weighting matrix
          *
          * weight_js joint space weighting symetric matrix,
-         * default : identity.
+         * 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
@@ -115,7 +134,7 @@ namespace KDL
          * Set the task space weighting matrix
          *
          * weight_ts task space weighting symetric matrix,
-         * default: identity
+         * 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
@@ -137,7 +156,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 +219,10 @@ namespace KDL
         Eigen::MatrixXd weight_ts;
         Eigen::MatrixXd weight_js;
         double lambda;
+		double lambda_scaled;
+		unsigned int nrZeroSigmas ;
+		int svdResult;
+		double sigmaMin;
     };
 }
 #endif

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<int>(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<chain.getNrOfSegments();i++) {
+        for (unsigned int i=0;i<segmentNr;i++) {
             //Calculate new Frame_base_ee
             if(chain.getSegment(i).getJoint().getType()!=Joint::None){
             	//pose of the new end-point expressed in the base
@@ -82,7 +95,13 @@ namespace KDL
 
             T_tmp = total;
         }
-        return 0;
+        return (error = E_NOERROR);
+    }
+
+    const char* ChainJntToJacSolver::strError(const int error) const
+    {
+        if (E_JAC_FAILED == error) return "Jac Failed";
+        else return SolverI::strError(error);
     }
 }
 

src/Mod/Robot/App/kdl_cp/chainjnttojacsolver.hpp

@@ -22,6 +22,7 @@
 #ifndef KDL_CHAINJNTTOJACSOLVER_HPP
 #define KDL_CHAINJNTTOJACSOLVER_HPP
 
+#include "solveri.hpp"
 #include "frames.hpp"
 #include "jacobian.hpp"
 #include "jntarray.hpp"
@@ -37,11 +38,13 @@ namespace KDL
      *
      */
 
-    class ChainJntToJacSolver
+    class ChainJntToJacSolver : public SolverI
     {
     public:
-        ChainJntToJacSolver(const Chain& chain);
-        ~ChainJntToJacSolver();
+        static const int E_JAC_FAILED = -100; //! Jac solver failed
+
+        explicit ChainJntToJacSolver(const Chain& chain);
+        virtual ~ChainJntToJacSolver();
         /**
          * Calculate the jacobian expressed in the base frame of the
          * chain, with reference point at the end effector of the
@@ -53,15 +56,19 @@ namespace KDL
          *
          * @return always returns 0
          */
-        int JntToJac(const JntArray& q_in,Jacobian& jac);
+        virtual int JntToJac(const JntArray& q_in, Jacobian& jac, int segmentNR=-1);
         
         int setLockedJoints(const std::vector<bool> 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<bool> locked_joints_;
-        int nr_of_unlocked_joints_;
+        unsigned int nr_of_unlocked_joints_;
     };
 }
 #endif

src/Mod/Robot/App/kdl_cp/config.h.in

@@ -0,0 +1,37 @@
+// Copyright  (C)  2014  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+
+// Version: 1.0
+// Author: Brian Jensen <Jensen dot J dot Brian at gmail dot com>
+// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// 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

src/Mod/Robot/App/kdl_cp/frameacc.hpp

@@ -40,6 +40,23 @@ namespace KDL {
 class TwistAcc;
 typedef Rall2d<double,double,double> 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);
+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
 {
@@ -243,7 +260,6 @@ public:
 
 
 
-
 }
 
 
@@ -256,4 +272,5 @@ public:
 
 
 
+
 #endif

src/Mod/Robot/App/kdl_cp/frameacc.inl

@@ -19,7 +19,6 @@
 
 namespace KDL {
 
-
 /////////////////// VectorAcc /////////////////////////////////////
 
 VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2) {
@@ -431,6 +430,7 @@ Twist FrameAcc::GetAccTwist() const {
 
 
 
+
 TwistAcc TwistAcc::Zero()
 {
     return TwistAcc(VectorAcc::Zero(),VectorAcc::Zero());

src/Mod/Robot/App/kdl_cp/frames.cpp

@@ -27,6 +27,9 @@
 
 #include "frames.hpp"
 
+#define _USE_MATH_DEFINES  // For MSVC
+#include <math.h>
+
 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,25 +351,38 @@ 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) {

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);
 
 
 
@@ -1078,15 +1130,123 @@ 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);
+/**
+ * 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);
+
+
+/**
+ * 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);
-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);
+
+/**
+ * \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);
 
 } // namespace KDL

src/Mod/Robot/App/kdl_cp/frames.inl

@@ -25,8 +25,13 @@
  *                                                                         *
  ***************************************************************************/
 
-namespace KDL {
+/**
+ * \file frames.inl
+ * Inlined member functions and global functions that relate to the classes in frames.cpp
+ *
+ */
 
+namespace KDL {
 
 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
@@ -1362,5 +1335,3 @@ IMETHOD bool operator!=(const Vector2& a,const Vector2& b) {
 }
 
 } // namespace KDL
-
-

src/Mod/Robot/App/kdl_cp/framevel.hpp

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

src/Mod/Robot/App/kdl_cp/jacobian.cpp

@@ -23,7 +23,7 @@
 
 namespace KDL
 {
-    USING_PART_OF_NAMESPACE_EIGEN
+    using namespace Eigen;
 
     Jacobian::Jacobian()
     {
@@ -147,8 +147,8 @@ namespace KDL
     }
     
     void Jacobian::setColumn(unsigned int i,const Twist& t){
-        data.col(i).start<3>()=Eigen::Map<Vector3d>(t.vel.data);
-        data.col(i).end<3>()=Eigen::Map<Vector3d>(t.rot.data);
+        data.col(i).head<3>()=Eigen::Map<const Vector3d>(t.vel.data);
+        data.col(i).tail<3>()=Eigen::Map<const Vector3d>(t.rot.data);
     }
 
 }

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);
+
+
     class Jacobian
     {
     public:
+
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
         Eigen::Matrix<double,6,Eigen::Dynamic> 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)

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<double,6,1> t=(jac.data*src.data).lazy();
+        Eigen::Matrix<double,6,1> t=jac.data.lazyProduct(src.data);
         dest=Twist(Vector(t(0),t(1),t(2)),Vector(t(3),t(4),t(5)));
     }
     

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
          */
@@ -140,17 +140,17 @@ class MyTask : public RTT::TaskContext
         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)
-         */
+        * 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);

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

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

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)

src/Mod/Robot/App/kdl_cp/jntspaceinertiamatrix.hpp

@@ -65,7 +65,7 @@ class MyTask : public RTT::TaskContext
 };
 \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 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);
+
+    /**
+     * Function to set all the values of the array to 0
+     *
+     * @param mat
+     */
+    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())
+     */
+    bool Equal(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2,double eps=epsilon);
+
+    bool operator==(const JntSpaceInertiaMatrix& src1,const JntSpaceInertiaMatrix& src2);
+
 }
 
 #endif

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

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,7 +74,7 @@ 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.
@@ -205,7 +205,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;

src/Mod/Robot/App/kdl_cp/kdl.hpp

@@ -1,4 +1,5 @@
 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
+// Copyright  (C)  2011  Erwin Aertbelien  <Erwin dot Aertbelien at mech dot kuleuven dot be>
 
 // Version: 1.0
 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
@@ -24,21 +25,107 @@
  * \ingroup ROBOT
  *
  * This is the API reference of the
- * <a href="http://www.orocos.org/kdl">Kinematics and Dynamics Library</a> (KDL), a sub-project of <a href="http://www.orocos.org">Orocos</a>, but that can be used independently of Orocos. KDL offers different kinds of functionality:
+ * <a href="http://www.orocos.org/kdl">Kinematics and Dynamics
+ * Library</a> (KDL), a sub-project of <a
+ * href="http://www.orocos.org">Orocos</a>, 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.
  *
- * 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 <a href="http://www.orocos.org/rtt/">Real-Time Toolkit</a> (RTT). */
 
-/* This code doesn't seems to be integrated with freecad
-- \ref KDLTK : the interface code to integrate KDL into the Orocos <a href="http://www.orocos.org/rtt/">Real-Time Toolkit</a> (RTT). */
 

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

src/Mod/Robot/App/kdl_cp/kinfam.hpp

@@ -20,7 +20,7 @@
 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 
 /**
- * @defgroup KinFam Kinematic Families
+ * @defgroup KinematicFamily Kinematic Families
  * \ingroup KDL
  * @brief All classes to support kinematic families.
  *

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()<<":["<<joint.getTypeName()
+              <<", axis: "<<joint.JointAxis() << ", origin"<<joint.JointOrigin()<<"]";
 }
 
 std::istream& operator >>(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<<root->first<<": "<<root->second.segment<<"\n";
-	os << root->first<<"(q_nr: "<<root->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";
 }

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() {}
 	};
 

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

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

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<static_cast<int>(dv.size()));
+	return gv[i].first;
+}
+
+double Path_Composite::GetLengthToEndOfSegment(int i) {
+	assert(i>=0);
+	assert(i<static_cast<int>(dv.size()));
+	return dv[i];
+}
+
+void Path_Composite::GetCurrentSegmentLocation(double s, int& segment_number,
+		double& inner_s)
+{
+	inner_s = Lookup(s);
+	segment_number= cached_index;
+}
+
 Path_Composite::~Path_Composite() {
 	PathVector::iterator it;
 	for (it=gv.begin();it!=gv.end();++it) {
@@ -132,4 +157,4 @@ Path_Composite::~Path_Composite() {
 	}
 }
 
-}
+} // namespace KDL

src/Mod/Robot/App/kdl_cp/path_composite.hpp

@@ -53,6 +53,17 @@ namespace KDL {
 
 	 /**
 	  * A Path being the composition of other Path objects.
+	  *
+	  * For several of its methods, this class needs to lookup the segment corresponding to a value
+	  * of the path variable s.  To increase efficiency, this value is cached.
+	  *
+	  * \TODO Currently a linear search is used to look up the segment. A binary search is more efficient.  Can STL be used for this ?
+	  * \TODO Increase the efficiency for caching for the common case of a fine grained monotonously increasing path variable s.
+	  *
+	  * \TODO For all Path.., VelocityProfile.., Trajectory... check the bounds on the inputs with asserts.
+	  *
+	  * \TODO explain this routine in the wiki.
+	  *
 	  * @ingroup Motion
 	  */
 	 class Path_Composite : public Path
@@ -113,6 +124,42 @@ namespace KDL {
 		 */
 		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_COMPOSITE;
+		}
+
 		virtual ~Path_Composite();
 	};
 

src/Mod/Robot/App/kdl_cp/path_cyclic_closed.hpp

@@ -73,6 +73,12 @@ namespace KDL {
 		virtual void Write(std::ostream& os);
 		static Path* Read(std::istream& is);
 		virtual Path* Clone();
+		/**
+		 * gets an identifier indicating the type of this Path object
+		 */
+		virtual IdentifierType getIdentifier() const {
+			return ID_CYCLIC_CLOSED;
+		}
 		virtual ~Path_Cyclic_Closed();
 	};
 

src/Mod/Robot/App/kdl_cp/path_line.hpp

@@ -121,6 +121,13 @@ class Path_Line : public Path
 		virtual Twist Acc(double s,double sd,double sdd) const;
 		virtual void Write(std::ostream& os);
 		virtual Path* Clone();
+
+		/**
+		 * gets an identifier indicating the type of this Path object
+		 */
+		virtual IdentifierType getIdentifier() const {
+			return ID_LINE;
+		}
 		virtual ~Path_Line();
 	};
 

src/Mod/Robot/App/kdl_cp/path_point.hpp

@@ -73,6 +73,13 @@ class Path_Point : public Path
 		virtual Twist Acc(double s,double sd,double sdd) const;
 		virtual void Write(std::ostream& os);
 		virtual Path* Clone();
+
+		/**
+		 * gets an identifier indicating the type of this Path object
+		 */
+		virtual IdentifierType getIdentifier() const {
+			return ID_POINT;
+		}
 		virtual ~Path_Point();
 	};
 

src/Mod/Robot/App/kdl_cp/path_roundedcomposite.cpp

@@ -49,71 +49,104 @@
 
 namespace KDL {
 
+// private constructor, to keep the type when cloning a Path_RoundedComposite, such that getIdentifier keeps on returning
+// the correct value:
+Path_RoundedComposite::Path_RoundedComposite(Path_Composite* _comp,
+		double _radius, double _eqradius, RotationalInterpolation* _orient,
+		bool _aggregate,int _nrofpoints):
+		comp(_comp), radius(_radius), eqradius(_eqradius), orient(_orient), nrofpoints(_nrofpoints), aggregate(_aggregate) {
+}
+
 Path_RoundedComposite::Path_RoundedComposite(double _radius,double _eqradius,RotationalInterpolation* _orient, bool _aggregate) :
 	comp( new Path_Composite()), radius(_radius),eqradius(_eqradius), orient(_orient), aggregate(_aggregate)
 {
 		nrofpoints = 0;
+		if (eqradius<=0) {
+			throw Error_MotionPlanning_Not_Feasible(1);
+		}
 }
 
 void Path_RoundedComposite::Add(const Frame& F_base_point) {
-	if (nrofpoints==0) {
+	double eps = 1E-7;
+	if (nrofpoints == 0) {
 		F_base_start = F_base_point;
-	} else if (nrofpoints==1) {
-		F_base_via   = F_base_point;
+	} else if (nrofpoints == 1) {
+		F_base_via = F_base_point;
 	} else {
 		// calculate rounded segment : line + circle,
 		// determine the angle between the line segments :
-			Vector ab = F_base_via.p - F_base_start.p;
-			Vector bc = F_base_point.p - F_base_via.p;
-			double abdist = ab.Normalize();
-			double alpha  = ::acos(dot(ab,bc)/(ab.Norm()*bc.Norm()));
-            //double alpha  = ::acos(dot(ab,bc));
-			double d      = radius/tan((PI-alpha)/2);
-            double bcdist = bc.Normalize();
+		Vector ab = F_base_via.p - F_base_start.p;
+		Vector bc = F_base_point.p - F_base_via.p;
+		double abdist = ab.Norm();
+		double bcdist = bc.Norm();
+		if (abdist < eps) {
+			throw Error_MotionPlanning_Not_Feasible(2);
+		}
+		if (bcdist < eps) {
+			throw Error_MotionPlanning_Not_Feasible(3);
+		}
+		double alpha = acos(dot(ab, bc) / abdist / bcdist);
+		if ((PI - alpha) < eps) {
+			throw Error_MotionPlanning_Not_Feasible(4);
+		}
+		if (alpha < eps) {
+			// no rounding is done in the case of parallel line segments
+			comp->Add(
+					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<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);
+				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<Path_Composite*>(comp->Clone()),radius,eqradius,orient->Clone(), true, nrofpoints);
 }
 
 }

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

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<Vector3d>(c_.data);
-        Map<Matrix3d>(I.data)=Map<Matrix3d>(Ic.data)-m_*(c_eig*c_eig.transpose()-c_eig.dot(c_eig)*Matrix3d::Identity());
+        Vector3d c_eig=Map<const Vector3d>(c_.data);
+        Map<Matrix3d>(I.data)=Map<const Matrix3d>(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<Vector3d>(X.p.data);
-        Vector3d h_eig = Map<Vector3d>(I.h.data);
+        Vector3d h_eig = Map<const Vector3d>(I.h.data);
         Vector3d hmr_eig = Map<Vector3d>(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<Matrix3d>(X.M.data);
         RotationalInertia Ib;
-        Map<Matrix3d>(Ib.data) = R*((Map<Matrix3d>(I.I.data)+rcrosshcross+hmrcrossrcross)*R.transpose());
+        Map<Matrix3d>(Ib.data) = R*((Map<const Matrix3d>(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<Matrix3d>(M.data);
+        Matrix3d R = Map<const Matrix3d>(M.data);
         RotationalInertia Ib;
-        Map<Matrix3d>(Ib.data) = R.transpose()*(Map<Matrix3d>(I.I.data)*R);
+        Map<Matrix3d>(Ib.data) = R.transpose()*(Map<const Matrix3d>(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<Vector3d>(p.data);
+        Vector3d r_eig = Map<const Vector3d>(p.data);
         Vector3d h_eig = Map<Vector3d>(this->h.data);
         Vector3d hmr_eig = Map<Vector3d>(hmr.data);
         Matrix3d rcrosshcross = h_eig * r_eig.transpose()-r_eig.dot(h_eig)*Matrix3d::Identity();

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
 
 

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;
 

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

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<Vector3d>(result.data)=Map<Matrix3d>(this->data)*Map<Vector3d>(omega.data);
+        Map<Vector3d>(result.data)=Map<const Matrix3d>(this->data)*Map<const Vector3d>(omega.data);
         return result;
  	}
 
     RotationalInertia operator*(double a, const RotationalInertia& I){
         RotationalInertia result;
-        Map<Matrix3d>(result.data)=a*Map<Matrix3d>(I.data);
+        Map<Matrix3d>(result.data)=a*Map<const Matrix3d>(I.data);
         return result;
     }
     
     RotationalInertia operator+(const RotationalInertia& Ia, const RotationalInertia& Ib){
         RotationalInertia result;
-        Map<Matrix3d>(result.data)=Map<Matrix3d>(Ia.data)+Map<Matrix3d>(Ib.data);
+        Map<Matrix3d>(result.data)=Map<const Matrix3d>(Ia.data)+Map<const Matrix3d>(Ib.data);
         return result;
     }
 }

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

src/Mod/Robot/App/kdl_cp/segment.hpp

@@ -62,7 +62,7 @@ namespace KDL {
          * the segment, default: Frame::Identity()
          * @param I 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
          *
@@ -72,7 +72,7 @@ namespace KDL {
          * the segment, default: Frame::Identity()
          * @param I 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;
         }
 
     };

src/Mod/Robot/App/kdl_cp/solveri.hpp

@@ -0,0 +1,129 @@
+// Copyright (C) 2013 Stephen Roderick <kiwi dot net at mac dot com>
+
+// 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

src/Mod/Robot/App/kdl_cp/trajectory.hpp

@@ -79,10 +79,10 @@ namespace KDL {
 	{
 	public:
         virtual Path*      GetPath() = 0;
-        // The underlying Path
+        // The underlying Path - FreeCAD change
 
         virtual VelocityProfile* GetProfile() = 0;
-        // The velocity profile
+        // The velocity profile - FreeCAD change
 
 		virtual double Duration() const = 0;
 		// The duration of the trajectory

src/Mod/Robot/App/kdl_cp/trajectory_composite.cpp

@@ -23,7 +23,7 @@ namespace KDL {
 
     Trajectory_Composite::Trajectory_Composite():duration(0.0)
     {
-        path = new Path_Composite();
+        path = new Path_Composite(); // FreeCAD change
     }
 
     double Trajectory_Composite::Duration() const{
@@ -72,7 +72,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 +93,7 @@ namespace KDL {
         vt.insert(vt.end(),elem);
         duration += elem->Duration();
         vd.insert(vd.end(),duration);
-        path->Add(elem->GetPath(),false);
+        path->Add(elem->GetPath(),false); // FreeCAD change
     }
 
     void Trajectory_Composite::Destroy() {
@@ -103,8 +103,8 @@ namespace KDL {
         }
         vt.erase(vt.begin(),vt.end());
         vd.erase(vd.begin(),vd.end());
-
-        delete path;
+        
+        delete path; // FreeCAD change
     }
 
     Trajectory_Composite::~Trajectory_Composite() {
@@ -128,7 +128,8 @@ namespace KDL {
         }
         return comp;
     }
-
+    
+    // FreeCAD change
     Path* Trajectory_Composite::GetPath()
     {
         return path;
@@ -138,7 +139,6 @@ namespace KDL {
     {
         return 0;
     }
-
 }
 
 

src/Mod/Robot/App/kdl_cp/trajectory_composite.hpp

@@ -37,7 +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;
+        Path_Composite* path; // FreeCAD change
 
 	public:
 		Trajectory_Composite();
@@ -54,11 +54,11 @@ 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();
-
+        virtual Path*      GetPath(); // FreeCAD change
+		virtual VelocityProfile* GetProfile(); // FreeCAD change
+        
         // access the single members
-        Trajectory *Get(unsigned int n){return vt[n];}
+        Trajectory *Get(unsigned int n){return vt[n];} // FreeCAD change
 
 		virtual ~Trajectory_Composite();
 	};

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