plug(robot.state, dyn.position)
plug(robot.velocity, dyn.velocity)
dyn.acceleration.value = robotDim * (0., )
dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()
dyn.setProperty('ComputeBackwardDynamics', 'true')
dyn.setProperty('ComputeAccelerationCoM', 'true')
robot.control.unplug()
# --- Task Dyn -----------------------------------------
# Task right hand
task = MetaTaskDyn6d('rh', dyn, 'task')
task.ref = ((0, 0, -1, 0.22), (0, 1, 0, -0.37), (1, 0, 0, .74), (0, 0, 0, 1))
# Task LFoot: Move the right foot up.
taskLF = MetaTaskDyn6d('lf', dyn, 'lf', 'left-ankle')
taskLF.ref = ((1, 0, 0, 0.0), (0, 1, 0, +0.29), (0, 0, 1, .15), (0, 0, 0, 1))
# --- TASK COM ------------------------------------------------------
dyn.setProperty('ComputeCoM', 'true')
featureCom = FeatureGeneric('featureCom')
featureComDes = FeatureGeneric('featureComDes')
plug(dyn.com, featureCom.errorIN)
plug(dyn.Jcom, featureCom.jacobianIN)
featureCom.setReference('featureComDes')
featureComDes.errorIN.value = (0.0478408688115, -0.0620357207995,
pg.parseCmd(':singlesupporttime 0.7')
# When velocity reference is at zero, the robot stops all motion after n steps
pg.parseCmd(':numberstepsbeforestop 4')
# Set constraints on XY
pg.parseCmd(':setfeetconstraint XY 0.09 0.06')
# Start the robot with a speed of 0.1 m/0.8 s.
pg.parseCmd(':HerdtOnline 0.1 0.0 0.0')
# You can now modifiy the speed of the robot using set pg.velocitydes [3]( x, y, yaw)
pg.velocitydes.value = (0.1, 0.0, 0.0)
#-----------------------------------------------------------------------------
# --- OPERATIONAL TASKS (For HRP2-14)---------------------------------------------
#-----------------------------------------------------------------------------
# --- Op task for the waist ------------------------------
taskWaist = MetaTaskDyn6d('taskWaist', dyn, 'waist', 'waist')
taskChest = MetaTaskDyn6d('taskChest', dyn, 'chest', 'chest')
taskHead = MetaTaskDyn6d('taskHead', dyn, 'head', 'gaze')
taskRH = MetaTaskDyn6d('rh', dyn, 'rh', 'right-wrist')
taskLH = MetaTaskDyn6d('lh', dyn, 'lh', 'left-wrist')
taskRF = MetaTaskDyn6d('rf', dyn, 'rf', 'right-ankle')
taskLF = MetaTaskDyn6d('lf', dyn, 'lf', 'left-ankle')
for task in [taskWaist, taskChest, taskHead, taskRH, taskLH, taskRF, taskLF]:
task.feature.frame('current')
task.gain.setConstant(50)
task.task.dt.value = dt
# --- TASK COM ------------------------------------------------------
taskCom = MetaTaskDynCom(dyn, dt)
pg.parseCmd(':doublesupporttime 0.1')
pg.parseCmd(':singlesupporttime 0.7')
# When velocity reference is at zero, the robot stops all motion after n steps
pg.parseCmd(':numberstepsbeforestop 4')
# Set constraints on XY
pg.parseCmd(':setfeetconstraint XY 0.09 0.06')
# Start the robot with a speed of 0.1 m/0.8 s.
pg.parseCmd(':HerdtOnline 0.1 0.0 0.0')
# You can now modifiy the speed of the robot using set pg.velocitydes [3]( x, y, yaw)
pg.velocitydes.value = (0.1, 0.0, 0.0)
# ---- TASKS -------------------------------------------------------------------
# ---- CONTACT -----------------------------------------
# Left foot contact
contactLF = MetaTaskDyn6d('contact_lleg', dyn, 'lf', 'left-ankle')
contactLF.support = ((0.11, -0.08, -0.08, 0.11), (-0.045, -0.045, 0.07, 0.07),
(-0.105, -0.105, -0.105, -0.105))
contactLF.feature.frame('desired')
# Right foot contact
contactRF = MetaTaskDyn6d('contact_rleg', dyn, 'rf', 'right-ankle')
contactRF.support = ((0.11, -0.08, -0.08, 0.11), (-0.07, -0.07, 0.045, 0.045),
(-0.105, -0.105, -0.105, -0.105))
contactRF.feature.frame('desired')
# ---- WAIST TASK ---
taskWaist = MetaTaskDyn6d('waist', dyn, 'waist', 'waist')
# Build the reference waist pos h**o-matrix from PG.
waistReferenceVector = Stack_of_vector('waistReferenceVector')