run /home/nmansard/src/StackOfTasks/script/base
new Dynamic dyn
dyn.setFiles /home/nmansard/src/PatternGeneratorJRL/src/data/ /home/nmansard/src/OpenHRP/etc/HRP2JRL/ HRP2JRLmain.wrl HRP2Specificities.xml HRP2LinkJointRank.xml 
dyn.parse
new VectorConstant zero
zero.resize 46
plug zero.out dyn.position
plug zero.out dyn.velocity
plug zero.out dyn.acceleration
dyn.createOpPoint 0 22
dyn.createOpPoint lleg 12 
dyn.createOpPoint rleg 6 
	
new FeaturePoint6d p6
new FeaturePoint6d p6d

new Compose<R+T> comp
new MatrixConstant eye3
eye3.resize 3 3
eye3.eye
plug eye3.out comp.in1
new VectorConstant t
t.resize 3
t.[] 1 -.3
t.[] 2 .3
t.[] 0 0.25
t.[] 1 -0.5
t.[] 2 .35
plug t.out comp.in2

plug comp.out p6d.position
plug dyn.J0 p6.Jq
plug dyn.0 p6.position
set p6.sdes p6d

new Task task
task.add p6
new GainAdaptative gain
gain.setConstant .5
plug task.error gain.error
plug gain.gain task.controlGain

new Constraint legs
legs.add dyn.Jlleg

dispmat matlab


eye3.[] 0 0 0
eye3.[] 2 2 0
eye3.[] 0 2 -1
eye3.[] 2 0 1


set p6.selec 000111

# --- COM 
dyn.setComputeCom 1

new FeatureGeneric featureCom
plug dyn.com featureCom.errorIN
plug dyn.Jcom featureCom.jacobianIN 
set featureCom.selec 11

new FeatureGeneric featureComDes
set featureComDes.errorIN [2](0,-0)
set featureCom.sdes featureComDes

new Task taskCom
taskCom.add featureCom
set taskCom.controlGain .1

# --- CHEST
new FeatureGeneric featureChest
set featureChest.jacobianIN [2,46]((0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0),(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0))
set featureChest.errorIN [2](0,0)
new Task taskChest
taskChest.add featureChest
set taskChest.controlGain 1

# --- LEGS
new FeatureGeneric featureLegs
new MatrixConstant jacobianLegs
jacobianLegs.resize 12 46
jacobianLegs.[] 0 6 1
jacobianLegs.[] 1 7 1
jacobianLegs.[] 2 8 1
jacobianLegs.[] 3 9 1
jacobianLegs.[] 4 10 1
jacobianLegs.[] 5 11 1
jacobianLegs.[] 6 12 1
jacobianLegs.[] 7 13 1
jacobianLegs.[] 8 14 1
jacobianLegs.[] 9 15 1
jacobianLegs.[] 10 16 1
jacobianLegs.[] 11 17 1
plug jacobianLegs.out featureLegs.jacobianIN 

new VectorConstant vectorLegs
vectorLegs.resize 12
vectorLegs.fill 0
plug vectorLegs.out featureLegs.errorIN
# set featureLegs.errorIN [12](0,0,0,0,0,0,0,0,0,0,0,0)
new Task taskLegs
taskLegs.add featureLegs
set taskLegs.controlGain 1

# --- TWOFEET
new Inverse<matrixhomo> rMw
plug dyn.rleg rMw.in 
new Multiply<matrixhomo> rMl
plug rMw.out rMl.in1 
plug dyn.lleg rMl.in2
new HomoToTwist rVl
plug rMl.out rVl.in
new Multiply<matrix> rJl
plug rVl.out rJl.in1
plug dyn.Jlleg rJl.in2
new Substract<matrix> Jtwofeet
plug rJl.out Jtwofeet.in1
plug dyn.Jrleg Jtwofeet.in2

new VectorConstant etwofeet
etwofeet.resize 6
etwofeet.fill 0

new FeatureGeneric featureTwofeet

plug etwofeet.out featureTwofeet.errorIN
# set featureTwofeet.errorIN [6](0,0,0,0,0,0)
plug Jtwofeet.out featureTwofeet.jacobianIN
new Task taskTwofeet
taskTwofeet.add featureTwofeet

# --- SOT
new SOT sot
# sot.addConstraint legs
# sot.push taskChest
# sot.push taskLegs
# sot.push task
# sot.push taskCom
sot.push taskTwofeet

new DShell dshell
dshell.open /tmp/nmansard/script.buffer
dshell.latency 50
dshell.start

# compute sot.control 0
etwofeet.[] 2 0.1



