sensitiv.cpp
Go to the documentation of this file.
00001 /*
00002 ROBOOP -- A robotics object oriented package in C++
00003 Copyright (C) 1996-2004  Richard Gourdeau
00004 
00005 This library is free software; you can redistribute it and/or modify
00006 it under the terms of the GNU Lesser General Public License as
00007 published by the Free Software Foundation; either version 2.1 of the
00008 License, or (at your option) any later version.
00009 
00010 This library is distributed in the hope that it will be useful,
00011 but WITHOUT ANY WARRANTY; without even the implied warranty of
00012 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013 GNU Lesser General Public License for more details.
00014 
00015 You should have received a copy of the GNU Lesser General Public
00016 License along with this library; if not, write to the Free Software
00017 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00018 
00019 
00020 Report problems and direct all questions to:
00021 
00022 Richard Gourdeau
00023 Professeur Agrege
00024 Departement de genie electrique
00025 Ecole Polytechnique de Montreal
00026 C.P. 6079, Succ. Centre-Ville
00027 Montreal, Quebec, H3C 3A7
00028 
00029 email: richard.gourdeau@polymtl.ca
00030 
00031 -------------------------------------------------------------------------------
00032 Revision_history:
00033 
00034 2004/07/01: Ethan Tira-Thompson
00035     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00036 
00037 2004/07/02: Etienne Lachance
00038     -Added Doxygen comments.
00039 -------------------------------------------------------------------------------
00040 */
00041 
00047 
00048 static const char rcsid[] = "$Id: sensitiv.cpp,v 1.13 2004/07/06 02:16:37 gourdeau Exp $";
00049 
00050 #include "robot.h"
00051 
00052 #ifdef use_namespace
00053 namespace ROBOOP {
00054   using namespace NEWMAT;
00055 #endif
00056 
00057 
00058 ReturnMatrix Robot_basic::dtau_dq(const ColumnVector & q,
00059                                   const ColumnVector & qp,
00060                                   const ColumnVector & qpp)
00066 {
00067    int i, j;
00068    Matrix ldtau_dq(dof,dof);
00069    ColumnVector ltorque(dof);
00070    ColumnVector dtorque(dof);
00071    ColumnVector dq(dof);
00072    for(i = 1; i <= dof; i++) {
00073       for(j = 1; j <= dof; j++) {
00074          dq(j) = (i == j ? 1.0 : 0.0);
00075       }
00076       dq_torque(q,qp,qpp,dq,ltorque,dtorque);
00077       for(j = 1; j <= dof; j++) {
00078          ldtau_dq(j,i) = dtorque(j);
00079       }
00080    }
00081    ldtau_dq.Release(); return ldtau_dq;
00082 }
00083 
00084 ReturnMatrix Robot_basic::dtau_dqp(const ColumnVector & q,
00085                                    const ColumnVector & qp)
00091 {
00092    int i, j;
00093    Matrix ldtau_dqp(dof,dof);
00094    ColumnVector ltorque(dof);
00095    ColumnVector dtorque(dof);
00096    ColumnVector dqp(dof);
00097    for(i = 1; i <= dof; i++) {
00098       for(j = 1; j <= dof; j++) {
00099          dqp(j) = (i == j ? 1.0 : 0.0);
00100       }
00101       dqp_torque(q,qp,dqp,ltorque,dtorque);
00102       for(j = 1; j <= dof; j++) {
00103          ldtau_dqp(j,i) = dtorque(j);
00104       }
00105    }
00106    ldtau_dqp.Release(); return ldtau_dqp;
00107 }
00108 
00109 #ifdef use_namespace
00110 }
00111 #endif
00112 
00113 
00114 


kni
Author(s): Martin Günther
autogenerated on Thu Jun 6 2019 21:42:34