s = ['left-ankle', 'right-ankle']
for i in s:
robot.dynamic.signal(i).recompute(robot.dynamic.signal(i).time + 1)
robot.features[i].reference.value = robot.dynamic.signal(i).value
robot.features[i]._feature.selec.value = '111111'
robot.tasks[i].controlGain.value = 180.
robot.featureComDes.errorIN.value = robot.dynamic.com.value
robot.featureComDes.selec.value = '111'
robot.comTask.controlGain.value = 180.
# Push com and feet tasks.
#
# The robot is currently in half-sitting, so this script freezes com
# and feet position so that the robot will remain stable while the
# user program is starting.
solver.push(robot.comTask.name)
for i in s:
solver.push(robot.tasks[i].name)
timeline.setState('start')
#for i in range(1000):
while True:
robot.device.increment(0.005)
print robot.device.state.time, timeline.scaledTime.value, timeline.scaledTime.time, bezier.state.value
robot.dynamic.signal(i).value
robot.features[i]._feature.selec.value = '111111'
robot.tasks[i].controlGain.value = 180.
robot.featureComDes.errorIN.value = robot.dynamic.com.value
robot.featureComDes.selec.value = '111'
robot.comTask.controlGain.value = 180.
robot.balanceTask.controlGain.value = 180.
# Push com and feet tasks.
#
# The robot is currently in half-sitting, so this script freezes com
# and feet position so that the robot will remain stable while the
# user program is starting.
solver.push(robot.balanceTask)
class Motion (object):
reachTime = 3.
stillTime = .5
def __init__ (self, robot, solver):
self.samplingPeriod = .005
self.robot = robot
self.solver = solver
self.ros = Ros (robot)
self.ros.rosExport.add ('matrixHomoStamped', 'ballInCamera',
'/wide/blobs/rose/transform')
self.ros.rosExport.add ('matrixHomoStamped', 'ballInWaist',
'/wide/blobs/rose/transform_base_link')
self.rightGripperId = 28
# Right hand task
