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 MakeInPlaceConstraint(robot, manipname): manips = robot.GetManipulators() manip = robot.GetManipulator(manipname) link = manip.GetEndEffector() T0_w = mat(link.GetTransform()) Tw_e = mat(eye(4)) for i in range(len(manips)): if manips[i].GetName() == manipname: manipindex = i break print manipindex tsr = TSR(T0_w, Tw_e, mat(zeros(12)), manipindex) print tsr.manipindex chain = TSRChain(0, 1, 1) chain.insertTSR(tsr) return chain
setInitialPose(robot) time.sleep(1) #Define manips used and goals z1=.01 theta=0.1 LH=0 RH=8 POST=8 RUNG0=16 RUNG=2 LF=0 RF=1 #Post grips at shoulder height rgrip1=TSR(grips[3+RH],MakeTransform(eye(3),matrix([.0,-.015,0]).T),mat([0,0, 0,0, -z1,z1, 0,0 ,0,0, -theta,theta]),1) lgrip1=TSR(grips[3],MakeTransform(eye(3),matrix([.0,.015,0]).T),mat([0,0, 0,0, -z1,z1, 0,0 ,0,0, -theta,theta]),0) #Rung grips at shoulder height rgrip2=TSR(grips[RUNG0+2*RUNG+RH],MakeTransform(eye(3),matrix([.0,-.015,0]).T),mat([0,0, 0,0, -z1,z1, 0,0 ,0,0, -theta,theta]),1) lgrip2=TSR(grips[RUNG0+2*RUNG],MakeTransform(eye(3),matrix([.0,.015,0]).T),mat([0,0, 0,0, -z1,z1, 0,0 ,0,0, -theta,theta]),0) #Step onto first rung lstep1=TSR(grips[RUNG0+LF],MakeTransform(eye(3),matrix([.0,0,.014]).T),mat([0,0, 0,0, 0,0, 0,0 ,-pi/3,0, 0,0]),2) rstep1=TSR(grips[RUNG0+RF],MakeTransform(eye(3),matrix([.0,0,0.014]).T),mat([0,0, 0,0, 0,0, 0,0 ,-theta,theta, 0,0]),3) # Define keyframe poses in terms of manips pose1={'rightArm':rgrip1,'leftArm':lgrip1,'leftFoot':lstep1} pose2={'rightArm':rgrip2,'leftArm':lgrip2,'leftFoot':lstep1} print "Place the robot in the desired starting position"
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()