# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
#sot.addContact(contactRF)
#sot.addContact(contactLF)
#gotoNd(taskWaist,(0,0,0),'111011',(1,))
#taskWaist.feature.frame('desired')
#sot.push(taskWaist.task.name)
#taskCom0.featureDes.errorIN.value = dyn.com.value
#taskCom0.task.controlGain.value = 10
#sot.push(taskCom0.task.name)
# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
pg.startHerdt()
# You can now modifiy the speed of the robot using set pg.pg.velocitydes [3]( x, y, yaw)
pg.pg.velocitydes.value =(0.1,0.0,0.0)
#ball0 = (2.5,-0.3,0.5)
ball0 = (0.5,-2.3,0.5)
w=(1/1.3,1/1.8,1/3.9)
h=(0.1,0.07,0.3)
sot.damping.value = 5e-2
sot.push(taskJL.name)
sot.push(taskWaist.task.name)
sot.push(taskRF.task.name)
sot.push(taskLF.task.name)
sot.push(taskCom.task.name)
taskPosture.gotoq((5,1,0.05,0.9),rhand=[1,])
plug(taskPosition.error,gainPosition.error)
plug(gainPosition.gain,taskPosition.controlGain)
featurePosition.selec.value = '000000000000001111111111111100000000000'
############################################################################
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
solver.push(taskWaist.task)
solver.push(taskRF.task)
solver.push(taskLF.task)
solver.push(taskCom.task)
# Stun the upper part of the body.
solver.push(taskHead.task) # constraint the head orientation: look straight ahead
solver.push(taskPosition) # stun the arms.
# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
pg.startHerdt(False)
print('You can now modifiy the speed of the robot by setting pg.pg.velocitydes')
print('example : pg.pg.velocitydes.value =(0.1,0.0,0.0)\n')
go()
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
#sot.addContact(contactRF)
#sot.addContact(contactLF)
#gotoNd(taskWaist,(0,0,0),'111011',(1,))
#taskWaist.feature.frame('desired')
#sot.push(taskWaist.task.name)
#taskCom0.featureDes.errorIN.value = dyn.com.value
#taskCom0.task.controlGain.value = 10
#sot.push(taskCom0.task.name)
# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
pg.startHerdt()
# You can now modifiy the speed of the robot using set pg.pg.velocitydes [3]( x, y, yaw)
pg.pg.velocitydes.value = (0.1, 0.0, 0.0)
#ball0 = (2.5,-0.3,0.5)
ball0 = (0.5, -2.3, 0.5)
w = (1 / 1.3, 1 / 1.8, 1 / 3.9)
h = (0.1, 0.07, 0.3)
sot.damping.value = 5e-2
sot.push(taskJL.name)
sot.push(taskWaist.task.name)
sot.push(taskRF.task.name)
sot.push(taskLF.task.name)
sot.push(taskCom.task.name)
taskPosture.gotoq((5, 1, 0.05, 0.9), rhand=[
