RTCBody.cpp
Go to the documentation of this file.
1 #include <hrpModel/Link.h>
2 #include <hrpModel/Sensor.h>
3 #include "RTCBody.h"
4 
5 using namespace hrp;
6 
8  m_tauIn("tau", m_tau),
9  m_qCmdIn("qCmd", m_qCmd),
10  m_dqCmdIn("dqCmd", m_dqCmd),
11  m_ddqCmdIn("ddqCmd", m_ddqCmd),
12  m_posOut("pos", m_pos),
13  m_rpyOut("rpy", m_rpy),
14  m_qOut("q", m_q),
15  m_highgain(false)
16 {
17 }
18 
20 {
21 }
22 
24 {
25  // input ports
26  for (unsigned int i=0; i<numJoints(); i++){
27  Link *l = joint(i);
28  if (l){
29  if (l->isHighGainMode){
30  m_highgain = true;
31  break;
32  }
33  }
34  }
35 
36  if (numJoints()){
37  if (m_highgain){
38  comp->addInPort("qCmd", m_qCmdIn);
39  comp->addInPort("dqCmd", m_dqCmdIn);
40  comp->addInPort("ddqCmd", m_ddqCmdIn);
41  }else{
42  comp->addInPort("tau", m_tauIn);
43  }
44  }
45 
46  // output ports
47  m_q.data.length(numJoints());
48  if (numJoints() > 0) comp->addOutPort("q", m_qOut);
49  if (rootLink()->jointType == Link::FREE_JOINT){
50  comp->addOutPort("pos", m_posOut);
51  comp->addOutPort("rpy", m_rpyOut);
52  }
53 
54  int ngyro = numSensors(Sensor::RATE_GYRO);
55  m_rate.resize(ngyro);
56  m_rateOut.resize(ngyro);
57  for (unsigned int i=0; i<m_rate.size(); i++){
58  Sensor *s = sensor(Sensor::RATE_GYRO, i);
60  comp->addOutPort(s->name.c_str(), *m_rateOut[i]);
61  }
62 
63  int nacc = numSensors(Sensor::ACCELERATION);
64  m_acc.resize(nacc);
65  m_accOut.resize(nacc);
66  for (unsigned int i=0; i<m_acc.size(); i++){
67  Sensor *s = sensor(Sensor::ACCELERATION, i);
69  comp->addOutPort(s->name.c_str(), *m_accOut[i]);
70  }
71 
72  int nforce = numSensors(Sensor::FORCE);
73  m_force.resize(nforce);
74  m_forceOut.resize(nforce);
75  for (unsigned int i=0; i<m_force.size(); i++){
76  Sensor *s = sensor(Sensor::FORCE, i);
78  m_force[i]);
79  m_force[i].data.length(6);
80  comp->addOutPort(s->name.c_str(), *m_forceOut[i]);
81  }
82 }
83 
85 {
86  if (m_highgain){
87  if (m_qCmdIn.isNew()){
88  do {
89  m_qCmdIn.read();
90  }while (m_qCmdIn.isNew());
91  for (unsigned int i=0; i<numJoints(); i++){
92  Link *l = joint(i);
93  if (l){
94  l->q = m_qCmd.data[l->jointId];
95  }
96  }
97  }
98  if (m_dqCmdIn.isNew()){
99  do {
100  m_dqCmdIn.read();
101  }while (m_dqCmdIn.isNew());
102  for (unsigned int i=0; i<numJoints(); i++){
103  Link *l = joint(i);
104  if (l){
105  l->dq = m_dqCmd.data[l->jointId];
106  }
107  }
108  }
109  if (m_ddqCmdIn.isNew()){
110  do {
111  m_ddqCmdIn.read();
112  }while (m_ddqCmdIn.isNew());
113  for (unsigned int i=0; i<numJoints(); i++){
114  Link *l = joint(i);
115  if (l){
116  l->ddq = m_ddqCmd.data[l->jointId];
117  }
118  }
119  }
120  }else{
121  if (m_tauIn.isNew()){
122  do {
123  m_tauIn.read();
124  }while (m_tauIn.isNew());
125  for (unsigned int i=0; i<numJoints(); i++){
126  Link *l = joint(i);
127  if (l){
128  l->u = m_tau.data[l->jointId];
129  }
130  }
131  }
132  }
133 }
134 
135 void RTCBody::output(OpenHRP::RobotState& state)
136 {
137  if (numJoints() > 0){
138  for (unsigned int i=0; i<numJoints(); i++){
139  Link *l = joint(i);
140  if (l){
141  m_q.data[l->jointId] = l->q;
142  }
143  }
144  state.q = m_q.data;
145  m_qOut.write();
146  }
147 
148  Link *root = rootLink();
149 
150  m_pos.data.x = root->p[0];
151  m_pos.data.y = root->p[1];
152  m_pos.data.z = root->p[2];
153  state.basePose.position = m_pos.data;
154 
155  Vector3 rpy(rpyFromRot(root->attitude()));
156  m_rpy.data.r = rpy[0];
157  m_rpy.data.p = rpy[1];
158  m_rpy.data.y = rpy[2];
159  state.basePose.orientation = m_rpy.data;
160 
161  if (root->jointType == Link::FREE_JOINT){
162  m_posOut.write();
163  m_rpyOut.write();
164  }
165 
166  int n = numSensors(Sensor::FORCE);
167  for(int id = 0; id < n; ++id){
168  ForceSensor* s = sensor<ForceSensor>(id);
169  setVector3(s->f, m_force[id].data, 0);
170  setVector3(s->tau, m_force[id].data, 3);
171  }
172 
173  n = numSensors(Sensor::RATE_GYRO);
174  for(int id=0; id < n; ++id){
175  RateGyroSensor* s = sensor<RateGyroSensor>(id);
176  m_rate[id].data.avx = s->w[0];
177  m_rate[id].data.avy = s->w[1];
178  m_rate[id].data.avz = s->w[2];
179  }
180 
181  n = numSensors(Sensor::ACCELERATION);
182  for(int id=0; id < n; ++id){
183  AccelSensor* s = sensor<AccelSensor>(id);
184  m_acc[id].data.ax = s->dv[0];
185  m_acc[id].data.ay = s->dv[1];
186  m_acc[id].data.az = s->dv[2];
187  }
188 }
189 
void output(OpenHRP::RobotState &state)
Definition: RTCBody.cpp:135
std::vector< RTC::TimedDoubleSeq > m_force
Definition: RTCBody.h:36
RTC::InPort< RTC::TimedDoubleSeq > m_tauIn
Definition: RTCBody.h:26
RTC::OutPort< RTC::TimedPoint3D > m_posOut
Definition: RTCBody.h:38
std::string name
RTCBody()
Definition: RTCBody.cpp:7
Link * rootLink() const
bool m_highgain
Definition: RTCBody.h:45
void setVector3(const Vector3 &v3, V &v, size_t top=0)
png_uint_32 i
RTC::InPort< RTC::TimedDoubleSeq > m_dqCmdIn
Definition: RTCBody.h:28
unsigned int numSensors(int sensorType) const
RTC::TimedDoubleSeq m_dqCmd
Definition: RTCBody.h:24
bool addOutPort(const char *name, OutPortBase &outport)
RTC::TimedPoint3D m_pos
Definition: RTCBody.h:31
void createPorts(RTC::DataFlowComponentBase *comp)
Definition: RTCBody.cpp:23
RTC::InPort< RTC::TimedDoubleSeq > m_qCmdIn
Definition: RTCBody.h:27
OpenHRP::vector3 Vector3
RTC::TimedDoubleSeq m_tau
Definition: RTCBody.h:23
RTC::InPort< RTC::TimedDoubleSeq > m_ddqCmdIn
Definition: RTCBody.h:29
RTC::TimedOrientation3D m_rpy
Definition: RTCBody.h:32
std::vector< RTC::OutPort< RTC::TimedAngularVelocity3D > * > m_rateOut
Definition: RTCBody.h:42
std::vector< RTC::TimedAcceleration3D > m_acc
Definition: RTCBody.h:34
n
std::vector< RTC::OutPort< RTC::TimedDoubleSeq > * > m_forceOut
Definition: RTCBody.h:43
RTC::OutPort< RTC::TimedOrientation3D > m_rpyOut
Definition: RTCBody.h:39
Link * joint(int id) const
void input()
Definition: RTCBody.cpp:84
HRP_UTIL_EXPORT Vector3 rpyFromRot(const Matrix33 &m)
~RTCBody()
Definition: RTCBody.cpp:19
unsigned int numJoints() const
virtual bool isNew()
static int id
virtual bool write(DataType &value)
RTC::OutPort< RTC::TimedDoubleSeq > m_qOut
Definition: RTCBody.h:40
std::vector< RTC::OutPort< RTC::TimedAcceleration3D > * > m_accOut
Definition: RTCBody.h:41
bool addInPort(const char *name, InPortBase &inport)
std::vector< RTC::TimedAngularVelocity3D > m_rate
Definition: RTCBody.h:35
Sensor * sensor(int sensorType, int sensorId) const
RTC::TimedDoubleSeq m_qCmd
Definition: RTCBody.h:24
RTC::TimedDoubleSeq m_ddqCmd
Definition: RTCBody.h:24
RTC::TimedDoubleSeq m_q
Definition: RTCBody.h:33


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