# --- RUN ---------------------------------------------------------------------- # --- RUN ---------------------------------------------------------------------- # --- RUN ---------------------------------------------------------------------- dyn.com.recompute(0) taskCom.featureDes.errorIN.value = dyn.com.value taskCom.task.controlGain.value = 10 taskPosture.gotoq(1,halfSittingConfig[robotName],chest=[],head=[],rleg=[],lleg=[],rarm=[],larm=[]) ball = BallPosition((0,-1.1,0.9)) sot.damping.value = 0.1 sot.addContact(contactRF) sot.addContact(contactLF) push(taskCom) push(taskRH) push(taskPosture) ball.move((0.5,-0.4,1.2),0) next() next() next() q0=(-0.013448627761452581, -0.0024679718155040885, 0.63088361991552966, -0.018777584993745728, -0.060941237730978988, 0.037292111609228754, -0.037510492296196345, 0.021074469596564682, -0.45832797319827417, 0.99704625676400405, -0.47711515439441948, -0.0045892937844194013, -0.0375232834669115, 0.021281798238735231, -0.47292880929139702, 1.00958227703776, -0.47505006770217456, -0.0047970164517906902, 0.072094769948381279, 0.011059478621976536, -0.00011090786283310123, 0.0010171428665263665, -0.52826992474554724, -0.45608428199514117, 0.042817559290084496, -1.2032584278571674, 0.18143720449535519, -0.89040683552343491, -0.029527723367927538, 0.24934327906079795, 0.15262968598049148, -0.0042541976395581915, -0.52055386436392925, 4.8770209960872496e-05, 0.00060030850701126319, 0.57928762243102871) ball.move((0.5,-0.2,1.2),0) taskPosture.gotoq(1,tuple(q0),chest=[],head=[],rleg=[],lleg=[],rarm=[],larm=[])
taskCom.task.controlGain.value = 10 taskPosture.gotoq(1, halfSittingConfig[robotName], chest=[], head=[], rleg=[], lleg=[], rarm=[], larm=[]) ball = BallPosition((0, -1.1, 0.9)) sot.damping.value = 0.1 sot.addContact(contactRF) sot.addContact(contactLF) push(taskCom) push(taskRH) push(taskPosture) ball.move((0.5, -0.4, 1.2), 0) next() next() next() q0 = (-0.013448627761452581, -0.0024679718155040885, 0.63088361991552966, -0.018777584993745728, -0.060941237730978988, 0.037292111609228754, -0.037510492296196345, 0.021074469596564682, -0.45832797319827417, 0.99704625676400405, -0.47711515439441948, -0.0045892937844194013,
# Problem from dynamic_graph.sot.core.meta_tasks_kine import gotoNd targetRH = (0.65,-0.2,0.9) targetLH = (0.1, 0.0,1.2) #targetLH = targetRH gotoNd(taskRH,targetRH,'111',(4.9,0.9,0.01,0.9)) gotoNd(taskLH,targetLH,'111',(4.9,0.9,0.01,0.9)) def push(task): if isinstance(task,str): taskName=task elif "task" in task.__dict__: taskName=task.task.name else: taskName=task.name if taskName not in sot.toList(): sot.push(taskName) if taskName!="taskposture" and "taskposture" in sot.toList(): sot.down("taskposture") return sot def pop(task): if isinstance(task,str): taskName=task elif "task" in task.__dict__: taskName=task.task.name else: taskName=task.name if taskName in sot.toList(): sot.rm(taskName) return sot push(taskRH) push(taskLH) sot.addContact(taskContact) push(taskJL)
targetRH = (0.65, -0.2, 0.9) targetLH = (0.1, 0.0, 1.2) #targetLH = targetRH gotoNd(taskRH, targetRH, '111', (4.9, 0.9, 0.01, 0.9)) gotoNd(taskLH, targetLH, '111', (4.9, 0.9, 0.01, 0.9)) def push(task): if isinstance(task, str): taskName = task elif "task" in task.__dict__: taskName = task.task.name else: taskName = task.name if taskName not in sot.toList(): sot.push(taskName) if taskName != "taskposture" and "taskposture" in sot.toList(): sot.down("taskposture") return sot def pop(task): if isinstance(task, str): taskName = task elif "task" in task.__dict__: taskName = task.task.name else: taskName = task.name if taskName in sot.toList(): sot.rm(taskName) return sot push(taskRH) push(taskLH) sot.addContact(taskContact) push(taskJL)