def setFallCounter():
tilt = sensors.getValue(core.angleY)
roll = sensors.getValue(core.angleX)
tiltFalling = core.DEG_T_RAD * 45
rollFalling = core.DEG_T_RAD * 45
if (abs(tilt) > tiltFalling):
if tilt > 0:
# on stomach
if walk_request.tilt_fallen_counter_ < 0:
walk_request.tilt_fallen_counter_ = 1
else:
walk_request.tilt_fallen_counter_ = walk_request.tilt_fallen_counter_ + 1
else:
# on back
if walk_request.tilt_fallen_counter_ > 0:
walk_request.tilt_fallen_counter_ = -1
else:
walk_request.tilt_fallen_counter_ = walk_request.tilt_fallen_counter_ - 1
else:
walk_request.tilt_fallen_counter_ = 0
if (abs(roll) > rollFalling):
if roll > 0:
# on stomach
if walk_request.roll_fallen_counter_ < 0:
walk_request.roll_fallen_counter_ = 1
else:
walk_request.roll_fallen_counter_ = walk_request.roll_fallen_counter_ + 1
else:
# on back
if walk_request.roll_fallen_counter_ > 0:
walk_request.roll_fallen_counter_ = -1
else:
walk_request.roll_fallen_counter_ = walk_request.roll_fallen_counter_ - 1
else:
walk_request.roll_fallen_counter_ = 0
UTdebug.log(10, "fall counter: ", walk_request.tilt_fallen_counter_,
walk_request.roll_fallen_counter_)
return tilt, roll, tiltFalling, rollFalling
def setFallCounter():
tilt = sensors.getValue(core.angleY)
roll = sensors.getValue(core.angleX)
tiltFalling = core.DEG_T_RAD * 45
rollFalling = core.DEG_T_RAD * 45
if (abs(tilt) > tiltFalling):
if tilt > 0:
# on stomach
if walk_request.tilt_fallen_counter_ < 0:
walk_request.tilt_fallen_counter_ = 1
else:
walk_request.tilt_fallen_counter_ = walk_request.tilt_fallen_counter_ + 1
else:
# on back
if walk_request.tilt_fallen_counter_ > 0:
walk_request.tilt_fallen_counter_ = -1
else:
walk_request.tilt_fallen_counter_ = walk_request.tilt_fallen_counter_ - 1
else:
walk_request.tilt_fallen_counter_ = 0
if (abs(roll) > rollFalling):
if roll > 0:
# on stomach
if walk_request.roll_fallen_counter_ < 0:
walk_request.roll_fallen_counter_ = 1
else:
walk_request.roll_fallen_counter_ = walk_request.roll_fallen_counter_ + 1
else:
# on back
if walk_request.roll_fallen_counter_ > 0:
walk_request.roll_fallen_counter_ = -1
else:
walk_request.roll_fallen_counter_ = walk_request.roll_fallen_counter_ - 1
else:
walk_request.roll_fallen_counter_ = 0
UTdebug.log(10, "fall counter: ", walk_request.tilt_fallen_counter_, walk_request.roll_fallen_counter_)
return tilt,roll,tiltFalling,rollFalling
def doEyePower(eye=ledsC.allBottomLeftEye):
battery = sensors.getValue(core.battery)
if (battery > 0.95):
eye(1, 1, 1)
return False
elif (battery > 0.8):
eye(1, 1, 0)
return False
elif (battery > 0.65):
eye(1, .5, 1)
return False
else:
eye(1, 0, 0)
return True
def run(self):
global start_frame, droll_off, dpitch_off, dlarmroll_off, dlarmpitch_off, drarmroll_off, drarmpitch_off
if(start_frame == -1):
start_frame = vision_frame_info.frame_id
traj_idx = vision_frame_info.frame_id - start_frame
if(traj_idx >= len(trajectory)):
start_frame = -1
self.finish()
print "done!"
return
# traj_idx = len(trajectory) - 1
# print "Kick is " + str(float(traj_idx)/float(len(trajectory)-1.)*100.) + " percent complete"
inverted_joints = [1, -1, 1, -1, 1, 1, 1, -1, 1, 1, 1, 1, 1, 1, -1, 1, 1, 1, -1, -1, -1, -1]
# inverted_joints[core.LShoulderPitch] = -1.
# inverted_joints[core.RShoulderPitch] = -1.
# inverted_joints[core.RShoulderRoll] = -1.
# inverted_joints[core.RElbowRoll] = -1.
pose = trajectory[traj_idx]
for i in range(2, core.NUM_JOINTS):
joint_commands.setJointCommand(i, pose[i]*inverted_joints[i])
# tweak left ankle commands to handle cop dynamics
fl = sensors.getValue(core.fsrLFL)
fr = sensors.getValue(core.fsrLFR)
rl = sensors.getValue(core.fsrLRL)
rr = sensors.getValue(core.fsrLRR)
dx = (fl + fr)/2. - (rl + rr)/2. + 0.
dy = (fl + rl)/2. - (fr + rr)/2. - 0.05
# print "dx: " + str(dx)
# print "dy: " + str(dy)
clamp = 0.2
# droll_off += -dy * 0.001
# dpitch_off += -dx * 0.0
dlarmroll_off = numpy.clip(dlarmroll_off - dy * 0.0025, -clamp, clamp)
# dlarmpitch_off += dx * 0.0
drarmroll_off = numpy.clip(drarmroll_off - dy * 0.0025, -clamp, clamp)
# drarmpitch_off += dx * 0.0
droll = dy * 0.01 + droll_off
dpitch = -dx * 0.0 + dpitch_off
dlarmroll = -dy * 0.6 + dlarmroll_off
dlarmpitch = dx * 0.0 + dlarmpitch_off
dlhiproll = -dy * 0.025
drarmroll = -dy * 0.6 + drarmroll_off
drarmpitch = dx * 0.0 + drarmpitch_off
# print "droll: " + str(droll)
# print "dpitch: " + str(dpitch)
# print "dlarmroll: " + str(dlarmroll)
# print "dlarmpitch: " + str(dlarmpitch)
# print "drarmroll: " + str(drarmroll)
# print "drarmpitch: " + str(drarmpitch)
# joint_commands.setJointCommand(core.LAnklePitch, (pose[core.LAnklePitch] + dpitch)*inverted_joints[core.LAnklePitch])
# joint_commands.setJointCommand(core.LAnkleRoll, (pose[core.LAnkleRoll] + droll)*inverted_joints[core.LAnkleRoll])
# joint_commands.setJointCommand(core.LShoulderPitch, (pose[core.LShoulderPitch] + dlarmpitch)*inverted_joints[core.LShoulderPitch])
# joint_commands.setJointCommand(core.LShoulderRoll, numpy.clip(pose[core.LShoulderRoll] + dlarmroll, 0.3, 10)*inverted_joints[core.LShoulderRoll])
# joint_commands.setJointCommand(core.LHipRoll, (pose[core.LHipRoll] + dlhiproll)*inverted_joints[core.LHipRoll])
# joint_commands.setJointCommand(core.RShoulderPitch, (pose[core.RShoulderPitch] + drarmpitch)*inverted_joints[core.RShoulderPitch])
# joint_commands.setJointCommand(core.RShoulderRoll, numpy.clip(pose[core.RShoulderRoll] + drarmroll, -0.3, -10)*inverted_joints[core.RShoulderRoll])
#send commands and prevent other controllers from operating
joint_commands.send_body_angles_ = True
joint_commands.body_angle_time = 1# 1.0/30.0 * 1000.0#todo: figure out what units this is
walk_request.noWalk()
kick_request.setNoKick()
def run(self):
global start_frame, droll_off, dpitch_off, dlarmroll_off, dlarmpitch_off, drarmroll_off, drarmpitch_off
if (start_frame == -1):
start_frame = vision_frame_info.frame_id
traj_idx = vision_frame_info.frame_id - start_frame
if (traj_idx >= len(trajectory)):
start_frame = -1
self.finish()
print "done!"
return
# traj_idx = len(trajectory) - 1
# print "Kick is " + str(float(traj_idx)/float(len(trajectory)-1.)*100.) + " percent complete"
inverted_joints = [
1, -1, 1, -1, 1, 1, 1, -1, 1, 1, 1, 1, 1, 1, -1, 1, 1, 1, -1,
-1, -1, -1
]
# inverted_joints[core.LShoulderPitch] = -1.
# inverted_joints[core.RShoulderPitch] = -1.
# inverted_joints[core.RShoulderRoll] = -1.
# inverted_joints[core.RElbowRoll] = -1.
pose = trajectory[traj_idx]
for i in range(2, core.NUM_JOINTS):
joint_commands.setJointCommand(i, pose[i] * inverted_joints[i])
# tweak left ankle commands to handle cop dynamics
fl = sensors.getValue(core.fsrLFL)
fr = sensors.getValue(core.fsrLFR)
rl = sensors.getValue(core.fsrLRL)
rr = sensors.getValue(core.fsrLRR)
dx = (fl + fr) / 2. - (rl + rr) / 2. + 0.
dy = (fl + rl) / 2. - (fr + rr) / 2. - 0.05
# print "dx: " + str(dx)
# print "dy: " + str(dy)
clamp = 0.2
# droll_off += -dy * 0.001
# dpitch_off += -dx * 0.0
dlarmroll_off = numpy.clip(dlarmroll_off - dy * 0.0025, -clamp,
clamp)
# dlarmpitch_off += dx * 0.0
drarmroll_off = numpy.clip(drarmroll_off - dy * 0.0025, -clamp,
clamp)
# drarmpitch_off += dx * 0.0
droll = dy * 0.01 + droll_off
dpitch = -dx * 0.0 + dpitch_off
dlarmroll = -dy * 0.6 + dlarmroll_off
dlarmpitch = dx * 0.0 + dlarmpitch_off
dlhiproll = -dy * 0.025
drarmroll = -dy * 0.6 + drarmroll_off
drarmpitch = dx * 0.0 + drarmpitch_off
# print "droll: " + str(droll)
# print "dpitch: " + str(dpitch)
# print "dlarmroll: " + str(dlarmroll)
# print "dlarmpitch: " + str(dlarmpitch)
# print "drarmroll: " + str(drarmroll)
# print "drarmpitch: " + str(drarmpitch)
# joint_commands.setJointCommand(core.LAnklePitch, (pose[core.LAnklePitch] + dpitch)*inverted_joints[core.LAnklePitch])
# joint_commands.setJointCommand(core.LAnkleRoll, (pose[core.LAnkleRoll] + droll)*inverted_joints[core.LAnkleRoll])
# joint_commands.setJointCommand(core.LShoulderPitch, (pose[core.LShoulderPitch] + dlarmpitch)*inverted_joints[core.LShoulderPitch])
# joint_commands.setJointCommand(core.LShoulderRoll, numpy.clip(pose[core.LShoulderRoll] + dlarmroll, 0.3, 10)*inverted_joints[core.LShoulderRoll])
# joint_commands.setJointCommand(core.LHipRoll, (pose[core.LHipRoll] + dlhiproll)*inverted_joints[core.LHipRoll])
# joint_commands.setJointCommand(core.RShoulderPitch, (pose[core.RShoulderPitch] + drarmpitch)*inverted_joints[core.RShoulderPitch])
# joint_commands.setJointCommand(core.RShoulderRoll, numpy.clip(pose[core.RShoulderRoll] + drarmroll, -0.3, -10)*inverted_joints[core.RShoulderRoll])
#send commands and prevent other controllers from operating
joint_commands.send_body_angles_ = True
joint_commands.body_angle_time = 1 # 1.0/30.0 * 1000.0#todo: figure out what units this is
walk_request.noWalk()
kick_request.setNoKick()