runmode user

angle a
angle b

joint body 0.0 2.0 0.0 0.0 none
joint shoulder 2.0 1.0 0.0 a force
joint elbow 1.5 -0.3 0.0 b force

tensor body     1.4 0 0 0 0 0 -1 0 0 1
tensor shoulder 1.4 0 0 0 0 0 -1 0 0 1
tensor elbow    1.4 0 0 0 0 0 -1 0 0 1

graph activate

numsteps 1000
timestep 0.02

include robot.from
include robot.to

schematic create sh_in inputnode 30 400
schematic create sh_tin inputnode 30 300
schematic create sh_diff difference 150 400
schematic create sh_out outputnode 300 400
schematic create sh_tran transformation 200 393
schematic create el_in inputnode 30 200
schematic create el_tin inputnode 30 100
schematic create el_diff difference 150 200
schematic create el_out outputnode 300 200
schematic create el_tran transformation 200 193

schematic connect sh_in output sh_diff inputb
schematic connect sh_tin output sh_diff inputa
schematic connect sh_diff output sh_tran input
schematic connect sh_tran output sh_out input
schematic connect el_in output el_diff inputb
schematic connect el_tin output el_diff inputa
schematic connect el_diff output el_tran input
schematic connect el_tran output el_out input

schematic position sh_in output right
schematic position sh_tin output right
schematic position sh_out input left
schematic position sh_diff inputa down
schematic position sh_diff inputb left
schematic position sh_diff output right
schematic position sh_tran input left
schematic position sh_tran output right
schematic position el_in output right
schematic position el_tin output right
schematic position el_out input left
schematic position el_diff inputa down
schematic position el_diff inputb left
schematic position el_diff output right
schematic position el_tran input left
schematic position el_tran output right

schematic program sh_tran 0.1/1-z
schematic program el_tran 0.03/1-z

schematic bind current sh_in shoulder theta
schematic bind to sh_tin shoulder theta
schematic bind current sh_out shoulder thetad

schematic bind current el_in elbow theta
schematic bind to el_tin elbow theta
schematic bind current el_out elbow thetad

graph create acc 10 10 500 300
graph mode acc auto auto
graph labels acc 3 5
graph trace acc shoulder thetadd red
graph trace acc elbow thetadd blue

graph create for 520 10 500 300
graph mode for auto auto
graph labels for 3 5
graph trace for shoulder ftheta red
graph trace for elbow ftheta blue

