def run(self):
UTdebug.log(15, "Blocking left")
self.setSubtask(pose.ToPose({ core.LShoulderRoll:80 , core.RShoulderRoll:5 ,
core.LShoulderPitch:-20 , core.RShoulderPitch:-80 }
, 0.5
)
)
def setup(self):
memory.speech.say("Let's calibrate'!")
kick_pose = pose.ToPose(
{
core.LHipYawPitch: 0.178185975067293,
core.LHipRoll: -16.3502707597166,
core.LHipPitch: -32.9569878020105,
core.LKneePitch: 65.9164003211764,
core.LAnklePitch: -35.3348771804154,
core.LAnkleRoll: 9.1383286566135,
core.RHipYawPitch: 0.178185975067293,
core.RHipRoll: 20.2174965470211,
core.RHipPitch: -28.3914298135855,
core.RKneePitch: 50.8038213107686,
core.RAnklePitch: -25.2225225670231,
core.RAnkleRoll: -20.1247972196057,
core.LShoulderPitch: -90.6139682089706,
core.LShoulderRoll: 7.81996554301764,
core.LElbowYaw: -0.881326629363425,
core.LElbowRoll: -2.37067669864139,
core.RShoulderPitch: -90.8824356244871,
core.RShoulderRoll: 6.33060181336134,
core.RElbowYaw: -0.96440905042746,
core.RElbowRoll: -2.72704864877597,
}, 1)
# Movements
stand = Stand()
sit = Sit()
fake_sit = FakeSit()
off = Off()
change_stiff = ChangeStiff()
self.trans(stand, C, fake_sit, C, kick_pose, T(10), change_stiff)
def run(self):
UTdebug.log(15, "Blocking center")
self.setSubtask(pose.ToPose({ core.LShoulderRoll:20 , core.RShoulderRoll:20 ,
core.LShoulderPitch:-20 , core.RShoulderPitch:-20 }
, 0.5
)
)
def setup(self):
init = InitBlocker()
blocker = Blocker()
blocks = {
"left": BlockLeft(),
"right": BlockRight(),
"center": pose.ToPose(cfgpose.sittingPoseV3,
0.5) #Squat()#BlockCenter()
}
reset = Reset()
self.trans(init, T(20), blocker)
for name in blocks:
b = blocks[name]
self.trans(blocker, S(name), b, T(5.0), reset, T(2), blocker)
def run(self):
self.setSubtask(
pose.ToPose(
{
core.LHipYawPitch: -50.7990128575929,
core.LHipRoll: -29.7098065875597,
core.LHipPitch: -36.1211002557756,
core.LKneePitch: 123.397585319279,
core.LAnklePitch: -48.8526839844773,
core.LAnkleRoll: 12.8,
core.RHipYawPitch: -50,
core.RHipRoll: -30.6718114113993,
core.RHipPitch: -37.3563946086858,
core.RKneePitch: 125.072320383009,
core.RAnklePitch: -48.5,
core.RAnkleRoll: 11.4,
core.LShoulderPitch: -79,
core.LShoulderRoll: 25,
core.RShoulderPitch: -79,
core.RShoulderRoll: 26
}, 1.0))
def run(self):
self.setSubtask(
pose.ToPose(
{
core.HeadYaw: 1.8433177928247,
core.HeadPitch: -22.497878144835,
core.LHipYawPitch: -4.18321199506924,
core.LHipRoll: 0.261268396131328,
core.LHipPitch: -46.5802099129511,
core.LKneePitch: 123.485476193518,
core.LAnklePitch: -70.5794592600033,
core.LAnkleRoll: -0.0902951008275686,
core.RHipYawPitch: -5.18321199506924,
core.RHipRoll: 0.266076849307017,
core.RHipPitch: -47.2002681461832,
core.RKneePitch: 124.72075688605,
core.RAnklePitch: -70.3988690583481,
core.RAnkleRoll: -0.0854866476518796,
core.LShoulderPitch: -92.0202358571845,
core.LShoulderRoll: 2.54645844712083,
core.RShoulderPitch: -92.1129420147891,
core.RShoulderRoll: 2.55126690029652,
}, 1.5))
def setup(self):
# commands.stand()
blocker = Blocker()
blocks = {
# "left": BlockLeft(),
# "right": BlockRight(),
# "center": BlockCenter(),
# "no_block": NoBlock()
"left": BlockLeft(),
"right": BlockRight(),
"center": blocker,
"no_block": blocker
}
spread_block = pose.ToPose({
core.LHipYawPitch: -46.1407555417525,
core.LHipRoll: -14.5924259541654,
core.LHipPitch: -4.831634837732,
core.LKneePitch: 63.8948760627169,
core.LAnklePitch: -28.4793206878252,
core.LAnkleRoll: 4.74374396349228,
core.RHipYawPitch: -46.1407555417525,
core.RHipRoll: -12.6540046073375,
core.RHipPitch: -10.6373092926212,
core.RKneePitch: 63.1086529873567,
core.RAnklePitch: -21.7068466162991,
core.RAnkleRoll: 0.881326629363425,
core.LShoulderPitch: -91.4049929073173,
core.LShoulderRoll: 8.34731078845598,
core.LElbowYaw: -0.0902951008275686,
core.LElbowRoll: -1.57964517010553,
core.RShoulderPitch: -91.7613785178302,
core.RShoulderRoll: 8.70368273859057,
core.RElbowYaw: -0.349159270371052,
core.RElbowRoll: -0.881326629363425}, 1)
super_spread = pose.ToPose({
core.LHipYawPitch: -53.7092735014381,
core.LHipRoll: -2.8149395230157,
core.LHipPitch: -50.1837767379148,
core.LKneePitch: 109.071229384231,
core.LAnklePitch: -31.1160742357736,
core.LAnkleRoll: 3.07380369255918,
core.RHipYawPitch: -53.7092735014381,
core.RHipRoll: -17.2243710489382,
core.RHipPitch: -59.3293180742919,
core.RKneePitch: 112.328034335222,
core.RAnklePitch: -29.9686707571032,
core.RAnkleRoll: 12.395140437794,
core.LShoulderPitch: -92.4596970585723,
core.LShoulderRoll: 8.43520166269571,
core.LElbowYaw: -1.1449992520826,
core.LElbowRoll: -1.8433177928247,
core.RShoulderPitch: -96.1559495505731,
core.RShoulderRoll: 7.20952421613692,
core.RElbowYaw: -1.22808167314663,
core.RElbowRoll: -1.40867187480177}, 1.5)
super_spread_2 = pose.ToPose({
core.LHipYawPitch: -53.7092735014381,
core.LHipRoll: -2.8149395230157,
core.LHipPitch: -50.1837767379148,
core.LKneePitch: 109.071229384231,
core.LAnklePitch: -31.1160742357736,
core.LAnkleRoll: 3.07380369255918,
core.RHipYawPitch: -53.7092735014381,
core.RHipRoll: -17.2243710489382,
core.RHipPitch: -59.3293180742919,
core.RKneePitch: 112.328034335222,
core.RAnklePitch: -29.9686707571032,
core.RAnkleRoll: 12.395140437794,
core.LShoulderPitch: -92.4596970585723,
core.LShoulderRoll: 8.43520166269571,
core.LElbowYaw: -1.1449992520826,
core.LElbowRoll: -1.8433177928247,
core.RShoulderPitch: -96.1559495505731,
core.RShoulderRoll: 7.20952421613692,
core.RElbowYaw: -1.22808167314663,
core.RElbowRoll: -1.40867187480177}, 3)
standingLeftArmPose = dict()
# standingLeftArmPose[core.LShoulderPitch] = 0
standingLeftArmPose[core.LShoulderRoll] = 90
# standingLeftArmPose[core.LElbowYaw] = -0
# standingLeftArmPose[core.LElbowRoll] = -0
standingRightArmPose = dict()
# standingRightArmPose[core.RShoulderPitch] = 0
standingRightArmPose[core.RShoulderRoll] = 90
# standingRightArmPose[core.RElbowYaw] = -0
# standingRightArmPose[core.RElbowRoll] = -0
standingBothArmPose = dict()
# standingBothArmPose[core.RShoulderPitch] = 1
standingBothArmPose[core.RShoulderRoll] = 90
# standingBothArmPose[core.RElbowYaw] = -0
# standingBothArmPose[core.RElbowRoll] = -0
# standingBothArmPose[core.LShoulderPitch] = 1
standingBothArmPose[core.LShoulderRoll] = 90
# standingBothArmPose[core.LElbowYaw] = -0
# standingBothArmPose[core.LElbowRoll] = -0
# for i in range(2, core.NUM_JOINTS):
# val = util.getPoseJoint(i, standingLeftArmPose, False)
# if val != None:
# joint_commands.setJointCommand(i, val * core.DEG_T_RAD)
toPoseTime = 0.2
poses = {
"left": pose.ToPose(standingLeftArmPose, toPoseTime),
"right": pose.ToPose(standingRightArmPose, toPoseTime),
"center": pose.ToPose(standingBothArmPose, toPoseTime),
"no_block": pose.ToPose(cfgpose.standingPose, toPoseTime)
}
forward = Forward()
stand = Stand()
stand_2 = Stand()
shuffle = Shuffle()
sit = Sit()
# self.trans(stand, C, pose.ToPose(cfgpose.standingPose), C, blocker)
self.trans(forward, C, stand, C, blocker)
# self.trans(forward, C, stand_2, C, blocker)
for name in blocks:
b = blocks[name]
p = poses[name]
# self.trans(blocker, S(name), p, C, b, T(3), pose.ToPose(cfgpose.standingPose, toPoseTime), C, blocker)
# self.trans(blocker, S(name), sit, t(0.5), super_spread, t(3), stand, c, shuffle, c, blocker)
self.trans(blocker, S(name), b, T(5.0), stand)
# pose.PoseSequence(cfgpose.sittingPoseV3, 1), T(2)
self.setFinish(None) # This ensures that the last node in trans is not the final node
def run(self):
return pose.ToPose(cfgpose.sittingPoseV3, 2.0)
def run(self):
print "Block Right"
UTdebug.log(15, "Blocking right")
joint_commands.setJointCommandDeg(core.RShoulderRoll, -90.0)
return pose.ToPose(cfgpose.blockright, 2.0, 100.0)
def run(self):
UTdebug.log(15, "Blocking left")
print "Block Left"
joint_commands.setJointCommandDeg(core.LShoulderRoll, 90.0)
return pose.ToPose(cfgpose.blockleft, 2.0, 100.0)