history.py
Go to the documentation of this file.
1 # This class is used to record the data during an execution and restore it at a
2 # given time isntant.
3 
4 from dynamic_graph.sot.core.matrix_util import vectorToTuple
5 from numpy import matrix, pi
6 
7 
8 class History:
9  def __init__(self, dynEnt, freq=100, zmpSig=None):
10  self.q = list()
11  self.qdot = list()
12  self.zmp = list()
13  self.freq = freq
14  self.zmpSig = zmpSig
15  self.dynEnt = dynEnt
16  self.withZmp = (self.zmpSig is not None) and (
17  "waist" in map(lambda x: x.name, self.dynEnt.signals())
18  )
19 
20  def record(self):
21  i = self.dynEnt.position.time
22  if i % self.freq == 0:
23  self.q.append(self.dynEnt.position.value)
24  self.qdot.append(self.dynEnt.velocity.value)
25  if self.withZmp:
26  waMwo = matrix(self.dynEnt.waist.value).I
27  wo_z = matrix(self.zmpSig.value + (1,)).T
28  self.zmp.append(list(vectorToTuple(waMwo * wo_z)))
29 
30  def restore(self, t):
31  t = int(t / self.freq)
32  print("robot.set(", self.q[t], ")")
33  print("robot.setVelocity(", self.qdot[t], ")")
34  print("T0 = ", t)
35  print("robot.state.time = T0")
36  print("[ t.feature.position.recompute(T0) for t in refreshTaskList]")
37  print("attime.fastForward(T0)")
38 
39  def dumpToOpenHRP(self, baseName="dyninv", sample=1):
40  filePos = open(baseName + ".pos", "w")
41  fileRPY = open(baseName + ".hip", "w")
42  fileWaist = open(baseName + ".waist", "w")
43  sampleT = 0.005
44  for nT, q in enumerate(self.q):
45  fileRPY.write(
46  str(sampleT * nT)
47  + " "
48  + str(q[3])
49  + " "
50  + str(q[4])
51  + " "
52  + str(q[5])
53  + "\n"
54  )
55  fileWaist.write(
56  str(sampleT * nT)
57  + " "
58  + str(q[0])
59  + " "
60  + str(q[1])
61  + " "
62  + str(q[2])
63  + " "
64  + str(q[3])
65  + " "
66  + str(q[4])
67  + " "
68  + str(q[5])
69  + "\n"
70  )
71  filePos.write(str(sampleT * nT) + " ")
72  for j in range(6, 36):
73  filePos.write(str(q[j]) + " ")
74  filePos.write(10 * " 0" + "\n")
75  if self.withZmp:
76  fileZMP = open(baseName + ".zmp", "w")
77  for nT, z in enumerate(self.zmp):
78  fileZMP.write(
79  str(sampleT * nT)
80  + " "
81  + str(z[0])
82  + " "
83  + str(z[1])
84  + " "
85  + str(z[2])
86  + "\n"
87  )
88 
89  filePos0 = open(baseName + "_pos0.py", "w")
90  filePos0.write("dyninv_posinit = '")
91  q0 = self.q[0]
92  for x in q0[6:36]:
93  filePos0.write(str(x * 180.0 / pi) + " ")
94  filePos0.write(" 0 0 0 0 0 0 0 0 0 0 '")
def __init__(self, dynEnt, freq=100, zmpSig=None)
Definition: history.py:9
def dumpToOpenHRP(self, baseName="dyninv", sample=1)
Definition: history.py:39


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Wed Jun 21 2023 02:51:26