RTCGLbody.cpp
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 }


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