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 #define UR10_PARAMS 00047 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 // These kinematics find the tranfrom from the base link to the end effector. 00058 // Though the raw D-H parameters specify a transform from the 0th link to the 6th link, 00059 // offset transforms are specified in this formulation. 00060 // To work with the raw D-H kinematics, use the inverses of the transforms below. 00061 00062 // Transform from base link to 0th link 00063 // -1, 0, 0, 0 00064 // 0, -1, 0, 0 00065 // 0, 0, 1, 0 00066 // 0, 0, 0, 1 00067 00068 // Transform from 6th link to end effector 00069 // 0, -1, 0, 0 00070 // 0, 0, -1, 0 00071 // 1, 0, 0, 0 00072 // 0, 0, 0, 1 00073 00074 namespace ur_kinematics { 00075 // @param q The 6 joint values 00076 // @param T The 4x4 end effector pose in row-major ordering 00077 void forward(const double* q, double* T); 00078 00079 // @param T The 4x4 end effector pose in row-major ordering 00080 // @param q_sols An 8x6 array of doubles returned, all angles should be in [0,2*PI) 00081 // @param q6_des An optional parameter which designates what the q6 value should take 00082 // in case of an infinite solution on that joint. 00083 // @return Number of solutions found (maximum of 8) 00084 int inverse(const double* T, double* q_sols, double q6_des=0.0); 00085 }; 00086 00087 #endif //UR_KIN_H