├── .gitignore ├── .travis.yml ├── LICENSE ├── doc ├── banner.svg ├── gifs │ ├── 3dquadrotor.gif │ ├── ICRA_example1_minobs.gif │ ├── ICRA_example1_minobs_dubins.gif │ ├── ICRA_example1_shift.gif │ ├── ICRA_example2_minobs.gif │ ├── agv.gif │ ├── anchor2D.gif │ ├── bicycle.gif │ ├── cannonballs.gif │ ├── formation_dubins.gif │ ├── formation_quad_rotatingwall.gif │ ├── formation_quad_ufo.gif │ ├── holonomic_3d.gif │ ├── lvd_load.gif │ ├── lvd_unload.gif │ ├── maze.gif │ ├── multiproblem_circ.gif │ ├── multiproblem_rect.gif │ ├── narrowpassage.gif │ ├── p2p_holonomic.gif │ ├── p2p_holonomic_blocking.gif │ ├── plate.gif │ ├── platform_landing.gif │ ├── racetrack.gif │ ├── revolving_door.gif │ ├── revolving_door_diffdrive.gif │ ├── trailer.gif │ └── warehouse.gif └── logo.svg ├── examples ├── GCode_examples │ ├── Star.nc │ ├── Star_Yverdon.nc │ ├── anchor2D.nc │ ├── benchmark_stair.nc │ ├── gcodeproblem_anchor.py │ ├── gcodeproblem_benchmark_stair.py │ ├── gcodeproblem_multi_z.py │ ├── gcodeproblem_racetrack.py │ ├── gcodeproblem_rsq5.py │ ├── gcodeproblem_star.py │ ├── racetrack.nc │ ├── rsq5.nc │ └── rsq5_multi.nc ├── annoying_obstacle.py ├── compare_buildoptions.py ├── compare_buildoptions_distributed.py ├── compare_distributed_optimization_quadrotors.py ├── compare_distributed_vs_central_quadrotors.py ├── deployer_example.py ├── environment.pickle ├── formation_dubins.py ├── formation_holonomic.py ├── formation_holonomic_central.py ├── formation_holonomic_export.py ├── formation_holonomic_multiproblem.py ├── formation_quadrotor.py ├── formation_quadrotor_rotating_wall.py ├── gui_examples │ ├── svg │ │ ├── drawing.svg │ │ ├── maze_big.svg │ │ └── maze_small.svg │ ├── vast_environment_example1.pickle │ ├── vast_environment_example1.py │ ├── vast_environment_example1_dubins.py │ ├── vast_environment_example2.pickle │ ├── vast_environment_example2.py │ ├── vast_environment_example_maze.pickle │ └── vast_environment_example_maze.py ├── p2p_3dquadrotor.py ├── p2p_agv.py ├── p2p_agv2.py ├── p2p_bicycle.py ├── p2p_dubins.py ├── p2p_holonomic.py ├── p2p_holonomic_3d.py ├── p2p_holonomic_balls.py ├── p2p_holonomic_blocking.py ├── p2p_holonomic_disturbances.py ├── p2p_holonomic_export.py ├── p2p_holonomic_interveh_avoidance.py ├── p2p_holonomic_multiproblem.py ├── p2p_holonomic_obstraj_export.py ├── p2p_holonomic_octroom.py ├── p2p_holonomic_orient.py ├── p2p_holonomic_solvertest.py ├── p2p_holonomic_warehouse.py ├── p2p_quadrotor.py ├── p2p_trailer.py ├── platform_landing.py ├── questions.py ├── rendezvous_holonomic_export.py ├── revolving_door.py ├── revolving_door_diffdrive.py ├── revolving_door_quadrotor.py ├── ros_example │ ├── readme.md │ └── src │ │ ├── CMakeLists.txt │ │ ├── p3dx_description │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ └── rviz.launch │ │ ├── meshes │ │ │ ├── Coordinates │ │ │ ├── back_rim.stl │ │ │ ├── back_sonar.stl │ │ │ ├── center_hubcap.stl │ │ │ ├── center_wheel.stl │ │ │ ├── chassis.stl │ │ │ ├── front_rim.stl │ │ │ ├── front_sonar.stl │ │ │ ├── left_hubcap.stl │ │ │ ├── left_wheel.stl │ │ │ ├── lms100.dae │ │ │ ├── right_hubcap.stl │ │ │ ├── right_wheel.stl │ │ │ ├── swivel.stl │ │ │ └── top.stl │ │ ├── package.xml │ │ └── urdf │ │ │ ├── materials.xacro │ │ │ ├── pioneer3dx.gazebo │ │ │ ├── pioneer3dx.xacro │ │ │ ├── pioneer3dx.xml │ │ │ └── pioneer3dx_wheel.xacro │ │ ├── p3dx_gazebo │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ ├── gazebo.launch │ │ │ └── one_robot.launch │ │ ├── package.xml │ │ └── worlds │ │ │ └── p3dx.world │ │ └── p3dx_motionplanner │ │ ├── CMakeLists.txt │ │ ├── launch │ │ └── motionplanner.launch │ │ ├── msg │ │ ├── FleetTrajectories.msg │ │ ├── Obstacle.msg │ │ ├── P3DXPose.msg │ │ ├── P3DXTrajectory.msg │ │ ├── Room.msg │ │ ├── Settings.msg │ │ └── Trigger.msg │ │ ├── package.xml │ │ └── src │ │ ├── controller.py │ │ └── motionplanner.py ├── schedulerproblem_example1.py ├── schedulerproblem_example2.py ├── schedulerproblem_example_dubins.py ├── testAstar.py ├── test_multiframe.py └── tutorial_example.py ├── omgtools ├── __init__.py ├── basics │ ├── __init__.py │ ├── geometry.py │ ├── optilayer.py │ ├── shape.py │ ├── spline.py │ └── spline_extra.py ├── environment │ ├── __init__.py │ ├── environment.py │ ├── frame.py │ └── obstacle.py ├── execution │ ├── __init__.py │ ├── deployer.py │ ├── plotlayer.py │ └── simulator.py ├── export │ ├── __init__.py │ ├── export.py │ ├── export_admm.py │ ├── export_formation.py │ ├── export_p2p.py │ ├── export_rendezvous.py │ ├── point2point │ │ ├── Makefile │ │ ├── Point2Point.cpp │ │ ├── Point2Point.hpp │ │ ├── admm │ │ │ ├── ADMMPoint2Point.cpp │ │ │ ├── ADMMPoint2Point.hpp │ │ │ ├── formation │ │ │ │ ├── FormationPoint2Point.cpp │ │ │ │ └── FormationPoint2Point.hpp │ │ │ └── rendezvous │ │ │ │ ├── RendezVous.cpp │ │ │ │ └── RendezVous.hpp │ │ ├── example.cpp │ │ └── instructions.txt │ ├── tests │ │ ├── formation │ │ │ └── test.cpp │ │ ├── point2point │ │ │ └── test.cpp │ │ └── rendezvous │ │ │ └── test.cpp │ └── vehicles │ │ ├── Holonomic.cpp │ │ ├── Holonomic.hpp │ │ ├── Vehicle.cpp │ │ └── Vehicle.hpp ├── gui │ ├── __init__.py │ ├── gcode_block.py │ ├── gcode_reader.py │ ├── gui.py │ └── svg_reader.py ├── problems │ ├── __init__.py │ ├── admm.py │ ├── distributedproblem.py │ ├── dualdecomposition.py │ ├── dualmethod.py │ ├── formation.py │ ├── formation_central.py │ ├── formation_dualdec.py │ ├── gcodeproblem.py │ ├── gcodeschedulerproblem.py │ ├── globalplanner.py │ ├── multiframeproblem.py │ ├── point2point.py │ ├── problem.py │ ├── rendezvous.py │ └── schedulerproblem.py └── vehicles │ ├── __init__.py │ ├── agv.py │ ├── bicycle.py │ ├── dubins.py │ ├── fleet.py │ ├── holonomic.py │ ├── holonomic1d.py │ ├── holonomic3d.py │ ├── holonomicorient.py │ ├── quadrotor.py │ ├── quadrotor3d.py │ ├── quadrotor3d_simple.py │ ├── tool.py │ ├── trailer.py │ └── vehicle.py ├── readme.md ├── setup.py └── tests └── test_examples.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | *~ 3 | examples/movies/* 4 | examples/images/* 5 | examples/export* 6 | examples/data/* 7 | examples/ros_example/build/* 8 | examples/ros_example/devel/* 9 | movies/* 10 | images/* 11 | export* 12 | data/* 13 | lvd/* 14 | dronekit_example/* 15 | .* 16 | !.gitignore 17 | !.travis.yml 18 | *.c 19 | *.so 20 | *.csv 21 | *.p 22 | *.sh 23 | coverage.xml 24 | todo* 25 | build/* 26 | dist/* 27 | omg_tools.egg-info/* 28 | lvd/* 29 | *.sublime-project 30 | *.sublime-workspace 31 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | env: 2 | global: 3 | - COVERALLS_PARALLEL=1 4 | - NODE_TOTAL=4 5 | matrix: 6 | - NODE_INDEX=0 PY=2.7 CA=3.1.1.post3 7 | - NODE_INDEX=1 PY=2.7 CA=3.1.1.post3 8 | - NODE_INDEX=2 PY=2.7 CA=3.1.1.post3 9 | - NODE_INDEX=3 PY=2.7 CA=3.1.1.post3 PIP_EXTRA=matplotlib2tikz==0.6.18 10 | - NODE_INDEX=0 PY=3.7 11 | - NODE_INDEX=1 PY=3.7 12 | - NODE_INDEX=2 PY=3.7 13 | - NODE_INDEX=3 PY=3.7 14 | 15 | sudo: required 16 | dist: trusty 17 | language: generic 18 | 19 | notifications: 20 | webhooks: https://coveralls.io/webhook?repo_token=COVERALLS_REPO_TOKEN 21 | 22 | before_script: 23 | - set -e 24 | - set -o pipefail # otherwise, piping with grep discards exit statuses 25 | 26 | script: 27 | - nosetests --verbosity=2 --nocapture --with-xcoverage --cover-package=omgtools --cover-tests 28 | 29 | after_success: 30 | - coveralls 31 | 32 | install: 33 | - export DISPLAY=':99.0' 34 | - Xvfb :99 -screen 0 1024x768x24 > /dev/null 2>&1 & 35 | - pip install . 36 | 37 | before_install: 38 | - wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh -O miniconda.sh 39 | - chmod +x miniconda.sh 40 | - ./miniconda.sh -b 41 | - export PATH=$HOME/miniconda3/bin:$PATH 42 | - conda create --quiet --yes -n condaenv python=$PY matplotlib scipy nosexcover coveralls 43 | - source activate condaenv 44 | - if [ ! -z $CA ]; then pip install casadi==$CA; fi 45 | - if [ ! -z $PIP_EXTRA ]; then pip install $PIP_EXTRA; fi 46 | - sudo add-apt-repository main 47 | - sudo apt-get update 48 | - sudo apt-get install -y valgrind xvfb imagemagick 49 | -------------------------------------------------------------------------------- /doc/gifs/3dquadrotor.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/3dquadrotor.gif -------------------------------------------------------------------------------- /doc/gifs/ICRA_example1_minobs.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/ICRA_example1_minobs.gif -------------------------------------------------------------------------------- /doc/gifs/ICRA_example1_minobs_dubins.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/ICRA_example1_minobs_dubins.gif -------------------------------------------------------------------------------- /doc/gifs/ICRA_example1_shift.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/ICRA_example1_shift.gif -------------------------------------------------------------------------------- /doc/gifs/ICRA_example2_minobs.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/ICRA_example2_minobs.gif -------------------------------------------------------------------------------- /doc/gifs/agv.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/agv.gif -------------------------------------------------------------------------------- /doc/gifs/anchor2D.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/anchor2D.gif -------------------------------------------------------------------------------- /doc/gifs/bicycle.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/bicycle.gif -------------------------------------------------------------------------------- /doc/gifs/cannonballs.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/cannonballs.gif -------------------------------------------------------------------------------- /doc/gifs/formation_dubins.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/formation_dubins.gif -------------------------------------------------------------------------------- /doc/gifs/formation_quad_rotatingwall.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/formation_quad_rotatingwall.gif -------------------------------------------------------------------------------- /doc/gifs/formation_quad_ufo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/formation_quad_ufo.gif -------------------------------------------------------------------------------- /doc/gifs/holonomic_3d.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/holonomic_3d.gif -------------------------------------------------------------------------------- /doc/gifs/lvd_load.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/lvd_load.gif -------------------------------------------------------------------------------- /doc/gifs/lvd_unload.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/lvd_unload.gif -------------------------------------------------------------------------------- /doc/gifs/maze.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/maze.gif -------------------------------------------------------------------------------- /doc/gifs/multiproblem_circ.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/multiproblem_circ.gif -------------------------------------------------------------------------------- /doc/gifs/multiproblem_rect.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/multiproblem_rect.gif -------------------------------------------------------------------------------- /doc/gifs/narrowpassage.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/narrowpassage.gif -------------------------------------------------------------------------------- /doc/gifs/p2p_holonomic.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/p2p_holonomic.gif -------------------------------------------------------------------------------- /doc/gifs/p2p_holonomic_blocking.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/p2p_holonomic_blocking.gif -------------------------------------------------------------------------------- /doc/gifs/plate.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/plate.gif -------------------------------------------------------------------------------- /doc/gifs/platform_landing.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/platform_landing.gif -------------------------------------------------------------------------------- /doc/gifs/racetrack.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/racetrack.gif -------------------------------------------------------------------------------- /doc/gifs/revolving_door.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/revolving_door.gif -------------------------------------------------------------------------------- /doc/gifs/revolving_door_diffdrive.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/revolving_door_diffdrive.gif -------------------------------------------------------------------------------- /doc/gifs/trailer.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/trailer.gif -------------------------------------------------------------------------------- /doc/gifs/warehouse.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/doc/gifs/warehouse.gif -------------------------------------------------------------------------------- /examples/GCode_examples/anchor2D.nc: -------------------------------------------------------------------------------- 1 | %% ISO 6983 code of anchor 2 | G00 X0 Y0 3 | G02 X-20 Y20 I5 J25 4 | G01 X-23 Y17 5 | G01 Y28 6 | G01 X-14 Y26 7 | G01 X-17 Y23 8 | G03 X-3 Y9 I18 J4 9 | G01 Y35 10 | G01 X-13 11 | G01 Y40 12 | G01 X-3 13 | G01 Y45 14 | G02 X3 Y45 I3 J4 15 | G01 Y40 16 | G01 X13 17 | G01 Y35 18 | G01 X3 19 | G01 Y9 20 | G03 X17 Y23 I-4 J18 21 | G01 X14 Y26 22 | G01 X23 Y28 23 | G01 Y17 24 | G01 X20 Y20 25 | G02 X0 Y0 I-25 J5 26 | -------------------------------------------------------------------------------- /examples/GCode_examples/benchmark_stair.nc: -------------------------------------------------------------------------------- 1 | N100 G00 X-10 Y0 2 | N120 G01 X20 Y2 3 | N125 G01 X50 Y0 4 | N130 G03 Y10 I0 J5 5 | N140 G01 X20 Y12 6 | N145 G01 X-10 Y10 7 | N150 G01 Y20 8 | N160 G01 X20 Y22 9 | N165 G01 X50 Y20 10 | N170 G03 Y30 I0 J5 11 | N180 G01 X20 Y32 12 | N185 G01 X-10 Y30 13 | N190 G01 Y40 14 | N200 G01 X20 Y42 15 | N205 G01 X50 Y40 16 | N210 G03 Y50 I0 J5 17 | N220 G01 X20 Y52 18 | N225 G01 X-10 Y50 19 | N230 G01 Y60 20 | N240 G01 X20 Y62 21 | N245 G01 X50 Y60 22 | -------------------------------------------------------------------------------- /examples/GCode_examples/gcodeproblem_benchmark_stair.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | 21 | # This example computes a trajectory that a tool of e.g. a milling machine has to follow 22 | # to machine a part, described by GCode in a .nc file, with a certain tolerance. 23 | 24 | from omgtools import * 25 | 26 | # make GCode reader and run it to obtain an object-oriented description of the GCode 27 | reader = GCodeReader() 28 | # this opens a file dialog in which you can select your GCode as an .nc-file 29 | # the settings in this file are made specifically for the rsq5.nc file 30 | GCode = reader.run() 31 | 32 | n_blocks = 1 # amount of GCode blocks to combine 33 | variable_tolerance = False 34 | split_circle = True 35 | tol = 5e-1 # required tolerance of the machined part [mm] 36 | bounds = {'vmin':-0.16e3, 'vmax':0.16e3, 37 | 'amin':-2.16e3, 'amax':2.16e3, 38 | 'jmin':-31.8e3, 'jmax':31.8e3} # [mm] 39 | tool = Tool(tol, bounds=bounds, options={'vel_limit':'axes'}) # tool to follow the GCode 40 | tool.define_knots(knot_intervals=20) 41 | tool.set_initial_conditions(GCode[0].start) # start position of first GCode block 42 | tool.set_terminal_conditions(GCode[-1].end) # goal position of last GCode block 43 | 44 | # solve with a GCodeSchedulerProblem: this problem will combine n_blocks of GCode 45 | # and compute trajectories for the tool 46 | # each block will be converted to a room, that is put inside the total environment 47 | # there are two room shapes: Rectangle and Ring (circle segment with inner and outer diameter) 48 | # if you want to compute trajectories by using the deployer, put with_deployer=True 49 | schedulerproblem = GCodeSchedulerProblem(tool, GCode, n_segments=n_blocks, split_circle=split_circle, 50 | variable_tolerance=variable_tolerance,) 51 | 52 | schedulerproblem.set_options({'solver_options': {'ipopt': {'ipopt.tol': 1e-5, 53 | 'ipopt.linear_solver': 'ma57', 54 | 'ipopt.warm_start_bound_push': 1e-6, 55 | 'ipopt.warm_start_mult_bound_push': 1e-6, 56 | 'ipopt.warm_start_mult_bound_push': 1e-6, 57 | 'ipopt.mu_init': 1e-5, 58 | # 'ipopt.hessian_approximation': 'limited-memory', 59 | 'ipopt.max_iter': 20000}}})#, 60 | # put problem in deployer: choose this if you just want to obtain the trajectories for the tool 61 | deployer = Deployer(schedulerproblem, sample_time=0.001) 62 | 63 | # define what you want to plot 64 | schedulerproblem.plot('scene') 65 | tool.plot('state', knots=True, prediction=True, labels=['x (m)', 'y (m)', 'z (m)']) 66 | tool.plot('input', knots=True, prediction=True, labels=['v_x (m/s)', 'v_y (m/s)', 'v_z (m/s)']) 67 | tool.plot('dinput', knots=True, prediction=True, labels=['a_x (m/s^2)', 'a_y (m/s^2)', 'a_z (m/s^2)']) 68 | tool.plot('ddinput', knots=True, prediction=True, labels=['j_x (m/s^3)', 'j_y (m/s^3)', 'j_z (m/s^3)']) 69 | 70 | # run using a receding horizon of one segment 71 | deployer.update_segment() 72 | 73 | import pdb; pdb.set_trace() # breakpoint c78baa4a // 74 | # plotting and saving afterwards is only available when using the simulator 75 | # schedulerproblem.plot_movie('scene', number_of_frames=100, repeat=False) 76 | # schedulerproblem.save_movie('scene', format='gif', name='gcodegif', number_of_frames=100, movie_time=10, axis=False) 77 | # schedulerproblem.save_movie('scene', number_of_frames=50, name='gcodescheduler', axis=False) -------------------------------------------------------------------------------- /examples/GCode_examples/gcodeproblem_rsq5.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | 21 | # This example computes a trajectory that a tool of e.g. a milling machine has to follow 22 | # to machine a part, described by GCode in a .nc file, with a certain tolerance. 23 | 24 | from omgtools import * 25 | 26 | # make GCode reader and run it to obtain an object-oriented description of the GCode 27 | reader = GCodeReader() 28 | # this opens a file dialog in which you can select your GCode as an .nc-file 29 | # the settings in this file are made specifically for the rsq5.nc file 30 | GCode = reader.run() 31 | 32 | n_blocks = 3 # amount of GCode blocks to combine 33 | variable_tolerance = False 34 | split_circle = True 35 | tol = 6e-3 # required tolerance of the machined part [mm] 36 | bounds = {'vmin':-150, 'vmax':150, 37 | 'amin':-20e3, 'amax':20e3, 38 | 'jmin':-1500e3, 'jmax':1500e3} # [mm] 39 | tool = Tool(tol, bounds=bounds, options={'vel_limit':'axes'}) # tool to follow the GCode 40 | tool.define_knots(knot_intervals=10) 41 | tool.set_initial_conditions(GCode[0].start) # start position of first GCode block 42 | tool.set_terminal_conditions(GCode[-1].end) # goal position of last GCode block 43 | 44 | # solve with a GCodeSchedulerProblem: this problem will combine n_blocks of GCode 45 | # and compute trajectories for the tool 46 | # each block will be converted to a room, that is put inside the total environment 47 | # there are two room shapes: Rectangle and Ring (circle segment with inner and outer diameter) 48 | # if you want to compute trajectories by using the deployer, put with_deployer=True 49 | schedulerproblem = GCodeSchedulerProblem(tool, GCode, n_segments=n_blocks, split_circle=split_circle, 50 | variable_tolerance=variable_tolerance,) 51 | 52 | schedulerproblem.set_options({'solver_options': {'ipopt': {'ipopt.tol': 1e-5, 53 | 'ipopt.linear_solver': 'ma57', 54 | 'ipopt.warm_start_bound_push': 1e-6, 55 | 'ipopt.warm_start_mult_bound_push': 1e-6, 56 | 'ipopt.warm_start_mult_bound_push': 1e-6, 57 | 'ipopt.mu_init': 1e-5, 58 | # 'ipopt.hessian_approximation': 'limited-memory', 59 | 'ipopt.max_iter': 20000}}})#, 60 | # put problem in deployer: choose this if you just want to obtain the trajectories for the tool 61 | deployer = Deployer(schedulerproblem, sample_time=0.001) 62 | 63 | # define what you want to plot 64 | schedulerproblem.plot('scene') 65 | tool.plot('state', knots=True, prediction=True, labels=['x (m)', 'y (m)', 'z (m)']) 66 | tool.plot('input', knots=True, prediction=True, labels=['v_x (m/s)', 'v_y (m/s)', 'v_z (m/s)']) 67 | tool.plot('dinput', knots=True, prediction=True, labels=['a_x (m/s^2)', 'a_y (m/s^2)', 'a_z (m/s^2)']) 68 | tool.plot('ddinput', knots=True, prediction=True, labels=['j_x (m/s^3)', 'j_y (m/s^3)', 'j_z (m/s^3)']) 69 | 70 | # run using a receding horizon of one segment 71 | deployer.update_segment() 72 | 73 | # plotting and saving afterwards is only available when using the simulator 74 | # schedulerproblem.plot_movie('scene', number_of_frames=100, repeat=False) 75 | # schedulerproblem.save_movie('scene', format='gif', name='gcodegif', number_of_frames=100, movie_time=10, axis=False) 76 | # schedulerproblem.save_movie('scene', number_of_frames=50, name='gcodescheduler', axis=False) -------------------------------------------------------------------------------- /examples/GCode_examples/racetrack.nc: -------------------------------------------------------------------------------- 1 | G00 X250.1 Y-617.8 2 | G01 X31.8 Y-761.7 3 | G02 X4.9 Y-736.7 I-11.815248227 J14.384751773 4 | G01 X51.2 Y-636.1 5 | G01 X81.6 Y-585.3 6 | G01 X231.1 Y-432.9 7 | G03 X263.4 Y-380.9 I-48.8616161768 J66.3776223205 8 | G02 X357.9 Y-307.3 I88.4687023944 J-16.1234697863 9 | G03 X440.3 Y-274.6 I16.034 J79.54 10 | G01 X541.0 Y-203.0 11 | G01 X1042.0 Y-8.4 12 | G02 X1101.6 Y-18.2 I24.3331868696 J-38.147149242 13 | G03 X1173.3 Y-28.0 I41.7913415242 J38.5687946206 14 | G02 X1262.4 Y-43.6 I36.0185302074 J-56.5278178539 15 | G01 X1368.2 Y-172.4 16 | G02 X1306.8 Y-215.1 I-26.8716358839 J-26.8283641161 17 | G01 X1264.1 Y-151.6 18 | G03 X1216.6 Y-147.8 I-25.6623973727 J-22.0049671593 19 | G01 X923.1 Y-242.2 20 | G03 X885.7 Y-294.8 I18.7035789208 J-52.8943357779 21 | G03 X986.4 Y-440.2 I124.825 J-21.275 22 | G01 X1165.4 Y-491.1 23 | G02 X1198.8 Y-568.6 I-18.520396877 J-53.9288549122 24 | G03 X1225.8 Y-650.9 I58.3913838617 J-26.4225715156 25 | G01 X1324.1 Y-700.3 26 | G02 X1339.4 Y-766.8 I-14.2601284121 J-38.2909769129 27 | G01 X1305.7 Y-813.8 28 | G02 X1238.1 Y-849.8 I-59.4439081813 J30.1535609182 29 | G01 X1146.9 Y-820.5 30 | G01 X1059.1 Y-761.7 31 | G01 X907.3 Y-564.7 32 | G01 X745.4 Y-496.7 33 | G03 X676.2 Y-499.3 I-31.6738018789 J-79.1818884528 34 | G01 X544.1 Y-564.7 35 | G01 X356.7 Y-606.5 36 | G02 X340.2 Y-585.6 I-2.55 J14.95 37 | G03 X306.4 Y-573.9 I-17.7393939394 J3.66060606061 38 | G01 X250.1 Y-617.8 39 | -------------------------------------------------------------------------------- /examples/GCode_examples/rsq5.nc: -------------------------------------------------------------------------------- 1 | G00 X0 Y0 2 | G01 X20 Y0 3 | G03 X25 Y5 I0 J5 4 | G01 X25 Y45 5 | G03 X20 Y50 I-5 J0 6 | G01 X-20 Y50 7 | G03 X-25 Y45 I0 J-5 8 | G01 X-25 Y5 9 | G03 X-20 Y0 I5 J0 10 | G01 X0 Y0 -------------------------------------------------------------------------------- /examples/GCode_examples/rsq5_multi.nc: -------------------------------------------------------------------------------- 1 | G00 X0 Y0 2 | G01 X20 Y0 3 | G03 X25 Y5 I0 J5 4 | G01 X25 Y45 5 | G03 X20 Y50 I-5 J0 6 | G01 X-20 Y50 7 | G03 X-25 Y45 I0 J-5 8 | G01 X-25 Y5 9 | G03 X-20 Y0 I5 J0 10 | G01 X0 Y0 11 | G01 Z0.5 12 | G00 X0 Y0 13 | G01 X20 Y0 14 | G03 X25 Y5 I0 J5 15 | G01 X25 Y45 16 | G03 X20 Y50 I-5 J0 17 | G01 X-20 Y50 18 | G03 X-25 Y45 I0 J-5 19 | G01 X-25 Y5 20 | G03 X-20 Y0 I5 J0 21 | G01 X0 Y0 22 | G01 Z1 23 | G00 X0 Y0 24 | G01 X20 Y0 25 | G03 X25 Y5 I0 J5 26 | G01 X25 Y45 27 | G03 X20 Y50 I-5 J0 28 | G01 X-20 Y50 29 | G03 X-25 Y45 I0 J-5 30 | G01 X-25 Y5 31 | G03 X-20 Y0 I5 J0 32 | G01 X0 Y0 33 | G01 Z1.5 34 | G00 X0 Y0 35 | G01 X20 Y0 36 | G03 X25 Y5 I0 J5 37 | G01 X25 Y45 38 | G03 X20 Y50 I-5 J0 39 | G01 X-20 Y50 40 | G03 X-25 Y45 I0 J-5 41 | G01 X-25 Y5 42 | G03 X-20 Y0 I5 J0 43 | G01 X0 Y0 44 | G01 Z2 45 | G00 X0 Y0 46 | G01 X20 Y0 47 | G03 X25 Y5 I0 J5 48 | G01 X25 Y45 49 | G03 X20 Y50 I-5 J0 50 | G01 X-20 Y50 51 | G03 X-25 Y45 I0 J-5 52 | G01 X-25 Y5 53 | G03 X-20 Y0 I5 J0 54 | G01 X0 Y0 -------------------------------------------------------------------------------- /examples/annoying_obstacle.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create vehicle 23 | vehicle = Holonomic() 24 | vehicle.set_options({'safety_distance': 0.2, 'safety_weight': 1.e2}) 25 | 26 | vehicle.set_initial_conditions([-4., 0.]) 27 | vehicle.set_terminal_conditions([4., 0.]) 28 | 29 | # create environment 30 | environment = Environment(room={'shape': Rectangle(width=10., height=5.)}) 31 | rectangle = Rectangle(width=3., height=0.2) 32 | 33 | # we provide another simulation model than the model that is used to predict 34 | # the motion for the obstacle: the obstacle is moving sinusoidal, but our 35 | # optimization problem uses a linear prediction. 36 | wn = 2*np.pi/5. 37 | a1 = np.array([[0., 1., 0.], [0., 0., 0.], [0., 0., 0.]]) 38 | a2 = np.array([[0., 1., 0.], [-wn**2, 0., 0.], [0., 0., 0.]]) 39 | s1 = np.array([[1., 0.], [0., 0.]]) 40 | s2 = np.array([[0., 0.], [0., 1.]]) 41 | A = np.kron(a1, s1) + np.kron(a2, s2) 42 | 43 | simulation = {'model': {'A': A}} 44 | environment.add_obstacle(Obstacle({'position': [-3.5, -1.], 'velocity': [0.4, 0.]}, 45 | shape=Circle(0.4), simulation=simulation)) 46 | 47 | # create a point-to-point problem 48 | problem = Point2point(vehicle, environment, freeT=False) 49 | problem.init() 50 | 51 | # create simulator 52 | simulator = Simulator(problem) 53 | problem.plot('scene') 54 | vehicle.plot('input', knots=True, labels=['v_x (m/s)', 'v_y (m/s)']) 55 | 56 | # run it! 57 | simulator.run() 58 | -------------------------------------------------------------------------------- /examples/compare_buildoptions.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create vehicle 23 | vehicle = Holonomic() 24 | vehicle.set_initial_conditions([-1.5, -1.5]) 25 | vehicle.set_terminal_conditions([2., 2.]) 26 | 27 | # create fleet 28 | N = 4 29 | fleet = Fleet([Holonomic() for l in range(N)]) 30 | configuration = RegularPolyhedron(0.2, N, np.pi/4.).vertices.T 31 | init_positions = [-1.5, -1.5] + configuration 32 | terminal_positions = [2., 2.] + configuration 33 | fleet.set_configuration(configuration.tolist()) 34 | fleet.set_initial_conditions(init_positions.tolist()) 35 | fleet.set_terminal_conditions(terminal_positions.tolist()) 36 | 37 | # create environment 38 | environment = Environment(room={'shape': Square(5.)}) 39 | rectangle = Rectangle(width=3., height=0.2) 40 | 41 | environment.add_obstacle(Obstacle({'position': [-2.1, -0.5]}, shape=rectangle)) 42 | environment.add_obstacle(Obstacle({'position': [1.7, -0.5]}, shape=rectangle)) 43 | 44 | problem1 = Point2point(vehicle, environment, freeT=False) 45 | problem1.set_options({'verbose': 1}) 46 | 47 | problem2 = Point2point(vehicle, environment, freeT=False) 48 | problem2.set_options({'verbose': 1}) 49 | problem2.set_options({'codegen': {'build': 'jit', 'flags': '-O0'}}) # just-in-time compilation 50 | 51 | problem3 = Point2point(vehicle, environment, freeT=False) 52 | problem3.set_options({'verbose': 1}) 53 | problem3.set_options({'codegen': {'build': 'shared', 'flags': '-O0'}}) # compile to shared object 54 | 55 | problem4 = Point2point(vehicle, environment, freeT=False) 56 | problem4.set_options({'verbose': 1}) 57 | problem4.set_options({'codegen': {'build': 'existing'}}) # use existing shared object 58 | 59 | vehicle.plot('input', knots=True) 60 | 61 | print ('No compilation single-vehicle') 62 | problem1.init() 63 | simulator = Simulator(problem1) 64 | simulator.run() 65 | 66 | print('\n') 67 | print('Just-in-time compilation single-vehicle') 68 | problem2.init() 69 | vehicle.overrule_state([-1.5, -1.5]) 70 | vehicle.overrule_input([0., 0.]) 71 | simulator.set_problem(problem2) 72 | simulator.run() 73 | 74 | print('\n') 75 | print('Compile to shared object single-vehicle') 76 | problem3.init() 77 | vehicle.overrule_state([-1.5, -1.5]) 78 | vehicle.overrule_input([0., 0.]) 79 | simulator.set_problem(problem3) 80 | simulator.run() 81 | 82 | print('\n') 83 | print('Use previous shared object single-vehicle') 84 | problem4.init() 85 | vehicle.overrule_state(np.array([-1.5, -1.5])) 86 | vehicle.overrule_input(np.zeros(2)) 87 | simulator.set_problem(problem4) 88 | simulator.run() 89 | -------------------------------------------------------------------------------- /examples/compare_buildoptions_distributed.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create fleet 23 | N = 4 24 | fleet = Fleet([Holonomic() for l in range(N)]) 25 | configuration = RegularPolyhedron(0.2, N, np.pi/4.).vertices.T 26 | init_positions = [-1.5, -1.5] + configuration 27 | terminal_positions = [2., 2.] + configuration 28 | fleet.set_configuration(configuration.tolist()) 29 | fleet.set_initial_conditions(init_positions.tolist()) 30 | fleet.set_terminal_conditions(terminal_positions.tolist()) 31 | 32 | # create environment 33 | environment = Environment(room={'shape': Square(5.)}) 34 | rectangle = Rectangle(width=3., height=0.2) 35 | 36 | environment.add_obstacle(Obstacle({'position': [-2.1, -0.5]}, shape=rectangle)) 37 | environment.add_obstacle(Obstacle({'position': [1.7, -0.5]}, shape=rectangle)) 38 | 39 | problem1 = FormationPoint2point(fleet, environment) 40 | problem1.set_options({'rho': 2., 'verbose': 1}) 41 | 42 | problem2 = FormationPoint2point(fleet, environment) 43 | problem2.set_options({'rho': 2., 'verbose': 1}) 44 | problem2.set_options({'codegen': {'build': 'jit', 'flags': '-O0'}}) # just-in-time compilation 45 | 46 | problem3 = FormationPoint2point(fleet, environment) 47 | problem3.set_options({'rho': 2., 'verbose': 1}) 48 | problem3.set_options({'codegen': {'build': 'shared', 'flags': '-O0'}}) # compile to shared objects 49 | 50 | problem4 = FormationPoint2point(fleet, environment) 51 | problem4.set_options({'rho': 2., 'verbose': 1}) 52 | problem4.set_options({'codegen': {'build': 'existing'}}) # use existing shared objects 53 | 54 | fleet.plot('input', knots=True) 55 | 56 | print ('No compilation multi-vehicle') 57 | problem1.init() 58 | simulator = Simulator(problem1) 59 | simulator.run() 60 | 61 | print('\n') 62 | print('Just-in-time compilation multi-vehicle') 63 | problem2.init() 64 | fleet.overrule_state(init_positions.tolist()) 65 | fleet.overrule_input([np.zeros(2) for n in range(N)]) 66 | problem2.reinitialize() 67 | simulator.set_problem(problem2) 68 | simulator.run() 69 | 70 | print('\n') 71 | print('Compile to shared object multi-vehicle') 72 | problem3.init() 73 | fleet.overrule_state(init_positions.tolist()) 74 | fleet.overrule_input([np.zeros(2) for n in range(N)]) 75 | problem3.reinitialize() 76 | simulator.set_problem(problem3) 77 | simulator.run() 78 | 79 | print('\n') 80 | print('Use previous shared object multi-vehicle') 81 | problem4.init() 82 | fleet.overrule_state(init_positions.tolist()) 83 | fleet.overrule_input([np.zeros(2) for n in range(N)]) 84 | problem4.reinitialize() 85 | simulator.set_problem(problem4) 86 | simulator.run() 87 | -------------------------------------------------------------------------------- /examples/compare_distributed_vs_central_quadrotors.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | fleet_sizes = [3, 5, 7, 9, 11] 23 | rho = 0.04 24 | 25 | obj_admm = {} 26 | t_it_admm = {} 27 | t_b_admm = {} 28 | obj_central = {} 29 | t_it_central = {} 30 | t_b_central = {} 31 | form_err = {} 32 | 33 | # distributed problem 34 | for N in fleet_sizes: 35 | fleet = Fleet([Quadrotor(0.2) for k in range(N)]) 36 | for quad in fleet.vehicles: 37 | quad.set_options({'safety_distance': 0.1, 'safety_weight': 1.}) 38 | configuration = RegularPolyhedron(0.4, N, orientation=np.pi/2).vertices.T 39 | if N == 2: 40 | configuration = np.array([[-0.4, 0.], [0.4, 0.]]) 41 | init_positions = [-4., -4.] + configuration 42 | terminal_positions = [4., 4.] + configuration 43 | fleet.set_configuration(configuration.tolist()) 44 | fleet.set_initial_conditions(init_positions.tolist()) 45 | fleet.set_terminal_conditions(terminal_positions.tolist()) 46 | environment = Environment(room={'shape': Square(9.3)}) 47 | environment.add_obstacle(Obstacle({'position': [0., 3.7]}, 48 | shape=Rectangle(width=0.2, height=3.))) 49 | environment.add_obstacle(Obstacle({'position': [0., -5.4]}, 50 | shape=Rectangle(width=0.2, height=10.))) 51 | options = {'rho': rho, 'horizon_time': 5.} 52 | problem_admm = FormationPoint2point(fleet, environment, options=options) 53 | t_b_admm[N] = problem_admm.init() 54 | simulator = Simulator(problem_admm) 55 | simulator.run() 56 | obj_admm[N] = problem_admm.compute_objective() 57 | t_it_admm[N] = sum(problem_admm.update_times)*1000. / \ 58 | len(problem_admm.update_times) 59 | form_err[N] = problem_admm.get_interaction_error() 60 | 61 | # central problem 62 | for N in fleet_sizes: 63 | fleet = Fleet([Quadrotor(0.2) for k in range(N)]) 64 | for quad in fleet.vehicles: 65 | quad.set_options({'safety_distance': 0.1, 'safety_weight': 1.}) 66 | configuration = RegularPolyhedron(0.4, N, orientation=np.pi/2).vertices.T 67 | if N == 2: 68 | configuration = np.array([[-0.4, 0.], [0.4, 0.]]) 69 | init_positions = [-4., -4.] + configuration 70 | terminal_positions = [4., 4.] + configuration 71 | environment = Environment(room={'shape': Square(9.3)}) 72 | environment.add_obstacle(Obstacle({'position': [0., 3.7]}, 73 | shape=Rectangle(width=0.2, height=3.))) 74 | environment.add_obstacle(Obstacle({'position': [0., -5.4]}, 75 | shape=Rectangle(width=0.2, height=10.))) 76 | fleet.set_configuration(configuration.tolist()) 77 | fleet.set_initial_conditions(init_positions.tolist()) 78 | fleet.set_terminal_conditions(terminal_positions.tolist()) 79 | options = {'horizon_time': 5.} 80 | problem_central = FormationPoint2pointCentral( 81 | fleet, environment, options=options) 82 | t_b_central[N] = problem_central.init() 83 | simulator = Simulator(problem_central) 84 | simulator.run() 85 | obj_central[N] = problem_central.compute_objective() 86 | t_it_central[N] = sum(problem_central.update_times) * \ 87 | 1000./len(problem_central.update_times) 88 | 89 | data = {'obj_admm': obj_admm, 'obj_central': obj_central, 't_it_admm': t_it_admm, 't_it_central': 90 | t_it_central, 't_b_admm': t_b_admm, 't_b_central': t_b_central, 'form_err': form_err} 91 | -------------------------------------------------------------------------------- /examples/deployer_example.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | import time 22 | import matplotlib.pyplot as plt 23 | import numpy as np 24 | 25 | # this file demonstrates how to use the deployer in a motion planning application 26 | 27 | # create vehicle 28 | vehicle = Holonomic() 29 | vehicle.set_options({'safety_distance': 0.1}) 30 | vehicle.set_options({'ideal_prediction': False}) 31 | vehicle.set_initial_conditions([0., 0.]) # dummy: required for problem.init() 32 | vehicle.set_terminal_conditions([0., 0.]) # dummy: required for problem.init() 33 | 34 | # create environment 35 | environment = Environment(room={'shape': Square(5.)}) 36 | rectangle = Rectangle(width=3., height=0.2) 37 | 38 | obstacle = Obstacle({'position': [0., 0.]}, shape=Circle(0.2)) 39 | environment.add_obstacle(obstacle) 40 | 41 | # create a point-to-point problem 42 | problem = Point2point(vehicle, environment, freeT=False) 43 | problem.init() 44 | 45 | # create deployer 46 | update_time = 0.1 47 | sample_time = 0.01 48 | deployer = Deployer(problem, sample_time, update_time) 49 | 50 | # simulation of a motion planning application: go through 3 via points, while 51 | # an obstacle is changing position 52 | via_points = [[2., -1.5], [2., 2.], [-1.5, 2.]] 53 | obstacle_positions = [[0., 0.], [1.5, 0.], [1., 2.]] 54 | 55 | current_time = 0 56 | current_state = [-1.5, -1.5] 57 | state_traj = np.c_[current_state] 58 | input_traj = np.c_[[0.0, 0.0]] 59 | 60 | n_samp = int(np.round(update_time/sample_time, 6)) 61 | t00 = time.time() 62 | 63 | for via_point, obstacle_pos in zip(via_points, obstacle_positions): 64 | vehicle.set_terminal_conditions(via_point) 65 | target_reached = False 66 | obstacle.set_state({'position': obstacle_pos}) 67 | vehicle.set_initial_conditions(via_point) # for init guess 68 | deployer.reset() # let's start from new initial guess 69 | while not target_reached: 70 | t0 = time.time() - t00 71 | if (t0-current_time-update_time) >= 0.: 72 | current_time = t0 73 | # 'measure' current state (here ideal trajectory following is simulated) 74 | if state_traj.shape[1] > 1: 75 | current_state = state_traj[:, -n_samp-1] 76 | else: 77 | current_state = state_traj[:, 0] 78 | # update motion planning 79 | trajectories = deployer.update(current_time, current_state) 80 | # store state & input trajectories -> simulation of ideal trajectory following 81 | state_traj = np.c_[state_traj, trajectories['state'][:, 1:n_samp+1]] 82 | input_traj = np.c_[input_traj, trajectories['input'][:, 1:n_samp+1]] 83 | # check target 84 | if (np.linalg.norm(via_point-state_traj[:, -1]) < 1e-2 and np.linalg.norm(input_traj[:, -1]) < 1e-2): 85 | target_reached = True 86 | if (problem.iteration > 300): 87 | target_reached = True 88 | 89 | # plot results 90 | n_t = state_traj.shape[1] 91 | time = np.linspace(0., n_t*sample_time, n_t) 92 | 93 | plt.figure() 94 | plt.subplot(2, 1, 1) 95 | plt.plot(time, state_traj[0, :]) 96 | plt.subplot(2, 1, 2) 97 | plt.plot(time, state_traj[1, :]) 98 | 99 | plt.figure() 100 | plt.subplot(2, 1, 1) 101 | plt.plot(time, input_traj[0, :]) 102 | plt.subplot(2, 1, 2) 103 | plt.plot(time, input_traj[1, :]) 104 | 105 | plt.figure() 106 | plt.plot(state_traj[0, :], state_traj[1, :]) -------------------------------------------------------------------------------- /examples/environment.pickle: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/examples/environment.pickle -------------------------------------------------------------------------------- /examples/formation_dubins.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | assert_ma57() 23 | 24 | # create fleet 25 | N = 3 26 | vehicles = [Dubins(options={'degree': 2}, bounds={'vmax': 1., 'wmax': np.pi/6., 'wmin': -np.pi/6.}) for l in range(N)] 27 | for vehicle in vehicles: 28 | vehicle.define_knots(knot_intervals=10) 29 | 30 | fleet = Fleet(vehicles) 31 | configuration = RegularPolyhedron(0.2, N, np.pi).vertices.T 32 | init_positions = [-0.5, -1.5] + configuration 33 | terminal_positions = [0.5, 1.5] + configuration 34 | # init_positions = [-0.7, -1.5] + configuration 35 | # terminal_positions = [0.7, 1.5] + configuration 36 | init_pose = np.c_[init_positions, (np.pi/2.)*np.ones(N)] 37 | terminal_pose = np.c_[terminal_positions, (np.pi/2.)*np.ones(N)] 38 | 39 | fleet.set_configuration(configuration.tolist(), orientation=np.pi/2.) 40 | fleet.set_initial_conditions(init_pose.tolist()) 41 | fleet.set_terminal_conditions(terminal_pose.tolist()) 42 | 43 | # create environment 44 | environment = Environment(room={'shape': Square(4.)}) 45 | beam0 = Beam(width=3., height=0.2, orientation=np.pi/2) 46 | environment.add_obstacle(Obstacle({'position': [0., -2.2]}, shape=beam0)) 47 | environment.add_obstacle(Obstacle({'position': [0., 2.2]}, shape=beam0)) 48 | # beam1 = Beam(width=2.6, height=0.6, orientation=np.pi/2) 49 | # environment.add_obstacle(Obstacle({'position': [0., -2.2]}, shape=beam1)) 50 | # environment.add_obstacle(Obstacle({'position': [0., 2.2]}, shape=beam1)) 51 | 52 | # create a formation point-to-point problem 53 | options = {'rho': 3., 'horizon_time': 5., 'hard_term_con': True} 54 | # options = {'rho': 5., 'horizon_time': 5., 'hard_term_con': True} 55 | problem = FormationPoint2point(fleet, environment, options=options) 56 | problem.set_options({'solver_options': {'ipopt': {'ipopt.linear_solver': 'ma57'}}}) # hsl solvers required for this!! 57 | problem.init() 58 | 59 | # create simulator 60 | simulator = Simulator(problem) 61 | problem.plot('scene') 62 | fleet.plot('input', knots=True, labels=['v (m/s)', 'w (rad/s)']) 63 | 64 | # run it! 65 | simulator.run() 66 | -------------------------------------------------------------------------------- /examples/formation_holonomic.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create fleet 23 | N = 4 24 | vehicles = [Holonomic() for l in range(N)] 25 | 26 | fleet = Fleet(vehicles) 27 | configuration = RegularPolyhedron(0.2, N, np.pi/4.).vertices.T 28 | init_positions = [-1.5, -1.5] + configuration 29 | terminal_positions = [2., 2.] + configuration 30 | 31 | fleet.set_configuration(configuration.tolist()) 32 | fleet.set_initial_conditions(init_positions.tolist()) 33 | fleet.set_terminal_conditions(terminal_positions.tolist()) 34 | 35 | # create environment 36 | environment = Environment(room={'shape': Square(5.)}) 37 | rectangle = Rectangle(width=3., height=0.2) 38 | environment.add_obstacle(Obstacle({'position': [-2.1, -0.5]}, shape=rectangle)) 39 | environment.add_obstacle(Obstacle({'position': [1.7, -0.5]}, shape=rectangle)) 40 | trajectories = {'velocity': {'time': [3., 4.], 41 | 'values': [[-0.15, 0.0], [0., 0.15]]}} 42 | environment.add_obstacle(Obstacle({'position': [1.5, 0.5]}, shape=Circle(0.4), 43 | simulation={'trajectories': trajectories})) 44 | 45 | # create a formation point-to-point problem 46 | options = {'rho': 1., 'horizon_time': 10} 47 | problem = FormationPoint2point(fleet, environment, options=options) 48 | problem.init() 49 | 50 | # create simulator 51 | simulator = Simulator(problem) 52 | fleet.plot('input', knots=True, predict=True, labels=['v_x (m/s)', 'v_y (m/s)']) 53 | problem.plot('scene') 54 | problem.plot('residuals') 55 | 56 | # run it! 57 | simulator.run() 58 | -------------------------------------------------------------------------------- /examples/formation_holonomic_central.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | from casadi import inf 22 | 23 | # create fleet 24 | N = 4 25 | vehicles = [Holonomic() for l in range(N)] 26 | 27 | for k, vehicle in enumerate(vehicles): 28 | vehicle.set_initial_conditions([-1. -0.5*N*0.5 + 0.5*k, -1.5]) 29 | 30 | fleet = Fleet(vehicles) 31 | configuration = RegularPolyhedron(0.2, N, np.pi/4.).vertices.T 32 | terminal_positions = [2., 2.] + configuration 33 | 34 | fleet.set_configuration(configuration.tolist()) 35 | fleet.set_terminal_conditions(terminal_positions.tolist()) 36 | 37 | # create environment 38 | environment = Environment(room={'shape': Square(5.)}) 39 | rectangle = Rectangle(width=3., height=0.2) 40 | environment.add_obstacle(Obstacle({'position': [-1.8, 0.5]}, shape=rectangle)) 41 | environment.add_obstacle(Obstacle({'position': [1.7, 0.5]}, shape=rectangle)) 42 | 43 | # create a formation point-to-point problem 44 | options = {'rho': 1., 'horizon_time': 15} 45 | problem = FormationPoint2pointCentral(fleet, environment, options=options) 46 | problem.set_options({'soft_formation': True}) 47 | problem.set_options({'soft_formation_weight': 100}) 48 | problem.set_options({'max_formation_deviation': inf}) 49 | problem.set_options({'inter_vehicle_avoidance': True}) 50 | problem.init() 51 | 52 | 53 | # create simulator 54 | simulator = Simulator(problem) 55 | fleet.plot('input', knots=True, predict=True, labels=['v_x (m/s)', 'v_y (m/s)']) 56 | problem.plot('scene') 57 | 58 | # run it! 59 | simulator.run() 60 | -------------------------------------------------------------------------------- /examples/formation_holonomic_export.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | import os 22 | import csv 23 | 24 | """ 25 | This file demonstrates how to export a point2point problem to c++. It generates 26 | some source and header files which can be compiled with your own source code or 27 | which can be compiled to a shared library and included in your own project. 28 | """ 29 | 30 | # create fleet 31 | N = 4 32 | options = {'room_constraints': None} 33 | vehicles = [Holonomic(shapes=Circle(0.1), options=options) for l in range(N)] 34 | 35 | fleet = Fleet(vehicles) 36 | configuration = RegularPolyhedron(0.2, N, np.pi/4.).vertices.T 37 | init_positions = [0.0, 0.0] + configuration 38 | terminal_positions = [3.5, 3.5] + configuration 39 | 40 | fleet.set_configuration(configuration.tolist()) 41 | fleet.set_initial_conditions(init_positions.tolist()) 42 | fleet.set_terminal_conditions(terminal_positions.tolist()) 43 | 44 | # create environment 45 | environment = Environment(room={'shape': Square(5.), 'position': [1.5, 1.5]}) 46 | rectangle = Rectangle(width=3., height=0.2) 47 | 48 | environment.add_obstacle(Obstacle({'position': [-0.6, 1.0]}, shape=rectangle)) 49 | environment.add_obstacle(Obstacle({'position': [3.2, 1.0]}, shape=rectangle)) 50 | 51 | # create a formation point-to-point problem 52 | options = {'rho': 2., 'horizon_time': 10, 'init_iter': 5} 53 | problem = FormationPoint2point(fleet, environment, options=options) 54 | problem.init() 55 | 56 | options = {} 57 | options['directory'] = os.path.join(os.getcwd(), 'export_f/') 58 | # path to object files of your exported optimization problem 59 | options['casadiobj'] = os.path.join(options['directory'], 'bin/') 60 | options['namespace'] = 'omgf' 61 | 62 | # export the problem 63 | problem.export(options) 64 | simulator = Simulator(problem) 65 | trajectories, signals = simulator.run() 66 | 67 | # save results for check in c++ 68 | testdir = os.path.join(options['directory'], 'test') 69 | if not os.path.isdir(testdir): 70 | os.makedirs(os.path.join(options['directory'], 'test')) 71 | jump = int(simulator.update_time/simulator.sample_time) 72 | size = len(trajectories[vehicles[0].label]['state']) 73 | with open(os.path.join(testdir, 'data_state.csv'), 'w') as f: 74 | w = csv.writer(f) 75 | for i in range(0, size, jump): 76 | for vehicle in vehicles: 77 | for k in range(trajectories[vehicle.label]['state'][i].shape[0]): 78 | w.writerow(trajectories[vehicle.label]['state'][i][k, :]) 79 | with open(os.path.join(testdir, 'data_input.csv'), 'w') as f: 80 | w = csv.writer(f) 81 | for i in range(0, size, jump): 82 | for vehicle in vehicles: 83 | for k in range(trajectories[vehicle.label]['input'][i].shape[0]): 84 | w.writerow(trajectories[vehicle.label]['input'][i][k, :]) 85 | 86 | 87 | # note: you need to implement your vehicle type in c++. Take a look at 88 | # Holonomic.cpp and Holonomic.hpp which are also exported as an example. 89 | -------------------------------------------------------------------------------- /examples/formation_holonomic_multiproblem.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create fleet 23 | N = 4 24 | vehicles = [Holonomic() for l in range(N)] 25 | 26 | fleet = Fleet(vehicles) 27 | configuration = RegularPolyhedron(0.2, N, np.pi/4.).vertices.T 28 | init_positions = [-1.5, -1.5] + configuration 29 | terminal_positions = [2., 2.] + configuration 30 | 31 | fleet.set_configuration(configuration.tolist()) 32 | fleet.set_initial_conditions(init_positions.tolist()) 33 | fleet.set_terminal_conditions(terminal_positions.tolist()) 34 | 35 | # create environment 36 | environment = Environment(room={'shape': Square(5.)}) 37 | rectangle = Rectangle(width=3., height=0.2) 38 | 39 | obstacle1 = Obstacle({'position': [-2.1, -0.5]}, shape=rectangle) 40 | obstacle2 = Obstacle({'position': [1.7, -0.5]}, shape=rectangle) 41 | trajectories = {'velocity': {'time': [3., 4.], 42 | 'values': [[-0.15, 0.0], [0., 0.15]]}} 43 | obstacle3 = Obstacle({'position': [1.5, 0.5]}, shape=Circle(0.4), 44 | simulation={'trajectories': trajectories}) 45 | 46 | environment.add_obstacle([obstacle1, obstacle2, obstacle3]) 47 | 48 | # create problems 49 | options = {'rho': 1., 'horizon_time': 10} 50 | problem1 = FormationPoint2point(fleet, environment, options=options) 51 | problem1.init() 52 | obstacle3.set_options({'avoid': False}) 53 | problem2 = FormationPoint2point(fleet, environment, options=options) 54 | problem2.init() 55 | 56 | # create simulator & plots 57 | simulator = Simulator(problem1) 58 | fleet.plot('input', knots=True, prediction=True, labels=['v_x (m/s)', 'v_y (m/s)']) 59 | problem1.plot('scene') 60 | # 1st task 61 | simulator.run() 62 | # 2nd task 63 | fleet.set_terminal_conditions(([2., 0.0]+configuration).tolist()) 64 | simulator.run() 65 | # 3th task (we can neglect circular obstacle), but first sleep 2 seconds 66 | simulator.sleep(2.) 67 | simulator.set_problem(problem2) 68 | fleet.set_terminal_conditions(([0.0, 1.0]+configuration).tolist()) 69 | simulator.run() 70 | 71 | # plot movie 72 | # problem2.plot_movie('scene', number_of_frames=100, repeat=False) 73 | -------------------------------------------------------------------------------- /examples/formation_quadrotor.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | import numpy as np 22 | 23 | # create fleet 24 | N = 3 25 | vehicles = [Quadrotor(0.2) for l in range(N)] 26 | fleet = Fleet(vehicles) 27 | for quad in fleet.vehicles: 28 | quad.set_options({'safety_distance': 0.1, 'safety_weight': 1.}) 29 | configuration = RegularPolyhedron(0.4, N, orientation=np.pi/2).vertices.T 30 | init_positions = [-4., -4.] + configuration 31 | terminal_positions = [4., 4.] + configuration 32 | 33 | fleet.set_configuration(configuration.tolist()) 34 | fleet.set_initial_conditions([np.r_[pos, np.zeros(3)] for pos in init_positions.tolist()]) 35 | fleet.set_terminal_conditions(terminal_positions.tolist()) 36 | 37 | # create environment 38 | environment = Environment(room={'shape': Square(9.3)}) 39 | environment.add_obstacle(Obstacle({'position': [0., 3.7]}, 40 | shape=Rectangle(width=0.2, height=3.))) 41 | environment.add_obstacle(Obstacle({'position': [0., -5.4]}, 42 | shape=Rectangle(width=0.2, height=10.))) 43 | trajectory = {'velocity': {'time': [0.8], 'values': [[-5., 0.]]}} 44 | environment.add_obstacle( 45 | Obstacle({'position': [6., 0.8]}, UFO(1.5, 0.6), {'trajectories': trajectory})) 46 | 47 | # create a formation point-to-point problem 48 | options = {'horizon_time': 5., 'rho': 0.04} 49 | problem = FormationPoint2point(fleet, environment, options=options) 50 | problem.init() 51 | 52 | # create simulator 53 | simulator = Simulator(problem) 54 | problem.plot('scene') 55 | problem.plot('residuals') 56 | fleet.plot('input', knots=True, labels=['Thrust force (N/kg)', 57 | 'Pitch rate (rad/s)']) 58 | 59 | # run it! 60 | simulator.run() 61 | -------------------------------------------------------------------------------- /examples/formation_quadrotor_rotating_wall.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | import numpy as np 22 | 23 | # create fleet 24 | N = 4 25 | vehicles = [Quadrotor(0.2) for l in range(N)] 26 | 27 | fleet = Fleet(vehicles) 28 | configuration = RegularPolyhedron(0.5, N, orientation=np.pi).vertices.T 29 | init_positions = [-4., -5.] + configuration 30 | terminal_positions = [4., 5.] + configuration 31 | 32 | fleet.set_configuration(configuration.tolist()) 33 | fleet.set_initial_conditions([np.r_[pos, np.zeros(3)] for pos in init_positions.tolist()]) 34 | fleet.set_terminal_conditions(terminal_positions.tolist()) 35 | 36 | # create environment 37 | environment = Environment(room={'shape': Square(12.)}) 38 | beam1 = Beam(width=4., height=0.2) 39 | environment.add_obstacle(Obstacle({'position': [-4., 0.]}, shape=beam1)) 40 | environment.add_obstacle(Obstacle({'position': [4., 0.]}, shape=beam1)) 41 | 42 | beam2 = Beam(width=3., height=0.2) 43 | horizon_time = 5. 44 | omega = 0.2*(2*np.pi/horizon_time) 45 | environment.add_obstacle(Obstacle({'position': [0., 0.], 'angular_velocity': omega}, 46 | shape=beam2, simulation={}, options={'horizon_time': horizon_time})) 47 | 48 | # create a formation point-to-point problem 49 | options = {'horizon_time': horizon_time, 'rho': 0.3} 50 | problem = FormationPoint2point(fleet, environment, options=options) 51 | problem.init() 52 | 53 | # create simulator 54 | simulator = Simulator(problem) 55 | problem.plot('scene') 56 | fleet.plot('input', knots=True, labels=['Thrust force (N/kg)', 57 | 'Pitch rate (rad/s)']) 58 | 59 | # run it! 60 | simulator.run() 61 | -------------------------------------------------------------------------------- /examples/gui_examples/svg/drawing.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 19 | 21 | 39 | 41 | 42 | 44 | image/svg+xml 45 | 47 | 48 | 49 | 50 | 51 | 55 | 61 | 67 | 74 | 81 | 88 | 89 | 90 | -------------------------------------------------------------------------------- /examples/gui_examples/vast_environment_example1.pickle: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/examples/gui_examples/vast_environment_example1.pickle -------------------------------------------------------------------------------- /examples/gui_examples/vast_environment_example2.pickle: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/examples/gui_examples/vast_environment_example2.pickle -------------------------------------------------------------------------------- /examples/gui_examples/vast_environment_example_maze.pickle: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/examples/gui_examples/vast_environment_example_maze.pickle -------------------------------------------------------------------------------- /examples/p2p_3dquadrotor.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | import numpy as np 22 | 23 | # create vehicle 24 | vehicle = Quadrotor3D(0.5) 25 | 26 | vehicle.set_initial_conditions([-3, -2, -0.5, 0, 0, 0, 0, 0]) 27 | vehicle.set_terminal_conditions([3, 2, 0.5]) 28 | 29 | vehicle.set_options({'safety_distance': 0.1, 'safety_weight': 10}) 30 | 31 | # create environment 32 | environment = Environment(room={'shape': Cuboid(8, 6, 8)}) 33 | 34 | trajectory = {'velocity': {'time': [1.5], 'values': [[0, 0, -0.6]]}} 35 | obst1 = Obstacle({'position': [-2, 0, -2]}, shape=Plate(Rectangle(5., 8.), 0.1, 36 | orientation=[0., np.pi/2, 0.]), options={'draw': True}) 37 | obst2 = Obstacle({'position': [2, 0, 3.5]}, shape=Plate(Rectangle(5., 8.), 0.1, 38 | orientation=[0., np.pi/2, 0.]), 39 | simulation={'trajectories': trajectory}, options={'draw': True}) 40 | 41 | environment.add_obstacle([obst1, obst2]) 42 | 43 | # create a point-to-point problem 44 | problem = Point2point(vehicle, environment, freeT=False, options={'horizon_time': 5.}) 45 | problem.init() 46 | 47 | vehicle.problem = problem 48 | # create simulator 49 | simulator = Simulator(problem, sample_time=0.01, update_time=0.4) 50 | vehicle.plot('input', knots=True) 51 | problem.plot('scene', view=[20, -80]) 52 | 53 | # run it! 54 | simulator.run() 55 | -------------------------------------------------------------------------------- /examples/p2p_agv.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | assert_ma57() 23 | 24 | # create vehicle 25 | vehicle = AGV(length=0.8, options={'plot_type': 'agv'}) 26 | # vehicle.set_options({'safety_distance': 0.3}) 27 | vehicle.define_knots(knot_intervals=5) # choose lower amount of knot intervals 28 | 29 | vehicle.set_initial_conditions([0.8, -0.05, 0., 0.]) # x, y, theta, delta 30 | vehicle.set_terminal_conditions([2.45, -0.35, 0.]) # x, y, theta 31 | 32 | # create environment 33 | environment = Environment(room={'shape': Rectangle(width=4, height=1), 'position': [2, 0.]}) 34 | rectangle = Rectangle(width=0.8, height=0.2) 35 | environment.add_obstacle(Obstacle({'position': [1., -0.35]}, shape=rectangle)) 36 | environment.add_obstacle(Obstacle({'position': [3.4, -0.35]}, shape=rectangle)) 37 | 38 | # create a point-to-point problem 39 | problem = Point2point(vehicle, environment, freeT=True) 40 | # extra solver settings which may improve performance 41 | problem.set_options({'solver_options': 42 | {'ipopt': {'ipopt.hessian_approximation': 'limited-memory', 43 | 'ipopt.linear_solver': 'ma57'}}}) 44 | problem.init() 45 | 46 | # create simulator 47 | simulator = Simulator(problem) 48 | problem.plot('scene') 49 | vehicle.plot('input', knots=True, labels=['v (m/s)', 'ddelta (rad/s)']) 50 | vehicle.plot('state', knots=True, labels=[ 51 | 'x (m)', 'y (m)', 'theta (rad)', 'delta (rad)']) 52 | 53 | # run it! 54 | simulator.run() 55 | -------------------------------------------------------------------------------- /examples/p2p_agv2.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | assert_ma57() 23 | 24 | # create vehicle 25 | vehicle = AGV(length=0.8, options={'plot_type': 'agv'}) 26 | vehicle.define_knots(knot_intervals=5) # choose lower amount of knot intervals 27 | 28 | vehicle.set_initial_conditions([-1., -0.5, 0., 0.]) # x, y, theta, delta 29 | vehicle.set_terminal_conditions([4., 2., 0.]) # x, y, theta 30 | 31 | # create environment 32 | environment = Environment(room={'shape': Square(6.), 'position': [1.5, 1.5]}) 33 | rectangle = Rectangle(width=0.8, height=0.2) 34 | 35 | # create a point-to-point problem 36 | problem = Point2point(vehicle, environment, freeT=True) 37 | # extra solver settings which may improve performance 38 | problem.set_options({'solver_options': 39 | {'ipopt': {'ipopt.hessian_approximation': 'limited-memory', 40 | 'ipopt.linear_solver': 'ma57'}}}) # hsl solvers required for this!! 41 | problem.init() 42 | 43 | # create simulator 44 | simulator = Simulator(problem) 45 | problem.plot('scene') 46 | vehicle.plot('input', knots=True, labels=['v (m/s)', 'ddelta (rad/s)']) 47 | vehicle.plot('state', knots=True, labels=[ 48 | 'x (m)', 'y (m)', 'theta (rad)', 'delta (rad)']) 49 | # run it! 50 | simulator.run() 51 | -------------------------------------------------------------------------------- /examples/p2p_bicycle.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | 23 | assert_ma57() 24 | 25 | # create vehicle 26 | vehicle = Bicycle(length=0.4, options={'plot_type': 'car', 'substitution': False}) 27 | vehicle.define_knots(knot_intervals=5) # choose lower amount of knot intervals 28 | 29 | vehicle.set_initial_conditions([0., 0., 0., 0.]) # x, y, theta, delta 30 | vehicle.set_terminal_conditions([3., 3., 0.]) # x, y, theta 31 | 32 | # create environment 33 | environment = Environment(room={'shape': Square(5.), 'position': [1.5, 1.5]}) 34 | trajectories = {'velocity': {'time': [0.5], 35 | 'values': [[0.3, 0.0]]}} 36 | environment.add_obstacle(Obstacle({'position': [1., 1.]}, shape=Circle(0.5), 37 | simulation={'trajectories': trajectories})) 38 | 39 | # create a point-to-point problem 40 | problem = Point2point(vehicle, environment, freeT=True) 41 | # extra solver settings which may improve performance 42 | problem.set_options({'solver_options': {'ipopt': {'ipopt.linear_solver': 'ma57'}}}) 43 | problem.init() 44 | 45 | vehicle.problem = problem # to plot error if using substitution 46 | 47 | # create simulator 48 | simulator = Simulator(problem) 49 | 50 | problem.plot('scene') 51 | vehicle.plot('input', knots=True, labels=['v (m/s)', 'ddelta (rad/s)']) 52 | vehicle.plot('state', knots=True, labels=[ 53 | 'x (m)', 'y (m)', 'theta (rad)', 'delta (rad)']) 54 | if vehicle.options['substitution']: 55 | vehicle.plot('err_pos', knots=True) 56 | vehicle.plot('err_dpos', knots=True) 57 | 58 | # run it! 59 | simulator.run() 60 | -------------------------------------------------------------------------------- /examples/p2p_dubins.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create vehicle 23 | vehicle = Dubins(bounds={'vmax': 0.7, 'wmax': np.pi/3., 'wmin': -np.pi/3.}, # in rad/s 24 | options={'substitution': True}) 25 | vehicle.define_knots(knot_intervals=5) # choose lower amount of knot intervals 26 | 27 | vehicle.set_initial_conditions([0., 0., 0.]) # input orientation in rad 28 | vehicle.set_terminal_conditions([3., 3., 0.]) 29 | 30 | # create environment 31 | environment = Environment(room={'shape': Square(5.), 'position': [1.5, 1.5]}) 32 | 33 | trajectories = {'velocity': {'time': [0.5], 34 | 'values': [[0.25, 0.0]]}} 35 | environment.add_obstacle(Obstacle({'position': [1., 1.]}, shape=Circle(0.5), 36 | simulation={'trajectories': trajectories})) 37 | 38 | # create a point-to-point problem 39 | problem = Point2point(vehicle, environment, freeT=True) 40 | # extra solver settings which may improve performance 41 | problem.set_options({'solver_options': 42 | {'ipopt': {'ipopt.hessian_approximation': 'limited-memory'}}}) 43 | 44 | problem.init() 45 | 46 | vehicle.problem = problem # to plot error when using substitution 47 | 48 | # create simulator 49 | simulator = Simulator(problem) 50 | problem.plot('scene') 51 | vehicle.plot('input', knots=True, labels=['v (m/s)', 'w (rad/s)']) 52 | vehicle.plot('state', knots=True, labels=['x (m)', 'y (m)', 'theta (rad)']) 53 | 54 | if vehicle.options['substitution']: 55 | vehicle.plot('err_pos', knots=True) 56 | vehicle.plot('err_dpos', knots=True) 57 | 58 | # run it! 59 | simulator.run() 60 | -------------------------------------------------------------------------------- /examples/p2p_holonomic.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create vehicle 23 | vehicle = Holonomic() 24 | vehicle.set_options({'safety_distance': 0.1}) 25 | vehicle.set_options({'ideal_prediction': False}) 26 | 27 | vehicle.set_initial_conditions([-1.5, -1.5]) 28 | vehicle.set_terminal_conditions([2., 2.]) 29 | 30 | # create environment 31 | environment = Environment(room={'shape': Square(5.)}) 32 | rectangle = Rectangle(width=3., height=0.2) 33 | 34 | # environment.add_obstacle(Obstacle({'position': [-2.1, -0.5]}, shape=rectangle)) 35 | # environment.add_obstacle(Obstacle({'position': [1.7, -0.5]}, shape=rectangle)) 36 | trajectories = {'velocity': {'time': [0., 40.], 37 | 'values': [[-0.35, 0.35], [0., 0.15]]}} 38 | environment.add_obstacle(Obstacle({'position': [1.5, -1]}, shape=Circle(0.5), options={'bounce':False}, 39 | simulation={'trajectories': trajectories})) 40 | 41 | # create a point-to-point problem 42 | problem = Point2point(vehicle, environment, freeT=False) 43 | problem.init() 44 | 45 | # create simulator 46 | simulator = Simulator(problem) 47 | problem.plot('scene') 48 | vehicle.plot('input', knots=True, prediction=True, labels=['v_x (m/s)', 'v_y (m/s)']) 49 | 50 | # run it! 51 | simulator.run() 52 | 53 | problem.save_movie('scene', format='gif', name='problemgif', number_of_frames=80, movie_time=4, axis=False) 54 | 55 | -------------------------------------------------------------------------------- /examples/p2p_holonomic_3d.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create vehicle 23 | vehicle = Holonomic3D(Plate(Rectangle(0.5, 1.), height=0.1)) 24 | 25 | vehicle.set_initial_conditions([-2., -2., -2]) 26 | vehicle.set_terminal_conditions([2., 2., -2]) 27 | 28 | # create environment 29 | environment = Environment(room={'shape': Cube(5.)}) 30 | environment.add_obstacle(Obstacle( 31 | {'position': [0., 0., -1.5]}, shape=Cuboid(width=0.5, depth=4., height=2.))) 32 | trajectories = {'velocity': {'time': [4.], 'values': [[0.0, 0.0, 1.]]}} 33 | environment.add_obstacle(Obstacle( 34 | {'position': [1., 1., -2.25]}, shape=RegularPrisma(0.25, 0.25, 6), 35 | simulation={'trajectories': trajectories})) 36 | 37 | # create a point-to-point problem 38 | problem = Point2point(vehicle, environment, freeT=False) 39 | problem.set_options({'hard_term_con': True, 'horizon_time': 12}) 40 | problem.init() 41 | 42 | # create simulator 43 | simulator = Simulator(problem) 44 | problem.plot('scene', view=[45, -45]) # elevation and azimuth of cam 45 | vehicle.plot('state', knots=True, labels=['x (m)', 'y (m)', 'z (m)']) 46 | vehicle.plot('input', knots=True, labels=['v_x (m/s)', 'v_y (m/s)', 'v_z (m/s)']) 47 | 48 | # run it! 49 | simulator.run() 50 | -------------------------------------------------------------------------------- /examples/p2p_holonomic_balls.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create vehicle 23 | vehicle = Holonomic(shapes=Circle(0.2), options={'syslimit': 'norm_2'}) 24 | vehicle.define_knots(knot_intervals=10) 25 | 26 | vehicle.set_initial_conditions([-4., 0]) 27 | vehicle.set_terminal_conditions([4., 0]) 28 | 29 | # create environment 30 | environment = Environment(room={'shape': Square(10.)}) 31 | 32 | trajectories1 = {'velocity': {'time': [0, 4.5], 33 | 'values': [[0., 0.0], [0., 0.35]]}} 34 | trajectories2 = {'velocity': {'time': [0, 5.], 35 | 'values': [[0., 0.0], [0., 0.25]]}} 36 | 37 | environment.add_obstacle(Obstacle({'position': [0.,-0.5]}, shape=Circle(0.75), 38 | simulation={'trajectories': trajectories1})) 39 | environment.add_obstacle(Obstacle({'position': [2.,0.5]}, shape=Circle(0.75))) 40 | environment.add_obstacle(Obstacle({'position': [-2.,0.5]}, shape=Circle(0.75))) 41 | 42 | environment.add_obstacle(Obstacle({'position': [0.,-2.25]}, shape=Circle(0.75), 43 | simulation={'trajectories': trajectories2})) 44 | 45 | # create a point-to-point problem 46 | problem = Point2point(vehicle, environment, freeT=True) 47 | problem.set_options({'solver_options': 48 | {'ipopt': {'ipopt.hessian_approximation': 'exact'}}}) 49 | problem.init() 50 | 51 | # create simulator 52 | simulator = Simulator(problem) 53 | problem.plot('scene') 54 | vehicle.plot('input', knots=True, labels=['v_x (m/s)', 'v_y (m/s)']) 55 | 56 | # run it! 57 | simulator.run() 58 | -------------------------------------------------------------------------------- /examples/p2p_holonomic_blocking.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create vehicle 23 | vehicle = Holonomic(shapes=Circle(0.1), options={'syslimit': 'norm_2'}, 24 | bounds={'vmin':-0.5, 'vmax': 0.5, 'amin':-5, 'amax':5}) 25 | 26 | vehicle.define_knots(knot_intervals = 6) 27 | vehicle.set_initial_conditions([3., 0.]) 28 | vehicle.set_terminal_conditions([6., 3.5]) 29 | 30 | # create environment 31 | environment = Environment(room={'shape': Rectangle(width=7.5, height=4.5), 32 | 'position': [2.75, 1.75]}) 33 | 34 | rectangle = Rectangle(width=1., height=1.) 35 | 36 | environment.add_obstacle(Obstacle({'position': [1., 1.]}, shape=rectangle)) 37 | environment.add_obstacle(Obstacle({'position': [3., 1.]}, shape=rectangle)) 38 | environment.add_obstacle(Obstacle({'position': [5., 1.]}, shape=rectangle)) 39 | environment.add_obstacle(Obstacle({'position': [1., 2.5]}, shape=rectangle)) 40 | environment.add_obstacle(Obstacle({'position': [3., 2.5]}, shape=rectangle)) 41 | environment.add_obstacle(Obstacle({'position': [5., 2.5]}, shape=rectangle)) 42 | trajectories1 = {'velocity': {'time': [0], 'values': [[0., 0.15]]}} 43 | trajectories2 = {'position': {'time': [0, 2.5], 'values': [[0, 0], [14, 12.5]]}} 44 | trajectories3 = {'position': {'time': [0, 2.5], 'values': [[0, 0], [15, 11.75]]}} 45 | circle1 = Circle(0.5) 46 | circle2 = Circle(0.25) 47 | environment.add_obstacle(Obstacle({'position': [2., 0.]}, shape=circle1, 48 | simulation={'trajectories': trajectories1})) 49 | environment.add_obstacle(Obstacle({'position': [-10., -10]}, shape=circle1, 50 | simulation={'trajectories': trajectories2})) 51 | environment.add_obstacle(Obstacle({'position': [-10, -10]}, shape=circle2, 52 | simulation={'trajectories': trajectories3})) 53 | 54 | # create a point-to-point problem 55 | problem = Point2point(vehicle, environment, freeT=True) 56 | problem.init() 57 | 58 | # create simulator 59 | simulator = Simulator(problem) 60 | problem.plot('scene') 61 | vehicle.plot('input', knots=True,labels=['vx (m/s)', 'vy (m/s)']) 62 | 63 | # run it! 64 | simulator.run() 65 | -------------------------------------------------------------------------------- /examples/p2p_holonomic_disturbances.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create vehicle 23 | vehicle = Holonomic() 24 | vehicle.set_options({'safety_distance': 0.1}) 25 | vehicle.set_options({'1storder_delay': True, 'time_constant': 0.1}) 26 | vehicle.set_options({'input_disturbance': {'fc': 0.01, 'stdev': 0.05*np.ones(2)}}) 27 | vehicle.set_options({'stop_tol': 1.e-2}) 28 | 29 | vehicle.set_initial_conditions([-1.5, -1.5]) 30 | vehicle.set_terminal_conditions([2., 2.]) 31 | 32 | # create environment 33 | environment = Environment(room={'shape': Square(5.)}) 34 | rectangle = Rectangle(width=3., height=0.2) 35 | 36 | environment.add_obstacle(Obstacle({'position': [-2.1, -0.5]}, shape=rectangle)) 37 | environment.add_obstacle(Obstacle({'position': [1.7, -0.5]}, shape=rectangle)) 38 | trajectories = {'velocity': {'time': [3., 4.], 39 | 'values': [[-0.15, 0.0], [0., 0.15]]}} 40 | environment.add_obstacle(Obstacle({'position': [1.5, 0.5]}, shape=Circle(0.4), 41 | simulation={'trajectories': trajectories})) 42 | 43 | # create a point-to-point problem 44 | problem = Point2point(vehicle, environment, freeT=False) 45 | problem.init() 46 | 47 | # create simulator 48 | simulator = Simulator(problem) 49 | problem.plot('scene') 50 | vehicle.plot('input', knots=True, labels=['v_x (m/s)', 'v_y (m/s)']) 51 | 52 | # run it! 53 | simulator.run() 54 | -------------------------------------------------------------------------------- /examples/p2p_holonomic_export.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | import os 22 | import csv 23 | 24 | """ 25 | This file demonstrates how to export a point2point problem to c++. It generates 26 | some source and header files which can be compiled with your own source code or 27 | which can be compiled to a shared library and included in your own project. 28 | """ 29 | 30 | # create vehicle 31 | options = {'room_constraint': None} 32 | vehicle = Holonomic(shapes=Circle(0.1), options=options) 33 | 34 | vehicle.set_initial_conditions([0.0, 0.0]) 35 | vehicle.set_terminal_conditions([3.5, 3.5]) 36 | 37 | # create environment 38 | environment = Environment(room={'shape': Square(5.), 'position': [1.5, 1.5]}) 39 | rectangle = Rectangle(width=3., height=0.2) 40 | 41 | environment.add_obstacle(Obstacle({'position': [-0.6, 1.0]}, shape=rectangle)) 42 | environment.add_obstacle(Obstacle({'position': [3.2, 1.0]}, shape=rectangle)) 43 | 44 | # create a point-to-point problem 45 | problem = Point2point(vehicle, environment, freeT=False) 46 | problem.init() 47 | 48 | options = {} 49 | options['directory'] = os.path.join(os.getcwd(), 'export/') 50 | # path to object files of your exported optimization problem 51 | options['casadiobj'] = os.path.join(options['directory'], 'bin/') 52 | 53 | # export the problem 54 | problem.export(options) 55 | simulator = Simulator(problem) 56 | trajectories, signals = simulator.run() 57 | 58 | # save results for check in c++ 59 | testdir = os.path.join(options['directory'], 'test') 60 | if not os.path.isdir(testdir): 61 | os.makedirs(os.path.join(options['directory'], 'test')) 62 | with open(os.path.join(testdir, 'data_state.csv'), 'w') as f: 63 | w = csv.writer(f) 64 | for i in range(0, len(trajectories['state']), int(simulator.update_time/simulator.sample_time)): 65 | for k in range(trajectories['state'][i].shape[0]): 66 | w.writerow(trajectories['state'][i][k, :]) 67 | with open(os.path.join(testdir, 'data_input.csv'), 'w') as f: 68 | w = csv.writer(f) 69 | for i in range(0, len(trajectories['input']), int(simulator.update_time/simulator.sample_time)): 70 | for k in range(trajectories['input'][i].shape[0]): 71 | w.writerow(trajectories['input'][i][k, :]) 72 | 73 | # note: you need to implement your vehicle type in c++. Take a look at 74 | # Holonomic.cpp and Holonomic.hpp which are also exported as an example. 75 | -------------------------------------------------------------------------------- /examples/p2p_holonomic_interveh_avoidance.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create vehicle 23 | N = 2 24 | vehicles = [Holonomic() for k in range(N)] 25 | 26 | for k, vehicle in enumerate(vehicles): 27 | vehicle.set_initial_conditions([1.5*np.cos((k*2.*np.pi)/N), 1.5*np.sin((k*2.*np.pi)/N)]) 28 | vehicle.set_terminal_conditions([-1.5*np.cos((k*2.*np.pi)/N), -1.5*np.sin((k*2.*np.pi)/N)]) 29 | 30 | # create environment 31 | environment = Environment(room={'shape': Square(5.)}) 32 | rectangle = Rectangle(width=3., height=0.2) 33 | 34 | # create a point-to-point problem 35 | problem = Point2point(vehicles, environment, freeT=False) 36 | problem.set_options({'inter_vehicle_avoidance': True}) 37 | problem.init() 38 | 39 | # create simulator 40 | simulator = Simulator(problem) 41 | problem.plot('scene') 42 | vehicles[0].plot('input', knots=True, prediction=True, labels=['v_x (m/s)', 'v_y (m/s)']) 43 | vehicles[1].plot('input', knots=True, prediction=True, labels=['v_x (m/s)', 'v_y (m/s)']) 44 | 45 | # run it! 46 | simulator.run() 47 | -------------------------------------------------------------------------------- /examples/p2p_holonomic_multiproblem.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create vehicle 23 | vehicle = Holonomic() 24 | vehicle.set_options({'safety_distance': 0.1}) 25 | 26 | vehicle.set_initial_conditions([-1.5, -1.5]) 27 | vehicle.set_terminal_conditions([2., 2.]) 28 | 29 | # create environment 30 | environment = Environment(room={'shape': Square(5.)}) 31 | rectangle = Rectangle(width=3., height=0.2) 32 | 33 | obstacle1 = Obstacle({'position': [-2.1, -0.5]}, shape=rectangle) 34 | obstacle2 = Obstacle({'position': [1.7, -0.5]}, shape=rectangle) 35 | trajectories = {'velocity': {'time': [3., 4.], 36 | 'values': [[-0.15, 0.0], [0., 0.15]]}} 37 | obstacle3 = Obstacle({'position': [1.5, 0.5]}, shape=Circle(0.4), 38 | simulation={'trajectories': trajectories}) 39 | 40 | environment.add_obstacle([obstacle1, obstacle2, obstacle3]) 41 | 42 | # create problems 43 | problem1 = Point2point(vehicle, environment, freeT=False) 44 | problem1.init() 45 | obstacle3.set_options({'avoid': False}) 46 | problem2 = Point2point(vehicle, environment, freeT=False) 47 | problem2.init() 48 | 49 | # create simulator & plots 50 | simulator = Simulator(problem1) 51 | vehicle.plot('input', knots=True, prediction=True, labels=['v_x (m/s)', 'v_y (m/s)']) 52 | problem1.plot('scene') 53 | # 1st task 54 | simulator.run() 55 | # 2nd task 56 | vehicle.set_terminal_conditions([2., 0.0]) 57 | simulator.run() 58 | # 3th task (we can neglect circular obstacle), but first sleep 2 seconds 59 | simulator.sleep(2.) 60 | simulator.set_problem(problem2) 61 | problem2.plot('scene') 62 | vehicle.set_terminal_conditions([0.0, 1.0]) 63 | simulator.run() 64 | -------------------------------------------------------------------------------- /examples/p2p_holonomic_obstraj_export.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | import os 22 | import numpy as np 23 | 24 | # create vehicle 25 | vehicle = Holonomic() 26 | vehicle.set_options({'safety_distance': 0.1}) 27 | vehicle.set_options({'ideal_prediction': False}) 28 | 29 | vehicle.set_initial_conditions([-1.5, -1.5]) 30 | vehicle.set_terminal_conditions([2., 2.]) 31 | 32 | # extract spline parameters 33 | coeffs = np.zeros([len(vehicle.basis.knots)-vehicle.basis.degree-1, 2]) 34 | spline_params = {'knots': vehicle.basis.knots, 'degree': vehicle.basis.degree, 'coeffs': coeffs} 35 | 36 | # create environment 37 | environment = Environment(room={'shape': Square(5.)}) 38 | rectangle = Rectangle(width=3., height=0.2) 39 | environment.add_obstacle(Obstacle({'position': [1.7, -0.5]}, shape=rectangle)) 40 | obstacle = Obstacle({'position': [1.5, 0.5]}, shape=Circle(0.4)) 41 | obstacle.set_options({'spline_traj': True}) 42 | obstacle.set_options({'spline_params': spline_params}) 43 | environment.add_obstacle(obstacle) 44 | 45 | # create a point-to-point problem 46 | problem = Point2point(vehicle, environment, freeT=False, options={'horizon_time': 10}) 47 | problem.init() 48 | 49 | options = {} 50 | options['directory'] = os.path.join(os.getcwd(), 'export_obsttraj/') 51 | # path to object files of your exported optimization problem 52 | options['casadiobj'] = os.path.join(options['directory'], 'bin/') 53 | problem.export(options) 54 | -------------------------------------------------------------------------------- /examples/p2p_holonomic_octroom.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create vehicle 23 | vehicle = Holonomic() 24 | vehicle.set_options({'safety_distance': 0.1}) 25 | 26 | vehicle.set_initial_conditions([-1.5, -1.5]) 27 | vehicle.set_terminal_conditions([1.0, 1.5]) 28 | 29 | # create environment 30 | environment = Environment(room={'shape': RegularPolyhedron(2.5, 8), 'draw': True}) 31 | rectangle = Rectangle(width=3., height=0.2) 32 | 33 | environment.add_obstacle(Obstacle({'position': [-2.1, -0.5]}, shape=rectangle)) 34 | environment.add_obstacle(Obstacle({'position': [1.7, -0.5]}, shape=rectangle)) 35 | trajectories = {'velocity': {'time': [3., 4.], 36 | 'values': [[-0.15, 0.0], [0., 0.15]]}} 37 | environment.add_obstacle(Obstacle({'position': [1.5, 0.5]}, shape=Circle(0.4), 38 | simulation={'trajectories': trajectories})) 39 | 40 | # create a point-to-point problem 41 | problem = Point2point(vehicle, environment, freeT=False) 42 | problem.init() 43 | 44 | # create simulator 45 | simulator = Simulator(problem) 46 | problem.plot('scene') 47 | vehicle.plot('input', knots=True, labels=['v_x (m/s)', 'v_y (m/s)']) 48 | 49 | # run it! 50 | simulator.run() 51 | -------------------------------------------------------------------------------- /examples/p2p_holonomic_orient.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create vehicle 23 | vehicle = HolonomicOrient() 24 | # Regularization on change in orientation may be required, since the orientation 25 | # of the holonomic vehicle is free. The weight has to be tuned for the application. 26 | # By default no regularization is added. 27 | vehicle.set_options({'reg_type': 'norm_1', 'reg_weight': 10}) 28 | 29 | vehicle.set_initial_conditions([-1.5, -1.5, np.pi/4.]) # input orientation in deg 30 | vehicle.set_terminal_conditions([2., 2., np.pi/2.]) 31 | 32 | # create environment 33 | environment = Environment(room={'shape': Square(5.)}) 34 | rectangle = Rectangle(width=3., height=0.2) 35 | 36 | environment.add_obstacle(Obstacle({'position': [-1.8, -0.5]}, shape=rectangle)) 37 | environment.add_obstacle(Obstacle({'position': [1.7, -0.5]}, shape=rectangle)) 38 | trajectories = {'velocity': {'time': [3., 4.], 39 | 'values': [[-0.15, 0.0], [0., 0.15]]}} 40 | environment.add_obstacle(Obstacle({'position': [1.5, 0.5]}, shape=Circle(0.4), 41 | simulation={'trajectories': trajectories})) 42 | 43 | # create a point-to-point problem 44 | problem = Point2point(vehicle, environment, freeT=True) 45 | problem.init() 46 | 47 | # create simulator 48 | simulator = Simulator(problem) 49 | problem.plot('scene') 50 | vehicle.plot('input', knots=True, labels=['v_x (m/s)', 'v_y (m/s)', 'w (rad/s)']) 51 | vehicle.plot('state', knots=True, labels=['x (m)', 'y (m)', 'theta (rad)']) 52 | 53 | # run it! 54 | simulator.run() 55 | -------------------------------------------------------------------------------- /examples/p2p_holonomic_solvertest.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create vehicle 23 | vehicle = Holonomic() 24 | vehicle.set_options({'safety_distance': 0.1}) 25 | 26 | vehicle.set_initial_conditions([-1.5, -1.5]) 27 | vehicle.set_terminal_conditions([2., 2.]) 28 | 29 | # create environment 30 | environment = Environment(room={'shape': Square(5.)}) 31 | rectangle = Rectangle(width=3., height=0.2) 32 | 33 | # environment.add_obstacle(Obstacle({'position': [-2.1, -0.5]}, shape=rectangle)) 34 | environment.add_obstacle(Obstacle({'position': [1.7, -0.5]}, shape=rectangle)) 35 | trajectories = {'velocity': {'time': [3., 4.], 36 | 'values': [[-0.15, 0.0], [0., 0.15]]}} 37 | environment.add_obstacle(Obstacle({'position': [1.5, 0.5]}, shape=Circle(0.4), 38 | simulation={'trajectories': trajectories})) 39 | 40 | # create a point-to-point problem 41 | # select solver 42 | solver = 'ipopt' 43 | if solver is 'ipopt': 44 | options = {'solver': solver} 45 | problem = Point2point(vehicle, environment, options, freeT=False) 46 | elif solver is 'worhp': 47 | options = {'solver': solver} 48 | worhp_options = { # 'worhp.qp_ipLsMethod': 'MA57', # todo: option not found? 49 | 'worhp.MaxIter': 200, 50 | 'worhp.TolOpti': 1e-6, 51 | # False = warm start 52 | 'worhp.InitialLMest': False, 53 | 'worhp.UserHM': True} # True = exact Hessian 54 | options['solver_options'] = {'worhp': worhp_options} 55 | problem = Point2point(vehicle, environment, options, freeT=False) 56 | elif solver is 'snopt': 57 | options = {'solver': solver} # todo: plugin snopt not found? 58 | problem = Point2point(vehicle, environment, options, freeT=False) 59 | problem.set_options({'solver_options': 60 | {'snopt': {'snopt.Hessian': 'limited memory', 61 | 'start': 'warm'}}}) 62 | elif solver is 'blocksqp': 63 | options = {'solver': solver} 64 | problem = Point2point(vehicle, environment, options, freeT=False) 65 | problem.set_options({'solver_options': 66 | {'blocksqp': {'warmstart': True, 'hess_lim_mem': 0}}}) 67 | elif solver is 'knitro': 68 | options = {'solver': solver} 69 | problem = Point2point(vehicle, environment, options, freeT=True) 70 | problem.set_options({'solver_options': 71 | {'knitro': {'knitro.bar_initpt': 2, 'knitro.honorbnds': 0, 'knitro.scale': 1}}}) 72 | # other possible options: 'knitro.linsolver': 2, 'knitro.bar_murule':5, 'knitro.algorithm':1 73 | else: 74 | print(('You selected solver: ' + solver + 75 | ' but this solver is not supported. ' + 76 | 'Choose between ipopt, worhp, snopt or blocksqp.')) 77 | problem.init() 78 | 79 | # create simulator 80 | simulator = Simulator(problem) 81 | problem.plot('scene') 82 | vehicle.plot('input', knots=True, labels=['v_x (m/s)', 'v_y (m/s)']) 83 | 84 | # run it! 85 | simulator.run() 86 | -------------------------------------------------------------------------------- /examples/p2p_holonomic_warehouse.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create vehicle 23 | vehicle = Holonomic(options={'syslimit': 'norm_2', 'safety_distance': 0.1}) 24 | 25 | vehicle.define_knots(knot_intervals = 10) 26 | vehicle.set_initial_conditions([0., 0.]) 27 | vehicle.set_terminal_conditions([6., 3.5]) 28 | 29 | # create environment 30 | environment = Environment(room={'shape': Rectangle(width=7., height=4.5), 'position': [3., 1.75]}) 31 | rectangle = Rectangle(width=1., height=1.) 32 | 33 | environment.add_obstacle(Obstacle({'position': [1., 1.]}, shape=rectangle)) 34 | environment.add_obstacle(Obstacle({'position': [3., 1.]}, shape=rectangle)) 35 | environment.add_obstacle(Obstacle({'position': [5., 1.]}, shape=rectangle)) 36 | environment.add_obstacle(Obstacle({'position': [1., 2.5]}, shape=rectangle)) 37 | environment.add_obstacle(Obstacle({'position': [3., 2.5]}, shape=rectangle)) 38 | environment.add_obstacle(Obstacle({'position': [5., 2.5]}, shape=rectangle)) 39 | trajectories1 = {'velocity': {'time': [0, 2], 40 | 'values': [[0., 0.0], [0., 0.15]]}} 41 | trajectories2 = {'velocity': {'time': [0, 2], 42 | 'values': [[0., 0.0], [0., -0.1]]}} 43 | environment.add_obstacle(Obstacle({'position': [4., 2.5]}, shape=Circle(0.5), 44 | simulation={'trajectories': trajectories2})) 45 | environment.add_obstacle(Obstacle({'position': [2., 1.]}, shape=Circle(0.5), 46 | simulation={'trajectories': trajectories1})) 47 | 48 | # create a point-to-point problem 49 | problem = Point2point(vehicle, environment, freeT=True) 50 | problem.set_options({'solver_options': 51 | {'ipopt': {'ipopt.hessian_approximation': 'limited-memory'}}}) 52 | problem.init() 53 | 54 | # create simulator 55 | simulator = Simulator(problem) 56 | problem.plot('scene') 57 | vehicle.plot('input', knots=True,labels=['v (m/s)', 'w (rad/s)']) 58 | 59 | # run it! 60 | simulator.run() 61 | 62 | # problem.save_movie('scene', number_of_frames=60, name='warehouse', axis=False) 63 | -------------------------------------------------------------------------------- /examples/p2p_quadrotor.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create vehicle 23 | vehicle = Quadrotor() 24 | vehicle.set_options({'safety_distance': 0.1}) 25 | vehicle.set_initial_conditions([-4., -4., 0., 0., 0.]) 26 | vehicle.set_terminal_conditions([4., 4.]) 27 | 28 | # create environment 29 | environment = Environment(room={'shape': Square(10.)}) 30 | environment.add_obstacle(Obstacle({'position': [-0.6, -5.4]}, 31 | shape=Rectangle(width=0.2, height=12.))) 32 | 33 | # create a point-to-point problem 34 | problem = Point2point(vehicle, environment, {'horizon_time': 5}) 35 | problem.init() 36 | 37 | # create simulator 38 | simulator = Simulator(problem) 39 | problem.plot('scene') 40 | vehicle.plot('input', knots=True, label=['Thrust force (N/kg)', 41 | 'Pitch rate (rad/s)']) 42 | 43 | # run it! 44 | simulator.run() 45 | -------------------------------------------------------------------------------- /examples/p2p_trailer.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | assert_ma57() 23 | 24 | # create vehicle 25 | vehicle = Dubins(shapes=Circle(0.2), bounds={'vmax': 0.8, 'wmax': np.pi/3., 'wmin': -np.pi/3.}) 26 | vehicle.define_knots(knot_intervals=9) # adapt amount of knot intervals 27 | vehicle.set_initial_conditions([0., 0., 0.]) # input orientation in rad 28 | vehicle.set_terminal_conditions([3.4, 3., 0.]) 29 | 30 | # create trailer 31 | trailer = Trailer(lead_veh=vehicle, shapes=Rectangle(0.2, 0.2), l_hitch = 0.6, 32 | bounds={'tmax': np.pi/4., 'tmin': -np.pi/4.}) # limit angle between vehicle and trailer 33 | # Note: the knot intervals of lead_veh and trailer should be the same 34 | trailer.define_knots(knot_intervals=9) # adapt amount of knot intervals 35 | trailer.set_initial_conditions(0.) # input orientation in rad 36 | trailer.set_terminal_conditions(0.) # this depends on the application e.g. driving vs parking 37 | 38 | # create environment 39 | environment = Environment(room={'shape': Square(5.), 'position': [1.5, 1.5]}) 40 | 41 | # create a point-to-point problem 42 | problem = Point2point(trailer, environment, freeT=True) # pass trailer to problem 43 | # todo: isn't there are a cleaner way? 44 | problem.father.add(vehicle) # add vehicle to optifather, such that it knows the trailer variables 45 | problem.vehicles.append(vehicle) 46 | # todo: isn't there are a cleaner way? 47 | vehicle.to_simulate = False 48 | # extra solver settings which may improve performance 49 | problem.set_options({'solver_options': 50 | {'ipopt': {'ipopt.hessian_approximation': 'limited-memory', 51 | 'ipopt.linear_solver': 'ma57'}}}) 52 | problem.init() 53 | 54 | # problem.set_options({'hard_term_con': True, 'horizon_time': 12}) 55 | # vehicle.problem = problem # to plot error 56 | 57 | # create simulator 58 | simulator = Simulator(problem) 59 | problem.plot('scene') 60 | trailer.plot('input', knots=True, prediction=True, labels=['v (m/s)', 'ddelta (rad/s)']) 61 | trailer.plot('state', knots=True, labels=['x_tr (m)', 'y_tr (m)', 'theta_tr (rad)', 'x_veh (m)', 'y_veh (m)', 'theta_veh (rad)']) 62 | 63 | # run it! 64 | simulator.run() 65 | -------------------------------------------------------------------------------- /examples/platform_landing.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create fleet 23 | N_quad = 2 24 | quadrotors = [Quadrotor(0.2) for l in range(N_quad)] 25 | fleet = Fleet(quadrotors + [Holonomic1D()]) 26 | 27 | configuration = [[0.25], [-0.25], [0.0]] 28 | init_positions = [[1.5, 3.], [-2., 2.], [1.]] 29 | terminal_positions = [[0., 0.1], [0., 0.1], [0.]] 30 | 31 | fleet.set_configuration(configuration) 32 | fleet.set_initial_conditions(init_positions) 33 | fleet.set_terminal_conditions(terminal_positions) 34 | 35 | # create environment 36 | environment = Environment(room={'shape': Square(5.), 'position': [0., 2.]}) 37 | environment.add_obstacle(Obstacle({'position': [1., 1.5]}, 38 | shape=Rectangle(width=1, height=0.2))) 39 | 40 | # create a formation point-to-point problem 41 | options = {'horizon_time': 5., 'codegen': {'jit': False}, 'rho': 3.} 42 | problem = RendezVous(fleet, environment, options=options) 43 | problem.init() 44 | 45 | # create simulator 46 | simulator = Simulator(problem) 47 | problem.plot('scene') 48 | fleet.plot('input', knots=True) 49 | 50 | # run it! 51 | simulator.run() 52 | -------------------------------------------------------------------------------- /examples/questions.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | 22 | # create fleet 23 | N = 8 24 | vehicles = [Quadrotor(0.2) for l in range(N)] 25 | fleet = Fleet(vehicles) 26 | 27 | configuration = [[-1.5, 0.], [-0.75, 1.29], [0.75, 1.29], [1.5, 0.], 28 | [0.75, -1.29], [0., -4.2], [0., -3.3], [0., -2.4]] 29 | init_positions = RegularPolyhedron(4., N, np.pi/4).vertices.T.tolist() 30 | terminal_positions = np.zeros((N, 2)).tolist() 31 | 32 | fleet.set_configuration(configuration) 33 | fleet.set_initial_conditions([np.r_[pos, np.zeros(3)] for pos in init_positions]) 34 | fleet.set_terminal_conditions(terminal_positions) 35 | 36 | # create environment 37 | environment = Environment(room={'shape': Square(10.)}) 38 | 39 | # create a formation point-to-point problem 40 | options = {'horizon_time': 5, 'codegen': {'jit': False}, 'rho': 3.} 41 | problem = RendezVous(fleet, environment, options=options) 42 | problem.init() 43 | 44 | # create simulator 45 | simulator = Simulator(problem) 46 | problem.plot('scene') 47 | fleet.plot('input', labels=['Thrust force (N/kg)', 48 | 'Pitch rate (rad/s)']) 49 | 50 | # run it! 51 | simulator.run() 52 | -------------------------------------------------------------------------------- /examples/rendezvous_holonomic_export.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | import os 22 | import csv 23 | 24 | """ 25 | This file demonstrates how to export a point2point problem to c++. It generates 26 | some source and header files which can be compiled with your own source code or 27 | which can be compiled to a shared library and included in your own project. 28 | """ 29 | 30 | # create fleet 31 | N = 4 32 | options = {'room_constraints': None} 33 | vehicles = [Holonomic(shapes=Circle(0.1), options=options) for l in range(N)] 34 | 35 | fleet = Fleet(vehicles) 36 | configuration = RegularPolyhedron(0.2, N, np.pi/4.).vertices.T 37 | init_positions = [[0., 3.], [3., 3.], [3., 0.], [0., 0.]] 38 | terminal_positions = np.zeros((N, 2)).tolist() 39 | 40 | fleet.set_configuration(configuration.tolist()) 41 | fleet.set_initial_conditions(init_positions) 42 | fleet.set_terminal_conditions(terminal_positions) 43 | 44 | # create environment 45 | environment = Environment(room={'shape': Square(5.), 'position': [1.5, 1.5]}) 46 | rectangle = Rectangle(width=3., height=0.2) 47 | 48 | environment.add_obstacle(Obstacle({'position': [3.2, 1.0]}, shape=rectangle)) 49 | 50 | # create a formation point-to-point problem 51 | options = {'rho': 2., 'horizon_time': 10, 'init_iter': 5} 52 | problem = RendezVous(fleet, environment, options=options) 53 | problem.init() 54 | 55 | options = {} 56 | options['directory'] = os.path.join(os.getcwd(), 'export_r/') 57 | # path to object files of your exported optimization problem 58 | options['casadiobj'] = os.path.join(options['directory'], 'bin/') 59 | 60 | # # export the problem 61 | problem.export(options) 62 | simulator = Simulator(problem) 63 | trajectories, signals = simulator.run() 64 | 65 | # save results for check in c++ 66 | testdir = os.path.join(options['directory'], 'test') 67 | if not os.path.isdir(testdir): 68 | os.makedirs(os.path.join(options['directory'], 'test')) 69 | jump = int(simulator.update_time/simulator.sample_time) 70 | size = len(trajectories[vehicles[0].label]['state']) 71 | with open(os.path.join(testdir, 'data_state.csv'), 'w') as f: 72 | w = csv.writer(f) 73 | for i in range(0, size, jump): 74 | for vehicle in vehicles: 75 | for k in range(trajectories[vehicle.label]['state'][i].shape[0]): 76 | w.writerow(trajectories[vehicle.label]['state'][i][k, :]) 77 | with open(os.path.join(testdir, 'data_input.csv'), 'w') as f: 78 | w = csv.writer(f) 79 | for i in range(0, size, jump): 80 | for vehicle in vehicles: 81 | for k in range(trajectories[vehicle.label]['input'][i].shape[0]): 82 | w.writerow(trajectories[vehicle.label]['input'][i][k, :]) 83 | 84 | # note: you need to implement your vehicle type in c++. Take a look at 85 | # Holonomic.cpp and Holonomic.hpp which are also exported as an example. 86 | -------------------------------------------------------------------------------- /examples/revolving_door.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | import numpy as np 22 | 23 | # create vehicle 24 | vehicle = Holonomic() 25 | vehicle.set_initial_conditions([0., -2.0]) 26 | vehicle.set_terminal_conditions([0., 2.0]) 27 | 28 | # create environment 29 | environment = Environment(room={'shape': Square(5.)}) 30 | beam1 = Beam(width=2.2, height=0.2) 31 | environment.add_obstacle(Obstacle({'position': [-2., 0.]}, shape=beam1)) 32 | environment.add_obstacle(Obstacle({'position': [2., 0.]}, shape=beam1)) 33 | 34 | beam2 = Beam(width=1.4, height=0.2) 35 | horizon_time = 10. 36 | omega = 1.5*(2*np.pi/horizon_time) 37 | velocity = [0., 0.] 38 | # velocity = [0., -0.2] # crazy revolving door 39 | environment.add_obstacle(Obstacle({'position': [0., 0.], 'velocity': velocity, 40 | 'angular_velocity': omega}, shape=beam2, simulation={}, 41 | options={'horizon_time': horizon_time})) 42 | environment.add_obstacle(Obstacle({'position': [0., 0.], 'velocity': velocity, 43 | 'orientation': 0.5*np.pi, 'angular_velocity': omega}, 44 | shape=beam2, simulation={}, options={'horizon_time': horizon_time})) 45 | 46 | # create a point-to-point problem 47 | problem = Point2point( 48 | vehicle, environment, freeT=False, options={'horizon_time': horizon_time}) 49 | problem.init() 50 | 51 | # create simulator 52 | simulator = Simulator(problem) 53 | problem.plot('scene') 54 | vehicle.plot('input', knots=True, labels=['v_x (m/s)', 'v_y (m/s)']) 55 | 56 | # run it! 57 | simulator.run() 58 | -------------------------------------------------------------------------------- /examples/revolving_door_diffdrive.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | import numpy as np 22 | 23 | assert_ma57() 24 | 25 | # create vehicle 26 | vehicle = Dubins(bounds={'vmax': 0.7, 'wmin': -30., 'wmax': 30.}) 27 | vehicle.define_knots(knot_intervals=6) 28 | vehicle.set_initial_conditions([0., -2.0, np.pi/2]) 29 | vehicle.set_terminal_conditions([-1.5, 2.0, np.pi/2]) 30 | 31 | # create environment 32 | environment = Environment(room={'shape': Square(5.)}) 33 | beam1 = Beam(width=2.2, height=0.2) 34 | environment.add_obstacle(Obstacle({'position': [-2., 0.]}, shape=beam1)) 35 | environment.add_obstacle(Obstacle({'position': [2., 0.]}, shape=beam1)) 36 | 37 | beam2 = Beam(width=1.4, height=0.2) 38 | horizon_time = 15. 39 | omega = 0.1*1.*(2*np.pi/horizon_time) 40 | velocity = [0., 0.] 41 | # velocity = [0., -0.2] # crazy revolving door 42 | environment.add_obstacle(Obstacle({'position': [0., 0.], 'velocity': velocity, 43 | 'orientation': 0.+np.pi/4., 'angular_velocity': omega}, 44 | shape=beam2, simulation={}, options={'horizon_time': horizon_time})) 45 | environment.add_obstacle(Obstacle({'position': [0., 0.], 'velocity': velocity, 46 | 'orientation': 0.5*np.pi+np.pi/4., 'angular_velocity': omega}, 47 | shape=beam2, simulation={}, options={'horizon_time': horizon_time})) 48 | 49 | # create a point-to-point problem 50 | solver = 'ipopt' 51 | if solver is 'knitro': 52 | options={'solver': solver, 'horizon_time': horizon_time, 'hard_term_con': True} 53 | problem = Point2point(vehicle, environment, options, freeT=False) 54 | problem.set_options( 55 | # {'solver_options':{'knitro':{'knitro.bar_initpt': 2, 'knitro.honorbnds': 0, 'knitro.scale': 1, 'knitro.linsolver':4, 'knitro.bar_murule':5, 'knitro.bar_directinterval':0, 'knitro.algorithm':1}}}) 56 | # {'solver_options': {'knitro': {'knitro.tuner':1}}}) 57 | # {'solver_options': {'knitro': {'knitro.ms_enable': 1, 'knitro.ms_maxsolves':3}}}) 58 | {'solver_options': {'knitro': {}}}) 59 | if solver is 'ipopt': 60 | options={'solver': solver, 'horizon_time': horizon_time, 'hard_term_con': True} 61 | problem = Point2point(vehicle, environment, options, freeT=False) 62 | problem.set_options( 63 | {'solver_options': {'ipopt': {'ipopt.hessian_approximation': 'limited-memory', 64 | 'ipopt.warm_start_mult_bound_push': 1e-6, 65 | 'ipopt.linear_solver': 'ma57'}}}) # hsl solvers required for this!! 66 | problem.init() 67 | 68 | # problem.export2AMPL() 69 | 70 | # create simulator 71 | simulator = Simulator(problem) 72 | problem.plot('scene') 73 | vehicle.plot('input', knots=True, labels=['v_x (m/s)', 'v_y (m/s)']) 74 | 75 | # run it! 76 | simulator.run() 77 | -------------------------------------------------------------------------------- /examples/revolving_door_quadrotor.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from omgtools import * 21 | import numpy as np 22 | 23 | # create vehicle 24 | vehicle = Quadrotor(radius=0.1, bounds={'u1max': 10, 'u2max': 8}) 25 | vehicle.define_knots(knot_intervals=10) 26 | vehicle.set_initial_conditions([0., -2.0]) 27 | vehicle.set_terminal_conditions([-0.5, 2.0]) 28 | 29 | # create environment 30 | environment = Environment(room={'shape': Square(5.)}) 31 | beam1 = Beam(width=2.2, height=0.2) 32 | environment.add_obstacle(Obstacle({'position': [-2., 0.]}, shape=beam1)) 33 | environment.add_obstacle(Obstacle({'position': [2., 0.]}, shape=beam1)) 34 | 35 | beam2 = Beam(width=1.4, height=0.2) 36 | horizon_time = 10. 37 | omega = 0.45*1.*(2*np.pi/horizon_time) 38 | velocity = [0., 0.] 39 | # velocity = [0., -0.2] # crazy revolving door 40 | environment.add_obstacle(Obstacle({'position': [0., 0.], 'velocity': velocity, 41 | 'orientation': 0.+np.pi/4, 'angular_velocity': omega}, 42 | shape=beam2, simulation={}, options={'horizon_time': horizon_time})) 43 | environment.add_obstacle(Obstacle({'position': [0., 0.], 'velocity': velocity, 44 | 'orientation': 0.5*np.pi+np.pi/4, 'angular_velocity': omega}, 45 | shape=beam2, simulation={}, options={'horizon_time': horizon_time})) 46 | 47 | # create a point-to-point problem 48 | problem = Point2point( 49 | vehicle, environment, freeT=False, options={'horizon_time': horizon_time}) 50 | problem.set_options({'solver_options': 51 | {'ipopt': {'ipopt.hessian_approximation': 'limited-memory'}}}) 52 | 53 | problem.init() 54 | 55 | # create simulator 56 | simulator = Simulator(problem) 57 | problem.plot('scene') 58 | vehicle.plot('input', knots=True, labels=['v_x (m/s)', 'v_y (m/s)']) 59 | 60 | # run it! 61 | simulator.run() 62 | -------------------------------------------------------------------------------- /examples/ros_example/readme.md: -------------------------------------------------------------------------------- 1 | # ROS integration example 2 | ## Prerequisites 3 | This example has updated to compatible with ROS melodic and Gazebo9. If you still using ROS Indogo and Gazebo2, you have to reset the commit back to https://github.com/meco-group/omg-tools/commit/beb4d3c78d2fb269671fde783b234d727b9c1fa1, because there are differences in xacro programming style between two versions. Please also check the previous README for usage if you reset the commit. 4 | 5 | 6 | Make sure you have the ROS packages *ros-control* and *ros-controllers*: 7 | ``` 8 | $ sudo apt install ros-melodic-ros-control ros-melodic-ros-controllers 9 | ``` 10 | For motion planner algorithm, recommended linear solver library is 'ma57', but this library is not available through 'apt' or 'pip', you have to compile it yourself, Please check https://github.com/casadi/casadi/wiki/Obtaining-HSL if you are interest. To check whether 'ma57' is available in your computer, you can run: 11 | ``` 12 | python /examples/p2p_agv.py 13 | ``` 14 | to see if it complains about the 'ma57' library missing. 15 | 16 | 17 | The substitution of 'ma57' is 'mumps', this changes already adressed here. 'mumps' is available through 'apt' 18 | ``` 19 | sudo apt install libmumps-dev 20 | ``` 21 | If you have 'ma57' library in your computer, go to line 77 and 82 in 'motionplanner.py', change the 'mumps' to 'ma57' to make palnner running faster. 22 | 23 | ## Building the example 24 | In the current folder (the one containing this readme): 25 | 26 | ``` 27 | $ catkin_make 28 | $ source devel/setup.bash 29 | ``` 30 | 31 | ## Running the example 32 | 33 | ``` 34 | $ roscore 35 | ``` 36 | ``` 37 | $ roslaunch p3dx_gazebo gazebo.launch 38 | ``` 39 | ``` 40 | $ roslaunch p3dx_motionplanner motionplanner.launch 41 | ``` 42 | -------------------------------------------------------------------------------- /examples/ros_example/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/melodic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(p3dx_description) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | rospy 10 | urdf 11 | ) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | # find_package(Boost REQUIRED COMPONENTS system) 15 | 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | ####################################### 23 | ## Declare ROS messages and services ## 24 | ####################################### 25 | 26 | ## Generate messages in the 'msg' folder 27 | # add_message_files( 28 | # FILES 29 | # Message1.msg 30 | # Message2.msg 31 | # ) 32 | 33 | ## Generate services in the 'srv' folder 34 | # add_service_files( 35 | # FILES 36 | # Service1.srv 37 | # Service2.srv 38 | # ) 39 | 40 | ## Generate added messages and services with any dependencies listed here 41 | # generate_messages( 42 | # DEPENDENCIES 43 | # std_msgs # Or other packages containing msgs 44 | # ) 45 | 46 | ################################### 47 | ## catkin specific configuration ## 48 | ################################### 49 | ## The catkin_package macro generates cmake config files for your package 50 | ## Declare things to be passed to dependent projects 51 | ## INCLUDE_DIRS: uncomment this if you package contains header files 52 | ## LIBRARIES: libraries you create in this project that dependent projects also need 53 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 54 | ## DEPENDS: system dependencies of this project that dependent projects also need 55 | catkin_package( 56 | # INCLUDE_DIRS include 57 | # LIBRARIES p3dx_description 58 | # CATKIN_DEPENDS roscpp rospy urdf 59 | # DEPENDS system_lib 60 | ) 61 | 62 | ########### 63 | ## Build ## 64 | ########### 65 | 66 | ## Specify additional locations of header files 67 | ## Your package locations should be listed before other locations 68 | # include_directories(include) 69 | include_directories( 70 | ${catkin_INCLUDE_DIRS} 71 | ) 72 | 73 | ## Declare a cpp library 74 | # add_library(p3dx_description 75 | # src/${PROJECT_NAME}/p3dx_description.cpp 76 | # ) 77 | 78 | ## Declare a cpp executable 79 | # add_executable(p3dx_description src/main.cpp) 80 | 81 | ## Add cmake target dependencies of the executable/library 82 | ## as an example, message headers may need to be generated before nodes 83 | # add_dependencies(p3dx_description_node p3dx_description_generate_messages_cpp) 84 | 85 | ## Specify libraries to link a library or executable target against 86 | # target_link_libraries(p3dx_description_node 87 | # ${catkin_LIBRARIES} 88 | # ) 89 | -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/meshes/Coordinates: -------------------------------------------------------------------------------- 1 | ========================================= 2 | Location Coordinates (x,y,z) 3 | ========================================= 4 | 5 | Chassis : -0.045 0 0.148 6 | Top : -0.045 0 0.234 7 | 8 | Swivel : -0.185 0 0.055 9 | Center Hubcap : -0.211 0 0.037 10 | Center Wheel : -0.211 0 0.038 11 | 12 | Right Hubcap : 0 0.158 0.091 13 | Right Wheel : 0 0.158 0.091 14 | 15 | Left Hubcap : 0 -0.158 0.091 16 | Left Wheel : 0 -0.158 0.091 17 | 18 | Front Sonar : -0.2 0 0.209 19 | Back Sonar : 0.109 0 0.209 20 | 21 | -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/meshes/back_rim.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/examples/ros_example/src/p3dx_description/meshes/back_rim.stl -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/meshes/back_sonar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/examples/ros_example/src/p3dx_description/meshes/back_sonar.stl -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/meshes/center_hubcap.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/examples/ros_example/src/p3dx_description/meshes/center_hubcap.stl -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/meshes/center_wheel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/examples/ros_example/src/p3dx_description/meshes/center_wheel.stl -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/meshes/chassis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/examples/ros_example/src/p3dx_description/meshes/chassis.stl -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/meshes/front_rim.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/examples/ros_example/src/p3dx_description/meshes/front_rim.stl -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/meshes/front_sonar.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/examples/ros_example/src/p3dx_description/meshes/front_sonar.stl -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/meshes/left_hubcap.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/examples/ros_example/src/p3dx_description/meshes/left_hubcap.stl -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/meshes/left_wheel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/examples/ros_example/src/p3dx_description/meshes/left_wheel.stl -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/meshes/right_hubcap.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/examples/ros_example/src/p3dx_description/meshes/right_hubcap.stl -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/meshes/right_wheel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/examples/ros_example/src/p3dx_description/meshes/right_wheel.stl -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/meshes/swivel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/examples/ros_example/src/p3dx_description/meshes/swivel.stl -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/meshes/top.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/meco-group/omg-tools/870ec4dd02c0ce45cbf5c5b9bc1983e5670eb9a9/examples/ros_example/src/p3dx_description/meshes/top.stl -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | p3dx_description 4 | 0.1.0 5 | 6 | A Pioneer 3DX description for Gazebo. First working version 7 | 8 | 9 | Rafael Berkvens 10 | GPLv3 11 | https://github.com/RafBerkvens/ua_ros_p3dx 12 | 13 | catkin 14 | roscpp 15 | rospy 16 | urdf 17 | roscpp 18 | rospy 19 | urdf 20 | -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/urdf/materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_description/urdf/pioneer3dx_wheel.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | pr2_mechanism_model/SimpleTransmission 108 | 109 | 110 | EffortJointInterface 111 | ${reflect * 624/35 * 80/19} 112 | 113 | 114 | 115 | 116 | Gazebo/Yellow 117 | 118 | 119 | 120 | Gazebo/Black 121 | 0.5 122 | 50.0 123 | 100000000.0 124 | 1.0 125 | 126 | 127 | 128 | 129 | 130 | 131 | -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(p3dx_gazebo) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | ##find_package(catkin REQUIRED COMPONENTS 8 | ## gazebo_ros 9 | ## p3dx_control 10 | ## p3dx_description 11 | ##) 12 | 13 | find_package(catkin REQUIRED COMPONENTS 14 | gazebo_ros 15 | p3dx_description 16 | ) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | # catkin_python_setup() 26 | 27 | ####################################### 28 | ## Declare ROS messages and services ## 29 | ####################################### 30 | 31 | ## Generate messages in the 'msg' folder 32 | # add_message_files( 33 | # FILES 34 | # Message1.msg 35 | # Message2.msg 36 | # ) 37 | 38 | ## Generate services in the 'srv' folder 39 | # add_service_files( 40 | # FILES 41 | # Service1.srv 42 | # Service2.srv 43 | # ) 44 | 45 | ## Generate added messages and services with any dependencies listed here 46 | # generate_messages( 47 | # DEPENDENCIES 48 | # std_msgs # Or other packages containing msgs 49 | # ) 50 | 51 | ################################### 52 | ## catkin specific configuration ## 53 | ################################### 54 | ## The catkin_package macro generates cmake config files for your package 55 | ## Declare things to be passed to dependent projects 56 | ## INCLUDE_DIRS: uncomment this if you package contains header files 57 | ## LIBRARIES: libraries you create in this project that dependent projects also need 58 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 59 | ## DEPENDS: system dependencies of this project that dependent projects also need 60 | catkin_package( 61 | # INCLUDE_DIRS include 62 | # LIBRARIES p3dx_gazebo 63 | # CATKIN_DEPENDS gazebo_ros p3dx_control p3dx_description 64 | # DEPENDS system_lib 65 | ) 66 | 67 | ########### 68 | ## Build ## 69 | ########### 70 | 71 | ## Specify additional locations of header files 72 | ## Your package locations should be listed before other locations 73 | # include_directories(include) 74 | include_directories( 75 | ${catkin_INCLUDE_DIRS} 76 | ) 77 | 78 | ## Declare a cpp library 79 | # add_library(p3dx_gazebo 80 | # src/${PROJECT_NAME}/p3dx_gazebo.cpp 81 | # ) 82 | 83 | ## Declare a cpp executable 84 | # add_executable(p3dx_gazebo_node src/p3dx_gazebo_node.cpp) 85 | 86 | ## Add cmake target dependencies of the executable/library 87 | ## as an example, message headers may need to be generated before nodes 88 | # add_dependencies(p3dx_gazebo_node p3dx_gazebo_generate_messages_cpp) 89 | 90 | ## Specify libraries to link a library or executable target against 91 | # target_link_libraries(p3dx_gazebo_node 92 | # ${catkin_LIBRARIES} 93 | # ) 94 | -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_gazebo/launch/gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_gazebo/launch/one_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 9 | -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | p3dx_gazebo 4 | 0.1.0 5 | 6 | The p3dx_gazebo package. This is a first working version. 7 | 8 | 9 | Rafael Berkvens 10 | GPLv3 11 | https://github.com/RafBerkvens/ua_ros_p3dx 12 | catkin 13 | gazebo_ros 14 | p3dx_description 15 | gazebo_ros 16 | p3dx_description 17 | 18 | -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_motionplanner/launch/motionplanner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_motionplanner/msg/FleetTrajectories.msg: -------------------------------------------------------------------------------- 1 | p3dx_motionplanner/P3DXTrajectory[] trajectories 2 | -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_motionplanner/msg/Obstacle.msg: -------------------------------------------------------------------------------- 1 | float32[] shape 2 | float32[] pose 3 | float32[] velocity 4 | -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_motionplanner/msg/P3DXPose.msg: -------------------------------------------------------------------------------- 1 | float32[] pose 2 | -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_motionplanner/msg/P3DXTrajectory.msg: -------------------------------------------------------------------------------- 1 | float32[] v_traj 2 | float32[] w_traj 3 | -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_motionplanner/msg/Room.msg: -------------------------------------------------------------------------------- 1 | float32[] position 2 | float32[] shape 3 | -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_motionplanner/msg/Settings.msg: -------------------------------------------------------------------------------- 1 | float32 sample_time 2 | float32 update_time 3 | p3dx_motionplanner/P3DXPose[] fleet_config 4 | p3dx_motionplanner/P3DXPose[] init_pose 5 | p3dx_motionplanner/P3DXPose[] terminal_pose 6 | Room room 7 | Obstacle[] obstacles 8 | int32[] robobst 9 | -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_motionplanner/msg/Trigger.msg: -------------------------------------------------------------------------------- 1 | p3dx_motionplanner/P3DXPose[] goal 2 | p3dx_motionplanner/P3DXPose[] state 3 | p3dx_motionplanner/Obstacle[] obstacles 4 | float32 current_time 5 | -------------------------------------------------------------------------------- /examples/ros_example/src/p3dx_motionplanner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | p3dx_motionplanner 4 | 0.0.0 5 | The p3dx_motionplanner package 6 | ruben 7 | LGPLv3 8 | message_generation 9 | message_runtime 10 | catkin 11 | roscpp 12 | rospy 13 | std_msgs 14 | roscpp 15 | rospy 16 | std_msgs 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /examples/schedulerproblem_example2.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | 21 | # This code shows an expansion of a normal multiframeproblem (schedulerproblem_example1.py), 22 | # in which a large environment is split over several frames. 23 | # In a multiframeproblem we only consider one room/frame at a time. 24 | # Using a schedulerproblem, you can combine frames, i.e. a trajectory is computed through multiple frames at a 25 | # time. The advantage is that e.g. moving obstacles around the corner can immediately be taken into account. 26 | # Note that when putting n_frames=1 you get exactly the same idea/behaviour as in the 27 | # p2p_multiframeproblem.py example. 28 | 29 | from omgtools import * 30 | 31 | # create vehicle 32 | vehicle = Holonomic(shapes = Circle(radius=0.5), options={'syslimit': 'norm_2', 'stop_tol': 1.e-2}, 33 | bounds={'vmax': 2, 'vmin':-2, 'amax':4, 'amin':-4}) 34 | veh_size = vehicle.shapes[0].radius 35 | 36 | # create environment 37 | start = [5,0] 38 | goal = [40,20] 39 | vehicle.set_initial_conditions(start) 40 | vehicle.set_terminal_conditions(goal) 41 | 42 | room1 = {'shape': Rectangle(width=60, height=30), 'position': [30, 10], 'draw':True} 43 | 44 | environment = Environment(room=room1) 45 | 46 | # create stationary obstacles 47 | 48 | rectangle = Rectangle(width=2., height=2) 49 | rectangle1 = Rectangle(width=0.2, height=0.2) 50 | 51 | obstacle1 = Obstacle({'position': [10,0]}, shape=rectangle) 52 | trajectories = {'velocity': {'time': [0.], 'values': [[0, -0.]]}} 53 | obstacle2 = Obstacle({'position': [22.5,12.5]}, shape=rectangle, simulation={'trajectories': trajectories}) 54 | 55 | # Warning: if you use fill room, then also fill the empty rooms with [] 56 | environment.fill_room(room1, [obstacle1, obstacle2]) 57 | 58 | # make global planner 59 | # [25,25] = number of cells in vertical and horizonal direction 60 | globalplanner = AStarPlanner(environment, [25,25], start, goal, options={'veh_size': veh_size}) 61 | 62 | # make problem 63 | # 'n_frames': number of frames to combine when searching for a trajectory 64 | # 'check_moving_obs_ts': check in steps of ts seconds if a moving obstacle is inside the frame 65 | # 'frame_type': 'corridor': creates corridors 66 | # 'scale_up_fine': tries to scale up the frame in small steps, leading to the largest possible corridor 67 | # 'l_shape': cuts off corridors, to obtain L-shapes, and minimize the influence of moving obstacles 68 | # 'frame_type': 'shift': creates frames of fixed size, around the vehicle 69 | # 'frame_size': size of the shifted frame 70 | options = {'frame_type': 'corridor', 'scale_up_fine': True, 'n_frames': 2, 'l_shape':True} 71 | schedulerproblem = SchedulerProblem(vehicle, environment, globalplanner, options=options) 72 | 73 | # simulate the problem 74 | simulator = Simulator(schedulerproblem) 75 | 76 | # define what you want to plot 77 | schedulerproblem.plot('scene') 78 | vehicle.plot('input', knots=True, prediction=True, labels=['v_x (m/s)', 'v_y (m/s)']) 79 | vehicle.plot('dinput', knots=True, prediction=True, labels=['a_x (m/s^2)', 'a_y (m/s^2)']) 80 | 81 | # run it! 82 | simulator.run() 83 | 84 | schedulerproblem.plot_movie('scene', number_of_frames=100, repeat=False) 85 | schedulerproblem.save_movie('scene', format='gif', name='schedulergif', number_of_frames=100, movie_time=10, axis=False) 86 | # schedulerproblem.save_movie('scene', number_of_frames=50, name='scheduler', axis=False) 87 | -------------------------------------------------------------------------------- /examples/schedulerproblem_example_dubins.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | # With fixed environment 21 | from omgtools import * 22 | 23 | # create vehicle 24 | vehicle = Dubins(shapes=Circle(radius=0.3), bounds={'vmax': 0.7, 'wmax': np.pi/3., 'wmin': -np.pi/3.}, # in rad/s 25 | options={'substitution': False}) 26 | veh_size = vehicle.shapes[0].radius 27 | vehicle.define_knots(knot_intervals=10) 28 | 29 | # create environment 30 | print('Using environment for known example') 31 | #for now fix environment to the one for which A*-path is known 32 | start = [2,2,0] 33 | goal = [8,8,0] 34 | vehicle.set_initial_conditions(start) 35 | vehicle.set_terminal_conditions(goal) 36 | 37 | envWidth = 10 38 | envHeight = 10 39 | environment = Environment(room={'shape': Rectangle(width=envWidth, height=envHeight), 'position': [5,5], 'draw':True}) 40 | 41 | rectangle = Rectangle(width=1, height=1) 42 | circle = Circle(radius=0.4) 43 | 44 | environment.add_obstacle(Obstacle({'position': [6,2]}, shape=rectangle)) 45 | environment.add_obstacle(Obstacle({'position': [4,2]}, shape=circle)) 46 | environment.add_obstacle(Obstacle({'position': [5,6]}, shape=circle)) 47 | 48 | # make global planner 49 | globalplanner = AStarPlanner(environment, [10,10], start, goal, options={'veh_size': veh_size}) 50 | 51 | # make schedulerproblem 52 | # 'n_frames': number of frames to combine when searching for a trajectory 53 | # 'check_moving_obs_ts': check in steps of ts seconds if a moving obstacle is inside the frame 54 | # 'frame_type': 'corridor': creates corridors 55 | # 'scale_up_fine': tries to scale up the frame in small steps, leading to the largest possible corridor 56 | # 'l_shape': cuts off corridors, to obtain L-shapes, and minimize the influence of moving obstacles 57 | # 'frame_type': 'shift': creates frames of fixed size, around the vehicle 58 | # 'frame_size': size of the shifted frame 59 | options={'freeT': True, 'horizon_time': 15, 'frame_type':'corridor','scale_up_fine': True, 'l_shape':True} 60 | multiproblem=SchedulerProblem(vehicle, environment, globalplanner, options=options) 61 | multiproblem.set_options({'solver_options': {'ipopt': {# 'ipopt.linear_solver': 'ma57', 62 | 'ipopt.hessian_approximation': 'limited-memory'}}}) 63 | multiproblem.init() 64 | 65 | simulator = Simulator(multiproblem) 66 | multiproblem.plot('scene') 67 | vehicle.plot('input', knots=True, prediction=True, labels=['v_x (m/s)', 'v_y (m/s)']) 68 | 69 | # run it! 70 | simulator.run() 71 | # multiproblem.save_movie('scene', format='gif', name='multiproblemgif', number_of_frames=100, movie_time=5, axis=False) 72 | # multiproblem.save_movie('scene', format='tikz', name='multiproblemtikz', number_of_frames=100, movie_time=5, axis=False) 73 | -------------------------------------------------------------------------------- /examples/testAstar.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | 21 | # This example finds a path through a room by dividing it into grid cells and using the 22 | # A-star algorithm. Place a breakpoint at line 39 to see the plot of the path. 23 | 24 | from omgtools import * 25 | 26 | start = [2,2] 27 | goal = [8,8] 28 | width = 10 29 | height = 10 30 | square_size = 0.5 31 | 32 | environment = Environment(room={'shape': Rectangle(width=width, height=height), 'position':[5,5]}) 33 | environment.add_obstacle(Obstacle({'position': [5, 5]}, shape=Rectangle(width=2,height=2))) 34 | 35 | planner = AStarPlanner(environment, [10,10], start, goal) 36 | waypoints = planner.get_path() 37 | planner.grid.draw() 38 | planner.plot_path(waypoints) 39 | # put breakpoint here to see the plot 40 | print(waypoints) -------------------------------------------------------------------------------- /examples/test_multiframe.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | 21 | # This example tests a separate multiframeproblem. Normally, a schedulerproblem creates 22 | # a multiframeproblem, and solves it with a receding horizon. This example allows testing 23 | # the behavior of a multiframeproblem on itself. Beware that this problem will not solve 24 | # completely if using simulator.run(), this is because the amount of frames that are coupled 25 | # stays the same. E.g. when coupling two frames, normally the first frame is dropped 26 | # (by the scheduler) as soon as the vehicle enters the second frame, this is not possible 27 | # when considering only a single multiframeproblem. Therefore, the vehicle cannot leave 28 | # the first frame. 29 | 30 | # The frames that are considered here are of the type 'corridor', i.e. frames that are as 31 | # large as possible, without containing any stationary obstacle. 32 | 33 | from omgtools import * 34 | 35 | # create vehicle 36 | vehicle = Holonomic(shapes = Circle(radius=0.5), options={'syslimit': 'norm_2', 'stop_tol': 1.e-2}, 37 | bounds={'vmax': 2, 'vmin':-2, 'amax':4, 'amin':-4}) 38 | # create environment 39 | start = [5,0] 40 | goal = [25,20] 41 | vehicle.set_initial_conditions(start) 42 | vehicle.set_terminal_conditions(goal) 43 | 44 | room1 = {'shape': Rectangle(width=30, height=10), 'position': [15, 0], 'draw':True} 45 | room2 = {'shape': Rectangle(width=10, height=30), 'position': [25, 10], 'draw':True} 46 | room3 = {'shape': Rectangle(width=40, height=10), 'position': [40, 20], 'draw':True} 47 | 48 | environment = Environment(room=[room1, room2]) 49 | 50 | # create stationary obstacles 51 | 52 | rectangle = Rectangle(width=2., height=2) 53 | rectangle1 = Rectangle(width=0.2, height=0.2) 54 | 55 | obstacle1 = Obstacle({'position': [10,0]}, shape=rectangle) 56 | trajectories = {'velocity': {'time': [0.], 'values': [[0, -0.]]}} 57 | obstacle2 = Obstacle({'position': [22.5,12.5]}, shape=rectangle, simulation={'trajectories': trajectories}) 58 | # environment.add_obstacle([obstacle1, obstacle2]) 59 | 60 | # Warning: if you use fill room, then also fill the empty rooms with [] 61 | environment.fill_room(room1, [])#[obstacle1]) 62 | environment.fill_room(room2, [])#[obstacle2]) 63 | # environment.fill_room(room3, []) 64 | 65 | # create moving obstacles 66 | # make global planner 67 | 68 | # make problem 69 | multiframeproblem=MultiFrameProblem(vehicle, environment, n_frames=2) 70 | multiframeproblem.init() 71 | 72 | # simulate the problem 73 | simulator = Simulator(multiframeproblem) 74 | 75 | # define what you want to plot 76 | multiframeproblem.plot('scene') 77 | # vehicle.plot('state') 78 | vehicle.plot('input', knots=True, prediction=True, labels=['v_x (m/s)', 'v_y (m/s)']) 79 | vehicle.plot('dinput') 80 | 81 | # run it! 82 | simulator.run_once() 83 | 84 | multiframeproblem.plot_movie('scene', number_of_frames=100, repeat=False) -------------------------------------------------------------------------------- /omgtools/__init__.py: -------------------------------------------------------------------------------- 1 | from .vehicles import * 2 | from .environment import * 3 | from .basics import * 4 | from .problems import * 5 | from .execution import * 6 | from .export import * 7 | from .gui import * 8 | 9 | 10 | def assert_ma57(): 11 | import casadi 12 | import sys 13 | 14 | x = casadi.MX.sym("x") 15 | nlp = casadi.nlpsol('nlp','ipopt',{"x":x,"f":x**2},{"ipopt": {"linear_solver": "ma57", "print_level":0}, "print_time": False}) 16 | 17 | nlp() 18 | 19 | if nlp.stats()["return_status"]=="Invalid_Option": 20 | print("Could not find ma57 (from hsl library), which is needed for the code to run.") 21 | sys.exit(0) 22 | -------------------------------------------------------------------------------- /omgtools/basics/__init__.py: -------------------------------------------------------------------------------- 1 | from .optilayer import OptiChild, OptiFather 2 | from .shape import * 3 | -------------------------------------------------------------------------------- /omgtools/environment/__init__.py: -------------------------------------------------------------------------------- 1 | from .environment import Environment 2 | from .obstacle import Obstacle 3 | from .frame import ShiftFrame, CorridorFrame 4 | -------------------------------------------------------------------------------- /omgtools/execution/__init__.py: -------------------------------------------------------------------------------- 1 | from .plotlayer import PlotLayer 2 | from .deployer import Deployer 3 | from .simulator import Simulator 4 | -------------------------------------------------------------------------------- /omgtools/export/__init__.py: -------------------------------------------------------------------------------- 1 | from .export import Export 2 | from .export_p2p import ExportP2P 3 | from .export_formation import ExportFormation 4 | from .export_rendezvous import ExportRendezVous 5 | from .export_admm import ExportADMM 6 | -------------------------------------------------------------------------------- /omgtools/export/export_formation.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from .export_admm import ExportADMM 21 | 22 | 23 | class ExportFormation(ExportADMM): 24 | 25 | def __init__(self, problem, options): 26 | ExportADMM.__init__(self, problem, options, 27 | ['point2point/admm/formation', 'tests/formation'], ['FormationPoint2Point.cpp']) 28 | 29 | def set_default_options(self): 30 | ExportADMM.set_default_options(self) 31 | self.options['executable'] = 'FormationPoint2Point' 32 | -------------------------------------------------------------------------------- /omgtools/export/export_p2p.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | import os 21 | import shutil 22 | from casadi import nlpsol 23 | from .export import Export 24 | 25 | 26 | class ExportP2P(Export): 27 | 28 | def __init__(self, problem, options): 29 | Export.__init__(self, problem, options) 30 | self.father = problem.father 31 | self.problem = problem 32 | if len(problem.vehicles) > 1: 33 | raise ValueError(('Only export for single vehicle ' + 34 | 'problems is supported.')) 35 | self.vehicle = problem.vehicles[0] 36 | src_files = ['Point2Point.cpp', 'Vehicle.cpp'] 37 | self.export(['point2point', 'vehicles', 'tests/point2point'], self.options['directory'], src_files, problem.father, problem) 38 | 39 | def set_default_options(self): 40 | Export.set_default_options(self) 41 | self.options['executable'] = 'Point2Point' 42 | 43 | def export_casadi_problems(self, destination, father, problem): 44 | filenames = Export.export_casadi_problems(self, destination, father, problem) 45 | obj = father.problem_description['obj'] 46 | con = father.problem_description['con'] 47 | var = father.problem_description['var'] 48 | par = father.problem_description['par'] 49 | opt = father.problem_description['opt'] 50 | nlp = {'x': var, 'p': par, 'f': obj, 'g': con} 51 | options = {} 52 | for key, value in opt['solver_options']['ipopt'].items(): 53 | options[key] = value 54 | options.update({'expand': True}) 55 | solver = nlpsol('solver', 'ipopt', nlp, options) 56 | solver.generate_dependencies('nlp.c') 57 | cwd = os.getcwd() 58 | shutil.move(cwd+'/nlp.c', destination+'src/nlp.c') 59 | filenames.append('nlp.c') 60 | return filenames 61 | -------------------------------------------------------------------------------- /omgtools/export/export_rendezvous.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from .export_admm import ExportADMM 21 | 22 | 23 | class ExportRendezVous(ExportADMM): 24 | 25 | def __init__(self, problem, options): 26 | ExportADMM.__init__(self, problem, options, 27 | ['point2point/admm/rendezvous', 'tests/rendezvous'], ['RendezVous.cpp']) 28 | 29 | def set_default_options(self): 30 | ExportADMM.set_default_options(self) 31 | self.options['executable'] = 'RendezVous' 32 | -------------------------------------------------------------------------------- /omgtools/export/point2point/Makefile: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | 21 | #------------------------------------------------------------------------------ 22 | # Change this 23 | #------------------------------------------------------------------------------ 24 | CASADILIB = @casadilib@ 25 | CASADIINC = @casadiinc@ 26 | CASADIOBJ = @casadiobj@ 27 | SOURCEFILES = @sourcefiles@ 28 | EXECUTABLE = @executable@ 29 | #------------------------------------------------------------------------------ 30 | 31 | CXX = g++ 32 | CC = gcc 33 | INCDIR = $(CASADIINC) 34 | LIBDIR = $(CASADILIB) 35 | CXXFLAGS = -I$(INCDIR) -DUSE_CXX11 -std=c++11 -fPIC -std=c++11 -DCASADIOBJ="\"$(CASADIOBJ)\"" 36 | CFLAGS = -fPIC -shared -std=c99 37 | LDFLAGS = -L$(LIBDIR) -lcasadi -lstdc++ -lcasadi -Wl,-rpath $(LIBDIR) 38 | PROBSOURCES = @probsources@ 39 | PROBOBJECTS = $(PROBSOURCES:.c=.so) 40 | SOURCES = $(SOURCEFILES) @sourcefiles2@ 41 | OBJECTS = $(SOURCES:.cpp=.o) 42 | LIB = @libname@ 43 | SRCDIR = src 44 | OBJDIR = obj 45 | OUTDIR = bin 46 | OBJ = $(patsubst %,$(OBJDIR)/%,$(OBJECTS)) 47 | PROBOBJ = $(patsubst %,$(OUTDIR)/%,$(PROBOBJECTS)) 48 | 49 | all: dir $(OUTDIR)/$(EXECUTABLE) 50 | 51 | lib: dir $(OUTDIR)/$(LIB) 52 | 53 | libdebug: DEBUG = -DDEBUG 54 | libdebug: lib 55 | 56 | debug: DEBUG = -DDEBUG 57 | debug: all 58 | 59 | $(OUTDIR)/%.so: $(SRCDIR)/%.c 60 | $(CC) $(CFLAGS) $< -o $@ 61 | 62 | $(OBJDIR)/%.o: $(SRCDIR)/%.cpp 63 | $(CXX) -c -o $@ $< $(CXXFLAGS) $(DEBUG) 64 | 65 | $(OUTDIR)/$(EXECUTABLE): $(OBJ) $(PROBOBJ) 66 | $(CXX) -o $@ $^ $(CXXFLAGS) $(DEBUG) $(LDFLAGS) 67 | 68 | $(OUTDIR)/$(LIB): $(OBJ) $(PROBOBJ) 69 | $(CXX) -o $@ $^ $(CXXFLAGS) -shared $(DEBUG) $(LDFLAGS) 70 | 71 | dir: 72 | mkdir -p $(OUTDIR) 73 | mkdir -p $(OBJDIR) 74 | 75 | .PHONY: clean 76 | 77 | clean: 78 | rm -rf $(OBJDIR) $(OUTDIR) 79 | -------------------------------------------------------------------------------- /omgtools/export/point2point/admm/ADMMPoint2Point.hpp: -------------------------------------------------------------------------------- 1 | // This file is part of OMG-tools. 2 | 3 | // OMG-tools -- Optimal Motion Generation-tools 4 | // Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | // All rights reserved. 6 | 7 | // OMG-tools is free software; you can redistribute it and/or 8 | // modify it under the terms of the GNU Lesser General Public 9 | // License as published by the Free Software Foundation; either 10 | // version 3 of the License, or (at your option) any later version. 11 | // This software is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | // Lesser General Public License for more details. 15 | 16 | // You should have received a copy of the GNU Lesser General Public 17 | // License along with this program; if not, write to the Free Software 18 | // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | #ifndef ADMMPOINT2POINT 21 | #define ADMMPOINT2POINT 22 | 23 | #include "Point2Point.hpp" 24 | 25 | namespace omg{ 26 | 27 | class ADMMPoint2Point: public Point2Point{ 28 | private: 29 | int iteration=0; 30 | int init_iter; 31 | casadi::Function updx_problem; 32 | casadi::Function updz_problem; 33 | casadi::Function updl_problem; 34 | casadi::Function get_residuals; 35 | std::map> variables_admm; 36 | std::vector residuals; 37 | double rho; 38 | 39 | const int n_nghb = N_NGHB; 40 | 41 | bool solveUpdx(double, std::vector&); 42 | bool solveUpdz(); 43 | bool solveUpdl(); 44 | bool computeResiduals(); 45 | void initVariablesADMM(); 46 | void extractData(); 47 | void retrieveSharedVariables(std::map>>&); 48 | void transformSharedSplines(double, double); 49 | void setParameters(std::vector&); 50 | virtual void generateProblem(); 51 | 52 | protected: 53 | virtual void fillParameterDict(std::vector&, std::map>>&); 54 | virtual bool update1(std::vector&, std::vector&, std::vector>&, std::vector>&, std::vector&, std::vector>&, std::vector>&, std::vector&); 55 | virtual bool update1(std::vector&, std::vector&, std::vector>&, std::vector>&, std::vector&, std::vector>&, std::vector>&, std::vector&, int); 56 | virtual bool update2(std::vector>&, std::vector>&, std::vector>&, std::vector&); 57 | 58 | public: 59 | const int n_shared = N_SHARED; 60 | ADMMPoint2Point(Vehicle* vehicle, double update_time, double sample_time, double horizon_time); 61 | ADMMPoint2Point(Vehicle* vehicle, double update_time, double sample_time, double horizon_time, int trajectory_length); 62 | ADMMPoint2Point(Vehicle* vehicle, double update_time, double sample_time, double horizon_time, int trajectory_length, int init_iter); 63 | ADMMPoint2Point(Vehicle* vehicle, double update_time, double sample_time, double horizon_time, int trajectory_length, int init_iter, double rho); 64 | virtual void reset(); 65 | virtual void resetTime(); 66 | int getIteration(); 67 | double getCurrentTime(); 68 | void stepBack(); 69 | }; 70 | } 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /omgtools/export/point2point/admm/formation/FormationPoint2Point.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of OMG-tools. 2 | 3 | // OMG-tools -- Optimal Motion Generation-tools 4 | // Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | // All rights reserved. 6 | 7 | // OMG-tools is free software; you can redistribute it and/or 8 | // modify it under the terms of the GNU Lesser General Public 9 | // License as published by the Free Software Foundation; either 10 | // version 3 of the License, or (at your option) any later version. 11 | // This software is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | // Lesser General Public License for more details. 15 | 16 | // You should have received a copy of the GNU Lesser General Public 17 | // License along with this program; if not, write to the Free Software 18 | // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | #include "FormationPoint2Point.hpp" 21 | 22 | using namespace std; 23 | using namespace casadi; 24 | 25 | namespace omg{ 26 | 27 | FormationPoint2Point::FormationPoint2Point(Vehicle* vehicle, 28 | double update_time, double sample_time, double horizon_time, 29 | int trajectory_length, int init_iter, double rho): 30 | ADMMPoint2Point(vehicle, update_time, sample_time, horizon_time, trajectory_length, init_iter, rho) { 31 | 32 | } 33 | 34 | FormationPoint2Point::FormationPoint2Point(Vehicle* vehicle, 35 | double update_time, double sample_time, double horizon_time): 36 | ADMMPoint2Point(vehicle, update_time, sample_time, horizon_time){ 37 | } 38 | 39 | FormationPoint2Point::FormationPoint2Point(Vehicle* vehicle, 40 | double update_time, double sample_time, double horizon_time, 41 | int trajectory_length): 42 | ADMMPoint2Point(vehicle, update_time, sample_time, horizon_time, trajectory_length){ 43 | } 44 | 45 | FormationPoint2Point::FormationPoint2Point(Vehicle* vehicle, 46 | double update_time, double sample_time, double horizon_time, 47 | int trajectory_length, int init_iter): 48 | ADMMPoint2Point(vehicle, update_time, sample_time, horizon_time, trajectory_length, init_iter){ 49 | } 50 | 51 | bool FormationPoint2Point::update1(vector& condition0, vector& conditionT, 52 | vector>& state_trajectory, vector>& input_trajectory, 53 | vector& x_var, vector>& z_ji_var, vector>& l_ji_var, 54 | vector& obstacles, vector& rel_pos_c){ 55 | update1(condition0, conditionT, state_trajectory, input_trajectory, x_var, z_ji_var, l_ji_var, obstacles, rel_pos_c, 0); 56 | } 57 | 58 | bool FormationPoint2Point::update1(vector& condition0, vector& conditionT, 59 | vector>& state_trajectory, vector>& input_trajectory, 60 | vector& x_var, vector>& z_ji_var, vector>& l_ji_var, 61 | vector& obstacles, vector& rel_pos_c, int predict_shift){ 62 | this->rel_pos_c = rel_pos_c; 63 | ADMMPoint2Point::update1(condition0, conditionT, state_trajectory, input_trajectory, 64 | x_var, z_ji_var, l_ji_var, obstacles, predict_shift); 65 | } 66 | 67 | bool FormationPoint2Point::update2(vector>& x_j_var, 68 | vector>& z_ij_var, vector>& l_ij_var, 69 | vector& residuals){ 70 | ADMMPoint2Point::update2(x_j_var, z_ij_var, l_ij_var, residuals); 71 | } 72 | 73 | void FormationPoint2Point::fillParameterDict(vector& obstacles, map>>& par_dict){ 74 | ADMMPoint2Point::fillParameterDict(obstacles, par_dict); 75 | par_dict[VEHICLELBL]["rel_pos_c"] = this->rel_pos_c; 76 | } 77 | 78 | } 79 | -------------------------------------------------------------------------------- /omgtools/export/point2point/admm/formation/FormationPoint2Point.hpp: -------------------------------------------------------------------------------- 1 | // This file is part of OMG-tools. 2 | 3 | // OMG-tools -- Optimal Motion Generation-tools 4 | // Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | // All rights reserved. 6 | 7 | // OMG-tools is free software; you can redistribute it and/or 8 | // modify it under the terms of the GNU Lesser General Public 9 | // License as published by the Free Software Foundation; either 10 | // version 3 of the License, or (at your option) any later version. 11 | // This software is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | // Lesser General Public License for more details. 15 | 16 | // You should have received a copy of the GNU Lesser General Public 17 | // License along with this program; if not, write to the Free Software 18 | // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | #ifndef FORMATIONPOINT2POINT 21 | #define FORMATIONPOINT2POINT 22 | 23 | #include "ADMMPoint2Point.hpp" 24 | 25 | namespace omg{ 26 | 27 | class FormationPoint2Point: public ADMMPoint2Point{ 28 | private: 29 | std::vector rel_pos_c; 30 | void fillParameterDict(std::vector&, std::map>>&); 31 | 32 | public: 33 | FormationPoint2Point(Vehicle* vehicle, double update_time, double sample_time, double horizon_time); 34 | FormationPoint2Point(Vehicle* vehicle, double update_time, double sample_time, double horizon_time, int trajectory_length); 35 | FormationPoint2Point(Vehicle* vehicle, double update_time, double sample_time, double horizon_time, int trajectory_length, int init_iter); 36 | FormationPoint2Point(Vehicle* vehicle, double update_time, double sample_time, double horizon_time, int trajectory_length, int init_iter, double rho); 37 | bool update1(std::vector&, std::vector&, std::vector>&, std::vector>&, std::vector&, std::vector>&, std::vector>&, std::vector&, std::vector &); 38 | bool update1(std::vector&, std::vector&, std::vector>&, std::vector>&, std::vector&, std::vector>&, std::vector>&, std::vector&, std::vector &, int); 39 | bool update2(std::vector>&, std::vector>&, std::vector>&, std::vector&); 40 | }; 41 | } 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /omgtools/export/point2point/admm/rendezvous/RendezVous.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of OMG-tools. 2 | 3 | // OMG-tools -- Optimal Motion Generation-tools 4 | // Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | // All rights reserved. 6 | 7 | // OMG-tools is free software; you can redistribute it and/or 8 | // modify it under the terms of the GNU Lesser General Public 9 | // License as published by the Free Software Foundation; either 10 | // version 3 of the License, or (at your option) any later version. 11 | // This software is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | // Lesser General Public License for more details. 15 | 16 | // You should have received a copy of the GNU Lesser General Public 17 | // License along with this program; if not, write to the Free Software 18 | // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | #include "RendezVous.hpp" 21 | 22 | using namespace std; 23 | using namespace casadi; 24 | 25 | namespace omg{ 26 | 27 | RendezVous::RendezVous(Vehicle* vehicle, 28 | double update_time, double sample_time, double horizon_time, 29 | int trajectory_length, int init_iter, double rho): 30 | ADMMPoint2Point(vehicle, update_time, sample_time, horizon_time, trajectory_length, init_iter, rho) { 31 | 32 | } 33 | 34 | RendezVous::RendezVous(Vehicle* vehicle, 35 | double update_time, double sample_time, double horizon_time): 36 | ADMMPoint2Point(vehicle, update_time, sample_time, horizon_time){ 37 | } 38 | 39 | RendezVous::RendezVous(Vehicle* vehicle, 40 | double update_time, double sample_time, double horizon_time, 41 | int trajectory_length): 42 | ADMMPoint2Point(vehicle, update_time, sample_time, horizon_time, trajectory_length){ 43 | } 44 | 45 | RendezVous::RendezVous(Vehicle* vehicle, 46 | double update_time, double sample_time, double horizon_time, 47 | int trajectory_length, int init_iter): 48 | ADMMPoint2Point(vehicle, update_time, sample_time, horizon_time, trajectory_length, init_iter){ 49 | } 50 | 51 | bool RendezVous::update1(vector& condition0, vector& conditionT, 52 | vector>& state_trajectory, vector>& input_trajectory, 53 | vector& x_var, vector>& z_ji_var, vector>& l_ji_var, 54 | vector& obstacles, vector& rel_pos_c){ 55 | update1(condition0, conditionT, state_trajectory, input_trajectory, x_var, z_ji_var, l_ji_var, obstacles, rel_pos_c, 0); 56 | } 57 | 58 | bool RendezVous::update1(vector& condition0, vector& conditionT, 59 | vector>& state_trajectory, vector>& input_trajectory, 60 | vector& x_var, vector>& z_ji_var, vector>& l_ji_var, 61 | vector& obstacles, vector& rel_pos_c, int predict_shift){ 62 | this->rel_pos_c = rel_pos_c; 63 | ADMMPoint2Point::update1(condition0, conditionT, state_trajectory, input_trajectory, 64 | x_var, z_ji_var, l_ji_var, obstacles, predict_shift); 65 | } 66 | 67 | bool RendezVous::update2(vector>& x_j_var, 68 | vector>& z_ij_var, vector>& l_ij_var, 69 | vector& residuals){ 70 | ADMMPoint2Point::update2(x_j_var, z_ij_var, l_ij_var, residuals); 71 | } 72 | 73 | void RendezVous::fillParameterDict(vector& obstacles, map>>& par_dict){ 74 | ADMMPoint2Point::fillParameterDict(obstacles, par_dict); 75 | par_dict[VEHICLELBL]["rel_pos_c"] = this->rel_pos_c; 76 | } 77 | 78 | } 79 | -------------------------------------------------------------------------------- /omgtools/export/point2point/admm/rendezvous/RendezVous.hpp: -------------------------------------------------------------------------------- 1 | // This file is part of OMG-tools. 2 | 3 | // OMG-tools -- Optimal Motion Generation-tools 4 | // Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | // All rights reserved. 6 | 7 | // OMG-tools is free software; you can redistribute it and/or 8 | // modify it under the terms of the GNU Lesser General Public 9 | // License as published by the Free Software Foundation; either 10 | // version 3 of the License, or (at your option) any later version. 11 | // This software is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | // Lesser General Public License for more details. 15 | 16 | // You should have received a copy of the GNU Lesser General Public 17 | // License along with this program; if not, write to the Free Software 18 | // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | #ifndef RENDEZVOUS 21 | #define RENDEZVOUS 22 | 23 | #include "ADMMPoint2Point.hpp" 24 | 25 | namespace omg{ 26 | 27 | class RendezVous: public ADMMPoint2Point{ 28 | private: 29 | std::vector rel_pos_c; 30 | void fillParameterDict(std::vector&, std::map>>&); 31 | 32 | public: 33 | RendezVous(Vehicle* vehicle, double update_time, double sample_time, double horizon_time); 34 | RendezVous(Vehicle* vehicle, double update_time, double sample_time, double horizon_time, int trajectory_length); 35 | RendezVous(Vehicle* vehicle, double update_time, double sample_time, double horizon_time, int trajectory_length, int init_iter); 36 | RendezVous(Vehicle* vehicle, double update_time, double sample_time, double horizon_time, int trajectory_length, int init_iter, double rho); 37 | bool update1(std::vector&, std::vector&, std::vector>&, std::vector>&, std::vector&, std::vector>&, std::vector>&, std::vector&, std::vector &); 38 | bool update1(std::vector&, std::vector&, std::vector>&, std::vector>&, std::vector&, std::vector>&, std::vector>&, std::vector&, std::vector &, int); 39 | bool update2(std::vector>&, std::vector>&, std::vector>&, std::vector&); 40 | }; 41 | } 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /omgtools/export/point2point/example.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of OMG-tools. 2 | 3 | // OMG-tools -- Optimal Motion Generation-tools 4 | // Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | // All rights reserved. 6 | 7 | // OMG-tools is free software; you can redistribute it and/or 8 | // modify it under the terms of the GNU Lesser General Public 9 | // License as published by the Free Software Foundation; either 10 | // version 3 of the License, or (at your option) any later version. 11 | // This software is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | // Lesser General Public License for more details. 15 | 16 | // You should have received a copy of the GNU Lesser General Public 17 | // License along with this program; if not, write to the Free Software 18 | // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | #include "Point2Point.hpp" 21 | #include "Holonomic.hpp" 22 | #include 23 | 24 | using namespace std; 25 | 26 | int main() 27 | { 28 | double horizon_time = 10; 29 | double sample_time = 0.01; 30 | double update_time = 0.1; 31 | int trajectory_length = 20; 32 | omg::Holonomic* vehicle = new omg::Holonomic(); 33 | // ideal update: prediction of initial state based on spline extrapolation 34 | // non-ideal update: prediction based on current state0 and model integration 35 | vehicle->setIdealPrediction(true); 36 | omg::Point2Point p2p(vehicle, update_time, sample_time, horizon_time, trajectory_length); 37 | 38 | // set initial state and terminal state and input 39 | vector state0(2); 40 | vector stateT(2); 41 | for (int i=0; i<2; i++){ 42 | state0.at(i) = 0.0; 43 | stateT.at(i) = 4.0; 44 | } 45 | 46 | // these will store the state and input trajectory 47 | vector> input_trajectory(trajectory_length, vector(2)); 48 | vector> state_trajectory(trajectory_length, vector(2)); 49 | 50 | // obstacles 51 | vector obstacles(p2p.n_obs); 52 | obstacles[0].position[0] = 0.5; 53 | obstacles[0].position[1] = 2.0; 54 | obstacles[0].velocity[0] = 0.0; 55 | obstacles[0].velocity[1] = 0.0; 56 | obstacles[0].acceleration[0] = 0.0; 57 | obstacles[0].acceleration[1] = 0.0; 58 | 59 | obstacles[1].position[0] = 4.2; 60 | obstacles[1].position[1] = 2.0; 61 | obstacles[1].velocity[0] = 0.0; 62 | obstacles[1].velocity[1] = 0.0; 63 | obstacles[1].acceleration[0] = 0.0; 64 | obstacles[1].acceleration[1] = 0.0; 65 | 66 | double time; 67 | for (int i=0; i<4; i++){ 68 | clock_t begin = clock(); 69 | p2p.update(state0, stateT, state_trajectory, input_trajectory, obstacles); 70 | clock_t end = clock(); 71 | time = double(end-begin)/CLOCKS_PER_SEC; 72 | cout << "it: " << i << ", " << "time: " << time << "s" << endl; 73 | } 74 | } 75 | 76 | -------------------------------------------------------------------------------- /omgtools/export/point2point/instructions.txt: -------------------------------------------------------------------------------- 1 | INSTRUCTIONS 2 | ============ 3 | Ruben Van Parys - March 2016 4 | 5 | Build instructions 6 | ------------------ 7 | 8 | * Assign the upper variables in the Makefile 9 | * CASADIDIR: path of your casadi binary installation. If you built casadi 10 | from sources, you can leave this blank. 11 | * CASADIOBJ: path of your generated casadi functions object files (grad_f.so, 12 | jac_g.so, hess_lag.so and nlp.so) 13 | * SOURCEFILES: extra cpp files that you want to built with the toolbox. If 14 | you decide to build the toolbox as library (.so), you can 15 | leave this blank. 16 | * EXECUTABLE: name of resulting executable. 17 | 18 | * Build the source files 19 | * Build to executable: make 20 | * Build to library (.so): make lib 21 | in the bin/ directory you can find your executable or library 22 | 23 | * Link the library with your project (only if built to .so) 24 | * You built casadi from sources: 25 | Suppose you have set the following variables 26 | * MPINCL: path to directory containing MotionPlanning.hpp 27 | * MPLIB: path to directory containing libmp.so 28 | then you can build your application (suppose application.cpp) with our 29 | toolbox as follows (eg. with g++): 30 | g++ -I$(MPINCL) -std=c++11 -o application -L$(MPLIB) -Wl,-rpath=$(MPLIB) application.cpp -lomg -lcasadi 31 | * You istalled the casadi binary: 32 | You need to tell the compiler where he can find the header files 33 | and the libcasadi.so library. These are located respectively in the 34 | include/ and casadi/ subfolder of you casadi binary path. 35 | Suppose you have set the following variables: 36 | * CASADIINCL: path to casadi include/ subfolder 37 | * CASADILIB: path to casadi casadi/ subfolder 38 | then you can build your application (suppose application.cpp) with our 39 | toolbox as follows (eg. with g++): 40 | g++ -I$(CASADIINCL) -I$(MPINCL) -std=c++11 -o application -L$(CASADILIB) -L$(MPLIB) -Wl,-rpath=$(MPLIB) application.cpp -lomg -lcasadi 41 | 42 | Example code 43 | ------------ 44 | 45 | Check out example.cpp to find out how you can use the exported c++ toolbox. 46 | -------------------------------------------------------------------------------- /omgtools/export/vehicles/Holonomic.cpp: -------------------------------------------------------------------------------- 1 | // This file is part of OMG-tools. 2 | 3 | // OMG-tools -- Optimal Motion Generation-tools 4 | // Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | // All rights reserved. 6 | 7 | // OMG-tools is free software; you can redistribute it and/or 8 | // modify it under the terms of the GNU Lesser General Public 9 | // License as published by the Free Software Foundation; either 10 | // version 3 of the License, or (at your option) any later version. 11 | // This software is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | // Lesser General Public License for more details. 15 | 16 | // You should have received a copy of the GNU Lesser General Public 17 | // License along with this program; if not, write to the Free Software 18 | // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | #include "Holonomic.hpp" 21 | 22 | using namespace std; 23 | 24 | namespace omg{ 25 | 26 | Holonomic::Holonomic() : Vehicle(2, 2, 2, 3), poseT(2){ 27 | 28 | } 29 | 30 | void Holonomic::setInitialConditions(vector& conditions){ 31 | vector zeros(2); 32 | setPrediction(conditions, zeros); 33 | } 34 | 35 | void Holonomic::setTerminalConditions(vector& conditions){ 36 | this->poseT = conditions; 37 | } 38 | 39 | void Holonomic::getInitSplineValue(vector>& init_value){ 40 | vector state0(2); 41 | vector input0(2); 42 | int n_spl = getNSplines(); 43 | int degree = getDegree(); 44 | int len_basis = getLenBasis(); 45 | getPrediction(state0, input0); 46 | // for(int k=0; kposeT[k]; 50 | // } 51 | // for (int j=0; jposeT[k]-state0[k])/(len_basis-2*degree-1); 53 | // } 54 | // } 55 | for(int k=0; kposeT[k]-state0[k])/(len_basis-1); 58 | } 59 | } 60 | } 61 | 62 | void Holonomic::setParameters(map>& parameters){ 63 | vector state0(2); 64 | vector input0(2); 65 | getPrediction(state0, input0); 66 | parameters["state0"] = state0; 67 | parameters["input0"] = input0; 68 | parameters["poseT"] = this->poseT; 69 | } 70 | 71 | void Holonomic::splines2State(vector>& spline_coeffs, vector time, vector>& state){ 72 | this->sampleSplines(spline_coeffs, time, state); 73 | } 74 | 75 | void Holonomic::splines2Input(vector>& spline_coeffs, vector time, vector>& input){ 76 | this->sampleSplines(spline_coeffs, time, 1, input); 77 | } 78 | 79 | void Holonomic::ode(vector& state, vector& input, vector& dstate){ 80 | dstate = input; 81 | } 82 | 83 | 84 | } 85 | -------------------------------------------------------------------------------- /omgtools/export/vehicles/Holonomic.hpp: -------------------------------------------------------------------------------- 1 | // This file is part of OMG-tools. 2 | 3 | // OMG-tools -- Optimal Motion Generation-tools 4 | // Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | // All rights reserved. 6 | 7 | // OMG-tools is free software; you can redistribute it and/or 8 | // modify it under the terms of the GNU Lesser General Public 9 | // License as published by the Free Software Foundation; either 10 | // version 3 of the License, or (at your option) any later version. 11 | // This software is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | // Lesser General Public License for more details. 15 | 16 | // You should have received a copy of the GNU Lesser General Public 17 | // License along with this program; if not, write to the Free Software 18 | // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | #ifndef HOLONOMIC 21 | #define HOLONOMIC 22 | 23 | #include "Vehicle.hpp" 24 | 25 | namespace omg{ 26 | 27 | class Holonomic: public Vehicle{ 28 | private: 29 | std::vector poseT; 30 | 31 | public: 32 | Holonomic(); 33 | 34 | void setInitialConditions(std::vector& conditions); 35 | void setTerminalConditions(std::vector& conditions); 36 | void setParameters(std::map>& par_dict); 37 | void ode(std::vector& state, std::vector& input, std::vector& dstate); 38 | void getInitSplineValue(std::vector>& init_value); 39 | void splines2State(std::vector>& spline_coeffs, std::vector time, std::vector>& state); 40 | void splines2Input(std::vector>& spline_coeffs, std::vector time, std::vector>& input); 41 | }; 42 | } 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /omgtools/export/vehicles/Vehicle.hpp: -------------------------------------------------------------------------------- 1 | // This file is part of OMG-tools. 2 | 3 | // OMG-tools -- Optimal Motion Generation-tools 4 | // Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | // All rights reserved. 6 | 7 | // OMG-tools is free software; you can redistribute it and/or 8 | // modify it under the terms of the GNU Lesser General Public 9 | // License as published by the Free Software Foundation; either 10 | // version 3 of the License, or (at your option) any later version. 11 | // This software is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | // Lesser General Public License for more details. 15 | 16 | // You should have received a copy of the GNU Lesser General Public 17 | // License along with this program; if not, write to the Free Software 18 | // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | #ifndef VEHICLE 21 | #define VEHICLE 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | namespace omg{ 28 | 29 | class Vehicle{ 30 | private: 31 | int n_st; 32 | int n_in; 33 | int n_spl; 34 | int degree; 35 | int len_basis; 36 | int knot_intervals; 37 | bool ideal_prediction; 38 | bool provide_prediction; 39 | double horizon_time; 40 | std::vector knots; 41 | std::vector predicted_state; 42 | std::vector predicted_input; 43 | std::vector> state_trajectory; 44 | std::vector> input_trajectory; 45 | std::vector>> derivative_T; 46 | 47 | void integrate(std::vector& state0, std::vector>& input, std::vector& stateT, double sample_time, int steps); 48 | double evalSpline(double x, std::vector& knots, std::vector& coeffs, int degree); 49 | void createDerivativeMatrices(); 50 | 51 | protected: 52 | void sampleSplines(std::vector>& spline_coeffs, std::vector time, int derivative, std::vector>& spline_sampled); 53 | void sampleSplines(std::vector>& spline_coeffs, std::vector time, std::vector>& spline_sampled); 54 | void getPrediction(std::vector& state, std::vector& input); 55 | void setPrediction(std::vector& state, std::vector& input); 56 | 57 | public: 58 | virtual void setInitialConditions(std::vector& conditions) = 0; 59 | virtual void setTerminalConditions(std::vector& conditions) = 0; 60 | virtual void getInitSplineValue(std::vector>& init_value) = 0; 61 | virtual void setParameters(std::map>& par_dict) = 0; 62 | virtual void ode(std::vector& state, std::vector& input, std::vector& dstate) = 0; 63 | virtual void splines2State(std::vector>& spline_coeffs, std::vector time, std::vector>& state) = 0; 64 | virtual void splines2Input(std::vector>& spline_coeffs, std::vector time, std::vector>& input) = 0; 65 | 66 | Vehicle(int n_st, int n_in, int n_spl, int degree, int knot_intervals); 67 | Vehicle(int n_st, int n_in, int n_spl, int degree); 68 | 69 | void predict(std::vector& state0, std::vector>& state_trajectory, std::vector>& input_trajectory, double predict_time, double sample_time, int predict_shift); 70 | void setKnotHorizon(double horizon_time); 71 | void setIdealPrediction(bool ideal_prediction); 72 | void setProvidePrediction(bool provide_prediction); 73 | int getNSplines(); 74 | int getNState(); 75 | int getNInput(); 76 | int getLenBasis(); 77 | int getDegree(); 78 | int getKnotIntervals(); 79 | }; 80 | } 81 | 82 | #endif 83 | -------------------------------------------------------------------------------- /omgtools/gui/__init__.py: -------------------------------------------------------------------------------- 1 | from .gui import * 2 | from .svg_reader import SVGReader 3 | from .gcode_reader import GCodeReader 4 | from .gcode_block import GCodeBlock 5 | -------------------------------------------------------------------------------- /omgtools/problems/__init__.py: -------------------------------------------------------------------------------- 1 | from .point2point import Point2point 2 | from .formation import FormationPoint2point 3 | from .rendezvous import RendezVous 4 | from .formation_central import FormationPoint2pointCentral 5 | from .formation_dualdec import FormationPoint2pointDualDecomposition 6 | from .schedulerproblem import SchedulerProblem 7 | from .multiframeproblem import MultiFrameProblem 8 | from .globalplanner import * 9 | from .gcodeproblem import GCodeProblem 10 | from .gcodeschedulerproblem import GCodeSchedulerProblem -------------------------------------------------------------------------------- /omgtools/problems/rendezvous.py: -------------------------------------------------------------------------------- 1 | # This file is part of OMG-tools. 2 | # 3 | # OMG-tools -- Optimal Motion Generation-tools 4 | # Copyright (C) 2016 Ruben Van Parys & Tim Mercy, KU Leuven. 5 | # All rights reserved. 6 | # 7 | # OMG-tools is free software; you can redistribute it and/or 8 | # modify it under the terms of the GNU Lesser General Public 9 | # License as published by the Free Software Foundation; either 10 | # version 3 of the License, or (at your option) any later version. 11 | # This software is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | # Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public 17 | # License along with this program; if not, write to the Free Software 18 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 19 | 20 | from .admm import ADMMProblem 21 | from .point2point import FreeEndPoint2point 22 | from ..export.export_rendezvous import ExportRendezVous 23 | import numpy as np 24 | 25 | 26 | class RendezVous(ADMMProblem): 27 | 28 | def __init__(self, fleet, environment, options=None): 29 | problems = [] 30 | for veh in fleet.vehicles: 31 | free_ind = list(fleet.configuration[veh].keys()) 32 | problems.append( 33 | FreeEndPoint2point(veh, environment.copy(), options, {veh: free_ind})) 34 | ADMMProblem.__init__(self, fleet, environment, problems, options) 35 | 36 | def construct(self): 37 | config = self.fleet.configuration 38 | rel_pos_c = {} 39 | for veh in self.vehicles: 40 | ind_veh = sorted(config[veh].keys()) 41 | rel_pos_c[veh] = veh.define_parameter('rel_pos_c', len(ind_veh)) 42 | problems_dic = {veh: self.problems[l] for l, veh in enumerate(self.fleet.vehicles)} 43 | # get fleet center as seen by vehicles 44 | centra = {} 45 | for veh in self.vehicles: 46 | conT = problems_dic[veh].define_symbol('conT0', len(problems_dic[veh].free_ind[veh])) 47 | centra[veh] = veh.get_fleet_center([conT], [rel_pos_c[veh]])[0] 48 | # rendez-vous constraints 49 | couples = {veh: [] for veh in self.vehicles} 50 | for veh in self.vehicles: 51 | ind_veh = sorted(config[veh].keys()) 52 | pos_c_veh = centra[veh] 53 | for nghb in self.fleet.get_neighbors(veh): 54 | if veh not in couples[nghb] and nghb not in couples[veh]: 55 | couples[veh].append(nghb) 56 | ind_nghb = sorted(config[nghb].keys()) 57 | pos_c_nghb = centra[nghb] 58 | for ind_v, ind_n in zip(ind_veh, ind_nghb): 59 | self.define_constraint(pos_c_veh[ind_v] - pos_c_nghb[ind_n], 0., 0.) 60 | ADMMProblem.construct(self) 61 | 62 | def set_parameters(self, current_time): 63 | parameters = {} 64 | for veh in self.vehicles: 65 | parameters[veh] = {'rel_pos_c': veh.rel_pos_c} 66 | return parameters 67 | 68 | def stop_criterium(self, current_time, update_time): 69 | res = 0. 70 | for veh in self.vehicles: 71 | ind_veh = sorted(self.fleet.configuration[veh].keys()) 72 | rel_conT = self.fleet.get_rel_config(veh) 73 | for nghb in self.fleet.get_neighbors(veh): 74 | ind_nghb = sorted(self.fleet.configuration[nghb].keys()) 75 | for k, (ind_v, ind_n) in enumerate(zip(ind_veh, ind_nghb)): 76 | rcT = rel_conT[nghb] 77 | rcT = rcT if isinstance(rcT, float) else rcT[k] 78 | res += np.linalg.norm(veh.trajectories['splines'][ind_v, 0] - 79 | nghb.trajectories['splines'][ind_n, 0] - 80 | rcT)**2 81 | if np.sqrt(res) > 5.e-2: 82 | return False 83 | return True 84 | 85 | def export(self, options=None): 86 | options = options or {} 87 | if not hasattr(self, 'father'): 88 | self.init() 89 | ExportRendezVous(self, options) 90 | -------------------------------------------------------------------------------- /omgtools/vehicles/__init__.py: -------------------------------------------------------------------------------- 1 | from .holonomic import Holonomic 2 | from .holonomicorient import HolonomicOrient 3 | from .holonomic3d import Holonomic3D 4 | from .holonomic1d import Holonomic1D 5 | from .quadrotor import Quadrotor 6 | from .dubins import Dubins 7 | from .bicycle import Bicycle 8 | from .agv import AGV 9 | from .trailer import Trailer 10 | from .fleet import Fleet 11 | from .quadrotor3d import Quadrotor3D 12 | from .quadrotor3d_simple import SimpleQuadrotor3D 13 | from .tool import Tool 14 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | 3 | setup( 4 | name='omg-tools', 5 | version='0.1.4', 6 | author='Ruben Van Parys', 7 | author_email='ruben.vanparys@kuleuven.be', 8 | description=('Optimal Motion Generation tools: a user-friendly tool for ' + 9 | 'modeling, simulating and embedding of (spline-based) motion ' + 10 | 'planning problems'), 11 | long_description=('Optimal Motion Generation-tools is a Python software ' + 12 | 'toolbox facilitating the modeling, simulation and ' + 13 | 'embedding of motion planning problems. ' + 14 | 'Its main goal is to collect research topics ' + 15 | 'concerning (spline-based) motion planning into a ' + 16 | 'user-friendly package in order to enlarge its ' + 17 | 'visibility towards the scientific and industrial world.'), 18 | license='LGPLv3', 19 | keywords='optimization motion planning splines distributed multi-agent mpc', 20 | url='https://github.com/meco-group/omg-tools', 21 | packages=find_packages(), 22 | package_data={'omgtools.export': ['*/Makefile', '*/instructions.txt', 23 | '*.cpp', '*.hpp', 24 | '*/*.cpp', '*/*.hpp', 25 | '*/*/*.cpp', '*/*/*.hpp', 26 | '*/*/*/*.cpp', '*/*/*/*.hpp']}, 27 | install_requires=[ 28 | 'matplotlib2tikz', 29 | 'casadi >= 3.1.1.post3, != 3.5.2', 30 | 'six >= 1.10.0' 31 | ], 32 | test_suite='nose.collector', 33 | tests_require=['nose>=1.0'], 34 | classifiers=[ 35 | 'Development Status :: 3 - Alpha', 36 | 'Intended Audience :: Science/Research', 37 | 'Topic :: Scientific/Engineering', 38 | 'License :: OSI Approved :: GNU Lesser General Public License v3 (LGPLv3)', 39 | 'Programming Language :: Python :: 2', 40 | 'Programming Language :: Python :: 2.7', 41 | 'Programming Language :: Python :: 3', 42 | 'Programming Language :: Python :: 3.5', 43 | 'Programming Language :: Python :: 3.6', 44 | 'Programming Language :: Python :: 3.7', 45 | ], 46 | ) 47 | -------------------------------------------------------------------------------- /tests/test_examples.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import os 3 | import imp 4 | import subprocess 5 | 6 | def test_generator(): 7 | if 'NODE_TOTAL' not in os.environ or 'NODE_INDEX' not in os.environ: 8 | node_total = 1 9 | node_index = 0 10 | else: 11 | node_total = int(os.environ['NODE_TOTAL']) 12 | node_index = int(os.environ['NODE_INDEX']) 13 | example_dir = os.path.join(os.getcwd(), 'examples') 14 | files = os.listdir(example_dir) 15 | example_files = [] 16 | for f in files: 17 | if (os.path.isfile(os.path.join(example_dir, f)) and f.endswith('.py') and not 'gui' in f): 18 | example_files.append(f) 19 | example_files.sort() 20 | n_files = len(example_files)//node_total 21 | if node_index == node_total-1: 22 | test_files = example_files[node_index*n_files:] 23 | else: 24 | test_files = example_files[node_index*n_files:(node_index+1)*n_files] 25 | for f in test_files: 26 | yield run_example, f 27 | 28 | 29 | def test_export(): 30 | files = {'export': 'Point2Point', 31 | 'export_f': 'FormationPoint2Point', 32 | 'export_r': 'RendezVous'} 33 | for d, f in files.items(): 34 | if os.path.isdir(os.path.join(os.getcwd(), d)): 35 | print(subprocess.check_output( 36 | ("cd %s && make && cd bin && ./%s && cd ../..") % 37 | (d, f), shell=True, stderr=subprocess.STDOUT)) 38 | 39 | 40 | def run_example(filename): 41 | example_dir = os.path.join(os.getcwd(), 'examples') 42 | print('') 43 | try: 44 | imp.load_source(filename.split('.')[-2], 45 | os.path.join(example_dir, filename)) 46 | 47 | # Forgive sys.exit(0) 48 | except SystemExit as e: 49 | if e.code!=0: 50 | import sys 51 | sys.exit(e.code) 52 | --------------------------------------------------------------------------------