00001 #include "KDLTypesC.h"
00002 #include <rtt/transports/corba/CorbaConversion.hpp>
00003 #include <kdl/frames.hpp>
00004 #include <kdl/jacobian.hpp>
00005 #include <kdl/jntarray.hpp>
00006 #include <Eigen/Dense>
00007
00008 namespace RTT
00009 {
00010 namespace corba{
00011
00012 template<>
00013 struct AnyConversion< KDL::Vector >
00014 {
00015 typedef KDL::Corba::DoubleSequence CorbaType;
00016 typedef KDL::Vector StdType;
00017 static CorbaType toAny(const KDL::Vector& orig) {
00018 log(Debug)<< "Converting type 'KDL::Vector' to sequence<CORBA::Double>." <<endlog();
00019 CorbaType ret;
00020 ret.length( 3 );
00021 ret[0] = orig.x();
00022 ret[1] = orig.y();
00023 ret[2] = orig.z();
00024 return ret;
00025 }
00026
00027 static bool updateAny( StdType const& t, CORBA::Any& any ) {
00028 any <<= toAny( t );
00029 return true;
00030 }
00031
00032
00033 static bool update(const CORBA::Any& any, StdType& _value) {
00034 log(Debug)<< "update KDL::Vector" <<endlog();
00035 CorbaType* result;
00036 if ( any >>= result ) {
00037 return toStdType(_value,*result);
00038 }
00039 return false;
00040 }
00041
00042 static CORBA::Any_ptr createAny( const StdType& t ) {
00043 CORBA::Any_ptr ret = new CORBA::Any();
00044 *ret <<= toAny( t );
00045 return ret;
00046 }
00047
00048 static bool toCorbaType(CorbaType& cb, const StdType& tp){
00049 cb = toAny(tp);
00050 return true;
00051 }
00052
00053 static bool toStdType(StdType& tp, const CorbaType& cb){
00054 log(Debug)<< "Converting type sequence<CORBA::Double> to 'KDL::Vector'" <<endlog();
00055 tp.x(cb[0]);
00056 tp.y(cb[1]);
00057 tp.z(cb[2]);
00058 return true;
00059 }
00060
00061 };
00062
00063 template<>
00064 struct AnyConversion< KDL::Rotation >
00065 {
00066 typedef KDL::Corba::DoubleSequence CorbaType;
00067 typedef KDL::Rotation StdType;
00068 static CorbaType toAny(const KDL::Rotation& orig) {
00069 log(Debug)<< "Converting type 'KDL::Rotation' to sequence<CORBA::Double>." <<endlog();
00070 CorbaType ret;
00071 ret.length( 9 );
00072 ret[0] = orig(0,0);
00073 ret[1] = orig(0,1);
00074 ret[2] = orig(0,2);
00075 ret[3] = orig(1,0);
00076 ret[4] = orig(1,1);
00077 ret[5] = orig(1,2);
00078 ret[6] = orig(2,0);
00079 ret[7] = orig(2,1);
00080 ret[8] = orig(2,2);
00081 return ret;
00082 }
00083
00084 static bool toStdType(StdType& tp ,const CorbaType& cb) {
00085 log(Debug)<< "Converting type sequence<CORBA::Double> to 'KDL::Rotation'" <<endlog();
00086 tp = StdType(cb[0],cb[1],cb[2],cb[3],cb[4],cb[5],cb[6],cb[7],cb[8]);
00087 return true;
00088 }
00089
00090 static bool toCorbaType(CorbaType& cb, const StdType& tp){
00091 cb = toAny(tp);
00092 return true;
00093 }
00094
00095 static bool update(const CORBA::Any& any, StdType& _value) {
00096 log(Debug)<< "update KDL::Rotation" <<endlog();
00097 CorbaType* result;
00098 if ( any >>= result ) {
00099 return toStdType(_value,*result);
00100 }
00101 return false;
00102 }
00103
00104 static CORBA::Any_ptr createAny( const KDL::Rotation& t ) {
00105 CORBA::Any_ptr ret = new CORBA::Any();
00106 *ret <<= toAny( t );
00107 return ret;
00108 }
00109
00110 static bool updateAny( StdType const& t, CORBA::Any& any ) {
00111 any <<= toAny( t );
00112 return true;
00113 }
00114
00115 };
00116
00117 template<>
00118 struct AnyConversion< KDL::Frame >
00119 {
00120 typedef KDL::Corba::DoubleSequence CorbaType;
00121 typedef KDL::Frame StdType;
00122 static CorbaType toAny(const KDL::Frame& orig) {
00123 log(Debug)<< "Converting type 'KDL::Frame' to sequence<CORBA::Double>." <<endlog();
00124 CorbaType ret;
00125 ret.length( 12 );
00126 ret[0] = orig.p.x();
00127 ret[1] = orig.p.y();
00128 ret[2] = orig.p.z();
00129 ret[3] = orig.M(0,0);
00130 ret[4] = orig.M(0,1);
00131 ret[5] = orig.M(0,2);
00132 ret[6] = orig.M(1,0);
00133 ret[7] = orig.M(1,1);
00134 ret[8] = orig.M(1,2);
00135 ret[9] = orig.M(2,0);
00136 ret[10] = orig.M(2,1);
00137 ret[11] = orig.M(2,2);
00138 return ret;
00139 }
00140
00141 static bool updateAny( StdType const& t, CORBA::Any& any ) {
00142 any <<= toAny( t );
00143 return true;
00144 }
00145 static bool update(const CORBA::Any& any, StdType& _value) {
00146 log(Debug)<< "update KDL::Frame" <<endlog();
00147 CorbaType* result;
00148 if ( any >>= result ) {
00149 return toStdType(_value,*result);
00150 }
00151 return false;
00152 }
00153
00154 static CORBA::Any_ptr createAny( const StdType& t ) {
00155 CORBA::Any_ptr ret = new CORBA::Any();
00156 *ret <<= toAny( t );
00157 return ret;
00158 }
00159
00160 static bool toStdType(StdType& tp, const CorbaType& cb) {
00161 log(Debug)<< "Converting type sequence<CORBA::Double> to 'KDL::Frame'" <<endlog();
00162 tp.p.x(cb[0]);
00163 tp.p.y(cb[1]);
00164 tp.p.z(cb[2]);
00165 tp.M(0,0)=cb[3];
00166 tp.M(0,1)=cb[4];
00167 tp.M(0,2)=cb[5];
00168 tp.M(1,0)=cb[6];
00169 tp.M(1,1)=cb[7];
00170 tp.M(1,2)=cb[8];
00171 tp.M(2,0)=cb[9];
00172 tp.M(2,1)=cb[10];
00173 tp.M(2,2)=cb[11];
00174 return true;
00175 }
00176
00177 static bool toCorbaType(CorbaType& cb, const StdType& tp){
00178 cb = toAny(tp);
00179 return true;
00180 }
00181
00182 };
00183
00184 template<>
00185 struct AnyConversion< KDL::Wrench >
00186 {
00187 typedef KDL::Corba::DoubleSequence CorbaType;
00188 typedef KDL::Wrench StdType;
00189 static CorbaType toAny(const KDL::Wrench& orig) {
00190 log(Debug)<< "Converting type 'KDL::Wrench' to sequence<CORBA::Double>." <<endlog();
00191 CorbaType ret;
00192 ret.length( 6 );
00193 ret[0] = orig.force.x();
00194 ret[1] = orig.force.y();
00195 ret[2] = orig.force.z();
00196 ret[3] = orig.torque.x();
00197 ret[4] = orig.torque.y();
00198 ret[5] = orig.torque.z();
00199 return ret;
00200 }
00201
00202 static bool updateAny( StdType const& t, CORBA::Any& any ) {
00203 any <<= toAny( t );
00204 return true;
00205 }
00206
00207
00208 static bool update(const CORBA::Any& any, StdType& _value) {
00209 log(Debug)<< "update KDL::Vector" <<endlog();
00210 CorbaType* result;
00211 if ( any >>= result ) {
00212 return toStdType(_value,*result);
00213 }
00214 return false;
00215 }
00216
00217 static CORBA::Any_ptr createAny( const StdType& t ) {
00218 CORBA::Any_ptr ret = new CORBA::Any();
00219 *ret <<= toAny( t );
00220 return ret;
00221 }
00222
00223 static bool toStdType(StdType& tp, const CorbaType& cb){
00224 log(Debug)<< "Converting type sequence<CORBA::Double> to 'KDL::Wrench'" <<endlog();
00225 tp.force.x(cb[0]);
00226 tp.force.y(cb[1]);
00227 tp.force.z(cb[2]);
00228 tp.torque.x(cb[3]);
00229 tp.torque.y(cb[4]);
00230 tp.torque.z(cb[5]);
00231 return true;
00232 }
00233 static bool toCorbaType(CorbaType& cb, const StdType& tp){
00234 cb = toAny(tp);
00235 return true;
00236 }
00237
00238 };
00239
00240 template<>
00241 struct AnyConversion< KDL::Twist >
00242 {
00243 typedef KDL::Corba::DoubleSequence CorbaType;
00244 typedef KDL::Twist StdType;
00245 static CorbaType toAny(const KDL::Twist& orig) {
00246 log(Debug)<< "Converting type 'KDL::Twist' to sequence<CORBA::Double>." <<endlog();
00247 CorbaType ret;
00248 ret.length( 6 );
00249 ret[0] = orig.vel.x();
00250 ret[1] = orig.vel.y();
00251 ret[2] = orig.vel.z();
00252 ret[3] = orig.rot.x();
00253 ret[4] = orig.rot.y();
00254 ret[5] = orig.rot.z();
00255 return ret;
00256 }
00257
00258 static bool updateAny( StdType const& t, CORBA::Any& any ) {
00259 any <<= toAny( t );
00260 return true;
00261 }
00262
00263
00264 static bool update(const CORBA::Any& any, StdType& _value) {
00265 log(Debug)<< "update KDL::Vector" <<endlog();
00266 CorbaType* result;
00267 if ( any >>= result ) {
00268 return toStdType(_value,*result);
00269 }
00270 return false;
00271 }
00272
00273 static CORBA::Any_ptr createAny( const StdType& t ) {
00274 CORBA::Any_ptr ret = new CORBA::Any();
00275 *ret <<= toAny( t );
00276 return ret;
00277 }
00278
00279 static bool toCorbaType(CorbaType& cb, const StdType& tp){
00280 cb = toAny(tp);
00281 return true;
00282 }
00283
00284 static bool toStdType(StdType& tp, const CorbaType& cb){
00285 log(Debug)<< "Converting type sequence<CORBA::Double> to 'KDL::Twist'" <<endlog();
00286 tp.vel.x(cb[0]);
00287 tp.vel.y(cb[1]);
00288 tp.vel.z(cb[2]);
00289 tp.rot.x(cb[3]);
00290 tp.rot.y(cb[4]);
00291 tp.rot.z(cb[5]);
00292 return true;
00293 }
00294 };
00295 template<>
00296 struct AnyConversion< KDL::Jacobian >
00297 {
00298 typedef KDL::Corba::DoubleSequence CorbaType;
00299 typedef KDL::Jacobian StdType;
00300 static CorbaType* toAny(const StdType& tp) {
00301 CorbaType* cb = new CorbaType();
00302 toCorbaType(*cb, tp);
00303 return cb;
00304 }
00305
00306 static bool update(const CORBA::Any& any, StdType& _value) {
00307 CorbaType* result;
00308 if ( any >>= result ) {
00309 return toStdType(_value, *result);
00310 }
00311 return false;
00312 }
00313
00314 static CORBA::Any_ptr createAny( const StdType& t ) {
00315 CORBA::Any_ptr ret = new CORBA::Any();
00316 *ret <<= toAny( t );
00317 return ret;
00318 }
00319
00320 static bool updateAny( StdType const& t, CORBA::Any& any ) {
00321 any <<= toAny( t );
00322 return true;
00323 }
00324
00325 static bool toCorbaType(CorbaType& cb, const StdType& tp){
00326
00327 size_t rows = static_cast<size_t>(tp.data.rows());
00328 size_t cols = static_cast<size_t>(tp.data.cols());
00329
00330 cb.length( (CORBA::ULong)(tp.data.size() + 2) );
00331
00332 cb[0] = static_cast<double>(rows);
00333 cb[1] = static_cast<double>(cols);
00334
00335 Eigen::Map<Eigen::MatrixXd>(cb.get_buffer() + 2 , rows, cols) = tp.data;
00336
00337 return true;
00338 }
00339
00340 static bool toStdType(StdType& tp, const CorbaType& cb){
00341 if(cb.length() < 2){
00342 return false;
00343 }
00344
00345 size_t rows = static_cast<size_t>(cb[0]);
00346 size_t cols = static_cast<size_t>(cb[1]);
00347
00348 tp.resize(cols);
00349
00350 tp.data = Eigen::MatrixXd::Map(cb.get_buffer()+2,rows,cols);
00351 return true;
00352 }
00353
00354 };
00355
00356 template<>
00357 struct AnyConversion< KDL::JntArray >
00358 {
00359 typedef KDL::Corba::DoubleSequence CorbaType;
00360 typedef KDL::JntArray StdType;
00361
00362 static CorbaType* toAny(const StdType& tp) {
00363 CorbaType* cb = new CorbaType();
00364 toCorbaType(*cb, tp);
00365 return cb;
00366 }
00367
00368 static bool update(const CORBA::Any& any, StdType& _value) {
00369 CorbaType* result;
00370 if ( any >>= result ) {
00371 return toStdType(_value, *result);
00372 }
00373 return false;
00374 }
00375
00376 static CORBA::Any_ptr createAny( const StdType& t ) {
00377 CORBA::Any_ptr ret = new CORBA::Any();
00378 *ret <<= toAny( t );
00379 return ret;
00380 }
00381
00382 static bool updateAny( StdType const& t, CORBA::Any& any ) {
00383 any <<= toAny( t );
00384 return true;
00385 }
00386
00387 static bool toCorbaType(CorbaType& cb, const StdType& tp){
00388 cb.length( (CORBA::ULong)(tp.rows() ));
00389 Eigen::Map<Eigen::VectorXd>(cb.get_buffer() , cb.length()) = tp.data;
00390 return true;
00391 }
00392
00393 static bool toStdType(StdType& tp, const CorbaType& cb){
00394 tp.resize( cb.length() );
00395 tp.data = Eigen::VectorXd::Map(cb.get_buffer() , cb.length());
00396 return true;
00397 }
00398 };
00399 }
00400 }
00401