add code to create KDL chain from URDF file

CMakeLists.txt

@@ -2,8 +2,11 @@ cmake_minimum_required(VERSION 3.5)
 
 project(KinovaControlExperiments VERSION 0.0.1 LANGUAGES CXX)
 
+find_package(kdl_parser)
+find_package(orocos_kdl)
+
 set(CMAKE_INCLUDE_CURRENT_DIR   ON)
-set(CMAKE_VERBOSE_MAKEFILE      ON)
+set(CMAKE_VERBOSE_MAKEFILE      OFF)
 set(CMAKE_COLOR_MAKEFILE        ON)
 
 set(CMAKE_BUILD_TYPE Release)
@@ -21,21 +24,39 @@ set(EXTRACT_DIR "${PROJECT_SOURCE_DIR}/kinova_libs/extracted")
 set(KORTEX_DIR "${EXTRACT_DIR}/linux_gcc_x86-64")
 
 # unclear what this is used for, needs further investigation
-include_directories(${EXTRACT_DIR}/cxxopts/)
+include_directories(
+    ${EXTRACT_DIR}/cxxopts/
+)
+
+link_libraries(
+    ${KORTEX_DIR}/lib/release/libKortexApiCpp.a
+)
+include_directories(
+    ${KORTEX_DIR}/include
+    ${KORTEX_DIR}/include/client
+    ${KORTEX_DIR}/include/common
+    ${KORTEX_DIR}/include/messages
+    ${KORTEX_DIR}/include/client_stubs
+)
 
-link_libraries(${KORTEX_DIR}/lib/release/libKortexApiCpp.a)
-include_directories(${KORTEX_DIR}/include)
-include_directories(${KORTEX_DIR}/include/client)
-include_directories(${KORTEX_DIR}/include/common)
-include_directories(${KORTEX_DIR}/include/messages)
-include_directories(${KORTEX_DIR}/include/client_stubs)
+# KDL related
+include_directories(
+    ${orocos_kdl_INCLUDE_DIRS}
+    ${kdl_parser_INCLUDE_DIRS}
+)
 
 # user code
-include_directories(${PROJECT_SOURCE_DIR}/include)
-include_directories(${PROJECT_SOURCE_DIR}/generated)
+include_directories(
+    ${PROJECT_SOURCE_DIR}/include
+    ${PROJECT_SOURCE_DIR}/generated
+)
 set_source_files_properties(generated/abag.c PROPERTIES LANGUAGE CXX )
 
-link_libraries(pthread)
+link_libraries(
+    ${orocos_kdl_LIBRARIES}
+    ${kdl_parser_LIBRARIES}
+    pthread
+)
 
 add_executable(control_kinova
     src/control_kinova.cpp

include/constants.hpp

@@ -0,0 +1,38 @@
+#ifndef _CONSTANTS_HPP_
+#define _CONSTANTS_HPP_
+
+#include <vector>
+#include <string>
+
+namespace constants {
+
+namespace kinova {
+
+    const std::vector<double> JOINT_ACCEL_LIMITS {5.19, 5.19, 5.19, 5.19, 9.99, 9.99, 9.99};
+    const std::vector<double> JOINT_TORQUE {39.0, 39.0, 39.0, 39.0, 9.0, 9.0, 9.0};
+    const std::vector<double> JOINT_STOP_TORQUE_LIMITS {39.0, 39.0, 39.0, 39.0, 13.0, 13.0, 13.0};
+    const std::vector<double> JOINT_INERTIA {0.5580, 0.5580, 0.5580, 0.5580, 0.1389, 0.1389, 0.1389};
+
+    const std::string URDF_PATH = "kinova_libs/extracted/urdf/kinova-gen3_urdf_V12.urdf";
+
+    const std::string FRAME_JOINT_0 = "base_link";
+
+    /**
+    * With Bracelet_Link parameter, the last frame is at joint 7.
+    * Mass and COM of the last (end-effector) link are included but not the real end-effector's frame.
+    * Arm length: 1.12586m
+    */
+   const std::string FRAME_JOINT_6 = "Bracelet_Link";
+
+   /**
+    * With EndEffector_Link parameter, last frame is at the real end-effector's frame.
+    * However, in the urdf model, joint between Bracelet_Link and EndEffector_Link is fixed (not counted in KDL).
+    * Vereshchagin does not support un-equal number of joints and segments
+    * Arm length: 1.1873m
+    */
+   const std::string FRAME_END_EFFECTOR = "EndEffector_Link";
+};
+
+};
+
+#endif

include/kinova_util.h

@@ -4,6 +4,8 @@
 #include <string>
 #include <memory>
 
+#include <kdl_parser/kdl_parser.hpp>
+
 #include <BaseClientRpc.h>
 #include <BaseCyclicClientRpc.h>
 #include <ActuatorConfigClientRpc.h>
@@ -35,6 +37,8 @@ class KinovaBaseConnection
     ~KinovaBaseConnection();
 };
 
+void loadUrdfModel(const std::string &pUrdfPath, KDL::Tree &pKinovaTree, KDL::Chain &pKinovaChain);
+
 void move_to_home_position(k_api::Base::BaseClient* pBase, uint32_t pTimeoutSec = 20);
 
 void handleKinovaException(k_api::KDetailedException& ex);

src/control_kinova.cpp

@@ -2,7 +2,6 @@
 * Copyright (c) 2020 Minh Nguyen inc. All rights reserved.
 *
 */
-
 #include <iostream>
 #include <string>
 #include <vector>
@@ -13,9 +12,12 @@
 
 #include "kinova_util.h"
 #include "abag.h"
+#include "constants.hpp"
 
 #include <unistd.h>
 
+#define REPO_DIR "/home/minh/workspace/kinova_control_experiments/"
+
 namespace k_api = Kinova::Api;
 namespace sc = std::chrono;
 
@@ -35,6 +37,11 @@ void example_cyclic_torque_control (
     k_api::ActuatorConfig::ActuatorConfigClient* actuator_config
 )
 {
+    const std::string UrdfPath = REPO_DIR + constants::kinova::URDF_PATH;
+    std::cout << "loading URDF file at: " << UrdfPath << std::endl;
+    KDL::Tree kinovaTree; KDL::Chain kinovaChain;
+    loadUrdfModel(UrdfPath, kinovaTree, kinovaChain);
+
     // Clearing faults
     try
     {

src/kinova_util.cpp

@@ -1,6 +1,8 @@
 #include <chrono>
+#include <urdf/model.h>
 #include <KDetailedException.h>
 #include "kinova_util.h"
+#include "constants.hpp"
 
 KinovaBaseConnection::KinovaBaseConnection (
     std::string pHost, uint32_t pTcpPort, uint32_t pUdpPort, std::string pUsername, std::string pPassword
@@ -133,6 +135,23 @@ void move_to_home_position(k_api::Base::BaseClient* base, uint32_t pTimeoutSec)
     }
 }
 
+void loadUrdfModel(const std::string &pUrdfPath, KDL::Tree &pKinovaTree, KDL::Chain &pKinovaChain) {
+    urdf::Model kinovaModel;
+    if (!kinovaModel.initFile(pUrdfPath))
+    {
+        throw std::runtime_error("Failed to parse urdf robot model from: " + pUrdfPath);
+    }
+
+    // Extract KDL tree from the URDF file
+    if (!kdl_parser::treeFromUrdfModel(kinovaModel, pKinovaTree))
+    {
+        throw std::runtime_error("Failed to construct kdl tree from URDF model");
+    }
+
+    // Extract KDL chain from KDL tree
+    pKinovaTree.getChain(constants::kinova::FRAME_JOINT_0, constants::kinova::FRAME_JOINT_6, pKinovaChain);
+}
+
 void handleKinovaException(k_api::KDetailedException& ex) {
     std::cerr << "Kortex exception: " << ex.what() << std::endl;
     std::cerr << "Error sub-code: "