def setPose(self, *pose):
if self.currPose == pose:
return
else:
if self.kind == 'SA':
jList = ['S', 'L', 'U', 'R', 'B', 'T', 'F']
else:
jList = ['S', 'L', 'U', 'R', 'B', 'T', 'F']
start = getFirstNonZeroIdx(pose, self.currPose)
print('start: ', start)
Tw = np.diag(np.ones(4, dtype=np.double))
if start != 0:
Tw = self.linkFrames[jList[start - 1]].getTWorld()
for i in range(start, len(self.DHparams)):
if self.kind == 'SA':
Tw = np.matmul(
Tw, DH.getTCraig2(pose[i], **self.DHparams[jList[i]]))
else:
Tw = np.matmul(
Tw, DH.getTSaha2(pose[i], **self.DHparams[jList[i]]))
self.nrk.DeleteObjects([self.linkFrames[jList[i]]])
self.nrk.ConstructFrame(self.col, jList[i], Tw)
self.linkFrames[jList[i]].setTWorld(Tw)
self.currPose = pose
pass
def genRobot(self):
if self.kind == 'SA':
jList = ['S', 'L', 'U', 'R', 'B', 'T', 'F']
else:
jList = ['S', 'L', 'U', 'R', 'B', 'T', 'F']
Tw = np.diag(np.ones(4, dtype=np.double))
params = {}
params['col'] = self.col
for j in range(len(self.DHparams)):
if self.kind == 'SA':
Tcurr = DH.getTCraig2(self.homePose[j],
**self.DHparams[jList[j]])
else:
Tcurr = DH.getTSaha2(self.homePose[j],
**self.DHparams[jList[j]])
Tw = np.matmul(Tw, Tcurr)
params['name'] = jList[j]
params['TWorld'] = Tw
self.linkFrames[jList[j]] = frame.frame(**params)
self.nrk.ConstructFrame(self.col, jList[j], Tcurr)
self.nrk.SetWorkingFrame(self.col, jList[j])
self.nrk.SetWorkingFrame("A", "World")
self.currPose = self.homePose
