diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 000000000..891fd14c0 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "ur_kinematics/pybind11"] + path = ur_kinematics/pybind11 + url = https://github.com/pybind/pybind11.git diff --git a/ur10_e_moveit_config/config/kinematics.yaml b/ur10_e_moveit_config/config/kinematics.yaml index 5d492ac1f..cc576af8b 100644 --- a/ur10_e_moveit_config/config/kinematics.yaml +++ b/ur10_e_moveit_config/config/kinematics.yaml @@ -1,4 +1,9 @@ -manipulator: +# manipulator: +# kinematics_solver: ur_kinematics/UR10EKinematicsPlugin +# kinematics_solver_search_resolution: 0.005 +# kinematics_solver_timeout: 0.005 +# kinematics_solver_attempts: 3 + manipulator: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 diff --git a/ur10_moveit_config/config/kinematics.yaml b/ur10_moveit_config/config/kinematics.yaml index e37d336c3..7f6d225e4 100644 --- a/ur10_moveit_config/config/kinematics.yaml +++ b/ur10_moveit_config/config/kinematics.yaml @@ -1,4 +1,4 @@ -#manipulator: +# manipulator: # kinematics_solver: ur_kinematics/UR10KinematicsPlugin # kinematics_solver_search_resolution: 0.005 # kinematics_solver_timeout: 0.005 diff --git a/ur3_e_moveit_config/config/kinematics.yaml b/ur3_e_moveit_config/config/kinematics.yaml index 5d492ac1f..3e607e29a 100644 --- a/ur3_e_moveit_config/config/kinematics.yaml +++ b/ur3_e_moveit_config/config/kinematics.yaml @@ -1,4 +1,9 @@ -manipulator: +# manipulator: +# kinematics_solver: ur_kinematics/UR3EKinematicsPlugin +# kinematics_solver_search_resolution: 0.005 +# kinematics_solver_timeout: 0.005 +# kinematics_solver_attempts: 3 + manipulator: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 diff --git a/ur3_moveit_config/config/kinematics.yaml b/ur3_moveit_config/config/kinematics.yaml index 7162d768c..3370e4d42 100644 --- a/ur3_moveit_config/config/kinematics.yaml +++ b/ur3_moveit_config/config/kinematics.yaml @@ -1,4 +1,4 @@ -#manipulator: +# manipulator: # kinematics_solver: ur_kinematics/UR3KinematicsPlugin # kinematics_solver_search_resolution: 0.005 # kinematics_solver_timeout: 0.005 diff --git a/ur5_e_moveit_config/config/kinematics.yaml b/ur5_e_moveit_config/config/kinematics.yaml index 5d492ac1f..34fc5221b 100644 --- a/ur5_e_moveit_config/config/kinematics.yaml +++ b/ur5_e_moveit_config/config/kinematics.yaml @@ -1,5 +1,10 @@ -manipulator: +# manipulator: +# kinematics_solver: ur_kinematics/UR5EKinematicsPlugin +# kinematics_solver_search_resolution: 0.005 +# kinematics_solver_timeout: 0.005 +# kinematics_solver_attempts: 3 + manipulator: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 + kinematics_solver_attempts: 3 \ No newline at end of file diff --git a/ur5_moveit_config/config/kinematics.yaml b/ur5_moveit_config/config/kinematics.yaml index e2fa9b157..803fa1316 100644 --- a/ur5_moveit_config/config/kinematics.yaml +++ b/ur5_moveit_config/config/kinematics.yaml @@ -1,4 +1,4 @@ -#manipulator: +# manipulator: # kinematics_solver: ur_kinematics/UR5KinematicsPlugin # kinematics_solver_search_resolution: 0.005 # kinematics_solver_timeout: 0.005 diff --git a/ur_kinematics/CMakeLists.txt b/ur_kinematics/CMakeLists.txt index 5d0eede00..c95eb7eb1 100644 --- a/ur_kinematics/CMakeLists.txt +++ b/ur_kinematics/CMakeLists.txt @@ -1,23 +1,31 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.1) project(ur_kinematics) -add_compile_options(-std=c++11) +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +option(BUILD_BENCHMARK "Build the ur_kinematics benchmarks (requires google benchmark)" OFF) +option(BUILD_TESTS "Build tests (requires GTest)" OFF) find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs moveit_core moveit_kinematics moveit_ros_planning pluginlib tf_conversions) find_package(Boost REQUIRED COMPONENTS system) +find_package(PythonLibs REQUIRED) catkin_python_setup() catkin_package( INCLUDE_DIRS include - LIBRARIES ur3_kin ur5_kin ur10_kin ur3_moveit_plugin ur5_moveit_plugin ur10_moveit_plugin + LIBRARIES ur3_kin ur3_moveit_plugin ur5_moveit_plugin ur10_moveit_plugin ur3_e_moveit_plugin ur5_e_moveit_plugin ur10_e_moveit_plugin CATKIN_DEPENDS roscpp geometry_msgs moveit_core moveit_kinematics moveit_ros_planning pluginlib tf_conversions DEPENDS Boost ) +include_directories(SYSTEM ${PYTHON_NUMPY_INCLUDE_DIR}) +include_directories(SYSTEM ${PYTHON_INCLUDE_DIRS}) +add_subdirectory(pybind11) ########### ## Build ## @@ -26,43 +34,63 @@ catkin_package( include_directories(SYSTEM ${Boost_INCLUDE_DIR}) include_directories(include ${catkin_INCLUDE_DIRS}) -add_library(ur3_kin src/ur_kin.cpp) -set_target_properties(ur3_kin PROPERTIES COMPILE_DEFINITIONS "UR3_PARAMS") +define_property( GLOBAL PROPERTY UR_TARGETS_PROP + BRIEF_DOCS "Targets list" + FULL_DOCS "Targets list") + +function(ADD_ROBOT ROBOT) + string(TOUPPER ${ROBOT} UPPER_ROBOT) + string(TOLOWER ${ROBOT} LOWER_ROBOT) + + set(ROBOT_PARAMS "${UPPER_ROBOT}_PARAMS") + set(TARGET_NAME "${LOWER_ROBOT}_moveit_plugin") -add_library(ur5_kin src/ur_kin.cpp) -set_target_properties(ur5_kin PROPERTIES COMPILE_DEFINITIONS "UR5_PARAMS") + add_library(${TARGET_NAME} src/ur_moveit_plugin.cpp) + set_target_properties(${TARGET_NAME} PROPERTIES COMPILE_DEFINITIONS ${ROBOT_PARAMS}) + target_link_libraries(${TARGET_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES}) -add_library(ur10_kin src/ur_kin.cpp) -set_target_properties(ur10_kin PROPERTIES COMPILE_DEFINITIONS "UR10_PARAMS") + set_property(GLOBAL APPEND PROPERTY UR_TARGETS_PROP ${TARGET_NAME}) +endfunction() -add_library(ur3_moveit_plugin src/ur_moveit_plugin.cpp) -set_target_properties(ur3_moveit_plugin PROPERTIES COMPILE_DEFINITIONS "UR3_PARAMS") -target_link_libraries(ur3_moveit_plugin - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ur3_kin) -add_library(ur5_moveit_plugin src/ur_moveit_plugin.cpp) -set_target_properties(ur5_moveit_plugin PROPERTIES COMPILE_DEFINITIONS "UR5_PARAMS") -target_link_libraries(ur5_moveit_plugin - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ur5_kin) +ADD_ROBOT("ur3") +ADD_ROBOT("ur5") +ADD_ROBOT("ur10") -add_library(ur10_moveit_plugin src/ur_moveit_plugin.cpp) -set_target_properties(ur10_moveit_plugin PROPERTIES COMPILE_DEFINITIONS "UR10_PARAMS") -target_link_libraries(ur10_moveit_plugin - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ur10_kin) +ADD_ROBOT("ur3_e") +ADD_ROBOT("ur5_e") +ADD_ROBOT("ur10_e") +pybind11_add_module(ur_kin_py src/ur_kin_py.cpp) +target_link_libraries(ur_kin_py PRIVATE ${catkin_LIBRARIES}) +set_target_properties(ur_kin_py PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}) + +if(BUILD_BENCHMARK) + find_package(benchmark REQUIRED) + add_executable(benchmark_kinematics src/benchmark_kinematics.cpp) + target_link_libraries(benchmark_kinematics + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + benchmark) + set_property(GLOBAL APPEND PROPERTY UR_TARGETS_PROP benchmark_kinematics) +endif() + +if(BUILD_TESTS) +find_package(GTest REQUIRED) + add_executable(ur_kinematics_tests src/test_kinematics.cpp ) + # Link test executable against gtest & gtest_main + target_link_libraries(ur_kinematics_tests GTest::Main GTest::GTest) + gtest_discover_tests(ur_kinematics_tests) +endif() ############# ## Install ## ############# - -install(TARGETS ur3_kin ur5_kin ur10_kin ur3_moveit_plugin ur5_moveit_plugin ur10_moveit_plugin +get_property(UR_TARGETS GLOBAL PROPERTY UR_TARGETS_PROP) +install(TARGETS ${$UR_TARGETS} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/ur_kinematics/include/ur_kinematics/ikfast.h b/ur_kinematics/include/ur_kinematics/ikfast.h deleted file mode 100644 index 9a2a2f17f..000000000 --- a/ur_kinematics/include/ur_kinematics/ikfast.h +++ /dev/null @@ -1,328 +0,0 @@ -// -*- coding: utf-8 -*- -// Copyright (C) 2012 Rosen Diankov -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -/** \brief Header file for all ikfast c++ files/shared objects. - - The ikfast inverse kinematics compiler is part of OpenRAVE. - - The file is divided into two sections: - - Common - the abstract classes section that all ikfast share regardless of their settings - - Library Specific - the library-specific definitions, which depends on the precision/settings that the library was compiled with - - The defines are as follows, they are also used for the ikfast C++ class: - - - IKFAST_HEADER_COMMON - common classes - - IKFAST_HAS_LIBRARY - if defined, will include library-specific functions. by default this is off - - IKFAST_CLIBRARY - Define this linking statically or dynamically to get correct visibility. - - IKFAST_NO_MAIN - Remove the ``main`` function, usually used with IKFAST_CLIBRARY - - IKFAST_ASSERT - Define in order to get a custom assert called when NaNs, divides by zero, and other invalid conditions are detected. - - IKFAST_REAL - Use to force a custom real number type for IkReal. - - IKFAST_NAMESPACE - Enclose all functions and classes in this namespace, the ``main`` function is excluded. - - */ -#include -#include -#include - -#ifndef IKFAST_HEADER_COMMON -#define IKFAST_HEADER_COMMON - -/// should be the same as ikfast.__version__ -#define IKFAST_VERSION 61 - -namespace ikfast { - -/// \brief holds the solution for a single dof -template -class IkSingleDOFSolutionBase -{ -public: - IkSingleDOFSolutionBase() : fmul(0), foffset(0), freeind(-1), maxsolutions(1) { - indices[0] = indices[1] = indices[2] = indices[3] = indices[4] = -1; - } - T fmul, foffset; ///< joint value is fmul*sol[freeind]+foffset - signed char freeind; ///< if >= 0, mimics another joint - unsigned char jointtype; ///< joint type, 0x01 is revolute, 0x11 is slider - unsigned char maxsolutions; ///< max possible indices, 0 if controlled by free index or a free joint itself - unsigned char indices[5]; ///< unique index of the solution used to keep track on what part it came from. sometimes a solution can be repeated for different indices. store at least another repeated root -}; - -/// \brief The discrete solutions are returned in this structure. -/// -/// Sometimes the joint axes of the robot can align allowing an infinite number of solutions. -/// Stores all these solutions in the form of free variables that the user has to set when querying the solution. Its prototype is: -template -class IkSolutionBase -{ -public: - virtual ~IkSolutionBase() { - } - /// \brief gets a concrete solution - /// - /// \param[out] solution the result - /// \param[in] freevalues values for the free parameters \se GetFree - virtual void GetSolution(T* solution, const T* freevalues) const = 0; - - /// \brief std::vector version of \ref GetSolution - virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const { - solution.resize(GetDOF()); - GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); - } - - /// \brief Gets the indices of the configuration space that have to be preset before a full solution can be returned - /// - /// \return vector of indices indicating the free parameters - virtual const std::vector& GetFree() const = 0; - - /// \brief the dof of the solution - virtual const int GetDOF() const = 0; -}; - -/// \brief manages all the solutions -template -class IkSolutionListBase -{ -public: - virtual ~IkSolutionListBase() { - } - - /// \brief add one solution and return its index for later retrieval - /// - /// \param vinfos Solution data for each degree of freedom of the manipulator - /// \param vfree If the solution represents an infinite space, holds free parameters of the solution that users can freely set. - virtual size_t AddSolution(const std::vector >& vinfos, const std::vector& vfree) = 0; - - /// \brief returns the solution pointer - virtual const IkSolutionBase& GetSolution(size_t index) const = 0; - - /// \brief returns the number of solutions stored - virtual size_t GetNumSolutions() const = 0; - - /// \brief clears all current solutions, note that any memory addresses returned from \ref GetSolution will be invalidated. - virtual void Clear() = 0; -}; - -/// \brief holds function pointers for all the exported functions of ikfast -template -class IkFastFunctions -{ -public: - IkFastFunctions() : _ComputeIk(NULL), _ComputeFk(NULL), _GetNumFreeParameters(NULL), _GetFreeParameters(NULL), _GetNumJoints(NULL), _GetIkRealSize(NULL), _GetIkFastVersion(NULL), _GetIkType(NULL), _GetKinematicsHash(NULL) { - } - virtual ~IkFastFunctions() { - } - typedef bool (*ComputeIkFn)(const T*, const T*, const T*, IkSolutionListBase&); - ComputeIkFn _ComputeIk; - typedef void (*ComputeFkFn)(const T*, T*, T*); - ComputeFkFn _ComputeFk; - typedef int (*GetNumFreeParametersFn)(); - GetNumFreeParametersFn _GetNumFreeParameters; - typedef int* (*GetFreeParametersFn)(); - GetFreeParametersFn _GetFreeParameters; - typedef int (*GetNumJointsFn)(); - GetNumJointsFn _GetNumJoints; - typedef int (*GetIkRealSizeFn)(); - GetIkRealSizeFn _GetIkRealSize; - typedef const char* (*GetIkFastVersionFn)(); - GetIkFastVersionFn _GetIkFastVersion; - typedef int (*GetIkTypeFn)(); - GetIkTypeFn _GetIkType; - typedef const char* (*GetKinematicsHashFn)(); - GetKinematicsHashFn _GetKinematicsHash; -}; - -// Implementations of the abstract classes, user doesn't need to use them - -/// \brief Default implementation of \ref IkSolutionBase -template -class IkSolution : public IkSolutionBase -{ -public: - IkSolution(const std::vector >& vinfos, const std::vector& vfree) { - _vbasesol = vinfos; - _vfree = vfree; - } - - virtual void GetSolution(T* solution, const T* freevalues) const { - for(std::size_t i = 0; i < _vbasesol.size(); ++i) { - if( _vbasesol[i].freeind < 0 ) - solution[i] = _vbasesol[i].foffset; - else { - solution[i] = freevalues[_vbasesol[i].freeind]*_vbasesol[i].fmul + _vbasesol[i].foffset; - if( solution[i] > T(3.14159265358979) ) { - solution[i] -= T(6.28318530717959); - } - else if( solution[i] < T(-3.14159265358979) ) { - solution[i] += T(6.28318530717959); - } - } - } - } - - virtual void GetSolution(std::vector& solution, const std::vector& freevalues) const { - solution.resize(GetDOF()); - GetSolution(&solution.at(0), freevalues.size() > 0 ? &freevalues.at(0) : NULL); - } - - virtual const std::vector& GetFree() const { - return _vfree; - } - virtual const int GetDOF() const { - return static_cast(_vbasesol.size()); - } - - virtual void Validate() const { - for(size_t i = 0; i < _vbasesol.size(); ++i) { - if( _vbasesol[i].maxsolutions == (unsigned char)-1) { - throw std::runtime_error("max solutions for joint not initialized"); - } - if( _vbasesol[i].maxsolutions > 0 ) { - if( _vbasesol[i].indices[0] >= _vbasesol[i].maxsolutions ) { - throw std::runtime_error("index >= max solutions for joint"); - } - if( _vbasesol[i].indices[1] != (unsigned char)-1 && _vbasesol[i].indices[1] >= _vbasesol[i].maxsolutions ) { - throw std::runtime_error("2nd index >= max solutions for joint"); - } - } - } - } - - virtual void GetSolutionIndices(std::vector& v) const { - v.resize(0); - v.push_back(0); - for(int i = (int)_vbasesol.size()-1; i >= 0; --i) { - if( _vbasesol[i].maxsolutions != (unsigned char)-1 && _vbasesol[i].maxsolutions > 1 ) { - for(size_t j = 0; j < v.size(); ++j) { - v[j] *= _vbasesol[i].maxsolutions; - } - size_t orgsize=v.size(); - if( _vbasesol[i].indices[1] != (unsigned char)-1 ) { - for(size_t j = 0; j < orgsize; ++j) { - v.push_back(v[j]+_vbasesol[i].indices[1]); - } - } - if( _vbasesol[i].indices[0] != (unsigned char)-1 ) { - for(size_t j = 0; j < orgsize; ++j) { - v[j] += _vbasesol[i].indices[0]; - } - } - } - } - } - - std::vector< IkSingleDOFSolutionBase > _vbasesol; ///< solution and their offsets if joints are mimiced - std::vector _vfree; -}; - -/// \brief Default implementation of \ref IkSolutionListBase -template -class IkSolutionList : public IkSolutionListBase -{ -public: - virtual size_t AddSolution(const std::vector >& vinfos, const std::vector& vfree) - { - size_t index = _listsolutions.size(); - _listsolutions.push_back(IkSolution(vinfos,vfree)); - return index; - } - - virtual const IkSolutionBase& GetSolution(size_t index) const - { - if( index >= _listsolutions.size() ) { - throw std::runtime_error("GetSolution index is invalid"); - } - typename std::list< IkSolution >::const_iterator it = _listsolutions.begin(); - std::advance(it,index); - return *it; - } - - virtual size_t GetNumSolutions() const { - return _listsolutions.size(); - } - - virtual void Clear() { - _listsolutions.clear(); - } - -protected: - std::list< IkSolution > _listsolutions; -}; - -} - -#endif // OPENRAVE_IKFAST_HEADER - -// The following code is dependent on the C++ library linking with. -#ifdef IKFAST_HAS_LIBRARY - -// defined when creating a shared object/dll -#ifdef IKFAST_CLIBRARY -#ifdef _MSC_VER -#define IKFAST_API extern "C" __declspec(dllexport) -#else -#define IKFAST_API extern "C" -#endif -#else -#define IKFAST_API -#endif - -#ifdef IKFAST_NAMESPACE -namespace IKFAST_NAMESPACE { -#endif - -#ifdef IKFAST_REAL -typedef IKFAST_REAL IkReal; -#else -typedef double IkReal; -#endif - -/** \brief Computes all IK solutions given a end effector coordinates and the free joints. - - - ``eetrans`` - 3 translation values. For iktype **TranslationXYOrientation3D**, the z-axis is the orientation. - - ``eerot`` - - For **Transform6D** it is 9 values for the 3x3 rotation matrix. - - For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target direction. - - For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D** the first value represents the angle. - - For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end effector coordinate system. - */ -IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, ikfast::IkSolutionListBase& solutions); - -/// \brief Computes the end effector coordinates given the joint values. This function is used to double check ik. -IKFAST_API void ComputeFk(const IkReal* joints, IkReal* eetrans, IkReal* eerot); - -/// \brief returns the number of free parameters users has to set apriori -IKFAST_API int GetNumFreeParameters(); - -/// \brief the indices of the free parameters indexed by the chain joints -IKFAST_API int* GetFreeParameters(); - -/// \brief the total number of indices of the chain -IKFAST_API int GetNumJoints(); - -/// \brief the size in bytes of the configured number type -IKFAST_API int GetIkRealSize(); - -/// \brief the ikfast version used to generate this file -IKFAST_API const char* GetIkFastVersion(); - -/// \brief the ik type ID -IKFAST_API int GetIkType(); - -/// \brief a hash of all the chain values used for double checking that the correct IK is used. -IKFAST_API const char* GetKinematicsHash(); - -#ifdef IKFAST_NAMESPACE -} -#endif - -#endif // IKFAST_HAS_LIBRARY diff --git a/ur_kinematics/include/ur_kinematics/ur_kin.h b/ur_kinematics/include/ur_kinematics/ur_kin.h deleted file mode 100644 index fd28291a7..000000000 --- a/ur_kinematics/include/ur_kinematics/ur_kin.h +++ /dev/null @@ -1,76 +0,0 @@ -/********************************************************************* - * - * Provides forward and inverse kinematics for Univeral robot designs - * Author: Kelsey Hawkins (kphawkins@gatech.edu) - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Georgia Institute of Technology - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Georgia Institute of Technology nor the names of - * its contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ -#ifndef UR_KIN_H -#define UR_KIN_H - -// These kinematics find the tranfrom from the base link to the end effector. -// Though the raw D-H parameters specify a transform from the 0th link to the 6th link, -// offset transforms are specified in this formulation. -// To work with the raw D-H kinematics, use the inverses of the transforms below. - -// Transform from base link to 0th link -// -1, 0, 0, 0 -// 0, -1, 0, 0 -// 0, 0, 1, 0 -// 0, 0, 0, 1 - -// Transform from 6th link to end effector -// 0, -1, 0, 0 -// 0, 0, -1, 0 -// 1, 0, 0, 0 -// 0, 0, 0, 1 - -namespace ur_kinematics { - // @param q The 6 joint values - // @param T The 4x4 end effector pose in row-major ordering - void forward(const double* q, double* T); - - // @param q The 6 joint values - // @param Ti The 4x4 link i pose in row-major ordering. If NULL, nothing is stored. - void forward_all(const double* q, double* T1, double* T2, double* T3, - double* T4, double* T5, double* T6); - - // @param T The 4x4 end effector pose in row-major ordering - // @param q_sols An 8x6 array of doubles returned, all angles should be in [0,2*PI) - // @param q6_des An optional parameter which designates what the q6 value should take - // in case of an infinite solution on that joint. - // @return Number of solutions found (maximum of 8) - int inverse(const double* T, double* q_sols, double q6_des=0.0); -}; - -#endif //UR_KIN_H diff --git a/ur_kinematics/include/ur_kinematics/ur_kin.hpp b/ur_kinematics/include/ur_kinematics/ur_kin.hpp new file mode 100644 index 000000000..dfbbe9065 --- /dev/null +++ b/ur_kinematics/include/ur_kinematics/ur_kin.hpp @@ -0,0 +1,616 @@ +/********************************************************************* + * + * Provides forward and inverse kinematics for Univeral robot designs + * Author: Kelsey Hawkins (kphawkins@gatech.edu) + * Leo Ghafari (leo@ascent.ai) + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Georgia Institute of Technology + * Copyright (c) 2019, Ascent Robotics inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Georgia Institute of Technology nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +#ifndef UR_KIN_H +#define UR_KIN_H + +// These kinematics find the tranfrom from the base link to the end effector. +// Though the raw D-H parameters specify a transform from the 0th link to the 6th link, +// offset transforms are specified in this formulation. +// To work with the raw D-H kinematics, use the inverses of the transforms below. + +// Transform from base link to 0th link +// -1, 0, 0, 0 +// 0, -1, 0, 0 +// 0, 0, 1, 0 +// 0, 0, 0, 1 + +// Transform from 6th link to end effector +// 0, -1, 0, 0 +// 0, 0, -1, 0 +// 1, 0, 0, 0 +// 0, 0, 0, 1 + +#include +#include +#include +#include +#include + +namespace ur_kinematics { + + const double ZERO_THRESH = 1e-8; + inline int SIGN(double x) { + return (x > 0) - (x < 0); + } + + // Denavit–Hartenberg parameters for calculations of kinematics of UR robots + // Values for the existing UR robots are provided for simplicity and are obtained from: + // https://www.universal-robots.com/how-tos-and-faqs/faq/ur-faq/parameters-for-calculations-of-kinematics-and-dynamics-45257/ + struct UR_PARAMS + { + double d1; + double a2; + double a3; + double d4; + double d5; + double d6; + }; + + constexpr UR_PARAMS UR3{0.1519, -0.24365, -0.21325, 0.11235, 0.08535, 0.0819}; + constexpr UR_PARAMS UR5{0.089159, -0.42500, -0.39225, 0.10915, 0.09465, 0.0823}; + constexpr UR_PARAMS UR10{ 0.1273, -0.612, -0.5723, 0.163941, 0.1157, 0.0922}; + + constexpr UR_PARAMS UR3E{0.15185, -0.24355, -0.2132, 0.13105, 0.08535, 0.0921}; + constexpr UR_PARAMS UR5E{0.1625, -0.425, -0.3922, 0.1333, 0.0997, 0.0996}; + constexpr UR_PARAMS UR10E{ 0.1807, -0.6127, -0.57155, 0.17415, 0.11985, 0.11655}; + + // @param p The Denavit–Hartenberg parameters of the robot + // @param q The 6 joint values (Must be pre-allocated) + // @param T The 4x4 end effector pose in row-major ordering (Must be pre-allocated) + template + inline void forward(const UR_PARAMS& p, const ValueType* const q, ValueType* const T) { + const auto s1 = std::sin(q[0]); + const auto c1 = std::cos(q[0]); + + const auto s2 = std::sin(q[1]); + const auto c2 = std::cos(q[1]); + + const auto s3 = std::sin(q[2]); + const auto c3 = std::cos(q[2]); + + const auto s5 = std::sin(q[4]); + const auto c5 = std::cos(q[4]); + + const auto s6 = std::sin(q[5]); + const auto c6 = std::cos(q[5]); + + + const auto q234 = q[1] + q[2] + q[3]; + const auto s234 = std::sin(q234); + const auto c234 = std::cos(q234); + + + T[0] = ((c1*c234-s1*s234)*s5)/2.0 - c5*s1 + ((c1*c234+s1*s234)*s5)/2.0; + + T[1] = (c6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0) - (s6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0); + + T[2] = (-(c6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0 - s6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0)); + + T[3] = ((p.d5*(s1*c234-c1*s234))/2.0 - (p.d5*(s1*c234+c1*s234))/2.0 - + p.d4*s1 + (p.d6*(c1*c234-s1*s234)*s5)/2.0 + (p.d6*(c1*c234+s1*s234)*s5)/2.0 - + p.a2*c1*c2 - p.d6*c5*s1 - p.a3*c1*c2*c3 + p.a3*c1*s2*s3); + + T[4] = c1*c5 + ((s1*c234+c1*s234)*s5)/2.0 + ((s1*c234-c1*s234)*s5)/2.0; + + T[5] = (c6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0) + + s6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0)); + + T[6] = (c6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0) - + s6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0)); + + T[7] = ((p.d5*(c1*c234-s1*s234))/2.0 - (p.d5*(c1*c234+s1*s234))/2.0 + p.d4*c1 + + (p.d6*(s1*c234+c1*s234)*s5)/2.0 + (p.d6*(s1*c234-c1*s234)*s5)/2.0 + p.d6*c1*c5 - + p.a2*c2*s1 - p.a3*c2*c3*s1 + p.a3*s1*s2*s3); + + T[8] = ((c234*c5-s234*s5)/2.0 - (c234*c5+s234*s5)/2.0); + + T[9] = ((s234*c6-c234*s6)/2.0 - (s234*c6+c234*s6)/2.0 - s234*c5*c6); + + T[10] = (s234*c5*s6 - (c234*c6+s234*s6)/2.0 - (c234*c6-s234*s6)/2.0); + + T[11] = (p.d1 + (p.d6*(c234*c5-s234*s5))/2.0 + p.a3*(s2*c3+c2*s3) + p.a2*s2 - + (p.d6*(c234*c5+s234*s5))/2.0 - p.d5*c234); + + T[12] = 0.0; + T[13] = 0.0; + T[14] = 0.0; + T[15] = 1.0; + } + + // @param p The Denavit–Hartenberg parameters of the robot + // @param q The 6 joint values + // @param T The 4x4 end effector pose in row-major ordering + template + inline int forward(const UR_PARAMS& p, const std::array& q , std::array& T){ + forward(p, q.data(), T.data()); + } + + // @param p The Denavit–Hartenberg parameters of the robot + // @param q The 6 joint values (Must be pre-allocated) + // @param Ti The 4x4 link i pose in row-major ordering (Must be pre-allocated). + template + inline void forward_all(const UR_PARAMS& p, const ValueType* const q, + ValueType* const T1, + ValueType* const T2, + ValueType* const T3, + ValueType* const T4, + ValueType* const T5, + ValueType* const T6) { + const auto s1 = std::sin(q[0]); + const auto c1 = std::cos(q[0]); + + const auto s2 = std::sin(q[1]); + const auto c2 = std::cos(q[1]); + + const auto s3 = std::sin(q[2]); + const auto c3 = std::cos(q[2]); + + const auto s5 = std::sin(q[4]); + const auto c5 = std::cos(q[4]); + + const auto s6 = std::sin(q[5]); + const auto c6 = std::cos(q[5]); + + const auto q23 = q[1] + q[2]; + const auto q234 = q23 + q[3]; + + const auto s23 = std::sin(q23); + const auto c23 = std::cos(q23); + const auto s234 = std::sin(q234); + const auto c234 = std::cos(q234); + + T1[0] = c1; + T1[1] = 0; + T1[2] = s1; + T1[3] = 0; + T1[4] = s1; + T1[5] = 0; + T1[6] = -c1; + T1[7] = 0; + T1[8] = 0; + T1[9] = 1; + T1[10] = 0; + T1[11] = p.d1; + T1[12] = 0; + T1[13] = 0; + T1[14] = 0; + T1[15] = 1; + + T2[0] = c1*c2; + T2[1] = -c1*s2; + T2[2] = s1; + T2[3] = p.a2*c1*c2; + T2[4] = c2*s1; + T2[5] = -s1*s2; + T2[6] = -c1; + T2[7] = p.a2*c2*s1; + T2[8] = s2; + T2[9] = c2; + T2[10] = 0; + T2[11] = p.d1 + p.a2*s2; + T2[12] = 0; + T2[13] = 0; + T2[14] = 0; + T2[15] = 1; + + T3[0] = c23*c1; + T3[1] = -s23*c1; + T3[2] = s1; + T3[3] = c1*(p.a3*c23 + p.a2*c2); + T3[4] = c23*s1; + T3[5] = -s23*s1; + T3[6] = -c1; + T3[7] = s1*(p.a3*c23 + p.a2*c2); + T3[8] = s23; + T3[9] = c23; + T3[10] = 0; + T3[11] = p.d1 + p.a3*s23 + p.a2*s2; + T3[12] = 0; + T3[13] = 0; + T3[14] = 0; + T3[15] = 1; + + T4[0] = c234*c1; + T4[1] = s1; + T4[2] = s234*c1; + T4[3] = c1*(p.a3*c23 + p.a2*c2) + p.d4*s1; + T4[4] = c234*s1; + T4[5] = -c1; + T4[6] = s234*s1; + T4[7] = s1*(p.a3*c23 + p.a2*c2) - p.d4*c1; + T4[8] = s234; + T4[9] = 0; + T4[10] = -c234; + T4[11] = p.d1 + p.a3*s23 + p.a2*s2; + T4[12] = 0; + T4[13] = 0; + T4[14] = 0; + T4[15] = 1; + + T5[0] = s1*s5 + c234*c1*c5; + T5[1] = -s234*c1; + T5[2] = c5*s1 - c234*c1*s5; + T5[3] = c1*(p.a3*c23 + p.a2*c2) + p.d4*s1 + p.d5*s234*c1; + T5[4] = c234*c5*s1 - c1*s5; + T5[5] = -s234*s1; + T5[6] = -c1*c5 - c234*s1*s5; + T5[7] = s1*(p.a3*c23 + p.a2*c2) - p.d4*c1 + p.d5*s234*s1; + T5[8] = s234*c5; + T5[9] = c234; + T5[10] = -s234*s5; + T5[11] = p.d1 + p.a3*s23 + p.a2*s2 - p.d5*c234; + T5[12] = 0; + T5[13] = 0; + T5[14] = 0; + T5[15] = 1; + + T6[0] = c6*(s1*s5 + c234*c1*c5) - s234*c1*s6; + T6[1] = -s6*(s1*s5 + c234*c1*c5) - s234*c1*c6; + T6[2] = c5*s1 - c234*c1*s5; + T6[3] = p.d6*(c5*s1 - c234*c1*s5) + c1*(p.a3*c23 + p.a2*c2) + p.d4*s1 + p.d5*s234*c1; + T6[4] = -c6*(c1*s5 - c234*c5*s1) - s234*s1*s6; + T6[5] = s6*(c1*s5 - c234*c5*s1) - s234*c6*s1; + T6[6] = -c1*c5 - c234*s1*s5; + T6[7] = s1*(p.a3*c23 + p.a2*c2) - p.d4*c1 - p.d6*(c1*c5 + c234*s1*s5) + p.d5*s234*s1; + T6[8] = c234*s6 + s234*c5*c6; + T6[9] = c234*c6 - s234*c5*s6; + T6[10] = -s234*s5; + T6[11] = p.d1 + p.a3*s23 + p.a2*s2 - p.d5*c234 - p.d6*s234*s5; + T6[12] = 0; + T6[13] = 0; + T6[14] = 0; + T6[15] = 1; + + } + + // @param p The Denavit–Hartenberg parameters of the robot + // @param q The 6 joint values + // @param Ti The 4x4 link i pose in row-major ordering. + template + inline void forward_all(const UR_PARAMS& p, const std::array& q, + std::array& T1, + std::array& T2, + std::array& T3, + std::array& T4, + std::array& T5, + std::array& T6) + { + return forward_all(p, q.data(), T1.data(), T2.data(), T3.data(), T4.data(), T5.data(), T6.data()); + } + + // @param p The Denavit–Hartenberg parameters of the robot + // @param T The 4x4 end effector pose in row-major ordering (Must be pre-allocated) + // @param q_sols An 8x6 array of doubles returned, all angles should be in [0,2*PI) (Must be pre-allocated) + // @param q6_des An optional parameter which designates what the q6 value should take + // in case of an infinite solution on that joint. + // @return Number of solutions found (maximum of 8) + template + inline int inverse(const UR_PARAMS& p, const ValueType* const T, ValueType* const q_sols, ValueType q6_des = 0.0) { + int num_sols = 0; + const auto T02 = -T[0]; + const auto T00 = T[1]; + const auto T01 = T[2]; + const auto T03 = -T[3]; + + const auto T12 = -T[4]; + const auto T10 = T[5]; + const auto T11 = T[6]; + const auto T13 = -T[7]; + + const auto T22 = T[8]; + const auto T20 = -T[9]; + const auto T21 = -T[10]; + const auto T23 = T[11]; + + ////////////////////////////// shoulder rotate joint (q1) ////////////////////////////// + ValueType q1[2]; + { + const auto A = p.d6*T12 - T13; + const auto B = p.d6*T02 - T03; + const auto R = A*A + B*B; + + if(std::abs(A) < ZERO_THRESH) { + ValueType div; + + if(std::abs(std::abs(p.d4) - std::abs(B)) < ZERO_THRESH) + div = -SIGN(p.d4)*SIGN(B); + else + div = -p.d4/B; + + auto arcsin = std::asin(div); + + if(std::abs(arcsin) < ZERO_THRESH) + arcsin = 0.0; + + if(arcsin < 0.0) + q1[0] = arcsin + 2.0*M_PI; + else + q1[0] = arcsin; + + q1[1] = M_PI - arcsin; + } + else if(std::abs(B) < ZERO_THRESH) { + ValueType div; + + if(std::abs(std::abs(p.d4) - std::abs(A)) < ZERO_THRESH) + div = SIGN(p.d4)*SIGN(A); + else + div = p.d4/A; + + const auto arccos = std::acos(div); + + q1[0] = arccos; + q1[1] = 2.0*M_PI - arccos; + } + else if(p.d4*p.d4 > R) { + return num_sols; + } + else { + const auto arccos = std::acos(p.d4 / sqrt(R)) ; + const auto arctan = std::atan2(-B, A); + auto pos = arccos + arctan; + auto neg = -arccos + arctan; + + if(std::abs(pos) < ZERO_THRESH) + pos = 0.0; + + if(std::abs(neg) < ZERO_THRESH) + neg = 0.0; + + if(pos >= 0.0) + q1[0] = pos; + else + q1[0] = 2.0*M_PI + pos; + + if(neg >= 0.0) + q1[1] = neg; + else + q1[1] = 2.0*M_PI + neg; + } + } + + + //////////////////////////////////////////////////////////////////////////////// + + ////////////////////////////// wrist 2 joint (q5) ////////////////////////////// + ValueType q5[2][2]; + { + for(auto i=0; i<2; ++i) { + const auto numer = (T03*std::sin(q1[i]) - T13*std::cos(q1[i])-p.d4); + ValueType div; + + if(std::abs(std::abs(numer) - std::abs(p.d6)) < ZERO_THRESH) + div = SIGN(numer) * SIGN(p.d6); + else + div = numer / p.d6; + + const auto arccos = std::acos(div); + + q5[i][0] = arccos; + q5[i][1] = 2.0*M_PI - arccos; + } + } + + + //////////////////////////////////////////////////////////////////////////////// + + { + for(auto i=0; i<2; ++i) { + for(auto j=0; j<2; ++j) { + const auto c1 = std::cos(q1[i]); + const auto s1 = std::sin(q1[i]); + const auto c5 = std::cos(q5[i][j]); + const auto s5 = std::sin(q5[i][j]); + ValueType q6; + + ////////////////////////////// wrist 3 joint (q6) ////////////////////////////// + if(std::abs(s5) < ZERO_THRESH) + q6 = q6_des; + else { + q6 = std::atan2(SIGN(s5)*-(T01*s1 - T11*c1), + SIGN(s5)*(T00*s1 - T10*c1)); + + if(std::abs(q6) < ZERO_THRESH) + q6 = 0.0; + + if(q6 < 0.0) + q6 += 2.0*M_PI; + } + //////////////////////////////////////////////////////////////////////////////// + + ValueType q2[2]; + ValueType q3[2]; + ValueType q4[2]; + + ///////////////////////////// RRR joints (q2,q3,q4) //////////////////////////// + const auto c6 = std::cos(q6); + const auto s6 = std::sin(q6); + + const auto x04x = -s5*(T02*c1 + T12*s1) - c5*(s6*(T01*c1 + T11*s1) - c6*(T00*c1 + T10*s1)); + const auto x04y = c5*(T20*c6 - T21*s6) - T22*s5; + const auto p13x = p.d5*(s6*(T00*c1 + T10*s1) + c6*(T01*c1 + T11*s1)) - p.d6*(T02*c1 + T12*s1) + + T03*c1 + T13*s1; + const auto p13y = T23 - p.d1 - p.d6*T22 + p.d5*(T21*c6 + T20*s6); + auto c3 = (p13x*p13x + p13y*p13y - p.a2*p.a2 - p.a3*p.a3) / (2.0*p.a2*p.a3); + + if(std::abs(std::abs(c3) - 1.0) < ZERO_THRESH) + c3 = SIGN(c3); + else if(std::abs(c3) > 1.0) { + // TODO NO SOLUTION + continue; + } + + const auto arccos = std::acos(c3); + q3[0] = arccos; + q3[1] = 2.0*M_PI - arccos; + + const auto denom = p.a2*p.a2 + p.a3*p.a3 + 2*p.a2*p.a3*c3; + const auto s3 = std::sin(arccos); + const auto A = (p.a2 + p.a3*c3); + const auto B = p.a3*s3; + + q2[0] = std::atan2((A*p13y - B*p13x) / denom, (A*p13x + B*p13y) / denom); + q2[1] = std::atan2((A*p13y + B*p13x) / denom, (A*p13x - B*p13y) / denom); + + const auto c23_0 = std::cos(q2[0]+q3[0]); + const auto s23_0 = std::sin(q2[0]+q3[0]); + const auto c23_1 = std::cos(q2[1]+q3[1]); + const auto s23_1 = std::sin(q2[1]+q3[1]); + + q4[0] = std::atan2(c23_0*x04y - s23_0*x04x, x04x*c23_0 + x04y*s23_0); + q4[1] = std::atan2(c23_1*x04y - s23_1*x04x, x04x*c23_1 + x04y*s23_1); + + //////////////////////////////////////////////////////////////////////////////// + for(auto k=0; k<2; ++k) { + if(std::abs(q2[k]) < ZERO_THRESH) + q2[k] = 0.0; + else if(q2[k] < 0.0) + q2[k] += 2.0*M_PI; + + if(std::abs(q4[k]) < ZERO_THRESH) + q4[k] = 0.0; + else if(q4[k] < 0.0) + q4[k] += 2.0*M_PI; + + q_sols[num_sols*6+0] = q1[i]; + q_sols[num_sols*6+1] = q2[k]; + q_sols[num_sols*6+2] = q3[k]; + q_sols[num_sols*6+3] = q4[k]; + q_sols[num_sols*6+4] = q5[i][j]; + q_sols[num_sols*6+5] = q6; + + num_sols++; + } + + } + } + } + + + return num_sols; + } + + // @param p The Denavit–Hartenberg parameters of the robot + // @param T The 4x4 end effector pose in row-major ordering + // @param q_sols An 8x6 array of doubles returned, all angles should be in [0,2*PI) + // @param q6_des An optional parameter which designates what the q6 value should take + // in case of an infinite solution on that joint. + // @return Number of solutions found (maximum of 8) + template + inline int inverse(const UR_PARAMS& p, const std::array& T , std::array& q_sols, ValueType q6_des = 0.0){ + return inverse(p, T.data(), q_sols.data(), q6_des); + } + + + constexpr auto joints_count = 6u; + + template + using Solution = std::array; + + template + using SolutionVec = std::vector>; + + template + using ExpandedSolutionVec = std::vector>; + + template + using Limit = std::pair; + + template + using JointsLimits = std::array, joints_count>; + + // @param initial_solution_set Vector of initial solutions for an end effector position + // @param joints_limit Joints limits of the robot + // @param partial_solution Current partial solution + // @param expanded_solutions List of expanded solutions + // @param current_joint Joint id being currently rotated + template + inline void expand_solution(const Solution& initial_solution, const JointsLimits& joints_limit, Solution& partial_solution, SolutionVec& expanded_solutions, std::size_t current_joint) + { + if(current_joint == joints_count) + { + expanded_solutions.emplace_back(partial_solution); + } + else + { + auto q = initial_solution[current_joint]; + if(joints_limit[current_joint].first <= q && q <= joints_limit[current_joint].second) + { + partial_solution[current_joint] = q; + expand_solution(initial_solution, joints_limit, partial_solution, expanded_solutions, current_joint+1); + } + + q = initial_solution[current_joint] + 2.0*M_PI; + while(q <= joints_limit[current_joint].second) + { + partial_solution[current_joint] = q; + q += 2.0*M_PI; + expand_solution(initial_solution, joints_limit, partial_solution, expanded_solutions, current_joint+1); + } + + q = initial_solution[current_joint] - 2.0*M_PI; + while(q >= joints_limit[current_joint].first) + { + partial_solution[current_joint] = q; + q -= 2.0*M_PI; + expand_solution(initial_solution, joints_limit, partial_solution, expanded_solutions, current_joint+1); + } + } + } + + // @param initial_solution_set Vector of initial solutions for an end effector position + // @param joints_limit Joints limits of the robot + // @return The list of all solutions for the end effector position + template + inline ExpandedSolutionVec expand_solutions(const SolutionVec& initial_solution_set, const JointsLimits& joints_limit) + { + ExpandedSolutionVec expanded_solutions; + Solution partial{0}; + for(const auto& sol : initial_solution_set) + { + SolutionVec current_expansion; + expand_solution(sol, joints_limit, partial, current_expansion, 0); + expanded_solutions.emplace_back(current_expansion); + } + return expanded_solutions; + } + +} +#endif //UR_KIN_H diff --git a/ur_kinematics/pybind11 b/ur_kinematics/pybind11 new file mode 160000 index 000000000..9fd471212 --- /dev/null +++ b/ur_kinematics/pybind11 @@ -0,0 +1 @@ +Subproject commit 9fd4712121fdbb6202a35be4c788525e6c8ab826 diff --git a/ur_kinematics/src/benchmark_kinematics.cpp b/ur_kinematics/src/benchmark_kinematics.cpp new file mode 100644 index 000000000..23abc2bfc --- /dev/null +++ b/ur_kinematics/src/benchmark_kinematics.cpp @@ -0,0 +1,191 @@ +/********************************************************************* + * + * Benchmarking for forward and inverse kinematics for Univeral robot designs + * Author: Leo Ghafari (leo@ascent.ai) + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Ascent Robotics inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Ascent Robotics inc. nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +#include +#include + +#include +#include + + +static void BM_forward(benchmark::State& state) { + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution<> dis(-M_PI*2.0, M_PI*2.0); + + double* T = new double[16]{0}; + double* q = new double[6]{dis(gen)}; + + for (auto _ : state) + { + ur_kinematics::forward(ur_kinematics::UR3, q, T); + benchmark::ClobberMemory(); + } + delete[] q; + delete[] T; +} +BENCHMARK(BM_forward); + +static void BM_forward_array(benchmark::State& state) { + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution<> dis(-M_PI*2.0, M_PI*2.0); + + std::array T{0}; + std::array q{dis(gen)}; + + for (auto _ : state) + { + ur_kinematics::forward(ur_kinematics::UR3, q, T); + benchmark::ClobberMemory(); + } +} +BENCHMARK(BM_forward_array); + + +static void BM_forward_all(benchmark::State& state) { + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution<> dis(-M_PI*2.0, M_PI*2.0); + + double* T1 = new double[16]{0}; + double* T2 = new double[16]{0}; + double* T3 = new double[16]{0}; + double* T4 = new double[16]{0}; + double* T5 = new double[16]{0}; + double* T6 = new double[16]{0}; + double* q = new double[6]{dis(gen)}; + + for (auto _ : state) + { + ur_kinematics::forward_all(ur_kinematics::UR3, q, T1, T2, T3, T4, T5, T6); + benchmark::ClobberMemory(); + } + + delete[] q; + delete[] T1; + delete[] T2; + delete[] T3; + delete[] T4; + delete[] T5; + delete[] T6; +} +BENCHMARK(BM_forward_all); + +static void BM_forward_all_array(benchmark::State& state) { + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution<> dis(-M_PI*2.0, M_PI*2.0); + + std::array T1{dis(gen)}; + std::array T2{dis(gen)}; + std::array T3{dis(gen)}; + std::array T4{dis(gen)}; + std::array T5{dis(gen)}; + std::array T6{dis(gen)}; + std::array q{dis(gen)}; + + for (auto _ : state) + { + ur_kinematics::forward_all(ur_kinematics::UR3, q, T1, T2, T3, T4, T5, T6); + benchmark::ClobberMemory(); + } +} +BENCHMARK(BM_forward_all_array); + + +static void BM_inverse(benchmark::State& state) { + double* T = new double[16]{0}; + double* q = new double[6]{0}; + double* q_sol = new double[48]{0}; + ur_kinematics::forward(ur_kinematics::UR3, q, T); + + for (auto _ : state) + benchmark::DoNotOptimize(ur_kinematics::inverse(ur_kinematics::UR3, T, q_sol)); + + delete[] q; + delete[] q_sol; + delete[] T; +} +BENCHMARK(BM_inverse); + +static void BM_inverse_array(benchmark::State& state) { + std::array T{0}; + std::array q{0}; + std::array q_sol{0}; + + ur_kinematics::forward(ur_kinematics::UR3, q, T); + + for (auto _ : state) + benchmark::DoNotOptimize(ur_kinematics::inverse(ur_kinematics::UR3, T, q_sol)); +} +BENCHMARK(BM_inverse_array); + +static void BM_enumerate_all(benchmark::State& state) { + std::array T{0}; + std::array q{0}; + std::array q_sol{0}; + + ur_kinematics::forward(ur_kinematics::UR3, q, T); + auto solution_count = ur_kinematics::inverse(ur_kinematics::UR3, T, q_sol); + + std::vector> solutions_as_vector; + solutions_as_vector.reserve(solution_count); + for(auto i = 0; i < solution_count; ++i) + { + std::array solution; + auto start = std::begin(q_sol) + i*6; + auto end = start + 6; + std::copy(start, end, std::begin(solution)); + solutions_as_vector.emplace_back(solution); + } + + constexpr std::array, 6> joint_limits{ + std::pair(-2 * M_PI, 2 * M_PI), + std::pair(-2 * M_PI, 2 * M_PI), + std::pair(-2 * M_PI, 2 * M_PI), + std::pair(-2 * M_PI, 2 * M_PI), + std::pair(-2 * M_PI, 2 * M_PI), + std::pair(-2 * M_PI, 2 * M_PI) + }; + + for (auto _ : state) + benchmark::DoNotOptimize(ur_kinematics::expand_solutions(solutions_as_vector, joint_limits)); +} +BENCHMARK(BM_enumerate_all); + +BENCHMARK_MAIN(); diff --git a/ur_kinematics/src/test_kinematics.cpp b/ur_kinematics/src/test_kinematics.cpp new file mode 100644 index 000000000..581b365cd --- /dev/null +++ b/ur_kinematics/src/test_kinematics.cpp @@ -0,0 +1,141 @@ +/********************************************************************* + * + * Unit testing for forward and inverse kinematics for Univeral robot designs + * Author: Leo Ghafari (leo@ascent.ai) + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, Ascent Robotics inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Ascent Robotics inc. nor the names of + * its contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +#include +#include +#include + +#include + +#include + +TEST(ConsistencyTests, pointer_array) +{ + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution<> dis(-M_PI*2.0, M_PI*2.0); + + double* q = new double[6]{0}; + double* T = new double[16]{0}; + std::array q_arr{0}; + std::array T_arr{0}; + + for (auto i = 0; i < 500; ++i) + { + for(auto j = 0; j < 6; ++j) + { + auto q_val = dis(gen); + q[j] = q_val; + q_arr[j] = q_val; + } + + ur_kinematics::forward(ur_kinematics::UR3, q, T); + ur_kinematics::forward(ur_kinematics::UR3, q_arr, T_arr); + + for(auto j = 0; j < 6; ++j) + EXPECT_EQ(T_arr[j], T[j]); + } + + delete[] q; + delete[] T; +} + + +TEST(ConsistencyTests, forward_inverse) +{ + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution<> dis(0, M_PI*2.0); + + std::array q_arr{0}; + std::array T_arr{0}; + std::array q_sol{0}; + + for (auto i = 0; i < 10000; ++i) + { + for(auto j = 0; j < 6; ++j) + { + auto q_val = dis(gen); + q_arr[j] = q_val; + } + ur_kinematics::forward(ur_kinematics::UR3, q_arr, T_arr); + auto solution_count = ur_kinematics::inverse(ur_kinematics::UR3, T_arr, q_sol, q_arr[5]); + + std::vector> solutions_as_vector; + solutions_as_vector.reserve(solution_count); + for(auto j = 0; j < solution_count; ++j) + { + std::array solution; + auto start = std::begin(q_sol) + j*6; + auto end = start + 6; + std::copy(start, end, std::begin(solution)); + solutions_as_vector.emplace_back(solution); + } + + constexpr std::array, 6> joint_limits{ + std::pair(-2 * M_PI, 2 * M_PI), + std::pair(-2 * M_PI, 2 * M_PI), + std::pair(-2 * M_PI, 2 * M_PI), + std::pair(-2 * M_PI, 2 * M_PI), + std::pair(-2 * M_PI, 2 * M_PI), + std::pair(-2 * M_PI, 2 * M_PI) + }; + + auto expanded = ur_kinematics::expand_solutions(solutions_as_vector, joint_limits); + auto found = false; + + for(const auto& expansion_set : expanded) + { + if(not found) + { + found = std::find_if(std::begin(expansion_set), std::end(expansion_set), [q_arr](const std::array& value) + { + for(int j = 0; j < 6; ++j) + { + if(std::abs(q_arr[j] - value[j]) > 1e-3) + { + return false; + } + } + return true; + }) != std::end(expansion_set); + } + } + + ASSERT_EQ(found, true); + } +} diff --git a/ur_kinematics/src/ur_kin.cpp b/ur_kinematics/src/ur_kin.cpp deleted file mode 100644 index c1f74bd83..000000000 --- a/ur_kinematics/src/ur_kin.cpp +++ /dev/null @@ -1,455 +0,0 @@ -#include - -#include -#include - - -namespace ur_kinematics { - - namespace { - const double ZERO_THRESH = 0.00000001; - int SIGN(double x) { - return (x > 0) - (x < 0); - } - const double PI = M_PI; - - //#define UR10_PARAMS - #ifdef UR10_PARAMS - const double d1 = 0.1273; - const double a2 = -0.612; - const double a3 = -0.5723; - const double d4 = 0.163941; - const double d5 = 0.1157; - const double d6 = 0.0922; - #endif - - //#define UR5_PARAMS - #ifdef UR5_PARAMS - const double d1 = 0.089159; - const double a2 = -0.42500; - const double a3 = -0.39225; - const double d4 = 0.10915; - const double d5 = 0.09465; - const double d6 = 0.0823; - #endif - - //#define UR3_PARAMS - #ifdef UR3_PARAMS - const double d1 = 0.1519; - const double a2 = -0.24365; - const double a3 = -0.21325; - const double d4 = 0.11235; - const double d5 = 0.08535; - const double d6 = 0.0819; - #endif - } - - void forward(const double* q, double* T) { - double s1 = sin(*q), c1 = cos(*q); q++; - double q234 = *q, s2 = sin(*q), c2 = cos(*q); q++; - double s3 = sin(*q), c3 = cos(*q); q234 += *q; q++; - q234 += *q; q++; - double s5 = sin(*q), c5 = cos(*q); q++; - double s6 = sin(*q), c6 = cos(*q); - double s234 = sin(q234), c234 = cos(q234); - *T = ((c1*c234-s1*s234)*s5)/2.0 - c5*s1 + ((c1*c234+s1*s234)*s5)/2.0; T++; - *T = (c6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0) - - (s6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0); T++; - *T = (-(c6*((s1*c234+c1*s234) - (s1*c234-c1*s234)))/2.0 - - s6*(s1*s5 + ((c1*c234-s1*s234)*c5)/2.0 + ((c1*c234+s1*s234)*c5)/2.0)); T++; - *T = ((d5*(s1*c234-c1*s234))/2.0 - (d5*(s1*c234+c1*s234))/2.0 - - d4*s1 + (d6*(c1*c234-s1*s234)*s5)/2.0 + (d6*(c1*c234+s1*s234)*s5)/2.0 - - a2*c1*c2 - d6*c5*s1 - a3*c1*c2*c3 + a3*c1*s2*s3); T++; - *T = c1*c5 + ((s1*c234+c1*s234)*s5)/2.0 + ((s1*c234-c1*s234)*s5)/2.0; T++; - *T = (c6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0) + - s6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0)); T++; - *T = (c6*((c1*c234-s1*s234)/2.0 - (c1*c234+s1*s234)/2.0) - - s6*(((s1*c234+c1*s234)*c5)/2.0 - c1*s5 + ((s1*c234-c1*s234)*c5)/2.0)); T++; - *T = ((d5*(c1*c234-s1*s234))/2.0 - (d5*(c1*c234+s1*s234))/2.0 + d4*c1 + - (d6*(s1*c234+c1*s234)*s5)/2.0 + (d6*(s1*c234-c1*s234)*s5)/2.0 + d6*c1*c5 - - a2*c2*s1 - a3*c2*c3*s1 + a3*s1*s2*s3); T++; - *T = ((c234*c5-s234*s5)/2.0 - (c234*c5+s234*s5)/2.0); T++; - *T = ((s234*c6-c234*s6)/2.0 - (s234*c6+c234*s6)/2.0 - s234*c5*c6); T++; - *T = (s234*c5*s6 - (c234*c6+s234*s6)/2.0 - (c234*c6-s234*s6)/2.0); T++; - *T = (d1 + (d6*(c234*c5-s234*s5))/2.0 + a3*(s2*c3+c2*s3) + a2*s2 - - (d6*(c234*c5+s234*s5))/2.0 - d5*c234); T++; - *T = 0.0; T++; *T = 0.0; T++; *T = 0.0; T++; *T = 1.0; - } - - void forward_all(const double* q, double* T1, double* T2, double* T3, - double* T4, double* T5, double* T6) { - double s1 = sin(*q), c1 = cos(*q); q++; // q1 - double q23 = *q, q234 = *q, s2 = sin(*q), c2 = cos(*q); q++; // q2 - double s3 = sin(*q), c3 = cos(*q); q23 += *q; q234 += *q; q++; // q3 - q234 += *q; q++; // q4 - double s5 = sin(*q), c5 = cos(*q); q++; // q5 - double s6 = sin(*q), c6 = cos(*q); // q6 - double s23 = sin(q23), c23 = cos(q23); - double s234 = sin(q234), c234 = cos(q234); - - if(T1 != NULL) { - *T1 = c1; T1++; - *T1 = 0; T1++; - *T1 = s1; T1++; - *T1 = 0; T1++; - *T1 = s1; T1++; - *T1 = 0; T1++; - *T1 = -c1; T1++; - *T1 = 0; T1++; - *T1 = 0; T1++; - *T1 = 1; T1++; - *T1 = 0; T1++; - *T1 =d1; T1++; - *T1 = 0; T1++; - *T1 = 0; T1++; - *T1 = 0; T1++; - *T1 = 1; T1++; - } - - if(T2 != NULL) { - *T2 = c1*c2; T2++; - *T2 = -c1*s2; T2++; - *T2 = s1; T2++; - *T2 =a2*c1*c2; T2++; - *T2 = c2*s1; T2++; - *T2 = -s1*s2; T2++; - *T2 = -c1; T2++; - *T2 =a2*c2*s1; T2++; - *T2 = s2; T2++; - *T2 = c2; T2++; - *T2 = 0; T2++; - *T2 = d1 + a2*s2; T2++; - *T2 = 0; T2++; - *T2 = 0; T2++; - *T2 = 0; T2++; - *T2 = 1; T2++; - } - - if(T3 != NULL) { - *T3 = c23*c1; T3++; - *T3 = -s23*c1; T3++; - *T3 = s1; T3++; - *T3 =c1*(a3*c23 + a2*c2); T3++; - *T3 = c23*s1; T3++; - *T3 = -s23*s1; T3++; - *T3 = -c1; T3++; - *T3 =s1*(a3*c23 + a2*c2); T3++; - *T3 = s23; T3++; - *T3 = c23; T3++; - *T3 = 0; T3++; - *T3 = d1 + a3*s23 + a2*s2; T3++; - *T3 = 0; T3++; - *T3 = 0; T3++; - *T3 = 0; T3++; - *T3 = 1; T3++; - } - - if(T4 != NULL) { - *T4 = c234*c1; T4++; - *T4 = s1; T4++; - *T4 = s234*c1; T4++; - *T4 =c1*(a3*c23 + a2*c2) + d4*s1; T4++; - *T4 = c234*s1; T4++; - *T4 = -c1; T4++; - *T4 = s234*s1; T4++; - *T4 =s1*(a3*c23 + a2*c2) - d4*c1; T4++; - *T4 = s234; T4++; - *T4 = 0; T4++; - *T4 = -c234; T4++; - *T4 = d1 + a3*s23 + a2*s2; T4++; - *T4 = 0; T4++; - *T4 = 0; T4++; - *T4 = 0; T4++; - *T4 = 1; T4++; - } - - if(T5 != NULL) { - *T5 = s1*s5 + c234*c1*c5; T5++; - *T5 = -s234*c1; T5++; - *T5 = c5*s1 - c234*c1*s5; T5++; - *T5 =c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T5++; - *T5 = c234*c5*s1 - c1*s5; T5++; - *T5 = -s234*s1; T5++; - *T5 = - c1*c5 - c234*s1*s5; T5++; - *T5 =s1*(a3*c23 + a2*c2) - d4*c1 + d5*s234*s1; T5++; - *T5 = s234*c5; T5++; - *T5 = c234; T5++; - *T5 = -s234*s5; T5++; - *T5 = d1 + a3*s23 + a2*s2 - d5*c234; T5++; - *T5 = 0; T5++; - *T5 = 0; T5++; - *T5 = 0; T5++; - *T5 = 1; T5++; - } - - if(T6 != NULL) { - *T6 = c6*(s1*s5 + c234*c1*c5) - s234*c1*s6; T6++; - *T6 = - s6*(s1*s5 + c234*c1*c5) - s234*c1*c6; T6++; - *T6 = c5*s1 - c234*c1*s5; T6++; - *T6 =d6*(c5*s1 - c234*c1*s5) + c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T6++; - *T6 = - c6*(c1*s5 - c234*c5*s1) - s234*s1*s6; T6++; - *T6 = s6*(c1*s5 - c234*c5*s1) - s234*c6*s1; T6++; - *T6 = - c1*c5 - c234*s1*s5; T6++; - *T6 =s1*(a3*c23 + a2*c2) - d4*c1 - d6*(c1*c5 + c234*s1*s5) + d5*s234*s1; T6++; - *T6 = c234*s6 + s234*c5*c6; T6++; - *T6 = c234*c6 - s234*c5*s6; T6++; - *T6 = -s234*s5; T6++; - *T6 = d1 + a3*s23 + a2*s2 - d5*c234 - d6*s234*s5; T6++; - *T6 = 0; T6++; - *T6 = 0; T6++; - *T6 = 0; T6++; - *T6 = 1; T6++; - } - } - - int inverse(const double* T, double* q_sols, double q6_des) { - int num_sols = 0; - double T02 = -*T; T++; double T00 = *T; T++; double T01 = *T; T++; double T03 = -*T; T++; - double T12 = -*T; T++; double T10 = *T; T++; double T11 = *T; T++; double T13 = -*T; T++; - double T22 = *T; T++; double T20 = -*T; T++; double T21 = -*T; T++; double T23 = *T; - - ////////////////////////////// shoulder rotate joint (q1) ////////////////////////////// - double q1[2]; - { - double A = d6*T12 - T13; - double B = d6*T02 - T03; - double R = A*A + B*B; - if(fabs(A) < ZERO_THRESH) { - double div; - if(fabs(fabs(d4) - fabs(B)) < ZERO_THRESH) - div = -SIGN(d4)*SIGN(B); - else - div = -d4/B; - double arcsin = asin(div); - if(fabs(arcsin) < ZERO_THRESH) - arcsin = 0.0; - if(arcsin < 0.0) - q1[0] = arcsin + 2.0*PI; - else - q1[0] = arcsin; - q1[1] = PI - arcsin; - } - else if(fabs(B) < ZERO_THRESH) { - double div; - if(fabs(fabs(d4) - fabs(A)) < ZERO_THRESH) - div = SIGN(d4)*SIGN(A); - else - div = d4/A; - double arccos = acos(div); - q1[0] = arccos; - q1[1] = 2.0*PI - arccos; - } - else if(d4*d4 > R) { - return num_sols; - } - else { - double arccos = acos(d4 / sqrt(R)) ; - double arctan = atan2(-B, A); - double pos = arccos + arctan; - double neg = -arccos + arctan; - if(fabs(pos) < ZERO_THRESH) - pos = 0.0; - if(fabs(neg) < ZERO_THRESH) - neg = 0.0; - if(pos >= 0.0) - q1[0] = pos; - else - q1[0] = 2.0*PI + pos; - if(neg >= 0.0) - q1[1] = neg; - else - q1[1] = 2.0*PI + neg; - } - } - //////////////////////////////////////////////////////////////////////////////// - - ////////////////////////////// wrist 2 joint (q5) ////////////////////////////// - double q5[2][2]; - { - for(int i=0;i<2;i++) { - double numer = (T03*sin(q1[i]) - T13*cos(q1[i])-d4); - double div; - if(fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH) - div = SIGN(numer) * SIGN(d6); - else - div = numer / d6; - double arccos = acos(div); - q5[i][0] = arccos; - q5[i][1] = 2.0*PI - arccos; - } - } - //////////////////////////////////////////////////////////////////////////////// - - { - for(int i=0;i<2;i++) { - for(int j=0;j<2;j++) { - double c1 = cos(q1[i]), s1 = sin(q1[i]); - double c5 = cos(q5[i][j]), s5 = sin(q5[i][j]); - double q6; - ////////////////////////////// wrist 3 joint (q6) ////////////////////////////// - if(fabs(s5) < ZERO_THRESH) - q6 = q6_des; - else { - q6 = atan2(SIGN(s5)*-(T01*s1 - T11*c1), - SIGN(s5)*(T00*s1 - T10*c1)); - if(fabs(q6) < ZERO_THRESH) - q6 = 0.0; - if(q6 < 0.0) - q6 += 2.0*PI; - } - //////////////////////////////////////////////////////////////////////////////// - - double q2[2], q3[2], q4[2]; - ///////////////////////////// RRR joints (q2,q3,q4) //////////////////////////// - double c6 = cos(q6), s6 = sin(q6); - double x04x = -s5*(T02*c1 + T12*s1) - c5*(s6*(T01*c1 + T11*s1) - c6*(T00*c1 + T10*s1)); - double x04y = c5*(T20*c6 - T21*s6) - T22*s5; - double p13x = d5*(s6*(T00*c1 + T10*s1) + c6*(T01*c1 + T11*s1)) - d6*(T02*c1 + T12*s1) + - T03*c1 + T13*s1; - double p13y = T23 - d1 - d6*T22 + d5*(T21*c6 + T20*s6); - - double c3 = (p13x*p13x + p13y*p13y - a2*a2 - a3*a3) / (2.0*a2*a3); - if(fabs(fabs(c3) - 1.0) < ZERO_THRESH) - c3 = SIGN(c3); - else if(fabs(c3) > 1.0) { - // TODO NO SOLUTION - continue; - } - double arccos = acos(c3); - q3[0] = arccos; - q3[1] = 2.0*PI - arccos; - double denom = a2*a2 + a3*a3 + 2*a2*a3*c3; - double s3 = sin(arccos); - double A = (a2 + a3*c3), B = a3*s3; - q2[0] = atan2((A*p13y - B*p13x) / denom, (A*p13x + B*p13y) / denom); - q2[1] = atan2((A*p13y + B*p13x) / denom, (A*p13x - B*p13y) / denom); - double c23_0 = cos(q2[0]+q3[0]); - double s23_0 = sin(q2[0]+q3[0]); - double c23_1 = cos(q2[1]+q3[1]); - double s23_1 = sin(q2[1]+q3[1]); - q4[0] = atan2(c23_0*x04y - s23_0*x04x, x04x*c23_0 + x04y*s23_0); - q4[1] = atan2(c23_1*x04y - s23_1*x04x, x04x*c23_1 + x04y*s23_1); - //////////////////////////////////////////////////////////////////////////////// - for(int k=0;k<2;k++) { - if(fabs(q2[k]) < ZERO_THRESH) - q2[k] = 0.0; - else if(q2[k] < 0.0) q2[k] += 2.0*PI; - if(fabs(q4[k]) < ZERO_THRESH) - q4[k] = 0.0; - else if(q4[k] < 0.0) q4[k] += 2.0*PI; - q_sols[num_sols*6+0] = q1[i]; q_sols[num_sols*6+1] = q2[k]; - q_sols[num_sols*6+2] = q3[k]; q_sols[num_sols*6+3] = q4[k]; - q_sols[num_sols*6+4] = q5[i][j]; q_sols[num_sols*6+5] = q6; - num_sols++; - } - - } - } - } - return num_sols; - } -}; - - -#define IKFAST_HAS_LIBRARY -#include -using namespace ikfast; - -// check if the included ikfast version matches what this file was compiled with -#define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x] -IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61); - -#ifdef IKFAST_NAMESPACE -namespace IKFAST_NAMESPACE { -#endif - -void to_mat44(double * mat4_4, const IkReal* eetrans, const IkReal* eerot) -{ - for(int i=0; i< 3;++i){ - mat4_4[i*4+0] = eerot[i*3+0]; - mat4_4[i*4+1] = eerot[i*3+1]; - mat4_4[i*4+2] = eerot[i*3+2]; - mat4_4[i*4+3] = eetrans[i]; - } - mat4_4[3*4+0] = 0; - mat4_4[3*4+1] = 0; - mat4_4[3*4+2] = 0; - mat4_4[3*4+3] = 1; -} - -void from_mat44(const double * mat4_4, IkReal* eetrans, IkReal* eerot) -{ - for(int i=0; i< 3;++i){ - eerot[i*3+0] = mat4_4[i*4+0]; - eerot[i*3+1] = mat4_4[i*4+1]; - eerot[i*3+2] = mat4_4[i*4+2]; - eetrans[i] = mat4_4[i*4+3]; - } -} - - -IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase& solutions) { - if(!pfree) return false; - - int n = GetNumJoints(); - double q_sols[8*6]; - double T[16]; - - to_mat44(T, eetrans, eerot); - - int num_sols = ur_kinematics::inverse(T, q_sols,pfree[0]); - - std::vector vfree(0); - - for (int i=0; i < num_sols; ++i){ - std::vector > vinfos(n); - for (int j=0; j < n; ++j) vinfos[j].foffset = q_sols[i*n+j]; - solutions.AddSolution(vinfos,vfree); - } - return num_sols > 0; -} - -IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) -{ - double T[16]; - ur_kinematics::forward(j,T); - from_mat44(T,eetrans,eerot); -} - -IKFAST_API int GetNumFreeParameters() { return 1; } -IKFAST_API int* GetFreeParameters() { static int freeparams[] = {5}; return freeparams; } -IKFAST_API int GetNumJoints() { return 6; } - -IKFAST_API int GetIkRealSize() { return sizeof(IkReal); } - -#ifdef IKFAST_NAMESPACE -} // end namespace -#endif - -#ifndef IKFAST_NO_MAIN - -using namespace std; -using namespace ur_kinematics; - -int main(int argc, char* argv[]) -{ - double q[6] = {0.0, 0.0, 1.0, 0.0, 1.0, 0.0}; - double* T = new double[16]; - forward(q, T); - for(int i=0;i<4;i++) { - for(int j=i*4;j<(i+1)*4;j++) - printf("%1.3f ", T[j]); - printf("\n"); - } - double q_sols[8*6]; - int num_sols; - num_sols = inverse(T, q_sols); - for(int i=0;i -#include +#include +#include +#include -#include +namespace py = pybind11; -namespace p = boost::python; -namespace np = boost::numpy; - -np::ndarray forward_wrapper(np::ndarray const & q_arr) { - if(q_arr.get_dtype() != np::dtype::get_builtin()) { - PyErr_SetString(PyExc_TypeError, "Incorrect array data type"); - p::throw_error_already_set(); - } - if(q_arr.get_nd() != 1) { - PyErr_SetString(PyExc_TypeError, "Incorrect number of dimensions"); - p::throw_error_already_set(); - } - if(q_arr.shape(0) != 6) { - PyErr_SetString(PyExc_TypeError, "Incorrect shape (should be 6)"); - p::throw_error_already_set(); - } - Py_intptr_t shape[2] = { 4, 4 }; - np::ndarray result = np::zeros(2,shape,np::dtype::get_builtin()); - ur_kinematics::forward(reinterpret_cast(q_arr.get_data()), - reinterpret_cast(result.get_data())); - return result; +std::array forward_wrapper(ur_kinematics::UR_PARAMS params, ur_kinematics::Solution q) { + std::array T; + ur_kinematics::forward(params, q, T); + return T; } -np::ndarray inverse_wrapper(np::ndarray const & array, PyObject * q6_des_py) { - if(array.get_dtype() != np::dtype::get_builtin()) { - PyErr_SetString(PyExc_TypeError, "Incorrect array data type"); - p::throw_error_already_set(); - } - if(array.get_nd() != 2) { - PyErr_SetString(PyExc_TypeError, "Incorrect number of dimensions"); - p::throw_error_already_set(); - } - if(array.shape(0) != 4 || array.shape(1) != 4) { - PyErr_SetString(PyExc_TypeError, "Incorrect shape (should be 4x4)"); - p::throw_error_already_set(); +ur_kinematics::ExpandedSolutionVec inverse_wrapper(ur_kinematics::UR_PARAMS params, std::array T, ur_kinematics::JointsLimits joint_limits, double q6_des) { + std::array solutions; + auto solution_count = ur_kinematics::inverse(params, T, solutions, q6_des); + + ur_kinematics::SolutionVec solutions_as_vector; + solutions_as_vector.reserve(solution_count); + for(auto i = 0; i < solution_count; ++i) + { + ur_kinematics::Solution solution; + auto start = std::begin(solutions) + i*6; + auto end = start + 6; + std::copy(start, end, std::begin(solution)); + solutions_as_vector.emplace_back(solution); } - double* T = reinterpret_cast(array.get_data()); - double* q_sols = (double*) malloc(8*6*sizeof(double)); - double q6_des = PyFloat_AsDouble(q6_des_py); - int num_sols = ur_kinematics::inverse(T, q_sols, q6_des); - q_sols = (double*) realloc(q_sols, num_sols*6*sizeof(double)); - return np::from_data(q_sols, np::dtype::get_builtin() , p::make_tuple(num_sols, 6), p::make_tuple(6*sizeof(double), sizeof(double)), p::object()); + + return ur_kinematics::expand_solutions(solutions_as_vector, joint_limits); } -BOOST_PYTHON_MODULE(ur_kin_py) { - np::initialize(); // have to put this in any module that uses Boost.NumPy - p::def("forward", forward_wrapper); - p::def("inverse", inverse_wrapper); +PYBIND11_MODULE(ur_kin_py, m) +{ + py::class_(m, "UR_PARAMS"); + + m.attr("UR3") = ur_kinematics::UR3; + m.attr("UR5") = ur_kinematics::UR5; + m.attr("UR10") = ur_kinematics::UR10; + m.attr("UR3E") = ur_kinematics::UR3E; + m.attr("UR5E") = ur_kinematics::UR5E; + m.attr("UR10E") = ur_kinematics::UR10E; + + + m.def("forward", &forward_wrapper, "Forward kinematics"); + m.def("inverse", &inverse_wrapper, "Inverse kinematics"); } diff --git a/ur_kinematics/src/ur_kin_py/__init__.py b/ur_kinematics/src/ur_kin_py/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/ur_kinematics/src/ur_kinematics/test_analytical_ik.py b/ur_kinematics/src/ur_kinematics/test_analytical_ik.py index c11860bf5..26bf101ab 100644 --- a/ur_kinematics/src/ur_kinematics/test_analytical_ik.py +++ b/ur_kinematics/src/ur_kinematics/test_analytical_ik.py @@ -1,8 +1,46 @@ +######################################################################### +# +# Basic test of the Python wrapper for UR kinematics +# Author: Leo Ghafari (leo@ascent.ai) +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2019, Ascent Robotics inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Ascent Robotics inc. nor the names of +# its contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +######################################################################### import numpy as np import sys +from ur_kinematics.ur_kin_py import forward, inverse, UR3 import roslib roslib.load_manifest("ur_kinematics") -from ur_kin_py import forward, inverse + def best_sol(sols, q_guess, weights): valid_sols = [] @@ -18,12 +56,13 @@ def best_sol(sols, q_guess, weights): valid_sols.append(test_sol) if len(valid_sols) == 0: return None - best_sol_ind = np.argmin(np.sum((weights*(valid_sols - np.array(q_guess)))**2,1)) + best_sol_ind = np.argmin(np.sum((weights*(valid_sols - np.array(q_guess)))**2, 1)) return valid_sols[best_sol_ind] + def test_q(q): - x = forward(q) - sols = inverse(np.array(x), float(q[5])) + x = forward(UR3, q) + sols = inverse(UR3, np.array(x), float(q[5])).reshape(8, 6) qsol = best_sol(sols, q, [1.]*6) if qsol is None: @@ -35,10 +74,10 @@ def test_q(q): print 'Actual:', np.array(q) print 'Diff: ', q - qsol print 'Difdiv:', (q - qsol)/np.pi - print i1-3, i2-3, i3-3, i4-3, i5-3, i6-3 if raw_input() == 'q': sys.exit() + def main(): np.set_printoptions(precision=3) print "Testing multiples of pi/2..." @@ -58,9 +97,6 @@ def main(): test_q(q) print "Done!" + if __name__ == "__main__": - if False: - import cProfile - cProfile.run('main()', 'ik_prof') - else: - main() + main() diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp index a565d19e1..c8577d594 100644 --- a/ur_kinematics/src/ur_moveit_plugin.cpp +++ b/ur_kinematics/src/ur_moveit_plugin.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2014, Georgia Tech +* Copyright (c) 2019, Ascent Robotics inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Kelsey Hawkins */ +/* Authors: Kelsey Hawkins, Leo Ghafari */ /* Based on orignal source from Willow Garage. License copied below */ @@ -86,7 +87,7 @@ // UR kin #include -#include +#include //register KDLKinematics as a KinematicsBase implementation CLASS_LOADER_REGISTER_CLASS(ur_kinematics::URKinematicsPlugin, kinematics::KinematicsBase) @@ -635,9 +636,31 @@ bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose, for(int i=0; i<3; i++) homo_ik_pose[i][3] *= 1000; // strange KDL fix #endif ///////////////////////////////////////////////////////////////////////////// + #ifdef UR3_PARAMS + constexpr auto parameters = ur_kinematics::UR3; + #endif + #ifdef UR5_PARAMS + constexpr auto parameters = ur_kinematics::UR5; + #endif + + #ifdef UR10_PARAMS + constexpr auto parameters = ur_kinematics::UR10; + #endif + + #ifdef UR3_E_PARAMS + constexpr auto parameters = ur_kinematics::UR3E; + #endif + + #ifdef UR5_E_PARAMS + constexpr auto parameters = ur_kinematics::UR5E; + #endif + + #ifdef UR10_E_PARAMS + constexpr auto parameters = ur_kinematics::UR10E; + #endif // Do the analytic IK - num_sols = inverse((double*) homo_ik_pose, (double*) q_ik_sols, + num_sols = ur_kinematics::inverse(parameters, (double*) homo_ik_pose, (double*) q_ik_sols, jnt_pos_test(ur_joint_inds_start_+5)); diff --git a/ur_kinematics/ur_moveit_plugins.xml b/ur_kinematics/ur_moveit_plugins.xml index f77bd7c66..197ba19de 100644 --- a/ur_kinematics/ur_moveit_plugins.xml +++ b/ur_kinematics/ur_moveit_plugins.xml @@ -27,3 +27,33 @@ + + + + + Analytic kinematics for the Universal Robots UR3. + Developed by Kelsey Hawkins from Georgia Tech. + See http://hdl.handle.net/1853/50782 for details. + + + + + + + + Analytic kinematics for the Universal Robots UR5. + Developed by Kelsey Hawkins from Georgia Tech. + See http://hdl.handle.net/1853/50782 for details. + + + + + + + + Analytic kinematics for the Universal Robots UR10. + Developed by Kelsey Hawkins from Georgia Tech. + See http://hdl.handle.net/1853/50782 for details. + + + \ No newline at end of file