-
Notifications
You must be signed in to change notification settings - Fork 0
/
motorJoint.py
69 lines (58 loc) · 1.7 KB
/
motorJoint.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
#!/usr/bin/env python
from numpy import dot, sqrt
class MotorJoint:
def __init__(self, robot, ohName, mName, gearRatio, torqueConstant, noLoadCurrent, resistance, vel = .3):
self.physics = robot.GetEnv().GetPhysicsEngine()
self.openHuboName = ohName
self.maestroName = mName
self.joint = robot.GetJoint(self.openHuboName)
self.interpolation = True
self.currentPos = 0
self.goalPos = 0
# Motor parameters
self.G = gearRatio
self.Kt = torqueConstant
self.R = resistance
self.nlc = noLoadCurrent
self.interpolationVel = vel
# Logs
self.power = 0.0
self.currentLog = []
self.torqueLog = []
self.velocityLog = []
self.positionLog = []
def getTorque(self):
[force, torque] = self.physics.GetJointForceTorque(self.joint)
#return dot(torque, self.joint.GetAxis())/self.G
return torque/self.G
def getVelocity(self):
v = self.joint.GetVelocities()[0]*self.G
self.velocityLog.append(v)
return v
def getCurrent(self, t):
t = sqrt(t[0]**2+t[1]**2+t[2]**2)
self.torqueLog.append(abs(t))
return t/self.Kt
def getVoltage(self, i):
return abs(self.getVelocity())*self.Kt + (i+self.nlc)*self.R
def setInterpolationVelocity(self, v):
if v > 0:
self.interpolationVel = v
def getPower(self, step, t):
i = self.getCurrent(t)
p = i*self.getVoltage(i)*step/3600.0
self.power += p
self.currentLog.append(p/54.0)
self.positionLog.append(self.joint.GetValue(0))
return p
def nextPos(self, step):
if interpolation:
posStep = step*self.interpolationVel
if(abs(self.goalPos - self.currentPos) <posStep):
return goalPos
elif self.goalPos < self.currentPos:
return self.currentPos - posStep
else
return self.currentPos + posStep
else:
return goalPos