VirtualRobotPortHandler.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  * General Robotix Inc.
9  */
15 #include "hrpUtil/Eigen3d.h"
17 
18 #include "Controller_impl.h"
19 
20 using namespace RTC;
21 
22 namespace {
23 
24  template <typename TSrc>
25  void copyDblArraySeqToTimedDoubleSeq(const TSrc& src, int arraySize, TimedDoubleSeq& dest)
26  {
27  CORBA::ULong n = src.length();
28  CORBA::ULong m = n * arraySize;
29  dest.data.length(m);
30  CORBA::ULong destPos = 0;
31  for(CORBA::ULong i=0; i < n; ++i){
32  for(int j=0; j < arraySize; ++j){
33  dest.data[destPos++] = src[i][j];
34  }
35  }
36  }
37 
38  template <typename TSrcSeq, typename TDestSeq>
39  void copyImageData();
40 
41 
42  DynamicsSimulator::LinkDataType toDynamicsSimulatorLinkDataType(DataTypeId id)
43  {
44  switch(id){
56  }
57  }
58 }
59 
61 {
62 
63 }
64 
65 
67  OutPortHandler(info),
68  outPort(info.portName.c_str(), values)
69 {
70  dataTypeId = info.dataTypeId;
71  stepTime = info.stepTime;
72 }
73 
74 
76 {
77  SensorState& state = controller->getCurrentSensorState();
78 
79  switch(dataTypeId) {
80  case JOINT_VALUE:
81  values.data = state.q;
82  break;
83  case JOINT_VELOCITY:
84  values.data = state.dq;
85  break;
86  case JOINT_TORQUE:
87  values.data = state.u;
88  break;
89  case FORCE_SENSOR:
90  copyDblArraySeqToTimedDoubleSeq(state.force, 6, values);
91  break;
92  case RATE_GYRO_SENSOR:
93  copyDblArraySeqToTimedDoubleSeq(state.rateGyro, 3, values);
94  break;
96  copyDblArraySeqToTimedDoubleSeq(state.accel, 3, values);
97  break;
98  default:
99  break;
100  }
101  setTime(values, controller->controlTime);
102 }
103 
104 
106 {
107  outPort.write();
108 }
109 
110 
112  OutPortHandler(info),
113  outPort(info.portName.c_str(), value),
114  linkName(info.dataOwnerName)
115 {
116  linkDataType = toDynamicsSimulatorLinkDataType(info.dataTypeId);
117  stepTime = info.stepTime;
118 }
119 
120 
122 {
123  size_t n;
124  CORBA::ULong m=0;
125  n = linkName.size();
126  for(size_t i=0, k=0; i<n; i++){
127  DblSequence_var data = controller->getLinkDataFromSimulator(linkName[i], linkDataType);
128  if(!i){
129  m = data->length();
130  value.data.length(n*m);
131  }
132  for(CORBA::ULong j=0; j < m; j++)
133  value.data[k++] = data[j];
134  }
135  setTime(value, controller->controlTime);
136 }
137 
138 
140 {
141  outPort.write();
142 }
143 
145  OutPortHandler(info),
146  outPort(info.portName.c_str(), value),
147  linkName(info.dataOwnerName)
148 {
149  linkDataType = toDynamicsSimulatorLinkDataType(info.dataTypeId);
150  stepTime = info.stepTime;
151 }
152 
153 
155 {
156  DblSequence_var data = controller->getLinkDataFromSimulator(linkName[0], linkDataType);
157  value.data.position.x = data[0];
158  value.data.position.y = data[1];
159  value.data.position.z = data[2];
160  hrp::Matrix33 R;
161  R << data[3], data[4], data[5],
162  data[6], data[7], data[8],
163  data[9], data[10], data[11];
164  hrp::Vector3 rpy = hrp::rpyFromRot(R);
165  value.data.orientation.r = rpy[0];
166  value.data.orientation.p = rpy[1];
167  value.data.orientation.y = rpy[2];
168  setTime(value, controller->controlTime);
169 }
170 
171 
173 {
174  outPort.write();
175 }
176 
177 
179  OutPortHandler(info),
180  outPort(info.portName.c_str(), value),
181  sensorName(info.dataOwnerName)
182 {
183  stepTime = info.stepTime;
184 }
185 
186 
188 {
189  size_t n;
190  CORBA::ULong m=0;
191  n = sensorName.size();
192  for(size_t i=0, k=0; i<n; i++){
193  DblSequence_var data = controller->getSensorDataFromSimulator(sensorName[i]);
194  if(!i){
195  m = data->length();
196  value.data.length(n*m);
197  }
198  for(CORBA::ULong j=0; j < m; j++)
199  value.data[k++] = data[j];
200  }
201  setTime(value, controller->controlTime);
202 }
203 
204 
206 {
207  outPort.write();
208 }
209 
210 
212  OutPortHandler(info),
213  outPort(info.portName.c_str(), value),
214  sensorName(info.dataOwnerName)
215 {
216  stepTime = info.stepTime;
217 }
218 
219 
221 {
222  size_t n;
223  n = sensorName.size();
224  for(size_t i=0; i<n; i++){
225  DblSequence_var data = controller->getSensorDataFromSimulator(sensorName[i]);
226  value.data.avx = data[0];
227  value.data.avy = data[1];
228  value.data.avz = data[2];
229  }
230  setTime(value, controller->controlTime);
231 }
232 
233 
235 {
236  outPort.write();
237 }
238 
239 
241  OutPortHandler(info),
242  outPort(info.portName.c_str(), value),
243  sensorName(info.dataOwnerName)
244 {
245  stepTime = info.stepTime;
246 }
247 
248 
250 {
251  size_t n;
252  n = sensorName.size();
253  for(size_t i=0; i<n; i++){
254  DblSequence_var data = controller->getSensorDataFromSimulator(sensorName[i]);
255  value.data.ax = data[0];
256  value.data.ay = data[1];
257  value.data.az = data[2];
258  }
259  setTime(value, controller->controlTime);
260 }
261 
262 
264 {
265  outPort.write();
266 }
267 
268 
270  OutPortHandler(info),
271  outPort(info.portName.c_str(), image),
272  cameraId(info.dataOwnerId)
273 {
274  stepTime = info.stepTime;
275 }
276 
277 
279 {
280  ImageData_var imageInput = controller->getCameraImageFromSimulator(cameraId);
281  image.data = imageInput->longData;
282  setTime(image, controller->controlTime);
283 }
284 
285 
287 {
288  outPort.write();
289 }
290 
291 
293  OutPortHandler(info),
294  outPort(info.portName.c_str(), image),
295  cameraId(info.dataOwnerId)
296 {
297  stepTime = info.stepTime;
298 }
299 
300 
302 {
303  ImageData_var imageInput = controller->getCameraImageFromSimulator(cameraId);
304  image.data = imageInput->octetData;
305  setTime(image, controller->controlTime);
306 }
307 
308 
310 {
311  outPort.write();
312 }
313 
314 
316  OutPortHandler(info),
317  outPort(info.portName.c_str(), image),
318  cameraId(info.dataOwnerId)
319 {
320  stepTime = info.stepTime;
321 }
322 
323 
325 {
326  ImageData_var imageInput = controller->getCameraImageFromSimulator(cameraId);
327  image.data = imageInput->floatData;
328  setTime(image, controller->controlTime);
329 }
330 
331 
333 {
334  outPort.write();
335 }
336 
337 
339  InPortHandler(info),
340  inPort(info.portName.c_str(), values)
341 {
342  linkDataType = toDynamicsSimulatorLinkDataType(info.dataTypeId);
343 }
344 
345 
347 {
349 }
350 
351 
353 {
354  if( inPort.isNew() == false ){
355  DblSequence& data = controller->getJointDataSeqRef(linkDataType);
356  data.length(0);
357  return;
358  }
359  inPort.read();
360 
361  DblSequence& data = controller->getJointDataSeqRef(linkDataType);
362 
363  CORBA::ULong n = values.data.length();
364  data.length(n);
365  for(CORBA::ULong i=0; i < n; ++i){
366  data[i] = values.data[i];
367  }
368 }
369 
371  InPortHandler(info),
372  inPort(info.portName.c_str(), values),
373  linkName(info.dataOwnerName)
374 {
375  linkDataType = toDynamicsSimulatorLinkDataType(info.dataTypeId);
376 }
377 
378 
380 {
381  if(!data.length())
382  return;
383  size_t n=linkName.size();
384  CORBA::ULong m=data.length()/n;
385  for(size_t i=0, k=0; i< n; i++){
386  DblSequence data0;
387  data0.length(m);
388  for(CORBA::ULong j=0; j<m; j++)
389  data0[j] = data[k++];
390  controller->flushLinkDataToSimulator(linkName[i], linkDataType, data0);
391  }
392 
393 }
394 
395 
397 {
398  if( inPort.isNew() == false ){
399  return;
400  }
401  inPort.read();
402 
403  CORBA::ULong n = values.data.length();
404  data.length(n);
405  for(CORBA::ULong i=0; i < n; ++i){
406  data[i] = values.data[i];
407  }
408 }
409 
411  InPortHandler(info),
412  inPort(info.portName.c_str(), values),
413  linkName(info.dataOwnerName)
414 {
415  linkDataType = toDynamicsSimulatorLinkDataType(info.dataTypeId);
416 }
417 
418 
420 {
421  if(!data.length())
422  return;
424 }
425 
426 
428 {
429  if( inPort.isNew() == false ){
430  return;
431  }
432  inPort.read();
433 
434  data.length(12);
435  data[0] = values.data.position.x;
436  data[1] = values.data.position.y;
437  data[2] = values.data.position.z;
438  hrp::Matrix33 R = hrp::rotFromRpy(values.data.orientation.r,
439  values.data.orientation.p,
440  values.data.orientation.y);
441  for (int i=0; i<3; i++){
442  for (int j=0; j<3; j++){
443  data[3+i*3+j] = R(i,j);
444  }
445  }
446 }
data0
RTC::InPort< RTC::TimedDoubleSeq > inPort
virtual void inputDataFromSimulator(Controller_impl *controller)
RTC::InPort< RTC::TimedDoubleSeq > inPort
DynamicsSimulator::LinkDataType linkDataType
virtual void inputDataFromSimulator(Controller_impl *controller)
state
DynamicsSimulator::LinkDataType linkDataType
virtual void readDataFromPort(Controller_impl *controller)
std::vector< std::string > sensorName
virtual void outputDataToSimulator(Controller_impl *controller)
std::vector< std::string > sensorName
DynamicsSimulator::LinkDataType linkDataType
RTC::OutPort< RTC::TimedDoubleSeq > outPort
virtual void inputDataFromSimulator(Controller_impl *controller)
ImageData * getCameraImageFromSimulator(int cameraId)
png_voidp int value
Definition: png.h:2113
std::vector< std::string > linkName
virtual void inputDataFromSimulator(Controller_impl *controller)
virtual void inputDataFromSimulator(Controller_impl *controller)
DblSequence & getJointDataSeqRef(DynamicsSimulator::LinkDataType linkDataType)
png_uint_32 i
Definition: png.h:2735
png_bytepp image
Definition: png.h:1772
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
RTC::OutPort< RTC::TimedAngularVelocity3D > outPort
DataTypeId
Definition: BridgeConf.h:34
Matrix33 rotFromRpy(const Vector3 &rpy)
Definition: Eigen3d.h:33
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12
std::vector< std::string > sensorName
void setTime(T &value, double _time)
SensorState & getCurrentSensorState()
virtual void readDataFromPort(Controller_impl *controller)
virtual void outputDataToSimulator(Controller_impl *controller)
void flushJointDataSeqToSimulator(DynamicsSimulator::LinkDataType linkDataType)
def j(str, encoding="cp932")
DataTypeId dataTypeId
Definition: BridgeConf.h:59
void flushLinkDataToSimulator(const std::string &linkName, DynamicsSimulator::LinkDataType linkDataType, const DblSequence &linkData)
virtual void inputDataFromSimulator(Controller_impl *controller)
RTC::OutPort< RTC::TimedFloatSeq > outPort
virtual void inputDataFromSimulator(Controller_impl *controller)
RTC::TimedAngularVelocity3D value
RTC::OutPort< RTC::TimedPose3D > outPort
HRP_UTIL_EXPORT Vector3 rpyFromRot(const Matrix33 &m)
Definition: Eigen3d.cpp:99
DblSequence * getLinkDataFromSimulator(const std::string &linkName, DynamicsSimulator::LinkDataType linkDataType)
DynamicsSimulator::LinkDataType linkDataType
backing_store_ptr info
Definition: jmemsys.h:181
virtual bool isNew()
virtual void readDataFromPort(Controller_impl *controller)
virtual void outputDataToSimulator(Controller_impl *controller)
std::vector< std::string > linkName
RTC::OutPort< RTC::TimedDoubleSeq > outPort
virtual bool write(DataType &value)
DynamicsSimulator::LinkDataType linkDataType
JSAMPIMAGE data
Definition: jpeglib.h:945
RTC::InPort< RTC::TimedPose3D > inPort
std::vector< std::string > linkName
DblSequence * getSensorDataFromSimulator(const std::string &sensorName)
RTC::OutPort< RTC::TimedLongSeq > outPort
std::vector< std::string > linkName
RTC::OutPort< RTC::TimedDoubleSeq > outPort
double stepTime
Definition: BridgeConf.h:62
RTC::OutPort< RTC::TimedOctetSeq > outPort
virtual void inputDataFromSimulator(Controller_impl *controller)
RTC::OutPort< RTC::TimedAcceleration3D > outPort
virtual void inputDataFromSimulator(Controller_impl *controller)


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat May 8 2021 02:42:41