def setup(self):
# commands.stand()
blocker = Blocker()
blocks = {
"left": BlockLeft(),
"right": BlockRight(),
"center": BlockCenter(),
"no_block": NoBlock()
}
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.BlockLeft(),
"right": pose.BlockRight(),
"center": pose.BlockLeft(),
"no_block": pose.BlockLeft()
}
stand = Stand()
self.trans(stand, C, blocker)
for name in blocks:
b = blocks[name]
p = poses[name]
self.trans(blocker, S(name), p, T(3),
pose.PoseSequence(cfgpose.sittingPoseV3, 3.0), C,
Stand(), C, blocker)
self.setFinish(
None
) # This ensures that the last node in trans is not the final node
def setup(self):
blocker = Blocker()
blocks = {
"left": pose.BlockLeft(),
"right": pose.BlockRight(),
"center": pose.BlockCenter()
}
for name in blocks:
b = blocks[name]
self.add_transition(blocker, S(name), b, T(10.25), blocker)
def setup(self):
blocker = Blocker()
blocks = {
"left": pose.BlockLeft(),
"right": pose.BlockRight(),
"center": pose.Squat() # Does not get up after squatting
}
for name in blocks:
b = blocks[name]
stand = pose.Stand()
# Time less than 6 turns off robot after squatting
#self.add_transition(blocker, S(name), b, T(3.25), blocker)
self.add_transition(blocker, S(name), b, T(6.0), blocker)
def setup(self):
blocker = Blocker()
penalty = ApproachBall()
blocks = {
"left": pose.BlockLeft(),
"right": pose.BlockRight(),
#"center": pose.SitBlock(),
"center": pose.BlockCenter(),
}
for name in blocks:
b = blocks[name]
self.add_transition(blocker, S(name), b, T(4.25), blocker)
self.add_transition(blocker, S("penalty"), penalty)
self.add_transition(penalty, S("blocker"), blocker)
def run(self):
UTdebug.log(15, "Blocking left")
return pose.BlockLeft()
def run(self):
UTdebug.log(15, "Blocking left")
print "Block Left"
# joint_commands.setJointCommandDeg(core.LShoulderRoll, 90.0)
return pose.BlockLeft(2.0)