VirtualRobotPortHandler.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  * General Robotix Inc. 
00009  */
00015 #include "hrpUtil/Eigen3d.h"
00016 #include "VirtualRobotPortHandler.h"
00017 
00018 #include "Controller_impl.h"
00019 
00020 using namespace RTC;
00021 
00022 namespace {
00023 
00024   template <typename TSrc>
00025   void copyDblArraySeqToTimedDoubleSeq(const TSrc& src, int arraySize, TimedDoubleSeq& dest)
00026   {
00027     CORBA::ULong n = src.length();
00028     CORBA::ULong m = n * arraySize;
00029     dest.data.length(m);
00030     CORBA::ULong destPos = 0;
00031     for(CORBA::ULong i=0; i < n; ++i){
00032       for(int j=0; j < arraySize; ++j){
00033         dest.data[destPos++] = src[i][j];
00034       }
00035     }
00036   }
00037 
00038   template <typename TSrcSeq, typename TDestSeq>
00039   void copyImageData();
00040 
00041 
00042   DynamicsSimulator::LinkDataType toDynamicsSimulatorLinkDataType(DataTypeId id)
00043   {
00044     switch(id){
00045     case JOINT_VALUE:        return DynamicsSimulator::JOINT_VALUE;
00046     case JOINT_VELOCITY:     return DynamicsSimulator::JOINT_VELOCITY;
00047     case JOINT_ACCELERATION: return DynamicsSimulator::JOINT_ACCELERATION;
00048     case JOINT_TORQUE:       return DynamicsSimulator::JOINT_TORQUE;
00049     case EXTERNAL_FORCE:     return DynamicsSimulator::EXTERNAL_FORCE;
00050     case ABS_TRANSFORM:      return DynamicsSimulator::ABS_TRANSFORM;
00051     case ABS_TRANSFORM2:     return DynamicsSimulator::ABS_TRANSFORM;
00052     case ABS_VELOCITY:       return DynamicsSimulator::ABS_VELOCITY;
00053     case ABS_ACCELERATION:   return DynamicsSimulator::ABS_ACCELERATION;
00054     case CONSTRAINT_FORCE:   return DynamicsSimulator::CONSTRAINT_FORCE;
00055     default:                 return DynamicsSimulator::INVALID_DATA_TYPE;
00056     }
00057   }
00058 }
00059 
00060 PortHandler::~PortHandler()
00061 {
00062 
00063 }
00064 
00065 
00066 SensorStateOutPortHandler::SensorStateOutPortHandler(PortInfo& info) : 
00067   OutPortHandler(info), 
00068   outPort(info.portName.c_str(), values)
00069 {
00070   dataTypeId = info.dataTypeId;
00071   stepTime = info.stepTime;
00072 }
00073 
00074 
00075 void SensorStateOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
00076 {
00077   SensorState& state = controller->getCurrentSensorState();
00078 
00079   switch(dataTypeId) {
00080   case JOINT_VALUE:
00081     values.data = state.q;
00082     break;
00083   case JOINT_VELOCITY:
00084     values.data = state.dq;
00085     break;
00086   case JOINT_TORQUE:
00087     values.data = state.u;
00088     break;
00089   case FORCE_SENSOR:
00090     copyDblArraySeqToTimedDoubleSeq(state.force, 6, values);
00091     break;
00092   case RATE_GYRO_SENSOR:
00093     copyDblArraySeqToTimedDoubleSeq(state.rateGyro, 3, values);
00094     break;
00095   case ACCELERATION_SENSOR:
00096     copyDblArraySeqToTimedDoubleSeq(state.accel, 3, values);
00097     break;
00098   default:
00099     break;
00100   }
00101    setTime(values, controller->controlTime);
00102 }
00103 
00104 
00105 void SensorStateOutPortHandler::writeDataToPort()
00106 {
00107   outPort.write();
00108 }
00109 
00110 
00111 LinkDataOutPortHandler::LinkDataOutPortHandler(PortInfo& info) :
00112   OutPortHandler(info),
00113   outPort(info.portName.c_str(), value),
00114   linkName(info.dataOwnerName)
00115 {
00116   linkDataType = toDynamicsSimulatorLinkDataType(info.dataTypeId);
00117   stepTime = info.stepTime;
00118 }
00119 
00120 
00121 void LinkDataOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
00122 {
00123     size_t n;
00124     CORBA::ULong m=0;
00125     n = linkName.size();
00126     for(size_t i=0, k=0; i<n; i++){
00127         DblSequence_var data = controller->getLinkDataFromSimulator(linkName[i], linkDataType);       
00128         if(!i){
00129             m = data->length();
00130             value.data.length(n*m);
00131         }
00132         for(CORBA::ULong j=0; j < m; j++)
00133             value.data[k++] = data[j];  
00134     }  
00135     setTime(value, controller->controlTime);
00136 }
00137 
00138 
00139 void LinkDataOutPortHandler::writeDataToPort()
00140 {
00141   outPort.write();
00142 }
00143 
00144 AbsTransformOutPortHandler::AbsTransformOutPortHandler(PortInfo& info) :
00145   OutPortHandler(info),
00146   outPort(info.portName.c_str(), value),
00147   linkName(info.dataOwnerName)
00148 {
00149   linkDataType = toDynamicsSimulatorLinkDataType(info.dataTypeId);
00150   stepTime = info.stepTime;
00151 }
00152 
00153 
00154 void AbsTransformOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
00155 {
00156     DblSequence_var data = controller->getLinkDataFromSimulator(linkName[0], linkDataType);       
00157     value.data.position.x = data[0];
00158     value.data.position.y = data[1];
00159     value.data.position.z = data[2];
00160     hrp::Matrix33 R;
00161     R << data[3], data[4], data[5], 
00162         data[6], data[7], data[8], 
00163         data[9], data[10], data[11];
00164     hrp::Vector3 rpy = hrp::rpyFromRot(R);
00165     value.data.orientation.r = rpy[0];
00166     value.data.orientation.p = rpy[1];
00167     value.data.orientation.y = rpy[2];
00168     setTime(value, controller->controlTime);
00169 }
00170 
00171 
00172 void AbsTransformOutPortHandler::writeDataToPort()
00173 {
00174   outPort.write();
00175 }
00176 
00177 
00178 SensorDataOutPortHandler::SensorDataOutPortHandler(PortInfo& info) :
00179   OutPortHandler(info),
00180   outPort(info.portName.c_str(), value),
00181   sensorName(info.dataOwnerName)
00182 {
00183     stepTime = info.stepTime;
00184 }
00185 
00186 
00187 void SensorDataOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
00188 {
00189     size_t n;
00190     CORBA::ULong m=0;
00191     n = sensorName.size();
00192     for(size_t i=0, k=0; i<n; i++){
00193         DblSequence_var data = controller->getSensorDataFromSimulator(sensorName[i]);    
00194         if(!i){
00195             m = data->length();
00196             value.data.length(n*m);
00197         }
00198         for(CORBA::ULong j=0; j < m; j++)
00199             value.data[k++] = data[j];  
00200     }  
00201     setTime(value, controller->controlTime);
00202 }
00203 
00204 
00205 void SensorDataOutPortHandler::writeDataToPort()
00206 {
00207   outPort.write();
00208 }
00209 
00210 
00211 GyroSensorOutPortHandler::GyroSensorOutPortHandler(PortInfo& info) :
00212   OutPortHandler(info),
00213   outPort(info.portName.c_str(), value),
00214   sensorName(info.dataOwnerName)
00215 {
00216     stepTime = info.stepTime;
00217 }
00218 
00219 
00220 void GyroSensorOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
00221 {
00222     size_t n;
00223     n = sensorName.size();
00224     for(size_t i=0; i<n; i++){
00225         DblSequence_var data = controller->getSensorDataFromSimulator(sensorName[i]);    
00226         value.data.avx = data[0];
00227         value.data.avy = data[1];
00228         value.data.avz = data[2];
00229     }  
00230     setTime(value, controller->controlTime);
00231 }
00232 
00233 
00234 void GyroSensorOutPortHandler::writeDataToPort()
00235 {
00236   outPort.write();
00237 }
00238 
00239 
00240 AccelerationSensorOutPortHandler::AccelerationSensorOutPortHandler(PortInfo& info) :
00241   OutPortHandler(info),
00242   outPort(info.portName.c_str(), value),
00243   sensorName(info.dataOwnerName)
00244 {
00245     stepTime = info.stepTime;
00246 }
00247 
00248 
00249 void AccelerationSensorOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
00250 {
00251     size_t n;
00252     n = sensorName.size();
00253     for(size_t i=0; i<n; i++){
00254         DblSequence_var data = controller->getSensorDataFromSimulator(sensorName[i]);    
00255         value.data.ax = data[0];
00256         value.data.ay = data[1];
00257         value.data.az = data[2];
00258     }  
00259     setTime(value, controller->controlTime);
00260 }
00261 
00262 
00263 void AccelerationSensorOutPortHandler::writeDataToPort()
00264 {
00265   outPort.write();
00266 }
00267 
00268 
00269 ColorImageOutPortHandler::ColorImageOutPortHandler(PortInfo& info) :
00270   OutPortHandler(info),
00271   outPort(info.portName.c_str(), image),
00272   cameraId(info.dataOwnerId)
00273 {
00274     stepTime = info.stepTime;
00275 }
00276 
00277 
00278 void ColorImageOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
00279 {
00280   ImageData_var imageInput = controller->getCameraImageFromSimulator(cameraId);
00281   image.data = imageInput->longData;
00282   setTime(image, controller->controlTime);
00283 }
00284 
00285 
00286 void ColorImageOutPortHandler::writeDataToPort()
00287 {
00288   outPort.write();
00289 }
00290 
00291 
00292 GrayScaleImageOutPortHandler::GrayScaleImageOutPortHandler(PortInfo& info) : 
00293   OutPortHandler(info),
00294   outPort(info.portName.c_str(), image),
00295   cameraId(info.dataOwnerId)
00296 {
00297     stepTime = info.stepTime;
00298 }
00299 
00300 
00301 void GrayScaleImageOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
00302 {
00303   ImageData_var imageInput = controller->getCameraImageFromSimulator(cameraId);
00304   image.data = imageInput->octetData;
00305   setTime(image, controller->controlTime);
00306 }
00307 
00308 
00309 void GrayScaleImageOutPortHandler::writeDataToPort()
00310 {
00311   outPort.write();
00312 }
00313 
00314 
00315 DepthImageOutPortHandler::DepthImageOutPortHandler(PortInfo& info) :
00316   OutPortHandler(info),
00317   outPort(info.portName.c_str(), image),
00318   cameraId(info.dataOwnerId)
00319 {
00320     stepTime = info.stepTime;
00321 }
00322 
00323 
00324 void DepthImageOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
00325 {
00326   ImageData_var imageInput = controller->getCameraImageFromSimulator(cameraId);
00327   image.data = imageInput->floatData;
00328   setTime(image, controller->controlTime);
00329 }
00330 
00331 
00332 void DepthImageOutPortHandler::writeDataToPort()
00333 {
00334   outPort.write();
00335 }
00336 
00337 
00338 JointDataSeqInPortHandler::JointDataSeqInPortHandler(PortInfo& info) :
00339   InPortHandler(info),
00340   inPort(info.portName.c_str(), values)
00341 {
00342   linkDataType = toDynamicsSimulatorLinkDataType(info.dataTypeId);
00343 }
00344 
00345 
00346 void JointDataSeqInPortHandler::outputDataToSimulator(Controller_impl* controller)
00347 {
00348   controller->flushJointDataSeqToSimulator(linkDataType);
00349 }
00350 
00351 
00352 void JointDataSeqInPortHandler::readDataFromPort(Controller_impl* controller)
00353 {
00354   if( inPort.isNew() == false ){
00355           DblSequence& data = controller->getJointDataSeqRef(linkDataType);
00356           data.length(0);
00357       return;
00358   }
00359   inPort.read();
00360 
00361   DblSequence& data = controller->getJointDataSeqRef(linkDataType);
00362   
00363   CORBA::ULong n = values.data.length();
00364   data.length(n);
00365   for(CORBA::ULong i=0; i < n; ++i){
00366     data[i] = values.data[i];
00367   }
00368 }
00369 
00370 LinkDataInPortHandler::LinkDataInPortHandler(PortInfo& info) :
00371   InPortHandler(info),
00372   inPort(info.portName.c_str(), values),
00373   linkName(info.dataOwnerName)
00374 {
00375   linkDataType = toDynamicsSimulatorLinkDataType(info.dataTypeId);
00376 }
00377 
00378 
00379 void LinkDataInPortHandler::outputDataToSimulator(Controller_impl* controller)
00380 {
00381     if(!data.length())
00382         return;
00383     size_t n=linkName.size();
00384     CORBA::ULong m=data.length()/n;
00385     for(size_t i=0, k=0; i< n; i++){
00386         DblSequence data0;
00387         data0.length(m);
00388         for(CORBA::ULong j=0; j<m; j++)
00389             data0[j] = data[k++];
00390         controller->flushLinkDataToSimulator(linkName[i], linkDataType, data0);
00391     }
00392 
00393 }
00394 
00395 
00396 void LinkDataInPortHandler::readDataFromPort(Controller_impl* controller)
00397 {
00398   if( inPort.isNew() == false ){
00399       return;
00400   }
00401   inPort.read();
00402 
00403   CORBA::ULong n = values.data.length();
00404   data.length(n);
00405   for(CORBA::ULong i=0; i < n; ++i){
00406     data[i] = values.data[i];
00407   }
00408 }
00409 
00410 AbsTransformInPortHandler::AbsTransformInPortHandler(PortInfo& info) :
00411   InPortHandler(info),
00412   inPort(info.portName.c_str(), values),
00413   linkName(info.dataOwnerName)
00414 {
00415   linkDataType = toDynamicsSimulatorLinkDataType(info.dataTypeId);
00416 }
00417 
00418 
00419 void AbsTransformInPortHandler::outputDataToSimulator(Controller_impl* controller)
00420 {
00421     if(!data.length())
00422         return;
00423     controller->flushLinkDataToSimulator(linkName[0], linkDataType, data);
00424 }
00425 
00426 
00427 void AbsTransformInPortHandler::readDataFromPort(Controller_impl* controller)
00428 {
00429   if( inPort.isNew() == false ){
00430       return;
00431   }
00432   inPort.read();
00433 
00434   data.length(12);
00435   data[0] = values.data.position.x;
00436   data[1] = values.data.position.y;
00437   data[2] = values.data.position.z;
00438   hrp::Matrix33 R = hrp::rotFromRpy(values.data.orientation.r,
00439                                     values.data.orientation.p,
00440                                     values.data.orientation.y);
00441   for (int i=0; i<3; i++){
00442       for (int j=0; j<3; j++){
00443           data[3+i*3+j] = R(i,j);
00444       }
00445   }
00446 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:19