ur_kin.h
Go to the documentation of this file.
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 q       The 6 joint values 
00090   // @param Ti      The 4x4 link i pose in row-major ordering. If NULL, nothing is stored.
00091   void forward_all(const double* q, double* T1, double* T2, double* T3, 
00092                                     double* T4, double* T5, double* T6);
00093 
00094   // @param T       The 4x4 end effector pose in row-major ordering
00095   // @param q_sols  An 8x6 array of doubles returned, all angles should be in [0,2*PI)
00096   // @param q6_des  An optional parameter which designates what the q6 value should take
00097   //                in case of an infinite solution on that joint.
00098   // @return        Number of solutions found (maximum of 8)
00099   int inverse(const double* T, double* q_sols, double q6_des=0.0);
00100 };
00101 
00102 #endif //UR_KIN_H


ur_kinematics
Author(s): Kelsey Hawkins
autogenerated on Fri Aug 28 2015 13:31:18