graspit: /opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit

00001 //######################################################################
00002 //
00003 // GraspIt!
00004 // Copyright (C) 2002-2009  Columbia University in the City of New York.
00005 // All rights reserved.
00006 //
00007 // GraspIt! is free software: you can redistribute it and/or modify
00008 // it under the terms of the GNU General Public License as published by
00009 // the Free Software Foundation, either version 3 of the License, or
00010 // (at your option) any later version.
00011 //
00012 // GraspIt! is distributed in the hope that it will be useful,
00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015 // GNU General Public License for more details.
00016 //
00017 // You should have received a copy of the GNU General Public License
00018 // along with GraspIt!.  If not, see <http://www.gnu.org/licenses/>.
00019 //
00020 // Author(s): Matei T. Ciocarlie
00021 //
00022 // $Id: jacobian.cpp,v 1.3 2009/04/21 14:53:08 cmatei Exp $
00023 //
00024 //######################################################################
00025 
00026 #include "jacobian.h"
00027 #include <math.h>
00028 
00029 void jacobian(double t1, double t2, double t3, double t4, double px, double py, double pz, double *J)
00030 {
00031         double dxdt1 = -52.11*cos(t2)*sin(t1) - 40.76*cos(t2)*cos(t3)*sin(t1) + py*(0.7193709695451316*cos(t1) - 0.6946260923516316*sin(t1)*sin(t2)) + 
00032   40.76*(0.6946260923516316*cos(t1) + 0.7193709695451316*sin(t1)*sin(t2))*sin(t3) + 
00033   px*(cos(t4)*((0.6946260923516316*cos(t1) + 0.7193709695451316*sin(t1)*sin(t2))*sin(t3) - cos(t2)*cos(t3)*sin(t1)) + 
00034     (cos(t3)*(0.6946260923516316*cos(t1) + 0.7193709695451316*sin(t1)*sin(t2)) + cos(t2)*sin(t1)*sin(t3))*sin(t4)) + 
00035   pz*(((0.6946260923516316*cos(t1) + 0.7193709695451316*sin(t1)*sin(t2))*sin(t3) - cos(t2)*cos(t3)*sin(t1))*sin(t4) - 
00036     cos(t4)*(cos(t3)*(0.6946260923516316*cos(t1) + 0.7193709695451316*sin(t1)*sin(t2)) + cos(t2)*sin(t1)*sin(t3)));
00037 
00038         double dxdt2 = 0.6946260923516316*py*cos(t1)*cos(t2) - 29.321560718659562*cos(t1)*sin(t3)*cos(t2) - 52.11*cos(t1)*sin(t2) - 40.76*cos(t1)*cos(t3)*sin(t2) + 
00039   pz*(((-cos(t1))*cos(t3)*sin(t2) - 0.7193709695451316*cos(t1)*cos(t2)*sin(t3))*sin(t4) - 
00040     cos(t4)*(cos(t1)*sin(t2)*sin(t3) - 0.7193709695451316*cos(t1)*cos(t2)*cos(t3))) + 
00041   px*(cos(t4)*((-cos(t1))*cos(t3)*sin(t2) - 0.7193709695451316*cos(t1)*cos(t2)*sin(t3)) + (cos(t1)*sin(t2)*sin(t3) - 0.7193709695451316*cos(t1)*cos(t2)*cos(t3))*
00042      sin(t4));
00043 
00044         double dxdt3 = 40.76*cos(t3)*(0.6946260923516316*sin(t1) - 0.7193709695451316*cos(t1)*sin(t2)) - 40.76*cos(t1)*cos(t2)*sin(t3) + 
00045   pz*((cos(t3)*(0.6946260923516316*sin(t1) - 0.7193709695451316*cos(t1)*sin(t2)) - cos(t1)*cos(t2)*sin(t3))*sin(t4) - 
00046     cos(t4)*((-cos(t1))*cos(t2)*cos(t3) - (0.6946260923516316*sin(t1) - 0.7193709695451316*cos(t1)*sin(t2))*sin(t3))) + 
00047   px*(cos(t4)*(cos(t3)*(0.6946260923516316*sin(t1) - 0.7193709695451316*cos(t1)*sin(t2)) - cos(t1)*cos(t2)*sin(t3)) + 
00048     ((-cos(t1))*cos(t2)*cos(t3) - (0.6946260923516316*sin(t1) - 0.7193709695451316*cos(t1)*sin(t2))*sin(t3))*sin(t4));
00049 
00050         double dxdt4 = pz*(cos(t4)*(cos(t1)*cos(t2)*cos(t3) + (0.6946260923516316*sin(t1) - 0.7193709695451316*cos(t1)*sin(t2))*sin(t3)) + 
00051     (cos(t3)*(0.6946260923516316*sin(t1) - 0.7193709695451316*cos(t1)*sin(t2)) - cos(t1)*cos(t2)*sin(t3))*sin(t4)) + 
00052   px*(cos(t4)*(cos(t3)*(0.6946260923516316*sin(t1) - 0.7193709695451316*cos(t1)*sin(t2)) - cos(t1)*cos(t2)*sin(t3)) - 
00053     (cos(t1)*cos(t2)*cos(t3) + (0.6946260923516316*sin(t1) - 0.7193709695451316*cos(t1)*sin(t2))*sin(t3))*sin(t4));
00054 
00055         double dydt1 = 52.11*cos(t1)*cos(t2) + 40.76*cos(t1)*cos(t3)*cos(t2) + py*(0.7193709695451316*sin(t1) + 0.6946260923516316*cos(t1)*sin(t2)) + 
00056   40.76*(0.6946260923516316*sin(t1) - 0.7193709695451316*cos(t1)*sin(t2))*sin(t3) + 
00057   px*(cos(t4)*(cos(t1)*cos(t2)*cos(t3) + (0.6946260923516316*sin(t1) - 0.7193709695451316*cos(t1)*sin(t2))*sin(t3)) + 
00058     (cos(t3)*(0.6946260923516316*sin(t1) - 0.7193709695451316*cos(t1)*sin(t2)) - cos(t1)*cos(t2)*sin(t3))*sin(t4)) + 
00059   pz*((cos(t1)*cos(t2)*cos(t3) + (0.6946260923516316*sin(t1) - 0.7193709695451316*cos(t1)*sin(t2))*sin(t3))*sin(t4) - 
00060     cos(t4)*(cos(t3)*(0.6946260923516316*sin(t1) - 0.7193709695451316*cos(t1)*sin(t2)) - cos(t1)*cos(t2)*sin(t3)));
00061 
00062         double dydt2 = 0.6946260923516316*py*cos(t2)*sin(t1) - 40.76*cos(t3)*sin(t2)*sin(t1) - 52.11*sin(t2)*sin(t1) - 29.321560718659562*cos(t2)*sin(t3)*sin(t1) + 
00063   pz*(((-cos(t3))*sin(t1)*sin(t2) - 0.7193709695451316*cos(t2)*sin(t1)*sin(t3))*sin(t4) - 
00064     cos(t4)*(sin(t1)*sin(t2)*sin(t3) - 0.7193709695451316*cos(t2)*cos(t3)*sin(t1))) + 
00065   px*(cos(t4)*((-cos(t3))*sin(t1)*sin(t2) - 0.7193709695451316*cos(t2)*sin(t1)*sin(t3)) + (sin(t1)*sin(t2)*sin(t3) - 0.7193709695451316*cos(t2)*cos(t3)*sin(t1))*
00066      sin(t4));
00067 
00068         double dydt3 = 40.76*cos(t3)*(-0.6946260923516316*cos(t1) - 0.7193709695451316*sin(t1)*sin(t2)) - 40.76*cos(t2)*sin(t1)*sin(t3) + 
00069   pz*((cos(t3)*(-0.6946260923516316*cos(t1) - 0.7193709695451316*sin(t1)*sin(t2)) - cos(t2)*sin(t1)*sin(t3))*sin(t4) - 
00070     cos(t4)*((-cos(t2))*cos(t3)*sin(t1) - (-0.6946260923516316*cos(t1) - 0.7193709695451316*sin(t1)*sin(t2))*sin(t3))) + 
00071   px*(cos(t4)*(cos(t3)*(-0.6946260923516316*cos(t1) - 0.7193709695451316*sin(t1)*sin(t2)) - cos(t2)*sin(t1)*sin(t3)) + 
00072     ((-cos(t2))*cos(t3)*sin(t1) - (-0.6946260923516316*cos(t1) - 0.7193709695451316*sin(t1)*sin(t2))*sin(t3))*sin(t4));
00073 
00074         double dydt4 = pz*(cos(t4)*(cos(t2)*cos(t3)*sin(t1) + (-0.6946260923516316*cos(t1) - 0.7193709695451316*sin(t1)*sin(t2))*sin(t3)) + 
00075     (cos(t3)*(-0.6946260923516316*cos(t1) - 0.7193709695451316*sin(t1)*sin(t2)) - cos(t2)*sin(t1)*sin(t3))*sin(t4)) + 
00076   px*(cos(t4)*(cos(t3)*(-0.6946260923516316*cos(t1) - 0.7193709695451316*sin(t1)*sin(t2)) - cos(t2)*sin(t1)*sin(t3)) - 
00077     (cos(t2)*cos(t3)*sin(t1) + (-0.6946260923516316*cos(t1) - 0.7193709695451316*sin(t1)*sin(t2))*sin(t3))*sin(t4));
00078 
00079         double dzdt1 = 0;
00080 
00081         double dzdt2 = 40.76*cos(t3)*cos(t2) + 52.11*cos(t2) + 0.6946260923516316*py*sin(t2) - 29.321560718659562*sin(t2)*sin(t3) + 
00082   px*(cos(t4)*(cos(t2)*cos(t3) - 0.7193709695451316*sin(t2)*sin(t3)) + (-0.7193709695451316*cos(t3)*sin(t2) - cos(t2)*sin(t3))*sin(t4)) + 
00083   pz*((cos(t2)*cos(t3) - 0.7193709695451316*sin(t2)*sin(t3))*sin(t4) - cos(t4)*(-0.7193709695451316*cos(t3)*sin(t2) - cos(t2)*sin(t3)));
00084 
00085         double dzdt3 = 29.321560718659562*cos(t2)*cos(t3) - 40.76*sin(t2)*sin(t3) + px*(cos(t4)*(0.7193709695451316*cos(t2)*cos(t3) - sin(t2)*sin(t3)) + 
00086     ((-cos(t3))*sin(t2) - 0.7193709695451316*cos(t2)*sin(t3))*sin(t4)) + pz*((0.7193709695451316*cos(t2)*cos(t3) - sin(t2)*sin(t3))*sin(t4) - 
00087     cos(t4)*((-cos(t3))*sin(t2) - 0.7193709695451316*cos(t2)*sin(t3)));
00088 
00089         double dzdt4 = px*(cos(t4)*(0.7193709695451316*cos(t2)*cos(t3) - sin(t2)*sin(t3)) - (cos(t3)*sin(t2) + 0.7193709695451316*cos(t2)*sin(t3))*sin(t4)) + 
00090   pz*(cos(t4)*(cos(t3)*sin(t2) + 0.7193709695451316*cos(t2)*sin(t3)) + (0.7193709695451316*cos(t2)*cos(t3) - sin(t2)*sin(t3))*sin(t4));
00091 
00092         J[0] = dxdt1;   J[3] = dxdt2;   J[6] = dxdt3;   J[9]  = dxdt4;
00093         J[1] = dydt1;   J[4] = dydt2;   J[7] = dydt3;   J[10] = dydt4;
00094         J[2] = dzdt1;   J[5] = dzdt2;   J[8] = dzdt3;   J[11] = dzdt4;
00095 }
00096 
00097 void compute_dTdG(double s0, double s1, double s2, double s3, double *dTdG)
00098 {
00099         dTdG[0] = s0;   dTdG[4] = s1;   dTdG[8] =   0;  dTdG[12] =  0;  dTdG[16] =  0;  dTdG[20] =  0;
00100         dTdG[1] =  0;   dTdG[5] =  0;   dTdG[9] =  s0;  dTdG[13] = s1;  dTdG[17] =  0;  dTdG[21] =  0;
00101         dTdG[2] =  0;   dTdG[6] =  0;   dTdG[10] =  0;  dTdG[14] =  0;  dTdG[18] = s2;  dTdG[22] =  0;
00102         dTdG[3] =  0;   dTdG[7] =  0;   dTdG[11] =  0;  dTdG[15] =  0;  dTdG[19] =  0;  dTdG[23] = s3;
00103 }
/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/jacobian.cpp
