-
Notifications
You must be signed in to change notification settings - Fork 87
/
RTCGLbody.cpp
48 lines (46 loc) · 1.58 KB
/
RTCGLbody.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
#include <rtm/DataFlowComponentBase.h>
#include "hrpsys/util/GLbody.h"
#include "hrpsys/util/GLlink.h"
#include "RTCGLbody.h"
RTCGLbody::RTCGLbody(GLbody *i_body, RTC::DataFlowComponentBase *comp) :
m_body(i_body),
m_qIn("q", m_q),
m_posIn("pos", m_pos),
m_rpyIn("rpy", m_rpy),
m_poseBaseIn("poseBase", m_poseBase)
{
if (m_body->numJoints() > 0) comp->addInPort("q", m_qIn);
if (m_body->rootLink()->jointType == hrp::Link::FREE_JOINT){
comp->addInPort("pos", m_posIn);
comp->addInPort("rpy", m_rpyIn);
comp->addInPort("poseBase", m_poseBaseIn);
}
}
void RTCGLbody::input()
{
if (m_qIn.isNew()){
while (m_qIn.isNew()) m_qIn.read();
m_body->setPosture(m_q.data.get_buffer());
}
if (m_posIn.isNew()){
while (m_posIn.isNew()) m_posIn.read();
m_body->setPosition(m_pos.data.x,
m_pos.data.y,
m_pos.data.z);
}
if (m_rpyIn.isNew()){
while (m_rpyIn.isNew()) m_rpyIn.read();
m_body->setRotation(m_rpy.data.r,
m_rpy.data.p,
m_rpy.data.y);
}
if (m_poseBaseIn.isNew()){
while (m_poseBaseIn.isNew()) m_poseBaseIn.read();
m_body->setPosition(m_poseBase.data.position.x,
m_poseBase.data.position.y,
m_poseBase.data.position.z);
m_body->setRotation(m_poseBase.data.orientation.r,
m_poseBase.data.orientation.p,
m_poseBase.data.orientation.y);
}
}