├── stage_worlds ├── circle.png ├── world_test.world ├── world1.world ├── world2.world ├── dense_diff.world └── dense_omni.world ├── gazebo_worlds ├── launch │ ├── rviz.launch │ ├── world0.launch │ ├── world1.launch │ ├── world2.launch │ └── world3.launch ├── urdf │ ├── macros.xacro │ ├── herbie.gazebo │ └── herbie.xacro ├── package.xml └── CMakeLists.txt ├── mpc ├── launch │ └── start_controller.launch ├── package.xml ├── src │ ├── controller.py │ └── MPC.py └── CMakeLists.txt ├── LICENSE ├── mpc_orca ├── launch │ ├── start_controllers_world1.launch │ ├── start_controllers_world2.launch │ └── start_controllers_world3.launch ├── src │ ├── test_controller.py │ ├── controller.py │ ├── pyorca.py │ └── MPC_ORCA.py ├── package.xml └── CMakeLists.txt ├── .gitignore ├── README.md ├── rvo ├── src │ ├── omni_controller.py │ ├── dense_omni_controller.py │ ├── diff_omni_controller.py │ ├── controller.py │ ├── dense_diff_controller.py │ └── RVO.py ├── package.xml └── CMakeLists.txt └── orca ├── src ├── omni_controller.py ├── controller.py ├── dense_omni_controller.py ├── diff_omni_controller.py ├── dense_diff_controller.py ├── test.py ├── halfplaneintersect.py └── pyorca.py ├── package.xml └── CMakeLists.txt /stage_worlds/circle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/glauberrleite/navigation-collision-avoidance/HEAD/stage_worlds/circle.png -------------------------------------------------------------------------------- /stage_worlds/world_test.world: -------------------------------------------------------------------------------- 1 | define robot position ( 2 | origin [0 0 0 0] 3 | drive "omni" 4 | bitmap "circle.png" 5 | ) 6 | 7 | 8 | #resolution 0.02 9 | 10 | #interval_sim 100 11 | 12 | #window ( 13 | # size [745 448] 14 | # rotate [0 0] 15 | # scale 18.806 16 | #) 17 | 18 | robot ( 19 | pose [ -5 0 0 0] 20 | name "r0" 21 | color "blue" 22 | ) -------------------------------------------------------------------------------- /stage_worlds/world1.world: -------------------------------------------------------------------------------- 1 | define robot position ( 2 | origin [0 0 0 0] 3 | drive "omni" 4 | bitmap "circle.png" 5 | ) 6 | 7 | 8 | #resolution 0.02 9 | 10 | #interval_sim 100 11 | 12 | #window ( 13 | # size [745 448] 14 | # rotate [0 0] 15 | # scale 18.806 16 | #) 17 | 18 | robot ( 19 | pose [ -5 0 0 0] 20 | name "r0" 21 | color "blue" 22 | ) 23 | 24 | robot ( 25 | pose [ 5 0 0 0] 26 | name "r1" 27 | color "red" 28 | ) 29 | 30 | robot ( 31 | pose [ 0 5 0 0] 32 | name "r2" 33 | color "black" 34 | ) 35 | 36 | robot ( 37 | pose [0 -5 0 0] 38 | name "r3" 39 | color "pink" 40 | ) 41 | -------------------------------------------------------------------------------- /stage_worlds/world2.world: -------------------------------------------------------------------------------- 1 | define robot position ( 2 | origin [0 0 0 0] 3 | drive "diff" 4 | bitmap "circle.png" 5 | ) 6 | 7 | 8 | #resolution 0.02 9 | 10 | #interval_sim 100 11 | 12 | #window ( 13 | # size [745 448] 14 | # rotate [0 0] 15 | # scale 18.806 16 | #) 17 | 18 | robot ( 19 | pose [ -5 0 0 0] 20 | name "r0" 21 | color "blue" 22 | ) 23 | 24 | robot ( 25 | pose [ 5 0 0 180] 26 | name "r1" 27 | color "red" 28 | ) 29 | 30 | robot ( 31 | pose [ 0 5 0 270] 32 | name "r2" 33 | color "black" 34 | ) 35 | 36 | robot ( 37 | pose [0 -5 0 90] 38 | name "r3" 39 | color "pink" 40 | ) 41 | -------------------------------------------------------------------------------- /gazebo_worlds/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /mpc/launch/start_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 15 | 16 | -------------------------------------------------------------------------------- /gazebo_worlds/launch/world0.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /gazebo_worlds/urdf/macros.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 17 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /stage_worlds/dense_diff.world: -------------------------------------------------------------------------------- 1 | define robot position ( 2 | origin [0 0 0 0] 3 | drive "diff" 4 | bitmap "circle.png" 5 | ) 6 | 7 | 8 | #resolution 0.02 9 | 10 | #interval_sim 100 11 | 12 | #window ( 13 | # size [745 448] 14 | # rotate [0 0] 15 | # scale 18.806 16 | #) 17 | 18 | robot ( 19 | pose [ -10 0 0 0] 20 | name "r0" 21 | color "blue" 22 | ) 23 | 24 | robot ( 25 | pose [ 10 0 0 180] 26 | name "r1" 27 | color "red" 28 | ) 29 | 30 | robot ( 31 | pose [ 0 10 0 270] 32 | name "r2" 33 | color "black" 34 | ) 35 | 36 | robot ( 37 | pose [0 -10 0 90] 38 | name "r3" 39 | color "pink" 40 | ) 41 | 42 | robot ( 43 | pose [-10 10 0 315] 44 | name "r4" 45 | color "yellow" 46 | ) 47 | 48 | robot ( 49 | pose [-10 -10 0 45] 50 | name "r5" 51 | color "green" 52 | ) 53 | 54 | robot ( 55 | pose [10 -10 0 135] 56 | name "r6" 57 | color "gray" 58 | ) 59 | 60 | robot ( 61 | pose [10 10 0 225] 62 | name "r7" 63 | color "orange" 64 | ) -------------------------------------------------------------------------------- /stage_worlds/dense_omni.world: -------------------------------------------------------------------------------- 1 | define robot position ( 2 | origin [0.000 0.000 0.000 0.000] 3 | drive "omni" 4 | bitmap "circle.png" 5 | ) 6 | 7 | 8 | #resolution 0.02 9 | 10 | #interval_sim 100 11 | 12 | #window ( 13 | # size [745 448] 14 | # rotate [0 0] 15 | # scale 18.806 16 | #) 17 | 18 | robot ( 19 | pose [ -10.000 0.000 0.000 0.000] 20 | name "r0" 21 | color "blue" 22 | ) 23 | 24 | robot ( 25 | pose [ 10.000 0.000 0.000 0.000] 26 | name "r1" 27 | color "red" 28 | ) 29 | 30 | robot ( 31 | pose [ 0.000 10.000 0.000 0.000] 32 | name "r2" 33 | color "black" 34 | ) 35 | 36 | robot ( 37 | pose [0.000 -10.000 0.000 0.000] 38 | name "r3" 39 | color "pink" 40 | ) 41 | 42 | robot ( 43 | pose [-10.000 10.000 0.000 0.000] 44 | name "r4" 45 | color "yellow" 46 | ) 47 | 48 | robot ( 49 | pose [-10.000 -10.000 0.000 0.000] 50 | name "r5" 51 | color "green" 52 | ) 53 | 54 | robot ( 55 | pose [10.000 -10.000 0.000 0.000] 56 | name "r6" 57 | color "gray" 58 | ) 59 | 60 | robot ( 61 | pose [10.000 10.000 0.000 0.000] 62 | name "r7" 63 | color "orange" 64 | ) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Glauber Rodrigues Leite 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /mpc_orca/launch/start_controllers_world1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 13 | 15 | 16 | 17 | 22 | 23 | -------------------------------------------------------------------------------- /mpc_orca/src/test_controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import rospy 4 | import numpy 5 | from nav_msgs.msg import Odometry 6 | from geometry_msgs.msg import Twist 7 | from MPC_ORCA import MPC_ORCA 8 | 9 | N_AGENTS = 0 10 | RADIUS = 0.7 11 | tau = 5 12 | 13 | Ts = 0.1 14 | X = [(-5, 0)] 15 | V = [(0, 0) for _ in xrange(len(X))] 16 | V_max = [1.0 for _ in xrange(len(X))] 17 | goal = [(5.0, 5.0)] 18 | 19 | 20 | def callback_0(msg): 21 | X[0] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 22 | V[0] = (float(msg.twist.twist.linear.x), float(msg.twist.twist.linear.y)) 23 | 24 | rospy.init_node('test_controller') 25 | 26 | sub_0 = rospy.Subscriber('/odom', Odometry, callback_0) 27 | pub_0 = rospy.Publisher('/cmd_vel', Twist, queue_size=10) 28 | t = 0 29 | 30 | controller = MPC_ORCA(goal[0], X[0], -V_max[0], V_max[0], 10, Ts) 31 | 32 | while not rospy.is_shutdown(): 33 | 34 | a_new = controller.getNewAcceleration(X[0], V[0]) 35 | velocity = V[0] + a_new * Ts 36 | 37 | vel_0 = Twist() 38 | vel_0.linear.x = velocity[0] 39 | vel_0.linear.y = velocity[1] 40 | 41 | 42 | pub_0.publish(vel_0) 43 | 44 | if t%100: 45 | print(velocity) 46 | print(X[0]) 47 | t += 1 48 | rospy.sleep(Ts) 49 | -------------------------------------------------------------------------------- /gazebo_worlds/urdf/herbie.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 6 | 0 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | true 28 | 100 29 | joint_left_wheel 30 | joint_right_wheel 31 | 0.108 32 | 0.0313 33 | 1 34 | 100 35 | cmd_vel 36 | odom 37 | odom 38 | link_chassis 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /mpc_orca/launch/start_controllers_world2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 19 | 20 | 22 | 23 | 25 | 26 | 27 | 32 | 33 | -------------------------------------------------------------------------------- /mpc_orca/launch/start_controllers_world3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 19 | 20 | 22 | 23 | 25 | 26 | 27 | 32 | 33 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | *.egg-info/ 24 | .installed.cfg 25 | *.egg 26 | MANIFEST 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | .pytest_cache/ 49 | 50 | # Translations 51 | *.mo 52 | *.pot 53 | 54 | # Django stuff: 55 | *.log 56 | local_settings.py 57 | db.sqlite3 58 | 59 | # Flask stuff: 60 | instance/ 61 | .webassets-cache 62 | 63 | # Scrapy stuff: 64 | .scrapy 65 | 66 | # Sphinx documentation 67 | docs/_build/ 68 | 69 | # PyBuilder 70 | target/ 71 | 72 | # Jupyter Notebook 73 | .ipynb_checkpoints 74 | 75 | # pyenv 76 | .python-version 77 | 78 | # celery beat schedule file 79 | celerybeat-schedule 80 | 81 | # SageMath parsed files 82 | *.sage.py 83 | 84 | # Environments 85 | .env 86 | .venv 87 | env/ 88 | venv/ 89 | ENV/ 90 | env.bak/ 91 | venv.bak/ 92 | 93 | # Spyder project settings 94 | .spyderproject 95 | .spyproject 96 | 97 | # Rope project settings 98 | .ropeproject 99 | 100 | # mkdocs documentation 101 | /site 102 | 103 | # mypy 104 | .mypy_cache/ 105 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Local Collision Avoidance Techniques on Multi-Agent Robot Navigation Systems 2 | 3 | This repository covers a set of algorithms that provides local control strategies for autonomous robots navigating on a multi-agent scenario, where each robot tries to reach its goal as fast as it can, while mantaining a collision-free trajectory, based only on other agents velocity readings without any direct communication between them. 4 | 5 | ## About the code 6 | 7 | The algorithms were created to work over the [ROS](https://www.ros.org/) environment, specifically the *Melodic* distribution running over Ubuntu 18.04, but they may work with other distributions. 8 | 9 | ## Dependencies 10 | 11 | * [ROS Melodic](https://www.ros.org/) 12 | * [Python 2.7](https://www.python.org/) 13 | * [Gazebo](http://gazebosim.org/) * 14 | * [Stage](http://wiki.ros.org/stage) * 15 | * [OSQP (Operator Splitting Quadratic Program)](https://osqp.org/) 16 | 17 | \* Generally comes with a full ROS install 18 | 19 | ## Simulation scenarios 20 | 21 | Firstly, the ROS MASTER needed to be started: 22 | 23 | `$ roscore` 24 | 25 | ### ROS Stage 26 | 27 | There are worlds created for Stage simulation inside the *stage_worlds* folder. To run a simulation, e.g. *world1.world*: 28 | 29 | `$ rosrun stage_ros stageros /stage_worlds/world1.world` 30 | 31 | ### Gazebo 32 | 33 | For this simulation, a [Yujin Robot Kobuki](http://kobuki.yujinrobot.com/) was used, so it needs the driver URDF description and components for that robot. If it is not found within the default ROS install, it can be downloaded and built inside the catkin workspace using this [repository](https://github.com/yujinrobot/kobuki) (select the right ROS distribution branch). 34 | 35 | With all set, the gazebo environment with the robots can be started using the launch file inside *mpc_orca* folder: 36 | 37 | `$ roslaunch mpc_orca kobuki_duo.launch` -------------------------------------------------------------------------------- /rvo/src/omni_controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import rospy 4 | import sys 5 | import numpy 6 | from nav_msgs.msg import Odometry 7 | from geometry_msgs.msg import Twist 8 | from RVO import RVO_update, compute_V_des 9 | 10 | scenario = dict() 11 | scenario['robot_radius'] = 0.4 12 | scenario['circular_obstacles'] = [] 13 | scenario['boundary'] = [] 14 | 15 | Ts = 0.01 16 | X = [[-5., 0.], [5., 0.], [0.0, 5.], [0., -5.]] 17 | V = [[0., 0.] for _ in xrange(len(X))] 18 | V_max = [1.0 for _ in xrange(len(X))] 19 | goal = [[5., 0.], [-5., 0.], [0.0, -5.], [0., 5.]] 20 | 21 | def callback_0(msg): 22 | X[0] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 23 | def callback_1(msg): 24 | X[1] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 25 | def callback_2(msg): 26 | X[2] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 27 | def callback_3(msg): 28 | X[3] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 29 | 30 | rospy.init_node('omni_controller') 31 | 32 | sub_0 = rospy.Subscriber('/robot_0/odom', Odometry, callback_0) 33 | sub_1 = rospy.Subscriber('/robot_1/odom', Odometry, callback_1) 34 | sub_2 = rospy.Subscriber('/robot_2/odom', Odometry, callback_2) 35 | sub_3 = rospy.Subscriber('/robot_3/odom', Odometry, callback_3) 36 | pub = [] 37 | pub.append(rospy.Publisher('/robot_0/cmd_vel', Twist, queue_size=10)) 38 | pub.append(rospy.Publisher('/robot_1/cmd_vel', Twist, queue_size=10)) 39 | pub.append(rospy.Publisher('/robot_2/cmd_vel', Twist, queue_size=10)) 40 | pub.append(rospy.Publisher('/robot_3/cmd_vel', Twist, queue_size=10)) 41 | t = 0 42 | while not rospy.is_shutdown(): 43 | 44 | V_des = compute_V_des(X, goal, V_max) 45 | V = RVO_update(X, V_des, V, scenario) 46 | 47 | for i in xrange(len(X)): 48 | vel = Twist() 49 | vel.linear.x = V[i][0] 50 | vel.linear.y = V[i][1] 51 | 52 | pub[i].publish(vel) 53 | 54 | 55 | if t%100: 56 | print(X) 57 | print('--------------') 58 | t += 1 59 | rospy.sleep(Ts) 60 | -------------------------------------------------------------------------------- /gazebo_worlds/launch/world1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 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 | 52 | -------------------------------------------------------------------------------- /mpc/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mpc 4 | 0.0.0 5 | The mpc package 6 | 7 | 8 | 9 | 10 | glauber 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 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 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /gazebo_worlds/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gazebo_worlds 4 | 0.0.0 5 | The gazebo_worlds package 6 | 7 | 8 | 9 | 10 | glauber 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 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 | catkin 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /orca/src/omni_controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import rospy 4 | import numpy 5 | from nav_msgs.msg import Odometry 6 | from geometry_msgs.msg import Twist 7 | from pyorca import Agent, orca, normalized 8 | 9 | N_AGENTS = 4 10 | RADIUS = 0.7 11 | tau = 5 12 | 13 | Ts = 0.1 14 | X = [(-5, 0.25), (5, 0), (0.00, 5), (-0.25, -5)] 15 | V = [(0, 0) for _ in xrange(len(X))] 16 | V_max = [1.0 for _ in xrange(len(X))] 17 | goal = [(5.0, 0.0), (-5.0, 0.0), (0.0, -5.0), (0.0, 5.0)] 18 | 19 | agents = [] 20 | 21 | for i in xrange(len(X)): 22 | vel = (numpy.array(goal[i]) - numpy.array(X[i])) 23 | if numpy.linalg.norm(vel) > V_max[i]: 24 | vel = normalized(vel) * V_max[i] 25 | agents.append(Agent(X[i], (0., 0.), RADIUS, V_max[i], vel)) 26 | 27 | def update_agents(agents): 28 | for i in xrange(len(X)): 29 | agents[i].pref_velocity = (numpy.array(goal[i]) - numpy.array(X[i])) 30 | if numpy.linalg.norm(agents[i].pref_velocity) > V_max[i]: 31 | agents[i].pref_velocity = normalized(agents[i].pref_velocity) * V_max[i] 32 | return agents 33 | 34 | def callback_0(msg): 35 | X[0] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 36 | def callback_1(msg): 37 | X[1] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 38 | def callback_2(msg): 39 | X[2] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 40 | def callback_3(msg): 41 | X[3] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 42 | 43 | 44 | rospy.init_node('omni_controller') 45 | 46 | sub_0 = rospy.Subscriber('/robot_0/odom', Odometry, callback_0) 47 | sub_1 = rospy.Subscriber('/robot_1/odom', Odometry, callback_1) 48 | sub_2 = rospy.Subscriber('/robot_2/odom', Odometry, callback_2) 49 | sub_3 = rospy.Subscriber('/robot_3/odom', Odometry, callback_3) 50 | pub = [] 51 | pub.append(rospy.Publisher('/robot_0/cmd_vel', Twist, queue_size=10)) 52 | pub.append(rospy.Publisher('/robot_1/cmd_vel', Twist, queue_size=10)) 53 | pub.append(rospy.Publisher('/robot_2/cmd_vel', Twist, queue_size=10)) 54 | pub.append(rospy.Publisher('/robot_3/cmd_vel', Twist, queue_size=10)) 55 | t = 0 56 | 57 | while not rospy.is_shutdown(): 58 | agents = update_agents(agents) 59 | for i, agent in enumerate(agents): 60 | candidates = agents[:i] + agents[i + 1:] 61 | agent.velocity, _ = orca(agent, candidates, tau, Ts) 62 | 63 | for i in xrange(len(X)): 64 | vel = Twist() 65 | vel.linear.x = agents[i].velocity[0] 66 | vel.linear.y = agents[i].velocity[1] 67 | 68 | pub[i].publish(vel) 69 | 70 | if t%100: 71 | print(X) 72 | #print(agents[0].velocity) 73 | t += 1 74 | rospy.sleep(Ts) 75 | -------------------------------------------------------------------------------- /orca/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | orca 4 | 0.0.0 5 | The orca package 6 | 7 | 8 | 9 | 10 | glauberrleite 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 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 | catkin 52 | rospy 53 | rospy 54 | rospy 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /rvo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rvo 4 | 0.0.0 5 | The rvo package 6 | 7 | 8 | 9 | 10 | glauberrleite 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 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 | catkin 52 | rospy 53 | rospy 54 | rospy 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /mpc_orca/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mpc_orca 4 | 0.0.0 5 | The mpc_orca package 6 | 7 | 8 | 9 | 10 | glauberrleite 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 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 | catkin 52 | rospy 53 | rospy 54 | rospy 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /rvo/src/dense_omni_controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import rospy 4 | import sys 5 | import numpy 6 | from nav_msgs.msg import Odometry 7 | from geometry_msgs.msg import Twist, Vector3 8 | from RVO import RVO_update, compute_V_des 9 | from rosgraph_msgs.msg import Clock 10 | 11 | scenario = dict() 12 | scenario['robot_radius'] = 0.5 13 | scenario['circular_obstacles'] = [] 14 | scenario['boundary'] = [] 15 | 16 | Ts = 0.01 17 | X = [[-10., 0.], [10., 0.], [0.0, 10.], [0., -10.], [-10., 10.], [-10., -10.], [10.0, -10.], [10., 10.]] 18 | V = [[0., 0.] for _ in xrange(len(X))] 19 | V_min = [-1.0 for _ in xrange(len(X))] 20 | V_max = [1.0 for _ in xrange(len(X))] 21 | goal = [[10., 0.], [-10., 0.], [0.0, -10.], [0., 10.], [10., -10.], [10., 10.], [-10.0, 10.], [-10., -10.]] 22 | 23 | def callback_0(msg): 24 | X[0] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 25 | def callback_1(msg): 26 | X[1] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 27 | def callback_2(msg): 28 | X[2] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 29 | def callback_3(msg): 30 | X[3] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 31 | def callback_4(msg): 32 | X[4] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 33 | def callback_5(msg): 34 | X[5] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 35 | def callback_6(msg): 36 | X[6] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 37 | def callback_7(msg): 38 | X[7] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 39 | 40 | 41 | rospy.init_node('omni_controller') 42 | 43 | sub_0 = rospy.Subscriber('/robot_0/odom', Odometry, callback_0) 44 | sub_1 = rospy.Subscriber('/robot_1/odom', Odometry, callback_1) 45 | sub_2 = rospy.Subscriber('/robot_2/odom', Odometry, callback_2) 46 | sub_3 = rospy.Subscriber('/robot_3/odom', Odometry, callback_3) 47 | sub_4 = rospy.Subscriber('/robot_4/odom', Odometry, callback_4) 48 | sub_5 = rospy.Subscriber('/robot_5/odom', Odometry, callback_5) 49 | sub_6 = rospy.Subscriber('/robot_6/odom', Odometry, callback_6) 50 | sub_7 = rospy.Subscriber('/robot_7/odom', Odometry, callback_7) 51 | pub = [] 52 | pub.append(rospy.Publisher('/robot_0/cmd_vel', Twist, queue_size=10)) 53 | pub.append(rospy.Publisher('/robot_1/cmd_vel', Twist, queue_size=10)) 54 | pub.append(rospy.Publisher('/robot_2/cmd_vel', Twist, queue_size=10)) 55 | pub.append(rospy.Publisher('/robot_3/cmd_vel', Twist, queue_size=10)) 56 | pub.append(rospy.Publisher('/robot_4/cmd_vel', Twist, queue_size=10)) 57 | pub.append(rospy.Publisher('/robot_5/cmd_vel', Twist, queue_size=10)) 58 | pub.append(rospy.Publisher('/robot_6/cmd_vel', Twist, queue_size=10)) 59 | pub.append(rospy.Publisher('/robot_7/cmd_vel', Twist, queue_size=10)) 60 | pub_setpoint = rospy.Publisher('/setpoint', Vector3, queue_size=10) 61 | 62 | setpoint = Vector3() 63 | setpoint.x = goal[4][0] 64 | setpoint.y = goal[4][1] 65 | 66 | rospy.wait_for_message('/clock', Clock) 67 | while not rospy.is_shutdown(): 68 | 69 | V_des = compute_V_des(X, goal, V_max) 70 | V = RVO_update(X, V_des, V, scenario) 71 | 72 | for i in xrange(len(X)): 73 | vel = Twist() 74 | vel.linear.x = V[i][0] 75 | vel.linear.y = V[i][1] 76 | 77 | pub[i].publish(vel) 78 | 79 | pub_setpoint.publish(setpoint) 80 | rospy.sleep(Ts) 81 | -------------------------------------------------------------------------------- /mpc/src/controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import sys 4 | import rospy 5 | import numpy as np 6 | from geometry_msgs.msg import Twist, Vector3 7 | from gazebo_msgs.msg import ModelStates 8 | from MPC import MPC 9 | 10 | N = 10 11 | N_c = 5 12 | Ts = 0.1 13 | X = np.array([0., 0.]) 14 | orientation = 0 15 | V = np.array([0., 0.]) 16 | V_min = -5 17 | V_max = 5 18 | 19 | goal = np.array([float(sys.argv[1]), float(sys.argv[2])]) 20 | 21 | def accelerationTransform(a, v, w, theta_0): 22 | """This function applies the linearization transformation on acceleration values 23 | based on equation 2.11 24 | """ 25 | d = 0.2 26 | cos_theta = np.cos(theta_0) 27 | sin_theta = np.sin(theta_0) 28 | inverse = np.linalg.inv(np.array([[cos_theta, -d * sin_theta],[sin_theta, d * cos_theta]])) 29 | term1 = a[0] + v * w * sin_theta + d * (w**2) * cos_theta 30 | term2 = a[1] - v * w * cos_theta + d * (w**2) * sin_theta 31 | acc = np.matmul(inverse, np.vstack([term1, term2])) 32 | acc = acc.T 33 | 34 | return acc[0] 35 | 36 | def updateWorld(msg): 37 | """This funcion is called whenever the gazebo/model_states publishes. This function 38 | updates the world variables as fast as possible""" 39 | global X, V, orientation 40 | X = np.array([float(msg.pose[1].position.x), float(msg.pose[1].position.y)]) 41 | V = np.array([float(msg.twist[1].linear.x), float(msg.twist[1].linear.y)]) 42 | orientation = np.arctan2(2 * float(msg.pose[1].orientation.w) * float(msg.pose[1].orientation.z), \ 43 | 1 - 2 * float(msg.pose[1].orientation.z)**2) 44 | 45 | rospy.init_node('mpc_controller') 46 | 47 | # Subscribing on model_states instead of robot/odom, to avoid unnecessary noise 48 | rospy.Subscriber('/gazebo/model_states', ModelStates, updateWorld) 49 | 50 | # Velocity publishers 51 | pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) 52 | 53 | # Setpoint Publishers 54 | pub_setpoint_pos = rospy.Publisher('/setpoint_pos', Vector3, queue_size=10) 55 | pub_setpoint_vel = rospy.Publisher('/setpoint_vel', Vector3, queue_size=10) 56 | 57 | setpoint_pos = Vector3() 58 | setpoint_vel = Vector3() 59 | 60 | # Initializing Controllers 61 | controller = MPC(X, V_min, V_max, N, N_c, Ts) 62 | 63 | # Global path planning 64 | initial = np.copy(X) 65 | t0 = 10.0 66 | growth = 0.5 67 | logistic = lambda t: 1/(1 + np.exp(- growth * (t - t0))) 68 | d_logistic = lambda t: growth * logistic(t) * (1 - logistic(t)) 69 | P_des = lambda t: goal * logistic(t) + initial * (1 - logistic(t)) 70 | V_des = lambda t: goal * d_logistic(t) - initial * d_logistic(t) 71 | 72 | t = 0 73 | 74 | vel = Twist() 75 | 76 | while not rospy.is_shutdown(): 77 | 78 | # Updating setpoint trajectory 79 | setpoint = np.ravel([np.append(P_des(t + k * Ts), V_des(t + k * Ts)) for k in range(0, N + 1)]) 80 | 81 | # Updating initial conditions 82 | controller.x_0 = np.array([X[0], X[1], V[0], V[1]]) 83 | 84 | # Computing optimal input values 85 | [_, acceleration] = controller.getNewVelocity(setpoint) 86 | 87 | [setpoint_pos.x, setpoint_pos.y] = P_des(t) 88 | 89 | [setpoint_vel.x, setpoint_vel.y] = V_des(t) 90 | 91 | acc = accelerationTransform(acceleration, vel.linear.x, vel.angular.z, orientation) 92 | 93 | vel.linear.x = vel.linear.x + acc[0] * Ts 94 | vel.angular.z = vel.angular.z + acc[1] * Ts 95 | 96 | pub.publish(vel) 97 | 98 | pub_setpoint_pos.publish(setpoint_pos) 99 | pub_setpoint_vel.publish(setpoint_vel) 100 | rospy.sleep(Ts) 101 | 102 | t += Ts 103 | -------------------------------------------------------------------------------- /rvo/src/diff_omni_controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import rospy 4 | import numpy as np 5 | from nav_msgs.msg import Odometry 6 | from geometry_msgs.msg import Twist 7 | from RVO import RVO_update, compute_V_des 8 | 9 | scenario = dict() 10 | scenario['robot_radius'] = 0.4 11 | scenario['circular_obstacles'] = [] 12 | scenario['boundary'] = [] 13 | 14 | Ts = 0.01 15 | X = [[-5,0], [5,0], [0, 5], [0, -5]] 16 | V = [[0,0] for _ in xrange(len(X))] 17 | orientation = [0, np.pi, -np.pi/2, np.pi/2] 18 | V_max = [1.0, 1.0, 1.0, 1.0] 19 | goal = [[5, 0], [-5,0], [0, -5], [0, 5]] 20 | 21 | def velocityTransform(v, theta_0): 22 | angular = np.arctan2(v[1], v[0]) - theta_0 23 | linear = np.sqrt(v[0]**2 + v[1]**2) 24 | 25 | # Handling singularity 26 | if np.abs(angular) > np.pi: 27 | angular -= np.sign(angular) * 2 * np.pi 28 | 29 | return [linear, angular] 30 | 31 | def callback_0(msg): 32 | X[0] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 33 | orientation[0] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 34 | def callback_1(msg): 35 | X[1] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 36 | orientation[1] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 37 | def callback_2(msg): 38 | X[2] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 39 | orientation[2] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 40 | def callback_3(msg): 41 | X[3] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 42 | orientation[3] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 43 | 44 | 45 | rospy.init_node('omni_controller') 46 | 47 | sub_0 = rospy.Subscriber('/robot_0/odom', Odometry, callback_0) 48 | sub_1 = rospy.Subscriber('/robot_1/odom', Odometry, callback_1) 49 | sub_2 = rospy.Subscriber('/robot_2/odom', Odometry, callback_2) 50 | sub_3 = rospy.Subscriber('/robot_3/odom', Odometry, callback_3) 51 | pub_0 = rospy.Publisher('/robot_0/cmd_vel', Twist, queue_size=10) 52 | pub_1 = rospy.Publisher('/robot_1/cmd_vel', Twist, queue_size=10) 53 | pub_2 = rospy.Publisher('/robot_2/cmd_vel', Twist, queue_size=10) 54 | pub_3 = rospy.Publisher('/robot_3/cmd_vel', Twist, queue_size=10) 55 | t = 0 56 | while not rospy.is_shutdown(): 57 | 58 | V_des = compute_V_des(X, goal, V_max) 59 | V = RVO_update(X, V_des, V, scenario) 60 | 61 | vel_0 = Twist() 62 | 63 | [linear_vel_0, angular_vel_0] = velocityTransform(V[0], orientation[0]) 64 | 65 | vel_0.linear.x = linear_vel_0 66 | vel_0.angular.z = angular_vel_0 67 | 68 | vel_1 = Twist() 69 | 70 | [linear_vel_1, angular_vel_1] = velocityTransform(V[1], orientation[1]) 71 | 72 | vel_1.linear.x = linear_vel_1 73 | vel_1.angular.z = angular_vel_1 74 | 75 | vel_2 = Twist() 76 | 77 | [linear_vel_2, angular_vel_2] = velocityTransform(V[2], orientation[2]) 78 | 79 | vel_2.linear.x = linear_vel_2 80 | vel_2.angular.z = angular_vel_2 81 | 82 | vel_3 = Twist() 83 | 84 | [linear_vel_3, angular_vel_3] = velocityTransform(V[3], orientation[3]) 85 | 86 | vel_3.linear.x = linear_vel_3 87 | vel_3.angular.z = angular_vel_3 88 | 89 | 90 | pub_0.publish(vel_0) 91 | pub_1.publish(vel_1) 92 | pub_2.publish(vel_2) 93 | pub_3.publish(vel_3) 94 | 95 | 96 | if t%100: 97 | print(X) 98 | t += 1 99 | rospy.sleep(Ts) 100 | -------------------------------------------------------------------------------- /orca/src/controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | # @author glauberrleite 4 | 5 | import rospy 6 | 7 | import numpy as np 8 | 9 | from RVO import RVO_update, reach, compute_V_des, reach 10 | from geometry_msgs.msg import Twist 11 | from gazebo_msgs.msg import ModelStates 12 | from kobuki_msgs.msg import MotorPower 13 | 14 | scenario = dict() 15 | scenario['robot_radius'] = 0.8 16 | scenario['circular_obstacles'] = [] 17 | scenario['boundary'] = [] 18 | 19 | Ts = 0.01 20 | X = [[0,0], [10,0]] 21 | orientation = [0, np.pi] 22 | V = [[0,0] for _ in xrange(len(X))] 23 | V_max = [1.0 for i in xrange(len(X))] 24 | goal = [[10, 0], [0,0]] 25 | 26 | def updateWorld(msg): 27 | X[0] = [float(msg.pose[1].position.x), float(msg.pose[1].position.y)] 28 | orientation[0] = 2* np.arctan2(float(msg.pose[1].orientation.z), float(msg.pose[1].orientation.w)) 29 | if np.abs(orientation[0]) > np.pi: 30 | orientation[0] = orientation[0] - np.sign(orientation[0]) * 2 * np.pi 31 | 32 | X[1] = [float(msg.pose[2].position.x), float(msg.pose[2].position.y)] 33 | orientation[1] = 2* np.arctan2(float(msg.pose[2].orientation.z), float(msg.pose[2].orientation.w)) 34 | if np.abs(orientation[1]) > np.pi: 35 | orientation[1] = orientation[1] - np.sign(orientation[1]) * 2 * np.pi 36 | 37 | 38 | rospy.init_node('rvo_controller') 39 | pub_vel_r0 = rospy.Publisher("robot0/mobile_base/commands/velocity", Twist, queue_size=10); 40 | pub_vel_r1 = rospy.Publisher("robot1/mobile_base/commands/velocity", Twist, queue_size=10); 41 | pub_mot_r0 = rospy.Publisher("robot0/mobile_base/commands/motor_power", MotorPower, queue_size=10); 42 | pub_mot_r1 = rospy.Publisher("robot1/mobile_base/commands/motor_power", MotorPower, queue_size=10); 43 | 44 | rospy.Subscriber('/gazebo/model_states', ModelStates, updateWorld) 45 | 46 | motor0 = MotorPower() 47 | motor1 = MotorPower() 48 | motor0.state = motor0.ON 49 | motor1.state = motor1.ON 50 | 51 | pub_mot_r0.publish(motor0) 52 | pub_mot_r1.publish(motor1) 53 | 54 | t = 0 55 | 56 | while not rospy.is_shutdown(): 57 | # compute desired vel to goal 58 | V_des = compute_V_des(X, goal, V_max) 59 | # compute the optimal vel to avoid collision 60 | V = RVO_update(X, V_des, V, scenario) 61 | 62 | # Compute ang_vel considering singularities 63 | angular_vel_0 = (np.arctan2(V[0][1], V[0][0]) - orientation[0]) 64 | if np.abs(angular_vel_0) > np.pi: 65 | angular_vel_0 = (orientation[0] - np.arctan2(V[0][1], V[0][0])) 66 | 67 | linear_vel_0 = np.sqrt(V[0][0]**2 + V[0][1]**2) 68 | vel_rob0 = Twist() 69 | vel_rob0.linear.x = linear_vel_0 70 | vel_rob0.linear.y = 0.0 71 | vel_rob0.linear.z = 0.0 72 | vel_rob0.angular.x = 0.0 73 | vel_rob0.angular.y = 0.0 74 | vel_rob0.angular.z = angular_vel_0 75 | 76 | angular_vel_1 = (np.arctan2(V[1][1], V[1][0]) - orientation[1]) 77 | if np.abs(angular_vel_1) > np.pi: 78 | angular_vel_1 = (orientation[0] - np.arctan2(V[1][1], V[1][0])) 79 | 80 | linear_vel_1 = np.sqrt(V[1][0]**2 + V[1][1]**2) 81 | vel_rob1 = Twist() 82 | vel_rob1.linear.x = linear_vel_1 83 | vel_rob1.linear.y = 0.0; 84 | vel_rob1.linear.z = 0.0; 85 | vel_rob1.angular.x = 0.0; 86 | vel_rob1.angular.y = 0.0; 87 | vel_rob1.angular.z = angular_vel_1 88 | 89 | pub_vel_r0.publish(vel_rob0) 90 | pub_vel_r1.publish(vel_rob1) 91 | 92 | #if t%100 == 0: 93 | # print(V_des[1]) 94 | # print(vel_rob1.linear.x) 95 | # print(vel_rob1.angular.z) 96 | # print(orientation) 97 | # print('---------') 98 | #t = t + 1 99 | 100 | rospy.sleep(Ts) 101 | -------------------------------------------------------------------------------- /rvo/src/controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | # @author glauberrleite 4 | 5 | import rospy 6 | 7 | import numpy as np 8 | 9 | from RVO import RVO_update, reach, compute_V_des, reach 10 | from geometry_msgs.msg import Twist 11 | from gazebo_msgs.msg import ModelStates 12 | from kobuki_msgs.msg import MotorPower 13 | 14 | scenario = dict() 15 | scenario['robot_radius'] = 0.8 16 | scenario['circular_obstacles'] = [] 17 | scenario['boundary'] = [] 18 | 19 | Ts = 0.01 20 | X = [[0,0], [10,0]] 21 | orientation = [0, np.pi] 22 | V = [[0,0] for _ in xrange(len(X))] 23 | V_max = [1.0 for i in xrange(len(X))] 24 | goal = [[10, 0], [0,0]] 25 | 26 | def updateWorld(msg): 27 | X[1] = [float(msg.pose[1].position.x), float(msg.pose[1].position.y)] 28 | orientation[1] = 2* np.arctan2(float(msg.pose[1].orientation.z), float(msg.pose[1].orientation.w)) 29 | if np.abs(orientation[1]) > np.pi: 30 | orientation[1] = orientation[1] - np.sign(orientation[1]) * 2 * np.pi 31 | 32 | X[0] = [float(msg.pose[2].position.x), float(msg.pose[2].position.y)] 33 | orientation[0] = 2* np.arctan2(float(msg.pose[2].orientation.z), float(msg.pose[2].orientation.w)) 34 | if np.abs(orientation[0]) > np.pi: 35 | orientation[0] = orientation[0] - np.sign(orientation[0]) * 2 * np.pi 36 | 37 | 38 | rospy.init_node('rvo_controller') 39 | pub_vel_r0 = rospy.Publisher("robot0/mobile_base/commands/velocity", Twist, queue_size=10); 40 | pub_vel_r1 = rospy.Publisher("robot1/mobile_base/commands/velocity", Twist, queue_size=10); 41 | pub_mot_r0 = rospy.Publisher("robot0/mobile_base/commands/motor_power", MotorPower, queue_size=10); 42 | pub_mot_r1 = rospy.Publisher("robot1/mobile_base/commands/motor_power", MotorPower, queue_size=10); 43 | 44 | rospy.Subscriber('/gazebo/model_states', ModelStates, updateWorld) 45 | 46 | motor0 = MotorPower() 47 | motor1 = MotorPower() 48 | motor0.state = motor0.ON 49 | motor1.state = motor1.ON 50 | 51 | pub_mot_r0.publish(motor0) 52 | pub_mot_r1.publish(motor1) 53 | 54 | t = 0 55 | 56 | while not rospy.is_shutdown(): 57 | # compute desired vel to goal 58 | V_des = compute_V_des(X, goal, V_max) 59 | # compute the optimal vel to avoid collision 60 | V = RVO_update(X, V_des, V, scenario) 61 | 62 | # Compute ang_vel considering singularities 63 | angular_vel_0 = (np.arctan2(V[0][1], V[0][0]) - orientation[0]) 64 | if np.abs(angular_vel_0) > np.pi: 65 | angular_vel_0 = (orientation[0] - np.arctan2(V[0][1], V[0][0])) 66 | 67 | linear_vel_0 = np.sqrt(V[0][0]**2 + V[0][1]**2) 68 | vel_rob0 = Twist() 69 | vel_rob0.linear.x = linear_vel_0 70 | vel_rob0.linear.y = 0.0 71 | vel_rob0.linear.z = 0.0 72 | vel_rob0.angular.x = 0.0 73 | vel_rob0.angular.y = 0.0 74 | vel_rob0.angular.z = angular_vel_0 75 | 76 | angular_vel_1 = (np.arctan2(V[1][1], V[1][0]) - orientation[1]) 77 | if np.abs(angular_vel_1) > np.pi: 78 | angular_vel_1 = (orientation[0] - np.arctan2(V[1][1], V[1][0])) 79 | 80 | linear_vel_1 = np.sqrt(V[1][0]**2 + V[1][1]**2) 81 | vel_rob1 = Twist() 82 | vel_rob1.linear.x = linear_vel_1 83 | vel_rob1.linear.y = 0.0; 84 | vel_rob1.linear.z = 0.0; 85 | vel_rob1.angular.x = 0.0; 86 | vel_rob1.angular.y = 0.0; 87 | vel_rob1.angular.z = angular_vel_1 88 | 89 | pub_vel_r0.publish(vel_rob0) 90 | pub_vel_r1.publish(vel_rob1) 91 | 92 | #if t%100 == 0: 93 | # print(V_des[1]) 94 | # print(vel_rob1.linear.x) 95 | # print(vel_rob1.angular.z) 96 | # print(orientation) 97 | # print('---------') 98 | #t = t + 1 99 | 100 | rospy.sleep(Ts) 101 | -------------------------------------------------------------------------------- /orca/src/dense_omni_controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import rospy 4 | import numpy 5 | from nav_msgs.msg import Odometry 6 | from geometry_msgs.msg import Twist, Vector3 7 | from rosgraph_msgs.msg import Clock 8 | from pyorca import Agent, orca, normalized 9 | 10 | RADIUS = 0.5 11 | tau = 15 12 | 13 | Ts = 0.01 14 | X = [[-10., 0.25], [10., 0.], [0.0, 10.], [-0.25, -10.], [-10., 10.25], [-10., -10.25], [10.0, -10.], [10., 10.]] 15 | V = [[0., 0.] for _ in xrange(len(X))] 16 | V_max = [1.0 for _ in xrange(len(X))] 17 | goal = [[10., 0.], [-10., 0.], [0.0, -10.], [0., 10.], [10., -10.], [10., 10.], [-10.0, 10.], [-10., -10.]] 18 | 19 | agents = [] 20 | 21 | for i in xrange(len(X)): 22 | vel = (numpy.array(goal[i]) - numpy.array(X[i])) 23 | if numpy.linalg.norm(vel) > V_max[i]: 24 | vel = normalized(vel) * V_max[i] 25 | agents.append(Agent(X[i], (0., 0.), RADIUS, V_max[i], vel)) 26 | 27 | def update_agents(agents): 28 | for i in xrange(len(X)): 29 | agents[i].pref_velocity = (numpy.array(goal[i]) - numpy.array(X[i])) 30 | if numpy.linalg.norm(agents[i].pref_velocity) > V_max[i]: 31 | agents[i].pref_velocity = normalized(agents[i].pref_velocity) * V_max[i] 32 | return agents 33 | 34 | def callback_0(msg): 35 | X[0] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 36 | def callback_1(msg): 37 | X[1] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 38 | def callback_2(msg): 39 | X[2] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 40 | def callback_3(msg): 41 | X[3] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 42 | def callback_4(msg): 43 | X[4] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 44 | def callback_5(msg): 45 | X[5] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 46 | def callback_6(msg): 47 | X[6] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 48 | def callback_7(msg): 49 | X[7] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 50 | 51 | 52 | rospy.init_node('omni_controller') 53 | 54 | sub_0 = rospy.Subscriber('/robot_0/odom', Odometry, callback_0) 55 | sub_1 = rospy.Subscriber('/robot_1/odom', Odometry, callback_1) 56 | sub_2 = rospy.Subscriber('/robot_2/odom', Odometry, callback_2) 57 | sub_3 = rospy.Subscriber('/robot_3/odom', Odometry, callback_3) 58 | sub_4 = rospy.Subscriber('/robot_4/odom', Odometry, callback_4) 59 | sub_5 = rospy.Subscriber('/robot_5/odom', Odometry, callback_5) 60 | sub_6 = rospy.Subscriber('/robot_6/odom', Odometry, callback_6) 61 | sub_7 = rospy.Subscriber('/robot_7/odom', Odometry, callback_7) 62 | pub = [] 63 | pub.append(rospy.Publisher('/robot_0/cmd_vel', Twist, queue_size=10)) 64 | pub.append(rospy.Publisher('/robot_1/cmd_vel', Twist, queue_size=10)) 65 | pub.append(rospy.Publisher('/robot_2/cmd_vel', Twist, queue_size=10)) 66 | pub.append(rospy.Publisher('/robot_3/cmd_vel', Twist, queue_size=10)) 67 | pub.append(rospy.Publisher('/robot_4/cmd_vel', Twist, queue_size=10)) 68 | pub.append(rospy.Publisher('/robot_5/cmd_vel', Twist, queue_size=10)) 69 | pub.append(rospy.Publisher('/robot_6/cmd_vel', Twist, queue_size=10)) 70 | pub.append(rospy.Publisher('/robot_7/cmd_vel', Twist, queue_size=10)) 71 | pub_setpoint = rospy.Publisher('/setpoint', Vector3, queue_size=10) 72 | t = 0 73 | 74 | setpoint = Vector3() 75 | setpoint.x = goal[4][0] 76 | setpoint.y = goal[4][1] 77 | 78 | rospy.wait_for_message('/clock', Clock) 79 | while not rospy.is_shutdown(): 80 | 81 | agents = update_agents(agents) 82 | 83 | for i, agent in enumerate(agents): 84 | candidates = agents[:i] + agents[i + 1:] 85 | agent.velocity, _ = orca(agent, candidates, tau, Ts) 86 | 87 | for i in xrange(len(X)): 88 | vel = Twist() 89 | vel.linear.x = agents[i].velocity[0] 90 | vel.linear.y = agents[i].velocity[1] 91 | 92 | pub[i].publish(vel) 93 | 94 | pub_setpoint.publish(setpoint) 95 | 96 | if t%100: 97 | #print(X) 98 | pass 99 | t += 1 100 | rospy.sleep(Ts) 101 | -------------------------------------------------------------------------------- /orca/src/diff_omni_controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import rospy 4 | import numpy as np 5 | from nav_msgs.msg import Odometry 6 | from geometry_msgs.msg import Twist 7 | from pyorca import Agent, orca, normalized 8 | 9 | N_AGENTS = 4 10 | RADIUS = 0.7 11 | tau = 5 12 | 13 | Ts = 0.01 14 | X = [(-5, 0.25), (5, 0), (0.00, 5), (-0.25, -5)] 15 | orientation = [0, np.pi, -np.pi/2, np.pi/2] 16 | V = [(0, 0) for _ in xrange(len(X))] 17 | V_max = [1.0 for _ in xrange(len(X))] 18 | goal = [(5.0, 0.0), (-5.0, 0.0), (0.0, -5.0), (0.0, 5.0)] 19 | 20 | agents = [] 21 | 22 | 23 | def velocityTransform(v, theta_0): 24 | angular = np.arctan2(v[1], v[0]) - theta_0 25 | linear = np.sqrt(v[0]**2 + v[1]**2) 26 | 27 | # Handling singularity 28 | if np.abs(angular) > np.pi: 29 | angular -= np.sign(angular) * 2 * np.pi 30 | 31 | return [linear, angular] 32 | for i in xrange(len(X)): 33 | vel = (np.array(goal[i]) - np.array(X[i])) 34 | if np.linalg.norm(vel) > V_max[i]: 35 | vel = normalized(vel) * V_max[i] 36 | agents.append(Agent(X[i], (0., 0.), RADIUS, V_max[i], vel)) 37 | 38 | def update_agents(agents): 39 | for i in xrange(len(X)): 40 | agents[i].pref_velocity = (np.array(goal[i]) - np.array(X[i])) 41 | if np.linalg.norm(agents[i].pref_velocity) > V_max[i]: 42 | agents[i].pref_velocity = normalized(agents[i].pref_velocity) * V_max[i] 43 | return agents 44 | 45 | def callback_0(msg): 46 | X[0] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 47 | orientation[0] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 48 | def callback_1(msg): 49 | X[1] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 50 | orientation[1] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 51 | def callback_2(msg): 52 | X[2] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 53 | orientation[2] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 54 | def callback_3(msg): 55 | X[3] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 56 | orientation[3] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 57 | 58 | 59 | rospy.init_node('omni_controller') 60 | 61 | sub_0 = rospy.Subscriber('/robot_0/odom', Odometry, callback_0) 62 | sub_1 = rospy.Subscriber('/robot_1/odom', Odometry, callback_1) 63 | sub_2 = rospy.Subscriber('/robot_2/odom', Odometry, callback_2) 64 | sub_3 = rospy.Subscriber('/robot_3/odom', Odometry, callback_3) 65 | pub_0 = rospy.Publisher('/robot_0/cmd_vel', Twist, queue_size=10) 66 | pub_1 = rospy.Publisher('/robot_1/cmd_vel', Twist, queue_size=10) 67 | pub_2 = rospy.Publisher('/robot_2/cmd_vel', Twist, queue_size=10) 68 | pub_3 = rospy.Publisher('/robot_3/cmd_vel', Twist, queue_size=10) 69 | t = 0 70 | 71 | while not rospy.is_shutdown(): 72 | agents = update_agents(agents) 73 | for i, agent in enumerate(agents): 74 | candidates = agents[:i] + agents[i + 1:] 75 | agent.velocity, _ = orca(agent, candidates, tau, Ts) 76 | 77 | vel_0 = Twist() 78 | 79 | [linear_vel_0, angular_vel_0] = velocityTransform(agents[0].velocity, orientation[0]) 80 | 81 | vel_0.linear.x = linear_vel_0 82 | vel_0.angular.z = angular_vel_0 83 | 84 | vel_1 = Twist() 85 | 86 | [linear_vel_1, angular_vel_1] = velocityTransform(agents[1].velocity, orientation[1]) 87 | 88 | vel_1.linear.x = linear_vel_1 89 | vel_1.angular.z = angular_vel_1 90 | 91 | vel_2 = Twist() 92 | 93 | [linear_vel_2, angular_vel_2] = velocityTransform(agents[2].velocity, orientation[2]) 94 | 95 | vel_2.linear.x = linear_vel_2 96 | vel_2.angular.z = angular_vel_2 97 | 98 | vel_3 = Twist() 99 | 100 | [linear_vel_3, angular_vel_3] = velocityTransform(agents[3].velocity, orientation[3]) 101 | 102 | vel_3.linear.x = linear_vel_3 103 | vel_3.angular.z = angular_vel_3 104 | 105 | pub_0.publish(vel_0) 106 | pub_1.publish(vel_1) 107 | pub_2.publish(vel_2) 108 | pub_3.publish(vel_3) 109 | 110 | if t%100: 111 | print(X) 112 | #print(agents[0].velocity) 113 | t += 1 114 | rospy.sleep(Ts) 115 | -------------------------------------------------------------------------------- /rvo/src/dense_diff_controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import rospy 4 | import numpy as np 5 | from nav_msgs.msg import Odometry 6 | from rosgraph_msgs.msg import Clock 7 | from geometry_msgs.msg import Twist, Vector3 8 | from RVO import RVO_update, compute_V_des 9 | 10 | scenario = dict() 11 | scenario['robot_radius'] = 0.5 12 | scenario['circular_obstacles'] = [] 13 | scenario['boundary'] = [] 14 | 15 | Ts = 0.01 16 | X = [[-10., 0.], [10., 0.], [0.0, 10.], [0., -10.], [-10., 10.], [-10., -10.], [10.0, -10.], [10., 10.]] 17 | orientation = [0, np.pi, -np.pi/2, np.pi/2, -np.pi/4, np.pi/4, 3*np.pi/4, -3*np.pi/4] 18 | V = [[0., 0.] for _ in xrange(len(X))] 19 | V_min = [-1.0 for _ in xrange(len(X))] 20 | V_max = [1.0 for _ in xrange(len(X))] 21 | goal = [[10., 0.], [-10., 0.], [0.0, -10.], [0., 10.], [10., -10.], [10., 10.], [-10.0, 10.], [-10., -10.]] 22 | 23 | def velocityTransform(v, theta_0): 24 | angular = np.arctan2(v[1], v[0]) - theta_0 25 | linear = np.sqrt(v[0]**2 + v[1]**2) 26 | 27 | # Handling singularity 28 | if np.abs(angular) > np.pi: 29 | angular -= np.sign(angular) * 2 * np.pi 30 | 31 | return [linear, angular] 32 | 33 | def callback_0(msg): 34 | X[0] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 35 | orientation[0] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 36 | def callback_1(msg): 37 | X[1] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 38 | orientation[1] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 39 | def callback_2(msg): 40 | X[2] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 41 | orientation[2] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 42 | def callback_3(msg): 43 | X[3] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 44 | orientation[3] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 45 | def callback_4(msg): 46 | X[4] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 47 | orientation[4] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 48 | def callback_5(msg): 49 | X[5] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 50 | orientation[5] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 51 | def callback_6(msg): 52 | X[6] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 53 | orientation[6] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 54 | def callback_7(msg): 55 | X[7] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 56 | orientation[7] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 57 | 58 | 59 | rospy.init_node('diff_controller') 60 | 61 | sub_0 = rospy.Subscriber('/robot_0/odom', Odometry, callback_0) 62 | sub_1 = rospy.Subscriber('/robot_1/odom', Odometry, callback_1) 63 | sub_2 = rospy.Subscriber('/robot_2/odom', Odometry, callback_2) 64 | sub_3 = rospy.Subscriber('/robot_3/odom', Odometry, callback_3) 65 | sub_4 = rospy.Subscriber('/robot_4/odom', Odometry, callback_4) 66 | sub_5 = rospy.Subscriber('/robot_5/odom', Odometry, callback_5) 67 | sub_6 = rospy.Subscriber('/robot_6/odom', Odometry, callback_6) 68 | sub_7 = rospy.Subscriber('/robot_7/odom', Odometry, callback_7) 69 | pub = [] 70 | pub.append(rospy.Publisher('/robot_0/cmd_vel', Twist, queue_size=10)) 71 | pub.append(rospy.Publisher('/robot_1/cmd_vel', Twist, queue_size=10)) 72 | pub.append(rospy.Publisher('/robot_2/cmd_vel', Twist, queue_size=10)) 73 | pub.append(rospy.Publisher('/robot_3/cmd_vel', Twist, queue_size=10)) 74 | pub.append(rospy.Publisher('/robot_4/cmd_vel', Twist, queue_size=10)) 75 | pub.append(rospy.Publisher('/robot_5/cmd_vel', Twist, queue_size=10)) 76 | pub.append(rospy.Publisher('/robot_6/cmd_vel', Twist, queue_size=10)) 77 | pub.append(rospy.Publisher('/robot_7/cmd_vel', Twist, queue_size=10)) 78 | pub_setpoint = rospy.Publisher('/setpoint', Vector3, queue_size=10) 79 | 80 | setpoint = Vector3() 81 | setpoint.x = goal[4][0] 82 | setpoint.y = goal[4][1] 83 | 84 | rospy.wait_for_message('/clock', Clock) 85 | while not rospy.is_shutdown(): 86 | 87 | V_des = compute_V_des(X, goal, V_max) 88 | V = RVO_update(X, V_des, V, scenario) 89 | 90 | 91 | for i in xrange(len(X)): 92 | vel = Twist() 93 | [vel.linear.x, vel.angular.z] = velocityTransform(V[i], orientation[i]) 94 | 95 | pub[i].publish(vel) 96 | 97 | pub_setpoint.publish(setpoint) 98 | rospy.sleep(Ts) 99 | -------------------------------------------------------------------------------- /gazebo_worlds/urdf/herbie.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 0 0 0 0 0 0 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 0 45 | 0 46 | 1.0 47 | 1.0 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 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 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | -------------------------------------------------------------------------------- /gazebo_worlds/launch/world2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 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 | 72 | 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 | -------------------------------------------------------------------------------- /gazebo_worlds/launch/world3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 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 | 72 | 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 | -------------------------------------------------------------------------------- /mpc/src/MPC.py: -------------------------------------------------------------------------------- 1 | import osqp 2 | import numpy 3 | import scipy.sparse as sparse 4 | 5 | class MPC: 6 | 7 | def __init__(self, position, v_min, v_max, N, N_c, Ts): 8 | """ MPC-ORCA controller instance 9 | 10 | :param goal: Goal position 11 | :type goal: Numpy Array 2x1 12 | :param position: Initial position 13 | :type position: Numpy Array 2x1 14 | :param v_min: Lower velocity constraint 15 | :type v_min: float 16 | :param v_max: Upper velocity constraint 17 | :type v_max: float 18 | :param N: Prediction Horizon 19 | :type N: int 20 | :param N_c: Control Horizon 21 | :type N_c: int 22 | :param Ts: Sampling Time 23 | :type Ts: float 24 | :returns: Controller instance 25 | :type: MPCORCA 26 | """ 27 | 28 | self.N = N 29 | self.N_c = N_c 30 | self.Ts = Ts 31 | 32 | # Linear Dynamics 33 | # x = [p_x, p_y, v_x, v_y]' 34 | # u = [a_x, a_y]' 35 | Ad = sparse.csc_matrix([ 36 | [1., 0., Ts, 0. ], 37 | [0., 1., 0., Ts ], 38 | [0., 0., 1., 0. ], 39 | [0., 0., 0., 1. ] 40 | ]) 41 | 42 | Bd = sparse.csc_matrix([ 43 | [0.5 * Ts ** 2, 0. ], 44 | [0., 0.5 * Ts ** 2 ], 45 | [Ts, 0. ], 46 | [0., Ts ] 47 | ]) 48 | [self.nx, self.nu] = Bd.shape 49 | 50 | # State constraints 51 | xmin = numpy.array([-numpy.inf, -numpy.inf, v_min, v_min]) 52 | xmax = numpy.array([numpy.inf, numpy.inf, v_max, v_max]) 53 | umin = numpy.array([v_min/Ts, v_min/Ts]) 54 | umax = numpy.array([v_max/Ts, v_max/Ts]) 55 | 56 | # Initial state 57 | self.x_0 = numpy.array([position[0], position[1], 0., 0.]) 58 | 59 | # Setpoint 60 | x_r = self.x_0 61 | 62 | # MPC objective function 63 | Q_0 = sparse.diags([3.0, 3.0, 0.0, 0.0]) 64 | Q = sparse.diags([1.0, 1.0, 0.0, 0.0]) 65 | R = 0.55 * sparse.eye(self.nu) 66 | 67 | # Casting QP format 68 | # QP objective 69 | P = sparse.block_diag([Q, Q_0, sparse.kron(sparse.eye(N-1), Q), sparse.kron(sparse.eye(N), R)]).tocsc() 70 | self.q = numpy.hstack([-Q.dot(x_r), -Q_0.dot(x_r), numpy.kron(numpy.ones(N-1), -Q.dot(x_r)), numpy.zeros(N * self.nu)]) 71 | 72 | # QP constraints 73 | # - linear dynamics 74 | Ax = sparse.kron(sparse.eye(N+1),-sparse.eye(self.nx)) + sparse.kron(sparse.eye(N+1, k=-1), Ad) 75 | Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), Bd) 76 | A_eq = sparse.hstack([Ax, Bu]) 77 | l_eq = numpy.hstack([-self.x_0, numpy.zeros(N*self.nx)]) 78 | u_eq = l_eq 79 | 80 | # - Control horizon constraint 81 | A_N_c = sparse.hstack([numpy.zeros((self.nu * (N - N_c), (N+1) * self.nx)), \ 82 | numpy.zeros((self.nu * (N - N_c), (N_c - 1) * self.nu)), \ 83 | -sparse.kron(numpy.ones(((N - N_c), 1)), sparse.eye(self.nu)), \ 84 | sparse.eye(self.nu * (N - N_c))]) 85 | l_N_c = numpy.zeros(self.nu * (N - N_c)) 86 | u_N_c = numpy.zeros(self.nu * (N - N_c)) 87 | 88 | # - input and state constraints 89 | A_ineq = sparse.eye((N+1) * self.nx + N * self.nu) 90 | l_ineq = numpy.hstack([numpy.kron(numpy.ones(N+1), xmin), numpy.kron(numpy.ones(N), umin)]) 91 | u_ineq = numpy.hstack([numpy.kron(numpy.ones(N+1), xmax), numpy.kron(numpy.ones(N), umax)]) 92 | 93 | # OSQP constraints 94 | A = sparse.vstack([A_eq, A_N_c, A_ineq]).tocsc() 95 | self.l = numpy.hstack([l_eq, l_N_c, l_ineq]) 96 | self.u = numpy.hstack([u_eq, u_N_c, u_ineq]) 97 | self.Q_0 = Q_0 98 | self.Q = Q 99 | self.R = R 100 | 101 | # Setting problem 102 | self.problem = osqp.OSQP() 103 | self.problem.setup(P, self.q, A, self.l, self.u, warm_start=True, verbose=False) 104 | 105 | def getNewVelocity(self, setpoint): 106 | # Updating initial conditions 107 | self.q = numpy.hstack([-self.Q.dot(setpoint[0:self.nx]), -self.Q_0.dot(setpoint[self.nx:2*self.nx]), numpy.dot(sparse.kron(sparse.eye(self.N-1), -self.Q).toarray(), setpoint[2*self.nx:]), numpy.zeros(self.N * self.nu)]) 108 | 109 | self.l[:self.nx] = -self.x_0 110 | self.u[:self.nx] = -self.x_0 111 | 112 | self.problem.update(q=self.q, l=self.l, u=self.u) 113 | result = self.problem.solve() 114 | 115 | if result.info.status == 'solved': 116 | # return the first resulting velocity after control action 117 | #print(result.x[-self.N*self.nu:]) 118 | return [result.x[(self.nx + 2):(self.nx + 4)], result.x[-self.N*self.nu:-(self.N-1)*self.nu]] 119 | else: 120 | print('unsolved') 121 | return [numpy.array([self.x_0[2], self.x_0[3]]), numpy.zeros(2)] 122 | #damping = 0.05 123 | #return numpy.array([self.agent.velocity[0] - numpy.sign(self.agent.velocity[0])*damping, self.agent.velocity[1] - numpy.sign(self.agent.velocity[1])*damping]) 124 | 125 | -------------------------------------------------------------------------------- /orca/src/dense_diff_controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import rospy 4 | import numpy as np 5 | from nav_msgs.msg import Odometry 6 | from rosgraph_msgs.msg import Clock 7 | from geometry_msgs.msg import Twist, Vector3 8 | from pyorca import Agent, orca, normalized 9 | 10 | N_AGENTS = 4 11 | RADIUS = 0.7 12 | tau = 15 13 | 14 | Ts = 0.01 15 | X = [[-10., 0.25], [10., 0.], [0.25, 10.], [0., -10.], [-10.25, 10.], [-10., -10.], [10.0, -10.25], [10., 10.]] 16 | orientation = [0, np.pi, -np.pi/2, np.pi/2, -np.pi/4, np.pi/4, 3*np.pi/4, -3*np.pi/4] 17 | V = [[0., 0.] for _ in xrange(len(X))] 18 | V_min = [-1.0 for _ in xrange(len(X))] 19 | V_max = [1.0 for _ in xrange(len(X))] 20 | goal = [[10., 0.], [-10., 0.], [0.0, -10.], [0., 10.], [10., -10.], [10., 10.], [-10.0, 10.], [-10., -10.]] 21 | 22 | agents = [] 23 | 24 | 25 | def velocityTransform(v, theta_0): 26 | angular = np.arctan2(v[1], v[0]) - theta_0 27 | linear = np.sqrt(v[0]**2 + v[1]**2) 28 | 29 | # Handling singularity 30 | if np.abs(angular) > np.pi: 31 | angular -= np.sign(angular) * 2 * np.pi 32 | 33 | return [linear, angular] 34 | for i in xrange(len(X)): 35 | vel = (np.array(goal[i]) - np.array(X[i])) 36 | if np.linalg.norm(vel) > V_max[i]: 37 | vel = normalized(vel) * V_max[i] 38 | agents.append(Agent(X[i], (0., 0.), RADIUS, V_max[i], vel)) 39 | 40 | def update_agents(agents): 41 | for i in xrange(len(X)): 42 | agents[i].pref_velocity = (np.array(goal[i]) - np.array(X[i])) 43 | if np.linalg.norm(agents[i].pref_velocity) > V_max[i]: 44 | agents[i].pref_velocity = normalized(agents[i].pref_velocity) * V_max[i] 45 | return agents 46 | 47 | def callback_0(msg): 48 | X[0] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 49 | orientation[0] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 50 | def callback_1(msg): 51 | X[1] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 52 | orientation[1] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 53 | def callback_2(msg): 54 | X[2] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 55 | orientation[2] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 56 | def callback_3(msg): 57 | X[3] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 58 | orientation[3] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 59 | def callback_4(msg): 60 | X[4] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 61 | orientation[4] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 62 | def callback_5(msg): 63 | X[5] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 64 | orientation[5] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 65 | def callback_6(msg): 66 | X[6] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 67 | orientation[6] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 68 | def callback_7(msg): 69 | X[7] = (float(msg.pose.pose.position.x), float(msg.pose.pose.position.y)) 70 | orientation[7] = 2 * np.arctan2(float(msg.pose.pose.orientation.z), float(msg.pose.pose.orientation.w)) 71 | 72 | rospy.init_node('diff_controller') 73 | 74 | sub_0 = rospy.Subscriber('/robot_0/odom', Odometry, callback_0) 75 | sub_1 = rospy.Subscriber('/robot_1/odom', Odometry, callback_1) 76 | sub_2 = rospy.Subscriber('/robot_2/odom', Odometry, callback_2) 77 | sub_3 = rospy.Subscriber('/robot_3/odom', Odometry, callback_3) 78 | sub_4 = rospy.Subscriber('/robot_4/odom', Odometry, callback_4) 79 | sub_5 = rospy.Subscriber('/robot_5/odom', Odometry, callback_5) 80 | sub_6 = rospy.Subscriber('/robot_6/odom', Odometry, callback_6) 81 | sub_7 = rospy.Subscriber('/robot_7/odom', Odometry, callback_7) 82 | pub = [] 83 | pub.append(rospy.Publisher('/robot_0/cmd_vel', Twist, queue_size=10)) 84 | pub.append(rospy.Publisher('/robot_1/cmd_vel', Twist, queue_size=10)) 85 | pub.append(rospy.Publisher('/robot_2/cmd_vel', Twist, queue_size=10)) 86 | pub.append(rospy.Publisher('/robot_3/cmd_vel', Twist, queue_size=10)) 87 | pub.append(rospy.Publisher('/robot_4/cmd_vel', Twist, queue_size=10)) 88 | pub.append(rospy.Publisher('/robot_5/cmd_vel', Twist, queue_size=10)) 89 | pub.append(rospy.Publisher('/robot_6/cmd_vel', Twist, queue_size=10)) 90 | pub.append(rospy.Publisher('/robot_7/cmd_vel', Twist, queue_size=10)) 91 | pub_setpoint = rospy.Publisher('/setpoint', Vector3, queue_size=10) 92 | 93 | setpoint = Vector3() 94 | setpoint.x = goal[4][0] 95 | setpoint.y = goal[4][1] 96 | 97 | rospy.wait_for_message('/clock', Clock) 98 | 99 | while not rospy.is_shutdown(): 100 | agents = update_agents(agents) 101 | for i, agent in enumerate(agents): 102 | candidates = agents[:i] + agents[i + 1:] 103 | agent.velocity, _ = orca(agent, candidates, tau, Ts) 104 | 105 | for i in xrange(len(X)): 106 | vel = Twist() 107 | [vel.linear.x, vel.angular.z] = velocityTransform(agents[i].velocity, orientation[i]) 108 | 109 | pub[i].publish(vel) 110 | 111 | pub_setpoint.publish(setpoint) 112 | rospy.sleep(Ts) 113 | -------------------------------------------------------------------------------- /mpc_orca/src/controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | 3 | import sys 4 | import rospy 5 | import numpy as np 6 | from gazebo_msgs.msg import ModelStates 7 | from geometry_msgs.msg import Twist, Vector3 8 | from MPC_ORCA import MPC_ORCA 9 | from pyorca import Agent 10 | 11 | # Reads robot index and goal values 12 | robot = int(sys.argv[1]) 13 | goal = np.array([float(sys.argv[2]), float(sys.argv[3])]) 14 | 15 | # Robot and controller parameters 16 | RADIUS = 0.4 17 | tau = 5 18 | N = 10 19 | N_c = N 20 | Ts = 0.1 21 | V_min = -5 22 | V_max = 5 23 | 24 | # Defining functions 25 | def updateWorld(msg): 26 | """This funcion is called whenever the gazebo/model_states publishes. This function 27 | updates the world variables as fast as possible""" 28 | for i in range(n_robots): 29 | X[i] = np.array([float(msg.pose[model[i]].position.x), float(msg.pose[model[i]].position.y)]) 30 | V[i] = np.array([float(msg.twist[model[i]].linear.x), float(msg.twist[model[i]].linear.y)]) 31 | orientation[i] = np.arctan2(2 * float(msg.pose[model[i]].orientation.w) * float(msg.pose[model[i]].orientation.z), 1 - 2 * float(msg.pose[model[i]].orientation.z)**2) 32 | 33 | def accelerationTransform(a, v, w, theta_0): 34 | """This function applies the linearization transformation on acceleration values 35 | based on equation 2.11 36 | """ 37 | d = 0.2 38 | cos_theta = np.cos(theta_0) 39 | sin_theta = np.sin(theta_0) 40 | term1 = a[0] + v * w * sin_theta + d * (w**2) * cos_theta 41 | term2 = a[1] - v * w * cos_theta + d * (w**2) * sin_theta 42 | acc_linear = cos_theta * term1 + sin_theta * term2 43 | acc_angular = -(sin_theta/d) * term1 + (cos_theta/d) * term2 44 | 45 | return [acc_linear, acc_angular] 46 | 47 | def update_positions(agents): 48 | """Each robot has a list of all agents in its neighborhood, including itself. This 49 | function updates that list, not as fast as updateWorld(). 50 | """ 51 | for i in range(n_robots): 52 | agents[i].position = np.array(X[i]) 53 | agents[i].velocity = np.array(V[i]) 54 | return agents 55 | 56 | # Initializing ros node 57 | rospy.init_node('mpc_orca_controller_robot_' + str(robot)) 58 | 59 | # Waiting gazebo first message 60 | data = rospy.wait_for_message('/gazebo/model_states', ModelStates) 61 | 62 | n_robots = len(data.name) - 1 63 | 64 | X = [np.zeros(2) for _ in range(n_robots)] 65 | V = [np.zeros(2) for _ in range(n_robots)] 66 | orientation = [0.0 for _ in range(n_robots)] 67 | model = [i+1 for i in range(n_robots)] 68 | 69 | # Getting robot model order on gazebo model_states 70 | for i, value in enumerate(data.name): 71 | # Skipping i == 0 because it's the ground_plane state 72 | if i > 0: 73 | idx = value.split('_') 74 | model[int(idx[1])] = i 75 | 76 | # Update initial X, V and orientation 77 | updateWorld(data) 78 | 79 | # Agents list 80 | agents = [] 81 | for i in range(n_robots): 82 | agents.append(Agent(X[i], np.zeros(2), np.zeros(2), RADIUS)) 83 | 84 | # Subscribing on model_states instead of robot/odom, to avoid unnecessary noise 85 | rospy.Subscriber('/gazebo/model_states', ModelStates, updateWorld) 86 | 87 | # Velocity publisher 88 | pub = rospy.Publisher('/robot_' + str(robot) + '/cmd_vel', Twist, queue_size=10) 89 | 90 | # Setpoint Publishers 91 | pub_setpoint_pos = rospy.Publisher('/robot_' + str(robot) + '/setpoint_pos', Vector3, queue_size=10) 92 | pub_setpoint_vel = rospy.Publisher('/robot_' + str(robot) + '/setpoint_vel', Vector3, queue_size=10) 93 | 94 | setpoint_pos = Vector3() 95 | setpoint_vel = Vector3() 96 | 97 | # Initializing Controllers 98 | colliders = agents[:robot] + agents[robot + 1:] 99 | controller = MPC_ORCA(agents[robot].position, V_min, V_max, N, N_c, Ts, colliders, tau, RADIUS) 100 | 101 | # Global path planning 102 | initial = np.copy(X[robot]) 103 | t_max = 10.0 104 | growth = 0.5 105 | logistic = lambda t: 1/(1 + np.exp(- growth * (t - t_max))) 106 | d_logistic = lambda t: growth * logistic(t) * (1 - logistic(t)) 107 | P_des = lambda t: goal * logistic(t) + initial * (1 - logistic(t)) 108 | V_des = lambda t: goal * d_logistic(t) - initial * d_logistic(t) 109 | 110 | t = 0 111 | 112 | vel = Twist() 113 | while not rospy.is_shutdown(): 114 | 115 | # Updating controller list of agents 116 | agents = update_positions(agents) 117 | controller.agent = agents[robot] 118 | controller.colliders = agents[:robot] + agents[robot + 1:] 119 | 120 | # Updating setpoint trajectory 121 | setpoint = np.ravel([np.append(P_des(t + k * Ts), V_des(t + k * Ts)) for k in range(0, N + 1)]) 122 | 123 | # Computing optimal input values 124 | [_, agents[robot].acceleration] = controller.compute(setpoint) 125 | 126 | # Saving setpoints 127 | [setpoint_pos.x, setpoint_pos.y] = P_des(t) 128 | [setpoint_vel.x, setpoint_vel.y] = V_des(t) 129 | 130 | # Linearization transformation for linear and angular acceleration 131 | [acc_linear, acc_angular] = accelerationTransform(agents[robot].acceleration, vel.linear.x, vel.angular.z, orientation[robot]) 132 | 133 | # Integrating acceleration to get velocity commands 134 | vel.linear.x = vel.linear.x + acc_linear * Ts 135 | vel.angular.z = vel.angular.z + acc_angular * Ts 136 | 137 | # Publishing to topics 138 | pub.publish(vel) 139 | pub_setpoint_pos.publish(setpoint_pos) 140 | pub_setpoint_vel.publish(setpoint_vel) 141 | 142 | # Sleep until the next iteration 143 | rospy.sleep(Ts) 144 | t += Ts -------------------------------------------------------------------------------- /orca/src/test.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013 Mak Nazecic-Andrlon 2 | # 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # 10 | # The above copyright notice and this permission notice shall be included in all 11 | # copies or substantial portions of the Software. 12 | # 13 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | # SOFTWARE. 20 | 21 | 22 | from __future__ import division 23 | 24 | from pyorca import Agent, get_avoidance_velocity, orca, normalized, perp 25 | from numpy import array, rint, linspace, pi, cos, sin, linalg 26 | from numpy.linalg import norm 27 | import pygame 28 | 29 | import itertools 30 | import random 31 | 32 | N_AGENTS = 2 33 | RADIUS = 1.0 34 | 35 | agents = [] 36 | 37 | X = [(-15, 0), (15, 0.5), (0.00, 15), (-0.5, -15)] 38 | V = [(0.0, 0.0) for _ in xrange(len(X))] 39 | V_max = [5.0 for _ in xrange(len(X))] 40 | goal = [(15.0, 0.0), (-15.0, 0.0), (0.0, -15.0), (0.0, 15.0)] 41 | for i in range(N_AGENTS): 42 | #theta = 2 * pi * i / N_AGENTS 43 | #x = RADIUS * array((cos(theta), sin(theta))) #+ random.uniform(-1, 1) 44 | #vel = normalized(-x) * SPEED 45 | #pos = (random.uniform(-20, 20), random.uniform(-20, 20)) 46 | pos = X[i] 47 | vel = (array(goal[i]) - array(X[i])) 48 | if norm(vel) > V_max[i]: 49 | vel = normalized(vel) * V_max[i] 50 | agents.append(Agent(pos, (0., 0.), RADIUS, V_max[i], vel)) 51 | 52 | 53 | colors = [ 54 | (255, 0, 0), 55 | (0, 255, 0), 56 | (0, 0, 255), 57 | (255, 255, 0), 58 | (0, 255, 255), 59 | (255, 0, 255), 60 | ] 61 | 62 | pygame.init() 63 | 64 | dim = (640, 480) 65 | screen = pygame.display.set_mode(dim) 66 | 67 | O = array(dim) / 2 # Screen position of origin. 68 | scale = 6 # Drawing scale. 69 | 70 | clock = pygame.time.Clock() 71 | FPS = 10 72 | dt = 1/FPS 73 | tau = 5 74 | 75 | def draw_agent(agent, color): 76 | pygame.draw.circle(screen, color, rint(agent.position * scale + O).astype(int), int(round(agent.radius * scale)), 0) 77 | 78 | def draw_orca_circles(a, b): 79 | for x in linspace(0, tau, 21): 80 | if x == 0: 81 | continue 82 | pygame.draw.circle(screen, pygame.Color(0, 0, 255), rint((-(a.position - b.position) / x + a.position) * scale + O).astype(int), int(round((a.radius + b.radius) * scale / x)), 1) 83 | 84 | def draw_velocity(a): 85 | pygame.draw.line(screen, pygame.Color(0, 255, 255), rint(a.position * scale + O).astype(int), rint((a.position + a.velocity) * scale + O).astype(int), 1) 86 | # pygame.draw.line(screen, pygame.Color(255, 0, 255), rint(a.position * scale + O).astype(int), rint((a.position + a.pref_velocity) * scale + O).astype(int), 1) 87 | 88 | running = True 89 | accum = 0 90 | all_lines = [[]] * len(agents) 91 | while running: 92 | accum += clock.tick(FPS) 93 | 94 | while accum >= dt * 1000: 95 | accum -= dt * 1000 96 | 97 | new_vels = [None] * len(agents) 98 | for i, agent in enumerate(agents): 99 | candidates = agents[:i] + agents[i + 1:] 100 | # print(candidates) 101 | new_vels[i], all_lines[i] = orca(agent, candidates, tau, dt) 102 | # print(i, agent.velocity) 103 | 104 | for i, agent in enumerate(agents): 105 | agent.velocity = new_vels[i] 106 | agent.position = agent.position + agent.velocity * dt 107 | 108 | screen.fill(pygame.Color(0, 0, 0)) 109 | 110 | for agent in agents[1:]: 111 | draw_orca_circles(agents[0], agent) 112 | 113 | for agent, color in zip(agents, itertools.cycle(colors)): 114 | draw_agent(agent, color) 115 | draw_velocity(agent) 116 | # print(sqrt(norm_sq(agent.velocity))) 117 | 118 | for line in all_lines[0]: 119 | # Draw ORCA line 120 | alpha = agents[0].position + line.point + perp(line.direction) * 100 121 | beta = agents[0].position + line.point + perp(line.direction) * -100 122 | pygame.draw.line(screen, (255, 255, 255), rint(alpha * scale + O).astype(int), rint(beta * scale + O).astype(int), 1) 123 | 124 | # Draw normal to ORCA line 125 | gamma = agents[0].position + line.point 126 | delta = agents[0].position + line.point + line.direction 127 | pygame.draw.line(screen, (255, 255, 255), rint(gamma * scale + O).astype(int), rint(delta * scale + O).astype(int), 1) 128 | 129 | pygame.display.flip() 130 | 131 | ## updateAgents 132 | for i in range(N_AGENTS): 133 | agents[i].pref_velocity = (array(goal[i]) - array(agents[i].position)) 134 | if norm(agents[i].pref_velocity) > V_max[i]: 135 | agents[i].pref_velocity = normalized(agents[i].pref_velocity) * V_max[i] 136 | 137 | for event in pygame.event.get(): 138 | if event.type == pygame.QUIT: 139 | running = False 140 | pygame.quit() 141 | -------------------------------------------------------------------------------- /orca/src/halfplaneintersect.py: -------------------------------------------------------------------------------- 1 | # The MIT License (MIT) 2 | # 3 | # Copyright (c) 2013 Mak Nazecic-Andrlon 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to 7 | # deal in the Software without restriction, including without limitation the 8 | # rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9 | # sell copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in 13 | # all copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21 | # IN THE SOFTWARE. 22 | 23 | """This module solves 2D linear programming using half-plane intersection.""" 24 | 25 | from __future__ import division 26 | 27 | import itertools 28 | 29 | from numpy import dot, clip, array, sqrt 30 | from numpy.linalg import det 31 | 32 | 33 | class InfeasibleError(RuntimeError): 34 | """Raised if an LP problem has no solution.""" 35 | pass 36 | 37 | 38 | class Line(object): 39 | """A line in space.""" 40 | def __init__(self, point, direction): 41 | super(Line, self).__init__() 42 | self.point = array(point) 43 | self.direction = normalized(array(direction)) 44 | 45 | def __repr__(self): 46 | return "Line(%s, %s)" % (self.point, self.direction) 47 | 48 | 49 | def halfplane_optimize(lines, optimal_point): 50 | """Find the point closest to optimal_point in the intersection of the 51 | closed half-planes defined by lines which are in Hessian normal form 52 | (point-normal form).""" 53 | # We implement the quadratic time (though linear expected given randomly 54 | # permuted input) incremental half-plane intersection algorithm as laid 55 | # out in http://www.mpi-inf.mpg.de/~kavitha/lecture3.ps 56 | point = optimal_point 57 | for i, line in enumerate(lines): 58 | # If this half-plane already contains the current point, all is well. 59 | if dot(point - line.point, line.direction) >= 0: 60 | # assert False, point 61 | continue 62 | 63 | # Otherwise, the new optimum must lie on the newly added line. Compute 64 | # the feasible interval of the intersection of all the lines added so 65 | # far with the current one. 66 | prev_lines = itertools.islice(lines, i) 67 | left_dist, right_dist = line_halfplane_intersect(line, prev_lines) 68 | 69 | # Now project the optimal point onto the line segment defined by the 70 | # the above bounds. This gives us our new best point. 71 | point = point_line_project(line, optimal_point, left_dist, right_dist) 72 | return point 73 | 74 | def point_line_project(line, point, left_bound, right_bound): 75 | """Project point onto the line segment defined by line, which is in 76 | point-normal form, and the left and right bounds with respect to line's 77 | anchor point.""" 78 | # print("left_bound=%s, right_bound=%s" % (left_bound, right_bound)) 79 | new_dir = perp(line.direction) 80 | # print("new_dir=%s" % new_dir) 81 | proj_len = dot(point - line.point, new_dir) 82 | # print("proj_len=%s" % proj_len) 83 | clamped_len = clip(proj_len, left_bound, right_bound) 84 | # print("clamped_len=%s" % clamped_len) 85 | return line.point + new_dir * clamped_len 86 | 87 | def line_halfplane_intersect(line, other_lines): 88 | """Compute the signed offsets of the interval on the edge of the 89 | half-plane defined by line that is included in the half-planes defined by 90 | other_lines. 91 | 92 | The offsets are relative to line's anchor point, in units of line's 93 | direction. 94 | 95 | """ 96 | # We use the line intersection algorithm presented in 97 | # http://stackoverflow.com/a/565282/126977 to determine the intersection 98 | # point. "Left" is the negative of the canonical direction of the line. 99 | # "Right" is positive. 100 | left_dist = float("-inf") 101 | right_dist = float("inf") 102 | for prev_line in other_lines: 103 | num1 = dot(prev_line.direction, line.point - prev_line.point) 104 | den1 = det((line.direction, prev_line.direction)) 105 | # num2 = det((perp(prev_line.direction), line.point - prev_line.point)) 106 | # den2 = det((perp(line.direction), perp(prev_line.direction))) 107 | 108 | # assert abs(den1 - den2) < 1e-6, (den1, den2) 109 | # assert abs(num1 - num2) < 1e-6, (num1, num2) 110 | 111 | num = num1 112 | den = den1 113 | 114 | # Check for zero denominator, since ZeroDivisionError (or rather 115 | # FloatingPointError) won't necessarily be raised if using numpy. 116 | if den == 0: 117 | # The half-planes are parallel. 118 | if num < 0: 119 | # The intersection of the half-planes is empty; there is no 120 | # solution. 121 | raise InfeasibleError 122 | else: 123 | # The *half-planes* intersect, but their lines don't cross, so 124 | # ignore. 125 | continue 126 | 127 | # Signed offset of the point of intersection, relative to the line's 128 | # anchor point, in units of the line's direction. 129 | offset = num / den 130 | if den > 0: 131 | # Point of intersection is to the right. 132 | right_dist = min((right_dist, offset)) 133 | else: 134 | # Point of intersection is to the left. 135 | left_dist = max((left_dist, offset)) 136 | 137 | if left_dist > right_dist: 138 | # The interval is inconsistent, so the feasible region is empty. 139 | raise InfeasibleError 140 | return left_dist, right_dist 141 | 142 | def perp(a): 143 | return array((a[1], -a[0])) 144 | 145 | def norm_sq(x): 146 | return dot(x, x) 147 | 148 | def norm(x): 149 | return sqrt(norm_sq(x)) 150 | 151 | def normalized(x): 152 | l = norm_sq(x) 153 | assert l > 0, (x, l) 154 | return x / sqrt(l) 155 | 156 | if __name__ == '__main__': 157 | lines = [ 158 | Line((-2, 0), (-1, 1)), 159 | Line((0, -1), (1, 0)) 160 | ] 161 | point = array((1, 0)) 162 | result = halfplane_optimize(lines, point) 163 | print(result, norm(result)) 164 | 165 | # a = point_line_project(lines[0], point, -20, 20) 166 | # print((a - lines[0].point)/(-perp(lines[0].direction))) 167 | print(a) 168 | 169 | a = point_line_project(lines[1], point, -10000, -3) 170 | # print((a - lines[1].point)/(-perp(lines[1].direction))) 171 | print(a) 172 | -------------------------------------------------------------------------------- /mpc_orca/src/pyorca.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013 Mak Nazecic-Andrlon 2 | # 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # 10 | # The above copyright notice and this permission notice shall be included in all 11 | # copies or substantial portions of the Software. 12 | # 13 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | # SOFTWARE. 20 | 21 | # Adapted by @glauberrleite 22 | 23 | """Implementation of the 2D ORCA algorithm as described by J. van der Berg, 24 | S. J. Guy, M. Lin and D. Manocha in 'Reciprocal n-body Collision Avoidance'.""" 25 | 26 | from __future__ import division 27 | 28 | import numpy 29 | from numpy import array, sqrt, copysign, dot 30 | from numpy.linalg import det 31 | 32 | # Method: 33 | # For each robot A and potentially colliding robot B, compute smallest change 34 | # in relative velocity 'u' that avoids collision. Find normal 'n' to VO at that 35 | # point. 36 | # For each such velocity 'u' and normal 'n', find half-plane as defined in (6). 37 | # Intersect half-planes and pick velocity closest to A's preferred velocity. 38 | 39 | class Agent(object): 40 | """A disk-shaped agent.""" 41 | def __init__(self, position, velocity, acceleration, radius): 42 | super(Agent, self).__init__() 43 | self.position = array(position) 44 | self.velocity = array(velocity) 45 | self.radius = radius 46 | self.acceleration = acceleration 47 | 48 | 49 | def orca(agent, colliding_agent, t, dt): 50 | """Compute ORCA solution for agent. NOTE: velocity must be _instantly_ 51 | changed on tick *edge*, like first-order integration, otherwise the method 52 | undercompensates and you will still risk colliding.""" 53 | dv, n = get_avoidance_velocity(agent, colliding_agent, t, dt) 54 | v0 = agent.velocity + dv / 2 55 | 56 | return v0, n 57 | 58 | def get_avoidance_velocity(agent, collider, t, dt): 59 | """Get the smallest relative change in velocity between agent and collider 60 | that will get them onto the boundary of each other's velocity obstacle 61 | (VO), and thus avert collision.""" 62 | 63 | # This is a summary of the explanation from the AVO paper. 64 | # 65 | # The set of all relative velocities that will cause a collision within 66 | # time tau is called the velocity obstacle (VO). If the relative velocity 67 | # is outside of the VO, no collision will happen for at least tau time. 68 | # 69 | # The VO for two moving disks is a circularly truncated triangle 70 | # (spherically truncated cone in 3D), with an imaginary apex at the 71 | # origin. It can be described by a union of disks: 72 | # 73 | # Define an open disk centered at p with radius r: 74 | # D(p, r) := {q | ||q - p|| < r} (1) 75 | # 76 | # Two disks will collide at time t iff ||x + vt|| < r, where x is the 77 | # displacement, v is the relative velocity, and r is the sum of their 78 | # radii. 79 | # 80 | # Divide by t: ||x/t + v|| < r/t, 81 | # Rearrange: ||v - (-x/t)|| < r/t. 82 | # 83 | # By (1), this is a disk D(-x/t, r/t), and it is the set of all velocities 84 | # that will cause a collision at time t. 85 | # 86 | # We can now define the VO for time tau as the union of all such disks 87 | # D(-x/t, r/t) for 0 < t <= tau. 88 | # 89 | # Note that the displacement and radius scale _inversely_ proportionally 90 | # to t, generating a line of disks of increasing radius starting at -x/t. 91 | # This is what gives the VO its cone shape. The _closest_ velocity disk is 92 | # at D(-x/tau, r/tau), and this truncates the VO. 93 | 94 | x = -(agent.position - collider.position) 95 | v = agent.velocity - collider.velocity 96 | r = agent.radius + collider.radius 97 | 98 | x_len_sq = norm_sq(x) 99 | 100 | if x_len_sq >= r * r: 101 | # We need to decide whether to project onto the disk truncating the VO 102 | # or onto the sides. 103 | # 104 | # The center of the truncating disk doesn't mark the line between 105 | # projecting onto the sides or the disk, since the sides are not 106 | # parallel to the displacement. We need to bring it a bit closer. How 107 | # much closer can be worked out by similar triangles. It works out 108 | # that the new point is at x/t cos(theta)^2, where theta is the angle 109 | # of the aperture (so sin^2(theta) = (r/||x||)^2). 110 | adjusted_center = x/t * (1 - (r*r)/x_len_sq) 111 | 112 | if dot(v - adjusted_center, adjusted_center) < 0: 113 | # v lies in the front part of the cone 114 | # print("front") 115 | # print("front", adjusted_center, x_len_sq, r, x, t) 116 | w = v - x/t 117 | u = normalized(w) * r/t - w 118 | n = normalized(w) 119 | else: # v lies in the rest of the cone 120 | # print("sides") 121 | # Rotate x in the direction of v, to make it a side of the cone. 122 | # Then project v onto that, and calculate the difference. 123 | leg_len = sqrt(x_len_sq - r*r) 124 | # The sign of the sine determines which side to project on. 125 | sine = copysign(r, det((v, x))) 126 | rot = array( 127 | ((leg_len, sine), 128 | (-sine, leg_len))) 129 | rotated_x = rot.dot(x) / x_len_sq 130 | n = perp(rotated_x) 131 | if sine < 0: 132 | # Need to flip the direction of the line to make the 133 | # half-plane point out of the cone. 134 | n = -n 135 | # print("rotated_x=%s" % rotated_x) 136 | u = rotated_x * dot(v, rotated_x) - v 137 | # print("u=%s" % u) 138 | else: 139 | # We're already intersecting. Pick the closest velocity to our 140 | # velocity that will get us out of the collision within the next 141 | # timestep. 142 | # print("intersecting") 143 | w = v - x/dt 144 | u = normalized(w) * r/dt - w 145 | n = normalized(w) 146 | return u, -n 147 | 148 | def norm_sq(x): 149 | return dot(x, x) 150 | 151 | def normalized(x): 152 | l = norm_sq(x) 153 | assert l > 0, (x, l) 154 | return x / sqrt(l) 155 | 156 | def dist_sq(a, b): 157 | return norm_sq(b - a) 158 | 159 | def perp(a): 160 | return array((a[1], -a[0])) -------------------------------------------------------------------------------- /mpc_orca/src/MPC_ORCA.py: -------------------------------------------------------------------------------- 1 | import osqp 2 | import numpy 3 | import scipy.sparse as sparse 4 | from pyorca import Agent, orca 5 | 6 | class MPC_ORCA: 7 | 8 | def __init__(self, position, v_min, v_max, N, N_c, Ts, colliders, tau, robot_radius): 9 | """ MPC-ORCA controller instance 10 | 11 | :param goal: Goal position 12 | :type goal: Numpy Array 2x1 13 | :param position: Initial position 14 | :type position: Numpy Array 2x1 15 | :param v_min: Lower velocity constraint 16 | :type v_min: float 17 | :param v_max: Upper velocity constraint 18 | :type v_max: float 19 | :param N: Prediction Horizon 20 | :type N: int 21 | :param Ts: Sampling Time 22 | :type Ts: float 23 | :returns: Controller instance 24 | :type: MPCORCA 25 | """ 26 | 27 | self.N = N 28 | self.N_c = N_c 29 | self.Ts = Ts 30 | self.tau = tau 31 | 32 | self.agent = Agent(position, numpy.zeros(2), numpy.zeros(2), robot_radius) 33 | self.colliders = colliders 34 | 35 | # Linear Dynamics 36 | # x = [p_x, p_y, v_x, v_y]' 37 | # u = [a_x, a_y]' 38 | Ad = sparse.csc_matrix([ 39 | [1., 0., Ts, 0. ], 40 | [0., 1., 0., Ts ], 41 | [0., 0., 1., 0. ], 42 | [0., 0., 0., 1. ] 43 | ]) 44 | 45 | Bd = sparse.csc_matrix([ 46 | [0.5 * Ts ** 2, 0. ], 47 | [0., 0.5 * Ts ** 2 ], 48 | [Ts, 0. ], 49 | [0., Ts ] 50 | ]) 51 | [self.nx, self.nu] = Bd.shape 52 | 53 | # State constraints 54 | xmin = numpy.array([-numpy.inf, -numpy.inf, v_min, v_min]) 55 | xmax = numpy.array([numpy.inf, numpy.inf, v_max, v_max]) 56 | umin = numpy.array([-numpy.inf, -numpy.inf]) 57 | umax = numpy.array([numpy.inf, numpy.inf]) 58 | 59 | # Initial state 60 | x_0 = numpy.array([position[0], position[1], 0., 0.]) 61 | 62 | # Setpoint 63 | x_r = x_0 64 | 65 | # MPC objective function 66 | #Q_0 = sparse.diags([100.0, 100.0, 0.0, 0.0]) 67 | Q_0 = sparse.diags([3, 3, 0.0, 0.0]) 68 | Q = sparse.diags([1.5, 1.5, 0.0, 0.0]) 69 | R = 1.5 * sparse.eye(self.nu) 70 | 71 | # Casting QP format 72 | # QP objective 73 | P = sparse.block_diag([Q, Q_0, sparse.kron(sparse.eye(N-1), Q), sparse.kron(sparse.eye(N), R)]).tocsc() 74 | self.q = numpy.hstack([-Q.dot(x_r), -Q_0.dot(x_r), numpy.kron(numpy.ones(N-1), -Q.dot(x_r)), numpy.zeros(N * self.nu)]) 75 | 76 | # QP constraints 77 | # - linear dynamics 78 | Ax = sparse.kron(sparse.eye(N+1),-sparse.eye(self.nx)) + sparse.kron(sparse.eye(N+1, k=-1), Ad) 79 | Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), Bd) 80 | A_eq = sparse.hstack([Ax, Bu]) 81 | l_eq = numpy.hstack([-x_0, numpy.zeros(N*self.nx)]) 82 | u_eq = l_eq 83 | 84 | # - Control horizon constraint 85 | A_N_c = sparse.hstack([numpy.zeros((self.nu * (N - N_c), (N+1) * self.nx)), \ 86 | numpy.zeros((self.nu * (N - N_c), (N_c - 1) * self.nu)), \ 87 | -sparse.kron(numpy.ones(((N - N_c), 1)), sparse.eye(self.nu)), \ 88 | sparse.eye(self.nu * (N - N_c))]) 89 | l_N_c = numpy.zeros(self.nu * (N - N_c)) 90 | u_N_c = numpy.zeros(self.nu * (N - N_c)) 91 | 92 | # - input and state constraints 93 | A_ineq = sparse.eye((N+1) * self.nx + N * self.nu) 94 | l_ineq = numpy.hstack([numpy.kron(numpy.ones(N+1), xmin), numpy.kron(numpy.ones(N), umin)]) 95 | u_ineq = numpy.hstack([numpy.kron(numpy.ones(N+1), xmax), numpy.kron(numpy.ones(N), umax)]) 96 | 97 | # ORCA Constraints 98 | A_ORCA_data = numpy.zeros(2 * len(self.colliders) * self.N) 99 | A_ORCA_rows = numpy.zeros(2 * len(self.colliders) * self.N) 100 | A_ORCA_cols = numpy.zeros(2 * len(self.colliders) * self.N) 101 | cnt = 0 102 | for k in range(N): 103 | for i in range(len(colliders)): 104 | A_ORCA_rows[cnt] = i * N + k 105 | A_ORCA_cols[cnt] = self.nx + k * self.nx + 2 106 | 107 | A_ORCA_rows[cnt + 1] = i * N + k 108 | A_ORCA_cols[cnt + 1] = self.nx + k * self.nx + 3 109 | cnt += 2 110 | A_ORCA = sparse.csc_matrix((A_ORCA_data, (A_ORCA_rows, A_ORCA_cols)), shape=(len(colliders) * N, A_eq.shape[1])) 111 | l_ORCA = numpy.zeros(len(colliders) * N) 112 | u_ORCA = numpy.zeros(len(colliders) * N) 113 | 114 | # OSQP constraints 115 | self.A = sparse.vstack([A_eq, A_N_c, A_ineq, A_ORCA]).tocsc() 116 | self.l = numpy.hstack([l_eq, l_N_c, l_ineq, l_ORCA]) 117 | self.u = numpy.hstack([u_eq, u_N_c, u_ineq, u_ORCA]) 118 | self.Q_0 = Q_0 119 | self.Q = Q 120 | self.R = R 121 | 122 | self.orca_rows_idx = A_eq.shape[0] + A_N_c.shape[0] + A_ineq.shape[0] 123 | 124 | # Setting problem 125 | self.problem = osqp.OSQP() 126 | self.problem.setup(P, self.q, self.A, self.l, self.u, warm_start=True, verbose=False) 127 | 128 | def compute(self, setpoint): 129 | # Updating initial conditions 130 | x_0 = numpy.array([self.agent.position[0], self.agent.position[1], self.agent.velocity[0], self.agent.velocity[1]]) 131 | 132 | self.q = numpy.hstack([-self.Q.dot(setpoint[0:self.nx]), -self.Q_0.dot(setpoint[self.nx:2*self.nx]), numpy.dot(sparse.kron(sparse.eye(self.N-1), -self.Q).toarray(), setpoint[2*self.nx:]), numpy.zeros(self.N * self.nu)]) 133 | 134 | self.l[:self.nx] = -x_0 135 | self.u[:self.nx] = -x_0 136 | 137 | # Predict future states with constant velocity, i.e. no acceleration 138 | for k in range(self.N): 139 | agent_k = Agent(self.agent.position + k * self.agent.velocity * self.Ts, self.agent.velocity, numpy.zeros(2), self.agent.radius) 140 | 141 | for i, collider in enumerate(self.colliders): 142 | collider_k = Agent(collider.position + k * collider.velocity * self.Ts, collider.velocity, numpy.zeros(2), collider.radius) 143 | 144 | # Discovering ORCA half-space 145 | v0, n = orca(agent_k, collider_k, self.tau, self.Ts) 146 | 147 | self.A[self.orca_rows_idx + i * self.N + k, self.nx + k * self.nx + 2] = n[0] 148 | self.A[self.orca_rows_idx + i * self.N + k, self.nx + k * self.nx + 3] = n[1] 149 | 150 | self.l[self.orca_rows_idx + i * self.N + k] = -numpy.inf 151 | self.u[self.orca_rows_idx + i * self.N + k] = numpy.dot(n, v0) 152 | 153 | self.problem.update(q=self.q, l=self.l, u=self.u, Ax=self.A.data) 154 | result = self.problem.solve() 155 | 156 | if result.info.status == 'solved': 157 | # return the first resulting velocity after control action 158 | return [result.x[(self.nx + 2):(self.nx + 4)], result.x[-self.N*self.nu:-(self.N-1)*self.nu]] 159 | else: 160 | print(result.info.status) 161 | 162 | damping = 3 163 | new_acceleration = (1 - damping) * self.agent.acceleration 164 | return [self.agent.velocity + new_acceleration * self.Ts, new_acceleration] -------------------------------------------------------------------------------- /orca/src/pyorca.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2013 Mak Nazecic-Andrlon 2 | # 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # 10 | # The above copyright notice and this permission notice shall be included in all 11 | # copies or substantial portions of the Software. 12 | # 13 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | # SOFTWARE. 20 | 21 | """Implementation of the 2D ORCA algorithm as described by J. van der Berg, 22 | S. J. Guy, M. Lin and D. Manocha in 'Reciprocal n-body Collision Avoidance'.""" 23 | 24 | from __future__ import division 25 | 26 | import numpy 27 | from numpy import array, sqrt, copysign, dot 28 | from numpy.linalg import det 29 | 30 | from halfplaneintersect import halfplane_optimize, Line, perp, InfeasibleError 31 | 32 | # Method: 33 | # For each robot A and potentially colliding robot B, compute smallest change 34 | # in relative velocity 'u' that avoids collision. Find normal 'n' to VO at that 35 | # point. 36 | # For each such velocity 'u' and normal 'n', find half-plane as defined in (6). 37 | # Intersect half-planes and pick velocity closest to A's preferred velocity. 38 | 39 | class Agent(object): 40 | """A disk-shaped agent.""" 41 | def __init__(self, position, velocity, radius, max_speed, pref_velocity): 42 | super(Agent, self).__init__() 43 | self.position = array(position) 44 | self.velocity = array(velocity) 45 | self.radius = radius 46 | self.max_speed = max_speed 47 | self.pref_velocity = array(pref_velocity) 48 | 49 | 50 | def orca(agent, colliding_agents, t, dt): 51 | """Compute ORCA solution for agent. NOTE: velocity must be _instantly_ 52 | changed on tick *edge*, like first-order integration, otherwise the method 53 | undercompensates and you will still risk colliding.""" 54 | lines = [] 55 | for collider in colliding_agents: 56 | dv, n = get_avoidance_velocity(agent, collider, t, dt) 57 | line = Line(agent.velocity + dv / 2, n) 58 | lines.append(line) 59 | try: 60 | return halfplane_optimize(lines, agent.pref_velocity), lines 61 | except (InfeasibleError): 62 | print('Infeasible') 63 | return agent.velocity, lines 64 | 65 | def get_avoidance_velocity(agent, collider, t, dt): 66 | """Get the smallest relative change in velocity between agent and collider 67 | that will get them onto the boundary of each other's velocity obstacle 68 | (VO), and thus avert collision.""" 69 | 70 | # This is a summary of the explanation from the AVO paper. 71 | # 72 | # The set of all relative velocities that will cause a collision within 73 | # time tau is called the velocity obstacle (VO). If the relative velocity 74 | # is outside of the VO, no collision will happen for at least tau time. 75 | # 76 | # The VO for two moving disks is a circularly truncated triangle 77 | # (spherically truncated cone in 3D), with an imaginary apex at the 78 | # origin. It can be described by a union of disks: 79 | # 80 | # Define an open disk centered at p with radius r: 81 | # D(p, r) := {q | ||q - p|| < r} (1) 82 | # 83 | # Two disks will collide at time t iff ||x + vt|| < r, where x is the 84 | # displacement, v is the relative velocity, and r is the sum of their 85 | # radii. 86 | # 87 | # Divide by t: ||x/t + v|| < r/t, 88 | # Rearrange: ||v - (-x/t)|| < r/t. 89 | # 90 | # By (1), this is a disk D(-x/t, r/t), and it is the set of all velocities 91 | # that will cause a collision at time t. 92 | # 93 | # We can now define the VO for time tau as the union of all such disks 94 | # D(-x/t, r/t) for 0 < t <= tau. 95 | # 96 | # Note that the displacement and radius scale _inversely_ proportionally 97 | # to t, generating a line of disks of increasing radius starting at -x/t. 98 | # This is what gives the VO its cone shape. The _closest_ velocity disk is 99 | # at D(-x/tau, r/tau), and this truncates the VO. 100 | 101 | x = -(agent.position - collider.position) 102 | v = agent.velocity - collider.velocity 103 | r = agent.radius + collider.radius 104 | 105 | x_len_sq = norm_sq(x) 106 | 107 | if x_len_sq >= r * r: 108 | # We need to decide whether to project onto the disk truncating the VO 109 | # or onto the sides. 110 | # 111 | # The center of the truncating disk doesn't mark the line between 112 | # projecting onto the sides or the disk, since the sides are not 113 | # parallel to the displacement. We need to bring it a bit closer. How 114 | # much closer can be worked out by similar triangles. It works out 115 | # that the new point is at x/t cos(theta)^2, where theta is the angle 116 | # of the aperture (so sin^2(theta) = (r/||x||)^2). 117 | adjusted_center = x/t * (1 - (r*r)/x_len_sq) 118 | 119 | if dot(v - adjusted_center, adjusted_center) < 0: 120 | # v lies in the front part of the cone 121 | # print("front") 122 | # print("front", adjusted_center, x_len_sq, r, x, t) 123 | w = v - x/t 124 | u = normalized(w) * r/t - w 125 | n = normalized(w) 126 | else: # v lies in the rest of the cone 127 | # print("sides") 128 | # Rotate x in the direction of v, to make it a side of the cone. 129 | # Then project v onto that, and calculate the difference. 130 | leg_len = sqrt(x_len_sq - r*r) 131 | # The sign of the sine determines which side to project on. 132 | sine = copysign(r, det((v, x))) 133 | rot = array( 134 | ((leg_len, sine), 135 | (-sine, leg_len))) 136 | rotated_x = rot.dot(x) / x_len_sq 137 | n = perp(rotated_x) 138 | if sine < 0: 139 | # Need to flip the direction of the line to make the 140 | # half-plane point out of the cone. 141 | n = -n 142 | # print("rotated_x=%s" % rotated_x) 143 | u = rotated_x * dot(v, rotated_x) - v 144 | # print("u=%s" % u) 145 | else: 146 | # We're already intersecting. Pick the closest velocity to our 147 | # velocity that will get us out of the collision within the next 148 | # timestep. 149 | # print("intersecting") 150 | w = v - x/dt 151 | u = normalized(w) * r/dt - w 152 | n = normalized(w) 153 | return u, n 154 | 155 | def norm_sq(x): 156 | return dot(x, x) 157 | 158 | def normalized(x): 159 | l = norm_sq(x) 160 | assert l > 0, (x, l) 161 | return x / sqrt(l) 162 | 163 | def dist_sq(a, b): 164 | return norm_sq(b - a) 165 | -------------------------------------------------------------------------------- /rvo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rvo) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ################################################ 24 | ## Declare ROS messages, services and actions ## 25 | ################################################ 26 | 27 | ## To declare and build messages, services or actions from within this 28 | ## package, follow these steps: 29 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 30 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 31 | ## * In the file package.xml: 32 | ## * add a build_depend tag for "message_generation" 33 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 34 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 35 | ## but can be declared for certainty nonetheless: 36 | ## * add a exec_depend tag for "message_runtime" 37 | ## * In this file (CMakeLists.txt): 38 | ## * add "message_generation" and every package in MSG_DEP_SET to 39 | ## find_package(catkin REQUIRED COMPONENTS ...) 40 | ## * add "message_runtime" and every package in MSG_DEP_SET to 41 | ## catkin_package(CATKIN_DEPENDS ...) 42 | ## * uncomment the add_*_files sections below as needed 43 | ## and list every .msg/.srv/.action file to be processed 44 | ## * uncomment the generate_messages entry below 45 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 46 | 47 | ## Generate messages in the 'msg' folder 48 | # add_message_files( 49 | # FILES 50 | # Message1.msg 51 | # Message2.msg 52 | # ) 53 | 54 | ## Generate services in the 'srv' folder 55 | # add_service_files( 56 | # FILES 57 | # Service1.srv 58 | # Service2.srv 59 | # ) 60 | 61 | ## Generate actions in the 'action' folder 62 | # add_action_files( 63 | # FILES 64 | # Action1.action 65 | # Action2.action 66 | # ) 67 | 68 | ## Generate added messages and services with any dependencies listed here 69 | # generate_messages( 70 | # DEPENDENCIES 71 | # std_msgs # Or other packages containing msgs 72 | # ) 73 | 74 | ################################################ 75 | ## Declare ROS dynamic reconfigure parameters ## 76 | ################################################ 77 | 78 | ## To declare and build dynamic reconfigure parameters within this 79 | ## package, follow these steps: 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 82 | ## * In this file (CMakeLists.txt): 83 | ## * add "dynamic_reconfigure" to 84 | ## find_package(catkin REQUIRED COMPONENTS ...) 85 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 86 | ## and list every .cfg file to be processed 87 | 88 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 89 | # generate_dynamic_reconfigure_options( 90 | # cfg/DynReconf1.cfg 91 | # cfg/DynReconf2.cfg 92 | # ) 93 | 94 | ################################### 95 | ## catkin specific configuration ## 96 | ################################### 97 | ## The catkin_package macro generates cmake config files for your package 98 | ## Declare things to be passed to dependent projects 99 | ## INCLUDE_DIRS: uncomment this if your package contains header files 100 | ## LIBRARIES: libraries you create in this project that dependent projects also need 101 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 102 | ## DEPENDS: system dependencies of this project that dependent projects also need 103 | catkin_package( 104 | # INCLUDE_DIRS include 105 | # LIBRARIES rvo 106 | # CATKIN_DEPENDS rospy 107 | # DEPENDS system_lib 108 | ) 109 | 110 | ########### 111 | ## Build ## 112 | ########### 113 | 114 | ## Specify additional locations of header files 115 | ## Your package locations should be listed before other locations 116 | include_directories( 117 | # include 118 | ${catkin_INCLUDE_DIRS} 119 | ) 120 | 121 | ## Declare a C++ library 122 | # add_library(${PROJECT_NAME} 123 | # src/${PROJECT_NAME}/rvo.cpp 124 | # ) 125 | 126 | ## Add cmake target dependencies of the library 127 | ## as an example, code may need to be generated before libraries 128 | ## either from message generation or dynamic reconfigure 129 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 130 | 131 | ## Declare a C++ executable 132 | ## With catkin_make all packages are built within a single CMake context 133 | ## The recommended prefix ensures that target names across packages don't collide 134 | # add_executable(${PROJECT_NAME}_node src/rvo_node.cpp) 135 | 136 | ## Rename C++ executable without prefix 137 | ## The above recommended prefix causes long target names, the following renames the 138 | ## target back to the shorter version for ease of user use 139 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 140 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 141 | 142 | ## Add cmake target dependencies of the executable 143 | ## same as for the library above 144 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 145 | 146 | ## Specify libraries to link a library or executable target against 147 | # target_link_libraries(${PROJECT_NAME}_node 148 | # ${catkin_LIBRARIES} 149 | # ) 150 | 151 | ############# 152 | ## Install ## 153 | ############# 154 | 155 | # all install targets should use catkin DESTINATION variables 156 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 157 | 158 | ## Mark executable scripts (Python etc.) for installation 159 | ## in contrast to setup.py, you can choose the destination 160 | # install(PROGRAMS 161 | # scripts/my_python_script 162 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 163 | # ) 164 | 165 | ## Mark executables and/or libraries for installation 166 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 167 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 168 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 169 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 170 | # ) 171 | 172 | ## Mark cpp header files for installation 173 | # install(DIRECTORY include/${PROJECT_NAME}/ 174 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 175 | # FILES_MATCHING PATTERN "*.h" 176 | # PATTERN ".svn" EXCLUDE 177 | # ) 178 | 179 | ## Mark other files for installation (e.g. launch and bag files, etc.) 180 | # install(FILES 181 | # # myfile1 182 | # # myfile2 183 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 184 | # ) 185 | 186 | ############# 187 | ## Testing ## 188 | ############# 189 | 190 | ## Add gtest based cpp test target and link libraries 191 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_rvo.cpp) 192 | # if(TARGET ${PROJECT_NAME}-test) 193 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 194 | # endif() 195 | 196 | ## Add folders to be run by python nosetests 197 | # catkin_add_nosetests(test) 198 | -------------------------------------------------------------------------------- /orca/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(orca) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ################################################ 24 | ## Declare ROS messages, services and actions ## 25 | ################################################ 26 | 27 | ## To declare and build messages, services or actions from within this 28 | ## package, follow these steps: 29 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 30 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 31 | ## * In the file package.xml: 32 | ## * add a build_depend tag for "message_generation" 33 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 34 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 35 | ## but can be declared for certainty nonetheless: 36 | ## * add a exec_depend tag for "message_runtime" 37 | ## * In this file (CMakeLists.txt): 38 | ## * add "message_generation" and every package in MSG_DEP_SET to 39 | ## find_package(catkin REQUIRED COMPONENTS ...) 40 | ## * add "message_runtime" and every package in MSG_DEP_SET to 41 | ## catkin_package(CATKIN_DEPENDS ...) 42 | ## * uncomment the add_*_files sections below as needed 43 | ## and list every .msg/.srv/.action file to be processed 44 | ## * uncomment the generate_messages entry below 45 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 46 | 47 | ## Generate messages in the 'msg' folder 48 | # add_message_files( 49 | # FILES 50 | # Message1.msg 51 | # Message2.msg 52 | # ) 53 | 54 | ## Generate services in the 'srv' folder 55 | # add_service_files( 56 | # FILES 57 | # Service1.srv 58 | # Service2.srv 59 | # ) 60 | 61 | ## Generate actions in the 'action' folder 62 | # add_action_files( 63 | # FILES 64 | # Action1.action 65 | # Action2.action 66 | # ) 67 | 68 | ## Generate added messages and services with any dependencies listed here 69 | # generate_messages( 70 | # DEPENDENCIES 71 | # std_msgs # Or other packages containing msgs 72 | # ) 73 | 74 | ################################################ 75 | ## Declare ROS dynamic reconfigure parameters ## 76 | ################################################ 77 | 78 | ## To declare and build dynamic reconfigure parameters within this 79 | ## package, follow these steps: 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 82 | ## * In this file (CMakeLists.txt): 83 | ## * add "dynamic_reconfigure" to 84 | ## find_package(catkin REQUIRED COMPONENTS ...) 85 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 86 | ## and list every .cfg file to be processed 87 | 88 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 89 | # generate_dynamic_reconfigure_options( 90 | # cfg/DynReconf1.cfg 91 | # cfg/DynReconf2.cfg 92 | # ) 93 | 94 | ################################### 95 | ## catkin specific configuration ## 96 | ################################### 97 | ## The catkin_package macro generates cmake config files for your package 98 | ## Declare things to be passed to dependent projects 99 | ## INCLUDE_DIRS: uncomment this if your package contains header files 100 | ## LIBRARIES: libraries you create in this project that dependent projects also need 101 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 102 | ## DEPENDS: system dependencies of this project that dependent projects also need 103 | catkin_package( 104 | # INCLUDE_DIRS include 105 | # LIBRARIES orca 106 | # CATKIN_DEPENDS rospy 107 | # DEPENDS system_lib 108 | ) 109 | 110 | ########### 111 | ## Build ## 112 | ########### 113 | 114 | ## Specify additional locations of header files 115 | ## Your package locations should be listed before other locations 116 | include_directories( 117 | # include 118 | ${catkin_INCLUDE_DIRS} 119 | ) 120 | 121 | ## Declare a C++ library 122 | # add_library(${PROJECT_NAME} 123 | # src/${PROJECT_NAME}/orca.cpp 124 | # ) 125 | 126 | ## Add cmake target dependencies of the library 127 | ## as an example, code may need to be generated before libraries 128 | ## either from message generation or dynamic reconfigure 129 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 130 | 131 | ## Declare a C++ executable 132 | ## With catkin_make all packages are built within a single CMake context 133 | ## The recommended prefix ensures that target names across packages don't collide 134 | # add_executable(${PROJECT_NAME}_node src/orca_node.cpp) 135 | 136 | ## Rename C++ executable without prefix 137 | ## The above recommended prefix causes long target names, the following renames the 138 | ## target back to the shorter version for ease of user use 139 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 140 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 141 | 142 | ## Add cmake target dependencies of the executable 143 | ## same as for the library above 144 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 145 | 146 | ## Specify libraries to link a library or executable target against 147 | # target_link_libraries(${PROJECT_NAME}_node 148 | # ${catkin_LIBRARIES} 149 | # ) 150 | 151 | ############# 152 | ## Install ## 153 | ############# 154 | 155 | # all install targets should use catkin DESTINATION variables 156 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 157 | 158 | ## Mark executable scripts (Python etc.) for installation 159 | ## in contrast to setup.py, you can choose the destination 160 | # install(PROGRAMS 161 | # scripts/my_python_script 162 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 163 | # ) 164 | 165 | ## Mark executables and/or libraries for installation 166 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 167 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 168 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 169 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 170 | # ) 171 | 172 | ## Mark cpp header files for installation 173 | # install(DIRECTORY include/${PROJECT_NAME}/ 174 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 175 | # FILES_MATCHING PATTERN "*.h" 176 | # PATTERN ".svn" EXCLUDE 177 | # ) 178 | 179 | ## Mark other files for installation (e.g. launch and bag files, etc.) 180 | # install(FILES 181 | # # myfile1 182 | # # myfile2 183 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 184 | # ) 185 | 186 | ############# 187 | ## Testing ## 188 | ############# 189 | 190 | ## Add gtest based cpp test target and link libraries 191 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_orca.cpp) 192 | # if(TARGET ${PROJECT_NAME}-test) 193 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 194 | # endif() 195 | 196 | ## Add folders to be run by python nosetests 197 | # catkin_add_nosetests(test) 198 | -------------------------------------------------------------------------------- /mpc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mpc) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES mpc 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/mpc.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/mpc_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # install(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mpc.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /mpc_orca/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mpc_orca) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ################################################ 24 | ## Declare ROS messages, services and actions ## 25 | ################################################ 26 | 27 | ## To declare and build messages, services or actions from within this 28 | ## package, follow these steps: 29 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 30 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 31 | ## * In the file package.xml: 32 | ## * add a build_depend tag for "message_generation" 33 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 34 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 35 | ## but can be declared for certainty nonetheless: 36 | ## * add a exec_depend tag for "message_runtime" 37 | ## * In this file (CMakeLists.txt): 38 | ## * add "message_generation" and every package in MSG_DEP_SET to 39 | ## find_package(catkin REQUIRED COMPONENTS ...) 40 | ## * add "message_runtime" and every package in MSG_DEP_SET to 41 | ## catkin_package(CATKIN_DEPENDS ...) 42 | ## * uncomment the add_*_files sections below as needed 43 | ## and list every .msg/.srv/.action file to be processed 44 | ## * uncomment the generate_messages entry below 45 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 46 | 47 | ## Generate messages in the 'msg' folder 48 | # add_message_files( 49 | # FILES 50 | # Message1.msg 51 | # Message2.msg 52 | # ) 53 | 54 | ## Generate services in the 'srv' folder 55 | # add_service_files( 56 | # FILES 57 | # Service1.srv 58 | # Service2.srv 59 | # ) 60 | 61 | ## Generate actions in the 'action' folder 62 | # add_action_files( 63 | # FILES 64 | # Action1.action 65 | # Action2.action 66 | # ) 67 | 68 | ## Generate added messages and services with any dependencies listed here 69 | # generate_messages( 70 | # DEPENDENCIES 71 | # std_msgs # Or other packages containing msgs 72 | # ) 73 | 74 | ################################################ 75 | ## Declare ROS dynamic reconfigure parameters ## 76 | ################################################ 77 | 78 | ## To declare and build dynamic reconfigure parameters within this 79 | ## package, follow these steps: 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 82 | ## * In this file (CMakeLists.txt): 83 | ## * add "dynamic_reconfigure" to 84 | ## find_package(catkin REQUIRED COMPONENTS ...) 85 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 86 | ## and list every .cfg file to be processed 87 | 88 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 89 | # generate_dynamic_reconfigure_options( 90 | # cfg/DynReconf1.cfg 91 | # cfg/DynReconf2.cfg 92 | # ) 93 | 94 | ################################### 95 | ## catkin specific configuration ## 96 | ################################### 97 | ## The catkin_package macro generates cmake config files for your package 98 | ## Declare things to be passed to dependent projects 99 | ## INCLUDE_DIRS: uncomment this if your package contains header files 100 | ## LIBRARIES: libraries you create in this project that dependent projects also need 101 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 102 | ## DEPENDS: system dependencies of this project that dependent projects also need 103 | catkin_package( 104 | # INCLUDE_DIRS include 105 | # LIBRARIES mpc_orca 106 | # CATKIN_DEPENDS rospy 107 | # DEPENDS system_lib 108 | ) 109 | 110 | ########### 111 | ## Build ## 112 | ########### 113 | 114 | ## Specify additional locations of header files 115 | ## Your package locations should be listed before other locations 116 | include_directories( 117 | # include 118 | ${catkin_INCLUDE_DIRS} 119 | ) 120 | 121 | ## Declare a C++ library 122 | # add_library(${PROJECT_NAME} 123 | # src/${PROJECT_NAME}/mpc_orca.cpp 124 | # ) 125 | 126 | ## Add cmake target dependencies of the library 127 | ## as an example, code may need to be generated before libraries 128 | ## either from message generation or dynamic reconfigure 129 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 130 | 131 | ## Declare a C++ executable 132 | ## With catkin_make all packages are built within a single CMake context 133 | ## The recommended prefix ensures that target names across packages don't collide 134 | # add_executable(${PROJECT_NAME}_node src/mpc_orca_node.cpp) 135 | 136 | ## Rename C++ executable without prefix 137 | ## The above recommended prefix causes long target names, the following renames the 138 | ## target back to the shorter version for ease of user use 139 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 140 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 141 | 142 | ## Add cmake target dependencies of the executable 143 | ## same as for the library above 144 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 145 | 146 | ## Specify libraries to link a library or executable target against 147 | # target_link_libraries(${PROJECT_NAME}_node 148 | # ${catkin_LIBRARIES} 149 | # ) 150 | 151 | ############# 152 | ## Install ## 153 | ############# 154 | 155 | # all install targets should use catkin DESTINATION variables 156 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 157 | 158 | ## Mark executable scripts (Python etc.) for installation 159 | ## in contrast to setup.py, you can choose the destination 160 | # install(PROGRAMS 161 | # scripts/my_python_script 162 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 163 | # ) 164 | 165 | ## Mark executables for installation 166 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 167 | # install(TARGETS ${PROJECT_NAME}_node 168 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 169 | # ) 170 | 171 | ## Mark libraries for installation 172 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 173 | # install(TARGETS ${PROJECT_NAME} 174 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 175 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 176 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 177 | # ) 178 | 179 | ## Mark cpp header files for installation 180 | # install(DIRECTORY include/${PROJECT_NAME}/ 181 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 182 | # FILES_MATCHING PATTERN "*.h" 183 | # PATTERN ".svn" EXCLUDE 184 | # ) 185 | 186 | ## Mark other files for installation (e.g. launch and bag files, etc.) 187 | # install(FILES 188 | # # myfile1 189 | # # myfile2 190 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 191 | # ) 192 | 193 | ############# 194 | ## Testing ## 195 | ############# 196 | 197 | ## Add gtest based cpp test target and link libraries 198 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mpc_orca.cpp) 199 | # if(TARGET ${PROJECT_NAME}-test) 200 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 201 | # endif() 202 | 203 | ## Add folders to be run by python nosetests 204 | # catkin_add_nosetests(test) 205 | -------------------------------------------------------------------------------- /gazebo_worlds/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(gazebo_worlds) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES gazebo_worlds 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/gazebo_worlds.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/gazebo_worlds_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # install(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_gazebo_worlds.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /rvo/src/RVO.py: -------------------------------------------------------------------------------- 1 | from math import ceil, floor, sqrt 2 | import copy 3 | import numpy 4 | 5 | from math import cos, sin, tan, atan2, asin 6 | 7 | from math import pi as PI 8 | 9 | 10 | 11 | def distance(pose1, pose2): 12 | """ compute Euclidean distance for 2D """ 13 | return sqrt((pose1[0]-pose2[0])**2+(pose1[1]-pose2[1])**2)+0.001 14 | 15 | 16 | def RVO_update(X, V_des, V_current, ws_model): 17 | """ compute best velocity given the desired velocity, current velocity and workspace model""" 18 | ROB_RAD = ws_model['robot_radius']+0.1 19 | V_opt = list(V_current) 20 | for i in range(len(X)): 21 | vA = [V_current[i][0], V_current[i][1]] 22 | pA = [X[i][0], X[i][1]] 23 | RVO_BA_all = [] 24 | for j in range(len(X)): 25 | if i!=j: 26 | vB = [V_current[j][0], V_current[j][1]] 27 | pB = [X[j][0], X[j][1]] 28 | # use RVO 29 | transl_vB_vA = [pA[0]+0.5*(vB[0]+vA[0]), pA[1]+0.5*(vB[1]+vA[1])] 30 | # use VO 31 | #transl_vB_vA = [pA[0]+vB[0], pA[1]+vB[1]] 32 | dist_BA = distance(pA, pB) 33 | theta_BA = atan2(pB[1]-pA[1], pB[0]-pA[0]) 34 | if 2*ROB_RAD > dist_BA: 35 | dist_BA = 2*ROB_RAD 36 | theta_BAort = asin(2*ROB_RAD/dist_BA) 37 | theta_ort_left = theta_BA+theta_BAort 38 | bound_left = [cos(theta_ort_left), sin(theta_ort_left)] 39 | theta_ort_right = theta_BA-theta_BAort 40 | bound_right = [cos(theta_ort_right), sin(theta_ort_right)] 41 | # use HRVO 42 | # dist_dif = distance([0.5*(vB[0]-vA[0]),0.5*(vB[1]-vA[1])],[0,0]) 43 | # transl_vB_vA = [pA[0]+vB[0]+cos(theta_ort_left)*dist_dif, pA[1]+vB[1]+sin(theta_ort_left)*dist_dif] 44 | RVO_BA = [transl_vB_vA, bound_left, bound_right, dist_BA, 2*ROB_RAD] 45 | RVO_BA_all.append(RVO_BA) 46 | for hole in ws_model['circular_obstacles']: 47 | # hole = [x, y, rad] 48 | vB = [0, 0] 49 | pB = hole[0:2] 50 | transl_vB_vA = [pA[0]+vB[0], pA[1]+vB[1]] 51 | dist_BA = distance(pA, pB) 52 | theta_BA = atan2(pB[1]-pA[1], pB[0]-pA[0]) 53 | # over-approximation of square to circular 54 | OVER_APPROX_C2S = 1.5 55 | rad = hole[2]*OVER_APPROX_C2S 56 | if (rad+ROB_RAD) > dist_BA: 57 | dist_BA = rad+ROB_RAD 58 | theta_BAort = asin((rad+ROB_RAD)/dist_BA) 59 | theta_ort_left = theta_BA+theta_BAort 60 | bound_left = [cos(theta_ort_left), sin(theta_ort_left)] 61 | theta_ort_right = theta_BA-theta_BAort 62 | bound_right = [cos(theta_ort_right), sin(theta_ort_right)] 63 | RVO_BA = [transl_vB_vA, bound_left, bound_right, dist_BA, rad+ROB_RAD] 64 | RVO_BA_all.append(RVO_BA) 65 | vA_post = intersect(pA, V_des[i], RVO_BA_all) 66 | V_opt[i] = vA_post[:] 67 | return V_opt 68 | 69 | 70 | def intersect(pA, vA, RVO_BA_all): 71 | # print '----------------------------------------' 72 | # print 'Start intersection test' 73 | norm_v = distance(vA, [0, 0]) 74 | suitable_V = [] 75 | unsuitable_V = [] 76 | for theta in numpy.arange(0, 2*PI, 0.1): 77 | for rad in numpy.arange(0.02, norm_v+0.02, norm_v/5.0): 78 | new_v = [rad*cos(theta), rad*sin(theta)] 79 | suit = True 80 | for RVO_BA in RVO_BA_all: 81 | p_0 = RVO_BA[0] 82 | left = RVO_BA[1] 83 | right = RVO_BA[2] 84 | dif = [new_v[0]+pA[0]-p_0[0], new_v[1]+pA[1]-p_0[1]] 85 | theta_dif = atan2(dif[1], dif[0]) 86 | theta_right = atan2(right[1], right[0]) 87 | theta_left = atan2(left[1], left[0]) 88 | if in_between(theta_right, theta_dif, theta_left): 89 | suit = False 90 | break 91 | if suit: 92 | suitable_V.append(new_v) 93 | else: 94 | unsuitable_V.append(new_v) 95 | new_v = vA[:] 96 | suit = True 97 | for RVO_BA in RVO_BA_all: 98 | p_0 = RVO_BA[0] 99 | left = RVO_BA[1] 100 | right = RVO_BA[2] 101 | dif = [new_v[0]+pA[0]-p_0[0], new_v[1]+pA[1]-p_0[1]] 102 | theta_dif = atan2(dif[1], dif[0]) 103 | theta_right = atan2(right[1], right[0]) 104 | theta_left = atan2(left[1], left[0]) 105 | if in_between(theta_right, theta_dif, theta_left): 106 | suit = False 107 | break 108 | if suit: 109 | suitable_V.append(new_v) 110 | else: 111 | unsuitable_V.append(new_v) 112 | #---------------------- 113 | if suitable_V: 114 | # print 'Suitable found' 115 | vA_post = min(suitable_V, key = lambda v: distance(v, vA)) 116 | new_v = vA_post[:] 117 | for RVO_BA in RVO_BA_all: 118 | p_0 = RVO_BA[0] 119 | left = RVO_BA[1] 120 | right = RVO_BA[2] 121 | dif = [new_v[0]+pA[0]-p_0[0], new_v[1]+pA[1]-p_0[1]] 122 | theta_dif = atan2(dif[1], dif[0]) 123 | theta_right = atan2(right[1], right[0]) 124 | theta_left = atan2(left[1], left[0]) 125 | else: 126 | # print 'Suitable not found' 127 | tc_V = dict() 128 | for unsuit_v in unsuitable_V: 129 | tc_V[tuple(unsuit_v)] = 0 130 | tc = [] 131 | for RVO_BA in RVO_BA_all: 132 | p_0 = RVO_BA[0] 133 | left = RVO_BA[1] 134 | right = RVO_BA[2] 135 | dist = RVO_BA[3] 136 | rad = RVO_BA[4] 137 | dif = [unsuit_v[0]+pA[0]-p_0[0], unsuit_v[1]+pA[1]-p_0[1]] 138 | theta_dif = atan2(dif[1], dif[0]) 139 | theta_right = atan2(right[1], right[0]) 140 | theta_left = atan2(left[1], left[0]) 141 | if in_between(theta_right, theta_dif, theta_left): 142 | small_theta = abs(theta_dif-0.5*(theta_left+theta_right)) 143 | if abs(dist*sin(small_theta)) >= rad: 144 | rad = abs(dist*sin(small_theta)) 145 | big_theta = asin(abs(dist*sin(small_theta))/rad) 146 | dist_tg = abs(dist*cos(small_theta))-abs(rad*cos(big_theta)) 147 | if dist_tg < 0: 148 | dist_tg = 0 149 | tc_v = dist_tg/distance(dif, [0,0]) 150 | tc.append(tc_v) 151 | tc_V[tuple(unsuit_v)] = min(tc)+0.001 152 | WT = 0.2 153 | vA_post = min(unsuitable_V, key = lambda v: ((WT/tc_V[tuple(v)])+distance(v, vA))) 154 | return vA_post 155 | 156 | def in_between(theta_right, theta_dif, theta_left): 157 | if abs(theta_right - theta_left) <= PI: 158 | if theta_right <= theta_dif <= theta_left: 159 | return True 160 | else: 161 | return False 162 | else: 163 | if (theta_left <0) and (theta_right >0): 164 | theta_left += 2*PI 165 | if theta_dif < 0: 166 | theta_dif += 2*PI 167 | if theta_right <= theta_dif <= theta_left: 168 | return True 169 | else: 170 | return False 171 | if (theta_left >0) and (theta_right <0): 172 | theta_right += 2*PI 173 | if theta_dif < 0: 174 | theta_dif += 2*PI 175 | if theta_left <= theta_dif <= theta_right: 176 | return True 177 | else: 178 | return False 179 | 180 | def compute_V_des(X, goal, V_max): 181 | V_des = [] 182 | for i in xrange(len(X)): 183 | dif_x = [goal[i][k]-X[i][k] for k in xrange(2)] 184 | norm = distance(dif_x, [0, 0]) 185 | norm_dif_x = [dif_x[k]*V_max[k]/norm for k in xrange(2)] 186 | V_des.append(norm_dif_x[:]) 187 | if reach(X[i], goal[i], 0.1): 188 | V_des[i][0] = 0 189 | V_des[i][1] = 0 190 | return V_des 191 | 192 | def reach(p1, p2, bound=0.5): 193 | if distance(p1,p2)< bound: 194 | return True 195 | else: 196 | return False 197 | 198 | 199 | --------------------------------------------------------------------------------