poppy_ros

CMakeLists.txt

@@ -0,0 +1,235 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(poppy_ros)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS 
+roscpp 
+rospy 
+std_msgs 
+genmsg
+)
+
+find_package(OpenCV REQUIRED COMPONENTS 
+   core
+   )
+   
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+set (OpenCV_DIR “/usr/lib/opencv”)
+
+## Generate services in the 'srv' folder
+add_service_files(
+   FILES
+   IK.srv
+ )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+   DEPENDENCIES
+  std_msgs
+ )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES poppy_ros
+#  CATKIN_DEPENDS roscpp std_msgs
+#  DEPENDS system_lib
+)
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+ include
+  ${catkin_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+  
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/poppy_ros.cpp
+# )
+
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/poppy_ros_node.cpp)
+add_executable(poppy_ros src/poppy_ros.cpp
+src/DynamixelHandler.cpp)
+add_executable(ik_server src/ik_server.cpp 
+src/Kinematics.cpp
+src/RotationMatrix.cpp 
+src/TransformationMatrix.cpp 
+src/TranslationMatrix.cpp)
+add_executable(ik_client src/ik_client.cpp)
+
+
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+target_link_libraries(poppy_ros dxl_x64_cpp rt ${catkin_LIBRARIES})
+target_link_libraries(ik_server ${catkin_LIBRARIES} ${OpenCV_LIBS})
+target_link_libraries(ik_client ${catkin_LIBRARIES} ${OpenCV_LIBS})
+add_dependencies(ik_server poppy_ros_gencpp)
+add_dependencies(ik_server poppy_ros_gencpp)
+
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_poppy_ros.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

include/DynamixelHandler.h

@@ -0,0 +1,82 @@
+#if defined(__linux__) || defined(__APPLE__)
+#include <fcntl.h>
+#include <getopt.h>
+#include <termios.h>
+#define STDIN_FILENO 0
+#elif defined(_WIN32) || defined(_WIN64)
+#include <conio.h>
+#endif
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string>
+#include <iostream>
+#include <vector>
+
+#include "dynamixel_sdk/dynamixel_sdk.h"    
+
+
+#define ADDR_XL320_TORQUE_ENABLE 24
+#define ADDR_XL320_LED_ON 25
+
+#define ADDR_XL320_D_GAIN 27
+#define ADDR_XL320_I_GAIN 28
+#define ADDR_XL320_P_GAIN 29
+
+#define ADDR_XL320_GOAL_POSITION 30
+#define ADDR_XL320_GOAL_VELOCITY 32
+#define ADDR_XL320_GOAL_TORQUE 35
+
+#define ADDR_XL320_PRESENT_POSITION 37
+#define ADDR_XL320_PRESENT_VELOCITY 39
+#define ADDR_XL320_PRESENT_LOAD 41
+#define ADDR_XL320_PRESENT_VOLTAGE 45
+#define ADDR_XL320_PRESENT_TEMPERATURE 46
+
+#define ADDR_XL320_HARDWARE_ERROR_STATUS 50
+
+#define NB_JOINTS 6
+
+
+class DynamixelHandler
+{
+	
+public:
+	DynamixelHandler();
+	~DynamixelHandler();
+
+public:
+	//scan();
+	//ping();
+	//reboot();
+	bool openPort();
+	void closePort();
+	bool setBaudRate(int);
+	void setDeviceName(std::string);
+	void setProtocolVersion(float);
+	bool enableTorque(bool);
+	
+	bool readCurrentJointPosition(std::vector<uint16_t>& vCurrentJointPosition);
+	bool readCurrentJointTorque(std::vector<uint16_t>& vCurrentJointTorque);
+	bool sendTargetJointPosition(std::vector<uint16_t>& vTargetJointPosition);
+
+	
+
+private:
+	std::string m_sDeviceName;
+	float m_fProtocolVersion;
+	int m_i32BaudRate;
+	
+	dynamixel::PortHandler* m_pPortHandler;
+	dynamixel::PacketHandler* m_pPacketHandler;
+	
+	bool m_bIsDeviceNameSet;
+	bool m_bIsProtocolVersionSet;
+	bool m_bIsPortOpened;
+	bool m_bIsBaudRateSet;
+	
+	int m_i32DxlCommunicationResult;             // Communication result
+	uint8_t m_ui8DxlError;                          // Dynamixel error
+	std::vector<uint16_t> m_vDxlCurrentPosition;              // Present position
+
+};

include/Kinematics.h

@@ -0,0 +1,81 @@
+#ifndef KINEMATICS_
+#define KINEMATICS_
+
+#include <vector>
+#include <string>
+#include <random>
+#include <limits>
+
+#include "opencv2/opencv.hpp"
+
+#include "eigen3/Eigen/Dense"
+
+#include "TransformationMatrix.h"
+
+#define KINEMATICS_REVOLUTE_JOINT 1
+#define KINEMATICS_PRISMATIC_JOINT 2
+
+class Kinematics
+{
+	public:
+		// Constructors
+		Kinematics();
+		Kinematics(std::vector<double>& vDHTheta, std::vector<double>& vDHD, std::vector<double>& vDHA, std::vector<double>& vDHAlpha, std::vector<int>& vJointType, std::vector<double>& vJointOffset, std::vector<double>& vQiValues, int i32NbJoints, int i32MaxNbIterations, double f64PController, double f64RankThreshold, double f64DistanceThreshold);
+		// Destructor
+		~Kinematics();
+			
+		// DH Params
+		void loadDHParameters(std::string sDHParametersFilename);
+		void setDHParameters(std::vector<double>& vDHTheta, std::vector<double>& vDHD, std::vector<double>& vDHA, std::vector<double>& vDHAlpha, std::vector<int>& vJointType, std::vector<double>& vJointOffset, std::vector<double>& vQiValues, int i32NbJoints);
+		void updateDHParameters(std::vector<double> vQiValues);
+		void updateDHParameters();
+		std::vector<double> getQiValues();
+
+		// FK
+		cv::Mat dh2FK(int i32StartJointNumber);
+		cv::Mat computeCurrentEndEffectorPosition();
+	
+		// IK
+		void loadIKParameters(std::string sIKParametersFilename);
+		void inverseKinematics(cv::Mat oXTarget);
+
+		// Database
+		void createKinematicDatabase(int i32NbSamples, std::string sKinematicDatabaseFilename, int i32MinValueRevoluteJoint, int i32MaxValueRevoluteJoint, int i32MinValuePrismaticJoint, int i32MaxValuePrismaticJoint);
+		void loadKinematicDatabase(std::string sKinematicDatabaseFilename);
+		void findClosestCandidateInDatabase(cv::Mat& oXtarget);
+
+	private:
+		cv::Mat skewSymmetricMatrix(double fX, double fY, double fZ);
+
+		cv::Mat computeVee(cv::Mat oXtarget);
+
+		cv::Mat computeNumericalJacobian();
+
+		cv::Mat computePseudoInverseJacobian(cv::Mat oNumericalJacobian);
+
+		double computeDistanceToTarget(cv::Mat& oXtarget);
+
+	private:
+		// DH Params
+		bool m_bAreDHParametersSet;
+		int m_i32NbJoints;
+		std::vector<double> m_vDHTheta;
+		std::vector<double> m_vDHD;
+		std::vector<double> m_vDHA;
+		std::vector<double> m_vDHAlpha;
+		std::vector<double> m_vJointOffset;
+		std::vector<int> m_vJointType;
+		std::vector<double> m_vQiValues;
+		std::vector<double> m_vQiOriginalValues;
+		// Database
+		bool m_bIsDatabaseLoaded;
+		cv::Mat m_oRandomEndEffectorPositions;
+		cv::Mat m_oRandomJointValues;
+		// IK
+		int m_i32MaxNbIterations;
+		double m_f64PController;
+		double m_f64RankThreshold;
+		double m_f64DistanceThreshold;
+};
+
+#endif

include/RotationMatrix.h

@@ -0,0 +1,84 @@
+#ifndef ROTATION_MATRIX_
+#define ROTATION_MATRIX_
+
+#define _USE_MATH_DEFINES
+#include <cmath>
+
+#include "opencv2/opencv.hpp"
+
+#define RxRyRz 0
+#define RzRyRx 1
+
+class RotationMatrix
+{
+	public:
+		RotationMatrix();
+	
+		RotationMatrix(double fRoll, double fPitch, double fYaw, int i32Order);
+	
+		RotationMatrix(cv::Mat& oRotationMatrix);
+
+		~RotationMatrix();
+
+		/**
+		* \brief Sets new roll, pitch and yaw angles.
+		* \param [in] roll : value of the roll angles in degrees
+		* \param [in] pitch : value of the pitch angles in degrees
+		* \param [in] yaw : value of the yaw angles in degrees
+		*/
+		void setRollPitchYawAngles(double fRoll, double fPitch, double fYaw);
+	
+		/**
+		* \brief Sets the order of operation RxRyRz or RzRyRx.
+		* \param [in] order : order of operation (either RxRyRz or RzRyRx i.e. Cardanian representation) 
+		*/
+		void setOrderOfOperations(int i32Order);
+	
+		cv::Mat getRotationMat();
+		
+		cv::Mat getRotationMatInverse();
+		
+		/**
+		* \brief Computes the rotation matrix from the roll, pitch and yaw angles and the order of operations
+		*/
+		void compute();
+		
+		/**
+		* \brief Computes the inverse of the rotation matrix i.e. its transpose
+		*/
+		void inverse();
+		
+		/**
+		* \brief Displays the rotation matrix and its inverse (if computed)
+		*/
+		void disp();
+
+	private:
+		/**
+		* \brief Converts an angle from degrees to radians
+		* \param [in] angleInDegree : input angle in degrees to be converted in radians
+		*\return a double corresponding to the angle in radians
+		*/
+		double deg2rad(double angleInDegrees);
+	
+		/**
+		* \brief Converts an angle from radian to degree
+		* \param [in] angleInRadians : input angle in radians to be converted in degrees
+		*\return a double corresponding to the angle in degrees
+		*/
+		double rad2deg(double angleInRadians);
+
+	private:
+		bool m_bIsMatrixComputed;		/**< boolean value to check if the rotation matrix is already computed */
+		bool m_bIsMatrixInverseComputed;	/**< boolean value to check if the rotation matrix inverse is already computed */
+		double m_fRollAngle;				/**< roll angle in degrees */
+		double m_fPitchAngle;				/**< pitch angle in degrees */
+		double m_fYawAngle;				/**< yaw angle in degrees */
+		int m_i32OrderOfOperations;		/**< order of operations */
+		cv::Mat m_oRotationMatrix; 		/**< rotation matrix */
+		cv::Mat m_oRotationMatrixInverse; 	/**< inverse rotation matrix */
+
+};
+
+
+#endif

include/TransformationMatrix.h

@@ -0,0 +1,69 @@
+#ifndef TRANSFORMATION_MATRIX_
+#define TRANSFORMATION_MATRIX_
+
+#define _USE_MATH_DEFINES
+#include <cmath>
+
+
+#include "opencv2/opencv.hpp"
+
+#include "RotationMatrix.h"
+#include "TranslationMatrix.h"
+
+class TransformationMatrix
+{
+	public:
+		TransformationMatrix();
+	
+		TransformationMatrix(double fRoll, double fPitch, double fYaw, int i32Order, double fTx, double fTy, double fTz);
+
+		TransformationMatrix(cv::Mat& oTransformationMatrix);
+	
+		TransformationMatrix(cv::Mat& rotationMatrix, cv::Mat& translationMatrix);
+	
+		~TransformationMatrix();
+	
+		void setRotationComponents(double fRoll, double fPitch, double fYaw, int i32Order);
+	
+		void setTranslationComponents(double fTx, double fTy, double fTz);
+
+		cv::Mat getTransformationMat();
+
+		cv::Mat getTransformationMatInverse();
+
+		RotationMatrix* getRotationMatrix();
+
+		TranslationMatrix* getTranslationMatrix();
+
+		/**
+		* \brief Computes the transformation matrix from the roll, pitch and yaw angles, the order of operations and the translation components
+		*/
+		void compute();
+		
+		/**
+		* \brief Computes the inverse of the transformation matrix i.e. 	(R^-1	|-R^-1t)
+		*											-----------------
+		*											(   0		|   1)
+		*/
+		void inverse();
+		
+		/**
+		* \brief Displays the transformation matrix and its inverse (if computed)
+		*/
+		void disp();
+
+	
+	private:
+		bool m_bAreRotationComponentsSet;
+		bool m_bAreTranslationComponentsSet;
+		bool m_bIsMatrixComputed;		/**< boolean value to check if the rotation matrix is already computed */
+		bool m_bIsMatrixInverseComputed;	/**< boolean value to check if the rotation matrix inverse is already computed */
+		RotationMatrix* m_pRotationMatrix;	/**< pointer to a rotation matrix (3x3) */
+		TranslationMatrix* m_pTranslationMatrix;	/**< pointer to a translation matrix (3x1) */
+		cv::Mat m_oTransformationMatrix;
+		cv::Mat m_oTransformationMatrixInverse;
+		
+};
+
+
+#endif

include/TranslationMatrix.h

@@ -0,0 +1,48 @@
+#ifndef TRANSLATION_MATRIX_
+#define TRANSLATION_MATRIX_
+
+#include "opencv2/opencv.hpp"
+
+
+class TranslationMatrix
+{
+	public:
+		TranslationMatrix();
+	
+		TranslationMatrix(double fTx, double fTy, double fTz);
+
+		TranslationMatrix(cv::Mat& oTranslationMatrix);
+	
+		~TranslationMatrix();
+
+		/**
+		* \brief Sets new translation components.
+		* \param [in] fTx : value of the translation along x-axis in meters
+		* \param [in] fTy : value of the translation along y-axis in meters
+		* \param [in] fTz : value of the translation along z-axis in meters
+		*/
+		void setTranslationComponents(double fTx, double fTy, double fTz);
+	
+		cv::Mat getTranslationMat();
+	
+		/**
+		* \brief Computes the rotation matrix from the roll, pitch and yaw angles and the order of operations
+		*/
+		void compute();
+		
+		/**
+		* \brief Displays the translation matrix(if computed)
+		*/
+		void disp();
+
+	private:
+		bool m_bIsMatrixComputed;		/**< boolean value to check if the translation matrix is already computed */
+		double m_fTx;					/**< translation along x in meters */
+		double m_fTy;					/**< translation along y in meters */
+		double m_fTz;					/**< translation along z in meters */
+		cv::Mat m_oTranslationMatrix; 		/**< translation matrix */
+		
+};
+
+
+#endif

package.xml

@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>poppy_ros</name>
+  <version>0.0.0</version>
+  <description>The poppy_ros package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="tim@todo.todo">tim</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/beginner_tutorials</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+    <build_depend>message_generation</build_depend> 
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <exec_depend>message_runtime</exec_depend> 
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

poppy_dh.yaml

@@ -0,0 +1,11 @@
+%YAML:1.0
+---
+dhThetaParams: [ 0., 0., 0., 0., 0., 0., 0. ]
+dhDParams: [ 2.5000000000000000e+00, 2.5000000000000000e+00, 0., 0., 0.,
+    0., 0. ]
+dhAParams: [ 0., 0., 5.5000000000000000e+00, 3., 0., 5., 12. ]
+dhAlphaParams: [ 0., 90., 0., 0., -90., 90., 0. ]
+jointType: [ 0, 1, 1, 1, 0, 1, 1 ]
+nbJoints: 5
+jointOffset: [ 0., 0., 90., 0., -90., 0., 0. ]
+qiValues: [ 0., 0., 0., 0., 90. ]

poppy_ik.yaml

@@ -0,0 +1,6 @@
+%YAML:1.0
+---
+maxNbIterations: 1000
+pController: 0.5
+rankThreshold: 0.2
+distanceThreshold: 0.01

src/DynamixelHandler.cpp

@@ -0,0 +1,256 @@
+#include "DynamixelHandler.h"
+
+DynamixelHandler::DynamixelHandler():
+	m_sDeviceName(""), m_fProtocolVersion(0.0), m_i32BaudRate(0),
+	m_pPacketHandler(nullptr), m_pPortHandler(nullptr),
+	m_bIsDeviceNameSet(false), m_bIsProtocolVersionSet(false), m_bIsPortOpened(false), m_bIsBaudRateSet(false),
+	m_ui8DxlError(0), m_i32DxlCommunicationResult(COMM_TX_FAIL)
+{
+	
+}
+
+DynamixelHandler::~DynamixelHandler()
+{
+	
+}
+
+bool DynamixelHandler::openPort()
+{
+	if (m_pPortHandler == nullptr)
+	{
+		std::cout << "[ERROR](DynamixelHandler::openPort) m_pPortHandler is null!" << std::endl;
+		m_bIsPortOpened = false;
+		return m_bIsPortOpened;
+	}
+	
+	if (!m_bIsDeviceNameSet)
+	{
+		std::cout << "[ERROR](DynamixelHandler::openPort) m_sDeviceName is not set!" << std::endl;
+		m_bIsPortOpened = false;
+		return m_bIsPortOpened;
+	}
+	
+	if (m_bIsPortOpened)
+	{
+		std::cout << "[WARNING](DynamixelHandler::openPort) port is already opened!" << std::endl;
+		return m_bIsPortOpened;
+	}
+	
+	if (m_pPortHandler->openPort())
+	{
+		std::cout << "[INFO](DynamixelHandler::openPort) Succeeded to open the port!" << std::endl;
+		m_bIsPortOpened = true;
+	}
+	else
+	{
+		std::cout << "[ERROR](DynamixelHandler::openPort) Failed to open the port!" << std::endl;
+		m_bIsPortOpened = false;
+	}
+	return m_bIsPortOpened;
+}
+
+void DynamixelHandler::closePort()
+{
+	if (m_pPortHandler == nullptr)
+	{
+		std::cout << "[ERROR](DynamixelHandler::closePort) m_pPortHandler is null!" << std::endl;
+		m_bIsPortOpened = false;
+		return;
+	}
+	
+	if (!m_bIsPortOpened)
+	{
+		std::cout << "[WARNING](DynamixelHandler::openPort) port is already closed!" << std::endl;
+		return;
+	}
+	
+	m_pPortHandler->closePort();
+	
+	std::cout << "[INFO](DynamixelHandler::closePort) Succeeded to close the port!" << std::endl;
+	m_bIsPortOpened = false;
+}
+
+bool DynamixelHandler::setBaudRate(int i32BaudRate)
+{
+	m_i32BaudRate = i32BaudRate;
+		
+	if (nullptr != m_pPortHandler)
+	{
+		if (m_pPortHandler->setBaudRate(m_i32BaudRate))
+		{
+			std::cout << "[INFO](DynamixelHandler::setBaudRate) Succeeded to change the baudrate!" << std::endl;
+			m_bIsBaudRateSet = true;
+		}
+		else
+		{
+			std::cout << "[ERROR](DynamixelHandler::setBaudRate) Failed to change the baudrate!" << std::endl;
+			m_bIsBaudRateSet = false;
+		}
+	}
+	else
+	{
+		std::cout << "[ERROR](DynamixelHandler::setBaudRate) m_pPortHandler is null!" << std::endl;
+		m_bIsBaudRateSet = false;
+	}
+	return m_bIsBaudRateSet;
+}
+
+void DynamixelHandler::setDeviceName(std::string sDeviceName)
+{
+	m_sDeviceName = sDeviceName;
+	m_bIsDeviceNameSet = true;
+	
+	if (nullptr != m_pPortHandler)
+	{
+		delete m_pPortHandler;
+		m_pPortHandler = nullptr;
+	}
+	
+	// Initialize PortHandler instance
+	m_pPortHandler = dynamixel::PortHandler::getPortHandler(m_sDeviceName.c_str());
+}
+
+void DynamixelHandler::setProtocolVersion(float fProtocolVersion)
+{
+	m_fProtocolVersion = fProtocolVersion;
+	m_bIsProtocolVersionSet = true;
+	
+	if (nullptr != m_pPacketHandler)
+	{
+		delete m_pPacketHandler;
+		m_pPacketHandler = nullptr;
+	}
+	
+	m_pPacketHandler = dynamixel::PacketHandler::getPacketHandler(m_fProtocolVersion);
+}
+
+bool DynamixelHandler::readCurrentJointPosition(std::vector<uint16_t>& vCurrentJointPosition)
+{
+	bool bIsReadSuccessfull = false;
+
+	for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++)
+	{
+		int dxl_comm_result = COMM_TX_FAIL;             // Communication result
+		uint8_t dxl_error = 0;
+		uint16_t dxl_present_position = 0;
+
+		dxl_comm_result = m_pPacketHandler->read2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_PRESENT_POSITION, &dxl_present_position, &dxl_error);
+		if (dxl_comm_result != COMM_SUCCESS)
+		{
+			std::cout << "[ERROR] " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl;
+			bIsReadSuccessfull = false;
+		}
+		else if (dxl_error != 0)
+		{
+			std::cout << "[ERROR] " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl;
+			bIsReadSuccessfull = false;
+		}
+		else
+		{
+			vCurrentJointPosition.push_back(dxl_present_position);
+			bIsReadSuccessfull = true;
+		}
+	}
+
+	return bIsReadSuccessfull;
+}
+
+
+bool DynamixelHandler::readCurrentJointTorque(std::vector<uint16_t>& vCurrentJointTorque)
+{
+	bool bIsReadSuccessfull = false;
+
+	for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++)
+	{
+		int dxl_comm_result = COMM_TX_FAIL;             // Communication result
+		uint8_t dxl_error = 0;
+		uint16_t dxl_present_torque = 0;
+
+		dxl_comm_result = m_pPacketHandler->read2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_PRESENT_LOAD, &dxl_present_torque, &dxl_error);
+		if (dxl_comm_result != COMM_SUCCESS)
+		{
+			std::cout << "[ERROR] " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl;
+			bIsReadSuccessfull = false;
+		}
+		else if (dxl_error != 0)
+		{
+			std::cout << "[ERROR] " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl;
+			bIsReadSuccessfull = false;
+		}
+		else
+		{
+			vCurrentJointTorque.push_back(dxl_present_torque);
+			bIsReadSuccessfull = true;
+		}
+	}
+
+	return bIsReadSuccessfull;
+}
+
+
+bool DynamixelHandler::sendTargetJointPosition(std::vector<uint16_t>& vTargetJointPosition)
+{
+	bool bIsSendSuccessfull = false;
+
+	// checks if the vector size is correct
+	if (vTargetJointPosition.size() != NB_JOINTS)
+	{
+		std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): Size of command vector is not correct: " << vTargetJointPosition.size() << " instead of " << NB_JOINTS << "!" << std::endl;
+		bIsSendSuccessfull = false;
+	}
+
+
+
+	for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++)
+	{
+		int dxl_comm_result = COMM_TX_FAIL;             // Communication result
+		uint8_t dxl_error = 0;
+		uint16_t dxl_present_position = 0;
+		dxl_comm_result = m_pPacketHandler->write2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_GOAL_POSITION, vTargetJointPosition[l_joint], &dxl_error);
+		if (dxl_comm_result != COMM_SUCCESS)
+		{
+			std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl;
+			bIsSendSuccessfull = false;
+		}
+		else if (dxl_error != 0)
+		{
+			std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl;
+			bIsSendSuccessfull = false;
+		}
+		else
+		{
+			bIsSendSuccessfull = true;
+		}
+	}
+	return bIsSendSuccessfull;
+
+}
+
+bool DynamixelHandler::enableTorque(bool bEnableTorque)
+{
+	bool bIsSendSuccessfull = false;
+	
+	for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++)
+	{
+		int dxl_comm_result = COMM_TX_FAIL;             // Communication result
+		uint8_t dxl_error = 0;
+		
+		dxl_comm_result = m_pPacketHandler->write1ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_TORQUE_ENABLE, bEnableTorque, &dxl_error);
+		if (dxl_comm_result != COMM_SUCCESS) 
+		{
+			std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl;
+			bIsSendSuccessfull = false;
+		}
+		else if (dxl_error != 0) 
+		{
+			std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl;
+			bIsSendSuccessfull = false;
+		}
+		else 
+		{
+			bIsSendSuccessfull = true;
+		}
+	}
+	return bIsSendSuccessfull;
+}
+

src/Kinematics.cpp

@@ -0,0 +1,600 @@
+#include "Kinematics.h"
+
+Kinematics::Kinematics():
+	m_bAreDHParametersSet(false), m_bIsDatabaseLoaded(false),
+	m_i32NbJoints(0), 
+	m_i32MaxNbIterations(0), m_f64PController(0.0), m_f64RankThreshold(0.0), m_f64DistanceThreshold(0.0)
+{}
+
+Kinematics::Kinematics(std::vector<double>& vDHTheta, std::vector<double>& vDHD, std::vector<double>& vDHA, std::vector<double>& vDHAlpha, std::vector<int>& vJointType, std::vector<double>& vJointOffset, std::vector<double>& vQiValues, int i32NbJoints, int i32MaxNbIterations, double f64PController, double f64RankThreshold, double f64DistanceThreshold):
+	m_bAreDHParametersSet(true), m_bIsDatabaseLoaded(false),
+	m_vDHTheta(vDHTheta), m_vDHD(vDHD), m_vDHA(vDHA), m_vDHAlpha(vDHAlpha), m_vJointType(vJointType), m_vJointOffset(vJointOffset), m_vQiValues(vQiValues), m_vQiOriginalValues(vQiValues),
+	m_i32NbJoints(i32NbJoints), 
+	m_i32MaxNbIterations(i32MaxNbIterations), m_f64PController(f64PController), m_f64RankThreshold(f64RankThreshold), m_f64DistanceThreshold(f64DistanceThreshold)
+{
+	updateDHParameters();
+}
+
+Kinematics::~Kinematics()
+{
+	m_oRandomJointValues.release();
+	m_oRandomEndEffectorPositions.release();
+}
+
+void Kinematics::updateDHParameters(std::vector<double> vQiValues)
+{
+	m_vQiValues = vQiValues;
+	updateDHParameters();
+}
+
+void Kinematics::updateDHParameters()
+{
+	int l_i32JointIndex = 0;
+	
+	for (int l_joint = 0; l_joint < m_vJointType.size(); l_joint++)
+	{
+		if (m_vJointType[l_joint] == KINEMATICS_REVOLUTE_JOINT)
+		{
+			/*std::cout << "l_i32JointIndex = " << l_i32JointIndex << std::endl;
+			std::cout << "l_joint = " << l_joint << std::endl;
+			std::cout << "m_vQiValues[l_i32JointIndex] = " << m_vQiValues[l_i32JointIndex] << std::endl;
+			std::cout << "m_vJointOffset[l_joint]= " << m_vJointOffset[l_joint] << std::endl;
+			*/
+			m_vDHTheta[l_joint] = m_vQiValues[l_i32JointIndex] + m_vJointOffset[l_joint];
+			l_i32JointIndex++;
+		}
+		else if (m_vJointType[l_joint] == KINEMATICS_PRISMATIC_JOINT)
+		{
+			m_vDHD[l_joint] = m_vQiValues[l_i32JointIndex] + m_vJointOffset[l_joint];
+			l_i32JointIndex++;
+		}
+		else //TODO find a way to avoid this else
+		{
+			m_vDHTheta[l_joint] = m_vJointOffset[l_joint];
+		}
+	}
+	
+	/*for (int l_joint = 0; l_joint < m_vDHTheta.size(); l_joint ++)
+	{
+		std::cout << "(" << m_vDHTheta[l_joint] << ", " << m_vDHD[l_joint] << ", " << m_vDHA[l_joint] << ", " << m_vDHAlpha[l_joint] << ") " << std::endl; 
+	}*/
+	
+}
+
+void Kinematics::setDHParameters(std::vector<double>& vDHTheta, std::vector<double>& vDHD, std::vector<double>& vDHA, std::vector<double>& vDHAlpha, std::vector<int>& vJointType, std::vector<double>& vJointOffset, std::vector<double>& vQiValues, int i32NbJoints)
+{
+	m_vDHTheta = vDHTheta;
+	m_vDHD = vDHD;
+	m_vDHA = vDHA;
+	m_vDHAlpha = vDHAlpha;
+	m_vJointType = vJointType;
+	m_vJointOffset = vJointOffset;
+	m_vQiValues = vQiValues;
+	m_i32NbJoints = i32NbJoints;
+
+	// check if the vectors have the same size
+	if (m_vDHTheta.size() != m_vDHD.size() || m_vDHTheta.size() != m_vDHA.size() || m_vDHTheta.size() != m_vDHAlpha.size() || m_vDHTheta.size() != m_vJointType.size() || m_vDHTheta.size() != m_vJointOffset.size())
+	{
+		std::cerr << "(Kinematics::setDHParameters) The vectors of DH parameters do not have the same size!" << std::endl;
+		m_bAreDHParametersSet = false;
+	}
+	else
+	{
+		updateDHParameters();
+		m_bAreDHParametersSet = true;
+	}
+}
+
+std::vector<double> Kinematics::getQiValues()
+{
+	return m_vQiValues;
+}
+
+cv::Mat Kinematics::dh2FK(int i32StartJointNumber)
+{
+	cv::Mat l_oTransformationMatrix_jTee = cv::Mat::eye(4, 4, CV_64F);
+
+	if (!m_bAreDHParametersSet)
+	{
+		std::cerr << "(Kinematics::dh2FK) D-H parameters have not been set yet!" << std::endl;
+		return l_oTransformationMatrix_jTee;
+	}
+
+	if (i32StartJointNumber < 0 || i32StartJointNumber > m_vDHTheta.size())
+	{
+		std::cerr << "(Kinematics::dh2FK) the value of i32StartJointNumber is not in the right range!, i32StartJointNumber= "
+			<< i32StartJointNumber << ", [0, " << m_vDHTheta.size() << "[" << std::endl;
+		return l_oTransformationMatrix_jTee;
+	}
+
+
+	for (int l_joint = i32StartJointNumber; l_joint < m_vDHTheta.size(); l_joint++)
+	{
+		TransformationMatrix l_oThetaTransformationMatrix(0.0f, 0.0f, m_vDHTheta[l_joint], 1, 0.0f, 0.0f, 0.0f); //Rz
+		TransformationMatrix l_oDTransformationMatrix(0.0f, 0.0f, 0.0f, 1, 0.0f, 0.0f, m_vDHD[l_joint]); //Tz
+		TransformationMatrix l_oATransformationMatrix(0.0f, 0.0f, 0.0f, 1, m_vDHA[l_joint], 0.0f, 0.0f); //Tx
+		TransformationMatrix l_oAlphaTransformationMatrix(m_vDHAlpha[l_joint], 0.0f, 0.0f, 1, 0.0f, 0.0f, 0.0f); //Rx
+
+		l_oTransformationMatrix_jTee *= l_oThetaTransformationMatrix.getTransformationMat() * l_oDTransformationMatrix.getTransformationMat() * l_oATransformationMatrix.getTransformationMat() * l_oAlphaTransformationMatrix.getTransformationMat();
+	}
+
+	return l_oTransformationMatrix_jTee;
+}
+
+
+cv::Mat Kinematics::skewSymmetricMatrix(double fX, double fY, double fZ)
+{
+	cv::Mat l_oSkewSymmetricMatrix(3, 3, CV_64F, 0.0f);
+
+	l_oSkewSymmetricMatrix.at<double>(0, 0) = 0.0f; l_oSkewSymmetricMatrix.at<double>(0, 1) = -fZ; l_oSkewSymmetricMatrix.at<double>(0, 2) = fY;
+	l_oSkewSymmetricMatrix.at<double>(1, 0) = fZ; l_oSkewSymmetricMatrix.at<double>(1, 1) = 0.0f; l_oSkewSymmetricMatrix.at<double>(1, 2) = -fX;
+	l_oSkewSymmetricMatrix.at<double>(2, 0) = -fY; l_oSkewSymmetricMatrix.at<double>(2, 1) = fX; l_oSkewSymmetricMatrix.at<double>(2, 2) = 0.0f;
+
+	return l_oSkewSymmetricMatrix;
+}
+
+cv::Mat Kinematics::computeVee(cv::Mat oXtarget)
+{
+	cv::Mat l_oVee(6, 1, CV_64F, 0.0f);
+	
+	// estimates the current X (position and orientation of the end-effector)
+	cv::Mat l_oXcurrent_hc = computeCurrentEndEffectorPosition();
+
+	cv::Mat l_oXcurrent(3, 1, CV_64F, 0.0f);
+	l_oXcurrent_hc(cv::Rect(0, 0, l_oXcurrent_hc.cols, l_oXcurrent_hc.rows-1)).copyTo(l_oXcurrent); // remove homogeneous coordinate part i.e. last element (= 1)
+	
+	// checks if the matrices have the same size
+	if (l_oXcurrent.rows != oXtarget.rows || l_oXcurrent.cols != oXtarget.cols)
+	{
+		std::cerr << "(Kinematics::computeVee) Xcurrent and Xtarget matrices do not have the same size!" << std::endl;
+		return l_oVee;
+	}
+
+	// computes deltaX
+	cv::Mat l_oDeltaX = oXtarget - l_oXcurrent;
+
+	// computes Xdot
+	cv::Mat l_oXdot(6, 1, CV_64F, 0.0f); // adds zeros for orientation as we are controlling position only
+	l_oDeltaX.copyTo(l_oXdot(cv::Rect(0, 0, l_oDeltaX.cols, l_oDeltaX.rows)));
+	l_oXdot *= m_f64PController;
+
+	// retrieves eeRb from bTee
+	cv::Mat l_oTransformationMatrix_bTee = dh2FK(0);
+	TransformationMatrix l_oTransformationMatrix(l_oTransformationMatrix_bTee);
+	cv::Mat l_oeeTb = l_oTransformationMatrix.getTransformationMatInverse(); 
+	cv::Mat l_oeeRb = l_oeeTb(cv::Rect(0, 0, 3, 3));
+
+	// creates the transformation matrix Xdot->Vee i.e. [eeRb(3x3) zeros(3x3); zeros(3x3) eeRb(3x3)]
+	cv::Mat l_oXdot2Vee(6, 6, CV_64F, 0.0f);
+	l_oeeRb.copyTo(l_oXdot2Vee(cv::Rect(0, 0, l_oeeRb.cols, l_oeeRb.rows)));
+	l_oeeRb.copyTo(l_oXdot2Vee(cv::Rect(3, 3, l_oeeRb.cols, l_oeeRb.rows)));
+
+	// computes Vee = Xdot2Vee * Xdot
+	l_oVee = l_oXdot2Vee * l_oXdot;
+		
+	return l_oVee;
+}
+
+cv::Mat Kinematics::computeNumericalJacobian()
+{
+	cv::Mat l_oNumericalJacobian(6, m_i32NbJoints, CV_64F, 0.0f);
+
+	int l_i32JointIndex = 0;
+	for (int l_joint = 0; l_joint < m_vJointType.size(); l_joint++)
+	{
+		// checks if the current index corresponds to either a revolute or prismatic joint
+		if ((m_vJointType[l_joint] == KINEMATICS_REVOLUTE_JOINT) || (m_vJointType[l_joint] == KINEMATICS_PRISMATIC_JOINT))
+		{
+			// computes the transformation matrix jTee from the DH parameters
+			cv::Mat l_oTransformationMatrix_jTee = dh2FK(l_joint);
+			TransformationMatrix l_oTransformationMatrix(l_oTransformationMatrix_jTee);
+
+			// extracts the translation part and computes the skew symmetric matrix
+			cv::Mat l_oTranslationMatrix_jtee = l_oTransformationMatrix.getTranslationMatrix()->getTranslationMat();
+			cv::Mat l_oSkewSymmetricMatrix = skewSymmetricMatrix(l_oTranslationMatrix_jtee.at<double>(0, 0), l_oTranslationMatrix_jtee.at<double>(1, 0), l_oTranslationMatrix_jtee.at<double>(2, 0));
+
+			// extracts the rotation part eeRj
+			cv::Mat l_oTransformationMatrix_eeTj = l_oTransformationMatrix.getTransformationMatInverse();
+			cv::Mat l_oRotationMatrix_eeRj = l_oTransformationMatrix_eeTj(cv::Rect(0, 0, 3, 3));
+
+			// computes Tj i.e. [eeRj -eeRj*ssm; 0 eeRj]]
+			cv::Mat l_oTj(6, 6, CV_64F, 0.0f);
+			l_oRotationMatrix_eeRj.copyTo(l_oTj(cv::Rect(0, 0, l_oRotationMatrix_eeRj.cols, l_oRotationMatrix_eeRj.rows)));
+			l_oRotationMatrix_eeRj.copyTo(l_oTj(cv::Rect(3, 3, l_oRotationMatrix_eeRj.cols, l_oRotationMatrix_eeRj.rows)));
+			cv::Mat l_oRightUpperPartOfTj = -l_oRotationMatrix_eeRj * l_oSkewSymmetricMatrix;
+			l_oRightUpperPartOfTj.copyTo(l_oTj(cv::Rect(3, 0, l_oRightUpperPartOfTj.cols, l_oRightUpperPartOfTj.rows)));
+
+			// selects the right column depending on the type of joint (revolute or prismatic)
+			int l_i32IndexColumn = 0;
+			if (m_vJointType[l_joint] == KINEMATICS_REVOLUTE_JOINT)
+				l_i32IndexColumn = 5;
+			else if (m_vJointType[l_joint] == KINEMATICS_PRISMATIC_JOINT)
+				l_i32IndexColumn = 2;
+
+			// fills the numerical Jacobian
+			l_oTj(cv::Rect(l_i32IndexColumn, 0, 1, l_oTj.rows)).copyTo(l_oNumericalJacobian(cv::Rect(l_i32JointIndex, 0, 1, l_oTj.rows)));
+
+			// increments the joint index
+			l_i32JointIndex++;
+		}
+	}
+
+	return l_oNumericalJacobian;
+}
+
+cv::Mat Kinematics::computePseudoInverseJacobian(cv::Mat oNumericalJacobian)
+{
+	cv::Mat l_oPseudoInverseJacobian;
+	if (false) // OpenCV SVD generates wrong pseudo inverse
+	{
+		// decomposes the numerical jacobian usind SVD
+		cv::SVD l_oSVD;
+		cv::Mat l_oW, l_oU, l_oVt;
+		l_oSVD.compute(oNumericalJacobian, l_oW, l_oU, l_oVt, cv::SVD::FULL_UV);
+
+		// converts the singular values to matrix format (OpenCV stored them in a column vector)
+		cv::Mat l_oSigma(6, m_i32NbJoints, CV_64F, 0.0);
+		for (int l_joint = 0; l_joint < m_i32NbJoints; l_joint++)
+			l_oSigma.at<double>(l_joint, l_joint) = l_oW.at<double>(l_joint, 0);
+
+		// computes the inverse of the sigma matrix (checks if the ratio between the current singular value and the first one is higher than a threshold)
+		cv::Mat l_oSigmaInv(l_oSigma.cols, l_oSigma.rows, CV_64F, 0.0);
+
+		for (int l_singVal = 0; l_singVal < l_oSigma.cols; l_singVal++)
+		{
+			if (abs(l_oSigma.at<double>(l_singVal, l_singVal) / l_oSigma.at<double>(0, 0)) < m_f64RankThreshold)
+				l_oSigmaInv.at<double>(l_singVal, l_singVal) = 0.0;
+			else
+				l_oSigmaInv.at<double>(l_singVal, l_singVal) = 1.0 / l_oSigma.at<double>(l_singVal, l_singVal);
+		}
+
+		// computes the pseudoinverse matrix of the numerical jacobian
+		l_oPseudoInverseJacobian = l_oVt * l_oSigmaInv * l_oU.t(); // = V * Sinv * Ut if U, S, V
+	}
+	else
+	{
+	
+		// maps the OpenCV matrix with Eigen:
+		Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> oNumJacobian_Eigen(oNumericalJacobian.ptr<double>(), oNumericalJacobian.rows, oNumericalJacobian.cols);
+		
+		// computes the Jacobi SVD of the Numerical Jacobian
+		//Eigen::JacobiSVD<Eigen::MatrixXd> svd(oNumJacobian_Eigen, Eigen::ComputeFullV | Eigen:: ComputeFullU);
+		Eigen::JacobiSVD<Eigen::MatrixXd> svd(oNumJacobian_Eigen, Eigen::ComputeFullV | Eigen:: ComputeFullU);
+		svd.computeU();
+		svd.computeV();
+		
+		// stores the U, V, S matrices
+		Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> oUMatrix_Eigen = svd.matrixU();
+		Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> oVMatrix_Eigen = svd.matrixV();
+		Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> oSigmaMatrix_Eigen = svd.singularValues();
+		
+		// maps back to OpenCV matrices
+		cv::Mat oUMatrix_OpenCV(oUMatrix_Eigen.rows(), oUMatrix_Eigen.cols(), CV_64F, oUMatrix_Eigen.data());
+		cv::Mat oVMatrix_OpenCV(oVMatrix_Eigen.rows(), oVMatrix_Eigen.cols(), CV_64F, oVMatrix_Eigen.data());
+		cv::Mat oSigmaMatrix_OpenCV(oSigmaMatrix_Eigen.rows(), oSigmaMatrix_Eigen.cols(), CV_64F, oSigmaMatrix_Eigen.data());
+
+		// computes the inverse of the sigma matrix (checks if the ratio between the current singular value and the first one is higher than a threshold)
+		cv::Mat l_oSigmaInv(m_i32NbJoints, 6, CV_64F, 0.0);
+
+		for (int l_singVal = 0; l_singVal < oSigmaMatrix_OpenCV.rows; l_singVal++)
+		{
+			if (abs(oSigmaMatrix_OpenCV.at<double>(l_singVal, 0) / oSigmaMatrix_OpenCV.at<double>(0, 0)) < m_f64RankThreshold)
+				l_oSigmaInv.at<double>(l_singVal, l_singVal) = 0.0;
+			else
+				l_oSigmaInv.at<double>(l_singVal, l_singVal) = 1.0 / oSigmaMatrix_OpenCV.at<double>(l_singVal, 0);
+		}
+
+		// computes the pseudo inverse of the jacobian matrix
+		l_oPseudoInverseJacobian = oVMatrix_OpenCV * l_oSigmaInv * oUMatrix_OpenCV.t(); // = V * Sinv * Ut if U, S, V
+	}
+	
+	return l_oPseudoInverseJacobian;
+}
+
+
+double Kinematics::computeDistanceToTarget(cv::Mat& oXtarget)
+{
+	// estimates the current X (position and orientation of the end-effector)
+	cv::Mat l_oXcurrent_hc = computeCurrentEndEffectorPosition();
+	
+	// computes the Euclidian distance between the current point and the target one
+	double l_fDistanceToTarget = 0.0f;
+	for (int l_coord = 0; l_coord < 3; l_coord++)
+	{
+		l_fDistanceToTarget += pow(oXtarget.at<double>(l_coord, 0) - l_oXcurrent_hc.at<double>(l_coord, 0), 2);
+	}
+
+	l_fDistanceToTarget = sqrt(l_fDistanceToTarget);
+
+	return l_fDistanceToTarget;
+}
+
+void Kinematics::inverseKinematics(cv::Mat oXTarget)
+{
+	bool l_bLoopInProgress = true;
+	int l_i32NbIterations = 0;
+	
+	if (m_bIsDatabaseLoaded)
+	{
+		findClosestCandidateInDatabase(oXTarget);
+	}
+	else
+	{
+		m_vQiValues = m_vQiOriginalValues;
+	}
+
+	while (l_bLoopInProgress)
+	{
+		cv::Mat l_oVee = computeVee(oXTarget);
+		//std::cout << "inverseKinematics : l_oVee = " << l_oVee << std::endl;
+		cv::Mat l_oJNum = computeNumericalJacobian();
+		//std::cout << "inverseKinematics : l_oJNum = " << l_oJNum << std::endl;
+		cv::Mat l_oJPseudoInv = computePseudoInverseJacobian(l_oJNum);
+		//std::cout << "inverseKinematics : l_oJPseudoInv = " << l_oJPseudoInv << std::endl;
+		cv::Mat deltaQ = l_oJPseudoInv * l_oVee;
+		//std::cout << "inverseKinematics : deltaQ = " << deltaQ << std::endl;
+		
+		int l_i32JointIndex = 0;
+		for (int l_joint = 0; l_joint < m_vJointType.size(); l_joint++)
+		{
+			if (m_vJointType[l_joint] == KINEMATICS_REVOLUTE_JOINT || m_vJointType[l_joint] == KINEMATICS_PRISMATIC_JOINT)
+			{
+				m_vQiValues[l_i32JointIndex] += deltaQ.at<double>(l_i32JointIndex, 0) / m_f64PController;
+				//std::cout << "(Kinematics::inverseKinematics) (joint index, joint value) = ( " << l_i32JointIndex << ", " << m_vQiValues[l_i32JointIndex]   << ")" << std::endl;
+				l_i32JointIndex++;
+			}
+		}
+		
+		updateDHParameters();
+
+		double l_fCurrentDistance = computeDistanceToTarget(oXTarget);
+		std::cout << "---> #" << l_i32NbIterations << ", distance = " << l_fCurrentDistance << std::endl;
+		
+		l_i32NbIterations++;
+
+		if (l_fCurrentDistance < m_f64DistanceThreshold || l_i32NbIterations > m_i32MaxNbIterations)
+			l_bLoopInProgress = false;
+	}
+}
+
+cv::Mat Kinematics::computeCurrentEndEffectorPosition()
+{
+	// estimates the current position of the end-effector)
+	cv::Mat l_oTransformationMatrix_bTee = dh2FK(0);
+	cv::Mat l_oXorigin_hc(4, 1, CV_64F, 0.0f); l_oXorigin_hc.at<double>(3, 0) = 1.0f; // homogeneous coordinates Xorigin = [0 0 0 1]'
+	cv::Mat l_oXcurrent_hc = l_oTransformationMatrix_bTee * l_oXorigin_hc;
+
+	return l_oXcurrent_hc;
+}
+
+void Kinematics::createKinematicDatabase(int i32NbSamples, std::string sKinematicDatabaseFilename, int i32MinValueRevoluteJoint, int i32MaxValueRevoluteJoint, int i32MinValuePrismaticJoint, int i32MaxValuePrismaticJoint)
+{
+	// output matrices where the random qi values and the corresponding end-effector positions will be stored
+	cv::Mat l_oRandomJointValues(i32NbSamples, m_i32NbJoints, CV_64F, 0.0f);
+	cv::Mat l_oRandomEndEffectorPositions(i32NbSamples, 3, CV_64F, 0.0f);
+
+	// setups random number generators
+	std::random_device l_oRandomDevice; // seed with a real random value, if available
+	std::default_random_engine l_oRandomEngine(l_oRandomDevice());// create a random engine
+	std::uniform_int_distribution<int> l_oUniformDistributionRevoluteJoint(i32MinValueRevoluteJoint, i32MaxValueRevoluteJoint);
+	std::uniform_int_distribution<int> l_oUniformDistributionPrismaticJoint(i32MinValuePrismaticJoint, i32MaxValuePrismaticJoint);
+
+	// loops over the samples and generates random qi values and the corresponding end-effector positions
+	for (int l_sample = 0; l_sample < i32NbSamples; l_sample++)
+	{
+		// generates random qi values depending on the joint type i.e. revolute or prismatic
+		int l_i32JointIndex = 0;
+		for (int l_joint = 0; l_joint < m_vJointType.size(); l_joint++)
+		{
+			if (m_vJointType[l_joint] == KINEMATICS_REVOLUTE_JOINT)
+			{
+				int l_i32RandomRevoluteJointValue = l_oUniformDistributionRevoluteJoint(l_oRandomEngine);
+				l_oRandomJointValues.at<double>(l_sample, l_i32JointIndex) = (double)l_i32RandomRevoluteJointValue;
+				m_vQiValues[l_i32JointIndex] = (double)l_i32RandomRevoluteJointValue;
+				l_i32JointIndex++;
+
+			}
+			else if (m_vJointType[l_joint] == KINEMATICS_PRISMATIC_JOINT)
+			{
+				int l_i32RandomPrismaticJointValue = l_oUniformDistributionPrismaticJoint(l_oRandomEngine);
+				l_oRandomJointValues.at<double>(l_sample, l_i32JointIndex) = (double)l_i32RandomPrismaticJointValue / 100.0f;
+				m_vQiValues[l_i32JointIndex] = (double)l_i32RandomPrismaticJointValue / 100.0f;
+				l_i32JointIndex++;
+			}
+		}
+		updateDHParameters();
+
+		// computes the end-effector position
+		cv::Mat l_oCurrentEndEffectorPosition = computeCurrentEndEffectorPosition();
+
+		// stores this data to a cv::Mat
+		for (int l_coord= 0; l_coord < 3; l_coord++)
+			l_oRandomEndEffectorPositions.at<double>(l_sample, l_coord) = l_oCurrentEndEffectorPosition.at<double>(l_coord, 0);
+		
+	}
+	
+	// creates a file storage to store the data
+	cv::FileStorage l_fsKinematicDatabaseFile(sKinematicDatabaseFilename, cv::FileStorage::WRITE);
+
+	// checks if it was opened correctly
+	if (l_fsKinematicDatabaseFile.isOpened() == false)
+	{
+		std::cerr << "[ERROR] (Kinematics::createKinematicDatabase) Could not open the kinematic database file for writing!";
+		return;
+	}
+
+	// writes data in the file
+	l_fsKinematicDatabaseFile << "randomJointValues" << l_oRandomJointValues;
+	l_fsKinematicDatabaseFile << "randomEndEffectorPositions" << l_oRandomEndEffectorPositions;
+	
+	l_fsKinematicDatabaseFile.release();
+
+}
+
+void Kinematics::loadKinematicDatabase(std::string sKinematicDatabaseFilename)
+{
+	// checks if the filename is empty
+	if (sKinematicDatabaseFilename.empty())
+	{
+		std::cerr << "[ERROR] (Kinematics::loadKinematicDatabase) Kinematic database filename is empty!";
+		return;
+	}
+		
+	// loads the databse file
+	cv::FileStorage l_fsKinematicDatabaseFile(sKinematicDatabaseFilename, cv::FileStorage::READ);
+
+	// checks if file was successfully opened
+	if (l_fsKinematicDatabaseFile.isOpened() == false)
+	{
+		std::cerr << "[ERROR] (Kinematics::loadKinematicDatabase) Could not open the kinematic database file for reading!";
+		return;
+	}
+
+	// reads the different variables
+	cv::Mat l_oRandomJointValues;
+	l_fsKinematicDatabaseFile["randomJointValues"] >> l_oRandomJointValues;
+	if (l_oRandomJointValues.rows != 0 && l_oRandomJointValues.cols != 0)
+		m_oRandomJointValues = l_oRandomJointValues;
+
+	cv::Mat l_oRandomEndEffectorPositions;
+	l_fsKinematicDatabaseFile["randomEndEffectorPositions"] >> l_oRandomEndEffectorPositions;
+	if (l_oRandomEndEffectorPositions.rows != 0 && l_oRandomEndEffectorPositions.cols != 0)
+		m_oRandomEndEffectorPositions = l_oRandomEndEffectorPositions;
+
+	
+
+	m_bIsDatabaseLoaded = true;
+}
+
+void Kinematics::loadDHParameters(std::string sDHParametersFilename)
+{
+	// checks if the filename is empty
+	if (sDHParametersFilename.empty())
+	{
+		std::cerr << "[ERROR] (Kinematics::loadDHParameters) DH Parameters filename is empty!";
+		return;
+	}
+		
+	// loads the databse file
+	cv::FileStorage l_fsDHParametersFile(sDHParametersFilename, cv::FileStorage::READ);
+		
+	// checks if file was successfully opened
+	if (l_fsDHParametersFile.isOpened() == false)
+	{
+		std::cerr << "[ERROR] (Kinematics::loadDHParameters) Could not open the DH Parameters file for reading!";
+		return;
+	}
+
+	// reads the different variables
+	std::vector<double> l_vDHTheta;
+	l_fsDHParametersFile["dhThetaParams"] >> l_vDHTheta;
+	if (l_vDHTheta.size() > 0)
+		m_vDHTheta = l_vDHTheta;
+	
+	std::vector<double> l_vDHD;
+	l_fsDHParametersFile["dhDParams"] >> l_vDHD;
+	if (l_vDHD.size() > 0)
+		m_vDHD = l_vDHD;
+	
+	std::vector<double> l_vDHA;
+	l_fsDHParametersFile["dhAParams"] >> l_vDHA;
+	if (l_vDHA.size() > 0)
+		m_vDHA = l_vDHA;
+	
+	std::vector<double> l_vDHAlpha;
+	l_fsDHParametersFile["dhAlphaParams"] >> l_vDHAlpha;
+	if (l_vDHAlpha.size() > 0)
+		m_vDHAlpha = l_vDHAlpha;
+	
+	std::vector<int> l_vJointType;
+	l_fsDHParametersFile["jointType"] >> l_vJointType;
+	if (l_vJointType.size() > 0)
+		m_vJointType = l_vJointType;
+	
+	l_fsDHParametersFile["nbJoints"] >> m_i32NbJoints;
+
+	std::vector<double> l_vJointOffset;
+	l_fsDHParametersFile["jointOffset"] >> l_vJointOffset;
+	if (l_vJointOffset.size() > 0)
+		m_vJointOffset = l_vJointOffset;
+	
+	std::vector<double> l_vQiValues;
+	l_fsDHParametersFile["qiValues"] >> l_vQiValues;
+	if (l_vQiValues.size() > 0)
+	{
+		m_vQiValues = l_vQiValues;
+		m_vQiOriginalValues = l_vQiValues;
+	}
+		
+	updateDHParameters();
+	
+	m_bAreDHParametersSet = true;
+}
+
+
+void Kinematics::loadIKParameters(std::string sIKParametersFilename)
+{
+	// checks if the filename is empty
+	if (sIKParametersFilename.empty())
+	{
+		std::cerr << "[ERROR] (Kinematics::loadIKParameters) IK Parameters filename is empty!";
+		return;
+	}
+		
+	// loads the databse file
+	cv::FileStorage l_fsIKParametersFile(sIKParametersFilename, cv::FileStorage::READ);
+
+	// checks if file was successfully opened
+	if (l_fsIKParametersFile.isOpened() == false)
+	{
+		std::cerr << "[ERROR] (Kinematics::loadIKParameters) Could not open the IK Parameters file for reading!";
+		return;
+	}
+
+	// reads the different variables
+	l_fsIKParametersFile["maxNbIterations"] >> m_i32MaxNbIterations;
+	l_fsIKParametersFile["pController"] >> m_f64PController;
+	l_fsIKParametersFile["rankThreshold"] >> m_f64RankThreshold;
+	l_fsIKParametersFile["distanceThreshold"] >> m_f64DistanceThreshold;
+
+	m_bAreDHParametersSet = true;
+}
+
+
+void Kinematics::findClosestCandidateInDatabase(cv::Mat& oXtarget)
+{
+	double l_fClosestSampleDistanceToTarget = std::numeric_limits<double>::max();
+	int l_i32ClosestSampleIndex = 0;
+
+	//std::cout << "m_oRandomEndEffectorPositions.rows = " << m_oRandomEndEffectorPositions.rows << std::endl;
+	//std::cout << "m_oRandomEndEffectorPositions.cols = " << m_oRandomEndEffectorPositions.cols << std::endl;
+	// looks for the closest candidate in the database
+	for (int l_sample = 0; l_sample < m_oRandomEndEffectorPositions.rows; l_sample++)
+	{
+		double l_fCurrentSampleDistanceToTarget = 0.0f;
+		for (int l_coord = 0; l_coord < 3; l_coord++)
+			l_fCurrentSampleDistanceToTarget += pow( oXtarget.at<double>(l_coord, 0) - m_oRandomEndEffectorPositions.at<double>(l_sample, l_coord), 2);
+
+		if (l_fCurrentSampleDistanceToTarget < l_fClosestSampleDistanceToTarget)
+		{
+			l_fClosestSampleDistanceToTarget = l_fCurrentSampleDistanceToTarget;
+			l_i32ClosestSampleIndex = l_sample;
+		}
+	}
+	
+	std::cout << "l_fClosestSampleDistanceToTarget = " << l_fClosestSampleDistanceToTarget << std::endl;
+	std::cout << "l_i32ClosestSampleIndex = " << l_i32ClosestSampleIndex << std::endl;
+
+	// change the DH parameters to become the closest candidate in the database
+	int l_i32JointIndex = 0;
+	for (int l_joint = 0; l_joint < m_vJointType.size(); l_joint++)
+	{
+		if (m_vJointType[l_joint] == KINEMATICS_REVOLUTE_JOINT || m_vJointType[l_joint] == KINEMATICS_PRISMATIC_JOINT)
+		{
+			m_vQiValues[l_joint] = m_oRandomJointValues.at<double>(l_i32ClosestSampleIndex, l_i32JointIndex);
+			l_i32JointIndex++;
+		}
+	}
+	
+	updateDHParameters();
+}

src/RotationMatrix.cpp

@@ -0,0 +1,162 @@
+#include "RotationMatrix.h"
+
+
+RotationMatrix::RotationMatrix():
+m_bIsMatrixComputed(false), m_bIsMatrixInverseComputed(false),
+m_fRollAngle(0.0f), m_fPitchAngle(0.0f), m_fYawAngle(0.0f),
+m_i32OrderOfOperations(-1)
+{}
+
+RotationMatrix::RotationMatrix(double fRoll, double fPitch, double fYaw, int i32Order):
+m_bIsMatrixComputed(false), m_bIsMatrixInverseComputed(false),
+m_fRollAngle(fRoll), m_fPitchAngle(fPitch), m_fYawAngle(fYaw),
+m_i32OrderOfOperations(i32Order)
+{
+	compute();
+	inverse();
+}
+
+RotationMatrix::RotationMatrix(cv::Mat& oRotationMatrix) :
+	m_bIsMatrixComputed(true), m_bIsMatrixInverseComputed(false),
+	m_fRollAngle(0.0f), m_fPitchAngle(0.0f), m_fYawAngle(0.0f),
+	m_i32OrderOfOperations(-1),
+	m_oRotationMatrix(oRotationMatrix)
+{
+	inverse();
+}
+
+
+RotationMatrix::~RotationMatrix()
+{
+}
+
+void RotationMatrix::setRollPitchYawAngles(double fRoll, double fPitch, double fYaw)
+{
+	m_fRollAngle = fRoll;
+	m_fPitchAngle = fPitch;
+	m_fYawAngle = fYaw;
+	
+	m_bIsMatrixComputed = false;
+}
+
+void RotationMatrix::setOrderOfOperations(int i32Order)
+{
+	m_i32OrderOfOperations = i32Order;
+	
+	m_bIsMatrixComputed = false;
+}
+
+cv::Mat RotationMatrix::getRotationMat()
+{
+	if (!m_bIsMatrixComputed)
+		std::cerr << "(RotationMatrix::getRotationMatrix) Rotation matrix has not been computed yet!" << std::endl;
+
+	return m_oRotationMatrix;
+}
+
+cv::Mat RotationMatrix::getRotationMatInverse()
+{
+	if (!m_bIsMatrixInverseComputed)
+		std::cerr << "(RotationMatrix::getRotationMatrix) Rotation matrix inverse has not been computed yet!" << std::endl;
+
+	return m_oRotationMatrixInverse;
+}
+
+double RotationMatrix::deg2rad(double angleInDegrees)
+{
+	return (double)(angleInDegrees*M_PI/180.0f);
+}
+
+double RotationMatrix::rad2deg(double angleInRadians)
+{
+	return (double)(angleInRadians/M_PI*180.0f);
+}
+
+void RotationMatrix::compute()
+{
+	double thetaX, thetaY, thetaZ = 0.0f;
+	
+	// computes the theta angles depending on the order of operations
+	switch(m_i32OrderOfOperations)
+	{
+		case RxRyRz:
+			thetaX = deg2rad(m_fYawAngle);
+			thetaY = deg2rad(m_fPitchAngle);
+			thetaZ = deg2rad(m_fRollAngle);
+			break;
+		case RzRyRx:
+			thetaX = deg2rad(m_fRollAngle);
+			thetaY = deg2rad(m_fPitchAngle);
+			thetaZ = deg2rad(m_fYawAngle);
+			break;
+		default:
+			std::cerr << "(RotationMatrix::compute) The order of operation is unknown!" << std::endl;
+			return;
+	}
+	
+	// creates matrices to store individual roation matrices Rx, Ry and Rz before product
+	cv::Mat l_oRx(3, 3, CV_64F);
+	l_oRx.at<double>(0, 0) = 1; 	l_oRx.at<double>(0, 1) = 0; 			l_oRx.at<double>(0, 2) = 0;
+	l_oRx.at<double>(1, 0) = 0; 	l_oRx.at<double>(1, 1) = cos(thetaX);	l_oRx.at<double>(1, 2) = -sin(thetaX);
+	l_oRx.at<double>(2, 0) = 0; 	l_oRx.at<double>(2, 1) = sin(thetaX); 	l_oRx.at<double>(2, 2) = cos(thetaX);
+	
+	cv::Mat l_oRy(3, 3, CV_64F);
+	l_oRy.at<double>(0, 0) = cos(thetaY); 	l_oRy.at<double>(0, 1) = 0;		l_oRy.at<double>(0, 2) = sin(thetaY);
+	l_oRy.at<double>(1, 0) = 0; 			l_oRy.at<double>(1, 1) = 1; 		l_oRy.at<double>(1, 2) = 0;
+	l_oRy.at<double>(2, 0) = -sin(thetaY); 	l_oRy.at<double>(2, 1) = 0; 		l_oRy.at<double>(2, 2) = cos(thetaY);
+	
+	cv::Mat l_oRz(3, 3, CV_64F);
+	l_oRz.at<double>(0, 0) = cos(thetaZ); 	l_oRz.at<double>(0, 1) = -sin(thetaZ); 	l_oRz.at<double>(0, 2) = 0;
+	l_oRz.at<double>(1, 0) = sin(thetaZ);		l_oRz.at<double>(1, 1) = cos(thetaZ); 	l_oRz.at<double>(1, 2) = 0;
+	l_oRz.at<double>(2, 0) = 0; 			l_oRz.at<double>(2, 1) = 0; 			l_oRz.at<double>(2, 2) = 1;
+	
+	// computes the rotation matrix
+	switch(m_i32OrderOfOperations)
+	{
+		case RxRyRz:
+			m_oRotationMatrix = l_oRx * l_oRy * l_oRz;
+			break;
+		case RzRyRx:
+			m_oRotationMatrix = l_oRz * l_oRy * l_oRx;
+			break;
+		default:
+			std::cerr << "(RotationMatrix::compute) The order of operation is unknown!" << std::endl;
+			return;
+	}
+	
+	m_bIsMatrixComputed = true;
+
+}
+
+void RotationMatrix::inverse()
+{
+	if (m_bIsMatrixComputed)
+	{
+		m_oRotationMatrixInverse = m_oRotationMatrix.t();
+		m_bIsMatrixInverseComputed =true;
+	}
+	else
+		std::cerr << "(RotationMatrix::inverse) The rotation matrix has not been  computed yet!" << std::endl;
+}
+
+void RotationMatrix::disp()
+{
+	if (m_bIsMatrixComputed)
+	{
+		std::cout << "Rotation matrix =" << std::endl;
+		std::cout << m_oRotationMatrix << std::endl;
+	}
+	else
+		std::cerr << "(RotationMatrix::disp) The rotation matrix has not been  computed yet!" << std::endl;
+	
+	if (m_bIsMatrixInverseComputed)
+	{	
+		std::cout << "Rotation matrix Inverse =" << std::endl;
+		std::cout << m_oRotationMatrixInverse << std::endl;
+	}
+	else
+		std::cerr << "(RotationMatrix::disp) The rotation matrix inverse has not been  computed yet!" << std::endl;
+	
+}
+
+

src/TransformationMatrix.cpp

@@ -0,0 +1,232 @@
+#include "TransformationMatrix.h"
+
+
+TransformationMatrix::TransformationMatrix():
+m_bAreRotationComponentsSet(false), m_bAreTranslationComponentsSet(false),
+m_bIsMatrixComputed(false), m_bIsMatrixInverseComputed(false)
+{
+	m_pRotationMatrix = new RotationMatrix();
+	m_pTranslationMatrix = new TranslationMatrix();
+}
+
+TransformationMatrix::TransformationMatrix(double fRoll, double fPitch, double fYaw, int i32Order, double fTx, double fTy, double fTz):
+m_bAreRotationComponentsSet(true), m_bAreTranslationComponentsSet(true),
+m_bIsMatrixComputed(false), m_bIsMatrixInverseComputed(false)
+{
+	m_pRotationMatrix = new RotationMatrix(fRoll, fPitch, fYaw, i32Order);
+	m_pTranslationMatrix = new TranslationMatrix(fTx, fTy, fTz);
+	
+	compute();
+}
+
+TransformationMatrix::TransformationMatrix(cv::Mat& oRotationMatrix, cv::Mat& oTranslationMatrix) :
+	m_bAreRotationComponentsSet(false), m_bAreTranslationComponentsSet(false),
+	m_bIsMatrixComputed(true), m_bIsMatrixInverseComputed(false)
+{
+	// feeds the translation part
+	m_pTranslationMatrix = new TranslationMatrix(oTranslationMatrix);
+
+	// feeds the rotation part
+	m_pRotationMatrix = new RotationMatrix(oRotationMatrix);
+
+	// feeds the transformation matrix
+	cv::Mat l_oTransformationMatrix(4, 4, CV_64F, 0.0f);
+	oTranslationMatrix.copyTo(l_oTransformationMatrix(cv::Rect(3, 0, oTranslationMatrix.cols, oTranslationMatrix.rows)));
+	oRotationMatrix.copyTo(l_oTransformationMatrix(cv::Rect(0, 0, oRotationMatrix.cols, oRotationMatrix.rows)));
+	l_oTransformationMatrix.at<double>(3, 3) = 1.0f;
+	m_oTransformationMatrix = l_oTransformationMatrix;
+
+	inverse();
+}
+
+TransformationMatrix::TransformationMatrix(cv::Mat& oTransformationMatrix) :
+	m_bAreRotationComponentsSet(false), m_bAreTranslationComponentsSet(false),
+	m_bIsMatrixComputed(true), m_bIsMatrixInverseComputed(false),
+	m_oTransformationMatrix(oTransformationMatrix)
+{
+	// feeds the translation part
+	cv::Mat l_oTranslationMatrix(3, 1, CV_64F);
+	oTransformationMatrix(cv::Rect(3, 0, 1, 3)).copyTo(l_oTranslationMatrix);
+	m_pTranslationMatrix = new TranslationMatrix(l_oTranslationMatrix);
+	
+	// feeds the rotation part
+	cv::Mat l_oRotationMatrix(3, 3, CV_64F);
+	oTransformationMatrix(cv::Rect(0, 0, 3, 3)).copyTo(l_oRotationMatrix);
+	m_pRotationMatrix = new RotationMatrix(l_oRotationMatrix);
+		
+	inverse();
+}
+
+TransformationMatrix::~TransformationMatrix()
+{
+	// lambda function to delete and nullify any pointer
+	auto deleteAndNullify = [](auto pointer) -> void
+	{
+		if (nullptr != pointer)
+		{
+			delete pointer;
+			pointer = nullptr;
+		}
+	};
+	
+	// release allocated memory
+	deleteAndNullify(m_pRotationMatrix);
+	deleteAndNullify(m_pTranslationMatrix);
+}
+
+void TransformationMatrix::setRotationComponents(double fRoll, double fPitch, double fYaw, int i32Order)
+{
+	if (nullptr != m_pRotationMatrix)
+	{
+		delete m_pRotationMatrix;
+		m_pRotationMatrix = nullptr;
+	}
+	
+	m_pRotationMatrix = new RotationMatrix(fRoll, fPitch, fYaw, i32Order);
+	
+	m_bAreRotationComponentsSet = true;
+	m_bIsMatrixComputed = false;
+	m_bIsMatrixInverseComputed = false;
+}
+
+
+void TransformationMatrix::setTranslationComponents(double fTx, double fTy, double fTz)
+{
+	if (nullptr != m_pTranslationMatrix)
+	{
+		delete m_pTranslationMatrix;
+		m_pTranslationMatrix = nullptr;
+	}
+	
+	m_pTranslationMatrix = new TranslationMatrix(fTx, fTy, fTz);
+	
+	m_bAreTranslationComponentsSet = true;
+	m_bIsMatrixComputed = false;
+	m_bIsMatrixInverseComputed = false;
+}
+
+
+cv::Mat TransformationMatrix::getTransformationMat()
+{
+	if (!m_bIsMatrixComputed)
+		std::cerr << "(TransformationMatrix::getTransformationMatrix) Transformation matrix has not been computed yet!" << std::endl;
+
+	return m_oTransformationMatrix;
+}
+
+cv::Mat TransformationMatrix::getTransformationMatInverse()
+{
+	if (!m_bIsMatrixInverseComputed)
+		std::cerr << "(TransformationMatrix::getTransformationMatrixInverse) Transformation matrix inverse has not been computed yet!" << std::endl;
+
+	return m_oTransformationMatrixInverse;
+}
+
+RotationMatrix* TransformationMatrix::getRotationMatrix()
+{
+	if (!m_bIsMatrixComputed)
+		std::cerr << "(TransformationMatrix::getRotationMatrix) Transformation matrix has not been computed yet!" << std::endl;
+
+	return m_pRotationMatrix;
+}
+
+TranslationMatrix* TransformationMatrix::getTranslationMatrix()
+{
+	if (!m_bIsMatrixComputed)
+		std::cerr << "(TransformationMatrix::getTranslationMatrix) Transformation matrix has not been computed yet!" << std::endl;
+
+	return m_pTranslationMatrix;
+}
+
+void TransformationMatrix::compute()
+{
+	if (m_bAreRotationComponentsSet && m_bAreTranslationComponentsSet)
+	{
+		cv::Mat l_oTransformationMatrix(4, 4, CV_64F); // homogeneous coordinates
+		/*	(R (3x3)	|	t (3x1) 	)
+		*	-----------------------------------
+		*	(   0	(1x3)	|	1 (1x1)	)
+		*/
+		
+		// retrieves the rotation components and copies them into the transformation matrix
+		cv::Mat l_oRotationMatrix = m_pRotationMatrix->getRotationMat();
+		l_oRotationMatrix.copyTo(l_oTransformationMatrix(cv::Rect(0, 0, l_oRotationMatrix.cols, l_oRotationMatrix.rows)));
+		
+		// retrieves the translation components and copies them into the transformation matrix
+		cv::Mat l_oTranslationMatrix = m_pTranslationMatrix->getTranslationMat();
+		l_oTranslationMatrix.copyTo(l_oTransformationMatrix(cv::Rect(3, 0, l_oTranslationMatrix.cols, l_oTranslationMatrix.rows)));
+		
+		// adds the homogeneous coordinate part
+		l_oTransformationMatrix.at<double>(3, 0) = 0.0f;
+		l_oTransformationMatrix.at<double>(3, 1) = 0.0f;
+		l_oTransformationMatrix.at<double>(3, 2) = 0.0f;
+		l_oTransformationMatrix.at<double>(3, 3) = 1.0f;
+		
+		// stores it in the member variable
+		m_oTransformationMatrix = l_oTransformationMatrix;
+		
+		m_bIsMatrixComputed = true;
+	}
+	else
+	{
+		std::cerr << "(TransformationMatrix::compute) Rotation and/or translation components are not set!" << std::endl;
+	}
+
+}
+
+void TransformationMatrix::inverse()
+{
+	if (m_bIsMatrixComputed)
+	{
+		cv::Mat l_oTransformationMatrixInverse(4, 4, CV_64F); // homogeneous coordinates
+		/*	(R^-1 (3x3)	|	-R^-1t (3x1) )
+		*	-----------------------------------
+		*	(   0	(1x3)		|	1 (1x1)	)
+		*/
+		
+		// retrieves the rotation components and copies them into the transformation matrix inverse
+		cv::Mat l_oRotationMatrixInverse = m_pRotationMatrix->getRotationMatInverse();
+		l_oRotationMatrixInverse.copyTo(l_oTransformationMatrixInverse(cv::Rect(0, 0, l_oRotationMatrixInverse.cols, l_oRotationMatrixInverse.rows)));
+		
+		// retrieves the translation components 
+		cv::Mat l_oTranslationMatrix = m_pTranslationMatrix->getTranslationMat();
+		// computes the inverse translation components 
+		cv::Mat l_oTranslationMatrixInverse = -l_oRotationMatrixInverse * l_oTranslationMatrix;
+		// copies them into the transformation matrix inverse
+		l_oTranslationMatrixInverse.copyTo(l_oTransformationMatrixInverse(cv::Rect(3, 0, l_oTranslationMatrixInverse.cols, l_oTranslationMatrixInverse.rows)));
+		
+		// adds the homogeneous coordinate part
+		l_oTransformationMatrixInverse.at<double>(3, 0) = 0.0f;
+		l_oTransformationMatrixInverse.at<double>(3, 1) = 0.0f;
+		l_oTransformationMatrixInverse.at<double>(3, 2) = 0.0f;
+		l_oTransformationMatrixInverse.at<double>(3, 3) = 1.0f;
+		
+		// stores it in the member variable
+		m_oTransformationMatrixInverse = l_oTransformationMatrixInverse;
+		m_bIsMatrixInverseComputed =true;
+	}
+	else
+		std::cerr << "(TransformationMatrix::inverse) The rotation matrix has not been  computed yet!" << std::endl;
+}
+
+void TransformationMatrix::disp()
+{
+	if (m_bIsMatrixComputed)
+	{
+		std::cout << "Transformation matrix =" << std::endl;
+		std::cout << m_oTransformationMatrix << std::endl;
+	}
+	else
+		std::cerr << "(TransformationMatrix::disp) The rotation matrix has not been  computed yet!" << std::endl;
+	
+	if (m_bIsMatrixInverseComputed)
+	{	
+		std::cout << "Transformation matrix Inverse =" << std::endl;
+		std::cout << m_oTransformationMatrixInverse << std::endl;
+	}
+	else
+		std::cerr << "(TransformationMatrix::disp) The rotation matrix inverse has not been  computed yet!" << std::endl;
+	
+}
+
+

src/TranslationMatrix.cpp

@@ -0,0 +1,70 @@
+#include "TranslationMatrix.h"
+
+
+TranslationMatrix::TranslationMatrix():
+m_bIsMatrixComputed(false), 
+m_fTx(0.0f), m_fTy(0.0f), m_fTz(0.0f)
+{}
+
+TranslationMatrix::TranslationMatrix(double fTx, double fTy, double fTz):
+m_bIsMatrixComputed(false), 
+m_fTx(fTx), m_fTy(fTy), m_fTz(fTz)
+{
+	compute();
+}
+
+TranslationMatrix::TranslationMatrix(cv::Mat& oTranslationMatrix)
+{
+	m_oTranslationMatrix = oTranslationMatrix;
+
+	m_fTx = oTranslationMatrix.at<double>(0, 0);
+	m_fTy = oTranslationMatrix.at<double>(1, 0);
+	m_fTz = oTranslationMatrix.at<double>(2, 0);
+
+	m_bIsMatrixComputed = true;
+}
+
+TranslationMatrix::~TranslationMatrix()
+{
+}
+
+void TranslationMatrix::setTranslationComponents(double fTx, double fTy, double fTz)
+{
+	m_fTx = fTx;
+	m_fTy = fTy;
+	m_fTz = fTz;
+	
+	m_bIsMatrixComputed = false;
+}
+
+cv::Mat TranslationMatrix::getTranslationMat()
+{
+	return m_oTranslationMatrix;
+}
+
+void TranslationMatrix::compute()
+{
+	// creates matrices to store individual roation matrices Rx, Ry and Rz before product
+	cv::Mat l_oTranslationMatrix(3, 1, CV_64F);
+	l_oTranslationMatrix.at<double>(0, 0) = m_fTx; 	
+	l_oTranslationMatrix.at<double>(1, 0) = m_fTy; 	
+	l_oTranslationMatrix.at<double>(2, 0) = m_fTz; 	
+	
+	m_oTranslationMatrix = l_oTranslationMatrix;
+	
+	m_bIsMatrixComputed = true;
+
+}
+
+void TranslationMatrix::disp()
+{
+	if (m_bIsMatrixComputed)
+	{
+		std::cout << "Translation matrix =" << std::endl;
+		std::cout << m_oTranslationMatrix << std::endl;
+	}
+	else
+		std::cerr << "(TranslationMatrix::disp) The translation matrix has not been  computed yet!" << std::endl;
+}
+
+

src/ik_client.cpp

@@ -0,0 +1,66 @@
+#include "ros/ros.h"
+#include "poppy_ros/IK.h"
+#include <cstdlib>
+#include <sensor_msgs/JointState.h>
+#include <ros/package.h>
+
+double deg2rad(double angle)
+
+{
+
+
+              return -angle / 180.0 * M_PI;
+
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "ik_client");
+  
+  //if (argc != 3)
+  //{
+  //  	ROS_INFO("usage: ik_client X Y");
+  //  	return 1;
+  //}
+
+  ros::NodeHandle nh;
+  ros::Publisher jointCmdPublisher = nh.advertise<sensor_msgs::JointState>("joint_states", 1);
+  ros::ServiceClient client = nh.serviceClient<poppy_ros::IK>("IK");
+  poppy_ros::IK srv;
+  
+  srv.request.x = atoll(argv[1]);
+  srv.request.y = atoll(argv[2]);
+  srv.request.z = atoll(argv[3]);
+  ros::Rate loopRate(10);
+  
+  
+  
+  if (client.call(srv))
+  {
+   	std::vector<double> jointCmdValueArray = {deg2rad(srv.response.m1), 			  		  		deg2rad(srv.response.m2), 		 	   				     			 		deg2rad(srv.response.m3),                                        			   		 		deg2rad(srv.response.m4),
+	deg2rad(srv.response.m5), 		 		          			  		 		deg2rad(srv.response.m6)};
+  	std::vector<std::string> jointCmdNameArray = {"m1", "m2", "m3", "m4", "m5", "m6"};
+  
+  
+  	while (ros::ok())
+      	{
+
+      		sensor_msgs::JointState jointCmdMsg;
+           	jointCmdMsg.header.stamp = ros::Time::now();
+               jointCmdMsg.header.seq++;
+               jointCmdMsg.position = jointCmdValueArray;
+               jointCmdMsg.name = jointCmdNameArray;
+		jointCmdPublisher.publish(jointCmdMsg);
+		ros::spinOnce();
+               loopRate.sleep();
+     	}                                         
+  }
+  
+  else
+  {
+    ROS_ERROR("Failed to call service add_two_ints");
+    return 1;
+  }
+
+  return 0;
+}

src/ik_server.cpp

@@ -0,0 +1,44 @@
+#include "ros/ros.h"
+#include "poppy_ros/IK.h"
+#include "Kinematics.h"
+Kinematics kin;
+
+bool add(poppy_ros::IK::Request  &req,
+         poppy_ros::IK::Response &res)
+{
+
+
+	cv::Mat Coord = cv::Mat(3,1,CV_64F,0.0f);
+	Coord.at<double>(0) = req.x;
+	Coord.at<double>(1) = req.y;
+	Coord.at<double>(2) = req.z;
+	kin.inverseKinematics(Coord);
+	std::vector<double> QI;
+	 QI = kin.getQiValues();
+  	res.m1 = QI[0];
+  	res.m2 = QI[1];
+  	res.m3 = QI[2];
+  	res.m4 = QI[3];
+ 	res.m5 = QI[4];
+  	res.m6 = QI[5];
+   
+  	ROS_INFO("request: m1=%lf, m2=%lf m3=%lf m4=%lf m5=%lf m6=%lf", (double)res.m1, (double)res.m2, (double)res.m3, (double)res.m4, (double)res.m5, (double)res.m6);
+ 	// ROS_INFO("sending back response: [%ld]", (long int)res.sum);
+ 	 
+  return true;
+}
+
+
+
+int main(int argc, char **argv)
+{
+  	ros::init(argc, argv, "ik_server");
+  	ros::NodeHandle nh;
+	kin.loadDHParameters("/home/tim/catkin_ws/src/poppy_ros/poppy_dh.yaml");
+	kin.loadIKParameters("/home/tim/catkin_ws/src/poppy_ros/poppy_ik.yaml");
+  	ros::ServiceServer service = nh.advertiseService("IK", add);
+  	ROS_INFO("Ready to move robot");
+  	ros::spin();
+
+  return 0;
+}

src/poppy_ros.cpp

@@ -0,0 +1,74 @@
+#include <ros/ros.h>
+#include <DynamixelHandler.h>
+#include <geometry_msgs/Twist.h>
+
+DynamixelHandler dxHandler;
+geometry_msgs::Twist Tw;
+
+
+void joint_cmd (const geometry_msgs::Twist& msg)
+{
+
+std::vector<uint16_t> vCurrentPosition;
+  vCurrentPosition.push_back(msg.linear.x);
+  vCurrentPosition.push_back(msg.linear.y);
+  vCurrentPosition.push_back(msg.linear.z);
+  vCurrentPosition.push_back(msg.angular.x);
+  vCurrentPosition.push_back(msg.angular.y);
+  vCurrentPosition.push_back(msg.angular.z);
+  
+  for (int i =0; i<6; i++)
+  {
+  vCurrentPosition[i] = vCurrentPosition[i]/0.3516;
+  }
+  
+  dxHandler.sendTargetJointPosition(vCurrentPosition);
+}
+
+
+int main(int argc, char **argv)
+{
+
+ros::init(argc, argv, "poppy_ros");	
+ros::NodeHandle nh;
+
+dxHandler.setDeviceName("/dev/ttyUSB0");
+dxHandler.setProtocolVersion(2.0);
+dxHandler.openPort();
+dxHandler.setBaudRate(1000000);
+dxHandler.enableTorque(true);
+
+
+ ros::Subscriber getjointvalue= nh.subscribe("joint_cmd",10, joint_cmd);
+ ros::Publisher jointCmdPublisher = nh.advertise<geometry_msgs::Twist>("joint_position", 1);	
+    
+             ros::Rate loopRate(10);
+        while (ros::ok())
+    	{
+		std::vector<uint16_t> vCurrentPosition;
+	       dxHandler.readCurrentJointPosition(vCurrentPosition);
+	       
+	       if (vCurrentPosition.size() >= 6)
+	       {
+		       Tw.linear.x=vCurrentPosition[0];
+		       Tw.linear.y=vCurrentPosition[1];
+		       Tw.linear.z=vCurrentPosition[2];
+		       Tw.angular.x=vCurrentPosition[3];
+		       Tw.angular.y=vCurrentPosition[4];
+		       Tw.angular.z=vCurrentPosition[5];
+			jointCmdPublisher.publish(Tw);
+		}
+		 
+		 ros::spinOnce();
+		 loopRate.sleep();
+    	}
+   
+   
+   dxHandler.closePort();
+    return 0;
+    
+    
+   }
+  
+    
+    

srv/IK.srv

@@ -0,0 +1,10 @@
+float64 x
+float64 y
+float64 z
+---
+float64 m1
+float64 m2
+float64 m3
+float64 m4
+float64 m5
+float64 m6