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 self.log_svc.clear()
00925
00926 if self.log and (self.log_use_owned_ec or not isinstance(self.log.owned_ecs[0], OpenRTM._objref_ExtTrigExecutionContextService)):
00927 print(self.configurator_name + "\033[32mruning DataLogger with own Execution Context\033[0m")
00928 self.log.owned_ecs[0].start()
00929 self.log.start(self.log.owned_ecs[0])
00930
00931 def waitForRTCManager(self, managerhost=nshost):
00932 '''!@brief
00933 Wait for RTC manager.
00934
00935 @param managerhost str: name of host computer that manager is running
00936 '''
00937 self.ms = None
00938 while self.ms == None:
00939 time.sleep(1)
00940 if managerhost == "localhost":
00941 managerhost = socket.gethostname()
00942 self.ms = rtm.findRTCmanager(managerhost)
00943 print(self.configurator_name + "wait for RTCmanager : " + str(managerhost))
00944
00945 def waitForRobotHardware(self, robotname="Robot"):
00946 '''!@brief
00947 Wait for RobotHardware is exists and activated.
00948
00949 @param robotname str: name of RobotHardware component.
00950 '''
00951 self.rh = None
00952 timeout_count = 0
00953
00954 while self.rh == None and timeout_count < 10:
00955 if timeout_count > 0:
00956 time.sleep(1);
00957 self.rh = rtm.findRTC("RobotHardware0")
00958 if not self.rh:
00959 self.rh = rtm.findRTC(robotname)
00960 print(self.configurator_name + "wait for %s : %s ( timeout %d < 10)" % ( robotname, self.rh, timeout_count))
00961 if self.rh and self.rh.isActive() == None:
00962 self.rh = None
00963 timeout_count += 1
00964 if not self.rh:
00965 print(self.configurator_name + "Could not find " + robotname)
00966 if self.ms:
00967 print(self.configurator_name + "Candidates are .... " + str([x.name() for x in self.ms.get_components()]))
00968 print(self.configurator_name + "Exitting.... " + robotname)
00969 exit(1)
00970
00971 print(self.configurator_name + "findComps -> %s : %s isActive? = %s " % (self.rh.name(), self.rh, self.rh.isActive()))
00972
00973 def checkSimulationMode(self):
00974 '''!@brief
00975 Check if this is running as simulation
00976 '''
00977
00978
00979 self.hgc = findRTC("HGcontroller0")
00980 self.pdc = findRTC("PDcontroller0")
00981 if self.hgc or self.pdc:
00982 self.simulation_mode = True
00983 else:
00984 self.simulation_mode = False
00985
00986 if rtm.findPort(self.rh.ref, "servoState"):
00987 self.rh_svc = narrow(self.rh.service("service0"), "RobotHardwareService")
00988 self.ep_svc = narrow(self.rh.ec, "ExecutionProfileService")
00989
00990 print(self.configurator_name + "simulation_mode : %s" % self.simulation_mode)
00991
00992 def waitForRTCManagerAndRoboHardware(self, robotname="Robot", managerhost=nshost):
00993 print("\033[93m%s waitForRTCManagerAndRoboHardware has renamed to " % self.configurator_name + \
00994 "waitForRTCManagerAndRoboHardware: Please update your code\033[0m")
00995 return self.waitForRTCManagerAndRobotHardware(robotname=robotname, managerhost=managerhost)
00996
00997 def waitForRTCManagerAndRobotHardware(self, robotname="Robot", managerhost=nshost):
00998 '''!@brief
00999 Wait for both RTC Manager (waitForRTCManager()) and RobotHardware (waitForRobotHardware())
01000
01001 @param managerhost str: name of host computer that manager is running
01002 @param robotname str: name of RobotHardware component.
01003 '''
01004 self.waitForRTCManager(managerhost)
01005 self.waitForRobotHardware(robotname)
01006 self.checkSimulationMode()
01007
01008 def findModelLoader(self):
01009 '''!@brief
01010 Try to find ModelLoader
01011 '''
01012 try:
01013 return rtm.findObject("ModelLoader")
01014 except:
01015 return None
01016
01017 def waitForModelLoader(self):
01018 '''!@brief
01019 Wait for ModelLoader.
01020 '''
01021 while self.findModelLoader() == None:
01022 print(self.configurator_name + "wait for ModelLoader")
01023 time.sleep(3)
01024
01025 def setSelfGroups(self):
01026 '''!@brief
01027 Set to the hrpsys.SequencePlayer the groups of links and joints that
01028 are statically defined as member variables (Groups) within this class.
01029 '''
01030 for item in self.Groups:
01031 self.seq_svc.addJointGroup(item[0], item[1])
01032
01033
01034
01035
01036 def goActual(self):
01037 '''!@brief
01038 Adjust commanded values to the angles in the physical state
01039 by calling StateHolder::goActual.
01040
01041 This needs to be run BEFORE servos are turned on.
01042 '''
01043 self.sh_svc.goActual()
01044
01045 def setJointAngle(self, jname, angle, tm):
01046 '''!@brief
01047 Set angle to the given joint.
01048 \verbatim
01049 NOTE-1: It's known that this method does not do anything after
01050 some group operation is done.
01051 TODO: at least need to warn users.
01052 NOTE-2: While this method does not check angle value range,
01053 any joints could emit position limit over error, which has not yet
01054 been thrown by hrpsys so that there's no way to catch on this python client.
01055 Worthwhile opening an enhancement ticket at designated issue tracker.
01056 \endverbatim
01057
01058 @param jname str: name of joint
01059 @param angle float: In degree.
01060 @param tm float: Time to complete.
01061 @rtype bool
01062 @return False upon any problem during execution.
01063 '''
01064 radangle = angle / 180.0 * math.pi
01065 return self.seq_svc.setJointAngle(jname, radangle, tm)
01066
01067 def setJointAngles(self, angles, tm):
01068 '''!@brief
01069 Set all joint angles.
01070 \verbatim
01071 NOTE: While this method does not check angle value range,
01072 any joints could emit position limit over error, which has not yet
01073 been thrown by hrpsys so that there's no way to catch on this python client.
01074 Worthwhile opening an enhancement ticket at designated issue tracker.
01075 \endverbatim
01076 @param angles list of float: In degree.
01077 @param tm float: Time to complete.
01078 @rtype bool
01079 @return False upon any problem during execution.
01080 '''
01081 ret = []
01082 for angle in angles:
01083 ret.append(angle / 180.0 * math.pi)
01084 return self.seq_svc.setJointAngles(ret, tm)
01085
01086 def setJointAnglesOfGroup(self, gname, pose, tm, wait=True):
01087 '''!@brief
01088 Set the joint angles to aim. By default it waits interpolation to be
01089 over.
01090
01091 \verbatim
01092 NOTE: While this method does not check angle value range,
01093 any joints could emit position limit over error, which has not yet
01094 been thrown by hrpsys so that there's no way to catch on this python client.
01095 Worthwhile opening an enhancement ticket at designated issue tracker.
01096 \endverbatim
01097
01098 @param gname str: Name of the joint group.
01099 @param pose list of float: list of positions and orientations
01100 @param tm float: Time to complete.
01101 @param wait bool: If true, all other subsequent commands wait until
01102 the movement commanded by this method call finishes.
01103 @rtype bool
01104 @return False upon any problem during execution.
01105 '''
01106 angles = [x / 180.0 * math.pi for x in pose]
01107 ret = self.seq_svc.setJointAnglesOfGroup(gname, angles, tm)
01108 if wait:
01109 self.waitInterpolationOfGroup(gname)
01110 return ret
01111
01112 def setJointAnglesSequence(self, angless, tms):
01113 '''!@brief
01114 Set all joint angles. len(angless) should be equal to len(tms).
01115 \verbatim
01116 NOTE: While this method does not check angle value range,
01117 any joints could emit position limit over error, which has not yet
01118 been thrown by hrpsys so that there's no way to catch on this python client.
01119 Worthwhile opening an enhancement ticket at designated issue tracker.
01120 \endverbatim
01121 @param sequential list of angles in float: In rad
01122 @param tm sequential list of time in float: Time to complete, In Second
01123 @rtype bool
01124 @return False upon any problem during execution.
01125 '''
01126 for angles in angless:
01127 for i in range(len(angles)):
01128 angles[i] = angles[i] / 180.0 * math.pi
01129 return self.seq_svc.setJointAnglesSequence(angless, tms)
01130
01131 def setJointAnglesSequenceOfGroup(self, gname, angless, tms):
01132 '''!@brief
01133 Set all joint angles.
01134 \verbatim
01135 NOTE: While this method does not check angle value range,
01136 any joints could emit position limit over error, which has not yet
01137 been thrown by hrpsys so that there's no way to catch on this python client.
01138 Worthwhile opening an enhancement ticket at designated issue tracker.
01139 \endverbatim
01140 @param gname str: Name of the joint group.
01141 @param sequential list of angles in float: In rad
01142 @param tm sequential list of time in float: Time to complete, In Second
01143 @rtype bool
01144 @return False upon any problem during execution.
01145 '''
01146 for angles in angless:
01147 for i in range(len(angles)):
01148 angles[i] = angles[i] / 180.0 * math.pi
01149 return self.seq_svc.setJointAnglesSequenceOfGroup(gname, angless, tms)
01150
01151 def loadPattern(self, fname, tm):
01152 '''!@brief
01153 Load a pattern file that is created offline.
01154
01155 Format of the pattern file:
01156 - example format:
01157 \verbatim
01158 t0 j0 j1 j2...jn
01159 t1 j0 j1 j2...jn
01160 :
01161 tn j0 j1 j2...jn
01162 \endverbatim
01163 - Delimmitted by space
01164 - Each line consists of an action.
01165 - Time between each action is defined by tn+1 - tn
01166 - The time taken for the 1st line is defined by the arg tm.
01167
01168 @param fname str: Name of the pattern file.
01169 @param tm float: - The time to take for the 1st line.
01170 @return: List of 2 oct(string) values.
01171 '''
01172 fname = self.parseUrl(fname)
01173 return self.seq_svc.loadPattern(fname, tm)
01174
01175 def waitInterpolation(self):
01176 '''!@brief
01177 Lets SequencePlayer wait until the movement currently happening to
01178 finish.
01179 '''
01180 self.seq_svc.waitInterpolation()
01181
01182 def waitInterpolationOfGroup(self, gname):
01183 '''!@brief
01184 Lets SequencePlayer wait until the movement currently happening to
01185 finish.
01186
01187 @see: SequencePlayer.waitInterpolationOfGroup
01188 @see: http://wiki.ros.org/joint_trajectory_action. This method
01189 corresponds to JointTrajectoryGoal in ROS.
01190
01191 @param gname str: Name of the joint group.
01192 '''
01193 self.seq_svc.waitInterpolationOfGroup(gname)
01194
01195 def setInterpolationMode(self, mode):
01196 '''!@brief
01197 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.
01198 @param mode new interpolation mode. Either { OpenHRP.SequencePlayerService.LINEAR, OpenHRP.SequencePlayerService.HOFFARBIB }.
01199 @return true if set successfully, false otherwise
01200 '''
01201 return self.seq_svc.setInterpolationMode(mode)
01202
01203 def getJointAngles(self):
01204 '''!@brief
01205 Returns the commanded joint angle values.
01206
01207 @see: HrpsysConfigurator.getJointAngles
01208
01209 Note that it's not the physical state of the robot's joints, which
01210 can be obtained by getActualState().angle.
01211
01212 @return List of float: List of angles (degree) of all joints, in the order defined
01213 in the member variable 'Groups' (eg. chest, head1, head2, ..).
01214 '''
01215 return [x * 180.0 / math.pi for x in self.sh_svc.getCommand().jointRefs]
01216
01217 def getCurrentPose(self, lname=None, frame_name=None):
01218 '''!@brief
01219 Returns the current physical pose of the specified joint in the joint space.
01220 cf. getReferencePose that returns commanded value.
01221
01222 eg.
01223 \verbatim
01224 IN: robot.getCurrentPose('LARM_JOINT5')
01225 OUT: [-0.0017702356144599085,
01226 0.00019034630541264752,
01227 -0.9999984150158207,
01228 0.32556275164378523,
01229 0.00012155879975329215,
01230 0.9999999745367515,
01231 0.0001901314142046251,
01232 0.18236394191140365,
01233 0.9999984257434246,
01234 -0.00012122202968358842,
01235 -0.001770258707652326,
01236 0.07462472659364472,
01237 0.0,
01238 0.0,
01239 0.0,
01240 1.0]
01241 \endverbatim
01242
01243 @type lname: str
01244 @param lname: Name of the link.
01245 @param frame_name str: set reference frame name (from 315.2.5)
01246 @rtype: list of float
01247 @return: Rotational matrix and the position of the given joint in
01248 1-dimensional list, that is:
01249 \verbatim
01250 [a11, a12, a13, x,
01251 a21, a22, a23, y,
01252 a31, a32, a33, z,
01253 0, 0, 0, 1]
01254 \endverbatim
01255 '''
01256 if not lname:
01257 for item in self.Groups:
01258 eef_name = item[1][-1]
01259 print("{}: {}".format(eef_name, self.getCurrentPose(eef_name)))
01260 raise RuntimeError("need to specify joint name")
01261 if frame_name:
01262 lname = lname + ':' + frame_name
01263 if StrictVersion(self.fk_version) < StrictVersion('315.2.5') and ':' in lname:
01264 raise RuntimeError('frame_name ('+lname+') is not supported')
01265 pose = self.fk_svc.getCurrentPose(lname)
01266 if not pose[0]:
01267 raise RuntimeError("Could not find reference : " + lname)
01268 return pose[1].data
01269
01270 def getCurrentPosition(self, lname=None, frame_name=None):
01271 '''!@brief
01272 Returns the current physical position of the specified joint in Cartesian space.
01273 cf. getReferencePosition that returns commanded value.
01274
01275 eg.
01276 \verbatim
01277 robot.getCurrentPosition('LARM_JOINT5')
01278 [0.325, 0.182, 0.074]
01279 \endverbatim
01280
01281 @type lname: str
01282 @param lname: Name of the link.
01283 @param frame_name str: set reference frame name (from 315.2.5)
01284 @rtype: list of float
01285 @return: List of x, y, z positions about the specified joint.
01286 '''
01287 if not lname:
01288 for item in self.Groups:
01289 eef_name = item[1][-1]
01290 print("{}: {}".format(eef_name, self.getCurrentPosition(eef_name)))
01291 raise RuntimeError("need to specify joint name")
01292 pose = self.getCurrentPose(lname, frame_name)
01293 return [pose[3], pose[7], pose[11]]
01294
01295 def getCurrentRotation(self, lname, frame_name=None):
01296 '''!@brief
01297 Returns the current physical rotation of the specified joint.
01298 cf. getReferenceRotation that returns commanded value.
01299
01300 @type lname: str
01301 @param lname: Name of the link.
01302 @param frame_name str: set reference frame name (from 315.2.5)
01303 @rtype: list of float
01304 @return: Rotational matrix of the given joint in 2-dimensional list,
01305 that is:
01306 \verbatim
01307 [[a11, a12, a13],
01308 [a21, a22, a23],
01309 [a31, a32, a33]]
01310 \endverbatim
01311 '''
01312 if not lname:
01313 for item in self.Groups:
01314 eef_name = item[1][-1]
01315 print("{}: {}".format(eef_name, self.getCurrentRotation(eef_name)))
01316 raise RuntimeError("need to specify joint name")
01317 pose = self.getCurrentPose(lname, frame_name)
01318 return [pose[0:3], pose[4:7], pose[8:11]]
01319
01320 def getCurrentRPY(self, lname, frame_name=None):
01321 '''!@brief
01322 Returns the current physical rotation in RPY of the specified joint.
01323 cf. getReferenceRPY that returns commanded value.
01324
01325 @type lname: str
01326 @param lname: Name of the link.
01327 @param frame_name str: set reference frame name (from 315.2.5)
01328 @rtype: list of float
01329 @return: List of orientation in rpy form about the specified joint.
01330 '''
01331 if not lname:
01332 for item in self.Groups:
01333 eef_name = item[1][-1]
01334 print("{}: {}".format(eef_name, self.getCurrentRPY(eef_name)))
01335 raise RuntimeError("need to specify joint name")
01336 return euler_from_matrix(self.getCurrentRotation(lname), 'sxyz')
01337
01338 def getReferencePose(self, lname, frame_name=None):
01339 '''!@brief
01340 Returns the current commanded pose of the specified joint in the joint space.
01341 cf. getCurrentPose that returns physical pose.
01342
01343 @type lname: str
01344 @param lname: Name of the link.
01345 @param frame_name str: set reference frame name (from 315.2.5)
01346 @rtype: list of float
01347 @return: Rotational matrix and the position of the given joint in
01348 1-dimensional list, that is:
01349 \verbatim
01350 [a11, a12, a13, x,
01351 a21, a22, a23, y,
01352 a31, a32, a33, z,
01353 0, 0, 0, 1]
01354 \endverbatim
01355 '''
01356 if not lname:
01357 for item in self.Groups:
01358 eef_name = item[1][-1]
01359 print("{}: {}".format(eef_name, self.getReferencePose(eef_name)))
01360 raise RuntimeError("need to specify joint name")
01361 if frame_name:
01362 lname = lname + ':' + frame_name
01363 if StrictVersion(self.fk_version) < StrictVersion('315.2.5') and ':' in lname:
01364 raise RuntimeError('frame_name ('+lname+') is not supported')
01365 pose = self.fk_svc.getReferencePose(lname)
01366 if not pose[0]:
01367 raise RuntimeError("Could not find reference : " + lname)
01368 return pose[1].data
01369
01370 def getReferencePosition(self, lname, frame_name=None):
01371 '''!@brief
01372 Returns the current commanded position of the specified joint in Cartesian space.
01373 cf. getCurrentPosition that returns physical value.
01374
01375 @type lname: str
01376 @param lname: Name of the link.
01377 @param frame_name str: set reference frame name (from 315.2.5)
01378 @rtype: list of float
01379 @return: List of angles (degree) of all joints, in the order defined
01380 in the member variable 'Groups' (eg. chest, head1, head2, ..).
01381 '''
01382 if not lname:
01383 for item in self.Groups:
01384 eef_name = item[1][-1]
01385 print("{}: {}".format(eef_name, self.getReferencePosition(eef_name)))
01386 raise RuntimeError("need to specify joint name")
01387 pose = self.getReferencePose(lname, frame_name)
01388 return [pose[3], pose[7], pose[11]]
01389
01390 def getReferenceRotation(self, lname, frame_name=None):
01391 '''!@brief
01392 Returns the current commanded rotation of the specified joint.
01393 cf. getCurrentRotation that returns physical value.
01394
01395 @type lname: str
01396 @param lname: Name of the link.
01397 @param frame_name str: set reference frame name (from 315.2.5)
01398 @rtype: list of float
01399 @return: Rotational matrix of the given joint in 2-dimensional list,
01400 that is:
01401 \verbatim
01402 [[a11, a12, a13],
01403 [a21, a22, a23],
01404 [a31, a32, a33]]
01405 \endverbatim
01406 '''
01407 if not lname:
01408 for item in self.Groups:
01409 eef_name = item[1][-1]
01410 print("{}: {}".format(eef_name, self.getReferencePotation(eef_name)))
01411 raise RuntimeError("need to specify joint name")
01412 pose = self.getReferencePose(lname, frame_name)
01413 return [pose[0:3], pose[4:7], pose[8:11]]
01414
01415 def getReferenceRPY(self, lname, frame_name=None):
01416 '''!@brief
01417 Returns the current commanded rotation in RPY of the specified joint.
01418 cf. getCurrentRPY that returns physical value.
01419
01420 @type lname: str
01421 @param lname: Name of the link.
01422 @param frame_name str: set reference frame name (from 315.2.5)
01423 @rtype: list of float
01424 @return: List of orientation in rpy form about the specified joint.
01425 '''
01426 if not lname:
01427 for item in self.Groups:
01428 eef_name = item[1][-1]
01429 print("{}: {}".format(eef_name, self.getReferenceRPY(eef_name)))
01430 raise RuntimeError("need to specify joint name")
01431 return euler_from_matrix(self.getReferenceRotation(lname, frame_name), 'sxyz')
01432
01433 def setTargetPose(self, gname, pos, rpy, tm, frame_name=None):
01434 '''!@brief
01435 Move the end-effector to the given absolute pose.
01436 All d* arguments are in meter.
01437
01438 @param gname str: Name of the joint group.
01439 @param pos list of float: In meter.
01440 @param rpy list of float: In radian.
01441 @param tm float: Second to complete the command. This only includes the time taken for execution
01442 (i.e. time for planning and other misc. processes are not considered).
01443 @param frame_name str: Name of the frame that this particular command
01444 references to.
01445 @return bool: False if unreachable.
01446 '''
01447 print(gname, frame_name, pos, rpy, tm)
01448 if frame_name:
01449 gname = gname + ':' + frame_name
01450 result = self.seq_svc.setTargetPose(gname, pos, rpy, tm)
01451 if not result:
01452 print("setTargetPose failed. Maybe SequencePlayer failed to solve IK.\n"
01453 + "Currently, returning IK result error\n"
01454 + "(like the one in https://github.com/start-jsk/rtmros_hironx/issues/103)"
01455 + " is not implemented. Patch is welcomed.")
01456 return result
01457
01458 def setTargetPoseRelative(self, gname, eename, dx=0, dy=0, dz=0,
01459 dr=0, dp=0, dw=0, tm=10, wait=True):
01460 '''!@brief
01461 Move the end-effector's location relative to its current pose.
01462
01463 Note that because of "relative" nature, the method waits for the commands
01464 run previously to finish. Do not get confused with the "wait" argument,
01465 which regulates all subsequent commands, not previous ones.
01466
01467 For d*, distance arguments are in meter while rotations are in degree.
01468
01469 Example usage: The following moves RARM_JOINT5 joint 0.1mm forward
01470 within 0.1sec.
01471 \verbatim
01472 robot.setTargetPoseRelative('rarm', 'RARM_JOINT5', dx=0.0001, tm=0.1)
01473 \endverbatim
01474 @param gname str: Name of the joint group that is to be manipulated.
01475 @param eename str: Name of the joint that the manipulated joint group references to.
01476 @param dx float: In meter.
01477 @param dy float: In meter.
01478 @param dz float: In meter.
01479 @param dr float: In radian.
01480 @param dp float: In radian.
01481 @param dw float: In radian.
01482 @param tm float: Second to complete the command. This only includes the time taken for execution
01483 (i.e. time for planning and other misc. processes are not considered).
01484 @param wait bool: If true, all other subsequent commands wait until
01485 the movement commanded by this method call finishes.
01486 @return bool: False if unreachable.
01487 '''
01488 self.waitInterpolationOfGroup(gname)
01489
01490 posRef = None
01491 rpyRef = None
01492 tds = self.getCurrentPose(eename)
01493 if tds:
01494 posRef = numpy.array([tds[3], tds[7], tds[11]])
01495 matRef = numpy.array([tds[0:3], tds[4:7], tds[8:11]])
01496 posRef += [dx, dy, dz]
01497 matRef = matRef.dot(numpy.array(euler_matrix(dr, dp, dw)[:3, :3]))
01498 rpyRef = euler_from_matrix(matRef)
01499 print(posRef, rpyRef)
01500 ret = self.setTargetPose(gname, list(posRef), list(rpyRef), tm)
01501 if ret and wait:
01502 self.waitInterpolationOfGroup(gname)
01503 return ret
01504 return False
01505
01506 def clear(self):
01507 '''!@brief
01508 Clears the Sequencer's current operation. Works for joint groups too.
01509 @see HrpsysConfigurator.clear
01510
01511 Discussed in https://github.com/fkanehiro/hrpsys-base/issues/158
01512 Examples is found in a unit test: https://github.com/start-jsk/rtmros_hironx/blob/bb0672be3e03e5366e03fe50520e215302b8419f/hironx_ros_bridge/test/test_hironx.py#L293
01513 '''
01514 self.seq_svc.clear()
01515
01516 def clearOfGroup(self, gname, tm=0.0):
01517 '''!@brief
01518 Clears the Sequencer's current operation for joint groups.
01519 '''
01520 self.seq_svc.clearOfGroup(gname, tm)
01521
01522 def saveLog(self, fname='sample'):
01523 '''!@brief
01524 Save log to the given file name
01525
01526 @param fname str: name of the file
01527 '''
01528 self.log_svc.save(fname)
01529 print(self.configurator_name + "saved data to " + fname)
01530
01531 def clearLog(self):
01532 '''!@brief
01533 Clear logger's buffer
01534 '''
01535 self.log_svc.clear()
01536
01537 def setMaxLogLength(self, length):
01538 '''!@brief
01539 Set logger's buffer
01540 @param length int: length of log, if the system runs at 500hz and you want to store 2min, set 2*60*500.
01541 '''
01542 self.log_svc.maxLength(length)
01543
01544 def lengthDigitalInput(self):
01545 '''!@brief
01546 Returns the length of digital input port
01547 '''
01548 return self.rh_svc.lengthDigitalInput()
01549
01550 def lengthDigitalOutput(self):
01551 '''!@brief
01552 Returns the length of digital output port
01553 '''
01554 return self.rh_svc.lengthDigitalOutput()
01555
01556 def writeDigitalOutput(self, dout):
01557 '''!@brief
01558 Using writeDigitalOutputWithMask is recommended for the less data
01559 transport.
01560
01561 @param dout list of int: List of bits, length of 32 bits where elements are
01562 0 or 1.
01563
01564 What each element stands for depends on how
01565 the robot's imlemented. Consult the hardware manual.
01566 @return bool: RobotHardware.writeDigitalOutput returns True if writable. False otherwise.
01567 '''
01568 if self.simulation_mode:
01569 return True
01570 doutBitLength = self.lengthDigitalOutput() * 8
01571 if len(dout) < doutBitLength:
01572 for i in range(doutBitLength - len(dout)):
01573 dout.append(0)
01574 outStr = ''
01575 for i in range(0, len(dout), 8):
01576 oneChar = 0
01577 for j in range(8):
01578 if dout[i + j]:
01579 oneChar += 1 << j
01580 outStr = outStr + chr(oneChar)
01581
01582
01583 return self.rh_svc.writeDigitalOutput(outStr)
01584
01585 def writeDigitalOutputWithMask(self, dout, mask):
01586 '''!@brief
01587 Both dout and mask are lists with length of 32. Only the bit in dout
01588 that corresponds to the bits in mask that are flagged as 1 will be
01589 evaluated.
01590
01591 Example:
01592 \verbatim
01593 Case-1. Only 18th bit will be evaluated as 1.
01594 dout [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
01595 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
01596 mask [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
01599 Case-2. Only 18th bit will be evaluated as 0.
01600 dout [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
01601 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
01602 mask [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
01603 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
01604
01605 Case-3. None will be evaluated since there's no flagged bit in mask.
01606 dout [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
01607 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
01608 mask [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
01609 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
01610 \endverbatim
01611 @param dout list of int: List of bits, length of 32 bits where elements are
01612 0 or 1.
01613 @param mask list of int: List of masking bits, length of 32 bits where elements are
01614 0 or 1.
01615 @return bool: RobotHardware.writeDigitalOutput returns True if writable. False otherwise.
01616 '''
01617 if self.simulation_mode:
01618 return True
01619 doutBitLength = self.lengthDigitalOutput() * 8
01620 if len(dout) < doutBitLength and \
01621 len(mask) < doutBitLength and \
01622 len(dout) == len(mask):
01623 for i in range(doutBitLength - len(dout)):
01624 dout.append(0)
01625 mask.append(0)
01626 outStr = ''
01627 outMsk = ''
01628 for i in range(0, len(dout), 8):
01629 oneChar = 0
01630 oneMask = 0
01631 for j in range(8):
01632 if dout[i + j]:
01633 oneChar += 1 << j
01634 if mask[i + j]:
01635 oneMask += 1 << j
01636 outStr = outStr + chr(oneChar)
01637 outMsk = outMsk + chr(oneMask)
01638
01639
01640 return self.rh_svc.writeDigitalOutputWithMask(outStr, outMsk)
01641
01642 def readDigitalInput(self):
01643 '''!@brief
01644 Read data from Digital Input
01645 @see: HrpsysConfigurator.readDigitalInput
01646
01647 Digital input consits of 14 bits. The last 2 bits are lacking
01648 and compensated, so that the last 4 bits are 0x4 instead of 0x1.
01649 @author Hajime Saito (\@emijah)
01650 @return list of int: List of the values (2 octtets) in digital input register. Range: 0 or 1.
01651
01652 #TODO: Catch AttributeError that occurs when RobotHardware not found.
01653 # Typically with simulator, erro msg might look like this;
01654 # 'NoneType' object has no attribute 'readDigitalInput'
01655 '''
01656 if self.simulation_mode:
01657 return []
01658 ret, din = self.rh_svc.readDigitalInput()
01659 retList = []
01660 for item in din:
01661 for i in range(8):
01662 if (ord(item) >> i) & 1:
01663 retList.append(1)
01664 else:
01665 retList.append(0)
01666 return retList
01667
01668 def readDigitalOutput(self):
01669 '''!@brief
01670 Read data from Digital Output
01671
01672 Digital input consits of 14 bits. The last 2 bits are lacking
01673 and compensated, so that the last 4 bits are 0x4 instead of 0x1.
01674
01675 #TODO: Catch AttributeError that occurs when RobotHardware not found.
01676 # Typically with simulator, erro msg might look like this;
01677 # 'NoneType' object has no attribute 'readDigitaloutput'
01678
01679 @author Hajime Saito (\@emijah)
01680 @return list of int: List of the values in digital input register. Range: 0 or 1.
01681 '''
01682 ret, dout = self.rh_svc.readDigitalOutput()
01683 retList = []
01684 for item in dout:
01685 for i in range(8):
01686 if (ord(item) >> i) & 1:
01687 retList.append(1)
01688 else:
01689 retList.append(0)
01690 return retList
01691
01692 def getActualState(self):
01693 '''!@brief
01694 Get actual states of the robot that includes current reference joint angles and joint torques.
01695 @return: This returns actual states of the robot, which is defined in
01696 RobotHardware.idl
01697 (found at https://github.com/fkanehiro/hrpsys-base/blob/3fd7671de5129101a4ade3f98e2eac39fd6b26c0/idl/RobotHardwareService.idl#L32_L57 as of version 315.11.0)
01698 \verbatim
01699 /**
01700 * @brief status of the robot
01701 */
01702 struct RobotState
01703 {
01704 DblSequence angle; ///< current joint angles[rad]
01705 DblSequence command;///< reference joint angles[rad]
01706 DblSequence torque; ///< joint torques[Nm]
01707 /**
01708 * @brief servo statuses(32bit+extra states)
01709 *
01710 * 0: calib status ( 1 => done )\n
01711 * 1: servo status ( 1 => on )\n
01712 * 2: power status ( 1 => supplied )\n
01713 * 3-18: servo alarms (see @ref iob.h)\n
01714 * 19-23: unused
01715 * 24-31: driver temperature (deg)
01716 */
01717 LongSequenceSequence servoState;
01718 sequence<DblSequence6> force; ///< forces[N] and torques[Nm]
01719 sequence<DblSequence3> rateGyro; ///< angular velocities[rad/s]
01720 sequence<DblSequence3> accel; ///< accelerations[m/(s^2)]
01721 double voltage; ///< voltage of power supply[V]
01722 double current; ///< current[A]
01723 };
01724 \endverbatim
01725 '''
01726 return self.rh_svc.getStatus()
01727
01728 def isCalibDone(self):
01729 '''!@brief
01730 Check whether joints have been calibrated.
01731 @return bool: True if all joints are calibrated
01732 '''
01733 if self.simulation_mode:
01734 return True
01735 else:
01736 rstt = self.rh_svc.getStatus()
01737 for item in rstt.servoState:
01738 if not item[0] & 1:
01739 return False
01740 return True
01741
01742 def isServoOn(self, jname='any'):
01743 '''!@brief
01744 Check whether servo control has been turned on.
01745 @param jname str: Name of a link (that can be obtained by "hiro.Groups"
01746 as lists of groups).
01747 When jname = 'all' or 'any' => If all joints are servoOn, return True, otherwise, return False.
01748 When jname = 'some' => If some joint is servoOn, return True, otherwise return False.
01749 @return bool: True if servo is on
01750 '''
01751 if self.simulation_mode:
01752 return True
01753 else:
01754 s_s = self.getActualState().servoState
01755 if jname.lower() == 'any' or jname.lower() == 'all':
01756 for s in s_s:
01757
01758 if (s[0] & 2) == 0:
01759 return False
01760 return True
01761 elif jname.lower() == 'some':
01762 for s in s_s:
01763
01764 if (s[0] & 2) != 0:
01765 return True
01766 return False
01767 else:
01768 jid = eval('self.' + jname)
01769 print(self.configurator_name + s_s[jid])
01770 if s_s[jid][0] & 1 == 0:
01771 return False
01772 else:
01773 return True
01774
01775 def flat2Groups(self, flatList):
01776 '''!@brief
01777 Convert list of angles into list of joint list for each groups.
01778 @param flatList list: single dimension list with its length of 15
01779 @return list of list: 2-dimensional list of Groups.
01780 '''
01781 retList = []
01782 index = 0
01783 for group in self.Groups:
01784 joint_num = len(group[1])
01785 retList.append(flatList[index: index + joint_num])
01786 index += joint_num
01787 return retList
01788
01789 def servoOn(self, jname='all', destroy=1, tm=3):
01790 '''!@brief
01791 Turn on servos.
01792 Joints need to be calibrated (otherwise error returns).
01793
01794 @param jname str: The value 'all' works iteratively for all servos.
01795 @param destroy int: Not used.
01796 @param tm float: Second to complete.
01797 @return int: 1 or -1 indicating success or failure, respectively.
01798 '''
01799
01800 if not self.isCalibDone():
01801 waitInputConfirm('!! Calibrate Encoders with checkEncoders first !!\n\n')
01802 return -1
01803
01804
01805 if self.isServoOn():
01806 return 1
01807
01808
01809 if jname == '':
01810 jname = 'all'
01811
01812 try:
01813 r = waitInputConfirm(\
01814 '!! Robot Motion Warning (SERVO_ON) !!\n\n'
01815 'Confirm RELAY switched ON\n'
01816 'Push [OK] to switch servo ON(%s).' % (jname))
01817 if not r:
01818 print(self.configurator_name, 'servo on: canceled')
01819 return 0
01820 except:
01821 self.rh_svc.power('all', SWITCH_OFF)
01822 raise
01823
01824 try:
01825 self.goActual()
01826 time.sleep(0.1)
01827 self.rh_svc.servo(jname, SWITCH_ON)
01828 time.sleep(2)
01829 if not self.isServoOn(jname):
01830 print(self.configurator_name + 'servo on failed.')
01831 raise
01832 except:
01833 print(self.configurator_name + 'exception occured')
01834
01835 return 1
01836
01837 def servoOff(self, jname='all', wait=True):
01838 '''!@brief
01839 Turn off servos.
01840 @param jname str: The value 'all' works iteratively for all servos.
01841 @param wait bool: Wait for user's confirmation via GUI
01842 @return int: 1 = all arm servo off. 2 = all servo on arms and hands off.
01843 -1 = Something wrong happened.
01844 '''
01845
01846 if self.simulation_mode:
01847 print(self.configurator_name + 'omit servo off')
01848 return 0
01849
01850 print('Turn off Hand Servo')
01851 if self.sc_svc:
01852 self.sc_svc.servoOff()
01853
01854 if not self.isServoOn(jname):
01855 if jname.lower() == 'all':
01856 self.rh_svc.power('all', SWITCH_OFF)
01857 return 1
01858
01859
01860 if jname == '':
01861 jname = 'all'
01862
01863 if wait:
01864 r = waitInputConfirm(
01865 '!! Robot Motion Warning (Servo OFF)!!\n\n'
01866 'Push [OK] to servo OFF (%s).' % (jname))
01867 if not r:
01868 print(self.configurator_name, 'servo off: canceled')
01869 return 2
01870 try:
01871 self.rh_svc.servo('all', SWITCH_OFF)
01872 time.sleep(0.2)
01873 if jname == 'all':
01874 self.rh_svc.power('all', SWITCH_OFF)
01875
01876
01877 print('Turn off Hand Servo')
01878 if self.sc_svc:
01879 self.sc_svc.servoOff()
01880
01881 return 2
01882 except:
01883 print(self.configurator_name + 'servo off: communication error')
01884 return -1
01885
01886 def checkEncoders(self, jname='all', option=''):
01887 '''!@brief
01888 Run the encoder checking sequence for specified joints,
01889 run goActual and turn on servos.
01890
01891 @param jname str: The value 'all' works iteratively for all servos.
01892 @param option str: Possible values are follows (w/o double quote):\
01893 "-overwrite": Overwrite calibration value.
01894 '''
01895 if self.isServoOn():
01896 waitInputConfirm('Servo must be off for calibration')
01897 return
01898
01899 elif self.isCalibDone():
01900 waitInputConfirm('System has been calibrated')
01901 return
01902
01903 self.rh_svc.power('all', SWITCH_ON)
01904 msg = '!! Robot Motion Warning !!\n'\
01905 'Turn Relay ON.\n'\
01906 'Then Push [OK] to '
01907 if option == '-overwrite':
01908 msg = msg + 'calibrate(OVERWRITE MODE) '
01909 else:
01910 msg = msg + 'check '
01911
01912 if jname == 'all':
01913 msg = msg + 'the Encoders of all.'
01914 else:
01915 msg = msg + 'the Encoder of the Joint "' + jname + '".'
01916
01917 try:
01918 waitInputConfirm(msg)
01919 except:
01920 print("If you're connecting to the robot from remote, " + \
01921 "make sure tunnel X (eg. -X option with ssh).")
01922 self.rh_svc.power('all', SWITCH_OFF)
01923 return 0
01924
01925 print(self.configurator_name + 'calib-joint ' + jname + ' ' + option)
01926 self.rh_svc.initializeJointAngle(jname, option)
01927 print(self.configurator_name + 'done')
01928 self.rh_svc.power('all', SWITCH_OFF)
01929 self.goActual()
01930 time.sleep(0.1)
01931 self.rh_svc.servo(jname, SWITCH_ON)
01932
01933
01934 print('Turn on Hand Servo')
01935 if self.sc_svc:
01936 self.sc_svc.servoOn()
01937
01938 def removeForceSensorOffset(self):
01939 '''!@brief
01940 remove force sensor offset
01941 '''
01942 self.rh_svc.removeForceSensorOffset()
01943
01944 def playPattern(self, jointangles, rpy, zmp, tm):
01945 '''!@brief
01946 Play motion pattern using a given trajectory that is represented by
01947 a list of joint angles, rpy, zmp and time.
01948
01949 @type jointangles: [[float]]
01950 @param jointangles: Sequence of the sets of joint angles in radian.
01951 The whole list represents a trajectory. Each element
01952 of the 1st degree in the list consists of the joint angles.
01953 @param rpy list of float: Orientation in rpy.
01954 @param zmp list of float: TODO: description
01955 @param tm float: Second to complete the command. This only includes the time taken for execution
01956 (i.e. time for planning and other misc. processes are not considered).
01957 @return bool:
01958 '''
01959 return self.seq_svc.playPattern(jointangles, rpy, zmp, tm)
01960
01961 def playPatternOfGroup(self, gname, jointangles, tm):
01962 '''!@brief
01963 Play motion pattern using a set of given trajectories that are represented by
01964 lists of joint angles. Each trajectory aims to run within the specified time (tm),
01965 and there's no slow down between trajectories unless the next one is the last.
01966
01967 Example:
01968 self.playPatternOfGroup('larm',
01969 [[0.0, 0.0, -2.2689280275926285, 0.0, 0.0, 0.0],
01970 [0.0, 0.0, -1.9198621771937625, 0.0, 0.0, 0.0],
01971 [0.0, 0.0, -1.5707963, 0.0, 0.0, 0.0]],
01972 [3, 3, 10])
01973
01974 @param gname str: Name of the joint group.
01975 @type jointangles: [[float]]
01976 @param jointangles: Sequence of the sets of joint angles in radian.
01977 The whole list represents a trajectory. Each element
01978 of the 1st degree in the list consists of the joint
01979 angles. To illustrate:
01980
01981 [[a0-0, a0-1,...,a0-n], # a)ngle. 1st path in trajectory
01982 [a1-0, a1-1,...,a1-n], # 2nd path in the trajectory.
01983 :
01984 [am-0, am-1,...,am-n]] # mth path in the trajectory
01985 @type tm: [float]
01986 @param tm float: Sequence of the time values to complete the task,
01987 each element of which corresponds to a set of jointangles
01988 in the same order.
01989 @return bool:
01990 '''
01991 return self.seq_svc.playPatternOfGroup(gname, jointangles, tm)
01992
01993 def setSensorCalibrationJointAngles(self):
01994 '''!@brief
01995 Set joint angles for sensor calibration.
01996 Please override joint angles to avoid self collision.
01997 '''
01998 self.setJointAngles([0]*len(self.rh_svc.getStatus().servoState), 4.0)
01999 self.waitInterpolation()
02000
02001 def calibrateInertiaSensor(self):
02002 '''!@brief
02003 Calibrate inertia sensor
02004 '''
02005 self.rh_svc.calibrateInertiaSensor()
02006
02007 def calibrateInertiaSensorWithDialog(self):
02008 '''!@brief
02009 Calibrate inertia sensor with dialog and setting calibration pose
02010 '''
02011 r = waitInputConfirm (
02012 '!! Robot Motion Warning (Move to Sensor Calibration Pose)!!\n\n'
02013 'Push [OK] to move to sensor calibration pose.')
02014 if not r: return False
02015 self.setSensorCalibrationJointAngles()
02016 r = waitInputConfirm (
02017 '!! Put the robot down!!\n\n'
02018 'Push [OK] to the next step.')
02019 if not r: return False
02020 self.calibrateInertiaSensor()
02021 r = waitInputConfirm (
02022 '!! Put the robot up!!\n\n'
02023 'Push [OK] to the next step.')
02024 if not r: return False
02025 return True
02026
02027
02028
02029
02030 def startAutoBalancer(self, limbs=None):
02031 '''!@brief
02032 Start AutoBalancer mode
02033 @param limbs list of end-effector name to control.
02034 If Groups has rarm and larm, rleg, lleg, rarm, larm by default.
02035 If Groups is not defined or Groups does not have rarm and larm, rleg and lleg by default.
02036 '''
02037 if limbs==None:
02038 if self.Groups != None and "rarm" in map (lambda x : x[0], self.Groups) and "larm" in map (lambda x : x[0], self.Groups):
02039 limbs=["rleg", "lleg", "rarm", "larm"]
02040 else:
02041 limbs=["rleg", "lleg"]
02042 self.abc_svc.startAutoBalancer(limbs)
02043
02044 def stopAutoBalancer(self):
02045 '''!@brief
02046 Stop AutoBalancer mode
02047 '''
02048 self.abc_svc.stopAutoBalancer()
02049
02050 def startStabilizer(self):
02051 '''!@brief
02052 Start Stabilzier mode
02053 '''
02054 self.st_svc.startStabilizer()
02055
02056 def stopStabilizer(self):
02057 '''!@brief
02058 Stop Stabilzier mode
02059 '''
02060 self.st_svc.stopStabilizer()
02061
02062 def startImpedance_315_4(self, arm,
02063 M_p = 100.0,
02064 D_p = 100.0,
02065 K_p = 100.0,
02066 M_r = 100.0,
02067 D_r = 2000.0,
02068 K_r = 2000.0,
02069 force_gain = [1, 1, 1],
02070 moment_gain = [0, 0, 0],
02071 sr_gain = 1.0,
02072 avoid_gain = 0.0,
02073 reference_gain = 0.0,
02074 manipulability_limit = 0.1,
02075 controller_mode = None,
02076 ik_optional_weight_vector = None):
02077 '''!@brief
02078 start impedance mode
02079
02080 @type arm: str name of artm to be controlled, this must be initialized using setSelfGroups()
02081 @param force_gain, moment_gain: multipliers to the eef offset position vel_p and orientation vel_r.
02082 3-dimensional vector (then converted internally into a diagonal matrix).
02083 '''
02084 r, p = self.ic_svc.getImpedanceControllerParam(arm)
02085 if not r:
02086 print('{}, Failt to getImpedanceControllerParam({})'.format(self.configurator_name, arm))
02087 return False
02088 if M_p != None: p.M_p = M_p
02089 if D_p != None: p.M_p = D_p
02090 if K_p != None: p.M_p = K_p
02091 if M_r != None: p.M_r = M_r
02092 if D_r != None: p.M_r = D_r
02093 if K_r != None: p.M_r = K_r
02094 if force_gain != None: p.force_gain = force_gain
02095 if moment_gain != None: p.moment_gain = moment_gain
02096 if sr_gain != None: p.sr_gain = sr_gain
02097 if avoid_gain != None: p.avoid_gain = avoid_gain
02098 if reference_gain != None: p.reference_gain = reference_gain
02099 if manipulability_limit != None: p.manipulability_limit = manipulability_limit
02100 if controller_mode != None: p.controller_mode = controller_mode
02101 if ik_optional_weight_vector != None: p.ik_optional_weight_vector = ik_optional_weight_vector
02102 self.ic_svc.setImpedanceControllerParam(arm, p)
02103 return self.ic_svc.startImpedanceController(arm)
02104
02105 def stopImpedance_315_4(self, arm):
02106 '''!@brief
02107 stop impedance mode
02108 '''
02109 return self.ic_svc.stopImpedanceController(arm)
02110
02111 def startImpedance(self, arm, **kwargs):
02112 '''!@brief
02113 Enable the ImpedanceController RT component.
02114 This method internally calls startImpedance-*, hrpsys version-specific method.
02115
02116 @requires: hrpsys version greather than 315.2.0.
02117 @requires: ImpedanceController RTC to be activated on the robot's controller.
02118 @change: From 315.2.0 onward, following arguments are dropped and can be set by
02119 self.seq_svc.setWrenches instead of this method.
02120 See an example at https://github.com/fkanehiro/hrpsys-base/pull/434/files#diff-6204f002204dd9ae80f203901f155fa9R44:
02121
02122 - 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:
02123
02124 self.seq_svc.setWrenches([0, 0, 0, 0, 0, 0,
02125 fx, fy, fz, tx, ty, tz,
02126 0, 0, 0, 0, 0, 0,
02127 0, 0, 0, 0, 0, 0,])
02128
02129 setWrenches takes 6 values per sensor, so the robot in the example above has 4 sensors where each line represents a sensor.
02130 See this link (https://github.com/fkanehiro/hrpsys-base/pull/434/files) for a concrete example.
02131
02132 @param arm: Name of the kinematic group (i.e. self.Groups[n][0]).
02133 @param kwargs: This varies depending on the version of hrpsys your robot's controller runs on
02134 (which you can find by "self.hrpsys_version" command). For instance, if your
02135 hrpsys is 315.10.1, refer to "startImpedance_315_4" method.
02136 '''
02137 if self.hrpsys_version and StrictVersion(self.hrpsys_version) < StrictVersion('315.2.0'):
02138 print(self.configurator_name + '\033[31mstartImpedance: Try to connect unsupported RTC' + str(self.hrpsys_version) + '\033[0m')
02139 else:
02140 self.startImpedance_315_4(arm, **kwargs)
02141
02142 def stopImpedance(self, arm):
02143 if self.hrpsys_version and StrictVersion(self.hrpsys_version) < StrictVersion('315.2.0'):
02144 print(self.configurator_name + '\033[31mstopImpedance: Try to connect unsupported RTC' + str(self.hrpsys_version) + '\033[0m')
02145 else:
02146 self.stopImpedance_315_4(arm)
02147
02148 def startDefaultUnstableControllers (self, ic_limbs=[], abc_limbs=[]):
02149 '''!@brief
02150 Start default unstable RTCs controller mode.
02151 Currently Stabilzier, AutoBalancer, and ImpedanceController are started based on "Groups" setting.
02152 If ic_limbs or abc_limbs is not specified, default setting is set according to "Groups".
02153 By default,
02154 If the robot has an arm, start impedance for the arm.
02155 If the robot has a leg, start st and autobalancer.
02156 autobalancer's fixed limbs are all existing arms and legs.
02157 Use cases:
02158 Biped robot (leg only) : no impedance, start st, start autobalancer with ["rleg", "lleg"].
02159 Dual-arm robot (arm only) : start impedance ["rarm", "larm"], no st, no autobalancer.
02160 Dual-arm+biped robot (=humanoid robot) : start impedance ["rarm", "larm"], start st, start autobalancer with ["rleg", "lleg", "rarm", "larm"].
02161 Quadruped robot : same as "humanoid robot" by default.
02162 '''
02163 print(self.configurator_name + "Start Default Unstable Controllers")
02164
02165 is_legged_robot = map(lambda x: x[0], filter(lambda x : re.match(".*leg", x[0]), self.Groups))
02166
02167 if not ic_limbs:
02168 ic_limbs = map(lambda x: x[0], filter(lambda x : re.match(".*(arm)", x[0]), self.Groups))
02169
02170 if not abc_limbs:
02171 abc_limbs = map(lambda x: x[0], filter(lambda x : re.match(".*(leg|arm)", x[0]), self.Groups))
02172
02173 for limb in ic_limbs:
02174 self.ic_svc.startImpedanceControllerNoWait(limb)
02175 if is_legged_robot:
02176 self.startAutoBalancer(abc_limbs)
02177 self.startStabilizer()
02178 for limb in ic_limbs:
02179 self.ic_svc.waitImpedanceControllerTransition(limb)
02180
02181 if is_legged_robot:
02182 print(self.configurator_name + " Start AutoBalancer = "+str(abc_limbs))
02183 print(self.configurator_name + " Start Stabilizer")
02184 if len(ic_limbs) != 0:
02185 print(self.configurator_name + " Start ImpedanceController = "+str(ic_limbs))
02186
02187 def stopDefaultUnstableControllers (self, ic_limbs=[]):
02188 '''!@brief
02189 Stop default unstable RTCs controller mode.
02190 Currently Stabilzier, AutoBalancer, and ImpedanceController are stopped based on "Groups" setting.
02191 Please see documentation of startDefaultUnstableControllers().
02192 '''
02193 print(self.configurator_name + "Stop Default Unstable Controllers")
02194
02195 is_legged_robot = map(lambda x: x[0], filter(lambda x : re.match(".*leg", x[0]), self.Groups))
02196
02197 if not ic_limbs:
02198 ic_limbs = map(lambda x: x[0], filter(lambda x : re.match(".*(arm)", x[0]), self.Groups))
02199
02200 if is_legged_robot:
02201 self.stopStabilizer()
02202 for limb in ic_limbs:
02203 self.ic_svc.stopImpedanceControllerNoWait(limb)
02204 if is_legged_robot:
02205 self.stopAutoBalancer()
02206 for limb in ic_limbs:
02207 self.ic_svc.waitImpedanceControllerTransition(limb)
02208
02209 if is_legged_robot:
02210 print(self.configurator_name + " Stop AutoBalancer")
02211 print(self.configurator_name + " Stop Stabilizer")
02212 if len(ic_limbs) != 0:
02213 print(self.configurator_name + " Stop ImpedanceController = "+str(ic_limbs))
02214
02215 def setFootSteps(self, footstep, overwrite_fs_idx = 0):
02216 '''!@brief
02217 setFootSteps
02218 @param footstep : FootstepSequence.
02219 @param overwrite_fs_idx : Index to be overwritten. overwrite_fs_idx is used only in walking.
02220 '''
02221 self.abc_svc.setFootSteps(footstep, overwrite_fs_idx)
02222
02223 def setFootStepsWithParam(self, footstep, stepparams, overwrite_fs_idx = 0):
02224 '''!@brief
02225 setFootSteps
02226 @param footstep : FootstepSequence.
02227 @param stepparams : StepParamSeuqnce.
02228 @param overwrite_fs_idx : Index to be overwritten. overwrite_fs_idx is used only in walking.
02229 '''
02230 self.abc_svc.setFootStepsWithParam(footstep, stepparams, overwrite_fs_idx)
02231
02232 def clearJointAngles(self):
02233 '''!@brief
02234 Cancels the commanded sequence of joint angle, by overwriting the command by the values of instant the method was invoked.
02235 Note that this only cancels the queue once. Icoming commands after this method is called will be processed as usual.
02236 @return bool
02237 '''
02238 return self.seq_svc.clearJointAngles()
02239
02240 def clearJointAnglesOfGroup(self, gname):
02241 '''!@brief
02242 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.
02243 Note that this only cancels the queue once. Icoming commands after this method is called will be processed as usual.
02244 @param gname: Name of the joint group.
02245 @return bool
02246 '''
02247 return self.seq_svc.clearJointAngles(gname)
02248
02249 def removeForceSensorOffsetRMFO(self, sensor_names=[], tm=8.0):
02250 '''!@brief
02251 remove force sensor offset by RemoveForceSensorOffset (RMFO) RTC.
02252 @param sensor_names : list of sensor names to be calibrated. If not specified, all sensors are calibrated by default.
02253 @param tm : calibration time[s]. 8.0[s] by default.
02254 @return bool : true if set successfully, false otherwise
02255 '''
02256 return self.rmfo_svc.removeForceSensorOffset(sensor_names, tm)
02257
02258
02259
02260
02261
02262 def init(self, robotname="Robot", url=""):
02263 '''!@brief
02264 Calls init from its superclass, which tries to connect RTCManager,
02265 looks for ModelLoader, and starts necessary RTC components. Also runs
02266 config, logger.
02267 Also internally calls setSelfGroups().
02268
02269 @type robotname: str
02270 @type url: str
02271 '''
02272 print(self.configurator_name + "waiting ModelLoader")
02273 self.waitForModelLoader()
02274 print(self.configurator_name + "start hrpsys")
02275
02276 print(self.configurator_name + "finding RTCManager and RobotHardware")
02277 self.waitForRTCManagerAndRobotHardware(robotname)
02278 self.sensors = self.getSensors(url)
02279
02280 print(self.configurator_name + "creating components")
02281 self.createComps()
02282
02283 print(self.configurator_name + "connecting components")
02284 self.connectComps()
02285
02286 print(self.configurator_name + "activating components")
02287 self.activateComps()
02288
02289 print(self.configurator_name + "setup logger")
02290 self.setupLogger()
02291
02292 print(self.configurator_name + "setup joint groups")
02293 self.setSelfGroups()
02294
02295 print(self.configurator_name + '\033[32minitialized successfully\033[0m')
02296
02297
02298 try:
02299 self.hrpsys_version = self.fk.ref.get_component_profile().version
02300 except:
02301 print(self.configurator_name + '\033[34mCould not get hrpsys_version\033[0m')
02302
02303 pass
02304
02305 def __init__(self, cname="[hrpsys.py] "):
02306 initCORBA()
02307 self.configurator_name = cname
02308
02309
02310 if __name__ == '__main__':
02311 hcf = HrpsysConfigurator()
02312 if len(sys.argv) > 2:
02313 hcf.init(sys.argv[1], sys.argv[2])
02314 elif len(sys.argv) > 1:
02315 hcf.init(sys.argv[1])
02316 else:
02317 hcf.init()