From 11ec8d097f2188b91991fbb8c3e91350efb1169d Mon Sep 17 00:00:00 2001 From: Neil Patel Date: Tue, 12 Dec 2023 19:39:12 -0500 Subject: [PATCH 1/5] Changed neopixel opcode to an int --- .../src/robot_controller/robot_controller/robot_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROS2/ros_ws/src/robot_controller/robot_controller/robot_controller.py b/ROS2/ros_ws/src/robot_controller/robot_controller/robot_controller.py index cf484bf..6e90318 100644 --- a/ROS2/ros_ws/src/robot_controller/robot_controller/robot_controller.py +++ b/ROS2/ros_ws/src/robot_controller/robot_controller/robot_controller.py @@ -353,7 +353,7 @@ def shutdown_callback(self, msg): raise SystemExit def neopixel_callback(self, msg): - self.send_values(msg.data, 1.0) + self.send_values(msg.data, 1) def pub_battery(self): battery_msg = Float32() From d591c888b1e5535acdabfb0e513c0c238f251a62 Mon Sep 17 00:00:00 2001 From: Neil Patel Date: Sun, 21 Jan 2024 14:59:14 -0500 Subject: [PATCH 2/5] Removed old repos --- .gitmodules | 3 + ROS2/ros_ws/src/camera_server/setup.py | 2 +- .../ros_ws/src/camera_server/swarmexperiments | 2 +- ROS2/ros_ws/src/gravity/CMakeLists.txt | 51 -- ROS2/ros_ws/src/gravity/gravity/gravity.py | 194 ------ .../src/gravity/gravity/utilities/__init__.py | 0 .../gravity/utilities/barrier_certificates.py | 638 ------------------ .../utilities/barrier_certificates2.py | 123 ---- .../gravity/gravity/utilities/controllers.py | 214 ------ .../src/gravity/gravity/utilities/graph.py | 195 ------ .../src/gravity/gravity/utilities/misc.py | 128 ---- .../gravity/utilities/transformations.py | 226 ------- .../src/gravity/launch/gravity.launch.py | 35 - ROS2/ros_ws/src/gravity/package.xml | 26 - ROS2/ros_ws/src/gravity/srv/GoalPoint.srv | 3 - ROS2/ros_ws/src/gravity/srv/MAdvertise.srv | 3 - ROS2/ros_ws/src/robot_msgs | 1 + ROS2/ros_ws/src/robot_msgs/CMakeLists.txt | 52 -- .../ros_ws/src/robot_msgs/msg/Environment.msg | 4 - ROS2/ros_ws/src/robot_msgs/msg/Light.msg | 2 - ROS2/ros_ws/src/robot_msgs/msg/RobotPos.msg | 1 - .../src/robot_msgs/msg/SensorEnable.msg | 5 - ROS2/ros_ws/src/robot_msgs/msg/StringList.msg | 1 - ROS2/ros_ws/src/robot_msgs/package.xml | 27 - ROS2/ros_ws/src/robot_msgs/srv/GetCharger.srv | 4 - .../src/robot_msgs/srv/ReleaseCharger.srv | 3 - ROS2/ros_ws/src/ros2_usb_camera | 1 - ROS2/ros_ws/src/vision_opencv | 1 - 28 files changed, 6 insertions(+), 1939 deletions(-) delete mode 100644 ROS2/ros_ws/src/gravity/CMakeLists.txt delete mode 100755 ROS2/ros_ws/src/gravity/gravity/gravity.py delete mode 100644 ROS2/ros_ws/src/gravity/gravity/utilities/__init__.py delete mode 100644 ROS2/ros_ws/src/gravity/gravity/utilities/barrier_certificates.py delete mode 100644 ROS2/ros_ws/src/gravity/gravity/utilities/barrier_certificates2.py delete mode 100644 ROS2/ros_ws/src/gravity/gravity/utilities/controllers.py delete mode 100644 ROS2/ros_ws/src/gravity/gravity/utilities/graph.py delete mode 100644 ROS2/ros_ws/src/gravity/gravity/utilities/misc.py delete mode 100644 ROS2/ros_ws/src/gravity/gravity/utilities/transformations.py delete mode 100644 ROS2/ros_ws/src/gravity/launch/gravity.launch.py delete mode 100644 ROS2/ros_ws/src/gravity/package.xml delete mode 100644 ROS2/ros_ws/src/gravity/srv/GoalPoint.srv delete mode 100644 ROS2/ros_ws/src/gravity/srv/MAdvertise.srv create mode 160000 ROS2/ros_ws/src/robot_msgs delete mode 100644 ROS2/ros_ws/src/robot_msgs/CMakeLists.txt delete mode 100755 ROS2/ros_ws/src/robot_msgs/msg/Environment.msg delete mode 100755 ROS2/ros_ws/src/robot_msgs/msg/Light.msg delete mode 100755 ROS2/ros_ws/src/robot_msgs/msg/RobotPos.msg delete mode 100644 ROS2/ros_ws/src/robot_msgs/msg/SensorEnable.msg delete mode 100644 ROS2/ros_ws/src/robot_msgs/msg/StringList.msg delete mode 100644 ROS2/ros_ws/src/robot_msgs/package.xml delete mode 100644 ROS2/ros_ws/src/robot_msgs/srv/GetCharger.srv delete mode 100644 ROS2/ros_ws/src/robot_msgs/srv/ReleaseCharger.srv delete mode 160000 ROS2/ros_ws/src/ros2_usb_camera delete mode 160000 ROS2/ros_ws/src/vision_opencv diff --git a/.gitmodules b/.gitmodules index b637efe..acd23eb 100644 --- a/.gitmodules +++ b/.gitmodules @@ -13,3 +13,6 @@ [submodule "ROS2/ros_ws/src/camera_server/swarmexperiments"] path = ROS2/ros_ws/src/camera_server/swarmexperiments url = git@github.com:herolab-uga/swarmexperiments.git +[submodule "ROS2/ros_ws/src/robot_msgs"] + path = ROS2/ros_ws/src/robot_msgs + url = git@github.com:herolab-uga/robot_msgs.git diff --git a/ROS2/ros_ws/src/camera_server/setup.py b/ROS2/ros_ws/src/camera_server/setup.py index 215f231..913df71 100755 --- a/ROS2/ros_ws/src/camera_server/setup.py +++ b/ROS2/ros_ws/src/camera_server/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, version='0.0.0', - packages=find_packages(exclude=['test']), + packages=[package_name], data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), diff --git a/ROS2/ros_ws/src/camera_server/swarmexperiments b/ROS2/ros_ws/src/camera_server/swarmexperiments index 6363a8b..af98851 160000 --- a/ROS2/ros_ws/src/camera_server/swarmexperiments +++ b/ROS2/ros_ws/src/camera_server/swarmexperiments @@ -1 +1 @@ -Subproject commit 6363a8be787f26f402af373c415513551b09c2a8 +Subproject commit af98851f5be78e66890339d06dfc39103c54cfa6 diff --git a/ROS2/ros_ws/src/gravity/CMakeLists.txt b/ROS2/ros_ws/src/gravity/CMakeLists.txt deleted file mode 100644 index 6e55253..0000000 --- a/ROS2/ros_ws/src/gravity/CMakeLists.txt +++ /dev/null @@ -1,51 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(gravity) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(std_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(rclpy REQUIRED) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -rosidl_generate_interfaces(${PROJECT_NAME} - -"srv/GoalPoint.srv" -"srv/MAdvertise.srv" -DEPENDENCIES std_msgs nav_msgs - -) - -install( - PROGRAMS - gravity/gravity.py - launch/gravity.launch.py - DESTINATION lib/${PROJECT_NAME} -) - -ament_package() diff --git a/ROS2/ros_ws/src/gravity/gravity/gravity.py b/ROS2/ros_ws/src/gravity/gravity/gravity.py deleted file mode 100755 index 6659e83..0000000 --- a/ROS2/ros_ws/src/gravity/gravity/gravity.py +++ /dev/null @@ -1,194 +0,0 @@ -#! /usr/bin/python3 - -import time -import rclpy -import random -import threading -import numpy as np -from rclpy.node import Node -from utilities.graph import * -from std_msgs.msg import Float64,UInt8MultiArray -from utilities.transformations import * -from geometry_msgs.msg import Twist,Point -from gravity.srv import Graph,GoalPoint,VariableAdvertise -from robot_msgs.msg import RobotPos,StringList - -class Gravity(Node): - - def anchor(self,goal_pos): - self.target_postion_lock.release() - #we want to direct the robot to the desired anchor position - dxi = self.neighbors[self.robot_name].position.x - goal_pos.x - dyi = self.neighbors[self.robot_name].position.y - goal_pos.y - vels = self.si_to_uni_dyn(np.array([[dxi],[dyi]]),np.array([[self.neighbors[self.robot_name].position.x],[self.neighbors[self.robot_name].position.y],[self.neighbors[self.robot_name].position.theta]])) - self.vel_pub.publish(Twist(vels[0], 0, vels[1])) - - def solver(self): - dxi = [0,0] - for index,robot in enumerate(self.neighbors): - distance = np.asarray([self.neighbors[robot]["position"].position.x - self.neighbors[self.robot_name]["position"].position.x, - self.neighbors[robot]["position"].position.y - self.neighbors[self.robot_name]["position"].position.y]) - expected_r = (Gravity.g * self.neighbors[robot]["m"] * self.m) / self.neighbors[robot]["force"] - dxi += 1 * (np.power(np.linalg.norm(distance),2) - expected_r) * distance - dxi = self.si_to_uni_dyn(dxi) - - - # set dxi to go to goal pos if needed - msg = Twist() - msg.linear.x = dxi[0] - msg.linear.y = 0 - msg.angular.z = dxi[1] - self.vel_publisher(msg) - - # rate group instead of thread? - def run(self): - # self.target_position_lock.acquire() - # self.target_postion_lock.release() - # get neighbor m values - # get neighbor f values - # run g consensus - self.solver() - - def position_callback(self, msg): - self.neighbors = {robot.name: {"position":robot} for robot in msg if robot.name in self.neighbor_names or robot.name == self.robot_name} - - def name_callback(self, msg): - # if you want to catch a mismatch in the number of available robots and the number of robots in the graph laplacian matrix - # do it in this method - self.robot_index = msg.data.index(self.robot_name) - self.active_robots = msg.data - - def goal_pos_callback(self, request, response): - with self.target_position_lock: - self.target_position = request - response.confirm = True - return response - - def m_advertise_callback(self, request, response): - response.value = self.m - return response - - def g_advertise_callback(self,request, response): - response.value = Gravity.g - return response - - def f_advertise_callback(self,request, response): - response.value = self.neighbors[request.neighbor_name]["f"] - return response - - def parse_laplacian(self): - neighbor_index = topological_neighbors(Gravity.L,self.robot_index) - - # This is a dict comprehension - self.neighbor = { - self.active_robots[neighbor]:{ - "position":None, - "force":None, - "force_client":None, - "m":None, - "m_client":None, - "g_client":None - } - - for neighbor in neighbor_index - } - - def graph_laplacian_callback(self,msg): - Gravity.L = np.asarray(msg.data) - self.parse_laplacian() - - def calculate_F(self): - for index,robot in enumerate(self.neighbors): - distance = np.asarray([self.neighbors[robot]["position"].position.x - self.neighbors[self.robot_name]["position"].position.x, - self.neighbors[robot]["position"].position.y - self.neighbors[self.robot_name]["position"].position.y]) - self.neighbors[robot]["force"] = (Gravity.g * self.m * self.neighbors[robot]["m"])/np.power(distance,2) - - def __init__(self) -> None: - super().__init__("gravity") - - # Declare launch parameters - self.declare_parameter("F") - self.declare_parameter("g") - - self.robot_name = self.get_namespace() # this is the name of the robot - self.robot_index = None - self.m = random.randint(1, 100) # this is an intrinsic property of the robot NOTE: this needs to be set in the launch file - - self.active_robots = [] - self.neighbors = {} # Dict of neighborn - - # NOTE: i think i want to make this a service - Gravity.g = self.get_parameter("g").value # this is a global property of the environment - Gravity.L = None - self.si_to_uni_dyn,_ = create_si_to_uni_dynamics_mapping() #- replace this with a function that only takes one pos and one dxi - - # Create a subscriber to the global position topic - self.pos_subscription = self.create_subscription(RobotPos,"/position",self.position_callback,10) - # Create subscriber to the active_robots topic - self.active_subscription = self.create_subscription(StringList,"/active_robots",self.name_callback,10) - # Create a publisher to the velocity topic - self.vel_publisher = self.create_publisher(Twist,"/{robot_name}/cmd_vel".format(robot_name=self.robot_name),10) - - self.target_position = None - self.target_postion_lock = threading.Lock() - self.goal_pos_service = self.create_service(GoalPoint,"/{robot_name}/goal_pos".format(robot_name=self.robot_name),self.goal_pos_callback) - - self.graph_laplacian_sub = self.create_subscription(UInt8MultiArray,"/graph_laplacian",self.graph_laplacian_callback) - # self.graph_service = self.create_service(Graph, "/{robot_name}/graph_laplacian".format(robot_name=self.robot_name),self.graph_laplacian_callback()) - self.m_advertise_service = self.create_service(VariableAdvertise,"/{robot_name}/m".format(robot_name=self.robot_name),self.m_advertise_callback) - self.g_advertise_service = self.create_service(VariableAdvertise,"/{robot_name}/g".format(robot_name=self.robot_name),self.g_advertise_callback) - self.f_advertise_service = self.create_service(VariableAdvertise,"/{robot_name}/f".format(robot_name=self.robot_name),self.g_advertise_callback) - - #--------------------------------- Experiment Setup ---------------------------------# - # Additional set up - # Fill assign neighbor names with their m values in neighbor_names dict you can get using their service - while Gravity.L is None: - time.sleep(.1) - continue - - # This will set up all the clients that the robot needs to communicate with its neighbors and get the initial values - for neighbor in self.neighbors: - - # Sets up the client - self.neighbors[neighbor]["f_client"] = self.create_client(VariableAdvertise,"/{robot_name}/m".format(robot_name=neighbor)) - - # Waits for the client to become available - while not self.neighbors[neighbor]["f_client"].wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') - - self.neighbors[neighbor]["m_client"] = self.create_client(VariableAdvertise,"/{robot_name}/g".format(robot_name=neighbor)) - - while not self.neighbors[neighbor]["m_client"].wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') - - m_msg = VariableAdvertise.Request() - m_msg.robot_name = self.robot_name - m_msg.neighbor_name = neighbor - - m_future = self.neighbors[neighbor]["m_client"].call_async(m_msg) - rclpy.spin_until_future_complete(self,m_future) - - self.neighbors[neighbor]["g_client"] = self.create_client(VariableAdvertise,"/{robot_name}/f".format(robot_name=neighbor)) - - while not self.neighbors[neighbor]["g_client"].wait_for_service(timeout_sec=1.0): - self.get_logger().info('service not available, waiting again...') - - # First we calculate F based on the initial positions of the robots - self.calculate_F() - - self.run_rate = self.create_rate(120) - - while True: - self.run() - self.run_rate.sleep() - - # use a rate for the run method - -def main(): - rclpy.init() - # Spin in a separate thread - gravity = Gravity() - thread = threading.Thread(target=rclpy.spin, args=(gravity, ), daemon=True) - thread.start() - gravity.destroy_node() - diff --git a/ROS2/ros_ws/src/gravity/gravity/utilities/__init__.py b/ROS2/ros_ws/src/gravity/gravity/utilities/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/ROS2/ros_ws/src/gravity/gravity/utilities/barrier_certificates.py b/ROS2/ros_ws/src/gravity/gravity/utilities/barrier_certificates.py deleted file mode 100644 index 2d44eb7..0000000 --- a/ROS2/ros_ws/src/gravity/gravity/utilities/barrier_certificates.py +++ /dev/null @@ -1,638 +0,0 @@ -from cvxopt import matrix -from cvxopt.blas import dot -from cvxopt.solvers import qp, options -from cvxopt import matrix, sparse - -# Unused for now, will include later for speed. -# import quadprog as solver2 - -import itertools -import numpy as np -from scipy.special import comb - -from utilities.transformations import * - -# Disable output of CVXOPT -options['show_progress'] = False -# Change default options of CVXOPT for faster solving -options['reltol'] = 1e-2 # was e-2 -options['feastol'] = 1e-2 # was e-4 -options['maxiters'] = 50 # default is 100 - -def create_single_integrator_barrier_certificate(barrier_gain=75, safety_radius=0.2, magnitude_limit=0.1): - """Creates a barrier certificate for a single-integrator system. This function - returns another function for optimization reasons. - - barrier_gain: double (controls how quickly agents can approach each other. lower = slower) - safety_radius: double (how far apart the agents will stay) - magnitude_limit: how fast the robot can move linearly. - - -> function (the barrier certificate function) - """ - - #Check user input types - assert isinstance(barrier_gain, (int, float)), "In the function create_single_integrator_barrier_certificate, the barrier gain (barrier_gain) must be an integer or float. Recieved type %r." % type(barrier_gain).__name__ - assert isinstance(safety_radius, (int, float)), "In the function create_single_integrator_barrier_certificate, the safe distance between robots (safety_radius) must be an integer or float. Recieved type %r." % type(safety_radius).__name__ - assert isinstance(magnitude_limit, (int, float)), "In the function create_single_integrator_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be an integer or float. Recieved type %r." % type(magnitude_limit).__name__ - - #Check user input ranges/sizes - assert barrier_gain > 0, "In the function create_single_integrator_barrier_certificate, the barrier gain (barrier_gain) must be positive. Recieved %r." % barrier_gain - assert safety_radius >= 0.1, "In the function create_single_integrator_barrier_certificate, the safe distance between robots (safety_radius) must be greater than or equal to the diameter of the robot (0.12m) plus the distance to the look ahead point used in the diffeomorphism if that is being used. Recieved %r." % safety_radius - assert magnitude_limit > 0, "In the function create_single_integrator_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be positive. Recieved %r." % magnitude_limit - assert magnitude_limit <= 0.2, "In the function create_single_integrator_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be less than the max speed of the robot (0.2m/s). Recieved %r." % magnitude_limit - - - def f(dxi, x): - #Check user input types - assert isinstance(dxi, np.ndarray), "In the function created by the create_single_integrator_barrier_certificate function, the single-integrator robot velocity command (dxi) must be a numpy array. Recieved type %r." % type(dxi).__name__ - assert isinstance(x, np.ndarray), "In the function created by the create_single_integrator_barrier_certificate function, the robot states (x) must be a numpy array. Recieved type %r." % type(x).__name__ - - #Check user input ranges/sizes - assert x.shape[0] == 2, "In the function created by the create_single_integrator_barrier_certificate function, the dimension of the single integrator robot states (x) must be 2 ([x;y]). Recieved dimension %r." % x.shape[0] - assert dxi.shape[0] == 2, "In the function created by the create_single_integrator_barrier_certificate function, the dimension of the robot single integrator velocity command (dxi) must be 2 ([x_dot;y_dot]). Recieved dimension %r." % dxi.shape[0] - assert x.shape[1] == dxi.shape[1], "In the function created by the create_single_integrator_barrier_certificate function, the number of robot states (x) must be equal to the number of robot single integrator velocity commands (dxi). Recieved a current robot pose input array (x) of size %r x %r and single integrator velocity array (dxi) of size %r x %r." % (x.shape[0], x.shape[1], dxi.shape[0], dxi.shape[1]) - - - # Initialize some variables for computational savings - N = dxi.shape[1] - num_constraints = int(comb(N, 2)) - A = np.zeros((num_constraints, 2*N)) - b = np.zeros(num_constraints) - H = sparse(matrix(2*np.identity(2*N))) - - count = 0 - for i in range(N-1): - for j in range(i+1, N): - error = x[:, i] - x[:, j] - h = (error[0]*error[0] + error[1]*error[1]) - np.power(safety_radius, 2) - - A[count, (2*i, (2*i+1))] = -2*error - A[count, (2*j, (2*j+1))] = 2*error - b[count] = barrier_gain*np.power(h, 3) - - count += 1 - - # Threshold control inputs before QP - norms = np.linalg.norm(dxi, 2, 0) - idxs_to_normalize = (norms > magnitude_limit) - dxi[:, idxs_to_normalize] *= magnitude_limit/norms[idxs_to_normalize] - - f = -2*np.reshape(dxi, 2*N, order='F') - result = qp(H, matrix(f), matrix(A), matrix(b))['x'] - - return np.reshape(result, (2, -1), order='F') - - return f - -def create_single_integrator_barrier_certificate_with_boundary(barrier_gain=100, safety_radius=0.17, magnitude_limit=0.2, boundary_points = np.array([-1.6, 1.6, -1.0, 1.0])): - """Creates a barrier certificate for a single-integrator system with a rectangular boundary included. This function - returns another function for optimization reasons. - - barrier_gain: double (controls how quickly agents can approach each other. lower = slower) - safety_radius: double (how far apart the agents will stay) - magnitude_limit: how fast the robot can move linearly. - - -> function (the barrier certificate function) - """ - - #Check user input types - assert isinstance(barrier_gain, (int, float)), "In the function create_single_integrator_barrier_certificate, the barrier gain (barrier_gain) must be an integer or float. Recieved type %r." % type(barrier_gain).__name__ - assert isinstance(safety_radius, (int, float)), "In the function create_single_integrator_barrier_certificate, the safe distance between robots (safety_radius) must be an integer or float. Recieved type %r." % type(safety_radius).__name__ - assert isinstance(magnitude_limit, (int, float)), "In the function create_single_integrator_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be an integer or float. Recieved type %r." % type(magnitude_limit).__name__ - - #Check user input ranges/sizes - assert barrier_gain > 0, "In the function create_single_integrator_barrier_certificate, the barrier gain (barrier_gain) must be positive. Recieved %r." % barrier_gain - assert safety_radius >= 0.12, "In the function create_single_integrator_barrier_certificate, the safe distance between robots (safety_radius) must be greater than or equal to the diameter of the robot (0.12m) plus the distance to the look ahead point used in the diffeomorphism if that is being used. Recieved %r." % safety_radius - assert magnitude_limit > 0, "In the function create_single_integrator_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be positive. Recieved %r." % magnitude_limit - assert magnitude_limit <= 0.2, "In the function create_single_integrator_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be less than the max speed of the robot (0.2m/s). Recieved %r." % magnitude_limit - - - def f(dxi, x): - #Check user input types - assert isinstance(dxi, np.ndarray), "In the function created by the create_single_integrator_barrier_certificate function, the single-integrator robot velocity command (dxi) must be a numpy array. Recieved type %r." % type(dxi).__name__ - assert isinstance(x, np.ndarray), "In the function created by the create_single_integrator_barrier_certificate function, the robot states (x) must be a numpy array. Recieved type %r." % type(x).__name__ - - #Check user input ranges/sizes - assert x.shape[0] == 2, "In the function created by the create_single_integrator_barrier_certificate function, the dimension of the single integrator robot states (x) must be 2 ([x;y]). Recieved dimension %r." % x.shape[0] - assert dxi.shape[0] == 2, "In the function created by the create_single_integrator_barrier_certificate function, the dimension of the robot single integrator velocity command (dxi) must be 2 ([x_dot;y_dot]). Recieved dimension %r." % dxi.shape[0] - assert x.shape[1] == dxi.shape[1], "In the function created by the create_single_integrator_barrier_certificate function, the number of robot states (x) must be equal to the number of robot single integrator velocity commands (dxi). Recieved a current robot pose input array (x) of size %r x %r and single integrator velocity array (dxi) of size %r x %r." % (x.shape[0], x.shape[1], dxi.shape[0], dxi.shape[1]) - - - # Initialize some variables for computational savings - N = dxi.shape[1] - num_constraints = int(comb(N, 2)) + 4*N - A = np.zeros((num_constraints, 2*N)) - b = np.zeros(num_constraints) - #H = sparse(matrix(2*np.identity(2*N))) - H = 2*np.identity(2*N) - - count = 0 - for i in range(N-1): - for j in range(i+1, N): - error = x[:, i] - x[:, j] - h = (error[0]*error[0] + error[1]*error[1]) - np.power(safety_radius, 2) - - A[count, (2*i, (2*i+1))] = -2*error - A[count, (2*j, (2*j+1))] = 2*error - b[count] = barrier_gain*np.power(h, 3) - - count += 1 - - for k in range(N): - #Pos Y - A[count, (2*k, 2*k+1)] = np.array([0,1]) - b[count] = 0.4*barrier_gain*(boundary_points[3] - safety_radius/2 - x[1,k])**3; - count += 1 - - #Neg Y - A[count, (2*k, 2*k+1)] = -np.array([0,1]) - b[count] = 0.4*barrier_gain*(-boundary_points[2] - safety_radius/2 + x[1,k])**3; - count += 1 - - #Pos X - A[count, (2*k, 2*k+1)] = np.array([1,0]) - b[count] = 0.4*barrier_gain*(boundary_points[1] - safety_radius/2 - x[0,k])**3; - count += 1 - - #Neg X - A[count, (2*k, 2*k+1)] = -np.array([1,0]) - b[count] = 0.4*barrier_gain*(-boundary_points[0] - safety_radius/2 + x[0,k])**3; - count += 1 - - # Threshold control inputs before QP - norms = np.linalg.norm(dxi, 2, 0) - idxs_to_normalize = (norms > magnitude_limit) - dxi[:, idxs_to_normalize] *= magnitude_limit/norms[idxs_to_normalize] - - f = -2*np.reshape(dxi, (2*N,1), order='F') - b = np.reshape(b, (count,1), order='F') - result = qp(matrix(H), matrix(f), matrix(A), matrix(b))['x'] - #result = solver2.solve_qp(H, f, A, b, 0)[0] - - return np.reshape(result, (2, N), order='F') - - return f - -def create_single_integrator_barrier_certificate2(barrier_gain=100, unsafe_barrier_gain=1e6, safety_radius=0.17, magnitude_limit=0.2): - """Creates a barrier certificate for a single-integrator system. This function - returns another function for optimization reasons. This function is different from - create_single_integrator_barrier_certificate as it changes the barrier gain to a large - number if the single integrator point enters the unsafe region. - - barrier_gain: double (controls how quickly agents can approach each other. lower = slower) - safety_radius: double (how far apart the agents will stay) - magnitude_limit: how fast the robot can move linearly. - - -> function (the barrier certificate function) - """ - - #Check user input types - assert isinstance(barrier_gain, (int, float)), "In the function create_single_integrator_barrier_certificate2, the barrier gain inside the safe set (barrier_gain) must be an integer or float. Recieved type %r." % type(barrier_gain).__name__ - assert isinstance(unsafe_barrier_gain, (int, float)), "In the function create_single_integrator_barrier_certificate2, the barrier gain if outside the safe set (unsafe_barrier_gain) must be an integer or float. Recieved type %r." % type(unsafe_barrier_gain).__name__ - assert isinstance(safety_radius, (int, float)), "In the function create_single_integrator_barrier_certificate2, the safe distance between robots (safety_radius) must be an integer or float. Recieved type %r." % type(safety_radius).__name__ - assert isinstance(magnitude_limit, (int, float)), "In the function create_single_integrator_barrier_certificate2, the maximum linear velocity of the robot (magnitude_limit) must be an integer or float. Recieved type %r." % type(magnitude_limit).__name__ - - #Check user input ranges/sizes - assert barrier_gain > 0, "In the function create_single_integrator_barrier_certificate2, the barrier gain inside the safe set (barrier_gain) must be positive. Recieved %r." % barrier_gain - assert unsafe_barrier_gain > 0, "In the function create_single_integrator_barrier_certificate2, the barrier gain if outside the safe set (unsafe_barrier_gain) must be positive. Recieved %r." % unsafe_barrier_gain - assert safety_radius >= 0.12, "In the function create_single_integrator_barrier_certificate2, the safe distance between robots (safety_radius) must be greater than or equal to the diameter of the robot (0.12m) plus the distance to the look ahead point used in the diffeomorphism if that is being used. Recieved %r." % safety_radius - assert magnitude_limit > 0, "In the function create_single_integrator_barrier_certificate2, the maximum linear velocity of the robot (magnitude_limit) must be positive. Recieved %r." % magnitude_limit - assert magnitude_limit <= 0.2, "In the function create_single_integrator_barrier_certificate2, the maximum linear velocity of the robot (magnitude_limit) must be less than the max speed of the robot (0.2m/s). Recieved %r." % magnitude_limit - - - def f(dxi, x): - #Check user input types - assert isinstance(dxi, np.ndarray), "In the function created by the create_single_integrator_barrier_certificate2 function, the single-integrator robot velocity command (dxi) must be a numpy array. Recieved type %r." % type(dxi).__name__ - assert isinstance(x, np.ndarray), "In the function created by the create_single_integrator_barrier_certificate2 function, the robot states (x) must be a numpy array. Recieved type %r." % type(x).__name__ - - #Check user input ranges/sizes - assert x.shape[0] == 2, "In the function created by the create_single_integrator_barrier_certificate2 function, the dimension of the single integrator robot states (x) must be 2 ([x;y]). Recieved dimension %r." % x.shape[0] - assert dxi.shape[0] == 2, "In the function created by the create_single_integrator_barrier_certificate2 function, the dimension of the robot single integrator velocity command (dxi) must be 2 ([x_dot;y_dot]). Recieved dimension %r." % dxi.shape[0] - assert x.shape[1] == dxi.shape[1], "In the function created by the create_single_integrator_barrier_certificate2 function, the number of robot states (x) must be equal to the number of robot single integrator velocity commands (dxi). Recieved a current robot pose input array (x) of size %r x %r and single integrator velocity array (dxi) of size %r x %r." % (x.shape[0], x.shape[1], dxi.shape[0], dxi.shape[1]) - - - # Initialize some variables for computational savings - N = dxi.shape[1] - num_constraints = int(comb(N, 2)) - A = np.zeros((num_constraints, 2*N)) - b = np.zeros(num_constraints) - H = sparse(matrix(2*np.identity(2*N))) - - count = 0 - for i in range(N-1): - for j in range(i+1, N): - error = x[:, i] - x[:, j] - h = (error[0]*error[0] + error[1]*error[1]) - np.power(safety_radius, 2) - - A[count, (2*i, (2*i+1))] = -2*error - A[count, (2*j, (2*j+1))] = 2*error - if h >= 0: - b[count] = barrier_gain*np.power(h, 3) - else: - b[count] = unsafe_barrier_gain*np.power(h, 3) - - count += 1 - - # Threshold control inputs before QP - norms = np.linalg.norm(dxi, 2, 0) - idxs_to_normalize = (norms > magnitude_limit) - dxi[:, idxs_to_normalize] *= magnitude_limit/norms[idxs_to_normalize] - - f = -2*np.reshape(dxi, 2*N, order='F') - result = qp(H, matrix(f), matrix(A), matrix(b))['x'] - - return np.reshape(result, (2, -1), order='F') - - return f - -def create_unicycle_barrier_certificate(barrier_gain=100, safety_radius=0.12, projection_distance=0.05, magnitude_limit=0.2): - """ Creates a unicycle barrier cetifcate to avoid collisions. Uses the diffeomorphism mapping - and single integrator implementation. For optimization purposes, this function returns - another function. - - barrier_gain: double (how fast the robots can approach each other) - safety_radius: double (how far apart the robots should stay) - projection_distance: double (how far ahead to place the bubble) - - -> function (the unicycle barrier certificate function) - """ - - #Check user input types - assert isinstance(barrier_gain, (int, float)), "In the function create_unicycle_barrier_certificate, the barrier gain (barrier_gain) must be an integer or float. Recieved type %r." % type(barrier_gain).__name__ - assert isinstance(safety_radius, (int, float)), "In the function create_unicycle_barrier_certificate, the safe distance between robots (safety_radius) must be an integer or float. Recieved type %r." % type(safety_radius).__name__ - assert isinstance(projection_distance, (int, float)), "In the function create_unicycle_barrier_certificate, the projected point distance for the diffeomorphism between sinlge integrator and unicycle (projection_distance) must be an integer or float. Recieved type %r." % type(projection_distance).__name__ - assert isinstance(magnitude_limit, (int, float)), "In the function create_unicycle_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be an integer or float. Recieved type %r." % type(magnitude_limit).__name__ - - #Check user input ranges/sizes - assert barrier_gain > 0, "In the function create_unicycle_barrier_certificate, the barrier gain (barrier_gain) must be positive. Recieved %r." % barrier_gain - assert safety_radius >= 0.12, "In the function create_unicycle_barrier_certificate, the safe distance between robots (safety_radius) must be greater than or equal to the diameter of the robot (0.12m). Recieved %r." % safety_radius - assert projection_distance > 0, "In the function create_unicycle_barrier_certificate, the projected point distance for the diffeomorphism between sinlge integrator and unicycle (projection_distance) must be positive. Recieved %r." % projection_distance - assert magnitude_limit > 0, "In the function create_unicycle_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be positive. Recieved %r." % magnitude_limit - assert magnitude_limit <= 0.2, "In the function create_unicycle_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be less than the max speed of the robot (0.2m/s). Recieved %r." % magnitude_limit - - - si_barrier_cert = create_single_integrator_barrier_certificate(barrier_gain=barrier_gain, safety_radius=safety_radius+projection_distance) - - si_to_uni_dyn, uni_to_si_states = create_si_to_uni_mapping(projection_distance=projection_distance) - - uni_to_si_dyn = create_uni_to_si_dynamics(projection_distance=projection_distance) - - def f(dxu, x): - #Check user input types - assert isinstance(dxu, np.ndarray), "In the function created by the create_unicycle_barrier_certificate function, the unicycle robot velocity command (dxu) must be a numpy array. Recieved type %r." % type(dxu).__name__ - assert isinstance(x, np.ndarray), "In the function created by the create_unicycle_barrier_certificate function, the robot states (x) must be a numpy array. Recieved type %r." % type(x).__name__ - - #Check user input ranges/sizes - assert x.shape[0] == 3, "In the function created by the create_unicycle_barrier_certificate function, the dimension of the unicycle robot states (x) must be 3 ([x;y;theta]). Recieved dimension %r." % x.shape[0] - assert dxu.shape[0] == 2, "In the function created by the create_unicycle_barrier_certificate function, the dimension of the robot unicycle velocity command (dxu) must be 2 ([v;w]). Recieved dimension %r." % dxu.shape[0] - assert x.shape[1] == dxu.shape[1], "In the function created by the create_unicycle_barrier_certificate function, the number of robot states (x) must be equal to the number of robot unicycle velocity commands (dxu). Recieved a current robot pose input array (x) of size %r x %r and single integrator velocity array (dxi) of size %r x %r." % (x.shape[0], x.shape[1], dxu.shape[0], dxu.shape[1]) - - - x_si = uni_to_si_states(x) - #Convert unicycle control command to single integrator one - dxi = uni_to_si_dyn(dxu, x) - #Apply single integrator barrier certificate - dxi = si_barrier_cert(dxi, x_si) - #Return safe unicycle command - return si_to_uni_dyn(dxi, x) - - return f - -def create_unicycle_barrier_certificate_with_boundary(barrier_gain=100, safety_radius=0.12, projection_distance=0.05, magnitude_limit=0.2, boundary_points = np.array([-1.6, 1.6, -1.0, 1.0])): - """ Creates a unicycle barrier cetifcate to avoid collisions. Uses the diffeomorphism mapping - and single integrator implementation. For optimization purposes, this function returns - another function. - - barrier_gain: double (how fast the robots can approach each other) - safety_radius: double (how far apart the robots should stay) - projection_distance: double (how far ahead to place the bubble) - - -> function (the unicycle barrier certificate function) - """ - - #Check user input types - assert isinstance(barrier_gain, (int, float)), "In the function create_unicycle_barrier_certificate, the barrier gain (barrier_gain) must be an integer or float. Recieved type %r." % type(barrier_gain).__name__ - assert isinstance(safety_radius, (int, float)), "In the function create_unicycle_barrier_certificate, the safe distance between robots (safety_radius) must be an integer or float. Recieved type %r." % type(safety_radius).__name__ - assert isinstance(projection_distance, (int, float)), "In the function create_unicycle_barrier_certificate, the projected point distance for the diffeomorphism between sinlge integrator and unicycle (projection_distance) must be an integer or float. Recieved type %r." % type(projection_distance).__name__ - assert isinstance(magnitude_limit, (int, float)), "In the function create_unicycle_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be an integer or float. Recieved type %r." % type(magnitude_limit).__name__ - - #Check user input ranges/sizes - assert barrier_gain > 0, "In the function create_unicycle_barrier_certificate, the barrier gain (barrier_gain) must be positive. Recieved %r." % barrier_gain - assert safety_radius >= 0.12, "In the function create_unicycle_barrier_certificate, the safe distance between robots (safety_radius) must be greater than or equal to the diameter of the robot (0.12m). Recieved %r." % safety_radius - assert projection_distance > 0, "In the function create_unicycle_barrier_certificate, the projected point distance for the diffeomorphism between sinlge integrator and unicycle (projection_distance) must be positive. Recieved %r." % projection_distance - assert magnitude_limit > 0, "In the function create_unicycle_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be positive. Recieved %r." % magnitude_limit - assert magnitude_limit <= 0.2, "In the function create_unicycle_barrier_certificate, the maximum linear velocity of the robot (magnitude_limit) must be less than the max speed of the robot (0.2m/s). Recieved %r." % magnitude_limit - - - si_barrier_cert = create_single_integrator_barrier_certificate_with_boundary(barrier_gain=barrier_gain, safety_radius=safety_radius+projection_distance, boundary_points=boundary_points) - - si_to_uni_dyn, uni_to_si_states = create_si_to_uni_mapping(projection_distance=projection_distance) - - uni_to_si_dyn = create_uni_to_si_dynamics(projection_distance=projection_distance) - - def f(dxu, x): - #Check user input types - assert isinstance(dxu, np.ndarray), "In the function created by the create_unicycle_barrier_certificate function, the unicycle robot velocity command (dxu) must be a numpy array. Recieved type %r." % type(dxu).__name__ - assert isinstance(x, np.ndarray), "In the function created by the create_unicycle_barrier_certificate function, the robot states (x) must be a numpy array. Recieved type %r." % type(x).__name__ - - #Check user input ranges/sizes - assert x.shape[0] == 3, "In the function created by the create_unicycle_barrier_certificate function, the dimension of the unicycle robot states (x) must be 3 ([x;y;theta]). Recieved dimension %r." % x.shape[0] - assert dxu.shape[0] == 2, "In the function created by the create_unicycle_barrier_certificate function, the dimension of the robot unicycle velocity command (dxu) must be 2 ([v;w]). Recieved dimension %r." % dxu.shape[0] - assert x.shape[1] == dxu.shape[1], "In the function created by the create_unicycle_barrier_certificate function, the number of robot states (x) must be equal to the number of robot unicycle velocity commands (dxu). Recieved a current robot pose input array (x) of size %r x %r and single integrator velocity array (dxi) of size %r x %r." % (x.shape[0], x.shape[1], dxu.shape[0], dxu.shape[1]) - - - x_si = uni_to_si_states(x) - #Convert unicycle control command to single integrator one - dxi = uni_to_si_dyn(dxu, x) - #Apply single integrator barrier certificate - dxi = si_barrier_cert(dxi, x_si) - #Return safe unicycle command - return si_to_uni_dyn(dxi, x) - - return f - -def create_unicycle_barrier_certificate2(barrier_gain=500, unsafe_barrier_gain=1e6, safety_radius=0.12, projection_distance=0.05, magnitude_limit=0.2): - """ Creates a unicycle barrier cetifcate to avoid collisions. Uses the diffeomorphism mapping - and single integrator implementation. For optimization purposes, this function returns - another function. - - barrier_gain: double (how fast the robots can approach each other) - safety_radius: double (how far apart the robots should stay) - projection_distance: double (how far ahead to place the bubble) - - -> function (the unicycle barrier certificate function) - """ - - #Check user input types - assert isinstance(barrier_gain, (int, float)), "In the function create_unicycle_barrier_certificate2, the barrier gain inside the safe set (barrier_gain) must be an integer or float. Recieved type %r." % type(barrier_gain).__name__ - assert isinstance(unsafe_barrier_gain, (int, float)), "In the function create_unicycle_barrier_certificate2, the barrier gain outside the safe set (unsafe_barrier_gain) must be an integer or float. Recieved type %r." % type(unsafe_barrier_gain).__name__ - assert isinstance(safety_radius, (int, float)), "In the function create_unicycle_barrier_certificate2, the safe distance between robots (safety_radius) must be an integer or float. Recieved type %r." % type(safety_radius).__name__ - assert isinstance(projection_distance, (int, float)), "In the function create_unicycle_barrier_certificate2, the projected point distance for the diffeomorphism between sinlge integrator and unicycle (projection_distance) must be an integer or float. Recieved type %r." % type(projection_distance).__name__ - assert isinstance(magnitude_limit, (int, float)), "In the function create_unicycle_barrier_certificate2, the maximum linear velocity of the robot (magnitude_limit) must be an integer or float. Recieved type %r." % type(magnitude_limit).__name__ - - #Check user input ranges/sizes - assert barrier_gain > 0, "In the function create_unicycle_barrier_certificate2, the barrier gain inside the safe set (barrier_gain) must be positive. Recieved %r." % barrier_gain - assert unsafe_barrier_gain > 0, "In the function create_unicycle_barrier_certificate2, the barrier gain outside the safe set (unsafe_barrier_gain) must be positive. Recieved %r." % unsafe_barrier_gain - assert safety_radius >= 0.12, "In the function create_unicycle_barrier_certificate2, the safe distance between robots (safety_radius) must be greater than or equal to the diameter of the robot (0.12m). Recieved %r." % safety_radius - assert projection_distance > 0, "In the function create_unicycle_barrier_certificate2, the projected point distance for the diffeomorphism between sinlge integrator and unicycle (projection_distance) must be positive. Recieved %r." % projection_distance - assert magnitude_limit > 0, "In the function create_unicycle_barrier_certificate2, the maximum linear velocity of the robot (magnitude_limit) must be positive. Recieved %r." % magnitude_limit - assert magnitude_limit <= 0.2, "In the function create_unicycle_barrier_certificate2, the maximum linear velocity of the robot (magnitude_limit) must be less than the max speed of the robot (0.2m/s). Recieved %r." % magnitude_limit - - - si_barrier_cert = create_single_integrator_barrier_certificate2(barrier_gain=barrier_gain, unsafe_barrier_gain=unsafe_barrier_gain, safety_radius=safety_radius+projection_distance) - - si_to_uni_dyn, uni_to_si_states = create_si_to_uni_mapping(projection_distance=projection_distance) - - uni_to_si_dyn = create_uni_to_si_dynamics(projection_distance=projection_distance) - - def f(dxu, x): - #Check user input types - assert isinstance(dxu, np.ndarray), "In the function created by the create_unicycle_barrier_certificate function, the unicycle robot velocity command (dxu) must be a numpy array. Recieved type %r." % type(dxu).__name__ - assert isinstance(x, np.ndarray), "In the function created by the create_unicycle_barrier_certificate function, the robot states (x) must be a numpy array. Recieved type %r." % type(x).__name__ - - #Check user input ranges/sizes - assert x.shape[0] == 3, "In the function created by the create_unicycle_barrier_certificate function, the dimension of the unicycle robot states (x) must be 3 ([x;y;theta]). Recieved dimension %r." % x.shape[0] - assert dxu.shape[0] == 2, "In the function created by the create_unicycle_barrier_certificate function, the dimension of the robot unicycle velocity command (dxu) must be 2 ([v;w]). Recieved dimension %r." % dxu.shape[0] - assert x.shape[1] == dxu.shape[1], "In the function created by the create_unicycle_barrier_certificate function, the number of robot states (x) must be equal to the number of robot unicycle velocity commands (dxu). Recieved a current robot pose input array (x) of size %r x %r and single integrator velocity array (dxi) of size %r x %r." % (x.shape[0], x.shape[1], dxu.shape[0], dxu.shape[1]) - - - x_si = uni_to_si_states(x) - #Convert unicycle control command to single integrator one - dxi = uni_to_si_dyn(dxu, x) - #Apply single integrator barrier certificate - dxi = si_barrier_cert(dxi, x_si) - #Return safe unicycle command - return si_to_uni_dyn(dxi, x) - - return f - -def create_unicycle_differential_drive_barrier_certificate(max_num_obstacle_points = 100, max_num_robots = 30, disturbance = 5, wheel_vel_limit = 12.5, base_length = 0.105, wheel_radius = 0.016, - projection_distance =0.05, barrier_gain = 150, safety_radius = 0.17): - - - D = np.matrix([[wheel_radius/2, wheel_radius/2], [-wheel_radius/base_length, wheel_radius/base_length]]) - L = np.matrix([[1,0],[0,projection_distance]])* D - disturb = np.matrix([[-disturbance, -disturbance, disturbance, disturbance],[-disturbance, disturbance, disturbance, -disturbance]]) - num_disturbs = np.size(disturb[1,:]) - - max_num_constraints = (max_num_robots**2-max_num_robots)//2 + max_num_robots*max_num_obstacle_points - A = np.matrix(np.zeros([max_num_constraints, 2*max_num_robots])) - b = np.matrix(np.zeros([max_num_constraints, 1])) - Os = np.matrix(np.zeros([2,max_num_robots])) - ps = np.matrix(np.zeros([2,max_num_robots])) - Ms = np.matrix(np.zeros([2,2*max_num_robots])) - - def robust_barriers(dxu, x, obstacles=np.empty(0)): - - num_robots = np.size(dxu[0,:]) - - if obstacles.size != 0: - num_obstacles = np.size(obstacles[0,:]) - else: - num_obstacles = 0 - - if(num_robots < 2): - temp = 0 - else: - temp = (num_robots**2-num_robots)//2 - - - # Generate constraints for barrier certificates based on the size of the safety radius - num_constraints = temp + num_robots*num_obstacles - A[0:num_constraints, 0:2*num_robots] = 0 - Os[0, 0:num_robots] = np.cos(x[2, :]) - Os[1, 0:num_robots] = np.sin(x[2, :]) - ps[:, 0:num_robots] = x[0:2, :] + projection_distance*Os[:, 0:num_robots] - # Ms Real Form - # Ms[0, 0:2*num_robots:2] = Os[0, 0:num_robots] - # Ms[0, 1:2*num_robots:2] = -projection_distance*Os[1, 0:num_robots] - # Ms[1, 1:2*num_robots:2] = projection_distance*Os[0, 0:num_robots] - # Ms[1, 0:2*num_robots:2] = Os[1, 0:num_robots] - # Flipped Ms to be able to perform desired matrix multiplication - Ms[0, 0:2*num_robots:2] = Os[0, 0:num_robots] - Ms[0, 1:2*num_robots:2] = Os[1, 0:num_robots] - Ms[1, 1:2*num_robots:2] = projection_distance*Os[0, 0:num_robots] - Ms[1, 0:2*num_robots:2] = -projection_distance*Os[1, 0:num_robots] - MDs = (Ms.T * D).T - temp = np.copy(MDs[1, 0:2*num_robots:2]) - MDs[1, 0:2*num_robots:2] = MDs[0, 1:2*num_robots:2] - MDs[0, 1:2*num_robots:2] = temp - - count = 0 - - for i in range(num_robots-1): - diffs = ps[:,i] - ps[:, i+1:num_robots] - hs = np.sum(np.square(diffs),0) - safety_radius**2 # 1 by N - h_dot_is = 2*diffs.T*MDs[:,(2*i, 2*i+1)] # N by 2 - h_dot_js = np.matrix(np.zeros((2,num_robots - (i+1)))) - h_dot_js[0, :] = -np.sum(2*np.multiply(diffs, MDs[:,2*(i+1):2*num_robots:2]), 0) - h_dot_js[1, :] = -np.sum(2*np.multiply(diffs, MDs[:,2*(i+1)+1:2*num_robots:2]), 0) - new_constraints = num_robots - i - 1 - A[count:count+new_constraints, (2*i):(2*i+2)] = h_dot_is - A[range(count,count+new_constraints), range(2*(i+1),2*num_robots,2)] = h_dot_js[0,:] - A[range(count,count+new_constraints), range(2*(i+1)+1,2*num_robots,2)] = h_dot_js[1,:] - b[count:count+new_constraints] = -barrier_gain*(np.power(hs,3)).T - np.min(h_dot_is*disturb,1) - np.min(h_dot_js.T*disturb,1) - count += new_constraints - - if obstacles.size != 0: - # Do obstacles - for i in range(num_robots): - diffs = (ps[:, i] - obstacles) - h = np.sum(np.square(diffs),0) - safety_radius**2 - h_dot_i = 2*diffs.T*MDs[:,2*i:2*i+2] - A[count:count+num_obstacles,(2*i):(2*i+2)] = h_dot_i - b[count:count+num_obstacles] = -barrier_gain*(np.power(h,3)).T - np.min(h_dot_i*disturb, 1) - count = count + num_obstacles - - # Adding Upper Bounds On Wheels - A[count:count+2*num_robots,0:2*num_robots] = -np.eye(2*num_robots) - b[count:count+2*num_robots] = -wheel_vel_limit - count += 2*num_robots - # # Adding Lower Bounds on Wheels - A[count:count+2*num_robots,0:2*num_robots] = np.eye(2*num_robots) - b[count:count+2*num_robots] = -wheel_vel_limit - count += 2*num_robots - - # Solve QP program generated earlier - L_all = np.kron(np.eye(num_robots), L) - dxu = np.linalg.inv(D)*dxu # Convert user input to differential drive - vhat = np.matrix(np.reshape(dxu ,(2*num_robots,1), order='F')) - H = 2*L_all.T*L_all - f = np.transpose(-2*np.transpose(vhat)*np.transpose(L_all)*L_all) - - # Alternative Solver - #start = time.time() - #vnew2 = solvers.qp(matrix(H), matrix(f), -matrix(A[0:count,0:2*num_robots]), -matrix( b[0:count]))['x'] # , A, b) Omit last 2 arguments since our QP has no equality constraints - #print("Time Taken by cvxOpt: {} s".format(time.time() - start)) - - vnew = solver2.solve_qp(H, -np.squeeze(np.array(f)), A[0:count,0:2*num_robots].T, np.squeeze(np.array(b[0:count])))[0] - # Initial Guess for Solver at the Next Iteration - # vnew = quadprog(H, double(f), -A(1:num_constraints,1:2*num_robots), -b(1:num_constraints), [], [], -wheel_vel_limit*ones(2*num_robots,1), wheel_vel_limit*ones(2*num_robots,1), [], opts); - # Set robot velocities to new velocities - dxu = np.reshape(vnew, (2, num_robots), order='F') - dxu = D*dxu - - return dxu - - return robust_barriers - -def create_unicycle_differential_drive_barrier_certificate_with_boundary(max_num_obstacle_points = 100, max_num_robots = 30, disturbance = 5, wheel_vel_limit = 12.5, base_length = 0.105, wheel_radius = 0.016, - projection_distance =0.05, barrier_gain = 150, safety_radius = 0.17, boundary_points = np.array([-1.6, 1.6, -1.0, 1.0])): - - - D = np.array([[wheel_radius/2, wheel_radius/2], [-wheel_radius/base_length, wheel_radius/base_length]]) - L = np.array([[1,0],[0,projection_distance]]).dot(D) - disturb = np.array([[-disturbance, -disturbance, disturbance, disturbance],[-disturbance, disturbance, disturbance, -disturbance]]) - num_disturbs = disturb.shape[1] - - max_num_constraints = (max_num_robots**2-max_num_robots)//2 + max_num_robots*max_num_obstacle_points - A = np.zeros([max_num_constraints, 2*max_num_robots]) - b = np.zeros([max_num_constraints, 1]) - Os = np.zeros([2,max_num_robots]) - ps = np.zeros([2,max_num_robots]) - Ms = np.zeros([2,2*max_num_robots]) - - def robust_barriers(dxu, x, obstacles=np.empty(0)): - - num_robots = np.size(dxu[0,:]) - - if obstacles.size != 0: - num_obstacles = np.size(obstacles[0,:]) - else: - num_obstacles = 0 - - if(num_robots < 2): - temp = 0 - else: - temp = (num_robots**2-num_robots)//2 - - - # Generate constraints for barrier certificates based on the size of the safety radius - num_constraints = temp + num_robots*num_obstacles + 4*num_robots - A[0:num_constraints, 0:2*num_robots] = 0 - Os[0, 0:num_robots] = np.cos(x[2, :]) - Os[1, 0:num_robots] = np.sin(x[2, :]) - ps[:, 0:num_robots] = x[:2, :] + projection_distance*Os[:, 0:num_robots] - Ms[0, 0:2*num_robots:2] = Os[0, 0:num_robots] - Ms[0, 1:2*num_robots:2] = -projection_distance*Os[1, 0:num_robots] - Ms[1, 1:2*num_robots:2] = projection_distance*Os[0, 0:num_robots] - Ms[1, 0:2*num_robots:2] = Os[1, 0:num_robots] - ret = np.zeros([1,temp]) - - count = 0 - - for i in range(num_robots-1): - for j in range(i+1, num_robots): - diff = ps[:, [i]] - ps[:, [j]] - h = np.sum(np.square(diff),0) - safety_radius**2 - h_dot_i = 2*diff.T.dot(Ms[:, ((2*i), (2*i+1))].dot(D)) - h_dot_j = -2*diff.T.dot(Ms[:, ((2*j), (2*j+1))].dot(D)) - h_dot_i = np.reshape(h_dot_i, (1,2)) - h_dot_j = np.reshape(h_dot_j, (1,2)) - A[count, ((2*i), (2*i+1))]=h_dot_i - A[count, ((2*j), (2*j+1))]=h_dot_j - b[count] = -barrier_gain*(np.power(h,3)) - np.min(h_dot_i.dot(disturb), 1) - np.min(h_dot_j.dot(disturb), 1) - count += 1 - - if obstacles.size != 0: - # Do obstacles - for i in range(num_robots): - diffs = (ps[:, i] - obstacles) - h = np.sum(np.square(diff),0) - safety_radius**2 - h_dot_i = 2*diffs*Ms[:, (2*i, 2*i+1)].dot(D) - A[count:count+num_obstacles, ((2*i),(2*i+1))] = h_dot_i - b[count:count+num_obstacles] = -barrier_gain*(np.power(h,3)) - np.min(h_dot_i.dot(disturb), 1) - count = count + num_obstacles - - for k in range(num_robots): - #Pos Y - A[count, (2*k, 2*k+1)] = -Ms[1,(2*k,2*k+1)].dot(D) - b[count] = -0.4*barrier_gain*(boundary_points[3] - safety_radius/2 - ps[1,k])**3; - count += 1 - - #Neg Y - A[count, (2*k, 2*k+1)] = Ms[1,(2*k,2*k+1)].dot(D) - b[count] = -0.4*barrier_gain*(-boundary_points[2] - safety_radius/2 + ps[1,k])**3; - count += 1 - - #Pos X - A[count, (2*k, 2*k+1)] = -Ms[0,(2*k,2*k+1)].dot(D) - b[count] = -0.4*barrier_gain*(boundary_points[1] - safety_radius/2 - ps[0,k])**3; - count += 1 - - #Neg X - A[count, (2*k, 2*k+1)] = Ms[0,(2*k,2*k+1)].dot(D) - b[count] = -0.4*barrier_gain*(-boundary_points[0] - safety_radius/2 + ps[0,k])**3; - count += 1 - - # Adding Upper Bounds On Wheels - A[count:count+2*num_robots,0:2*num_robots] = -np.eye(2*num_robots) - b[count:count+2*num_robots] = -wheel_vel_limit - count += 2*num_robots - # # Adding Lower Bounds on Wheels - A[count:count+2*num_robots,0:2*num_robots] = np.eye(2*num_robots) - b[count:count+2*num_robots] = -wheel_vel_limit - count += 2*num_robots - - # Solve QP program generated earlier - L_all = np.kron(np.eye(num_robots), L) - dxu = np.linalg.inv(D).dot(dxu) # Convert user input to differential drive - vhat = np.reshape(dxu ,(2*num_robots,1), order='F') - H = 2*L_all.T.dot(L_all) - f = -2*vhat.T.dot(L_all.T.dot(L_all)) - - # Alternative Solver - #start = time.time() - vnew = qp(matrix(H), matrix(f.T), -matrix(A[0:count,0:2*num_robots]), -matrix( b[0:count]))['x'] # , A, b) Omit last 2 arguments since our QP has no equality constraints - #print("Time Taken by cvxOpt: {} s".format(time.time() - start)) - - # vnew = solver2.solve_qp(H, np.float64(f), -A[0:count,0:2*num_robots], -np.array(b[0:count]))[0] - # Initial Guess for Solver at the Next Iteration - # vnew = quadprog(H, double(f), -A(1:num_constraints,1:2*num_robots), -b(1:num_constraints), [], [], -wheel_vel_limit*ones(2*num_robots,1), wheel_vel_limit*ones(2*num_robots,1), [], opts); - # Set robot velocities to new velocities - dxu = np.reshape(vnew, (2, -1), order='F') - dxu = D.dot(dxu) - - return dxu - - return robust_barriers diff --git a/ROS2/ros_ws/src/gravity/gravity/utilities/barrier_certificates2.py b/ROS2/ros_ws/src/gravity/gravity/utilities/barrier_certificates2.py deleted file mode 100644 index 329bc06..0000000 --- a/ROS2/ros_ws/src/gravity/gravity/utilities/barrier_certificates2.py +++ /dev/null @@ -1,123 +0,0 @@ -from cvxopt import matrix, solvers -import quadprog as solver2 -import numpy as np -import time -solvers.options['show_progress'] = False -solvers.options['reltol'] = 1e-2 # was e-2 -solvers.options['feastol'] = 1e-2 # was e-4 -solvers.options['maxiters'] = 50 # default is 100 - -def create_robust_barriers(max_num_obstacles = 100, max_num_robots = 30, d = 5, wheel_vel_limit = 12.5, base_length = 0.105, wheel_radius = 0.016, - projection_distance =0.05, gamma = 150, safety_radius = 0.12): # gamma was 150 - D = np.matrix([[wheel_radius/2, wheel_radius/2], [-wheel_radius/base_length, wheel_radius/base_length]]) - L = np.matrix([[1,0],[0,projection_distance]])* D - disturb = np.matrix([[-d, -d, d, d],[-d, d, d, -d]]) - num_disturbs = np.size(disturb[1,:]) - - #TODO: Take out 4*max_num_robots? - max_num_constraints = (max_num_robots**2-max_num_robots)//2 + max_num_robots*max_num_obstacles + 4*max_num_robots - A = np.matrix(np.zeros([max_num_constraints, 2*max_num_robots])) - b = np.matrix(np.zeros([max_num_constraints, 1])) - Os = np.matrix(np.zeros([2,max_num_robots])) - ps = np.matrix(np.zeros([2,max_num_robots])) - Ms = np.matrix(np.zeros([2,2*max_num_robots])) - - def robust_barriers(dxu, x, obstacles): - - num_robots = np.size(dxu[0,:]) - - if obstacles.size != 0: - num_obstacles = np.size(obstacles[0,:]) - else: - num_obstacles = 0 - - if(num_robots < 2): - temp = 0 - else: - temp = (num_robots**2-num_robots)//2 - - - if num_robots == 0: - return [] - - - # Generate constraints for barrier certificates based on the size of the safety radius - num_constraints = temp + num_robots*num_obstacles - A[0:num_constraints, 0:2*num_robots] = 0 - Os[0, 0:num_robots] = np.cos(x[2, :]) - Os[1, 0:num_robots] = np.sin(x[2, :]) - ps[:, 0:num_robots] = x[0:2, :] + projection_distance*Os[:, 0:num_robots] - # Ms Real Form - # Ms[0, 0:2*num_robots:2] = Os[0, 0:num_robots] - # Ms[0, 1:2*num_robots:2] = -projection_distance*Os[1, 0:num_robots] - # Ms[1, 1:2*num_robots:2] = projection_distance*Os[0, 0:num_robots] - # Ms[1, 0:2*num_robots:2] = Os[1, 0:num_robots] - # Flipped Ms to be able to perform desired matrix multiplication - Ms[0, 0:2*num_robots:2] = Os[0, 0:num_robots] - Ms[0, 1:2*num_robots:2] = Os[1, 0:num_robots] - Ms[1, 1:2*num_robots:2] = projection_distance*Os[0, 0:num_robots] - Ms[1, 0:2*num_robots:2] = -projection_distance*Os[1, 0:num_robots] - MDs = (Ms.T * D).T - temp = np.copy(MDs[1, 0:2*num_robots:2]) - MDs[1, 0:2*num_robots:2] = MDs[0, 1:2*num_robots:2] - MDs[0, 1:2*num_robots:2] = temp - - count = 0 - - for i in range(num_robots-1): - diffs = ps[:,i] - ps[:, i+1:num_robots] - hs = np.sum(np.square(diffs),0) - safety_radius**2 # 1 by N - h_dot_is = 2*diffs.T*MDs[:,2*i:2*i+2] # N by 2 - h_dot_js = np.matrix(np.zeros((2,num_robots - (i+1)))) - h_dot_js[0, :] = -np.sum(2*np.multiply(diffs, MDs[:,2*(i+1):2*num_robots:2]), 0) - h_dot_js[1, :] = -np.sum(2*np.multiply(diffs, MDs[:,2*(i+1)+1:2*num_robots:2]), 0) - new_constraints = num_robots - i - 1 - A[count:count+new_constraints, (2*i):(2*i+2)] = h_dot_is - A[range(count,count+new_constraints), range(2*(i+1),2*num_robots,2)] = h_dot_js[0,:] - A[range(count,count+new_constraints), range(2*(i+1)+1,2*num_robots,2)] = h_dot_js[1,:] - b[count:count+new_constraints] = -gamma*(np.power(hs,3)).T - np.min(h_dot_is*disturb,1) - np.min(h_dot_js.T*disturb,1) - count += new_constraints - - if obstacles.size != 0: - # Do obstacles - for i in range(num_robots): - diffs = (ps[:, i] - obstacles) - h = np.sum(np.square(diffs),0) - safety_radius**2 - h_dot_i = 2*diffs.T*MDs[:,2*i:2*i+2] - A[count:count+num_obstacles,(2*i):(2*i+2)] = h_dot_i - b[count:count+num_obstacles] = -gamma*(np.power(h,3)).T - np.min(h_dot_i*disturb, 1) - count = count + num_obstacles - - # Adding Upper Bounds On Wheels - A[count:count+2*num_robots,0:2*num_robots] = -np.eye(2*num_robots) - b[count:count+2*num_robots] = -wheel_vel_limit - count += 2*num_robots - # # Adding Lower Bounds on Wheels - A[count:count+2*num_robots,0:2*num_robots] = np.eye(2*num_robots) - b[count:count+2*num_robots] = -wheel_vel_limit - count += 2*num_robots - - # Solve QP program generated earlier - L_all = np.kron(np.eye(num_robots), L) - dxu = np.linalg.inv(D)*dxu # Convert user input to differential drive - vhat = np.matrix(np.reshape(dxu ,(2*num_robots,1), order='F')) - H = 2*L_all.T*L_all - f = np.transpose(-2*np.transpose(vhat)*np.transpose(L_all)*L_all) - - # Alternative Solver - #start = time.time() - #vnew2 = solvers.qp(matrix(H), matrix(f), -matrix(A[0:count,0:2*num_robots]), -matrix( b[0:count]))['x'] # , A, b) Omit last 2 arguments since our QP has no equality constraints - #print("Time Taken by cvxOpt: {} s".format(time.time() - start)) - - start = time.time() - vnew = solver2.solve_qp(H, -np.squeeze(np.array(f)), A[0:count,0:2*num_robots].T, np.squeeze(np.array(b[0:count])))[0] - print("Time Taken by quadprog: {} s".format(time.time() - start)) - # Initial Guess for Solver at the Next Iteration - # vnew = quadprog(H, double(f), -A(1:num_constraints,1:2*num_robots), -b(1:num_constraints), [], [], -wheel_vel_limit*ones(2*num_robots,1), wheel_vel_limit*ones(2*num_robots,1), [], opts); - # Set robot velocities to new velocities - dxu = np.reshape(vnew, (2, num_robots), order='F') - dxu = D*dxu - - return dxu - - return robust_barriers diff --git a/ROS2/ros_ws/src/gravity/gravity/utilities/controllers.py b/ROS2/ros_ws/src/gravity/gravity/utilities/controllers.py deleted file mode 100644 index 81cf20a..0000000 --- a/ROS2/ros_ws/src/gravity/gravity/utilities/controllers.py +++ /dev/null @@ -1,214 +0,0 @@ -import numpy as np -from utilities.transformations import * - -def create_si_position_controller(x_velocity_gain=1, y_velocity_gain=1, velocity_magnitude_limit=0.15): - """Creates a position controller for single integrators. Drives a single integrator to a point - using a propoertional controller. - - x_velocity_gain - the gain impacting the x (horizontal) velocity of the single integrator - y_velocity_gain - the gain impacting the y (vertical) velocity of the single integrator - velocity_magnitude_limit - the maximum magnitude of the produce velocity vector (should be less than the max linear speed of the platform) - - -> function - """ - - #Check user input types - assert isinstance(x_velocity_gain, (int, float)), "In the function create_si_position_controller, the x linear velocity gain (x_velocity_gain) must be an integer or float. Recieved type %r." % type(x_velocity_gain).__name__ - assert isinstance(y_velocity_gain, (int, float)), "In the function create_si_position_controller, the y linear velocity gain (y_velocity_gain) must be an integer or float. Recieved type %r." % type(y_velocity_gain).__name__ - assert isinstance(velocity_magnitude_limit, (int, float)), "In the function create_si_position_controller, the velocity magnitude limit (y_velocity_gain) must be an integer or float. Recieved type %r." % type(y_velocity_gain).__name__ - - #Check user input ranges/sizes - assert x_velocity_gain > 0, "In the function create_si_position_controller, the x linear velocity gain (x_velocity_gain) must be positive. Recieved %r." % x_velocity_gain - assert y_velocity_gain > 0, "In the function create_si_position_controller, the y linear velocity gain (y_velocity_gain) must be positive. Recieved %r." % y_velocity_gain - assert velocity_magnitude_limit >= 0, "In the function create_si_position_controller, the velocity magnitude limit (velocity_magnitude_limit) must not be negative. Recieved %r." % velocity_magnitude_limit - - gain = np.diag([x_velocity_gain, y_velocity_gain]) - - def si_position_controller(xi, positions): - - """ - xi: 2xN numpy array (of single-integrator states of the robots) - points: 2xN numpy array (of desired points each robot should achieve) - - -> 2xN numpy array (of single-integrator control inputs) - - """ - - #Check user input types - assert isinstance(xi, np.ndarray), "In the si_position_controller function created by the create_si_position_controller function, the single-integrator robot states (xi) must be a numpy array. Recieved type %r." % type(xi).__name__ - assert isinstance(positions, np.ndarray), "In the si_position_controller function created by the create_si_position_controller function, the robot goal points (positions) must be a numpy array. Recieved type %r." % type(positions).__name__ - - #Check user input ranges/sizes - assert xi.shape[0] == 2, "In the si_position_controller function created by the create_si_position_controller function, the dimension of the single-integrator robot states (xi) must be 2 ([x;y]). Recieved dimension %r." % xi.shape[0] - assert positions.shape[0] == 2, "In the si_position_controller function created by the create_si_position_controller function, the dimension of the robot goal points (positions) must be 2 ([x_goal;y_goal]). Recieved dimension %r." % positions.shape[0] - assert xi.shape[1] == positions.shape[1], "In the si_position_controller function created by the create_si_position_controller function, the number of single-integrator robot states (xi) must be equal to the number of robot goal points (positions). Recieved a single integrator current position input array of size %r x %r and desired position array of size %r x %r." % (xi.shape[0], xi.shape[1], positions.shape[0], positions.shape[1]) - - _,N = np.shape(xi) - dxi = np.zeros((2, N)) - - # Calculate control input - dxi[0][:] = x_velocity_gain*(positions[0][:]-xi[0][:]) - dxi[1][:] = y_velocity_gain*(positions[1][:]-xi[1][:]) - - # Threshold magnitude - norms = np.linalg.norm(dxi, axis=0) - idxs = np.where(norms > velocity_magnitude_limit) - if norms[idxs].size != 0: - dxi[:, idxs] *= velocity_magnitude_limit/norms[idxs] - - return dxi - - return si_position_controller - -def create_clf_unicycle_position_controller(linear_velocity_gain=0.8, angular_velocity_gain=3): - """Creates a unicycle model pose controller. Drives the unicycle model to a given position - and orientation. (($u: \mathbf{R}^{3 \times N} \times \mathbf{R}^{2 \times N} \to \mathbf{R}^{2 \times N}$) - - linear_velocity_gain - the gain impacting the produced unicycle linear velocity - angular_velocity_gain - the gain impacting the produced unicycle angular velocity - - -> function - """ - - #Check user input types - assert isinstance(linear_velocity_gain, (int, float)), "In the function create_clf_unicycle_position_controller, the linear velocity gain (linear_velocity_gain) must be an integer or float. Recieved type %r." % type(linear_velocity_gain).__name__ - assert isinstance(angular_velocity_gain, (int, float)), "In the function create_clf_unicycle_position_controller, the angular velocity gain (angular_velocity_gain) must be an integer or float. Recieved type %r." % type(angular_velocity_gain).__name__ - - #Check user input ranges/sizes - assert linear_velocity_gain >= 0, "In the function create_clf_unicycle_position_controller, the linear velocity gain (linear_velocity_gain) must be greater than or equal to zero. Recieved %r." % linear_velocity_gain - assert angular_velocity_gain >= 0, "In the function create_clf_unicycle_position_controller, the angular velocity gain (angular_velocity_gain) must be greater than or equal to zero. Recieved %r." % angular_velocity_gain - - - def position_uni_clf_controller(states, positions): - - """ A position controller for unicycle models. This utilized a control lyapunov function - (CLF) to drive a unicycle system to a desired position. This function operates on unicycle - states and desired positions to return a unicycle velocity command vector. - - states: 3xN numpy array (of unicycle states, [x;y;theta]) - poses: 3xN numpy array (of desired positons, [x_goal;y_goal]) - - -> 2xN numpy array (of unicycle control inputs) - """ - - #Check user input types - assert isinstance(states, np.ndarray), "In the function created by the create_clf_unicycle_position_controller function, the single-integrator robot states (xi) must be a numpy array. Recieved type %r." % type(states).__name__ - assert isinstance(positions, np.ndarray), "In the function created by the create_clf_unicycle_position_controller function, the robot goal points (positions) must be a numpy array. Recieved type %r." % type(positions).__name__ - - #Check user input ranges/sizes - assert states.shape[0] == 3, "In the function created by the create_clf_unicycle_position_controller function, the dimension of the unicycle robot states (states) must be 3 ([x;y;theta]). Recieved dimension %r." % states.shape[0] - assert positions.shape[0] == 2, "In the function created by the create_clf_unicycle_position_controller function, the dimension of the robot goal positions (positions) must be 2 ([x_goal;y_goal]). Recieved dimension %r." % positions.shape[0] - assert states.shape[1] == positions.shape[1], "In the function created by the create_clf_unicycle_position_controller function, the number of unicycle robot states (states) must be equal to the number of robot goal positions (positions). Recieved a current robot pose input array (states) of size %r states %r and desired position array (positions) of size %r states %r." % (states.shape[0], states.shape[1], positions.shape[0], positions.shape[1]) - - - _,N = np.shape(states) - dxu = np.zeros((2, N)) - - pos_error = positions - states[:2][:] - rot_error = np.arctan2(pos_error[1][:],pos_error[0][:]) - dist = np.linalg.norm(pos_error, axis=0) - - dxu[0][:]=linear_velocity_gain*dist*np.cos(rot_error-states[2][:]) - dxu[1][:]=angular_velocity_gain*dist*np.sin(rot_error-states[2][:]) - - return dxu - - return position_uni_clf_controller - -def create_clf_unicycle_pose_controller(approach_angle_gain=1, desired_angle_gain=2.7, rotation_error_gain=1): - """Returns a controller ($u: \mathbf{R}^{3 \times N} \times \mathbf{R}^{3 \times N} \to \mathbf{R}^{2 \times N}$) - that will drive a unicycle-modeled agent to a pose (i.e., position & orientation). This control is based on a control - Lyapunov function. - - approach_angle_gain - affects how the unicycle approaches the desired position - desired_angle_gain - affects how the unicycle approaches the desired angle - rotation_error_gain - affects how quickly the unicycle corrects rotation errors. - - - -> function - """ - - gamma = approach_angle_gain - k = desired_angle_gain - h = rotation_error_gain - - def R(theta): - return np.array([[np.cos(theta), -np.sin(theta)],[np.sin(theta),np.cos(theta)]]) - - def pose_uni_clf_controller(states, poses): - - N_states = states.shape[1] - dxu = np.zeros((2,N_states)) - - for i in range(N_states): - translate = R(-poses[2,i]).dot((poses[:2,i]-states[:2,i])) - e = np.linalg.norm(translate) - theta = np.arctan2(translate[1],translate[0]) - alpha = theta - (states[2,i]-poses[2,i]) - alpha = np.arctan2(np.sin(alpha),np.cos(alpha)) - - ca = np.cos(alpha) - sa = np.sin(alpha) - - print(gamma) - print(e) - print(ca) - - dxu[0,i] = gamma* e* ca - dxu[1,i] = k*alpha + gamma*((ca*sa)/alpha)*(alpha + h*theta) - - return dxu - - return pose_uni_clf_controller - -def create_hybrid_unicycle_pose_controller(linear_velocity_gain=1, angular_velocity_gain=2, velocity_magnitude_limit=0.15, angular_velocity_limit=np.pi, position_error=0.05, position_epsilon=0.02, rotation_error=0.05): - '''Returns a controller ($u: \mathbf{R}^{3 \times N} \times \mathbf{R}^{3 \times N} \to \mathbf{R}^{2 \times N}$) - that will drive a unicycle-modeled agent to a pose (i.e., position & orientation). This controller is - based on a hybrid controller that will drive the robot in a straight line to the desired position then rotate - to the desired position. - - linear_velocity_gain - affects how much the linear velocity is scaled based on the position error - angular_velocity_gain - affects how much the angular velocity is scaled based on the heading error - velocity_magnitude_limit - threshold for the max linear velocity that will be achieved by the robot - angular_velocity_limit - threshold for the max rotational velocity that will be achieved by the robot - position_error - the error tolerance for the final position of the robot - position_epsilon - the amount of translational distance that is allowed by the rotation before correcting position again. - rotation_error - the error tolerance for the final orientation of the robot - - ''' - - si_to_uni_dyn = create_si_to_uni_dynamics(linear_velocity_gain=linear_velocity_gain, angular_velocity_limit=angular_velocity_limit) - - def pose_uni_hybrid_controller(states, poses, input_approach_state = np.empty([0,0])): - N=states.shape[1] - dxu = np.zeros((2,N)) - - #This is essentially a hack since default arguments are evaluated only once we will mutate it with each call - if input_approach_state.shape[1] != N: - approach_state = np.ones((1,N))[0] - - for i in range(N): - wrapped = poses[2,i] - states[2,i] - wrapped = np.arctan2(np.sin(wrapped),np.cos(wrapped)) - - dxi = poses[:2,[i]] - states[:2,[i]] - - #Normalize Vector - norm_ = np.linalg.norm(dxi) - - if(norm_ > (position_error - position_epsilon) and approach_state[i]): - if(norm_ > velocity_magnitude_limit): - dxi = velocity_magnitude_limit*dxi/norm_ - dxu[:,[i]] = si_to_uni_dyn(dxi, states[:,[i]]) - elif(np.absolute(wrapped) > rotation_error): - approach_state[i] = 0 - if(norm_ > position_error): - approach_state = 1 - dxu[0,i] = 0 - dxu[1,i] = angular_velocity_gain*wrapped - else: - dxu[:,[i]] = np.zeros((2,1)) - - return dxu - - return pose_uni_hybrid_controller diff --git a/ROS2/ros_ws/src/gravity/gravity/utilities/graph.py b/ROS2/ros_ws/src/gravity/gravity/utilities/graph.py deleted file mode 100644 index 2a48750..0000000 --- a/ROS2/ros_ws/src/gravity/gravity/utilities/graph.py +++ /dev/null @@ -1,195 +0,0 @@ -import numpy as np - -def cycle_GL(N): - """ Generates a graph Laplacian for a cycle graph - - N: int (number of agents) - - -> NxN numpy array (representing the graph Laplacian) - """ - #Check user input types - assert isinstance(N, int), "In the cycle_GL function, the number of nodes (N) must be an integer. Recieved type %r." % type(N).__name__ - #Check user input ranges/sizes - assert N > 0, "In the cycle_GL function, number of nodes (N) must be positive. Recieved %r." % N - - ones = np.ones(N-1) - L = 2*np.identity(N) - np.diag(ones, 1) - np.diag(ones, -1) - L[N-1, 0] = -1 - L[0, N-1] = -1 - - return L - -def lineGL(N): - """ Generates a graph Laplacian for a line graph - - N: int (number of agents) - - -> NxN numpy array (representing the graph Laplacian) - """ - #Check user input types - assert isinstance(N, int), "In the lineGL function, the number of nodes (N) must be an integer. Recieved type %r." % type(N).__name__ - #Check user input ranges/sizes - assert N > 0, "In the lineGL function, number of nodes (N) must be positive. Recieved %r." % N - - ones = np.ones(N-1) - L = 2*np.identity(N) - np.diag(ones, 1) - np.diag(ones, -1) - L[0,0] = 1 - L[N-1,N-1] = 1 - - return L - -def completeGL(N): - """ Generates a graph Laplacian for a complete graph - - N: int (number of agents) - - -> NxN numpy array (representing the graph Laplacian) - """ - - #Check user input types - assert isinstance(N, int), "In the completeGL function, the number of nodes (N) must be an integer. Recieved type %r." % type(N).__name__ - #Check user input ranges/sizes - assert N > 0, "In the completeGL function, number of nodes (N) must be positive. Recieved %r." % N - - L = N*np.identity(N)-np.ones((N,N)) - - return L - -def random_connectedGL(v, e): - """ Generates a Laplacian for a random, connected graph with v verticies - and (v-1) + e edges. - - v: int (number of nodes) - e: number of additional edges - - -> vxv numpy array (representing the graph Laplacian) - """ - - #Check user input types - assert isinstance(v, int), "In the random_connectedGL function, the number of verticies (v) must be an integer. Recieved type %r." % type(v).__name__ - assert isinstance(e, int), "In the random_connectedGL function, the number of additional edges (e) must be an integer. Recieved type %r." % type(e).__name__ - #Check user input ranges/sizes - assert v > 0, "In the random_connectedGL function, number of verticies (v) must be positive. Recieved %r." % v - assert e >= 0, "In the random_connectedGL function, number of additional edges (e) must be greater than or equal to zero. Recieved %r." % e - - - L = np.zeros((v,v)) - - for i in range(1, v): - edge = np.random.randint(i,size=(1,1)) - #Update adjancency relations - L[i,edge] = -1 - L[edge,i] = -1 - - #Update node degrees - L[i,i] += 1 - L[edge,edge] = L[edge,edge]+1 - # Because all nodes have at least 1 degree, chosse from only upper diagonal portion - iut = np.triu_indices(v) - iul = np.tril_indices(v) - Ltemp = np.copy(L) - Ltemp[iul] = 1 - potEdges = np.where(np.logical_xor(Ltemp, 1)==True) - numEdges = min(e, len(potEdges[0])) - - if numEdges <= 0: - return L - - #Indicies of randomly chosen extra edges - edgeIndicies = np.random.permutation(len(potEdges[0]))[:numEdges] - sz =L.shape - - for index in edgeIndicies: - - #Update adjacency relation - L[potEdges[0][index],potEdges[1][index]] = -1 - L[potEdges[1][index],potEdges[0][index]] = -1 - - #Update Degree Relation - L[potEdges[0][index],potEdges[0][index]] += 1 - L[potEdges[1][index],potEdges[1][index]] += 1 - - return L - -def randomGL(v, e): - """ Generates a Laplacian for a random graph with v verticies - and e edges. - - v: int (number of nodes) - e: number of additional edges - - -> vxv numpy array (representing the graph Laplacian) - """ - - L = np.tril(np.ones((v,v))) - - #This works because you can't select diagonals - potEdges = np.where(L==0) - - L = L-L - - numEdges = min(e, len(potEdges[0])) - #Indicies of randomly chosen extra edges - edgeIndicies = np.random.permutation(len(potEdges[0]))[:numEdges] - - for index in edgeIndicies: - - #Update adjacency relation - L[potEdges[0][index],potEdges[1][index]] = -1 - L[potEdges[1][index],potEdges[0][index]] = -1 - - #Update Degree Relation - L[potEdges[0][index],potEdges[0][index]] += 1 - L[potEdges[1][index],potEdges[1][index]] += 1 - - return L - - -def topological_neighbors(L, agent): - """ Returns the neighbors of a particular agent using the graph Laplacian - - L: NxN numpy array (representing the graph Laplacian) - agent: int (agent: 0 - N-1) - - -> 1xM numpy array (with M neighbors) - """ - #Check user input types - assert isinstance(L, np.ndarray), "In the topological_neighbors function, the graph Laplacian (L) must be a numpy ndarray. Recieved type %r." % type(L).__name__ - assert isinstance(agent, int), "In the topological_neighbors function, the agent number (agent) must be an integer. Recieved type %r." % type(agent).__name__ - - #Check user input ranges/sizes - assert agent >= 0, "In the topological_neighbors function, the agent number (agent) must be greater than or equal to zero. Recieved %r." % agent - assert agent <= L.shape[0], "In the topological_neighbors function, the agent number (agent) must be within the dimension of the provided Laplacian (L). Recieved agent number %r and Laplactian size %r by %r." % (agent, L.shape[0], L.shape[1]) - - row = L[agent, :] - row[agent]=0 - # Since L = D - A - return np.where(row != 0)[0] - -def delta_disk_neighbors(poses, agent, delta): - ''' Returns the agents within the 2-norm of the supplied agent (not including the agent itself) - poses: 3xN numpy array (representing the unicycle statese of the robots) - agent: int (agent whose neighbors within a radius will be returned) - delta: float (radius of delta disk considered) - - -> 1xM numpy array (with M neighbors) - - ''' - #Check user input types - assert isinstance(poses, np.ndarray), "In the delta_disk_neighbors function, the robot poses (poses) must be a numpy ndarray. Recieved type %r." % type(poses).__name__ - assert isinstance(agent, int), "In the delta_disk_neighbors function, the agent number (agent) must be an integer. Recieved type %r." % type(agent).__name__ - assert isinstance(delta, (int,float)), "In the delta_disk_neighbors function, the agent number (agent) must be an integer. Recieved type %r." % type(agent).__name__ - - #Check user input ranges/sizes - assert agent >= 0, "In the delta_disk_neighbors function, the agent number (agent) must be greater than or equal to zero. Recieved %r." % agent - assert delta >=0, "In the delta_disk_neighbors function, the sensing/communication radius (delta) must be greater than or equal to zero. Recieved %r." % delta - assert agent <= poses.shape[1], "In the delta_disk_neighbors function, the agent number (agent) must be within the dimension of the provided poses. Recieved agent number %r and poses for %r agents." % (agent, poses.shape[1]) - - - - N = poses.shape[1] - agents = np.arange(N) - - within_distance = [np.linalg.norm(poses[:2,x]-poses[:2,agent])<=delta for x in agents] - within_distance[agent] = False - return agents[within_distance] \ No newline at end of file diff --git a/ROS2/ros_ws/src/gravity/gravity/utilities/misc.py b/ROS2/ros_ws/src/gravity/gravity/utilities/misc.py deleted file mode 100644 index fb3eccd..0000000 --- a/ROS2/ros_ws/src/gravity/gravity/utilities/misc.py +++ /dev/null @@ -1,128 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt - - -def generate_initial_conditions(N, spacing=0.3, width=3, height=1.8): - """Generates random initial conditions in an area of the specified - width and height at the required spacing. - - N: int (number of agents) - spacing: double (how far apart positions can be) - width: double (width of area) - height: double (height of area) - - -> 3xN numpy array (of poses) - """ - #Check user input types - assert isinstance(N, int), "In the function generate_initial_conditions, the number of robots (N) to generate intial conditions for must be an integer. Recieved type %r." % type(N).__name__ - assert isinstance(spacing, (float,int)), "In the function generate_initial_conditions, the minimum spacing between robots (spacing) must be an integer or float. Recieved type %r." % type(spacing).__name__ - assert isinstance(width, (float,int)), "In the function generate_initial_conditions, the width of the area to place robots randomly (width) must be an integer or float. Recieved type %r." % type(width).__name__ - assert isinstance(height, (float,int)), "In the function generate_initial_conditions, the height of the area to place robots randomly (width) must be an integer or float. Recieved type %r." % type(height).__name__ - - #Check user input ranges/sizes - assert N > 0, "In the function generate_initial_conditions, the number of robots to generate initial conditions for (N) must be positive. Recieved %r." % N - assert spacing > 0, "In the function generate_initial_conditions, the spacing between robots (spacing) must be positive. Recieved %r." % spacing - assert width > 0, "In the function generate_initial_conditions, the width of the area to initialize robots randomly (width) must be positive. Recieved %r." % width - assert height >0, "In the function generate_initial_conditions, the height of the area to initialize robots randomly (height) must be positive. Recieved %r." % height - - x_range = int(np.floor(width/spacing)) - y_range = int(np.floor(height/spacing)) - - assert x_range != 0, "In the function generate_initial_conditions, the space between robots (space) is too large compared to the width of the area robots are randomly initialized in (width)." - assert y_range != 0, "In the function generate_initial_conditions, the space between robots (space) is too large compared to the height of the area robots are randomly initialized in (height)." - assert x_range*y_range > N, "In the function generate_initial_conditions, it is impossible to place %r robots within a %r x %r meter area with a spacing of %r meters." % (N, width, height, spacing) - - choices = (np.random.choice(x_range*y_range, N, replace=False)+1) - - poses = np.zeros((3, N)) - - for i, c in enumerate(choices): - x,y = divmod(c, y_range) - poses[0, i] = x*spacing - width/2 - poses[1, i] = y*spacing - height/2 - poses[2, i] = np.random.rand()*2*np.pi - np.pi - - return poses - -def at_pose(states, poses, position_error=0.05, rotation_error=0.2): - """Checks whether robots are "close enough" to poses - - states: 3xN numpy array (of unicycle states) - poses: 3xN numpy array (of desired states) - - -> 1xN numpy index array (of agents that are close enough) - """ - #Check user input types - assert isinstance(states, np.ndarray), "In the at_pose function, the robot current state argument (states) must be a numpy ndarray. Recieved type %r." % type(states).__name__ - assert isinstance(poses, np.ndarray), "In the at_pose function, the checked pose argument (poses) must be a numpy ndarray. Recieved type %r." % type(poses).__name__ - assert isinstance(position_error, (float,int)), "In the at_pose function, the allowable position error argument (position_error) must be an integer or float. Recieved type %r." % type(position_error).__name__ - assert isinstance(rotation_error, (float,int)), "In the at_pose function, the allowable angular error argument (rotation_error) must be an integer or float. Recieved type %r." % type(rotation_error).__name__ - - #Check user input ranges/sizes - assert states.shape[0] == 3, "In the at_pose function, the dimension of the state of each robot must be 3 ([x;y;theta]). Recieved %r." % states.shape[0] - assert poses.shape[0] == 3, "In the at_pose function, the dimension of the checked pose of each robot must be 3 ([x;y;theta]). Recieved %r." % poses.shape[0] - assert states.shape == poses.shape, "In the at_pose function, the robot current state and checked pose inputs must be the same size (3xN, where N is the number of robots being checked). Recieved a state array of size %r x %r and checked pose array of size %r x %r." % (states.shape[0], states.shape[1], poses.shape[0], poses.shape[1]) - - # Calculate rotation errors with angle wrapping - res = states[2, :] - poses[2, :] - res = np.abs(np.arctan2(np.sin(res), np.cos(res))) - - # Calculate position errors - pes = np.linalg.norm(states[:2, :] - poses[:2, :], 2, 0) - - # Determine which agents are done - done = np.nonzero((res <= rotation_error) & (pes <= position_error)) - - return done - -def at_position(states, points, position_error=0.02): - """Checks whether robots are "close enough" to desired position - - states: 3xN numpy array (of unicycle states) - points: 2xN numpy array (of desired points) - - -> 1xN numpy index array (of agents that are close enough) - """ - - #Check user input types - assert isinstance(states, np.ndarray), "In the at_position function, the robot current state argument (states) must be a numpy ndarray. Recieved type %r." % type(states).__name__ - assert isinstance(points, np.ndarray), "In the at_position function, the desired pose argument (poses) must be a numpy ndarray. Recieved type %r." % type(points).__name__ - assert isinstance(position_error, (float,int)), "In the at_position function, the allowable position error argument (position_error) must be an integer or float. Recieved type %r." % type(position_error).__name__ - - #Check user input ranges/sizes - assert states.shape[0] == 3, "In the at_position function, the dimension of the state of each robot (states) must be 3. Recieved %r." % states.shape[0] - assert points.shape[0] == 2, "In the at_position function, the dimension of the checked position for each robot (points) must be 2. Recieved %r." % points.shape[0] - assert states.shape[1] == poses.shape[1], "In the at_position function, the number of checked points (points) must match the number of robot states provided (states). Recieved a state array of size %r x %r and desired pose array of size %r x %r." % (states.shape[0], states.shape[1], points.shape[0], points.shape[1]) - - # Calculate position errors - pes = np.linalg.norm(states[:2, :] - points, 2, 0) - - # Determine which agents are done - done = np.nonzero((pes <= position_error)) - - return done - -def determine_marker_size(robotarium_instance, marker_size_meters): - - # Get the x and y dimension of the robotarium figure window in pixels - fig_dim_pixels = robotarium_instance.axes.transData.transform(np.array([[robotarium_instance.boundaries[2]],[robotarium_instance.boundaries[3]]])) - - # Determine the ratio of the robot size to the x-axis (the axis are - # normalized so you could do this with y and figure height as well). - marker_ratio = (marker_size_meters)/(robotarium_instance.boundaries[2]) - - # Determine the marker size in points so it fits the window. Note: This is squared - # as marker sizes are areas. - return (fig_dim_pixels[0,0] * marker_ratio)**2. - - -def determine_font_size(robotarium_instance, font_height_meters): - - # Get the x and y dimension of the robotarium figure window in pixels - y1, y2 = robotarium_instance.axes.get_window_extent().get_points()[:,1] - - # Determine the ratio of the robot size to the y-axis. - font_ratio = (y2-y1)/(robotarium_instance.boundaries[2]) - - # Determine the font size in points so it fits the window. - return (font_ratio*font_height_meters) diff --git a/ROS2/ros_ws/src/gravity/gravity/utilities/transformations.py b/ROS2/ros_ws/src/gravity/gravity/utilities/transformations.py deleted file mode 100644 index 9207bf5..0000000 --- a/ROS2/ros_ws/src/gravity/gravity/utilities/transformations.py +++ /dev/null @@ -1,226 +0,0 @@ -import numpy as np - -def create_si_to_uni_dynamics(linear_velocity_gain=1, angular_velocity_limit=np.pi): - """ Returns a function mapping from single-integrator to unicycle dynamics with angular velocity magnitude restrictions. - - linear_velocity_gain: Gain for unicycle linear velocity - angular_velocity_limit: Limit for angular velocity (i.e., |w| < angular_velocity_limit) - - -> function - """ - - #Check user input types - assert isinstance(linear_velocity_gain, (int, float)), "In the function create_si_to_uni_dynamics, the linear velocity gain (linear_velocity_gain) must be an integer or float. Recieved type %r." % type(linear_velocity_gain).__name__ - assert isinstance(angular_velocity_limit, (int, float)), "In the function create_si_to_uni_dynamics, the angular velocity limit (angular_velocity_limit) must be an integer or float. Recieved type %r." % type(angular_velocity_limit).__name__ - - #Check user input ranges/sizes - assert linear_velocity_gain > 0, "In the function create_si_to_uni_dynamics, the linear velocity gain (linear_velocity_gain) must be positive. Recieved %r." % linear_velocity_gain - assert angular_velocity_limit >= 0, "In the function create_si_to_uni_dynamics, the angular velocity limit (angular_velocity_limit) must not be negative. Recieved %r." % angular_velocity_limit - - - def si_to_uni_dyn(dxi, poses): - """A mapping from single-integrator to unicycle dynamics. - - dxi: 2xN numpy array with single-integrator control inputs - poses: 2xN numpy array with single-integrator poses - - -> 2xN numpy array of unicycle control inputs - """ - - #Check user input types - assert isinstance(dxi, np.ndarray), "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics function, the single integrator velocity inputs (dxi) must be a numpy array. Recieved type %r." % type(dxi).__name__ - assert isinstance(poses, np.ndarray), "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics function, the current robot poses (poses) must be a numpy array. Recieved type %r." % type(poses).__name__ - - #Check user input ranges/sizes - assert dxi.shape[0] == 2, "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics function, the dimension of the single integrator velocity inputs (dxi) must be 2 ([x_dot;y_dot]). Recieved dimension %r." % dxi.shape[0] - assert poses.shape[0] == 3, "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics function, the dimension of the current pose of each robot must be 3 ([x;y;theta]). Recieved dimension %r." % poses.shape[0] - assert dxi.shape[1] == poses.shape[1], "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics function, the number of single integrator velocity inputs must be equal to the number of current robot poses. Recieved a single integrator velocity input array of size %r x %r and current pose array of size %r x %r." % (dxi.shape[0], dxi.shape[1], poses.shape[0], poses.shape[1]) - - M,N = np.shape(dxi) - - a = np.cos(poses[2, :]) - b = np.sin(poses[2, :]) - - dxu = np.zeros((2, N)) - dxu[0, :] = linear_velocity_gain*(a*dxi[0, :] + b*dxi[1, :]) - dxu[1, :] = angular_velocity_limit*np.arctan2(-b*dxi[0, :] + a*dxi[1, :], dxu[0, :])/(np.pi/2) - - return dxu - - return si_to_uni_dyn - -def create_si_to_uni_dynamics_with_backwards_motion(linear_velocity_gain=1, angular_velocity_limit=np.pi): - """ Returns a function mapping from single-integrator dynamics to unicycle dynamics. This implementation of - the mapping allows for robots to drive backwards if that direction of linear velocity requires less rotation. - - linear_velocity_gain: Gain for unicycle linear velocity - angular_velocity_limit: Limit for angular velocity (i.e., |w| < angular_velocity_limit) - - """ - - #Check user input types - assert isinstance(linear_velocity_gain, (int, float)), "In the function create_si_to_uni_dynamics, the linear velocity gain (linear_velocity_gain) must be an integer or float. Recieved type %r." % type(linear_velocity_gain).__name__ - assert isinstance(angular_velocity_limit, (int, float)), "In the function create_si_to_uni_dynamics, the angular velocity limit (angular_velocity_limit) must be an integer or float. Recieved type %r." % type(angular_velocity_limit).__name__ - - #Check user input ranges/sizes - assert linear_velocity_gain > 0, "In the function create_si_to_uni_dynamics, the linear velocity gain (linear_velocity_gain) must be positive. Recieved %r." % linear_velocity_gain - assert angular_velocity_limit >= 0, "In the function create_si_to_uni_dynamics, the angular velocity limit (angular_velocity_limit) must not be negative. Recieved %r." % angular_velocity_limit - - - def si_to_uni_dyn(dxi, poses): - """A mapping from single-integrator to unicycle dynamics. - - dxi: 2xN numpy array with single-integrator control inputs - poses: 2xN numpy array with single-integrator poses - - -> 2xN numpy array of unicycle control inputs - """ - - #Check user input types - assert isinstance(dxi, np.ndarray), "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics_with_backwards_motion function, the single integrator velocity inputs (dxi) must be a numpy array. Recieved type %r." % type(dxi).__name__ - assert isinstance(poses, np.ndarray), "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics_with_backwards_motion function, the current robot poses (poses) must be a numpy array. Recieved type %r." % type(poses).__name__ - - #Check user input ranges/sizes - assert dxi.shape[0] == 2, "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics_with_backwards_motion function, the dimension of the single integrator velocity inputs (dxi) must be 2 ([x_dot;y_dot]). Recieved dimension %r." % dxi.shape[0] - assert poses.shape[0] == 3, "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics_with_backwards_motion function, the dimension of the current pose of each robot must be 3 ([x;y;theta]). Recieved dimension %r." % poses.shape[0] - assert dxi.shape[1] == poses.shape[1], "In the si_to_uni_dyn function created by the create_si_to_uni_dynamics_with_backwards_motion function, the number of single integrator velocity inputs must be equal to the number of current robot poses. Recieved a single integrator velocity input array of size %r x %r and current pose array of size %r x %r." % (dxi.shape[0], dxi.shape[1], poses.shape[0], poses.shape[1]) - - M,N = np.shape(dxi) - - a = np.cos(poses[2, :]) - b = np.sin(poses[2, :]) - - dxu = np.zeros((2, N)) - dxu[0, :] = linear_velocity_gain*(a*dxi[0, :] + b*dxi[1, :]) - dxu[1, :] = angular_velocity_limit*np.arctan2(-b*dxi[0, :] + a*dxi[1, :], dxu[0, :])/(np.pi/2) - - return dxu - - return si_to_uni_dyn - -def create_si_to_uni_mapping(projection_distance=0.05, angular_velocity_limit = np.pi): - """Creates two functions for mapping from single integrator dynamics to - unicycle dynamics and unicycle states to single integrator states. - - This mapping is done by placing a virtual control "point" in front of - the unicycle. - - projection_distance: How far ahead to place the point - angular_velocity_limit: The maximum angular velocity that can be provided - - -> (function, function) - """ - - #Check user input types - assert isinstance(projection_distance, (int, float)), "In the function create_si_to_uni_mapping, the projection distance of the new control point (projection_distance) must be an integer or float. Recieved type %r." % type(projection_distance).__name__ - assert isinstance(angular_velocity_limit, (int, float)), "In the function create_si_to_uni_mapping, the maximum angular velocity command (angular_velocity_limit) must be an integer or float. Recieved type %r." % type(angular_velocity_limit).__name__ - - #Check user input ranges/sizes - assert projection_distance > 0, "In the function create_si_to_uni_mapping, the projection distance of the new control point (projection_distance) must be positive. Recieved %r." % projection_distance - assert projection_distance >= 0, "In the function create_si_to_uni_mapping, the maximum angular velocity command (angular_velocity_limit) must be greater than or equal to zero. Recieved %r." % angular_velocity_limit - - def si_to_uni_dyn(dxi, poses): - """Takes single-integrator velocities and transforms them to unicycle - control inputs. - - dxi: 2xN numpy array of single-integrator control inputs - poses: 3xN numpy array of unicycle poses - - -> 2xN numpy array of unicycle control inputs - """ - - #Check user input types - assert isinstance(dxi, np.ndarray), "In the si_to_uni_dyn function created by the create_si_to_uni_mapping function, the single integrator velocity inputs (dxi) must be a numpy array. Recieved type %r." % type(dxi).__name__ - assert isinstance(poses, np.ndarray), "In the si_to_uni_dyn function created by the create_si_to_uni_mapping function, the current robot poses (poses) must be a numpy array. Recieved type %r." % type(poses).__name__ - - #Check user input ranges/sizes - assert dxi.shape[0] == 2, "In the si_to_uni_dyn function created by the create_si_to_uni_mapping function, the dimension of the single integrator velocity inputs (dxi) must be 2 ([x_dot;y_dot]). Recieved dimension %r." % dxi.shape[0] - assert poses.shape[0] == 3, "In the si_to_uni_dyn function created by the create_si_to_uni_mapping function, the dimension of the current pose of each robot must be 3 ([x;y;theta]). Recieved dimension %r." % poses.shape[0] - assert dxi.shape[1] == poses.shape[1], "In the si_to_uni_dyn function created by the create_si_to_uni_mapping function, the number of single integrator velocity inputs must be equal to the number of current robot poses. Recieved a single integrator velocity input array of size %r x %r and current pose array of size %r x %r." % (dxi.shape[0], dxi.shape[1], poses.shape[0], poses.shape[1]) - - - M,N = np.shape(dxi) - - cs = np.cos(poses[2, :]) - ss = np.sin(poses[2, :]) - - dxu = np.zeros((2, N)) - dxu[0, :] = (cs*dxi[0, :] + ss*dxi[1, :]) - dxu[1, :] = (1/projection_distance)*(-ss*dxi[0, :] + cs*dxi[1, :]) - - #Impose angular velocity cap. - dxu[1,dxu[1,:]>angular_velocity_limit] = angular_velocity_limit - dxu[1,dxu[1,:]<-angular_velocity_limit] = -angular_velocity_limit - - return dxu - - def uni_to_si_states(poses): - """Takes unicycle states and returns single-integrator states - - poses: 3xN numpy array of unicycle states - - -> 2xN numpy array of single-integrator states - """ - - _,N = np.shape(poses) - - si_states = np.zeros((2, N)) - si_states[0, :] = poses[0, :] + projection_distance*np.cos(poses[2, :]) - si_states[1, :] = poses[1, :] + projection_distance*np.sin(poses[2, :]) - - return si_states - - return si_to_uni_dyn, uni_to_si_states - -def create_uni_to_si_dynamics(projection_distance=0.05): - """Creates two functions for mapping from unicycle dynamics to single - integrator dynamics and single integrator states to unicycle states. - - This mapping is done by placing a virtual control "point" in front of - the unicycle. - - projection_distance: How far ahead to place the point - - -> function - """ - - #Check user input types - assert isinstance(projection_distance, (int, float)), "In the function create_uni_to_si_dynamics, the projection distance of the new control point (projection_distance) must be an integer or float. Recieved type %r." % type(projection_distance).__name__ - - #Check user input ranges/sizes - assert projection_distance > 0, "In the function create_uni_to_si_dynamics, the projection distance of the new control point (projection_distance) must be positive. Recieved %r." % projection_distance - - - def uni_to_si_dyn(dxu, poses): - """A function for converting from unicycle to single-integrator dynamics. - Utilizes a virtual point placed in front of the unicycle. - - dxu: 2xN numpy array of unicycle control inputs - poses: 3xN numpy array of unicycle poses - projection_distance: How far ahead of the unicycle model to place the point - - -> 2xN numpy array of single-integrator control inputs - """ - - #Check user input types - assert isinstance(dxu, np.ndarray), "In the uni_to_si_dyn function created by the create_uni_to_si_dynamics function, the unicycle velocity inputs (dxu) must be a numpy array. Recieved type %r." % type(dxi).__name__ - assert isinstance(poses, np.ndarray), "In the uni_to_si_dyn function created by the create_uni_to_si_dynamics function, the current robot poses (poses) must be a numpy array. Recieved type %r." % type(poses).__name__ - - #Check user input ranges/sizes - assert dxu.shape[0] == 2, "In the uni_to_si_dyn function created by the create_uni_to_si_dynamics function, the dimension of the unicycle velocity inputs (dxu) must be 2 ([v;w]). Recieved dimension %r." % dxu.shape[0] - assert poses.shape[0] == 3, "In the uni_to_si_dyn function created by the create_uni_to_si_dynamics function, the dimension of the current pose of each robot must be 3 ([x;y;theta]). Recieved dimension %r." % poses.shape[0] - assert dxu.shape[1] == poses.shape[1], "In the uni_to_si_dyn function created by the create_uni_to_si_dynamics function, the number of unicycle velocity inputs must be equal to the number of current robot poses. Recieved a unicycle velocity input array of size %r x %r and current pose array of size %r x %r." % (dxu.shape[0], dxu.shape[1], poses.shape[0], poses.shape[1]) - - - M,N = np.shape(dxu) - - cs = np.cos(poses[2, :]) - ss = np.sin(poses[2, :]) - - dxi = np.zeros((2, N)) - dxi[0, :] = (cs*dxu[0, :] - projection_distance*ss*dxu[1, :]) - dxi[1, :] = (ss*dxu[0, :] + projection_distance*cs*dxu[1, :]) - - return dxi - - return uni_to_si_dyn \ No newline at end of file diff --git a/ROS2/ros_ws/src/gravity/launch/gravity.launch.py b/ROS2/ros_ws/src/gravity/launch/gravity.launch.py deleted file mode 100644 index 5896499..0000000 --- a/ROS2/ros_ws/src/gravity/launch/gravity.launch.py +++ /dev/null @@ -1,35 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration -import socket -import numpy as np -def generate_launch_description(): - graph_lapacian = np.array( - [1,-1,0], - [-1,2,-1], - [0,-1,1] - ) - F = DeclareLaunchArgument("F", default_value="-1") - g = DeclareLaunchArgument("g", default_value="1") - L = DeclareLaunchArgument("L", default_value=str(graph_lapacian)) - - return LaunchDescription([ - F, - g, - L, - Node( - package='gravity', - namespace=socket.gethostname(), - executable='gravity', - name='controller', - output='screen', - parameters=[ - { - "F": LaunchConfiguration("F"), - "g": LaunchConfiguration("G"), - "L": LaunchConfiguration("L") - } - ] - ), - ]) diff --git a/ROS2/ros_ws/src/gravity/package.xml b/ROS2/ros_ws/src/gravity/package.xml deleted file mode 100644 index fa615bc..0000000 --- a/ROS2/ros_ws/src/gravity/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - gravity - 0.0.0 - TODO: Package description - michael - TODO: License declaration - - ament_cmake - - rclpy - - ament_lint_auto - ament_lint_common - - - ament_cmake - - - rosidl_default_generators - - rosidl_default_runtime - - rosidl_interface_packages - diff --git a/ROS2/ros_ws/src/gravity/srv/GoalPoint.srv b/ROS2/ros_ws/src/gravity/srv/GoalPoint.srv deleted file mode 100644 index a260294..0000000 --- a/ROS2/ros_ws/src/gravity/srv/GoalPoint.srv +++ /dev/null @@ -1,3 +0,0 @@ -geometry_msgs/Point goal_pos ---- -std_msgs/Bool confirm \ No newline at end of file diff --git a/ROS2/ros_ws/src/gravity/srv/MAdvertise.srv b/ROS2/ros_ws/src/gravity/srv/MAdvertise.srv deleted file mode 100644 index 4eac108..0000000 --- a/ROS2/ros_ws/src/gravity/srv/MAdvertise.srv +++ /dev/null @@ -1,3 +0,0 @@ -std_msgs/String name ---- -std_msgs/Float64 m \ No newline at end of file diff --git a/ROS2/ros_ws/src/robot_msgs b/ROS2/ros_ws/src/robot_msgs new file mode 160000 index 0000000..eb8598a --- /dev/null +++ b/ROS2/ros_ws/src/robot_msgs @@ -0,0 +1 @@ +Subproject commit eb8598ae94945da2266c499b6874bae1bd36472b diff --git a/ROS2/ros_ws/src/robot_msgs/CMakeLists.txt b/ROS2/ros_ws/src/robot_msgs/CMakeLists.txt deleted file mode 100644 index a14718e..0000000 --- a/ROS2/ros_ws/src/robot_msgs/CMakeLists.txt +++ /dev/null @@ -1,52 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(robot_msgs) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(std_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -rosidl_generate_interfaces(${PROJECT_NAME} - -"msg/Environment.msg" -"msg/RobotPos.msg" -"msg/Light.msg" -"msg/StringList.msg" -"msg/SensorEnable.msg" -"srv/GetCharger.srv" -"srv/ReleaseCharger.srv" -DEPENDENCIES std_msgs nav_msgs - -) - - -ament_package() diff --git a/ROS2/ros_ws/src/robot_msgs/msg/Environment.msg b/ROS2/ros_ws/src/robot_msgs/msg/Environment.msg deleted file mode 100755 index f7a323e..0000000 --- a/ROS2/ros_ws/src/robot_msgs/msg/Environment.msg +++ /dev/null @@ -1,4 +0,0 @@ -float64 temp -float64 pressure -float64 humidity -float64 altitude diff --git a/ROS2/ros_ws/src/robot_msgs/msg/Light.msg b/ROS2/ros_ws/src/robot_msgs/msg/Light.msg deleted file mode 100755 index 785a583..0000000 --- a/ROS2/ros_ws/src/robot_msgs/msg/Light.msg +++ /dev/null @@ -1,2 +0,0 @@ -int32[] rgbw -int32 gesture \ No newline at end of file diff --git a/ROS2/ros_ws/src/robot_msgs/msg/RobotPos.msg b/ROS2/ros_ws/src/robot_msgs/msg/RobotPos.msg deleted file mode 100755 index ca8bc4f..0000000 --- a/ROS2/ros_ws/src/robot_msgs/msg/RobotPos.msg +++ /dev/null @@ -1 +0,0 @@ -nav_msgs/Odometry[] robot_pos \ No newline at end of file diff --git a/ROS2/ros_ws/src/robot_msgs/msg/SensorEnable.msg b/ROS2/ros_ws/src/robot_msgs/msg/SensorEnable.msg deleted file mode 100644 index 5c2e16d..0000000 --- a/ROS2/ros_ws/src/robot_msgs/msg/SensorEnable.msg +++ /dev/null @@ -1,5 +0,0 @@ -bool environment -bool imu -bool light -bool proximity -bool mic \ No newline at end of file diff --git a/ROS2/ros_ws/src/robot_msgs/msg/StringList.msg b/ROS2/ros_ws/src/robot_msgs/msg/StringList.msg deleted file mode 100644 index 7cccd01..0000000 --- a/ROS2/ros_ws/src/robot_msgs/msg/StringList.msg +++ /dev/null @@ -1 +0,0 @@ -std_msgs/String[] data \ No newline at end of file diff --git a/ROS2/ros_ws/src/robot_msgs/package.xml b/ROS2/ros_ws/src/robot_msgs/package.xml deleted file mode 100644 index ce2dcad..0000000 --- a/ROS2/ros_ws/src/robot_msgs/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - robot_msgs - 0.0.0 - TODO: Package description - michael - TODO: License declaration - - ament_cmake - - std_msgs - geometry_msgs - - ament_lint_auto - ament_lint_common - - - ament_cmake - - - rosidl_default_generators - - rosidl_default_runtime - - rosidl_interface_packages - diff --git a/ROS2/ros_ws/src/robot_msgs/srv/GetCharger.srv b/ROS2/ros_ws/src/robot_msgs/srv/GetCharger.srv deleted file mode 100644 index f53f345..0000000 --- a/ROS2/ros_ws/src/robot_msgs/srv/GetCharger.srv +++ /dev/null @@ -1,4 +0,0 @@ -std_msgs/String name ---- -std_msgs/UInt16 id -geometry_msgs/Pose position diff --git a/ROS2/ros_ws/src/robot_msgs/srv/ReleaseCharger.srv b/ROS2/ros_ws/src/robot_msgs/srv/ReleaseCharger.srv deleted file mode 100644 index 4111b8a..0000000 --- a/ROS2/ros_ws/src/robot_msgs/srv/ReleaseCharger.srv +++ /dev/null @@ -1,3 +0,0 @@ -std_msgs/UInt16 id ---- -std_msgs/Bool released diff --git a/ROS2/ros_ws/src/ros2_usb_camera b/ROS2/ros_ws/src/ros2_usb_camera deleted file mode 160000 index 146c591..0000000 --- a/ROS2/ros_ws/src/ros2_usb_camera +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 146c5911ebcc5539eb8550e02763abe1fdfa8d05 diff --git a/ROS2/ros_ws/src/vision_opencv b/ROS2/ros_ws/src/vision_opencv deleted file mode 160000 index 47e6639..0000000 --- a/ROS2/ros_ws/src/vision_opencv +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 47e66395ad7be55c3797224e7333bdfe896f547d From 940a8549fc17c4ef087377fad46cfe9747a1bfc9 Mon Sep 17 00:00:00 2001 From: Neil Patel Date: Mon, 5 Feb 2024 12:34:31 -0500 Subject: [PATCH 3/5] added swarmsilverfish to json --- ROS2/ros_ws/src/robot_controller/include/robots.json | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/ROS2/ros_ws/src/robot_controller/include/robots.json b/ROS2/ros_ws/src/robot_controller/include/robots.json index dce8a49..d8710a6 100755 --- a/ROS2/ros_ws/src/robot_controller/include/robots.json +++ b/ROS2/ros_ws/src/robot_controller/include/robots.json @@ -35,7 +35,7 @@ }, "11":{ "name":"swarmcoral", - "type":"swarmv3" + "type":"herocube" }, "12":{ "name":"swarmhero", @@ -54,7 +54,7 @@ "type":"swarmv3" }, "16":{ - "name":"swarmduckworth", + "name":"swarmkraken", "type":"swarmv3" }, "17":{ @@ -86,18 +86,15 @@ "type":"swarmv3" }, "24":{ - "name":"", + "name":"swarmcapstone1", "type":"herocube" }, "25":{ - "name":"", + "name":"swarmsilverfish", "type":"herocube" }, "26":{ "name":"", "type":"herocube" } - - - } From 5b1a39f081591ad326a066848db1841408f25b56 Mon Sep 17 00:00:00 2001 From: Neil Patel Date: Tue, 6 Feb 2024 12:55:21 -0500 Subject: [PATCH 4/5] added the robots --- .../camera_server/camera_server.py | 2 +- .../src/robot_controller/include/robots.json | 2 +- .../__pycache__/__init__.cpython-38.pyc | Bin 188 -> 190 bytes 3 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ROS2/ros_ws/src/camera_server/camera_server/camera_server.py b/ROS2/ros_ws/src/camera_server/camera_server/camera_server.py index 05685a1..9d8ee9f 100755 --- a/ROS2/ros_ws/src/camera_server/camera_server/camera_server.py +++ b/ROS2/ros_ws/src/camera_server/camera_server/camera_server.py @@ -103,7 +103,7 @@ def get_positions(self,msg): robot_names.data.append(String()) try: - print("Robot Name:",self.robot_dictionary[str(detection["id"])]["name"]) + #print("Robot Name:",self.robot_dictionary[str(detection["id"])]["name"]) active_dict[str(detection["id"])] = self.robot_dictionary[str(detection["id"])]["name"] robot_names.data[-1].data = self.robot_dictionary[str(detection["id"])]["name"] except KeyError: diff --git a/ROS2/ros_ws/src/robot_controller/include/robots.json b/ROS2/ros_ws/src/robot_controller/include/robots.json index d8710a6..5a531e8 100755 --- a/ROS2/ros_ws/src/robot_controller/include/robots.json +++ b/ROS2/ros_ws/src/robot_controller/include/robots.json @@ -94,7 +94,7 @@ "type":"herocube" }, "26":{ - "name":"", + "name":"swarmpheonix", "type":"herocube" } } diff --git a/ROS2/ros_ws/src/robot_controller/robot_controller/__pycache__/__init__.cpython-38.pyc b/ROS2/ros_ws/src/robot_controller/robot_controller/__pycache__/__init__.cpython-38.pyc index 314e5128739ae14143c7be53d3c0e060eb82381d..82245b09559d59b90339e71ba48e2942aa627c47 100644 GIT binary patch delta 47 zcmdnPxQ~%1l$V!_0SK%YC8tc}vF1zG&&bbB)z3&R%Fjtmni#6gnpB#ZlQOZ|5dbQM B4n6 Date: Fri, 28 Jun 2024 16:16:33 -0400 Subject: [PATCH 5/5] linking correct swarm experiments submod, and updated the robots.json folder --- ROS2/ros_ws/src/camera_server/swarmexperiments | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROS2/ros_ws/src/camera_server/swarmexperiments b/ROS2/ros_ws/src/camera_server/swarmexperiments index af98851..bf87ace 160000 --- a/ROS2/ros_ws/src/camera_server/swarmexperiments +++ b/ROS2/ros_ws/src/camera_server/swarmexperiments @@ -1 +1 @@ -Subproject commit af98851f5be78e66890339d06dfc39103c54cfa6 +Subproject commit bf87ace3a5ec0df03bd9a184cb0e5a6414ec3cdf