sensitiv.cpp
Go to the documentation of this file.
1 /*
2 ROBOOP -- A robotics object oriented package in C++
3 Copyright (C) 1996-2004 Richard Gourdeau
4 
5 This library is free software; you can redistribute it and/or modify
6 it under the terms of the GNU Lesser General Public License as
7 published by the Free Software Foundation; either version 2.1 of the
8 License, or (at your option) any later version.
9 
10 This library is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU Lesser General Public License for more details.
14 
15 You should have received a copy of the GNU Lesser General Public
16 License along with this library; if not, write to the Free Software
17 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 
19 
20 Report problems and direct all questions to:
21 
22 Richard Gourdeau
23 Professeur Agrege
24 Departement de genie electrique
25 Ecole Polytechnique de Montreal
26 C.P. 6079, Succ. Centre-Ville
27 Montreal, Quebec, H3C 3A7
28 
29 email: richard.gourdeau@polymtl.ca
30 
31 -------------------------------------------------------------------------------
32 Revision_history:
33 
34 2004/07/01: Ethan Tira-Thompson
35  -Added support for newmat's use_namespace #define, using ROBOOP namespace
36 
37 2004/07/02: Etienne Lachance
38  -Added Doxygen comments.
39 -------------------------------------------------------------------------------
40 */
41 
47 static const char rcsid[] = "$Id: sensitiv.cpp,v 1.13 2004/07/06 02:16:37 gourdeau Exp $";
49 
50 #include "robot.h"
51 
52 #ifdef use_namespace
53 namespace ROBOOP {
54  using namespace NEWMAT;
55 #endif
56 
57 
59  const ColumnVector & qp,
60  const ColumnVector & qpp)
66 {
67  int i, j;
68  Matrix ldtau_dq(dof,dof);
69  ColumnVector ltorque(dof);
70  ColumnVector dtorque(dof);
71  ColumnVector dq(dof);
72  for(i = 1; i <= dof; i++) {
73  for(j = 1; j <= dof; j++) {
74  dq(j) = (i == j ? 1.0 : 0.0);
75  }
76  dq_torque(q,qp,qpp,dq,ltorque,dtorque);
77  for(j = 1; j <= dof; j++) {
78  ldtau_dq(j,i) = dtorque(j);
79  }
80  }
81  ldtau_dq.Release(); return ldtau_dq;
82 }
83 
85  const ColumnVector & qp)
91 {
92  int i, j;
93  Matrix ldtau_dqp(dof,dof);
94  ColumnVector ltorque(dof);
95  ColumnVector dtorque(dof);
96  ColumnVector dqp(dof);
97  for(i = 1; i <= dof; i++) {
98  for(j = 1; j <= dof; j++) {
99  dqp(j) = (i == j ? 1.0 : 0.0);
100  }
101  dqp_torque(q,qp,dqp,ltorque,dtorque);
102  for(j = 1; j <= dof; j++) {
103  ldtau_dqp(j,i) = dtorque(j);
104  }
105  }
106  ldtau_dqp.Release(); return ldtau_dqp;
107 }
108 
109 #ifdef use_namespace
110 }
111 #endif
112 
113 
114 
void Release()
Definition: newmat.h:514
ReturnMatrix dtau_dq(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)
Sensitivity of the dynamics with respect to .
Definition: sensitiv.cpp:58
Robots class definitions.
static const char rcsid[]
RCS/CVS version.
Definition: sensitiv.cpp:48
The usual rectangular matrix.
Definition: newmat.h:625
Column vector.
Definition: newmat.h:1008
ReturnMatrix dtau_dqp(const ColumnVector &q, const ColumnVector &qp)
Sensitivity of the dynamics with respect to .
Definition: sensitiv.cpp:84


kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:16