Refactoring of MMM framework to SimoxUtility: Added string helpers and...

Refactoring of MMM framework to SimoxUtility: Added string helpers and conversion tools, a distinguishable color lookup table e.g. for creating graphs, a rapidxml wrapper for easier parsing of xml files and moved deprecated RobotPoseDiffIK from MMMTools to VirtualRobot

SimoxUtility/xml/rapidxml/RapidXMLForwardDecl.h

+#pragma once
+
+#include <memory>
+#include <filesystem>
+#include "SimoxUtility/error/SimoxError.h"
+
+namespace rapidxml
+{
+    template<class Ch>
+    class xml_node;
+}
+
+namespace simox
+{
+
+namespace error
+{
+    class XMLFormatError : public SimoxError
+    {
+    public:
+        XMLFormatError(const std::string &message = std::string()) : SimoxError(message) {}
+    };
+}
+
+namespace xml
+{
+
+namespace attribute
+{
+    template<typename T, typename std::enable_if<std::is_fundamental<T>::value>::type* = nullptr>
+    struct XMLAttribute
+    {
+        XMLAttribute(const std::string &attributeName) : attributeName(attributeName)
+        {
+        }
+
+        std::string attributeName;
+    };
+}
+
+class RapidXMLWrapperNode;
+typedef std::shared_ptr<RapidXMLWrapperNode> RapidXMLWrapperNodePtr;
+
+class RapidXMLWrapperRootNode;
+typedef std::shared_ptr<RapidXMLWrapperRootNode> RapidXMLWrapperRootNodePtr;
+
+}
+
+}

SimoxUtility/xml/rapidxml/rapidxml_license.txt

+Use of this software is granted under one of the following two licenses,
+to be chosen freely by the user.
+
+1. Boost Software License - Version 1.0 - August 17th, 2003
+===============================================================================
+
+Copyright (c) 2006, 2007 Marcin Kalicinski
+
+Permission is hereby granted, free of charge, to any person or organization
+obtaining a copy of the software and accompanying documentation covered by
+this license (the "Software") to use, reproduce, display, distribute,
+execute, and transmit the Software, and to prepare derivative works of the
+Software, and to permit third-parties to whom the Software is furnished to
+do so, all subject to the following:
+
+The copyright notices in the Software and this entire statement, including
+the above license grant, this restriction and the following disclaimer,
+must be included in all copies of the Software, in whole or in part, and
+all derivative works of the Software, unless such copies or derivative
+works are solely in the form of machine-executable object code generated by
+a source language processor.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.
+
+2. The MIT License
+===============================================================================
+
+Copyright (c) 2006, 2007 Marcin Kalicinski
+
+Permission is hereby granted, free of charge, to any person obtaining a copy 
+of this software and associated documentation files (the "Software"), to deal 
+in the Software without restriction, including without limitation the rights 
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies 
+of the Software, and to permit persons to whom the Software is furnished to do so, 
+subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all 
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 
+IN THE SOFTWARE.
\ No newline at end of file

VirtualRobot/CMakeLists.txt

@@ -217,6 +217,7 @@ SET(SOURCES
     IK/PoseQualityExtendedManipulability.cpp
     IK/PoseQualityManipulability.cpp
     IK/PoseQualityMeasurement.cpp
+    IK/RobotPoseDifferentialIK.cpp
     IK/StackedIK.cpp
     IK/SupportPolygon.cpp
     IK/constraints/BalanceConstraint.cpp
@@ -418,6 +419,7 @@ SET(INCLUDES
     IK/PoseQualityExtendedManipulability.h
     IK/PoseQualityManipulability.h
     IK/PoseQualityMeasurement.h
+    IK/RobotPoseDifferentialIK.h
     IK/StackedIK.h
     IK/SupportPolygon.h
     IK/constraints/BalanceConstraint.h

VirtualRobot/IK/RobotPoseDifferentialIK.h

+/**
+* This file is part of Simox.
+*
+* Simox 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 of
+* the License, or (at your option) any later version.
+*
+* Simox 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 program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package    VirtualRobot
+* @author     Nikolaus Vahrenkamp
+* @copyright  2013 Nikolaus Vahrenkamp
+*             GNU Lesser General Public License
+*
+*/
+
+#pragma once
+
+#include "DifferentialIK.h"
+
+#include <string>
+#include <vector>
+
+namespace VirtualRobot
+{
+    class [[deprecated("MMMTools_LegacyConverter")]] VIRTUAL_ROBOT_IMPORT_EXPORT RobotPoseDifferentialIK : public DifferentialIK, public std::enable_shared_from_this<RobotPoseDifferentialIK>
+    {
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        /*!
+            @brief Initialize a Jacobian object.
+            \param rns The robotNodes (i.e., joints) for which the Jacobians should be calculated.
+            \param coordSystem The coordinate system in which the Jacobians are defined. By default the global coordinate system is used.
+        */
+        RobotPoseDifferentialIK(RobotPtr robot,RobotNodeSetPtr rns, RobotNodePtr coordSystem = RobotNodePtr(), JacobiProvider::InverseJacobiMethod invJacMethod = eSVDDamped);
+
+        Eigen::MatrixXf getJacobianMatrix(SceneObjectPtr tcp, IKSolver::CartesianSelection mode);
+
+        Eigen::MatrixXf getJacobianMatrix();
+
+        /*!	@brief Compute a single IK step.
+            @param stepSize Controls the amount of error to be reduced in each step: \f$ 0 < \beta \leq 1 \f$
+            @return The changes \f$\Delta \theta\f$ in the joint angles.
+            \note{Note} This does not affect the joints values of the robot.
+        */
+        Eigen::VectorXf computeStep(float stepSize=1.0);
+
+        /*!	@brief Computes the complete inverse kinematics.
+            @param stepSize Controls the amount of error to be reduced in each step: \f$ 0 < \beta \leq 1 \f$
+            @param maxSteps Maximal numbers of steps.
+            @param minChange The minimal change in joint angles (euclidean distance in radians)
+            @param performMinOneStep If set, at least one step is performed (helps escaping local minima, but may also cause pose jittering)
+            @note{Note}  Sets the node's joint angles automatically.
+        */
+        bool computeSteps(float stepSize, float minChange, int maxSteps, bool performMinOneStep = true);
+
+        /*!
+            If enabled (standard), joint limits are considered via box constraints.
+        */
+        void boxConstraints(bool enable);
+
+    protected:
+
+        bool checkTolerances();
+
+        Eigen::MatrixXd computePseudoInverseJacobianMatrixDampedD(const Eigen::MatrixXd &m);
+        std::vector<Eigen::MatrixXf> localJacobians;
+        RobotPtr robot;
+
+        bool considerBoxConstraints;
+
+        Eigen::VectorXf _lLimits;
+        Eigen::VectorXf _uLimits;
+    };
+
+    [[deprecated("MMMTools_LegacyConverter")]] typedef std::shared_ptr<RobotPoseDifferentialIK> RobotPoseDifferentialIKPtr;
+}
+