VirtualCamera/RTCGLbody.cpp
Go to the documentation of this file.
1 #include <rtm/DataFlowComponentBase.h>
2 #include "hrpsys/util/GLbody.h"
3 #include "hrpsys/util/GLlink.h"
4 #include "RTCGLbody.h"
5 
7  m_body(i_body),
8  m_qIn("q", m_q),
9  m_posIn("pos", m_pos),
10  m_rpyIn("rpy", m_rpy),
11  m_poseBaseIn("poseBase", m_poseBase)
12 {
13  if (m_body->numJoints() > 0) comp->addInPort("q", m_qIn);
15  comp->addInPort("pos", m_posIn);
16  comp->addInPort("rpy", m_rpyIn);
17  comp->addInPort("poseBase", m_poseBaseIn);
18  }
19 }
20 
21 void RTCGLbody::input()
22 {
23  if (m_qIn.isNew()){
24  while (m_qIn.isNew()) m_qIn.read();
25  m_body->setPosture(m_q.data.get_buffer());
26  }
27  if (m_posIn.isNew()){
28  while (m_posIn.isNew()) m_posIn.read();
29  m_body->setPosition(m_pos.data.x,
30  m_pos.data.y,
31  m_pos.data.z);
32  }
33  if (m_rpyIn.isNew()){
34  while (m_rpyIn.isNew()) m_rpyIn.read();
35  m_body->setRotation(m_rpy.data.r,
36  m_rpy.data.p,
37  m_rpy.data.y);
38  }
39  if (m_poseBaseIn.isNew()){
40  while (m_poseBaseIn.isNew()) m_poseBaseIn.read();
41  m_body->setPosition(m_poseBase.data.position.x,
42  m_poseBase.data.position.y,
43  m_poseBase.data.position.z);
44  m_body->setRotation(m_poseBase.data.orientation.r,
45  m_poseBase.data.orientation.p,
46  m_poseBase.data.orientation.y);
47  }
48 }
RTC::InPort< RTC::TimedPose3D > m_poseBaseIn
RTC::TimedOrientation3D m_rpy
void setPosition(double x, double y, double z)
Definition: GLbody.cpp:27
RTC::TimedPose3D m_poseBase
Link * rootLink() const
RTC::InPort< RTC::TimedOrientation3D > m_rpyIn
Definition: GLbody.h:11
void setRotation(const double *R)
Definition: GLbody.cpp:37
RTC::InPort< RTC::TimedPoint3D > m_posIn
RTC::TimedDoubleSeq m_q
RTC::InPort< RTC::TimedDoubleSeq > m_qIn
GLbody * m_body
void setPosture(const double *i_angles)
Definition: GLbody.cpp:21
RTC::TimedPoint3D m_pos
unsigned int numJoints() const
virtual bool isNew()
bool addInPort(const char *name, InPortBase &inport)
RTCGLbody(GLbody *i_body, RTC::DataFlowComponentBase *comp)


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:51