Go to the documentation of this file.00001 #include <rtm/DataFlowComponentBase.h>
00002 #include "RTCGLbody.h"
00003 #if 0
00004 #include "IrrModel.h"
00005 #else
00006 #include "hrpsys/util/GLbody.h"
00007 #include "hrpsys/util/GLlink.h"
00008 #endif
00009
00010 RTCGLbody::RTCGLbody(GLbody *i_body, RTC::DataFlowComponentBase *comp) :
00011 m_body(i_body),
00012 m_qIn("q", m_q),
00013 m_posIn("pos", m_pos),
00014 m_rpyIn("rpy", m_rpy)
00015 {
00016 if (m_body->numJoints() > 0) comp->addInPort("q", m_qIn);
00017 if (m_body->rootLink()->jointType == hrp::Link::FREE_JOINT){
00018 comp->addInPort("pos", m_posIn);
00019 comp->addInPort("rpy", m_rpyIn);
00020 }
00021 }
00022
00023 void RTCGLbody::input()
00024 {
00025 if (m_qIn.isNew()){
00026 while (m_qIn.isNew()) m_qIn.read();
00027 m_body->setPosture(m_q.data.get_buffer());
00028 }
00029 if (m_posIn.isNew()){
00030 while (m_posIn.isNew()) m_posIn.read();
00031 m_body->setPosition(m_pos.data.x,
00032 m_pos.data.y,
00033 m_pos.data.z);
00034 }
00035 if (m_rpyIn.isNew()){
00036 while (m_rpyIn.isNew()) m_rpyIn.read();
00037 m_body->setRotation(m_rpy.data.r,
00038 m_rpy.data.p,
00039 m_rpy.data.y);
00040 }
00041 }