00001
00010 #include "StateHolder.h"
00011 #include <rtm/CorbaNaming.h>
00012 #include <hrpUtil/Eigen3d.h>
00013 #include <hrpModel/ModelLoaderUtil.h>
00014
00015
00016
00017 static const char* stateholder_spec[] =
00018 {
00019 "implementation_id", "StateHolder",
00020 "type_name", "StateHolder",
00021 "description", "state holder",
00022 "version", HRPSYS_PACKAGE_VERSION,
00023 "vendor", "AIST",
00024 "category", "example",
00025 "activity_type", "DataFlowComponent",
00026 "max_instance", "10",
00027 "language", "C++",
00028 "lang_type", "compile",
00029
00030
00031 ""
00032 };
00033
00034
00035 StateHolder::StateHolder(RTC::Manager* manager)
00036 : RTC::DataFlowComponentBase(manager),
00037
00038 m_currentQIn("currentQIn", m_currentQ),
00039 m_qIn("qIn", m_q),
00040 m_tqIn("tqIn", m_tq),
00041 m_basePosIn("basePosIn", m_basePos),
00042 m_baseRpyIn("baseRpyIn", m_baseRpy),
00043 m_zmpIn("zmpIn", m_zmp),
00044 m_optionalDataIn("optionalDataIn", m_optionalData),
00045 m_qOut("qOut", m_q),
00046 m_tqOut("tqOut", m_tq),
00047 m_basePosOut("basePosOut", m_basePos),
00048 m_baseRpyOut("baseRpyOut", m_baseRpy),
00049 m_baseTformOut("baseTformOut", m_baseTform),
00050 m_basePoseOut("basePoseOut", m_basePose),
00051 m_zmpOut("zmpOut", m_zmp),
00052 m_optionalDataOut("optionalDataOut", m_optionalData),
00053 m_StateHolderServicePort("StateHolderService"),
00054 m_TimeKeeperServicePort("TimeKeeperService"),
00055
00056 m_timeCount(0),
00057 dummy(0)
00058 {
00059
00060 m_service0.setComponent(this);
00061 m_service1.setComponent(this);
00062 m_requestGoActual = false;
00063
00064 sem_init(&m_waitSem, 0, 0);
00065 sem_init(&m_timeSem, 0, 0);
00066 }
00067
00068 StateHolder::~StateHolder()
00069 {
00070 }
00071
00072
00073
00074 RTC::ReturnCode_t StateHolder::onInitialize()
00075 {
00076 std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl;
00077
00078
00079
00080
00081
00082
00083
00084
00085 addInPort("currentQIn", m_currentQIn);
00086 addInPort("qIn", m_qIn);
00087 addInPort("tqIn", m_tqIn);
00088 addInPort("basePosIn", m_basePosIn);
00089 addInPort("baseRpyIn", m_baseRpyIn);
00090 addInPort("zmpIn", m_zmpIn);
00091 addInPort("optionalDataIn", m_optionalDataIn);
00092
00093
00094 addOutPort("qOut", m_qOut);
00095 addOutPort("tqOut", m_tqOut);
00096 addOutPort("basePosOut", m_basePosOut);
00097 addOutPort("baseRpyOut", m_baseRpyOut);
00098 addOutPort("baseTformOut", m_baseTformOut);
00099 addOutPort("basePoseOut", m_basePoseOut);
00100 addOutPort("zmpOut", m_zmpOut);
00101 addOutPort("optionalDataOut", m_optionalDataOut);
00102
00103
00104 m_StateHolderServicePort.registerProvider("service0", "StateHolderService", m_service0);
00105 m_TimeKeeperServicePort.registerProvider("service1", "TimeKeeperService", m_service1);
00106
00107
00108
00109
00110 addPort(m_StateHolderServicePort);
00111 addPort(m_TimeKeeperServicePort);
00112
00113
00114
00115 RTC::Properties& prop = getProperties();
00116 coil::stringTo(m_dt, prop["dt"].c_str());
00117 std::cout << "StateHolder: dt = " << m_dt << std::endl;
00118 RTC::Manager& rtcManager = RTC::Manager::instance();
00119 std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
00120 int comPos = nameServer.find(",");
00121 if (comPos < 0){
00122 comPos = nameServer.length();
00123 }
00124 nameServer = nameServer.substr(0, comPos);
00125 RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
00126 OpenHRP::BodyInfo_var binfo;
00127 binfo = hrp::loadBodyInfo(prop["model"].c_str(),
00128 CosNaming::NamingContext::_duplicate(naming.getRootContext()));
00129 OpenHRP::LinkInfoSequence_var lis = binfo->links();
00130 const OpenHRP::LinkInfo& li = lis[0];
00131 hrp::Vector3 p, axis;
00132 p << li.translation[0], li.translation[1], li.translation[2];
00133 axis << li.rotation[0], li.rotation[1], li.rotation[2];
00134 hrp::Matrix33 R = hrp::rodrigues(axis, li.rotation[3]);
00135 hrp::Vector3 rpy = hrp::rpyFromRot(R);
00136
00137 m_baseTform.data.length(12);
00138 double *T = m_baseTform.data.get_buffer();
00139 T[0] = R(0,0); T[1] = R(0,1); T[ 2] = R(0,2); T[ 3] = p[0];
00140 T[4] = R(0,0); T[5] = R(0,1); T[ 6] = R(0,2); T[ 7] = p[1];
00141 T[8] = R(0,0); T[9] = R(0,1); T[10] = R(0,2); T[11] = p[2];
00142 m_basePos.data.x = m_basePose.data.position.x = p[0];
00143 m_basePos.data.y = m_basePose.data.position.y = p[1];
00144 m_basePos.data.z = m_basePose.data.position.z = p[2];
00145 m_baseRpy.data.r = m_basePose.data.orientation.r = rpy[0];
00146 m_baseRpy.data.p = m_basePose.data.orientation.p = rpy[1];
00147 m_baseRpy.data.y = m_basePose.data.orientation.y = rpy[2];
00148 m_zmp.data.x = m_zmp.data.y = m_zmp.data.z = 0.0;
00149
00150
00151 std::vector<std::string> fsensor_names;
00152
00153 for ( unsigned int k = 0; k < lis->length(); k++ ) {
00154 OpenHRP::SensorInfoSequence& sensors = lis[k].sensors;
00155 for ( unsigned int l = 0; l < sensors.length(); l++ ) {
00156 if ( std::string(sensors[l].type) == "Force" ) {
00157 fsensor_names.push_back(std::string(sensors[l].name));
00158 }
00159 }
00160 }
00161 int npforce = fsensor_names.size();
00162
00163 coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ",");
00164 unsigned int nvforce = virtual_force_sensor.size()/10;
00165 for (unsigned int i=0; i<nvforce; i++){
00166 fsensor_names.push_back(virtual_force_sensor[i*10+0]);
00167 }
00168
00169 unsigned int nforce = npforce + nvforce;
00170 m_wrenches.resize(nforce);
00171 m_wrenchesIn.resize(nforce);
00172 m_wrenchesOut.resize(nforce);
00173 for (unsigned int i=0; i<nforce; i++){
00174 m_wrenchesIn[i] = new InPort<TimedDoubleSeq>(std::string(fsensor_names[i]+"In").c_str(), m_wrenches[i]);
00175 m_wrenchesOut[i] = new OutPort<TimedDoubleSeq>(std::string(fsensor_names[i]+"Out").c_str(), m_wrenches[i]);
00176 m_wrenches[i].data.length(6);
00177 m_wrenches[i].data[0] = m_wrenches[i].data[1] = m_wrenches[i].data[2] = 0.0;
00178 m_wrenches[i].data[3] = m_wrenches[i].data[4] = m_wrenches[i].data[5] = 0.0;
00179 registerInPort(std::string(fsensor_names[i]+"In").c_str(), *m_wrenchesIn[i]);
00180 registerOutPort(std::string(fsensor_names[i]+"Out").c_str(), *m_wrenchesOut[i]);
00181 }
00182
00183
00184 return RTC::RTC_OK;
00185 }
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224 RTC::ReturnCode_t StateHolder::onExecute(RTC::UniqueId ec_id)
00225 {
00226
00227 RTC::Time tm;
00228
00229 if (m_currentQIn.isNew()){
00230 m_currentQIn.read();
00231 tm = m_currentQ.tm;
00232 }else{
00233 coil::TimeValue coiltm(coil::gettimeofday());
00234 tm.sec = coiltm.sec();
00235 tm.nsec = coiltm.usec()*1000;
00236 }
00237
00238 if (m_qIn.isNew()){
00239 m_qIn.read();
00240 }
00241 if (m_tqIn.isNew()){
00242 m_tqIn.read();
00243 }
00244 if (m_requestGoActual || (m_q.data.length() == 0 && m_currentQ.data.length() > 0)){
00245 m_q = m_currentQ;
00246 if (m_q.data.length() != m_tq.data.length()) m_tq.data.length(m_q.data.length());
00247
00248 for (unsigned int i=0; i<m_wrenchesIn.size(); i++){
00249 m_wrenches[i].data[0] = m_wrenches[i].data[1] = m_wrenches[i].data[2] = 0.0;
00250 m_wrenches[i].data[3] = m_wrenches[i].data[4] = m_wrenches[i].data[5] = 0.0;
00251 }
00252 }
00253
00254 if (m_requestGoActual){
00255 m_requestGoActual = false;
00256 sem_post(&m_waitSem);
00257 }
00258
00259 if (m_basePosIn.isNew()){
00260 m_basePosIn.read();
00261 }
00262
00263 if (m_baseRpyIn.isNew()){
00264 m_baseRpyIn.read();
00265 }
00266
00267 if (m_zmpIn.isNew()){
00268 m_zmpIn.read();
00269 }
00270
00271 if (m_optionalDataIn.isNew()){
00272 m_optionalDataIn.read();
00273 }
00274
00275 for (size_t i = 0; i < m_wrenchesIn.size(); i++) {
00276 if ( m_wrenchesIn[i]->isNew() ) {
00277 m_wrenchesIn[i]->read();
00278 }
00279 }
00280
00281 double *a = m_baseTform.data.get_buffer();
00282 a[0] = m_basePos.data.x;
00283 a[1] = m_basePos.data.y;
00284 a[2] = m_basePos.data.z;
00285 hrp::Matrix33 R = hrp::rotFromRpy(m_baseRpy.data.r,
00286 m_baseRpy.data.p,
00287 m_baseRpy.data.y);
00288 hrp::setMatrix33ToRowMajorArray(R, a, 3);
00289
00290 m_basePose.data.position = m_basePos.data;
00291 m_basePose.data.orientation = m_baseRpy.data;
00292
00293
00294 m_q.tm = tm;
00295 m_tq.tm = tm;
00296 m_baseTform.tm = tm;
00297 m_basePos.tm = tm;
00298 m_baseRpy.tm = tm;
00299 m_zmp.tm = tm;
00300 m_basePose.tm = tm;
00301 for (size_t i = 0; i < m_wrenches.size(); i++) {
00302 m_wrenches[i].tm = tm;
00303 }
00304
00305
00306 if (m_q.data.length() > 0){
00307 m_qOut.write();
00308 }
00309 if (m_tq.data.length() > 0){
00310 m_tqOut.write();
00311 }
00312 m_baseTformOut.write();
00313 m_basePosOut.write();
00314 m_baseRpyOut.write();
00315 m_zmpOut.write();
00316 m_basePoseOut.write();
00317 m_optionalDataOut.write();
00318 for (size_t i = 0; i < m_wrenchesOut.size(); i++) {
00319 m_wrenchesOut[i]->write();
00320 }
00321
00322 if (m_timeCount > 0){
00323 m_timeCount--;
00324 if (m_timeCount == 0) sem_post(&m_timeSem);
00325 }
00326
00327 return RTC::RTC_OK;
00328 }
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366 void StateHolder::goActual()
00367 {
00368 std::cout << "StateHolder::goActual()" << std::endl;
00369 m_requestGoActual = true;
00370 sem_wait(&m_waitSem);
00371 }
00372
00373 void StateHolder::getCommand(StateHolderService::Command &com)
00374 {
00375 com.jointRefs.length(m_q.data.length());
00376 memcpy(com.jointRefs.get_buffer(), m_q.data.get_buffer(), sizeof(double)*m_q.data.length());
00377 com.baseTransform.length(12);
00378 com.baseTransform[0] = m_basePos.data.x;
00379 com.baseTransform[1] = m_basePos.data.y;
00380 com.baseTransform[2] = m_basePos.data.z;
00381 hrp::Matrix33 R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
00382 double *a = com.baseTransform.get_buffer();
00383 hrp::setMatrix33ToRowMajorArray(R, a, 3);
00384 com.zmp.length(3);
00385 com.zmp[0] = m_zmp.data.x; com.zmp[1] = m_zmp.data.y; com.zmp[2] = m_zmp.data.z;
00386 }
00387
00388 void StateHolder::wait(CORBA::Double tm)
00389 {
00390 m_timeCount = tm/m_dt;
00391 sem_wait(&m_timeSem);
00392 }
00393
00394 extern "C"
00395 {
00396
00397 void StateHolderInit(RTC::Manager* manager)
00398 {
00399 RTC::Properties profile(stateholder_spec);
00400 manager->registerFactory(profile,
00401 RTC::Create<StateHolder>,
00402 RTC::Delete<StateHolder>);
00403 }
00404
00405 };
00406
00407