# --- 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)
