00001 /********************************************************************* 00002 * 00003 * Provides forward and inverse kinematics for Univeral robot designs 00004 * Author: Kelsey Hawkins (kphawkins@gatech.edu) 00005 * 00006 * Software License Agreement (BSD License) 00007 * 00008 * Copyright (c) 2013, Georgia Institute of Technology 00009 * All rights reserved. 00010 * 00011 * Redistribution and use in source and binary forms, with or without 00012 * modification, are permitted provided that the following conditions 00013 * are met: 00014 * 00015 * * Redistributions of source code must retain the above copyright 00016 * notice, this list of conditions and the following disclaimer. 00017 * * Redistributions in binary form must reproduce the above 00018 * copyright notice, this list of conditions and the following 00019 * disclaimer in the documentation and/or other materials provided 00020 * with the distribution. 00021 * * Neither the name of the Georgia Institute of Technology nor the names of 00022 * its contributors may be used to endorse or promote products derived 00023 * from this software without specific prior written permission. 00024 * 00025 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00026 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00027 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00028 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00029 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00030 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00031 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00032 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00033 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00034 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00035 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00036 * POSSIBILITY OF SUCH DAMAGE. 00037 *********************************************************************/ 00038 #ifndef UR_KIN_H 00039 #define UR_KIN_H 00040 #include <math.h> 00041 #include <stdio.h> 00042 00043 #define ZERO_THRESH 0.00000001 00044 #define SIGN(x) ( ( (x) > 0 ) - ( (x) < 0 ) ) 00045 #define PI M_PI 00046 00047 //#define UR10_PARAMS 00048 #ifdef UR10_PARAMS 00049 #define d1 0.1273 00050 #define a2 -0.612 00051 #define a3 -0.5723 00052 #define d4 0.163941 00053 #define d5 0.1157 00054 #define d6 0.0922 00055 #endif 00056 00057 //#define UR5_PARAMS 00058 #ifdef UR5_PARAMS 00059 #define d1 0.089159 00060 #define a2 -0.42500 00061 #define a3 -0.39225 00062 #define d4 0.10915 00063 #define d5 0.09465 00064 #define d6 0.0823 00065 #endif 00066 00067 // These kinematics find the tranfrom from the base link to the end effector. 00068 // Though the raw D-H parameters specify a transform from the 0th link to the 6th link, 00069 // offset transforms are specified in this formulation. 00070 // To work with the raw D-H kinematics, use the inverses of the transforms below. 00071 00072 // Transform from base link to 0th link 00073 // -1, 0, 0, 0 00074 // 0, -1, 0, 0 00075 // 0, 0, 1, 0 00076 // 0, 0, 0, 1 00077 00078 // Transform from 6th link to end effector 00079 // 0, -1, 0, 0 00080 // 0, 0, -1, 0 00081 // 1, 0, 0, 0 00082 // 0, 0, 0, 1 00083 00084 namespace ur_kinematics { 00085 // @param q The 6 joint values 00086 // @param T The 4x4 end effector pose in row-major ordering 00087 void forward(const double* q, double* T); 00088 00089 // @param T The 4x4 end effector pose in row-major ordering 00090 // @param q_sols An 8x6 array of doubles returned, all angles should be in [0,2*PI) 00091 // @param q6_des An optional parameter which designates what the q6 value should take 00092 // in case of an infinite solution on that joint. 00093 // @return Number of solutions found (maximum of 8) 00094 int inverse(const double* T, double* q_sols, double q6_des=0.0); 00095 }; 00096 00097 #endif //UR_KIN_H