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 switch:
commands.setStiffness(cfgstiff.Zero)
else:
readings = core.joint_values
# TODO: Set joint values
# commands.setStiffness()
print("Set stiffness")
for i, value in enumerate(readings):
joint_commands.setJointCommand(i, value)
# commands.setStiffness()
if self.getTime() > 0.5:
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): UTdebug.log(15, "Blocking right") print "Block Center" joint_commands.setJointCommand(core.RShoulderPitch,0.0) joint_commands.setJointCommand(core.LShoulderPitch,0.0)
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):
UTdebug.log(15, "Blocking right")
print "Block Center"
joint_commands.setJointCommand(core.RShoulderPitch, 0.0)
joint_commands.setJointCommand(core.LShoulderPitch, 0.0)
