def supportTorsoPose(supports):
#Calculate torso pose based on grip manips (i.e. best guess based on some parameters
# Update the passed in reference with the Torso Manip
t = mat([0, 0, 0]).T
#Define ideal vertical offsets from each end effector
numS = len(supports)
L = {
'leftArmManip': .1,
'rightArmManip': .1,
'leftFootManip': .75,
'rightFootManip': .75
}
w = {
'leftArmManip': 1,
'rightArmManip': 1,
'leftFootManip': 5,
'rightFootManip': 5
}
wsum = 0
for s in supports.keys():
#TODO: remove expensive string comparison? maybe pre-subtract the affine manip
#TODO: use centroid calculation instead of mean?
if s == 'affineManip':
continue
T = supports[s].endPose()
#print T[:3,-1]
t = t + (T[:3, -1] + mat([-.1, 0, L[s]]).T) * w[s]
wsum += w[s]
affineTSR = TSR(MakeTransform(mat(eye(3)), t / wsum))
affineTSR.Bw = mat(
[-.05, .05, -.05, .05, -.1, .15, 0, 0, 0, 0, -pi / 16, pi / 16])
affineTSR.manipindex = 4
return affineTSR
def supportTorsoPose(supports):
# Calculate torso pose based on grip manips (i.e. best guess based on some parameters
# Update the passed in reference with the Torso Manip
t = mat([0, 0, 0]).T
# Define ideal vertical offsets from each end effector
numS = len(supports)
L = {"leftArmManip": 0.1, "rightArmManip": 0.1, "leftFootManip": 0.75, "rightFootManip": 0.75}
w = {"leftArmManip": 1, "rightArmManip": 1, "leftFootManip": 5, "rightFootManip": 5}
wsum = 0
for s in supports.keys():
# TODO: remove expensive string comparison? maybe pre-subtract the affine manip
# TODO: use centroid calculation instead of mean?
if s == "affineManip":
continue
T = supports[s].endPose()
# print T[:3,-1]
t = t + (T[:3, -1] + mat([-0.1, 0, L[s]]).T) * w[s]
wsum += w[s]
affineTSR = TSR(MakeTransform(mat(eye(3)), t / wsum))
affineTSR.Bw = mat([-0.05, 0.05, -0.05, 0.05, -0.1, 0.15, 0, 0, 0, 0, -pi / 16, pi / 16])
affineTSR.manipindex = 4
return affineTSR
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()
