/
powerTest.py
executable file
·332 lines (283 loc) · 10.5 KB
/
powerTest.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
#!/usr/bin/env python
from openravepy import *
from openhubo import *
import hubo_ach
import sys
import time
import numpy
import cmd
import readline, glob
COMMANDS = ['runTrajectory', 'getPosition', 'setPosition', 'stepSimulation', 'getVelocity', 'setVelocity']
from powerModel import * # Import for power computing and joint properties
# Hubo-ach status Logger to track computer and sim time and rate
class StatusLogger:
"""Simple and efficient status updater for the main loop"""
def __init__(self,skipcount=100,init_time=0.0):
self.t_last=init_time
self.skip=skipcount
self.count=0
self.rate=0.0
def tick(self):
self.count+=1
if self.count>=self.skip:
self.show()
def show(self):
ideal_time = hubo_ach.HUBO_LOOP_PERIOD*self.count
t=time.time()
actual_time = t-self.t_last
log('Sim time: {:.3f}, Actual time: {:.3f}, RT rate: {:.3f}% T= {:.6f}'.format(ideal_time,actual_time,ideal_time/actual_time*100,TIMESTEP))
self.t_last=t
self.count=0
def zero(self):
self.t_last = time.time()
self.count = 0
def log(string,level=4):
raveLog(string,level)
def powerLog(usage):
[jointUsage, idle, jointCurrent, jointVelocity, jointTorque, jointPosition] = power.zero()
f = open('currentLog.txt', 'w')
g = open('torqueLog.txt', 'w')
h = open('velocityLog.txt', 'w')
z = open('positionLog.txt', 'w')
# Write joint headers
joints = []
for joint in jointCurrent:
f.write(joint + " ")
g.write(joint + " ")
h.write(joint + " ")
z.write(joint + " ")
joints.append(joint)
f.write("\n")
g.write("\n")
h.write("\n")
z.write("\n")
for i in xrange(len(jointCurrent["RSR"])):
for joint in joints:
f.write(str(jointCurrent[joint][i]) + " ")
g.write(str(jointTorque[joint][i]) + " ")
h.write(str(jointVelocity[joint][i]) + " ")
z.write(str(jointPosition[joint][i]) + " ")
f.write("\n")
g.write("\n")
h.write("\n")
z.write("\n")
print "Power used: %.6fWh" % usage
print "Idle Usage: %.6fWh" % idle
for joint in jointUsage:
print joint + ": %.6fWh" % jointUsage[joint]
def runTrajectory(fileName):
# Check to see if file exists and is a trajectory file
try:
f = open(fileName, 'r')
except IOError:
print "File does not exist"
return
if(not(fileName[len(fileName) - 5:] == ".traj")):
print("Not a valid trajectory file")
return
print("\nLoadingTrajectory")
lines = f.readlines()
trajJointNames = lines[0].split()
jointNames = []
positions = dict()
# Check if all joints are valid
for jointName in trajJointNames:
fileJointName = jointName
jointName = power.getName(jointName)
if jointName is None:
print 'Warning: Joint %s not recognized or cannot be set' % fileJointName
jointNames.append(jointName)
positions[jointName] = []
# Load joint positions into arrays
del(lines[0])
i = 0
for line in lines:
vals = line.split()
j = 0
for jointName in jointNames:
positions[jointName].append(float(vals[j]))
j += 1
i += 1
print("Trajectory file loaded")
print("\nStarting trajectory")
usage = 0.0
N = int(numpy.ceil(hubo_ach.HUBO_LOOP_PERIOD/TIMESTEP)) # Simulation runs faster than hubo-ach
statusLogger.zero() # Restart timer
for i in xrange(len(lines)):
posei = Pose(robot, ctrl)
pose = posei
# Approximate positions to send in between trajectory values
for j in xrange(1, N+1):
for jointName in jointNames:
if jointName is not None:
pose[jointName] = float(j)/N*(positions[jointName][i]-posei[jointName]) + posei[jointName]
pose.send()
env.StepSimulation(TIMESTEP)
power.addTorques()
statusLogger.tick()
usage += power.calcPowerUsage(TIMESTEP*N)
powerLog(usage)
# Continues the simulation without moving the robot
def stepSimulation(sleepTime):
try:
t = float(sleepTime)
except:
print("Argument of sleep not understood")
return
pose = Pose(robot, ctrl)##
usage = 0.0
N = int(numpy.ceil(hubo_ach.HUBO_LOOP_PERIOD/TIMESTEP))
statusLogger.zero() # Restart timer
for i in xrange(int(t/TIMESTEP/N)):
for j in xrange(N):
env.StepSimulation(TIMESTEP)
power.addTorques()
statusLogger.tick()
usage += power.calcPowerUsage(TIMESTEP*N)
powerLog(usage)
def getVelocity(jointName):
jointName = power.getName(jointName)
if jointName is not None:
print jointName + " vel = " +"%.4f" % power.getMotor(jointName).interpolationVel
else:
print "Invalid joint name"
def setVelocity(jointName, vel):
jointName = power.getName(jointName)
if jointName is not None:
try:
vel = float(vel)
power.getMotor(jointName).setInterpolationVelocity(vel)
except:
print "Argument of setVelocity not understood"
else:
print "Invalid joint name"
def getPosition(jointName):
# Check to see if jointName is valid
jointName = power.getName(jointName)
if jointName is not None:
pose = Pose(robot, ctrl)
print(jointName + " pos = " + "%.4f" % pose[jointName])
else:
print "Invalid joint name"
def setPosition(jointName, position):
# Check to see if jointName is valid
jointName = power.getName(jointName)
if(jointName is not None):
try:
# Check to see if position is a decimal number
position = float(position)
except:
print "Argument of setPosition not understood"
return
pose = Pose(robot, ctrl)
jointPos = pose[jointName]
step = power.getMotor(jointName).interpolationVel*TIMESTEP # Change in osition per timestep
usage = 0
count = 0
N = int(numpy.ceil(hubo_ach.HUBO_LOOP_PERIOD/TIMESTEP))
statusLogger.zero() # Restart timer
if(jointPos > position):
step *= -1
jointPos += step
# While joint is not near desired position yet
while(True):
for i in xrange(N):
if abs(jointPos - position) >= abs(step):
pose[jointName] = jointPos
pose.send()
env.StepSimulation(TIMESTEP)
power.addTorques()
jointPos += step
else:
break
else: # So outer loop continues
usage += power.calcPowerUsage(TIMESTEP*N)
statusLogger.tick()
continue
break # So inner loop breaks out of outer loop
# Joint is near desired position, set position to desired position
pose[jointName] = position
env.StepSimulation(TIMESTEP)
powerLog(usage)
else:
print "Invalid joint name"
# Tab completion for trajectory file paths and commands
def complete(text, state):
for cmd in COMMANDS:
if cmd.startswith(text):
if not state:
return cmd
else:
state -=1
return (glob.glob(text+'*')+[None])[state]
if __name__ == '__main__':
# Initialize tab completion
readline.set_completer_delims(' \t\n;')
readline.parse_and_bind("tab: complete")
readline.set_completer(complete)
# Setup simulation options
[env, options] = setup(None, True)
env.SetDebugLevel(4)
time.sleep(.25)
options.physics = True
options.stop = True
options.ghost = True
#options.robotfile = 'rlhuboplus.fingerless.robot.xml'
# Start OpenHubo
[robot, ctrl, ind, ghost, recorder] = load_scene(env, options.robotfile, options.scenefile, options.stop, options.physics, options.ghost)
env.StopSimulation()
env.StepSimulation(.0005)
TIMESTEP = .001
statusLogger = StatusLogger(100, time.time())
# Initialize power model, motors
power = PowerModel(robot, 5)
print "\nMaestro OpenHubo script for modelling power usage"
print "\tCommands: runTrajectory stepSimulation getPosition setPosition getVelocity setVelocity\n"
# Continuously ask for command
print "Enter command or press Ctrl+C to exit"
while(True):
try:
userInput = raw_input("> ")
words = userInput.split()
if(len(words) == 0):
continue
if(words[0] == "runTrajectory"):
if(len(words) == 2):
runTrajectory(words[1])
continue
else:
print "Runs given trajectory file and calculates power use\nUsage: runTrajectory FILEPATH"
continue
if(words[0] == "stepSimulation"):
if(len(words) == 2):
stepSimulation(words[1])
continue
else:
print "Runs the simulation without moving the robot and calculates power use\nUsage: stepSimulation TIME"
continue
if(words[0] == "getPosition"):
if(len(words) == 2):
getPosition(words[1])
continue
else:
print "Returns the position of a joint\nUsage: getPosition JOINTNAME"
continue
if(words[0] == "setPosition"):
if(len(words) == 3):
setPosition(words[1], words[2])
else:
print "Moves joint to specified position\nUsage: setPosition JOINTNAME POSITION"
if(words[0] == "getVelocity"):
if(len(words) == 2):
getVelocity(words[1])
else:
print "Returns interpolation velocity of a joint\nUsage: getVelocity JOINTNAME"
if(words[0] == "setVelocity"):
if(len(words) == 3):
setVelocity(words[1], words[2])
else:
print "Sets interpolation velocity of a joint\nUsage: setVelocity JOINTNAME VELOCITY"
# Exit
except KeyboardInterrupt:
print("\nExiting")
sys.exit()