taskJL.referenceInf.value = robot.dynamic.lowerJl.value taskJL.referenceSup.value = robot.dynamic.upperJl.value taskJL.dt.value = dt taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35)) # --- CONTACTS # define contactLF and contactRF for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]: contact = MetaTaskKine6d('contact'+name,robot.dynamic,name,joint) contact.feature.frame('desired') contact.gain.setConstant(10) locals()['contact'+name] = contact # --- TRACER ----------------------------------------------------------------- tr = Tracer('tr') tr.open('/tmp/','','.dat') tr.start() robot.device.after.addSignal('tr.triger') # tr.add('dyn2.com','com') history = History(robot.dynamic,1) # --- SHORTCUTS ---------------------------------------------------------------- qn = taskJL.normalizedPosition @optionalparentheses def pqn(details=True): ''' Display the normalized configuration vector. ''' qn.recompute(robot.state.time) s = [ "{0:.1f}".format(v) for v in qn.value]
taskJL.referenceSup.value = dyn.upperJl.value taskJL.dt.value = dt taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35)) # --- CONTACTS # define contactLF and contactRF for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]: contact = MetaTaskKine6d('contact'+name,dyn,name,joint) contact.feature.frame('desired') contact.gain.setConstant(10) locals()['contact'+name] = contact # --- TRACER ----------------------------------------------------------------- from dynamic_graph.tracer import Tracer tr = Tracer('tr') tr.open('/tmp/','','.dat') tr.start() robot.after.addSignal('tr.triger') tr.add('dyn.com','com') history = History(dyn,1) # --- SHORTCUTS ---------------------------------------------------------------- qn = taskJL.normalizedPosition @optionalparentheses def pqn(details=True): ''' Display the normalized configuration vector. ''' qn.recompute(robot.state.time) s = [ "{0:.1f}".format(v) for v in qn.value]
device.kd.value = nj * (2 * pow(device.kp.value[0], 0.5), ) # Create an instance to send a sinusoid control = TestSinusoidControl('control') control.setAmplitude(0.5) control.setFrequency(0.1) control.setJointID(3) control.dt.value = dt # Plug device and control plug(device.state, control.positionMeas) plug(control.positionDes, device.control) # Increment the device device.increment(dt) # Start the control control.start() tr = Tracer('tr') tr.open('/tmp/', 'dyn_', '.dat') tr.add('control.positionDes', '') tr.start() device.after.addSignal('tr.triger') for i in range(1000): device.increment(dt) print("finished ...")