Implementation of DH <--> Model conversions without KDL

Porting the Model ---> DH algorithm from the KDL-based
code, port the urdf2dh utility and add method to automatically
extract a iKinLimb from a iDynTree model.

src/core/include/iDynTree/Core/TestUtils.h

@@ -107,11 +107,11 @@ namespace iDynTree
      * Fill a vector with random double.
      */
     template<typename VectorType>
-    void getRandomVector(VectorType & vec)
+    void getRandomVector(VectorType & vec, double min=0.0, double max=1.0)
     {
         for(unsigned int i=0; i<vec.size(); i++)
         {
-            vec(i) = getRandomDouble();
+            vec(i) = getRandomDouble(min,max);
         }
     }
 

src/core/src/Direction.cpp

@@ -8,6 +8,7 @@
 #include <iDynTree/Core/Direction.h>
 
 #include <iDynTree/Core/EigenHelpers.h>
+
 #include <Eigen/Dense>
 
 #include <cstdio>
@@ -106,7 +107,6 @@ namespace iDynTree
                          -this->m_data[2]);
     }
 
-
     std::string Direction::toString() const
     {
         std::stringstream ss;

src/icub/CMakeLists.txt

@@ -4,9 +4,11 @@
 
 project(iDynTree_ICUB CXX)
 
-SET(iDynTree_ICUB_source src/skinDynLibConversions.cpp)
+SET(iDynTree_ICUB_source src/iKinConversions.cpp
+                         src/skinDynLibConversions.cpp)
 
-SET(iDynTree_ICUB_header include/iDynTree/iCub/skinDynLibConversions.h)
+SET(iDynTree_ICUB_header include/iDynTree/iCub/iKinConversions.h
+                         include/iDynTree/iCub/skinDynLibConversions.h)
 
 SOURCE_GROUP("Source Files" FILES ${iDynTree_ICUB_source})
 SOURCE_GROUP("Header Files" FILES ${iDynTree_ICUB_header})
@@ -34,13 +36,14 @@ set_target_properties(${libraryname} PROPERTIES PUBLIC_HEADER "${iDynTree_ICUB_h
 target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include;${IDYNTREE_TREE_INCLUDE_DIRS}>"
                                                 "$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}>")
 
-target_link_libraries(${libraryname} LINK_PUBLIC skinDynLib idyntree-core idyntree-model idyntree-yarp idyntree-estimation)
+target_link_libraries(${libraryname} LINK_PUBLIC iKin skinDynLib idyntree-core idyntree-model idyntree-yarp idyntree-estimation)
 
 if(NOT CMAKE_VERSION VERSION_LESS 2.8.12)
     target_compile_options(${libraryname} PRIVATE ${IDYNTREE_WARNING_FLAGS})
 endif()
 
 target_include_directories(${libraryname} INTERFACE ${skinDynLib_INCLUDE_DIRS})
+target_include_directories(${libraryname} PRIVATE ${iKin_INCLUDE_DIRS})
 
 include_directories(SYSTEM ${skinDynLib_INCLUDE_DIRS})
 

src/icub/include/iDynTree/iCub/iKinConversions.h

@@ -0,0 +1,89 @@
+/*
+ * Copyright (C) 2017 Fondazione Istituto Italiano di Tecnologia
+ * Authors: Silvio Traversaro
+ * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+ *
+ */
+
+#ifndef IDYNTREE_IKIN_CONVERSIONS_H
+#define IDYNTREE_IKIN_CONVERSIONS_H
+
+#include <string>
+
+#include <iCub/iKin/iKinFwd.h>
+
+namespace iDynTree
+
+{
+
+class Model;
+class DHChain;
+
+/**
+ *
+ * Load a iDynTree::DHChain object from a iCub::iKin::iKinChain .
+ *
+ * @return true if all went ok, false otherwise.
+ */
+bool DHChainFromiKinChain(iCub::iKin::iKinChain & ikinChain,
+                          DHChain & out);
+
+/**
+ *
+ * Load a iDynTree::Model object from a iCub::iKin::iKinChain .
+ *
+ * @return true if all went ok, false otherwise.
+ */
+bool modelFromiKinChain(iCub::iKin::iKinChain & ikinChain,
+                        Model & output);
+
+/**
+ * iKinLimb class to extract a iKinLimb from iDynTree structures.
+ */
+class iKinLimbImported : public iCub::iKin::iKinLimb
+{
+public:
+    /**
+     * Default constructor.
+     */
+    iKinLimbImported();
+
+    /**
+     * Default destructor.
+     */
+    virtual ~iKinLimbImported();
+
+    /**
+     * Initialize the limb properties from a chain in a iDynTree::Model
+     */
+    bool fromModel(const Model & model,
+                   const std::string& baseFrame,
+                   const std::string& distalFrame);
+
+    /**
+     * Initialize the limb properties from a iDynTree::DHChain
+     */
+    bool fromDHChain(const DHChain & dhChain);
+};
+
+/**
+ *
+ * Extract an iCub::iKin::iKinLimb from an iDynTree::Model .
+ *
+ * @return true if all went ok, false otherwise.
+ */
+bool iKinLimbFromModel(const Model & model,
+                        const std::string& baseFrame,
+                        const std::string& distalFrame,
+                        iCub::iKin::iKinLimb & ikinLimb);
+
+/**
+ *
+ * Create a iCub::iKin::iKinLimb from an iDynTree::DHChain
+ */
+bool iKinLimbFromDHChain(const DHChain & dhChain,
+                         iCub::iKin::iKinLimb& ikinLimb);
+
+}
+
+#endif

src/icub/src/iKinConversions.cpp

@@ -0,0 +1,146 @@
+/*
+ * Copyright (C) 2017 IIT - Istituto Italiano di Tecnologia - http://www.iit.it
+ * Author: Silvio Traversaro
+ * CopyPolicy: Released under the terms of the GNU LGPL v2.0 (or any later version)
+ *
+ * The development of this software was supported by the FP7 EU project
+ * CoDyCo (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics (b))
+ * http://www.codyco.eu
+ */
+
+#include <iDynTree/iCub/iKinConversions.h>
+#include <iDynTree/Model/DenavitHartenberg.h>
+
+#include <iDynTree/yarp/YARPConversions.h>
+
+#include <iCub/iKin/iKinFwd.h>
+
+#include <cassert>
+
+namespace iDynTree
+{
+
+DHLink iKinLink2DHLink(const iCub::iKin::iKinLink & ikinlink)
+{
+    DHLink ret;
+
+    ret.A = ikinlink.getA();
+    ret.D = ikinlink.getD();
+    ret.Alpha = ikinlink.getAlpha();
+    ret.Offset = ikinlink.getOffset();
+    ret.Min = ikinlink.getMin();
+    ret.Max = ikinlink.getMax();
+
+    return ret;
+}
+
+iCub::iKin::iKinLink DHLink2iKinLink(const DHLink & dhLink)
+{
+    return iCub::iKin::iKinLink(dhLink.A,dhLink.D,
+                                dhLink.Alpha,dhLink.Offset,
+                                dhLink.Min,dhLink.Max);
+}
+
+bool DHChainFromiKinChain(iCub::iKin::iKinChain& ikinChain,
+                                DHChain& dhChain)
+{
+    assert(ikinChain.getN() == ikinChain.getDOF());
+
+    iDynTree::Transform H0, HN;
+    dhChain.setNrOfDOFs(ikinChain.getN());
+
+    toiDynTree(ikinChain.getH0(),H0);
+    dhChain.setH0(H0);
+
+    for(size_t i=0; i < dhChain.getNrOfDOFs(); i++)
+    {
+        dhChain(i) = iKinLink2DHLink(ikinChain(i));
+        std::stringstream ss;
+        ss << i;
+        dhChain.setDOFName(i, "iKinDOF" + ss.str());
+    }
+
+    toiDynTree(ikinChain.getHN(),HN);
+    dhChain.setHN(HN);
+
+    return true;
+}
+
+bool modelFromiKinChain(iCub::iKin::iKinChain& ikinChain, Model& output)
+{
+    DHChain chain;
+    bool ok = DHChainFromiKinChain(ikinChain, chain);
+
+    if( !ok )
+    {
+        return false;
+    }
+
+    ok = CreateModelFromDHChain(chain,output);
+
+    return ok;
+}
+
+iKinLimbImported::iKinLimbImported(): iCub::iKin::iKinLimb()
+{
+}
+
+iKinLimbImported::~iKinLimbImported()
+{}
+
+bool iKinLimbImported::fromDHChain(const DHChain &dhChain)
+{
+    // Cleanup existing data
+    dispose();
+
+    yarp::sig::Matrix yarpMatBuf;
+    toYarp(dhChain.getH0().asHomogeneousTransform(), yarpMatBuf);
+    this->setH0(yarpMatBuf);
+
+    for(size_t i=0; i < dhChain.getNrOfDOFs(); i++)
+    {
+        // The ownership of the pointer is transferred to the iKinLimb object,
+        // and it will be freed by the denstructor (or a call to dispose)
+        this->pushLink(new iCub::iKin::iKinLink(DHLink2iKinLink(dhChain(i))));
+    }
+
+    toYarp(dhChain.getHN().asHomogeneousTransform(), yarpMatBuf);
+    this->setHN(yarpMatBuf);
+
+    return true;
+}
+
+bool iKinLimbImported::fromModel(const Model &model,
+                                 const std::string &baseFrame,
+                                 const std::string &distalFrame)
+{
+    DHChain chain;
+    bool conversionSuccessful = ExtractDHChainFromModel(model, baseFrame, distalFrame, chain);
+    conversionSuccessful = conversionSuccessful && this->fromDHChain(chain);
+    return conversionSuccessful;
+}
+
+bool iKinLimbFromDHChain(const DHChain & dhChain,
+                         iCub::iKin::iKinLimb& ikinLimb)
+{
+    iKinLimbImported ikinLimbImported;
+
+    bool ok = ikinLimbImported.fromDHChain(dhChain);
+
+    ikinLimb = ikinLimbImported;
+
+    return ok;
+}
+
+bool iKinLimbFromModel(const Model & model,
+                       const std::string& baseFrame,
+                       const std::string& distalFrame,
+                       iCub::iKin::iKinLimb & ikinLimb)
+{
+    DHChain chain;
+    bool conversionSuccessful = ExtractDHChainFromModel(model, baseFrame, distalFrame, chain);
+    conversionSuccessful = conversionSuccessful && iKinLimbFromDHChain(chain, ikinLimb);
+    return conversionSuccessful;
+}
+
+}

src/model/CMakeLists.txt

@@ -5,6 +5,7 @@
 project(iDynTree_Model CXX)
 
 set(IDYNTREE_MODEL_HEADERS include/iDynTree/Model/ContactWrench.h
+                           include/iDynTree/Model/DenavitHartenberg.h
                            include/iDynTree/Model/FixedJoint.h
                            include/iDynTree/Model/ForwardKinematics.h
                            include/iDynTree/Model/FreeFloatingState.h
@@ -32,6 +33,7 @@ set(IDYNTREE_MODEL_HEADERS include/iDynTree/Model/ContactWrench.h
 set(IDYNTREE_MODEL_PRIVATE_INCLUDES include/iDynTree/Model/ModelTestUtils.h)
 
 set(IDYNTREE_MODEL_SOURCES src/ContactWrench.cpp
+                           src/DenavitHartenberg.cpp
                            src/FixedJoint.cpp
                            src/ForwardKinematics.cpp
                            src/FreeFloatingState.cpp