00001
00002
00003
00004
00005
00006
00007
00008
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 }