ServoController.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include <rtm/CorbaNaming.h>
11 #include <hrpModel/Link.h>
12 #include <hrpModel/ModelLoaderUtil.h>
13 #include "ServoController.h"
14 #include "hrpsys/util/VectorConvert.h"
15 
16 #include "ServoSerial.h"
17 
18 using namespace std;
19 
20 // Module specification
21 // <rtc-template block="module_spec">
22 static const char* nullcomponent_spec[] =
23  {
24  "implementation_id", "ServoController",
25  "type_name", "ServoController",
26  "description", "null component",
27  "version", HRPSYS_PACKAGE_VERSION,
28  "vendor", "AIST",
29  "category", "example",
30  "activity_type", "DataFlowComponent",
31  "max_instance", "10",
32  "language", "C++",
33  "lang_type", "compile",
34  // Configuration variables
35  ""
36  };
37 // </rtc-template>
38 
40  : RTC::DataFlowComponentBase(manager),
41  // <rtc-template block="initializer">
42  m_ServoControllerServicePort("ServoControllerService")
43  // </rtc-template>
44 {
45  m_service0.servo(this);
46 }
47 
49 {
50 }
51 
52 
53 
54 RTC::ReturnCode_t ServoController::onInitialize()
55 {
56  std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
57  // <rtc-template block="bind_config">
58  // Bind variables and configuration variable
59 
60  // </rtc-template>
61 
62  // Registration: InPort/OutPort/Service
63  // <rtc-template block="registration">
64  // Set InPort buffers
65 
66  // Set OutPort buffer
67 
68  // Set service provider to Ports
69  m_ServoControllerServicePort.registerProvider("service0", "ServoControllerService", m_service0);
70 
71  // Set service consumers to Ports
72 
73  // Set CORBA Service Ports
75 
76  // </rtc-template>
77 
79 
80  // get servo.devname
81  std::string devname = prop["servo.devname"];
82  if ( devname == "" ) {
83  std::cerr << "\e[1;31m[WARNING] " << m_profile.instance_name << ": needs servo.devname property\e[0m" << std::endl;
84  std::cerr << "\e[1;31m[WARNING] " << m_profile.instance_name << ": running in dummy mode\e[0m" << std::endl;
85  return RTC::RTC_OK;
86  }
87 
88  // get servo.id
89  coil::vstring servo_ids = coil::split(prop["servo.id"], ",");
90  if ( servo_ids.size() == 0 ) {
91  std::cerr << "\e[1;31m[ERROR] " << m_profile.instance_name << ": needs servo.id property\e[0m" << std::endl;
92  return RTC::RTC_ERROR;
93  }
94  servo_id.resize(servo_ids.size());
95  for(unsigned int i = 0; i < servo_ids.size(); i++) {
96  coil::stringTo(servo_id[i], servo_ids[i].c_str());
97  }
98 
99  std::cout << m_profile.instance_name << ": servo_id : ";
100  for(unsigned int i = 0; i < servo_id.size(); i++) {
101  std::cerr << servo_id[i] << " ";
102  }
103  std::cerr << std::endl;
104 
105  // get servo.offset
106  coil::vstring servo_offsets = coil::split(prop["servo.offset"], ",");
107  if ( servo_offsets.size() == 0 ) {
108  servo_offset.resize(servo_ids.size());
109  } else {
110  servo_offset.resize(servo_offsets.size());
111  }
112  if ( servo_ids.size() != servo_offset.size() ) {
113  std::cerr << "\e[1;31m[ERROR] " << m_profile.instance_name << ": servo.id and servo.offset property must have same length\e[0m" << std::endl;
114  return RTC::RTC_ERROR;
115  }
116  for(unsigned int i = 0; i < servo_offsets.size(); i++) {
117  coil::stringTo(servo_offset[i], servo_offsets[i].c_str());
118  }
119 
120  std::cout << m_profile.instance_name << ": servo_offset : ";
121  for(unsigned int i = 0; i < servo_offset.size(); i++) {
122  std::cerr << servo_offset[i] << " ";
123  }
124  std::cerr << std::endl;
125 
126  // get servo.dir
127  coil::vstring servo_dirs = coil::split(prop["servo.dir"], ",");
128  if ( servo_dirs.size() == 0 ) {
129  servo_dir.assign(servo_ids.size(), 1);
130  } else {
131  servo_dir.assign(servo_dirs.size(), 1);
132  }
133  if ( servo_ids.size() != servo_dir.size() ) {
134  std::cerr << "\e[1;31m[ERROR] " << m_profile.instance_name << ": servo.id and servo.dir property must have same length\e[0m" << std::endl;
135  return RTC::RTC_ERROR;
136  }
137  for(unsigned int i = 0; i < servo_dirs.size(); i++) {
138  coil::stringTo(servo_dir[i], servo_dirs[i].c_str());
139  }
140 
141  std::cout << m_profile.instance_name << ": servo_dir : ";
142  for(unsigned int i = 0; i < servo_dir.size(); i++) {
143  std::cerr << servo_dir[i] << " ";
144  }
145  std::cerr << std::endl;
146 
147  serial = new ServoSerial((char *)(devname.c_str()));
148 
149  return RTC::RTC_OK;
150 }
151 
152 
153 
154 RTC::ReturnCode_t ServoController::onFinalize()
155 {
156  if ( serial ) delete serial;
157  return RTC::RTC_OK;
158 }
159 
160 /*
161 RTC::ReturnCode_t ServoController::onStartup(RTC::UniqueId ec_id)
162 {
163  return RTC::RTC_OK;
164 }
165 */
166 
167 /*
168 RTC::ReturnCode_t ServoController::onShutdown(RTC::UniqueId ec_id)
169 {
170  return RTC::RTC_OK;
171 }
172 */
173 
175 {
176  std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
177  if ( ! serial ) return RTC::RTC_OK;
178 
179  return RTC::RTC_OK;
180 }
181 
183 {
184  std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
185  if ( ! serial ) return RTC::RTC_OK;
186 
187  return RTC::RTC_OK;
188 }
189 
191 {
192  return RTC::RTC_OK;
193 }
194 
195 /*
196 RTC::ReturnCode_t ServoController::onAborting(RTC::UniqueId ec_id)
197 {
198  return RTC::RTC_OK;
199 }
200 */
201 
202 /*
203 RTC::ReturnCode_t ServoController::onError(RTC::UniqueId ec_id)
204 {
205  return RTC::RTC_OK;
206 }
207 */
208 
209 /*
210 RTC::ReturnCode_t ServoController::onReset(RTC::UniqueId ec_id)
211 {
212  return RTC::RTC_OK;
213 }
214 */
215 
216 /*
217 RTC::ReturnCode_t ServoController::onStateUpdate(RTC::UniqueId ec_id)
218 {
219  return RTC::RTC_OK;
220 }
221 */
222 
223 /*
224 RTC::ReturnCode_t ServoController::onRateChanged(RTC::UniqueId ec_id)
225 {
226  return RTC::RTC_OK;
227 }
228 */
229 
230 bool ServoController::setJointAngle(short id, double angle, double tm)
231 {
232  if ( ! serial ) return true;
233  double rad = angle * M_PI / 180;
234  for(unsigned int i=0; i<servo_id.size(); i++){
235  if(servo_id[i]==id) serial->setPosition(id,rad+servo_offset[i], tm);
236  }
237  return true;
238 }
239 
240 bool ServoController::setJointAngles(const OpenHRP::ServoControllerService::dSequence angles, double tm)
241 {
242  if ( ! serial ) return true;
243 
244  int id[servo_id.size()];
245  double tms[servo_id.size()];
246  double rad[servo_id.size()];
247  for( unsigned int i = 0; i < servo_id.size(); i++ ) {
248  id[i] = servo_id[i];
249  tms[i] = tm;
250  rad[i] = (angles.get_buffer()[i]*servo_dir[i]+servo_offset[i]);
251  }
252  if ( angles.length() != servo_id.size() ) {
253  std::cerr << "[ERROR] " << m_profile.instance_name << ": size of servo.id(" << angles.length() << ") is not correct, expected" << servo_id.size() << std::endl;
254  return false;
255  }
256  serial->setPositions(servo_id.size(), id, rad, tms);
257  return true;
258 }
259 
260 bool ServoController::getJointAngle(short id, double &angle)
261 {
262  if ( ! serial ) return true;
263 
264  int ret = serial->getPosition(id, &angle);
265  for(unsigned int i=0; i<servo_id.size(); i++){
266  if(servo_id[i]==id){
267  double servo_offset_angle = servo_offset[i] * 180 / M_PI;
268  angle -= servo_offset_angle;
269  }
270  }
271 
272  if (ret < 0) return false;
273  return true;
274 }
275 
276 bool ServoController::getJointAngles(OpenHRP::ServoControllerService::dSequence_out &angles)
277 {
278  if ( ! serial ) return true;
279 
280  int ret;
281 
282  angles = new OpenHRP::ServoControllerService::dSequence();
283  angles->length(servo_id.size());
284  for(unsigned int i=0; i < servo_id.size(); i++){
285  ret = serial->getPosition(servo_id[i], &(angles->get_buffer()[i]));
286  if (ret < 0) return false;
287  }
288  return true;
289 }
290 
291 bool ServoController::addJointGroup(const char *gname, const ::OpenHRP::ServoControllerService::iSequence ids)
292 {
293  if ( ! serial ) return true;
294 
295  std::vector<int> indices;
296  for (size_t i=0; i<ids.length(); i++){
297  indices.push_back(ids[i]);
298  }
299  joint_groups[gname] = indices;
300 
301  return true;
302 }
303 
304 bool ServoController::removeJointGroup(const char *gname)
305 {
306  if ( ! serial ) return true;
307 
308  joint_groups.erase(gname);
309 }
310 
311 bool ServoController::setJointAnglesOfGroup(const char *gname, const OpenHRP::ServoControllerService::dSequence angles, double tm)
312 {
313  if ( ! serial ) return true;
314 
315  if ( joint_groups.find(gname) != joint_groups.end()) {
316  unsigned int len = joint_groups[gname].size();
317  if ( angles.length() != len ) {
318  std::cerr << "[ERROR] " << m_profile.instance_name << ": size of servo.id(" << angles.length() << ") is not correct, expected" << len << std::endl;
319  return false;
320  }
321  int id[len];
322  double tms[len];
323  double rad[len];
324  for( unsigned int i = 0; i < len; i++ ) {
325  id[i] = joint_groups[gname][i];
326  tms[i] = tm;
327  double offset, dir;
328  for( unsigned int j = 0; j < servo_id.size(); j++ ) {
329  if ( servo_id[j] == id[i]) {
330  offset = servo_offset[j];
331  dir = servo_dir[j];
332  }
333  }
334  rad[i] = (angles.get_buffer()[i])*dir+offset;
335  }
336  serial->setPositions(servo_id.size(), id, rad, tms);
337  }
338  return true;
339 }
340 
341 bool ServoController::setMaxTorque(short id, short percentage)
342 {
343  if ( ! serial ) return true;
344 
345  int ret = serial->setMaxTorque(id, percentage);
346 
347  if (ret < 0) return false;
348  return true;
349 }
350 
352 {
353  if ( ! serial ) return true;
354 
355  int ret = serial->setReset(id);
356 
357  if (ret < 0) return false;
358  return true;
359 }
360 
361 bool ServoController::getDuration(short id, double &duration)
362 {
363  if ( ! serial ) return true;
364 
365  int ret = serial->getDuration(id, &duration);
366 
367  if (ret < 0) return false;
368  return true;
369 }
370 
371 bool ServoController::getSpeed(short id, double &speed)
372 {
373  if ( ! serial ) return true;
374 
375  int ret = serial->getSpeed(id, &speed);
376 
377  if (ret < 0) return false;
378  return true;
379 }
380 
381 bool ServoController::getMaxTorque(short id, short &percentage)
382 {
383  if ( ! serial ) return true;
384 
385  int ret = serial->getMaxTorque(id, &percentage);
386 
387  if (ret < 0) return false;
388  return true;
389 }
390 
391 bool ServoController::getTorque(short id, double &torque)
392 {
393  if ( ! serial ) return true;
394 
395  int ret = serial->getTorque(id, &torque);
396 
397  if (ret < 0) return false;
398  return true;
399 }
400 
402 {
403  if ( ! serial ) return true;
404 
405  int ret = serial->getTemperature(id, &temperature);
406 
407  if (ret < 0) return false;
408  return true;
409 }
410 
411 bool ServoController::getVoltage(short id, double &voltage)
412 {
413  if ( ! serial ) return true;
414 
415  int ret = serial->getVoltage(id, &voltage);
416 
417  if (ret < 0) return false;
418  return true;
419 }
420 
422 {
423  if ( ! serial ) return true;
424 
425  int ret;
426 
427  for (vector<int>::iterator it = servo_id.begin(); it != servo_id.end(); it++ ){
428  ret = serial->setTorqueOn(*it);
429  if (ret < 0) return false;
430  }
431 
432  return true;
433 }
434 
436 {
437  if ( ! serial ) return true;
438 
439  int ret;
440 
441  for (vector<int>::iterator it = servo_id.begin(); it != servo_id.end(); it++ ){
442  ret = serial->setTorqueOff(*it);
443  if (ret < 0) return false;
444  }
445  return true;
446 }
447 
448 
449 extern "C"
450 {
451 
453  {
455  manager->registerFactory(profile,
456  RTC::Create<ServoController>,
457  RTC::Delete<ServoController>);
458  }
459 
460 };
461 
462 
ComponentProfile m_profile
png_infop png_charpp int png_charpp profile
bool setJointAngles(const OpenHRP::ServoControllerService::dSequence angles, double tm)
void ServoControllerInit(RTC::Manager *manager)
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
static const char * nullcomponent_spec[]
std::vector< int > servo_id
bool stringTo(To &val, const char *str)
bool getVoltage(short id, double &voltage)
bool removeJointGroup(const char *gname)
int getSpeed(int id, double *duration)
Definition: ServoSerial.h:178
null component
bool setMaxTorque(short id, short percentage)
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
int getVoltage(int id, double *voltage)
Definition: ServoSerial.h:234
int getPosition(int id, double *angle)
Definition: ServoSerial.h:150
int getDuration(int id, double *duration)
Definition: ServoSerial.h:164
vstring split(const std::string &input, const std::string &delimiter, bool ignore_empty)
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
virtual RTC::ReturnCode_t onInitialize()
bool getJointAngle(short id, double &angle)
bool getJointAngles(OpenHRP::ServoControllerService::dSequence_out &angles)
std::vector< double > servo_dir
std::vector< double > servo_offset
png_uint_32 i
virtual RTC::ReturnCode_t onFinalize()
coil::Properties & getProperties()
int temperature(int s)
bool setJointAnglesOfGroup(const char *gname, const ::OpenHRP::ServoControllerService::dSequence angles, double tm)
virtual ~ServoController()
Destructor.
int setTorqueOff(int id)
Definition: ServoSerial.h:138
bool addJointGroup(const char *gname, const ::OpenHRP::ServoControllerService::iSequence ids)
ServoControllerService_impl m_service0
int getTemperature(int id, double *temperature)
Definition: ServoSerial.h:220
bool getTemperature(short id, double &temperature)
bool setReset(short id)
bool getTorque(short id, double &torque)
std::vector< std::string > vstring
std::map< std::string, std::vector< int > > joint_groups
RTC::CorbaPort m_ServoControllerServicePort
int setMaxTorque(int id, short percentage)
Definition: ServoSerial.h:125
ExecutionContextHandle_t UniqueId
int getTorque(int id, double *torque)
Definition: ServoSerial.h:206
bool getSpeed(short id, double &speed)
int getMaxTorque(int id, short *percentage)
Definition: ServoSerial.h:192
ServoSerial * serial
bool setJointAngle(short id, double angle, double tm)
int setPosition(int id, double rad)
Definition: ServoSerial.h:78
prop
int setTorqueOn(int id)
Definition: ServoSerial.h:132
#define M_PI
int setReset(int id)
Definition: ServoSerial.h:74
int setPositions(int len, int *id, double *rad)
Definition: ServoSerial.h:86
bool getMaxTorque(short id, short &percentage)
static int id
bool addPort(PortBase &port)
bool getDuration(short id, double &duration)
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
void servo(ServoController *i_servo)
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
ServoController(RTC::Manager *manager)
Constructor.
bool registerProvider(const char *instance_name, const char *type_name, PortableServer::RefCountServantBase &provider)


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