Directory: | ./ |
---|---|
File: | src/matrix/operator.cpp |
Date: | 2024-08-13 12:13:25 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 3 | 63 | 4.8% |
Branches: | 3 | 126 | 2.4% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | /* | ||
2 | * Copyright 2010, | ||
3 | * François Bleibel, | ||
4 | * Olivier Stasse, | ||
5 | * Nicolas Mansard | ||
6 | * Joseph Mirabel | ||
7 | * | ||
8 | * CNRS/AIST | ||
9 | * | ||
10 | */ | ||
11 | |||
12 | #include "operator.hh" | ||
13 | |||
14 | namespace dg = ::dynamicgraph; | ||
15 | |||
16 | /* ---------------------------------------------------------------------------*/ | ||
17 | /* ------- GENERIC HELPERS -------------------------------------------------- */ | ||
18 | /* ---------------------------------------------------------------------------*/ | ||
19 | |||
20 | #define REGISTER_UNARY_OP(OpType, name) \ | ||
21 | template <> \ | ||
22 | const std::string UnaryOp<OpType>::CLASS_NAME = std::string(#name); \ | ||
23 | Entity *regFunction_##name(const std::string &objname) { \ | ||
24 | return new UnaryOp<OpType>(objname); \ | ||
25 | } \ | ||
26 | EntityRegisterer regObj_##name(std::string(#name), ®Function_##name) | ||
27 | |||
28 | /* ---------------------------------------------------------------------------*/ | ||
29 | /* ---------------------------------------------------------------------------*/ | ||
30 | /* ---------------------------------------------------------------------------*/ | ||
31 | |||
32 | namespace dynamicgraph { | ||
33 | namespace sot { | ||
34 | |||
35 | /* ---------------------------------------------------------------------- */ | ||
36 | /* --- ALGEBRA SELECTORS ------------------------------------------------ */ | ||
37 | /* ---------------------------------------------------------------------- */ | ||
38 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | REGISTER_UNARY_OP(VectorSelecter, Selec_of_vector); |
39 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | REGISTER_UNARY_OP(VectorComponent, Component_of_vector); |
40 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | REGISTER_UNARY_OP(MatrixSelector, Selec_of_matrix); |
41 | ✗ | REGISTER_UNARY_OP(MatrixColumnSelector, Selec_column_of_matrix); | |
42 | ✗ | REGISTER_UNARY_OP(MatrixTranspose, MatrixTranspose); | |
43 | ✗ | REGISTER_UNARY_OP(Diagonalizer, MatrixDiagonal); | |
44 | |||
45 | /* ---------------------------------------------------------------------- */ | ||
46 | /* --- INVERSION -------------------------------------------------------- */ | ||
47 | /* ---------------------------------------------------------------------- */ | ||
48 | |||
49 | ✗ | REGISTER_UNARY_OP(Inverser<Matrix>, Inverse_of_matrix); | |
50 | ✗ | REGISTER_UNARY_OP(Inverser<MatrixHomogeneous>, Inverse_of_matrixHomo); | |
51 | ✗ | REGISTER_UNARY_OP(Inverser<MatrixTwist>, Inverse_of_matrixtwist); | |
52 | ✗ | REGISTER_UNARY_OP(Normalize, Norm_of_vector); | |
53 | ✗ | REGISTER_UNARY_OP(InverserRotation, Inverse_of_matrixrotation); | |
54 | ✗ | REGISTER_UNARY_OP(InverserQuaternion, Inverse_of_unitquat); | |
55 | |||
56 | /* ----------------------------------------------------------------------- */ | ||
57 | /* --- SE3/SO3 conversions ----------------------------------------------- */ | ||
58 | /* ----------------------------------------------------------------------- */ | ||
59 | |||
60 | ✗ | REGISTER_UNARY_OP(MatrixHomoToPoseUTheta, MatrixHomoToPoseUTheta); | |
61 | ✗ | REGISTER_UNARY_OP(MatrixHomoToSE3Vector, MatrixHomoToSE3Vector); | |
62 | ✗ | REGISTER_UNARY_OP(SE3VectorToMatrixHomo, SE3VectorToMatrixHomo); | |
63 | ✗ | REGISTER_UNARY_OP(SkewSymToVector, SkewSymToVector); | |
64 | ✗ | REGISTER_UNARY_OP(PoseUThetaToMatrixHomo, PoseUThetaToMatrixHomo); | |
65 | ✗ | REGISTER_UNARY_OP(MatrixHomoToPoseQuaternion, MatrixHomoToPoseQuaternion); | |
66 | ✗ | REGISTER_UNARY_OP(PoseQuaternionToMatrixHomo, PoseQuaternionToMatrixHomo); | |
67 | ✗ | REGISTER_UNARY_OP(MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseRollPitchYaw); | |
68 | ✗ | REGISTER_UNARY_OP(PoseRollPitchYawToMatrixHomo, PoseRollPitchYawToMatrixHomo); | |
69 | ✗ | REGISTER_UNARY_OP(PoseRollPitchYawToPoseUTheta, PoseRollPitchYawToPoseUTheta); | |
70 | ✗ | REGISTER_UNARY_OP(HomoToMatrix, HomoToMatrix); | |
71 | ✗ | REGISTER_UNARY_OP(MatrixToHomo, MatrixToHomo); | |
72 | ✗ | REGISTER_UNARY_OP(HomoToTwist, HomoToTwist); | |
73 | ✗ | REGISTER_UNARY_OP(HomoToRotation, HomoToRotation); | |
74 | ✗ | REGISTER_UNARY_OP(MatrixHomoToPose, MatrixHomoToPose); | |
75 | ✗ | REGISTER_UNARY_OP(RPYToMatrix, RPYToMatrix); | |
76 | ✗ | REGISTER_UNARY_OP(MatrixToRPY, MatrixToRPY); | |
77 | ✗ | REGISTER_UNARY_OP(RPYToQuaternion, RPYToQuaternion); | |
78 | ✗ | REGISTER_UNARY_OP(QuaternionToRPY, QuaternionToRPY); | |
79 | ✗ | REGISTER_UNARY_OP(QuaternionToMatrix, QuaternionToMatrix); | |
80 | ✗ | REGISTER_UNARY_OP(MatrixToQuaternion, MatrixToQuaternion); | |
81 | ✗ | REGISTER_UNARY_OP(MatrixToUTheta, MatrixToUTheta); | |
82 | ✗ | REGISTER_UNARY_OP(UThetaToQuaternion, UThetaToQuaternion); | |
83 | |||
84 | /* ---------------------------------------------------------------------------*/ | ||
85 | /* ---------------------------------------------------------------------------*/ | ||
86 | /* ---------------------------------------------------------------------------*/ | ||
87 | |||
88 | #define REGISTER_BINARY_OP(OpType, name) \ | ||
89 | template <> \ | ||
90 | const std::string BinaryOp<OpType>::CLASS_NAME = std::string(#name); \ | ||
91 | Entity *regFunction_##name(const std::string &objname) { \ | ||
92 | return new BinaryOp<OpType>(objname); \ | ||
93 | } \ | ||
94 | EntityRegisterer regObj_##name(std::string(#name), ®Function_##name) | ||
95 | |||
96 | /* --- MULTIPLICATION --------------------------------------------------- */ | ||
97 | |||
98 | ✗ | REGISTER_BINARY_OP(Multiplier_double_vector, Multiply_double_vector); | |
99 | ✗ | REGISTER_BINARY_OP(Multiplier_matrix_vector, Multiply_matrix_vector); | |
100 | ✗ | REGISTER_BINARY_OP(Multiplier_matrixHomo_vector, Multiply_matrixHomo_vector); | |
101 | ✗ | REGISTER_BINARY_OP(Multiplier_matrixTwist_vector, Multiply_matrixTwist_vector); | |
102 | |||
103 | /* --- SUBSTRACTION ----------------------------------------------------- */ | ||
104 | ✗ | REGISTER_BINARY_OP(Substraction<dynamicgraph::Matrix>, Substract_of_matrix); | |
105 | ✗ | REGISTER_BINARY_OP(Substraction<dynamicgraph::Vector>, Substract_of_vector); | |
106 | ✗ | REGISTER_BINARY_OP(Substraction<double>, Substract_of_double); | |
107 | |||
108 | /* --- STACK ------------------------------------------------------------ */ | ||
109 | ✗ | REGISTER_BINARY_OP(VectorStack, Stack_of_vector); | |
110 | |||
111 | /* ---------------------------------------------------------------------- */ | ||
112 | |||
113 | ✗ | REGISTER_BINARY_OP(Composer, Compose_R_and_T); | |
114 | |||
115 | /* --- CONVOLUTION PRODUCT ---------------------------------------------- */ | ||
116 | ✗ | REGISTER_BINARY_OP(ConvolutionTemporal, ConvolutionTemporal); | |
117 | |||
118 | /* --- BOOLEAN REDUCTION ------------------------------------------------ */ | ||
119 | ✗ | REGISTER_BINARY_OP(Comparison<double>, CompareDouble); | |
120 | ✗ | REGISTER_BINARY_OP(MatrixComparison<Vector>, CompareVector); | |
121 | |||
122 | ✗ | REGISTER_BINARY_OP(WeightedAdder<dynamicgraph::Matrix>, WeightAdd_of_matrix); | |
123 | ✗ | REGISTER_BINARY_OP(WeightedAdder<dynamicgraph::Vector>, WeightAdd_of_vector); | |
124 | ✗ | REGISTER_BINARY_OP(WeightedAdder<double>, WeightAdd_of_double); | |
125 | |||
126 | #define REGISTER_VARIADIC_OP(OpType, name) \ | ||
127 | template <> \ | ||
128 | const std::string VariadicOp<OpType>::CLASS_NAME = std::string(#name); \ | ||
129 | Entity *regFunction_##name(const std::string &objname) { \ | ||
130 | return new VariadicOp<OpType>(objname); \ | ||
131 | } \ | ||
132 | EntityRegisterer regObj_##name(std::string(#name), ®Function_##name) | ||
133 | |||
134 | /* --- VectorMix ------------------------------------------------------------ */ | ||
135 | ✗ | REGISTER_VARIADIC_OP(VectorMix, Mix_of_vector); | |
136 | |||
137 | /* --- ADDITION --------------------------------------------------------- */ | ||
138 | ✗ | REGISTER_VARIADIC_OP(AdderVariadic<Matrix>, Add_of_matrix); | |
139 | ✗ | REGISTER_VARIADIC_OP(AdderVariadic<Vector>, Add_of_vector); | |
140 | ✗ | REGISTER_VARIADIC_OP(AdderVariadic<double>, Add_of_double); | |
141 | |||
142 | /* --- MULTIPLICATION --------------------------------------------------- */ | ||
143 | ✗ | REGISTER_VARIADIC_OP(Multiplier<Matrix>, Multiply_of_matrix); | |
144 | ✗ | REGISTER_VARIADIC_OP(Multiplier<Vector>, Multiply_of_vector); | |
145 | ✗ | REGISTER_VARIADIC_OP(Multiplier<MatrixRotation>, Multiply_of_matrixrotation); | |
146 | ✗ | REGISTER_VARIADIC_OP(Multiplier<MatrixHomogeneous>, Multiply_of_matrixHomo); | |
147 | ✗ | REGISTER_VARIADIC_OP(Multiplier<MatrixTwist>, Multiply_of_matrixtwist); | |
148 | ✗ | REGISTER_VARIADIC_OP(Multiplier<VectorQuaternion>, Multiply_of_quaternion); | |
149 | ✗ | REGISTER_VARIADIC_OP(Multiplier<double>, Multiply_of_double); | |
150 | |||
151 | /* --- BOOLEAN --------------------------------------------------------- */ | ||
152 | ✗ | REGISTER_VARIADIC_OP(BoolOp<0>, And); | |
153 | ✗ | REGISTER_VARIADIC_OP(BoolOp<1>, Or); | |
154 | |||
155 | } // namespace sot | ||
156 | } // namespace dynamicgraph | ||
157 | |||
158 | /* --- TODO ------------------------------------------------------------------*/ | ||
159 | // The following commented lines are sot-v1 entities that are still waiting | ||
160 | // for conversion. Help yourself! | ||
161 | |||
162 | // /* -------------------------------------------------------------------------- | ||
163 | // */ | ||
164 | |||
165 | // struct WeightedDirection | ||
166 | // { | ||
167 | // public: | ||
168 | // void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Vector& | ||
169 | // v2,dynamicgraph::Vector& res ) const | ||
170 | // { | ||
171 | // const double norm1 = v1.norm(); | ||
172 | // const double norm2 = v2.norm(); | ||
173 | // res=v2; res*=norm1; | ||
174 | // res*= (1/norm2); | ||
175 | // } | ||
176 | // }; | ||
177 | // typedef BinaryOp< Vector,Vector,Vector,WeightedDirection > weightdir; | ||
178 | // SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_ExE_E(weightdir,vector,weight_dir,"WeightDir") | ||
179 | |||
180 | // /* -------------------------------------------------------------------------- | ||
181 | // */ | ||
182 | |||
183 | // struct Nullificator | ||
184 | // { | ||
185 | // public: | ||
186 | // void operator()( const dynamicgraph::Vector& v1,const dynamicgraph::Vector& | ||
187 | // v2,dynamicgraph::Vector& res ) const | ||
188 | // { | ||
189 | // const unsigned int s = std::max( v1.size(),v2.size() ); | ||
190 | // res.resize(s); | ||
191 | // for( unsigned int i=0;i<s;++i ) | ||
192 | // { | ||
193 | // if( v1(i)>v2(i) ) res(i)=v1(i)-v2(i); | ||
194 | // else if( v1(i)<-v2(i) ) res(i)=v1(i)+v2(i); | ||
195 | // else res(i)=0; | ||
196 | // } | ||
197 | // } | ||
198 | // }; | ||
199 | // typedef BinaryOp< Vector,Vector,Vector,Nullificator > vectNil; | ||
200 | // SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_ExE_E(vectNil,vector,vectnil_,"Nullificator") | ||
201 | |||
202 | // /* -------------------------------------------------------------------------- | ||
203 | // */ | ||
204 | |||
205 | // struct VirtualSpring | ||
206 | // { | ||
207 | // public: | ||
208 | // double spring; | ||
209 | |||
210 | // void operator()( const dynamicgraph::Vector& pos,const | ||
211 | // dynamicgraph::Vector& ref,dynamicgraph::Vector& res ) const | ||
212 | // { | ||
213 | // double norm = ref.norm(); | ||
214 | // double dist = ref.scalarProduct(pos) / (norm*norm); | ||
215 | |||
216 | // res.resize( ref.size() ); | ||
217 | // res = ref; res *= dist; res -= pos; | ||
218 | // res *= spring; | ||
219 | // } | ||
220 | // }; | ||
221 | // typedef BinaryOp< Vector,Vector,Vector,VirtualSpring > virtspring; | ||
222 | // SOT_FACTORY_TEMPLATE_ENTITY_PLUGIN_ExE_E_CMD | ||
223 | // (virtspring,vector,virtspring_, | ||
224 | // "VirtualSpring" | ||
225 | // ,else if( cmdLine=="spring" ){ CMDARGS_INOUT(op.spring); } | ||
226 | // ,"VirtualSpring<pos,ref> compute the virtual force of a spring attache " | ||
227 | // "to the reference line <ref>. The eq is: k.(<ref|pos>/<ref|ref>.ref-pos)" | ||
228 | // "Params:\n - spring: get/set the spring factor.") | ||
229 |