RTCBody.cpp
Go to the documentation of this file.
00001 #include <hrpModel/Link.h>
00002 #include <hrpModel/Sensor.h>
00003 #include "RTCBody.h"
00004 
00005 using namespace hrp;
00006 
00007 RTCBody::RTCBody() :
00008     m_tauIn("tau", m_tau),
00009     m_qCmdIn("qCmd", m_qCmd),
00010     m_dqCmdIn("dqCmd", m_dqCmd),
00011     m_ddqCmdIn("ddqCmd", m_ddqCmd),
00012     m_posOut("pos", m_pos),
00013     m_rpyOut("rpy", m_rpy),
00014     m_qOut("q", m_q),
00015     m_highgain(false)
00016 {
00017 }
00018 
00019 RTCBody::~RTCBody()
00020 {
00021 }
00022 
00023 void RTCBody::createPorts(RTC::DataFlowComponentBase *comp)
00024 {
00025     // input ports
00026     for (unsigned int i=0; i<numJoints(); i++){
00027         Link *l = joint(i);
00028         if (l){
00029             if (l->isHighGainMode){
00030                 m_highgain = true;
00031                 break;
00032             }
00033         }
00034     }
00035 
00036     if (numJoints()){
00037         if (m_highgain){
00038             comp->addInPort("qCmd", m_qCmdIn);
00039             comp->addInPort("dqCmd", m_dqCmdIn);
00040             comp->addInPort("ddqCmd", m_ddqCmdIn);
00041         }else{
00042             comp->addInPort("tau", m_tauIn);
00043         }
00044     }
00045 
00046     // output ports
00047     m_q.data.length(numJoints());
00048     if (numJoints() > 0) comp->addOutPort("q", m_qOut);
00049     if (rootLink()->jointType == Link::FREE_JOINT){
00050         comp->addOutPort("pos", m_posOut);
00051         comp->addOutPort("rpy", m_rpyOut);
00052     }
00053 
00054     int ngyro = numSensors(Sensor::RATE_GYRO);
00055     m_rate.resize(ngyro);
00056     m_rateOut.resize(ngyro);
00057     for (unsigned int i=0; i<m_rate.size(); i++){
00058         Sensor *s = sensor(Sensor::RATE_GYRO, i);
00059         m_rateOut[i] = new RTC::OutPort<RTC::TimedAngularVelocity3D>(s->name.c_str(), m_rate[i]);
00060         comp->addOutPort(s->name.c_str(), *m_rateOut[i]);
00061     }
00062     
00063     int nacc = numSensors(Sensor::ACCELERATION);
00064     m_acc.resize(nacc);
00065     m_accOut.resize(nacc);
00066     for (unsigned int i=0; i<m_acc.size(); i++){
00067         Sensor *s = sensor(Sensor::ACCELERATION, i);
00068         m_accOut[i] = new RTC::OutPort<RTC::TimedAcceleration3D>(s->name.c_str(), m_acc[i]);
00069         comp->addOutPort(s->name.c_str(), *m_accOut[i]);
00070     }
00071     
00072     int nforce = numSensors(Sensor::FORCE);
00073     m_force.resize(nforce);
00074     m_forceOut.resize(nforce);
00075     for (unsigned int i=0; i<m_force.size(); i++){
00076         Sensor *s = sensor(Sensor::FORCE, i);
00077         m_forceOut[i] = new RTC::OutPort<RTC::TimedDoubleSeq>(s->name.c_str(), 
00078                                                               m_force[i]);
00079         m_force[i].data.length(6);
00080         comp->addOutPort(s->name.c_str(), *m_forceOut[i]);
00081     }
00082 }
00083 
00084 void RTCBody::input()
00085 {
00086     if (m_highgain){
00087         if (m_qCmdIn.isNew()){
00088             do {
00089                 m_qCmdIn.read();
00090             }while (m_qCmdIn.isNew());
00091             for (unsigned int i=0; i<numJoints(); i++){
00092                 Link *l = joint(i);
00093                 if (l){
00094                     l->q = m_qCmd.data[l->jointId];
00095                 }
00096             }
00097         }
00098         if (m_dqCmdIn.isNew()){
00099             do {
00100                 m_dqCmdIn.read();
00101             }while (m_dqCmdIn.isNew());
00102             for (unsigned int i=0; i<numJoints(); i++){
00103                 Link *l = joint(i);
00104                 if (l){
00105                     l->dq = m_dqCmd.data[l->jointId];
00106                 }
00107             }
00108         }
00109         if (m_ddqCmdIn.isNew()){
00110             do {
00111                 m_ddqCmdIn.read();
00112             }while (m_ddqCmdIn.isNew());
00113             for (unsigned int i=0; i<numJoints(); i++){
00114                 Link *l = joint(i);
00115                 if (l){
00116                     l->ddq = m_ddqCmd.data[l->jointId];
00117                 }
00118             }
00119         }
00120     }else{
00121         if (m_tauIn.isNew()){
00122             do {
00123                 m_tauIn.read();
00124             }while (m_tauIn.isNew());
00125             for (unsigned int i=0; i<numJoints(); i++){
00126                 Link *l = joint(i);
00127                 if (l){
00128                     l->u = m_tau.data[l->jointId];
00129                 }
00130             }
00131         }
00132     }
00133 }
00134 
00135 void RTCBody::output(OpenHRP::RobotState& state)
00136 {
00137     if (numJoints() > 0){
00138         for (unsigned int i=0; i<numJoints(); i++){
00139             Link *l = joint(i);
00140             if (l){
00141                 m_q.data[l->jointId] = l->q;
00142             }
00143         }
00144         state.q = m_q.data;
00145         m_qOut.write();
00146     }
00147 
00148     Link *root = rootLink();
00149 
00150     m_pos.data.x = root->p[0];
00151     m_pos.data.y = root->p[1];
00152     m_pos.data.z = root->p[2];
00153     state.basePose.position = m_pos.data;
00154     
00155     Vector3 rpy(rpyFromRot(root->attitude()));
00156     m_rpy.data.r = rpy[0];
00157     m_rpy.data.p = rpy[1];
00158     m_rpy.data.y = rpy[2];
00159     state.basePose.orientation = m_rpy.data;
00160 
00161     if (root->jointType == Link::FREE_JOINT){
00162         m_posOut.write();
00163         m_rpyOut.write();
00164     }
00165 
00166     int n = numSensors(Sensor::FORCE);
00167     for(int id = 0; id < n; ++id){
00168         ForceSensor* s = sensor<ForceSensor>(id);
00169         setVector3(s->f,   m_force[id].data, 0);
00170         setVector3(s->tau, m_force[id].data, 3);
00171     }
00172 
00173     n = numSensors(Sensor::RATE_GYRO);
00174     for(int id=0; id < n; ++id){
00175         RateGyroSensor* s = sensor<RateGyroSensor>(id);
00176         m_rate[id].data.avx = s->w[0];
00177         m_rate[id].data.avy = s->w[1];
00178         m_rate[id].data.avz = s->w[2];
00179     }
00180     
00181     n = numSensors(Sensor::ACCELERATION);
00182     for(int id=0; id < n; ++id){
00183         AccelSensor* s = sensor<AccelSensor>(id);
00184         m_acc[id].data.ax = s->dv[0];
00185         m_acc[id].data.ay = s->dv[1];
00186         m_acc[id].data.az = s->dv[2];
00187     }           
00188 }
00189 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55