Implementation of DH <--> Model conversions without KDL

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/iKinConversions.h
+                         include/iDynTree/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})
 
@@ -49,9 +52,13 @@ install(TARGETS ${libraryname}
         RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin
         LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib
         ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib
-        PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/iCub" COMPONENT dev)
+        PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/iDynTree" COMPONENT dev)
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS idyntree-icub)
 
 set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include)
 
+# Install headers in deprecated location
+install(DIRECTORY include/iDynTree/iCub
+        DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/iCub")
+

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

@@ -1,182 +1,17 @@
 /*
- * Copyright (C) 2016 Fondazione Istituto Italiano di Tecnologia
+ * 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_SKINDYNLIB_CONVERSIONS_H
-#define IDYNTREE_SKINDYNLIB_CONVERSIONS_H
+#ifndef IDYNTREE_SKINDYNLIB_CONVERSIONS_H_LEGACY_LOCATION
+#define IDYNTREE_SKINDYNLIB_CONVERSIONS_H_LEGACY_LOCATION
 
-#include <map>
-#include <string>
-
-#include <iDynTree/Model/Indices.h>
-#include <iDynTree/Estimation/ExternalWrenchesEstimation.h>
-
-#include <iCub/skinDynLib/dynContactList.h>
-#include <iCub/skinDynLib/skinContactList.h>
-
-namespace iDynTree
-{
-class Model;
-class LinkContactWrenches;
-
-/**
- * Identifier for a link and a body frame in skinDynLib.
- */
-class skinDynLibLinkID
-{
-public:
-    int body_part;
-    int local_link_index;
-
-    bool operator<(const skinDynLibLinkID& k) const
-    {
-        if(this->body_part < k.body_part)
-        {
-           return true;
-        }
-        else if(this->body_part > k.body_part)
-        {
-           return false;
-        }
-        else
-        {
-            return this->local_link_index < k.local_link_index;
-        }
-    }
-
-    bool operator==(const skinDynLibLinkID& k) const
-    {
-        return (this->body_part == k.body_part &&
-                this->local_link_index == k.local_link_index);
-    }
-};
-
-/**
- * Identifier for a link and frame couple in an iDynTree model.
- */
-class iDynTreeLinkAndFrame
-{
-     public:
-          LinkIndex  link_index;
-          FrameIndex frame_index;
-};
-
-
-/**
- * \brief Helper for conversion between iDynTree data structures and skinDynLib data structures.
- *
- * There are several differences to handle:
- *  * In iDynTree, link and frames and identified by name
- *    and (in the context of an instantiated model) by their index, while in
- *    skinDynLib link are identified by the bodyPart, a numeric id representing a part of the robot
- *    the linkIndex, a progressive numeric id uniquely identifyng  the link in the part.
- *  * The link frame of a link in iDynTree does not match the link frame assumed in skinDynLib.
- *    For this reason we need to specify an additional frame to specify the frame of the link
- *    used by skinDynLib, to properly convert the contact data back and forth.
- */
-class skinDynLibConversionsHelper
-{
-private:
-    std::map<skinDynLibLinkID,iDynTreeLinkAndFrame> skinDynLibLinkMap;
-
-public:
-    /**
-     * This function enables interoperability between the iDynTree library
-     * and the iCub skinDynLib library.
-     *
-     * This function can be used to assign a match between a skinDynLib link,
-     * and a iDynTree link.
-     *
-     * In skinDynLib a link is represented by two numbers:
-     *   * the bodyPart, a numeric id representing a part of the robot
-     *   * the linkIndex, a progressive numeric id uniquely identifyng  the link in the part
-     * We associate this to two iDynTree concept
-     *   * the link_name of the considered link
-     *   * the frame_name of the frame used by skinDynLib when dealing with that link.
-     *         This frame can be the link frame, in this case frame_name == link_name,
-     *          otherwise another frame can be used, under the constraint that this frame
-     *          must be rigidly attached to the considered link.
-     *
-     */
-    bool addSkinDynLibAlias(const Model& model,
-                            const std::string iDynTree_link_name, const std::string iDynTree_frame_name,
-                            const int skinDynLib_body_part, const int skinDynLib_link_index);
-
-    /**
-     * Retrieve the skinDynLib alias of a link, added to the class using the addSkinDynLibAlias method.
-     */
-    bool getSkinDynLibAlias(const Model & model,
-                            const std::string iDynTree_link_name,
-                                  std::string & iDynTree_frame_name,
-                                  int & skinDynLib_body_part,
-                                  int & skinDynLib_link_index) const;
-
-    /**
-     * Retrieve the skinDynLib alias of a link, added to the class using the addSkinDynLibAlias method.
-     */
-    bool getSkinDynLibAlias(const Model & model,
-                            const LinkIndex iDynTree_link_index,
-                                  LinkIndex & iDynTree_frame_index,
-                             int & skinDynLib_body_part,
-                             int & skinDynLib_link_index) const;
-
-    /**
-     * Convert a skinDynLib identifier to a iDynTree link/frame identifier.
-     */
-    bool skinDynLib2iDynTree(const int skinDynLib_body_part,
-                             const int skinDynLib_link_index,
-                             int & iDynTree_link_index,
-                             int & iDynTree_frame_index) const;
-    /**
-     * Remove a alias in the form (body_part, link_index) for a link
-     */
-    bool removeSkinDynLibAlias(const Model & model, const std::string linkName);
-
-    /**
-     * Convert a dynContactList to a LinkUnknownWrenchContacts.
-     *
-     * The contactId contained in the dynContactList is preserved and saved
-     * in the appropriate attribute in the LinkUnknownWrenchContacts class.
-     */
-    bool fromSkinDynLibToiDynTree(const Model& model,
-                                  const iCub::skinDynLib::dynContactList & dynList,
-                                        LinkUnknownWrenchContacts & unknowns);
-
-    /**
-     * Convert a skinContactList to a LinkUnknownWrenchContacts.
-     *
-     * The contactId contained in the skinContactList is preserved and saved
-     * in the appropriate attribute in the LinkUnknownWrenchContacts class.
-     */
-    bool fromSkinDynLibToiDynTree(const Model& model,
-                                  const iCub::skinDynLib::skinContactList & skinList,
-                                        LinkUnknownWrenchContacts & unknowns);
-
-    /**
-     * Convert a LinkContactWrenches to a iCub::skinDynLib::dynContactList.
-     *
-     * This function creates a new dynContactList.
-     *
-     */
-    bool fromiDynTreeToSkinDynLib(const Model& model,
-                                  const LinkContactWrenches & contactWrenches,
-                                        iCub::skinDynLib::dynContactList & dynList);
-
-    /**
-     * Update an existing skinContactList in which some forces and torque are
-     * unknown using the estimated contact wrenches contained in
-     * a LinkContactWrenches. The matching between the skinContactList and
-     * the LinkContactWrenches contacts is done through the contactId, that then
-     * should be consistent between the two functions.
-     */
-    bool updateSkinContactListFromLinkContactWrenches(const Model& model,
-                                                      const LinkContactWrenches & contactWrenches,
-                                                            iCub::skinDynLib::skinContactList & skinContactListToUpdate);
-};
+#ifdef __DEPRECATED
+#warning <iDynTree/iCub/skinDynLibConversions.h> is deprecated. Please use <iDynTree/skinDynLibConversions.h>. To disable this warning use -Wno-deprecated.
+#endif
 
-}
+#include <iDynTree/skinDynLibConversions.h>
 
-#endif
+#endif /* IDYNTREE_SKINDYNLIB_CONVERSIONS_H_LEGACY_LOCATION */

src/icub/include/iDynTree/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