Directory: | ./ |
---|---|
File: | src/math/op-point-modifier.cpp |
Date: | 2024-08-13 12:13:25 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 39 | 64 | 60.9% |
Branches: | 56 | 180 | 31.1% |
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 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(OpPointModifier, "OpPointModifier"); |
22 | |||
23 | /* --------------------------------------------------------------------- */ | ||
24 | /* --------------------------------------------------------------------- */ | ||
25 | /* --------------------------------------------------------------------- */ | ||
26 | |||
27 | 4 | OpPointModifier::OpPointModifier(const std::string &name) | |
28 | : Entity(name), | ||
29 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | jacobianSIN(NULL, |
30 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | "OpPointModifior(" + name + ")::input(matrix)::jacobianIN"), |
31 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | positionSIN( |
32 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
8 | NULL, "OpPointModifior(" + name + ")::input(matrixhomo)::positionIN"), |
33 |
4/8✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
|
4 | jacobianSOUT( |
34 | boost::bind(&OpPointModifier::jacobianSOUT_function, this, _1, _2), | ||
35 | jacobianSIN, | ||
36 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
8 | "OpPointModifior(" + name + ")::output(matrix)::jacobian"), |
37 |
4/8✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
|
4 | positionSOUT( |
38 | boost::bind(&OpPointModifier::positionSOUT_function, this, _1, _2), | ||
39 | positionSIN, | ||
40 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
8 | "OpPointModifior(" + name + ")::output(matrixhomo)::position"), |
41 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | transformation(), |
42 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | isEndEffector(true) { |
43 | sotDEBUGIN(15); | ||
44 | |||
45 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
8 | signalRegistration(jacobianSIN << positionSIN << jacobianSOUT |
46 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | << positionSOUT); |
47 | { | ||
48 | using namespace dynamicgraph::command; | ||
49 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | addCommand( |
50 | "getTransformation", | ||
51 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | makeDirectGetter(*this, &transformation.matrix(), |
52 |
3/6✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
|
8 | docDirectGetter("transformation", "matrix 4x4 homo"))); |
53 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | addCommand( |
54 | "setTransformation", | ||
55 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
4 | makeDirectSetter(*this, &transformation.matrix(), |
56 |
3/6✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
|
8 | docDirectSetter("dimension", "matrix 4x4 homo"))); |
57 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | addCommand("getEndEffector", |
58 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | makeDirectGetter(*this, &isEndEffector, |
59 |
3/6✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
|
8 | docDirectGetter("end effector mode", "bool"))); |
60 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | addCommand("setEndEffector", |
61 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | makeDirectSetter(*this, &isEndEffector, |
62 |
3/6✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
|
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 |
1/2✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
|
4 | if (isEndEffector) { |
71 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | const dynamicgraph::Matrix &aJa = jacobianSIN(iter); |
72 | 4 | const MatrixHomogeneous &aMb = transformation; | |
73 | |||
74 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
4 | MatrixTwist bVa; |
75 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
4 | buildFrom(aMb.inverse(), bVa); |
76 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
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 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
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 | } | ||
145 |