BodyRTC.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include <hrpModel/Sensor.h>
3 #include <hrpModel/Link.h>
4 #include "BodyRTC.h"
5 #include "PortHandler.h"
6 
7 using namespace hrp;
8 using namespace RTC;
9 
10 // Module specification
11 // <rtc-template block="module_spec">
12 const char* BodyRTC::bodyrtc_spec[] =
13 {
14  "implementation_id", "BodyRTC",
15  "type_name", "BodyRTC",
16  "description", "BodyRTC component",
17  "version", "0.1",
18  "vendor", "AIST",
19  "category", "Generic",
20  "activity_type", "DataFlowComponent",
21  "max_instance", "10",
22  "language", "C++",
23  "lang_type", "compile",
24  // Configuration variables
25 
26  ""
27 };
28 
30  : Body(),
31  DataFlowComponentBase(manager),
32  m_RobotHardwareServicePort("RobotHardwareService"),
33  m_resetPosition(true),
34  dummy(0)
35 {
36  //std::cout << "constructor of BodyRTC" << std::endl;
37 }
38 
40 {
41  //std::cout << "destructor of BodyRTC" << std::endl;
42  for (size_t i=0; i<m_inports.size(); i++){
43  delete m_inports[i];
44  }
45  for (size_t i=0; i<m_outports.size(); i++){
46  delete m_outports[i];
47  }
48 }
49 
50 void BodyRTC::writeDataPorts(double time)
51 {
52  for (size_t i=0; i<m_outports.size(); i++){
53  m_outports[i]->update(time);
54  }
55 }
56 
58 {
59  for (size_t i=0; i<m_inports.size(); i++){
60  m_inports[i]->update();
61  }
62 }
63 
64 #define DEFAULT_ANGLE_ERROR_LIMIT 0.2 // [rad] // copied from robot.cpp
65 RTC::ReturnCode_t BodyRTC::setup(){
66  std::cout << "BodyRTC::setup(), numJoints = " << numJoints() << std::endl;
67  angles.resize(numJoints());
68  commands.resize(numJoints());
72  calib_status.resize(numJoints());
73  servo_status.resize(numJoints());
74  power_status.resize(numJoints());
75  m_servoErrorLimit.resize(numJoints());
76  for(unsigned int i = 0; i < numJoints(); i++) {
77  calib_status[i] = servo_status[i] = power_status[i] = OpenHRP::RobotHardwareService::SWITCH_ON;
79  }
80  m_emergencyReason = EMG_NONE; // clear
81  m_emergencyId = -1;
82  return RTC::RTC_OK;
83 }
84 
85 void parsePortConfig(const std::string &config,
86  std::string &name, std::string &type,
87  std::vector<std::string> &elements)
88 {
89  std::string::size_type colon = 0, start=0;
90  colon = config.find(':', start);
91  if (colon == std::string::npos){
92  std::cerr << "can't find the first separator in [" << config << "]"
93  << std::endl;
94  return;
95  }
96  name = config.substr(start, colon);
97  start = colon+1;
98  colon = config.find(':', start);
99  if (colon == std::string::npos){
100  type = config.substr(start);
101  return;
102  }
103  std::string elist = config.substr(start, colon-start);
104  std::string::size_type comma;
105  start = 0;
106  comma = elist.find(',', start);
107  while (comma != std::string::npos){
108  std::string e = elist.substr(start, comma-start);
109  elements.push_back(e);
110  start = comma+1;
111  comma = elist.find(',', start);
112  }
113  elements.push_back(elist.substr(start));
114  start = colon+1;
115  type = config.substr(start);
116 }
117 
118 bool getJointList(hrp::Body *body, const std::vector<std::string> &elements,
119  std::vector<hrp::Link *> &joints)
120 {
121  if (elements.size() == 0){
122  for (unsigned int i=0; i<body->numJoints(); i++){
123  joints.push_back(body->joint(i));
124  }
125  }else{
126  for (size_t i=0; i<elements.size(); i++){
127  hrp::Link *j = body->link(elements[i]);
128  if (j){
129  joints.push_back(j);
130  }else{
131  std::cerr << "can't find a joint(" << elements[i] << ")"
132  << std::endl;
133  return false;
134  }
135  }
136  }
137  return true;
138 }
139 
140 void BodyRTC::createInPort(const std::string &config)
141 {
142  std::string name, type;
143  std::vector<std::string> elements;
144  parsePortConfig(config, name, type, elements);
145  if (type == "JOINT_VALUE"){
146  std::vector<hrp::Link *> joints;
147  if (getJointList(this, elements, joints)){
148  m_inports.push_back(
149  new JointValueInPortHandler(this, name.c_str(), joints, &servo_status));
150  }
151  }else if(type == "JOINT_VELOCITY"){
152  std::vector<hrp::Link *> joints;
153  if (getJointList(this, elements, joints)){
154  m_inports.push_back(
155  new JointVelocityInPortHandler(this, name.c_str(), joints, &servo_status));
156  }
157  }else if(type == "JOINT_ACCELERATION"){
158  std::vector<hrp::Link *> joints;
159  if (getJointList(this, elements, joints)){
160  m_inports.push_back(
161  new JointAccelerationInPortHandler(this, name.c_str(),joints, &servo_status));
162  }
163  }else if(type == "JOINT_TORQUE"){
164  std::vector<hrp::Link *> joints;
165  if (getJointList(this, elements, joints)){
166  m_inports.push_back(
167  new JointTorqueInPortHandler(this, name.c_str(), joints, &servo_status));
168  }
169  }else if(type == "EXTERNAL_FORCE"){
170  std::cout << "EXTERNAL_FORCE is not implemented yet" << std::endl;
171  }else if(type == "ABS_TRANSFORM"){
172  std::vector<hrp::Link *> joints;
173  if (getJointList(this, elements, joints) && joints.size() == 1){
174  m_inports.push_back(
175  new AbsTransformInPortHandler(this, name.c_str(), joints[0]));
176  } else if (elements.size() == 1) {
177  hrp::Link *l=this->link(elements[0]);
178  if (l){
179  m_inports.push_back(
180  new AbsTransformInPortHandler(this, name.c_str(), l));
181  return;
182  }
183  std::cerr << "can't find a link(or a sensor)(" << elements[0] << ")"
184  << std::endl;
185  }
186  }else if(type == "ABS_VELOCITY"){
187  std::vector<hrp::Link *> joints;
188  if (getJointList(this, elements, joints) && joints.size() == 1){
189  m_inports.push_back(
190  new AbsVelocityInPortHandler(this, name.c_str(), joints[0]));
191  }
192  }else if(type == "ABS_ACCELERATION"){
193  std::vector<hrp::Link *> joints;
194  if (getJointList(this, elements, joints) && joints.size() == 1){
195  m_inports.push_back(
196  new AbsAccelerationInPortHandler(this,name.c_str(),joints[0]));
197  }
198  }else if(type == "FRAME_RATE"){
199  VisionSensor *s = this->sensor<VisionSensor>(elements[0]);
200  if (!s){
201  std::cerr << "can't find a sensor(" << elements[0] << ")"
202  << std::endl;
203  return;
204  }
205  m_inports.push_back(new FrameRateInPortHandler(this,name.c_str(),s));
206  }else if(type == "LIGHT_SWITCH"){
207  Light *l = this->light(elements[0]);
208  if (!l){
209  std::cerr << "can't find a light(" << elements[0] << ")"
210  << std::endl;
211  return;
212  }
213  m_inports.push_back(new LightSwitchInPortHandler(this,name.c_str(),l));
214  }else{
215  std::cerr << "unknown InPort data type(" << type << ")" << std::endl;
216  }
217 }
218 
219 void BodyRTC::createOutPort(const std::string &config)
220 {
221  std::string name, type;
222  std::vector<std::string> elements;
223  parsePortConfig(config, name, type, elements);
224  if (type == "JOINT_VALUE"){
225  std::vector<hrp::Link *> joints;
226  if (getJointList(this, elements, joints)){
227  m_outports.push_back(
228  new JointValueOutPortHandler(this, name.c_str(), joints));
229  }
230  }else if(type == "JOINT_VELOCITY"){
231  std::vector<hrp::Link *> joints;
232  if (getJointList(this, elements, joints)){
233  m_outports.push_back(
234  new JointVelocityOutPortHandler(this, name.c_str(), joints));
235  }
236  }else if(type == "JOINT_ACCELERATION"){
237  std::vector<hrp::Link *> joints;
238  if (getJointList(this, elements, joints)){
239  m_outports.push_back(
240  new JointAccelerationOutPortHandler(this,name.c_str(),joints));
241  }
242  }else if(type == "JOINT_TORQUE"){
243  std::vector<hrp::Link *> joints;
244  if (getJointList(this, elements, joints)){
245  m_outports.push_back(
246  new JointTorqueOutPortHandler(this, name.c_str(), joints));
247  }
248  }else if(type == "ABS_TRANSFORM"){
249  if (elements.size()!=1){
250  std::cerr << "link name is not specified for port " << name
251  << std::endl;
252  return;
253  }
254  hrp::Link *l=this->link(elements[0]);
255  if (l){
256  m_outports.push_back(
257  new AbsTransformOutPortHandler(this, name.c_str(), l));
258  return;
259  }
260  hrp::Sensor *s;
261  s = this->sensor<AccelSensor>(elements[0]);
262  if (!s) s = this->sensor<RateGyroSensor>(elements[0]);
263  if (!s) s = this->sensor<ForceSensor>(elements[0]);
264  if (!s) s = this->sensor<RangeSensor>(elements[0]);
265  if (!s) s = this->sensor<VisionSensor>(elements[0]);
266  if (s){
267  m_outports.push_back(
268  new AbsTransformOutPortHandler(this, name.c_str(), s));
269  return;
270  }
271  std::cerr << "can't find a link(or a sensor)(" << elements[0] << ")"
272  << std::endl;
273  }else if(type == "ABS_VELOCITY"){
274  if (elements.size()!=1){
275  std::cerr << "link name is not specified for port " << name
276  << std::endl;
277  return;
278  }
279  hrp::Link *l=this->link(elements[0]);
280  if (l){
281  m_outports.push_back(
282  new AbsVelocityOutPortHandler(this, name.c_str(), l));
283  }else{
284  std::cerr << "can't find a link(" << elements[0] << ")"
285  << std::endl;
286  }
287  }else if(type == "ABS_ACCELERATION"){
288  if (elements.size()!=1){
289  std::cerr << "link name is not specified for port " << name
290  << std::endl;
291  return;
292  }
293  hrp::Link *l=this->link(elements[0]);
294  if (l){
295  m_outports.push_back(
296  new AbsAccelerationOutPortHandler(this, name.c_str(), l));
297  }else{
298  std::cerr << "can't find a link(" << elements[0] << ")"
299  << std::endl;
300  }
301  }else if(type == "FORCE_SENSOR"){
302  if (elements.size()!=1){
303  std::cerr << "sensor name is not specified for port" << name
304  << std::endl;
305  return;
306  }
307  ForceSensor *s = this->sensor<ForceSensor>(elements[0]);
308  if (!s){
309  std::cerr << "can't find a sensor(" << elements[0] << ")"
310  << std::endl;
311  return;
312  }
313  m_outports.push_back(new ForceSensorPortHandler(this, name.c_str(),s));
314 
315  }else if(type == "RATE_GYRO_SENSOR"){
316  if (elements.size()!=1){
317  std::cerr << "sensor name is not specified for port " << name
318  << std::endl;
319  return;
320  }
321  RateGyroSensor *s = this->sensor<RateGyroSensor>(elements[0]);
322  if (!s){
323  std::cerr << "can't find a sensor(" << elements[0] << ")"
324  << std::endl;
325  return;
326  }
327  m_outports.push_back(new RateGyroSensorPortHandler(this, name.c_str(),
328  s));
329  }else if(type == "ACCELERATION_SENSOR"){
330  if (elements.size()!=1){
331  std::cerr << "sensor name is not specified for port " << name
332  << std::endl;
333  return;
334  }
335  AccelSensor *s = this->sensor<AccelSensor>(elements[0]);
336  if (!s){
337  std::cerr << "can't find a sensor(" << elements[0] << ")"
338  << std::endl;
339  return;
340  }
341  m_outports.push_back(new AccelSensorPortHandler(this, name.c_str(),s));
342 
343  }else if(type == "RANGE_SENSOR"){
344  if (elements.size()!=1){
345  std::cerr << "sensor name is not specified for port " << name
346  << std::endl;
347  return;
348  }
349  RangeSensor *s = this->sensor<RangeSensor>(elements[0]);
350  if (!s){
351  std::cerr << "can't find a sensor(" << elements[0] << ")"
352  << std::endl;
353  return;
354  }
355  m_outports.push_back(new RangeSensorPortHandler(this, name.c_str(),s));
356 
357  }else if(type == "VISION_SENSOR"){
358  if (elements.size()!=1){
359  std::cerr << "sensor name is not specified for port " << name
360  << std::endl;
361  return;
362  }
363  VisionSensor *s = this->sensor<VisionSensor>(elements[0]);
364  if (!s){
365  std::cerr << "can't find a sensor(" << elements[0] << ")"
366  << std::endl;
367  return;
368  }
369  m_outports.push_back(new VisionSensorPortHandler(this,name.c_str(),s));
370  }else if(type == "POINT_CLOUD"){
371  if (elements.size()!=1){
372  std::cerr << "sensor name is not specified for port " << name
373  << std::endl;
374  return;
375  }
376  VisionSensor *s = this->sensor<VisionSensor>(elements[0]);
377  if (!s){
378  std::cerr << "can't find a sensor(" << elements[0] << ")"
379  << std::endl;
380  return;
381  }
382  m_outports.push_back(new PointCloudPortHandler(this,name.c_str(),s));
383  }else if(type == "CONSTRAINT_FORCE"){
384  std::cout << "CONSTRAINT_FORCE is not implemented yet" << std::endl;
385  }else{
386  std::cerr << "unknown OutPort data type(" << type << ")" << std::endl;
387  }
388 
389  m_outports.push_back(new EmergencySignalPortHandler(this, "emergencySignal", this));
390  m_outports.push_back(new ServoStatePortHandler(this, "servoState", this));
391 
392  m_service0.setRobot(this);
393  m_RobotHardwareServicePort.registerProvider("service0", "RobotHardwareService", m_service0);
395 }
396 
397 bool BodyRTC::names2ids(const std::vector<std::string> &i_names,
398  std::vector<int> &o_ids)
399 {
400  bool ret = true;
401  for (unsigned int i=0; i<i_names.size(); i++){
402  hrp::Link *l = this->link(i_names[i].c_str());
403  if (!l){
404  std::cout << "joint named [" << i_names[i] << "] not found"
405  << std::endl;
406  ret = false;
407  }else{
408  o_ids.push_back(l->jointId);
409  }
410  }
411  return ret;
412 }
413 
414 void BodyRTC::getStatus(OpenHRP::RobotHardwareService::RobotState* rs) {
415  rs->angle.length(numJoints());
416  rs->command.length(numJoints());
417  for(size_t i = 0; i < numJoints(); i++) {
418  rs->angle[i] = angles[i];
419  rs->command[i] = commands[i];
420  }
421  rs->force.length(forces.size());
422  for(size_t j = 0; j < forces.size(); j++) {
423  rs->force[j].length(6);
424  for(size_t i = 0; i < 6; i++ ) rs->force[j][i] = forces[j][i];
425  }
426  rs->rateGyro.length(gyros.size());
427  for(size_t j = 0; j < gyros.size() ; j++) {
428  rs->rateGyro[j].length(3);
429  for(size_t i = 0; i < 3; i++ ) rs->rateGyro[j][i] = gyros[j][i];
430  }
431  rs->accel.length(accels.size());
432  for(size_t j = 0; j < accels.size(); j++) {
433  rs->accel[j].length(3);
434  for(size_t i = 0; i < 3; i++ ) rs->accel[j][i] = accels[j][i];
435  }
436 
437  rs->servoState.length(numJoints());
438  int v, status;
439  for(unsigned int i=0; i < rs->servoState.length(); ++i){
440  //size_t len = lengthOfExtraServoState(i)+1;
441  rs->servoState[i].length(1);
442  status = 0;
443  v = readCalibState(i);
444  status |= v<< OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT;
445  v = readPowerState(i);
446  status |= v<< OpenHRP::RobotHardwareService::POWER_STATE_SHIFT;
447  v = readServoState(i);
448  status |= v<< OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT;
449  //v = readServoAlarm(i);
450  //status |= v<< OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT;
451  //v = readDriverTemperature(i);
452  //status |= v<< OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT;
453  rs->servoState[i][0] = status;
454  //readExtraServoState(i, (int *)(rs->servoState[i].get_buffer()+1));
455  }
456  //readPowerStatus(rs->voltage, rs->current);
457 }
458 
459 void BodyRTC::getStatus2(OpenHRP::RobotHardwareService::RobotState2* rs) {
460 }
461 
462 bool BodyRTC::setServoErrorLimit(const char *i_jname, double i_limit)
463 {
464  Link *l = NULL;
465  if (strcmp(i_jname, "all") == 0 || strcmp(i_jname, "ALL") == 0){
466  for (unsigned int i=0; i<numJoints(); i++){
467  m_servoErrorLimit[i] = i_limit;
468  }
469  }else if ((l = link(i_jname))){
470  m_servoErrorLimit[l->jointId] = i_limit;
471  }else{
472  char *s = (char *)i_jname; while(*s) {*s=toupper(*s);s++;}
473  const std::vector<int> jgroup = m_jointGroups[i_jname];
474  if (jgroup.size()==0) return false;
475  for (unsigned int i=0; i<jgroup.size(); i++){
476  m_servoErrorLimit[jgroup[i]] = i_limit;
477  }
478  }
479  return true;
480 }
481 
482 char *time_string()
483 {
484  struct timeval tv;
485  gettimeofday(&tv, NULL);
486  struct tm *tm_ = localtime(&tv.tv_sec);
487  static char time[20];
488  sprintf(time, "%02d:%02d:%02d.%06d", tm_->tm_hour, tm_->tm_min, tm_->tm_sec, (int)tv.tv_usec);
489  return time;
490 }
491 
492 #define ON 1
493 #define OFF 0
494 bool BodyRTC::checkEmergency(emg_reason &o_reason, int &o_id) {
495  int state;
496 
497  o_reason = EMG_NONE; // clear
498  o_id = -1;
499 
500  for (unsigned int i=0; i<numJoints(); i++){
501  state = readServoState(i);
502  if (state == ON && m_servoErrorLimit[i] != 0){
503  double angle, command;
504  angle = angles[i];
505  command = commands[i];
506  if (fabs(angle-command) > m_servoErrorLimit[i]){
507  std::cerr << time_string()
508  << ": servo error limit over: joint = "
509  << joint(i)->name
510  << ", qRef = " << command/M_PI*180 << "[deg], q = "
511  << angle/M_PI*180 << "[deg]" << std::endl;
512  o_reason = EMG_SERVO_ERROR;
513  o_id = i;
514  return true;
515  }
516  }
517  }
518  return false;
519 }
520 
522  // Simulate servo off in HighGain mode simulation
523  hrp::Vector3 g(0, 0, 9.8);
524  calcCM();
525  rootLink()->calcSubMassCM();
526  bool all_servo_off = true;
527  bool emulate_highgain_servo_off_mode = (numJoints() > 0); // If no joints, do not use servo off emulation
528  for(unsigned int i = 0; i < numJoints(); ++i){
529  Link *j = joint(i);
530  commands[i] = j->q;
531  int p = readPowerState(i);
532  int s = readServoState(i);
533  if ( p && s ) { all_servo_off = false; continue; }
534  switch(j->jointType){
535  case Link::ROTATIONAL_JOINT:
536  {
537  j->q += (j->subm*g).cross(j->submwc / j->subm - j->p).dot(j->R * j->a) *0.005*0.01;
538  if ( j->q < j->llimit ) {
539  j->q = j->llimit;
540  }else if ( j->q > j->ulimit ) {
541  j->q = j->ulimit;
542  }
543  }
544  break;
545 
546  default:
547  std::cerr << "calcCMJacobian() : unsupported jointType("
548  << j->jointType << std::endl;
549  }
550  }
551  if ( m_resetPosition ) {
555  m_resetPosition = false;
556  }
557  if (emulate_highgain_servo_off_mode) {
558  if ( all_servo_off ) { // when all servo is off, do not move root joint
561  } else {
564  }
565  }
566  return true;
567 }
568 
570 
571  for(unsigned int i = 0; i < numJoints(); ++i){
572  angles[i] = joint(i)->q;
573  }
574  for(unsigned int i = 0; i < numSensors(hrp::Sensor::ACCELERATION); i++ ){
575  hrp::AccelSensor *s = sensor<AccelSensor>(i);
576  accels[i][0] = s->dv[0];
577  accels[i][1] = s->dv[1];
578  accels[i][2] = s->dv[2];
579  }
580  for(unsigned int i = 0; i < numSensors(hrp::Sensor::RATE_GYRO); i++ ){
581  hrp::RateGyroSensor *s = sensor<RateGyroSensor>(i);
582  gyros[i][0] = s->w[0];
583  gyros[i][1] = s->w[1];
584  gyros[i][2] = s->w[2];
585  }
586  for(unsigned int i = 0; i < numSensors(hrp::Sensor::FORCE); i++ ){
587  hrp::ForceSensor *s = sensor<ForceSensor>(i);
588  forces[i][0] = s->f[0];
589  forces[i][1] = s->f[1];
590  forces[i][2] = s->f[2];
591  forces[i][3] = s->tau[0];
592  forces[i][4] = s->tau[1];
593  forces[i][5] = s->tau[2];
594  }
596  servo("all", false);
597  }
598  return true;
599 }
600 
601 // lib/io/iob.h:
602 #define JID_ALL -1
603 #define JID_INVALID -2
604 // these code are copied from rtc/RobotHardware/robot.cpp
605 bool BodyRTC::servo(const char *jname, bool turnon)
606 {
607  Link *l = NULL;
608  if (strcmp(jname, "all") == 0 || strcmp(jname, "ALL") == 0){
609  bool ret = true;
610  for (unsigned int i=0; i<numJoints(); i++){
611  ret = ret && servo(i, turnon);
612  }
613  return ret;
614  }else if ((l = link(jname))){
615  return servo(l->jointId, turnon);
616  }else{
617  char *s = (char *)jname; while(*s) {*s=toupper(*s);s++;}
618  const std::vector<int> jgroup = m_jointGroups[jname];
619  if (jgroup.size() == 0) return false;
620  bool ret = true;
621  for (unsigned int i=0; i<jgroup.size(); i++){
622  ret = ret && servo(jgroup[i], turnon);
623  return ret;
624  }
625  }
626  return false;
627 }
628 
629 bool BodyRTC::power(const char *jname, bool turnon)
630 {
631  int jid = JID_INVALID;
632 
633  if (strcmp(jname, "all") == 0 || strcmp(jname, "ALL") == 0){
634  jid = JID_ALL;
635  }else{
636  Link *l = link(jname);
637  if (!l) return false;
638  jid = l->jointId;
639  }
640  return power(jid, turnon);
641 }
642 
644 }
646 }
647 
648 void RobotHardwareServicePort::getStatus(OpenHRP::RobotHardwareService::RobotState_out rs) {
649  rs = new OpenHRP::RobotHardwareService::RobotState();
650  m_robot->getStatus(rs);
651 }
652 
653 void RobotHardwareServicePort::getStatus2(OpenHRP::RobotHardwareService::RobotState2_out rs) {
654  rs = new OpenHRP::RobotHardwareService::RobotState2();
655  m_robot->getStatus2(rs);
656 }
657 
658 CORBA::Boolean RobotHardwareServicePort::power(const char* jname, OpenHRP::RobotHardwareService::SwitchStatus turnon) {
659  return m_robot->power(jname, turnon == OpenHRP::RobotHardwareService::SWITCH_ON);
660 }
661 
662 CORBA::Boolean RobotHardwareServicePort::servo(const char* jname, OpenHRP::RobotHardwareService::SwitchStatus turnon) {
663  return m_robot->servo(jname, turnon == OpenHRP::RobotHardwareService::SWITCH_ON);
664 }
665 void RobotHardwareServicePort::setServoGainPercentage(const char *jname, double limit) {
666 }
667 void RobotHardwareServicePort::setServoTorqueGainPercentage(const char *jname, double limit) {
668 }
669 void RobotHardwareServicePort::setServoErrorLimit(const char *jname, double limit) {
670  m_robot->setServoErrorLimit(jname, limit);
671 }
672 void RobotHardwareServicePort::setJointControlMode(const char *jname, OpenHRP::RobotHardwareService::JointControlMode jcm){
673 }
675 }
677 }
678 void RobotHardwareServicePort::initializeJointAngle(const char* name, const char* option) {
679 }
680 CORBA::Boolean RobotHardwareServicePort::addJointGroup(const char* gname, const OpenHRP::RobotHardwareService::StrSequence& jnames) {
681  char *s = (char *)gname; while(*s) {*s=toupper(*s);s++;}
682  std::vector<int> jids;
683  std::vector<std::string> joints;
684  joints.resize(jnames.length());
685  for (unsigned int i=0; i<jnames.length(); i++){
686  joints[i] = jnames[i];
687  }
688  bool ret = m_robot->names2ids(joints, jids);
689  //m_jointGroups[gname] = jids;
690  m_robot->addJointGroup(gname, jids);
691  return ret;
692 }
693 CORBA::Boolean RobotHardwareServicePort::readDigitalInput(::OpenHRP::RobotHardwareService::OctSequence_out din) {
694  return false;
695 }
697  return 0;
698 }
699 CORBA::Boolean RobotHardwareServicePort::writeDigitalOutput(const ::OpenHRP::RobotHardwareService::OctSequence& dout) {
700  return false;
701 }
702 CORBA::Boolean RobotHardwareServicePort::writeDigitalOutputWithMask(const ::OpenHRP::RobotHardwareService::OctSequence& dout, const ::OpenHRP::RobotHardwareService::OctSequence& mask) {
703  return false;
704 }
706  return 0;
707 }
708 CORBA::Boolean RobotHardwareServicePort::readDigitalOutput(::OpenHRP::RobotHardwareService::OctSequence_out dout) {
709  return false;
710 }
711 
712 CORBA::Boolean RobotHardwareServicePort::setJointInertia(const char* name, ::CORBA::Double mn)
713 {
714  return true;
715 }
716 
717 void RobotHardwareServicePort::setJointInertias(const ::OpenHRP::RobotHardwareService::DblSequence& mns)
718 {
719 }
723 //
725 
726 template <class _Delete>
728 {
729  // BodyRTC will be released when BodyPtr is released
730 }
731 
733 {
735  manager->registerFactory(profile,
736  RTC::Create<BodyRTC>,
737  DummyDelete<BodyRTC>
738  //RTC::Delete<BodyRTC>
739  );
740 }
std::vector< OpenHRP::RobotHardwareService::SwitchStatus > servo_status
Definition: BodyRTC.h:163
hrp::Vector3 m_lastServoOn_p
Definition: BodyRTC.h:169
void createInPort(const std::string &config)
Definition: BodyRTC.cpp:140
png_infop png_charpp int png_charpp profile
unsigned int numSensors(int sensorType) const
void setJointControlMode(const char *jname, OpenHRP::RobotHardwareService::JointControlMode jcm)
Definition: BodyRTC.cpp:672
std::vector< hrp::dvector6 > forces
Definition: BodyRTC.h:161
BodyRTC::emg_reason m_emergencyReason
Definition: BodyRTC.h:141
emg_reason
Definition: BodyRTC.h:122
void setServoErrorLimit(const char *jname, double limit)
Definition: BodyRTC.cpp:669
void setServoTorqueGainPercentage(const char *jname, double limit)
Definition: BodyRTC.cpp:667
Link * rootLink() const
std::vector< OpenHRP::RobotHardwareService::SwitchStatus > calib_status
Definition: BodyRTC.h:162
void setDisturbanceObserverGain(::CORBA::Double gain)
Definition: BodyRTC.cpp:722
png_infop png_charp png_int_32 png_int_32 int * type
CORBA::Long lengthDigitalInput()
Definition: BodyRTC.cpp:696
RobotHardwareServicePort m_service0
Definition: BodyRTC.h:154
state
void setJointInertias(const ::OpenHRP::RobotHardwareService::DblSequence &mns)
Definition: BodyRTC.cpp:717
unsigned int numJoints() const
const std::vector< Link *> & joints() const
CORBA::Boolean setJointInertia(const char *name, ::CORBA::Double mn)
Definition: BodyRTC.cpp:712
hrp::Matrix33 m_lastServoOn_R
Definition: BodyRTC.h:170
RTC::ReturnCode_t setup()
Definition: BodyRTC.cpp:65
int m_emergencyId
Definition: BodyRTC.h:142
bool power(int jid, bool turnon)
Definition: BodyRTC.h:93
Vector3 calcCM()
BodyRTC(RTC::Manager *manager=&RTC::Manager::instance())
Definition: BodyRTC.cpp:29
char * time_string()
Definition: BodyRTC.cpp:482
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
std::vector< InPortHandlerBase * > m_inports
Definition: BodyRTC.h:147
RTC::CorbaPort m_RobotHardwareServicePort
Definition: BodyRTC.h:153
std::vector< hrp::Vector3 > gyros
Definition: BodyRTC.h:160
void getDefaultRootPosition(Vector3 &out_p, Matrix33 &out_R)
int readPowerState(const int i)
Definition: BodyRTC.h:105
Link * link(int index) const
CORBA::Boolean readDigitalInput(::OpenHRP::RobotHardwareService::OctSequence_out din)
Definition: BodyRTC.cpp:693
std::vector< double > commands
Definition: BodyRTC.h:158
png_uint_32 i
bool names2ids(const std::vector< std::string > &i_names, std::vector< int > &o_ids)
Definition: BodyRTC.cpp:397
void getStatus2(OpenHRP::RobotHardwareService::RobotState2 *rs)
Definition: BodyRTC.cpp:459
void setRobot(BodyRTC *i_robot)
Definition: BodyRTC.cpp:724
void setServoGainPercentage(const char *jname, double limit)
Definition: BodyRTC.cpp:665
int readServoState(const int i)
Definition: BodyRTC.h:106
std::map< std::string, std::vector< int > > m_jointGroups
Definition: BodyRTC.h:165
static void moduleInit(RTC::Manager *)
Definition: BodyRTC.cpp:732
Eigen::Vector3d Vector3
CORBA::Boolean addJointGroup(const char *gname, const OpenHRP::RobotHardwareService::StrSequence &jnames)
Definition: BodyRTC.cpp:680
bool checkEmergency(emg_reason &o_reason, int &o_id)
Definition: BodyRTC.cpp:494
CORBA::Long lengthDigitalOutput()
Definition: BodyRTC.cpp:705
#define JID_INVALID
Definition: BodyRTC.cpp:603
bool m_resetPosition
Definition: BodyRTC.h:168
int gettimeofday(struct timeval *tv, struct timezone *tz)
static const char * bodyrtc_spec[]
Definition: BodyRTC.h:145
void parsePortConfig(const std::string &config, std::string &name, std::string &type, std::vector< std::string > &elements)
Definition: BodyRTC.cpp:85
CORBA::Boolean writeDigitalOutput(const ::OpenHRP::RobotHardwareService::OctSequence &dout)
Definition: BodyRTC.cpp:699
CORBA::Boolean readDigitalOutput(::OpenHRP::RobotHardwareService::OctSequence_out dout)
Definition: BodyRTC.cpp:708
virtual ~BodyRTC(void)
Definition: BodyRTC.cpp:39
bool servo(const char *jname, bool turnon)
Definition: BodyRTC.cpp:605
const std::string & name()
void writeDataPorts(double time)
Definition: BodyRTC.cpp:50
bool setServoErrorLimit(const char *i_jname, double i_limit)
Definition: BodyRTC.cpp:462
void enableDisturbanceObserver()
Definition: BodyRTC.cpp:720
CORBA::Boolean writeDigitalOutputWithMask(const ::OpenHRP::RobotHardwareService::OctSequence &dout, const ::OpenHRP::RobotHardwareService::OctSequence &mask)
Definition: BodyRTC.cpp:702
png_size_t start
void DummyDelete(RTC::RTObject_impl *rtc)
Definition: BodyRTC.cpp:727
Light * light(const std::string &name)
#define DEFAULT_ANGLE_ERROR_LIMIT
Definition: BodyRTC.cpp:64
Vector3 cross(const Vector3 &v1, const Vector3 &v2)
#define M_PI
std::vector< OutPortHandlerBase * > m_outports
Definition: BodyRTC.h:150
void getStatus2(OpenHRP::RobotHardwareService::RobotState2_out rs)
Definition: BodyRTC.cpp:653
CORBA::Boolean servo(const char *jname, OpenHRP::RobotHardwareService::SwitchStatus ss)
Definition: BodyRTC.cpp:662
std::string sprintf(char const *__restrict fmt,...)
CORBA::Boolean power(const char *jname, OpenHRP::RobotHardwareService::SwitchStatus ss)
Definition: BodyRTC.cpp:658
bool addPort(PortBase &port)
std::vector< double > m_servoErrorLimit
Definition: BodyRTC.h:128
void initializeJointAngle(const char *name, const char *option)
Definition: BodyRTC.cpp:678
hrp::BodyPtr m_robot
void disableDisturbanceObserver()
Definition: BodyRTC.cpp:721
std::vector< hrp::Vector3 > accels
Definition: BodyRTC.h:159
static std::vector< double > command
Definition: iob.cpp:8
void getStatus(OpenHRP::RobotHardwareService::RobotState_out rs)
Definition: BodyRTC.cpp:648
bool postOneStep()
Definition: BodyRTC.cpp:569
std::vector< double > angles
Definition: BodyRTC.h:157
bool getJointList(hrp::Body *body, const std::vector< std::string > &elements, std::vector< hrp::Link *> &joints)
Definition: BodyRTC.cpp:118
std::vector< OpenHRP::RobotHardwareService::SwitchStatus > power_status
Definition: BodyRTC.h:164
void getStatus(OpenHRP::RobotHardwareService::RobotState *rs)
Definition: BodyRTC.cpp:414
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
#define JID_ALL
Definition: BodyRTC.cpp:602
void readDataPorts()
Definition: BodyRTC.cpp:57
#define ON
Definition: BodyRTC.cpp:492
Link * joint(int id) const
bool preOneStep()
Definition: BodyRTC.cpp:521
bool registerProvider(const char *instance_name, const char *type_name, PortableServer::RefCountServantBase &provider)
int readCalibState(const int i)
Definition: BodyRTC.h:104
void createOutPort(const std::string &config)
Definition: BodyRTC.cpp:219


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20