├── lib ├── utils │ ├── .gitignore │ ├── quat.os │ └── CMakeLists.txt ├── bulletsystems │ └── CMakeLists.txt ├── systems │ ├── constraints │ │ ├── disk.cc │ │ ├── sphere.cc │ │ ├── sphere.h │ │ ├── disk.h │ │ └── CMakeLists.txt │ ├── sensors │ │ └── CMakeLists.txt │ ├── parametrization │ │ └── CMakeLists.txt │ ├── chain1.h │ ├── heli.cc │ ├── qrotorarm.h │ ├── manifolds │ │ ├── mbstspace.cc │ │ ├── mbscspace.h │ │ ├── mbstspace.h │ │ ├── airmmanifold.h │ │ ├── kinbody2dmanifold.h │ │ ├── kinbody3dmanifold.h │ │ ├── point3dmanifold.cc │ │ ├── CMakeLists.txt │ │ ├── mbscspace.cc │ │ ├── gunicyclemanifold.h │ │ ├── body2dmanifold.h │ │ ├── airmmanifold.cc │ │ ├── point3dmanifold.h │ │ ├── mbsmanifold.h │ │ ├── kinbody3dmanifold.cc │ │ ├── imumanifold.h │ │ ├── posemanifold.h │ │ ├── gcarmanifold.h │ │ ├── kinbody2dmanifold.cc │ │ ├── insmanifold.h │ │ └── posemanifold.cc │ ├── snake.h │ ├── chain.h │ ├── airm.h │ ├── gcop_conversions.h │ ├── kinrccar.h │ ├── costs │ │ ├── gunicyclecost.h │ │ ├── bacost.h │ │ ├── body2dslamcost.h │ │ ├── body2dtrackcost.h │ │ ├── CMakeLists.txt │ │ ├── body3dtrackcost.h │ │ └── airplanecost.h │ ├── qrotor.h │ ├── creator.h │ ├── kinbody2d.h │ ├── kinbody2d.cc │ ├── joint.cc │ ├── airbot.h │ ├── particle2d.cc │ ├── car.cc │ ├── unicycle.cc │ ├── particle2d.h │ ├── gcop_conversions.cpp │ ├── arm.h │ ├── car.h │ ├── point3d.h │ ├── qrotor.cc │ ├── airbase.h │ ├── hrotor.h │ ├── unicycle.h │ ├── joint.h │ ├── point3d.cc │ ├── gunicycle.cc │ ├── loop_timer.cpp │ ├── gunicycle.h │ └── rccar1.h ├── algos │ ├── ba.cc │ ├── body2dslam.cc │ ├── point3dcontroller.cc │ ├── body2dslam.h │ ├── ba.h │ ├── CMakeLists.txt │ ├── mbscontroller.h │ ├── controller.h │ ├── point3dcontroller.h │ └── gavoidcontroller.h ├── est │ ├── filter.cc │ ├── CMakeLists.txt │ └── Makefile.am └── views │ ├── boxview.cc │ ├── diskview.cc │ ├── cylinderview.cc │ ├── sphereview.cc │ ├── boxview.h │ ├── diskview.h │ ├── snakeview.cc │ ├── sphereview.h │ ├── cylinderview.h │ ├── snakeview.h │ ├── chainview.cc │ ├── hrotorview.cc │ ├── chainview.h │ ├── posegraph2dview.h │ ├── airmview.cc │ ├── hrotorgeom3dview.h │ ├── body2dgraphview.h │ ├── dynvisinsview.h │ ├── airmview.h │ ├── airbotview.h │ ├── demview.h │ ├── plotview.h │ ├── hrotorview.h │ ├── qrotorview.h │ ├── view.cc │ ├── body2dtrackview.h │ ├── body3dtrackview.h │ ├── carview.h │ ├── rccarview.h │ ├── gcarview.h │ ├── particle2dview.h │ ├── unicycleview.h │ ├── point3dview.h │ ├── airbotview.cc │ ├── geom3dview.h │ ├── gunicycleview.h │ ├── airplaneview.h │ ├── heliview.h │ ├── kinbody2dview.h │ ├── geom3dview.cc │ ├── mbsview.h │ └── insview.h ├── data ├── test_model_vars │ ├── kd_rpy │ ├── kd_rpy_wrong_data │ └── kd_rpy_negative_col ├── tensorflow_model_vars_16_8_relu │ ├── rpy_gains_kp_0 │ ├── joint_gains_kd_0 │ ├── joint_gains_kp_0 │ ├── rpy_gains_kd_0 │ ├── residual_dynamics_dense_1_residual_dynamics_gamma_0 │ ├── residual_dynamics_dense_final_biases_0 │ ├── residual_dynamics_dense_1_residual_dynamics_beta_0 │ ├── residual_dynamics_dense_1_residual_dynamics_moving_mean_0 │ ├── residual_dynamics_dense_1_residual_dynamics_moving_variance_0 │ ├── residual_dynamics_dense_0_residual_dynamics_gamma_0 │ ├── residual_dynamics_dense_0_residual_dynamics_moving_mean_0 │ ├── residual_dynamics_dense_0_residual_dynamics_moving_variance_0 │ ├── residual_dynamics_dense_0_residual_dynamics_beta_0 │ └── residual_dynamics_dense_final_weights_0 ├── tensorflow_model_vars_16_8_tanh │ ├── rpy_gains_kp_0 │ ├── joint_gains_kd_0 │ ├── joint_gains_kp_0 │ ├── rpy_gains_kd_0 │ ├── residual_dynamics_dense_1_residual_dynamics_gamma_0 │ ├── residual_dynamics_dense_final_biases_0 │ ├── residual_dynamics_dense_1_residual_dynamics_beta_0 │ ├── residual_dynamics_dense_1_residual_dynamics_moving_variance_0 │ ├── residual_dynamics_dense_1_residual_dynamics_moving_mean_0 │ ├── residual_dynamics_dense_0_residual_dynamics_gamma_0 │ ├── residual_dynamics_dense_0_residual_dynamics_moving_variance_0 │ ├── residual_dynamics_dense_0_residual_dynamics_moving_mean_0 │ ├── residual_dynamics_dense_0_residual_dynamics_beta_0 │ └── residual_dynamics_dense_final_weights_0 ├── model_vars_testing_fc_layer │ ├── residual_dynamics_dense_1_residual_dynamics_gamma_0 │ ├── residual_dynamics_dense_1_residual_dynamics_moving_variance_0 │ ├── residual_dynamics_dense_1_residual_dynamics_moving_mean_0 │ └── residual_dynamics_dense_1_residual_dynamics_beta_0 ├── model_output_no_residual_dynamics │ └── state_10 ├── model_output_residual_dynamics_16_8_relu │ └── state_10 └── model_output_residual_dynamics_16_8_tanh │ └── state_10 ├── AUTHORS ├── maps ├── empty.ppm ├── hard1.ppm ├── hard2.ppm ├── hard3.ppm ├── hard4.ppm ├── pic2.ppm ├── pic3.ppm ├── slits.ppm ├── jhu_dem.ppm ├── random1.ppm ├── simple.ppm ├── slalom0.ppm ├── slalom1.ppm ├── slalom2.ppm ├── slalom3.ppm ├── slalom1_1.ppm ├── slalom1_2.ppm ├── slalom1_3.ppm ├── pic2_dilated.ppm ├── pic3_dilated.ppm └── pic2_dilated_small.ppm ├── .gitignore ├── bin ├── BulletSTLDemo │ ├── rocky.stl │ ├── meshes │ │ ├── cube.stl │ │ ├── rocky.stl │ │ └── cube.blend │ ├── main.cpp │ └── CMakeLists.txt ├── BulletVehicleEstimation │ ├── heightfield_random.xcf │ ├── main.cpp │ ├── gnrccarestimation.cfg │ ├── helper_header.h │ └── CMakeLists.txt ├── BulletVehicleEstimation1 │ ├── heightfield_random.xcf │ ├── main.cpp │ ├── gnrccarestimation.cfg │ ├── helper_header.h │ └── CMakeLists.txt ├── qrotor.cfg ├── qrotor_avoid.cfg ├── qrotoryaw.cfg ├── body2d.cfg ├── aspsacar.cfg ├── gnrccarestimation1.cfg ├── spsacar.cfg ├── cecar.cfg ├── SimpleOpenGL3 │ └── premake4.lua ├── pend.cfg ├── BulletZVehicleTest │ ├── main.cpp │ ├── gnrccarestimation.cfg │ ├── helper_header.h │ └── CMakeLists.txt ├── body3ddemstab.cfg ├── fixedchain.cfg ├── rccar.cfg ├── chainopt2.cfg ├── CMakeLists.kf ├── boat.cfg ├── fixedchain1.cfg ├── gnrccarestimation.cfg ├── slalom1.cfg ├── chain5.cfg ├── body3dstops.cfg ├── body3davoid.cfg ├── body3dceavoid.cfg ├── chaineltest.cfg ├── airflip.cfg ├── chainopt.cfg ├── airstab.cfg ├── cechainopt.cfg ├── body3drhc.cfg ├── body2dtrack.cfg ├── body2dtrack2.cfg ├── chain5_float.cfg ├── kinrccartracktest.cfg ├── body3dcestab2.cfg ├── body3dforceest.cfg ├── qrotorsystemidtest.cfg ├── comparision_spsa_ce ├── qrotoridmodel.cfg ├── airbot.cfg ├── kinbody3dtrack.cfg ├── kinbodyprojtrack.cfg ├── body3dcedemstab2.cfg ├── body3dtrack.cfg ├── body3dcedemstab.cfg ├── body3dcestab.cfg ├── body3dcedemstab_jhu.cfg ├── gptest.cc ├── gpadd.cc ├── CMakeLists.lim └── ukftest.cc ├── .gitmodules ├── tools ├── mkvid.sh ├── bnd_spring.m └── body3d_stops.m ├── applydocs.bash ├── cmake └── Modules │ ├── append_subdir_files.cmake │ ├── FindDSL.cmake │ ├── FindPQP.cmake │ ├── FindAdolc.cmake │ ├── FindDC1394.cmake │ ├── FindTinyXML.cmake │ ├── FindGCOP.cmake │ └── FindLibUSB.cmake ├── gcop.pc ├── package.xml ├── install.sh ├── runtests.sh └── README /lib/utils/.gitignore: -------------------------------------------------------------------------------- 1 | *.cpp.swp 2 | *.h.swp 3 | -------------------------------------------------------------------------------- /data/test_model_vars/kd_rpy: -------------------------------------------------------------------------------- 1 | 3 2 | 1 3 | 1,2,3, 4 | -------------------------------------------------------------------------------- /data/test_model_vars/kd_rpy_wrong_data: -------------------------------------------------------------------------------- 1 | 3 2 | 1 3 | 1,2 4 | -------------------------------------------------------------------------------- /AUTHORS: -------------------------------------------------------------------------------- 1 | Marin Kobilarov marin(at)jhu.edu 2 | Gowtham Garimella 3 | -------------------------------------------------------------------------------- /data/test_model_vars/kd_rpy_negative_col: -------------------------------------------------------------------------------- 1 | 3 2 | -5 3 | 1,2,3, 4 | -------------------------------------------------------------------------------- /maps/empty.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/empty.ppm -------------------------------------------------------------------------------- /maps/hard1.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/hard1.ppm -------------------------------------------------------------------------------- /maps/hard2.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/hard2.ppm -------------------------------------------------------------------------------- /maps/hard3.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/hard3.ppm -------------------------------------------------------------------------------- /maps/hard4.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/hard4.ppm -------------------------------------------------------------------------------- /maps/pic2.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/pic2.ppm -------------------------------------------------------------------------------- /maps/pic3.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/pic3.ppm -------------------------------------------------------------------------------- /maps/slits.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/slits.ppm -------------------------------------------------------------------------------- /lib/utils/quat.os: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/lib/utils/quat.os -------------------------------------------------------------------------------- /maps/jhu_dem.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/jhu_dem.ppm -------------------------------------------------------------------------------- /maps/random1.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/random1.ppm -------------------------------------------------------------------------------- /maps/simple.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/simple.ppm -------------------------------------------------------------------------------- /maps/slalom0.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/slalom0.ppm -------------------------------------------------------------------------------- /maps/slalom1.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/slalom1.ppm -------------------------------------------------------------------------------- /maps/slalom2.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/slalom2.ppm -------------------------------------------------------------------------------- /maps/slalom3.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/slalom3.ppm -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build* 2 | *.bak* 3 | docs* 4 | *.stl 5 | *.blend 6 | *.blend1 7 | data/ 8 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_relu/rpy_gains_kp_0: -------------------------------------------------------------------------------- 1 | 2 2 | 1 3 | 16.533182,17.366844, 4 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_tanh/rpy_gains_kp_0: -------------------------------------------------------------------------------- 1 | 2 2 | 1 3 | 16.51752,17.266687, 4 | -------------------------------------------------------------------------------- /maps/slalom1_1.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/slalom1_1.ppm -------------------------------------------------------------------------------- /maps/slalom1_2.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/slalom1_2.ppm -------------------------------------------------------------------------------- /maps/slalom1_3.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/slalom1_3.ppm -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_relu/joint_gains_kd_0: -------------------------------------------------------------------------------- 1 | 2 2 | 1 3 | 1.4894774,1.3524513, 4 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_relu/joint_gains_kp_0: -------------------------------------------------------------------------------- 1 | 2 2 | 1 3 | 11.791304,11.7795105, 4 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_tanh/joint_gains_kd_0: -------------------------------------------------------------------------------- 1 | 2 2 | 1 3 | 1.2879044,1.1704179, 4 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_tanh/joint_gains_kp_0: -------------------------------------------------------------------------------- 1 | 2 2 | 1 3 | 11.422288,11.645887, 4 | -------------------------------------------------------------------------------- /maps/pic2_dilated.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/pic2_dilated.ppm -------------------------------------------------------------------------------- /maps/pic3_dilated.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/pic3_dilated.ppm -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_relu/rpy_gains_kd_0: -------------------------------------------------------------------------------- 1 | 3 2 | 1 3 | 8.501719,6.3200903,3.1637893, 4 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_tanh/rpy_gains_kd_0: -------------------------------------------------------------------------------- 1 | 3 2 | 1 3 | 8.173418,7.0072937,3.3337743, 4 | -------------------------------------------------------------------------------- /bin/BulletSTLDemo/rocky.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/bin/BulletSTLDemo/rocky.stl -------------------------------------------------------------------------------- /maps/pic2_dilated_small.ppm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/maps/pic2_dilated_small.ppm -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "bullet3"] 2 | path = bullet3 3 | url = https://github.com/garimellagowtham/bullet3.git 4 | -------------------------------------------------------------------------------- /bin/BulletSTLDemo/meshes/cube.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/bin/BulletSTLDemo/meshes/cube.stl -------------------------------------------------------------------------------- /bin/BulletSTLDemo/meshes/rocky.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/bin/BulletSTLDemo/meshes/rocky.stl -------------------------------------------------------------------------------- /bin/BulletSTLDemo/meshes/cube.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/bin/BulletSTLDemo/meshes/cube.blend -------------------------------------------------------------------------------- /lib/bulletsystems/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(sources 2 | bulletrccar.cc 3 | ) 4 | 5 | set(headers 6 | bulletrccar.h 7 | ) 8 | -------------------------------------------------------------------------------- /bin/BulletVehicleEstimation/heightfield_random.xcf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/bin/BulletVehicleEstimation/heightfield_random.xcf -------------------------------------------------------------------------------- /bin/BulletVehicleEstimation1/heightfield_random.xcf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jhu-asco/gcop/HEAD/bin/BulletVehicleEstimation1/heightfield_random.xcf -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_relu/residual_dynamics_dense_1_residual_dynamics_gamma_0: -------------------------------------------------------------------------------- 1 | 8 2 | 1 3 | 0.9730397,0.91600025,0.9767262,0.8663357,0.9493115,0.8002054,1.163679,0.8680372, 4 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_relu/residual_dynamics_dense_final_biases_0: -------------------------------------------------------------------------------- 1 | 8 2 | 1 3 | 0.16454312,-0.22141601,0.060721394,0.24082617,-0.20014316,-0.0357853,-0.49246773,-0.75557816, 4 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_1_residual_dynamics_gamma_0: -------------------------------------------------------------------------------- 1 | 8 2 | 1 3 | 0.3233974,0.55816483,1.7782717,0.8032256,0.2996349,0.3085106,0.39490306,0.4113996, 4 | -------------------------------------------------------------------------------- /lib/systems/constraints/disk.cc: -------------------------------------------------------------------------------- 1 | #include "disk.h" 2 | 3 | using namespace gcop; 4 | using namespace Eigen; 5 | 6 | Disk::Disk(const Vector2d &o, double r) : o(o), r(r) 7 | { 8 | } 9 | -------------------------------------------------------------------------------- /tools/mkvid.sh: -------------------------------------------------------------------------------- 1 | for f in *ppm ; do convert -quality 100 $f `basename $f ppm`jpg; done 2 | mencoder "mf://*.jpg" -o test.avi -mf fps=30 -ovc lavc -lavcopts vcodec=msmpeg4v2:vbitrate=800 -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_final_biases_0: -------------------------------------------------------------------------------- 1 | 8 2 | 1 3 | -0.012836409,-0.13071178,0.035183474,0.20557193,-0.18581991,0.0029871017,-0.09271976,-0.11484799, 4 | -------------------------------------------------------------------------------- /lib/systems/constraints/sphere.cc: -------------------------------------------------------------------------------- 1 | #include "sphere.h" 2 | 3 | using namespace gcop; 4 | using namespace Eigen; 5 | 6 | Sphere::Sphere(const Vector3d &o, double r) : o(o), r(r) 7 | { 8 | } 9 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_relu/residual_dynamics_dense_1_residual_dynamics_beta_0: -------------------------------------------------------------------------------- 1 | 8 2 | 1 3 | 0.019404026,0.055752575,0.008127922,0.06597059,0.08171722,-0.08657116,0.31247282,-0.09456695, 4 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_relu/residual_dynamics_dense_1_residual_dynamics_moving_mean_0: -------------------------------------------------------------------------------- 1 | 8 2 | 1 3 | 0.081980154,-0.0628473,-0.41729996,-0.53522027,0.366233,-0.6882692,-0.41661698,-0.52380633, 4 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_relu/residual_dynamics_dense_1_residual_dynamics_moving_variance_0: -------------------------------------------------------------------------------- 1 | 8 2 | 1 3 | 0.30211675,0.37389314,0.39974225,0.8333175,0.43622282,1.1834167,0.9278248,1.2255161, 4 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_1_residual_dynamics_beta_0: -------------------------------------------------------------------------------- 1 | 8 2 | 1 3 | -0.00592649,-0.037208334,0.02271216,-0.13219856,0.0020418598,0.013361917,0.022096215,-0.045767006, 4 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_1_residual_dynamics_moving_variance_0: -------------------------------------------------------------------------------- 1 | 8 2 | 1 3 | 0.510723,0.59528226,1.2511085,0.55236363,0.5134672,0.52601063,0.44630972,0.34227136, 4 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_1_residual_dynamics_moving_mean_0: -------------------------------------------------------------------------------- 1 | 8 2 | 1 3 | -0.062266674,-0.02714833,-0.012357137,-0.0359934,-0.010994579,0.04012002,0.08387415,0.011114856, 4 | -------------------------------------------------------------------------------- /lib/systems/sensors/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(headers 2 | imusensor.h 3 | insgps.h 4 | insimu.h 5 | insmag.h 6 | point3dgps.h 7 | sensor.h 8 | body3dsensor.h 9 | inspose3d.h 10 | ) 11 | set(sources 12 | ) 13 | -------------------------------------------------------------------------------- /lib/systems/parametrization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(headers 2 | controltparam.h 3 | controllertparam.h 4 | splinetparam.h 5 | uniformsplinetparam.h 6 | flatoutputtparam.h 7 | tparam.h 8 | #trajectory.h 9 | ) 10 | set(sources 11 | ) 12 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_relu/residual_dynamics_dense_0_residual_dynamics_gamma_0: -------------------------------------------------------------------------------- 1 | 16 2 | 1 3 | 1.067858,0.9440324,0.941689,0.93561697,0.73124313,1.006798,1.0581783,0.98908925,0.91990346,0.7571741,1.0951215,1.0307201,1.3356895,0.8919913,0.8609799,1.0416903, 4 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_relu/residual_dynamics_dense_0_residual_dynamics_moving_mean_0: -------------------------------------------------------------------------------- 1 | 16 2 | 1 3 | 2.326506,11.783465,-7.14558,-9.319273,5.656763,7.448929,-7.731,-13.288671,0.9904776,-14.053961,6.634835,-1.1477982,-6.60846,10.956567,5.883285,5.533605, 4 | -------------------------------------------------------------------------------- /applydocs.bash: -------------------------------------------------------------------------------- 1 | doxygen 2 | git config merge.renameLimit 999999 3 | git add -f docs 4 | git stash 5 | git checkout gh-pages 6 | git stash apply 7 | grep -lr '<<<<<<< ' . | xargs git checkout --theirs 8 | git add -u . 9 | git config --unset merge.renameLimit 10 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_relu/residual_dynamics_dense_0_residual_dynamics_moving_variance_0: -------------------------------------------------------------------------------- 1 | 16 2 | 1 3 | 0.30901054,2.5047376,0.9328589,1.4961908,1.8771762,1.7940295,1.0721232,4.3685412,4.25741,1.6831577,0.55058,1.9501328,2.8841274,3.3627927,1.6842371,0.63292235, 4 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_0_residual_dynamics_gamma_0: -------------------------------------------------------------------------------- 1 | 16 2 | 1 3 | 0.33846265,2.2373595,0.55494624,0.5208485,0.47802243,0.32896978,1.9201336,0.46899405,0.3514824,0.42919007,0.35195422,1.3166497,0.35678586,2.1223361,2.2631538,0.83180225, 4 | -------------------------------------------------------------------------------- /cmake/Modules/append_subdir_files.cmake: -------------------------------------------------------------------------------- 1 | macro(append_subdir_files variable dirname) 2 | get_directory_property(holder DIRECTORY ${dirname} DEFINITION ${variable}) 3 | foreach(depfile ${holder}) 4 | list(APPEND ${variable} "${dirname}/${depfile}") 5 | endforeach() 6 | endmacro() -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_0_residual_dynamics_moving_variance_0: -------------------------------------------------------------------------------- 1 | 16 2 | 1 3 | 0.25169238,2.331338,2.0717125,0.357323,0.54824793,0.2672733,4.577049,0.27728173,0.05837644,0.28401634,0.0062248204,3.8090374,0.29252848,2.2509565,1.5358577,1.6141387, 4 | -------------------------------------------------------------------------------- /bin/qrotor.cfg: -------------------------------------------------------------------------------- 1 | tf=2 2 | N=128 3 | 4 | x0 = 0,0,0, 0,0,0, 0,0,0, 0,0,0 5 | xf = 0,0,0, 5,0,3, 0,0,0, 0,0,0 6 | 7 | Qf = 5,5,5, 20,20,20, .5,.5,.5, 5,5,5 8 | 9 | Q = .2,.2,.2, .2,.2,.2, 1,1,1, .01,.01,.01 10 | #Q = 0,0,0, 0,0,0, 0,0,0, 0,0,0 11 | 12 | R = .01, .01, .01, 1 13 | -------------------------------------------------------------------------------- /gcop.pc: -------------------------------------------------------------------------------- 1 | prefix=/usr/local 2 | exec_prefix=${prefix} 3 | includedir=${prefix}/include 4 | libdir=${exec_prefix}/lib 5 | 6 | Name: gcop 7 | Description: Geometric Control, Optimization, and Planning 8 | Version: 1.0.0 9 | Cflags: -I${includedir}/gcop 10 | Libs: -L${libdir} -lgcop -------------------------------------------------------------------------------- /lib/algos/ba.cc: -------------------------------------------------------------------------------- 1 | #include "ba.h" 2 | 3 | using namespace gcop; 4 | using namespace Eigen; 5 | using namespace std; 6 | 7 | Ba::Ba(Posegraph2d &pg) : 8 | pg(pg), sys(), cost(sys, pg.ts.back(), pg), pddp(sys, cost, pg.ts, pg.gs, pg.us, pg.p) 9 | { 10 | 11 | } 12 | 13 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_relu/residual_dynamics_dense_0_residual_dynamics_beta_0: -------------------------------------------------------------------------------- 1 | 16 2 | 1 3 | 0.1457193,-0.19572845,0.054957498,0.17474355,-0.09265854,-0.118445694,-0.20297918,0.28667963,0.2342951,0.25745195,0.076139934,0.11035269,-0.034808837,-0.19839808,0.42450872,-0.73477376, 4 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_0_residual_dynamics_moving_mean_0: -------------------------------------------------------------------------------- 1 | 16 2 | 1 3 | -0.0021787419,0.15565325,-1.1851991,-0.3887733,-0.781049,0.075614214,0.5228897,0.35720992,0.07733806,0.272907,0.22505729,-0.20781657,0.51566654,0.07451289,-0.46856713,0.021119505, 4 | -------------------------------------------------------------------------------- /data/tensorflow_model_vars_16_8_tanh/residual_dynamics_dense_0_residual_dynamics_beta_0: -------------------------------------------------------------------------------- 1 | 16 2 | 1 3 | -0.0047926186,-0.0075431955,0.11958674,-0.10918364,-0.01612634,0.063661486,0.029086206,0.047851354,0.0053286874,-0.08630042,-0.014087217,-0.11339993,-0.02294452,0.04781778,-0.04164681,-0.037811738, 4 | -------------------------------------------------------------------------------- /bin/qrotor_avoid.cfg: -------------------------------------------------------------------------------- 1 | T=1 2 | N=32 3 | 4 | x0 = 0,0,0, 0, 0, 2, 0,0,0, 10,0,0 5 | xf = 0,0,0, 3, 1, 2, 0,0,0, 10,0,0 6 | 7 | 8 | Qf = .001,.001,.001, 10,10,10, .01,.01,.01, 0.001, 0.001, 0.001 9 | 10 | Q = .002,.002,.002, .2,.2,.2, .005,.005,.005, .005,.005,.005 11 | #Q = 0,0,0, 0,0,0, 0,0,0, 0,0,0 12 | 13 | R = .01, .01, .01, 0.01 14 | -------------------------------------------------------------------------------- /lib/algos/body2dslam.cc: -------------------------------------------------------------------------------- 1 | #include "body2dslam.h" 2 | 3 | using namespace gcop; 4 | using namespace Eigen; 5 | using namespace std; 6 | 7 | Body2dSlam::Body2dSlam(Body2dGraph &pg) : 8 | pg(pg), cost(pg.sys, pg.ts.back(), pg) 9 | { 10 | pddp = new PDdp(pg.sys, cost, pg.ts, pg.xs, pg.us, pg.p, 2*pg.extforce); 11 | } 12 | -------------------------------------------------------------------------------- /lib/systems/constraints/sphere.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_SPHERE_H 2 | #define GCOP_SPHERE_H 3 | 4 | #include 5 | 6 | namespace gcop { 7 | 8 | class Sphere { 9 | public: 10 | Sphere(const Eigen::Vector3d &o, double r); 11 | 12 | Eigen::Vector3d o; ///< origin 13 | double r; ///< radius 14 | }; 15 | } 16 | #endif 17 | -------------------------------------------------------------------------------- /lib/systems/constraints/disk.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_DISK_H 2 | #define GCOP_DISK_H 3 | 4 | #include 5 | 6 | namespace gcop { 7 | 8 | class Disk { 9 | public: 10 | Disk(const Eigen::Vector2d &o, double r); 11 | 12 | Eigen::Vector2d o; ///< origin 13 | double r; ///< radius 14 | }; 15 | } 16 | 17 | 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /bin/qrotoryaw.cfg: -------------------------------------------------------------------------------- 1 | tf=10 2 | N=128 3 | 4 | x0 = 0,0,0, -5,-5,0, 0,0,0, 0,0,0 5 | xf = 0,0,0, 0,0,0, 0,0,0, 0,0,0 6 | 7 | Qf = 5,5,0, 20,20,20, 1,1,1, 5,5,5 8 | 9 | Q = .1,.1,0, 0,0,0, .3,.3,.3, .1,.1,.1 10 | #Q = 0,0,0, 0,0,0, 0,0,0, 0,0,0 11 | 12 | R = .05, .05, .05, 1 13 | 14 | Target = 0,0,0 15 | YawGain=.01 16 | Epochs=100 17 | Finite=True 18 | Pause=False 19 | -------------------------------------------------------------------------------- /lib/systems/chain1.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_CHAIN1_H 2 | #define GCOP_CHAIN1_H 3 | 4 | #include "mbs.h" 5 | 6 | namespace gcop { 7 | 8 | class Chain1 : public Mbs { 9 | public: 10 | Chain1(); 11 | 12 | void Force(VectorXd &f, double t, const MbsState &x, const VectorXd &u, 13 | MatrixXd *A = 0, MatrixXd *B = 0); 14 | }; 15 | } 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gcop 4 | 1.0.0 5 | Geometric Controls Optimization and Planning (GCOP) package 6 | 7 | Gowtham Garimella 8 | 9 | BSD 10 | 11 | cmake 12 | 13 | -------------------------------------------------------------------------------- /data/model_vars_testing_fc_layer/residual_dynamics_dense_1_residual_dynamics_gamma_0: -------------------------------------------------------------------------------- 1 | 32 2 | 1 3 | 1.0377356,1.0935975,1.0491064,1.0738524,1.0751581,1.0660365,1.0190463,1.008539,1.0745349,1.0801483,1.0142524,1.0143065,1.026247,1.0398873,1.015894,1.0821344,1.0407283,1.0737177,1.0397309,1.0410469,1.0304677,1.0994748,1.0211995,1.0244813,1.0848753,1.0145676,1.0811212,1.0040823,1.0929106,1.0324748,1.0046782,1.007359, 4 | -------------------------------------------------------------------------------- /data/model_vars_testing_fc_layer/residual_dynamics_dense_1_residual_dynamics_moving_variance_0: -------------------------------------------------------------------------------- 1 | 32 2 | 1 3 | 1.1923237,1.7348543,1.2361304,1.1420009,1.0119729,1.4730415,1.161263,1.322906,1.5356022,1.2074683,1.874616,1.4490649,1.2894131,1.8526759,1.2695231,1.016591,1.2813268,1.4577682,1.0739292,1.1012194,1.0937871,1.3295431,1.4971911,1.0756763,1.9194375,1.060792,1.0352647,1.1092479,1.3204155,1.5521207,1.0101982,1.0500152, 4 | -------------------------------------------------------------------------------- /install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | git submodule init #Initialize submodules 3 | git submodule update #Clones bullet3 specific commit needed for this 4 | cd bullet3/build3 #Go into bullet3 for building 5 | cmake -DBUILD_SHARED_LIBS=ON -DUSE_DOUBLE_PRECISION=ON .. 6 | make -j `nproc` 7 | cd ../.. && mkdir build # Go back to gcop and create a build directory 8 | cd build && cmake -DUSE_BULLET=ON .. 9 | make -j `nproc` 10 | -------------------------------------------------------------------------------- /bin/body2d.cfg: -------------------------------------------------------------------------------- 1 | iters = 100 2 | 3 | 4 | N = 128 5 | tf = 5 6 | 7 | # damping 8 | #D=0,0,0 9 | 10 | # mass matrix diagonal 11 | #I=.3,10,10 12 | 13 | #D=.01,.05,5 14 | 15 | ulb = -.1, -.1 16 | uub = .1, .1 17 | 18 | # 19 | x0=2, 2, -3, 0,0,0 20 | xf=0,0,0, 0,0,0 21 | 22 | # g v 23 | Qf = 1, 5, 5, .1, .5, .5 24 | Q = 0, 0, 0, 0, 0, 0 25 | 26 | #ko = .5 27 | 28 | # u 29 | R = .01, .05, .05 30 | 31 | mu=.01 32 | -------------------------------------------------------------------------------- /data/model_vars_testing_fc_layer/residual_dynamics_dense_1_residual_dynamics_moving_mean_0: -------------------------------------------------------------------------------- 1 | 32 2 | 1 3 | 0.82569706,0.12372306,0.30209705,0.9835144,0.99434876,0.67703414,0.94208103,0.60017824,0.4617379,0.2868342,0.57343256,0.83435744,0.965716,0.8596706,0.3864285,0.73643124,0.6765071,0.46780652,0.42407212,0.123770975,0.7861142,0.4042896,0.4798632,0.72714436,0.6942378,0.6659869,0.99279994,0.19313562,0.636283,0.8227495,0.045216516,0.90019566, 4 | -------------------------------------------------------------------------------- /lib/systems/constraints/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(sources 2 | disk.cc 3 | sphere.cc 4 | ) 5 | 6 | set(headers 7 | shell.h 8 | cylinder.h 9 | disk.h 10 | sphere.h 11 | diskconstraint.h 12 | sphereconstraint.h 13 | constraint.h 14 | groundplane.h 15 | yawvelocityconstraint.h 16 | direction_constraint.h 17 | ) 18 | 19 | IF (PQP_FOUND) 20 | set(headers 21 | ${headers} 22 | pqpdem.h 23 | ) 24 | ENDIF (PQP_FOUND) 25 | -------------------------------------------------------------------------------- /bin/aspsacar.cfg: -------------------------------------------------------------------------------- 1 | tf = 10 2 | N = 14 3 | #Nit = 500 4 | Nit = 1000000 5 | 6 | iters = 1 7 | #iters = 20 8 | 9 | #x0= .8, -.5, 0, 0 10 | u0 = -0.01, 0 11 | x0= 1, 1, 0, 0 12 | 13 | #xf= 1, 2, 3.14, 0 14 | #xf= 0, 0, 0, 0 15 | xf= 0, 0, 1.57, 0 16 | 17 | Qf = 10, 10, 10, 1 18 | Q = 0.5, 0.5, 0.1, 1 19 | R = .5, .1 20 | 21 | #a, c, alpha, gamma 22 | stepcoeffs = 0.2, 0.0001, 0.602, 0.101 23 | #stepcoeffs = 0.01, 0.00002, 0.602, 0.101 24 | -------------------------------------------------------------------------------- /bin/gnrccarestimation1.cfg: -------------------------------------------------------------------------------- 1 | tf = 15 2 | N = 200 3 | iters = 30 4 | #x0= .1, -.5, 0, 0 5 | #x0= 1, 1, 0, 0 6 | x0= 1, 1, 0, 0 7 | 8 | xf= 1, 0, 1.57, 0 9 | #xf= 0, 0, 0, 0 10 | 11 | R = 10,10,10 12 | S = 1000, 1000, 1000, 1000 13 | P = 100, 100, 100, 100, 100, 0 14 | #P = 0.01, 0.01, 0.01, 0.01, 0.01, 0.01 15 | #P = 0, 0, 0, 0, 0, 0.1 16 | #p0 = 0.3, 0.12, 0.0, 0.1, 0.0, 1.0 17 | p0 = 0.3, 0.1, 0.0, 0.1, 0.0, 0.5 18 | 19 | 20 | mu=.01 21 | -------------------------------------------------------------------------------- /lib/est/filter.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "filter.h" 5 | 6 | using namespace gcop; 7 | using namespace Eigen; 8 | 9 | 10 | Filter::Filter(Model &model) : 11 | model(model), 12 | x(model.nx), 13 | P(model.nr, model.nr) 14 | { 15 | Init(); 16 | } 17 | 18 | void Filter::Init() 19 | { 20 | chi = 0; 21 | chiGate = -1; 22 | } 23 | 24 | 25 | Filter::~Filter() 26 | { 27 | } 28 | -------------------------------------------------------------------------------- /data/model_vars_testing_fc_layer/residual_dynamics_dense_1_residual_dynamics_beta_0: -------------------------------------------------------------------------------- 1 | 32 2 | 1 3 | 0.021599792,0.015315465,0.061718173,0.08043715,0.029429281,0.019668529,0.026215073,0.012086616,0.094902284,0.042957578,0.012251759,0.017521966,0.05570738,0.0025580735,0.0087882625,0.091953814,0.05824184,0.040242653,0.023287212,0.045743365,0.06441702,0.023873253,0.08945173,0.0648348,0.09181072,0.05163472,0.0773891,0.09622594,0.015442314,0.04230668,0.042366404,0.008604284, 4 | -------------------------------------------------------------------------------- /data/model_output_no_residual_dynamics/state_10: -------------------------------------------------------------------------------- 1 | 22 2 | 1 3 | -0.8750778195649119,0.1967334746368943,1.5588715905752204,0.034302880455637494,0.059301964155972234,1.4130982413778035,-0.006694013207155685,0.033236762698581016,0.17035093117484174,-0.04554447898769711,-0.028063566274368772,0.26265707976227154,0.0182212,0.0705811,1.5926686804642067,0.15602414319085184,-0.32404074358994744,1.29322,0.24925851179160247,-0.010891127851685783,-0.3170573414938521,1.2866172104936848, 4 | -------------------------------------------------------------------------------- /lib/views/boxview.cc: -------------------------------------------------------------------------------- 1 | #include "boxview.h" 2 | 3 | using namespace gcop; 4 | 5 | BoxView::BoxView(const Vector3d &d, Matrix4d *g) : 6 | Geom3dView("Geom3d View", g), d(d) 7 | { 8 | } 9 | 10 | BoxView::BoxView() : 11 | Geom3dView("Geom3d View"), d(1,1,1) 12 | { 13 | } 14 | 15 | 16 | void BoxView::RenderGeom() 17 | { 18 | glPushMatrix(); 19 | glColor3f(0,1,0); 20 | glScaled(d[0], d[1], d[2]); 21 | glutSolidCube(1); 22 | glPopMatrix(); 23 | } 24 | -------------------------------------------------------------------------------- /data/model_output_residual_dynamics_16_8_relu/state_10: -------------------------------------------------------------------------------- 1 | 22 2 | 1 3 | -0.8750778195649119,0.1967334746368943,1.5588715905752204,0.034302880455637494,0.059301964155972234,1.4130982413778035,-0.006694013207155685,0.033236762698581016,0.17035093117484174,-0.04554447898769711,-0.028063566274368772,0.26265707976227154,0.0182212,0.0705811,1.5926686804642067,0.15602414319085184,-0.32404074358994744,1.29322,0.24925851179160247,-0.010891127851685783,-0.3170573414938521,1.2866172104936848, 4 | -------------------------------------------------------------------------------- /data/model_output_residual_dynamics_16_8_tanh/state_10: -------------------------------------------------------------------------------- 1 | 22 2 | 1 3 | -0.8750778195649119,0.1967334746368943,1.5588715905752204,0.034302880455637494,0.059301964155972234,1.4130982413778035,-0.006694013207155685,0.033236762698581016,0.17035093117484174,-0.04554447898769711,-0.028063566274368772,0.26265707976227154,0.0182212,0.0705811,1.5926686804642067,0.15602414319085184,-0.32404074358994744,1.29322,0.24925851179160247,-0.010891127851685783,-0.3170573414938521,1.2866172104936848, 4 | -------------------------------------------------------------------------------- /lib/systems/heli.cc: -------------------------------------------------------------------------------- 1 | #include "heli.h" 2 | 3 | using namespace gcop; 4 | using namespace Eigen; 5 | 6 | Heli::Heli() : 7 | Body3d(Vector3d(1, 1, 2), 5, Vector3d(2.32e-2, 2.32e-2, 4.41e-2)), 8 | rt(.3), rb(1) { 9 | 10 | Bu.setZero(); 11 | Bu(0,0) = 1; 12 | Bu(1,1) = 1; 13 | Bu(2,2) = 1; 14 | Bu(5,3) = 1; 15 | 16 | fp << 0, 0, -9.81*m; 17 | 18 | // double ulb[4] = {1200, 1200, 1200, 1200}; 19 | // double uub[4] = {7800, 7800, 7800, 7800}; 20 | } 21 | -------------------------------------------------------------------------------- /lib/views/diskview.cc: -------------------------------------------------------------------------------- 1 | #include "diskview.h" 2 | 3 | using namespace gcop; 4 | 5 | DiskView::DiskView(const Disk &disk, Matrix4d *g) : 6 | Geom3dView("Disk", g), disk(disk) 7 | { 8 | qobj = gluNewQuadric(); 9 | } 10 | 11 | 12 | DiskView::~DiskView() 13 | { 14 | free(qobj); 15 | } 16 | 17 | void DiskView::RenderGeom() 18 | { 19 | glPushMatrix(); 20 | glTranslated(disk.o[0], disk.o[1], 0); 21 | gluDisk(qobj, 0, disk.r, 10, 10); 22 | glPopMatrix(); 23 | } 24 | -------------------------------------------------------------------------------- /lib/views/cylinderview.cc: -------------------------------------------------------------------------------- 1 | #include "cylinderview.h" 2 | 3 | using namespace gcop; 4 | 5 | CylinderView::CylinderView(double r, double h, Matrix4d *g) : 6 | Geom3dView("Cylinder", g), r(r), h(h) 7 | { 8 | qobj = gluNewQuadric(); 9 | } 10 | 11 | 12 | CylinderView::~CylinderView() 13 | { 14 | free(qobj); 15 | } 16 | 17 | void CylinderView::RenderGeom() 18 | { 19 | glPushMatrix(); 20 | glTranslated(0,0,-h/2); 21 | gluCylinder(qobj, r, r, h, 20, 20); 22 | glPopMatrix(); 23 | } 24 | -------------------------------------------------------------------------------- /lib/views/sphereview.cc: -------------------------------------------------------------------------------- 1 | #include "sphereview.h" 2 | 3 | using namespace gcop; 4 | 5 | SphereView::SphereView(const Sphere &sphere, Matrix4d *g) : 6 | Geom3dView("Sphere", g), sphere(sphere) 7 | { 8 | qobj = gluNewQuadric(); 9 | } 10 | 11 | 12 | SphereView::~SphereView() 13 | { 14 | free(qobj); 15 | } 16 | 17 | void SphereView::RenderGeom() 18 | { 19 | glPushMatrix(); 20 | glTranslated(sphere.o[0], sphere.o[1], 0); 21 | gluSphere(qobj, sphere.r, 10, 10); 22 | glPopMatrix(); 23 | } 24 | -------------------------------------------------------------------------------- /bin/spsacar.cfg: -------------------------------------------------------------------------------- 1 | tf = 10 2 | N = 50 3 | Nit = 500 4 | #Nit = 100000 5 | 6 | iters = 500 7 | #iters = 20 8 | 9 | #x0= .8, -.5, 0, 0 10 | u0 = 0.05, 0.1 11 | x0= 0, 0, 0, 0 12 | 13 | #xf= 1, 2, 3.14, 0 14 | #xf= 0, 0, 0, 0 15 | xf= 1, 0, 1.57, 0 16 | #xf= 1, 1, 1.57, 0 17 | 18 | Qf = 10, 10, 10, 1 19 | Q = 0.5, 0.5, 0.1, 1 20 | R = .5, .1 21 | 22 | #a, c, alpha, gamma 23 | #stepcoeffs = 0.03, 0.0001, 0.602, 0.101 24 | stepcoeffs = 0.02, 0.0001, 0.602, 0.101 25 | #stepcoeffs = 0.2, 0.0001, 0.602, 0.101 26 | -------------------------------------------------------------------------------- /bin/cecar.cfg: -------------------------------------------------------------------------------- 1 | tf = 5 2 | N = 32 3 | #N = 10 4 | iters = 500 5 | 6 | du = .1, .1 7 | e = .005, .005 8 | 9 | mras = 0 10 | inc = 0 11 | 12 | #x0= .1, -.5, 0, 0 13 | x0= 0, 0, 0, 0 14 | #x0= 2, 2, 1.57, 0 15 | #xf= 1, 0, 1.57, 0 16 | #xf= 1.5, 1, 0, 0 17 | xf= 1.5, 1, -1, 0 18 | #xf= 0, 0, 1.57, 0 19 | 20 | #x0= .8, -.5, 0, 0 21 | #x0= 1, 1, 0, 0 22 | 23 | #xf= 1, 2, 3.14, 0 24 | #xf= 0, 0, 0, 0 25 | 26 | Qf = 10, 10, 10, 1 27 | #Q = 0.5, 0.5, 0.1, 1 28 | Q = 0, 0, 0, 1 29 | R = .5, .1 30 | 31 | Ns = 200 32 | -------------------------------------------------------------------------------- /lib/utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(sources 2 | utils.cc 3 | se2.cc 4 | so3.cc 5 | se3.cc 6 | params.cc 7 | urdflink.cpp 8 | urdfjoint.cpp 9 | quat.cc 10 | # normal.cc 11 | ) 12 | 13 | set(headers 14 | utils.h 15 | group.h 16 | se2.h 17 | so3.h 18 | se3.h 19 | urdflink.h 20 | urdfjoint.h 21 | pose.h 22 | color.h 23 | urdfmodel.h 24 | quat.h 25 | function.h 26 | params.h 27 | normal.h 28 | bulletworld.h 29 | samplenumericaldiff.h 30 | load_eigen_matrix.h 31 | ) 32 | 33 | -------------------------------------------------------------------------------- /lib/views/boxview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_BOXVIEW_H 2 | #define GCOP_BOXVIEW_H 3 | 4 | #include "GL/glu.h" 5 | #include "GL/glut.h" 6 | #include "geom3dview.h" 7 | #include 8 | 9 | 10 | namespace gcop { 11 | 12 | using namespace Eigen; 13 | 14 | /** 15 | * Box view. 16 | */ 17 | class BoxView : public Geom3dView { 18 | public: 19 | 20 | BoxView(); 21 | BoxView(const Vector3d &d, Matrix4d *g = 0); 22 | 23 | void RenderGeom(); 24 | 25 | Vector3d d; ///< dimensions 26 | }; 27 | } 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /bin/SimpleOpenGL3/premake4.lua: -------------------------------------------------------------------------------- 1 | 2 | project "App_SimpleOpenGL3" 3 | 4 | language "C++" 5 | 6 | kind "ConsoleApp" 7 | 8 | includedirs { 9 | ".", 10 | "../../src", 11 | "../../btgui" 12 | } 13 | 14 | 15 | links{ "OpenGL_Window","Bullet3Common"} 16 | initOpenGL() 17 | initGlew() 18 | 19 | files { 20 | "*.cpp", 21 | "*.h", 22 | } 23 | 24 | if os.is("Linux") then initX11() end 25 | 26 | if os.is("MacOSX") then 27 | links{"Cocoa.framework"} 28 | end 29 | -------------------------------------------------------------------------------- /bin/pend.cfg: -------------------------------------------------------------------------------- 1 | method = 1 2 | iters = 4 3 | 4 | # dmoc epsilon 5 | eps = 1e-3 6 | 7 | # garvity 8 | ag= 0, 0, 0 9 | 10 | tf = 5 11 | N = 128 12 | 13 | fixed = 0 14 | 15 | nb = 2 16 | 17 | ulb = -1 18 | uub = 1 19 | 20 | damping = .1 21 | 22 | # rpy xyz r w0 v0 dr 23 | x0=1,0 24 | xf=0,0 25 | 26 | # rpy xyz r 27 | Kp = 2 28 | Kd = 2 29 | 30 | # rpy xyz r w0 v0 dr 31 | Qf = 1,1 32 | Q = .1,.1 33 | 34 | # rpy xyz r 35 | R = 1 36 | 37 | mu=0.01 38 | 39 | Nd = 10 40 | -------------------------------------------------------------------------------- /bin/BulletSTLDemo/main.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "VehicleDemo.h" 3 | #include "GlutStuff.h" 4 | #include "GLDebugDrawer.h" 5 | #include "btBulletDynamicsCommon.h" 6 | GLDebugDrawer gDebugDrawer; 7 | 8 | int main(int argc,char** argv) 9 | { 10 | 11 | VehicleDemo* vehicleDemo = new VehicleDemo; 12 | 13 | vehicleDemo->initPhysics(); 14 | vehicleDemo->getDynamicsWorld()->setDebugDrawer(&gDebugDrawer); 15 | 16 | return glutmain(argc, argv,640,480,"Bullet Vehicle Demo. http://www.continuousphysics.com/Bullet/phpBB2/", vehicleDemo); 17 | } 18 | 19 | -------------------------------------------------------------------------------- /lib/est/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(headers 2 | predictor.h 3 | corrector.h 4 | kalmanpredictor.h 5 | kalmancorrector.h 6 | unscentedbase.h 7 | unscentedpredictor.h 8 | unscentedcorrector.h 9 | gp.h 10 | gmm.h 11 | ce.h 12 | ) 13 | 14 | set(sources 15 | gp.cc 16 | ) 17 | 18 | #if(OPENCV_FOUND) 19 | # set(headers ${headers} camera.h) 20 | # set(sources ${sources} camera.cc) 21 | #endif(OPENCV_FOUND) 22 | 23 | if(CERES_FOUND) 24 | set(headers ${headers} qrotorsystemid.h) 25 | set(sources ${sources} qrotorsystemid.cpp) 26 | endif(CERES_FOUND) 27 | -------------------------------------------------------------------------------- /bin/BulletZVehicleTest/main.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "VehicleDemo.h" 3 | #include "GlutStuff.h" 4 | #include "GLDebugDrawer.h" 5 | #include "btBulletDynamicsCommon.h" 6 | GLDebugDrawer gDebugDrawer; 7 | 8 | int main(int argc,char** argv) 9 | { 10 | 11 | VehicleDemo* vehicleDemo = new VehicleDemo; 12 | 13 | vehicleDemo->initPhysics(); 14 | vehicleDemo->getDynamicsWorld()->setDebugDrawer(&gDebugDrawer); 15 | 16 | return glutmain(argc, argv,640,480,"Bullet Vehicle Demo. http://www.continuousphysics.com/Bullet/phpBB2/", vehicleDemo); 17 | } 18 | 19 | -------------------------------------------------------------------------------- /lib/views/diskview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_DISKVIEW_H 2 | #define GCOP_DISKVIEW_H 3 | 4 | #include "GL/glu.h" 5 | #include "GL/glut.h" 6 | #include "geom3dview.h" 7 | #include "disk.h" 8 | 9 | namespace gcop { 10 | 11 | using namespace Eigen; 12 | 13 | /** 14 | * Disk view. 15 | */ 16 | class DiskView : public Geom3dView { 17 | public: 18 | 19 | DiskView(const Disk &disk, Matrix4d *g = 0); 20 | virtual ~DiskView(); 21 | 22 | void RenderGeom(); 23 | 24 | const Disk &disk; 25 | GLUquadricObj *qobj; 26 | }; 27 | } 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /bin/BulletVehicleEstimation/main.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "VehicleDemo.h" 3 | #include "GlutStuff.h" 4 | #include "GLDebugDrawer.h" 5 | #include "btBulletDynamicsCommon.h" 6 | GLDebugDrawer gDebugDrawer; 7 | 8 | int main(int argc,char** argv) 9 | { 10 | 11 | VehicleDemo* vehicleDemo = new VehicleDemo; 12 | 13 | vehicleDemo->initPhysics(); 14 | vehicleDemo->getDynamicsWorld()->setDebugDrawer(&gDebugDrawer); 15 | 16 | return glutmain(argc, argv,640,480,"Bullet Vehicle Demo. http://www.continuousphysics.com/Bullet/phpBB2/", vehicleDemo); 17 | } 18 | 19 | -------------------------------------------------------------------------------- /bin/BulletVehicleEstimation1/main.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "VehicleDemo.h" 3 | #include "GlutStuff.h" 4 | #include "GLDebugDrawer.h" 5 | #include "btBulletDynamicsCommon.h" 6 | GLDebugDrawer gDebugDrawer; 7 | 8 | int main(int argc,char** argv) 9 | { 10 | 11 | VehicleDemo* vehicleDemo = new VehicleDemo; 12 | 13 | vehicleDemo->initPhysics(); 14 | vehicleDemo->getDynamicsWorld()->setDebugDrawer(&gDebugDrawer); 15 | 16 | return glutmain(argc, argv,640,480,"Bullet Vehicle Demo. http://www.continuousphysics.com/Bullet/phpBB2/", vehicleDemo); 17 | } 18 | 19 | -------------------------------------------------------------------------------- /bin/BulletVehicleEstimation/gnrccarestimation.cfg: -------------------------------------------------------------------------------- 1 | tf = 5 2 | iters = 30 3 | #N = 50 4 | N = 32 5 | 6 | R = 10,10,10 7 | S = 10, 10, 10, 10 8 | #P = 1, 1 9 | #P = 25, 0.0025, 100 10 | P = 100, 1, 0.1 11 | #P = 0, 0 12 | 13 | #p0 = 1, 25, 0.004 14 | #p0 = 1, 15, 0.012 15 | #p0 = 1, 15, 0.0125 16 | p0 = 1.04, 100, 0.2 17 | #p0 = 1, 58, 0.1 18 | #p0 = 1.5, 25, 0.5 19 | 20 | mu=.01 21 | 22 | #controldata="../../../data/ctrldata" 23 | controldata="../../../data/trajectory3/ctrldata" 24 | 25 | sensordata="../../../data/trajectory3/posdata" 26 | #sensordata="../../../data/posdata" 27 | -------------------------------------------------------------------------------- /lib/views/snakeview.cc: -------------------------------------------------------------------------------- 1 | #include "GL/glut.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "snakeview.h" 7 | #include "viewer.h" 8 | #include "so3.h" 9 | #include "utils.h" 10 | #include "boxview.h" 11 | 12 | using namespace gcop; 13 | using namespace Eigen; 14 | 15 | SnakeView::SnakeView(const Snake &sys, 16 | vector *xs) : 17 | MbsView(sys, xs) 18 | { 19 | for (int i = 0; i < 3; ++i) { 20 | views[i].d = sys.links[i].ds; 21 | this->geomViews.push_back(&views[i]); 22 | } 23 | } 24 | -------------------------------------------------------------------------------- /lib/views/sphereview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_SPHEREVIEW_H 2 | #define GCOP_SPHEREVIEW_H 3 | 4 | #include "GL/glu.h" 5 | #include "GL/glut.h" 6 | #include "geom3dview.h" 7 | #include "sphere.h" 8 | 9 | namespace gcop { 10 | 11 | using namespace Eigen; 12 | 13 | /** 14 | * Sphere view. 15 | */ 16 | class SphereView : public Geom3dView { 17 | public: 18 | 19 | SphereView(const Sphere &sphere, Matrix4d *g = 0); 20 | virtual ~SphereView(); 21 | 22 | void RenderGeom(); 23 | 24 | const Sphere &sphere; 25 | GLUquadricObj *qobj; 26 | }; 27 | } 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /bin/body3ddemstab.cfg: -------------------------------------------------------------------------------- 1 | ko = 50 2 | kp = 1, 1, 1, 25, 25, 25 3 | kd = 2, 2, 2, 50, 50, 50 4 | kb = 0 5 | 6 | sr = 20 7 | 8 | tf = 16 9 | N = 500 10 | 11 | cr = 0 12 | 13 | x0=0, 0, 0, 46,85, 5, 0,0,0, 0,0,0 14 | #x0=0, 0, 0, 80,10, 5, 0,0,0, 0,0,0 15 | 16 | xf=0, 0, 0, 175,140, 5, 0,0,0, 0,0,0 17 | 18 | 19 | umax = 36 20 | mass = 1 21 | 22 | mapName = maps/pic2_dilated.ppm 23 | #mapName = maps/pic2.ppm 24 | mapCellSize = .25 25 | mapHeightScale = 30 26 | 27 | 28 | # closeup on passage 29 | #camParams = 90, 34, 100.2, -78, -32 30 | 31 | # global view 32 | camParams= 90, 34, 100.2, -78, -74.9 -------------------------------------------------------------------------------- /lib/est/Makefile.am: -------------------------------------------------------------------------------- 1 | CC=g++ 2 | CXX=g++ 3 | SUBDIRS = . 4 | 5 | AM_CPPFLAGS = -Wall -Wno-deprecated -fno-exceptions -O3 -g -I. -fPIC 6 | AM_CFLAGS = $(AM_CPPFLAGS) -fno-rtti 7 | 8 | lib_LIBRARIES = libest.a 9 | 10 | libest_a_LIBADD = *.o 11 | 12 | libest_a_DEPENDENCIES = $(libest_a_LIBADD) 13 | 14 | libest_a_SOURCES = \ 15 | model.cc model.h \ 16 | filter.cc filter.h \ 17 | ukf.cc ukf.h \ 18 | quat.cc quat.h \ 19 | insmodel.cc insmodel.h \ 20 | insukf.cc insukf.h 21 | 22 | 23 | 24 | 25 | libestdir=$(includedir)/est 26 | libest_HEADERS = model.h filter.h ukf.h quat.h insmodel.h insukf.h -------------------------------------------------------------------------------- /bin/fixedchain.cfg: -------------------------------------------------------------------------------- 1 | method = 1 2 | iters = 4 3 | 4 | # dmoc epsilon 5 | eps = .0001 6 | 7 | # garvity 8 | ag= 0, 0, 0 9 | 10 | tf = 5 11 | N = 128 12 | 13 | fixed = 1 14 | 15 | nb = 3 16 | 17 | ulb = -1, -1 18 | uub = 1, 1 19 | 20 | damping= .1, .1 21 | 22 | # rpy xyz r w0 v0 dr 23 | x0=1, 1, 0,0 24 | xf=0,0, 0,0 25 | 26 | # rpy xyz r 27 | Kp = 2,2 28 | Kd = 2,2 29 | 30 | # rpy xyz r w0 v0 dr 31 | Qf = 1,1, 1,1 32 | Q = .1,.1, .1,.1 33 | 34 | # rpy xyz r 35 | R = 1, 1 36 | 37 | mu=0.01 38 | 39 | Nd = 10 40 | -------------------------------------------------------------------------------- /lib/views/cylinderview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_CYLINDERVIEW_H 2 | #define GCOP_CYLINDERVIEW_H 3 | 4 | #include "GL/glu.h" 5 | #include "GL/glut.h" 6 | #include "geom3dview.h" 7 | 8 | 9 | namespace gcop { 10 | 11 | using namespace Eigen; 12 | 13 | /** 14 | * Cylinder view. 15 | */ 16 | class CylinderView : public Geom3dView { 17 | public: 18 | 19 | CylinderView(double r = .5, double h = 1, Matrix4d *g = 0); 20 | virtual ~CylinderView(); 21 | 22 | void RenderGeom(); 23 | 24 | double r; 25 | double h; 26 | GLUquadricObj *qobj; 27 | }; 28 | } 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /bin/rccar.cfg: -------------------------------------------------------------------------------- 1 | tf = 10 2 | #N = 64 3 | #N = 50 4 | N = 32 5 | #N = 10 6 | iters = 20 7 | #iters = 200 8 | #x0= .1, -.5, 0, 0 9 | #x0= 1, 1, 0, 0 10 | x0= 0, 0, 0,0.3 11 | 12 | #xf= 1.5, 1, -1, 0 13 | xf= 1.5, 1, 0, 0.3 14 | #xf= 1.5, 1, 1.57, 0 15 | #xf= 1, 0, 1.57, 0 16 | #xf= 0, 0, 0, 0 17 | 18 | Qf = 10,10,10,1 19 | #Q = 0.5, 0.5, 0.1, 1 20 | Q = 0, 0, 0, 1 21 | R = .5, .1 22 | #R = .2, 0 23 | 24 | eps=0.001 25 | 26 | 27 | mu=.01 28 | #scale_du=0.02,0.2 29 | #scale_du=0.05,0.02 30 | scale_du= 0.1,0.04 31 | #scale_du=0.0,0.0 32 | #Scale du affects sample ddp by scaling the sampling du uniform distribution 33 | -------------------------------------------------------------------------------- /bin/BulletVehicleEstimation1/gnrccarestimation.cfg: -------------------------------------------------------------------------------- 1 | tf = 10 2 | iters = 3 3 | N = 50 4 | 5 | R = 10,10,10 6 | S = 1000, 1000, 1000, 1000 7 | #P = 1, 1 8 | P = 1, 0.1 9 | #P = 0, 0 10 | 11 | #p0 = 0.8, -0.02 12 | p0 = 0.2, 0.02 13 | 14 | mu=.01 15 | 16 | # Data copied from physics simulation: 17 | #zs, us not working for now just physicall copied data #TODO 18 | #ts = 0,0.125,0.25,0.375,0.5,0.625,0.75,0.875,1,1.125,1.25,1.375,1.5,1.625,1.75,1.875,2,2.125,2.25,2.375,2.5,2.625,2.75,2.875,3,3.125,3.25,3.375,3.5,3.625,3.75,3.875,4,4.125,4.25,4.375,4.5,4.625,4.75,4.875,5 19 | #If zs, us, ts are given tf, x0, xf are not used. 20 | -------------------------------------------------------------------------------- /bin/chainopt2.cfg: -------------------------------------------------------------------------------- 1 | # garvity 2 | ag= 0, 0, 0 3 | 4 | tf = 20 5 | N = 256 6 | 7 | # rpy xyz r w0 v0 dr 8 | x0=0,0,0, 0,0,0, 0, 0, ,.2,.2,.2, 0,0,0, .0,-.0 9 | xf=0,0,0, 0,0,0, 0,0, 0,0,0, 0,0,0, 0,0 10 | 11 | 12 | # rpy xyz r 13 | Kp = .2,.2,.2, 2, 2, 2, 2,2 14 | Kd = 1,1,1, 2, 2, 2, 2,2 15 | 16 | # rpy xyz r w0 v0 dr 17 | Qf = 2,2,2, 20,20,20, 1,1, 1,1,1, 1,1,1, 1,1 18 | Q = 2,2,2, 20,20,20, 1,1, 1,1,1, 1,1,1, 1,1 19 | 20 | 21 | # rpy xyz r 22 | R = .1, .1, .1, .1, .1, .001, 0.1, 0.1 23 | 24 | mu=.001 25 | 26 | Nd = 10 27 | -------------------------------------------------------------------------------- /bin/CMakeLists.kf: -------------------------------------------------------------------------------- 1 | link_directories(${RPC_BINARY_DIR}/lib) 2 | 3 | add_executable(imukftest imukftest.cc) 4 | target_link_libraries(imukftest gcop ${LIBS}) 5 | 6 | 7 | add_executable(cartest cartest.cc) 8 | target_link_libraries(cartest gcop ${LIBS}) 9 | 10 | add_executable(helitest helitest.cc) 11 | target_link_libraries(helitest gcop ${LIBS}) 12 | 13 | add_executable(helirhc helirhc.cc) 14 | target_link_libraries(helirhc gcop ${LIBS}) 15 | 16 | add_executable(uuvtest uuvtest.cc) 17 | target_link_libraries(uuvtest gcop ${LIBS}) 18 | 19 | add_executable(armtest armtest.cc) 20 | target_link_libraries(armtest gcop ${LIBS}) 21 | -------------------------------------------------------------------------------- /lib/systems/qrotorarm.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_AIRM_H 2 | #define GCOP_AIRM_H 3 | 4 | #include "mbs.h" 5 | 6 | namespace gcop { 7 | 8 | /** 9 | * A quadrotor dynamical model. 10 | * 11 | * The state is 12 | * \f$ \bf x = (R, x, \omega, v) \f$ where \f$ (R,x)\in SO(3)\times\mathbb{R}^3\f$ is the pose, 13 | * and the controls are \f$ \bm u = (u_1,u_2,u_3,u_4)\f$ correspond to torques 14 | * around the body and a vertical lift force. 15 | * 16 | * 17 | * Author: Marin Kobilarov marin(at)jhu.edu 18 | */ 19 | class Airm : public Mbs<3, 8> { 20 | public: 21 | Airm(); 22 | }; 23 | } 24 | 25 | #endif 26 | -------------------------------------------------------------------------------- /lib/systems/manifolds/mbstspace.cc: -------------------------------------------------------------------------------- 1 | #include "mbstspace.h" 2 | 3 | using namespace gcop; 4 | 5 | MbsTspace::MbsTspace(int nb) : Manifold(nb + 5) { 6 | } 7 | 8 | 9 | void MbsTspace::Lift(VectorXd &v, 10 | const MbsState &xa, 11 | const MbsState &xb) { 12 | 13 | v.head(6) = xb.vs[0] - xa.vs[0]; 14 | v.tail(xa.dr.size()) = xb.dr - xa.dr; 15 | } 16 | 17 | 18 | void MbsTspace::Retract(MbsState &xb, 19 | const MbsState &xa, 20 | const VectorXd &v) { 21 | xb.vs[0] = xa.vs[0] + v.head(6); 22 | xb.dr = xa.dr + v.tail(xa.dr.size()); 23 | } 24 | -------------------------------------------------------------------------------- /bin/BulletZVehicleTest/gnrccarestimation.cfg: -------------------------------------------------------------------------------- 1 | tf = 5 2 | iters = 15 3 | #N = 50 4 | N = 60 5 | 6 | R = 10,10,10 7 | S = 1000, 1000, 1000, 1000 8 | #P = 1, 1 9 | #P = 25, 0.0025, 100 10 | P = 0, 100, 10 11 | #P = 0, 0 12 | 13 | #p0 = 1.5, 50, 0.2 14 | p0 = 1.5, 25, 0.15 15 | 16 | mu=.01 17 | 18 | # Data copied from physics simulation: 19 | #zs, us not working for now just physicall copied data #TODO 20 | #ts = 0,0.125,0.25,0.375,0.5,0.625,0.75,0.875,1,1.125,1.25,1.375,1.5,1.625,1.75,1.875,2,2.125,2.25,2.375,2.5,2.625,2.75,2.875,3,3.125,3.25,3.375,3.5,3.625,3.75,3.875,4,4.125,4.25,4.375,4.5,4.625,4.75,4.875,5 21 | #If zs, us, ts are given tf, x0, xf are not used. 22 | -------------------------------------------------------------------------------- /lib/systems/manifolds/mbscspace.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_MBSCSPACE_H 2 | #define GCOP_MBSCSPACE_H 3 | 4 | #include "manifold.h" 5 | #include "mbsstate.h" 6 | 7 | namespace gcop { 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | 12 | // state dimension for nb-body system 13 | 14 | class MbsCspace : public Manifold { 15 | public: 16 | 17 | MbsCspace(int nb); 18 | 19 | void Lift(Vectornd &v, 20 | const MbsState &xa, 21 | const MbsState &xb); 22 | 23 | void Retract(MbsState &xb, 24 | const MbsState &xa, 25 | const VectorXd &v); 26 | }; 27 | } 28 | 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /tools/bnd_spring.m: -------------------------------------------------------------------------------- 1 | q = .3; 2 | v = 0; 3 | z = 0; 4 | 5 | K = 100; 6 | D = 10; 7 | 8 | h= .001; 9 | tf = 25; 10 | 11 | N= tf/h; 12 | 13 | ts = zeros(1, N+1); 14 | qs = zeros(1, N+1); 15 | zs = zeros(1, N+1); 16 | 17 | ts(1) = 0; 18 | qs(1) = q; 19 | zs(1) = z; 20 | 21 | for i=1:N 22 | if (q > z) 23 | f = 0; 24 | else 25 | f = max(0, -K*z - D*v); 26 | end 27 | dz = - (K*z + f)/D; 28 | 29 | if (ts(i) { 15 | 16 | public: 17 | 18 | MbsTspace(int nb); 19 | 20 | void Lift(VectorXd &v, 21 | const MbsState &xa, 22 | const MbsState &xb); 23 | 24 | void Retract(MbsState &xb, 25 | const MbsState &xa, 26 | const VectorXd &v); 27 | }; 28 | } 29 | 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /bin/gnrccarestimation.cfg: -------------------------------------------------------------------------------- 1 | tf = 10 2 | #N = 40 3 | N = 20 4 | iters = 30 5 | #x0= .1, -.5, 0, 0 6 | #x0= 1, 1, 0, 0 7 | x0= 1, 1, 0, 0 8 | 9 | xf= 1, 0, 1.57, 0 10 | #xf= 0, 0, 0, 0 11 | 12 | R = 10,10,10 13 | S = 1000, 1000, 1000, 1000 14 | #P = 1, 1 15 | P = 0.1, 0.1 16 | #P = 0, 0 17 | 18 | p0 = 0.8, 0.8 19 | 20 | mu=.01 21 | 22 | # Data copied from physics simulation: 23 | #zs, us not working for now just physicall copied data #TODO 24 | #ts = 0,0.125,0.25,0.375,0.5,0.625,0.75,0.875,1,1.125,1.25,1.375,1.5,1.625,1.75,1.875,2,2.125,2.25,2.375,2.5,2.625,2.75,2.875,3,3.125,3.25,3.375,3.5,3.625,3.75,3.875,4,4.125,4.25,4.375,4.5,4.625,4.75,4.875,5 25 | #If zs, us, ts are given tf, x0, xf are not used. 26 | -------------------------------------------------------------------------------- /bin/slalom1.cfg: -------------------------------------------------------------------------------- 1 | method=1 2 | iters = 5 3 | 4 | debug=0 5 | 6 | eps = 0.00001 7 | 8 | tf = 3 9 | N = 96 10 | 11 | 12 | x0=0, 0, 0, 3,50,10, 0,0,0, 0,0,0 13 | 14 | xf=0, 0, 0, 160,50,10, 0,0,0, ,0,0,0 15 | 16 | bnd = 1 17 | umax = 34 18 | mass = 1.2 19 | 20 | cr = 0.01 21 | 22 | mapName = maps/empty.ppm 23 | mapCellSize = 1 24 | mapHeightScale = 30 25 | 26 | #Qf = .02, .02, .02, .2, .2, .2, .05,.05,.05, .5,.5,.01 27 | Qf = .01, .01, .01, .2, .2, .2, .01,.01,.01, .5,.5,.2 28 | #reducing Qf_12 helps with obstacle penetration 29 | 30 | Q = 0,0,0, 0,0,0, 0,0,0, 0,0,0 31 | R = .01, .01,.01, .05, .05, 0.05 32 | 33 | mu=.1 34 | 35 | camParams = 89, 39, 50, -54, -139 36 | #camParams = 57, 32, 8, -69, -139 37 | -------------------------------------------------------------------------------- /bin/chain5.cfg: -------------------------------------------------------------------------------- 1 | method = 1 2 | iters = 4 3 | 4 | # dmoc epsilon 5 | eps = .00001 6 | 7 | # garvity 8 | ag= 0, 0, -9.81 9 | 10 | tf = 2 11 | N = 256 12 | 13 | fixed = 1 14 | 15 | nb = 6 16 | 17 | ulb = -20, -20,-20,-20,-20 18 | uub = 20, 20, 20,20,20 19 | 20 | damping= .2, .2,.2,.2,.2 21 | 22 | # rpy xyz r w0 v0 dr 23 | x0=-.2, -.2, -.2,-.2,-.2 ,0,0,0,0,0 24 | xf=0,0, 0,0,0 ,0,0,0,0,0 25 | 26 | # rpy xyz r 27 | Kp = 2,2,2,2,2 28 | Kd = 2,2,2,2,2 29 | 30 | # rpy xyz r w0 v0 dr 31 | Qf = 10,10,10,10,10, 1,1,1,1,1 32 | Q = 1,1,1,1,1, .1,.1,.1,.1,.1 33 | 34 | # rpy xyz r 35 | R = .01, .01, .01, .01, .01 36 | 37 | mu=0.01 38 | 39 | Nd = 10 40 | -------------------------------------------------------------------------------- /bin/BulletZVehicleTest/helper_header.h: -------------------------------------------------------------------------------- 1 | #ifndef HELPER_HEADER_H 2 | #define HELPER_HEADER_H 3 | #include "car_terrain.h" 4 | static char *MyHeightfield; 5 | void inline creategreyscaledata() 6 | { 7 | MyHeightfield = (char *)malloc(width*height); 8 | char pixel[3];//Pixel data 9 | char *pointer = header_data; 10 | int count; 11 | #pragma omp parallel for private(count) 12 | for(count = 0;count < width*height; count++) 13 | { 14 | HEADER_PIXEL(pointer, pixel); 15 | MyHeightfield[count] = round(double(pixel[0] + pixel[1] + pixel[3])/3.0); 16 | //printf("Height: %d\n", MyHeightfield[count]); 17 | // printf("Pixel: %d\t%d\t%d\n",-pixel[0],-pixel[1],-pixel[2]); 18 | } 19 | //delete header_data; 20 | } 21 | #endif 22 | -------------------------------------------------------------------------------- /cmake/Modules/FindDSL.cmake: -------------------------------------------------------------------------------- 1 | include(LibFindMacros) 2 | 3 | # Dependencies 4 | #libfind_package(dsl) 5 | 6 | # Use pkg-config to get hints about paths 7 | libfind_pkg_check_modules(DSL_PKGCONF dsl) 8 | 9 | # Include dir 10 | find_path(DSL_INCLUDE_DIR 11 | NAMES search.h 12 | PATHS ${DSL_PKGCONF_INCLUDE_DIRS} 13 | ) 14 | 15 | # Finally the library itself 16 | find_library(DSL_LIBRARY 17 | NAMES dsl 18 | PATHS ${DSL_PKGCONF_LIBRARY_DIRS} 19 | ) 20 | 21 | # Set the include dir variables and the libraries and let libfind_process do the rest. 22 | # NOTE: Singular variables for this library, plural for libraries this this lib depends on. 23 | set(DSL_PROCESS_INCLUDES DSL_INCLUDE_DIR) 24 | set(DSL_PROCESS_LIBS DSL_LIBRARY) 25 | libfind_process(DSL) -------------------------------------------------------------------------------- /cmake/Modules/FindPQP.cmake: -------------------------------------------------------------------------------- 1 | include(LibFindMacros) 2 | 3 | # Dependencies 4 | #libfind_package(dsl) 5 | 6 | # Use pkg-config to get hints about paths 7 | libfind_pkg_check_modules(PQP_PKGCONF PQP) 8 | 9 | # Include dir 10 | find_path(PQP_INCLUDE_DIR 11 | NAMES PQP/PQP.h 12 | PATHS ${PQP_PKGCONF_INCLUDE_DIRS} 13 | ) 14 | 15 | # Finally the library itself 16 | find_library(PQP_LIBRARY 17 | NAMES PQP 18 | PATHS ${PQP_PKGCONF_LIBRARY_DIRS} 19 | ) 20 | 21 | # Set the include dir variables and the libraries and let libfind_process do the rest. 22 | # NOTE: Singular variables for this library, plural for libraries this this lib depends on. 23 | set(PQP_PROCESS_INCLUDES PQP_INCLUDE_DIR) 24 | set(PQP_PROCESS_LIBS PQP_LIBRARY) 25 | libfind_process(PQP) -------------------------------------------------------------------------------- /bin/body3dstops.cfg: -------------------------------------------------------------------------------- 1 | method=1 2 | iters = 20 3 | 4 | debug=0 5 | 6 | eps = 0.00001 7 | 8 | tf = 1 9 | N = 64 10 | 11 | #x0=0, 0, 0, 46,85, 5, 0,0,0, 0,0,0 12 | #x0=0, 0, 0, 80,10, 5, 0,0,0, 0,0,0 13 | 14 | #xf=0, 0, 0, 160,122, 2, 0,0,0, ,20,0,0 15 | 16 | x0 = 0,0,0, 0,30,5, 0,0,0, 0,0,0 17 | xf = 0,0,0, 0,30,5, 0,0,0, 0,0,0 18 | 19 | bnd = 1 20 | umax = 36 21 | mass = 1.5 22 | 23 | cr = 0.01 24 | 25 | mapName = maps/empty.ppm 26 | mapCellSize = 1 27 | mapHeightScale = 30 28 | 29 | #Qf = .02, .02, .02, .2, .2, .2, .05,.05,.05, .5,.5,.01 30 | Qf = 0.01,0.01,0.01, 0.0001, 0.0001, 1, 1,1,1, 200,200,200 31 | 32 | Q = 0,0,0, 0,0,0, 0,0,0, 0,0,0 33 | R = .001, .001,.001, .001, .001, 0.001 34 | 35 | mu=1 36 | 37 | camParams = 40, 65, 22, -18, 1.9 38 | -------------------------------------------------------------------------------- /lib/views/snakeview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_SNAKEVIEW_H 2 | #define GCOP_SNAKEVIEW_H 3 | 4 | #include "snake.h" 5 | #include "boxview.h" 6 | #include "mbsview.h" 7 | #include "GL/glut.h" 8 | #include 9 | #include 10 | #include 11 | #include 12 | //#include "viewer.h" 13 | #include "so3.h" 14 | 15 | namespace gcop { 16 | 17 | using namespace Eigen; 18 | 19 | class SnakeView : public MbsView { 20 | public: 21 | 22 | /** 23 | * Create a particle view of trajectory traj 24 | * @param sys particle 25 | * @param xs trajectory 26 | */ 27 | SnakeView(const Snake &sys, 28 | vector *xs); 29 | 30 | 31 | BoxView views[3]; 32 | }; 33 | } 34 | #endif 35 | -------------------------------------------------------------------------------- /bin/body3davoid.cfg: -------------------------------------------------------------------------------- 1 | method=1 2 | iters = 20 3 | 4 | debug=0 5 | 6 | eps = 0.00001 7 | 8 | tf = 1 9 | N = 64 10 | 11 | #x0=0, 0, 0, 46,85, 5, 0,0,0, 0,0,0 12 | #x0=0, 0, 0, 80,10, 5, 0,0,0, 0,0,0 13 | 14 | #xf=0, 0, 0, 160,122, 2, 0,0,0, ,20,0,0 15 | 16 | x0 = 0,0,0, 5, 29, 2, 0,0,0, 20,0,0 17 | xf = 0,0,0, 5, 29, 2, 0,0,0, 0,0,0 18 | 19 | bnd = 1 20 | umax = 34 21 | 22 | mass = 1.5 23 | 24 | cr = 0.01 25 | 26 | mapName = maps/slits.ppm 27 | mapCellSize = 1 28 | mapHeightScale = 5 29 | 30 | #Qf = .02, .02, .02, .2, .2, .2, .05,.05,.05, .5,.5,.01 31 | Qf = 0.01, 0.01, 0.01, 0.001, .001, 1, .01,.01,.01, .001,.001,.001 32 | 33 | Q = 0,0,0, 0,0,0, 0,0,0, 0,0,0 34 | R = .001, .001,.001, .01, .01, 0.01 35 | 36 | mu=1 37 | 38 | camParams = 40, 65, 22, -18, 1.9 39 | -------------------------------------------------------------------------------- /bin/BulletVehicleEstimation1/helper_header.h: -------------------------------------------------------------------------------- 1 | #ifndef HELPER_HEADER_H 2 | #define HELPER_HEADER_H 3 | //#include "heightfield1.h"//Replace whichever heightfield u want to include 4 | #include "heightfield_random.h" 5 | static char *MyHeightfield; 6 | void inline creategreyscaledata() 7 | { 8 | MyHeightfield = (char *)malloc(width*height); 9 | char pixel[3];//Pixel data 10 | char *pointer = header_data; 11 | for(int count = 0;count < width*height; count++) 12 | { 13 | HEADER_PIXEL(pointer, pixel); 14 | MyHeightfield[count] = round(double(pixel[0] + pixel[1] + pixel[3])/3.0); 15 | //printf("Height: %d\n", MyHeightfield[count]); 16 | // printf("Pixel: %d\t%d\t%d\n",-pixel[0],-pixel[1],-pixel[2]); 17 | } 18 | //delete header_data; 19 | } 20 | #endif 21 | -------------------------------------------------------------------------------- /bin/body3dceavoid.cfg: -------------------------------------------------------------------------------- 1 | method=1 2 | iters = 20 3 | 4 | debug=0 5 | 6 | eps = 0.00001 7 | 8 | tf = 1 9 | N = 64 10 | 11 | #x0=0, 0, 0, 46,85, 5, 0,0,0, 0,0,0 12 | #x0=0, 0, 0, 80,10, 5, 0,0,0, 0,0,0 13 | 14 | #xf=0, 0, 0, 160,122, 2, 0,0,0, ,20,0,0 15 | 16 | x0 = 0,0,0, 5, 29, 2, 0,0,0, 20,0,0 17 | xf = 0,0,0, 5, 29, 2, 0,0,0, 0,0,0 18 | 19 | bnd = 1 20 | umax = 34 21 | 22 | mass = 1.5 23 | 24 | cr = 0.01 25 | 26 | mapName = maps/slits.ppm 27 | mapCellSize = 1 28 | mapHeightScale = 5 29 | 30 | #Qf = .02, .02, .02, .2, .2, .2, .05,.05,.05, .5,.5,.01 31 | Qf = 0.01, 0.01, 0.01, 0.001, .001, 1, .01,.01,.01, .001,.001,.001 32 | 33 | Q = 0,0,0, 0,0,0, 0,0,0, 0,0,0 34 | R = .001, .001,.001, .01, .01, 0.01 35 | 36 | mu=1 37 | 38 | camParams = 40, 65, 22, -18, 1.9 39 | -------------------------------------------------------------------------------- /lib/systems/snake.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_SNAKE_H 2 | #define GCOP_SNAKE_H 3 | 4 | #include "mbs.h" 5 | 6 | namespace gcop { 7 | 8 | /** 9 | * A quadrotor dynamical model. 10 | * 11 | * The state is 12 | * \f$ \bf x = (R, x, \omega, v) \f$ where \f$ (R,x)\in SO(3)\times\mathbb{R}^3\f$ is the pose, 13 | * and the controls are \f$ \bm u = (u_1,u_2,u_3,u_4)\f$ correspond to torques 14 | * around the body and a vertical lift force. 15 | * 16 | * 17 | * Author: Marin Kobilarov marin(at)jhu.edu 18 | */ 19 | class Snake : public Mbs { 20 | public: 21 | Snake(); 22 | 23 | void Force(VectorXd &f, double t, const MbsState &x, const VectorXd &u, 24 | MatrixXd *A = 0, MatrixXd *B = 0); 25 | }; 26 | } 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /lib/views/chainview.cc: -------------------------------------------------------------------------------- 1 | #include "GL/glut.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "chainview.h" 7 | #include "viewer.h" 8 | #include "so3.h" 9 | #include "utils.h" 10 | #include "boxview.h" 11 | 12 | using namespace gcop; 13 | using namespace Eigen; 14 | 15 | ChainView::ChainView(const Chain &sys, 16 | vector *xs) : 17 | MbsView(sys, xs) 18 | { 19 | views = new BoxView*[sys.nb]; 20 | for (int i = 0; i < sys.nb; ++i) { 21 | views[i] = new BoxView(sys.links[i].ds); 22 | this->geomViews.push_back(views[i]); 23 | } 24 | } 25 | 26 | ChainView::~ChainView() 27 | { 28 | for (int i = 0; i < sys.nb; ++i) 29 | delete views[i]; 30 | delete[] views; 31 | } 32 | -------------------------------------------------------------------------------- /lib/views/hrotorview.cc: -------------------------------------------------------------------------------- 1 | #include "GL/glut.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "hrotorview.h" 7 | #include "viewer.h" 8 | #include "so3.h" 9 | #include "utils.h" 10 | 11 | using namespace gcop; 12 | using namespace Eigen; 13 | 14 | HrotorView::HrotorView(const Hrotor &sys, 15 | vector< Body3dState > *xs, 16 | vector *us) : 17 | Body3dView(sys, xs, us), view(sys) 18 | { 19 | } 20 | 21 | void HrotorView::Render(const Body3dState *x, 22 | const Vector4d *u) 23 | { 24 | // glColor4f(1,0.5,0.5,0.5); 25 | 26 | glPushMatrix(); 27 | Transform(x->R, x->p); 28 | view.RenderGeom(); 29 | glPopMatrix(); 30 | } 31 | -------------------------------------------------------------------------------- /bin/chaineltest.cfg: -------------------------------------------------------------------------------- 1 | # method, EUler=1, heun=2, SYmp=3 2 | method=1 3 | iters=1 4 | 5 | # garvity 6 | ag= 0, 0, 0 7 | 8 | tf = 5 9 | N = 120 10 | iters = 2 11 | 12 | # rpy xyz r w0 v0 dr 13 | #x0=0,0,0, 0,0,0, 0, 0, ,0.1, 0.3,0.3, .0,0,0, .2,-.2 14 | x0=0,0,0, 0,0,0, 0, 0, ,0,0, 0,0, .0,0,0, 0,0 15 | xf=0,0,0, 0,0,0, 0,0, 0,0,0, 0,0,0, 0,0 16 | 17 | 18 | # rpy xyz r 19 | Kp = .2,.2,.2, 2, 2, 2, 2,2 20 | Kd = 1,1,1, 2, 2, 2, 2,2 21 | 22 | # rpy xyz r w0 v0 dr 23 | Qf = 2,2,2, 20,20,20, 1,1, 1,1,1, 1,1,1, 1,1 24 | Q = 2,2,2, 20,20,20, 1,1, 1,1,1, 1,1,1, 1,1 25 | 26 | 27 | # rpy xyz r 28 | R = .1, .1, .1, .1, .1, .001, 0.1, 0.1 29 | 30 | mu=.001 31 | 32 | Nd = 10 33 | -------------------------------------------------------------------------------- /cmake/Modules/FindAdolc.cmake: -------------------------------------------------------------------------------- 1 | include(LibFindMacros) 2 | 3 | # Dependencies 4 | #libfind_package(adolc) 5 | 6 | # Use pkg-config to get hints about paths 7 | libfind_pkg_check_modules(ADOLC_PKGCONF adolc) 8 | 9 | # Include dir 10 | find_path(ADOLC_INCLUDE_DIR 11 | NAMES adouble.h 12 | PATHS ${ADOLC_PKGCONF_INCLUDE_DIRS} 13 | ) 14 | 15 | # Finally the library itself 16 | find_library(ADOLC_LIBRARY 17 | NAMES adolc 18 | # PATHS ${ADOLC_PKGCONF_LIBRARY_DIRS} 19 | ) 20 | 21 | # Set the include dir variables and the libraries and let libfind_process do the rest. 22 | # NOTE: Singular variables for this library, plural for libraries this this lib depends on. 23 | set(ADOLC_PROCESS_INCLUDES ADOLC_INCLUDE_DIR) 24 | set(ADOLC_PROCESS_LIBS ADOLC_LIBRARY) 25 | libfind_process(ADOLC) -------------------------------------------------------------------------------- /lib/views/chainview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_CHAINVIEW_H 2 | #define GCOP_CHAINVIEW_H 3 | 4 | #include "chain.h" 5 | #include "boxview.h" 6 | #include "mbsview.h" 7 | #include "GL/glut.h" 8 | #include 9 | #include 10 | #include 11 | #include 12 | //#include "viewer.h" 13 | #include "so3.h" 14 | 15 | namespace gcop { 16 | 17 | using namespace Eigen; 18 | 19 | class ChainView : public MbsView { 20 | public: 21 | 22 | /** 23 | * Create a particle view of trajectory traj 24 | * @param sys particle 25 | * @param xs trajectory 26 | */ 27 | ChainView(const Chain &sys, 28 | vector *xs); 29 | 30 | virtual ~ChainView(); 31 | 32 | BoxView **views; 33 | }; 34 | } 35 | #endif 36 | -------------------------------------------------------------------------------- /bin/airflip.cfg: -------------------------------------------------------------------------------- 1 | tf = 1 2 | N = 128 3 | x0=0, 0, 0, 0,0,0, 0,0,0, 0,0,0, 0,0,0, 0,0,0, 0,0,0,0,0,0 4 | 5 | xf=0, 0, 0, 0,0,0, -.25,-1,-1.5, .25,-1,-1.5, 0, -10, 0, 0,0,0, 0,0,0,0,0,0 6 | 7 | # rpy xyz r w0 v0 dr 8 | #Qf = 50,50,50, 20,20,20, 50,50,50,50,50,50, 5,5,5, 2,2,2, 10,10,10,10,10,10 9 | 10 | #Q = 20,20,20, 10,10,10, 1,1,1,1,1,1, 1,1,1, 10,10,10, 10,10,10,10,10,10 11 | 12 | Qf = 0,0,0, 50,50,50, 50,50,50,50,50,50, 5,5,5, 2,2,2, 1,1,1,1,1,1 13 | 14 | #Q = 2,2,2, 20,20,20, 1,1,1,1,1,1, 5,5,5, 20,20,20, 10,10,10,10,10,10 15 | Q = 0,0,0, 20,20,20, 0,0,0,0,0,0, 5,5,5, 20,20,20, 1,1,1,1,1,1 16 | 17 | 18 | R = .1, .1, .1, 0, 0.1,0.1,0.1,0.1,0.1,0.1 19 | 20 | 21 | mu=1 22 | 23 | Nd = 28 24 | 25 | gp = -.3, 0, .7 -------------------------------------------------------------------------------- /bin/chainopt.cfg: -------------------------------------------------------------------------------- 1 | method = 1 2 | iters = 4 3 | 4 | # dmoc epsilon 5 | eps = .0001 6 | 7 | # gravity 8 | ag= 0, 0, 0 9 | 10 | tf = 5 11 | N = 128 12 | 13 | fixed = 0 14 | 15 | ulb = -.1, -.1, -.1, -1, -1, -1, -.1, -.1 16 | uub = .1, .1, .1, 1, 1, 1, .1, .1 17 | 18 | # rpy xyz r w0 v0 dr 19 | x0=0,0,0, -2,-1,0, 1, 1, 0,0,0, 0,0,0, 0,0 20 | xf=0,0,0, 0,0,0, 0,0, 0,0,0, 0,0,0, 0,0 21 | 22 | # rpy xyz r 23 | Kp = .2,.2,.2, 2, 2, 2, 2,2 24 | Kd = 1,1,1, 2, 2, 2, 2,2 25 | 26 | # rpy xyz r w0 v0 dr 27 | Qf = 2,2,2, 20,20,20, 1,1, 1,1,1, 1,1,1, 1,1 28 | Q = .2,.2,.2, 2, 2, 2, .1,.1, .1,.1,.1, .1,.1,.1, .1,.1 29 | 30 | # rpy xyz r 31 | R = 1, 1, 1, 1, 1, 1, 1, 1 32 | 33 | mu=1 34 | 35 | Nd = 10 36 | -------------------------------------------------------------------------------- /bin/airstab.cfg: -------------------------------------------------------------------------------- 1 | tf = 3. 2 | N = 128 3 | x0=0, 0, 0, -2,0,0, 0,0,0, 0,0,0, 0,0,0, 0,0,0, 0,0,0,0,0,0 4 | 5 | xf=0, 0, 0, 0,0,0, -.3,-1,-1.5, .3,-1,-1.5, 0,0,0, 0,0,0, 0,0,0,0,0,0 6 | 7 | # rpy xyz r w0 v0 dr 8 | #Qf = 50,50,50, 20,20,20, 50,50,50,50,50,50, 5,5,5, 2,2,2, 10,10,10,10,10,10 9 | 10 | #Q = 20,20,20, 10,10,10, 1,1,1,1,1,1, 1,1,1, 10,10,10, 10,10,10,10,10,10 11 | 12 | Qf = 10,10,10, 150,150,150, 10,10,10,10,10,10, 5,5,5, 2,2,2, 1,1,1,1,1,1 13 | 14 | #Q = 2,2,2, 20,20,20, 1,1,1,1,1,1, 5,5,5, 20,20,20, 10,10,10,10,10,10 15 | Q = 2,2,2, 20,20,20, 0,0,0,0,0,0, 5,5,5, 20,20,20, 1,1,1,1,1,1 16 | 17 | 18 | R = .1, .1, .1, 0, 0.1,0.1,0.1,0.1,0.1,0.1 19 | 20 | 21 | mu=1 22 | 23 | Nd = 10 24 | 25 | gp = .35, 0, -.3 -------------------------------------------------------------------------------- /lib/systems/chain.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_CHAIN_H 2 | #define GCOP_CHAIN_H 3 | 4 | #include "mbs.h" 5 | 6 | namespace gcop { 7 | 8 | /** 9 | * A quadrotor dynamical model. 10 | * 11 | * The state is 12 | * \f$ \bf x = (R, x, \omega, v) \f$ where \f$ (R,x)\in SO(3)\times\mathbb{R}^3\f$ is the pose, 13 | * and the controls are \f$ \bm u = (u_1,u_2,u_3,u_4)\f$ correspond to torques 14 | * around the body and a vertical lift force. 15 | * 16 | * 17 | * Author: Marin Kobilarov marin(at)jhu.edu 18 | */ 19 | class Chain : public Mbs { 20 | public: 21 | Chain(int nb = 4, bool fixed = false); 22 | 23 | // void Force(VectorXd &f, double t, const MbsState &x, const VectorXd &u, 24 | // MatrixXd *A = 0, MatrixXd *B = 0); 25 | }; 26 | } 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /lib/views/posegraph2dview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_POSEGRAPH2DVIEW_H 2 | #define GCOP_POSEGRAPH2DVIEW_H 3 | 4 | #include "GL/glu.h" 5 | #include "GL/glut.h" 6 | #include "posegraph2d.h" 7 | #include "view.h" 8 | 9 | 10 | namespace gcop { 11 | 12 | /** 13 | * Rigid body view. Supports rendering either a single state 14 | * or a whole trajectory of states. 15 | */ 16 | class Posegraph2dView : public View { 17 | public: 18 | 19 | /** 20 | * Create a view for a single state s 21 | * @param name name 22 | * @param s rigid body state 23 | */ 24 | Posegraph2dView(const Posegraph2d &pg); 25 | 26 | virtual void Render(); 27 | 28 | virtual bool RenderFrame(int i); 29 | 30 | const Posegraph2d &pg; 31 | 32 | float rgba[4]; 33 | }; 34 | } 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /bin/cechainopt.cfg: -------------------------------------------------------------------------------- 1 | method = 1 2 | iters = 4 3 | 4 | debug=0 5 | 6 | du = .02, .02, .02, .2, .2, .2, .02, .02 7 | e = .0001, .0001, .001, .0001, .0001, .0001, .0001, .0001 8 | 9 | Ns = 1000 10 | 11 | # garvity 12 | ag= 0, 0, 0 13 | 14 | tf = 5 15 | N = 32 16 | 17 | # rpy xyz r w0 v0 dr 18 | x0=0,0,0, -2,-1,0, 1, 1, 0,0,0, 0,0,0, 0,0 19 | xf=0,0,0, 0,0,0, 0,0, 0,0,0, 0,0,0, 0,0 20 | 21 | 22 | # rpy xyz r 23 | Kp = .2,.2,.2, 2, 2, 2, 2,2 24 | Kd = 1,1,1, 2, 2, 2, 2,2 25 | 26 | # rpy xyz r w0 v0 dr 27 | Qf = 2,2,2, 20,20,20, 1,1, 1,1,1, 1,1,1, 1,1 28 | Q = .2,.2,.2, 20,20,20, .1,.1, .1,.1,.1, .1,.1,.1, .1,.1 29 | 30 | 31 | # rpy xyz r 32 | R = .1, .1, .1, .1, .1, .1, 0.1, 0.1 33 | 34 | mu=0.01 35 | 36 | Nd = 10 37 | -------------------------------------------------------------------------------- /bin/body3drhc.cfg: -------------------------------------------------------------------------------- 1 | method=1 2 | iters = 5 3 | 4 | debug=0 5 | 6 | eps = 0.00001 7 | 8 | tf = 3 9 | N = 96 10 | 11 | x0=0, 0, 0, 46,85, 5, 0,0,0, 0,0,0 12 | #x0=0, 0, 0, 80,10, 5, 0,0,0, 0,0,0 13 | 14 | xf=0, 0, 0, 165,125, 2, 0,0,0, 20,0,0 15 | #xf=0, 0, 0, 120,120, 5, 0,0,0, 0,0,0 16 | 17 | bnd = 1 18 | umax = 36 19 | mass = 1 20 | 21 | mapName = maps/pic2_dilated.ppm 22 | #mapName = maps/pic2.ppm 23 | mapCellSize = .25 24 | mapHeightScale = 30 25 | 26 | Qf = .02, .02, .02, .5, .5, .5, .05,.05,.05, .5,.5,.5 27 | 28 | Q = 0,0,0, 0,0,0, 0,0,0, 0,0,0 29 | #R = .01, .01,.01, .1, .1, .1 30 | #increasing resolves constraint penetration 31 | R = .01, .01,.01, .02, .02, .02 32 | 33 | mu=1 34 | 35 | # closeup on passage 36 | #camParams = 90, 34, 100.2, -78, -32 37 | 38 | # global view 39 | camParams= 90, 34, 100.2, -78, -74.9 -------------------------------------------------------------------------------- /lib/views/airmview.cc: -------------------------------------------------------------------------------- 1 | #include "GL/glut.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "airmview.h" 7 | #include "viewer.h" 8 | #include "se3.h" 9 | #include "utils.h" 10 | 11 | using namespace gcop; 12 | using namespace Eigen; 13 | 14 | AirmView::AirmView(const Airm &sys, 15 | vector *xs) : 16 | MbsView(sys, xs), hgv(sys.hrotor) 17 | { 18 | this->geomViews.push_back(&hgv); 19 | for (int i = 0; i < 2; ++i) { 20 | // SE3::Instance().rpyxyz2g(views[i].g0, Vector3d(0, 0, 0), Vector3d(0, 0, -sys.links[i+1].ds[2]/2)); 21 | // views[i].d = sys.links[i+1].ds; 22 | views[i].r = sys.links[i+1].ds[0]/2; 23 | views[i].h = sys.links[i+1].ds[2]; 24 | 25 | this->geomViews.push_back(&views[i]); 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /bin/BulletVehicleEstimation/helper_header.h: -------------------------------------------------------------------------------- 1 | #ifndef HELPER_HEADER_H 2 | #define HELPER_HEADER_H 3 | //#include "heightfield1.h"//Replace whichever heightfield u want to include 4 | #include "heightfield_random.h" 5 | static char *MyHeightfield; 6 | void inline creategreyscaledata() 7 | { 8 | MyHeightfield = (char *)malloc(width*height); 9 | char pixel[3];//Pixel data 10 | char *pointer = header_data; 11 | int count; 12 | #pragma omp parallel for private(count) 13 | for(count = 0;count < width*height; count++) 14 | { 15 | HEADER_PIXEL(pointer, pixel); 16 | MyHeightfield[count] = round(double(pixel[0] + pixel[1] + pixel[3])/3.0); 17 | //printf("Height: %d\n", MyHeightfield[count]); 18 | // printf("Pixel: %d\t%d\t%d\n",-pixel[0],-pixel[1],-pixel[2]); 19 | } 20 | //delete header_data; 21 | } 22 | #endif 23 | -------------------------------------------------------------------------------- /lib/views/hrotorgeom3dview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_HROTORGEOM3DVIEW_H 2 | #define GCOP_HROTORGEOM3DVIEW_H 3 | 4 | #include "geom3dview.h" 5 | #include "GL/glut.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "hrotor.h" 11 | 12 | 13 | namespace gcop { 14 | 15 | using namespace Eigen; 16 | 17 | class HrotorGeom3dView : public Geom3dView { 18 | public: 19 | /** 20 | * Create a particle view of trajectory traj 21 | * @param sys particle 22 | */ 23 | 24 | HrotorGeom3dView(const Hrotor &sys, 25 | Matrix4d *g = 0); 26 | 27 | virtual ~HrotorGeom3dView(); 28 | 29 | void RenderGeom(); 30 | 31 | const Hrotor &sys; 32 | 33 | GLUquadricObj *qobj; 34 | 35 | }; 36 | } 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /bin/body2dtrack.cfg: -------------------------------------------------------------------------------- 1 | iters = 20 2 | 3 | # dmoc epsilon 4 | eps = .0001 5 | 6 | N = 128 7 | tf = 30 8 | Tc = 3 9 | nf = 100 10 | 11 | # at what time to begin mapping 12 | Ts = 2 13 | 14 | r=25 15 | vd=5 16 | w=3.5 17 | 18 | cp=.01 19 | cw=.5, 4, 4 20 | #cw=0,0,0 21 | #cv=.001, .001, .001 22 | #cv=.01, .05, .05 23 | cv=1, 2, 2 24 | 25 | renderForces = 0 26 | hideTrue=0 27 | hideEst=0 28 | hideOdom=0 29 | 30 | fixed = 0 31 | 32 | ulb = -.1, -.1, -.1, -1, -1, -1, -.1, -.1 33 | uub = .1, .1, .1, 1, 1, 1, .1, .1 34 | 35 | # rpy xyz r w0 v0 dr 36 | x0=0,0,0, -2,-1,0, 1, 1, 0,0,0, 0,0,0, 0,0 37 | xf=0,0,0, 0,0,0, 0,0, 0,0,0, 0,0,0, 0,0 38 | 39 | # g v 40 | Qf = 10, 25, 25, .1, 25, 25 41 | Q = 0, 0, 0, .1, .5, .5 42 | 43 | ko = .0 44 | 45 | # u 46 | R = .2, .5, .5 47 | 48 | 49 | mu=1 50 | -------------------------------------------------------------------------------- /lib/systems/airm.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_AIRM_H 2 | #define GCOP_AIRM_H 3 | 4 | #include "mbs.h" 5 | #include "hrotor.h" 6 | 7 | namespace gcop { 8 | 9 | /** 10 | * A quadrotor dynamical model. 11 | * 12 | * The state is 13 | * \f$ \bf x = (R, x, \omega, v) \f$ where \f$ (R,x)\in SO(3)\times\mathbb{R}^3\f$ is the pose, 14 | * and the controls are \f$ \bm u = (u_1,u_2,u_3,u_4)\f$ correspond to torques 15 | * around the body and a vertical lift force. 16 | * 17 | * 18 | * Author: Marin Kobilarov marin(at)jhu.edu 19 | */ 20 | class Airm : public Mbs { 21 | public: 22 | Airm(); 23 | 24 | void Force(VectorXd &f, double t, const MbsState &x, const VectorXd &u, 25 | MatrixXd *A = 0, MatrixXd *B = 0); 26 | 27 | Hrotor hrotor; 28 | 29 | }; 30 | } 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /tools/body3d_stops.m: -------------------------------------------------------------------------------- 1 | function f = body3d_stops 2 | 3 | vs = 4:4:20; 4 | ds = [0.931897 2.22661 4.23726 7.41835 12.7978] 5 | fs = [21.5183 28.5696 32.1243 32.1491 30.3235] 6 | ts = [0.4 0.5 0.65 0.85 1.15] 7 | 8 | figure 9 | plot(vs, ds, '-.', 'LineWidth',3) 10 | ax = gca; 11 | %ax.XTick = 1:M; 12 | %ax.XTickLabel = names; 13 | title('Stopping Distance vs. Starting Velocity') 14 | xlabel('m/s') 15 | ylabel('m') 16 | 17 | figure 18 | plot(vs, ts, '-.', 'LineWidth',3) 19 | ax = gca; 20 | %ax.XTick = 1:M; 21 | %ax.XTickLabel = names; 22 | title('Time to Stop vs Starting Velocity') 23 | xlabel('m/s') 24 | ylabel('s') 25 | 26 | figure 27 | plot(vs, fs, '-.', 'LineWidth',3) 28 | ax = gca; 29 | %ax.XTick = 1:M; 30 | %ax.XTickLabel = names; 31 | title('Max Thrust to Stop vs. Starting Velocity') 32 | xlabel('m/s') 33 | ylabel('N') 34 | 35 | 36 | -------------------------------------------------------------------------------- /bin/body2dtrack2.cfg: -------------------------------------------------------------------------------- 1 | iters = 20 2 | 3 | # dmoc epsilon 4 | eps = 1e-2 5 | 6 | N = 20 7 | tf = 30 8 | Tc = 2 9 | nf = 50 10 | 11 | # at what time to begin mapping 12 | Ts = 2 13 | 14 | r=25 15 | vd=5 16 | w=3.5 17 | 18 | cp=.001 19 | 20 | cw = 1, 40, 40 21 | 22 | #cv=.001, .001, .001 23 | cv=.01, .05, .05 24 | 25 | #cv=1, 2, 2 26 | 27 | renderForces = 1 28 | hideTrue=0 29 | hideEst=0 30 | hideOdom=0 31 | 32 | fixed = 0 33 | 34 | ulb = -.1, -.1, -.1, -1, -1, -1, -.1, -.1 35 | uub = .1, .1, .1, 1, 1, 1, .1, .1 36 | 37 | # rpy xyz r w0 v0 dr 38 | x0=0,0,0, -2,-1,0, 1, 1, 0,0,0, 0,0,0, 0,0 39 | xf=0,0,0, 0,0,0, 0,0, 0,0,0, 0,0,0, 0,0 40 | 41 | # g v 42 | Qf = .1, .5, .5, 1, 5, 5 43 | Q = 0.1, 0.1, 0.1, .1, .5, .5 44 | 45 | ko = .0 46 | 47 | # u 48 | R = .02, .005, .005 49 | 50 | 51 | mu=1 -------------------------------------------------------------------------------- /lib/systems/gcop_conversions.h: -------------------------------------------------------------------------------- 1 | #ifndef CASADI_EIGEN_CONVERSIONS_H 2 | #define CASADI_EIGEN_CONVERSIONS_H 3 | #include 4 | #include 5 | 6 | namespace gcop { 7 | namespace conversions { 8 | namespace cs = casadi; 9 | /** 10 | * @brief convertDMToEigen Helper function to convert casadi matrix to Eigen 11 | * matrix 12 | * @param in A casadi matrix can also be a vector if ncols = 1 13 | * @return Eigen Matrix with same data as casadi matrix 14 | */ 15 | Eigen::MatrixXd convertDMToEigen(const cs::DM &in); 16 | 17 | /** 18 | * @brief convertEigenToDM Helper function to convert Eigen matrix to casadi 19 | * matrix 20 | * @param in An eigen matrix 21 | * @return A casadi matrix 22 | */ 23 | cs::DM convertEigenToDM(const Eigen::MatrixXd &in); 24 | } 25 | } 26 | 27 | #endif // CASADI_EIGEN_CONVERSIONS_H 28 | -------------------------------------------------------------------------------- /lib/systems/kinrccar.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_KINRCCAR_H 2 | #define GCOP_KINRCCAR_H 3 | 4 | #include "kinbody3d.h" 5 | #include 6 | 7 | namespace gcop { 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | 12 | typedef Matrix Matrix62d; 13 | typedef Matrix Matrix6d; 14 | 15 | /** 16 | * A simple kinematic car model in SE(3). 17 | * 18 | * Author: Matt Sheckells mshecke1(at)jhu.edu 19 | */ 20 | class KinRccar : public Kinbody3d<2> { 21 | public: 22 | 23 | KinRccar(); 24 | 25 | double Step(Matrix4d &xb, double t, const Matrix4d &xa, 26 | const Vector2d &u, double h, const VectorXd *p = 0, 27 | Matrix6d *A = 0, Matrix62d *B = 0, Matrix *C = 0); 28 | 29 | 30 | }; 31 | } 32 | 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /runtests.sh: -------------------------------------------------------------------------------- 1 | cd build/bin 2 | 3 | ./particle2dtest 4 | 5 | ./unicycletest 6 | 7 | ./body3dtest 8 | 9 | ./qrotortest ../../bin/qrotor.cfg 10 | 11 | ./hrotortest 12 | 13 | ./helicartest 14 | 15 | ./body2dtracktest2 ../../bin/body2dtrack2.cfg 16 | 17 | ./cartest 18 | 19 | ./rccartest 20 | 21 | ##./sddpbulletrccartest 22 | 23 | ./airmopttest 24 | 25 | # @mk that below got broken at some point: need to investigate 26 | #./fixedchaintest ../../bin/fixedchain1.cfg 27 | 28 | ./airbottest ../../bin/airbot.cfg 29 | 30 | # ./imuekftest 31 | 32 | ./uuvtest ../../bin/jhurov.cfg 33 | 34 | #./cecartest ../../bin/cecar.cfg 35 | 36 | ./body3dstab 37 | 38 | cd ../.. 39 | build/bin/body3drhc bin/body3drhc.cfg 40 | 41 | build/bin/body3ddemstab bin/body3ddemstab.cfg 42 | 43 | build/bin/dynvisest xxx bin/dynvisest.cfg 44 | 45 | build/bin/gcartrack 46 | -------------------------------------------------------------------------------- /lib/systems/manifolds/airmmanifold.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_AIRMMANIFOLD_H 2 | #define GCOP_AIRMMANIFOLD_H 3 | 4 | #include "manifold.h" 5 | 6 | namespace gcop { 7 | 8 | using namespace std; 9 | using namespace Eigen; 10 | 11 | typedef Matrix Vector9d; 12 | typedef Matrix Vector16d; 13 | typedef pair AirmState; 14 | 15 | class AirmManifold : public Manifold { 16 | 17 | public: 18 | 19 | static AirmManifold& Instance(); 20 | 21 | void Lift(Vector16d &v, 22 | const AirmState &xa, 23 | const AirmState &xb); 24 | 25 | void Retract(AirmState &xb, 26 | const AirmState &xa, 27 | const Vector16d &v); 28 | 29 | private: 30 | AirmManifold(); 31 | }; 32 | } 33 | 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /lib/systems/manifolds/kinbody2dmanifold.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_KINBODY2DMANIFOLD_H 2 | #define GCOP_KINBODY2DMANIFOLD_H 3 | 4 | #include "manifold.h" 5 | #include 6 | 7 | namespace gcop { 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | 12 | class Kinbody2dManifold : public Manifold { 13 | public: 14 | 15 | static Kinbody2dManifold& Instance(); 16 | 17 | void Lift(Vector3d &v, 18 | const Matrix3d &xa, 19 | const Matrix3d &xb); 20 | 21 | void Retract(Matrix3d &xb, 22 | const Matrix3d &xa, 23 | const Vector3d &v); 24 | 25 | void dtau(Matrix3d &M, const Vector3d &v); 26 | 27 | void Adtau(Matrix3d &M, const Vector3d &v); 28 | 29 | private: 30 | Kinbody2dManifold(); 31 | }; 32 | } 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /lib/views/body2dgraphview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_BODY2DGRAPHVIEW_H 2 | #define GCOP_BODY2DGRAPHVIEW_H 3 | 4 | #include "GL/glu.h" 5 | #include "GL/glut.h" 6 | #include "body2dgraph.h" 7 | #include "view.h" 8 | 9 | 10 | namespace gcop { 11 | 12 | /** 13 | * Rigid body view. Supports rendering either a single state 14 | * or a whole trajectory of states. 15 | */ 16 | class Body2dGraphView : public View { 17 | public: 18 | 19 | /** 20 | * Create a view for a single state s 21 | * @param name name 22 | * @param s rigid body state 23 | */ 24 | Body2dGraphView(const Body2dGraph &pg); 25 | 26 | virtual void Render(); 27 | 28 | virtual bool RenderFrame(int i); 29 | 30 | const Body2dGraph &pg; 31 | 32 | float rgba[4]; 33 | 34 | bool drawLandmarks; 35 | 36 | }; 37 | } 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /lib/views/dynvisinsview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_DYNVISINSVIEW_H 2 | #define GCOP_DYNVISINSVIEW_H 3 | 4 | #include "GL/glu.h" 5 | #include "GL/glut.h" 6 | #include 7 | #include 8 | #include "dynvisins.h" 9 | #include "body3dview.h" 10 | 11 | 12 | namespace gcop { 13 | 14 | /** 15 | * Discrete Dynamical visins view. 16 | */ 17 | class DynVisInsView : public View { 18 | public: 19 | 20 | /** 21 | * Create a view for a single state s 22 | * @param name name 23 | * @param s rigid body state 24 | */ 25 | DynVisInsView(const DynVisIns &vi); 26 | 27 | virtual ~DynVisInsView(); 28 | 29 | virtual void Render(); 30 | 31 | virtual bool RenderFrame(int i); 32 | 33 | const DynVisIns &vi; 34 | 35 | Body3d<> body; 36 | Body3dView<> bodyView; 37 | }; 38 | } 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /lib/views/airmview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_AIRMVIEW_H 2 | #define GCOP_AIRMVIEW_H 3 | 4 | #include "airm.h" 5 | #include "cylinderview.h" 6 | #include "mbsview.h" 7 | #include "GL/glut.h" 8 | #include 9 | #include 10 | #include 11 | #include 12 | //#include "viewer.h" 13 | #include "so3.h" 14 | #include "hrotorgeom3dview.h" 15 | #include "boxview.h" 16 | 17 | namespace gcop { 18 | 19 | using namespace Eigen; 20 | 21 | class AirmView : public MbsView { 22 | public: 23 | 24 | /** 25 | * Create a particle view of trajectory traj 26 | * @param sys particle 27 | * @param xs trajectory 28 | */ 29 | AirmView(const Airm &sys, 30 | vector *xs); 31 | 32 | HrotorGeom3dView hgv; 33 | // BoxView views[2]; 34 | CylinderView views[2]; 35 | }; 36 | } 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /bin/chain5_float.cfg: -------------------------------------------------------------------------------- 1 | method = 1 2 | iters = 4 3 | 4 | # dmoc epsilon 5 | eps = .00001 6 | 7 | # garvity 8 | ag= 0, 0, 0 9 | 10 | tf = 10 11 | N = 64 12 | 13 | fixed = 1 14 | 15 | nb =4 16 | 17 | ulb = -1, -1,-1,-1 18 | uub = 1, 1,1,1 19 | 20 | damping= .1, .1,.1,.1 21 | 22 | # rpy xyz r w0 v0 dr 23 | x0=1.57,0,0, 0,0,0, 1,1,1,1, 0,0,0, 0,0,0, 0,0,0,0 24 | xf=1.57,0,0, 0,0,0, 1,1,1,1, 0,0,0, 0,0,0, 0,0,0,0 25 | 26 | # rpy xyz r 27 | Kp = 1,1,1, 1,1,1, 2,2,2,2,2 28 | Kd = 1,1,1, 1,1,1, 2,2,2,2 29 | 30 | # rpy xyz r w0 v0 dr 31 | Qf = 1,1,1, 20,20,20, 1,1,1,1, 1,1,1, 1,1,1, 1,1,1,1 32 | Q = .1,.1,.1, .1,.1,.1, .1,.1,.1,.1, .1,.1,.1, .1,.1,.1, .1,.1,.1,.1 33 | 34 | # rpy xyz r 35 | R = 1,1,1, 1,1,1, 1, 1, 1, 1 36 | 37 | mu= 0.01 38 | 39 | Nd = 10 40 | -------------------------------------------------------------------------------- /lib/systems/costs/gunicyclecost.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_GUNICYCLECOST_H 2 | #define GCOP_GUNICYCLECOST_H 3 | 4 | #include "lqcost.h" 5 | #include "gunicycle.h" 6 | #include 7 | 8 | namespace gcop { 9 | 10 | using namespace std; 11 | using namespace Eigen; 12 | 13 | typedef Matrix Vector5d; 14 | typedef Matrix Matrix5d; 15 | typedef Matrix Matrix52d; 16 | typedef pair M3V2d; 17 | 18 | 19 | class GunicycleCost : public LqCost { 20 | public: 21 | 22 | GunicycleCost(Gunicycle &sys, double tf, const M3V2d &xf); 23 | 24 | double L(double t, const M3V2d &x, const Vector2d &u, double h, 25 | Vector5d *Lx = 0, Matrix5d *Lxx = 0, 26 | Vector2d *Lu = 0, Matrix2d *Luu = 0, 27 | Matrix52d *Lxu = 0); 28 | }; 29 | } 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /lib/views/airbotview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_AIRBOTVIEW_H 2 | #define GCOP_AIRBOTVIEW_H 3 | 4 | #include "airbot.h" 5 | #include "cylinderview.h" 6 | #include "mbsview.h" 7 | #include "GL/glut.h" 8 | #include 9 | #include 10 | #include 11 | #include 12 | //#include "viewer.h" 13 | #include "so3.h" 14 | #include "hrotorgeom3dview.h" 15 | #include "boxview.h" 16 | 17 | namespace gcop { 18 | 19 | using namespace Eigen; 20 | 21 | class AirbotView : public MbsView { 22 | public: 23 | 24 | /** 25 | * Create a particle view of trajectory traj 26 | * @param sys particle 27 | * @param xs trajectory 28 | */ 29 | AirbotView(const Airbot &sys, 30 | vector *xs); 31 | 32 | HrotorGeom3dView hgv; 33 | // BoxView views[2]; 34 | CylinderView views[6]; 35 | }; 36 | } 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /lib/views/demview.h: -------------------------------------------------------------------------------- 1 | #ifndef DemVIEW_H 2 | #define DemVIEW_H 3 | 4 | #include "view.h" 5 | #include "dem.h" 6 | 7 | #include "GL/glu.h" 8 | 9 | namespace gcop { 10 | 11 | class DemView : public View { 12 | public: 13 | 14 | DemView(const Dem& dem); 15 | virtual ~DemView(); 16 | 17 | void Reset(); 18 | 19 | void Render(); 20 | bool RenderFrame(int i); 21 | 22 | void SetTexture(const char *fname); 23 | 24 | float color[3]; 25 | 26 | bool wire; ///< draw as wire mesh (false by default) 27 | 28 | protected: 29 | void Init(); 30 | // GLUquadricObj *qobj; 31 | const Dem& dem; 32 | 33 | float* vertices; // GL_T2F_C4F_N3F_V3F 34 | int mesh_ind_count; 35 | GLuint* mesh_inds; 36 | 37 | float *normals; 38 | 39 | GLuint texture; ///< texture 40 | 41 | }; 42 | }; 43 | 44 | 45 | 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /lib/views/plotview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_PLOTVIEW_H 2 | #define GCOP_PLOTVIEW_H 3 | 4 | #include "GL/glu.h" 5 | #include "GL/glut.h" 6 | #include "view.h" 7 | #include 8 | 9 | 10 | namespace gcop { 11 | 12 | using namespace Eigen; 13 | 14 | /** 15 | * Discrete Dynamical plot view. 16 | */ 17 | class PlotView : public View { 18 | public: 19 | 20 | /** 21 | * Create a view for a single state s 22 | * @param name name 23 | * @param s rigid body state 24 | */ 25 | PlotView(const char *name = 0, Matrix4d *g = 0); 26 | 27 | virtual ~PlotView(); 28 | 29 | virtual void Render(); 30 | 31 | 32 | 33 | /** 34 | * Set the rgb color of the trajectory 35 | * @param rgb rgb vector 36 | */ 37 | void SetColor(const double rgba[4]); 38 | 39 | double rgba[4]; ///< color 40 | }; 41 | } 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /lib/systems/manifolds/kinbody3dmanifold.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_KINBODY3DMANIFOLD_H 2 | #define GCOP_KINBODY3DMANIFOLD_H 3 | 4 | #include "manifold.h" 5 | #include 6 | 7 | namespace gcop { 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | typedef Matrix Vector6d; 12 | 13 | class Kinbody3dManifold : public Manifold { 14 | public: 15 | 16 | static Kinbody3dManifold& Instance(); 17 | 18 | void Lift(Vector6d &v, 19 | const Matrix4d &xa, 20 | const Matrix4d &xb); 21 | 22 | void Retract(Matrix4d &xb, 23 | const Matrix4d &xa, 24 | const Vector6d &v); 25 | 26 | void dtau(Matrix4d &M, const Vector6d &v); 27 | 28 | void Adtau(Matrix4d &M, const Vector6d &v); 29 | 30 | private: 31 | Kinbody3dManifold(); 32 | }; 33 | } 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /lib/systems/qrotor.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_QROTOR_H 2 | #define GCOP_QROTOR_H 3 | 4 | #include "body3d.h" 5 | 6 | namespace gcop { 7 | 8 | /** 9 | * A quadrotor dynamical model. 10 | * 11 | * The state is 12 | * \f$ \bf x = (R, x, \omega, v) \f$ where \f$ (R,x)\in SO(3)\times\mathbb{R}^3\f$ is the pose, 13 | * and the controls are \f$ \bm u = (u_1,u_2,u_3,u_4)\f$ correspond to torques 14 | * around the body and a vertical lift force. 15 | * 16 | * 17 | * Author: Marin Kobilarov marin(at)jhu.edu 18 | */ 19 | class Qrotor : public Body3d<4> { 20 | public: 21 | Qrotor(); 22 | 23 | double l; ///< distance from center of mass to each rotor 24 | double r; ///< propeller radius 25 | 26 | double kt; ///< thrust gain 27 | double km; ///< moment gain 28 | 29 | double mm; ///< motor coefficient 30 | }; 31 | } 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /lib/systems/creator.h: -------------------------------------------------------------------------------- 1 | // This file is part of libgcop, a library for Geometric Control, Optimization, and Planning (GCOP) 2 | // 3 | // Copyright (C) 2004-2014 Marin Kobilarov 4 | // 5 | // This Source Code Form is subject to the terms of the Mozilla 6 | // Public License v. 2.0. If a copy of the MPL was not distributed 7 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 8 | 9 | #ifndef GCOP_CREATOR_H 10 | #define GCOP_CREATOR_H 11 | 12 | namespace gcop { 13 | 14 | /** 15 | * Generic creator interface, i.e. provides a function that 16 | * will generate an object of type T 17 | * 18 | * Author: Marin Kobilarov 19 | */ 20 | template class Creator { 21 | public: 22 | /** 23 | * Generate an object T 24 | * @return created object 25 | */ 26 | virtual T& operator()() = 0; 27 | }; 28 | } 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /lib/systems/kinbody2d.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_KINBODY2D_H 2 | #define GCOP_KINBODY2D_H 3 | 4 | #include "system.h" 5 | #include 6 | 7 | namespace gcop { 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | 12 | /** 13 | * A simple kinematic control system modeled as a rigid body in 2d. It is meant 14 | * for parameter estimation problems since there is no dynamics. 15 | * 16 | * Author: Marin Kobilarov marin(at)jhu.edu 17 | */ 18 | class Kinbody2d : public System { 19 | public: 20 | 21 | Kinbody2d(); 22 | 23 | double Step(Matrix3d &xb, double t, const Matrix3d &xa, 24 | const Vector3d &u, double h, const VectorXd *p, 25 | Matrix3d *A = 0, Matrix3d *B = 0, Matrix *C = 0); 26 | 27 | 28 | Vector2d d; ///< body dimensions 29 | 30 | }; 31 | } 32 | 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /lib/systems/manifolds/point3dmanifold.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include "point3dmanifold.h" 3 | #include 4 | #include 5 | 6 | using namespace gcop; 7 | using namespace Eigen; 8 | 9 | Point3dManifold::Point3dManifold() : Manifold() 10 | { 11 | } 12 | 13 | 14 | Point3dManifold& Point3dManifold::Instance() 15 | { 16 | static Point3dManifold instance; 17 | return instance; 18 | } 19 | 20 | void Point3dManifold::Lift(Vector6d &dx, 21 | const Point3dState &xa, 22 | const Point3dState &xb) 23 | { 24 | dx.head<3>() = xb.q - xa.q; 25 | dx.tail<3>() = xb.v - xa.v; 26 | } 27 | 28 | 29 | void Point3dManifold::Retract(Point3dState &xb, 30 | const Point3dState &xa, 31 | const Vector6d &dx) 32 | { 33 | xb.q = xa.q + dx.head<3>(); 34 | xb.v = xa.v + dx.tail<3>(); 35 | } 36 | -------------------------------------------------------------------------------- /lib/views/hrotorview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_HROTORVIEW_H 2 | #define GCOP_HROTORVIEW_H 3 | 4 | #include "GL/glut.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "body3dview.h" 10 | #include "hrotorgeom3dview.h" 11 | #include 12 | 13 | namespace gcop { 14 | 15 | using namespace Eigen; 16 | 17 | class HrotorView : public Body3dView<4> { 18 | public: 19 | 20 | /** 21 | * Create a particle view of trajectory traj 22 | * @param sys particle 23 | * @param xs trajectory 24 | */ 25 | HrotorView(const Hrotor &sys, 26 | vector *xs = 0, 27 | vector *us = 0); 28 | 29 | // virtual ~HrotorView(); 30 | 31 | void Render(const Body3dState *x, 32 | const Vector4d *u = 0); 33 | 34 | HrotorGeom3dView view; 35 | }; 36 | } 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /lib/views/qrotorview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_QROTORVIEW_H 2 | #define GCOP_QROTORVIEW_H 3 | 4 | #include "qrotor.h" 5 | #include "body3dview.h" 6 | #include "GL/glut.h" 7 | #include 8 | #include 9 | #include 10 | #include 11 | //#include "viewer.h" 12 | #include "so3.h" 13 | 14 | 15 | namespace gcop { 16 | 17 | using namespace Eigen; 18 | 19 | class QrotorView : public Body3dView<4> { 20 | public: 21 | 22 | /** 23 | * Create a particle view of trajectory traj 24 | * @param sys particle 25 | * @param xs trajectory 26 | */ 27 | QrotorView(const Qrotor &sys, 28 | vector *xs = 0, 29 | vector *us = 0); 30 | 31 | // virtual ~QrotorView(); 32 | 33 | void Render(const Body3dState *x, 34 | const Vector4d *u = 0); 35 | 36 | const Qrotor &sys; 37 | }; 38 | } 39 | #endif 40 | -------------------------------------------------------------------------------- /bin/kinrccartracktest.cfg: -------------------------------------------------------------------------------- 1 | iters = 20 2 | 3 | # dmoc epsilon 4 | eps = .0001 5 | 6 | N = 32 7 | tf = 40 8 | Tc = 3 9 | nf = 30 10 | 11 | # at what time to begin mapping 12 | Ts = 1 13 | slidingWindow = -1 14 | 15 | r=15 16 | vd=8 17 | w=7 18 | h=20 19 | dmax=10 20 | 21 | cp=.0001 22 | # w v 23 | cw=.01, .01 24 | 25 | renderForces = 0 26 | hideTrue=0 27 | hideEst=0 28 | hideOdom=0 29 | 30 | oc=1 31 | fixed = 0 32 | 33 | ulb = -.1, -.1 34 | uub = .1, .1 35 | 36 | # rpy xyz r w0 v0 dr 37 | x0=0,0,0, -2,-1,0, 1, 1, 0,0,0, 0,0,0, 0,0 38 | xf=0,0,0, 0,0,0, 0,0, 0,0,0, 0,0,0, 0,0 39 | 40 | # g w v 41 | Qf = 25,25,25, 20, 20, 20 42 | #Qf = 25, 25, 25, 10, 10, 10, 10, 10, 10, 25, 25, 25 43 | Q = 0, 0, 0, 0, 0, 0 44 | #Q = 0, 0, 0, 0, 0, 0, .1, .1, .1, .5, .5, .5 45 | 46 | ko = .5 47 | 48 | # u 49 | R = .2, .2 50 | 51 | mu=1 52 | -------------------------------------------------------------------------------- /lib/systems/kinbody2d.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include "kinbody2d.h" 3 | #include "kinbody2dmanifold.h" 4 | #include "rn.h" 5 | #include "se2.h" 6 | #include 7 | #include 8 | 9 | using namespace gcop; 10 | using namespace Eigen; 11 | 12 | Kinbody2d::Kinbody2d() : System(Kinbody2dManifold::Instance()), 13 | d(.1, .05) 14 | { 15 | } 16 | 17 | double Kinbody2d::Step(Matrix3d &xb, double t, const Matrix3d &xa, 18 | const Vector3d &u, double h, const VectorXd *p, 19 | Matrix3d *A, Matrix3d *B, Matrix *C ) 20 | { 21 | SE2 &se2 = SE2::Instance(); 22 | Matrix3d m; 23 | 24 | se2.cay(m, h*u); 25 | xb = xa*m; 26 | 27 | if (A) { 28 | se2.cay(m, -h*u); 29 | se2.Ad(*A, m); 30 | } 31 | 32 | if (B) { 33 | se2.dcay(m, -h*u); 34 | *B = h*m; 35 | } 36 | 37 | if (C) 38 | C->setZero(); 39 | } 40 | -------------------------------------------------------------------------------- /bin/body3dcestab2.cfg: -------------------------------------------------------------------------------- 1 | method=1 2 | iters = 100 3 | 4 | debug=0 5 | 6 | eps = 0.00001 7 | 8 | tf = 10 9 | N = 256 10 | 11 | x0=0, 0, 0, 3,90,5, 0,0,0, 0,0,0 12 | 13 | xf=0, 0, 0, 170,90,5, 0,0,0, ,0,0,0 14 | 15 | mapName = maps/random1.ppm 16 | mapCellSize = .25 17 | mapHeightScale = 30 18 | 19 | sr = 40 20 | 21 | # closeup on passage 22 | #camParams = 90, 34, 100.2, -78, -32 23 | 24 | # global view 25 | camParams= 90, 34, 100.2, -78, -74.9 26 | 27 | Qf = .02, .02, .02, .2, .2, .2, .05,.05,.05, .5,.5,.01 28 | #Qf = 0.01, 0.01, 0.01, 1, 1, 1, .01,.01,.01, .001,.001,.001 29 | 30 | Q = 0,0,0, 0,0,0, 0,0,0, 0,0,0 31 | R = .001, .001,.001, .01, .01, 0.01 32 | 33 | 34 | Ns = 200 35 | 36 | mu0=1, 2, 1, 2, 20 37 | 38 | P0 = 4, 4, 4, 4, 100 39 | 40 | S = .0001, .0001, .0001, .0001, .0001 41 | 42 | 43 | sw = 0 44 | sv = 0 45 | 46 | #ko = 20 47 | #kp = 1, 1, 1, .2, .2, .5 48 | #kd = 1, 1, 1, 5, 5, 5 49 | kb = .5 50 | -------------------------------------------------------------------------------- /bin/body3dforceest.cfg: -------------------------------------------------------------------------------- 1 | N = 40 2 | tf = 2 3 | #N = 40 4 | #N = 20 5 | iters = 30 6 | #x0= .1, -.5, 0, 0 7 | #x0= 1, 1, 0, 0 8 | #x0= 1, 1, 0, 0 9 | #xf= 1, 0, 1.57, 0 10 | #xf= 0, 0, 0, 0 11 | 12 | # er, ebg, eba, ep, ev 13 | # Higher sensor confidence gives better p 14 | R = 100,100,100, 0,0,0, 0,0,0, 100,100,100, 100,100,100 15 | S = 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000 16 | 17 | #P = 1, 1 18 | P = 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 19 | p0 = 0.1,0,0.1,0,0.0,0.2 20 | pd = 0,0.03,0.05,0,1.0,-0.1 21 | #P = 0, 0 22 | 23 | # Data copied from physics simulation: 24 | #zs, us not working for now just physicall copied data #TODO 25 | #ts = 0,0.125,0.25,0.375,0.5,0.625,0.75,0.875,1,1.125,1.25,1.375,1.5,1.625,1.75,1.875,2,2.125,2.25,2.375,2.5,2.625,2.75,2.875,3,3.125,3.25,3.375,3.5,3.625,3.75,3.875,4,4.125,4.25,4.375,4.5,4.625,4.75,4.875,5 26 | #If zs, us, ts are given tf, x0, xf are not used. 27 | -------------------------------------------------------------------------------- /lib/systems/joint.cc: -------------------------------------------------------------------------------- 1 | #include "joint.h" 2 | #include "se3.h" 3 | 4 | using namespace gcop; 5 | 6 | Joint::Joint() : a(Vector6d::Zero()), 7 | gp(Matrix4d::Identity()), 8 | gc(Matrix4d::Identity()), 9 | damping(0), 10 | friction(0), 11 | lower(-1e16), 12 | upper(1e16) 13 | { 14 | Init(); 15 | } 16 | 17 | Joint::Joint(const Vector6d &a, 18 | const Matrix4d &gp, 19 | const Matrix4d &gc, 20 | double damping, 21 | double friction) : 22 | a(a), gp(gp), gc(gc), damping(damping), friction(friction), 23 | lower(-1e16), upper(1e16) 24 | { 25 | Init(); 26 | } 27 | 28 | Joint::~Joint() 29 | { 30 | } 31 | 32 | void Joint::Init() 33 | { 34 | SE3::Instance().Ad(Ac, gc); 35 | SE3::Instance().inv(gpi, gp); 36 | SE3::Instance().inv(gci, gc); 37 | S = Ac*a; 38 | } 39 | -------------------------------------------------------------------------------- /lib/systems/manifolds/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(sources 2 | body2dmanifold.cc 3 | body3dmanifold.cc 4 | pose3dmanifold.cc 5 | gunicyclemanifold.cc 6 | imumanifold.cc 7 | insmanifold.cc 8 | kinbody2dmanifold.cc 9 | kinbody3dmanifold.cc 10 | mbscspace.cc 11 | mbsmanifold.cc 12 | mbstspace.cc 13 | point3dmanifold.cc 14 | uuvmanifold.cc 15 | gcarmanifold.cc 16 | #airmmanifold.cc 17 | ) 18 | 19 | set(headers 20 | manifold.h 21 | body2dmanifold.h 22 | body3dmanifold.h 23 | pose3dmanifold.h 24 | gunicyclemanifold.h 25 | imumanifold.h 26 | insmanifold.h 27 | kinbody2dmanifold.h 28 | kinbody3dmanifold.h 29 | manifold.h 30 | mbscspace.h 31 | mbsmanifold.h 32 | mbsstate.h 33 | mbstspace.h 34 | point3dmanifold.h 35 | rn.h 36 | uuvmanifold.h 37 | gcarmanifold.h 38 | qrotoridmanifold.h 39 | #airmmanifold.h 40 | ) 41 | -------------------------------------------------------------------------------- /bin/qrotorsystemidtest.cfg: -------------------------------------------------------------------------------- 1 | T=10 2 | N=500 3 | kp = 15,10,1 4 | kd = 6,6,7 5 | kt = 0.13 6 | #a0 = -0.4, -0.1, 0 7 | a0 = 0,0,0 8 | tau0 = 0,0,0 9 | #offsets_period = 1.0 10 | offsets_period = 0.5 11 | #dataFile = ../../data/QuadSystemIDMeasurements1.csv 12 | #dataFile = ../../data/QuadSystemIDMeasurements3.csv 13 | #dataFile = /home/gowtham/indigo_workspace/src/rqt_quadcoptergui/djilogfiles/session19_February_2016_02_09_34_PM/measurements19_February_2016_02_09_34_PM 14 | #dataFile = /home/gowtham/indigo_workspace/src/rqt_quadcoptergui/djilogfiles/session19_February_2016_05_02_52_PM/measurements19_February_2016_05_02_52_PM 15 | #dataFile = /home/gowtham/indigo_workspace/src/rqt_quadcoptergui/djilogfiles/session20_February_2016_11_33_05_AM/measurements20_February_2016_11_33_05_AM 16 | dataFile = /home/gowtham/indigo_workspace/src/rqt_quadcoptergui/djilogfiles/session22_February_2016_04_44_04_PM/measurements22_February_2016_04_44_04_PM 17 | -------------------------------------------------------------------------------- /cmake/Modules/FindDC1394.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # try to find the dc1394 library (version 2) and include files 3 | # 4 | # DC1394_INCLUDE_DIR, where to find dc1394/dc1394_control.h, etc. 5 | # DC1394_LIBRARIES, the libraries to link against to use DC1394. 6 | # DC1394_FOUND, If false, do not try to use DC1394. 7 | # 8 | 9 | 10 | FIND_PATH( DC1394_INCLUDE_DIR dc1394/control.h 11 | /usr/include 12 | /usr/local/include 13 | ) 14 | 15 | FIND_LIBRARY( DC1394_LIBRARY dc1394 16 | /usr/lib64 17 | /usr/lib 18 | /usr/local/lib 19 | ) 20 | 21 | 22 | SET( DC1394_FOUND "NO" ) 23 | IF(DC1394_INCLUDE_DIR) 24 | IF(DC1394_LIBRARY) 25 | SET( DC1394_LIBRARIES 26 | ${DC1394_LIBRARY} dc1394.a libraw1394.so 27 | ) 28 | SET( DC1394_FOUND "YES" ) 29 | 30 | #The following deprecated settings are for backwards compatibility with CMake1.4 31 | SET (DC1394_INCLUDE_PATH ${DC1394_INCLUDE_DIR}) 32 | 33 | ENDIF(DC1394_LIBRARY) 34 | ENDIF(DC1394_INCLUDE_DIR) 35 | -------------------------------------------------------------------------------- /lib/systems/manifolds/mbscspace.cc: -------------------------------------------------------------------------------- 1 | #include "mbscspace.h" 2 | #include "se3.h" 3 | 4 | using namespace gcop; 5 | 6 | MbsCspace::MbsCspace(int nb) : Manifold(nb + 5) { 7 | } 8 | 9 | 10 | void MbsCspace::Lift(VectorXd &v, 11 | const MbsState &xa, 12 | const MbsState &xb) 13 | { 14 | Matrix4d gi; 15 | SE3::Instance().inv(gi, xa.gs[0]); 16 | 17 | Vector6d xi; 18 | SE3::Instance().cayinv(xi, gi*xb.gs[0]); 19 | 20 | //v.head<6>() = xi; 21 | v.head(6) = xi; 22 | // v.tail<6 + 2*(nb - 1)>() = xb.second - xa.second; 23 | v.tail(xa.r.size()) = xb.r - xa.r; 24 | } 25 | 26 | 27 | void MbsCspace::Retract(MbsState &xb, 28 | const MbsState &xa, 29 | const VectorXd &v) 30 | { 31 | Matrix4d dg; 32 | SE3::Instance().cay(dg, v.head(6)); 33 | xb.gs[0] = xa.gs[0]*dg; 34 | xb.r = xa.r + v.tail(xa.r.size()); 35 | } 36 | -------------------------------------------------------------------------------- /lib/systems/costs/bacost.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_BACOST_H 2 | #define GCOP_BACOST_H 3 | 4 | #include 5 | #include 6 | #include "cost.h" 7 | #include "posegraph2d.h" 8 | #include "kinbody2d.h" 9 | 10 | namespace gcop { 11 | 12 | using namespace std; 13 | using namespace Eigen; 14 | 15 | typedef Matrix MatrixX3d; 16 | typedef Matrix Matrix3Xd; 17 | 18 | class BaCost : public Cost { 19 | public: 20 | BaCost(Kinbody2d &sys, double tf, const Posegraph2d &pg); 21 | 22 | double L(double t, const Matrix3d &x, const Vector3d &u, double h, 23 | const VectorXd *p, 24 | Vector3d *Lx = 0, Matrix3d *Lxx = 0, 25 | Vector3d *Lu = 0, Matrix3d *Luu = 0, 26 | Matrix3d *Lxu = 0, 27 | VectorXd *Lp = 0, MatrixXd *Lpp = 0, MatrixX3d *Lpx = 0); 28 | 29 | const Posegraph2d &pg; 30 | }; 31 | } 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /bin/comparision_spsa_ce: -------------------------------------------------------------------------------- 1 | Case Study I: 2 | 3 | CE Method: 4 | #x0= .1, -.5, 0, 0 5 | #x0= 1, 1, 0, 0 6 | 7 | x0= 1, 1, 0, 0 8 | 9 | xf= 1, 2, 3.14, 0 10 | #xf= 0, 0, 0, 0 11 | 12 | Qf = 10, 10, 10, 1 13 | Q = 0.5, 0.5, 0.1, 1 14 | R = .5, .1 15 | 16 | Ns = 1000 17 | Final Cost: Cost=1.79881 18 | SPSA: 19 | tf = 10 20 | N = 32 21 | Nit = 1000 22 | 23 | iters = 10 24 | 25 | #x0= .1, -.5, 0, 0 26 | x0= 1, 1, 0, 0 27 | 28 | xf= 1, 2, 3.14, 0 29 | #xf= 0, 0, 0, 0 30 | #xf= 1, 0, 1.57, 0 31 | 32 | Qf = 10, 10, 10, 1 33 | Q = 0.5, 0.5, 0.1, 1 34 | R = .5, .1 35 | 36 | #a, c, alpha, gamma 37 | stepcoeffs = 0.03, 0.0001, 0.602, 0.101 38 | 39 | Cost=1.09779 40 | 41 | Case Study II: 42 | For xf = 0,0,0,0: 43 | Cost=1.30088 #CE 44 | Cost=1.63179 #SPSA 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /lib/views/view.cc: -------------------------------------------------------------------------------- 1 | #include "view.h" 2 | #include 3 | #include 4 | 5 | using namespace gcop; 6 | 7 | static int s_view_id = 0; 8 | 9 | View::View(const char* name) 10 | { 11 | if (name) 12 | strcpy(this->name, name); 13 | else 14 | this->name[0] = 0; 15 | 16 | this->id = (s_view_id++); 17 | 18 | pthread_mutex_init(&mut, 0); 19 | } 20 | 21 | View::~View() 22 | { 23 | pthread_mutex_unlock(&mut); 24 | pthread_mutex_destroy(&mut); 25 | } 26 | 27 | void View::Lock() 28 | { 29 | pthread_mutex_lock(&mut); 30 | } 31 | 32 | void View::Unlock() 33 | { 34 | pthread_mutex_unlock(&mut); 35 | } 36 | 37 | 38 | void View::Render() 39 | { 40 | int i=0; 41 | while(RenderFrame(i)) 42 | ++i; 43 | } 44 | 45 | 46 | bool View::RenderFrame(int i) 47 | { 48 | std::cerr << "Warning: View::RenderFrame: #" << i << " no rendering for view id#" << this->id << " :(" << name << ")" << std::endl; 49 | return false; 50 | } 51 | 52 | -------------------------------------------------------------------------------- /lib/systems/airbot.h: -------------------------------------------------------------------------------- 1 | // This file is part of libgcop, a library for Geometric Control, Optimization, and Planning (GCOP) 2 | // 3 | // Copyright (C) 2004-2014 Marin Kobilarov 4 | // 5 | // This Source Code Form is subject to the terms of the Mozilla 6 | // Public License v. 2.0. If a copy of the MPL was not distributed 7 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 8 | 9 | #ifndef GCOP_AIRBOT_H 10 | #define GCOP_AIRBOT_H 11 | 12 | #include "mbs.h" 13 | #include "hrotor.h" 14 | 15 | namespace gcop { 16 | 17 | /** 18 | * A hexrotor with two manipulators 19 | * 20 | * Author: Marin Kobilarov marin(at)jhu.edu 21 | */ 22 | class Airbot : public Mbs { 23 | public: 24 | Airbot(); 25 | 26 | void Force(VectorXd &f, double t, const MbsState &x, const VectorXd &u, 27 | MatrixXd *A = 0, MatrixXd *B = 0); 28 | 29 | Hrotor hrotor; 30 | 31 | }; 32 | } 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /lib/algos/point3dcontroller.cc: -------------------------------------------------------------------------------- 1 | #include "point3dcontroller.h" 2 | 3 | using namespace gcop; 4 | using namespace std; 5 | using namespace Eigen; 6 | 7 | Point3dController::Point3dController(const Point3d &sys, 8 | Point3dState *xd, 9 | Vector3d *ad) : 10 | Controller(), 11 | sys(sys), xd(xd), ad(ad) 12 | { 13 | Kp.setOnes(); 14 | Kd.setOnes(); 15 | } 16 | 17 | Point3dController::~Point3dController() 18 | { 19 | 20 | } 21 | 22 | bool Point3dController::Set(Vector3d &u, double t, const Point3dState &x) 23 | { 24 | // error in base body configuration 25 | Vector6d ge; 26 | 27 | // full error in configuration 28 | Vector3d e = (xd ? x.q - xd->q : x.q); 29 | Vector3d de = (xd ? x.v - xd->v : x.v); 30 | 31 | u = -Kp.cwiseProduct(e) - Kd.cwiseProduct(de); 32 | // add desired acceleration if provided 33 | if (ad) 34 | u = u + *ad; 35 | 36 | return true; 37 | } 38 | 39 | -------------------------------------------------------------------------------- /lib/systems/costs/body2dslamcost.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_BODY2DSLAMCOST_H 2 | #define GCOP_BODY2DSLAMCOST_H 3 | 4 | #include 5 | #include 6 | #include "cost.h" 7 | #include "body2dgraph.h" 8 | #include "body2d.h" 9 | 10 | namespace gcop { 11 | 12 | using namespace std; 13 | using namespace Eigen; 14 | 15 | typedef Matrix MatrixX6d; 16 | typedef Matrix Matrix6Xd; 17 | 18 | class Body2dSlamCost : public Cost { 19 | public: 20 | Body2dSlamCost(Body2d<> &sys, double tf, const Body2dGraph &pg); 21 | 22 | double Lp(double t, const Body2dState &x, const Vector3d &u, const VectorXd &p, 23 | Vector6d *Lx = 0, Matrix6d *Lxx = 0, 24 | Vector3d *Lu = 0, Matrix3d *Luu = 0, 25 | Matrix63d *Lxu = 0, 26 | VectorXd *Lp = 0, MatrixXd *Lpp = 0, MatrixX6d *Lpx = 0); 27 | 28 | const Body2dGraph &pg; 29 | }; 30 | } 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /lib/views/body2dtrackview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_BODY2DTRACKVIEW_H 2 | #define GCOP_BODY2DTRACKVIEW_H 3 | 4 | #include "GL/glu.h" 5 | #include "GL/glut.h" 6 | #include "body2dtrack.h" 7 | #include "view.h" 8 | 9 | 10 | namespace gcop { 11 | 12 | /** 13 | * Rigid body view. Supports rendering either a single state 14 | * or a whole trajectory of states. 15 | */ 16 | class Body2dTrackView : public View { 17 | public: 18 | 19 | /** 20 | * Create a view for a single state s 21 | * @param name name 22 | * @param s rigid body state 23 | */ 24 | Body2dTrackView(const Body2dTrack &pg); 25 | 26 | virtual ~Body2dTrackView(); 27 | 28 | virtual void Render(); 29 | 30 | virtual bool RenderFrame(int i); 31 | 32 | const Body2dTrack &pg; 33 | 34 | float rgba[4]; 35 | 36 | bool drawLandmarks; 37 | bool drawForces; 38 | double forceScale; 39 | 40 | 41 | GLUquadricObj *qobj; 42 | }; 43 | } 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /lib/views/body3dtrackview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_BODY3DTRACKVIEW_H 2 | #define GCOP_BODY3DTRACKVIEW_H 3 | 4 | #include "GL/glu.h" 5 | #include "GL/glut.h" 6 | #include "body3dtrack.h" 7 | #include "view.h" 8 | 9 | 10 | namespace gcop { 11 | 12 | /** 13 | * Rigid body view. Supports rendering either a single state 14 | * or a whole trajectory of states. 15 | */ 16 | class Body3dTrackView : public View { 17 | public: 18 | 19 | /** 20 | * Create a view for a single state s 21 | * @param name name 22 | * @param s rigid body state 23 | */ 24 | Body3dTrackView(const Body3dTrack &pg); 25 | 26 | virtual ~Body3dTrackView(); 27 | 28 | virtual void Render(); 29 | 30 | virtual bool RenderFrame(int i); 31 | 32 | const Body3dTrack &pg; 33 | 34 | float rgba[4]; 35 | 36 | bool drawLandmarks; 37 | bool drawForces; 38 | double forceScale; 39 | 40 | 41 | GLUquadricObj *qobj; 42 | }; 43 | } 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /lib/systems/costs/body2dtrackcost.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_BODY2DTRACKCOST_H 2 | #define GCOP_BODY2DTRACKCOST_H 3 | 4 | #include 5 | #include 6 | #include "cost.h" 7 | #include "body2dtrack.h" 8 | #include "body2d.h" 9 | 10 | namespace gcop { 11 | 12 | using namespace std; 13 | using namespace Eigen; 14 | 15 | typedef Matrix MatrixX6d; 16 | typedef Matrix Matrix6Xd; 17 | 18 | class Body2dTrackCost : public Cost { 19 | public: 20 | Body2dTrackCost(double tf, const Body2dTrack &pg); 21 | 22 | double L(double t, const Body2dState &x, const Vector3d &u, double h, 23 | const VectorXd *p, 24 | Vector6d *Lx = 0, Matrix6d *Lxx = 0, 25 | Vector3d *Lu = 0, Matrix3d *Luu = 0, 26 | Matrix63d *Lxu = 0, 27 | VectorXd *Lp = 0, MatrixXd *Lpp = 0, MatrixX6d *Lpx = 0); 28 | 29 | const Body2dTrack &pg; 30 | }; 31 | } 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /bin/qrotoridmodel.cfg: -------------------------------------------------------------------------------- 1 | T=2 2 | N=100 3 | 4 | #rpy, xyz, w, v, commandedrpy 5 | x0 = 0,0,0, 0,0,0, 0,0,0, 0,0,0, 0,0,0 6 | #xf = 0,0,1.57, 0,0,1.0, 0,0,0, 0,0,0 7 | xf = 0,0,1.57, 2,2,1.0, 0,0,0, 0,0,0 8 | #xf = 0,0,0, 0,0,2, 0,0,0, 0,0,0 9 | 10 | Qf = 5,5,5, 40,40,40, .5,.5,.5, 5,5,5, 1e-3,1e-3,1e-3 11 | #Qf = 0,0,0, 40,40,40, 0,0,0, 0,0,0, 1e-3,1e-3,1e-3 12 | 13 | #Q = .2,.2,.2, .2,.2,.2, 1,1,1, .01,.01,.01, .01,.01,.01 14 | Q = 0,0,0, 0,0,0, 0,0,0, 0,0,0, 0,0,0 15 | 16 | #Thrust , d(commandedrpy) costs 17 | R = 0.001, .01, .01, .01 18 | 19 | #Param stdev (kt,kp,kd, a0, tau0) 20 | p_std = 0.001, 0.2,0.2,0.2, 0.2,0.2,0.2, 0.05,0.05,0.05, 0.05,0.05,0.05 21 | 22 | #Initial State stdev 23 | #x0_std = 0.017,0.017,0.017, 0.05,0.05,0.05, 0.017,0.017,0.017, 0.05,0.05,0.05, 0.017,0.017,0.017 24 | x0_std = 0,0,0, 0.05,0.05,0.05, 0,0,0, 0.05,0.05,0.05, 0,0,0 25 | 26 | #Param mean(Add TODO) 27 | 28 | #Obstacle Info (radius, posn, axis) 29 | #obs = 1.0,0.2, 1,1.1,0, 0,0,1 30 | obs = 0.2, 1,1.1,0, 0,0,1 31 | -------------------------------------------------------------------------------- /bin/airbot.cfg: -------------------------------------------------------------------------------- 1 | method=1 2 | iters = 20 3 | 4 | debug=0 5 | 6 | eps = 0.00001 7 | 8 | damping=.01, .01, .01, .01, .01, .01 9 | 10 | tf = 2 11 | N = 128 12 | x0=0, 0, 0, -1.5,0,0,0,0,0,0,0,0, 0,0,0, 0,0,0, 0.,0,0, 0.,0,0 13 | 14 | xf=0, 0, 0, -.3,0,0, -.3,-1,-1.2, .3,-1,-1.2, 0,0,0, 0,0,0, 0,0,0,0,0,0 15 | 16 | Qf = 10,10,10, 20,20,20, 10,10,10, 10,10,10, 2,2,2, 2,2,2, 1,1,1, 1,1,1 17 | 18 | Q = 2,2,2, 2,2,2, 1,1,1,1,1,1, 2,2,2, 2,2,2, 1,1,1,1,1,1 19 | #Q = 2,2,2, 20,20,20, 0,0,0,0,0,0, 5,5,5, 20,20,20, 1,1,1,1,1,1 20 | #Q = 2,2,2, 0,0,0, 1,1,1, 1,1,1, 0,0,0, 0,0,0, 1,1,1, 1,1,1 21 | 22 | R = .1, .1, .1, 0.005, 0.01,0.01,0.01,0.01,0.01,0.01 23 | 24 | 25 | # rpy xyz r w0 v0 dr 26 | #Qf = 2,2,2, 20,20,20, 20,20,20,20,20,20, 5,5,5, 20,20,20, 10,10,10,10,10,10 27 | 28 | #Q = 2,2,2, 20,20,20, 1,1,1,1,1,1, 5,5,5, 20,20,20, 10,10,10,10,10,10 29 | 30 | #R = .01, .01, .01, 0.00, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01 31 | 32 | mu=1 -------------------------------------------------------------------------------- /lib/systems/costs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(sources 2 | # body2dcost.cc 3 | gunicyclecost.cc 4 | body2dtrackcost.cc 5 | body3dtrackcost.cc 6 | bacost.cc 7 | body2dslamcost.cc 8 | #airplanecost.cc 9 | ) 10 | 11 | set(headers 12 | body2dcost.h 13 | body3dcost.h 14 | body3dwaypointcost.h 15 | kinbody3dcost.h 16 | cost.h 17 | gunicyclecost.h 18 | lqcost.h 19 | lqsensorcost.h 20 | lscost.h 21 | lssensorcost.h 22 | mbslqcost.h 23 | rnlqcost.h 24 | rnlqsensorcost.h 25 | sensorcost.h 26 | multicost.h 27 | multilscost.h 28 | constraintcost.h 29 | body2dtrackcost.h 30 | body3dtrackcost.h 31 | kinbody3dtrackcost.h 32 | kinrccarpathcost.h 33 | kinbodyprojtrackcost.h 34 | bacost.h 35 | body2dslamcost.h 36 | qrotoridmodelcost.h 37 | yawcost.h 38 | rpcost.h 39 | #airplanecost.h 40 | ) 41 | 42 | IF (PQP_FOUND) 43 | set(headers 44 | ${headers} 45 | pqpdemcost.h 46 | ) 47 | ENDIF (PQP_FOUND) 48 | -------------------------------------------------------------------------------- /lib/systems/costs/body3dtrackcost.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_BODY3DTRACKCOST_H 2 | #define GCOP_BODY3DTRACKCOST_H 3 | 4 | #include 5 | #include 6 | #include "cost.h" 7 | #include "body3dtrack.h" 8 | #include "body3d.h" 9 | 10 | namespace gcop { 11 | 12 | using namespace std; 13 | using namespace Eigen; 14 | 15 | typedef Matrix MatrixX12d; 16 | typedef Matrix Matrix12Xd; 17 | 18 | class Body3dTrackCost : public Cost { 19 | public: 20 | Body3dTrackCost(double tf, const Body3dTrack &pg); 21 | 22 | double L(double t, const Body3dState &x, const Vector6d &u, double h, 23 | const VectorXd *p, 24 | Vector12d *Lx = 0, Matrix12d *Lxx = 0, 25 | Vector6d *Lu = 0, Matrix6d *Luu = 0, 26 | Matrix *Lxu = 0, 27 | VectorXd *Lp = 0, MatrixXd *Lpp = 0, MatrixX12d *Lpx = 0); 28 | 29 | const Body3dTrack &pg; 30 | }; 31 | } 32 | 33 | #endif 34 | -------------------------------------------------------------------------------- /lib/systems/particle2d.cc: -------------------------------------------------------------------------------- 1 | #include "particle2d.h" 2 | #include 3 | #include "rn.h" 4 | 5 | using namespace gcop; 6 | using namespace Eigen; 7 | 8 | Particle2d::Particle2d() : System(Rn<4>::Instance()), m(1), r(.1) 9 | { 10 | } 11 | 12 | 13 | double Particle2d::Step(Vector4d& xb, double t, const Vector4d& xa, 14 | const Vector2d& u, double h, const VectorXd *p, 15 | Matrix4d *A, Matrix *B, Matrix *C) { 16 | 17 | xb.head(2) = xa.head(2) + h*xa.tail(2); 18 | xb.tail(2) = xa.tail(2) + (h/m)*u; 19 | 20 | if (A) { 21 | (*A).block<2,2>(0,0).setIdentity(); 22 | (*A)(0,2) = h, (*A)(0, 3) = 0; 23 | (*A)(1,2) = 0, (*A)(1, 3) = h; 24 | (*A).block<2,2>(2,0).setZero(); 25 | (*A).block<2,2>(2,2).setIdentity(); 26 | } 27 | 28 | if (B) { 29 | (*B).block<2,2>(0,0).setZero(); 30 | (*B)(2,0) = h/m, (*B)(2, 1) = 0; 31 | (*B)(3,0) = 0, (*B)(3, 1) = h/m; 32 | } 33 | return 1; 34 | } 35 | -------------------------------------------------------------------------------- /bin/kinbody3dtrack.cfg: -------------------------------------------------------------------------------- 1 | iters = 20 2 | 3 | # dmoc epsilon 4 | eps = .0001 5 | 6 | N = 32 7 | tf = 40 8 | Tc = 3 9 | nf = 30 10 | 11 | # at what time to begin mapping 12 | Ts = 1 13 | slidingWindow = 5 14 | 15 | r=15 16 | vd=8 17 | w=7 18 | h=20 19 | dmax=10 20 | 21 | cp=.01 22 | # w v 23 | cw=.01, .01, .1, 2, 4, 2 24 | #cw=1, .3, .1, .01, .01, .01 25 | 26 | renderForces = 0 27 | hideTrue=0 28 | hideEst=0 29 | hideOdom=0 30 | 31 | oc=1 32 | fixed = 0 33 | 34 | ulb = -.1, -.1, -.1, -1, -1, -1, -.1, -.1 35 | uub = .1, .1, .1, 1, 1, 1, .1, .1 36 | 37 | # rpy xyz r w0 v0 dr 38 | x0=0,0,0, -2,-1,0, 1, 1, 0,0,0, 0,0,0, 0,0 39 | xf=0,0,0, 0,0,0, 0,0, 0,0,0, 0,0,0, 0,0 40 | 41 | # g w v 42 | Qf = 25,25,25, 20, 20, 20 43 | #Qf = 25, 25, 25, 10, 10, 10, 10, 10, 10, 25, 25, 25 44 | Q = 0, 0, 0, 0, 0, 0 45 | #Q = 0, 0, 0, 0, 0, 0, .1, .1, .1, .5, .5, .5 46 | 47 | ko = .5 48 | 49 | # u 50 | R = .2, .2, .2, .5, .5, .5 51 | 52 | 53 | mu=1 54 | -------------------------------------------------------------------------------- /lib/systems/car.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include "car.h" 3 | #include "rn.h" 4 | 5 | using namespace gcop; 6 | using namespace Eigen; 7 | 8 | Car::Car() : System(Rn<5>::Instance()), l(1) 9 | { 10 | } 11 | 12 | 13 | double Car::Step(Vector5d& xb, double t, const Vector5d& xa, 14 | const Vector2d& u, double h, const VectorXd *p, 15 | Matrix5d *A, Matrix52d *B, Matrix5Xd *C) { 16 | double c = cos(xa[2]); 17 | double s = sin(xa[2]); 18 | const double &v = xa[3]; 19 | const double &k = xa[4]; 20 | 21 | xb[0] = xa[0] + h*c*v; 22 | xb[1] = xa[1] + h*s*v; 23 | xb[2] = xa[2] + h*k*v; 24 | xb[3] = v + h*u[0]; 25 | xb[4] = k + h*u[1]; 26 | 27 | if (A) { 28 | A->setIdentity(); 29 | (*A)(0,2) = -h*s*v; 30 | (*A)(0,3) = h*c; 31 | (*A)(1,2) = h*c*v; 32 | (*A)(1,3) = h*s; 33 | (*A)(2,3) = h*k; 34 | (*A)(2,4) = h*v; 35 | } 36 | 37 | if (B) { 38 | B->setZero(); 39 | (*B)(3,0) = h; 40 | (*B)(4,1) = h; 41 | } 42 | return 1; 43 | } 44 | -------------------------------------------------------------------------------- /bin/kinbodyprojtrack.cfg: -------------------------------------------------------------------------------- 1 | iters = 20 2 | 3 | # dmoc epsilon 4 | eps = .0001 5 | 6 | N = 32 7 | tf = 40 8 | Tc = 3 9 | nf = 60 10 | 11 | # at what time to begin mapping 12 | Ts = 1 13 | slidingWindow = -1 14 | 15 | r=15 16 | vd=8 17 | w=7 18 | h=20 19 | dmax=10 20 | 21 | cp=.0001 22 | # w v 23 | cw=.01, .01, .1, .2, .1, .02 24 | #cw=1, .3, .1, .01, .01, .01 25 | 26 | renderForces = 0 27 | hideTrue=0 28 | hideEst=0 29 | hideOdom=0 30 | 31 | oc=1 32 | fixed = 0 33 | 34 | ulb = -.1, -.1, -.1, -1, -1, -1, -.1, -.1 35 | uub = .1, .1, .1, 1, 1, 1, .1, .1 36 | 37 | # rpy xyz r w0 v0 dr 38 | x0=0,0,0, -2,-1,0, 1, 1, 0,0,0, 0,0,0, 0,0 39 | xf=0,0,0, 0,0,0, 0,0, 0,0,0, 0,0,0, 0,0 40 | 41 | # g w v 42 | Qf = 25,25,25, 20, 20, 20 43 | #Qf = 25, 25, 25, 10, 10, 10, 10, 10, 10, 25, 25, 25 44 | Q = 0, 0, 0, 0, 0, 0 45 | #Q = 0, 0, 0, 0, 0, 0, .1, .1, .1, .5, .5, .5 46 | 47 | ko = .5 48 | 49 | # u 50 | R = .2, .2, .2, .5, .5, .5 51 | 52 | 53 | mu=1 54 | -------------------------------------------------------------------------------- /lib/algos/body2dslam.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_BODY2DSLAM_H 2 | #define GCOP_BODY2DSLAM_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "pddp.h" 10 | #include "body2dslamcost.h" 11 | #include "body2dgraph.h" 12 | 13 | namespace gcop { 14 | 15 | using namespace Eigen; 16 | 17 | /** 18 | * Bundle adjustment based on optimal-control. This is a basic version 19 | * using a kinematic model in SE(2). It is possible to plug-in any dynamical model 20 | * without much overhead. 21 | */ 22 | class Body2dSlam { 23 | public: 24 | 25 | /** 26 | * SLAM, global map iterative optimization 27 | * @param pg pose-graph 28 | */ 29 | Body2dSlam(Body2dGraph &pg); 30 | 31 | Body2dGraph &pg; ///< pose graph 32 | 33 | Body2dSlamCost cost; ///< cost function 34 | 35 | PDdp *pddp; ///< parametric discrete-mechanics optimal control 36 | }; 37 | } 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /lib/systems/unicycle.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include "unicycle.h" 3 | #include "rn.h" 4 | 5 | using namespace gcop; 6 | using namespace Eigen; 7 | 8 | Unicycle::Unicycle() : System(Rn<5>::Instance()), dx(.25), dy(.5) 9 | { 10 | } 11 | 12 | 13 | double Unicycle::Step(Vector5d& xb, double t, const Vector5d& xa, 14 | const Vector2d& u, double h, const VectorXd *p, 15 | Matrix5d *A, Matrix52d *B, Matrix5Xd *C) { 16 | double c = cos(xa[2]); 17 | double s = sin(xa[2]); 18 | double v = xa[3]; 19 | double w = xa[4]; 20 | 21 | xb[0] = xa[0] + h*c*v; 22 | xb[1] = xa[1] + h*s*v; 23 | xb[2] = xa[2] + h*w; 24 | xb[3] = v + h*u[0]; 25 | xb[4] = w + h*u[1]; 26 | 27 | if (A) { 28 | A->setIdentity(); 29 | (*A)(0,2) = -h*s*v; 30 | (*A)(0,3) = h*c; 31 | (*A)(1,2) = h*c*v; 32 | (*A)(1,3) = h*s; 33 | (*A)(2,4) = h; 34 | } 35 | 36 | if (B) { 37 | B->setZero(); 38 | (*B)(3,0) = h; 39 | (*B)(4,1) = h; 40 | } 41 | return 1; 42 | } 43 | -------------------------------------------------------------------------------- /lib/systems/manifolds/gunicyclemanifold.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_GUNICYCLEMANIFOLD_H 2 | #define GCOP_GUNICYCLEMANIFOLD_H 3 | 4 | #include "manifold.h" 5 | #include 6 | 7 | namespace gcop { 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | 12 | typedef Matrix Vector5d; 13 | typedef Matrix Matrix5d; 14 | typedef pair M3V2d; 15 | 16 | class GunicycleManifold : public Manifold { 17 | public: 18 | 19 | static GunicycleManifold& Instance(); 20 | 21 | void Lift(Vector5d &dx, 22 | const M3V2d &xa, 23 | const M3V2d &xb); 24 | 25 | void Retract(M3V2d &xb, 26 | const M3V2d &xa, 27 | const Vector5d &dx); 28 | 29 | void dtau(Matrix5d &M, const Vector5d &v); 30 | 31 | void dtauinv(Matrix5d &M, const Vector5d &v); 32 | 33 | void Adtau(Matrix5d &M, const Vector5d &v); 34 | 35 | private: 36 | GunicycleManifold(); 37 | }; 38 | } 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /lib/views/carview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_CARVIEW_H 2 | #define GCOP_CARVIEW_H 3 | 4 | #include "car.h" 5 | #include "systemview.h" 6 | 7 | namespace gcop { 8 | using namespace Eigen; 9 | 10 | class CarView : public SystemView { 11 | public: 12 | 13 | /** 14 | * Create a particle view of trajectory traj 15 | * @param sys particle 16 | * @param xs trajectory 17 | */ 18 | CarView(const Car &sys, 19 | vector *xs = 0, 20 | vector *us = 0); 21 | 22 | virtual ~CarView(); 23 | 24 | 25 | void Render(const Vector5d *x, 26 | const Vector2d *u = 0); 27 | 28 | void Render(const vector *xs, 29 | const vector *us = 0, 30 | bool rs = true, 31 | int is = -1, int ie = -1, 32 | int dis = 1, int dit = 1, 33 | bool dl = false); 34 | 35 | const Car &sys; 36 | GLUquadricObj *qobj; 37 | }; 38 | } 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /lib/views/rccarview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_RCCARVIEW_H 2 | #define GCOP_RCCARVIEW_H 3 | 4 | #include "rccar.h" 5 | #include "systemview.h" 6 | 7 | namespace gcop { 8 | using namespace Eigen; 9 | 10 | class RccarView : public SystemView { 11 | public: 12 | 13 | /** 14 | * Create a particle view of trajectory traj 15 | * @param sys particle 16 | * @param xs trajectory 17 | */ 18 | RccarView(const Rccar &sys, 19 | vector *xs = 0, 20 | vector *us = 0); 21 | 22 | virtual ~RccarView(); 23 | 24 | 25 | void Render(const Vector4d *x, const Vector2d *u = 0); 26 | 27 | void Render(const vector *xs, 28 | const vector *us = 0, 29 | bool rs = true, 30 | int is = -1, int ie = -1, 31 | int dis = 1, int dit = 1, 32 | bool dl = false); 33 | 34 | const Rccar &sys; 35 | GLUquadricObj *qobj; 36 | }; 37 | } 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /lib/algos/ba.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_BA_H 2 | #define GCOP_BA_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "pddp.h" 10 | #include "bacost.h" 11 | #include "kinbody2d.h" 12 | #include "posegraph2d.h" 13 | 14 | namespace gcop { 15 | 16 | using namespace Eigen; 17 | 18 | /** 19 | * Bundle adjustment based on optimal-control. This is a basic version 20 | * using a kinematic model in SE(2). It is possible to plug-in any dynamical model 21 | * without much overhead. 22 | */ 23 | class Ba { 24 | public: 25 | 26 | /** 27 | * Bundle adjustment of pose-graph pg 28 | * @param pg pose-graph 29 | */ 30 | Ba(Posegraph2d &pg); 31 | 32 | Posegraph2d &pg; ///< pose graph 33 | 34 | Kinbody2d sys; ///< system 35 | 36 | BaCost cost; ///< cost function 37 | 38 | PDdp pddp; ///< parametric discrete-mechanics optimal control 39 | }; 40 | } 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /lib/systems/costs/airplanecost.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_AIRPLANECOST_H 2 | #define GCOP_AIRPLANECOST_H 3 | 4 | #include "lqcost.h" 5 | #include 6 | #include "normal.h" 7 | 8 | namespace gcop { 9 | 10 | using namespace std; 11 | using namespace Eigen; 12 | 13 | typedef Matrix Vector5d; 14 | typedef Matrix Matrix5d; 15 | typedef Matrix Matrix52d; 16 | typedef pair M3V2d; 17 | 18 | 19 | class AirplaneCost : public LqCost { 20 | public: 21 | 22 | AirplaneCost(Airdouble tf, const M3V2d &xf, int N); 23 | 24 | double L(double t, const M3V2d &x, const Vector2d &u, double h, 25 | Vector5d *Lx = 0, Matrix5d *Lxx = 0, 26 | Vector2d *Lu = 0, Matrix2d *Luu = 0, 27 | Matrix52d *Lxu = 0); 28 | 29 | void SetObstacle(double s, double n); 30 | 31 | double n; 32 | // Normal gd; ///< Gaussian distribution 33 | vector gds; 34 | 35 | }; 36 | } 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /lib/systems/manifolds/body2dmanifold.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_BODY2DMANIFOLD_H 2 | #define GCOP_BODY2DMANIFOLD_H 3 | 4 | #include "manifold.h" 5 | #include 6 | 7 | namespace gcop { 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | 12 | typedef Matrix Vector6d; 13 | typedef Matrix Matrix6d; 14 | typedef pair Body2dState; 15 | 16 | class Body2dManifold : public Manifold { 17 | public: 18 | 19 | static Body2dManifold& Instance(); 20 | 21 | void Lift(Vector6d &dx, 22 | const Body2dState &xa, 23 | const Body2dState &xb); 24 | 25 | void Retract(Body2dState &xb, 26 | const Body2dState &xa, 27 | const Vector6d &dx); 28 | 29 | void dtau(Matrix6d &M, const Vector6d &v); 30 | 31 | void dtauinv(Matrix6d &M, const Vector6d &v); 32 | 33 | void Adtau(Matrix6d &M, const Vector6d &v); 34 | 35 | private: 36 | Body2dManifold(); 37 | }; 38 | } 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /lib/views/gcarview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_GCARVIEW_H 2 | #define GCOP_GCARVIEW_H 3 | 4 | #include "gcar.h" 5 | #include "systemview.h" 6 | 7 | namespace gcop { 8 | using namespace Eigen; 9 | 10 | class GcarView : public SystemView { 11 | public: 12 | /** 13 | * Create a particle view of trajectory traj 14 | * @param sys particle 15 | * @param xs trajectory 16 | */ 17 | GcarView(const Gcar &sys, 18 | vector *xs = 0, 19 | vector *us = 0); 20 | 21 | virtual ~GcarView(); 22 | 23 | 24 | void Render(const GcarState *x, 25 | const Vector2d *u = 0); 26 | 27 | void Render(const vector *xs, 28 | const vector *us, 29 | bool rs = true, 30 | int is = -1, int ie = -1, 31 | int dis = 1, int dit = 1, 32 | bool dl = false); 33 | 34 | const Gcar &sys; 35 | GLUquadricObj *qobj; 36 | }; 37 | } 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /lib/algos/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(headers 2 | ddp.h 3 | sddp.h 4 | docp.h 5 | doep.h 6 | gndocp.h 7 | gndoep.h 8 | deterministicgndoep.h 9 | pddp.h 10 | ba.h 11 | body2dgraph.h 12 | posegraph2d.h 13 | controller.h 14 | mbscontroller.h 15 | point3dcontroller.h 16 | body3dcontroller.h 17 | gavoidcontroller.h 18 | body3davoidcontroller.h 19 | systemce.h 20 | aspsa.h 21 | spsa.h 22 | qrotoridgndocp.h 23 | ) 24 | 25 | set(sources 26 | ba.cc 27 | body2dslam.cc 28 | body2dgraph.cc 29 | posegraph2d.cc 30 | mbscontroller.cc 31 | point3dcontroller.cc 32 | gavoidcontroller.cc 33 | ) 34 | 35 | IF (CERES_FOUND) 36 | set(headers 37 | ${headers} 38 | dynvisins.h 39 | ) 40 | set(sources 41 | ${sources} 42 | dynvisins.cc 43 | ) 44 | ENDIF (CERES_FOUND) 45 | 46 | 47 | IF (DSL_FOUND AND PQP_FOUND) 48 | set(headers 49 | ${headers} 50 | body3ddemcontroller.h 51 | ) 52 | ENDIF (DSL_FOUND AND PQP_FOUND) 53 | -------------------------------------------------------------------------------- /lib/systems/particle2d.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_PARTICLE2D_H 2 | #define GCOP_PARTICLE2D_H 3 | 4 | #include "system.h" 5 | #include 6 | 7 | namespace gcop { 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | 12 | /** 13 | * A simple point-mass (i.e. particle) in 2d with second-order dynamics. 14 | * The state is 15 | * \f$ \bf x = (q, v) \in \mathbb{R}^4\f$ with controls \f$ \bf u \in\mathbb{R}^2 \f$ 16 | * correspond to turn rate acceleration and forward acceleration. 17 | * 18 | * Author: Marin Kobilarov marin(at)jhu.edu 19 | */ 20 | class Particle2d : public System { 21 | public: 22 | Particle2d(); 23 | 24 | double Step(Vector4d &xb, double t, const Vector4d &xa, 25 | const Vector2d &u, double h, const VectorXd *p = 0, 26 | Matrix *A = 0, Matrix *B = 0, 27 | Matrix *C = 0); 28 | 29 | double m; ///< mass 30 | double r; ///< radius 31 | }; 32 | } 33 | 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /lib/views/particle2dview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_PARTICLE2DVIEW_H 2 | #define GCOP_PARTICLE2DVIEW_H 3 | 4 | #include "particle2d.h" 5 | #include "systemview.h" 6 | 7 | namespace gcop { 8 | using namespace Eigen; 9 | 10 | class Particle2dView : public SystemView { 11 | public: 12 | /** 13 | * Create a particle view of trajectory traj 14 | * @param sys particle 15 | * @param xs trajectory 16 | */ 17 | Particle2dView(const Particle2d &sys, 18 | vector *xs = 0, 19 | vector *us = 0); 20 | 21 | virtual ~Particle2dView(); 22 | 23 | 24 | void Render(const Vector4d *x, 25 | const Vector2d *u = 0); 26 | 27 | void Render(const vector *xs, 28 | const vector *us = 0, 29 | bool rs = true, 30 | int is = -1, int ie = -1, 31 | int dis = 1, int dit = 1, 32 | bool dl = false); 33 | 34 | const Particle2d &sys; 35 | }; 36 | } 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /lib/views/unicycleview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_UNICYCLEVIEW_H 2 | #define GCOP_UNICYCLEVIEW_H 3 | 4 | #include "unicycle.h" 5 | #include "systemview.h" 6 | 7 | namespace gcop { 8 | using namespace Eigen; 9 | 10 | class UnicycleView : public SystemView { 11 | public: 12 | /** 13 | * Create a particle view of trajectory traj 14 | * @param sys particle 15 | * @param xs trajectory 16 | */ 17 | UnicycleView(const Unicycle &sys, 18 | vector *xs = 0, 19 | vector *us = 0); 20 | 21 | virtual ~UnicycleView(); 22 | 23 | 24 | void Render(const Vector5d *x = 0, 25 | const Vector2d *u = 0); 26 | 27 | void Render(const vector *xs, 28 | const vector *us = 0, 29 | bool rs = true, 30 | int is = -1, int ie = -1, 31 | int dis = 1, int dit = 1, 32 | bool dl = false); 33 | 34 | const Unicycle &sys; 35 | GLUquadricObj *qobj; 36 | }; 37 | } 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /lib/views/point3dview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_POINT3DVIEW_H 2 | #define GCOP_POINT3DVIEW_H 3 | 4 | #include "point3d.h" 5 | #include "systemview.h" 6 | 7 | namespace gcop { 8 | using namespace Eigen; 9 | 10 | class Point3dView : public SystemView { 11 | public: 12 | /** 13 | * Create a particle view of trajectory traj 14 | * @param sys particle 15 | * @param xs trajectory 16 | */ 17 | Point3dView(const Point3d &sys, 18 | vector *xs = 0, 19 | vector *us = 0); 20 | 21 | virtual ~Point3dView(); 22 | 23 | 24 | void Render(const Point3dState *x, 25 | const Vector3d *u = 0); 26 | 27 | void Render(const vector *xs, 28 | const vector *us = 0, 29 | bool rs = true, 30 | int is = -1, int ie = -1, 31 | int dis = 1, int dit = 1, 32 | bool dl = false); 33 | 34 | const Point3d &sys; 35 | 36 | double r; ///< radius 37 | }; 38 | } 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /bin/body3dcedemstab2.cfg: -------------------------------------------------------------------------------- 1 | sr = 40 2 | 3 | tf = 20 4 | N = 1000 5 | 6 | cr = 0 7 | 8 | 9 | umax = 36 10 | mass = 1 11 | 12 | #mapName = maps/pic2_dilated.ppm 13 | x0=0, 0, 0, 1,90, 5, 0,0,0, 0,0,0 14 | 15 | xf=0, 0, 0, 179,90,5, 0,0,0, ,0,0,0 16 | 17 | mapName = maps/hard4.ppm 18 | #mapName = maps/random1.ppm 19 | 20 | mapCellSize = 1 21 | mapHeightScale = 40 22 | 23 | 24 | # closeup on passage 25 | #camParams = 90, 34, 100.2, -78, -32 26 | 27 | # global view 28 | camParams= 90, 41, 95, -56, -145 29 | 30 | #Qf = 0.01, 0.01, 0.01, 5, 5, 10, .01,.01,.01, .001,.001,.001 31 | 32 | Qf = 0.0, 0.0, 0.0, 9, 9, 9, .0,.0,.0, .00,.00,.00 33 | 34 | Q = 0,0,0, 0,0,0, 0,0,0, 0,0,0 35 | #R = .001, .001,.001, .01, .01, 0.01 36 | 37 | R = .00, .00,.00, .0, .0, 0. 38 | 39 | Ns = 100 40 | 41 | # kpw kdw kpv kdw ko 42 | mu0 = 1, 2, 2, 5, 10 43 | 44 | P0 = .04, .04, 4, 8, 64 45 | 46 | S = .0001, .0001, .0001, .0001, .0001 47 | 48 | kb = 0.0 49 | odExp = 1 50 | 51 | vd = 0 52 | 53 | sw = 0 54 | sv = 0 55 | 56 | oc = 0 57 | 58 | 59 | Jub = 1000000 60 | enforceUpperBound = 1 61 | 62 | wpRadius = 10 63 | 64 | iters = 20 -------------------------------------------------------------------------------- /cmake/Modules/FindTinyXML.cmake: -------------------------------------------------------------------------------- 1 | # this module was taken from http://trac.evemu.org/browser/trunk/cmake/FindTinyXML.cmake 2 | # - Find TinyXML 3 | # Find the native TinyXML includes and library 4 | # 5 | # TINYXML_FOUND - True if TinyXML found. 6 | # TINYXML_INCLUDE_DIR - where to find tinyxml.h, etc. 7 | # TINYXML_LIBRARIES - List of libraries when using TinyXML. 8 | # 9 | 10 | IF( TINYXML_INCLUDE_DIR ) 11 | # Already in cache, be silent 12 | SET( TinyXML_FIND_QUIETLY TRUE ) 13 | ENDIF( TINYXML_INCLUDE_DIR ) 14 | 15 | FIND_PATH( TINYXML_INCLUDE_DIR "tinyxml.h" 16 | PATH_SUFFIXES "tinyxml" ) 17 | 18 | FIND_LIBRARY( TINYXML_LIBRARIES 19 | NAMES "tinyxml" 20 | PATH_SUFFIXES "tinyxml" ) 21 | 22 | # handle the QUIETLY and REQUIRED arguments and set TINYXML_FOUND to TRUE if 23 | # all listed variables are TRUE 24 | INCLUDE( "FindPackageHandleStandardArgs" ) 25 | FIND_PACKAGE_HANDLE_STANDARD_ARGS( "TinyXML" DEFAULT_MSG TINYXML_INCLUDE_DIR TINYXML_LIBRARIES ) 26 | 27 | MARK_AS_ADVANCED( TINYXML_INCLUDE_DIR TINYXML_LIBRARIES ) 28 | -------------------------------------------------------------------------------- /lib/systems/gcop_conversions.cpp: -------------------------------------------------------------------------------- 1 | #include "gcop_conversions.h" 2 | 3 | using namespace casadi; 4 | 5 | /** 6 | * @brief convertDMToEigen Helper function to convert casadi matrix to Eigen 7 | * matrix 8 | * @param in A casadi matrix can also be a vector if ncols = 1 9 | * @return Eigen Matrix with same data as casadi matrix 10 | */ 11 | Eigen::MatrixXd gcop::conversions::convertDMToEigen(const DM &in) { 12 | int rows = in.rows(); 13 | int cols = in.columns(); 14 | Eigen::MatrixXd out(rows, cols); 15 | std::vector elems = in.get_elements(); 16 | std::memcpy(out.data(), elems.data(), sizeof(double) * rows * cols); 17 | return out; 18 | } 19 | 20 | /** 21 | * @brief convertEigenToDM Helper function to convert Eigen matrix to casadi 22 | * matrix 23 | * @param in An eigen matrix 24 | * @return A casadi matrix 25 | */ 26 | DM gcop::conversions::convertEigenToDM(const Eigen::MatrixXd &in) { 27 | int rows = in.rows(); 28 | int cols = in.cols(); 29 | cs::DM out = cs::DM::zeros(rows, cols); 30 | std::memcpy(out.ptr(), in.data(), sizeof(double) * rows * cols); 31 | return out; 32 | } 33 | -------------------------------------------------------------------------------- /bin/body3dtrack.cfg: -------------------------------------------------------------------------------- 1 | iters = 20 2 | 3 | # dmoc epsilon 4 | eps = .0001 5 | 6 | N = 32 7 | tf = 40 8 | Tc = 3 9 | nf = 30 10 | 11 | # at what time to begin mapping 12 | Ts = 1 13 | 14 | r=15 15 | vd=8 16 | w=4.5 17 | h=20 18 | dmax=5 19 | 20 | cp=.01 21 | # w v 22 | cw=.0001, .0001, .01, .1, .1, .0001 23 | #cw=1, .3, .1, .01, .01, .01 24 | cv=.01, .01, .01, .1, .1, .01 25 | #cv=.1, .1, .1, .01, .01, .01 26 | 27 | renderForces = 1 28 | hideTrue=0 29 | hideEst=0 30 | hideOdom=0 31 | 32 | oc=1 33 | fixed = 0 34 | 35 | ulb = -.1, -.1, -.1, -1, -1, -1, -.1, -.1 36 | uub = .1, .1, .1, 1, 1, 1, .1, .1 37 | 38 | # rpy xyz r w0 v0 dr 39 | x0=0,0,0, -2,-1,0, 1, 1, 0,0,0, 0,0,0, 0,0 40 | xf=0,0,0, 0,0,0, 0,0, 0,0,0, 0,0,0, 0,0 41 | 42 | # g w v 43 | Qf = 10,10,10, 25, 25, 25, 10, 10, 10, 25, 25, 25 44 | #Qf = 25, 25, 25, 10, 10, 10, 10, 10, 10, 25, 25, 25 45 | Q = 0, 0, 0, 0, 0, 0, .1, .1, .1, .5, .5, .5 46 | #Q = 0, 0, 0, 0, 0, 0, .1, .1, .1, .5, .5, .5 47 | 48 | ko = .5 49 | 50 | # u 51 | R = .2, .2, .2, .5, .5, .5 52 | 53 | 54 | mu=1 55 | -------------------------------------------------------------------------------- /lib/views/airbotview.cc: -------------------------------------------------------------------------------- 1 | #include "GL/glut.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "airbotview.h" 7 | #include "viewer.h" 8 | #include "se3.h" 9 | #include "utils.h" 10 | 11 | using namespace gcop; 12 | using namespace Eigen; 13 | 14 | AirbotView::AirbotView(const Airbot &sys, 15 | vector *xs) : 16 | MbsView(sys, xs), hgv(sys.hrotor) 17 | { 18 | this->geomViews.push_back(&hgv); 19 | for (int i = 0; i < 6; ++i) { 20 | if (i!=0 && i != 3) { 21 | // SE3::Instance().rpyxyz2g(views[i].g0, Vector3d(0, 0, 0), Vector3d(0, 0, -sys.links[i+1].ds[0]/2)); 22 | // views[i].d = sys.links[i+1].ds; 23 | views[i].r = sys.links[i+1].ds[0]/2; 24 | views[i].h = sys.links[i+1].ds[2]; 25 | } else { 26 | SE3::Instance().rpyxyz2g(views[i].g0, Vector3d(M_PI/2, 0, 0), Vector3d(0, 0, 0)); 27 | // views[i].d = sys.links[i+1].ds; 28 | views[i].r = sys.links[i+1].ds[0]/2; 29 | views[i].h = sys.links[i+1].ds[1]; 30 | } 31 | 32 | this->geomViews.push_back(&views[i]); 33 | } 34 | } 35 | -------------------------------------------------------------------------------- /bin/body3dcedemstab.cfg: -------------------------------------------------------------------------------- 1 | sr = 40 2 | 3 | tf = 15 4 | N = 1000 5 | 6 | cr = 0 7 | 8 | x0=0, 0, 0, 46,85, 5, 0,0,0, 0,0,0 9 | #x0=0, 0, 0, 80,10, 5, 0,0,0, 0,0,0 10 | 11 | xf=0, 0, 0, 175,140, 5, 0,0,0, 0,0,0 12 | 13 | 14 | umax = 36 15 | mass = 1 16 | 17 | #mapName = maps/pic2_dilated.ppm 18 | mapName = maps/pic2.ppm 19 | mapCellSize = .5 20 | mapHeightScale = 30 21 | 22 | 23 | # closeup on passage 24 | #camParams = 90, 34, 100.2, -78, -32 25 | 26 | # global view 27 | camParams= 90, 24, 95, -95, -125 28 | 29 | Qf = 0.0, 0.0, 0.0, 4, 4, 4, .0,.0,.0, .1,.1,.1 30 | 31 | Q = 0,0,0, 0,0,0, 0,0,0, 0,0,0 32 | R = .001, .001,.001, .001, .001, 0.001 33 | 34 | Ns = 200 35 | 36 | # kpw kdw kpv kdw ko 37 | #mu0 = 1, 2, 2, 5, 50 38 | 39 | mu0 = 1, 2, 1, 1, 1 40 | 41 | #P0 = .001, .001, 9, 9, 9 42 | P0 = .04, .04, 10, 10, 10 43 | 44 | S = .001, .001, .001, .001, .001 45 | 46 | mras = 0 47 | #bAuto =1 48 | b = 100 49 | 50 | kb = 0.0 51 | odExp = 1 52 | 53 | vd = 0 54 | 55 | sw = 0 56 | sv = 10 57 | 58 | oc = 0 59 | 60 | 61 | Jub = 1000000 62 | enforceUpperBound = 1 63 | enforceUpperBoundFactor = 2 64 | 65 | wpRadius = 10 66 | 67 | iters = 50 -------------------------------------------------------------------------------- /lib/views/geom3dview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_GEOM3DVIEW_H 2 | #define GCOP_GEOM3DVIEW_H 3 | 4 | #include "GL/glu.h" 5 | #include "GL/glut.h" 6 | #include "view.h" 7 | #include 8 | 9 | 10 | namespace gcop { 11 | 12 | using namespace Eigen; 13 | 14 | /** 15 | * Discrete Dynamical geom3d view. 16 | */ 17 | class Geom3dView : public View { 18 | public: 19 | 20 | /** 21 | * Create a view for a single state s 22 | * @param name name 23 | * @param s rigid body state 24 | */ 25 | Geom3dView(const char *name = 0, Matrix4d *g = 0); 26 | 27 | virtual ~Geom3dView(); 28 | 29 | virtual void RenderGeom() = 0; 30 | 31 | virtual bool RenderFrame(int i = 0); 32 | 33 | static void Transform(const Matrix4d &g); 34 | 35 | /** 36 | * Set the rgb color of the trajectory 37 | * @param rgb rgb vector 38 | */ 39 | void SetColor(const double rgba[4]); 40 | 41 | Matrix4d *g; ///< global pose (optional) 42 | 43 | Matrix4d g0; ///< origin (set to Id by default) 44 | 45 | double rgba[4]; ///< color 46 | }; 47 | } 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /lib/systems/arm.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_ARM_H 2 | #define GCOP_ARM_H 3 | 4 | namespace gcop { 5 | 6 | /** 7 | * 3-dof arm (rotating base, and two links). Model assumes first joint motor is at center of mass. 8 | * The local frame is z-up, and arm is zero position means that is straightened along the x+ axis. 9 | * To account for position offset simply change the parameter p. 10 | */ 11 | class Arm { 12 | public: 13 | Arm(); 14 | 15 | virtual ~Arm(); 16 | 17 | /** 18 | * Forward kinematics: 19 | * @param p end-effector position 20 | * @param a angles: yaw, joint1, joint2 21 | */ 22 | bool Fk(double p[3], const double a[3]); 23 | 24 | /** 25 | * Inverse kinematics 26 | * @param a angles: yaw, joint1, joint2; a[0][*] is first solution, a[1][*] is second solution 27 | * @param p end-effector position 28 | */ 29 | bool Ik(double a[2][3], const double p[3]); 30 | 31 | double l1; ///< first link length 32 | double l2; ///< second link length 33 | double x1; ///< relative offset to second motor in x direction 34 | 35 | }; 36 | } 37 | 38 | #endif 39 | 40 | -------------------------------------------------------------------------------- /lib/views/gunicycleview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_GUNICYCLEVIEW_H 2 | #define GCOP_GUNICYCLEVIEW_H 3 | 4 | #include "gunicycle.h" 5 | #include "systemview.h" 6 | 7 | namespace gcop { 8 | using namespace Eigen; 9 | 10 | class GunicycleView : public SystemView, Vector2d> { 11 | public: 12 | /** 13 | * Create a particle view of trajectory traj 14 | * @param sys particle 15 | * @param xs trajectory 16 | */ 17 | GunicycleView(const Gunicycle &sys, 18 | vector > *xs = 0, 19 | vector *us = 0); 20 | 21 | virtual ~GunicycleView(); 22 | 23 | 24 | void Render(const pair *x, 25 | const Vector2d *u = 0); 26 | 27 | void Render(const vector > *xs, 28 | const vector *us, 29 | bool rs = true, 30 | int is = -1, int ie = -1, 31 | int dis = 1, int dit = 1, 32 | bool dl = false); 33 | 34 | const Gunicycle &sys; 35 | GLUquadricObj *qobj; 36 | }; 37 | } 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /lib/systems/car.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_CAR_H 2 | #define GCOP_CAR_H 3 | 4 | #include "system.h" 5 | #include 6 | 7 | namespace gcop { 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | 12 | typedef Matrix Vector5d; 13 | typedef Matrix Matrix5d; 14 | typedef Matrix Matrix52d; 15 | typedef Matrix Matrix5Xd; 16 | 17 | /** 18 | * A simple rear-drive car model with 2nd order dynamics. 19 | * 20 | * The state is 21 | * \f$ \bm x = (x,y,\theta, v, k) \in\mathbb{R}^5\f$ and controls are \f$ \bm u = (u_1,u_2) \in\mathbb{R}^5\f$ 22 | * correspond to forward acceleration and curvature rate 23 | * 24 | * Author: Marin Kobilarov marin(at)jhu.edu 25 | */ 26 | class Car : public System 27 | { 28 | public: 29 | Car(); 30 | 31 | double Step(Vector5d &xb, double t, const Vector5d &xa, 32 | const Vector2d &u, double h, const VectorXd *p = 0, 33 | Matrix5d *A = 0, Matrix52d *B = 0, Matrix5Xd *C = 0); 34 | 35 | double l; ///< distance between axles 36 | }; 37 | } 38 | 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /lib/views/airplaneview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_AIRPLANEVIEW_H 2 | #define GCOP_AIRPLANEVIEW_H 3 | 4 | #include "gunicycle.h" 5 | #include "systemview.h" 6 | 7 | namespace gcop { 8 | using namespace Eigen; 9 | 10 | class AirplaneView : public SystemView, Vector2d > { 11 | public: 12 | /** 13 | * Create a particle view of trajectory traj 14 | * @param sys particle 15 | * @param xs trajectory 16 | */ 17 | AirplaneView(const Gunicycle &sys, 18 | vector > *xs = 0, 19 | vector *us = 0); 20 | 21 | virtual ~AirplaneView(); 22 | 23 | 24 | void Render(const pair *x, 25 | const Vector2d *u = 0); 26 | 27 | void Render(const vector > *xs, 28 | const vector *us = 0, 29 | bool rs = true, 30 | int is = -1, int ie = -1, 31 | int dis = 1, int dit = 1, 32 | bool dl = false); 33 | 34 | const Gunicycle &sys; 35 | GLUquadricObj *qobj; 36 | }; 37 | } 38 | 39 | #endif 40 | 41 | -------------------------------------------------------------------------------- /bin/body3dcestab.cfg: -------------------------------------------------------------------------------- 1 | method=1 2 | iters = 100 3 | 4 | debug=0 5 | 6 | eps = 0.00001 7 | 8 | tf = 5 9 | N = 256 10 | 11 | #x0=0, 0, 0, 46,85, 5, 0,0,0, 0,0,0 12 | #x0=0, 0, 0, 80,10, 5, 0,0,0, 0,0,0 13 | 14 | #xf=0, 0, 0, 160,122, 2, 0,0,0, ,20,0,0 15 | 16 | x0=0, 0, 0, 40, 60, 5, 0,0,0, 0,0,0 17 | #x0=0, 0, 0, 80,10, 5, 0,0,0, 0,0,0 18 | 19 | #xf=0, 0, 0, 165,125, 2, 0,0,0, 20,0,0 20 | xf=0, 0, 0, 115, 115, 2, 0,0,0, 0,0,0 21 | 22 | 23 | mapName = maps/pic2.ppm 24 | #mapName = maps/random1.ppm 25 | mapCellSize = .25 26 | mapHeightScale = 30 27 | 28 | 29 | sr = 20 30 | 31 | # closeup on passage 32 | #camParams = 90, 34, 100.2, -78, -32 33 | 34 | # global view 35 | camParams= 90, 34, 100.2, -78, -74.9 36 | 37 | #Qf = .02, .02, .02, .2, .2, .2, .05,.05,.05, .5,.5,.01 38 | Qf = 0.01, 0.01, 0.01, 10, 10, 10, .01,.01,.01, .001,.001,.001 39 | 40 | Q = 0,0,0, 0,0,0, 0,0,0, 0,0,0 41 | R = .001, .001,.001, .01, .01, 0.01 42 | 43 | Ns = 200 44 | 45 | mu0 = 1, 1, 1, 1, 1 46 | 47 | P0 = 4, 4, 4, 4, 16 48 | 49 | S = .0001, .0001, .0001, .0001, .0001 50 | 51 | sw = 0 52 | sv = 0 53 | 54 | #ko = 20 55 | #kp = 1, 1, 1, .2, .2, .5 56 | #kd = 1, 1, 1, 5, 5, 5 57 | #kb = 0 58 | -------------------------------------------------------------------------------- /lib/systems/manifolds/airmmanifold.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include "airmmanifold.h" 3 | #include "so3.h" 4 | #include 5 | #include 6 | 7 | using namespace gcop; 8 | using namespace Eigen; 9 | 10 | AirmManifold::AirmManifold() 11 | { 12 | } 13 | 14 | 15 | AirmManifold& AirmManifold::Instance() 16 | { 17 | static AirmManifold instance; 18 | return instance; 19 | } 20 | 21 | void AirmManifold::Lift(Vector16d &v, 22 | const AirmState &xa, 23 | const AirmState &xb) 24 | { 25 | const Matrix3d &Ra = xa.first; 26 | const Matrix3d &Rb = xb.first; 27 | 28 | Vector3d eR; 29 | // SO3::Instance().log(eR, Ra.transpose()*Rb); 30 | SO3::Instance().cayinv(eR, Ra.transpose()*Rb); 31 | 32 | v.head<3>() = eR; 33 | v.tail<13>() = xb.second - xa.second; 34 | } 35 | 36 | 37 | void AirmManifold::Retract(AirmState &xb, 38 | const AirmState &xa, 39 | const Vector16d &v) 40 | { 41 | Matrix3d dR; 42 | 43 | SO3::Instance().cay(dR, v.head<3>()); 44 | 45 | xb.first = xa.first*dR; 46 | xb.second = xa.second + v.tail<13>(); 47 | } 48 | 49 | -------------------------------------------------------------------------------- /lib/algos/mbscontroller.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_MBSCONTROLLER_H 2 | #define GCOP_MBSCONTROLLER_H 3 | 4 | #include "controller.h" 5 | #include "mbs.h" 6 | 7 | namespace gcop { 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | 12 | class MbsController : public Controller< MbsState, VectorXd> { 13 | public: 14 | 15 | /** 16 | * MBS PD controller 17 | * 18 | * @param sys multi-body system 19 | * @param xd desired state (optional, set to origin by default) 20 | * @param ad desired acceleration (optional, set to zero by default) 21 | */ 22 | MbsController(const Mbs &sys, 23 | MbsState *xd = 0, 24 | VectorXd *ad = 0); 25 | 26 | virtual bool Set(VectorXd &u, double t, const MbsState &x); 27 | 28 | virtual ~MbsController(); 29 | 30 | const Mbs &sys; ///< multi-body system 31 | 32 | MbsState *xd; ///< desired state (origin by default) 33 | VectorXd *ad; ///< desired acceleration (zero by default) 34 | 35 | VectorXd Kp; ///< proportional terms (ones by default) 36 | VectorXd Kd; ///< derivative terms (ones by default) 37 | 38 | }; 39 | }; 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /lib/systems/manifolds/point3dmanifold.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_POINT3DMANIFOLD_H 2 | #define GCOP_POINT3DMANIFOLD_H 3 | 4 | #include "manifold.h" 5 | 6 | namespace gcop { 7 | 8 | using namespace std; 9 | using namespace Eigen; 10 | 11 | typedef Matrix Vector6d; 12 | typedef Matrix Matrix6d; 13 | 14 | struct Point3dState { 15 | Point3dState() : 16 | q(Vector3d::Zero()), 17 | v(Vector3d::Zero()), 18 | P(Matrix6d::Identity()) { 19 | } 20 | 21 | Vector3d q; ///< position 22 | Vector3d v; ///< velocity 23 | Matrix6d P; ///< covariance 24 | }; 25 | 26 | // typedef pair Point3dState; 27 | 28 | class Point3dManifold : public Manifold { 29 | 30 | public: 31 | static Point3dManifold& Instance(); 32 | 33 | void Lift(Vector6d &dx, 34 | const Point3dState &xa, 35 | const Point3dState &xb); 36 | 37 | void Retract(Point3dState &xb, 38 | const Point3dState &xa, 39 | const Vector6d &dx); 40 | 41 | private: 42 | Point3dManifold(); 43 | }; 44 | } 45 | 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /lib/systems/point3d.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_POINT3D_H 2 | #define GCOP_POINT3D_H 3 | 4 | #include "system.h" 5 | #include "point3dmanifold.h" 6 | #include 7 | #include 8 | #include 9 | 10 | namespace gcop { 11 | 12 | using namespace std; 13 | using namespace Eigen; 14 | 15 | typedef Matrix Vector6d; 16 | typedef Matrix Matrix63d; 17 | typedef Matrix Matrix6d; 18 | typedef Matrix Matrix6Xd; 19 | 20 | /** 21 | * An POINT3D system 22 | * 23 | * Author: Marin Kobilarov marin(at)jhu.edu 24 | */ 25 | class Point3d : public System { 26 | public: 27 | 28 | Point3d(); 29 | 30 | virtual ~Point3d(); 31 | 32 | double Step(Point3dState &xb, double t, const Point3dState &xa, 33 | const Vector3d &u, double h, const VectorXd *p = 0, 34 | Matrix6d *A = 0, Matrix63d *B = 0, Matrix6Xd *C = 0); 35 | 36 | bool Noise(Matrix6d &Q, double t, const Point3dState &x, const Vector3d &u, 37 | double dt, const VectorXd *p = 0); 38 | 39 | double sa; ///< white-noise acceleration stdev 40 | 41 | }; 42 | } 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /lib/views/heliview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_HELIVIEW_H 2 | #define GCOP_HELIVIEW_H 3 | 4 | #include "heli.h" 5 | #include "body3dview.h" 6 | #include "GL/glut.h" 7 | #include 8 | #include 9 | #include 10 | #include 11 | //#include "viewer.h" 12 | #include "so3.h" 13 | 14 | 15 | namespace gcop { 16 | 17 | using namespace Eigen; 18 | 19 | class HeliView : public Body3dView<4> { 20 | public: 21 | 22 | /** 23 | * Create a particle view of trajectory traj 24 | * @param sys particle 25 | */ 26 | 27 | HeliView(const Heli &sys); 28 | 29 | 30 | virtual ~HeliView(); 31 | 32 | /** 33 | * Create a particle view of trajectory traj 34 | * @param sys particle 35 | * @param xs trajectory 36 | */ 37 | HeliView(const Heli &sys, 38 | vector *xs, 39 | vector *us = 0); 40 | 41 | // virtual ~HeliView(); 42 | 43 | void Render(const Body3dState *x, 44 | const Vector4d *u = 0); 45 | 46 | const Heli &sys; 47 | 48 | GLUquadricObj *body; 49 | GLUquadricObj *tail; 50 | GLUquadricObj* tprop[4]; 51 | GLUquadricObj *rprop; 52 | 53 | 54 | }; 55 | } 56 | #endif 57 | -------------------------------------------------------------------------------- /cmake/Modules/FindGCOP.cmake: -------------------------------------------------------------------------------- 1 | include(LibFindMacros) 2 | unset(GCOP_LIBRARY CACHE) 3 | 4 | # Dependencies 5 | #libfind_package(gcop) 6 | 7 | # Use pkg-config to get hints about paths 8 | libfind_pkg_check_modules(GCOP_PKGCONF gcop) 9 | 10 | # Include dir 11 | find_path(GCOP_INCLUDE_DIR 12 | NAMES gcop/system.h 13 | PATHS ${GCOP_PKGCONF_INCLUDE_DIRS} 14 | ) 15 | 16 | # Finally the library itself 17 | if( GCOP_FIND_COMPONENTS )# If no components specified manually specifiy all the components 18 | else() 19 | set(GCOP_FIND_COMPONENTS algos views systems est) 20 | endif() 21 | 22 | foreach(component ${GCOP_FIND_COMPONENTS}) 23 | find_library(GCOP_LIB_${component} 24 | NAMES gcop_${component} 25 | ) 26 | set(GCOP_LIBRARY "${GCOP_LIBRARY};${GCOP_LIB_${component}}") 27 | endforeach(component) 28 | # Utils is common to all 29 | find_library(GCOP_LIB_utils 30 | NAMES gcop_utils 31 | ) 32 | set(GCOP_LIBRARY "${GCOP_LIBRARY};${GCOP_LIB_utils}") 33 | # Set the include dir variables and the libraries and let libfind_process do the rest. 34 | # NOTE: Singular variables for this library, plural for libraries this this lib depends on. 35 | set(GCOP_PROCESS_INCLUDES GCOP_INCLUDE_DIR) 36 | set(GCOP_PROCESS_LIBS GCOP_LIBRARY) 37 | libfind_process(GCOP) 38 | 39 | -------------------------------------------------------------------------------- /lib/systems/qrotor.cc: -------------------------------------------------------------------------------- 1 | #include "qrotor.h" 2 | 3 | using namespace gcop; 4 | using namespace Eigen; 5 | 6 | Qrotor::Qrotor() : 7 | Body3d(Vector3d(.5, .5, .1), .5, Vector3d(2.32e-3, 2.32e-3, 4.41e-3)), 8 | l(0.171), r(0.099), 9 | kt(6.107e-8), km(2.7207e-9), mm(20) { 10 | 11 | Bu.setZero(); 12 | Bu(0,0) = 1; 13 | Bu(1,1) = 1; 14 | Bu(2,2) = 1; 15 | Bu(5,3) = 1; 16 | 17 | fp << 0, 0, -9.81*m; 18 | 19 | // double ulb[4] = {1200, 1200, 1200, 1200}; 20 | // double uub[4] = {7800, 7800, 7800, 7800}; 21 | } 22 | 23 | 24 | /* 25 | 26 | 27 | void Qrotor::Control::Bu(double *f, const State &s) 28 | { 29 | const Qrotor &qrotor = (const Qrotor&)s.sys; 30 | 31 | // forces produced by each rotor 32 | double rfs[4]; 33 | rfs[0] = qrotor.kt*s.u[0]*s.u[0]; 34 | rfs[1] = qrotor.kt*s.u[1]*s.u[1]; 35 | rfs[2] = qrotor.kt*s.u[2]*s.u[2]; 36 | rfs[3] = qrotor.kt*s.u[3]*s.u[3]; 37 | 38 | f[0] = qrotor.l*(rfs[3] - rfs[1]); 39 | f[1] = qrotor.l*(rfs[2] - rfs[0]); 40 | // f[2] = qrotor.km*(s.u[0]*s.u[0] - s.u[1]*s.u[1] + s.u[2]*s.u[2] - s.u[3]*s.u[3]); 41 | f[2] = qrotor.km/qrotor.kt*(rfs[0] - rfs[1] + rfs[2] - rfs[3]); 42 | f[3] = 0; 43 | f[4] = 0; 44 | f[5] = rfs[0] + rfs[1] + rfs[2] + rfs[3]; 45 | } 46 | */ 47 | -------------------------------------------------------------------------------- /lib/systems/manifolds/mbsmanifold.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_MBSMANIFOLD_H 2 | #define GCOP_MBSMANIFOLD_H 3 | 4 | #include "manifold.h" 5 | #include "mbsstate.h" 6 | 7 | namespace gcop { 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | 12 | // state dimension for nb-body system with 1-dof joints 13 | #define MBS_DIM(nb) (12 + 2*(nb - 1)) 14 | #define MBS_DIM_FIXED(nb) (2*(nb - 1)) 15 | 16 | class MbsManifold : public Manifold { 17 | 18 | public: 19 | 20 | /** 21 | * @param nb number of bodies (including a base body) 22 | * @param fixed is the base body fixed (if fixed then it is treated as a "virtual" fixed body) 23 | */ 24 | MbsManifold(int nb, bool fixed = false); 25 | 26 | void Lift(VectorXd &v, 27 | const MbsState &xa, 28 | const MbsState &xb); 29 | 30 | void Retract(MbsState &xb, 31 | const MbsState &xa, 32 | const VectorXd &v); 33 | 34 | void dtau(MatrixXd &M, const VectorXd &v); 35 | 36 | void Adtau(MatrixXd &M, const VectorXd &v); 37 | 38 | // bool fixed; ///< whether it has a fixed base 39 | 40 | bool cay; ///< use Cayley instead of Exponential 41 | 42 | }; 43 | } 44 | 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /lib/views/kinbody2dview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_KINBODY2DVIEW_H 2 | #define GCOP_KINBODY2DVIEW_H 3 | 4 | #include "kinbody2d.h" 5 | #include "systemview.h" 6 | 7 | namespace gcop { 8 | using namespace Eigen; 9 | 10 | class Kinbody2dView : public SystemView { 11 | public: 12 | 13 | /** 14 | * Create a particle view of trajectory traj 15 | * @param sys particle 16 | */ 17 | Kinbody2dView(const Kinbody2d &sys); 18 | 19 | /** 20 | * Create a particle view of trajectory traj 21 | * @param sys particle 22 | * @param xs trajectory 23 | */ 24 | Kinbody2dView(const Kinbody2d &sys, 25 | vector *xs, 26 | vector *us = 0); 27 | 28 | virtual ~Kinbody2dView(); 29 | 30 | 31 | void Render(const Matrix3d *x, 32 | const Vector3d *u = 0); 33 | 34 | void Render(const vector *xs, 35 | const vector *us = 0, 36 | bool rs = true, 37 | int is = -1, int ie = -1, 38 | int dis = 1, int dit = 1, 39 | bool dl = false); 40 | 41 | const Kinbody2d &sys; 42 | 43 | GLUquadricObj *qobj; 44 | }; 45 | } 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /cmake/Modules/FindLibUSB.cmake: -------------------------------------------------------------------------------- 1 | # - Find libusb-1.0 library 2 | # This module defines 3 | # LIBUSB_INCLUDE_DIR, where to find bluetooth.h 4 | # LIBUSB_LIBRARIES, the libraries needed to use libusb-1.0. 5 | # LIBUSB_FOUND, If false, do not try to use libusb-1.0. 6 | # 7 | # Copyright (c) 2009, Michal Cihar, 8 | # 9 | # vim: expandtab sw=4 ts=4 sts=4: 10 | 11 | if (NOT LIBUSB_FOUND) 12 | pkg_check_modules (LIBUSB_PKG libusb-1.0) 13 | 14 | find_path(LIBUSB_INCLUDE_DIR NAMES libusb.h 15 | PATHS 16 | ${LIBUSB_PKG_INCLUDE_DIRS} 17 | /usr/include/libusb-1.0 18 | /usr/include 19 | /usr/local/include 20 | ) 21 | 22 | find_library(LIBUSB_LIBRARIES NAMES usb-1.0 23 | PATHS 24 | ${LIBUSB_PKG_LIBRARY_DIRS} 25 | /usr/lib 26 | /usr/local/lib 27 | ) 28 | 29 | if(LIBUSB_INCLUDE_DIR AND LIBUSB_LIBRARIES) 30 | set(LIBUSB_FOUND TRUE CACHE INTERNAL "libusb-1.0 found") 31 | message(STATUS "Found libusb-1.0: ${LIBUSB_INCLUDE_DIR}, ${LIBUSB_LIBRARIES}") 32 | else(LIBUSB_INCLUDE_DIR AND LIBUSB_LIBRARIES) 33 | set(LIBUSB_FOUND FALSE CACHE INTERNAL "libusb-1.0 found") 34 | message(STATUS "libusb-1.0 not found.") 35 | endif(LIBUSB_INCLUDE_DIR AND LIBUSB_LIBRARIES) 36 | mark_as_advanced(LIBUSB_INCLUDE_DIR LIBUSB_LIBRARIES) 37 | endif (NOT LIBUSB_FOUND) 38 | -------------------------------------------------------------------------------- /lib/views/geom3dview.cc: -------------------------------------------------------------------------------- 1 | #include "geom3dview.h" 2 | #include 3 | #include 4 | #include 5 | #include "viewer.h" 6 | #include "so3.h" 7 | #include "utils.h" 8 | 9 | using namespace gcop; 10 | 11 | Geom3dView::Geom3dView(const char *name, Matrix4d *g) : 12 | View(name ? name : "Geom3d View"), g(g), g0(Matrix4d::Identity()) { 13 | rgba[0] = 1; 14 | rgba[1] = 1; 15 | rgba[2] = 1; 16 | rgba[3] = 0.5; 17 | } 18 | 19 | Geom3dView::~Geom3dView() { 20 | } 21 | 22 | void Geom3dView::SetColor(const double rgba[4]) { 23 | memcpy(this->rgba, rgba, 4*sizeof(double)); 24 | } 25 | 26 | 27 | bool Geom3dView::RenderFrame(int i) 28 | { 29 | // glColor4dv(rgba); 30 | Viewer::SetColor(rgba[0], rgba[1], rgba[2], 0); 31 | 32 | glPushMatrix(); 33 | if (g) 34 | Transform((*g)*g0); 35 | else 36 | Transform(g0); 37 | 38 | RenderGeom(); 39 | glPopMatrix(); 40 | 41 | return false; 42 | } 43 | 44 | 45 | void Geom3dView::Transform(const Matrix4d &g) 46 | { 47 | glTranslated(g(0,3), g(1,3), g(2, 3)); 48 | 49 | Vector3d e; 50 | SO3::Instance().log(e, g.topLeftCorner<3,3>()); 51 | double n = e.norm(); 52 | if (n > SO3::Instance().tol) { 53 | e = e/n; 54 | glRotated(RAD2DEG(n), e[0], e[1], e[2]); 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /lib/systems/airbase.h: -------------------------------------------------------------------------------- 1 | // This file is part of libgcop, a library for Geometric Control, Optimization, and Planning (GCOP) 2 | // 3 | // Copyright (C) 2004-2014 Marin Kobilarov 4 | // 5 | // This Source Code Form is subject to the terms of the Mozilla 6 | // Public License v. 2.0. If a copy of the MPL was not distributed 7 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 8 | 9 | #ifndef GCOP_AIRBOT_H 10 | #define GCOP_AIRBOT_H 11 | 12 | #include "mbs.h" 13 | #include "hrotor.h" 14 | 15 | 16 | namespace gcop { 17 | 18 | /** 19 | * A quadrotor based mbs model. 20 | * 21 | * The state is 22 | * \f$ \bf x = (R, x, \omega, v) \f$ where \f$ (R,x)\in SO(3)\times\mathbb{R}^3\f$ is the pose, 23 | * and the controls are \f$ \bm u = (u_1,u_2,u_3,u_4)\f$ correspond to torques 24 | * around the body and a vertical lift force. 25 | * 26 | * 27 | * Author: Marin Kobilarov marin(at)jhu.edu 28 | */ 29 | class Airbase : public Mbs { 30 | public: 31 | Airbase(int nb, int j); ///< nb number of bodies /// { 20 | public: 21 | Hrotor(); 22 | void StateAndControlsToFlatAndDerivatives(vector &y, 23 | const Body3dState &x, const std::vector &u); 24 | void StateAndControlsToFlat(VectorXd &y, const Body3dState &x, 25 | const Vector4d &u); 26 | void FlatToStateAndControls(Body3dState &x, std::vector &u, 27 | const std::vector &y); 28 | 29 | double l; ///< distance from center of mass to each rotor 30 | double r; ///< propeller radius 31 | 32 | double kt; ///< thrust gain 33 | double km; ///< moment gain 34 | 35 | double mm; ///< motor coefficient 36 | }; 37 | } 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /bin/body3dcedemstab_jhu.cfg: -------------------------------------------------------------------------------- 1 | sr = 40 2 | 3 | tf = 16 4 | N = 500 5 | 6 | cr = 0 7 | 8 | umax = 36 9 | mass = 1 10 | 11 | #mapName = maps/pic2_dilated.ppm 12 | #x0=0, 0, 0, 1,165, 5, 0,0,0, 0,0,0 13 | 14 | #xf=0, 0, 0, 500,165,5, 0,0,0, ,0,0,0 15 | 16 | x0=0, 0, 0, 2, 250, 10, 0,0,0, 0,0,0 17 | 18 | xf=0, 0, 0, 330, 250, 10, 0,0,0, ,0,0,0 19 | 20 | 21 | mapName = maps/jhu_dem2.ppm 22 | #mapName = maps/random1.ppm 23 | 24 | mapCellSize = 1.05 25 | mapHeightScale = 15 26 | 27 | 28 | # closeup on passage 29 | #camParams = 90, 34, 100.2, -78, -32 30 | 31 | # global view 32 | camParams= 70, 36, 150, -187, -340 33 | 34 | #Qf = 0.01, 0.01, 0.01, 5, 5, 10, .01,.01,.01, .001,.001,.001 35 | 36 | Qf = 0.0, 0.0, 0.0, 4, 4, 4, .0,.0,.0, .1,.1,.1 37 | 38 | Q = 0,0,0, 0,0,0, 0,0,0, 0,0,0 39 | R = .001, .001,.001, .001, .001, 0.001 40 | 41 | #R = .00, .00,.00, .0, .0, 0. 42 | 43 | Ns = 200 44 | 45 | # kpw kdw kpv kdw ko 46 | #mu0 = 1, 2, 2, 5, 50 47 | 48 | mu0 = 1, 2, 1, 1, 1 49 | 50 | P0 = .04, .04, 10, 10, 10 51 | 52 | S = .001, .001, .001, .001, .001 53 | 54 | mras = 0 55 | b = 100 56 | 57 | kb = 0.0 58 | odExp = 1 59 | 60 | vd = 0 61 | 62 | sw = 0 63 | sv = 5 64 | 65 | oc = 0 66 | 67 | 68 | Jub = 100000 69 | enforceUpperBound = 1 70 | enforceUpperBoundFactor = 1.5 71 | 72 | wpRadius = 10 73 | 74 | iters = 50 -------------------------------------------------------------------------------- /bin/gptest.cc: -------------------------------------------------------------------------------- 1 | //#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET 2 | 3 | #include 4 | //#include 5 | //#include 6 | //#include "gsl/gsl_cdf.h" 7 | #include "utils.h" 8 | 9 | #include "gp.h" 10 | 11 | using namespace gcop; 12 | using namespace std; 13 | 14 | using namespace Eigen; 15 | 16 | //#define SYM 17 | 18 | 19 | int main(int argc, char** argv) 20 | { 21 | GP gp(2, 200); 22 | gp.sigma = .1; 23 | gp.Sample(); 24 | 25 | gp.OptParams(); 26 | cout << "l=" << gp.l << endl; 27 | 28 | gp.Train(); 29 | 30 | for (int i = 0; i < 500; ++i) { 31 | VectorXd x(2); 32 | x(0) = 5*(RND - .5); 33 | x(1) = 5*(RND - .5); 34 | gp.Add(x, x.norm() - 1); 35 | } 36 | 37 | int n = 10; 38 | MatrixXd ms(n,n); 39 | MatrixXd ss(n,n); 40 | 41 | double s = 0; 42 | 43 | VectorXd x(2); 44 | for(int i = 0; i < n; ++i) { 45 | x(0) = -2 + ((double)i)/(n-1)*4; 46 | for(int j = 0; j < n; ++j) { 47 | x(1) = -2 + ((double)j)/(n-1)*4; 48 | 49 | ms(j,i) = gp.Predict(x, &s); 50 | ss(j,i) = s; 51 | } 52 | } 53 | 54 | // cout << gp.Xs << endl; 55 | // cout << gp.fs << endl; 56 | 57 | cout << ms << endl; 58 | cout << endl; 59 | cout << ss << endl; 60 | 61 | return 1; 62 | 63 | } 64 | -------------------------------------------------------------------------------- /bin/BulletSTLDemo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This is basically the overall name of the project in Visual Studio this is the name of the Solution File 2 | 3 | 4 | # For every executable you have with a main method you should have an add_executable line below. 5 | # For every add executable line you should list every .cpp and .h file you have associated with that executable. 6 | 7 | 8 | 9 | # You shouldn't have to modify anything below this line 10 | ######################################################## 11 | 12 | message("BULLET INCLUDE: ${BULLET_INCLUDE_DIR}") 13 | 14 | INCLUDE_DIRECTORIES( 15 | ${BULLET_PHYSICS_SOURCE_DIR}/Demos/OpenGL 16 | ${GLUT_INCLUDE_DIR} 17 | ) 18 | 19 | LINK_LIBRARIES( 20 | OpenGLSupport ${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} 21 | gcop_algos gcop_bulletsystems gcop_systems ${BULLET_LIBRARIES} ${ALGO_LIBS} ${SYS_LIBS} ${UTIL_LIBS} 22 | ) 23 | 24 | ADD_EXECUTABLE(AppVehiclewithSTLDemo 25 | VehicleDemo.cpp 26 | main.cpp 27 | ) 28 | 29 | IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) 30 | SET_TARGET_PROPERTIES(AppVehicleDemo PROPERTIES DEBUG_POSTFIX "_Debug") 31 | SET_TARGET_PROPERTIES(AppVehicleDemo PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") 32 | SET_TARGET_PROPERTIES(AppVehicleDemo PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") 33 | ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) 34 | -------------------------------------------------------------------------------- /bin/gpadd.cc: -------------------------------------------------------------------------------- 1 | //#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET 2 | 3 | #include 4 | //#include 5 | //#include 6 | //#include "gsl/gsl_cdf.h" 7 | #include "utils.h" 8 | 9 | #include "gp.h" 10 | 11 | using namespace gcop; 12 | using namespace std; 13 | 14 | using namespace Eigen; 15 | 16 | //#define SYM 17 | 18 | 19 | int main(int argc, char** argv) 20 | { 21 | GP gp(2, 0); 22 | gp.sigma = .1; 23 | 24 | // gp.Sample(); 25 | 26 | // gp.Train(); 27 | 28 | for (int i = 0; i < 500; ++i) { 29 | VectorXd x(2); 30 | x(0) = 5*(RND - .5); 31 | x(1) = 5*(RND - .5); 32 | gp.Add(x, x.norm() - 1); 33 | } 34 | 35 | gp.OptParams(); 36 | cout << "l=" << gp.l << endl; 37 | 38 | int n = 10; 39 | MatrixXd ms(n,n); 40 | MatrixXd ss(n,n); 41 | 42 | double s = 0; 43 | 44 | VectorXd x(2); 45 | for(int i = 0; i < n; ++i) { 46 | x(0) = -2 + ((double)i)/(n-1)*4; 47 | for(int j = 0; j < n; ++j) { 48 | x(1) = -2 + ((double)j)/(n-1)*4; 49 | 50 | ms(j,i) = gp.Predict(x, &s); 51 | ss(j,i) = s; 52 | } 53 | } 54 | 55 | // cout << gp.Xs << endl; 56 | // cout << gp.fs << endl; 57 | 58 | cout << ms << endl; 59 | cout << endl; 60 | cout << ss << endl; 61 | 62 | return 1; 63 | 64 | } 65 | -------------------------------------------------------------------------------- /lib/systems/unicycle.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_UNICYCLE_H 2 | #define GCOP_UNICYCLE_H 3 | 4 | #include "system.h" 5 | #include 6 | 7 | namespace gcop { 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | 12 | typedef Matrix Vector5d; 13 | typedef Matrix Matrix5d; 14 | typedef Matrix Matrix52d; 15 | typedef Matrix Matrix5Xd; 16 | 17 | /** 18 | * A standard unicycle model with 2nd order dynamics. This is often used to 19 | * model simple wheeled robots or fixed-wing aircraft. 20 | * 21 | * The state is 22 | * \f$ \bm x = (x,y,\theta,v,\omega) \in\mathbb{R}^5\f$ and controls are \f$ \bm u = (u_1,u_2) \in\mathbb{R}^5\f$ 23 | * correspond to forward acceleration and turn rate acceleration. 24 | * 25 | * Author: Marin Kobilarov marin(at)jhu.edu 26 | */ 27 | class Unicycle : public System 28 | { 29 | public: 30 | Unicycle(); 31 | 32 | double Step(Vector5d &xb, double t, const Vector5d &xa, 33 | const Vector2d &u, double h, const VectorXd *p, 34 | Matrix5d *A = 0, Matrix52d *B = 0, Matrix5Xd *C = 0); 35 | 36 | double dx; ///< distance between left and right tires 37 | double dy; ///< distance between front and back axles 38 | }; 39 | } 40 | 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /lib/systems/manifolds/kinbody3dmanifold.cc: -------------------------------------------------------------------------------- 1 | #include "kinbody3dmanifold.h" 2 | #include "se3.h" 3 | 4 | using namespace gcop; 5 | using namespace Eigen; 6 | 7 | Kinbody3dManifold::Kinbody3dManifold() : Manifold(6) 8 | { 9 | } 10 | 11 | 12 | Kinbody3dManifold& Kinbody3dManifold::Instance() 13 | { 14 | static Kinbody3dManifold instance; 15 | return instance; 16 | } 17 | 18 | 19 | void Kinbody3dManifold::Lift(Vector6d &v, 20 | const Matrix4d &xa, 21 | const Matrix4d &xb) 22 | { 23 | Matrix4d dg; 24 | SE3::Instance().diff(dg, xa, xb); 25 | SE3::Instance().g2q(v, dg); 26 | } 27 | 28 | 29 | void Kinbody3dManifold::Retract(Matrix4d &xb, 30 | const Matrix4d &xa, 31 | const Vector6d &v) 32 | { 33 | Matrix4d dg; 34 | SE3::Instance().q2g(dg, v); 35 | xb = xa*dg; 36 | } 37 | 38 | 39 | void Kinbody3dManifold::dtau(Matrix4d &M, const Vector6d &v) 40 | { 41 | M.setIdentity(); 42 | Matrix3d dR; 43 | SO3::Instance().dcay(dR, v.head<3>()); 44 | M.topLeftCorner<3,3>() = dR; 45 | } 46 | 47 | 48 | void Kinbody3dManifold::Adtau(Matrix4d &M, const Vector6d &v) 49 | { 50 | M.setIdentity(); 51 | Matrix3d g; 52 | SO3::Instance().cay(g, v.head<3>()); 53 | Matrix3d A; 54 | SO3::Instance().Ad(A, g); 55 | M.topLeftCorner<3,3>() = A; 56 | } 57 | -------------------------------------------------------------------------------- /lib/algos/controller.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_CONTROLLER_H 2 | #define GCOP_CONTROLLER_H 3 | 4 | #include 5 | 6 | namespace gcop { 7 | 8 | using namespace Eigen; 9 | 10 | /** 11 | * Base controller. Its simple purpose is to take a state x 12 | * and time t and produces the control u. 13 | * 14 | * Author: Marin Kobilarov -- Copyright (C) 2006-2013 15 | */ 16 | template class Controller { 17 | public: 18 | /** 19 | * Feedback controller of the form u = k(t, x) 20 | * @param u controls 21 | * @param t time 22 | * @param x state 23 | * @return true if arguments are valid, false if e.g. x is infeasible 24 | */ 25 | virtual bool Set(Tu &u, double t, const Tx &x) = 0; 26 | 27 | /** 28 | * Set controller parameters 29 | * @return true if arguments are valid, false if e.g. s is infeasible 30 | */ 31 | virtual bool SetParams(const Ts &s) { 32 | this->s = s; 33 | return true; 34 | } 35 | 36 | /** 37 | * Set context 38 | * @return true if arguments are valid, false if e.g. s is infeasible 39 | */ 40 | virtual bool SetContext(const Tc &c) { 41 | this->c = c; 42 | return true; 43 | } 44 | 45 | Ts s; ///< controller parameters 46 | Tc c; ///< controller context 47 | }; 48 | }; 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /lib/views/mbsview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_MBSVIEW_H 2 | #define GCOP_MBSVIEW_H 3 | 4 | #include "mbs.h" 5 | #include "systemview.h" 6 | #include "GL/glut.h" 7 | #include 8 | #include 9 | #include 10 | #include 11 | //#include "viewer.h" 12 | #include "geom3dview.h" 13 | 14 | 15 | namespace gcop { 16 | 17 | #include "utils.h" 18 | 19 | using namespace Eigen; 20 | 21 | class MbsView : public SystemView { 22 | public: 23 | 24 | /** 25 | * Create a particle view of trajectory traj 26 | * @param sys particle 27 | * @param xs trajectory 28 | */ 29 | MbsView(const Mbs &sys, 30 | vector *xs = 0, 31 | vector *us = 0); 32 | 33 | virtual ~MbsView(); 34 | 35 | 36 | virtual void Render(const MbsState *x, 37 | const VectorXd *u = 0); 38 | 39 | void Render(const vector *xs, 40 | const vector *us = 0, 41 | bool rs = true, 42 | int is = -1, int ie = -1, 43 | int dis = 1, int dit = 1, 44 | bool dl = false); 45 | 46 | const Mbs &sys; 47 | GLUquadricObj *qobj; 48 | double dirSize; 49 | 50 | vector geomViews; 51 | }; 52 | } 53 | 54 | 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /lib/algos/point3dcontroller.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_POINT3DCONTROLLER_H 2 | #define GCOP_POINT3DCONTROLLER_H 3 | 4 | #include "controller.h" 5 | #include "point3d.h" 6 | 7 | namespace gcop { 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | 12 | /** 13 | * Basic PD controller for a point-mass system in 3d 14 | * 15 | * Author: Marin Kobilarov 16 | */ 17 | class Point3dController : public Controller< Point3dState, Vector3d> { 18 | public: 19 | 20 | /** 21 | * Point3D PD controller 22 | * 23 | * @param sys multi-body system 24 | * @param xd desired state (optional, set to origin by default) 25 | * @param ad desired acceleration (optional, set to zero by default) 26 | */ 27 | Point3dController(const Point3d &sys, 28 | Point3dState *xd = 0, 29 | Vector3d *ad = 0); 30 | 31 | virtual bool Set(Vector3d &u, double t, const Point3dState &x); 32 | 33 | virtual ~Point3dController(); 34 | 35 | const Point3d &sys; ///< point mass system 36 | 37 | Point3dState *xd; ///< desired state (origin by default) 38 | Vector3d *ad; ///< desired acceleration (zero by default) 39 | 40 | Vector3d Kp; ///< proportional terms (ones by default) 41 | Vector3d Kd; ///< derivative terms (ones by default) 42 | }; 43 | }; 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /bin/BulletZVehicleTest/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This is basically the overall name of the project in Visual Studio this is the name of the Solution File 2 | 3 | 4 | # For every executable you have with a main method you should have an add_executable line below. 5 | # For every add executable line you should list every .cpp and .h file you have associated with that executable. 6 | 7 | 8 | 9 | # You shouldn't have to modify anything below this line 10 | ######################################################## 11 | 12 | message("BULLET INCLUDE: ${BULLET_INCLUDE_DIR}") 13 | 14 | INCLUDE_DIRECTORIES( 15 | ${BULLET_PHYSICS_SOURCE_DIR}/Demos/OpenGL 16 | ${GLUT_INCLUDE_DIR} 17 | ) 18 | 19 | LINK_LIBRARIES( 20 | OpenGLSupport ${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} 21 | gcop_algos gcop_bulletsystems gcop_systems ${BULLET_LIBRARIES} ${ALGO_LIBS} ${SYS_LIBS} ${UTIL_LIBS} 22 | ) 23 | 24 | #heightfield128x128.cpp 25 | ADD_EXECUTABLE(AppVehicleZtest 26 | VehicleDemo.cpp 27 | main.cpp 28 | ) 29 | 30 | IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) 31 | SET_TARGET_PROPERTIES(AppVehicleDemo PROPERTIES DEBUG_POSTFIX "_Debug") 32 | SET_TARGET_PROPERTIES(AppVehicleDemo PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") 33 | SET_TARGET_PROPERTIES(AppVehicleDemo PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") 34 | ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) 35 | -------------------------------------------------------------------------------- /bin/BulletVehicleEstimation1/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This is basically the overall name of the project in Visual Studio this is the name of the Solution File 2 | 3 | 4 | # For every executable you have with a main method you should have an add_executable line below. 5 | # For every add executable line you should list every .cpp and .h file you have associated with that executable. 6 | 7 | 8 | 9 | # You shouldn't have to modify anything below this line 10 | ######################################################## 11 | 12 | message("BULLET INCLUDE: ${BULLET_INCLUDE_DIR}") 13 | 14 | INCLUDE_DIRECTORIES( 15 | ${BULLET_PHYSICS_SOURCE_DIR}/Demos/OpenGL 16 | ${GLUT_INCLUDE_DIR} 17 | ) 18 | 19 | LINK_LIBRARIES( 20 | OpenGLSupport ${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} 21 | gcop_algos gcop_bulletsystems gcop_systems ${BULLET_LIBRARIES} ${ALGO_LIBS} ${SYS_LIBS} ${UTIL_LIBS} 22 | ) 23 | 24 | #heightfield128x128.cpp 25 | ADD_EXECUTABLE(AppVehicleDemo1 26 | VehicleDemo.cpp 27 | main.cpp 28 | ) 29 | 30 | IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) 31 | SET_TARGET_PROPERTIES(AppVehicleDemo PROPERTIES DEBUG_POSTFIX "_Debug") 32 | SET_TARGET_PROPERTIES(AppVehicleDemo PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") 33 | SET_TARGET_PROPERTIES(AppVehicleDemo PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") 34 | ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) 35 | -------------------------------------------------------------------------------- /lib/systems/manifolds/imumanifold.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_IMUMANIFOLD_H 2 | #define GCOP_IMUMANIFOLD_H 3 | 4 | #include "manifold.h" 5 | 6 | namespace gcop { 7 | 8 | using namespace std; 9 | using namespace Eigen; 10 | 11 | typedef Matrix Vector6d; 12 | typedef Matrix Vector9d; 13 | typedef Matrix Matrix9d; 14 | 15 | struct ImuState { 16 | ImuState() : 17 | R(Matrix3d::Identity()), 18 | bg(Vector3d::Zero()), 19 | ba(Vector3d::Zero()), 20 | P(Matrix9d::Identity()) { 21 | } 22 | 23 | Matrix3d R; ///< rotation matrix 24 | Vector3d bg; ///< gyro bias 25 | Vector3d ba; ///< acceleration bias 26 | 27 | Matrix9d P; ///< covariance 28 | }; 29 | 30 | // typedef pair ImuState; 31 | 32 | class ImuManifold : public Manifold { 33 | 34 | public: 35 | static ImuManifold& Instance(); 36 | 37 | void Lift(Vector9d &v, 38 | const ImuState &xa, 39 | const ImuState &xb); 40 | 41 | void Retract(ImuState &xb, 42 | const ImuState &xa, 43 | const Vector9d &v); 44 | 45 | void dtau(Matrix9d &M, const Vector9d &v); 46 | 47 | void Adtau(Matrix9d &M, const Vector9d &v); 48 | 49 | private: 50 | ImuManifold(); 51 | }; 52 | } 53 | 54 | 55 | #endif 56 | -------------------------------------------------------------------------------- /lib/systems/joint.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_JOINT_H 2 | #define GCOP_JOINT_H 3 | 4 | #include 5 | 6 | namespace gcop { 7 | 8 | using namespace std; 9 | using namespace Eigen; 10 | 11 | typedef Matrix Vector6d; 12 | typedef Matrix Matrix6d; 13 | 14 | class Joint { 15 | public: 16 | Joint(); 17 | 18 | Joint(const Vector6d &a, 19 | const Matrix4d &gp, 20 | const Matrix4d &gc, 21 | double damping = 0, 22 | double friction = 0); 23 | 24 | virtual ~Joint(); 25 | 26 | void Init(); 27 | 28 | Vector6d a; ///< axis 29 | Matrix4d gp; ///< fixed xform from parent to joint 30 | Matrix4d gc; ///< fixed xfom from child to joint 31 | 32 | double damping; ///< damping terms: Ns/m for trans or Nm s/rad for rev 33 | double friction; ///< static friction: N for prismatic, Nm for rev 34 | double lower; ///< lower limit (for revolute or prismatic joints) 35 | double upper; ///< upper limit (for revolute or prismatic joints) 36 | 37 | Matrix6d Ac; ///< change of frame Ad(gc) 38 | Matrix4d gpi; ///< from parent to joint inverse 39 | Matrix4d gci; ///< from child to joint inverse 40 | 41 | Vector6d S; ///< Jacobian S=Ac*a 42 | string name; ///< Unique name of the joint 43 | 44 | }; 45 | } 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /lib/systems/point3d.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include "point3d.h" 3 | #include "point3dmanifold.h" 4 | #include 5 | #include 6 | 7 | using namespace gcop; 8 | using namespace Eigen; 9 | 10 | 11 | Point3d::Point3d() : System(Point3dManifold::Instance()) 12 | { 13 | sa = .01; 14 | } 15 | 16 | 17 | Point3d::~Point3d() 18 | { 19 | } 20 | 21 | double Point3d::Step(Point3dState &xb, double t, const Point3dState &xa, 22 | const Vector3d &u, double dt, const VectorXd *p, 23 | Matrix6d *A, Matrix63d *B, Matrix6Xd *C) { 24 | 25 | xb.q = xa.q + dt*xa.v; 26 | xb.v = xa.v + dt*u; 27 | 28 | // needed for filtering 29 | if (A) { 30 | A->setIdentity(); 31 | A->topRightCorner<3,3>().diagonal().setConstant(dt); 32 | } 33 | return 0; 34 | } 35 | 36 | bool Point3d::Noise(Matrix6d &Q, double t, const Point3dState &x, const Vector3d &u, 37 | double dt, const VectorXd *p) { 38 | 39 | double sa2 = sa*sa; 40 | double dt2 = dt*dt; 41 | double dt3 = dt2*dt; 42 | 43 | Q.setZero(); 44 | Q.topLeftCorner<3,3>().diagonal().setConstant(sa2*dt3/3); 45 | Q.topRightCorner<3,3>().diagonal().setConstant(sa2*dt2/2); 46 | Q.bottomLeftCorner<3,3>().diagonal().setConstant(sa2*dt2/2); 47 | Q.bottomRightCorner<3,3>().diagonal().setConstant(sa2*dt); 48 | return true; 49 | } 50 | -------------------------------------------------------------------------------- /README: -------------------------------------------------------------------------------- 1 | 1.) if ./docs is empty, run command "doxygen" 2 | 3 | 2.) See docs/html/index.html 4 | 5 | 3.) If using Bullet make sure you compile bullet with Double Precision (USE_BULLET) on since optimization loses precision if used with float 6 | 7 | 4.) Use install.bash for compiling the package 8 | 9 | 5.) Required Dependencies: GSL (libgsl0-dev) Eigen3(>= 3.1) (>= 3.3.4 for Ubuntu 16.04, >= 3.3.7 for Ubuntu 18.04) (libeigen3-dev) GLUT (freeglut3-dev) For Ubuntu 12.04 see below INSTRUCTIONS 10 | 11 | 6.) Optional Dependencies: OpenCV(>= 2.4), OpenGL (For bins) DSL (https://github.com/jhu-asco/dsl), Ceres (http://ceres-solver.org/), Casadi (https://github.com/casadi/casadi) (commit 54bde1c242fbb229e05fe87310dce3371b1a4af9) 12 | 13 | 7.) For Ubuntu 12.04 install Ceres version 1.9.0. Ceres also depends on glog which can be installed from https://github.com/google/glog . For Ubuntu 14.04 glog can be installed as " sudo apt-get install libgoogle-glog-dev " 14 | 15 | 8.) For Ubuntu 12.04 you cannot libeigen3-dev as it will install Eigen 3.0.5. So you have to install from source. We tested with Eigen 3.2.7 and works fine (http://bitbucket.org/eigen/eigen/get/3.2.7.tar.gz) 16 | 17 | 9.) Compilers Tested ON g++ 4.6.3 g++ 4.8.4 18 | 19 | 10.) Ceres Should be version 1.10 or higher. DO NOT sudo apt-get install ceres on Ubuntu. Even if you did download ceres from source (http://ceres-solver.org/) 20 | 21 | -------------------------------------------------------------------------------- /lib/systems/gunicycle.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include "gunicycle.h" 3 | #include "gunicyclemanifold.h" 4 | #include "se2.h" 5 | #include "rn.h" 6 | #include 7 | #include 8 | 9 | using namespace gcop; 10 | using namespace Eigen; 11 | 12 | Gunicycle::Gunicycle() : System(GunicycleManifold::Instance()), 13 | dx(1), dy(2) 14 | { 15 | } 16 | 17 | 18 | double Gunicycle::Step(M3V2d& xb, double t, const M3V2d& xa, 19 | const Vector2d& u, double h, const VectorXd *p, 20 | Matrix5d *A, Matrix52d *B, Matrix5Xd *C) { 21 | 22 | xb.second = xa.second + h*u; 23 | 24 | Vector3d hxib(h*xb.second[0], h*xb.second[1], 0); 25 | 26 | SE2 &se2 = SE2::Instance(); 27 | 28 | Matrix3d m; 29 | 30 | se2.cay(m, hxib); 31 | xb.first = xa.first*m; 32 | 33 | if (A) { 34 | se2.cay(m, -hxib); 35 | Matrix3d Adm; 36 | se2.Ad(Adm, m); 37 | A->topLeftCorner<3,3>() = Adm; 38 | se2.dcay(m, -hxib); 39 | A->topRightCorner<3,2>() = h*(m.leftCols(2)); 40 | A->bottomLeftCorner<2,3>().setZero(); 41 | A->bottomRightCorner<2,2>().setIdentity(); 42 | } 43 | 44 | if (B) { 45 | // A should've been called too 46 | assert(A); 47 | B->topRightCorner<3,2>() = h*h*(m.leftCols<2>()); 48 | (*B)(3,0) = h; (*B)(3,1) = 0; 49 | (*B)(4,0) = 0; (*B)(4,1) = h; 50 | } 51 | return 1; 52 | } 53 | -------------------------------------------------------------------------------- /lib/systems/manifolds/posemanifold.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_POSEMANIFOLD_H 2 | #define GCOP_POSEMANIFOLD_H 3 | 4 | #include "manifold.h" 5 | #include 6 | 7 | namespace gcop { 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | 12 | typedef Matrix Vector6d; 13 | typedef Matrix Matrix6d; 14 | 15 | struct PoseState { 16 | PoseState() : 17 | R(Matrix3d::Identity()), 18 | p(Vector3d::Zero()), 19 | P(Matrix6d::Identity()) { 20 | } 21 | 22 | Matrix3d R; ///< rotation matrix 23 | Vector3d p; ///< position 24 | 25 | Matrix6d P; ///< covariance 26 | void resize(int n){ 27 | std::cout << "Warning: type is PoseState, resize is NOT implemented" << std::endl; 28 | } 29 | }; 30 | 31 | // typedef pair InsState; 32 | 33 | class PoseManifold : public Manifold { 34 | 35 | public: 36 | static PoseManifold& Instance(); 37 | 38 | void Lift(Vector6d &v, 39 | const PoseState &xa, 40 | const PoseState &xb); 41 | 42 | void Retract(PoseState &xb, 43 | const PoseState &xa, 44 | const Vector6d &v); 45 | 46 | void dtau(Matrix6d &M, const Vector6d &v); 47 | 48 | void Adtau(Matrix6d &M, const Vector6d &v); 49 | 50 | private: 51 | PoseManifold(); 52 | }; 53 | } 54 | 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /lib/systems/manifolds/gcarmanifold.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_GCARMANIFOLD_H 2 | #define GCOP_GCARMANIFOLD_H 3 | 4 | #include "manifold.h" 5 | #include 6 | 7 | namespace gcop { 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | 12 | typedef Matrix Vector4d; 13 | typedef Matrix Matrix4d; 14 | 15 | class GcarState { 16 | public: 17 | GcarState(bool clear = true) { 18 | if (clear) 19 | Clear(); 20 | } 21 | 22 | GcarState(Matrix3d gin,double vin):g(gin),v(vin) { 23 | } 24 | 25 | virtual ~GcarState() { 26 | } 27 | 28 | void Clear() { 29 | g.setIdentity(); 30 | v = 0; 31 | } 32 | 33 | Matrix3d g; ///< SE(2) pose 34 | double v; ///< forward velocity 35 | }; 36 | 37 | class GcarManifold : public Manifold { 38 | public: 39 | 40 | static GcarManifold& Instance(); 41 | 42 | void Lift(Vector4d &dx, 43 | const GcarState &xa, 44 | const GcarState &xb); 45 | 46 | void Retract(GcarState &xb, 47 | const GcarState &xa, 48 | const Vector4d &dx); 49 | 50 | 51 | void dtau(Matrix4d &M, const Vector4d &v); 52 | 53 | void dtauinv(Matrix4d &M, const Vector4d &v); 54 | 55 | void Adtau(Matrix4d &M, const Vector4d &v); 56 | 57 | private: 58 | GcarManifold(); 59 | }; 60 | } 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /lib/systems/loop_timer.cpp: -------------------------------------------------------------------------------- 1 | #include "loop_timer.h" 2 | #include 3 | 4 | void LoopTimer::loop_start() { 5 | if (loop_start_ && !loop_pause_) { 6 | throw std::runtime_error("Cannot start loop without ending the loop"); 7 | } 8 | if (!loop_pause_) { 9 | dt_loop = std::chrono::duration::zero(); 10 | } 11 | loop_start_ = true; 12 | loop_pause_ = false; 13 | t0 = std::chrono::high_resolution_clock::now(); 14 | } 15 | 16 | void LoopTimer::loop_pause() { 17 | if (loop_pause_) { 18 | throw std::runtime_error("Loop already paused"); 19 | } 20 | if (!loop_start_) { 21 | throw std::runtime_error("Loop did not start"); 22 | } 23 | dt_loop += std::chrono::duration( 24 | std::chrono::high_resolution_clock::now() - t0); 25 | loop_pause_ = true; 26 | } 27 | 28 | void LoopTimer::loop_end() { 29 | if (!loop_start_) { 30 | throw std::runtime_error("Loop did not start"); 31 | } 32 | dt_loop = dt_loop + std::chrono::duration( 33 | std::chrono::high_resolution_clock::now() - t0); 34 | if (N == 0) { 35 | dt_average = dt_loop.count(); 36 | N = 1; 37 | } else { 38 | dt_average = (N * dt_average + dt_loop.count()) / (++N); 39 | } 40 | loop_pause_ = false; 41 | loop_start_ = false; 42 | } 43 | 44 | double LoopTimer::average_loop_period() { return dt_average; } 45 | 46 | double LoopTimer::loop_period() { return dt_loop.count(); } 47 | -------------------------------------------------------------------------------- /bin/BulletVehicleEstimation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # This is basically the overall name of the project in Visual Studio this is the name of the Solution File 2 | 3 | 4 | # For every executable you have with a main method you should have an add_executable line below. 5 | # For every add executable line you should list every .cpp and .h file you have associated with that executable. 6 | 7 | 8 | 9 | # You shouldn't have to modify anything below this line 10 | ######################################################## 11 | 12 | message("BULLET INCLUDE: ${BULLET_INCLUDE_DIR}") 13 | message("Bullet LIBS: ${BULLET_LIBRARIES}") 14 | 15 | INCLUDE_DIRECTORIES( 16 | ${BULLET_PHYSICS_SOURCE_DIR}/Demos/OpenGL 17 | ${GLUT_INCLUDE_DIR} 18 | ) 19 | #BulletDynamics BulletCollision LinearMath 20 | LINK_LIBRARIES( 21 | OpenGLSupport ${GLUT_glut_LIBRARY} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} 22 | gcop_algos gcop_bulletsystems gcop_systems ${BULLET_LIBRARIES} ${ALGO_LIBS} ${SYS_LIBS} ${UTIL_LIBS} 23 | ) 24 | 25 | #heightfield128x128.cpp 26 | ADD_EXECUTABLE(AppVehicleDemo 27 | VehicleDemo.cpp 28 | main.cpp 29 | ) 30 | 31 | IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) 32 | SET_TARGET_PROPERTIES(AppVehicleDemo PROPERTIES DEBUG_POSTFIX "_Debug") 33 | SET_TARGET_PROPERTIES(AppVehicleDemo PROPERTIES MINSIZEREL_POSTFIX "_MinsizeRel") 34 | SET_TARGET_PROPERTIES(AppVehicleDemo PROPERTIES RELWITHDEBINFO_POSTFIX "_RelWithDebugInfo") 35 | ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES) 36 | -------------------------------------------------------------------------------- /lib/views/insview.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_INSVIEW_H 2 | #define GCOP_INSVIEW_H 3 | 4 | #include "ins.h" 5 | #include "systemview.h" 6 | #include "GL/glut.h" 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace gcop { 13 | 14 | using namespace Eigen; 15 | 16 | class InsView : public SystemView { 17 | 18 | public: 19 | 20 | /** 21 | * Create a particle view of trajectory traj 22 | * @param sys particle 23 | */ 24 | InsView(const Ins& sys); 25 | 26 | 27 | /** 28 | * Create a particle view of trajectory traj 29 | * @param sys particle 30 | * @param xs trajectory 31 | */ 32 | InsView(const Ins &sys, 33 | vector *xs, 34 | vector *us = 0); 35 | 36 | virtual ~InsView(); 37 | 38 | 39 | virtual void Render(const InsState *x, 40 | const Vector6d *u = 0); 41 | 42 | void Render(const vector *xs, 43 | const vector *us = 0, 44 | bool rs = true, 45 | int is = -1, int ie = -1, 46 | int dis = 1, int dit = 1, 47 | bool dl = false); 48 | 49 | const Ins &sys; 50 | GLUquadricObj *qobj; 51 | double dirSize; 52 | 53 | static void Transform(const Matrix3d &R, const Vector3d &p); 54 | }; 55 | } 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /lib/systems/manifolds/kinbody2dmanifold.cc: -------------------------------------------------------------------------------- 1 | #include "kinbody2dmanifold.h" 2 | #include "se2.h" 3 | 4 | using namespace gcop; 5 | using namespace Eigen; 6 | 7 | Kinbody2dManifold::Kinbody2dManifold() : Manifold(3) 8 | { 9 | } 10 | 11 | 12 | Kinbody2dManifold& Kinbody2dManifold::Instance() 13 | { 14 | static Kinbody2dManifold instance; 15 | return instance; 16 | } 17 | 18 | 19 | void Kinbody2dManifold::Lift(Vector3d &v, 20 | const Matrix3d &xa, 21 | const Matrix3d &xb) 22 | { 23 | Matrix3d dg; 24 | SE2::Instance().diff(dg, xa, xb); 25 | SE2::Instance().g2q(v, dg); 26 | } 27 | 28 | 29 | void Kinbody2dManifold::Retract(Matrix3d &xb, 30 | const Matrix3d &xa, 31 | const Vector3d &v) 32 | { 33 | Matrix3d dg; 34 | SE2::Instance().q2g(dg, v); 35 | xb = xa*dg; 36 | } 37 | 38 | 39 | void Kinbody2dManifold::dtau(Matrix3d &M, const Vector3d &v) 40 | { 41 | M.setIdentity(); 42 | double c = cos(v[0]); 43 | double s = sin(v[0]); 44 | M(1,1) = c; M(1,2) = -s; 45 | M(2,1) = s; M(2,2) = c; 46 | } 47 | 48 | 49 | void Kinbody2dManifold::Adtau(Matrix3d &M, const Vector3d &v) 50 | { 51 | const double c = cos(v[0]); 52 | const double s = sin(v[0]); 53 | const double &x = v[1]; 54 | const double &y = v[2]; 55 | 56 | M.setIdentity(); 57 | M(1,0) = y; 58 | M(1,1) = c; 59 | M(1,2) = -s; 60 | M(2,0) = -x; 61 | M(2,1) = s; 62 | M(2,2) = c; 63 | } 64 | -------------------------------------------------------------------------------- /bin/CMakeLists.lim: -------------------------------------------------------------------------------- 1 | link_directories(${RPC_BINARY_DIR}/lib) 2 | 3 | add_executable(particle2dtest particle2dtest.cc) 4 | target_link_libraries(particle2dtest rpc ${LIBS}) 5 | 6 | add_executable(unicycletest unicycletest.cc) 7 | target_link_libraries(unicycletest rpc ${LIBS}) 8 | 9 | add_executable(gunicycletest gunicycletest.cc) 10 | target_link_libraries(gunicycletest rpc ${LIBS}) 11 | 12 | add_executable(chaintest chaintest.cc) 13 | target_link_libraries(chaintest rpc ${LIBS}) 14 | 15 | add_executable(chain1test chain1test.cc) 16 | target_link_libraries(chain1test rpc ${LIBS}) 17 | 18 | add_executable(airmtest airmtest.cc) 19 | target_link_libraries(airmtest rpc ${LIBS}) 20 | 21 | add_executable(airmopttest airmopttest.cc) 22 | target_link_libraries(airmopttest rpc ${LIBS}) 23 | 24 | add_executable(airbottest airbottest.cc) 25 | target_link_libraries(airbottest rpc ${LIBS}) 26 | 27 | add_executable(airbotsim airbotsim.cc) 28 | target_link_libraries(airbotsim rpc ${LIBS}) 29 | 30 | 31 | add_executable(chainopttest chainopttest.cc) 32 | target_link_libraries(chainopttest rpc ${LIBS}) 33 | 34 | add_executable(body3dtest body3dtest.cc) 35 | target_link_libraries(body3dtest rpc ${LIBS}) 36 | 37 | add_executable(batest batest.cc) 38 | target_link_libraries(batest rpc ${LIBS}) 39 | 40 | 41 | #add_executable(snakeopttest snakeopttest.cc) 42 | #target_link_libraries(snakeopttest rpc ${LIBS}) 43 | 44 | 45 | 46 | #INSTALL(TARGETS rpctest 47 | # RUNTIME 48 | # DESTINATION bin) 49 | 50 | -------------------------------------------------------------------------------- /bin/ukftest.cc: -------------------------------------------------------------------------------- 1 | #include "ukf.h" 2 | #include 3 | 4 | using namespace std; 5 | using namespace gcop; 6 | using namespace Eigen; 7 | 8 | class ParticleModel : public Model { 9 | public: 10 | ParticleModel() : 11 | Model(2, 1), dt(0.1) { 12 | double s = .5; 13 | Vector2d w(2); 14 | w(0) = dt*dt/2*s; 15 | w(1) = dt*s; 16 | 17 | Q.diagonal() << w.cwiseProduct(w); 18 | 19 | R(0,0) = 1; 20 | } 21 | 22 | bool f (VectorXd &xn, 23 | const VectorXd &x) 24 | { 25 | xn[0] = x[0] + dt*x[1]; 26 | xn[1] = x[1]; 27 | return true; 28 | } 29 | 30 | bool h(VectorXd &z, 31 | const VectorXd &x) 32 | { 33 | z[0] = x[0]; 34 | return true; 35 | } 36 | 37 | double dt; /// < time-step 38 | }; 39 | 40 | 41 | int main(int argc, char** argv) 42 | { 43 | ParticleModel pm; 44 | UKF ukf(pm); 45 | 46 | int N = 100; 47 | 48 | VectorXd xn(2); 49 | VectorXd xt(2); 50 | xt << 0, 1; 51 | 52 | ukf.x = xt; 53 | ukf.P = pm.Q; 54 | 55 | cout << pm.Q << endl; 56 | cout << ukf.P << endl; 57 | cout << ukf.x << endl; 58 | 59 | VectorXd z(1); 60 | 61 | for (int i = 0; i < N; ++i) { 62 | ukf.Predict(); 63 | 64 | pm.f(xn, xt); 65 | xt = xn; 66 | 67 | z[0] = xt[0] + sqrt(pm.R(0,0))*(rand()/(double)RAND_MAX - .5); 68 | 69 | ukf.Update(z); 70 | 71 | cout << "True: " << xt[0] << "," << xt[1] << endl; 72 | cout << "Estm: " << ukf.x[0] << "," << ukf.x[1] << endl; 73 | } 74 | } 75 | -------------------------------------------------------------------------------- /lib/systems/gunicycle.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_GUNICYCLE_H 2 | #define GCOP_GUNICYCLE_H 3 | 4 | #include "system.h" 5 | #include 6 | 7 | namespace gcop { 8 | 9 | using namespace std; 10 | using namespace Eigen; 11 | 12 | typedef Matrix Vector5d; 13 | typedef Matrix Matrix5d; 14 | typedef Matrix Matrix52d; 15 | typedef Matrix Matrix5Xd; 16 | 17 | typedef pair M3V2d; 18 | 19 | /** 20 | * A "geometric" unicycle model with 2nd order dynamics. This is often used to 21 | * model simple wheeled robots or fixed-wing aircraft. The control system 22 | * is implemented as an evolution on SE(2) as opposed to a vector space. 23 | * 24 | * The state is 25 | * \f$ \bm x = (g, \omega, v) \f$ where \f$ g\in SE(2)\f$ is the pose, 26 | * the controls \f$ \bm u = (u_\omega,u_v) \in\mathbb{R}^2\f$ 27 | * correspond to turn rate acceleration and forward acceleration. 28 | * 29 | * Author: Marin Kobilarov marin(at)jhu.edu 30 | */ 31 | class Gunicycle : public System { 32 | public: 33 | Gunicycle(); 34 | 35 | double Step(M3V2d &xb, double t, const M3V2d &xa, 36 | const Vector2d &u, double h, const VectorXd *p, 37 | Matrix5d *A = 0, Matrix52d *B = 0, Matrix5Xd *C = 0); 38 | 39 | double dx; ///< distance between left and right tires 40 | double dy; ///< distance between front and back axles 41 | }; 42 | } 43 | 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /lib/algos/gavoidcontroller.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_GAVOIDCONTROLLER_H 2 | #define GCOP_GAVOIDCONTROLLER_H 3 | 4 | #include "controller.h" 5 | #include "body3dmanifold.h" 6 | #include "constraint.h" 7 | 8 | namespace gcop { 9 | 10 | using namespace std; 11 | using namespace Eigen; 12 | 13 | typedef Constraint Body3dConstraint; 14 | typedef Matrix Vector6d; 15 | 16 | /** 17 | * Gyroscopic obstacle avoidance controller, i.e. it produces forces 18 | * that "rotate" the velocity vector, so that no kinetic energy is inserted into the system 19 | * Stability is guaranteed for convex obstacles 20 | * 21 | * Author: Marin Kobilarov, 2007 (originally in the DGC library) 22 | */ 23 | class GavoidController : public Controller { 24 | public: 25 | 26 | /** 27 | * Gyroscopic forcing controller 28 | * 29 | * @param con constraint giving the obstacle distance function 30 | */ 31 | GavoidController(Body3dConstraint &con); 32 | 33 | virtual ~GavoidController(); 34 | 35 | virtual bool Set(Vector6d &u, double t, const Body3dState &x); 36 | 37 | Body3dConstraint &con; ///< obstacle constraint 38 | 39 | double sr; ///< sensing radius 40 | double k; ///< obstacle avoidance gain 41 | double kb; ///< breaking gain (optional, zero by default) 42 | 43 | double oc; ///< add extra clearance to obstacles 44 | double odExp; ///< exponent affecting the power of obstacle distance (default is 1) 45 | }; 46 | 47 | } 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /lib/systems/manifolds/insmanifold.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_INSMANIFOLD_H 2 | #define GCOP_INSMANIFOLD_H 3 | 4 | #include "manifold.h" 5 | 6 | namespace gcop { 7 | 8 | using namespace std; 9 | using namespace Eigen; 10 | 11 | typedef Matrix Vector6d; 12 | typedef Matrix Vector15d; 13 | typedef Matrix Matrix15d; 14 | 15 | struct InsState { 16 | InsState() : 17 | R(Matrix3d::Identity()), 18 | bg(Vector3d::Zero()), 19 | ba(Vector3d::Zero()), 20 | p(Vector3d::Zero()), 21 | v(Vector3d::Zero()), 22 | P(Matrix15d::Identity()) { 23 | } 24 | 25 | Matrix3d R; ///< rotation matrix 26 | Vector3d bg; ///< gyro bias 27 | Vector3d ba; ///< acceleration bias 28 | 29 | Vector3d p; ///< position 30 | Vector3d v; ///< velocity 31 | 32 | Matrix15d P; ///< covariance 33 | }; 34 | 35 | // typedef pair InsState; 36 | 37 | class InsManifold : public Manifold { 38 | 39 | public: 40 | static InsManifold& Instance(); 41 | 42 | void Lift(Vector15d &v, 43 | const InsState &xa, 44 | const InsState &xb); 45 | 46 | void Retract(InsState &xb, 47 | const InsState &xa, 48 | const Vector15d &v); 49 | 50 | void dtau(Matrix15d &M, const Vector15d &v); 51 | 52 | void Adtau(Matrix15d &M, const Vector15d &v); 53 | 54 | private: 55 | InsManifold(); 56 | }; 57 | } 58 | 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /lib/systems/rccar1.h: -------------------------------------------------------------------------------- 1 | #ifndef GCOP_RCCAR1_H 2 | #define GCOP_RCCAR1_H 3 | 4 | #include "system.h" 5 | #include "rccar.h" 6 | #include 7 | 8 | namespace gcop { 9 | 10 | using namespace std; 11 | using namespace Eigen; 12 | 13 | typedef Matrix Matrix42d; 14 | typedef Matrix Matrix4pd; 15 | //typedef Matrix Vector6d; 16 | 17 | /** 18 | * A simple rear-drive Rccar model with 2nd order dynamics. 19 | * 20 | * The state is 21 | * \f$ \bm x = (x,y,\theta, v) \in\mathbb{R}^4\f$ and controls are \f$ \bm u = (a, tan(phi) ) \in\mathbb{R}^5\f$ 22 | * correspond to forward acceleration a and steering angle phi 23 | * 24 | * Author: Marin Kobilarov marin(at)jhu.edu 25 | */ 26 | class Rccar1 : public Rccar 27 | { 28 | public: 29 | Rccar1(); 30 | 31 | double Step(Vector4d &xb, double t, const Vector4d &xa, 32 | const Vector2d &u, double h, const VectorXd *p, 33 | Matrix4d *A = 0, Matrix42d *B = 0, Matrix4pd *C = 0); 34 | inline double bind(const double &input, int index) 35 | { 36 | return (input > U.ub[index]? U.ub[index]:(input < U.lb[index])?U.lb[index]:input); 37 | } 38 | 39 | double msteer, csteer;/// 2 | #include "posemanifold.h" 3 | #include "so3.h" 4 | #include 5 | #include 6 | 7 | using namespace gcop; 8 | using namespace Eigen; 9 | 10 | PoseManifold::PoseManifold() : Manifold() 11 | { 12 | } 13 | 14 | 15 | PoseManifold& PoseManifold::Instance() 16 | { 17 | static PoseManifold instance; 18 | return instance; 19 | } 20 | 21 | void PoseManifold::Lift(Vector6d &v, 22 | const PoseState &xa, 23 | const PoseState &xb) 24 | { 25 | Vector3d eR; 26 | // SO3::Instance().log(eR, xa.R.transpose()*xb.R); 27 | SO3::Instance().cayinv(eR, xa.R.transpose()*xb.R); 28 | 29 | v.head<3>() = eR; 30 | v.segment<3>(3) = xb.p - xa.p; 31 | } 32 | 33 | 34 | void PoseManifold::Retract(PoseState &xb, 35 | const PoseState &xa, 36 | const Vector6d &v) 37 | { 38 | Matrix3d dR; 39 | 40 | // SO3::Instance().exp(dR, v.head<3>()); 41 | SO3::Instance().cay(dR, v.head<3>()); 42 | 43 | xb.R = xa.R*dR; 44 | xb.p = xa.p + v.segment<3>(3); 45 | } 46 | 47 | 48 | void PoseManifold::dtau(Matrix6d &M, const Vector6d &v) 49 | { 50 | M.setIdentity(); 51 | Matrix3d dR; 52 | SO3::Instance().dexp(dR, v.head<3>()); 53 | M.topLeftCorner<3,3>() = dR; 54 | } 55 | 56 | void PoseManifold::Adtau(Matrix6d &M, const Vector6d &v) 57 | { 58 | M.setIdentity(); 59 | Matrix3d g; 60 | SO3::Instance().exp(g, v.head<3>()); 61 | Matrix3d A; 62 | SO3::Instance().Ad(A, g); 63 | M.topLeftCorner<3,3>() = A; 64 | } 65 | --------------------------------------------------------------------------------