def __init__(self, pose, time = 0.50, reverse=False): Task.__init__(self) self.pose = pose self.time = time self.reverse = reverse walk_request.noWalk() kick_request.setNoKick()
def __init__(self):
Task.__init__(self)
kick_request.setNoKick()
walk_request.noWalk()
kick_request.kick_running_ = False
behavior_mem.keeperDiving = core.Dive.NONE
self.state = state_machine.SimpleStateMachine('stop', 'checkarms', 'movearms', 'sit', 'relaxknee', 'relaxbody', 'finish')
self.skippedState = False
self.lower_time = 0
def __init__(self):
Task.__init__(self)
kick_request.setNoKick()
walk_request.noWalk()
kick_request.kick_running_ = False
behavior_mem.keeperDiving = core.Dive.NONE
self.state = state_machine.SimpleStateMachine('stop', 'checkarms', 'movearms', 'sit', 'relaxknee', 'relaxbody', 'finish')
self.skippedState = False
self.lower_time = 0
def run(self):
finished = walk_response.finished_standing_
received = walk_response.received_
kick_request.setNoKick()
walk_request.standStraight()
if received and self.state.inState('requested'):
self.state.transition('running')
elif self.state.inState('running') and finished:
self.finish()
if self.getTime() > 2:
self.finish()
def run(self):
finished = walk_response.finished_standing_
received = walk_response.received_
kick_request.setNoKick()
walk_request.standStraight()
if received and self.state.inState('requested'):
self.state.transition('running')
elif self.state.inState('running') and finished:
self.finish()
if self.getTime() > 2:
self.finish()
def run(self):
for i in range(2, core.NUM_JOINTS):
val = util.getPoseJoint(i, self.pose, self.reverse)
if val != None:
joint_commands.setJointCommand(i, val * core.DEG_T_RAD)
joint_commands.send_body_angles_ = True
joint_commands.body_angle_time = self.time * 1000.0
walk_request.noWalk()
kick_request.setNoKick()
if self.getTime() > self.time:
self.finish()
def run(self):
if self.first:
for i in range(2, core.NUM_JOINTS):
val = util.getPoseJoint(i, self.pose, self.reverse)
if val != None:
joint_commands.setJointCommand(i, val * core.DEG_T_RAD)
joint_commands.send_body_angles_ = True
joint_commands.body_angle_time_ = self.time * self.interpolate #1000.0
walk_request.noWalk()
kick_request.setNoKick()
self.first = False
if self.getTime() > self.time:
self.finish()
def run(self):
if self.getTime() < 2.0:
walk_request.noWalk()
kick_request.setNoKick()
commands.setStiffness(cfgstiff.One, 0.3)
return
st = self.state
if st.inState(st.stop):
st.transition(st.checkarms)
if st.inState(st.checkarms):
shoulderCutoff = core.DEG_T_RAD * -90
lpitch = core.joint_values[core.LShoulderPitch]
rpitch = core.joint_values[core.RShoulderPitch]
if lpitch > shoulderCutoff and rpitch > shoulderCutoff:
st.transition(st.sit)
else:
st.transition(st.movearms)
elif st.inState(st.movearms):
pose = util.deepcopy(cfgpose.sittingPoseV3)
for joint, val in cfgpose.armSidePose.items():
pose[joint] = val
st.transition(st.sit)
return ToPoseMoveHead(tilt=0.0, pose=pose)
elif st.inState(st.sit):
self.skippedState = False
st.transition(st.relaxknee)
return ToPoseMoveHead(pose=cfgpose.sittingPoseV3, time=1.0)
elif st.inState(st.relaxknee):
self.lower_time = self.getTime()
commands.setStiffness(cfgstiff.ZeroKneeAnklePitch, 0.3)
st.transition(st.relaxbody)
elif st.inState(st.relaxbody) and st.timeSinceTransition() > 0.7:
commands.setStiffness(cfgstiff.Zero, 0.3)
st.transition(st.finish)
elif st.inState(st.finish):
self.finish()
def run(self):
if self.getTime() < 2.0:
walk_request.noWalk()
kick_request.setNoKick()
commands.setStiffness(cfgstiff.One, 0.3)
return
st = self.state
if st.inState(st.stop):
st.transition(st.checkarms)
if st.inState(st.checkarms):
shoulderCutoff = core.DEG_T_RAD * -90
lpitch = core.joint_values[core.LShoulderPitch]
rpitch = core.joint_values[core.RShoulderPitch]
if lpitch > shoulderCutoff and rpitch > shoulderCutoff:
st.transition(st.sit)
else:
st.transition(st.movearms)
elif st.inState(st.movearms):
pose = util.deepcopy(cfgpose.sittingPoseV3)
for joint, val in cfgpose.armSidePose.items():
pose[joint] = val
st.transition(st.sit)
return ToPoseMoveHead(tilt = 0.0, pose = pose)
elif st.inState(st.sit):
self.skippedState = False
st.transition(st.relaxknee)
return ToPoseMoveHead(pose = cfgpose.sittingPoseV3)
elif st.inState(st.relaxknee):
self.lower_time = self.getTime()
commands.setStiffness(cfgstiff.ZeroKneeAnklePitch, 0.3)
st.transition(st.relaxbody)
elif st.inState(st.relaxbody) and st.timeSinceTransition() > 0.7:
commands.setStiffness(cfgstiff.Zero, 0.3)
st.transition(st.finish)
elif st.inState(st.finish):
self.finish()
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()