RobotHardware.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include <rtm/CorbaNaming.h>
11 #include "hrpsys/util/VectorConvert.h"
12 #include "RobotHardware.h"
13 #include "robot.h"
14 
15 #include <hrpModel/Sensor.h>
16 #include <hrpModel/ModelLoaderUtil.h>
17 
18 using namespace OpenHRP;
19 using namespace hrp;
20 
21 // Module specification
22 // <rtc-template block="module_spec">
23 static const char* robothardware_spec[] =
24  {
25  "implementation_id", "RobotHardware",
26  "type_name", "RobotHardware",
27  "description", "RobotHardware",
28  "version", HRPSYS_PACKAGE_VERSION,
29  "vendor", "AIST",
30  "category", "example",
31  "activity_type", "DataFlowComponent",
32  "max_instance", "1",
33  "language", "C++",
34  "lang_type", "compile",
35  // Configuration variables
36  "conf.default.isDemoMode", "0",
37  "conf.default.fzLimitRatio", "2.0",
38  "conf.default.servoErrorLimit", ",",
39  "conf.default.jointAccelerationLimit", "0",
40  "conf.default.servoOnDelay", "0",
41 
42  ""
43  };
44 // </rtc-template>
45 
47  : RTC::DataFlowComponentBase(manager),
48  // <rtc-template block="initializer">
49  m_isDemoMode(0),
50  m_qRefIn("qRef", m_qRef),
51  m_dqRefIn("dqRef", m_dqRef),
52  m_ddqRefIn("ddqRef", m_ddqRef),
53  m_tauRefIn("tauRef", m_tauRef),
54  m_qOut("q", m_q),
55  m_dqOut("dq", m_dq),
56  m_tauOut("tau", m_tau),
57  m_ctauOut("ctau", m_ctau),
58  m_pdtauOut("pdtau", m_pdtau),
59  m_servoStateOut("servoState", m_servoState),
60  m_emergencySignalOut("emergencySignal", m_emergencySignal),
61  m_rstate2Out("rstate2", m_rstate2),
62  m_RobotHardwareServicePort("RobotHardwareService"),
63  // </rtc-template>
64  dummy(0)
65 {
66 }
67 
69 {
70 }
71 
72 
73 RTC::ReturnCode_t RobotHardware::onInitialize()
74 {
75  // Registration: InPort/OutPort/Service
76  // <rtc-template block="registration">
77 
78  addInPort("qRef", m_qRefIn);
79  addInPort("dqRef", m_dqRefIn);
80  addInPort("ddqRef", m_ddqRefIn);
81  addInPort("tauRef", m_tauRefIn);
82 
83  addOutPort("q", m_qOut);
84  addOutPort("dq", m_dqOut);
85  addOutPort("tau", m_tauOut);
86  addOutPort("ctau", m_ctauOut);
87  addOutPort("pdtau", m_pdtauOut);
88  addOutPort("servoState", m_servoStateOut);
89  addOutPort("emergencySignal", m_emergencySignalOut);
90  addOutPort("rstate2", m_rstate2Out);
91 
92  // Set service provider to Ports
93  m_RobotHardwareServicePort.registerProvider("service0", "RobotHardwareService", m_service0);
94 
95  // Set service consumers to Ports
96 
97  // Set CORBA Service Ports
99 
100  // </rtc-template>
101 
103  double dt = 0.0;
104  coil::stringTo(dt, prop["dt"].c_str());
105  if (!dt) {
106  std::cerr << m_profile.instance_name << ": joint command velocity check is disabled" << std::endl;
107  }
109 
110 
111  RTC::Manager& rtcManager = RTC::Manager::instance();
112  std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
113  int comPos = nameServer.find(",");
114  if (comPos < 0){
115  comPos = nameServer.length();
116  }
117  nameServer = nameServer.substr(0, comPos);
118  RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
119  if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
120  CosNaming::NamingContext::_duplicate(naming.getRootContext())
121  )){
122  if (prop["model"] == ""){
123  std::cerr << "[" << m_profile.instance_name << "] path to the model file is not defined. Please check the configuration file" << std::endl;
124  }else{
125  std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl;
126  }
127  }
128 
129  std::vector<std::string> keys = prop.propertyNames();
130  for (unsigned int i=0; i<keys.size(); i++){
131  m_robot->setProperty(keys[i].c_str(), prop[keys[i]].c_str());
132  }
133  std::cout << "dof = " << m_robot->numJoints() << std::endl;
134  if (!m_robot->init()) return RTC::RTC_ERROR;
135 
137 
138  m_q.data.length(m_robot->numJoints());
139  m_dq.data.length(m_robot->numJoints());
140  m_tau.data.length(m_robot->numJoints());
141  m_ctau.data.length(m_robot->numJoints());
142  m_pdtau.data.length(m_robot->numJoints());
143  m_servoState.data.length(m_robot->numJoints());
144  m_qRef.data.length(m_robot->numJoints());
145  m_dqRef.data.length(m_robot->numJoints());
146  m_ddqRef.data.length(m_robot->numJoints());
147  m_tauRef.data.length(m_robot->numJoints());
148 
149  int ngyro = m_robot->numSensors(Sensor::RATE_GYRO);
150  std::cout << "the number of gyros = " << ngyro << std::endl;
151  m_rate.resize(ngyro);
152  m_rateOut.resize(ngyro);
153  for (unsigned int i=0; i<m_rate.size(); i++){
154  Sensor *s = m_robot->sensor(Sensor::RATE_GYRO, i);
155  std::cout << s->name << std::endl;
157  registerOutPort(s->name.c_str(), *m_rateOut[i]);
158  }
159 
160  int nacc = m_robot->numSensors(Sensor::ACCELERATION);
161  std::cout << "the number of accelerometers = " << nacc << std::endl;
162  m_acc.resize(nacc);
163  m_accOut.resize(nacc);
164  for (unsigned int i=0; i<m_acc.size(); i++){
165  Sensor *s = m_robot->sensor(Sensor::ACCELERATION, i);
166  std::cout << s->name << std::endl;
167  m_accOut[i] = new OutPort<TimedAcceleration3D>(s->name.c_str(), m_acc[i]);
168  registerOutPort(s->name.c_str(), *m_accOut[i]);
169  }
170 
171  int nforce = m_robot->numSensors(Sensor::FORCE);
172  std::cout << "the number of force sensors = " << nforce << std::endl;
173  m_force.resize(nforce);
174  m_forceOut.resize(nforce);
175  for (unsigned int i=0; i<m_force.size(); i++){
176  Sensor *s = m_robot->sensor(Sensor::FORCE, i);
177  std::cout << s->name << std::endl;
178  m_forceOut[i] = new OutPort<TimedDoubleSeq>(s->name.c_str(), m_force[i]);
179  m_force[i].data.length(6);
180  registerOutPort(s->name.c_str(), *m_forceOut[i]);
181  }
182 
183  // <rtc-template block="bind_config">
184  // Bind variables and configuration variable
185  bindParameter("isDemoMode", m_isDemoMode, "0");
186  bindParameter("servoErrorLimit", m_robot->m_servoErrorLimit, ",");
187  bindParameter("fzLimitRatio", m_robot->m_fzLimitRatio, "2");
188  bindParameter("jointAccelerationLimit", m_robot->m_accLimit, "0");
189  bindParameter("servoOnDelay", m_robot->m_servoOnDelay, "0");
190 
191  // </rtc-template>
192 
193  return RTC::RTC_OK;
194 }
195 
196 
197 /*
198 RTC::ReturnCode_t RobotHardware::onFinalize()
199 {
200  return RTC::RTC_OK;
201 }
202 */
203 
204 /*
205 RTC::ReturnCode_t RobotHardware::onStartup(RTC::UniqueId ec_id)
206 {
207  return RTC::RTC_OK;
208 }
209 */
210 
211 /*
212 RTC::ReturnCode_t RobotHardware::onShutdown(RTC::UniqueId ec_id)
213 {
214  return RTC::RTC_OK;
215 }
216 */
217 
218 /*
219 RTC::ReturnCode_t RobotHardware::onActivated(RTC::UniqueId ec_id)
220 {
221  return RTC::RTC_OK;
222 }
223 */
224 
225 /*
226 RTC::ReturnCode_t RobotHardware::onDeactivated(RTC::UniqueId ec_id)
227 {
228  return RTC::RTC_OK;
229 }
230 */
231 
232 RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id)
233 {
234  //std::cout << "RobotHardware:onExecute(" << ec_id << ")" << std::endl;
235  Time tm;
236  this->getTimeNow(tm);
237 
238  if (!m_isDemoMode){
239  robot::emg_reason reason;
240  int id;
241  if (m_robot->checkEmergency(reason, id)){
242  if (reason == robot::EMG_SERVO_ERROR || reason == robot::EMG_POWER_OFF){
243  m_robot->servo("all", false);
244  m_emergencySignal.data = reason;
246  } else if (reason == robot::EMG_SERVO_ALARM) {
247  m_emergencySignal.data = reason;
249  }
250  }
251  }
252 
253  if (m_qRefIn.isNew()){
254  m_qRefIn.read();
255  //std::cout << "RobotHardware: qRef[21] = " << m_qRef.data[21] << std::endl;
256  if (!m_isDemoMode
257  && m_robot->checkJointCommands(m_qRef.data.get_buffer())){
258  m_robot->servo("all", false);
261  }else{
262  // output to iob
263  m_robot->writeJointCommands(m_qRef.data.get_buffer());
264  }
265  }
266  if (m_dqRefIn.isNew()){
267  m_dqRefIn.read();
268  //std::cout << "RobotHardware: dqRef[21] = " << m_dqRef.data[21] << std::endl;
269  // output to iob
270  m_robot->writeVelocityCommands(m_dqRef.data.get_buffer());
271  }
272  if (m_ddqRefIn.isNew()){
273  m_ddqRefIn.read();
274  //std::cout << "RobotHardware: dqRef[21] = " << m_dqRef.data[21] << std::endl;
275  // output to iob
276  m_robot->writeAccelerationCommands(m_ddqRef.data.get_buffer());
277  }
278  if (m_tauRefIn.isNew()){
279  m_tauRefIn.read();
280  //std::cout << "RobotHardware: tauRef[21] = " << m_tauRef.data[21] << std::endl;
281  // output to iob
282  m_robot->writeTorqueCommands(m_tauRef.data.get_buffer());
283  }
284 
285  // read from iob
286  m_robot->readJointAngles(m_q.data.get_buffer());
287  m_q.tm = tm;
288  m_robot->readJointVelocities(m_dq.data.get_buffer());
289  m_dq.tm = tm;
290  m_robot->readJointTorques(m_tau.data.get_buffer());
291  m_tau.tm = tm;
292  m_robot->readJointCommandTorques(m_ctau.data.get_buffer());
293  m_ctau.tm = tm;
294  m_robot->readPDControllerTorques(m_pdtau.data.get_buffer());
295  m_pdtau.tm = tm;
296  for (unsigned int i=0; i<m_rate.size(); i++){
297  double rate[3];
298  m_robot->readGyroSensor(i, rate);
299  m_rate[i].data.avx = rate[0];
300  m_rate[i].data.avy = rate[1];
301  m_rate[i].data.avz = rate[2];
302  m_rate[i].tm = tm;
303  }
304 
305  for (unsigned int i=0; i<m_acc.size(); i++){
306  double acc[3];
307  m_robot->readAccelerometer(i, acc);
308  m_acc[i].data.ax = acc[0];
309  m_acc[i].data.ay = acc[1];
310  m_acc[i].data.az = acc[2];
311  m_acc[i].tm = tm;
312  }
313 
314  for (unsigned int i=0; i<m_force.size(); i++){
315  m_robot->readForceSensor(i, m_force[i].data.get_buffer());
316  m_force[i].tm = tm;
317  }
318 
319  for (unsigned int i=0; i<m_servoState.data.length(); i++){
320  size_t len = m_robot->lengthOfExtraServoState(i)+1;
321  m_servoState.data[i].length(len);
322  int status = 0, v;
323  v = m_robot->readCalibState(i);
324  status |= v<< OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT;
325  v = m_robot->readPowerState(i);
326  status |= v<< OpenHRP::RobotHardwareService::POWER_STATE_SHIFT;
327  v = m_robot->readServoState(i);
328  status |= v<< OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT;
329  v = m_robot->readServoAlarm(i);
330  status |= v<< OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT;
331  v = m_robot->readDriverTemperature(i);
332  status |= v<< OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT;
333  m_servoState.data[i][0] = status;
334  m_robot->readExtraServoState(i, (int *)(m_servoState.data[i].get_buffer()+1));
335  }
336  m_servoState.tm = tm;
337 
338  getStatus2(m_rstate2.data);
339  m_rstate2.tm = tm;
340 
341  m_robot->oneStep();
342 
343  m_qOut.write();
344  m_dqOut.write();
345  m_tauOut.write();
346  m_ctauOut.write();
347  m_pdtauOut.write();
349  for (unsigned int i=0; i<m_rateOut.size(); i++){
350  m_rateOut[i]->write();
351  }
352  for (unsigned int i=0; i<m_accOut.size(); i++){
353  m_accOut[i]->write();
354  }
355  for (unsigned int i=0; i<m_forceOut.size(); i++){
356  m_forceOut[i]->write();
357  }
359 
360  return RTC::RTC_OK;
361 }
362 
363 template<class T>
365 {
366  rstate.angle.length(robot->numJoints());
367  robot->readJointAngles(rstate.angle.get_buffer());
368 
369  rstate.command.length(robot->numJoints());
370  robot->readJointCommands(rstate.command.get_buffer());
371 
372  rstate.torque.length(robot->numJoints());
373  if (!robot->readJointTorques(rstate.torque.get_buffer())){
374  for (unsigned int i=0; i<rstate.torque.length(); i++){
375  rstate.torque[i] = 0.0;
376  }
377  }
378 
379  rstate.servoState.length(robot->numJoints());
380  int v, status;
381  for(unsigned int i=0; i < rstate.servoState.length(); ++i){
382  size_t len = robot->lengthOfExtraServoState(i)+1;
383  rstate.servoState[i].length(len);
384  status = 0;
385  v = robot->readCalibState(i);
386  status |= v<< OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT;
387  v = robot->readPowerState(i);
388  status |= v<< OpenHRP::RobotHardwareService::POWER_STATE_SHIFT;
389  v = robot->readServoState(i);
390  status |= v<< OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT;
391  v = robot->readServoAlarm(i);
392  status |= v<< OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT;
393  v = robot->readDriverTemperature(i);
394  status |= v<< OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT;
395  rstate.servoState[i][0] = status;
396  robot->readExtraServoState(i, (int *)(rstate.servoState[i].get_buffer()+1));
397  }
398 
399  rstate.rateGyro.length(robot->numSensors(Sensor::RATE_GYRO));
400  for (unsigned int i=0; i<rstate.rateGyro.length(); i++){
401  rstate.rateGyro[i].length(3);
402  robot->readGyroSensor(i, rstate.rateGyro[i].get_buffer());
403  }
404 
405  rstate.accel.length(robot->numSensors(Sensor::ACCELERATION));
406  for (unsigned int i=0; i<rstate.accel.length(); i++){
407  rstate.accel[i].length(3);
408  robot->readAccelerometer(i, rstate.accel[i].get_buffer());
409  }
410 
411  rstate.force.length(robot->numSensors(Sensor::FORCE));
412  for (unsigned int i=0; i<rstate.force.length(); i++){
413  rstate.force[i].length(6);
414  robot->readForceSensor(i, rstate.force[i].get_buffer());
415  }
416 
417  robot->readPowerStatus(rstate.voltage, rstate.current);
418 }
419 
420 void RobotHardware::getStatus2(OpenHRP::RobotHardwareService::RobotState2 &rstate2)
421 {
422  getStatus(m_robot, rstate2);
423 #if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 2
424  rstate2.batteries.length(m_robot->numBatteries());
425  for(unsigned int i=0; i<rstate2.batteries.length(); i++){
426  m_robot->readBatteryState(i,
427  rstate2.batteries[i].voltage,
428  rstate2.batteries[i].current,
429  rstate2.batteries[i].soc);
430  }
431  rstate2.temperature.length(m_robot->numThermometers());
432  for (unsigned int i=0; i<rstate2.temperature.length(); i++){
433  m_robot->readThermometer(i, rstate2.temperature[i]);
434  }
435 #endif
436 }
437 
438 /*
439 RTC::ReturnCode_t RobotHardware::onAborting(RTC::UniqueId ec_id)
440 {
441  return RTC::RTC_OK;
442 }
443 */
444 
445 /*
446 RTC::ReturnCode_t RobotHardware::onError(RTC::UniqueId ec_id)
447 {
448  return RTC::RTC_OK;
449 }
450 */
451 
452 /*
453 RTC::ReturnCode_t RobotHardware::onReset(RTC::UniqueId ec_id)
454 {
455  return RTC::RTC_OK;
456 }
457 */
458 
459 /*
460 RTC::ReturnCode_t RobotHardware::onStateUpdate(RTC::UniqueId ec_id)
461 {
462  return RTC::RTC_OK;
463 }
464 */
465 
466 /*
467 RTC::ReturnCode_t RobotHardware::onRateChanged(RTC::UniqueId ec_id)
468 {
469  return RTC::RTC_OK;
470 }
471 */
472 
473 
474 
475 extern "C"
476 {
477 
479  {
481  manager->registerFactory(profile,
482  RTC::Create<RobotHardware>,
483  RTC::Delete<RobotHardware>);
484  }
485 
486 };
487 
488 
ComponentProfile m_profile
png_infop png_charpp int png_charpp profile
list keys
TimedDoubleSeq m_q
array of actual angles of joint with jointId
TimedDoubleSeq m_pdtau
array of PD controller torques of joint with jointId
std::string name
bool stringTo(To &val, const char *str)
InPort< TimedDoubleSeq > m_qRefIn
virtual RTC::ReturnCode_t onInitialize()
RobotHardwareService_impl m_service0
RTC::CorbaPort m_RobotHardwareServicePort
TimedDoubleSeq m_tau
array of actual torques of joint with jointId
InPort< TimedDoubleSeq > m_ddqRefIn
RobotHardware(RTC::Manager *manager)
Constructor.
std::vector< TimedAcceleration3D > m_acc
vector of actual acceleration (vector length = number of acceleration sensors)
std::vector< OutPort< TimedDoubleSeq > * > m_forceOut
TimedDoubleSeq m_tauRef
array of reference torques of joint with jointId
png_uint_32 i
coil::Properties & getProperties()
static Manager & instance()
bool addOutPort(const char *name, OutPortBase &outport)
OpenHRP::RobotHardwareService::TimedRobotState2 m_rstate2
TimedLong m_emergencySignal
TimedDoubleSeq m_ddqRef
array of reference accelerations of joint with jointId
boost::shared_ptr< robot > m_robot
std::vector< TimedAngularVelocity3D > m_rate
vector of actual angular velocity (vector length = number of rate sensors)
InPort< TimedDoubleSeq > m_tauRefIn
TimedDoubleSeq m_dqRef
array of reference velocities of joint with jointId
void RobotHardwareInit(RTC::Manager *manager)
OutPort< TimedDoubleSeq > m_dqOut
ExecutionContextHandle_t UniqueId
std::vector< OutPort< TimedAngularVelocity3D > * > m_rateOut
void getStatus2(OpenHRP::RobotHardwareService::RobotState2 &rstate2)
OutPort< OpenHRP::RobotHardwareService::TimedRobotState2 > m_rstate2Out
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
int loadBodyFromModelLoader(::World *world, const char *name, const char *url, CosNaming::NamingContext_var cxt)
emg_reason
reasons of emergency
OutPort< TimedLong > m_emergencySignalOut
TimedDoubleSeq m_qRef
array of reference angles of joint with jointId
void getStatus(boost::shared_ptr< robot > robot, T &rstate)
prop
virtual ~RobotHardware()
Destructor.
naming
TimedDoubleSeq m_dq
array of actual velocities of joint with jointId
static const char * robothardware_spec[]
std::vector< OutPort< TimedAcceleration3D > * > m_accOut
std::vector< TimedDoubleSeq > m_force
vector of actual 6D wrench (vector length = number of F/T sensors) 6D wrench vector = 3D force + 3D m...
virtual bool isNew()
OutPort< TimedDoubleSeq > m_pdtauOut
OutPort< TimedDoubleSeq > m_qOut
OutPort< TimedDoubleSeq > m_ctauOut
static int id
bool addPort(PortBase &port)
virtual bool write(DataType &value)
TimedDoubleSeq m_ctau
array of commanded torques of joint with jointId
OutPort< OpenHRP::TimedLongSeqSeq > m_servoStateOut
std::vector< std::string > propertyNames(void) const
JSAMPIMAGE data
bool addInPort(const char *name, InPortBase &inport)
virtual void getTimeNow(Time &tm)
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
OutPort< TimedDoubleSeq > m_tauOut
void setRobot(boost::shared_ptr< robot > &i_robot)
void registerOutPort(const char *name, OutPortBase &outport)
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
robot hardware component
OpenHRP::TimedLongSeqSeq m_servoState
InPort< TimedDoubleSeq > m_dqRefIn
bool registerProvider(const char *instance_name, const char *type_name, PortableServer::RefCountServantBase &provider)


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