/home/laradmin/lar/bases/phantom_control/include/phantom_control/omni.h
Go to the documentation of this file.00001 /************************************************************************************************** 00002 Software License Agreement (BSD License) 00003 00004 Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt 00005 All rights reserved. 00006 00007 Redistribution and use in source and binary forms, with or without modification, are permitted 00008 provided that the following conditions are met: 00009 00010 *Redistributions of source code must retain the above copyright notice, this list of 00011 conditions and the following disclaimer. 00012 *Redistributions in binary form must reproduce the above copyright notice, this list of 00013 conditions and the following disclaimer in the documentation and/or other materials provided 00014 with the distribution. 00015 *Neither the name of the University of Aveiro nor the names of its contributors may be used to 00016 endorse or promote products derived from this software without specific prior written permission. 00017 00018 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR 00019 IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND 00020 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 00021 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 00022 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 00023 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER 00024 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT 00025 OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00026 ***************************************************************************************************/ 00033 #include <ros/ros.h> 00034 #include <geometry_msgs/PoseStamped.h> 00035 #include <tf/transform_broadcaster.h> 00036 00037 #include <string.h> 00038 #include <stdio.h> 00039 #include <math.h> 00040 #include <assert.h> 00041 #include <sstream> 00042 #include <vector> 00043 00044 #include <HL/hl.h> 00045 #include <HD/hd.h> 00046 #include <HDU/hduError.h> 00047 #include <HDU/hduVector.h> 00048 #include <HDU/hduMatrix.h> 00049 00050 00051 #define HOME_XX 0 00052 #define HOME_YY 0 00053 #define HOME_ZZ 0 00054 00068 struct OmniState { 00069 hduVector3Dd position; //3x1 vector of position 00070 //hduVector3Dd velocity; //3x1 vector of velocity 00071 //hduVector3Dd inp_vel1; //3x1 history of velocity used for filtering velocity estimate 00072 //hduVector3Dd inp_vel2; 00073 //hduVector3Dd inp_vel3; 00074 //hduVector3Dd out_vel1; 00075 //hduVector3Dd out_vel2; 00076 //hduVector3Dd out_vel3; 00077 hduVector3Dd pos_hist1; //3x1 history of position used for 2nd order backward difference estimate of velocity 00078 //hduVector3Dd pos_hist2; 00079 hduVector3Dd rot; 00080 hduVector3Dd joints; 00081 hduVector3Dd force; //3 element double vector force[0], force[1], force[2] 00082 float thetas[7]; 00083 HDdouble temp[3]; // Motors Temperture 00084 int buttons[2]; 00085 //int buttons_prev[2]; 00086 bool home; 00087 hduVector3Dd home_pos; 00088 };