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