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 |
REGISTER_UNARY_OP(VectorSelecter, Selec_of_vector); |
39 |
✓✗ |
1 |
REGISTER_UNARY_OP(VectorComponent, Component_of_vector); |
40 |
✓✗ |
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.") |