class Bone(object):
def __init__(self, name=None, parent=None, length=0, x=0, y=0, scaleX=1, scaleY=1, rotation=0):
self.bone_data = None
self.name = name
self.local_tsr = TSR((x, y), scaleX, scaleY, rotation)
self.global_tsr = TSR()
self.parent = parent
self.length = length
self.childs = []
def update_childs(self):
data = self.global_tsr
reduce(lambda _, child: child.update(data), self.childs, None)
def update(self, data):
#self.position, self.scale, self.rotation = data
#pos, scale_x, scale_y, rot = self.bone_data[3:7:]
#par_pos, par_scale_x, par_scale_y, par_rot = data
#self.position, self.scale_x, self.scale_y, self.rotation = tsr_transform(data, self.bone_data[3:7:])
self.global_tsr.set_by_named_pack(self.local_tsr.tsr_transform(data))
self.update_childs()
def add_bone(self, bone):
self.childs.append(bone)
def apply_bone_data(self):
self.bone_data = Bone_Data(self.name, self.parent, self.length, self.local_tsr.copy())
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 __init__(self, name=None, parent=None, length=0, x=0, y=0, scaleX=1, scaleY=1, rotation=0):
self.bone_data = None
self.name = name
self.local_tsr = TSR((x, y), scaleX, scaleY, rotation)
self.global_tsr = TSR()
self.parent = parent
self.length = length
self.childs = []
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
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()
