run script/dyn2
new Trajectory traj
traj.contactsDef tests/adrien/contacts.txt
traj.postures tests/adrien/postures.txt
traj.createContact dyn dyn2
traj.init



# --- Task posture
new FeatureGeneric featureSeqplay
new FeatureGeneric featureSeqplayDes

new Selec<vector> posart
posart.selec 6 46
plug dyn.position posart.in
plug posart.out featureSeqplay.errorIN

new MatrixConstant jacobianSeqplay
jacobianSeqplay.resize 40 46
jacobianSeqplay.[]  0  6 1
jacobianSeqplay.[]  1  7 1
jacobianSeqplay.[]  2  8 1
jacobianSeqplay.[]  3  9 1
jacobianSeqplay.[]  4 10 1
jacobianSeqplay.[]  5 11 1
jacobianSeqplay.[]  6 12 1
jacobianSeqplay.[]  7 13 1
jacobianSeqplay.[]  8 14 1
jacobianSeqplay.[]  9 15 1
jacobianSeqplay.[] 10 16 1
jacobianSeqplay.[] 11 17 1
jacobianSeqplay.[] 12 18 1
jacobianSeqplay.[] 13 19 1
jacobianSeqplay.[] 14 20 1
jacobianSeqplay.[] 15 21 1
jacobianSeqplay.[] 16 22 1
jacobianSeqplay.[] 17 23 1
jacobianSeqplay.[] 18 24 1
jacobianSeqplay.[] 19 25 1
jacobianSeqplay.[] 20 26 1
jacobianSeqplay.[] 21 27 1
jacobianSeqplay.[] 22 28 1
jacobianSeqplay.[] 23 29 1
jacobianSeqplay.[] 24 30 1
jacobianSeqplay.[] 25 31 1
jacobianSeqplay.[] 26 32 1
jacobianSeqplay.[] 27 33 1
jacobianSeqplay.[] 28 34 1
jacobianSeqplay.[] 29 35 1
jacobianSeqplay.[] 30 36 1
jacobianSeqplay.[] 31 37 1
jacobianSeqplay.[] 32 38 1
jacobianSeqplay.[] 33 39 1
jacobianSeqplay.[] 34 40 1
jacobianSeqplay.[] 35 41 1
jacobianSeqplay.[] 36 42 1
jacobianSeqplay.[] 37 43 1
jacobianSeqplay.[] 38 44 1
jacobianSeqplay.[] 39 45 1
plug jacobianSeqplay.out featureSeqplay.jacobianIN

set featureSeqplay.sdes featureSeqplayDes
new Task taskSeqplay
taskSeqplay.add featureSeqplay
set taskSeqplay.controlGain .1

# --- REPLUG
plug posKF.out dyn.ffposition


# --- SOT
sot.push taskCom
sot.push taskSeqplay

