# Create a simple kinematic solver. from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize solver = initialize ( robot ) #------------------------------------------------------------------------------- #----- MAIN LOOP --------------------------------------------------------------- #------------------------------------------------------------------------------- # define the macro allowing to run the simulation. from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts dt=5e-3 @loopInThread def inc(): robot.device.increment(dt) runner=inc() [go,stop,next,n]=loopShortcuts(runner) # ---- TASKS ------------------------------------------------------------------- # ---- TASKS ------------------------------------------------------------------- # ---- TASKS ------------------------------------------------------------------- # ---- TASK GRIP --- # Defines a task for the right hand. taskRH=MetaTaskKine6d('rh',robot.dynamic,'right-wrist','right-wrist') # Move the operational point. handMgrip=eye(4); handMgrip[0:3,3] = (0.1,0,0) taskRH.opmodif = matrixToTuple(handMgrip) taskRH.feature.frame('desired') # robot.tasks['right-wrist'].add(taskRH.feature.name) # --- STATIC COM (if not walking)
# Alternate visualization tool from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer addRobotViewer(robot.device,small=True,small_extra=24,verbose=False) #------------------------------------------------------------------------------- #----- MAIN LOOP --------------------------------------------------------------- #------------------------------------------------------------------------------- # define the macro allowing to run the simulation. from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts dt=5e-3 @loopInThread def inc(): robot.device.increment(dt) runner=inc() [go,stop,next,n]=loopShortcuts(runner) # --- 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) contact.keep() locals()['contact'+name] = contact # ---- TASKS -------------------------------------------------------------------