00001
00002
00003 import os
00004 import rtm
00005
00006 from rtm import *
00007 from OpenHRP import *
00008 from hrpsys import *
00009 from hrpsys import ImpedanceControllerService_idl
00010 from waitInput import waitInputConfirm
00011
00012 import socket
00013 import time
00014 import subprocess
00015 from distutils.version import StrictVersion
00016
00017
00018
00019 import numpy
00020
00021 _AXES2TUPLE = {
00022 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0),
00023 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0),
00024 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0),
00025 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0),
00026 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1),
00027 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1),
00028 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1),
00029 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)}
00030
00031
00032 _NEXT_AXIS = [1, 2, 0, 1]
00033
00034
00035 _EPS = numpy.finfo(float).eps * 4.0
00036
00037
00038 def euler_matrix(ai, aj, ak, axes='sxyz'):
00039 """Return homogeneous rotation matrix from Euler angles and axis sequence.
00040
00041 ai, aj, ak : Euler's roll, pitch and yaw angles
00042 axes : One of 24 axis sequences as string or encoded tuple
00043
00044 >>> R = euler_matrix(1, 2, 3, 'syxz')
00045 >>> numpy.allclose(numpy.sum(R[0]), -1.34786452)
00046 True
00047 >>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1))
00048 >>> numpy.allclose(numpy.sum(R[0]), -0.383436184)
00049 True
00050 >>> ai, aj, ak = (4.0*math.pi) * (numpy.random.random(3) - 0.5)
00051 >>> for axes in _AXES2TUPLE.keys():
00052 ... R = euler_matrix(ai, aj, ak, axes)
00053 >>> for axes in _TUPLE2AXES.keys():
00054 ... R = euler_matrix(ai, aj, ak, axes)
00055
00056 """
00057 try:
00058 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes]
00059 except (AttributeError, KeyError):
00060 _ = _TUPLE2AXES[axes]
00061 firstaxis, parity, repetition, frame = axes
00062
00063 i = firstaxis
00064 j = _NEXT_AXIS[i + parity]
00065 k = _NEXT_AXIS[i - parity + 1]
00066
00067 if frame:
00068 ai, ak = ak, ai
00069 if parity:
00070 ai, aj, ak = -ai, -aj, -ak
00071
00072 si, sj, sk = math.sin(ai), math.sin(aj), math.sin(ak)
00073 ci, cj, ck = math.cos(ai), math.cos(aj), math.cos(ak)
00074 cc, cs = ci * ck, ci * sk
00075 sc, ss = si * ck, si * sk
00076
00077 M = numpy.identity(4)
00078 if repetition:
00079 M[i, i] = cj
00080 M[i, j] = sj * si
00081 M[i, k] = sj * ci
00082 M[j, i] = sj * sk
00083 M[j, j] = -cj * ss + cc
00084 M[j, k] = -cj * cs - sc
00085 M[k, i] = -sj * ck
00086 M[k, j] = cj * sc + cs
00087 M[k, k] = cj * cc - ss
00088 else:
00089 M[i, i] = cj * ck
00090 M[i, j] = sj * sc - cs
00091 M[i, k] = sj * cc + ss
00092 M[j, i] = cj * sk
00093 M[j, j] = sj * ss + cc
00094 M[j, k] = sj * cs - sc
00095 M[k, i] = -sj
00096 M[k, j] = cj * si
00097 M[k, k] = cj * ci
00098 return M
00099
00100
00101 def euler_from_matrix(matrix, axes='sxyz'):
00102 """Return Euler angles from rotation matrix for specified axis sequence.
00103
00104 axes : One of 24 axis sequences as string or encoded tuple
00105
00106 Note that many Euler angle triplets can describe one matrix.
00107
00108 >>> R0 = euler_matrix(1, 2, 3, 'syxz')
00109 >>> al, be, ga = euler_from_matrix(R0, 'syxz')
00110 >>> R1 = euler_matrix(al, be, ga, 'syxz')
00111 >>> numpy.allclose(R0, R1)
00112 True
00113 >>> angles = (4.0*math.pi) * (numpy.random.random(3) - 0.5)
00114 >>> for axes in _AXES2TUPLE.keys():
00115 ... R0 = euler_matrix(axes=axes, *angles)
00116 ... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes))
00117 ... if not numpy.allclose(R0, R1): print axes, "failed"
00118
00119 """
00120 try:
00121 firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
00122 except (AttributeError, KeyError):
00123 _ = _TUPLE2AXES[axes]
00124 firstaxis, parity, repetition, frame = axes
00125
00126 i = firstaxis
00127 j = _NEXT_AXIS[i + parity]
00128 k = _NEXT_AXIS[i - parity + 1]
00129
00130 M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3]
00131 if repetition:
00132 sy = math.sqrt(M[i, j] * M[i, j] + M[i, k] * M[i, k])
00133 if sy > _EPS:
00134 ax = math.atan2(M[i, j], M[i, k])
00135 ay = math.atan2(sy, M[i, i])
00136 az = math.atan2(M[j, i], -M[k, i])
00137 else:
00138 ax = math.atan2(-M[j, k], M[j, j])
00139 ay = math.atan2(sy, M[i, i])
00140 az = 0.0
00141 else:
00142 cy = math.sqrt(M[i, i] * M[i, i] + M[j, i] * M[j, i])
00143 if cy > _EPS:
00144 ax = math.atan2(M[k, j], M[k, k])
00145 ay = math.atan2(-M[k, i], cy)
00146 az = math.atan2(M[j, i], M[i, i])
00147 else:
00148 ax = math.atan2(-M[j, k], M[j, j])
00149 ay = math.atan2(-M[k, i], cy)
00150 az = 0.0
00151
00152 if parity:
00153 ax, ay, az = -ax, -ay, -az
00154 if frame:
00155 ax, az = az, ax
00156 return ax, ay, az
00157
00158
00159
00160
00161 class HrpsysConfigurator(object):
00162
00163
00164 rh = None
00165 rh_svc = None
00166 ep_svc = None
00167 rh_version = None
00168
00169
00170 seq = None
00171 seq_svc = None
00172 seq_version = None
00173
00174
00175 sh = None
00176 sh_svc = None
00177 sh_version = None
00178
00179
00180 sc= None
00181 sc_svc = None
00182 sc_version = None
00183
00184
00185 fk = None
00186 fk_svc = None
00187 fk_version = None
00188
00189 tf = None
00190 kf = None
00191 vs = None
00192 rmfo = None
00193 ic = None
00194 abc = None
00195 st = None
00196
00197 tf_version = None
00198 kf_version = None
00199 vs_version = None
00200 rmfo_version = None
00201 ic_version = None
00202 abc_version = None
00203 st_version = None
00204
00205
00206 es = None
00207 es_svc = None
00208 es_version = None
00209
00210
00211 hes = None
00212 hes_svc = None
00213 hes_version = None
00214
00215
00216 co = None
00217 co_svc = None
00218 co_version = None
00219
00220
00221 gc = None
00222 gc_svc = None
00223 gc_version = None
00224
00225
00226 el = None
00227 el_svc = None
00228 el_version = None
00229
00230
00231 te = None
00232 te_svc = None
00233 te_version = None
00234
00235
00236 tl = None
00237 tl_svc = None
00238 tl_version = None
00239
00240
00241 tc = None
00242 tc_svc = None
00243 tc_version = None
00244
00245
00246 log = None
00247 log_svc = None
00248 log_version = None
00249 log_use_owned_ec = False
00250
00251
00252 bp = None
00253 bp_svc = None
00254 bp_version = None
00255
00256
00257 rfu = None
00258 rfu_svc = None
00259 rfu_version = None
00260
00261
00262 acf = None
00263 acf_svc = None
00264 acf_version = None
00265
00266
00267 octd = None
00268 octd_svc = None
00269 octd_version = None
00270
00271
00272 ms = None
00273
00274
00275 hgc = None
00276
00277
00278 pdc = None
00279
00280
00281 simulation_mode = None
00282
00283
00284 sensors = None
00285
00286
00287 Groups = []
00288
00289 hrpsys_version = None
00290
00291
00292 kinematics_only_mode = False
00293
00294
00295 def connectComps(self):
00296 '''!@brief
00297 Connect components(plugins)
00298 '''
00299 if self.rh == None or self.seq == None or self.sh == None or self.fk == None:
00300 print(self.configurator_name + "\033[31m connectComps : hrpsys requries rh, seq, sh and fk, please check rtcd.conf or rtcd arguments\033[0m")
00301 return
00302
00303 tmp_controllers = self.getJointAngleControllerList()
00304 if len(tmp_controllers) > 0:
00305 connectPorts(self.sh.port("qOut"), tmp_controllers[0].port("qRef"))
00306 for i in range(len(tmp_controllers) - 1):
00307 connectPorts(tmp_controllers[i].port("q"),
00308 tmp_controllers[i + 1].port("qRef"))
00309 if self.simulation_mode:
00310 if self.pdc:
00311 connectPorts(tmp_controllers[-1].port("q"), self.pdc.port("angleRef"))
00312 else:
00313 connectPorts(tmp_controllers[-1].port("q"), self.hgc.port("qIn"))
00314 connectPorts(self.hgc.port("qOut"), self.rh.port("qRef"))
00315 else:
00316 connectPorts(tmp_controllers[-1].port("q"), self.rh.port("qRef"))
00317 else:
00318 if self.simulation_mode:
00319 if self.pdc:
00320 connectPorts(self.sh.port("qOut"), self.pdc.port("angleRef"))
00321 else:
00322 connectPorts(self.sh.port("qOut"), self.hgc.port("qIn"))
00323 connectPorts(self.hgc.port("qOut"), self.rh.port("qRef"))
00324 else:
00325 connectPorts(self.sh.port("qOut"), self.rh.port("qRef"))
00326
00327
00328 if rtm.findPort(self.rh.ref, "basePoseRef"):
00329 self.kinematics_only_mode = True
00330 if self.abc:
00331 connectPorts(self.abc.port("basePoseOut"), self.rh.port("basePoseRef"))
00332 else:
00333 connectPorts(self.sh.port("basePoseOut"), self.rh.port("basePoseRef"))
00334
00335
00336 if self.kf:
00337
00338 s_acc = filter(lambda s: s.type == 'Acceleration', self.sensors)
00339 if (len(s_acc) > 0) and self.rh.port(s_acc[0].name) != None:
00340 connectPorts(self.rh.port(s_acc[0].name), self.kf.port('acc'))
00341 s_rate = filter(lambda s: s.type == 'RateGyro', self.sensors)
00342 if (len(s_rate) > 0) and self.rh.port(s_rate[0].name) != None:
00343 connectPorts(self.rh.port(s_rate[0].name), self.kf.port("rate"))
00344 connectPorts(self.rh.port("q"), self.kf.port("qCurrent"))
00345
00346
00347 if self.rh.port("servoState") != None:
00348 if self.te and self.el:
00349 connectPorts(self.rh.port("servoState"),
00350 self.te.port("servoStateIn"))
00351 connectPorts(self.te.port("servoStateOut"),
00352 self.el.port("servoStateIn"))
00353 elif self.el:
00354 connectPorts(self.rh.port("servoState"),
00355 self.el.port("servoStateIn"))
00356 elif self.te:
00357 connectPorts(self.rh.port("servoState"),
00358 self.te.port("servoStateIn"))
00359
00360
00361 connectPorts(self.rh.port("q"), [self.sh.port("currentQIn"),
00362 self.fk.port("q")])
00363 connectPorts(self.sh.port("qOut"), self.fk.port("qRef"))
00364 connectPorts(self.seq.port("qRef"), self.sh.port("qIn"))
00365 connectPorts(self.seq.port("tqRef"), self.sh.port("tqIn"))
00366 connectPorts(self.seq.port("basePos"), self.sh.port("basePosIn"))
00367 connectPorts(self.seq.port("baseRpy"), self.sh.port("baseRpyIn"))
00368 connectPorts(self.seq.port("zmpRef"), self.sh.port("zmpIn"))
00369 if StrictVersion(self.seq_version) >= StrictVersion('315.2.6'):
00370 connectPorts(self.seq.port("optionalData"), self.sh.port("optionalDataIn"))
00371 connectPorts(self.sh.port("basePosOut"), [self.seq.port("basePosInit"),
00372 self.fk.port("basePosRef")])
00373 connectPorts(self.sh.port("baseRpyOut"), [self.seq.port("baseRpyInit"),
00374 self.fk.port("baseRpyRef")])
00375 connectPorts(self.sh.port("qOut"), self.seq.port("qInit"))
00376 if StrictVersion(self.seq_version) >= StrictVersion('315.2.0'):
00377 connectPorts(self.sh.port("zmpOut"), self.seq.port("zmpRefInit"))
00378 for sen in self.getForceSensorNames():
00379 connectPorts(self.seq.port(sen + "Ref"),
00380 self.sh.port(sen + "In"))
00381
00382
00383 if rtm.findPort(self.rh.ref, "lfsensor") and rtm.findPort(
00384 self.rh.ref, "rfsensor") and self.st:
00385 connectPorts(self.kf.port("rpy"), self.st.port("rpy"))
00386 connectPorts(self.sh.port("zmpOut"), self.abc.port("zmpIn"))
00387 connectPorts(self.sh.port("basePosOut"), self.abc.port("basePosIn"))
00388 connectPorts(self.sh.port("baseRpyOut"), self.abc.port("baseRpyIn"))
00389 connectPorts(self.sh.port("optionalDataOut"), self.abc.port("optionalData"))
00390 connectPorts(self.abc.port("zmpOut"), self.st.port("zmpRef"))
00391 connectPorts(self.abc.port("baseRpyOut"), self.st.port("baseRpyIn"))
00392 connectPorts(self.abc.port("basePosOut"), self.st.port("basePosIn"))
00393 connectPorts(self.abc.port("accRef"), self.kf.port("accRef"))
00394 connectPorts(self.abc.port("contactStates"), self.st.port("contactStates"))
00395 connectPorts(self.abc.port("controlSwingSupportTime"), self.st.port("controlSwingSupportTime"))
00396 connectPorts(self.rh.port("q"), self.st.port("qCurrent"))
00397 connectPorts(self.seq.port("qRef"), self.st.port("qRefSeq"))
00398 connectPorts(self.abc.port("walkingStates"), self.st.port("walkingStates"))
00399 connectPorts(self.abc.port("sbpCogOffset"), self.st.port("sbpCogOffset"))
00400 connectPorts(self.abc.port("toeheelRatio"), self.st.port("toeheelRatio"))
00401 if self.es:
00402 connectPorts(self.st.port("emergencySignal"), self.es.port("emergencySignal"))
00403 connectPorts(self.st.port("emergencySignal"), self.abc.port("emergencySignal"))
00404 connectPorts(self.st.port("diffCapturePoint"), self.abc.port("diffCapturePoint"))
00405 connectPorts(self.st.port("actContactStates"), self.abc.port("actContactStates"))
00406 if self.rfu:
00407 connectPorts(self.st.port("diffFootOriginExtMoment"), self.rfu.port("diffFootOriginExtMoment"))
00408 connectPorts(self.rfu.port("refFootOriginExtMoment"), self.abc.port("refFootOriginExtMoment"))
00409 connectPorts(self.rfu.port("refFootOriginExtMomentIsHoldValue"), self.abc.port("refFootOriginExtMomentIsHoldValue"))
00410 if self.octd:
00411 connectPorts(self.abc.port("contactStates"), self.octd.port("contactStates"))
00412
00413
00414 for sen in self.getForceSensorNames():
00415 if self.abc and self.st:
00416 connectPorts(self.abc.port(sen),
00417 self.st.port(sen + "Ref"))
00418 connectPorts(self.abc.port("limbCOPOffset_"+sen),
00419 self.st.port("limbCOPOffset_"+sen))
00420 if self.rfu:
00421 ref_force_port_from = self.rfu.port("ref_"+sen+"Out")
00422 elif self.es:
00423 ref_force_port_from = self.es.port(sen+"Out")
00424 else:
00425 ref_force_port_from = self.sh.port(sen+"Out")
00426 if self.ic:
00427 connectPorts(ref_force_port_from,
00428 self.ic.port("ref_" + sen+"In"))
00429 if self.abc:
00430 connectPorts(ref_force_port_from,
00431 self.abc.port("ref_" + sen))
00432 if self.es:
00433 connectPorts(self.sh.port(sen+"Out"),
00434 self.es.port(sen+"In"))
00435 if self.rfu:
00436 connectPorts(self.es.port(sen+"Out"),
00437 self.rfu.port("ref_" + sen+"In"))
00438 else:
00439 if self.rfu:
00440 connectPorts(self.sh.port(sen+"Out"),
00441 self.rfu.port("ref_" + sen+"In"))
00442
00443
00444 if self.rmfo:
00445 if self.kf:
00446
00447 connectPorts(self.kf.port("rpy"), self.rmfo.port("rpy"))
00448 connectPorts(self.rh.port("q"), self.rmfo.port("qCurrent"))
00449 for sen in filter(lambda x: x.type == "Force", self.sensors):
00450 connectPorts(self.rh.port(sen.name), self.rmfo.port(sen.name))
00451 if self.ic:
00452 connectPorts(self.rmfo.port("off_" + sen.name),
00453 self.ic.port(sen.name))
00454 if self.rfu:
00455 connectPorts(self.rmfo.port("off_" + sen.name),
00456 self.rfu.port(sen.name))
00457 if self.st:
00458 connectPorts(self.rmfo.port("off_" + sen.name),
00459 self.st.port(sen.name))
00460 elif self.ic:
00461 for sen in filter(lambda x: x.type == "Force", self.sensors):
00462 connectPorts(self.rh.port(sen.name),
00463 self.ic.port(sen.name))
00464
00465
00466 if self.ic:
00467 connectPorts(self.rh.port("q"), self.ic.port("qCurrent"))
00468 if StrictVersion(self.seq_version) >= StrictVersion('315.3.0'):
00469 connectPorts(self.sh.port("basePosOut"), self.ic.port("basePosIn"))
00470 connectPorts(self.sh.port("baseRpyOut"), self.ic.port("baseRpyIn"))
00471
00472 if self.rfu:
00473 if self.es:
00474 connectPorts(self.es.port("q"), self.rfu.port("qRef"))
00475 if StrictVersion(self.seq_version) >= StrictVersion('315.3.0'):
00476 connectPorts(self.sh.port("basePosOut"), self.rfu.port("basePosIn"))
00477 connectPorts(self.sh.port("baseRpyOut"), self.rfu.port("baseRpyIn"))
00478
00479 if self.tf:
00480
00481 if rtm.findPort(self.rh.ref, "tau") != None:
00482 connectPorts(self.rh.port("tau"), self.tf.port("tauIn"))
00483 connectPorts(self.rh.port("q"), self.tf.port("qCurrent"))
00484
00485 if self.vs:
00486 connectPorts(self.rh.port("q"), self.vs.port("qCurrent"))
00487 connectPorts(self.tf.port("tauOut"), self.vs.port("tauIn"))
00488
00489 if self.ic:
00490 for vfp in filter(lambda x: str.find(x, 'v') >= 0 and
00491 str.find(x, 'sensor') >= 0, self.vs.ports.keys()):
00492 connectPorts(self.vs.port(vfp), self.ic.port(vfp))
00493
00494 if self.co:
00495 connectPorts(self.rh.port("q"), self.co.port("qCurrent"))
00496 connectPorts(self.rh.port("servoState"), self.co.port("servoStateIn"))
00497
00498 if self.octd:
00499 connectPorts(self.rh.port("q"), self.octd.port("qCurrent"))
00500 if self.kf:
00501 connectPorts(self.kf.port("rpy"), self.octd.port("rpy"))
00502 if self.rmfo:
00503 for sen in filter(lambda x: x.type == "Force", self.sensors):
00504 connectPorts(self.rmfo.port("off_" + sen.name), self.octd.port(sen.name))
00505
00506
00507 if self.gc:
00508 connectPorts(self.rh.port("q"), self.gc.port("qCurrent"))
00509 tmp_controller = self.getJointAngleControllerList()
00510 index = tmp_controller.index(self.gc)
00511 if index == 0:
00512 connectPorts(self.sh.port("qOut"), self.gc.port("qIn"))
00513 else:
00514 connectPorts(tmp_controller[index - 1].port("q"),
00515 self.gc.port("qIn"))
00516
00517
00518 if self.te:
00519 connectPorts(self.rh.port("q"), self.te.port("qCurrentIn"))
00520 connectPorts(self.sh.port("qOut"), self.te.port("qRefIn"))
00521 if self.tf:
00522 connectPorts(self.tf.port("tauOut"), self.te.port("tauIn"))
00523 else:
00524 connectPorts(self.rh.port("tau"), self.te.port("tauIn"))
00525
00526
00527
00528 if self.tl:
00529 if self.te:
00530 connectPorts(self.te.port("tempOut"), self.tl.port("tempIn"))
00531
00532
00533 if self.tc:
00534 connectPorts(self.rh.port("q"), self.tc.port("qCurrent"))
00535 if self.tf:
00536 connectPorts(self.tf.port("tauOut"),
00537 self.tc.port("tauCurrent"))
00538 else:
00539 connectPorts(self.rh.port("tau"), self.tc.port("tauCurrent"))
00540 if self.tl:
00541 connectPorts(self.tl.port("tauMax"), self.tc.port("tauMax"))
00542
00543
00544 if self.el:
00545 connectPorts(self.rh.port("q"), self.el.port("qCurrent"))
00546
00547
00548 if self.es:
00549 connectPorts(self.rh.port("servoState"), self.es.port("servoStateIn"))
00550
00551
00552 if self.bp:
00553 if self.tl:
00554 connectPorts(self.tl.port("beepCommand"), self.bp.port("beepCommand"))
00555 if self.es:
00556 connectPorts(self.es.port("beepCommand"), self.bp.port("beepCommand"))
00557 if self.el:
00558 connectPorts(self.el.port("beepCommand"), self.bp.port("beepCommand"))
00559 if self.co:
00560 connectPorts(self.co.port("beepCommand"), self.bp.port("beepCommand"))
00561
00562
00563 if self.acf:
00564
00565 s_acc = filter(lambda s: s.type == 'Acceleration', self.sensors)
00566 if (len(s_acc) > 0) and self.rh.port(s_acc[0].name) != None:
00567 connectPorts(self.rh.port(s_acc[0].name), self.acf.port('accIn'))
00568 s_rate = filter(lambda s: s.type == 'RateGyro', self.sensors)
00569 if (len(s_rate) > 0) and self.rh.port(s_rate[0].name) != None:
00570 connectPorts(self.rh.port(s_rate[0].name), self.acf.port("rateIn"))
00571 if self.kf:
00572 connectPorts(self.kf.port("rpy"), self.acf.port("rpyIn"))
00573 if self.abc:
00574 connectPorts(self.abc.port("basePosOut"), self.acf.port("posIn"))
00575
00576 def activateComps(self):
00577 '''!@brief
00578 Activate components(plugins)
00579 '''
00580 rtcList = self.getRTCInstanceList()
00581 rtm.serializeComponents(rtcList)
00582 for r in rtcList:
00583 r.start()
00584
00585 def deactivateComps(self):
00586 '''!@brief
00587 Deactivate components(plugins)
00588 '''
00589 rtcList = self.getRTCInstanceList()
00590 for r in reversed(rtcList):
00591 r.stop()
00592
00593 def createComp(self, compName, instanceName):
00594 '''!@brief
00595 Create RTC component (plugins)
00596
00597 @param instanceName str: name of instance, choose one of https://github.com/fkanehiro/hrpsys-base/tree/master/rtc
00598 @param comName str: name of component that to be created.
00599 '''
00600 self.ms.load(compName)
00601 comp = self.ms.create(compName, instanceName)
00602 version = comp.ref.get_component_profile().version
00603 print(self.configurator_name + "create Comp -> %s : %s (%s)" % (compName, comp, version))
00604 if comp == None:
00605 raise RuntimeError("Cannot create component: " + compName)
00606 if comp.service("service0"):
00607 comp_svc = narrow(comp.service("service0"), compName + "Service")
00608 print(self.configurator_name + "create CompSvc -> %s Service : %s" % (compName, comp_svc))
00609 return [comp, comp_svc, version]
00610 else:
00611 return [comp, None, version]
00612
00613 def createComps(self):
00614 '''!@brief
00615 Create components(plugins) in getRTCList()
00616 '''
00617 for rn in self.getRTCList():
00618 try:
00619 rn2 = 'self.' + rn[0]
00620 if eval(rn2) == None:
00621 create_str = "[self." + rn[0] + ", self." + rn[0] + "_svc, self." + rn[0] + "_version] = self.createComp(\"" + rn[1] + "\",\"" + rn[0] + "\")"
00622 print(self.configurator_name + " eval : " + create_str)
00623 exec(create_str)
00624 except Exception:
00625 _, e, _ = sys.exc_info()
00626 print(self.configurator_name + '\033[31mFail to createComps ' + str(e) + '\033[0m')
00627
00628
00629 def deleteComp(self, compName):
00630 '''!@brief
00631 Delete RTC component (plugins)
00632
00633 @param compName str: name of component that to be deleted
00634 '''
00635
00636 comp = rtm.findRTC(compName)
00637 comp.stop()
00638 return self.ms.delete(compName)
00639
00640 def deleteComps(self):
00641 '''!@brief
00642 Delete components(plugins) in getRTCInstanceList()
00643 '''
00644 self.deactivateComps()
00645 rtcList = self.getRTCInstanceList()
00646 if rtcList:
00647 try:
00648 rtcList.remove(self.rh)
00649 for r in reversed(rtcList):
00650 if r.isActive():
00651 print(self.configurator_name, '\033[31m ' + r.name() + ' is staill active\033[0m')
00652 self.deleteComp(r.name())
00653 except Exception:
00654 _, e, _ = sys.exc_info()
00655 print(self.configurator_name + '\033[31mFail to deleteComps' + str(e) + '\033[0m')
00656
00657 def findComp(self, compName, instanceName, max_timeout_count=10, verbose=True):
00658 '''!@brief
00659 Find component(plugin)
00660
00661 @param compName str: component name
00662 @param instanceName str: instance name
00663 @max_timeout_count int: max timeout in seconds
00664 '''
00665 timeout_count = 0
00666 comp = rtm.findRTC(instanceName)
00667 version = None
00668 while comp == None and timeout_count < max_timeout_count:
00669 comp = rtm.findRTC(instanceName)
00670 if comp != None:
00671 break
00672 if verbose:
00673 print(self.configurator_name + " find Comp wait for " + instanceName)
00674 time.sleep(1)
00675 timeout_count += 1
00676 if comp and comp.ref:
00677 version = comp.ref.get_component_profile().version
00678 if verbose:
00679 print(self.configurator_name + " find Comp : %s = %s (%s)" % (instanceName, comp, version))
00680 if comp == None:
00681 if verbose:
00682 print(self.configurator_name + " Cannot find component: %s (%s)" % (instanceName, compName))
00683 return [None, None, None]
00684 comp_svc_port = comp.service("service0")
00685 if comp_svc_port:
00686 comp_svc = narrow(comp_svc_port, compName + "Service")
00687 if verbose:
00688 print(self.configurator_name + " find CompSvc : %s_svc = %s"%(instanceName, comp_svc))
00689 return [comp, comp_svc, version]
00690 else:
00691 return [comp, None, version]
00692
00693 def findComps(self, max_timeout_count = 10, verbose=True):
00694 '''!@brief
00695 Check if all components in getRTCList() are created
00696 '''
00697 for rn in self.getRTCList():
00698 rn2 = 'self.' + rn[0]
00699 if eval(rn2) == None:
00700 create_str = "[self." + rn[0] + ", self." + rn[0] + "_svc, self." + rn[0] + "_version] = self.findComp(\"" + rn[1] + "\",\"" + rn[0] + "\"," + str(max_timeout_count) + "," + str(verbose) + ")"
00701 if verbose:
00702 print(self.configurator_name + create_str)
00703 exec(create_str)
00704 if eval(rn2) == None:
00705 max_timeout_count = 0
00706
00707
00708 def getRTCList(self):
00709 '''!@brief
00710 Get list of available STABLE components
00711 @return list of list: List of available components. Each element consists of a list
00712 of abbreviated and full names of the component.
00713 '''
00714 return [
00715 ['seq', "SequencePlayer"],
00716 ['sh', "StateHolder"],
00717 ['fk', "ForwardKinematics"],
00718
00719
00720
00721
00722
00723
00724
00725 ['co', "CollisionDetector"],
00726
00727
00728
00729 ['el', "SoftErrorLimiter"],
00730 ['log', "DataLogger"]
00731 ]
00732
00733
00734 def getRTCListUnstable(self):
00735 '''!@brief
00736 Get list of available components including stable and unstable.
00737
00738 @return list of list: List of available unstable components. Each element consists
00739 of a list of abbreviated and full names of the component.
00740 '''
00741 return [
00742 ['seq', "SequencePlayer"],
00743 ['sh', "StateHolder"],
00744 ['fk', "ForwardKinematics"],
00745 ['tf', "TorqueFilter"],
00746 ['kf', "KalmanFilter"],
00747 ['vs', "VirtualForceSensor"],
00748 ['rmfo', "RemoveForceSensorLinkOffset"],
00749 ['octd', "ObjectContactTurnaroundDetector"],
00750 ['es', "EmergencyStopper"],
00751 ['rfu', "ReferenceForceUpdater"],
00752 ['ic', "ImpedanceController"],
00753 ['abc', "AutoBalancer"],
00754 ['st', "Stabilizer"],
00755 ['co', "CollisionDetector"],
00756 ['tc', "TorqueController"],
00757 ['te', "ThermoEstimator"],
00758 ['hes', "EmergencyStopper"],
00759 ['el', "SoftErrorLimiter"],
00760 ['tl', "ThermoLimiter"],
00761 ['bp', "Beeper"],
00762 ['acf', "AccelerationFilter"],
00763 ['log', "DataLogger"]
00764 ]
00765
00766 def getJointAngleControllerList(self):
00767 '''!@brief
00768 Get list of controller list that need to control joint angles
00769 '''
00770 controller_list = [self.es, self.ic, self.gc, self.abc, self.st, self.co,
00771 self.tc, self.hes, self.el]
00772 return filter(lambda c: c != None, controller_list)
00773
00774 def getRTCInstanceList(self, verbose=True):
00775 '''!@brief
00776 Get list of RTC Instance
00777 '''
00778 ret = [self.rh]
00779 for rtc in self.getRTCList():
00780 r = 'self.'+rtc[0]
00781 try:
00782 if eval(r):
00783 ret.append(eval(r))
00784 else:
00785 if verbose:
00786 print(self.configurator_name + '\033[31mFail to find instance ('+str(rtc)+') for getRTCInstanceList\033[0m')
00787 except Exception:
00788 _, e, _ = sys.exc_info()
00789 print(self.configurator_name + '\033[31mFail to getRTCInstanceList'+str(e)+'\033[0m')
00790 return ret
00791
00792
00793
00794
00795 def parseUrl(self, url):
00796 if '$(PROJECT_DIR)' in url:
00797 path = subprocess.Popen(['pkg-config', 'openhrp3.1', '--variable=prefix'], stdout=subprocess.PIPE).communicate()[0].rstrip()
00798 path = os.path.join(path, 'share/OpenHRP-3.1/sample/project')
00799 url = url.replace('$(PROJECT_DIR)', path)
00800 return url
00801
00802
00803 def getBodyInfo(self, url):
00804 '''!@brief
00805 Get bodyInfo
00806 '''
00807 import CosNaming
00808 obj = rtm.rootnc.resolve([CosNaming.NameComponent('ModelLoader', '')])
00809 mdlldr = obj._narrow(ModelLoader_idl._0_OpenHRP__POA.ModelLoader)
00810 url = self.parseUrl(url)
00811 print(self.configurator_name + " bodyinfo URL = file://" + url)
00812 return mdlldr.getBodyInfo("file://" + url)
00813
00814
00815 def getSensors(self, url):
00816 '''!@brief
00817 Get list of sensors
00818
00819 @param url str: model file url
00820 '''
00821 if url == '':
00822 return []
00823 else:
00824 return sum(map(lambda x: x.sensors,
00825 filter(lambda x: len(x.sensors) > 0,
00826 self.getBodyInfo(url)._get_links())), [])
00827
00828
00829 def getForceSensorNames(self):
00830 '''!@brief
00831 Get list of force sensor names. Returns existence force sensors and virtual force sensors. self.sensors and virtual force sensors are assumed.
00832 '''
00833 ret = map (lambda x : x.name, filter(lambda x: x.type == "Force", self.sensors))
00834 if self.vs != None:
00835 ret += filter(lambda x: str.find(x, 'v') >= 0 and str.find(x, 'sensor') >= 0, self.vs.ports.keys())
00836 return ret
00837
00838 def connectLoggerPort(self, artc, sen_name, log_name=None):
00839 '''!@brief
00840 Connect port to logger
00841
00842 @param artc object: object of component that contains sen_name port
00843 @param sen_name str: name of port for logging
00844 @param log_name str: name of port in logger
00845 '''
00846 log_name = log_name if log_name else artc.name() + "_" + sen_name
00847 if artc and rtm.findPort(artc.ref, sen_name) != None:
00848 sen_type = rtm.dataTypeOfPort(artc.port(sen_name)).split("/")[1].split(":")[0]
00849 if rtm.findPort(self.log.ref, log_name) == None:
00850 print(self.configurator_name + " setupLogger : record type = %s, name = %s to %s" % ( sen_type, sen_name, log_name))
00851 self.log_svc.add(sen_type, log_name)
00852 else:
00853 print(self.configurator_name + " setupLogger : %s arleady exists in DataLogger"% sen_name)
00854 connectPorts(artc.port(sen_name), self.log.port(log_name))
00855
00856
00857 def setupLogger(self, maxLength=4000):
00858 '''!@brief
00859 Setup logging function.
00860 @param maxLength : max length of data from DataLogger.h #define DEFAULT_MAX_LOG_LENGTH (200*20)
00861 if the robot running at 200hz (5msec) 4000 means 20 secs
00862 '''
00863 if self.log == None:
00864 print(self.configurator_name + "\033[31m setupLogger : self.log is not defined, please check rtcd.conf or rtcd arguments\033[0m")
00865 return
00866 self.log_svc.maxLength(maxLength);
00867
00868 for pn in ['q', 'dq', 'tau']:
00869 self.connectLoggerPort(self.rh, pn)
00870
00871 print(self.configurator_name + "sensor names for DataLogger")
00872 for sen in self.sensors:
00873 self.connectLoggerPort(self.rh, sen.name)
00874
00875 if self.kf != None:
00876 self.connectLoggerPort(self.kf, 'rpy')
00877 if self.sh != None:
00878 self.connectLoggerPort(self.sh, 'qOut')
00879 self.connectLoggerPort(self.sh, 'tqOut')
00880 self.connectLoggerPort(self.sh, 'basePosOut')
00881 self.connectLoggerPort(self.sh, 'baseRpyOut')
00882 self.connectLoggerPort(self.sh, 'zmpOut')
00883 if self.ic != None:
00884 self.connectLoggerPort(self.ic, 'q')
00885 if self.abc != None:
00886 self.connectLoggerPort(self.abc, 'zmpOut')
00887 self.connectLoggerPort(self.abc, 'baseTformOut')
00888 self.connectLoggerPort(self.abc, 'q')
00889 self.connectLoggerPort(self.abc, 'contactStates')
00890 self.connectLoggerPort(self.abc, 'controlSwingSupportTime')
00891 self.connectLoggerPort(self.abc, 'cogOut')
00892 if self.st != None:
00893 self.connectLoggerPort(self.st, 'zmp')
00894 self.connectLoggerPort(self.st, 'originRefZmp')
00895 self.connectLoggerPort(self.st, 'originRefCog')
00896 self.connectLoggerPort(self.st, 'originRefCogVel')
00897 self.connectLoggerPort(self.st, 'originNewZmp')
00898 self.connectLoggerPort(self.st, 'originActZmp')
00899 self.connectLoggerPort(self.st, 'originActCog')
00900 self.connectLoggerPort(self.st, 'originActCogVel')
00901 self.connectLoggerPort(self.st, 'allRefWrench')
00902 self.connectLoggerPort(self.st, 'allEEComp')
00903 self.connectLoggerPort(self.st, 'q')
00904 self.connectLoggerPort(self.st, 'actBaseRpy')
00905 self.connectLoggerPort(self.st, 'currentBasePos')
00906 self.connectLoggerPort(self.st, 'currentBaseRpy')
00907 self.connectLoggerPort(self.st, 'debugData')
00908 if self.el != None:
00909 self.connectLoggerPort(self.el, 'q')
00910 if self.rh != None:
00911 self.connectLoggerPort(self.rh, 'emergencySignal',
00912 'emergencySignal')
00913 self.connectLoggerPort(self.rh, 'servoState')
00914 if self.simulation_mode:
00915 self.connectLoggerPort(self.rh, 'WAIST')
00916 for sen in filter(lambda x: x.type == "Force", self.sensors):
00917 self.connectLoggerPort(self.sh, sen.name + "Out")
00918 if self.rmfo != None:
00919 for sen in filter(lambda x: x.type == "Force", self.sensors):
00920 self.connectLoggerPort(self.rmfo, "off_"+sen.name)
00921 if self.rfu != None:
00922 for sen in filter(lambda x: x.type == "Force", self.sensors):
00923 self.connectLoggerPort(self.rfu, "ref_"+sen.name+"Out")
00924 if self.octd != None:
00925 self.connectLoggerPort(self.octd, "octdData")
00926 self.log_svc.clear()
00927
00928 if self.log and (self.log_use_owned_ec or not isinstance(self.log.owned_ecs[0], OpenRTM._objref_ExtTrigExecutionContextService)):
00929 print(self.configurator_name + "\033[32mruning DataLogger with own Execution Context\033[0m")
00930 self.log.owned_ecs[0].start()
00931 self.log.start(self.log.owned_ecs[0])
00932
00933 def waitForRTCManager(self, managerhost=nshost):
00934 '''!@brief
00935 Wait for RTC manager.
00936
00937 @param managerhost str: name of host computer that manager is running
00938 '''
00939 self.ms = None
00940 while self.ms == None:
00941 time.sleep(1)
00942 if managerhost == "localhost":
00943 managerhost = socket.gethostname()
00944 self.ms = rtm.findRTCmanager(managerhost)
00945 print(self.configurator_name + "wait for RTCmanager : " + str(managerhost))
00946
00947 def waitForRobotHardware(self, robotname="Robot"):
00948 '''!@brief
00949 Wait for RobotHardware is exists and activated.
00950
00951 @param robotname str: name of RobotHardware component.
00952 '''
00953 self.rh = None
00954 timeout_count = 0
00955
00956 while self.rh == None and timeout_count < 10:
00957 if timeout_count > 0:
00958 time.sleep(1);
00959 self.rh = rtm.findRTC("RobotHardware0")
00960 if not self.rh:
00961 self.rh = rtm.findRTC(robotname)
00962 print(self.configurator_name + "wait for %s : %s ( timeout %d < 10)" % ( robotname, self.rh, timeout_count))
00963 if self.rh and self.rh.isActive() == None:
00964 self.rh = None
00965 timeout_count += 1
00966 if not self.rh:
00967 print(self.configurator_name + "Could not find " + robotname)
00968 if self.ms:
00969 print(self.configurator_name + "Candidates are .... " + str([x.name() for x in self.ms.get_components()]))
00970 print(self.configurator_name + "Exitting.... " + robotname)
00971 exit(1)
00972
00973 print(self.configurator_name + "findComps -> %s : %s isActive? = %s " % (self.rh.name(), self.rh, self.rh.isActive()))
00974
00975 def checkSimulationMode(self):
00976 '''!@brief
00977 Check if this is running as simulation
00978 '''
00979
00980
00981 self.hgc = findRTC("HGcontroller0")
00982 self.pdc = findRTC("PDcontroller0")
00983 if self.hgc or self.pdc:
00984 self.simulation_mode = True
00985 else:
00986 self.simulation_mode = False
00987
00988 if rtm.findPort(self.rh.ref, "servoState"):
00989 self.rh_svc = narrow(self.rh.service("service0"), "RobotHardwareService")
00990 self.ep_svc = narrow(self.rh.ec, "ExecutionProfileService")
00991
00992 print(self.configurator_name + "simulation_mode : %s" % self.simulation_mode)
00993
00994 def waitForRTCManagerAndRoboHardware(self, robotname="Robot", managerhost=nshost):
00995 print("\033[93m%s waitForRTCManagerAndRoboHardware has renamed to " % self.configurator_name + \
00996 "waitForRTCManagerAndRoboHardware: Please update your code\033[0m")
00997 return self.waitForRTCManagerAndRobotHardware(robotname=robotname, managerhost=managerhost)
00998
00999 def waitForRTCManagerAndRobotHardware(self, robotname="Robot", managerhost=nshost):
01000 '''!@brief
01001 Wait for both RTC Manager (waitForRTCManager()) and RobotHardware (waitForRobotHardware())
01002
01003 @param managerhost str: name of host computer that manager is running
01004 @param robotname str: name of RobotHardware component.
01005 '''
01006 self.waitForRTCManager(managerhost)
01007 self.waitForRobotHardware(robotname)
01008 self.checkSimulationMode()
01009
01010 def findModelLoader(self):
01011 '''!@brief
01012 Try to find ModelLoader
01013 '''
01014 try:
01015 return rtm.findObject("ModelLoader")
01016 except:
01017 return None
01018
01019 def waitForModelLoader(self):
01020 '''!@brief
01021 Wait for ModelLoader.
01022 '''
01023 while self.findModelLoader() == None:
01024 print(self.configurator_name + "wait for ModelLoader")
01025 time.sleep(3)
01026
01027 def setSelfGroups(self):
01028 '''!@brief
01029 Set to the hrpsys.SequencePlayer the groups of links and joints that
01030 are statically defined as member variables (Groups) within this class.
01031 '''
01032 for item in self.Groups:
01033 self.seq_svc.addJointGroup(item[0], item[1])
01034
01035
01036
01037
01038 def goActual(self):
01039 '''!@brief
01040 Adjust commanded values to the angles in the physical state
01041 by calling StateHolder::goActual.
01042
01043 This needs to be run BEFORE servos are turned on.
01044 '''
01045 self.sh_svc.goActual()
01046
01047 def setJointAngle(self, jname, angle, tm):
01048 '''!@brief
01049 Set angle to the given joint.
01050 \verbatim
01051 NOTE-1: It's known that this method does not do anything after
01052 some group operation is done.
01053 TODO: at least need to warn users.
01054 NOTE-2: While this method does not check angle value range,
01055 any joints could emit position limit over error, which has not yet
01056 been thrown by hrpsys so that there's no way to catch on this python client.
01057 Worthwhile opening an enhancement ticket at designated issue tracker.
01058 \endverbatim
01059
01060 @param jname str: name of joint
01061 @param angle float: In degree.
01062 @param tm float: Time to complete.
01063 @rtype bool
01064 @return False upon any problem during execution.
01065 '''
01066 radangle = angle / 180.0 * math.pi
01067 return self.seq_svc.setJointAngle(jname, radangle, tm)
01068
01069 def setJointAngles(self, angles, tm):
01070 '''!@brief
01071 Set all joint angles.
01072 \verbatim
01073 NOTE: While this method does not check angle value range,
01074 any joints could emit position limit over error, which has not yet
01075 been thrown by hrpsys so that there's no way to catch on this python client.
01076 Worthwhile opening an enhancement ticket at designated issue tracker.
01077 \endverbatim
01078 @param angles list of float: In degree.
01079 @param tm float: Time to complete.
01080 @rtype bool
01081 @return False upon any problem during execution.
01082 '''
01083 ret = []
01084 for angle in angles:
01085 ret.append(angle / 180.0 * math.pi)
01086 return self.seq_svc.setJointAngles(ret, tm)
01087
01088 def setJointAnglesOfGroup(self, gname, pose, tm, wait=True):
01089 '''!@brief
01090 Set the joint angles to aim. By default it waits interpolation to be
01091 over.
01092
01093 \verbatim
01094 NOTE: While this method does not check angle value range,
01095 any joints could emit position limit over error, which has not yet
01096 been thrown by hrpsys so that there's no way to catch on this python client.
01097 Worthwhile opening an enhancement ticket at designated issue tracker.
01098 \endverbatim
01099
01100 @param gname str: Name of the joint group.
01101 @param pose list of float: list of positions and orientations
01102 @param tm float: Time to complete.
01103 @param wait bool: If true, all other subsequent commands wait until
01104 the movement commanded by this method call finishes.
01105 @rtype bool
01106 @return False upon any problem during execution.
01107 '''
01108 angles = [x / 180.0 * math.pi for x in pose]
01109 ret = self.seq_svc.setJointAnglesOfGroup(gname, angles, tm)
01110 if wait:
01111 self.waitInterpolationOfGroup(gname)
01112 return ret
01113
01114 def setJointAnglesSequence(self, angless, tms):
01115 '''!@brief
01116 Set all joint angles. len(angless) should be equal to len(tms).
01117 \verbatim
01118 NOTE: While this method does not check angle value range,
01119 any joints could emit position limit over error, which has not yet
01120 been thrown by hrpsys so that there's no way to catch on this python client.
01121 Worthwhile opening an enhancement ticket at designated issue tracker.
01122 \endverbatim
01123 @param sequential list of angles in float: In rad
01124 @param tm sequential list of time in float: Time to complete, In Second
01125 @rtype bool
01126 @return False upon any problem during execution.
01127 '''
01128 for angles in angless:
01129 for i in range(len(angles)):
01130 angles[i] = angles[i] / 180.0 * math.pi
01131 return self.seq_svc.setJointAnglesSequence(angless, tms)
01132
01133 def setJointAnglesSequenceOfGroup(self, gname, angless, tms):
01134 '''!@brief
01135 Set all joint angles.
01136 \verbatim
01137 NOTE: While this method does not check angle value range,
01138 any joints could emit position limit over error, which has not yet
01139 been thrown by hrpsys so that there's no way to catch on this python client.
01140 Worthwhile opening an enhancement ticket at designated issue tracker.
01141 \endverbatim
01142 @param gname str: Name of the joint group.
01143 @param sequential list of angles in float: In rad
01144 @param tm sequential list of time in float: Time to complete, In Second
01145 @rtype bool
01146 @return False upon any problem during execution.
01147 '''
01148 for angles in angless:
01149 for i in range(len(angles)):
01150 angles[i] = angles[i] / 180.0 * math.pi
01151 return self.seq_svc.setJointAnglesSequenceOfGroup(gname, angless, tms)
01152
01153 def loadPattern(self, fname, tm):
01154 '''!@brief
01155 Load a pattern file that is created offline.
01156
01157 Format of the pattern file:
01158 - example format:
01159 \verbatim
01160 t0 j0 j1 j2...jn
01161 t1 j0 j1 j2...jn
01162 :
01163 tn j0 j1 j2...jn
01164 \endverbatim
01165 - Delimmitted by space
01166 - Each line consists of an action.
01167 - Time between each action is defined by tn+1 - tn
01168 - The time taken for the 1st line is defined by the arg tm.
01169
01170 @param fname str: Name of the pattern file.
01171 @param tm float: - The time to take for the 1st line.
01172 @return: List of 2 oct(string) values.
01173 '''
01174 fname = self.parseUrl(fname)
01175 return self.seq_svc.loadPattern(fname, tm)
01176
01177 def waitInterpolation(self):
01178 '''!@brief
01179 Lets SequencePlayer wait until the movement currently happening to
01180 finish.
01181 '''
01182 self.seq_svc.waitInterpolation()
01183
01184 def waitInterpolationOfGroup(self, gname):
01185 '''!@brief
01186 Lets SequencePlayer wait until the movement currently happening to
01187 finish.
01188
01189 @see: SequencePlayer.waitInterpolationOfGroup
01190 @see: http://wiki.ros.org/joint_trajectory_action. This method
01191 corresponds to JointTrajectoryGoal in ROS.
01192
01193 @param gname str: Name of the joint group.
01194 '''
01195 self.seq_svc.waitInterpolationOfGroup(gname)
01196
01197 def setInterpolationMode(self, mode):
01198 '''!@brief
01199 Set interpolation mode. You may need to import OpenHRP in order to pass an argument. For more info See https://github.com/fkanehiro/hrpsys-base/pull/1012#issue-160802911.
01200 @param mode new interpolation mode. Either { OpenHRP.SequencePlayerService.LINEAR, OpenHRP.SequencePlayerService.HOFFARBIB }.
01201 @return true if set successfully, false otherwise
01202 '''
01203 return self.seq_svc.setInterpolationMode(mode)
01204
01205 def getJointAngles(self):
01206 '''!@brief
01207 Returns the commanded joint angle values.
01208
01209 @see: HrpsysConfigurator.getJointAngles
01210
01211 Note that it's not the physical state of the robot's joints, which
01212 can be obtained by getActualState().angle.
01213
01214 @return List of float: List of angles (degree) of all joints, in the order defined
01215 in the member variable 'Groups' (eg. chest, head1, head2, ..).
01216 '''
01217 return [x * 180.0 / math.pi for x in self.sh_svc.getCommand().jointRefs]
01218
01219 def getCurrentPose(self, lname=None, frame_name=None):
01220 '''!@brief
01221 Returns the current physical pose of the specified joint in the joint space.
01222 cf. getReferencePose that returns commanded value.
01223
01224 eg.
01225 \verbatim
01226 IN: robot.getCurrentPose('LARM_JOINT5')
01227 OUT: [-0.0017702356144599085,
01228 0.00019034630541264752,
01229 -0.9999984150158207,
01230 0.32556275164378523,
01231 0.00012155879975329215,
01232 0.9999999745367515,
01233 0.0001901314142046251,
01234 0.18236394191140365,
01235 0.9999984257434246,
01236 -0.00012122202968358842,
01237 -0.001770258707652326,
01238 0.07462472659364472,
01239 0.0,
01240 0.0,
01241 0.0,
01242 1.0]
01243 \endverbatim
01244
01245 @type lname: str
01246 @param lname: Name of the link.
01247 @param frame_name str: set reference frame name (from 315.2.5)
01248 @rtype: list of float
01249 @return: Rotational matrix and the position of the given joint in
01250 1-dimensional list, that is:
01251 \verbatim
01252 [a11, a12, a13, x,
01253 a21, a22, a23, y,
01254 a31, a32, a33, z,
01255 0, 0, 0, 1]
01256 \endverbatim
01257 '''
01258 if not lname:
01259 for item in self.Groups:
01260 eef_name = item[1][-1]
01261 print("{}: {}".format(eef_name, self.getCurrentPose(eef_name)))
01262 raise RuntimeError("need to specify joint name")
01263 if frame_name:
01264 lname = lname + ':' + frame_name
01265 if StrictVersion(self.fk_version) < StrictVersion('315.2.5') and ':' in lname:
01266 raise RuntimeError('frame_name ('+lname+') is not supported')
01267 pose = self.fk_svc.getCurrentPose(lname)
01268 if not pose[0]:
01269 raise RuntimeError("Could not find reference : " + lname)
01270 return pose[1].data
01271
01272 def getCurrentPosition(self, lname=None, frame_name=None):
01273 '''!@brief
01274 Returns the current physical position of the specified joint in Cartesian space.
01275 cf. getReferencePosition that returns commanded value.
01276
01277 eg.
01278 \verbatim
01279 robot.getCurrentPosition('LARM_JOINT5')
01280 [0.325, 0.182, 0.074]
01281 \endverbatim
01282
01283 @type lname: str
01284 @param lname: Name of the link.
01285 @param frame_name str: set reference frame name (from 315.2.5)
01286 @rtype: list of float
01287 @return: List of x, y, z positions about the specified joint.
01288 '''
01289 if not lname:
01290 for item in self.Groups:
01291 eef_name = item[1][-1]
01292 print("{}: {}".format(eef_name, self.getCurrentPosition(eef_name)))
01293 raise RuntimeError("need to specify joint name")
01294 pose = self.getCurrentPose(lname, frame_name)
01295 return [pose[3], pose[7], pose[11]]
01296
01297 def getCurrentRotation(self, lname, frame_name=None):
01298 '''!@brief
01299 Returns the current physical rotation of the specified joint.
01300 cf. getReferenceRotation that returns commanded value.
01301
01302 @type lname: str
01303 @param lname: Name of the link.
01304 @param frame_name str: set reference frame name (from 315.2.5)
01305 @rtype: list of float
01306 @return: Rotational matrix of the given joint in 2-dimensional list,
01307 that is:
01308 \verbatim
01309 [[a11, a12, a13],
01310 [a21, a22, a23],
01311 [a31, a32, a33]]
01312 \endverbatim
01313 '''
01314 if not lname:
01315 for item in self.Groups:
01316 eef_name = item[1][-1]
01317 print("{}: {}".format(eef_name, self.getCurrentRotation(eef_name)))
01318 raise RuntimeError("need to specify joint name")
01319 pose = self.getCurrentPose(lname, frame_name)
01320 return [pose[0:3], pose[4:7], pose[8:11]]
01321
01322 def getCurrentRPY(self, lname, frame_name=None):
01323 '''!@brief
01324 Returns the current physical rotation in RPY of the specified joint.
01325 cf. getReferenceRPY that returns commanded value.
01326
01327 @type lname: str
01328 @param lname: Name of the link.
01329 @param frame_name str: set reference frame name (from 315.2.5)
01330 @rtype: list of float
01331 @return: List of orientation in rpy form about the specified joint.
01332 '''
01333 if not lname:
01334 for item in self.Groups:
01335 eef_name = item[1][-1]
01336 print("{}: {}".format(eef_name, self.getCurrentRPY(eef_name)))
01337 raise RuntimeError("need to specify joint name")
01338 return euler_from_matrix(self.getCurrentRotation(lname), 'sxyz')
01339
01340 def getReferencePose(self, lname, frame_name=None):
01341 '''!@brief
01342 Returns the current commanded pose of the specified joint in the joint space.
01343 cf. getCurrentPose that returns physical pose.
01344
01345 @type lname: str
01346 @param lname: Name of the link.
01347 @param frame_name str: set reference frame name (from 315.2.5)
01348 @rtype: list of float
01349 @return: Rotational matrix and the position of the given joint in
01350 1-dimensional list, that is:
01351 \verbatim
01352 [a11, a12, a13, x,
01353 a21, a22, a23, y,
01354 a31, a32, a33, z,
01355 0, 0, 0, 1]
01356 \endverbatim
01357 '''
01358 if not lname:
01359 for item in self.Groups:
01360 eef_name = item[1][-1]
01361 print("{}: {}".format(eef_name, self.getReferencePose(eef_name)))
01362 raise RuntimeError("need to specify joint name")
01363 if frame_name:
01364 lname = lname + ':' + frame_name
01365 if StrictVersion(self.fk_version) < StrictVersion('315.2.5') and ':' in lname:
01366 raise RuntimeError('frame_name ('+lname+') is not supported')
01367 pose = self.fk_svc.getReferencePose(lname)
01368 if not pose[0]:
01369 raise RuntimeError("Could not find reference : " + lname)
01370 return pose[1].data
01371
01372 def getReferencePosition(self, lname, frame_name=None):
01373 '''!@brief
01374 Returns the current commanded position of the specified joint in Cartesian space.
01375 cf. getCurrentPosition that returns physical value.
01376
01377 @type lname: str
01378 @param lname: Name of the link.
01379 @param frame_name str: set reference frame name (from 315.2.5)
01380 @rtype: list of float
01381 @return: List of angles (degree) of all joints, in the order defined
01382 in the member variable 'Groups' (eg. chest, head1, head2, ..).
01383 '''
01384 if not lname:
01385 for item in self.Groups:
01386 eef_name = item[1][-1]
01387 print("{}: {}".format(eef_name, self.getReferencePosition(eef_name)))
01388 raise RuntimeError("need to specify joint name")
01389 pose = self.getReferencePose(lname, frame_name)
01390 return [pose[3], pose[7], pose[11]]
01391
01392 def getReferenceRotation(self, lname, frame_name=None):
01393 '''!@brief
01394 Returns the current commanded rotation of the specified joint.
01395 cf. getCurrentRotation that returns physical value.
01396
01397 @type lname: str
01398 @param lname: Name of the link.
01399 @param frame_name str: set reference frame name (from 315.2.5)
01400 @rtype: list of float
01401 @return: Rotational matrix of the given joint in 2-dimensional list,
01402 that is:
01403 \verbatim
01404 [[a11, a12, a13],
01405 [a21, a22, a23],
01406 [a31, a32, a33]]
01407 \endverbatim
01408 '''
01409 if not lname:
01410 for item in self.Groups:
01411 eef_name = item[1][-1]
01412 print("{}: {}".format(eef_name, self.getReferencePotation(eef_name)))
01413 raise RuntimeError("need to specify joint name")
01414 pose = self.getReferencePose(lname, frame_name)
01415 return [pose[0:3], pose[4:7], pose[8:11]]
01416
01417 def getReferenceRPY(self, lname, frame_name=None):
01418 '''!@brief
01419 Returns the current commanded rotation in RPY of the specified joint.
01420 cf. getCurrentRPY that returns physical value.
01421
01422 @type lname: str
01423 @param lname: Name of the link.
01424 @param frame_name str: set reference frame name (from 315.2.5)
01425 @rtype: list of float
01426 @return: List of orientation in rpy form about the specified joint.
01427 '''
01428 if not lname:
01429 for item in self.Groups:
01430 eef_name = item[1][-1]
01431 print("{}: {}".format(eef_name, self.getReferenceRPY(eef_name)))
01432 raise RuntimeError("need to specify joint name")
01433 return euler_from_matrix(self.getReferenceRotation(lname, frame_name), 'sxyz')
01434
01435 def setTargetPose(self, gname, pos, rpy, tm, frame_name=None):
01436 '''!@brief
01437 Move the end-effector to the given absolute pose.
01438 All d* arguments are in meter.
01439
01440 @param gname str: Name of the joint group.
01441 @param pos list of float: In meter.
01442 @param rpy list of float: In radian.
01443 @param tm float: Second to complete the command. This only includes the time taken for execution
01444 (i.e. time for planning and other misc. processes are not considered).
01445 @param frame_name str: Name of the frame that this particular command
01446 references to.
01447 @return bool: False if unreachable.
01448 '''
01449 print(gname, frame_name, pos, rpy, tm)
01450 if frame_name:
01451 gname = gname + ':' + frame_name
01452 result = self.seq_svc.setTargetPose(gname, pos, rpy, tm)
01453 if not result:
01454 print("setTargetPose failed. Maybe SequencePlayer failed to solve IK.\n"
01455 + "Currently, returning IK result error\n"
01456 + "(like the one in https://github.com/start-jsk/rtmros_hironx/issues/103)"
01457 + " is not implemented. Patch is welcomed.")
01458 return result
01459
01460 def setTargetPoseRelative(self, gname, eename, dx=0, dy=0, dz=0,
01461 dr=0, dp=0, dw=0, tm=10, wait=True):
01462 '''!@brief
01463 Move the end-effector's location relative to its current pose.
01464
01465 Note that because of "relative" nature, the method waits for the commands
01466 run previously to finish. Do not get confused with the "wait" argument,
01467 which regulates all subsequent commands, not previous ones.
01468
01469 For d*, distance arguments are in meter while rotations are in degree.
01470
01471 Example usage: The following moves RARM_JOINT5 joint 0.1mm forward
01472 within 0.1sec.
01473 \verbatim
01474 robot.setTargetPoseRelative('rarm', 'RARM_JOINT5', dx=0.0001, tm=0.1)
01475 \endverbatim
01476 @param gname str: Name of the joint group that is to be manipulated.
01477 @param eename str: Name of the joint that the manipulated joint group references to.
01478 @param dx float: In meter.
01479 @param dy float: In meter.
01480 @param dz float: In meter.
01481 @param dr float: In radian.
01482 @param dp float: In radian.
01483 @param dw float: In radian.
01484 @param tm float: Second to complete the command. This only includes the time taken for execution
01485 (i.e. time for planning and other misc. processes are not considered).
01486 @param wait bool: If true, all other subsequent commands wait until
01487 the movement commanded by this method call finishes.
01488 @return bool: False if unreachable.
01489 '''
01490 self.waitInterpolationOfGroup(gname)
01491
01492 posRef = None
01493 rpyRef = None
01494 tds = self.getCurrentPose(eename)
01495 if tds:
01496 posRef = numpy.array([tds[3], tds[7], tds[11]])
01497 matRef = numpy.array([tds[0:3], tds[4:7], tds[8:11]])
01498 posRef += [dx, dy, dz]
01499 matRef = matRef.dot(numpy.array(euler_matrix(dr, dp, dw)[:3, :3]))
01500 rpyRef = euler_from_matrix(matRef)
01501 print(posRef, rpyRef)
01502 ret = self.setTargetPose(gname, list(posRef), list(rpyRef), tm)
01503 if ret and wait:
01504 self.waitInterpolationOfGroup(gname)
01505 return ret
01506 return False
01507
01508 def clear(self):
01509 '''!@brief
01510 Clears the Sequencer's current operation. Works for joint groups too.
01511 @see HrpsysConfigurator.clear
01512
01513 Discussed in https://github.com/fkanehiro/hrpsys-base/issues/158
01514 Examples is found in a unit test: https://github.com/start-jsk/rtmros_hironx/blob/bb0672be3e03e5366e03fe50520e215302b8419f/hironx_ros_bridge/test/test_hironx.py#L293
01515 '''
01516 self.seq_svc.clear()
01517
01518 def clearOfGroup(self, gname, tm=0.0):
01519 '''!@brief
01520 Clears the Sequencer's current operation for joint groups.
01521 '''
01522 self.seq_svc.clearOfGroup(gname, tm)
01523
01524 def saveLog(self, fname='sample'):
01525 '''!@brief
01526 Save log to the given file name
01527
01528 @param fname str: name of the file
01529 '''
01530 self.log_svc.save(fname)
01531 print(self.configurator_name + "saved data to " + fname)
01532
01533 def clearLog(self):
01534 '''!@brief
01535 Clear logger's buffer
01536 '''
01537 self.log_svc.clear()
01538
01539 def setMaxLogLength(self, length):
01540 '''!@brief
01541 Set logger's buffer
01542 @param length int: length of log, if the system runs at 500hz and you want to store 2min, set 2*60*500.
01543 '''
01544 self.log_svc.maxLength(length)
01545
01546 def lengthDigitalInput(self):
01547 '''!@brief
01548 Returns the length of digital input port
01549 '''
01550 return self.rh_svc.lengthDigitalInput()
01551
01552 def lengthDigitalOutput(self):
01553 '''!@brief
01554 Returns the length of digital output port
01555 '''
01556 return self.rh_svc.lengthDigitalOutput()
01557
01558 def writeDigitalOutput(self, dout):
01559 '''!@brief
01560 Using writeDigitalOutputWithMask is recommended for the less data
01561 transport.
01562
01563 @param dout list of int: List of bits, length of 32 bits where elements are
01564 0 or 1.
01565
01566 What each element stands for depends on how
01567 the robot's imlemented. Consult the hardware manual.
01568 @return bool: RobotHardware.writeDigitalOutput returns True if writable. False otherwise.
01569 '''
01570 if self.simulation_mode:
01571 return True
01572 doutBitLength = self.lengthDigitalOutput() * 8
01573 if len(dout) < doutBitLength:
01574 for i in range(doutBitLength - len(dout)):
01575 dout.append(0)
01576 outStr = ''
01577 for i in range(0, len(dout), 8):
01578 oneChar = 0
01579 for j in range(8):
01580 if dout[i + j]:
01581 oneChar += 1 << j
01582 outStr = outStr + chr(oneChar)
01583
01584
01585 return self.rh_svc.writeDigitalOutput(outStr)
01586
01587 def writeDigitalOutputWithMask(self, dout, mask):
01588 '''!@brief
01589 Both dout and mask are lists with length of 32. Only the bit in dout
01590 that corresponds to the bits in mask that are flagged as 1 will be
01591 evaluated.
01592
01593 Example:
01594 \verbatim
01595 Case-1. Only 18th bit will be evaluated as 1.
01596 dout [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
01597 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
01598 mask [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
01599 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
01600
01601 Case-2. Only 18th bit will be evaluated as 0.
01602 dout [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
01603 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
01604 mask [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
01605 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
01606
01607 Case-3. None will be evaluated since there's no flagged bit in mask.
01608 dout [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
01609 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
01610 mask [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
01611 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
01612 \endverbatim
01613 @param dout list of int: List of bits, length of 32 bits where elements are
01614 0 or 1.
01615 @param mask list of int: List of masking bits, length of 32 bits where elements are
01616 0 or 1.
01617 @return bool: RobotHardware.writeDigitalOutput returns True if writable. False otherwise.
01618 '''
01619 if self.simulation_mode:
01620 return True
01621 doutBitLength = self.lengthDigitalOutput() * 8
01622 if len(dout) < doutBitLength and \
01623 len(mask) < doutBitLength and \
01624 len(dout) == len(mask):
01625 for i in range(doutBitLength - len(dout)):
01626 dout.append(0)
01627 mask.append(0)
01628 outStr = ''
01629 outMsk = ''
01630 for i in range(0, len(dout), 8):
01631 oneChar = 0
01632 oneMask = 0
01633 for j in range(8):
01634 if dout[i + j]:
01635 oneChar += 1 << j
01636 if mask[i + j]:
01637 oneMask += 1 << j
01638 outStr = outStr + chr(oneChar)
01639 outMsk = outMsk + chr(oneMask)
01640
01641
01642 return self.rh_svc.writeDigitalOutputWithMask(outStr, outMsk)
01643
01644 def readDigitalInput(self):
01645 '''!@brief
01646 Read data from Digital Input
01647 @see: HrpsysConfigurator.readDigitalInput
01648
01649 Digital input consits of 14 bits. The last 2 bits are lacking
01650 and compensated, so that the last 4 bits are 0x4 instead of 0x1.
01651 @author Hajime Saito (\@emijah)
01652 @return list of int: List of the values (2 octtets) in digital input register. Range: 0 or 1.
01653
01654 #TODO: Catch AttributeError that occurs when RobotHardware not found.
01655 # Typically with simulator, erro msg might look like this;
01656 # 'NoneType' object has no attribute 'readDigitalInput'
01657 '''
01658 if self.simulation_mode:
01659 return []
01660 ret, din = self.rh_svc.readDigitalInput()
01661 retList = []
01662 for item in din:
01663 for i in range(8):
01664 if (ord(item) >> i) & 1:
01665 retList.append(1)
01666 else:
01667 retList.append(0)
01668 return retList
01669
01670 def readDigitalOutput(self):
01671 '''!@brief
01672 Read data from Digital Output
01673
01674 Digital input consits of 14 bits. The last 2 bits are lacking
01675 and compensated, so that the last 4 bits are 0x4 instead of 0x1.
01676
01677 #TODO: Catch AttributeError that occurs when RobotHardware not found.
01678 # Typically with simulator, erro msg might look like this;
01679 # 'NoneType' object has no attribute 'readDigitaloutput'
01680
01681 @author Hajime Saito (\@emijah)
01682 @return list of int: List of the values in digital input register. Range: 0 or 1.
01683 '''
01684 ret, dout = self.rh_svc.readDigitalOutput()
01685 retList = []
01686 for item in dout:
01687 for i in range(8):
01688 if (ord(item) >> i) & 1:
01689 retList.append(1)
01690 else:
01691 retList.append(0)
01692 return retList
01693
01694 def getActualState(self):
01695 '''!@brief
01696 Get actual states of the robot that includes current reference joint angles and joint torques.
01697 @return: This returns actual states of the robot, which is defined in
01698 RobotHardware.idl
01699 (found at https://github.com/fkanehiro/hrpsys-base/blob/3fd7671de5129101a4ade3f98e2eac39fd6b26c0/idl/RobotHardwareService.idl#L32_L57 as of version 315.11.0)
01700 \verbatim
01701 /**
01702 * @brief status of the robot
01703 */
01704 struct RobotState
01705 {
01706 DblSequence angle; ///< current joint angles[rad]
01707 DblSequence command;///< reference joint angles[rad]
01708 DblSequence torque; ///< joint torques[Nm]
01709 /**
01710 * @brief servo statuses(32bit+extra states)
01711 *
01712 * 0: calib status ( 1 => done )\n
01713 * 1: servo status ( 1 => on )\n
01714 * 2: power status ( 1 => supplied )\n
01715 * 3-18: servo alarms (see @ref iob.h)\n
01716 * 19-23: unused
01717 * 24-31: driver temperature (deg)
01718 */
01719 LongSequenceSequence servoState;
01720 sequence<DblSequence6> force; ///< forces[N] and torques[Nm]
01721 sequence<DblSequence3> rateGyro; ///< angular velocities[rad/s]
01722 sequence<DblSequence3> accel; ///< accelerations[m/(s^2)]
01723 double voltage; ///< voltage of power supply[V]
01724 double current; ///< current[A]
01725 };
01726 \endverbatim
01727 '''
01728 return self.rh_svc.getStatus()
01729
01730 def isCalibDone(self):
01731 '''!@brief
01732 Check whether joints have been calibrated.
01733 @return bool: True if all joints are calibrated
01734 '''
01735 if self.simulation_mode:
01736 return True
01737 else:
01738 rstt = self.rh_svc.getStatus()
01739 for item in rstt.servoState:
01740 if not item[0] & 1:
01741 return False
01742 return True
01743
01744 def isServoOn(self, jname='any'):
01745 '''!@brief
01746 Check whether servo control has been turned on.
01747 @param jname str: Name of a link (that can be obtained by "hiro.Groups"
01748 as lists of groups).
01749 When jname = 'all' or 'any' => If all joints are servoOn, return True, otherwise, return False.
01750 When jname = 'some' => If some joint is servoOn, return True, otherwise return False.
01751 @return bool: True if servo is on
01752 '''
01753 if self.simulation_mode:
01754 return True
01755 else:
01756 s_s = self.getActualState().servoState
01757 if jname.lower() == 'any' or jname.lower() == 'all':
01758 for s in s_s:
01759
01760 if (s[0] & 2) == 0:
01761 return False
01762 return True
01763 elif jname.lower() == 'some':
01764 for s in s_s:
01765
01766 if (s[0] & 2) != 0:
01767 return True
01768 return False
01769 else:
01770 jid = eval('self.' + jname)
01771 print(self.configurator_name + s_s[jid])
01772 if s_s[jid][0] & 1 == 0:
01773 return False
01774 else:
01775 return True
01776
01777 def flat2Groups(self, flatList):
01778 '''!@brief
01779 Convert list of angles into list of joint list for each groups.
01780 @param flatList list: single dimension list with its length of 15
01781 @return list of list: 2-dimensional list of Groups.
01782 '''
01783 retList = []
01784 index = 0
01785 for group in self.Groups:
01786 joint_num = len(group[1])
01787 retList.append(flatList[index: index + joint_num])
01788 index += joint_num
01789 return retList
01790
01791 def servoOn(self, jname='all', destroy=1, tm=3):
01792 '''!@brief
01793 Turn on servos.
01794 Joints need to be calibrated (otherwise error returns).
01795
01796 @param jname str: The value 'all' works iteratively for all servos.
01797 @param destroy int: Not used.
01798 @param tm float: Second to complete.
01799 @return int: 1 or -1 indicating success or failure, respectively.
01800 '''
01801
01802 if not self.isCalibDone():
01803 waitInputConfirm('!! Calibrate Encoders with checkEncoders first !!\n\n')
01804 return -1
01805
01806
01807 if self.isServoOn():
01808 return 1
01809
01810
01811 if jname == '':
01812 jname = 'all'
01813
01814 try:
01815 r = waitInputConfirm(\
01816 '!! Robot Motion Warning (SERVO_ON) !!\n\n'
01817 'Confirm RELAY switched ON\n'
01818 'Push [OK] to switch servo ON(%s).' % (jname))
01819 if not r:
01820 print(self.configurator_name, 'servo on: canceled')
01821 return 0
01822 except:
01823 self.rh_svc.power('all', SWITCH_OFF)
01824 raise
01825
01826 try:
01827 self.goActual()
01828 time.sleep(0.1)
01829 self.rh_svc.servo(jname, SWITCH_ON)
01830 time.sleep(2)
01831 if not self.isServoOn(jname):
01832 print(self.configurator_name + 'servo on failed.')
01833 raise
01834 except:
01835 print(self.configurator_name + 'exception occured')
01836
01837 return 1
01838
01839 def servoOff(self, jname='all', wait=True):
01840 '''!@brief
01841 Turn off servos.
01842 @param jname str: The value 'all' works iteratively for all servos.
01843 @param wait bool: Wait for user's confirmation via GUI
01844 @return int: 1 = all arm servo off. 2 = all servo on arms and hands off.
01845 -1 = Something wrong happened.
01846 '''
01847
01848 if self.simulation_mode:
01849 print(self.configurator_name + 'omit servo off')
01850 return 0
01851
01852 print('Turn off Hand Servo')
01853 if self.sc_svc:
01854 self.sc_svc.servoOff()
01855
01856 if not self.isServoOn(jname):
01857 if jname.lower() == 'all':
01858 self.rh_svc.power('all', SWITCH_OFF)
01859 return 1
01860
01861
01862 if jname == '':
01863 jname = 'all'
01864
01865 if wait:
01866 r = waitInputConfirm(
01867 '!! Robot Motion Warning (Servo OFF)!!\n\n'
01868 'Push [OK] to servo OFF (%s).' % (jname))
01869 if not r:
01870 print(self.configurator_name, 'servo off: canceled')
01871 return 2
01872 try:
01873 self.rh_svc.servo('all', SWITCH_OFF)
01874 time.sleep(0.2)
01875 if jname == 'all':
01876 self.rh_svc.power('all', SWITCH_OFF)
01877
01878
01879 print('Turn off Hand Servo')
01880 if self.sc_svc:
01881 self.sc_svc.servoOff()
01882
01883 return 2
01884 except:
01885 print(self.configurator_name + 'servo off: communication error')
01886 return -1
01887
01888 def checkEncoders(self, jname='all', option=''):
01889 '''!@brief
01890 Run the encoder checking sequence for specified joints,
01891 run goActual and turn on servos.
01892
01893 @param jname str: The value 'all' works iteratively for all servos.
01894 @param option str: Possible values are follows (w/o double quote):\
01895 "-overwrite": Overwrite calibration value.
01896 '''
01897 if self.isServoOn():
01898 waitInputConfirm('Servo must be off for calibration')
01899 return
01900
01901 elif self.isCalibDone():
01902 waitInputConfirm('System has been calibrated')
01903 return
01904
01905 self.rh_svc.power('all', SWITCH_ON)
01906 msg = '!! Robot Motion Warning !!\n'\
01907 'Turn Relay ON.\n'\
01908 'Then Push [OK] to '
01909 if option == '-overwrite':
01910 msg = msg + 'calibrate(OVERWRITE MODE) '
01911 else:
01912 msg = msg + 'check '
01913
01914 if jname == 'all':
01915 msg = msg + 'the Encoders of all.'
01916 else:
01917 msg = msg + 'the Encoder of the Joint "' + jname + '".'
01918
01919 try:
01920 waitInputConfirm(msg)
01921 except:
01922 print("If you're connecting to the robot from remote, " + \
01923 "make sure tunnel X (eg. -X option with ssh).")
01924 self.rh_svc.power('all', SWITCH_OFF)
01925 return 0
01926
01927 print(self.configurator_name + 'calib-joint ' + jname + ' ' + option)
01928 self.rh_svc.initializeJointAngle(jname, option)
01929 print(self.configurator_name + 'done')
01930 self.rh_svc.power('all', SWITCH_OFF)
01931 self.goActual()
01932 time.sleep(0.1)
01933 self.rh_svc.servo(jname, SWITCH_ON)
01934
01935
01936 print('Turn on Hand Servo')
01937 if self.sc_svc:
01938 self.sc_svc.servoOn()
01939
01940 def removeForceSensorOffset(self):
01941 '''!@brief
01942 remove force sensor offset
01943 '''
01944 self.rh_svc.removeForceSensorOffset()
01945
01946 def playPattern(self, jointangles, rpy, zmp, tm):
01947 '''!@brief
01948 Play motion pattern using a given trajectory that is represented by
01949 a list of joint angles, rpy, zmp and time.
01950
01951 @type jointangles: [[float]]
01952 @param jointangles: Sequence of the sets of joint angles in radian.
01953 The whole list represents a trajectory. Each element
01954 of the 1st degree in the list consists of the joint angles.
01955 @param rpy list of float: Orientation in rpy.
01956 @param zmp list of float: TODO: description
01957 @param tm float: Second to complete the command. This only includes the time taken for execution
01958 (i.e. time for planning and other misc. processes are not considered).
01959 @return bool:
01960 '''
01961 return self.seq_svc.playPattern(jointangles, rpy, zmp, tm)
01962
01963 def playPatternOfGroup(self, gname, jointangles, tm):
01964 '''!@brief
01965 Play motion pattern using a set of given trajectories that are represented by
01966 lists of joint angles. Each trajectory aims to run within the specified time (tm),
01967 and there's no slow down between trajectories unless the next one is the last.
01968
01969 Example:
01970 self.playPatternOfGroup('larm',
01971 [[0.0, 0.0, -2.2689280275926285, 0.0, 0.0, 0.0],
01972 [0.0, 0.0, -1.9198621771937625, 0.0, 0.0, 0.0],
01973 [0.0, 0.0, -1.5707963, 0.0, 0.0, 0.0]],
01974 [3, 3, 10])
01975
01976 @param gname str: Name of the joint group.
01977 @type jointangles: [[float]]
01978 @param jointangles: Sequence of the sets of joint angles in radian.
01979 The whole list represents a trajectory. Each element
01980 of the 1st degree in the list consists of the joint
01981 angles. To illustrate:
01982
01983 [[a0-0, a0-1,...,a0-n], # a)ngle. 1st path in trajectory
01984 [a1-0, a1-1,...,a1-n], # 2nd path in the trajectory.
01985 :
01986 [am-0, am-1,...,am-n]] # mth path in the trajectory
01987 @type tm: [float]
01988 @param tm float: Sequence of the time values to complete the task,
01989 each element of which corresponds to a set of jointangles
01990 in the same order.
01991 @return bool:
01992 '''
01993 return self.seq_svc.playPatternOfGroup(gname, jointangles, tm)
01994
01995 def setSensorCalibrationJointAngles(self):
01996 '''!@brief
01997 Set joint angles for sensor calibration.
01998 Please override joint angles to avoid self collision.
01999 '''
02000 self.setJointAngles([0]*len(self.rh_svc.getStatus().servoState), 4.0)
02001 self.waitInterpolation()
02002
02003 def calibrateInertiaSensor(self):
02004 '''!@brief
02005 Calibrate inertia sensor
02006 '''
02007 self.rh_svc.calibrateInertiaSensor()
02008
02009 def calibrateInertiaSensorWithDialog(self):
02010 '''!@brief
02011 Calibrate inertia sensor with dialog and setting calibration pose
02012 '''
02013 r = waitInputConfirm (
02014 '!! Robot Motion Warning (Move to Sensor Calibration Pose)!!\n\n'
02015 'Push [OK] to move to sensor calibration pose.')
02016 if not r: return False
02017 self.setSensorCalibrationJointAngles()
02018 r = waitInputConfirm (
02019 '!! Put the robot down!!\n\n'
02020 'Push [OK] to the next step.')
02021 if not r: return False
02022 self.calibrateInertiaSensor()
02023 r = waitInputConfirm (
02024 '!! Put the robot up!!\n\n'
02025 'Push [OK] to the next step.')
02026 if not r: return False
02027 return True
02028
02029
02030
02031
02032 def startAutoBalancer(self, limbs=None):
02033 '''!@brief
02034 Start AutoBalancer mode
02035 @param limbs list of end-effector name to control.
02036 If Groups has rarm and larm, rleg, lleg, rarm, larm by default.
02037 If Groups is not defined or Groups does not have rarm and larm, rleg and lleg by default.
02038 '''
02039 if limbs==None:
02040 if self.Groups != None and "rarm" in map (lambda x : x[0], self.Groups) and "larm" in map (lambda x : x[0], self.Groups):
02041 limbs=["rleg", "lleg", "rarm", "larm"]
02042 else:
02043 limbs=["rleg", "lleg"]
02044 self.abc_svc.startAutoBalancer(limbs)
02045
02046 def stopAutoBalancer(self):
02047 '''!@brief
02048 Stop AutoBalancer mode
02049 '''
02050 self.abc_svc.stopAutoBalancer()
02051
02052 def startStabilizer(self):
02053 '''!@brief
02054 Start Stabilzier mode
02055 '''
02056 self.st_svc.startStabilizer()
02057
02058 def stopStabilizer(self):
02059 '''!@brief
02060 Stop Stabilzier mode
02061 '''
02062 self.st_svc.stopStabilizer()
02063
02064 def startImpedance_315_4(self, arm,
02065 M_p = 100.0,
02066 D_p = 100.0,
02067 K_p = 100.0,
02068 M_r = 100.0,
02069 D_r = 2000.0,
02070 K_r = 2000.0,
02071 force_gain = [1, 1, 1],
02072 moment_gain = [0, 0, 0],
02073 sr_gain = 1.0,
02074 avoid_gain = 0.0,
02075 reference_gain = 0.0,
02076 manipulability_limit = 0.1,
02077 controller_mode = None,
02078 ik_optional_weight_vector = None):
02079 '''!@brief
02080 start impedance mode
02081
02082 @type arm: str name of artm to be controlled, this must be initialized using setSelfGroups()
02083 @param force_gain, moment_gain: multipliers to the eef offset position vel_p and orientation vel_r.
02084 3-dimensional vector (then converted internally into a diagonal matrix).
02085 '''
02086 r, p = self.ic_svc.getImpedanceControllerParam(arm)
02087 if not r:
02088 print('{}, Failt to getImpedanceControllerParam({})'.format(self.configurator_name, arm))
02089 return False
02090 if M_p != None: p.M_p = M_p
02091 if D_p != None: p.M_p = D_p
02092 if K_p != None: p.M_p = K_p
02093 if M_r != None: p.M_r = M_r
02094 if D_r != None: p.M_r = D_r
02095 if K_r != None: p.M_r = K_r
02096 if force_gain != None: p.force_gain = force_gain
02097 if moment_gain != None: p.moment_gain = moment_gain
02098 if sr_gain != None: p.sr_gain = sr_gain
02099 if avoid_gain != None: p.avoid_gain = avoid_gain
02100 if reference_gain != None: p.reference_gain = reference_gain
02101 if manipulability_limit != None: p.manipulability_limit = manipulability_limit
02102 if controller_mode != None: p.controller_mode = controller_mode
02103 if ik_optional_weight_vector != None: p.ik_optional_weight_vector = ik_optional_weight_vector
02104 self.ic_svc.setImpedanceControllerParam(arm, p)
02105 return self.ic_svc.startImpedanceController(arm)
02106
02107 def stopImpedance_315_4(self, arm):
02108 '''!@brief
02109 stop impedance mode
02110 '''
02111 return self.ic_svc.stopImpedanceController(arm)
02112
02113 def startImpedance(self, arm, **kwargs):
02114 '''!@brief
02115 Enable the ImpedanceController RT component.
02116 This method internally calls startImpedance-*, hrpsys version-specific method.
02117
02118 @requires: hrpsys version greather than 315.2.0.
02119 @requires: ImpedanceController RTC to be activated on the robot's controller.
02120 @change: From 315.2.0 onward, following arguments are dropped and can be set by
02121 self.seq_svc.setWrenches instead of this method.
02122 See an example at https://github.com/fkanehiro/hrpsys-base/pull/434/files#diff-6204f002204dd9ae80f203901f155fa9R44:
02123
02124 - ref_force[fx, fy, fz] (unit: N) and ref_moment[tx, ty, tz] (unit: Nm) can be set via self.seq_svc.setWrenches. For example:
02125
02126 self.seq_svc.setWrenches([0, 0, 0, 0, 0, 0,
02127 fx, fy, fz, tx, ty, tz,
02128 0, 0, 0, 0, 0, 0,
02129 0, 0, 0, 0, 0, 0,])
02130
02131 setWrenches takes 6 values per sensor, so the robot in the example above has 4 sensors where each line represents a sensor.
02132 See this link (https://github.com/fkanehiro/hrpsys-base/pull/434/files) for a concrete example.
02133
02134 @param arm: Name of the kinematic group (i.e. self.Groups[n][0]).
02135 @param kwargs: This varies depending on the version of hrpsys your robot's controller runs on
02136 (which you can find by "self.hrpsys_version" command). For instance, if your
02137 hrpsys is 315.10.1, refer to "startImpedance_315_4" method.
02138 '''
02139 if self.hrpsys_version and StrictVersion(self.hrpsys_version) < StrictVersion('315.2.0'):
02140 print(self.configurator_name + '\033[31mstartImpedance: Try to connect unsupported RTC' + str(self.hrpsys_version) + '\033[0m')
02141 else:
02142 self.startImpedance_315_4(arm, **kwargs)
02143
02144 def stopImpedance(self, arm):
02145 if self.hrpsys_version and StrictVersion(self.hrpsys_version) < StrictVersion('315.2.0'):
02146 print(self.configurator_name + '\033[31mstopImpedance: Try to connect unsupported RTC' + str(self.hrpsys_version) + '\033[0m')
02147 else:
02148 self.stopImpedance_315_4(arm)
02149
02150 def startDefaultUnstableControllers (self, ic_limbs=[], abc_limbs=[]):
02151 '''!@brief
02152 Start default unstable RTCs controller mode.
02153 Currently Stabilzier, AutoBalancer, and ImpedanceController are started based on "Groups" setting.
02154 If ic_limbs or abc_limbs is not specified, default setting is set according to "Groups".
02155 By default,
02156 If the robot has an arm, start impedance for the arm.
02157 If the robot has a leg, start st and autobalancer.
02158 autobalancer's fixed limbs are all existing arms and legs.
02159 Use cases:
02160 Biped robot (leg only) : no impedance, start st, start autobalancer with ["rleg", "lleg"].
02161 Dual-arm robot (arm only) : start impedance ["rarm", "larm"], no st, no autobalancer.
02162 Dual-arm+biped robot (=humanoid robot) : start impedance ["rarm", "larm"], start st, start autobalancer with ["rleg", "lleg", "rarm", "larm"].
02163 Quadruped robot : same as "humanoid robot" by default.
02164 '''
02165 print(self.configurator_name + "Start Default Unstable Controllers")
02166
02167 is_legged_robot = map(lambda x: x[0], filter(lambda x : re.match(".*leg", x[0]), self.Groups))
02168
02169 if not ic_limbs:
02170 ic_limbs = map(lambda x: x[0], filter(lambda x : re.match(".*(arm)", x[0]), self.Groups))
02171
02172 if not abc_limbs:
02173 abc_limbs = map(lambda x: x[0], filter(lambda x : re.match(".*(leg|arm)", x[0]), self.Groups))
02174
02175 for limb in ic_limbs:
02176 self.ic_svc.startImpedanceControllerNoWait(limb)
02177 if is_legged_robot:
02178 self.startAutoBalancer(abc_limbs)
02179 self.startStabilizer()
02180 for limb in ic_limbs:
02181 self.ic_svc.waitImpedanceControllerTransition(limb)
02182
02183 if is_legged_robot:
02184 print(self.configurator_name + " Start AutoBalancer = "+str(abc_limbs))
02185 print(self.configurator_name + " Start Stabilizer")
02186 if len(ic_limbs) != 0:
02187 print(self.configurator_name + " Start ImpedanceController = "+str(ic_limbs))
02188
02189 def stopDefaultUnstableControllers (self, ic_limbs=[]):
02190 '''!@brief
02191 Stop default unstable RTCs controller mode.
02192 Currently Stabilzier, AutoBalancer, and ImpedanceController are stopped based on "Groups" setting.
02193 Please see documentation of startDefaultUnstableControllers().
02194 '''
02195 print(self.configurator_name + "Stop Default Unstable Controllers")
02196
02197 is_legged_robot = map(lambda x: x[0], filter(lambda x : re.match(".*leg", x[0]), self.Groups))
02198
02199 if not ic_limbs:
02200 ic_limbs = map(lambda x: x[0], filter(lambda x : re.match(".*(arm)", x[0]), self.Groups))
02201
02202 if is_legged_robot:
02203 self.stopStabilizer()
02204 for limb in ic_limbs:
02205 self.ic_svc.stopImpedanceControllerNoWait(limb)
02206 if is_legged_robot:
02207 self.stopAutoBalancer()
02208 for limb in ic_limbs:
02209 self.ic_svc.waitImpedanceControllerTransition(limb)
02210
02211 if is_legged_robot:
02212 print(self.configurator_name + " Stop AutoBalancer")
02213 print(self.configurator_name + " Stop Stabilizer")
02214 if len(ic_limbs) != 0:
02215 print(self.configurator_name + " Stop ImpedanceController = "+str(ic_limbs))
02216
02217 def setFootSteps(self, footstep, overwrite_fs_idx = 0):
02218 '''!@brief
02219 setFootSteps
02220 @param footstep : FootstepSequence.
02221 @param overwrite_fs_idx : Index to be overwritten. overwrite_fs_idx is used only in walking.
02222 '''
02223 self.abc_svc.setFootSteps(footstep, overwrite_fs_idx)
02224
02225 def setFootStepsWithParam(self, footstep, stepparams, overwrite_fs_idx = 0):
02226 '''!@brief
02227 setFootSteps
02228 @param footstep : FootstepSequence.
02229 @param stepparams : StepParamSeuqnce.
02230 @param overwrite_fs_idx : Index to be overwritten. overwrite_fs_idx is used only in walking.
02231 '''
02232 self.abc_svc.setFootStepsWithParam(footstep, stepparams, overwrite_fs_idx)
02233
02234 def clearJointAngles(self):
02235 '''!@brief
02236 Cancels the commanded sequence of joint angle, by overwriting the command by the values of instant the method was invoked.
02237 Note that this only cancels the queue once. Icoming commands after this method is called will be processed as usual.
02238 @return bool
02239 '''
02240 return self.seq_svc.clearJointAngles()
02241
02242 def clearJointAnglesOfGroup(self, gname):
02243 '''!@brief
02244 Cancels the commanded sequence of joint angle for the specified joint group, by overwriting the command by the values of instant the method was invoked.
02245 Note that this only cancels the queue once. Icoming commands after this method is called will be processed as usual.
02246 @param gname: Name of the joint group.
02247 @return bool
02248 '''
02249 return self.seq_svc.clearJointAnglesOfGroup(gname)
02250
02251 def removeForceSensorOffsetRMFO(self, sensor_names=[], tm=8.0):
02252 '''!@brief
02253 remove force sensor offset by RemoveForceSensorOffset (RMFO) RTC.
02254 @param sensor_names : list of sensor names to be calibrated. If not specified, all sensors are calibrated by default.
02255 @param tm : calibration time[s]. 8.0[s] by default.
02256 @return bool : true if set successfully, false otherwise
02257 '''
02258 return self.rmfo_svc.removeForceSensorOffset(sensor_names, tm)
02259
02260
02261
02262
02263
02264 def init(self, robotname="Robot", url=""):
02265 '''!@brief
02266 Calls init from its superclass, which tries to connect RTCManager,
02267 looks for ModelLoader, and starts necessary RTC components. Also runs
02268 config, logger.
02269 Also internally calls setSelfGroups().
02270
02271 @type robotname: str
02272 @type url: str
02273 '''
02274 print(self.configurator_name + "waiting ModelLoader")
02275 self.waitForModelLoader()
02276 print(self.configurator_name + "start hrpsys")
02277
02278 print(self.configurator_name + "finding RTCManager and RobotHardware")
02279 self.waitForRTCManagerAndRobotHardware(robotname)
02280 self.sensors = self.getSensors(url)
02281
02282 print(self.configurator_name + "creating components")
02283 self.createComps()
02284
02285 print(self.configurator_name + "connecting components")
02286 self.connectComps()
02287
02288 print(self.configurator_name + "activating components")
02289 self.activateComps()
02290
02291 print(self.configurator_name + "setup logger")
02292 self.setupLogger()
02293
02294 print(self.configurator_name + "setup joint groups")
02295 self.setSelfGroups()
02296
02297 print(self.configurator_name + '\033[32minitialized successfully\033[0m')
02298
02299
02300 try:
02301 self.hrpsys_version = self.fk.ref.get_component_profile().version
02302 except:
02303 print(self.configurator_name + '\033[34mCould not get hrpsys_version\033[0m')
02304
02305 pass
02306
02307 def __init__(self, cname="[hrpsys.py] "):
02308 initCORBA()
02309 self.configurator_name = cname
02310
02311
02312 if __name__ == '__main__':
02313 hcf = HrpsysConfigurator()
02314 if len(sys.argv) > 2:
02315 hcf.init(sys.argv[1], sys.argv[2])
02316 elif len(sys.argv) > 1:
02317 hcf.init(sys.argv[1])
02318 else:
02319 hcf.init()