├── .gitignore ├── studywolf_control ├── __init__.py ├── arms │ ├── __init__.py │ ├── one_link │ │ ├── __init__.py │ │ ├── arm1link.msim │ │ ├── py1LinkArm.so │ │ ├── build │ │ │ └── temp.linux-x86_64-2.7 │ │ │ │ └── py1LinkArm.o │ │ ├── setup.py │ │ ├── py1LinkArm.pyx │ │ ├── arm_python.py │ │ ├── arm.py │ │ ├── onelinkarm.cpp │ │ └── mpltable.h │ ├── two_link │ │ ├── __init__.py │ │ ├── MidLevel2Link.msim │ │ ├── build │ │ │ ├── temp.linux-x86_64-2.7 │ │ │ │ └── py2LinkArm.o │ │ │ └── temp.linux-x86_64-3.5 │ │ │ │ └── py2LinkArm.o │ │ ├── py2LinkArm.cpython-35m-x86_64-linux-gnu.so │ │ ├── setup.py │ │ ├── py2LinkArm.pyx │ │ ├── arm_python.py │ │ ├── arm_todorov.py │ │ ├── arm.py │ │ ├── config.py │ │ ├── arm_python_todorov.py │ │ ├── arm2base.py │ │ └── mpltable.h │ ├── three_link │ │ ├── __init__.py │ │ ├── arm3link.msim │ │ ├── py3LinkArm.so │ │ ├── py3LinkArm.pyd │ │ ├── arm3link_damping.msim │ │ ├── arm3link_gravity.msim │ │ ├── py3LinkArm_damping.so │ │ ├── py3LinkArm_gravity.so │ │ ├── arm3linksmallmass.msim │ │ ├── py3LinkArm_smallmass.so │ │ ├── arm3link_gravity_damping.msim │ │ ├── py3LinkArm_gravity_damping.so │ │ ├── build │ │ │ ├── temp.linux-x86_64-2.7 │ │ │ │ ├── py3LinkArm.o │ │ │ │ ├── py3LinkArm_damping.o │ │ │ │ ├── py3LinkArm_gravity.o │ │ │ │ ├── py3LinkArm_smallmass.o │ │ │ │ └── py3LinkArm_gravity_damping.o │ │ │ └── temp.linux-x86_64-3.5 │ │ │ │ ├── py3LinkArm.o │ │ │ │ ├── py3LinkArm_damping.o │ │ │ │ ├── py3LinkArm_gravity.o │ │ │ │ ├── py3LinkArm_smallmass.o │ │ │ │ └── py3LinkArm_gravity_damping.o │ │ ├── py3LinkArm.cpython-35m-x86_64-linux-gnu.so │ │ ├── setup.py │ │ ├── py3LinkArm.pyx │ │ ├── py3LinkArm_damping.pyx │ │ ├── py3LinkArm_gravity.pyx │ │ ├── py3LinkArm_smallmass.pyx │ │ ├── py3LinkArm_gravity_damping.pyx │ │ ├── config.py │ │ ├── arm.py │ │ └── mpltable.h │ └── ArmBase.py ├── tasks │ ├── __init__.py │ ├── write_data │ │ ├── __init__.py │ │ ├── l.txt │ │ ├── 1.txt │ │ ├── r.txt │ │ ├── o.txt │ │ ├── h.txt │ │ ├── e.txt │ │ ├── 0.txt │ │ ├── 7.txt │ │ ├── 6.txt │ │ ├── d.txt │ │ ├── 2.txt │ │ ├── 9.txt │ │ ├── 3.txt │ │ ├── 8.txt │ │ ├── 5.txt │ │ ├── read_path.py │ │ └── w.txt │ ├── random_movements.py │ ├── follow_mouse.py │ ├── reach.py │ ├── write.py │ └── postural.py ├── controllers │ ├── __init__.py │ ├── weights │ │ └── rnn_weights-trial13405-err0.00005.npz │ ├── signal.py │ ├── shell.py │ ├── recorder.py │ ├── forcefield.py │ ├── control.py │ ├── gc.py │ ├── trace.py │ ├── trajectory.py │ ├── dmp.py │ ├── ahf.py │ ├── osc.py │ ├── target_list.py │ ├── lqr.py │ └── gradient_approximation.py ├── img │ ├── whiteboard.jpg │ └── three_link │ │ ├── svghand2.png │ │ ├── svgforearm2.png │ │ └── svgupperarm2.png ├── run.py └── sim_and_plot.py └── README.rst /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | -------------------------------------------------------------------------------- /studywolf_control/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /studywolf_control/arms/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /studywolf_control/tasks/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /studywolf_control/arms/one_link/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /studywolf_control/arms/two_link/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /studywolf_control/controllers/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write_data/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /studywolf_control/img/whiteboard.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/img/whiteboard.jpg -------------------------------------------------------------------------------- /studywolf_control/arms/one_link/arm1link.msim: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/one_link/arm1link.msim -------------------------------------------------------------------------------- /studywolf_control/arms/one_link/py1LinkArm.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/one_link/py1LinkArm.so -------------------------------------------------------------------------------- /studywolf_control/img/three_link/svghand2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/img/three_link/svghand2.png -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/arm3link.msim: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/arm3link.msim -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/py3LinkArm.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/py3LinkArm.so -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/py3LinkArm.pyd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/py3LinkArm.pyd -------------------------------------------------------------------------------- /studywolf_control/arms/two_link/MidLevel2Link.msim: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/two_link/MidLevel2Link.msim -------------------------------------------------------------------------------- /studywolf_control/img/three_link/svgforearm2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/img/three_link/svgforearm2.png -------------------------------------------------------------------------------- /studywolf_control/img/three_link/svgupperarm2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/img/three_link/svgupperarm2.png -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/arm3link_damping.msim: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/arm3link_damping.msim -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/arm3link_gravity.msim: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/arm3link_gravity.msim -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/py3LinkArm_damping.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/py3LinkArm_damping.so -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/py3LinkArm_gravity.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/py3LinkArm_gravity.so -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/arm3linksmallmass.msim: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/arm3linksmallmass.msim -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/py3LinkArm_smallmass.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/py3LinkArm_smallmass.so -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/arm3link_gravity_damping.msim: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/arm3link_gravity_damping.msim -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/py3LinkArm_gravity_damping.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/py3LinkArm_gravity_damping.so -------------------------------------------------------------------------------- /studywolf_control/arms/one_link/build/temp.linux-x86_64-2.7/py1LinkArm.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/one_link/build/temp.linux-x86_64-2.7/py1LinkArm.o -------------------------------------------------------------------------------- /studywolf_control/arms/two_link/build/temp.linux-x86_64-2.7/py2LinkArm.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/two_link/build/temp.linux-x86_64-2.7/py2LinkArm.o -------------------------------------------------------------------------------- /studywolf_control/arms/two_link/build/temp.linux-x86_64-3.5/py2LinkArm.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/two_link/build/temp.linux-x86_64-3.5/py2LinkArm.o -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/build/temp.linux-x86_64-2.7/py3LinkArm.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/build/temp.linux-x86_64-2.7/py3LinkArm.o -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/build/temp.linux-x86_64-3.5/py3LinkArm.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/build/temp.linux-x86_64-3.5/py3LinkArm.o -------------------------------------------------------------------------------- /studywolf_control/arms/two_link/py2LinkArm.cpython-35m-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/two_link/py2LinkArm.cpython-35m-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /studywolf_control/controllers/weights/rnn_weights-trial13405-err0.00005.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/controllers/weights/rnn_weights-trial13405-err0.00005.npz -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/py3LinkArm.cpython-35m-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/py3LinkArm.cpython-35m-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/build/temp.linux-x86_64-2.7/py3LinkArm_damping.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/build/temp.linux-x86_64-2.7/py3LinkArm_damping.o -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/build/temp.linux-x86_64-2.7/py3LinkArm_gravity.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/build/temp.linux-x86_64-2.7/py3LinkArm_gravity.o -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/build/temp.linux-x86_64-3.5/py3LinkArm_damping.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/build/temp.linux-x86_64-3.5/py3LinkArm_damping.o -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/build/temp.linux-x86_64-3.5/py3LinkArm_gravity.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/build/temp.linux-x86_64-3.5/py3LinkArm_gravity.o -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/build/temp.linux-x86_64-2.7/py3LinkArm_smallmass.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/build/temp.linux-x86_64-2.7/py3LinkArm_smallmass.o -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/build/temp.linux-x86_64-3.5/py3LinkArm_smallmass.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/build/temp.linux-x86_64-3.5/py3LinkArm_smallmass.o -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/build/temp.linux-x86_64-2.7/py3LinkArm_gravity_damping.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/build/temp.linux-x86_64-2.7/py3LinkArm_gravity_damping.o -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/build/temp.linux-x86_64-3.5/py3LinkArm_gravity_damping.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/studywolf/control/HEAD/studywolf_control/arms/three_link/build/temp.linux-x86_64-3.5/py3LinkArm_gravity_damping.o -------------------------------------------------------------------------------- /studywolf_control/arms/one_link/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from distutils.extension import Extension 3 | from Cython.Distutils import build_ext 4 | 5 | setup( 6 | cmdclass = {'build_ext': build_ext}, 7 | ext_modules=[Extension("py1LinkArm", 8 | sources=["py1LinkArm.pyx"], 9 | language="c++"),], 10 | ) 11 | 12 | -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from distutils.extension import Extension 3 | from Cython.Distutils import build_ext 4 | import numpy 5 | 6 | setup( 7 | cmdclass = {'build_ext': build_ext}, 8 | ext_modules=[Extension("py3LinkArm", 9 | sources=["py3LinkArm.pyx"], 10 | language="c++", 11 | include_dirs=[numpy.get_include()]),], 12 | ) 13 | -------------------------------------------------------------------------------- /studywolf_control/arms/two_link/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from distutils.extension import Extension 3 | from Cython.Distutils import build_ext 4 | import numpy as np 5 | 6 | setup( 7 | cmdclass = {'build_ext': build_ext}, 8 | ext_modules=[Extension("py2LinkArm", 9 | sources=["py2LinkArm.pyx"], 10 | include_dirs=[np.get_include()], 11 | language="c++"),], 12 | ) 13 | 14 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write_data/l.txt: -------------------------------------------------------------------------------- 1 | 832.0,884.0 2 | 829.0,883.0 3 | 824.0,882.0 4 | 814.0,880.0 5 | 800.0,878.0 6 | 782.0,874.0 7 | 759.0,870.0 8 | 732.0,866.0 9 | 701.0,861.0 10 | 667.0,856.0 11 | 633.0,852.0 12 | 598.0,847.0 13 | 564.0,843.0 14 | 532.0,839.0 15 | 503.0,836.0 16 | 477.0,833.0 17 | 451.0,830.0 18 | 427.0,827.0 19 | 406.0,826.0 20 | 385.0,825.0 21 | 366.0,825.0 22 | 349.0,824.0 23 | 334.0,824.0 24 | 318.0,824.0 25 | 302.0,824.0 26 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write_data/1.txt: -------------------------------------------------------------------------------- 1 | 919.0,752.0 2 | 918.0,752.0 3 | 916.0,752.0 4 | 912.0,752.0 5 | 904.0,752.0 6 | 892.0,752.0 7 | 876.0,752.0 8 | 856.0,752.0 9 | 832.0,751.0 10 | 807.0,751.0 11 | 782.0,750.0 12 | 756.0,750.0 13 | 729.0,749.0 14 | 704.0,749.0 15 | 677.0,749.0 16 | 650.0,750.0 17 | 622.0,751.0 18 | 595.0,753.0 19 | 568.0,755.0 20 | 541.0,757.0 21 | 517.0,759.0 22 | 494.0,761.0 23 | 473.0,763.0 24 | 453.0,764.0 25 | 436.0,766.0 26 | 420.0,767.0 27 | 406.0,767.0 28 | 393.0,768.0 29 | 381.0,768.0 30 | 370.0,768.0 31 | 361.0,768.0 32 | 353.0,768.0 33 | 346.0,768.0 34 | 340.0,768.0 35 | 335.0,768.0 36 | 330.0,768.0 37 | 325.0,768.0 38 | 321.0,768.0 39 | 317.0,768.0 40 | 314.0,768.0 41 | 311.0,768.0 42 | 308.0,768.0 43 | 305.0,768.0 44 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write_data/r.txt: -------------------------------------------------------------------------------- 1 | 726.0,951.0 2 | 722.0,952.0 3 | 713.0,954.0 4 | 700.0,956.0 5 | 683.0,959.0 6 | 661.0,963.0 7 | 637.0,967.0 8 | 611.0,971.0 9 | 587.0,974.0 10 | 564.0,976.0 11 | 543.0,978.0 12 | 525.0,979.0 13 | 512.0,980.0 14 | 502.0,981.0 15 | 494.0,981.0 16 | 488.0,981.0 17 | 484.0,981.0 18 | 481.0,981.0 19 | 480.0,981.0 20 | 480.0,980.0 21 | 482.0,979.0 22 | 490.0,978.0 23 | 507.0,975.0 24 | 530.0,972.0 25 | 553.0,970.0 26 | 576.0,968.0 27 | 596.0,968.0 28 | 614.0,968.0 29 | 630.0,970.0 30 | 645.0,973.0 31 | 658.0,977.0 32 | 669.0,983.0 33 | 679.0,990.0 34 | 687.0,999.0 35 | 695.0,1012.0 36 | 701.0,1024.0 37 | 705.0,1036.0 38 | 707.0,1045.0 39 | 707.0,1053.0 40 | 706.0,1059.0 41 | 704.0,1063.0 42 | 702.0,1066.0 43 | 700.0,1068.0 44 | 696.0,1070.0 45 | 693.0,1072.0 46 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write_data/o.txt: -------------------------------------------------------------------------------- 1 | 641.0,1032.0 2 | 641.0,1031.0 3 | 641.0,1029.0 4 | 641.0,1026.0 5 | 641.0,1023.0 6 | 641.0,1019.0 7 | 641.0,1013.0 8 | 640.0,1005.0 9 | 639.0,995.0 10 | 636.0,984.0 11 | 632.0,973.0 12 | 627.0,962.0 13 | 621.0,951.0 14 | 613.0,940.0 15 | 605.0,930.0 16 | 596.0,921.0 17 | 586.0,912.0 18 | 571.0,904.0 19 | 554.0,898.0 20 | 536.0,895.0 21 | 517.0,896.0 22 | 497.0,900.0 23 | 481.0,906.0 24 | 469.0,913.0 25 | 460.0,922.0 26 | 451.0,936.0 27 | 446.0,952.0 28 | 446.0,969.0 29 | 451.0,988.0 30 | 459.0,1005.0 31 | 471.0,1020.0 32 | 484.0,1031.0 33 | 501.0,1043.0 34 | 519.0,1052.0 35 | 537.0,1059.0 36 | 555.0,1064.0 37 | 573.0,1066.0 38 | 589.0,1066.0 39 | 603.0,1063.0 40 | 614.0,1058.0 41 | 624.0,1051.0 42 | 632.0,1041.0 43 | 640.0,1027.0 44 | 645.0,1009.0 45 | 645.0,984.0 46 | 637.0,961.0 47 | 624.0,934.0 48 | 620.0,925.0 49 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write_data/h.txt: -------------------------------------------------------------------------------- 1 | 884.0,560.0 2 | 882.0,559.0 3 | 877.0,558.0 4 | 866.0,557.0 5 | 848.0,555.0 6 | 819.0,551.0 7 | 783.0,546.0 8 | 741.0,542.0 9 | 700.0,538.0 10 | 659.0,534.0 11 | 618.0,530.0 12 | 577.0,526.0 13 | 538.0,523.0 14 | 503.0,520.0 15 | 469.0,516.0 16 | 441.0,512.0 17 | 417.0,509.0 18 | 398.0,506.0 19 | 382.0,503.0 20 | 371.0,501.0 21 | 363.0,499.0 22 | 358.0,498.0 23 | 355.0,497.0 24 | 354.0,496.0 25 | 358.0,496.0 26 | 372.0,498.0 27 | 396.0,503.0 28 | 425.0,511.0 29 | 453.0,521.0 30 | 481.0,533.0 31 | 509.0,544.0 32 | 536.0,556.0 33 | 559.0,567.0 34 | 579.0,579.0 35 | 594.0,590.0 36 | 606.0,599.0 37 | 614.0,606.0 38 | 619.0,611.0 39 | 622.0,614.0 40 | 623.0,616.0 41 | 622.0,617.0 42 | 620.0,618.0 43 | 612.0,620.0 44 | 593.0,622.0 45 | 564.0,623.0 46 | 529.0,624.0 47 | 491.0,625.0 48 | 451.0,627.0 49 | 413.0,630.0 50 | 378.0,633.0 51 | 355.0,634.0 52 | 340.0,634.0 53 | 329.0,634.0 54 | 320.0,635.0 55 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write_data/e.txt: -------------------------------------------------------------------------------- 1 | 596.0,643.0 2 | 595.0,644.0 3 | 594.0,647.0 4 | 592.0,654.0 5 | 590.0,663.0 6 | 588.0,674.0 7 | 587.0,687.0 8 | 587.0,704.0 9 | 589.0,720.0 10 | 592.0,733.0 11 | 596.0,745.0 12 | 601.0,755.0 13 | 608.0,765.0 14 | 615.0,774.0 15 | 622.0,781.0 16 | 631.0,788.0 17 | 641.0,794.0 18 | 653.0,797.0 19 | 663.0,798.0 20 | 672.0,797.0 21 | 680.0,794.0 22 | 686.0,790.0 23 | 690.0,785.0 24 | 695.0,775.0 25 | 698.0,761.0 26 | 699.0,747.0 27 | 698.0,733.0 28 | 694.0,717.0 29 | 689.0,700.0 30 | 682.0,685.0 31 | 675.0,673.0 32 | 666.0,660.0 33 | 655.0,646.0 34 | 643.0,634.0 35 | 626.0,621.0 36 | 608.0,610.0 37 | 589.0,603.0 38 | 569.0,599.0 39 | 549.0,600.0 40 | 532.0,604.0 41 | 516.0,610.0 42 | 501.0,618.0 43 | 488.0,627.0 44 | 476.0,639.0 45 | 463.0,655.0 46 | 452.0,674.0 47 | 444.0,692.0 48 | 436.0,713.0 49 | 430.0,740.0 50 | 427.0,759.0 51 | 426.0,782.0 52 | 426.0,808.0 53 | 428.0,825.0 54 | 432.0,843.0 55 | 437.0,861.0 56 | 443.0,876.0 57 | 445.0,881.0 58 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write_data/0.txt: -------------------------------------------------------------------------------- 1 | 882.0,1212.0 2 | 881.0,1208.0 3 | 879.0,1200.0 4 | 876.0,1190.0 5 | 871.0,1177.0 6 | 864.0,1161.0 7 | 855.0,1144.0 8 | 844.0,1128.0 9 | 832.0,1113.0 10 | 819.0,1098.0 11 | 804.0,1084.0 12 | 784.0,1068.0 13 | 761.0,1053.0 14 | 735.0,1041.0 15 | 709.0,1032.0 16 | 683.0,1026.0 17 | 656.0,1024.0 18 | 632.0,1026.0 19 | 610.0,1031.0 20 | 588.0,1040.0 21 | 567.0,1051.0 22 | 549.0,1065.0 23 | 530.0,1084.0 24 | 512.0,1107.0 25 | 495.0,1139.0 26 | 485.0,1167.0 27 | 480.0,1200.0 28 | 481.0,1229.0 29 | 489.0,1258.0 30 | 502.0,1286.0 31 | 518.0,1309.0 32 | 538.0,1331.0 33 | 562.0,1352.0 34 | 590.0,1372.0 35 | 618.0,1388.0 36 | 644.0,1399.0 37 | 669.0,1406.0 38 | 693.0,1411.0 39 | 718.0,1412.0 40 | 742.0,1410.0 41 | 764.0,1406.0 42 | 786.0,1399.0 43 | 808.0,1388.0 44 | 828.0,1376.0 45 | 846.0,1363.0 46 | 861.0,1347.0 47 | 876.0,1325.0 48 | 887.0,1304.0 49 | 894.0,1277.0 50 | 897.0,1251.0 51 | 897.0,1229.0 52 | 894.0,1204.0 53 | 888.0,1179.0 54 | 880.0,1161.0 55 | 868.0,1141.0 56 | 850.0,1120.0 57 | 844.0,1113.0 58 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write_data/7.txt: -------------------------------------------------------------------------------- 1 | 836.0,783.0 2 | 835.0,784.0 3 | 834.0,787.0 4 | 833.0,794.0 5 | 831.0,804.0 6 | 829.0,818.0 7 | 827.0,839.0 8 | 826.0,862.0 9 | 825.0,886.0 10 | 824.0,916.0 11 | 823.0,942.0 12 | 823.0,971.0 13 | 823.0,1001.0 14 | 824.0,1024.0 15 | 825.0,1049.0 16 | 826.0,1071.0 17 | 826.0,1088.0 18 | 826.0,1100.0 19 | 826.0,1111.0 20 | 825.0,1120.0 21 | 824.0,1127.0 22 | 823.0,1132.0 23 | 822.0,1136.0 24 | 821.0,1139.0 25 | 820.0,1141.0 26 | 819.0,1143.0 27 | 818.0,1144.0 28 | 817.0,1145.0 29 | 816.0,1146.0 30 | 815.0,1147.0 31 | 814.0,1148.0 32 | 813.0,1148.0 33 | 812.0,1148.0 34 | 811.0,1148.0 35 | 810.0,1148.0 36 | 809.0,1148.0 37 | 808.0,1148.0 38 | 807.0,1148.0 39 | 806.0,1148.0 40 | 805.0,1148.0 41 | 804.0,1147.0 42 | 801.0,1145.0 43 | 796.0,1142.0 44 | 785.0,1134.0 45 | 767.0,1119.0 46 | 744.0,1100.0 47 | 715.0,1078.0 48 | 683.0,1053.0 49 | 652.0,1029.0 50 | 619.0,1009.0 51 | 587.0,990.0 52 | 552.0,969.0 53 | 522.0,950.0 54 | 489.0,934.0 55 | 461.0,921.0 56 | 431.0,906.0 57 | 403.0,891.0 58 | 381.0,880.0 59 | 362.0,871.0 60 | 344.0,863.0 61 | 330.0,857.0 62 | 317.0,852.0 63 | 310.0,849.0 64 | 305.0,847.0 65 | 301.0,845.0 66 | 299.0,843.0 67 | 297.0,842.0 68 | 295.0,840.0 69 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write_data/6.txt: -------------------------------------------------------------------------------- 1 | 880.0,1083.0 2 | 879.0,1080.0 3 | 878.0,1076.0 4 | 876.0,1069.0 5 | 873.0,1058.0 6 | 868.0,1046.0 7 | 862.0,1032.0 8 | 853.0,1015.0 9 | 842.0,996.0 10 | 830.0,978.0 11 | 817.0,960.0 12 | 802.0,943.0 13 | 786.0,925.0 14 | 769.0,908.0 15 | 749.0,891.0 16 | 725.0,874.0 17 | 700.0,859.0 18 | 674.0,845.0 19 | 645.0,832.0 20 | 618.0,822.0 21 | 590.0,815.0 22 | 562.0,810.0 23 | 537.0,808.0 24 | 511.0,809.0 25 | 486.0,814.0 26 | 461.0,821.0 27 | 439.0,830.0 28 | 418.0,841.0 29 | 401.0,854.0 30 | 385.0,869.0 31 | 370.0,887.0 32 | 357.0,907.0 33 | 348.0,931.0 34 | 344.0,952.0 35 | 344.0,973.0 36 | 348.0,995.0 37 | 356.0,1016.0 38 | 365.0,1033.0 39 | 376.0,1049.0 40 | 389.0,1065.0 41 | 402.0,1078.0 42 | 417.0,1090.0 43 | 433.0,1100.0 44 | 449.0,1107.0 45 | 466.0,1111.0 46 | 481.0,1112.0 47 | 497.0,1109.0 48 | 514.0,1103.0 49 | 530.0,1094.0 50 | 545.0,1085.0 51 | 558.0,1075.0 52 | 569.0,1063.0 53 | 582.0,1046.0 54 | 593.0,1029.0 55 | 600.0,1013.0 56 | 605.0,993.0 57 | 607.0,968.0 58 | 604.0,949.0 59 | 599.0,932.0 60 | 591.0,911.0 61 | 582.0,894.0 62 | 572.0,880.0 63 | 560.0,869.0 64 | 543.0,857.0 65 | 525.0,845.0 66 | 504.0,835.0 67 | 478.0,826.0 68 | 453.0,821.0 69 | 421.0,821.0 70 | 416.0,821.0 71 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write_data/d.txt: -------------------------------------------------------------------------------- 1 | 627.0,1093.0 2 | 627.0,1091.0 3 | 628.0,1089.0 4 | 629.0,1086.0 5 | 631.0,1081.0 6 | 634.0,1072.0 7 | 636.0,1060.0 8 | 637.0,1046.0 9 | 636.0,1029.0 10 | 633.0,1011.0 11 | 628.0,994.0 12 | 621.0,979.0 13 | 613.0,967.0 14 | 604.0,957.0 15 | 593.0,948.0 16 | 578.0,939.0 17 | 560.0,932.0 18 | 541.0,927.0 19 | 523.0,925.0 20 | 507.0,927.0 21 | 495.0,930.0 22 | 487.0,935.0 23 | 481.0,941.0 24 | 477.0,950.0 25 | 478.0,959.0 26 | 484.0,970.0 27 | 497.0,982.0 28 | 523.0,999.0 29 | 555.0,1015.0 30 | 589.0,1027.0 31 | 623.0,1036.0 32 | 657.0,1043.0 33 | 690.0,1048.0 34 | 725.0,1051.0 35 | 759.0,1053.0 36 | 790.0,1055.0 37 | 819.0,1056.0 38 | 843.0,1057.0 39 | 862.0,1057.0 40 | 875.0,1056.0 41 | 884.0,1055.0 42 | 891.0,1053.0 43 | 894.0,1052.0 44 | 895.0,1050.0 45 | 895.0,1048.0 46 | 894.0,1046.0 47 | 890.0,1043.0 48 | 881.0,1038.0 49 | 862.0,1033.0 50 | 834.0,1030.0 51 | 804.0,1030.0 52 | 767.0,1030.0 53 | 731.0,1031.0 54 | 688.0,1035.0 55 | 651.0,1040.0 56 | 613.0,1044.0 57 | 580.0,1050.0 58 | 553.0,1054.0 59 | 534.0,1057.0 60 | 518.0,1060.0 61 | 507.0,1062.0 62 | 498.0,1063.0 63 | 491.0,1064.0 64 | 486.0,1064.0 65 | 483.0,1064.0 66 | 481.0,1064.0 67 | 479.0,1064.0 68 | 478.0,1064.0 69 | 477.0,1064.0 70 | -------------------------------------------------------------------------------- /studywolf_control/controllers/signal.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2014 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | import numpy as np 19 | 20 | class Signal(object): 21 | """ 22 | The base class for classes generating signals to be 23 | used with to the outgoing control signal. 24 | """ 25 | def __init__(self): 26 | """ 27 | """ 28 | 29 | def generate(self, u, arm): 30 | """Generate the signal to add to the control signal. 31 | 32 | u np.array: the outgoing control signal 33 | arm Arm: the arm currently being controlled 34 | """ 35 | raise NotImplementedError 36 | -------------------------------------------------------------------------------- /studywolf_control/controllers/shell.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2014 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | 19 | class Shell(object): 20 | 21 | def __init__(self, controller, pen_down=False, **kwargs): 22 | """ 23 | control Control instance: the controller to use 24 | pen_down boolean: True if the end-effector is drawing 25 | """ 26 | 27 | self.controller = controller 28 | self.pen_down = pen_down 29 | self.kwargs = kwargs 30 | 31 | def control(self, arm): 32 | """Call the controllers control function. 33 | """ 34 | self.u = self.controller.control(arm, **self.kwargs) 35 | return self.u 36 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write_data/2.txt: -------------------------------------------------------------------------------- 1 | 642.0,507.0 2 | 643.0,507.0 3 | 646.0,507.0 4 | 650.0,508.0 5 | 655.0,510.0 6 | 662.0,513.0 7 | 670.0,518.0 8 | 680.0,524.0 9 | 690.0,531.0 10 | 702.0,539.0 11 | 714.0,549.0 12 | 727.0,562.0 13 | 741.0,581.0 14 | 754.0,599.0 15 | 767.0,622.0 16 | 778.0,646.0 17 | 786.0,668.0 18 | 791.0,690.0 19 | 793.0,714.0 20 | 791.0,733.0 21 | 787.0,751.0 22 | 781.0,770.0 23 | 773.0,788.0 24 | 764.0,802.0 25 | 753.0,814.0 26 | 737.0,826.0 27 | 716.0,837.0 28 | 689.0,846.0 29 | 661.0,851.0 30 | 633.0,852.0 31 | 602.0,848.0 32 | 576.0,841.0 33 | 551.0,830.0 34 | 526.0,817.0 35 | 504.0,803.0 36 | 484.0,787.0 37 | 466.0,768.0 38 | 448.0,748.0 39 | 431.0,727.0 40 | 414.0,703.0 41 | 400.0,682.0 42 | 388.0,665.0 43 | 375.0,647.0 44 | 363.0,629.0 45 | 353.0,612.0 46 | 345.0,601.0 47 | 339.0,591.0 48 | 334.0,583.0 49 | 330.0,577.0 50 | 326.0,572.0 51 | 323.0,568.0 52 | 321.0,566.0 53 | 319.0,564.0 54 | 318.0,563.0 55 | 317.0,562.0 56 | 316.0,562.0 57 | 315.0,562.0 58 | 315.0,563.0 59 | 315.0,567.0 60 | 315.0,575.0 61 | 316.0,587.0 62 | 317.0,602.0 63 | 317.0,616.0 64 | 317.0,630.0 65 | 317.0,651.0 66 | 317.0,678.0 67 | 317.0,700.0 68 | 317.0,739.0 69 | 318.0,768.0 70 | 320.0,806.0 71 | 321.0,833.0 72 | 323.0,864.0 73 | 324.0,894.0 74 | 325.0,916.0 75 | 325.0,945.0 76 | 325.0,968.0 77 | 324.0,989.0 78 | 324.0,1018.0 79 | 324.0,1041.0 80 | 324.0,1046.0 81 | -------------------------------------------------------------------------------- /studywolf_control/controllers/recorder.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | class Recorder(): 4 | """ 5 | A class for recording a signal and writing out at command, 6 | clearing the cache after writing out to a compressed file. 7 | """ 8 | def __init__(self, name, task, controller): 9 | """ 10 | name string: the name of the file to write out to 11 | """ 12 | 13 | self.name = name 14 | self.task = task 15 | self.controller = controller 16 | 17 | self.count = 0 18 | self.signal = [] 19 | 20 | self.start_recording = False 21 | self.write_out = False 22 | 23 | def record(self, t, x): 24 | """ 25 | The function to hook up to a node. 26 | 27 | t float: the time signal (ignored) 28 | x float, np.array: the signal to record 29 | """ 30 | 31 | if self.start_recording == True: 32 | self.signal.append(x.copy()) 33 | 34 | if self.write_out == True: 35 | print 'saving' 36 | np.savez_compressed( 37 | 'data/%s/%s/%s%.3i'%(self.task, \ 38 | self.controller, self.name, self.count), 39 | array1=np.asarray(self.signal)) 40 | self.count += 1 41 | self.signal = [] 42 | self.start_recording = False 43 | self.write_out = False 44 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write_data/9.txt: -------------------------------------------------------------------------------- 1 | 861.0,1255.0 2 | 861.0,1254.0 3 | 862.0,1252.0 4 | 864.0,1249.0 5 | 867.0,1244.0 6 | 871.0,1237.0 7 | 876.0,1226.0 8 | 882.0,1209.0 9 | 886.0,1191.0 10 | 888.0,1171.0 11 | 888.0,1149.0 12 | 886.0,1127.0 13 | 881.0,1108.0 14 | 874.0,1090.0 15 | 865.0,1071.0 16 | 854.0,1056.0 17 | 842.0,1044.0 18 | 825.0,1032.0 19 | 805.0,1022.0 20 | 783.0,1015.0 21 | 763.0,1011.0 22 | 744.0,1010.0 23 | 727.0,1013.0 24 | 715.0,1019.0 25 | 707.0,1026.0 26 | 699.0,1038.0 27 | 696.0,1058.0 28 | 699.0,1081.0 29 | 708.0,1101.0 30 | 722.0,1124.0 31 | 740.0,1148.0 32 | 758.0,1166.0 33 | 778.0,1183.0 34 | 796.0,1197.0 35 | 811.0,1208.0 36 | 823.0,1216.0 37 | 831.0,1221.0 38 | 837.0,1225.0 39 | 841.0,1227.0 40 | 844.0,1228.0 41 | 846.0,1229.0 42 | 847.0,1229.0 43 | 848.0,1229.0 44 | 848.0,1228.0 45 | 847.0,1227.0 46 | 846.0,1226.0 47 | 844.0,1225.0 48 | 841.0,1224.0 49 | 836.0,1224.0 50 | 827.0,1224.0 51 | 816.0,1224.0 52 | 801.0,1224.0 53 | 780.0,1223.0 54 | 754.0,1223.0 55 | 725.0,1222.0 56 | 691.0,1222.0 57 | 659.0,1221.0 58 | 623.0,1220.0 59 | 593.0,1220.0 60 | 563.0,1220.0 61 | 537.0,1220.0 62 | 514.0,1220.0 63 | 493.0,1220.0 64 | 474.0,1220.0 65 | 459.0,1220.0 66 | 447.0,1219.0 67 | 437.0,1218.0 68 | 429.0,1218.0 69 | 421.0,1218.0 70 | 414.0,1218.0 71 | 409.0,1218.0 72 | 405.0,1218.0 73 | 401.0,1218.0 74 | 398.0,1218.0 75 | 396.0,1218.0 76 | 394.0,1218.0 77 | 393.0,1218.0 78 | 392.0,1218.0 79 | 391.0,1218.0 80 | 390.0,1218.0 81 | 389.0,1218.0 82 | 388.0,1219.0 83 | 387.0,1220.0 84 | 386.0,1221.0 85 | -------------------------------------------------------------------------------- /studywolf_control/controllers/forcefield.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2014 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | from . import signal 19 | 20 | import numpy as np 21 | 22 | class Addition(signal.Signal): 23 | """This adds a forcefield as described in Shademehr 1994. 24 | """ 25 | def __init__(self, scale=1.0): 26 | """ 27 | """ 28 | self.force_matrix = np.array([[-10.1, -11.1, -10.1], 29 | [-11.2, 11.1, 10.1], 30 | [-11.2, 11.1, -10.1]]) 31 | self.force_matrix *= scale 32 | 33 | def generate(self, u, arm): 34 | """Generate the signal to add to the control signal. 35 | 36 | u np.array: the outgoing control signal 37 | arm Arm: the arm currently being controlled 38 | """ 39 | # calculate force to add 40 | force = np.dot(self.force_matrix[:arm.DOF, :arm.DOF], 41 | arm.dq) 42 | 43 | # translate to joint torques 44 | return force 45 | -------------------------------------------------------------------------------- /studywolf_control/arms/two_link/py2LinkArm.pyx: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | cimport numpy as np 3 | 4 | cdef extern from "twolinkarm.cpp": 5 | cdef cppclass Sim: 6 | Sim(double dt, double* params) 7 | void reset(double* out, double* ic) 8 | void step(double* out, double* u) 9 | 10 | cdef class pySim: 11 | cdef Sim* thisptr 12 | def __cinit__(self, double dt = .00001, 13 | np.ndarray[double, mode="c"] params=None): 14 | """ 15 | param float dt: simulation timestep, must be < 1e-5 16 | param array params: MapleSim model internal parameters 17 | """ 18 | if params: self.thisptr = new Sim(dt, ¶ms[0]) 19 | else: self.thisptr = new Sim(dt, NULL) 20 | 21 | def __dealloc__(self): 22 | del self.thisptr 23 | 24 | def reset(self, np.ndarray[double, mode="c"] out, 25 | np.ndarray[double, mode="c"] ic=None): 26 | """ 27 | Reset the state of the simulation. 28 | param np.ndarray out: where to store the system output 29 | NOTE: output is of form [time, output] 30 | param np.ndarray ic: the initial conditions of the system 31 | """ 32 | if ic is not None: self.thisptr.reset(&out[0], &ic[0]) 33 | else: self.thisptr.reset(&out[0], NULL) 34 | 35 | def step(self, np.ndarray[double, mode="c"] out, 36 | np.ndarray[double, mode="c"] u): 37 | """ 38 | Step the simulation forward one timestep. 39 | param np.ndarray out: where to store the system output 40 | NOTE: output is of form [time, output] 41 | param np.ndarray u: the control signal 42 | """ 43 | self.thisptr.step(&out[0], &u[0]) 44 | -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/py3LinkArm.pyx: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | cimport numpy as np 3 | 4 | cdef extern from "threelinkarm.cpp": 5 | cdef cppclass Sim: 6 | Sim(double dt, double* params) 7 | void reset(double* out, double* ic) 8 | void step(double* out, double* u) 9 | 10 | cdef class pySim: 11 | cdef Sim* thisptr 12 | def __cinit__(self, double dt = .00001, 13 | np.ndarray[double, mode="c"] params=None): 14 | """ 15 | param float dt: simulation timestep, must be < 1e-5 16 | param array params: MapleSim model internal parameters 17 | """ 18 | if params: self.thisptr = new Sim(dt, ¶ms[0]) 19 | else: self.thisptr = new Sim(dt, NULL) 20 | 21 | def __dealloc__(self): 22 | del self.thisptr 23 | 24 | def reset(self, np.ndarray[double, mode="c"] out, 25 | np.ndarray[double, mode="c"] ic=None): 26 | """ 27 | Reset the state of the simulation. 28 | param np.ndarray out: where to store the system output 29 | NOTE: output is of form [time, output] 30 | param np.ndarray ic: the initial conditions of the system 31 | """ 32 | if ic is not None: self.thisptr.reset(&out[0], &ic[0]) 33 | else: self.thisptr.reset(&out[0], NULL) 34 | 35 | def step(self, np.ndarray[double, mode="c"] out, 36 | np.ndarray[double, mode="c"] u): 37 | """ 38 | Step the simulation forward one timestep. 39 | param np.ndarray out: where to store the system output 40 | NOTE: output is of form [time, output] 41 | param np.ndarray u: the control signal 42 | """ 43 | self.thisptr.step(&out[0], &u[0]) 44 | -------------------------------------------------------------------------------- /studywolf_control/arms/one_link/py1LinkArm.pyx: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | cimport numpy as np 3 | 4 | cdef extern from "onelinkarm.cpp": 5 | cdef cppclass Sim: 6 | Sim(double dt, double* params) 7 | void reset(double* out, double* ic) 8 | void step(double* out, double* u) 9 | 10 | cdef class pySim: 11 | cdef Sim* thisptr 12 | def __cinit__(self, double dt = .00001, 13 | np.ndarray[double, mode="c"] params=None): 14 | """ 15 | param float dt: simulation timestep, must be < 1e-5 16 | param array params: MapleSim model internal parameters 17 | """ 18 | if params is not None: self.thisptr = new Sim(dt, ¶ms[0]) 19 | else: self.thisptr = new Sim(dt, NULL) 20 | 21 | def __dealloc__(self): 22 | del self.thisptr 23 | 24 | def reset(self, np.ndarray[double, mode="c"] out, 25 | np.ndarray[double, mode="c"] ic=None): 26 | """ 27 | Reset the state of the simulation. 28 | param np.ndarray out: where to store the system output 29 | NOTE: output is of form [time, output] 30 | param np.ndarray ic: the initial conditions of the system 31 | """ 32 | if ic is not None: self.thisptr.reset(&out[0], &ic[0]) 33 | else: self.thisptr.reset(&out[0], NULL) 34 | 35 | def step(self, np.ndarray[double, mode="c"] out, 36 | np.ndarray[double, mode="c"] u): 37 | """ 38 | Step the simulation forward one timestep. 39 | param np.ndarray out: where to store the system output 40 | NOTE: output is of form [time, output] 41 | param np.ndarray u: the control signal 42 | """ 43 | self.thisptr.step(&out[0], &u[0]) 44 | -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/py3LinkArm_damping.pyx: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | cimport numpy as np 3 | 4 | cdef extern from "threelinkarm_damping.cpp": 5 | cdef cppclass Sim: 6 | Sim(double dt, double* params) 7 | void reset(double* out, double* ic) 8 | void step(double* out, double* u) 9 | 10 | cdef class pySim: 11 | cdef Sim* thisptr 12 | def __cinit__(self, double dt = .00001, 13 | np.ndarray[double, mode="c"] params=None): 14 | """ 15 | param float dt: simulation timestep, must be < 1e-5 16 | param array params: MapleSim model internal parameters 17 | """ 18 | if params is not None: self.thisptr = new Sim(dt, ¶ms[0]) 19 | else: self.thisptr = new Sim(dt, NULL) 20 | 21 | def __dealloc__(self): 22 | del self.thisptr 23 | 24 | def reset(self, np.ndarray[double, mode="c"] out, 25 | np.ndarray[double, mode="c"] ic=None): 26 | """ 27 | Reset the state of the simulation. 28 | param np.ndarray out: where to store the system output 29 | NOTE: output is of form [time, output] 30 | param np.ndarray ic: the initial conditions of the system 31 | """ 32 | if ic is not None: self.thisptr.reset(&out[0], &ic[0]) 33 | else: self.thisptr.reset(&out[0], NULL) 34 | 35 | def step(self, np.ndarray[double, mode="c"] out, 36 | np.ndarray[double, mode="c"] u): 37 | """ 38 | Step the simulation forward one timestep. 39 | param np.ndarray out: where to store the system output 40 | NOTE: output is of form [time, output] 41 | param np.ndarray u: the control signal 42 | """ 43 | self.thisptr.step(&out[0], &u[0]) 44 | -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/py3LinkArm_gravity.pyx: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | cimport numpy as np 3 | 4 | cdef extern from "threelinkarm_gravity.cpp": 5 | cdef cppclass Sim: 6 | Sim(double dt, double* params) 7 | void reset(double* out, double* ic) 8 | void step(double* out, double* u) 9 | 10 | cdef class pySim: 11 | cdef Sim* thisptr 12 | def __cinit__(self, double dt = .00001, 13 | np.ndarray[double, mode="c"] params=None): 14 | """ 15 | param float dt: simulation timestep, must be < 1e-5 16 | param array params: MapleSim model internal parameters 17 | """ 18 | if params is not None: self.thisptr = new Sim(dt, ¶ms[0]) 19 | else: self.thisptr = new Sim(dt, NULL) 20 | 21 | def __dealloc__(self): 22 | del self.thisptr 23 | 24 | def reset(self, np.ndarray[double, mode="c"] out, 25 | np.ndarray[double, mode="c"] ic=None): 26 | """ 27 | Reset the state of the simulation. 28 | param np.ndarray out: where to store the system output 29 | NOTE: output is of form [time, output] 30 | param np.ndarray ic: the initial conditions of the system 31 | """ 32 | if ic is not None: self.thisptr.reset(&out[0], &ic[0]) 33 | else: self.thisptr.reset(&out[0], NULL) 34 | 35 | def step(self, np.ndarray[double, mode="c"] out, 36 | np.ndarray[double, mode="c"] u): 37 | """ 38 | Step the simulation forward one timestep. 39 | param np.ndarray out: where to store the system output 40 | NOTE: output is of form [time, output] 41 | param np.ndarray u: the control signal 42 | """ 43 | self.thisptr.step(&out[0], &u[0]) 44 | -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/py3LinkArm_smallmass.pyx: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | cimport numpy as np 3 | 4 | cdef extern from "threelinkarm_smallmass.cpp": 5 | cdef cppclass Sim: 6 | Sim(double dt, double* params) 7 | void reset(double* out, double* ic) 8 | void step(double* out, double* u) 9 | 10 | cdef class pySim: 11 | cdef Sim* thisptr 12 | def __cinit__(self, double dt = .00001, 13 | np.ndarray[double, mode="c"] params=None): 14 | """ 15 | param float dt: simulation timestep, must be < 1e-5 16 | param array params: MapleSim model internal parameters 17 | """ 18 | if params is not None: self.thisptr = new Sim(dt, ¶ms[0]) 19 | else: self.thisptr = new Sim(dt, NULL) 20 | 21 | def __dealloc__(self): 22 | del self.thisptr 23 | 24 | def reset(self, np.ndarray[double, mode="c"] out, 25 | np.ndarray[double, mode="c"] ic=None): 26 | """ 27 | Reset the state of the simulation. 28 | param np.ndarray out: where to store the system output 29 | NOTE: output is of form [time, output] 30 | param np.ndarray ic: the initial conditions of the system 31 | """ 32 | if ic is not None: self.thisptr.reset(&out[0], &ic[0]) 33 | else: self.thisptr.reset(&out[0], NULL) 34 | 35 | def step(self, np.ndarray[double, mode="c"] out, 36 | np.ndarray[double, mode="c"] u): 37 | """ 38 | Step the simulation forward one timestep. 39 | param np.ndarray out: where to store the system output 40 | NOTE: output is of form [time, output] 41 | param np.ndarray u: the control signal 42 | """ 43 | self.thisptr.step(&out[0], &u[0]) 44 | -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/py3LinkArm_gravity_damping.pyx: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | cimport numpy as np 3 | 4 | cdef extern from "threelinkarm_gravity_damping.cpp": 5 | cdef cppclass Sim: 6 | Sim(double dt, double* params) 7 | void reset(double* out, double* ic) 8 | void step(double* out, double* u) 9 | 10 | cdef class pySim: 11 | cdef Sim* thisptr 12 | def __cinit__(self, double dt = .00001, 13 | np.ndarray[double, mode="c"] params=None): 14 | """ 15 | param float dt: simulation timestep, must be < 1e-5 16 | param array params: MapleSim model internal parameters 17 | """ 18 | if params is not None: self.thisptr = new Sim(dt, ¶ms[0]) 19 | else: self.thisptr = new Sim(dt, NULL) 20 | 21 | def __dealloc__(self): 22 | del self.thisptr 23 | 24 | def reset(self, np.ndarray[double, mode="c"] out, 25 | np.ndarray[double, mode="c"] ic=None): 26 | """ 27 | Reset the state of the simulation. 28 | param np.ndarray out: where to store the system output 29 | NOTE: output is of form [time, output] 30 | param np.ndarray ic: the initial conditions of the system 31 | """ 32 | if ic is not None: self.thisptr.reset(&out[0], &ic[0]) 33 | else: self.thisptr.reset(&out[0], NULL) 34 | 35 | def step(self, np.ndarray[double, mode="c"] out, 36 | np.ndarray[double, mode="c"] u): 37 | """ 38 | Step the simulation forward one timestep. 39 | param np.ndarray out: where to store the system output 40 | NOTE: output is of form [time, output] 41 | param np.ndarray u: the control signal 42 | """ 43 | self.thisptr.step(&out[0], &u[0]) 44 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write_data/3.txt: -------------------------------------------------------------------------------- 1 | 747.0,785.0 2 | 748.0,785.0 3 | 751.0,786.0 4 | 755.0,789.0 5 | 761.0,794.0 6 | 768.0,801.0 7 | 777.0,811.0 8 | 786.0,823.0 9 | 796.0,838.0 10 | 805.0,856.0 11 | 812.0,872.0 12 | 816.0,887.0 13 | 819.0,903.0 14 | 819.0,920.0 15 | 817.0,935.0 16 | 813.0,949.0 17 | 807.0,962.0 18 | 798.0,975.0 19 | 787.0,987.0 20 | 774.0,998.0 21 | 756.0,1009.0 22 | 735.0,1018.0 23 | 714.0,1022.0 24 | 695.0,1022.0 25 | 676.0,1019.0 26 | 658.0,1014.0 27 | 643.0,1007.0 28 | 629.0,999.0 29 | 618.0,989.0 30 | 606.0,975.0 31 | 594.0,961.0 32 | 582.0,946.0 33 | 570.0,926.0 34 | 559.0,907.0 35 | 551.0,891.0 36 | 544.0,876.0 37 | 539.0,864.0 38 | 534.0,851.0 39 | 531.0,841.0 40 | 529.0,833.0 41 | 527.0,828.0 42 | 526.0,824.0 43 | 525.0,821.0 44 | 525.0,819.0 45 | 525.0,818.0 46 | 525.0,819.0 47 | 526.0,822.0 48 | 528.0,827.0 49 | 531.0,835.0 50 | 535.0,847.0 51 | 540.0,864.0 52 | 545.0,884.0 53 | 550.0,901.0 54 | 555.0,922.0 55 | 559.0,947.0 56 | 562.0,970.0 57 | 563.0,990.0 58 | 563.0,1012.0 59 | 560.0,1034.0 60 | 557.0,1049.0 61 | 552.0,1063.0 62 | 545.0,1078.0 63 | 536.0,1093.0 64 | 528.0,1103.0 65 | 521.0,1111.0 66 | 511.0,1118.0 67 | 499.0,1125.0 68 | 487.0,1129.0 69 | 475.0,1131.0 70 | 462.0,1131.0 71 | 450.0,1130.0 72 | 437.0,1126.0 73 | 426.0,1121.0 74 | 414.0,1113.0 75 | 403.0,1102.0 76 | 390.0,1089.0 77 | 377.0,1072.0 78 | 364.0,1049.0 79 | 352.0,1025.0 80 | 343.0,1007.0 81 | 334.0,982.0 82 | 327.0,958.0 83 | 322.0,940.0 84 | 319.0,920.0 85 | 318.0,896.0 86 | 318.0,877.0 87 | 319.0,860.0 88 | 321.0,843.0 89 | 324.0,825.0 90 | 327.0,812.0 91 | 330.0,801.0 92 | 333.0,793.0 93 | 336.0,785.0 94 | 339.0,778.0 95 | 343.0,771.0 96 | 347.0,764.0 97 | 352.0,757.0 98 | 361.0,749.0 99 | 365.0,745.0 100 | -------------------------------------------------------------------------------- /studywolf_control/controllers/control.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2014 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | import numpy as np 19 | 20 | class Control(object): 21 | """ 22 | The base class for controllers. 23 | """ 24 | def __init__(self, kp=10, kv=np.sqrt(10), 25 | additions=[], task='', write_to_file=False): 26 | """ 27 | additions list: list of Addition classes to append to 28 | the outgoing control signal 29 | kp float: the position error term gain value 30 | kv float: the velocity error term gain value 31 | """ 32 | 33 | self.u = np.zeros((2,1)) # control signal 34 | 35 | self.additions = additions 36 | self.kp = kp 37 | self.kv = kv 38 | self.task = task 39 | self.target = None 40 | 41 | self.write_to_file = write_to_file 42 | self.recorders = [] 43 | 44 | def check_distance(self, arm): 45 | """Checks the distance to target""" 46 | return np.sum(abs(arm.x - self.target)) + np.sum(abs(arm.dq)) 47 | 48 | def control(self): 49 | """Generates a control signal to apply to the arm""" 50 | raise NotImplementedError 51 | 52 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write_data/8.txt: -------------------------------------------------------------------------------- 1 | 882.0,1170.0 2 | 882.0,1169.0 3 | 883.0,1167.0 4 | 884.0,1163.0 5 | 885.0,1157.0 6 | 886.0,1147.0 7 | 887.0,1133.0 8 | 888.0,1115.0 9 | 888.0,1094.0 10 | 887.0,1069.0 11 | 885.0,1047.0 12 | 881.0,1023.0 13 | 876.0,999.0 14 | 870.0,980.0 15 | 864.0,964.0 16 | 858.0,951.0 17 | 851.0,939.0 18 | 844.0,929.0 19 | 838.0,923.0 20 | 830.0,918.0 21 | 821.0,915.0 22 | 812.0,915.0 23 | 803.0,917.0 24 | 794.0,922.0 25 | 784.0,933.0 26 | 774.0,949.0 27 | 764.0,970.0 28 | 755.0,997.0 29 | 748.0,1021.0 30 | 742.0,1046.0 31 | 738.0,1072.0 32 | 734.0,1093.0 33 | 730.0,1114.0 34 | 725.0,1136.0 35 | 720.0,1155.0 36 | 714.0,1170.0 37 | 707.0,1182.0 38 | 697.0,1195.0 39 | 685.0,1207.0 40 | 670.0,1219.0 41 | 650.0,1232.0 42 | 630.0,1241.0 43 | 611.0,1248.0 44 | 592.0,1251.0 45 | 574.0,1252.0 46 | 558.0,1251.0 47 | 545.0,1248.0 48 | 532.0,1242.0 49 | 519.0,1234.0 50 | 508.0,1225.0 51 | 497.0,1213.0 52 | 485.0,1194.0 53 | 475.0,1171.0 54 | 467.0,1148.0 55 | 460.0,1120.0 56 | 455.0,1095.0 57 | 454.0,1074.0 58 | 454.0,1051.0 59 | 457.0,1029.0 60 | 460.0,1013.0 61 | 466.0,997.0 62 | 473.0,980.0 63 | 481.0,967.0 64 | 489.0,957.0 65 | 501.0,948.0 66 | 516.0,940.0 67 | 532.0,935.0 68 | 548.0,932.0 69 | 565.0,932.0 70 | 583.0,932.0 71 | 599.0,934.0 72 | 612.0,937.0 73 | 624.0,941.0 74 | 634.0,948.0 75 | 643.0,955.0 76 | 653.0,964.0 77 | 662.0,973.0 78 | 672.0,983.0 79 | 683.0,996.0 80 | 693.0,1012.0 81 | 704.0,1032.0 82 | 712.0,1046.0 83 | 722.0,1062.0 84 | 733.0,1082.0 85 | 743.0,1100.0 86 | 753.0,1116.0 87 | 763.0,1130.0 88 | 774.0,1146.0 89 | 783.0,1158.0 90 | 792.0,1168.0 91 | 800.0,1175.0 92 | 809.0,1182.0 93 | 818.0,1186.0 94 | 826.0,1189.0 95 | 834.0,1189.0 96 | 841.0,1187.0 97 | 848.0,1184.0 98 | 854.0,1179.0 99 | 860.0,1173.0 100 | 867.0,1165.0 101 | 873.0,1157.0 102 | 878.0,1149.0 103 | 884.0,1139.0 104 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write_data/5.txt: -------------------------------------------------------------------------------- 1 | 821.0,1093.0 2 | 821.0,1091.0 3 | 821.0,1088.0 4 | 822.0,1083.0 5 | 822.0,1075.0 6 | 822.0,1062.0 7 | 821.0,1048.0 8 | 819.0,1032.0 9 | 816.0,1011.0 10 | 813.0,990.0 11 | 811.0,971.0 12 | 809.0,952.0 13 | 808.0,931.0 14 | 807.0,913.0 15 | 806.0,898.0 16 | 806.0,885.0 17 | 806.0,873.0 18 | 805.0,862.0 19 | 804.0,853.0 20 | 803.0,846.0 21 | 802.0,841.0 22 | 801.0,837.0 23 | 800.0,834.0 24 | 799.0,832.0 25 | 798.0,831.0 26 | 797.0,830.0 27 | 795.0,829.0 28 | 792.0,829.0 29 | 787.0,829.0 30 | 780.0,829.0 31 | 771.0,829.0 32 | 760.0,829.0 33 | 749.0,829.0 34 | 737.0,829.0 35 | 725.0,828.0 36 | 712.0,827.0 37 | 700.0,827.0 38 | 689.0,826.0 39 | 679.0,825.0 40 | 670.0,824.0 41 | 663.0,823.0 42 | 655.0,823.0 43 | 649.0,822.0 44 | 643.0,822.0 45 | 639.0,822.0 46 | 636.0,822.0 47 | 633.0,822.0 48 | 631.0,822.0 49 | 630.0,822.0 50 | 629.0,822.0 51 | 628.0,822.0 52 | 627.0,823.0 53 | 627.0,824.0 54 | 627.0,826.0 55 | 628.0,828.0 56 | 630.0,831.0 57 | 632.0,834.0 58 | 635.0,837.0 59 | 638.0,841.0 60 | 641.0,845.0 61 | 646.0,851.0 62 | 652.0,858.0 63 | 659.0,868.0 64 | 667.0,880.0 65 | 674.0,893.0 66 | 679.0,905.0 67 | 683.0,915.0 68 | 685.0,929.0 69 | 685.0,944.0 70 | 684.0,959.0 71 | 681.0,972.0 72 | 679.0,984.0 73 | 675.0,996.0 74 | 669.0,1013.0 75 | 663.0,1028.0 76 | 657.0,1040.0 77 | 651.0,1050.0 78 | 643.0,1059.0 79 | 633.0,1069.0 80 | 620.0,1079.0 81 | 606.0,1089.0 82 | 589.0,1096.0 83 | 570.0,1101.0 84 | 551.0,1102.0 85 | 534.0,1101.0 86 | 517.0,1098.0 87 | 499.0,1092.0 88 | 483.0,1085.0 89 | 469.0,1078.0 90 | 455.0,1069.0 91 | 442.0,1059.0 92 | 428.0,1043.0 93 | 415.0,1026.0 94 | 403.0,1007.0 95 | 393.0,985.0 96 | 386.0,956.0 97 | 382.0,937.0 98 | 380.0,913.0 99 | 380.0,889.0 100 | 382.0,870.0 101 | 385.0,850.0 102 | 390.0,829.0 103 | 396.0,811.0 104 | 402.0,797.0 105 | 409.0,782.0 106 | 416.0,769.0 107 | 423.0,756.0 108 | 430.0,746.0 109 | 437.0,738.0 110 | 448.0,730.0 111 | 460.0,725.0 112 | 471.0,722.0 113 | 502.0,718.0 114 | -------------------------------------------------------------------------------- /README.rst: -------------------------------------------------------------------------------- 1 | ============================================ 2 | StudyWolf Control repo 3 | ============================================ 4 | 5 | This is a repository to hold the code I've developed for simulating 6 | control systems performing different benchmarking tasks. The development 7 | and theory of the controllers are described at http://studywolf.com 8 | 9 | Installation 10 | ------------ 11 | 12 | The control directory requires that you have docopt installed:: 13 | 14 | pip install docopt 15 | 16 | Additionally, there are a number of arm models available, if you 17 | wish to use anything other than the 2 link arm coded in python, 18 | then you will have to compile the arm. You can compile the arms by 19 | going in to the arms/num_link/ folder and running setup:: 20 | 21 | python setup.py build_ext -i 22 | 23 | This will compile the arm for you into a shared object library that's 24 | accessible from Python. 25 | 26 | A final requirement is the pydmps library, which can be installed:: 27 | 28 | pip install pydmps 29 | 30 | NOTE: The arms have only been tested on linux and currently don't compile on Mac. 31 | 32 | Running 33 | ------- 34 | 35 | To run the basic control code, from the base directory:: 36 | 37 | python run.py ARM CONTROL TASK 38 | 39 | Where you can find the arm options in the Arm directory subfolders (arm1, arm1_python, arm2, arm2_python, arm2_python_todorov, arm3),the control types available are in the controllers subfolder (gc, osc, lqr, ilqr, dmp, trace), and the tasks are those listed in the Task directory (follow_mouse, postural, random_movements, reach, write). 40 | 41 | There are also a bunch of options, browse through the run.py header code to find them all! 42 | 43 | If you would like to use the PyGame visualization you must have PyGame installed. To call up the PyGame visualization append --use_pygame=True to the end of your call. 44 | 45 | Writing to file 46 | --------------- 47 | 48 | To write to file, run setting the write_to_file flag to True:: 49 | 50 | python run.py ARM CONTROL TASK --write_to_file=True 51 | 52 | NOTE: You must create a directory structure in the root folder 'data/arm{2,3}/task_name/controller_name'. 53 | -------------------------------------------------------------------------------- /studywolf_control/tasks/random_movements.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2014 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | import controllers.shell as shell 19 | import controllers.forcefield as forcefield 20 | 21 | import numpy as np 22 | 23 | def Task(arm, controller_class, 24 | force=None, write_to_file=False, **kwargs): 25 | """ 26 | This task sets up the arm to move to random 27 | target positions ever t_target seconds. 28 | """ 29 | 30 | # check controller type ------------------ 31 | controller_name = controller_class.__name__.split('.')[1] 32 | if controller_name not in ('gc', 'lqr', 'osc'): 33 | raise Exception('Cannot perform reaching task with this controller.') 34 | 35 | # set arm specific parameters ------------ 36 | if arm.DOF == 1: 37 | kp = 5 38 | elif arm.DOF == 2: 39 | kp = 10 40 | elif arm.DOF == 3: 41 | kp = 50 42 | 43 | # generate control shell ----------------- 44 | additions = [] 45 | if force is not None: 46 | print 'applying joint velocity based forcefield...' 47 | additions.append(forcefield.Addition(scale=force)) 48 | task = 'arm%i/forcefield'%arm.DOF 49 | 50 | controller = controller_class.Control( 51 | additions=additions, 52 | kp=kp, 53 | kv=np.sqrt(kp), 54 | task='arm%i/random'%arm.DOF, 55 | write_to_file=write_to_file) 56 | control_shell = shell.Shell(controller=controller) 57 | 58 | # generate runner parameters ----------- 59 | runner_pars = {'control_type':'random', 60 | 'title':'Task: Random movements'} 61 | 62 | return (control_shell, runner_pars) 63 | -------------------------------------------------------------------------------- /studywolf_control/tasks/follow_mouse.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2014 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | import controllers.shell as shell 19 | import controllers.forcefield as forcefield 20 | 21 | import numpy as np 22 | 23 | def Task(arm, controller_class, 24 | force=None, write_to_file=False, **kwargs): 25 | """ 26 | This task sets up the arm to follow the mouse 27 | with its end-effector. 28 | """ 29 | 30 | # check controller type ------------------ 31 | controller_name = controller_class.__name__.split('.')[1] 32 | if controller_name not in ('lqr', 'osc'): 33 | raise Exception('Cannot perform reaching task with this controller.') 34 | 35 | # set arm specific parameters ------------ 36 | if arm.DOF == 1: 37 | kp = 5 38 | elif arm.DOF == 2: 39 | kp = 20 40 | elif arm.DOF == 3: 41 | kp = 50 42 | 43 | # generate control shell ----------------- 44 | additions = [] 45 | if force is not None: 46 | print 'applying joint velocity based forcefield...' 47 | additions.append(forcefield.Addition(scale=force)) 48 | task = 'arm%i/forcefield'%arm.DOF 49 | 50 | controller = controller_class.Control( 51 | additions=additions, 52 | kp=kp, 53 | kv=np.sqrt(kp), 54 | task='arm%i/follow_mouse'%arm.DOF, 55 | write_to_file=write_to_file) 56 | control_shell = shell.Shell(controller=controller) 57 | 58 | # generate runner parameters ----------- 59 | runner_pars = {'control_type':'osc', 60 | 'title':'Task: Follow mouse', 61 | 'mouse_control':True} 62 | 63 | return (control_shell, runner_pars) 64 | 65 | -------------------------------------------------------------------------------- /studywolf_control/arms/two_link/arm_python.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2015 Travis DeWolf & Terry Stewart 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | from .arm2base import Arm2Base 19 | import numpy as np 20 | 21 | 22 | class Arm(Arm2Base): 23 | """ 24 | A class that holds the simulation and control dynamics for 25 | a two link arm, with the dynamics carried out in Python. 26 | """ 27 | def __init__(self, **kwargs): 28 | 29 | Arm2Base.__init__(self, **kwargs) 30 | 31 | # compute non changing constants 32 | self.K1 = ((1/3. * self.m1 + self.m2) * self.l1**2. + 33 | 1/3. * self.m2 * self.l2**2.) 34 | self.K2 = self.m2 * self.l1 * self.l2 35 | self.K3 = 1/3. * self.m2 * self.l2**2. 36 | self.K4 = 1/2. * self.m2 * self.l1 * self.l2 37 | 38 | self.reset() # set to init_q and init_dq 39 | 40 | def apply_torque(self, u, dt=None): 41 | """Takes in a torque and time step and updates the 42 | arm simulation accordingly. 43 | 44 | u np.array: the control signal to apply 45 | dt float: the time step 46 | """ 47 | if dt is None: 48 | dt = self.dt 49 | 50 | # equations solved for angles 51 | C2 = np.cos(self.q[1]) 52 | S2 = np.sin(self.q[1]) 53 | M11 = (self.K1 + self.K2*C2) 54 | M12 = (self.K3 + self.K4*C2) 55 | M21 = M12 56 | M22 = self.K3 57 | H1 = (-self.K2*S2*self.dq[0]*self.dq[1] - 58 | 1/2.0*self.K2*S2*self.dq[1]**2.0) 59 | H2 = 1./2.*self.K2*S2*self.dq[0]**2.0 60 | 61 | ddq1 = ((H2*M11 - H1*M21 - M11*u[1] + M21*u[0]) / 62 | (M12**2. - M11*M22)) 63 | ddq0 = (-H2 + u[1] - M22*ddq1) / M21 64 | self.dq += np.array([ddq0, ddq1]) * dt 65 | self.q += self.dq * dt 66 | 67 | # transfer to next time step 68 | self.t += dt 69 | -------------------------------------------------------------------------------- /studywolf_control/arms/two_link/arm_todorov.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2015 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | from arm2base import Arm2Base 19 | import numpy as np 20 | 21 | class Arm(Arm2Base): 22 | """ 23 | A class that holds the simulation and control dynamics for 24 | a two link arm, with the dynamics carried out in Python. 25 | This implementation is based off of the model described in 26 | 'Optimal control for nonlinear biological movement systems' 27 | by Weiwei Li. 28 | """ 29 | def __init__(self, **kwargs): 30 | 31 | Arm2Base.__init__(self, **kwargs) 32 | 33 | self.reset() # set to init_q and init_dq 34 | 35 | def apply_torque(self, u, dt=None): 36 | if dt is None: 37 | dt = self.dt 38 | 39 | tau = 0.04 # actuator time constant (sec) 40 | 41 | # arm model parameters 42 | m = np.array([1.4, 1.1]) # segment mass 43 | l = np.array([0.3, 0.33]) # segment length 44 | s = np.array([0.11, 0.16]) # segment center of mass 45 | i = np.array([0.025, 0.045]) # segment moment of inertia 46 | 47 | # inertia matrix 48 | a1 = i[0] + i[1] + m[1]*l[0]**2 49 | a2 = m[1]*l[0]*s[1] 50 | a3 = i[1] 51 | I = np.array([[a1 + 2*a2*np.cos(q[1]), a3 + a2*np.cos(q[1])], 52 | [a3 + a2*np.cos(q[1]), a3]]) 53 | 54 | # centripital and Coriolis effects 55 | C = np.array([[-dq[1] * (2 * dq[0] + dq[1])], 56 | [dq[0]]]) * a2 * np.sin(q[1]) 57 | 58 | # joint friction 59 | B = np.array([[.05, .025], 60 | [.025, .05]]) 61 | 62 | # calculate forward dynamics 63 | ddq = np.linalg.pinv(I) * (u - C - np.dot(B, dq)) 64 | 65 | # transfer to next time step 66 | self.q += dt * self.dq 67 | self.dq += dt * ddq 68 | 69 | self.t += dt 70 | -------------------------------------------------------------------------------- /studywolf_control/arms/two_link/arm.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2015 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | from .arm2base import Arm2Base 19 | from . import py2LinkArm 20 | 21 | import numpy as np 22 | 23 | 24 | class Arm(Arm2Base): 25 | """A wrapper around a MapleSim generated C simulation 26 | of a two link arm.""" 27 | 28 | def __init__(self, **kwargs): 29 | 30 | Arm2Base.__init__(self, **kwargs) 31 | 32 | # stores information returned from maplesim 33 | self.state = np.zeros(7) 34 | # maplesim arm simulation 35 | self.sim = py2LinkArm.pySim(dt=1e-5) 36 | self.sim.reset(self.state) 37 | self.reset() # set to init_q and init_dq 38 | 39 | def apply_torque(self, u, dt=None): 40 | """Takes in a torque and timestep and updates the 41 | arm simulation accordingly. 42 | 43 | u np.array: the control signal to apply 44 | dt float: the timestep 45 | """ 46 | if dt is None: 47 | dt = self.dt 48 | u = np.array(u, dtype='float') 49 | 50 | for i in range(int(np.ceil(dt/1e-5))): 51 | self.sim.step(self.state, u) 52 | self.update_state() 53 | 54 | def reset(self, q=[], dq=[]): 55 | if isinstance(q, np.ndarray): 56 | q = q.tolist() 57 | if isinstance(dq, np.ndarray): 58 | dq = dq.tolist() 59 | 60 | if q: 61 | assert len(q) == self.DOF 62 | if dq: 63 | assert len(dq) == self.DOF 64 | 65 | state = np.zeros(self.DOF*2) 66 | state[::2] = self.init_q if not q else np.copy(q) 67 | state[1::2] = self.init_dq if not dq else np.copy(dq) 68 | 69 | self.sim.reset(self.state, state) 70 | self.update_state() 71 | 72 | def update_state(self): 73 | """Separate out the state variable into time, angles, 74 | velocities, and accelerations.""" 75 | 76 | self.t = self.state[0] 77 | self.q = self.state[1:3] 78 | self.dq = self.state[3:5] 79 | self.ddq = self.state[5:] 80 | -------------------------------------------------------------------------------- /studywolf_control/run.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2015 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | 17 | Usage: 18 | run ARM CONTROLLER TASK [options] 19 | 20 | Arguments: 21 | ARM the arm to control 22 | CONTROLLER the controller to use 23 | TASK the task to perform 24 | 25 | Options: 26 | --dt=DT specify float step size for simulations 27 | --end_time=ENDTIME specify float time to end (seconds) 28 | default is None 29 | --force=FORCEPARS specify float strength of force to add 30 | default is None 31 | --sequence=PHRASEPARS specify sequence details for a given TASK 32 | --scale=SCALEPARS specify the scale of the DMP if TASK=write 33 | --write_to_file=WRITEPARS specify boolean for writing to file, 34 | default is False 35 | ''' 36 | 37 | from sim_and_plot import Runner 38 | 39 | from docopt import docopt 40 | import importlib 41 | 42 | args = docopt(__doc__) 43 | 44 | dt = 1e-2 if args['CONTROLLER'] == 'ilqr' else 1e-3 45 | dt = dt if args.get('--dt', None) is None else float(args['--dt']) 46 | 47 | # get and initialize the arm 48 | if args['ARM'][:4] == 'arm1': 49 | subfolder = 'one_link' 50 | if args['ARM'][:4] == 'arm2': 51 | subfolder = 'two_link' 52 | elif args['ARM'][:4] == 'arm3': 53 | subfolder = 'three_link' 54 | arm_name = 'arms.%s.%s' % (subfolder, 'arm'+args['ARM'][4:]) 55 | arm_module = importlib.import_module(name=arm_name) 56 | arm = arm_module.Arm(dt=dt) 57 | 58 | # get the chosen controller class 59 | controller_name = 'controllers.%s' % args['CONTROLLER'].lower() 60 | controller_class = importlib.import_module(name=controller_name) 61 | 62 | # get the chosen task class 63 | task_name = 'tasks.%s' % args['TASK'] 64 | task_module = importlib.import_module(name=task_name) 65 | print('task: ', task_module) 66 | task = task_module.Task 67 | 68 | # instantiate the controller for the chosen task 69 | # and get the sim_and_plot parameters 70 | control_shell, runner_pars = task( 71 | arm, controller_class, 72 | sequence=args['--sequence'], scale=args['--scale'], 73 | force=float(args['--force']) if args['--force'] is not None else None, 74 | write_to_file=bool(args['--write_to_file'])) 75 | 76 | # set up simulate and plot system 77 | runner = Runner(dt=dt, **runner_pars) 78 | runner.run(arm=arm, control_shell=control_shell, 79 | end_time=(float(args['--end_time']) 80 | if args['--end_time'] is not None else None)) 81 | runner.show() 82 | -------------------------------------------------------------------------------- /studywolf_control/controllers/gc.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2013 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | from . import control 19 | 20 | import numpy as np 21 | 22 | class Control(control.Control): 23 | """ 24 | A class that holds the simulation and control dynamics for 25 | a two link arm, with the dynamics carried out in Python. 26 | """ 27 | def __init__(self, **kwargs): 28 | 29 | super(Control, self).__init__(**kwargs) 30 | 31 | # generalized coordinates 32 | self.target_gain = 2*np.pi 33 | self.target_bias = -np.pi 34 | 35 | if self.write_to_file is True: 36 | from controllers.recorder import Recorder 37 | # set up recorders 38 | self.u_recorder = Recorder('control signal', self.task, 'gc') 39 | self.xy_recorder = Recorder('end-effector position', self.task, 'gc') 40 | self.dist_recorder = Recorder('distance from target', self.task, 'gc') 41 | self.recorders = [self.u_recorder, 42 | self.xy_recorder, 43 | self.dist_recorder] 44 | 45 | def check_distance(self, arm): 46 | """Checks the distance to target""" 47 | return np.sum(abs(arm.q - self.target)) 48 | 49 | def control(self, arm, q_des=None): 50 | """Generate a control signal to move the arm through 51 | joint space to the desired joint angle position""" 52 | 53 | # calculated desired joint angle acceleration 54 | if q_des is None: 55 | prop_val = ((self.target.reshape(1,-1) - arm.q) + np.pi) % \ 56 | (np.pi*2) - np.pi 57 | else: 58 | # if a desired location is specified on input 59 | prop_val = q_des - arm.q 60 | 61 | # add in velocity compensation 62 | q_des = (self.kp * prop_val + \ 63 | self.kv * -arm.dq).reshape(-1,) 64 | 65 | Mq = arm.gen_Mq() 66 | 67 | # tau = Mq * q_des + tau_grav, but gravity = 0 68 | self.u = np.dot(Mq, q_des).reshape(-1,) 69 | 70 | if self.write_to_file is True: 71 | # feed recorders their signals 72 | self.u_recorder.record(0.0, self.U) 73 | self.xy_recorder.record(0.0, self.arm.x) 74 | self.dist_recorder.record(0.0, self.target - self.arm.x) 75 | 76 | return self.u 77 | 78 | def gen_target(self, arm): 79 | """Generate a random target""" 80 | self.target = np.random.random(size=arm.DOF,) * \ 81 | self.target_gain + self.target_bias 82 | 83 | return self.target 84 | -------------------------------------------------------------------------------- /studywolf_control/controllers/trace.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2014 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | from . import trajectory 19 | 20 | import numpy as np 21 | 22 | class Shell(trajectory.Shell): 23 | """ 24 | A controller that uses a given trajectory to 25 | control a robotic arm end-effector. 26 | """ 27 | 28 | def __init__(self, **kwargs): 29 | """ 30 | """ 31 | self.time = 0.0 32 | super(Shell, self).__init__(**kwargs) 33 | 34 | def check_pen_up(self): 35 | """Check to see if the pen should be lifted. 36 | """ 37 | if self.time >= 1. - self.tau: 38 | self.time = 0.0 39 | return True 40 | else: 41 | return False 42 | 43 | def gen_path(self, trajectory): 44 | """Generates the trajectories for the 45 | position, velocity, and acceleration to follow 46 | during run time to reproduce the given trajectory. 47 | 48 | trajectory np.array: a list of points to follow 49 | """ 50 | 51 | if trajectory.ndim == 1: 52 | trajectory = trajectory.reshape(1,len(trajectory)) 53 | dt = 1.0 / trajectory.shape[1] 54 | 55 | # break up the trajectory into its different words 56 | # NaN or None signals a new word / break in drawing 57 | breaks = np.where(trajectory != trajectory) 58 | # some vector manipulation to get what we want 59 | breaks = breaks[1][:len(breaks[1])/2] 60 | self.num_seqs = len(breaks) - 1 61 | 62 | import scipy.interpolate 63 | self.seqs_x = [] 64 | self.seqs_y = [] 65 | for ii in range(self.num_seqs): 66 | # get the ii'th sequence 67 | seq_x = trajectory[0, breaks[ii]+1:breaks[ii+1]] 68 | seq_y = trajectory[1, breaks[ii]+1:breaks[ii+1]] 69 | 70 | # generate function to interpolate the desired trajectory 71 | vals = np.linspace(0, 1, len(seq_x)) 72 | self.seqs_x.append(scipy.interpolate.interp1d(vals, seq_x)) 73 | self.seqs_y.append(scipy.interpolate.interp1d(vals, seq_y)) 74 | 75 | self.trajectory = [self.seqs_x[0], self.seqs_y[0]] 76 | 77 | def set_next_seq(self): 78 | """Get the next sequence in the list. 79 | """ 80 | self.trajectory = [self.seqs_x[self.num_seq], 81 | self.seqs_y[self.num_seq]] 82 | 83 | def set_target(self): 84 | """Get the next target in the sequence. 85 | """ 86 | self.controller.target = np.array([self.trajectory[d](self.time) \ 87 | for d in range(2)]) 88 | if self.time < 1.: 89 | self.time += self.tau 90 | -------------------------------------------------------------------------------- /studywolf_control/tasks/reach.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2015 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | import controllers.target_list as target_list 19 | import controllers.forcefield as forcefield 20 | 21 | import numpy as np 22 | 23 | def Task(arm, controller_class, x_bias=0., y_bias=2., dist=.4, 24 | force=None, write_to_file=False, sequence=None, **kwargs): 25 | """ 26 | This task sets up the arm to reach to 8 targets center out from 27 | (x_bias, y_bias) at a distance=dist. 28 | """ 29 | 30 | # check controller type ------------------ 31 | controller_name = controller_class.__name__.split('.')[1] 32 | if controller_name not in ('ilqr', 'lqr', 'osc', 'gradient_approximation', 'ahf'): 33 | raise Exception('Cannot perform reaching task with this controller.') 34 | 35 | # set arm specific parameters ------------ 36 | if arm.DOF == 2: 37 | dist = .075 38 | kp = 20 # position error gain on the PD controller 39 | threshold = .01 40 | y_bias = .35 41 | elif arm.DOF == 3: 42 | kp = 100 43 | threshold = .02 44 | else: 45 | raise Exception('Cannot perform reaching task with this arm.') 46 | 47 | # generate the path to follow ------------- 48 | # set up the reaching trajectories, 8 points around unit circle 49 | targets_x = [dist * np.cos(theta) + x_bias \ 50 | for theta in np.linspace(0, np.pi*2, 9)][:-1] 51 | targets_y = [dist * np.sin(theta) + y_bias \ 52 | for theta in np.linspace(0, np.pi*2, 9)][:-1] 53 | trajectory = np.ones((3*len(targets_x)+3, 2))*np.nan 54 | 55 | start = 0 if sequence is None else int(sequence) 56 | for ii in range(start,len(targets_x)): 57 | trajectory[ii*3+1] = [0, y_bias] 58 | trajectory[ii*3+2] = [targets_x[ii], targets_y[ii]] 59 | trajectory[-2] = [0, y_bias] 60 | 61 | # generate control shell ----------------- 62 | additions = [] 63 | if force is not None: 64 | print('applying joint velocity based forcefield...') 65 | additions.append(forcefield.Addition(scale=force)) 66 | task = 'arm%i/forcefield'%arm.DOF 67 | 68 | controller = controller_class.Control( 69 | additions=additions, 70 | kp=kp, 71 | kv=np.sqrt(kp), 72 | task='arm%i/reach'%arm.DOF, 73 | write_to_file=write_to_file) 74 | 75 | control_pars = {'target_list':trajectory, 76 | 'threshold':threshold} # how close to get to each target} 77 | control_shell = target_list.Shell(controller=controller, **control_pars) 78 | 79 | # generate runner parameters ----------- 80 | runner_pars = {'infinite_trail':True, 81 | 'title':'Task: Reaching', 82 | 'trajectory':trajectory} 83 | 84 | return (control_shell, runner_pars) 85 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write_data/read_path.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def get_raw_data(input_name, writebox, spaces=False): 4 | 5 | f = open('tasks/write_data/'+input_name+'.txt', 'r') 6 | row = f.readline() 7 | 8 | points = [] 9 | for row in f: 10 | points.append(row.strip('\n').split(',')) 11 | f.close() 12 | 13 | points = np.array(points, dtype='float') 14 | points = points[:, :2] 15 | 16 | # need to rotate the points 17 | theta = np.pi/2. 18 | R = np.array([[np.cos(theta), -np.sin(theta)], 19 | [np.sin(theta), np.cos(theta)]]) 20 | 21 | for ii in range(points.shape[0]): 22 | points[ii] = np.dot(R, points[ii]) 23 | 24 | # need to mirror the x values 25 | for ii in range(points.shape[0]): 26 | points[ii, 0] *= -1 27 | 28 | # center numbers 29 | points[:, 0] -= np.min(points[:,0]) 30 | points[:, 1] -= np.min(points[:,1]) 31 | 32 | # normalize 33 | # TODO: solve weird scaling for 1 and l, and 9 34 | points[:, 0] /= max(points[:,0]) 35 | points[:, 1] /= max(points[:,1]) 36 | 37 | # center numbers 38 | points[:, 0] -= .5 - (max(points[:,0]) - min(points[:, 0])) / 2.0 39 | 40 | points[:,0] *= 5.0 / 6.0 * (writebox[1] - writebox[0]) 41 | points[:,1] *= (writebox[3] - writebox[2]) 42 | 43 | if input_name in ('1'): 44 | points[:, 0] /= 15. 45 | if input_name in ('s'): 46 | points[:, 0] /= 5. 47 | if input_name in ('9'): 48 | points[:, 0] /= 2. 49 | if input_name in ('e','o','w','r'): 50 | points[:, 1] /= 2. 51 | 52 | points[:,0] += writebox[0] 53 | points[:,1] += writebox[2] 54 | 55 | return points 56 | 57 | def get_single(**kwargs): 58 | """Wrap the number with np.nans on either end 59 | """ 60 | 61 | num = get_raw_data(**kwargs) 62 | new_array = np.zeros((num.shape[0]+2, num.shape[1])) 63 | new_array[0] = [np.nan, np.nan] 64 | new_array[-1] = [np.nan, np.nan] 65 | new_array[1:-1] = num 66 | 67 | return new_array 68 | 69 | def get_sequence(sequence, writebox, spaces=False): 70 | """Returns a sequence 71 | 72 | sequence list: the sequence of integers 73 | writebox list: [min x, max x, min y, max y] 74 | """ 75 | 76 | nans = np.array([np.nan, np.nan]) 77 | nums= nans.copy() 78 | 79 | if spaces is False: 80 | each_num_width = (writebox[1] - writebox[0]) / float(len(sequence)) 81 | else: 82 | each_num_width = (writebox[1] - writebox[0]) / float(len(sequence)*2 - 1) 83 | 84 | for ii, nn in enumerate(sequence): 85 | 86 | if spaces is False: 87 | num_writebox = [writebox[0] + each_num_width * ii , 88 | writebox[0] + each_num_width * (ii+1), 89 | writebox[2], writebox[3]] 90 | else: 91 | num_writebox = [writebox[0] + each_num_width * 2 * ii , 92 | writebox[0] + each_num_width * 2 * (ii+.5), 93 | writebox[2], writebox[3]] 94 | if isinstance(nn, int): 95 | nn = str(nn) 96 | num = get_raw_data(nn, num_writebox) 97 | nums = np.vstack([nums, num, nans]) 98 | 99 | return nums 100 | 101 | 102 | ### Testing code ### 103 | if __name__ == '__main__': 104 | 105 | import matplotlib.pyplot as plt 106 | 107 | files=['h','e','l','l','o','w','o','r','l','d'] 108 | nums = get_sequence(files, writebox=[-1,1,0,1], spaces=False) 109 | plt.plot(nums[:,0], nums[:,1]) 110 | plt.show() 111 | 112 | -------------------------------------------------------------------------------- /studywolf_control/arms/two_link/config.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import nengo 3 | from nengo.dists import Choice 4 | 5 | 6 | class OSCConfig(object): 7 | """A class for storing all the specific 8 | configuration parameters for the neural implementation 9 | of OSC""" 10 | 11 | # OSC ------------------------------------------------------------------- # 12 | 13 | n_neurons = 2000 14 | CB = { 15 | 'dimensions': 4, 16 | 'max_rates': np.random.uniform(low=10, high=200, size=n_neurons), 17 | 'n_neurons': n_neurons, 18 | # 'neuron_type': nengo.Direct(), 19 | 'radius': 5, 20 | } 21 | 22 | n_neurons = 2500 23 | M1 = { 24 | 'dimensions': 4, 25 | 'max_rates': np.random.uniform(low=00, high=100, size=n_neurons), 26 | 'n_neurons': n_neurons, 27 | 'neuron_type': nengo.Direct(), 28 | 'radius': .25, 29 | } 30 | 31 | n_neurons = 2500 32 | M1_mult = { 33 | 'encoders': Choice([[1,1],[-1,1],[-1,-1],[1,-1]]), 34 | 'ens_dimensions': 2, 35 | 'n_ensembles': 4, 36 | 'n_neurons': n_neurons, 37 | 'neuron_type': nengo.Direct(), 38 | 'radius': np.sqrt(2), 39 | } 40 | 41 | # DMPs ------------------------------------------------------------------ # 42 | 43 | oscillator = { 44 | 'dimensions': 2, 45 | 'n_neurons': 500, 46 | # 'neuron_type': nengo.Direct(), 47 | 'radius': .01, 48 | } 49 | 50 | forcing_func = { 51 | 'dimensions': 2, 52 | 'n_neurons': 2000, 53 | } 54 | 55 | y = { 56 | 'dimensions': 1, 57 | 'n_neurons': 1000, 58 | # 'neuron_type': nengo.Direct(), 59 | 'radius': 5, 60 | } 61 | 62 | 63 | target_ybias = 2.0 64 | 65 | # ----------------------------------------------------------------------- # 66 | CBmean = np.array([ 0.74859865, 1.83029154, 0.01458004, -0.02742442 ]) 67 | CBscale = np.array([ 0.49086854, 0.65507996, 2.18998496, 3.09952941]) 68 | 69 | def CB_scaledown(self, x): 70 | return (x - self.CBmean) / self.CBscale 71 | 72 | def CB_scaleup(self, x): 73 | return x * self.CBscale + self.CBmean 74 | 75 | # ----------------------------------------------------------------------- # 76 | M1mean = np.array([ 0.67332909, 0.52630158, 0.72467918, -0.83424157]) 77 | M1scale = np.array([ 0.36273571, 0.48462947, 0.323381, 0.29300704]) 78 | 79 | def M1_scaledown(self, x): 80 | return (x - self.M1mean) / self.M1scale 81 | 82 | def M1_scaleup(self, x): 83 | return x * self.M1scale + self.M1mean 84 | 85 | # ----------------------------------------------------------------------- # 86 | u_scaling = np.array([1., 1.]) 87 | DPmean = np.array([-66.13644345, 5.93401211, -22.82797318, -21.08624692]) 88 | DPscale = np.array([ 29.21381785, 30.42525345, 12.93365678, 9.4640815 ]) 89 | 90 | def DP_scaledown(self, x): 91 | return (x - self.DPmean) / self.DPscale 92 | 93 | def DP_scaleup(self, x, index): 94 | return self.u_scaling[index%2] * x[0] * \ 95 | (x[1] * self.DPscale[index] + self.DPmean[index]) 96 | 97 | def __init__(self, adaptation): 98 | 99 | if adaptation == 'kinematic': 100 | self.DPmean *= 2.0 101 | 102 | self.DP_scaleup_list = [ 103 | lambda x: self.DP_scaleup(x, 0), 104 | lambda x: self.DP_scaleup(x, 1), 105 | lambda x: self.DP_scaleup(x, 2), 106 | lambda x: self.DP_scaleup(x, 3)] 107 | -------------------------------------------------------------------------------- /studywolf_control/arms/one_link/arm_python.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2015 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | from ..Arm import Arm 19 | import numpy as np 20 | 21 | class Arm1Link(Arm): 22 | """ 23 | A class that holds the simulation and control dynamics for 24 | a two link arm, with the dynamics carried out in Python. 25 | """ 26 | def __init__(self, dt=1e-2, l1=.31, **kwargs): 27 | self.DOF = 1 28 | Arm.__init__(self, **kwargs) 29 | 30 | self.dt = dt # timestep 31 | 32 | # length of arm links 33 | self.l1=l1 34 | self.L = np.array([self.l1]) 35 | # mass of links 36 | self.m1=1.98 37 | # z axis inertia moment of links 38 | izz1=15 39 | # create mass matrices at COM for each link 40 | self.M1 = np.zeros((6,6)) 41 | self.M1[0:3,0:3] = np.eye(3)*self.m1 42 | 43 | # initial arm joint and end-effector position 44 | self.q = np.array([0.0]) # matching the C simulation 45 | # initial arm joint and end-effector velocity 46 | self.dq = np.zeros(1) 47 | # initial arm joint and end-effector acceleration 48 | self.ddq = np.zeros(1) 49 | 50 | self.t = 0.0 51 | 52 | def apply_torque(self, u, dt=None): 53 | if dt is None: 54 | dt = self.dt 55 | 56 | # equations solved for angles 57 | self.ddq = 1.0 / (self.m1 * self.l1**2) * u 58 | self.dq += self.ddq * self.dt 59 | self.q += self.dq * self.dt 60 | 61 | def gen_jacCOM1(self, q=None): 62 | """Generates the Jacobian from the COM of the first 63 | link to the origin frame""" 64 | if q is None: 65 | q = self.q 66 | q0 = q[0] 67 | 68 | JCOM1 = np.zeros((6,1)) 69 | JCOM1[0,0] = self.l1 * -np.sin(q0) 70 | JCOM1[1,0] = self.l1 * np.cos(q0) 71 | JCOM1[5,0] = 1.0 72 | 73 | return JCOM1 74 | 75 | def gen_jacEE(self, q=None): 76 | """Generates the Jacobian from end-effector to 77 | the origin frame""" 78 | if q is None: 79 | q = self.q 80 | q0 = q[0] 81 | 82 | JEE = np.zeros((2,1)) 83 | # define column entries right to left 84 | JEE[0,0] = self.l1 * -np.sin(q0) 85 | JEE[1,0] = self.l1 * np.cos(q0) 86 | 87 | return JEE 88 | 89 | def gen_Mq(self, q=None): 90 | """Generates the mass matrix for the arm in joint space""" 91 | 92 | # get the instantaneous Jacobians 93 | JCOM1 = self.gen_jacCOM1(q=q) 94 | # generate the mass matrix in joint space 95 | Mq = np.dot(JCOM1.T, np.dot(self.M1, JCOM1)) 96 | 97 | return Mq 98 | 99 | def position(self, q=None): 100 | """Compute x,y position of the hand""" 101 | if q is None: q0 = self.q[0] 102 | else: q0 = q[0] 103 | 104 | x = np.cumsum([0, 105 | self.l1 * np.cos(q0)]) 106 | y = np.cumsum([0, 107 | self.l1 * np.sin(q0)]) 108 | 109 | return np.array([x, y]) 110 | -------------------------------------------------------------------------------- /studywolf_control/arms/two_link/arm_python_todorov.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2015 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | from arm2base import Arm2Base 19 | import numpy as np 20 | 21 | class Arm(Arm2Base): 22 | """ 23 | A class that holds the simulation and control dynamics for 24 | a two link arm, with the dynamics carried out in Python. 25 | """ 26 | def __init__(self, **kwargs): 27 | 28 | Arm2Base.__init__(self, **kwargs) 29 | 30 | # arm model parameters 31 | self.m1 = 1.4 # segment mass 32 | self.m2 = 1.1 33 | s1 = 0.11 # segment center of mass 34 | s2 = 0.16 35 | i1 = 0.025 # segment moment of inertia 36 | i2 = 0.045 37 | b11 = b22 = b12 = b21 = 0.0 38 | # b11_ = 0.7 # joint friction 39 | # b22_ = 0.8 40 | # b12_ = 0.08 41 | # b21_ = 0.08 42 | 43 | tau = 0.04 # actuator time constant (sec) 44 | 45 | self.reset() # set to init_q and init_dq 46 | 47 | def apply_torque(self, u, dt=None): 48 | if dt is None: 49 | dt = self.dt 50 | 51 | tau = 0.04 # actuator time constant (sec) 52 | 53 | # arm model parameters 54 | m1_ = 1.4 # segment mass 55 | m2_ = 1.1 56 | l1_ = self.l1 # segment length 57 | l2_ = self.l2 58 | s1_ = 0.11 # segment center of mass 59 | s2_ = 0.16 60 | i1_ = 0.025 # segment moment of inertia 61 | i2_ = 0.045 62 | b11_ = b22_ = b12_ = b21_ = 0.0 63 | # b11_ = 0.7 # joint friction 64 | # b22_ = 0.8 65 | # b12_ = 0.08 66 | # b21_ = 0.08 67 | 68 | #------------------------ compute inertia I and extra torque H -------- 69 | # temp vars 70 | mls = m2_* l1_*s2_ 71 | iml = i1_ + i2_ + m2_*l1_**2 72 | dd = i2_ * iml - i2_**2 73 | sy = np.sin(self.q[1]) 74 | cy = np.cos(self.q[1]) 75 | 76 | # inertia 77 | I_11 = iml + 2 * mls * cy 78 | I_12 = i2_ + mls * cy 79 | I_22 = i2_ * np.ones_like(cy) 80 | 81 | # determinant 82 | det = dd - mls**2 * cy**2 83 | 84 | # inverse inertia I1 85 | I1_11 = i2_ / det 86 | I1_12 = (-i2_ - mls * cy) / det 87 | I1_22 = (iml + 2 * mls * cy) / det 88 | 89 | # temp vars 90 | sw = np.sin(self.q[1]) 91 | cw = np.cos(self.q[1]) 92 | y = self.dq[0] 93 | z = self.dq[1] 94 | 95 | # extra torque H (Coriolis, centripetal, friction) 96 | H = np.array([-mls * (2 * y + z) * z * sw + b11_ * y + b12_ * z, 97 | mls * y**2 * sw + b22_ * z + b12_ * y]) 98 | 99 | #------------- compute xdot = inv(I) * (torque - H) ------------ 100 | torque = u.T - H 101 | 102 | self.q += dt * self.dq 103 | self.dq += dt * np.array([(I1_11 * torque[0] + I1_12 * torque[1]), 104 | (I1_12 * torque[0] + I1_22 * torque[1])]) 105 | 106 | # transfer to next time step 107 | self.t += dt 108 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2015 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | import controllers.osc as osc 19 | import controllers.forcefield as forcefield 20 | 21 | import tasks.write_data.read_path as rp 22 | 23 | import numpy as np 24 | 25 | 26 | def Task(arm, controller_class, sequence=None, scale=None, 27 | force=None, write_to_file=False, **kwargs): 28 | """ 29 | This task sets up the arm to write numbers inside 30 | a specified area (-x_bias, x_bias, -y_bias, y_bias). 31 | """ 32 | 33 | # check controller type ------------------ 34 | controller_name = controller_class.__name__.split('.')[1] 35 | if controller_name not in ('dmp', 'trace'): 36 | raise Exception('Cannot perform reaching task with this controller.') 37 | 38 | # set arm specific parameters ------------ 39 | if arm.DOF == 2: 40 | kp = 20 # position error gain on the PD controller 41 | threshold = .01 42 | writebox = np.array([-.1, .1, .2, .25]) 43 | elif arm.DOF == 3: 44 | kp = 100 # position error gain on the PD controller 45 | threshold = .05 46 | writebox = np.array([-.25, .25, 1.65, 2.]) 47 | 48 | # generate the path to follow ------------- 49 | sequence = 'hello' if sequence is None else sequence 50 | sequence = [c for c in sequence] 51 | 52 | if scale is None: 53 | scale = [1.0] * len(sequence) 54 | else: 55 | scale = [float(c) for c in scale] 56 | 57 | trajectory = rp.get_sequence(sequence, writebox, spaces=True) 58 | 59 | # generate control shell ----------------- 60 | additions = [] 61 | if force is not None: 62 | print('applying joint velocity based forcefield...') 63 | additions.append(forcefield.Addition(scale=force)) 64 | 65 | control_pars = {'additions': additions, 66 | 'gain': 1000, # pd gain for trajectory following 67 | 'pen_down': False, 68 | 'threshold': threshold, 69 | 'trajectory': trajectory.T} 70 | 71 | controller_name = controller_class.__name__.split('.')[1] 72 | if controller_name == 'dmp': 73 | # number of goals is the number of (NANs - 1) * number of DMPs 74 | num_goals = (np.sum(trajectory[:, 0] != trajectory[:, 0]) - 1) * 2 75 | # respecify goals for spatial scaling by changing add_to_goals 76 | control_pars['add_to_goals'] = [0]*num_goals 77 | control_pars['bfs'] = 1000 # number of basis function per DMP 78 | control_pars['tau'] = .1 # how fast the trajectory rolls out 79 | elif controller_name == 'trace': 80 | control_pars['tau'] = .005 # how fast the trajectory rolls out 81 | 82 | print('Using operational space controller...') 83 | controller = osc.Control(kp=kp, kv=np.sqrt(kp), 84 | write_to_file=write_to_file) 85 | control_shell = controller_class.Shell(controller=controller, 86 | **control_pars) 87 | 88 | # generate runner parameters ----------- 89 | runner_pars = {'infinite_trail': True, 90 | 'title': 'Task: Writing numbers', 91 | 'trajectory': trajectory} 92 | 93 | return (control_shell, runner_pars) 94 | -------------------------------------------------------------------------------- /studywolf_control/controllers/trajectory.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2014 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | from . import gc 19 | from . import osc 20 | from . import shell 21 | 22 | import numpy as np 23 | 24 | class Shell(shell.Shell): 25 | """ 26 | """ 27 | 28 | def __init__(self, gain, tau, trajectory, threshold=.01, **kwargs): 29 | """ 30 | control Control instance: the controller to use 31 | trajectory np.array: the time series of points to follow 32 | [DOFs, time], with a column of None 33 | wherever the pen should be lifted 34 | tau float: the time scaling term 35 | threshold float: how close the system must get to initial target 36 | """ 37 | 38 | super(Shell, self).__init__(**kwargs) 39 | 40 | self.done = False 41 | self.gain = gain 42 | self.not_at_start = True 43 | self.num_seq = 0 44 | self.tau = tau 45 | self.threshold = threshold 46 | self.x = None 47 | 48 | self.gen_path(trajectory) 49 | self.set_target() 50 | 51 | def check_pen_up(self): 52 | """Check to see if the pen should be lifted. 53 | """ 54 | raise NotImplementedError 55 | 56 | def control(self, arm): 57 | """Apply a given control signal in (x,y) 58 | space to the arm""" 59 | 60 | self.x = np.copy(arm.x) 61 | 62 | if self.controller.check_distance(arm) < self.threshold: 63 | self.not_at_start = False 64 | 65 | if self.not_at_start or self.done: 66 | self.u = self.controller.control(arm) 67 | 68 | else: 69 | self.set_target() 70 | 71 | # check to see if it's pen up time 72 | if self.check_pen_up(): 73 | self.pen_down = False 74 | 75 | if self.num_seq >= self.num_seqs - 1: 76 | # if we're finished the last DMP 77 | self.done = True 78 | import sys; sys.exit() 79 | else: 80 | # else move on to the next DMP 81 | self.not_at_start = True 82 | self.num_seq += 1 83 | self.set_next_seq() 84 | self.set_target() 85 | else: 86 | self.pen_down = True 87 | 88 | if isinstance(self.controller, osc.Control): 89 | pos = arm.x 90 | elif isinstance(self.controller, gc.Control): 91 | pos = arm.q 92 | 93 | pos_des = self.gain * (self.controller.target - pos) 94 | self.u = self.controller.control(arm, pos_des) 95 | 96 | return self.u 97 | 98 | def gen_path(self, trajectory): 99 | """Generate the sequences to follow. 100 | """ 101 | raise NotImplementedError 102 | 103 | def set_next_seq(self): 104 | """Get the next sequence in the list. 105 | """ 106 | raise NotImplementedError 107 | 108 | def set_target(self): 109 | """Get the next target in the sequence. 110 | """ 111 | raise NotImplementedError 112 | -------------------------------------------------------------------------------- /studywolf_control/arms/ArmBase.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2013 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | import numpy as np 18 | 19 | 20 | class ArmBase: 21 | """A base class for arm simulators""" 22 | 23 | def __init__(self, init_q=None, init_dq=None, 24 | dt=1e-5, singularity_thresh=.00025, options=None): 25 | """ 26 | dt float: the timestep for simulation 27 | singularity_thresh float: the point at which to singular values 28 | from the matrix SVD to zero. 29 | """ 30 | 31 | self.dt = dt 32 | self.options = options 33 | self.singularity_thresh = singularity_thresh 34 | 35 | self.init_q = np.zeros(self.DOF) if init_q is None else init_q 36 | self.init_dq = np.zeros(self.DOF) if init_dq is None else init_dq 37 | 38 | def apply_torque(self, u, dt): 39 | """Takes in a torque and timestep and updates the 40 | arm simulation accordingly. 41 | 42 | u np.array: the control signal to apply 43 | dt float: the timestep 44 | """ 45 | raise NotImplementedError 46 | 47 | def gen_jacEE(self): 48 | """Generates the Jacobian from end-effector to 49 | the origin frame""" 50 | raise NotImplementedError 51 | 52 | def gen_Mq(self): 53 | """Generates the mass matrix for the arm in joint space""" 54 | raise NotImplementedError 55 | 56 | def gen_Mx(self, JEE=None, q=None, **kwargs): 57 | """Generate the mass matrix in operational space""" 58 | if q is None: 59 | q = self.q 60 | 61 | Mq = self.gen_Mq(q=q, **kwargs) 62 | 63 | if JEE is None: 64 | JEE = self.gen_jacEE(q=q) 65 | Mx_inv = np.dot(JEE, np.dot(np.linalg.inv(Mq), JEE.T)) 66 | u, s, v = np.linalg.svd(Mx_inv) 67 | # cut off any singular values that could cause control problems 68 | for i in range(len(s)): 69 | s[i] = 0 if s[i] < self.singularity_thresh else 1./float(s[i]) 70 | # numpy returns U,S,V.T, so have to transpose both here 71 | Mx = np.dot(v.T, np.dot(np.diag(s), u.T)) 72 | 73 | return Mx 74 | 75 | def position(self, q=None): 76 | """Compute x,y position of the hand 77 | 78 | q list: a list of the joint angles, 79 | if None use current system state 80 | """ 81 | raise NotImplementedError 82 | 83 | def reset(self, q=[], dq=[]): 84 | """Resets the state of the arm 85 | q list: a list of the joint angles 86 | dq list: a list of the joint velocities 87 | """ 88 | if isinstance(q, np.ndarray): 89 | q = q.tolist() 90 | if isinstance(dq, np.ndarray): 91 | dq = dq.tolist() 92 | 93 | if q: 94 | assert len(q) == self.DOF 95 | if dq: 96 | assert len(dq) == self.DOF 97 | 98 | self.q = np.copy(self.init_q) if not q else np.copy(q) 99 | self.dq = np.copy(self.init_dq) if not dq else np.copy(dq) 100 | self.t = 0.0 101 | 102 | def update_state(self): 103 | """Update the state (for MapleSim models)""" 104 | pass 105 | 106 | @property 107 | def x(self): 108 | return self.position()[:, -1] 109 | -------------------------------------------------------------------------------- /studywolf_control/tasks/postural.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2015 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | import controllers.target_list as target_list 19 | import controllers.signal as signal 20 | 21 | import numpy as np 22 | 23 | class Addition(signal.Signal): 24 | def __init__(self, index=0): 25 | forces = np.array([[-0.90937641, -0.64614133, -0.89626731], 26 | [ 0.56228211, 0.80956427, -0.47827398], 27 | [ 0.77974938, -0.5750045 , 0.17895108], 28 | [-0.25071541, 0.54737452, -0.92822244], 29 | [ 0.19228047, 0.034985 , -0.37703873], 30 | [-0.7975092 , -0.56974833, 0.31316189], 31 | [-0.52069927, 0.44450567, -0.55619732], 32 | [-0.65013087, -0.88908786, 0.07065782]]) 33 | self.scale = 400 34 | self.force_vector = forces[index] * self.scale 35 | 36 | def generate(self, u, arm): 37 | return self.force_vector[:arm.DOF] 38 | 39 | 40 | def Task(arm, controller_type, x_bias=0., y_bias=2., dist=.4, 41 | force_index=7, write_to_file=False, **kwargs): 42 | """ 43 | This task sets up the arm to move to 8 target points 44 | around the unit circle, and then attempt to hold these 45 | positions while an unexpected force is applied. 46 | """ 47 | 48 | # set arm specific parameters ------------ 49 | repeat = 0 50 | if arm.DOF == 2: 51 | dist = .075 52 | y_bias = .35 53 | threshold = .0075 54 | if arm.DOF == 3: 55 | threshold = .015 56 | 57 | # generate the path to follow ------------- 58 | # set up the reaching trajectories, 8 points around unit circle 59 | targets_x = [dist * np.cos(theta) + x_bias \ 60 | for theta in np.linspace(0, np.pi*2, 9)][:-1] 61 | targets_x += targets_x * repeat 62 | targets_y = [dist * np.sin(theta) + y_bias \ 63 | for theta in np.linspace(0, np.pi*2, 9)][:-1] 64 | targets_y += targets_y * repeat 65 | trajectory = np.ones((2*len(targets_x)+3, 2))*np.nan 66 | 67 | for ii in range(len(targets_x)): 68 | trajectory[ii*2] = [targets_x[ii], targets_y[ii]] 69 | trajectory[-2] = [0, y_bias] 70 | 71 | 72 | # generate control shell ----------------- 73 | additions = [] 74 | force_index = np.random.randint(8) if force_index is None else force_index 75 | print 'applying force %i...'%force_index 76 | additions.append(Addition(index=force_index)) 77 | task = 'arm%i/postural%i'%(arm.DOF, force_index) 78 | 79 | control_pars = {'additions':additions, 80 | 'task':task, 81 | 'write_to_file':write_to_file} 82 | if controller_type.__class__ == 'osc': 83 | kp = 100 # position error gain on the PD controller 84 | control_pars['kp'] = kp 85 | control_pars['kv'] = np.sqrt(kp) 86 | controller = controller_type.Control(**control_pars) 87 | 88 | control_pars = {'target_list':trajectory, 89 | 'threshold':threshold, # how close to get to each target 90 | 'timer_time':2000,# how many ns to stay at each target 91 | 'postural':True} 92 | 93 | runner_pars = {'infinite_trail':True, 94 | 'title':'Task: Reaching', 95 | 'trajectory':trajectory} 96 | 97 | control_shell = target_list.Shell(controller=controller, **control_pars) 98 | 99 | return (control_shell, runner_pars) 100 | -------------------------------------------------------------------------------- /studywolf_control/tasks/write_data/w.txt: -------------------------------------------------------------------------------- 1 | 732.0,859.0 2 | 731.0,858.0 3 | 730.0,857.0 4 | 729.0,856.0 5 | 728.0,854.0 6 | 727.0,852.0 7 | 726.0,849.0 8 | 724.0,845.0 9 | 722.0,841.0 10 | 720.0,837.0 11 | 718.0,832.0 12 | 716.0,827.0 13 | 713.0,822.0 14 | 710.0,818.0 15 | 707.0,814.0 16 | 704.0,810.0 17 | 700.0,805.0 18 | 696.0,801.0 19 | 692.0,797.0 20 | 687.0,793.0 21 | 682.0,788.0 22 | 677.0,784.0 23 | 673.0,780.0 24 | 668.0,776.0 25 | 663.0,772.0 26 | 658.0,768.0 27 | 652.0,764.0 28 | 646.0,761.0 29 | 640.0,758.0 30 | 634.0,755.0 31 | 628.0,753.0 32 | 622.0,751.0 33 | 616.0,750.0 34 | 610.0,749.0 35 | 605.0,748.0 36 | 599.0,748.0 37 | 593.0,748.0 38 | 587.0,748.0 39 | 580.0,749.0 40 | 572.0,750.0 41 | 563.0,752.0 42 | 555.0,754.0 43 | 546.0,756.0 44 | 538.0,758.0 45 | 531.0,761.0 46 | 524.0,764.0 47 | 518.0,767.0 48 | 512.0,770.0 49 | 506.0,773.0 50 | 500.0,776.0 51 | 494.0,780.0 52 | 488.0,783.0 53 | 483.0,787.0 54 | 477.0,792.0 55 | 471.0,797.0 56 | 466.0,803.0 57 | 461.0,809.0 58 | 456.0,815.0 59 | 452.0,821.0 60 | 448.0,827.0 61 | 445.0,832.0 62 | 442.0,838.0 63 | 440.0,842.0 64 | 438.0,846.0 65 | 436.0,851.0 66 | 435.0,855.0 67 | 434.0,860.0 68 | 433.0,865.0 69 | 433.0,870.0 70 | 433.0,874.0 71 | 433.0,879.0 72 | 433.0,884.0 73 | 434.0,889.0 74 | 435.0,895.0 75 | 436.0,899.0 76 | 438.0,903.0 77 | 440.0,907.0 78 | 442.0,911.0 79 | 444.0,915.0 80 | 447.0,919.0 81 | 450.0,922.0 82 | 453.0,926.0 83 | 457.0,930.0 84 | 460.0,933.0 85 | 464.0,937.0 86 | 469.0,941.0 87 | 474.0,945.0 88 | 479.0,949.0 89 | 484.0,953.0 90 | 489.0,957.0 91 | 493.0,960.0 92 | 497.0,963.0 93 | 500.0,966.0 94 | 503.0,968.0 95 | 506.0,970.0 96 | 509.0,972.0 97 | 512.0,974.0 98 | 515.0,976.0 99 | 519.0,978.0 100 | 522.0,979.0 101 | 525.0,980.0 102 | 529.0,981.0 103 | 532.0,982.0 104 | 535.0,983.0 105 | 537.0,984.0 106 | 539.0,985.0 107 | 540.0,985.0 108 | 541.0,985.0 109 | 542.0,985.0 110 | 543.0,985.0 111 | 542.0,985.0 112 | 541.0,985.0 113 | 540.0,985.0 114 | 539.0,985.0 115 | 538.0,985.0 116 | 537.0,984.0 117 | 536.0,984.0 118 | 535.0,984.0 119 | 534.0,984.0 120 | 532.0,983.0 121 | 530.0,983.0 122 | 528.0,983.0 123 | 526.0,983.0 124 | 524.0,983.0 125 | 521.0,983.0 126 | 518.0,982.0 127 | 515.0,981.0 128 | 512.0,980.0 129 | 509.0,980.0 130 | 506.0,980.0 131 | 502.0,980.0 132 | 498.0,980.0 133 | 495.0,980.0 134 | 491.0,980.0 135 | 487.0,981.0 136 | 483.0,982.0 137 | 479.0,983.0 138 | 475.0,984.0 139 | 471.0,985.0 140 | 468.0,986.0 141 | 465.0,987.0 142 | 462.0,988.0 143 | 459.0,989.0 144 | 456.0,990.0 145 | 454.0,991.0 146 | 452.0,992.0 147 | 449.0,994.0 148 | 447.0,996.0 149 | 445.0,998.0 150 | 443.0,1001.0 151 | 441.0,1003.0 152 | 439.0,1006.0 153 | 437.0,1009.0 154 | 435.0,1012.0 155 | 434.0,1014.0 156 | 432.0,1017.0 157 | 430.0,1021.0 158 | 429.0,1025.0 159 | 427.0,1028.0 160 | 426.0,1031.0 161 | 425.0,1035.0 162 | 424.0,1038.0 163 | 423.0,1042.0 164 | 422.0,1045.0 165 | 421.0,1048.0 166 | 420.0,1052.0 167 | 419.0,1056.0 168 | 418.0,1061.0 169 | 417.0,1066.0 170 | 416.0,1072.0 171 | 415.0,1079.0 172 | 415.0,1086.0 173 | 415.0,1094.0 174 | 415.0,1101.0 175 | 415.0,1107.0 176 | 416.0,1113.0 177 | 417.0,1120.0 178 | 418.0,1126.0 179 | 419.0,1132.0 180 | 421.0,1138.0 181 | 423.0,1144.0 182 | 426.0,1151.0 183 | 429.0,1159.0 184 | 433.0,1166.0 185 | 437.0,1173.0 186 | 442.0,1179.0 187 | 447.0,1185.0 188 | 452.0,1190.0 189 | 459.0,1196.0 190 | 466.0,1201.0 191 | 473.0,1206.0 192 | 481.0,1211.0 193 | 488.0,1215.0 194 | 496.0,1218.0 195 | 504.0,1221.0 196 | 513.0,1224.0 197 | 522.0,1227.0 198 | 531.0,1228.0 199 | 540.0,1230.0 200 | 547.0,1231.0 201 | 554.0,1232.0 202 | 562.0,1232.0 203 | 570.0,1232.0 204 | 579.0,1231.0 205 | 588.0,1230.0 206 | 597.0,1228.0 207 | 606.0,1226.0 208 | 614.0,1224.0 209 | 621.0,1222.0 210 | 628.0,1219.0 211 | 634.0,1217.0 212 | 640.0,1215.0 213 | 646.0,1212.0 214 | 652.0,1209.0 215 | 658.0,1206.0 216 | 664.0,1203.0 217 | 669.0,1201.0 218 | 674.0,1198.0 219 | 679.0,1196.0 220 | 683.0,1194.0 221 | 687.0,1191.0 222 | 690.0,1189.0 223 | 693.0,1187.0 224 | 696.0,1185.0 225 | 698.0,1183.0 226 | 700.0,1181.0 227 | 702.0,1179.0 228 | 704.0,1177.0 229 | 706.0,1175.0 230 | 708.0,1173.0 231 | 710.0,1172.0 232 | 711.0,1171.0 233 | 712.0,1170.0 234 | 713.0,1168.0 235 | 714.0,1166.0 236 | 715.0,1165.0 237 | 716.0,1164.0 238 | 716.0,1163.0 239 | 716.0,1162.0 240 | 716.0,1161.0 241 | 716.0,1160.0 242 | -------------------------------------------------------------------------------- /studywolf_control/controllers/dmp.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2014 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | from pydmps import dmp_discrete as DMP_discrete 19 | from pydmps import dmp_rhythmic as DMP_rhythmic 20 | from . import trajectory 21 | 22 | import numpy as np 23 | 24 | 25 | class Shell(trajectory.Shell): 26 | """ 27 | A shell that uses dynamic movement primitives to 28 | control a robotic arm end-effector. 29 | """ 30 | 31 | def __init__(self, bfs, add_to_goals=None, 32 | pattern='discrete', **kwargs): 33 | """ 34 | bfs int: the number of basis functions per DMP 35 | add_to_goals np.array: floats to add to the DMP goals 36 | used to scale the DMPs spatially 37 | pattern string: specifies either 'discrete' or 'rhythmic' DMPs 38 | """ 39 | 40 | self.bfs = bfs 41 | self.add_to_goals = add_to_goals 42 | self.pattern = pattern 43 | 44 | super(Shell, self).__init__(**kwargs) 45 | 46 | if add_to_goals is not None: 47 | for ii, dmp in enumerate(self.dmp_sets): 48 | dmp.goal[0] += add_to_goals[ii*2] 49 | dmp.goal[1] += add_to_goals[ii*2+1] 50 | 51 | def check_pen_up(self): 52 | """Check to see if the pen should be lifted. 53 | """ 54 | if (self.dmps.cs.x < 55 | np.exp(-self.dmps.cs.ax * self.dmps.cs.run_time)): 56 | return True 57 | else: 58 | return False 59 | 60 | def gen_path(self, trajectory): 61 | """Generate the DMPs necessary to follow the 62 | specified trajectory. 63 | 64 | trajectory np.array: the time series of points to follow 65 | [DOFs, time], with a column of None 66 | wherever the pen should be lifted 67 | """ 68 | 69 | if trajectory.ndim == 1: 70 | trajectory = trajectory.reshape(1, len(trajectory)) 71 | 72 | num_DOF = trajectory.shape[0] 73 | # break up the trajectory into its different words 74 | # NaN or None signals a new word / break in drawing 75 | breaks = np.array(np.where(trajectory[0] != trajectory[0]))[0] 76 | self.num_seqs = len(breaks) - 1 77 | 78 | self.dmp_sets = [] 79 | for ii in range(self.num_seqs): 80 | # get the ii'th sequence 81 | seq = trajectory[:, breaks[ii]+1:breaks[ii+1]] 82 | 83 | if self.pattern == 'discrete': 84 | dmps = DMP_discrete.DMPs_discrete(n_dmps=num_DOF, n_bfs=self.bfs) 85 | elif self.pattern == 'rhythmic': 86 | dmps = DMP_rhythmic.DMPs_rhythmic(n_dmps=num_DOF, n_bfs=self.bfs) 87 | else: 88 | raise Exception('Invalid pattern type specified. Valid choices \ 89 | are discrete or rhythmic.') 90 | 91 | dmps.imitate_path(y_des=seq) 92 | self.dmp_sets.append(dmps) 93 | 94 | self.dmps = self.dmp_sets[0] 95 | 96 | def set_next_seq(self): 97 | """Get the next sequence in the list. 98 | """ 99 | self.dmps = self.dmp_sets[self.num_seq] 100 | 101 | def set_target(self): 102 | """Get the next target in the sequence. 103 | """ 104 | error = 0.0 105 | if self.controller.target is not None: 106 | error = np.sqrt(np.sum((self.x - 107 | self.controller.target)**2)) * 1000 108 | self.controller.target, _, _ = self.dmps.step(tau=self.tau, 109 | error=error) 110 | -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/config.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import nengo 3 | from nengo.dists import Choice 4 | 5 | 6 | class OSCConfig(object): 7 | """A class for storing all the specific 8 | configuration parameters for the neural implementation 9 | of OSC""" 10 | 11 | # OSC ------------------------------------------------------------------- # 12 | 13 | n_neurons = 3000 14 | # n_neurons = 5000 15 | CB = { 16 | 'dimensions': 6, 17 | 'max_rates': np.random.uniform(low=10, high=200, size=n_neurons), 18 | 'n_neurons': n_neurons, 19 | # 'neuron_type': nengo.Direct(), 20 | 'radius': 5, 21 | } 22 | 23 | # n_neurons = 2000 24 | M1 = { 25 | 'dimensions': 6, 26 | 'max_rates': np.random.uniform(low=00, high=100, size=n_neurons), 27 | 'n_neurons': n_neurons, 28 | # 'neuron_type': nengo.Direct(), 29 | 'radius': .25, 30 | } 31 | 32 | # n_neurons = 7000 33 | M1_mult = { 34 | 'encoders': Choice([[1,1],[-1,1],[-1,-1],[1,-1]]), 35 | 'ens_dimensions': 2, 36 | 'n_ensembles': 6, 37 | 'n_neurons': n_neurons, 38 | # 'neuron_type': nengo.Direct(), 39 | 'radius': np.sqrt(2), 40 | } 41 | 42 | # n_neurons = 3000 43 | M1_null = { 44 | 'dimensions': 3, 45 | 'max_rates': np.random.uniform(low=10, high=200, size=n_neurons), 46 | 'n_neurons': n_neurons, 47 | # 'neuron_type': nengo.Direct(), 48 | 'radius': np.sqrt(3), 49 | } 50 | 51 | # DMPs ------------------------------------------------------------------ # 52 | 53 | oscillator = { 54 | 'dimensions': 2, 55 | 'n_neurons': 500, 56 | # 'neuron_type': nengo.Direct(), 57 | 'radius': .01, 58 | } 59 | 60 | forcing_func = { 61 | 'dimensions': 2, 62 | 'n_neurons': 2000, 63 | } 64 | 65 | y = { 66 | 'dimensions': 1, 67 | 'n_neurons': 1000, 68 | # 'neuron_type': nengo.Direct(), 69 | 'radius': 5, 70 | } 71 | 72 | 73 | target_ybias = 2.0 74 | 75 | # ----------------------------------------------------------------------- # 76 | CBmean = np.array([.61, 1.86, .416, -0.03, -.012, .021]) 77 | CBscale = np.array([.4, .6, .3, 1.0, 1.0, 1.0]) 78 | 79 | def CB_scaledown(self, x): 80 | return (x - self.CBmean) / self.CBscale 81 | 82 | def CB_scaleup(self, x): 83 | return x * self.CBscale + self.CBmean 84 | 85 | # ----------------------------------------------------------------------- # 86 | M1mean = np.array([.58, .57, .23, .79, -.76, -.96]) 87 | M1scale = np.array([.43, .525, .3, .25, .4, .08]) 88 | 89 | def M1_scaledown(self, x): 90 | return (x - self.M1mean) / self.M1scale 91 | 92 | def M1_scaleup(self, x): 93 | return x * self.M1scale + self.M1mean 94 | 95 | # ----------------------------------------------------------------------- # 96 | M1nullmean = np.array([0.61, 1.9, 0.4]) 97 | M1nullscale = np.array([.45, .55, .32]) 98 | 99 | def M1null_scaledown(self, x): 100 | return (x - self.M1nullmean) / self.M1nullscale 101 | 102 | def M1null_scaleup(self, x): 103 | return x * self.M1nullscale + self.M1nullmean 104 | 105 | # ----------------------------------------------------------------------- # 106 | u_scaling = np.array([2., 1.]) 107 | DPmean = np.array([-167., -26., -90., -81., -20., -33.]) 108 | DPscale = np.array([50., 65., 35., 35., 5., 9.]) 109 | 110 | def DP_scaledown(self, x): 111 | return (x - self.DPmean) / self.DPscale 112 | 113 | def DP_scaleup(self, x, index): 114 | return self.u_scaling[index%2] * x[0] * \ 115 | (x[1] * self.DPscale[index] + self.DPmean[index]) 116 | 117 | def __init__(self, adaptation): 118 | 119 | if adaptation == 'kinematic': 120 | self.DPmean *= 2.0 121 | 122 | self.DP_scaleup_list = [ 123 | lambda x: self.DP_scaleup(x, 0), 124 | lambda x: self.DP_scaleup(x, 1), 125 | lambda x: self.DP_scaleup(x, 2), 126 | lambda x: self.DP_scaleup(x, 3), 127 | lambda x: self.DP_scaleup(x, 4), 128 | lambda x: self.DP_scaleup(x, 5)] 129 | -------------------------------------------------------------------------------- /studywolf_control/controllers/ahf.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2016 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | import control 19 | 20 | import numpy as np 21 | 22 | from hessianfree.rnnet import RNNet 23 | from hessianfree.nonlinearities import (Tanh, Linear) 24 | 25 | class Control(control.Control): 26 | """ 27 | A controller that loads in a neural network trained using the 28 | hessianfree (https://github.com/drasmuss/hessianfree) library 29 | to control a simulated arm. 30 | """ 31 | def __init__(self, **kwargs): 32 | 33 | super(Control, self).__init__(**kwargs) 34 | 35 | self.old_target = [None, None] 36 | 37 | # load up our network 38 | import glob 39 | 40 | # this code goes into the weights folder, finds the most 41 | # recent trial, and loads up the weights 42 | files = sorted(glob.glob('controllers/weights/rnn*')) 43 | print 'loading weights from %s'%files[-1] 44 | W = np.load(files[-1])['arr_0'] 45 | num_states = 4 46 | self.rnn = RNNet(shape=[num_states * 2, 32, 32, num_states, num_states], 47 | layers=[Linear(), Tanh(), Tanh(), Linear(), Linear()], 48 | rec_layers=[1,2], 49 | conns={0:[1, 2], 1:[2], 2:[3], 3:[4]}, 50 | load_weights=W, 51 | use_GPU=False) 52 | 53 | offset, W_end, b_end = self.rnn.offsets[(3,4)] 54 | self.rnn.mask = np.zeros(self.rnn.W.shape, dtype=bool) 55 | self.rnn.mask[offset:b_end] = True 56 | self.rnn.W[offset:W_end] = np.eye(4).flatten() 57 | 58 | self.joint_targets = None 59 | self.act = None 60 | 61 | # set up recorders 62 | if self.write_to_file is True: 63 | from recorder import Recorder 64 | self.u_recorder = Recorder('control signal', self.task, 'hf') 65 | self.xy_recorder = Recorder('end-effector position', self.task, 'hf') 66 | self.dist_recorder = Recorder('distance from target', self.task, 'hf') 67 | self.recorders = [self.u_recorder, 68 | self.xy_recorder, 69 | self.dist_recorder] 70 | 71 | def control(self, arm, x_des=None): 72 | """Generates a control signal to move the 73 | arm to the specified target. 74 | 75 | arm Arm: the arm model being controlled 76 | des list: the desired system position 77 | x_des np.array: desired task-space force, 78 | system goes to self.target if None 79 | """ 80 | 81 | self.x = arm.x 82 | 83 | # if the target has changed, convert into joint angles again 84 | if np.any(self.old_target != self.target): 85 | self.joint_targets = arm.inv_kinematics(xy=self.target) 86 | self.old_target = self.target 87 | 88 | inputs = np.concatenate([self.joint_targets, np.zeros(2), arm.q, arm.dq])[None,None,:] 89 | self.act = [a[:,-1,:] for a in self.rnn.forward(inputs, init_activations=self.act)] 90 | u = self.act[-1][0] 91 | # NOTE: Make sure this is set up the same way as in training 92 | # use all the network output is the control signal 93 | self.u = np.array([np.sum(u[ii::arm.DOF]) for ii in range(arm.DOF)]) 94 | 95 | if self.write_to_file is True: 96 | # feed recorders their signals 97 | self.u_recorder.record(0.0, self.u) 98 | self.xy_recorder.record(0.0, self.x) 99 | self.dist_recorder.record(0.0, self.target - self.x) 100 | 101 | # add in any additional signals 102 | for addition in self.additions: 103 | self.u += addition.generate(self.u, arm) 104 | 105 | return self.u 106 | 107 | def gen_target(self, arm): 108 | pass 109 | 110 | 111 | -------------------------------------------------------------------------------- /studywolf_control/controllers/osc.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2013 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | from . import control 19 | 20 | import numpy as np 21 | 22 | class Control(control.Control): 23 | """ 24 | A controller that implements operational space control. 25 | Controls the (x,y) position of a robotic arm end-effector. 26 | """ 27 | def __init__(self, null_control=True, **kwargs): 28 | """ 29 | null_control boolean: apply second controller in null space or not 30 | """ 31 | 32 | super(Control, self).__init__(**kwargs) 33 | 34 | self.DOF = 2 # task space dimensionality 35 | self.null_control = null_control 36 | 37 | if self.write_to_file is True: 38 | from recorder import Recorder 39 | # set up recorders 40 | self.u_recorder = Recorder('control signal', self.task, 'osc') 41 | self.xy_recorder = Recorder('end-effector position', self.task, 'osc') 42 | self.dist_recorder = Recorder('distance from target', self.task, 'osc') 43 | self.recorders = [self.u_recorder, 44 | self.xy_recorder, 45 | self.dist_recorder] 46 | 47 | def control(self, arm, x_des=None): 48 | """Generates a control signal to move the 49 | arm to the specified target. 50 | 51 | arm Arm: the arm model being controlled 52 | des list: the desired system position 53 | x_des np.array: desired task-space force, 54 | system goes to self.target if None 55 | """ 56 | 57 | # calculate desired end-effector acceleration 58 | if x_des is None: 59 | self.x = arm.x 60 | x_des = self.kp * (self.target - self.x) 61 | 62 | # generate the mass matrix in end-effector space 63 | Mq = arm.gen_Mq() 64 | Mx = arm.gen_Mx() 65 | 66 | # calculate force 67 | Fx = np.dot(Mx, x_des) 68 | 69 | # calculate the Jacobian 70 | JEE = arm.gen_jacEE() 71 | # tau = J^T * Fx + tau_grav, but gravity = 0 72 | # add in velocity compensation in GC space for stability 73 | self.u = (np.dot(JEE.T, Fx).reshape(-1,) - 74 | np.dot(Mq, self.kv * arm.dq)) 75 | 76 | # if null_control is selected and the task space has 77 | # fewer DOFs than the arm, add a control signal in the 78 | # null space to try to move the arm to its resting state 79 | if self.null_control and self.DOF < len(arm.L): 80 | 81 | # calculate our secondary control signal 82 | # calculated desired joint angle acceleration 83 | prop_val = ((arm.rest_angles - arm.q) + np.pi) % (np.pi*2) - np.pi 84 | q_des = (self.kp * prop_val + \ 85 | self.kv * -arm.dq).reshape(-1,) 86 | 87 | Mq = arm.gen_Mq() 88 | u_null = np.dot(Mq, q_des) 89 | 90 | # calculate the null space filter 91 | Jdyn_inv = np.dot(Mx, np.dot(JEE, np.linalg.inv(Mq))) 92 | null_filter = np.eye(len(arm.L)) - np.dot(JEE.T, Jdyn_inv) 93 | 94 | null_signal = np.dot(null_filter, u_null).reshape(-1,) 95 | 96 | self.u += null_signal 97 | 98 | if self.write_to_file is True: 99 | # feed recorders their signals 100 | self.u_recorder.record(0.0, self.u) 101 | self.xy_recorder.record(0.0, self.x) 102 | self.dist_recorder.record(0.0, self.target - self.x) 103 | 104 | # add in any additional signals 105 | for addition in self.additions: 106 | self.u += addition.generate(self.u, arm) 107 | 108 | return self.u 109 | 110 | def gen_target(self, arm): 111 | """Generate a random target""" 112 | gain = np.sum(arm.L) * .75 113 | bias = -np.sum(arm.L) * 0 114 | 115 | self.target = np.random.random(size=(2,)) * gain + bias 116 | 117 | return self.target.tolist() 118 | -------------------------------------------------------------------------------- /studywolf_control/arms/one_link/arm.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2013 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | from ..ArmBase import ArmBase 19 | import numpy as np 20 | import py1LinkArm 21 | 22 | class Arm(ArmBase): 23 | """A wrapper around a MapleSim generated C simulation 24 | of a one link arm""" 25 | 26 | def __init__(self, JEE='x', **kwargs): 27 | """ 28 | JEE string: specifies whether the Jacobian for OSC control of 29 | the end-effector should relay the x or y position. 30 | """ 31 | self.DOF = 1 32 | ArmBase.__init__(self, singularity_thresh=1e-10, **kwargs) 33 | 34 | # length of arm links 35 | self.l1 = .8 36 | self.L = np.array([self.l1]) 37 | # mass of links 38 | m1=1.0 39 | # z axis inertia moment of links 40 | izz1=15. 41 | # create mass matrices at COM for each link 42 | self.M1 = np.zeros((6,6)) 43 | self.M1[0:3,0:3] = np.eye(3)*m1 44 | self.M1[3:,3:] = np.eye(3)*izz1 45 | 46 | self.resting_position = np.array([np.pi/4.0]) 47 | 48 | # stores information returned from maplesim 49 | self.state = np.zeros(3) 50 | # maplesim arm simulation 51 | self.sim = py1LinkArm.pySim(dt=1e-5) 52 | self.sim.reset(self.state) 53 | self.reset() 54 | self.update_state() 55 | 56 | def apply_torque(self, u, dt): 57 | """Takes in a torque and timestep and updates the 58 | arm simulation accordingly. 59 | 60 | u np.array: the control signal to apply 61 | dt float: the timestep 62 | """ 63 | u = -1 * np.array(u, dtype='float') 64 | 65 | for i in range(int(np.ceil(dt/1e-5))): 66 | self.sim.step(self.state, u) 67 | self.update_state() 68 | 69 | def gen_jacCOM1(self, q=None): 70 | """Generates the Jacobian from the COM of the first 71 | link to the origin frame""" 72 | q = self.q if q is None else q 73 | 74 | JCOM1 = np.zeros((6,1)) 75 | JCOM1[0,0] = self.l1 / 2. * -np.sin(q[0]) 76 | JCOM1[1,0] = self.l1 / 2. * np.cos(q[0]) 77 | JCOM1[5,0] = 1.0 78 | 79 | return JCOM1 80 | 81 | def gen_jacEE(self, q=None): 82 | """Generates the Jacobian from end-effector to 83 | the origin frame""" 84 | q = self.q if q is None else q 85 | 86 | JEE = np.zeros((2,1)) 87 | JEE[0,0] = self.l1 * -np.sin(q[0]) 88 | JEE[1,0] = self.l1 * np.cos(q[0]) 89 | 90 | return JEE 91 | 92 | def gen_Mq(self, q=None): 93 | """Generates the mass matrix for the arm in joint space""" 94 | q = self.q if q is None else q 95 | 96 | # get the instantaneous Jacobians 97 | JCOM1 = self.gen_jacCOM1() 98 | # generate the mass matrix in joint space 99 | Mq = np.dot(JCOM1.T, np.dot(self.M1, JCOM1)) 100 | 101 | return Mq 102 | 103 | def position(self, q=None): 104 | """Compute x,y position of the hand 105 | 106 | q np.array: a set of angles to return positions for 107 | ee_only boolean: only return the (x,y) of the end-effector 108 | """ 109 | q = self.q if q is None else q 110 | 111 | x = np.cumsum([0, 112 | self.l1 * np.cos(q[0])]) 113 | y = np.cumsum([0, 114 | self.l1 * np.sin(q[0])]) 115 | return np.array([x, y]) 116 | 117 | def reset(self, q=[], dq=[]): 118 | if isinstance(q, np.ndarray): q = q.tolist() 119 | if isinstance(dq, np.ndarray): dq = dq.tolist() 120 | 121 | if q: assert len(q) == self.DOF 122 | if dq: assert len(dq) == self.DOF 123 | 124 | state = np.zeros(self.DOF*2) 125 | state[::2] = self.init_q if not q else np.copy(q) 126 | state[1::2] = self.init_dq if not dq else np.copy(dq) 127 | 128 | self.sim.reset(self.state, state) 129 | self.update_state() 130 | 131 | def update_state(self): 132 | """Separate out the state variable into time, angles, 133 | velocities, and accelerations.""" 134 | 135 | self.t = self.state[0] 136 | self.q = self.state[1:2] 137 | self.dq = self.state[2:] 138 | -------------------------------------------------------------------------------- /studywolf_control/arms/two_link/arm2base.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2015 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | from ..ArmBase import ArmBase 19 | import numpy as np 20 | 21 | class Arm2Base(ArmBase): 22 | """A wrapper around a MapleSim generated C simulation 23 | of a two link arm.""" 24 | 25 | def __init__(self, init_q=[.75613, 1.8553], init_dq=[0.,0.], 26 | l1=.31, l2=.27, **kwargs): 27 | 28 | self.DOF = 2 29 | ArmBase.__init__(self, init_q=init_q, init_dq=init_dq, 30 | singularity_thresh=.00025, **kwargs) 31 | 32 | # length of arm links 33 | self.l1 = l1 34 | self.l2 = l2 35 | self.L = np.array([self.l1, self.l2]) 36 | # mass of links 37 | self.m1=1.98 38 | self.m2=1.32 39 | # z axis inertia moment of links 40 | izz1=15.; izz2=8. 41 | # create mass matrices at COM for each link 42 | self.M1 = np.zeros((6,6)) 43 | self.M2 = np.zeros((6,6)) 44 | self.M1[0:3,0:3] = np.eye(3)*self.m1 45 | self.M1[3:,3:] = np.eye(3)*izz1 46 | self.M2[0:3,0:3] = np.eye(3)*self.m2 47 | self.M2[3:,3:] = np.eye(3)*izz2 48 | 49 | self.rest_angles = np.array([np.pi/4.0, np.pi/4.0]) 50 | 51 | def gen_jacCOM1(self, q=None): 52 | """Generates the Jacobian from the COM of the first 53 | link to the origin frame""" 54 | q = self.q if q is None else q 55 | 56 | JCOM1 = np.zeros((6,2)) 57 | JCOM1[0,0] = self.l1 / 2. * -np.sin(q[0]) 58 | JCOM1[1,0] = self.l1 / 2. * np.cos(q[0]) 59 | JCOM1[5,0] = 1.0 60 | 61 | return JCOM1 62 | 63 | def gen_jacCOM2(self, q=None): 64 | """Generates the Jacobian from the COM of the second 65 | link to the origin frame""" 66 | q = self.q if q is None else q 67 | 68 | JCOM2 = np.zeros((6,2)) 69 | # define column entries right to left 70 | JCOM2[0,1] = self.l2 / 2. * -np.sin(q[0]+q[1]) 71 | JCOM2[1,1] = self.l2 / 2. * np.cos(q[0]+q[1]) 72 | JCOM2[5,1] = 1.0 73 | 74 | JCOM2[0,0] = self.l1 * -np.sin(q[0]) + JCOM2[0,1] 75 | JCOM2[1,0] = self.l1 * np.cos(q[0]) + JCOM2[1,1] 76 | JCOM2[5,0] = 1.0 77 | 78 | return JCOM2 79 | 80 | def gen_jacEE(self, q=None): 81 | """Generates the Jacobian from end-effector to 82 | the origin frame""" 83 | q = self.q if q is None else q 84 | 85 | JEE = np.zeros((2,2)) 86 | # define column entries right to left 87 | JEE[0,1] = self.l2 * -np.sin(q[0]+q[1]) 88 | JEE[1,1] = self.l2 * np.cos(q[0]+q[1]) 89 | 90 | JEE[0,0] = self.l1 * -np.sin(q[0]) + JEE[0,1] 91 | JEE[1,0] = self.l1 * np.cos(q[0]) + JEE[1,1] 92 | 93 | return JEE 94 | 95 | def gen_Mq(self, q=None): 96 | """Generates the mass matrix for the arm in joint space""" 97 | # get the instantaneous Jacobians 98 | JCOM1 = self.gen_jacCOM1(q=q) 99 | JCOM2 = self.gen_jacCOM2(q=q) 100 | # generate the mass matrix in joint space 101 | Mq = np.dot(JCOM1.T, np.dot(self.M1, JCOM1)) + \ 102 | np.dot(JCOM2.T, np.dot(self.M2, JCOM2)) 103 | 104 | return Mq 105 | 106 | def inv_kinematics(self, xy): 107 | """Calculate the joint angles for a given (x,y) 108 | hand position""" 109 | import scipy.optimize 110 | # function to optimize 111 | def distance_to_target(q, xy, L): 112 | x = L[0] * np.cos(q[0]) + L[1] * np.cos(q[0] + q[1]) 113 | y = L[0] * np.sin(q[0]) + L[1] * np.sin(q[0] + q[1]) 114 | return np.sqrt((x - xy[0])**2 + (y - xy[1])**2) 115 | 116 | return scipy.optimize.minimize(fun=distance_to_target, x0=self.q, 117 | args=([xy[0], xy[1]], self.L))['x'] 118 | 119 | def position(self, q=None): 120 | """Compute x,y position of the hand 121 | 122 | q np.array: a set of angles to return positions for 123 | rotate float: how much to rotate the first joint by 124 | """ 125 | q = self.q if q is None else q 126 | 127 | x = np.cumsum([0, 128 | self.l1 * np.cos(q[0]), 129 | self.l2 * np.cos(q[0]+q[1])]) 130 | y = np.cumsum([0, 131 | self.l1 * np.sin(q[0]), 132 | self.l2 * np.sin(q[0]+q[1])]) 133 | return np.array([x, y]) 134 | -------------------------------------------------------------------------------- /studywolf_control/controllers/target_list.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2014 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | import numpy as np 19 | 20 | class Shell(object): 21 | """ 22 | """ 23 | 24 | def __init__(self, controller, target_list, 25 | threshold=.01, pen_down=False, 26 | timer_time=1, timer2_time=50, 27 | postural=False): 28 | """ 29 | controller Control instance: the controller to use 30 | pen_down boolean: True if the end-effector is drawing 31 | """ 32 | 33 | self.controller = controller 34 | self.pen_down = pen_down 35 | self.target_list = target_list 36 | self.threshold = threshold 37 | 38 | self.postural = postural 39 | self.not_at_start = True 40 | self.target_index = 0 41 | self.set_target() 42 | 43 | # starts when a movement is complete before starting the next one 44 | self.run_timer = False 45 | self.timer = 0 46 | self.hold_time = timer_time # ns 47 | 48 | # starts when target is presented, while running arm can't move 49 | self.run_timer2 = False 50 | self.timer2 = 0 51 | self.hold_time2 = timer2_time # ns 52 | 53 | if self.postural == True: 54 | self.additions = self.controller.additions[0] 55 | self.controller.additions = [] 56 | print('additional force removed...') 57 | 58 | def control(self, arm): 59 | """Move to a series of targets. 60 | """ 61 | 62 | if self.controller.check_distance(arm) < self.threshold and self.run_timer == False: 63 | # start the timer, holding for self.hold_time ms before beginning next movement 64 | self.run_timer = True 65 | 66 | if self.postural == True and \ 67 | self.target_index != len(self.target_list) - 1: 68 | self.pen_down = not self.pen_down 69 | self.controller.additions = [self.additions] 70 | print('additional force added...') 71 | 72 | print('start recording') 73 | for recorder in self.controller.recorders: 74 | recorder.start_recording = True 75 | 76 | elif (self.target_index % 3) == 1: 77 | print('start recording') 78 | for recorder in self.controller.recorders: 79 | recorder.start_recording = True 80 | 81 | if self.run_timer == True: 82 | 83 | self.timer += 1 84 | 85 | if self.timer == self.hold_time: 86 | # then move the target list forward now 87 | 88 | if self.target_index < len(self.target_list)-1: 89 | self.target_index += 1 90 | self.set_target() 91 | 92 | self.not_at_start = not self.not_at_start 93 | self.pen_down = not self.pen_down 94 | 95 | self.timer = 0 96 | self.run_timer = False 97 | self.run_timer2 = True 98 | self.controller.block_output = True 99 | 100 | print('target shown...') 101 | 102 | if self.postural == True: 103 | self.controller.additions = [] 104 | print('additional force removed...') 105 | 106 | print('write to file') 107 | for recorder in self.controller.recorders: 108 | recorder.write_out = True 109 | 110 | 111 | if self.run_timer2 == True: 112 | 113 | self.timer2 += 1 114 | 115 | if self.timer2 == self.hold_time2: 116 | # stop blocking output, let control signal be applied 117 | 118 | self.timer2 = 0 119 | self.run_timer2 = False 120 | self.controller.block_output = False 121 | 122 | print('start movement...') 123 | 124 | self.u = self.controller.control(arm) 125 | 126 | return self.u 127 | 128 | def set_target(self): 129 | """ 130 | Set the current target for the controller. 131 | """ 132 | 133 | # write to file if it's time 134 | if self.postural is not True and \ 135 | self.target_index % 3 == 0 and \ 136 | self.target_index > 0: # 24 == 0 137 | print('write to file') 138 | for recorder in self.controller.recorders: 139 | recorder.write_out = True 140 | 141 | # and then exit or go to the next target 142 | if self.target_index == len(self.target_list)-1: 143 | exit() 144 | else: 145 | target = self.target_list[self.target_index] 146 | 147 | # if it's NANs then skip to next target 148 | if target[0] != target[0]: 149 | self.target_index += 1 150 | self.set_target() 151 | else: 152 | self.controller.target = target 153 | -------------------------------------------------------------------------------- /studywolf_control/controllers/lqr.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2015 Brent Komer and Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | import control 19 | 20 | import numpy as np 21 | import scipy.linalg as sp_linalg 22 | 23 | class Control(control.Control): 24 | """ 25 | A controller that implements operational space control. 26 | Controls the (x,y) position of a robotic arm end-effector. 27 | """ 28 | def __init__(self, solve_continuous=False, **kwargs): 29 | 30 | super(Control, self).__init__(**kwargs) 31 | 32 | self.DOF = 2 # task space dimensionality 33 | self.u = None 34 | self.solve_continuous = solve_continuous 35 | 36 | if self.write_to_file is True: 37 | from recorder import Recorder 38 | # set up recorders 39 | self.u_recorder = Recorder('control signal', self.task, 'lqr') 40 | self.xy_recorder = Recorder('end-effector position', self.task, 'lqr') 41 | self.dist_recorder = Recorder('distance from target', self.task, 'lqr') 42 | self.recorders = [self.u_recorder, 43 | self.xy_recorder, 44 | self.dist_recorder] 45 | 46 | def calc_derivs(self, x, u): 47 | eps = 0.00001 # finite difference epsilon 48 | #----------- compute xdot_x and xdot_u using finite differences -------- 49 | # NOTE: here each different run is in its own column 50 | x1 = np.tile(x, (self.arm.DOF*2,1)).T + np.eye(self.arm.DOF*2) * eps 51 | x2 = np.tile(x, (self.arm.DOF*2,1)).T - np.eye(self.arm.DOF*2) * eps 52 | uu = np.tile(u, (self.arm.DOF*2,1)) 53 | f1 = self.plant_dynamics(x1, uu) 54 | f2 = self.plant_dynamics(x2, uu) 55 | xdot_x = (f1 - f2) / 2 / eps 56 | 57 | xx = np.tile(x, (self.arm.DOF,1)).T 58 | u1 = np.tile(u, (self.arm.DOF,1)) + np.eye(self.arm.DOF) * eps 59 | u2 = np.tile(u, (self.arm.DOF,1)) - np.eye(self.arm.DOF) * eps 60 | f1 = self.plant_dynamics(xx, u1) 61 | f2 = self.plant_dynamics(xx, u2) 62 | xdot_u = (f1 - f2) / 2 / eps 63 | 64 | return xdot_x, xdot_u 65 | 66 | def control(self, arm, x_des=None): 67 | """Generates a control signal to move the 68 | arm to the specified target. 69 | 70 | arm Arm: the arm model being controlled 71 | des list: the desired system position 72 | x_des np.array: desired task-space force, 73 | system goes to self.target if None 74 | """ 75 | if self.u is None: 76 | self.u = np.zeros(arm.DOF) 77 | 78 | self.Q = np.zeros((arm.DOF*2, arm.DOF*2)) 79 | self.Q[:arm.DOF, :arm.DOF] = np.eye(arm.DOF) * 1000.0 80 | self.R = np.eye(arm.DOF) * 0.001 81 | 82 | # calculate desired end-effector acceleration 83 | if x_des is None: 84 | self.x = arm.x 85 | x_des = self.x - self.target 86 | 87 | self.arm, state = self.copy_arm(arm) 88 | A, B = self.calc_derivs(state, self.u) 89 | 90 | if self.solve_continuous is True: 91 | X = sp_linalg.solve_continuous_are(A, B, self.Q, self.R) 92 | K = np.dot(np.linalg.pinv(self.R), np.dot(B.T, X)) 93 | else: 94 | X = sp_linalg.solve_discrete_are(A, B, self.Q, self.R) 95 | K = np.dot(np.linalg.pinv(self.R + np.dot(B.T, np.dot(X, B))), np.dot(B.T, np.dot(X, A))) 96 | 97 | # transform the command from end-effector space to joint space 98 | J = self.arm.gen_jacEE() 99 | 100 | u = np.hstack([np.dot(J.T, x_des), arm.dq]) 101 | 102 | self.u = -np.dot(K, u) 103 | 104 | if self.write_to_file is True: 105 | # feed recorders their signals 106 | self.u_recorder.record(0.0, self.u) 107 | self.xy_recorder.record(0.0, self.x) 108 | self.dist_recorder.record(0.0, self.target - self.x) 109 | 110 | # add in any additional signals 111 | for addition in self.additions: 112 | self.u += addition.generate(self.u, arm) 113 | 114 | return self.u 115 | 116 | def copy_arm(self, real_arm): 117 | """ Make a copy of the arm for local simulation. """ 118 | arm = real_arm.__class__() 119 | arm.dt = real_arm.dt 120 | 121 | # reset arm position to x_0 122 | arm.reset(q = real_arm.q, dq = real_arm.dq) 123 | 124 | return arm, np.hstack([real_arm.q, real_arm.dq]) 125 | 126 | def plant_dynamics(self, x, u): 127 | """ Simulate the arm dynamics locally. """ 128 | if x.ndim == 1: 129 | x = x[:,None] 130 | u = u[None,:] 131 | 132 | xnext = np.zeros((x.shape)) 133 | for ii in range(x.shape[1]): 134 | # set the arm position to x 135 | self.arm.reset(q=x[:self.arm.DOF, ii], 136 | dq=x[self.arm.DOF:self.arm.DOF*2, ii]) 137 | 138 | # apply the control signal 139 | # TODO: should we be using a constant timestep here instead of arm.dt? 140 | # to even things out when running at different dts? 141 | self.arm.apply_torque(u[ii], self.arm.dt) 142 | # get the system state from the arm 143 | xnext[:,ii] = np.hstack([np.copy(self.arm.q), 144 | np.copy(self.arm.dq)]) 145 | 146 | if self.solve_continuous is True: 147 | xdot = ((np.asarray(xnext) - np.asarray(x)) / self.arm.dt).squeeze() 148 | return xdot 149 | return xnext 150 | 151 | def gen_target(self, arm): 152 | gain = np.sum(arm.L) * .75 153 | bias = -np.sum(arm.L) * 0 154 | 155 | self.target = np.random.random(size=(2,)) * gain + bias 156 | 157 | return self.target.tolist() 158 | -------------------------------------------------------------------------------- /studywolf_control/controllers/gradient_approximation.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2016 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | import control 19 | 20 | import numpy as np 21 | import scipy.linalg as spla 22 | 23 | from arms.two_link.arm_python import Arm as Arm2_python 24 | 25 | class Control(control.Control): 26 | """ 27 | A controller that implements operational space control. 28 | Controls the (x,y) position of a robotic arm end-effector. 29 | """ 30 | def __init__(self, **kwargs): 31 | 32 | super(Control, self).__init__(**kwargs) 33 | 34 | self.DOF = 2 # task space dimensionality 35 | self.u = None 36 | 37 | # specify which gradient approximation method to use 38 | self.gradient_approximation = self.spsa 39 | 40 | if self.write_to_file is True: 41 | from recorder import Recorder 42 | # set up recorders 43 | self.xy_recorder = Recorder('end-effector position', self.task, 'gradient_spsa') 44 | self.recorders = [self.xy_recorder] 45 | 46 | def fdsa(self, x, u, ck): 47 | """ Estimate the gradient of a self.cost using 48 | Finite Differences Stochastic Approximation (FDSA). 49 | 50 | x np.array: state of the function 51 | u np.array: control signal 52 | ck: magnitude of perturbation 53 | """ 54 | gk = np.zeros(u.shape) 55 | for ii in range(gk.shape[0]): 56 | # Generate perturbations one parameter at a time 57 | inc_u = np.copy(u) 58 | inc_u[ii] += ck 59 | dec_u = np.copy(u) 60 | dec_u[ii] -= ck 61 | 62 | # Function evaluation 63 | cost_inc = self.cost(np.copy(x), inc_u) 64 | cost_dec = self.cost(np.copy(x), dec_u) 65 | 66 | # Gradient approximation 67 | gk[ii] = (cost_inc - cost_dec) / (2.0 * ck) 68 | return gk 69 | 70 | def spsa(self, x, u, ck): 71 | """ Estimate the gradient of a self.cost using 72 | Simultaneous Perturbation Stochastic Approximation (SPSA). 73 | Implemented base on (Spall, 1998). 74 | 75 | x np.array: state of the function 76 | u np.array: control signal 77 | ck: magnitude of perturbation 78 | """ 79 | # Generate simultaneous perturbation vector. 80 | # Choose each component from a bernoulli +-1 distribution 81 | # with probability of .5 for each +-1 outcome. 82 | delta_k = np.random.choice([-1,1], size=self.arm.DOF, 83 | p=[.5, .5]) 84 | 85 | # Function evaluations 86 | inc_u = np.copy(u) + ck * delta_k 87 | cost_inc = self.cost(np.copy(x), inc_u) 88 | dec_u = np.copy(u) - ck * delta_k 89 | cost_dec = self.cost(np.copy(x), dec_u) 90 | 91 | # Gradient approximation 92 | gk = np.dot((cost_inc - cost_dec) / (2.0*ck), delta_k) 93 | return gk 94 | 95 | def cost(self, x, u): 96 | """ Calculate the cost of applying u in state x. """ 97 | dt = .1 if self.arm.DOF == 3 else .01 98 | next_x = self.plant_dynamics(x, u, dt=dt) 99 | vel_gain = 100 if self.arm.DOF == 3 else 10 100 | return (np.sqrt(np.sum((self.arm.x - self.target)**2)) * 1000 \ 101 | + np.sum((next_x[self.arm.DOF:])**2) * vel_gain) 102 | 103 | def control(self, arm, x_des=None): 104 | """ Use gradient approximation to calculate a 105 | control signal that minimizes self.cost() 106 | 107 | arm Arm: the arm model being controlled 108 | x_des np.array: desired task-space force, 109 | system goes to self.target if None 110 | """ 111 | 112 | self.x = arm.x 113 | self.arm, state = self.copy_arm(arm) 114 | 115 | # Step 1: Initialization and coefficient selection 116 | max_iters = 10 117 | converge_thresh = 1e-5 118 | 119 | alpha = 0.602 # from (Spall, 1998) 120 | gamma = 0.101 121 | a = .101 # found empirically using HyperOpt 122 | A = .193 123 | c = .0277 124 | 125 | delta_K = None 126 | delta_J = None 127 | u = np.copy(self.u) if self.u is not None \ 128 | else np.zeros(self.arm.DOF) 129 | 130 | for k in range(max_iters): 131 | ak = a / (A + k + 1)**alpha 132 | ck = c / (k + 1)**gamma 133 | 134 | # Estimate gradient 135 | gk = self.gradient_approximation(state, u, ck) 136 | 137 | # Update u estimate 138 | old_u = np.copy(u) 139 | u -= ak * gk 140 | 141 | # Check for convergence 142 | if np.sum(abs(u - old_u)) < converge_thresh: 143 | break 144 | 145 | self.u = np.copy(u) 146 | 147 | if self.write_to_file is True: 148 | # feed recorders their signals 149 | self.xy_recorder.record(0.0, self.x) 150 | 151 | # add in any additional signals 152 | for addition in self.additions: 153 | self.u += addition.generate(self.u, arm) 154 | 155 | return self.u 156 | 157 | def copy_arm(self, real_arm): 158 | """ Make a copy of the arm for local simulation. """ 159 | arm = real_arm.__class__() 160 | arm.dt = real_arm.dt 161 | 162 | # reset arm position to x_0 163 | arm.reset(q = real_arm.q, dq = real_arm.dq) 164 | 165 | return arm, np.hstack([real_arm.q, real_arm.dq]) 166 | 167 | def plant_dynamics(self, x, u, dt=None): 168 | """ Simulate the arm dynamics locally. """ 169 | dt = self.arm.dt if dt is None else dt 170 | 171 | if x.ndim == 1: 172 | x = x[:,None] 173 | u = u[None,:] 174 | 175 | xnext = np.zeros((x.shape)) 176 | for ii in range(x.shape[1]): 177 | # set the arm position to x 178 | self.arm.reset(q=x[:self.arm.DOF, ii], 179 | dq=x[self.arm.DOF:self.arm.DOF*2, ii]) 180 | 181 | # apply the control signal 182 | self.arm.apply_torque(u[ii], dt) 183 | # get the system state from the arm 184 | xnext[:,ii] = np.hstack([np.copy(self.arm.q), 185 | np.copy(self.arm.dq)]) 186 | 187 | return xnext 188 | 189 | def gen_target(self, arm): 190 | pass 191 | 192 | -------------------------------------------------------------------------------- /studywolf_control/sim_and_plot.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2014 Terry Stewart and Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | 18 | import numpy as np 19 | from matplotlib import pyplot as plt 20 | from matplotlib import animation 21 | 22 | class Runner: 23 | """ 24 | A class for drawing the arm simulation. 25 | 26 | NOTE: If you're getting an error along the lines of 27 | 'xrange is not an iterator', make sure that you have 28 | the most recent version of matplotlib, from their github. 29 | """ 30 | def __init__(self, title='', dt=1e-3, control_steps=1, 31 | display_steps=1, t_target=1.0, 32 | control_type='', trajectory=None, 33 | infinite_trail=False, mouse_control=False): 34 | self.dt = dt 35 | self.control_steps = control_steps 36 | self.display_steps = display_steps 37 | self.target_steps = int(t_target/float(dt*display_steps)) 38 | self.trajectory = trajectory 39 | 40 | self.control_type = control_type 41 | self.infinite_trail = infinite_trail 42 | self.mouse_control = mouse_control 43 | self.title = title 44 | 45 | self.sim_step = 0 46 | self.trail_index = 0 47 | 48 | self.tau = None 49 | 50 | def run(self, arm, control_shell, end_time=None): 51 | 52 | self.end_time = end_time 53 | 54 | self.arm = arm 55 | if arm.DOF == 1: 56 | box = [-1, 1, -.25, 1.5] 57 | elif arm.DOF == 2: 58 | box = [-.5, .5, -.25, .75] 59 | elif arm.DOF == 3: 60 | box = [-2, 2, -.5, 4] 61 | 62 | self.shell = control_shell 63 | 64 | fig = plt.figure(figsize=(5.1,5.1), dpi=None) 65 | fig.suptitle(self.title); 66 | # set the padding of the subplot explicitly 67 | fig.subplotpars.left=.1; fig.subplotpars.right=.9 68 | fig.subplotpars.bottom=.1; fig.subplotpars.top=.9 69 | 70 | ax = fig.add_subplot(1, 1, 1, 71 | xlim=(box[0], box[1]), 72 | ylim=(box[2], box[3])) 73 | ax.xaxis.grid(); ax.yaxis.grid() 74 | # make it a square plot 75 | ax.set_aspect(1) 76 | 77 | # set up plot elements 78 | self.trail, = ax.plot([], [], color='#888888', lw=3) 79 | self.arm_line, = ax.plot([], [], 'o-', mew=4, color='b', lw=5) 80 | self.target_line, = ax.plot([], [], 'r-x', mew=4) 81 | self.info = ax.text(box[0]+abs(.1*box[0]), \ 82 | box[3]-abs(.1*box[3]), \ 83 | '', va='top') 84 | self.trail_data = np.ones((self.target_steps, 2), \ 85 | dtype='float') * np.NAN 86 | 87 | if self.trajectory is not None: 88 | ax.plot(self.trajectory[:,0], self.trajectory[:,1], alpha=.3) 89 | 90 | # connect up mouse event if specified 91 | if self.mouse_control: 92 | self.target = self.shell.controller.gen_target(arm) 93 | # get pixel width of fig (-.2 for the padding) 94 | self.fig_width = (fig.get_figwidth() - .2 \ 95 | * fig.get_figwidth()) * fig.get_dpi() 96 | def move_target(event): 97 | # get mouse position and scale appropriately to convert to (x,y) 98 | if event.xdata is not None: 99 | self.target = np.array([event.xdata, event.ydata]) 100 | # set target for the controller 101 | self.shell.controller.target = self.target 102 | 103 | # hook up function to mouse event 104 | fig.canvas.mpl_connect('motion_notify_event', move_target) 105 | 106 | frames = 50 107 | 108 | anim = animation.FuncAnimation(fig, self.anim_animate, 109 | init_func=self.anim_init, frames=5000, interval=0, blit=True) 110 | 111 | self.anim = anim 112 | 113 | def make_info_text(self): 114 | text = [] 115 | text.append('t = %1.4g'%(self.sim_step*self.dt)) 116 | q_text = ' '.join('%4.3f,'%F for F in self.arm.q) 117 | text.append('q = ['+q_text+']') 118 | u_text = ' '.join('%4.3f,'%F for F in self.shell.u) 119 | text.append('u = ['+u_text+']') 120 | 121 | return '\n'.join(text) 122 | 123 | def anim_init(self): 124 | self.info.set_text('') 125 | self.arm_line.set_data([], []) 126 | self.target_line.set_data([], []) 127 | self.trail.set_data([], []) 128 | return self.arm_line, self.target_line, self.info, self.trail 129 | 130 | def anim_animate(self, i): 131 | 132 | if self.end_time is not None: 133 | # check for run time 134 | if (self.sim_step * self.dt) > self.end_time: 135 | self.anim._stop() 136 | plt.close() 137 | 138 | if self.control_type == 'random': 139 | # update target after specified period of time passes 140 | if self.sim_step % (self.target_steps*self.display_steps) == 0: 141 | self.target = self.shell.controller.gen_target(self.arm) 142 | else: 143 | self.target = self.shell.controller.target 144 | 145 | # before drawing 146 | for j in range(self.display_steps): 147 | # update control signal 148 | if (self.sim_step % self.control_steps) == 0 or self.tau is None: 149 | self.tau = self.shell.control(self.arm) 150 | # apply control signal and simulate 151 | self.arm.apply_torque(u=self.tau, dt=self.dt) 152 | 153 | self.sim_step +=1 154 | 155 | # update figure 156 | self.arm_line.set_data(*self.arm.position()) 157 | self.info.set_text(self.make_info_text()) 158 | self.trail.set_data(self.trail_data[:,0], self.trail_data[:,1]) 159 | if self.target is not None: 160 | target = self.target 161 | self.target_line.set_data(target) 162 | 163 | # update hand trail 164 | if self.shell.pen_down: 165 | if self.infinite_trail: 166 | # if we're writing, keep all pen_down history 167 | self.trail_index += 1 168 | 169 | # if we've hit the end of the trail, double it and copy 170 | if self.trail_index >= self.trail_data.shape[0]-1: 171 | trail_data = np.zeros((self.trail_data.shape[0]*2, 172 | self.trail_data.shape[1]))*np.nan 173 | trail_data[:self.trail_index+1] = self.trail_data 174 | self.trail_data = trail_data 175 | 176 | self.trail_data[self.trail_index] = \ 177 | self.arm_line.get_xydata()[-1] 178 | else: 179 | # else just use a buffer window 180 | self.trail_data[:-1] = self.trail_data[1:] 181 | self.trail_data[-1] = self.arm_line.get_xydata()[-1] 182 | else: 183 | # if pen up add a break in the trail 184 | self.trail_data[self.trail_index] = [np.nan, np.nan] 185 | 186 | return self.target_line, self.info, self.trail, self.arm_line 187 | 188 | def show(self): 189 | try: 190 | plt.show() 191 | except AttributeError: 192 | pass 193 | -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/arm.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2013 Travis DeWolf 3 | 4 | This program is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | This program is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with this program. If not, see . 16 | ''' 17 | import importlib 18 | 19 | from ..ArmBase import ArmBase 20 | 21 | import numpy as np 22 | 23 | 24 | class Arm(ArmBase): 25 | """A wrapper around a MapleSim generated C simulation 26 | of a three link arm.""" 27 | 28 | def __init__(self, init_q=[np.pi/5.5, np.pi/1.7, np.pi/6.], 29 | init_dq=[0., 0., 0.], **kwargs): 30 | 31 | self.DOF = 3 32 | ArmBase.__init__(self, init_q=init_q, init_dq=init_dq, 33 | **kwargs) 34 | 35 | # build the arm you would like to use by editing 36 | # the setup file to import the desired model and running 37 | # python setup.py build_ext -i 38 | # name the resulting .so file to match and go 39 | arm_import_name = 'arms.three_link.py3LinkArm' 40 | arm_import_name = (arm_import_name if self.options is None 41 | else '_' + self.options) 42 | print(arm_import_name) 43 | pyArm = importlib.import_module(name=arm_import_name) 44 | 45 | # length of arm links 46 | l1 = 2.0 47 | l2 = 1.2 48 | l3 = .7 49 | self.L = np.array([l1, l2, l3]) 50 | # mass of links 51 | m1 = 10 52 | m2 = m1 53 | m3 = m1 54 | # inertia moment of links 55 | izz1 = 100 56 | izz2 = 100 57 | izz3 = 100 58 | # create mass matrices at COM for each link 59 | self.M1 = np.zeros((6, 6)) 60 | self.M2 = np.zeros((6, 6)) 61 | self.M3 = np.zeros((6, 6)) 62 | self.M1[0:3, 0:3] = np.eye(3)*m1 63 | self.M1[5, 5] = izz1 64 | self.M2[0:3, 0:3] = np.eye(3)*m2 65 | self.M2[5, 5] = izz2 66 | self.M3[0:3, 0:3] = np.eye(3)*m3 67 | self.M3[5, 5] = izz3 68 | if self.options == 'smallmass': 69 | self.M1 *= .001 70 | self.M2 *= .001 71 | self.M3 *= .001 72 | 73 | self.rest_angles = np.array([np.pi/4.0, np.pi/4.0, np.pi/4.0]) 74 | 75 | # stores information returned from maplesim 76 | self.state = np.zeros(7) 77 | # maplesim arm simulation 78 | self.sim = pyArm.pySim(dt=1e-5) 79 | self.sim.reset(self.state) 80 | self.reset() 81 | self.update_state() 82 | 83 | def apply_torque(self, u, dt=None): 84 | """Takes in a torque and timestep and updates the 85 | arm simulation accordingly. 86 | 87 | u np.array: the control signal to apply 88 | dt float: the timestep 89 | """ 90 | if dt is None: 91 | dt = self.dt 92 | 93 | u = -1 * np.array(u, dtype='float') 94 | 95 | for ii in range(int(np.ceil(dt/1e-5))): 96 | self.sim.step(self.state, u) 97 | self.update_state() 98 | 99 | def gen_jacCOM1(self, q=None): 100 | """Generates the Jacobian from the COM of the first 101 | link to the origin frame""" 102 | q = self.q if q is None else q 103 | 104 | JCOM1 = np.zeros((6, 3)) 105 | JCOM1[0, 0] = self.L[0] / 2. * -np.sin(q[0]) 106 | JCOM1[1, 0] = self.L[0] / 2. * np.cos(q[0]) 107 | JCOM1[5, 0] = 1.0 108 | 109 | return JCOM1 110 | 111 | def gen_jacCOM2(self, q=None): 112 | """Generates the Jacobian from the COM of the second 113 | link to the origin frame""" 114 | q = self.q if q is None else q 115 | q0 = q[0] 116 | q01 = q[0] + q[1] 117 | 118 | JCOM2 = np.zeros((6, 3)) 119 | # define column entries right to left 120 | JCOM2[0, 1] = self.L[1] / 2. * -np.sin(q01) 121 | JCOM2[1, 1] = self.L[1] / 2. * np.cos(q01) 122 | JCOM2[5, 1] = 1.0 123 | 124 | JCOM2[0, 0] = self.L[0] * -np.sin(q0) + JCOM2[0, 1] 125 | JCOM2[1, 0] = self.L[0] * np.cos(q0) + JCOM2[1, 1] 126 | JCOM2[5, 0] = 1.0 127 | 128 | return JCOM2 129 | 130 | def gen_jacCOM3(self, q=None): 131 | """Generates the Jacobian from the COM of the third 132 | link to the origin frame""" 133 | q = self.q if q is None else q 134 | 135 | q0 = q[0] 136 | q01 = q[0] + q[1] 137 | q012 = q[0] + q[1] + q[2] 138 | 139 | JCOM3 = np.zeros((6, 3)) 140 | # define column entries right to left 141 | JCOM3[0, 2] = self.L[2] / 2. * -np.sin(q012) 142 | JCOM3[1, 2] = self.L[2] / 2. * np.cos(q012) 143 | JCOM3[5, 2] = 1.0 144 | 145 | JCOM3[0, 1] = self.L[1] * -np.sin(q01) + JCOM3[0, 2] 146 | JCOM3[1, 1] = self.L[1] * np.cos(q01) + JCOM3[1, 2] 147 | JCOM3[5, 1] = 1.0 148 | 149 | JCOM3[0, 0] = self.L[0] * -np.sin(q0) + JCOM3[0, 1] 150 | JCOM3[1, 0] = self.L[0] * np.cos(q0) + JCOM3[1, 1] 151 | JCOM3[5, 0] = 1.0 152 | 153 | return JCOM3 154 | 155 | def gen_jacEE(self, q=None): 156 | """Generates the Jacobian from end-effector to 157 | the origin frame""" 158 | q = self.q if q is None else q 159 | 160 | q0 = q[0] 161 | q01 = q[0] + q[1] 162 | q012 = q[0] + q[1] + q[2] 163 | 164 | JEE = np.zeros((2, 3)) 165 | 166 | l3 = self.L[2] 167 | l2 = self.L[1] 168 | l1 = self.L[0] 169 | 170 | # define column entries right to left 171 | JEE[0, 2] = l3 * -np.sin(q012) 172 | JEE[1, 2] = l3 * np.cos(q012) 173 | 174 | JEE[0, 1] = l2 * -np.sin(q01) + JEE[0, 2] 175 | JEE[1, 1] = l2 * np.cos(q01) + JEE[1, 2] 176 | 177 | JEE[0, 0] = l1 * -np.sin(q0) + JEE[0, 1] 178 | JEE[1, 0] = l1 * np.cos(q0) + JEE[1, 1] 179 | 180 | return JEE 181 | 182 | def gen_Mq(self, q=None): 183 | """Generates the mass matrix of the arm in joint space""" 184 | 185 | # get the instantaneous Jacobians 186 | JCOM1 = self.gen_jacCOM1(q=q) 187 | JCOM2 = self.gen_jacCOM2(q=q) 188 | JCOM3 = self.gen_jacCOM3(q=q) 189 | 190 | M1 = self.M1 191 | M2 = self.M2 192 | M3 = self.M3 193 | # generate the mass matrix in joint space 194 | Mq = (np.dot(JCOM1.T, np.dot(M1, JCOM1)) + 195 | np.dot(JCOM2.T, np.dot(M2, JCOM2)) + 196 | np.dot(JCOM3.T, np.dot(M3, JCOM3))) 197 | 198 | return Mq 199 | 200 | def inv_kinematics(self, xy): 201 | """Calculate the joint angles for a given (x,y) 202 | hand position, using minimal distance to resting 203 | joint angles to solve kinematic redundancies.""" 204 | import scipy.optimize 205 | 206 | # function to optimize 207 | def distance_to_default(q, *args): 208 | # weights found with trial and error, 209 | # get some wrist bend, but not much 210 | weight = [1, 1, 1.3] 211 | return np.sqrt(np.sum([(qi - q0i)**2 * wi 212 | for qi, q0i, wi in zip(q, 213 | self.rest_angles, 214 | weight)])) 215 | 216 | # constraint functions 217 | def x_constraint(q, xy): 218 | x = (self.L[0]*np.cos(q[0]) + self.L[1]*np.cos(q[0]+q[1]) + 219 | self.L[2]*np.cos(np.sum(q))) - xy[0] 220 | return x 221 | 222 | def y_constraint(q, xy): 223 | y = (self.L[0]*np.sin(q[0]) + self.L[1]*np.sin(q[0]+q[1]) + 224 | self.L[2]*np.sin(np.sum(q))) - xy[1] 225 | return y 226 | 227 | return scipy.optimize.fmin_slsqp( 228 | func=distance_to_default, 229 | x0=self.rest_angles, eqcons=[x_constraint, y_constraint], 230 | args=((xy[0], xy[1]),), iprint=0) 231 | 232 | def position(self, q=None): 233 | """Compute x,y position of the hand 234 | 235 | q np.array: a set of angles to return positions for 236 | """ 237 | if q is None: 238 | q0 = self.q[0] 239 | q1 = self.q[1] 240 | q2 = self.q[2] 241 | else: 242 | q0 = q[0] 243 | q1 = q[1] 244 | q2 = q[2] 245 | 246 | x = np.cumsum([0, 247 | self.L[0] * np.cos(q0), 248 | self.L[1] * np.cos(q0+q1), 249 | self.L[2] * np.cos(q0+q1+q2)]) 250 | y = np.cumsum([0, 251 | self.L[0] * np.sin(q0), 252 | self.L[1] * np.sin(q0+q1), 253 | self.L[2] * np.sin(q0+q1+q2)]) 254 | return np.array([x, y]) 255 | 256 | def reset(self, q=[], dq=[]): 257 | if isinstance(q, np.ndarray): 258 | q = q.tolist() 259 | if isinstance(dq, np.ndarray): 260 | dq = dq.tolist() 261 | 262 | if q: 263 | assert len(q) == self.DOF 264 | if dq: 265 | assert len(dq) == self.DOF 266 | 267 | state = np.zeros(self.DOF*2) 268 | state[::2] = self.init_q if not q else np.copy(q) 269 | state[1::2] = self.init_dq if not dq else np.copy(dq) 270 | 271 | self.sim.reset(self.state, state) 272 | self.update_state() 273 | 274 | def update_state(self): 275 | """Update the local variables""" 276 | self.t = self.state[0] 277 | self.q = self.state[1:4] 278 | self.dq = self.state[4:] 279 | -------------------------------------------------------------------------------- /studywolf_control/arms/one_link/onelinkarm.cpp: -------------------------------------------------------------------------------- 1 | /*************************************************** 2 | * Automatically generated by Maple. 3 | * Created On: Wed Oct 16 15:29:00 2013. 4 | ***************************************************/ 5 | #ifdef WMI_WINNT 6 | #define EXP __declspec(dllexport) 7 | #else 8 | #define EXP 9 | #endif 10 | #include 11 | #include 12 | #include 13 | #include "mplshlib.h" 14 | static MKernelVector kv; 15 | EXP ALGEB M_DECL SetKernelVector(MKernelVector kv_in, ALGEB args) { kv=kv_in; return(kv->toMapleNULL()); } 16 | 17 | /*************************************************** 18 | * Variable Definition for System: 19 | 20 | * State variable(s): 21 | * x[ 0] = `Main.DFPSubsys1inst.theta_arm1_R1`(t) 22 | * x[ 1] = diff(`Main.DFPSubsys1inst.theta_arm1_R1`(t),t) 23 | * 24 | * Output variable(s): 25 | * y[ 0] = `Main.DFPSubsys1inst.theta_arm1_R1`(t) 26 | * y[ 1] = diff(`Main.DFPSubsys1inst.theta_arm1_R1`(t),t) 27 | * 28 | * Input variable(s): 29 | * u[ 0] = `Main.'arm1::u1'`(t) 30 | * 31 | ************************************************/ 32 | 33 | /* Fixed parameters */ 34 | #define NDIFF 2 35 | #define NDFA 2 36 | #define NEQ 3 37 | #define NPAR 0 38 | #define NINP 1 39 | #define NDISC 0 40 | #define NIX1 1 41 | #define NOUT 2 42 | #define NCON 0 43 | #define NEVT 0 44 | #ifdef EVTHYST 45 | #define NZC 2*NEVT 46 | #else 47 | #define NZC NEVT 48 | #endif 49 | 50 | typedef struct { 51 | double h; /* Integration step size */ 52 | double *w; /* Float workspace */ 53 | long *iw; /* Integer workspace */ 54 | long err; /* Error flag */ 55 | char *buf; /* Error message */ 56 | } SolverStruct; 57 | 58 | static void SolverError(SolverStruct *S, char *errmsg) 59 | { 60 | sprintf(S->buf,"Error at t=%20.16e: %s\n",S->w[0],errmsg); 61 | if(S->err==-1) kv->error(S->buf); 62 | S->err=1; 63 | } 64 | 65 | static double dsn_zero=0.0; 66 | static unsigned char dsn_undefC[8] = { 0, 0, 0, 0, 0, 0, 0xF8, 0x7F }; 67 | static double *dsn_undef = (double *)&dsn_undefC; 68 | static unsigned char dsn_posinfC[8] = { 0, 0, 0, 0, 0, 0, 0xF0, 0x7F }; 69 | static double *dsn_posinf = (double *)&dsn_posinfC; 70 | static unsigned char dsn_neginfC[8] = { 0, 0, 0, 0, 0, 0, 0xF0, 0xFF }; 71 | static double *dsn_neginf = (double *)&dsn_neginfC; 72 | #define trunc(v) ( (v>0.0) ? floor(v) : ceil(v) ) 73 | 74 | 75 | static void fp(long N, double T, double *Y, double *YP) 76 | { 77 | double Z[2], m, v; 78 | 79 | YP[0] = Y[1]; 80 | Z[0] = sin(Y[0]); 81 | Z[1] = cos(Y[0]); 82 | m = 1.+Z[0]*Z[0]+Z[1]*Z[1]; 83 | v = -Y[2]; 84 | YP[1] = v/m; 85 | } 86 | 87 | static void inpfn(double T, double *U) 88 | { 89 | U[0] = 0.; 90 | } 91 | 92 | static void SolverUpdate(double *u, double *p, long first, long internal, SolverStruct *S) 93 | { 94 | long i; 95 | 96 | //inpfn(S->w[0],u); 97 | for(i=0; iw[i+NDIFF+NIX1-NINP+1]=u[i]; 98 | fp(NEQ,S->w[0],&S->w[1],&S->w[NEQ+NPAR+1]); 99 | if(S->w[NEQ+NPAR+1]-S->w[NEQ+NPAR+1]!=0.0) { 100 | SolverError(S,"index-1 and derivative evaluation failure"); 101 | return; 102 | } 103 | if(internal) return; 104 | } 105 | 106 | static void SolverOutputs(double *y, SolverStruct *S) 107 | { 108 | y[ 0]=S->w[ 1]; 109 | y[ 1]=S->w[ 2]; 110 | } 111 | 112 | static void EulerStep(double *u, SolverStruct *S) 113 | { 114 | long i; 115 | 116 | S->w[0]+=S->h; 117 | for(i=1;i<=NDIFF;i++) S->w[i]+=S->h*S->w[NEQ+NPAR+i]; 118 | SolverUpdate(u,NULL,0,0,S); 119 | } 120 | 121 | static void SolverSetup(double t0, double *ic, double *u, double *p, double *y, double h, SolverStruct *S) 122 | { 123 | long i; 124 | 125 | S->h = h; 126 | S->iw=NULL; 127 | S->w[0] = t0; 128 | S->w[1] = 7.85398163397448279e-01; 129 | S->w[2] = 0.00000000000000000e+00; 130 | S->w[3] = 1.00000000000000000e+00; 131 | if(ic) for(i=0;iw[i+1]=ic[i]; 133 | S->w[i+NEQ+NPAR+1]=0.0; 134 | } 135 | SolverUpdate(u,p,1,0,S); 136 | SolverOutputs(y,S); 137 | } 138 | 139 | /* 140 | Parametrized simulation driver 141 | */ 142 | EXP long M_DECL ParamDriverC(double t0, double dt, long npts, double *ic, double *p, double *out, char *errbuf, long internal) 143 | { 144 | double u[NINP],y[NOUT],w[1+2*NEQ+NPAR+NDFA+NEVT]; 145 | long i,j; 146 | SolverStruct S; 147 | 148 | /* Setup */ 149 | for(i=0;i0 ) break; 161 | /* Output */ 162 | SolverOutputs(y,&S); 163 | out[i*(NOUT+1)]=S.w[0]; for(j=0;jnumArgs((ALGEB)args); 179 | if( nargs<5 || nargs>6 ) 180 | kv->error("incorrect number of arguments"); 181 | 182 | /* Process time vals */ 183 | if( !kv->isNumeric(args[1]) ) 184 | kv->error("argument #1, the initial time, must be numeric"); 185 | t0=kv->mapleToFloat64(args[1]); 186 | if( !kv->isNumeric(args[2]) ) 187 | kv->error("argument #2, the final time, must be numeric"); 188 | tf=kv->mapleToFloat64(args[2]); 189 | if( t0>=tf ) 190 | kv->error("the final time must be larger than the initial time"); 191 | if( !kv->isNumeric(args[3]) ) 192 | kv->error("argument #3, the time step, must be a positive numeric value"); 193 | dt=kv->mapleToFloat64(args[3]); 194 | if(dt<=0) 195 | kv->error("argument #3, the time step, must be a positive numeric value"); 196 | npts=(M_INT)ceil((tf+1e-10-t0)/dt)+1; 197 | 198 | /* Processing ic in */ 199 | if( NDIFF==0 ) 200 | ic=NULL; 201 | else if( kv->isInteger(args[4]) && kv->mapleToInteger32(args[4])==0 ) 202 | ic=NULL; 203 | else if( !kv->isRTable(args[4]) ) { 204 | ic=NULL; 205 | kv->error("argument #4, the initial data, must be a 1..ndiff rtable"); 206 | } 207 | else { 208 | kv->rtableGetSettings(&s,args[4]); 209 | if( s.storage != RTABLE_RECT || s.data_type != RTABLE_FLOAT64 || 210 | s.num_dimensions != 1 || kv->rtableLowerBound(args[4],1)!=1 || 211 | kv->rtableUpperBound(args[4],1) != NDIFF ) 212 | kv->error("argument #4, the initial data, must be a 1..ndiff rtable"); 213 | ic=(double *)kv->rtableData(args[4]); 214 | } 215 | 216 | /* Processing parameters in */ 217 | if( NPAR==0 ) 218 | p=NULL; 219 | else if( kv->isInteger(args[5]) && kv->mapleToInteger32(args[5])==0 ) 220 | p=NULL; 221 | else if( !kv->isRTable(args[5]) ) { 222 | p=NULL; 223 | kv->error("argument #5, the parameter data, must be a 1..npar rtable"); 224 | } 225 | else { 226 | kv->rtableGetSettings(&s,args[5]); 227 | if( s.storage != RTABLE_RECT || s.data_type != RTABLE_FLOAT64 || 228 | s.num_dimensions != 1 || kv->rtableLowerBound(args[5],1)!=1 || 229 | kv->rtableUpperBound(args[5],1) != NPAR ) 230 | kv->error("argument #5, the parameter data, must be a 1..npar rtable"); 231 | p=(double *)kv->rtableData(args[5]); 232 | } 233 | 234 | /* Output data table */ 235 | if( nargs==6 ) { 236 | outd=NULL; 237 | if( !kv->isRTable(args[6]) ) { 238 | out=NULL; 239 | naout=0; 240 | kv->error("argument #6, the output data, must be a 1..npts,1..nout+1 C_order rtable"); 241 | } 242 | else { 243 | kv->rtableGetSettings(&s,args[6]); 244 | if( s.storage != RTABLE_RECT || s.data_type != RTABLE_FLOAT64 || 245 | s.order != RTABLE_C || s.num_dimensions != 2 || 246 | kv->rtableLowerBound(args[6],1)!=1 || 247 | kv->rtableLowerBound(args[6],2)!=1 || 248 | kv->rtableUpperBound(args[6],2) != NOUT+1 ) 249 | kv->error("argument #6, the output data, must be a 1..npts,1..nout+1 C_order rtable"); 250 | naout=kv->rtableUpperBound(args[6],1); 251 | if( naout<1 ) 252 | kv->error("argument #6, the output data, must have at least 1 output slot"); 253 | out=(double *)kv->rtableData(args[6]); 254 | if(naoutrtableGetDefaults(&s); 259 | bounds[0]=1; bounds[1]=npts; 260 | bounds[2]=1; bounds[3]=NOUT+1; 261 | s.storage=RTABLE_RECT; 262 | s.data_type=RTABLE_FLOAT64; 263 | s.order=RTABLE_C; 264 | s.num_dimensions=2; 265 | s.subtype=RTABLE_ARRAY; 266 | outd=kv->rtableCreate(&s,NULL,bounds); 267 | out=(double *)kv->rtableData(outd); 268 | naout=npts; 269 | } 270 | for(i=0;itoMapleInteger(i)); 277 | else 278 | return(outd); 279 | } 280 | 281 | 282 | 283 | /* A class to contain all the information that needs to 284 | be passed around between these functions, and can 285 | encapsulate it and hide it from the Python interface. 286 | 287 | Written by Travis DeWolf (May, 2013) 288 | */ 289 | class Sim { 290 | /* Very simple class, just stores the variables we 291 | need for simulation, and has 2 functions. Reset 292 | resets the state of the simulation, and step steps it 293 | forward. Tautology ftw!*/ 294 | 295 | double* params; 296 | double dt, t0; 297 | double u0[NINP], other_out[NOUT+1], y[NOUT]; 298 | double w[5 + 2 * NEQ + NPAR + NDFA + NEVT]; 299 | 300 | SolverStruct S; 301 | 302 | public: 303 | Sim(double dt_val, double* params_pointer); 304 | void reset(double* out, double* ic); 305 | void step(double* out, double* u); 306 | }; 307 | 308 | Sim::Sim(double dt_val, double* params_pointer) 309 | { 310 | t0 = 0.0; // set up start time 311 | dt = dt_val; // set time step 312 | for (int i = 0; i < NINP; i++) u0[i] = 0.0; // initial control signal 313 | 314 | params = params_pointer; // set up parameters reference 315 | 316 | /* Setup */ 317 | S.w = w; 318 | S.err = 0; 319 | } 320 | 321 | void Sim::reset(double* out, double* ic) 322 | { 323 | SolverSetup(t0, ic, u0, params, y, dt, &S); 324 | 325 | /* Output */ 326 | out[0] = t0; 327 | for(int j = 0; j < NOUT; j++) { 328 | out[j + 1] = y[j]; 329 | } 330 | } 331 | 332 | void Sim::step(double* out, double* u) 333 | /* u: control signal */ 334 | { 335 | for (int k = 0; k < NOUT; k++) 336 | out[k] = *dsn_undef; // clear values to nan 337 | 338 | /* Integration loop */ 339 | /* Take a step with states */ 340 | EulerStep(u, &S); 341 | 342 | if (S.err <= 0) 343 | { 344 | /* Output */ 345 | SolverOutputs(y, &S); 346 | 347 | out[0] = S.w[0]; 348 | for(long j = 0; j < NOUT; j++) 349 | out[j + 1] = y[j]; 350 | } 351 | } 352 | -------------------------------------------------------------------------------- /studywolf_control/arms/one_link/mpltable.h: -------------------------------------------------------------------------------- 1 | /* Maple RTable Definitions Header File 2 | Copyright (c) 1998 by Waterloo Maple, Inc. 3 | All rights reserved. Unauthorized duplication prohibited. 4 | Written June 1998 by Paul DeMarco. 5 | 6 | *** THIS FILE IS INTENDED FOR DISTRIBUTION WITH THE MAPLE PRODUCT *** 7 | 8 | This file provides rtable definitions required to implement shared modules 9 | that can be linked into a running Maple kernel. Typically, such modules are 10 | generated automatically by Maple's define_external function, and are then 11 | used to call other non-Maple shared modules. However, a user could, in 12 | principle, write a Maple-specific shared module that doesn't require this 13 | layering. */ 14 | 15 | #ifndef __MPLTABLE_H__ 16 | #define __MPLTABLE_H__ 17 | 18 | /* Values for the RTABLE_DATATYPE sub-field. */ 19 | #define RTABLE_DAG 0 20 | #define RTABLE_CXDAG 1 21 | #define RTABLE_INTEGER8 2 22 | #define RTABLE_INTEGER16 3 23 | #define RTABLE_INTEGER32 4 24 | #define RTABLE_INTEGER64 5 25 | #define RTABLE_FLOAT 6 26 | #define RTABLE_FLOAT64 6 27 | #define RTABLE_COMPLEX 7 28 | #define RTABLE_FLOAT32 8 29 | #define RTABLE_NUM_DATATYPES 9 30 | 31 | #define RTABLE_UINTEGER8 9 32 | #define RTABLE_UINTEGER16 10 33 | #define RTABLE_UINTEGER32 11 34 | #define RTABLE_UINTEGER64 12 35 | 36 | /* Datatypes that match above sizes. */ 37 | #define INTEGER8 signed char 38 | #define INTEGER16 short 39 | #define UINTEGER16 unsigned INTEGER16 40 | #define INTEGER32 int 41 | #define UINTEGER32 unsigned INTEGER32 42 | 43 | #if defined _MSC_VER 44 | # define INTEGER64 __int64 45 | #elif defined __alpha \ 46 | || defined __ia64__ \ 47 | || defined __x86_64__ \ 48 | || defined __ppc64__ /* 64-bit Architectures (non-Windows) */ 49 | # define INTEGER64 long 50 | #else 51 | # define INTEGER64 long long 52 | #endif 53 | #define FLOAT32 float 54 | #define FLOAT64 double 55 | #define COMPLEXF32 ComplexFloat32 56 | #define COMPLEXF64 ComplexFloat64 57 | #define CXDAG ComplexFloatDAG 58 | 59 | /* Sizes for integer types. */ 60 | #define MAX_INTEGER8 127 61 | #define MIN_INTEGER8 (-MAX_INTEGER8-1) 62 | #define MAX_INTEGER16 32767 63 | #define MIN_INTEGER16 (-MAX_INTEGER16-1) 64 | #define MAX_INTEGER32 2147483647 65 | #define MIN_INTEGER32 (-MAX_INTEGER32-1) 66 | #if defined NO_INTEGER64 67 | # define MAX_INTEGER64 MAX_INTEGER32 68 | # define MIN_INTEGER64 MIN_INTEGER32 69 | #elif defined _MSC_VER 70 | # define MAX_INTEGER64 9223372036854775807 71 | # define MIN_INTEGER64 (-MAX_INTEGER64-1) 72 | #else 73 | # define MAX_INTEGER64 9223372036854775807LL 74 | # define MIN_INTEGER64 (-MAX_INTEGER64-1) 75 | #endif 76 | 77 | /* Values for the RTABLE_SUBTYPE sub-field. */ 78 | #define RTABLE_ARRAY 0 79 | #define RTABLE_MATRIX 1 80 | #define RTABLE_COLUMN 2 81 | #define RTABLE_ROW 3 82 | 83 | #define RTABLE_VECTOR(rt) (RTABLE_SUBTYPE(rt) > RTABLE_MATRIX) 84 | 85 | /* Values for the RTABLE_STORAGE sub-field. */ 86 | #define RTABLE_SPARSE 0 87 | #define RTABLE_EMPTY 1 88 | #define RTABLE_DIAG 2 89 | #define RTABLE_BAND 3 90 | #define RTABLE_RECT 4 91 | #define RTABLE_UPPTRI 5 92 | #define RTABLE_UPPTRIMINUS 6 93 | #define RTABLE_UPPTRIPLUS 7 94 | #define RTABLE_LOWTRI 8 95 | #define RTABLE_LOWTRIMINUS 9 96 | #define RTABLE_LOWTRIPLUS 10 97 | #define RTABLE_SCALAR 11 98 | 99 | /* Sparse-Storage Qualifiers */ 100 | #define RTABLE_SPARSE_DEFAULT 0 101 | #define RTABLE_SPARSE_UPPER 1 102 | #define RTABLE_SPARSE_LOWER 2 103 | 104 | #define RTABLE_IS_SPARSE(rt) (RTABLE_STORAGE(rt)==RTABLE_SPARSE) 105 | 106 | /* Values for the RTABLE_ORDER sub-field. */ 107 | #define RTABLE_FORTRAN 0 108 | #define RTABLE_C 1 109 | 110 | /* The RTABLE_READONLY and RTABLE_FOREIGN sub-fields are TRUE or FALSE. */ 111 | 112 | /* Kernel-defined indexing functions */ 113 | #define NON_KERNEL_INDFN -1 114 | #define RTABLE_INDEX_ZERO 0 115 | #define RTABLE_INDEX_BAND 1 116 | #define RTABLE_INDEX_CONSTANT 2 117 | #define RTABLE_INDEX_DIAGONAL 3 118 | #define RTABLE_INDEX_SCALAR 4 119 | #define RTABLE_INDEX_ANTIHERMITIAN 5 120 | #define RTABLE_INDEX_HERMITIAN 6 121 | #define RTABLE_INDEX_UPPHESSEN 7 122 | #define RTABLE_INDEX_LOWHESSEN 8 123 | #define RTABLE_INDEX_IDENTITY 9 124 | #define RTABLE_INDEX_ANTISYMMETRIC 10 125 | #define RTABLE_INDEX_SYMMETRIC 11 126 | #define RTABLE_INDEX_LOWTRI 12 127 | #define RTABLE_INDEX_UPPTRI 13 128 | #define RTABLE_INDEX_UNIUPPTRI 14 129 | #define RTABLE_INDEX_UNILOWTRI 15 130 | #define RTABLE_INDEX_UNIT 16 131 | #define RTABLE_INDEX_NONE 17 132 | 133 | /* Restrictions. */ 134 | #define RTABLE_MAX_DIMS 63 135 | #define RTABLE_MAX_BOUNDS (2*RTABLE_MAX_DIMS) 136 | 137 | /* Offset Macros */ 138 | #define INDEX_NOT_IN_STORAGE -1 139 | 140 | /* Vector Offset Macros */ 141 | /* for a vector, 'V' of length m, give the offset for 'V'[i] */ 142 | /* Note, these all assume m>=i>=1 */ 143 | #define VECTOR_OFFSET_RECT(i) ((i)-1) 144 | #define VECTOR_OFFSET_EMPTY(i) (INDEX_NOT_IN_STORAGE) 145 | #define VECTOR_OFFSET_SCALAR(i) (0) 146 | #define VECTOR_OFFSET_DIAG(i) ((i)-1) 147 | 148 | /* Matrix Offset Macros */ 149 | /* for an mxn matrix, 'A', give the offset for 'A'[i,j] */ 150 | /* Note, these all assume m>=i>=1 and n>=j>=1 */ 151 | #define INDEX_NOT_IN_STORAGE -1 152 | 153 | #define MATRIX_OFFSET_FORTRAN_UPPTRI(i,j,m,n) \ 154 | (((i)>(j)) ? INDEX_NOT_IN_STORAGE \ 155 | : (((j)>(m)) ? (2*(j)-1-(m))*(m)/2+(i)-1 \ 156 | : (j)*((j)-1)/2+(i)-1)) 157 | #define MATRIX_OFFSET_C_UPPTRI(i,j,m,n) \ 158 | (((i)>(j)) ? INDEX_NOT_IN_STORAGE \ 159 | : (((i)-1)*(2*(n)+2-(i))/2+(j)-(i))) 160 | 161 | #define MATRIX_OFFSET_FORTRAN_LOWTRI(i,j,m,n) \ 162 | (((i)<(j)) ? INDEX_NOT_IN_STORAGE \ 163 | : ((j)-1)*(2*(m)+2-(j))/2+(i)-(j)) 164 | #define MATRIX_OFFSET_C_LOWTRI(i,j,m,n) \ 165 | (((i)<(j)) ? INDEX_NOT_IN_STORAGE \ 166 | : ((i)>(n)) ? (2*(i)-2+1-(n))*(n)/2+(j)-1 \ 167 | : ((i)-1)*(i)/2+(j)-1) 168 | 169 | #define MATRIX_OFFSET_FORTRAN_UPPTRIMINUS(i,j,m,n) \ 170 | (((i)>=(j)) ? INDEX_NOT_IN_STORAGE \ 171 | : ((j)>(m)) ? (2*(j)-3-(m))*(m)/2+(i)-1 \ 172 | : ((j)-1)*((j)-2)/2+(i)-1) 173 | #define MATRIX_OFFSET_C_UPPTRIMINUS(i,j,m,n) \ 174 | (((i)>=(j)) ? INDEX_NOT_IN_STORAGE \ 175 | : ((i)-1)*(2*(n)-(i))/2+(j)-(i)-1) 176 | 177 | #define MATRIX_OFFSET_FORTRAN_LOWTRIMINUS(i,j,m,n) \ 178 | (((i)<=(j)) ? INDEX_NOT_IN_STORAGE \ 179 | : ((j)-1)*(2*(m)-(j))/2+(i)-(j)-1) 180 | #define MATRIX_OFFSET_C_LOWTRIMINUS(i,j,m,n) \ 181 | (((i)<=(j)) ? INDEX_NOT_IN_STORAGE \ 182 | : ((i)>(n)) ? (2*(i)-3-(n))*(n)/2+(j)-1 \ 183 | : ((i)-1)*((i)-2)/2+(j)-1) 184 | 185 | #define MATRIX_OFFSET_FORTRAN_UPPTRIPLUS(i,j,m,n) \ 186 | (((i)>(j)+1) ? INDEX_NOT_IN_STORAGE \ 187 | : ((j)>(m)) ? (2*(j)+1-(m))*(m)/2+(i)-2 \ 188 | : (j)*((j)+1)/2+(i)-2) 189 | #define MATRIX_OFFSET_C_UPPTRIPLUS(i,j,m,n) \ 190 | (((i)>(j)+1) ? INDEX_NOT_IN_STORAGE \ 191 | : ((j)>=(i)) ? ((i)-1)*(2*(n)+4-(i))/2+(j)-(i) \ 192 | : ((i)-1)*(2*(n)+2-(i))/2 +2*(j)-(i)) 193 | 194 | #define MATRIX_OFFSET_FORTRAN_LOWTRIPLUS(i,j,m,n) \ 195 | (((j)>(i)+1) ? INDEX_NOT_IN_STORAGE \ 196 | : ((i)>=(j)) ? ((j)-1)*(2*(m)+4-(j))/2+(i)-(j) \ 197 | : ((j)-1)*(2*(m)+2-(j))/2 +2*(i)-(j)) 198 | #define MATRIX_OFFSET_C_LOWTRIPLUS(i,j,m,n) \ 199 | (((j)>(i)+1) ? INDEX_NOT_IN_STORAGE \ 200 | : ((i)>(n)) ? (2*(i)+1-(n))*(n)/2+(j)-2 \ 201 | : (i)*((i)+1)/2+(j)-2) 202 | 203 | #define MATRIX_OFFSET_FORTRAN_RECT(i,j,m,n) \ 204 | (((j)-1)*(m)+((i)-1)) 205 | #define MATRIX_OFFSET_C_RECT(i,j,m,n) \ 206 | (((i)-1)*(n)+((j)-1)) 207 | 208 | #define MATRIX_OFFSET_FORTRAN_EMPTY(i,j,m,n) \ 209 | (INDEX_NOT_IN_STORAGE) 210 | #define MATRIX_OFFSET_C_EMPTY(i,j,m,n) \ 211 | (INDEX_NOT_IN_STORAGE) 212 | 213 | #define MATRIX_OFFSET_FORTRAN_SCALAR(i,j,m,n) \ 214 | (0) 215 | #define MATRIX_OFFSET_C_SCALAR(i,j,m,n) \ 216 | (0) 217 | 218 | #define MATRIX_OFFSET_FORTRAN_DIAG(i,j,m,n) \ 219 | (((i)!=(j)) ? INDEX_NOT_IN_STORAGE \ 220 | : ((i)-1)) 221 | #define MATRIX_OFFSET_C_DIAG(i,j,m,n) \ 222 | (((i)!=(j)) ? INDEX_NOT_IN_STORAGE \ 223 | : ((i)-1)) 224 | 225 | #define MATRIX_OFFSET_FORTRAN_BAND(i,j,m,n,b1,b2) \ 226 | (((i)-(j)<-(b2) || (i)-(j)>(b1)) ? INDEX_NOT_IN_STORAGE \ 227 | : (((j)-1)*((b2)+(b1)+1)+(i)-(j)+(b2))) 228 | #define MATRIX_OFFSET_C_BAND(i,j,m,n,b1,b2) \ 229 | (((i)-(j)<-(b2) || (i)-(j)>(b1)) ? INDEX_NOT_IN_STORAGE \ 230 | : (((i)-(j)+(b2))*(n)+(j)-1)) 231 | 232 | /* Specific select/assign functions, tailored to a given rtable */ 233 | typedef void (*RTableIndexChainCallback)( ALGEB rt, void *data ); 234 | typedef ALGEB (*RTableSelectFunction)( ALGEB rt, ALGEB ind ); 235 | typedef ALGEB (*RTable1DSelectFunction)( ALGEB rt, M_INT i ); 236 | typedef ALGEB (*RTable2DSelectFunction)( ALGEB rt, M_INT i, M_INT j ); 237 | 238 | typedef ALGEB (*RTableAssignFunction)( ALGEB rt, ALGEB ind, ALGEB val ); 239 | typedef ALGEB (*RTable1DAssignFunction)( ALGEB rt, M_INT i, ALGEB val ); 240 | typedef ALGEB (*RTable2DAssignFunction)( ALGEB rt, M_INT i, M_INT j, ALGEB val ); 241 | 242 | typedef M_INT (*RTableOffsetFunction)( ALGEB rt, ALGEB ind ); 243 | typedef M_INT (*RTable1DOffsetFunction)( ALGEB rt, M_INT i ); 244 | typedef M_INT (*RTable2DOffsetFunction)( ALGEB rt, M_INT i, M_INT j ); 245 | 246 | /* Types */ 247 | typedef struct _ComplexFloat32 { 248 | FLOAT32 re, im; 249 | } ComplexFloat32; 250 | 251 | typedef struct _ComplexFloat64 { 252 | FLOAT64 re, im; 253 | } ComplexFloat64; 254 | 255 | typedef struct _ComplexFloatDAG { 256 | ALGEB re, im; 257 | } ComplexFloatDAG; 258 | 259 | typedef union { 260 | INTEGER8 int8; 261 | INTEGER16 int16; 262 | INTEGER32 int32; 263 | INTEGER64 int64; 264 | FLOAT32 float32; 265 | FLOAT64 float64; 266 | ComplexFloat64 complexf64; 267 | CXDAG cxdag; 268 | ALGEB dag; 269 | } RTableData; 270 | 271 | typedef struct { 272 | int data_type; /* type of data in the rtable */ 273 | ALGEB maple_type; /* set this if data_type is RTABLE_DAG */ 274 | int subtype; /* type of rtable -- array, matrix, vector */ 275 | int storage; /* storage layout */ 276 | M_INT p1, p2; /* optional parameters for some storage layouts */ 277 | int order; /* C or Fortran order indexing */ 278 | M_BOOL read_only; /* disallow data writes when true */ 279 | M_BOOL foreign; /* true if external program manages data memory */ 280 | int num_dimensions; /* a 2x3 rtable has 2 dimensions */ 281 | ALGEB index_functions; /* list of array indexing functions */ 282 | ALGEB attributes; /* attributes given to this rtable */ 283 | M_BOOL transpose; /* when true, transpose the matrix at creation */ 284 | ALGEB fill; /* the value for unspecified elements */ 285 | } RTableSettings; 286 | 287 | #endif /* MPLTABLE_H */ 288 | 289 | -------------------------------------------------------------------------------- /studywolf_control/arms/two_link/mpltable.h: -------------------------------------------------------------------------------- 1 | /* Maple RTable Definitions Header File 2 | Copyright (c) 1998 by Waterloo Maple, Inc. 3 | All rights reserved. Unauthorized duplication prohibited. 4 | Written June 1998 by Paul DeMarco. 5 | 6 | *** THIS FILE IS INTENDED FOR DISTRIBUTION WITH THE MAPLE PRODUCT *** 7 | 8 | This file provides rtable definitions required to implement shared modules 9 | that can be linked into a running Maple kernel. Typically, such modules are 10 | generated automatically by Maple's define_external function, and are then 11 | used to call other non-Maple shared modules. However, a user could, in 12 | principle, write a Maple-specific shared module that doesn't require this 13 | layering. */ 14 | 15 | #ifndef __MPLTABLE_H__ 16 | #define __MPLTABLE_H__ 17 | 18 | /* Values for the RTABLE_DATATYPE sub-field. */ 19 | #define RTABLE_DAG 0 20 | #define RTABLE_CXDAG 1 21 | #define RTABLE_INTEGER8 2 22 | #define RTABLE_INTEGER16 3 23 | #define RTABLE_INTEGER32 4 24 | #define RTABLE_INTEGER64 5 25 | #define RTABLE_FLOAT 6 26 | #define RTABLE_FLOAT64 6 27 | #define RTABLE_COMPLEX 7 28 | #define RTABLE_FLOAT32 8 29 | #define RTABLE_NUM_DATATYPES 9 30 | 31 | #define RTABLE_UINTEGER8 9 32 | #define RTABLE_UINTEGER16 10 33 | #define RTABLE_UINTEGER32 11 34 | #define RTABLE_UINTEGER64 12 35 | 36 | /* Datatypes that match above sizes. */ 37 | #define INTEGER8 signed char 38 | #define INTEGER16 short 39 | #define UINTEGER16 unsigned INTEGER16 40 | #define INTEGER32 int 41 | #define UINTEGER32 unsigned INTEGER32 42 | 43 | #if defined _MSC_VER 44 | # define INTEGER64 __int64 45 | #elif defined __alpha \ 46 | || defined __ia64__ \ 47 | || defined __x86_64__ \ 48 | || defined __ppc64__ /* 64-bit Architectures (non-Windows) */ 49 | # define INTEGER64 long 50 | #else 51 | # define INTEGER64 long long 52 | #endif 53 | #define FLOAT32 float 54 | #define FLOAT64 double 55 | #define COMPLEXF32 ComplexFloat32 56 | #define COMPLEXF64 ComplexFloat64 57 | #define CXDAG ComplexFloatDAG 58 | 59 | /* Sizes for integer types. */ 60 | #define MAX_INTEGER8 127 61 | #define MIN_INTEGER8 (-MAX_INTEGER8-1) 62 | #define MAX_INTEGER16 32767 63 | #define MIN_INTEGER16 (-MAX_INTEGER16-1) 64 | #define MAX_INTEGER32 2147483647 65 | #define MIN_INTEGER32 (-MAX_INTEGER32-1) 66 | #if defined NO_INTEGER64 67 | # define MAX_INTEGER64 MAX_INTEGER32 68 | # define MIN_INTEGER64 MIN_INTEGER32 69 | #elif defined _MSC_VER 70 | # define MAX_INTEGER64 9223372036854775807 71 | # define MIN_INTEGER64 (-MAX_INTEGER64-1) 72 | #else 73 | # define MAX_INTEGER64 9223372036854775807LL 74 | # define MIN_INTEGER64 (-MAX_INTEGER64-1) 75 | #endif 76 | 77 | /* Values for the RTABLE_SUBTYPE sub-field. */ 78 | #define RTABLE_ARRAY 0 79 | #define RTABLE_MATRIX 1 80 | #define RTABLE_COLUMN 2 81 | #define RTABLE_ROW 3 82 | 83 | #define RTABLE_VECTOR(rt) (RTABLE_SUBTYPE(rt) > RTABLE_MATRIX) 84 | 85 | /* Values for the RTABLE_STORAGE sub-field. */ 86 | #define RTABLE_SPARSE 0 87 | #define RTABLE_EMPTY 1 88 | #define RTABLE_DIAG 2 89 | #define RTABLE_BAND 3 90 | #define RTABLE_RECT 4 91 | #define RTABLE_UPPTRI 5 92 | #define RTABLE_UPPTRIMINUS 6 93 | #define RTABLE_UPPTRIPLUS 7 94 | #define RTABLE_LOWTRI 8 95 | #define RTABLE_LOWTRIMINUS 9 96 | #define RTABLE_LOWTRIPLUS 10 97 | #define RTABLE_SCALAR 11 98 | 99 | /* Sparse-Storage Qualifiers */ 100 | #define RTABLE_SPARSE_DEFAULT 0 101 | #define RTABLE_SPARSE_UPPER 1 102 | #define RTABLE_SPARSE_LOWER 2 103 | 104 | #define RTABLE_IS_SPARSE(rt) (RTABLE_STORAGE(rt)==RTABLE_SPARSE) 105 | 106 | /* Values for the RTABLE_ORDER sub-field. */ 107 | #define RTABLE_FORTRAN 0 108 | #define RTABLE_C 1 109 | 110 | /* The RTABLE_READONLY and RTABLE_FOREIGN sub-fields are TRUE or FALSE. */ 111 | 112 | /* Kernel-defined indexing functions */ 113 | #define NON_KERNEL_INDFN -1 114 | #define RTABLE_INDEX_ZERO 0 115 | #define RTABLE_INDEX_BAND 1 116 | #define RTABLE_INDEX_CONSTANT 2 117 | #define RTABLE_INDEX_DIAGONAL 3 118 | #define RTABLE_INDEX_SCALAR 4 119 | #define RTABLE_INDEX_ANTIHERMITIAN 5 120 | #define RTABLE_INDEX_HERMITIAN 6 121 | #define RTABLE_INDEX_UPPHESSEN 7 122 | #define RTABLE_INDEX_LOWHESSEN 8 123 | #define RTABLE_INDEX_IDENTITY 9 124 | #define RTABLE_INDEX_ANTISYMMETRIC 10 125 | #define RTABLE_INDEX_SYMMETRIC 11 126 | #define RTABLE_INDEX_LOWTRI 12 127 | #define RTABLE_INDEX_UPPTRI 13 128 | #define RTABLE_INDEX_UNIUPPTRI 14 129 | #define RTABLE_INDEX_UNILOWTRI 15 130 | #define RTABLE_INDEX_UNIT 16 131 | #define RTABLE_INDEX_NONE 17 132 | 133 | /* Restrictions. */ 134 | #define RTABLE_MAX_DIMS 63 135 | #define RTABLE_MAX_BOUNDS (2*RTABLE_MAX_DIMS) 136 | 137 | /* Offset Macros */ 138 | #define INDEX_NOT_IN_STORAGE -1 139 | 140 | /* Vector Offset Macros */ 141 | /* for a vector, 'V' of length m, give the offset for 'V'[i] */ 142 | /* Note, these all assume m>=i>=1 */ 143 | #define VECTOR_OFFSET_RECT(i) ((i)-1) 144 | #define VECTOR_OFFSET_EMPTY(i) (INDEX_NOT_IN_STORAGE) 145 | #define VECTOR_OFFSET_SCALAR(i) (0) 146 | #define VECTOR_OFFSET_DIAG(i) ((i)-1) 147 | 148 | /* Matrix Offset Macros */ 149 | /* for an mxn matrix, 'A', give the offset for 'A'[i,j] */ 150 | /* Note, these all assume m>=i>=1 and n>=j>=1 */ 151 | #define INDEX_NOT_IN_STORAGE -1 152 | 153 | #define MATRIX_OFFSET_FORTRAN_UPPTRI(i,j,m,n) \ 154 | (((i)>(j)) ? INDEX_NOT_IN_STORAGE \ 155 | : (((j)>(m)) ? (2*(j)-1-(m))*(m)/2+(i)-1 \ 156 | : (j)*((j)-1)/2+(i)-1)) 157 | #define MATRIX_OFFSET_C_UPPTRI(i,j,m,n) \ 158 | (((i)>(j)) ? INDEX_NOT_IN_STORAGE \ 159 | : (((i)-1)*(2*(n)+2-(i))/2+(j)-(i))) 160 | 161 | #define MATRIX_OFFSET_FORTRAN_LOWTRI(i,j,m,n) \ 162 | (((i)<(j)) ? INDEX_NOT_IN_STORAGE \ 163 | : ((j)-1)*(2*(m)+2-(j))/2+(i)-(j)) 164 | #define MATRIX_OFFSET_C_LOWTRI(i,j,m,n) \ 165 | (((i)<(j)) ? INDEX_NOT_IN_STORAGE \ 166 | : ((i)>(n)) ? (2*(i)-2+1-(n))*(n)/2+(j)-1 \ 167 | : ((i)-1)*(i)/2+(j)-1) 168 | 169 | #define MATRIX_OFFSET_FORTRAN_UPPTRIMINUS(i,j,m,n) \ 170 | (((i)>=(j)) ? INDEX_NOT_IN_STORAGE \ 171 | : ((j)>(m)) ? (2*(j)-3-(m))*(m)/2+(i)-1 \ 172 | : ((j)-1)*((j)-2)/2+(i)-1) 173 | #define MATRIX_OFFSET_C_UPPTRIMINUS(i,j,m,n) \ 174 | (((i)>=(j)) ? INDEX_NOT_IN_STORAGE \ 175 | : ((i)-1)*(2*(n)-(i))/2+(j)-(i)-1) 176 | 177 | #define MATRIX_OFFSET_FORTRAN_LOWTRIMINUS(i,j,m,n) \ 178 | (((i)<=(j)) ? INDEX_NOT_IN_STORAGE \ 179 | : ((j)-1)*(2*(m)-(j))/2+(i)-(j)-1) 180 | #define MATRIX_OFFSET_C_LOWTRIMINUS(i,j,m,n) \ 181 | (((i)<=(j)) ? INDEX_NOT_IN_STORAGE \ 182 | : ((i)>(n)) ? (2*(i)-3-(n))*(n)/2+(j)-1 \ 183 | : ((i)-1)*((i)-2)/2+(j)-1) 184 | 185 | #define MATRIX_OFFSET_FORTRAN_UPPTRIPLUS(i,j,m,n) \ 186 | (((i)>(j)+1) ? INDEX_NOT_IN_STORAGE \ 187 | : ((j)>(m)) ? (2*(j)+1-(m))*(m)/2+(i)-2 \ 188 | : (j)*((j)+1)/2+(i)-2) 189 | #define MATRIX_OFFSET_C_UPPTRIPLUS(i,j,m,n) \ 190 | (((i)>(j)+1) ? INDEX_NOT_IN_STORAGE \ 191 | : ((j)>=(i)) ? ((i)-1)*(2*(n)+4-(i))/2+(j)-(i) \ 192 | : ((i)-1)*(2*(n)+2-(i))/2 +2*(j)-(i)) 193 | 194 | #define MATRIX_OFFSET_FORTRAN_LOWTRIPLUS(i,j,m,n) \ 195 | (((j)>(i)+1) ? INDEX_NOT_IN_STORAGE \ 196 | : ((i)>=(j)) ? ((j)-1)*(2*(m)+4-(j))/2+(i)-(j) \ 197 | : ((j)-1)*(2*(m)+2-(j))/2 +2*(i)-(j)) 198 | #define MATRIX_OFFSET_C_LOWTRIPLUS(i,j,m,n) \ 199 | (((j)>(i)+1) ? INDEX_NOT_IN_STORAGE \ 200 | : ((i)>(n)) ? (2*(i)+1-(n))*(n)/2+(j)-2 \ 201 | : (i)*((i)+1)/2+(j)-2) 202 | 203 | #define MATRIX_OFFSET_FORTRAN_RECT(i,j,m,n) \ 204 | (((j)-1)*(m)+((i)-1)) 205 | #define MATRIX_OFFSET_C_RECT(i,j,m,n) \ 206 | (((i)-1)*(n)+((j)-1)) 207 | 208 | #define MATRIX_OFFSET_FORTRAN_EMPTY(i,j,m,n) \ 209 | (INDEX_NOT_IN_STORAGE) 210 | #define MATRIX_OFFSET_C_EMPTY(i,j,m,n) \ 211 | (INDEX_NOT_IN_STORAGE) 212 | 213 | #define MATRIX_OFFSET_FORTRAN_SCALAR(i,j,m,n) \ 214 | (0) 215 | #define MATRIX_OFFSET_C_SCALAR(i,j,m,n) \ 216 | (0) 217 | 218 | #define MATRIX_OFFSET_FORTRAN_DIAG(i,j,m,n) \ 219 | (((i)!=(j)) ? INDEX_NOT_IN_STORAGE \ 220 | : ((i)-1)) 221 | #define MATRIX_OFFSET_C_DIAG(i,j,m,n) \ 222 | (((i)!=(j)) ? INDEX_NOT_IN_STORAGE \ 223 | : ((i)-1)) 224 | 225 | #define MATRIX_OFFSET_FORTRAN_BAND(i,j,m,n,b1,b2) \ 226 | (((i)-(j)<-(b2) || (i)-(j)>(b1)) ? INDEX_NOT_IN_STORAGE \ 227 | : (((j)-1)*((b2)+(b1)+1)+(i)-(j)+(b2))) 228 | #define MATRIX_OFFSET_C_BAND(i,j,m,n,b1,b2) \ 229 | (((i)-(j)<-(b2) || (i)-(j)>(b1)) ? INDEX_NOT_IN_STORAGE \ 230 | : (((i)-(j)+(b2))*(n)+(j)-1)) 231 | 232 | /* Specific select/assign functions, tailored to a given rtable */ 233 | typedef void (*RTableIndexChainCallback)( ALGEB rt, void *data ); 234 | typedef ALGEB (*RTableSelectFunction)( ALGEB rt, ALGEB ind ); 235 | typedef ALGEB (*RTable1DSelectFunction)( ALGEB rt, M_INT i ); 236 | typedef ALGEB (*RTable2DSelectFunction)( ALGEB rt, M_INT i, M_INT j ); 237 | 238 | typedef ALGEB (*RTableAssignFunction)( ALGEB rt, ALGEB ind, ALGEB val ); 239 | typedef ALGEB (*RTable1DAssignFunction)( ALGEB rt, M_INT i, ALGEB val ); 240 | typedef ALGEB (*RTable2DAssignFunction)( ALGEB rt, M_INT i, M_INT j, ALGEB val ); 241 | 242 | typedef M_INT (*RTableOffsetFunction)( ALGEB rt, ALGEB ind ); 243 | typedef M_INT (*RTable1DOffsetFunction)( ALGEB rt, M_INT i ); 244 | typedef M_INT (*RTable2DOffsetFunction)( ALGEB rt, M_INT i, M_INT j ); 245 | 246 | /* Types */ 247 | typedef struct _ComplexFloat32 { 248 | FLOAT32 re, im; 249 | } ComplexFloat32; 250 | 251 | typedef struct _ComplexFloat64 { 252 | FLOAT64 re, im; 253 | } ComplexFloat64; 254 | 255 | typedef struct _ComplexFloatDAG { 256 | ALGEB re, im; 257 | } ComplexFloatDAG; 258 | 259 | typedef union { 260 | INTEGER8 int8; 261 | INTEGER16 int16; 262 | INTEGER32 int32; 263 | INTEGER64 int64; 264 | FLOAT32 float32; 265 | FLOAT64 float64; 266 | ComplexFloat64 complexf64; 267 | CXDAG cxdag; 268 | ALGEB dag; 269 | } RTableData; 270 | 271 | typedef struct { 272 | int data_type; /* type of data in the rtable */ 273 | ALGEB maple_type; /* set this if data_type is RTABLE_DAG */ 274 | int subtype; /* type of rtable -- array, matrix, vector */ 275 | int storage; /* storage layout */ 276 | M_INT p1, p2; /* optional parameters for some storage layouts */ 277 | int order; /* C or Fortran order indexing */ 278 | M_BOOL read_only; /* disallow data writes when true */ 279 | M_BOOL foreign; /* true if external program manages data memory */ 280 | int num_dimensions; /* a 2x3 rtable has 2 dimensions */ 281 | ALGEB index_functions; /* list of array indexing functions */ 282 | ALGEB attributes; /* attributes given to this rtable */ 283 | M_BOOL transpose; /* when true, transpose the matrix at creation */ 284 | ALGEB fill; /* the value for unspecified elements */ 285 | } RTableSettings; 286 | 287 | #endif /* MPLTABLE_H */ 288 | 289 | -------------------------------------------------------------------------------- /studywolf_control/arms/three_link/mpltable.h: -------------------------------------------------------------------------------- 1 | /* Maple RTable Definitions Header File 2 | Copyright (c) 1998 by Waterloo Maple, Inc. 3 | All rights reserved. Unauthorized duplication prohibited. 4 | Written June 1998 by Paul DeMarco. 5 | 6 | *** THIS FILE IS INTENDED FOR DISTRIBUTION WITH THE MAPLE PRODUCT *** 7 | 8 | This file provides rtable definitions required to implement shared modules 9 | that can be linked into a running Maple kernel. Typically, such modules are 10 | generated automatically by Maple's define_external function, and are then 11 | used to call other non-Maple shared modules. However, a user could, in 12 | principle, write a Maple-specific shared module that doesn't require this 13 | layering. */ 14 | 15 | #ifndef __MPLTABLE_H__ 16 | #define __MPLTABLE_H__ 17 | 18 | /* Values for the RTABLE_DATATYPE sub-field. */ 19 | #define RTABLE_DAG 0 20 | #define RTABLE_CXDAG 1 21 | #define RTABLE_INTEGER8 2 22 | #define RTABLE_INTEGER16 3 23 | #define RTABLE_INTEGER32 4 24 | #define RTABLE_INTEGER64 5 25 | #define RTABLE_FLOAT 6 26 | #define RTABLE_FLOAT64 6 27 | #define RTABLE_COMPLEX 7 28 | #define RTABLE_FLOAT32 8 29 | #define RTABLE_NUM_DATATYPES 9 30 | 31 | #define RTABLE_UINTEGER8 9 32 | #define RTABLE_UINTEGER16 10 33 | #define RTABLE_UINTEGER32 11 34 | #define RTABLE_UINTEGER64 12 35 | 36 | /* Datatypes that match above sizes. */ 37 | #define INTEGER8 signed char 38 | #define INTEGER16 short 39 | #define UINTEGER16 unsigned INTEGER16 40 | #define INTEGER32 int 41 | #define UINTEGER32 unsigned INTEGER32 42 | 43 | #if defined _MSC_VER 44 | # define INTEGER64 __int64 45 | #elif defined __alpha \ 46 | || defined __ia64__ \ 47 | || defined __x86_64__ \ 48 | || defined __ppc64__ /* 64-bit Architectures (non-Windows) */ 49 | # define INTEGER64 long 50 | #else 51 | # define INTEGER64 long long 52 | #endif 53 | #define FLOAT32 float 54 | #define FLOAT64 double 55 | #define COMPLEXF32 ComplexFloat32 56 | #define COMPLEXF64 ComplexFloat64 57 | #define CXDAG ComplexFloatDAG 58 | 59 | /* Sizes for integer types. */ 60 | #define MAX_INTEGER8 127 61 | #define MIN_INTEGER8 (-MAX_INTEGER8-1) 62 | #define MAX_INTEGER16 32767 63 | #define MIN_INTEGER16 (-MAX_INTEGER16-1) 64 | #define MAX_INTEGER32 2147483647 65 | #define MIN_INTEGER32 (-MAX_INTEGER32-1) 66 | #if defined NO_INTEGER64 67 | # define MAX_INTEGER64 MAX_INTEGER32 68 | # define MIN_INTEGER64 MIN_INTEGER32 69 | #elif defined _MSC_VER 70 | # define MAX_INTEGER64 9223372036854775807 71 | # define MIN_INTEGER64 (-MAX_INTEGER64-1) 72 | #else 73 | # define MAX_INTEGER64 9223372036854775807LL 74 | # define MIN_INTEGER64 (-MAX_INTEGER64-1) 75 | #endif 76 | 77 | /* Values for the RTABLE_SUBTYPE sub-field. */ 78 | #define RTABLE_ARRAY 0 79 | #define RTABLE_MATRIX 1 80 | #define RTABLE_COLUMN 2 81 | #define RTABLE_ROW 3 82 | 83 | #define RTABLE_VECTOR(rt) (RTABLE_SUBTYPE(rt) > RTABLE_MATRIX) 84 | 85 | /* Values for the RTABLE_STORAGE sub-field. */ 86 | #define RTABLE_SPARSE 0 87 | #define RTABLE_EMPTY 1 88 | #define RTABLE_DIAG 2 89 | #define RTABLE_BAND 3 90 | #define RTABLE_RECT 4 91 | #define RTABLE_UPPTRI 5 92 | #define RTABLE_UPPTRIMINUS 6 93 | #define RTABLE_UPPTRIPLUS 7 94 | #define RTABLE_LOWTRI 8 95 | #define RTABLE_LOWTRIMINUS 9 96 | #define RTABLE_LOWTRIPLUS 10 97 | #define RTABLE_SCALAR 11 98 | 99 | /* Sparse-Storage Qualifiers */ 100 | #define RTABLE_SPARSE_DEFAULT 0 101 | #define RTABLE_SPARSE_UPPER 1 102 | #define RTABLE_SPARSE_LOWER 2 103 | 104 | #define RTABLE_IS_SPARSE(rt) (RTABLE_STORAGE(rt)==RTABLE_SPARSE) 105 | 106 | /* Values for the RTABLE_ORDER sub-field. */ 107 | #define RTABLE_FORTRAN 0 108 | #define RTABLE_C 1 109 | 110 | /* The RTABLE_READONLY and RTABLE_FOREIGN sub-fields are TRUE or FALSE. */ 111 | 112 | /* Kernel-defined indexing functions */ 113 | #define NON_KERNEL_INDFN -1 114 | #define RTABLE_INDEX_ZERO 0 115 | #define RTABLE_INDEX_BAND 1 116 | #define RTABLE_INDEX_CONSTANT 2 117 | #define RTABLE_INDEX_DIAGONAL 3 118 | #define RTABLE_INDEX_SCALAR 4 119 | #define RTABLE_INDEX_ANTIHERMITIAN 5 120 | #define RTABLE_INDEX_HERMITIAN 6 121 | #define RTABLE_INDEX_UPPHESSEN 7 122 | #define RTABLE_INDEX_LOWHESSEN 8 123 | #define RTABLE_INDEX_IDENTITY 9 124 | #define RTABLE_INDEX_ANTISYMMETRIC 10 125 | #define RTABLE_INDEX_SYMMETRIC 11 126 | #define RTABLE_INDEX_LOWTRI 12 127 | #define RTABLE_INDEX_UPPTRI 13 128 | #define RTABLE_INDEX_UNIUPPTRI 14 129 | #define RTABLE_INDEX_UNILOWTRI 15 130 | #define RTABLE_INDEX_UNIT 16 131 | #define RTABLE_INDEX_NONE 17 132 | 133 | /* Restrictions. */ 134 | #define RTABLE_MAX_DIMS 63 135 | #define RTABLE_MAX_BOUNDS (2*RTABLE_MAX_DIMS) 136 | 137 | /* Offset Macros */ 138 | #define INDEX_NOT_IN_STORAGE -1 139 | 140 | /* Vector Offset Macros */ 141 | /* for a vector, 'V' of length m, give the offset for 'V'[i] */ 142 | /* Note, these all assume m>=i>=1 */ 143 | #define VECTOR_OFFSET_RECT(i) ((i)-1) 144 | #define VECTOR_OFFSET_EMPTY(i) (INDEX_NOT_IN_STORAGE) 145 | #define VECTOR_OFFSET_SCALAR(i) (0) 146 | #define VECTOR_OFFSET_DIAG(i) ((i)-1) 147 | 148 | /* Matrix Offset Macros */ 149 | /* for an mxn matrix, 'A', give the offset for 'A'[i,j] */ 150 | /* Note, these all assume m>=i>=1 and n>=j>=1 */ 151 | #define INDEX_NOT_IN_STORAGE -1 152 | 153 | #define MATRIX_OFFSET_FORTRAN_UPPTRI(i,j,m,n) \ 154 | (((i)>(j)) ? INDEX_NOT_IN_STORAGE \ 155 | : (((j)>(m)) ? (2*(j)-1-(m))*(m)/2+(i)-1 \ 156 | : (j)*((j)-1)/2+(i)-1)) 157 | #define MATRIX_OFFSET_C_UPPTRI(i,j,m,n) \ 158 | (((i)>(j)) ? INDEX_NOT_IN_STORAGE \ 159 | : (((i)-1)*(2*(n)+2-(i))/2+(j)-(i))) 160 | 161 | #define MATRIX_OFFSET_FORTRAN_LOWTRI(i,j,m,n) \ 162 | (((i)<(j)) ? INDEX_NOT_IN_STORAGE \ 163 | : ((j)-1)*(2*(m)+2-(j))/2+(i)-(j)) 164 | #define MATRIX_OFFSET_C_LOWTRI(i,j,m,n) \ 165 | (((i)<(j)) ? INDEX_NOT_IN_STORAGE \ 166 | : ((i)>(n)) ? (2*(i)-2+1-(n))*(n)/2+(j)-1 \ 167 | : ((i)-1)*(i)/2+(j)-1) 168 | 169 | #define MATRIX_OFFSET_FORTRAN_UPPTRIMINUS(i,j,m,n) \ 170 | (((i)>=(j)) ? INDEX_NOT_IN_STORAGE \ 171 | : ((j)>(m)) ? (2*(j)-3-(m))*(m)/2+(i)-1 \ 172 | : ((j)-1)*((j)-2)/2+(i)-1) 173 | #define MATRIX_OFFSET_C_UPPTRIMINUS(i,j,m,n) \ 174 | (((i)>=(j)) ? INDEX_NOT_IN_STORAGE \ 175 | : ((i)-1)*(2*(n)-(i))/2+(j)-(i)-1) 176 | 177 | #define MATRIX_OFFSET_FORTRAN_LOWTRIMINUS(i,j,m,n) \ 178 | (((i)<=(j)) ? INDEX_NOT_IN_STORAGE \ 179 | : ((j)-1)*(2*(m)-(j))/2+(i)-(j)-1) 180 | #define MATRIX_OFFSET_C_LOWTRIMINUS(i,j,m,n) \ 181 | (((i)<=(j)) ? INDEX_NOT_IN_STORAGE \ 182 | : ((i)>(n)) ? (2*(i)-3-(n))*(n)/2+(j)-1 \ 183 | : ((i)-1)*((i)-2)/2+(j)-1) 184 | 185 | #define MATRIX_OFFSET_FORTRAN_UPPTRIPLUS(i,j,m,n) \ 186 | (((i)>(j)+1) ? INDEX_NOT_IN_STORAGE \ 187 | : ((j)>(m)) ? (2*(j)+1-(m))*(m)/2+(i)-2 \ 188 | : (j)*((j)+1)/2+(i)-2) 189 | #define MATRIX_OFFSET_C_UPPTRIPLUS(i,j,m,n) \ 190 | (((i)>(j)+1) ? INDEX_NOT_IN_STORAGE \ 191 | : ((j)>=(i)) ? ((i)-1)*(2*(n)+4-(i))/2+(j)-(i) \ 192 | : ((i)-1)*(2*(n)+2-(i))/2 +2*(j)-(i)) 193 | 194 | #define MATRIX_OFFSET_FORTRAN_LOWTRIPLUS(i,j,m,n) \ 195 | (((j)>(i)+1) ? INDEX_NOT_IN_STORAGE \ 196 | : ((i)>=(j)) ? ((j)-1)*(2*(m)+4-(j))/2+(i)-(j) \ 197 | : ((j)-1)*(2*(m)+2-(j))/2 +2*(i)-(j)) 198 | #define MATRIX_OFFSET_C_LOWTRIPLUS(i,j,m,n) \ 199 | (((j)>(i)+1) ? INDEX_NOT_IN_STORAGE \ 200 | : ((i)>(n)) ? (2*(i)+1-(n))*(n)/2+(j)-2 \ 201 | : (i)*((i)+1)/2+(j)-2) 202 | 203 | #define MATRIX_OFFSET_FORTRAN_RECT(i,j,m,n) \ 204 | (((j)-1)*(m)+((i)-1)) 205 | #define MATRIX_OFFSET_C_RECT(i,j,m,n) \ 206 | (((i)-1)*(n)+((j)-1)) 207 | 208 | #define MATRIX_OFFSET_FORTRAN_EMPTY(i,j,m,n) \ 209 | (INDEX_NOT_IN_STORAGE) 210 | #define MATRIX_OFFSET_C_EMPTY(i,j,m,n) \ 211 | (INDEX_NOT_IN_STORAGE) 212 | 213 | #define MATRIX_OFFSET_FORTRAN_SCALAR(i,j,m,n) \ 214 | (0) 215 | #define MATRIX_OFFSET_C_SCALAR(i,j,m,n) \ 216 | (0) 217 | 218 | #define MATRIX_OFFSET_FORTRAN_DIAG(i,j,m,n) \ 219 | (((i)!=(j)) ? INDEX_NOT_IN_STORAGE \ 220 | : ((i)-1)) 221 | #define MATRIX_OFFSET_C_DIAG(i,j,m,n) \ 222 | (((i)!=(j)) ? INDEX_NOT_IN_STORAGE \ 223 | : ((i)-1)) 224 | 225 | #define MATRIX_OFFSET_FORTRAN_BAND(i,j,m,n,b1,b2) \ 226 | (((i)-(j)<-(b2) || (i)-(j)>(b1)) ? INDEX_NOT_IN_STORAGE \ 227 | : (((j)-1)*((b2)+(b1)+1)+(i)-(j)+(b2))) 228 | #define MATRIX_OFFSET_C_BAND(i,j,m,n,b1,b2) \ 229 | (((i)-(j)<-(b2) || (i)-(j)>(b1)) ? INDEX_NOT_IN_STORAGE \ 230 | : (((i)-(j)+(b2))*(n)+(j)-1)) 231 | 232 | /* Specific select/assign functions, tailored to a given rtable */ 233 | typedef void (*RTableIndexChainCallback)( ALGEB rt, void *data ); 234 | typedef ALGEB (*RTableSelectFunction)( ALGEB rt, ALGEB ind ); 235 | typedef ALGEB (*RTable1DSelectFunction)( ALGEB rt, M_INT i ); 236 | typedef ALGEB (*RTable2DSelectFunction)( ALGEB rt, M_INT i, M_INT j ); 237 | 238 | typedef ALGEB (*RTableAssignFunction)( ALGEB rt, ALGEB ind, ALGEB val ); 239 | typedef ALGEB (*RTable1DAssignFunction)( ALGEB rt, M_INT i, ALGEB val ); 240 | typedef ALGEB (*RTable2DAssignFunction)( ALGEB rt, M_INT i, M_INT j, ALGEB val ); 241 | 242 | typedef M_INT (*RTableOffsetFunction)( ALGEB rt, ALGEB ind ); 243 | typedef M_INT (*RTable1DOffsetFunction)( ALGEB rt, M_INT i ); 244 | typedef M_INT (*RTable2DOffsetFunction)( ALGEB rt, M_INT i, M_INT j ); 245 | 246 | /* Types */ 247 | typedef struct _ComplexFloat32 { 248 | FLOAT32 re, im; 249 | } ComplexFloat32; 250 | 251 | typedef struct _ComplexFloat64 { 252 | FLOAT64 re, im; 253 | } ComplexFloat64; 254 | 255 | typedef struct _ComplexFloatDAG { 256 | ALGEB re, im; 257 | } ComplexFloatDAG; 258 | 259 | typedef union { 260 | INTEGER8 int8; 261 | INTEGER16 int16; 262 | INTEGER32 int32; 263 | INTEGER64 int64; 264 | FLOAT32 float32; 265 | FLOAT64 float64; 266 | ComplexFloat64 complexf64; 267 | CXDAG cxdag; 268 | ALGEB dag; 269 | } RTableData; 270 | 271 | typedef struct { 272 | int data_type; /* type of data in the rtable */ 273 | ALGEB maple_type; /* set this if data_type is RTABLE_DAG */ 274 | int subtype; /* type of rtable -- array, matrix, vector */ 275 | int storage; /* storage layout */ 276 | M_INT p1, p2; /* optional parameters for some storage layouts */ 277 | int order; /* C or Fortran order indexing */ 278 | M_BOOL read_only; /* disallow data writes when true */ 279 | M_BOOL foreign; /* true if external program manages data memory */ 280 | int num_dimensions; /* a 2x3 rtable has 2 dimensions */ 281 | ALGEB index_functions; /* list of array indexing functions */ 282 | ALGEB attributes; /* attributes given to this rtable */ 283 | M_BOOL transpose; /* when true, transpose the matrix at creation */ 284 | ALGEB fill; /* the value for unspecified elements */ 285 | } RTableSettings; 286 | 287 | #endif /* MPLTABLE_H */ 288 | 289 | --------------------------------------------------------------------------------