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

# -- FF from leg
new Inverse<matrixhomo> worldMwaist
plug dyn2.lleg worldMwaist.in
new MatrixHomoToPoseRollPitchYaw posFF
plug worldMwaist.out posFF.in
new ComposeVector<RPY+T> posKF
plug posFF.out posKF.in2 

# --- KF 
dyn2.createOpPoint chest 14
new Inverse<matrixhomo> chestMwaist
plug dyn2.chest chestMwaist.in
new HomoToRotation chestRwaist
plug  chestMwaist.out chestRwaist.in
new RPYToMatrix worldRchest
# Dont forget to plug OpenHRP.attitude on worldRchest.in
new Multiply<matrixrotation> worldRwaist
plug worldRchest.out worldRwaist.in1
plug chestRwaist.out worldRwaist.in2
new MatrixToRPY worldrwaist
plug worldRwaist.out worldrwaist.in
plug worldrwaist.out posKF.in1 
     
# --- DYN With true posFF	
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
plug zero.out dyn.velocity
plug zero.out dyn.acceleration
plug posKF.out dyn.ffposition

# -- TASK Manip
dyn.createOpPoint 0 22
# dyn.createOpPoint 0 0
dyn.createOpPoint rleg 6 
dyn.createOpPoint lleg 12 

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
# WAIST
t.[] 0 0
t.[] 1 0.095
t.[] 2 0.563816
# HAND
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 .02
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 111
	
new FeatureGeneric featureComDes
# set featureComDes.errorIN [2](0,-0)
set featureCom.sdes featureComDes

new Task taskCom
taskCom.add featureCom
set taskCom.controlGain .3
# 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

# --- JL
new FeatureJointLimits featureJl
featureJl.actuate
plug dyn.position featureJl.joint
plug dyn.upperJl featureJl.upperJl
plug dyn.lowerJl featureJl.lowerJl
new Task taskJl
taskJl.add featureJl
set taskJl.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 Compose<R+T> semelle

new MatrixConstant Rsemelle
Rsemelle.resize 3 3
Rsemelle.eye
plug Rsemelle.out semelle.in1

new VectorConstant Tsemelle
Tsemelle.resize 3
Tsemelle.[] 0 0.
Tsemelle.[] 1 0.
Tsemelle.[] 2 -.105
plug Tsemelle.out semelle.in2

new Multiply<matrixhomo> wMrf
plug dyn.rleg wMrf.in1 
plug semelle.out wMrf.in2

new Multiply<matrixhomo> wMlf
plug dyn.lleg wMlf.in1 
plug semelle.out wMlf.in2

new Inverse<matrixhomo> rMw
plug wMrf.out rMw.in 

new Multiply<matrixhomo> rMl
plug rMw.out rMl.in1 
plug wMlf.out 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 FeaturePoint6dRelative p6r
# plug dyn.Jrleg p6r.Jq 
# plug dyn.Jlleg p6r.JqRef
# plug wMrf.out p6r.position 
# plug wMlf.out p6r.positionRef



new FeatureGeneric featureTwofeet

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


# --- SOT
new SOT sot
set sot.damping 1e-6
sot.addConstraint legs

sot.push taskTwofeet
# sot.push taskLegs
# sot.push task
# sot.push taskCom
# sot.push taskChest
# sot.gradient taskJl

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

# compute sot.control 0
# vectorLegs.[] 1 .1

etwofeet.[] 2 0

set featureComDes.errorIN [3](0.004,-0.09,.6)
