VirtualRobotPortHandler.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
 * All rights reserved. This program is made available under the terms of the
 * Eclipse Public License v1.0 which accompanies this distribution, and is
 * available at http://www.eclipse.org/legal/epl-v10.html
 * Contributors:
 * National Institute of Advanced Industrial Science and Technology (AIST)
 * General Robotix Inc. 
 */
#include "hrpUtil/Eigen3d.h"
#include "VirtualRobotPortHandler.h"

#include "Controller_impl.h"

using namespace RTC;

namespace {

  template <typename TSrc>
  void copyDblArraySeqToTimedDoubleSeq(const TSrc& src, int arraySize, TimedDoubleSeq& dest)
  {
    CORBA::ULong n = src.length();
    CORBA::ULong m = n * arraySize;
    dest.data.length(m);
    CORBA::ULong destPos = 0;
    for(CORBA::ULong i=0; i < n; ++i){
      for(int j=0; j < arraySize; ++j){
        dest.data[destPos++] = src[i][j];
      }
    }
  }

  template <typename TSrcSeq, typename TDestSeq>
  void copyImageData();


  DynamicsSimulator::LinkDataType toDynamicsSimulatorLinkDataType(DataTypeId id)
  {
    switch(id){
    case JOINT_VALUE:        return DynamicsSimulator::JOINT_VALUE;
    case JOINT_VELOCITY:     return DynamicsSimulator::JOINT_VELOCITY;
    case JOINT_ACCELERATION: return DynamicsSimulator::JOINT_ACCELERATION;
    case JOINT_TORQUE:       return DynamicsSimulator::JOINT_TORQUE;
    case EXTERNAL_FORCE:     return DynamicsSimulator::EXTERNAL_FORCE;
    case ABS_TRANSFORM:      return DynamicsSimulator::ABS_TRANSFORM;
    case ABS_TRANSFORM2:     return DynamicsSimulator::ABS_TRANSFORM;
    case ABS_VELOCITY:       return DynamicsSimulator::ABS_VELOCITY;
    case ABS_ACCELERATION:   return DynamicsSimulator::ABS_ACCELERATION;
    case CONSTRAINT_FORCE:   return DynamicsSimulator::CONSTRAINT_FORCE;
    default:                 return DynamicsSimulator::INVALID_DATA_TYPE;
    }
  }
}

PortHandler::~PortHandler()
{

}


SensorStateOutPortHandler::SensorStateOutPortHandler(PortInfo& info) : 
  OutPortHandler(info), 
  outPort(info.portName.c_str(), values)
{
  dataTypeId = info.dataTypeId;
  stepTime = info.stepTime;
}


void SensorStateOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
{
  SensorState& state = controller->getCurrentSensorState();

  switch(dataTypeId) {
  case JOINT_VALUE:
    values.data = state.q;
    break;
  case JOINT_VELOCITY:
    values.data = state.dq;
    break;
  case JOINT_TORQUE:
    values.data = state.u;
    break;
  case FORCE_SENSOR:
    copyDblArraySeqToTimedDoubleSeq(state.force, 6, values);
    break;
  case RATE_GYRO_SENSOR:
    copyDblArraySeqToTimedDoubleSeq(state.rateGyro, 3, values);
    break;
  case ACCELERATION_SENSOR:
    copyDblArraySeqToTimedDoubleSeq(state.accel, 3, values);
    break;
  default:
    break;
  }
   setTime(values, controller->controlTime);
}


void SensorStateOutPortHandler::writeDataToPort()
{
  outPort.write();
}


LinkDataOutPortHandler::LinkDataOutPortHandler(PortInfo& info) :
  OutPortHandler(info),
  outPort(info.portName.c_str(), value),
  linkName(info.dataOwnerName)
{
  linkDataType = toDynamicsSimulatorLinkDataType(info.dataTypeId);
  stepTime = info.stepTime;
}


void LinkDataOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
{
    size_t n;
    CORBA::ULong m=0;
    n = linkName.size();
    for(size_t i=0, k=0; i<n; i++){
        DblSequence_var data = controller->getLinkDataFromSimulator(linkName[i], linkDataType);       
        if(!i){
            m = data->length();
            value.data.length(n*m);
        }
        for(CORBA::ULong j=0; j < m; j++)
            value.data[k++] = data[j];  
    }  
    setTime(value, controller->controlTime);
}


void LinkDataOutPortHandler::writeDataToPort()
{
  outPort.write();
}

AbsTransformOutPortHandler::AbsTransformOutPortHandler(PortInfo& info) :
  OutPortHandler(info),
  outPort(info.portName.c_str(), value),
  linkName(info.dataOwnerName)
{
  linkDataType = toDynamicsSimulatorLinkDataType(info.dataTypeId);
  stepTime = info.stepTime;
}


void AbsTransformOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
{
    DblSequence_var data = controller->getLinkDataFromSimulator(linkName[0], linkDataType);       
    value.data.position.x = data[0];
    value.data.position.y = data[1];
    value.data.position.z = data[2];
    hrp::Matrix33 R;
    R << data[3], data[4], data[5], 
        data[6], data[7], data[8], 
        data[9], data[10], data[11];
    hrp::Vector3 rpy = hrp::rpyFromRot(R);
    value.data.orientation.r = rpy[0];
    value.data.orientation.p = rpy[1];
    value.data.orientation.y = rpy[2];
    setTime(value, controller->controlTime);
}


void AbsTransformOutPortHandler::writeDataToPort()
{
  outPort.write();
}


SensorDataOutPortHandler::SensorDataOutPortHandler(PortInfo& info) :
  OutPortHandler(info),
  outPort(info.portName.c_str(), value),
  sensorName(info.dataOwnerName)
{
    stepTime = info.stepTime;
}


void SensorDataOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
{
    size_t n;
    CORBA::ULong m=0;
    n = sensorName.size();
    for(size_t i=0, k=0; i<n; i++){
        DblSequence_var data = controller->getSensorDataFromSimulator(sensorName[i]);    
        if(!i){
            m = data->length();
            value.data.length(n*m);
        }
        for(CORBA::ULong j=0; j < m; j++)
            value.data[k++] = data[j];  
    }  
    setTime(value, controller->controlTime);
}


void SensorDataOutPortHandler::writeDataToPort()
{
  outPort.write();
}


GyroSensorOutPortHandler::GyroSensorOutPortHandler(PortInfo& info) :
  OutPortHandler(info),
  outPort(info.portName.c_str(), value),
  sensorName(info.dataOwnerName)
{
    stepTime = info.stepTime;
}


void GyroSensorOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
{
    size_t n;
    n = sensorName.size();
    for(size_t i=0; i<n; i++){
        DblSequence_var data = controller->getSensorDataFromSimulator(sensorName[i]);    
        value.data.avx = data[0];
        value.data.avy = data[1];
        value.data.avz = data[2];
    }  
    setTime(value, controller->controlTime);
}


void GyroSensorOutPortHandler::writeDataToPort()
{
  outPort.write();
}


AccelerationSensorOutPortHandler::AccelerationSensorOutPortHandler(PortInfo& info) :
  OutPortHandler(info),
  outPort(info.portName.c_str(), value),
  sensorName(info.dataOwnerName)
{
    stepTime = info.stepTime;
}


void AccelerationSensorOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
{
    size_t n;
    n = sensorName.size();
    for(size_t i=0; i<n; i++){
        DblSequence_var data = controller->getSensorDataFromSimulator(sensorName[i]);    
        value.data.ax = data[0];
        value.data.ay = data[1];
        value.data.az = data[2];
    }  
    setTime(value, controller->controlTime);
}


void AccelerationSensorOutPortHandler::writeDataToPort()
{
  outPort.write();
}


ColorImageOutPortHandler::ColorImageOutPortHandler(PortInfo& info) :
  OutPortHandler(info),
  outPort(info.portName.c_str(), image),
  cameraId(info.dataOwnerId)
{
    stepTime = info.stepTime;
}


void ColorImageOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
{
  ImageData_var imageInput = controller->getCameraImageFromSimulator(cameraId);
  image.data = imageInput->longData;
  setTime(image, controller->controlTime);
}


void ColorImageOutPortHandler::writeDataToPort()
{
  outPort.write();
}


GrayScaleImageOutPortHandler::GrayScaleImageOutPortHandler(PortInfo& info) : 
  OutPortHandler(info),
  outPort(info.portName.c_str(), image),
  cameraId(info.dataOwnerId)
{
    stepTime = info.stepTime;
}


void GrayScaleImageOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
{
  ImageData_var imageInput = controller->getCameraImageFromSimulator(cameraId);
  image.data = imageInput->octetData;
  setTime(image, controller->controlTime);
}


void GrayScaleImageOutPortHandler::writeDataToPort()
{
  outPort.write();
}


DepthImageOutPortHandler::DepthImageOutPortHandler(PortInfo& info) :
  OutPortHandler(info),
  outPort(info.portName.c_str(), image),
  cameraId(info.dataOwnerId)
{
    stepTime = info.stepTime;
}


void DepthImageOutPortHandler::inputDataFromSimulator(Controller_impl* controller)
{
  ImageData_var imageInput = controller->getCameraImageFromSimulator(cameraId);
  image.data = imageInput->floatData;
  setTime(image, controller->controlTime);
}


void DepthImageOutPortHandler::writeDataToPort()
{
  outPort.write();
}


JointDataSeqInPortHandler::JointDataSeqInPortHandler(PortInfo& info) :
  InPortHandler(info),
  inPort(info.portName.c_str(), values)
{
  linkDataType = toDynamicsSimulatorLinkDataType(info.dataTypeId);
}


void JointDataSeqInPortHandler::outputDataToSimulator(Controller_impl* controller)
{
  controller->flushJointDataSeqToSimulator(linkDataType);
}


void JointDataSeqInPortHandler::readDataFromPort(Controller_impl* controller)
{
  if( inPort.isNew() == false ){
          DblSequence& data = controller->getJointDataSeqRef(linkDataType);
          data.length(0);
      return;
  }
  inPort.read();

  DblSequence& data = controller->getJointDataSeqRef(linkDataType);
  
  CORBA::ULong n = values.data.length();
  data.length(n);
  for(CORBA::ULong i=0; i < n; ++i){
    data[i] = values.data[i];
  }
}

LinkDataInPortHandler::LinkDataInPortHandler(PortInfo& info) :
  InPortHandler(info),
  inPort(info.portName.c_str(), values),
  linkName(info.dataOwnerName)
{
  linkDataType = toDynamicsSimulatorLinkDataType(info.dataTypeId);
}


void LinkDataInPortHandler::outputDataToSimulator(Controller_impl* controller)
{
    if(!data.length())
        return;
    size_t n=linkName.size();
    CORBA::ULong m=data.length()/n;
    for(size_t i=0, k=0; i< n; i++){
        DblSequence data0;
        data0.length(m);
        for(CORBA::ULong j=0; j<m; j++)
            data0[j] = data[k++];
        controller->flushLinkDataToSimulator(linkName[i], linkDataType, data0);
    }

}


void LinkDataInPortHandler::readDataFromPort(Controller_impl* controller)
{
  if( inPort.isNew() == false ){
      return;
  }
  inPort.read();

  CORBA::ULong n = values.data.length();
  data.length(n);
  for(CORBA::ULong i=0; i < n; ++i){
    data[i] = values.data[i];
  }
}

AbsTransformInPortHandler::AbsTransformInPortHandler(PortInfo& info) :
  InPortHandler(info),
  inPort(info.portName.c_str(), values),
  linkName(info.dataOwnerName)
{
  linkDataType = toDynamicsSimulatorLinkDataType(info.dataTypeId);
}


void AbsTransformInPortHandler::outputDataToSimulator(Controller_impl* controller)
{
    if(!data.length())
        return;
    controller->flushLinkDataToSimulator(linkName[0], linkDataType, data);
}


void AbsTransformInPortHandler::readDataFromPort(Controller_impl* controller)
{
  if( inPort.isNew() == false ){
      return;
  }
  inPort.read();

  data.length(12);
  data[0] = values.data.position.x;
  data[1] = values.data.position.y;
  data[2] = values.data.position.z;
  hrp::Matrix33 R = hrp::rotFromRpy(values.data.orientation.r,
                                    values.data.orientation.p,
                                    values.data.orientation.y);
  for (int i=0; i<3; i++){
      for (int j=0; j<3; j++){
          data[3+i*3+j] = R(i,j);
      }
  }
}