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