| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
/* |
||
2 |
* Copyright 2010, |
||
3 |
* François Bleibel, |
||
4 |
* Olivier Stasse, |
||
5 |
* |
||
6 |
* CNRS/AIST |
||
7 |
* |
||
8 |
*/ |
||
9 |
|||
10 |
#include <dynamic-graph/all-commands.h> |
||
11 |
#include <dynamic-graph/all-signals.h> |
||
12 |
#include <dynamic-graph/factory.h> |
||
13 |
|||
14 |
#include <sot/core/matrix-geometry.hh> |
||
15 |
#include <sot/core/op-point-modifier.hh> |
||
16 |
|||
17 |
using namespace std; |
||
18 |
using namespace dynamicgraph::sot; |
||
19 |
using namespace dynamicgraph; |
||
20 |
|||
21 |
✓✗ | 4 |
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(OpPointModifier, "OpPointModifier"); |
22 |
|||
23 |
/* --------------------------------------------------------------------- */ |
||
24 |
/* --------------------------------------------------------------------- */ |
||
25 |
/* --------------------------------------------------------------------- */ |
||
26 |
|||
27 |
4 |
OpPointModifier::OpPointModifier(const std::string &name) |
|
28 |
: Entity(name), |
||
29 |
jacobianSIN(NULL, |
||
30 |
✓✗ | 8 |
"OpPointModifior(" + name + ")::input(matrix)::jacobianIN"), |
31 |
positionSIN( |
||
32 |
✓✗ | 8 |
NULL, "OpPointModifior(" + name + ")::input(matrixhomo)::positionIN"), |
33 |
jacobianSOUT( |
||
34 |
boost::bind(&OpPointModifier::jacobianSOUT_function, this, _1, _2), |
||
35 |
jacobianSIN, |
||
36 |
✓✗ | 8 |
"OpPointModifior(" + name + ")::output(matrix)::jacobian"), |
37 |
positionSOUT( |
||
38 |
boost::bind(&OpPointModifier::positionSOUT_function, this, _1, _2), |
||
39 |
positionSIN, |
||
40 |
✓✗ | 8 |
"OpPointModifior(" + name + ")::output(matrixhomo)::position"), |
41 |
transformation(), |
||
42 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
36 |
isEndEffector(true) { |
43 |
sotDEBUGIN(15); |
||
44 |
|||
45 |
✓✗✓✗ |
8 |
signalRegistration(jacobianSIN << positionSIN << jacobianSOUT |
46 |
✓✗✓✗ |
4 |
<< positionSOUT); |
47 |
{ |
||
48 |
using namespace dynamicgraph::command; |
||
49 |
✓✗✓✗ |
4 |
addCommand( |
50 |
"getTransformation", |
||
51 |
✓✗ | 4 |
makeDirectGetter(*this, &transformation.matrix(), |
52 |
✓✗✓✗ ✓✗ |
8 |
docDirectGetter("transformation", "matrix 4x4 homo"))); |
53 |
✓✗✓✗ |
4 |
addCommand( |
54 |
"setTransformation", |
||
55 |
✓✗ | 4 |
makeDirectSetter(*this, &transformation.matrix(), |
56 |
✓✗✓✗ ✓✗ |
8 |
docDirectSetter("dimension", "matrix 4x4 homo"))); |
57 |
✓✗✓✗ |
4 |
addCommand("getEndEffector", |
58 |
✓✗ | 4 |
makeDirectGetter(*this, &isEndEffector, |
59 |
✓✗✓✗ ✓✗ |
8 |
docDirectGetter("end effector mode", "bool"))); |
60 |
✓✗✓✗ |
4 |
addCommand("setEndEffector", |
61 |
✓✗ | 4 |
makeDirectSetter(*this, &isEndEffector, |
62 |
✓✗✓✗ ✓✗ |
8 |
docDirectSetter("end effector mode", "bool"))); |
63 |
} |
||
64 |
|||
65 |
sotDEBUGOUT(15); |
||
66 |
4 |
} |
|
67 |
|||
68 |
4 |
dynamicgraph::Matrix &OpPointModifier::jacobianSOUT_function( |
|
69 |
dynamicgraph::Matrix &res, const int &iter) { |
||
70 |
✓✗ | 4 |
if (isEndEffector) { |
71 |
✓✗ | 4 |
const dynamicgraph::Matrix &aJa = jacobianSIN(iter); |
72 |
4 |
const MatrixHomogeneous &aMb = transformation; |
|
73 |
|||
74 |
✓✗ | 4 |
MatrixTwist bVa; |
75 |
✓✗✓✗ |
4 |
buildFrom(aMb.inverse(), bVa); |
76 |
✓✗✓✗ |
4 |
res = bVa * aJa; // res := bJb |
77 |
4 |
return res; |
|
78 |
} else { |
||
79 |
/* Consider that the jacobian of point A in frame A is given: J = aJa |
||
80 |
* and that homogenous transformation from A to B is given aMb in |
||
81 |
* getTransfo() and homo transfo from 0 to A is given oMa in positionSIN. |
||
82 |
* Then return oJb, the jacobian of point B expressed in frame O: |
||
83 |
* oJb = ( oRa 0 ; 0 oRa ) * bVa * aJa |
||
84 |
* = [ I skew(oAB); 0 I ] * oJa |
||
85 |
* with oAB = oRb bAB = oRb (-bRa aAB ) = -oRa aAB, and aAB = |
||
86 |
* translation(aMb). |
||
87 |
*/ |
||
88 |
|||
89 |
const dynamicgraph::Matrix &oJa = jacobianSIN(iter); |
||
90 |
const MatrixHomogeneous &aMb = transformation; |
||
91 |
const MatrixHomogeneous &oMa = positionSIN(iter); |
||
92 |
MatrixRotation oRa; |
||
93 |
oRa = oMa.linear(); |
||
94 |
dynamicgraph::Vector aAB(3); |
||
95 |
aAB = aMb.translation(); |
||
96 |
dynamicgraph::Vector oAB = oRa * aAB; |
||
97 |
|||
98 |
const dynamicgraph::Matrix::Index nq = oJa.cols(); |
||
99 |
res.resize(6, oJa.cols()); |
||
100 |
for (int j = 0; j < nq; ++j) { |
||
101 |
/* This is a I*Jtrans + skew*Jrot product, unrolled by hand ... */ |
||
102 |
res(0, j) = oJa(0, j) - oAB(1) * oJa(2 + 3, j) + oAB(2) * oJa(1 + 3, j); |
||
103 |
res(1, j) = oJa(1, j) - oAB(2) * oJa(0 + 3, j) + oAB(0) * oJa(2 + 3, j); |
||
104 |
res(2, j) = oJa(2, j) - oAB(0) * oJa(1 + 3, j) + oAB(1) * oJa(0 + 3, j); |
||
105 |
for (int i = 0; i < 3; ++i) { |
||
106 |
res(i + 3, j) = oJa(i + 3, j); |
||
107 |
} |
||
108 |
} |
||
109 |
return res; // res := 0Jb |
||
110 |
} |
||
111 |
} |
||
112 |
|||
113 |
4 |
MatrixHomogeneous &OpPointModifier::positionSOUT_function( |
|
114 |
MatrixHomogeneous &res, const int &iter) { |
||
115 |
sotDEBUGIN(15); |
||
116 |
sotDEBUGIN(15) << iter << " " << positionSIN.getTime() |
||
117 |
<< positionSOUT.getTime() << endl; |
||
118 |
4 |
const MatrixHomogeneous &position = positionSIN(iter); |
|
119 |
✓✗ | 4 |
res = position * transformation; |
120 |
sotDEBUGOUT(15); |
||
121 |
4 |
return res; |
|
122 |
} |
||
123 |
|||
124 |
void OpPointModifier::setTransformation(const Eigen::Matrix4d &tr) { |
||
125 |
transformation.matrix() = tr; |
||
126 |
} |
||
127 |
const Eigen::Matrix4d &OpPointModifier::getTransformation(void) { |
||
128 |
return transformation.matrix(); |
||
129 |
} |
||
130 |
|||
131 |
/* The following function needs an access to a specific signal via |
||
132 |
* the pool, using the signal path <entity.signal>. this functionnality |
||
133 |
* is deprecated, and the following function will have to be removed |
||
134 |
* in a near future. A similar functionality is available using |
||
135 |
* the <setTransformation> mthod, bound in python. |
||
136 |
*/ |
||
137 |
#include <dynamic-graph/pool.h> |
||
138 |
[[deprecated("use setTransformation")]] void |
||
139 |
OpPointModifier::setTransformationBySignalName(std::istringstream &cmdArgs) { |
||
140 |
Signal<Eigen::Matrix4d, int> &sig = |
||
141 |
dynamic_cast<Signal<Eigen::Matrix4d, int> &>( |
||
142 |
PoolStorage::getInstance()->getSignal(cmdArgs)); |
||
143 |
setTransformation(sig.accessCopy()); |
||
144 |
} |
| Generated by: GCOVR (Version 4.2) |