operator.cpp
Go to the documentation of this file.
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), &regFunction_##name)
27 
28 /* ---------------------------------------------------------------------------*/
29 /* ---------------------------------------------------------------------------*/
30 /* ---------------------------------------------------------------------------*/
31 
32 namespace dynamicgraph {
33 namespace sot {
34 
35 /* ---------------------------------------------------------------------- */
36 /* --- ALGEBRA SELECTORS ------------------------------------------------ */
37 /* ---------------------------------------------------------------------- */
38 REGISTER_UNARY_OP(VectorSelecter, Selec_of_vector);
39 REGISTER_UNARY_OP(VectorComponent, Component_of_vector);
40 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);
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), &regFunction_##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), &regFunction_##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 std::size_t s = std::max( v1.size(),v2.size() );
190 // res.resize(s);
191 // for( std::size_t 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.")
dynamicgraph::sot::Multiplier_matrixTwist_vector
Multiplier_FxE__E< MatrixTwist, dynamicgraph::Vector > Multiplier_matrixTwist_vector
Definition: operator.hh:584
dynamicgraph
dynamicgraph::sot::Multiplier_double_vector
Multiplier_FxE__E< double, dynamicgraph::Vector > Multiplier_double_vector
Definition: operator.hh:578
dynamicgraph::sot::Multiplier_matrixHomo_vector
Multiplier_FxE__E< MatrixHomogeneous, dynamicgraph::Vector > Multiplier_matrixHomo_vector
Definition: operator.hh:582
dynamicgraph::sot::REGISTER_VARIADIC_OP
REGISTER_VARIADIC_OP(VectorMix, Mix_of_vector)
dynamicgraph::sot::REGISTER_BINARY_OP
REGISTER_BINARY_OP(Multiplier_double_vector, Multiply_double_vector)
dynamicgraph::sot::REGISTER_UNARY_OP
REGISTER_UNARY_OP(VectorSelecter, Selec_of_vector)
dynamicgraph::sot::Multiplier_matrix_vector
Multiplier_FxE__E< dynamicgraph::Matrix, dynamicgraph::Vector > Multiplier_matrix_vector
Definition: operator.hh:580
operator.hh
dynamic_graph.sot.core.matrix_util.RPYToMatrix
def RPYToMatrix(pr)
Definition: matrix_util.py:99


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Tue Oct 24 2023 02:26:31