return self.solved()
def continousSolve(self,itrs=1000,auto=False,extra=[]):
#Enable TSR sampling
self.sample_bw=True
for k in range(itrs):
if not(self.robot.GetEnv().CheckCollision(self.robot)) and not(self.robot.CheckSelfCollision()) and self.solved():
print "Valid solution found!"
#TODO: log solutuon + affine DOF
#rezero to prevent getting stuck...at major speed penalty
self.robot.SetDOFValues(self.zero)
self.run(auto,extra)
time.sleep(.1)
return self.solved()
def resetZero(self):
self.zero=self.robot.GetDOFValues()
if __name__ == '__main__':
juiceTSR = TSR()
juiceTSR.Tw_e = MakeTransform(rodrigues([pi/2, 0, 0]),mat([0, 0.22, 0.1]).T)
juiceTSR.Bw = mat([0, 0, 0, 0, -0.02, 0.02, 0, 0, 0, 0, -pi, pi])
juiceTSR.manipindex = 0
test=GeneralIK('','',[juiceTSR],False)
test.supportlinks=['leftFootBase']
test.cogtarget=(.1,.2,.3)
print test.Serialize()
