def inc():
robot.device.increment(robot.timeStep)
attime.run(robot.device.control.time)
updateComDisplay(robot.device, robot.dynamic.com)
if (robot.device.state.value[9] == 0.7):
print "\nReached right joint limit"
if (robot.device.state.value[15] == 0.7):
print "\nReached left joint limit"
def inc():
catch_exception = False
robot.device.increment(robot.timeStep)
attime.run(robot.device.control.time)
updateComDisplay(robot.device, robot.dynamic.com)
if state == 'tryF':
updateToolDisplay(robot.mTasks['screw'],
linalg.inv(TwoHandToolToScrewMatrix), robot.device)
if 'state' in globals(): supervision()
def inc():
robot.device.increment(robot.timeStep)
attime.run(robot.device.control.time)
updateComDisplay(robot.device, robot.dynamic.com)
if state >= 3 and state <= 21:
updateToolDisplay(robot.mTasks['screw'],
linalg.inv(TwoHandToolToScrewMatrix), robot.device)
"""
if state >= 3 and state <= 21:
record_zmp(robot.device,robot.dynamic,zmp_out,robot.timeStep)
record_hip(robot.device,robot.dynamic,hip_out,robot.timeStep)
record_pos(robot.device,robot.dynamic,pos_out,robot.timeStep)
"""
if 'state' in globals(): supervision()
def inc():
robot.increment(dt)
attime.run(robot.control.time)
updateComDisplay(robot,dyn.com)
history.record()
def inc():
robot.increment(dt)
attime.run(robot.control.time)
updateComDisplay(robot, dyn.com)
history.record()
def inc():
robot.increment(dt)
updateComDisplay(robot,dyn.com)
def inc():
robot.device.increment(robot.timeStep)
attime.run(robot.device.control.time)
updateComDisplay(robot.device, robot.dynamic.com)
def inc():
robot.device.increment(dt)
attime.run(robot.device.control.time)
updateComDisplay(robot,robot.dynamic.com)
def inc():
robot.device.increment(dt)
updateComDisplay(robot.device,robot.dynamic.com)
def inc():
robot.increment(dt)
attime.run(robot.control.time)
updateComDisplay(robot, dyn.com)
updateToolDisplay(taskRH, RHToTwoHandToolMatrix, robot)
DIM = 5
driftVector = [0] * DIM
for j in range(DIM):
x0 = random.uniform(0, 1)
y0 = random.uniform(0, 1)
z0 = random.uniform(0.5, 1.5)
roll = random.uniform(0, pi / 2)
pitch = random.uniform(0, pi / 2)
yaw = random.uniform(0, pi)
halfSittingConfig[robotName] = (x0, y0, z0, roll, pitch,
yaw) + halfSittingConfig[robotName][6:]
q0 = list(halfSittingConfig[robotName])
initialConfig[robotName] = tuple(q0)
robot.set(initialConfig[robotName])
for i in range(100):
robot.increment(dt)
updateComDisplay(robot, dyn.com)
history.record()
dyn.dynamicDrift.recompute(robot.control.time)
driftVector[j] = linalg.norm(dyn.dynamicDrift.value)
print "Iteration (" + str(j) + ") . DynamicDrift is: ||u|| = " + str(
driftVector[j])
driftVector.sort()
print driftVector
def do():
robot.increment(dt)
attime.run(robot.control.time)
updateComDisplay(robot, dyn.com)
updateToolDisplay(taskScrew, linalg.inv(TwoHandToolToScrewMatrix), robot)
def inc():
robot.increment(dt)
updateComDisplay(robot, dyn.com)