diff --git a/modules/Motor_Controller b/modules/Motor_Controller
deleted file mode 160000
index bdad5d0f6f49b1d5ae7f1ed59e6d0d308cad605f..0000000000000000000000000000000000000000
--- a/modules/Motor_Controller
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit bdad5d0f6f49b1d5ae7f1ed59e6d0d308cad605f
diff --git a/modules/__init__.py b/modules/__init__.py
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/modules/robot.yaml b/modules/robot.yaml
deleted file mode 100644
index f23471a1be074697ae3c5d878e121bbcc0e49829..0000000000000000000000000000000000000000
--- a/modules/robot.yaml
+++ /dev/null
@@ -1,45 +0,0 @@
-base_footprint2base_link : [0., 0., 0.125]
-base_link2wheel_left_link : [0., 0.220, 0.0]
-base_link2wheel_right_link : [0., -0.220, 0.0]
-base_link2caster_left_link : [-0.455, 0.102, -0.080]
-base_link2caster_right_link : [-0.455, -0.102, -0.080]
-base_link2head_1_link : [-0.226, 0.000, 1.192]
-base_link2head_2_link : [-0.183, 0.000, 1.284]
-base_link2head_front_camera_link : [-0.120, -0.000, 1.466]
-base_link2head_front_camera_optical_frame : [-0.120, -0.000, 1.466]
-base_footprint_radius: 0.3
-wheel_radius : 0.125
-wheel_width : 0.075
-wheel_separation : 0.44
-wheel_torque : 6.0
-wheel_velocity : 1.0
-caster_radius : 0.045
-caster_width : 0.055
-caster_separation_x : 0.91
-caster_offset_y : 0.1025
-caster_offset_z : -0.08
-total_mass : 54.893
-wheel_mass : 1.65651
-L1 : [[0, 0, 1, 0], [1, 0, 0, 0.182], [0, 1, 0, -0.063], [0, 0, 0, 1]]
-L2 : [[0, 0, -1, 0.093], [0, 1, 0, -0.043], [1, 0, 0, 0], [0, 0, 0, 1]]
-L3 : [[1, 0, 0, 0], [0, 1, 0, 0.225], [0, 0, 1, -1.192], [0, 0, 0, 1]]
-local_map_depth: 3.0
-local_map_px_size: 0.05
-max_speed_motor_wheel : 1.0
-max_forward_speed: 0.5
-max_backward_speed: -0.1
-max_forward_acc: 0.2
-max_ang_vel: 0.5
-max_ang_acc: 0.4
-max_pan_angle : 1.309
-min_pan_angle : -1.309
-max_tilt_angle : 0.4363
-min_tilt_angle : -0.2618
-max_speed_motor_head : 3.0
-head_friction : 1.0
-head_damping : 0.5
-head_cam_fov_h : 62.0
-head_cam_fov_v : 48.0
-base_cam_fov_h : 360.0
-has_pan : True
-has_tilt : False
diff --git a/modules/social_mpc.yaml b/modules/social_mpc.yaml
deleted file mode 100644
index c8c94a312b13bdbc9c111f17dc68b7f22f4f5a85..0000000000000000000000000000000000000000
--- a/modules/social_mpc.yaml
+++ /dev/null
@@ -1,49 +0,0 @@
-logging_mode: 'INFO' # INFO, WARNING, ERROR, CRITICAL DEBUG
-# mpc parameters
-h: 0.2
-fw_time: 5.
-u_lb: [-3.0, -0.5, -0.1]
-u_ub: [3.0, 0.5, 0.2]
-reg_parameter: [1., 10., 1.]
-max_acceleration: [0.4, 0.4, 0.3]
-max_iter_optim: 5
-# targets
-goto_target_dim: 8 # x, y, ang, v, w, loss, angle flag, flag
-human_target_dim: 8 # x, y, ang, v, w, loss, angle flag, flag
-pan_target_dim: 5 # x, y, v, w, flag
-target_pose_goto_dim: 5 # x, y, ang, local/global flag, flag
-target_pose_look_dim: 4 # x, y, local/global flag, flag
-goto_target_id: 14 # id
-goto_target_pose: [0., 0., 0., 0., 0.] # pose (x, y, ang),local 1./global 0. flag, flag
-human_target_id: -1
-pan_target_id: 13
-min_dist_to_robot: 0.85 # meter
-relative_h_pose: [0.90, -0.2] # meter
-check_object_fw_traj_offset: 0.4 # meter
-# mpc losses parameters
-loss: 0 # max_out_y_dist_ang, maxout_x_y_ang by default
-wall_avoidance_points: [[0., -0.3]] # [[0., 0.2], [0.15, 0.1], [-0.15, 0.1], [0., -0.3]] # [[0., -0.3]]
-weights_description: ['goto_weight', 'human_features_weight', 'object_cost_map_weight', 'social_cost_map_weight', 'cost_map_weight', 'wall_avoidance_weight']
-goto_weight: 1.
-human_features_weight: 1.
-object_cost_map_weight: 1000.
-social_cost_map_weight: 20.
-cost_map_weight: 1.
-wall_avoidance_weight: 100.
-loss_rad: [[0.2, 0.2, 0.1, 0.2, 0.2, 0.2, 0.1], [0.2, 0.2, 0.1, 0.2, 0.2, 0.2, 0.1], [0.2, 0.2, 0.1, 0.2, 0.2, 0.2, 0.1]] # [(x, y , ang) goto goal, (x, y, ang) human features, pan] x3 (for each mpc mode)
-loss_coef: [[50, 50, 1, 0, 0, 0, 3], [0, 0, 0, 50, 50, 3, 3], [10, 10, 1, 50, 50, 2, 3]] # [(x, y , ang) goto goal, (x, y, ang) human features, pan] x3 (for each mpc mode)
-# cost_map parameters
-step_meshes: 5 # sampling every x cm
-# ssn model parameters
-group_detection_stride: 0.6
-# mpc preprocessing parameters
-waypoint_distance: 1.0 #1.2
-orientation_distance_threshold: 0.5
-# controller parameters
-path_loop_time: 1.
-goal_loop_time: 1.
-path_planner_enabled: true
-goal_finder_enabled: true
-update_goals_enabled: true
-# map parameters
-dilation_iter: 1
\ No newline at end of file
diff --git a/mpi_sim/components/__init__.py b/mpi_sim/components/__init__.py
index 7b16621ec69a9cb57e8668fb407e13aa44e6d395..4359fe81f65bdebc1dfc1112c2323a471e07946a 100644
--- a/mpi_sim/components/__init__.py
+++ b/mpi_sim/components/__init__.py
@@ -3,9 +3,15 @@ try:
from mpi_sim.components.ari_navigation import ARINavigation
except ImportError:
pass
+# Uncomment to debug
+# from mpi_sim.components.ari_navigation import ARINavigation
+
from mpi_sim.components.gaze_generator import GazeGenerator
from mpi_sim.components.object_detector import ObjectDetector
-from mpi_sim.components.occupany_grid_mapping import OccupancyGridMapping
+from mpi_sim.components.occupancy_grid_mapping import OccupancyGridMapping
+from mpi_sim.components.social_mapping import SocialMapping
from mpi_sim.components.social_force_navigation import SocialForceNavigation
from mpi_sim.components.speech_generator import SpeechGenerator
+from mpi_sim.components.raycast_sensor import RayCastSensor
+from mpi_sim.components.point_drawer import PointDrawer
diff --git a/mpi_sim/components/ari_navigation.py b/mpi_sim/components/ari_navigation.py
index 6663b12cc42c9fea90cf6c60a757193605b7d987..e30a0ae07e7153d093dec4f5c5c3731cbc78544c 100644
--- a/mpi_sim/components/ari_navigation.py
+++ b/mpi_sim/components/ari_navigation.py
@@ -9,7 +9,8 @@ import yaml
from collections import namedtuple
from mpi_sim.utils.box2d_utils import constraint_angle
import cv2
-
+import os
+import exputils as eu
# TODO: need group identification for SocialMPC
@@ -69,6 +70,54 @@ class ARINavigation(GeneratorComponent):
dc.max_human_nb = 20
dc.max_groups_world = 10
+ dc.robot_config = mpi_sim.AttrDict()
+ dc.robot_config.base_footprint2base_link = [0., 0., 0.125]
+ dc.robot_config.base_link2wheel_left_link = [0., 0.220, 0.0]
+ dc.robot_config.base_link2wheel_right_link = [0., -0.220, 0.0]
+ dc.robot_config.base_link2caster_left_link = [-0.455, 0.102, -0.080]
+ dc.robot_config.base_link2caster_right_link = [-0.455, -0.102, -0.080]
+ dc.robot_config.base_link2head_1_link = [-0.226, 0.000, 1.192]
+ dc.robot_config.base_link2head_2_link = [-0.183, 0.000, 1.284]
+ dc.robot_config.base_link2head_front_camera_link = [-0.120, -0.000, 1.466]
+ dc.robot_config.base_link2head_front_camera_optical_frame = [-0.120, -0.000, 1.466]
+ dc.robot_config.base_footprint_radius= 0.3
+ dc.robot_config.wheel_radius = 0.125
+ dc.robot_config.wheel_width = 0.075
+ dc.robot_config.wheel_separation = 0.44
+ dc.robot_config.wheel_torque = 6.0
+ dc.robot_config.wheel_velocity = 1.0
+ dc.robot_config.caster_radius = 0.045
+ dc.robot_config.caster_width = 0.055
+ dc.robot_config.caster_separation_x = 0.91
+ dc.robot_config.caster_offset_y = 0.1025
+ dc.robot_config.caster_offset_z = -0.08
+ dc.robot_config.total_mass = 54.893
+ dc.robot_config.wheel_mass = 1.65651
+ dc.robot_config.L1 = [[0, 0, 1, 0], [1, 0, 0, 0.182], [0, 1, 0, -0.063], [0, 0, 0, 1]]
+ dc.robot_config.L2 = [[0, 0, -1, 0.093], [0, 1, 0, -0.043], [1, 0, 0, 0], [0, 0, 0, 1]]
+ dc.robot_config.L3 = [[1, 0, 0, 0], [0, 1, 0, 0.225], [0, 0, 1, -1.192], [0, 0, 0, 1]]
+ dc.robot_config.local_map_depth= 3.0
+ dc.robot_config.local_map_px_size= 0.05
+ dc.robot_config.max_speed_motor_wheel = 1.0
+ dc.robot_config.max_forward_speed= 0.5
+ dc.robot_config.max_backward_speed= -0.1
+ dc.robot_config.max_forward_acc= 0.2
+ dc.robot_config.max_ang_vel= 0.5
+ dc.robot_config.max_ang_acc= 0.4
+ dc.robot_config.max_pan_angle = 1.309
+ dc.robot_config.min_pan_angle = -1.309
+ dc.robot_config.max_tilt_angle = 0.4363
+ dc.robot_config.min_tilt_angle = -0.2618
+ dc.robot_config.max_speed_motor_head = 3.0
+ dc.robot_config.head_friction = 1.0
+ dc.robot_config.head_damping = 0.5
+ dc.robot_config.head_cam_fov_h = 62.0
+ dc.robot_config.head_cam_fov_v = 48.0
+ dc.robot_config.base_cam_fov_h = 360.0
+ dc.robot_config.has_pan = True
+ dc.robot_config.has_tilt = False
+
+
return dc
@@ -99,13 +148,15 @@ class ARINavigation(GeneratorComponent):
# TODO: remove the need for configuration in yml files
# add the configuration in the default config under a social_mpc and robot sub dictionary
- config_filename = pkg_resources.resource_filename('modules', 'social_mpc.yaml')
- self.mpc_controller = RobotController(config_filename=config_filename)
-
+
+ # config_filename = pkg_resources.resource_filename('modules', '/local_scratch/victor/Documents/RobotLearn/Projects/drl_robot_navigation/drl-navigation-training/code/multiparty_interaction_simulator/modules/social_mpc.yaml')
+ self.mpc_controller = RobotController()
+
# get the robot config
- robot_config_filename = pkg_resources.resource_filename('modules', 'robot.yaml')
- config = yaml.load(open(robot_config_filename), Loader=yaml.FullLoader)
- self.robot_config = RobotConfig(config)
+ # robot_config_filename = pkg_resources.resource_filename('modules', 'robot.yaml')
+ # config = yaml.load(open(robot_config_filename), Loader=yaml.FullLoader)
+ config_dict = eu.AttrDict.from_yaml('/local_scratch/victor/Documents/RobotLearn/Projects/drl_robot_navigation/drl-navigation-training/code/multiparty_interaction_simulator/modules/Motor_Controller/social_mpc/config/robot.yaml').toDict()
+ self.robot_config = RobotConfig(dict=config_dict)
# initialize the robot state
self.state.robot_state = RobotState()
@@ -387,7 +438,7 @@ class ARINavigation(GeneratorComponent):
x_robot = self.object.position[0]
y_robot = self.object.position[1]
yaw_robot = self.object.orientation
- yaw_robot_pan = yaw_robot - self.object.joints[-1].angle
+ yaw_robot_pan = yaw_robot - self.object.joints[-1].bodyA.angle
x_final = self.mpc_controller.shared_target[2]
y_final = self.mpc_controller.shared_target[3]
th_final = self.mpc_controller.shared_target[4]
@@ -433,8 +484,8 @@ class ARINavigation(GeneratorComponent):
v = np.array([-np.sin(angle), np.cos(angle)])
self.state.robot_state.robot_velocity[0] = np.dot(np.asarray(self.object.position_velocity), v)
self.state.robot_state.robot_velocity[1] = self.object.orientation_velocity
- self.state.robot_state.joint_angles[:] = constraint_angle(-self.object.joints[-1].angle)
- self.state.robot_state.joint_velocities[:] = -self.object.joints[-1].speed
+ self.state.robot_state.joint_angles[:] = constraint_angle(-self.object.joints[-1].bodyA.angle)
+ # self.state.robot_state.joint_velocities[:] = -np.array(self.object.joints[-1].bodyA.linearVelocity)
kernel = cv2.getStructuringElement(
cv2.MORPH_ELLIPSE,
(2, 2)
diff --git a/mpi_sim/components/occupancy_grid_mapping.py b/mpi_sim/components/occupancy_grid_mapping.py
new file mode 100644
index 0000000000000000000000000000000000000000..4b1620d78d4a1868c1786f5f9b5fb9c1c9286a5b
--- /dev/null
+++ b/mpi_sim/components/occupancy_grid_mapping.py
@@ -0,0 +1,249 @@
+import mpi_sim
+from mpi_sim.core.sensor_component import SensorComponent
+import numpy as np
+from mpi_sim.utils.box2d_utils import constraint_angle
+import cv2
+
+# TODO: if there are several agents that do mapping, use a common mapping process to generate the global map to save resources
+
+
+class OccupancyGridMapping(SensorComponent):
+ r"""Component that creates an occupancy grid map centered and oriented around its object, for example a robot agent."""
+
+ @staticmethod
+ def default_config():
+ dc = SensorComponent.default_config()
+ dc.name = 'occupancy_grid_mapping'
+ dc.area = None # area of global map ((x_min, x_max), (y_min, y_max)), if none it is simulation.visible_area
+ dc.depth = 5.0 # depth if local in meters
+ dc.object_filter = None # filter for the elements that should be mapped
+ dc.resolution = 0.05 # resolution of the maps in m per cell
+ dc.update_global_map_automatically = False # should the global map be automatically updated each step
+ dc.transform = None # transform functions of the global map
+ dc.update_local_map_automatically = False # update local map each time the smulation is step
+ dc.imported_global_map_path = False # Import Global map - path
+ dc.imported_global_map_resolution = False # Import Global map - resolution
+ dc.imported_global_map_area = False # Import Global map - area (can either be a tuple of a function is the area is not squared)
+ dc.imported_global_map_px_coord_center = None
+ dc.transform_position_to_map_coordinate = None
+ dc.transform_real_to_sim_orientation = None
+ dc.position_noise = 0
+ dc.orientation_noise = 0
+ return dc
+
+
+ @property
+ def resolution(self):
+ r"""Get the tesolution of the maps in m per cell."""
+ return self._resolution
+
+
+ @property
+ def global_map(self):
+ r"""Get the global occupancy grid map from which the local map is generated."""
+ if self.state.global_map is None:
+ return None
+ else:
+ return self.update_global_map()
+
+
+ @property
+ def global_map_area(self):
+ r"""The area that is spanned by the global map defined as ((x_min, x_max), (y_min, y_max))"""
+ return self._global_map_area
+
+
+ @property
+ def global_map_shape(self):
+ r"""The shape of the global map defined as (width, height)."""
+ return self._global_map_shape
+
+
+ @property
+ def local_map(self):
+ r"""Get the local occupancy grid map that is centered and oriented around the object of the component."""
+ if self.state.local_map is None:
+ return None
+ else:
+ return self.update_local_map()
+
+ @property
+ def local_map_noisy(self):
+ r"""Get the noisy version of the local occupancy grid map that is centered and oriented around the object of the component."""
+ if self.state.local_map_noisy is None:
+ return None
+ else:
+ return self.update_noisy_local_map()
+
+
+ @property
+ def local_map_area(self):
+ r"""The area that is spanned by the local map defined as ((x_min, x_max), (y_min, y_max)) in reference frame of the local map."""
+ return self._local_map_area
+
+
+ @property
+ def local_map_shape(self):
+ r"""The shape of the local map defined as (width, height)."""
+ return self._local_map_shape
+
+
+ def set_global_map(self, global_map):
+ self.state.global_map = mpi_sim.utils.OccupancyGridMap(
+ map=global_map,
+ area=self._global_map_area,
+ resolution=self._resolution
+ )
+
+ def __init__(self, config=None, **kwargs):
+ super().__init__(config=config, **kwargs)
+
+
+
+ if not self.config.imported_global_map_path :
+ self._resolution = self.config.resolution
+ self._global_map_area = self.config.area
+
+ self._global_map_shape = None
+ self.state.global_map = None
+
+ # is the global map initially created, if not do it before creating the local map
+ self.state.is_global_map_initialized = False
+
+ # initialize the global map if possible
+ if self._global_map_area is not None:
+ self._global_map_shape = mpi_sim.utils.get_occupancy_grid_map_shape(area=self._global_map_area, resolution=self._resolution)
+ self.state.global_map = mpi_sim.utils.OccupancyGridMap(
+ map=np.zeros(self._global_map_shape),
+ area=self._global_map_area,
+ resolution=self._resolution
+ )
+ else :
+ self._resolution = self.config.imported_global_map_resolution
+ self._global_map_area = self.config.imported_global_map_area
+ self.state.is_global_map_initialized = True #The global map is initialized and doesn't need to be updated
+
+ if isinstance(self.config.imported_global_map_path, str):
+
+ self.imported_global_map = np.load(file=self.config.imported_global_map_path)
+
+ self.state.global_map = mpi_sim.utils.OccupancyGridMap(
+ map=self.imported_global_map,
+ area=self._global_map_area,
+ resolution=self._resolution
+ )
+
+
+ else :
+ raise UserWarning('The parameter \'imported_global_map_path\' should be a type str but it is {}'.format(type(self.config.imported_global_map_path)))
+
+ self._local_map_area = ((-self.config.depth, self.config.depth), (-self.config.depth, self.config.depth))
+ local_map_length = int((self.config.depth / self._resolution) * 2)
+ self._local_map_shape = (local_map_length, local_map_length)
+ self.state.local_map = mpi_sim.utils.OccupancyGridMap(
+ map=np.zeros(self._local_map_shape),
+ area=None,
+ resolution=self._resolution
+ )
+
+ self.state.local_map_noisy = mpi_sim.utils.OccupancyGridMap(
+ map=np.zeros(self._local_map_shape),
+ area=None,
+ resolution=self._resolution
+ )
+
+
+ def _add_to_simulation(self):
+
+ # if the global map area should span the whole simulation (config.area=None), then read visible_area from the simulation when added to it
+ if self._global_map_area is None:
+ self._global_map_area = self.simulation.visible_area
+ self._global_map_shape = mpi_sim.utils.get_occupancy_grid_map_shape(area=self._global_map_area, resolution=self._resolution)
+ self.state.global_map = mpi_sim.utils.OccupancyGridMap(
+ map=np.zeros(self._global_map_shape),
+ area=self._global_map_area,
+ resolution=self._resolution
+ )
+
+
+ def update_global_map(self):
+ if not self.config.imported_global_map_path :
+ self.state.global_map = mpi_sim.utils.create_occupancy_grid_map(
+ self.simulation,
+ area=self._global_map_area,
+ resolution=self._resolution,
+ object_filter=self.config.object_filter,
+ transform=self.config.transform
+ )
+ else :
+
+ #imported_global_map = cv2.circle(imported_global_map*255, center = [75, 65], radius = 7, color = (255, 255, 255), thickness=-1)/255
+ if self.config.transform:
+ self.imported_global_map=self.config.transform(np.load(file=self.config.imported_global_map_path))
+ self.state.global_map = mpi_sim.utils.OccupancyGridMap(
+ map=self.imported_global_map,
+ area=self._global_map_area,
+ resolution=self._resolution
+ )
+
+
+ return self.state.global_map.map
+
+
+ def update_local_map(self):
+ if self.state.global_map is None:
+ self.update_global_map()
+ if not self.config.imported_global_map_path :
+
+ self.state.local_map = mpi_sim.utils.create_local_perspective_occupancy_grid_map(
+ global_map=self.state.global_map,
+ position=self.object.position,
+ orientation=constraint_angle(self.object.orientation),
+ depth=self.config.depth,
+ )
+ else :
+ # Same but we need to relocate the position from the center of the imported room
+ self.state.local_map = mpi_sim.utils.create_local_perspective_occupancy_grid_map(
+ global_map=self.state.global_map,
+ # find a way to explain this
+ position=self.object.position,
+ orientation=constraint_angle(self.config.transform_real_to_sim_orientation(self.object.orientation)),
+ depth=self.config.depth,
+ from_given_center = self.config.imported_global_map_px_coord_center,
+ transform_position_to_map_coordinate = self.config.transform_position_to_map_coordinate
+ )
+
+ return self.state.local_map.map
+
+ def update_noisy_local_map(self):
+ if self.state.global_map is None:
+ self.update_global_map()
+ if not self.config.imported_global_map_path :
+
+ self.state.local_map_noisy = mpi_sim.utils.create_local_perspective_occupancy_grid_map(
+ global_map=self.state.global_map,
+ position=self.object.position + self.config.position_noise*np.array([np.random.uniform(low=-1, high=1),np.random.uniform(low=-1, high=1)]),
+ orientation=constraint_angle(self.object.orientation)+ self.config.orientation_noise*np.random.uniform(low=-1, high=1),
+ depth=self.config.depth,
+ )
+ else :
+ # Same but we need to relocate the position from the center of the imported room
+ self.state.local_map_noisy = mpi_sim.utils.create_local_perspective_occupancy_grid_map(
+ global_map=self.state.global_map,
+ position=self.object.position + self.config.position_noise*np.array([np.random.uniform(low=-1, high=1),np.random.uniform(low=-1, high=1)]),
+ orientation=constraint_angle(self.config.transform_real_to_sim_orientation(self.object.orientation)) + self.config.orientation_noise*np.random.uniform(low=-1, high=1),
+ depth=self.config.depth,
+ from_given_center = self.config.imported_global_map_px_coord_center,
+ transform_position_to_map_coordinate = self.config.transform_position_to_map_coordinate
+ )
+
+ return self.state.local_map_noisy.map
+
+
+ def _step(self):
+ if self.config.update_global_map_automatically or not self.state.is_global_map_initialized:
+ self.update_global_map()
+ self.state.is_global_map_initialized = True
+ if self.config.update_local_map_automatically :
+ self.update_local_map()
+ self.update_noisy_local_map()
diff --git a/mpi_sim/components/occupany_grid_mapping.py b/mpi_sim/components/occupany_grid_mapping.py
deleted file mode 100644
index 6ec48ccca723ef59a0b9323a4bffddbf6687c806..0000000000000000000000000000000000000000
--- a/mpi_sim/components/occupany_grid_mapping.py
+++ /dev/null
@@ -1,151 +0,0 @@
-import mpi_sim
-import mpi_sim.processes
-from mpi_sim.core.sensor_component import SensorComponent
-import numpy as np
-from mpi_sim.utils.box2d_utils import constraint_angle
-import cv2
-
-
-# TODO: if there are several agents that do mapping, use a common mapping process to generate the global map to save resources
-
-
-class OccupancyGridMapping(SensorComponent):
- r"""Component that creates an occupancy grid map centered and oriented around its object, for example a robot agent."""
-
- @staticmethod
- def default_config():
- dc = SensorComponent.default_config()
- dc.name = 'occupancy_grid_mapping'
- dc.area = None # area of global map ((x_min, x_max), (y_min, y_max)), if none it is simulation.visible_area
- dc.depth = 5.0 # depth if local in meters
- dc.object_filter = None # filter for the elements that should be mapped
- dc.resolution = 0.05 # resolution of the maps in m per cell
- dc.update_global_map_automatically = True # should the global map be automatically updated each step
- return dc
-
-
- @property
- def resolution(self):
- r"""Get the tesolution of the maps in m per cell."""
- return self._resolution
-
-
- @property
- def global_map(self):
- r"""Get the global occupancy grid map from which the local map is generated."""
- if self.state.global_map is None:
- return None
- else:
- return self.state.global_map.map
-
-
- @property
- def global_map_area(self):
- r"""The area that is spanned by the global map defined as ((x_min, x_max), (y_min, y_max))"""
- return self._global_map_area
-
-
- @property
- def global_map_shape(self):
- r"""The shape of the global map defined as (width, height)."""
- return self._global_map_shape
-
-
- @property
- def local_map(self):
- r"""Get the local occupancy grid map that is centered and oriented around the object of the component."""
- if self.state.local_map is None:
- return None
- else:
- return self.state.local_map.map
-
-
- @property
- def local_map_area(self):
- r"""The area that is spanned by the local map defined as ((x_min, x_max), (y_min, y_max)) in reference frame of the local map."""
- return self._local_map_area
-
-
- @property
- def local_map_shape(self):
- r"""The shape of the local map defined as (width, height)."""
- return self._local_map_shape
-
-
- def __init__(self, config=None, **kwargs):
- super().__init__(config=config, **kwargs)
-
- self._resolution = self.config.resolution
-
- self._global_map_area = self.config.area
- self._global_map_shape = None
- self.state.global_map = None
-
- # is the global map initially created, if not do it before creating the local map
- self.state.is_global_map_initialized = False
-
- # initialize the global map if possible
- if self._global_map_area is not None:
- self._global_map_shape = mpi_sim.utils.get_occupancy_grid_map_shape(area=self._global_map_area, resolution=self._resolution)
- self.state.global_map = mpi_sim.utils.OccupancyGridMap(
- map=np.zeros(self._global_map_shape),
- area=self._global_map_area,
- resolution=self._resolution
- )
-
- self._local_map_area = ((-self.config.depth, self.config.depth), (-self.config.depth, self.config.depth))
- local_map_length = int((self.config.depth / self._resolution) * 2)
- self._local_map_shape = (local_map_length, local_map_length)
- self.state.local_map = mpi_sim.utils.OccupancyGridMap(
- map=np.zeros(self._local_map_shape),
- area=None,
- resolution=self._resolution
- )
-
-
- def _add_to_simulation(self):
-
- # if the global map area should span the whole simulation (config.area=None), then read visible_area from the simulation when added to it
- if self._global_map_area is None:
- self._global_map_area = self.simulation.visible_area
- self._global_map_shape = mpi_sim.utils.get_occupancy_grid_map_shape(area=self._global_map_area, resolution=self._resolution)
- self.state.global_map = mpi_sim.utils.OccupancyGridMap(
- map=np.zeros(self._global_map_shape),
- area=self._global_map_area,
- resolution=self._resolution
- )
-
-
- def update_global_map(self):
- self.state.global_map = mpi_sim.utils.create_occupancy_grid_map(
- self.simulation,
- area=self._global_map_area,
- resolution=self._resolution,
- object_filter=self.config.object_filter
- )
-
- return self.state.global_map.map
-
-
- def update_local_map(self):
-
- if self.state.global_map is None:
- self.update_global_map()
-
- self.state.local_map = mpi_sim.utils.create_local_perspective_occupancy_grid_map(
- global_map=self.state.global_map,
- position=self.object.position,
- orientation=constraint_angle(self.object.orientation),
- depth=self.config.depth,
- )
-
- return self.state.local_map.map
-
-
- def _step(self):
-
- if self.config.update_global_map_automatically or not self.state.is_global_map_initialized:
- self.update_global_map()
- self.state.is_global_map_initialized = True
-
- self.update_local_map()
diff --git a/mpi_sim/components/point_drawer.py b/mpi_sim/components/point_drawer.py
new file mode 100644
index 0000000000000000000000000000000000000000..d04f8ba7117aa08bb2e50b6b6cff1cac2e971207
--- /dev/null
+++ b/mpi_sim/components/point_drawer.py
@@ -0,0 +1,49 @@
+## Software gym_box2d_diff_drive
+##
+## Derived from pybox2d distributed under zlib License (c), 2008-2011 Ken Lauer
+## sirkne at gmail dot com
+##
+## This file, corresponding to gym_box2d_diff_drive module of WP6, is part of a
+## project that has received funding from the European Union’s Horizon 2020
+## research and innovation programme under grant agreement No 871245.
+##
+## Copyright (C) 2022 by INRIA
+## Authors : Chris Reinke, Alex Auternaud, Victor Sanchez
+## alex dot auternaud at inria dot fr
+## victor dot sanchez at inria dot fr
+
+from mpi_sim.core.component import Component
+import mpi_sim as sim
+from Box2D import (b2Color, b2Vec2)
+import pygame
+
+class PointDrawer(Component):
+
+ @staticmethod
+ def default_config():
+ dc = sim.SensorComponent.default_config()
+ dc.name = 'point_drawer'
+ return dc
+
+
+ def add_point_to_draw(self, point):
+ self.list_of_point_to_draw.append(point)
+
+ @property
+ def clear_list_of_point(self):
+ self.list_of_point_to_draw.clear()
+
+ def __init__(self, config=None, **kwargs):
+ super().__init__(config=config, **kwargs)
+ self.list_of_point_to_draw =[]
+
+ def _step(self):
+ pass
+ # for point in self.list_of_point_to_draw :
+ # self.simulation.box2d_world.renderer.DrawPoint(b2Vec2(point[0],point[1]), 100.0, b2Color(1, 1, 1))
+ # pygame.display.flip()
+
+
+
+
+
diff --git a/mpi_sim/components/raycast.py b/mpi_sim/components/raycast.py
index d52379b548e376ccddacbb8d46bfeaeb9bcb9b86..d87026f8490a3d8194b7b1040cd9923e4d31ba6b 100644
--- a/mpi_sim/components/raycast.py
+++ b/mpi_sim/components/raycast.py
@@ -43,7 +43,7 @@ class RayCastClosestCallback(b2RayCastCallback):
returning 1, you continue with the original ray clipping. By returning
-1, you will filter out the current fixture (the ray will not hit it).
"""
- if fixture.body.userData['name'] not in self.wrong_fix:
+ if fixture.body.userData['name'] not in self.wrong_fix or not isinstance(fixture.body.userData['obj'], sim.objects.ari_robot.ARIRobot):
self.hit = True
self.fixture = fixture
self.point = b2Vec2(point)
diff --git a/mpi_sim/components/raycast_sensor.py b/mpi_sim/components/raycast_sensor.py
new file mode 100644
index 0000000000000000000000000000000000000000..0397a280ba07beddb8ee1f4bbb3aeccbd2a6a7a5
--- /dev/null
+++ b/mpi_sim/components/raycast_sensor.py
@@ -0,0 +1,230 @@
+## Software gym_box2d_diff_drive
+##
+## Derived from pybox2d distributed under zlib License (c), 2008-2011 Ken Lauer
+## sirkne at gmail dot com
+##
+## This file, corresponding to gym_box2d_diff_drive module of WP6, is part of a
+## project that has received funding from the European Union’s Horizon 2020
+## research and innovation programme under grant agreement No 871245.
+##
+## Copyright (C) 2022 by INRIA
+## Authors : Chris Reinke, Alex Auternaud, Victor Sanchez
+## alex dot auternaud at inria dot fr
+## victor dot sanchez at inria dot fr
+
+import pygame
+import numpy as np
+from mpi_sim.core.sensor_component import SensorComponent
+import mpi_sim as sim
+from Box2D import (b2Color, b2Vec2, b2RayCastCallback)
+# TODO: rename this component to something like ObjectDetector which can be configured to say which object types should be detected
+
+
+class RayCastClosestCallback(b2RayCastCallback):
+ """This callback finds the closest hit"""
+
+ def __init__(self, wrong_fix, **kwargs):
+ b2RayCastCallback.__init__(self, **kwargs)
+ self.wrong_fix = wrong_fix
+ self.fixture = None
+ self.point = None
+ self.normal = None
+ self.hit = False
+
+
+ def ReportFixture(self, fixture, point, normal, fraction):
+ """
+ Called for each fixture found in the query. You control how the ray
+ proceeds by returning a float that indicates the fractional length of
+ the ray. By returning 0, you set the ray length to zero. By returning
+ the current fraction, you proceed to find the closest point. By
+ returning 1, you continue with the original ray clipping. By returning
+ -1, you will filter out the current fixture (the ray will not hit it).
+ """
+
+ if fixture.body.userData['name'] not in self.wrong_fix :
+ self.hit = True
+ self.fixture = fixture
+ self.point = b2Vec2(point)
+ self.normal = b2Vec2(normal)
+
+ return fraction
+
+class RayCastSensor(SensorComponent):
+
+ @staticmethod
+ def default_config():
+ dc = sim.SensorComponent.default_config()
+ dc.name = 'raycast_sensor'
+ dc.field_of_view = 360 # Range of the lidar in degree
+ dc.delta_angle = 2. # Angle between two beams of lidar in degree
+ dc.ray_max_range = 20 # Max distance readable by a given beam
+ dc.noise_ratio = 0
+ dc.draw_raycast = False
+ dc.update_raycast_every_step = False
+ dc.max_raycast_shape = 'square' # can either be a 'circle' or a 'square'
+ return dc
+
+ @property
+ def raycast_distances(self):
+ if self.update_raycast_every_step :
+ return np.round(self.ray_distance_list, 4)
+ else :
+ return self.update_raycast()
+
+ @property
+ def raycast_angles(self):
+ if self.update_raycast_every_step :
+ return self.ray_angle_list
+ else :
+ self.update_raycast()
+ return self.ray_angle_list
+
+ @property
+ def raycast_angles_from_agent_view(self):
+ if self.update_raycast_every_step :
+ return sim.utils.constraint_angle(np.round(self.ray_angle_list-np.degrees(self.orientation),2), min_value=self.min_value_angle, max_value=self.max_value_angle)
+ else :
+ self.update_raycast()
+ return sim.utils.constraint_angle(np.round(self.ray_angle_list-np.degrees(self.orientation),2), min_value=self.min_value_angle, max_value=self.max_value_angle)
+
+
+ def __init__(self, config=None, **kwargs):
+ super().__init__(config=config, **kwargs)
+ self.wrong_fix = ['left wheel', 'right wheel', 'pan']
+ self.fov = self.config.field_of_view
+ self.delta_angle = self.config.delta_angle
+ self.min_angle_fov = None
+ self.max_angle_fov = None
+ self.ray_distance_list = None # List containing the raycast distance
+ self.ray_angle_list = None # List containing the angle of each ray
+ self.coord_points_max_range_rays = None
+ self.position = None
+ self.orientation = None
+ self.coord_point_rays = None
+ self.draw = self.config.draw_raycast
+ self.check_done = False
+ self.max_value_angle = 180
+ self.min_value_angle = -180
+ self.update_raycast_every_step = self.config.update_raycast_every_step
+
+
+ def _polar_to_cartesian(self, distance, angle):
+ return distance*np.cos(angle), distance*np.sin(angle)
+
+ def _update_coord_max_range_rays(self):
+ r''' Update the cartesian coordinates of the max range rays
+
+ Note : It takes into account the orientation and position of the object '''
+
+ self.coord_points_max_range_rays = []
+ if self.config.max_raycast_shape == 'circle':
+ for angle in self.ray_angle_list :
+ self.coord_points_max_range_rays.append(self._polar_to_cartesian(distance=self.config.ray_max_range, angle=np.radians(angle) + np.pi/2))
+ elif self.config.max_raycast_shape == 'square':
+ for angle in self.ray_angle_list :
+ self.coord_points_max_range_rays.append(self._polar_to_cartesian(distance=np.sqrt(2)*self.config.ray_max_range, angle=np.radians(angle) + np.pi/2))
+ self.coord_points_max_range_rays = np.clip(self.coord_points_max_range_rays, -self.config.ray_max_range, +self.config.ray_max_range)
+ self.coord_points_max_range_rays = np.array(self.coord_points_max_range_rays) + np.array(self.position)
+
+ def _coord_around_object(self, position, angle, radius_object = 0.3):
+ r''' Computes the coord of a point around object given a certain angle
+
+ Note : This function exists because the ARIRobot was overlapping with the fixture detection
+ To avoid the phenomenon we measure the fixture between a circle around the object and the max range '''
+ return position + radius_object*np.array([np.cos(angle), np.sin(angle)])
+
+ def update_raycast(self):
+ self.position = self.object.position # cartesian
+ self.orientation = self.object.orientation # in radians
+
+ # Update the field of view range w.r.t to the robot's orientation
+ self.min_angle_fov = np.round(np.degrees(self.orientation) + self.fov/2, 3)
+ self.max_angle_fov = np.round(np.degrees(self.orientation) - self.fov/2, 3)
+
+ # Update the angle of the point cloud and constraint it around -180 and 180 degrees
+ self.ray_angle_list = np.linspace(self.min_angle_fov, self.max_angle_fov, int(self.fov/self.config.delta_angle) + 1)
+
+ # Restriction and modulo in the range -180/180
+ length = self.max_value_angle - self.min_value_angle
+ self.ray_angle_list = np.where(self.ray_angle_list > self.max_value_angle, self.min_value_angle + ((self.ray_angle_list - self.max_value_angle) % length), self.ray_angle_list)
+ self.ray_angle_list = np.where(self.ray_angle_list < self.min_value_angle, self.max_value_angle - ((self.min_value_angle - self.ray_angle_list) % length), self.ray_angle_list)
+ ray_angle_list_from_agent_viewpoint = np.round(self.ray_angle_list-np.degrees(self.orientation),3)
+
+ # Update the variable self.coord_points_max_range_rays
+ self._update_coord_max_range_rays()
+
+ # Update the raycast points whether there are obstacle between the object and the max range
+ self.ray_distance_list = []
+ self.coord_point_rays = []
+ # Measurement Noise
+ noise = np.random.normal(0,1,size=2)*self.config.noise_ratio
+
+
+
+ for id, point_ray in enumerate(self.coord_points_max_range_rays):
+ callback = RayCastClosestCallback(wrong_fix=self.wrong_fix)
+ # Origin of rays around the object
+ position = self._coord_around_object(self.position, angle=np.radians(self.ray_angle_list[id]))
+ self.simulation.box2d_world.RayCast(callback, point1=position, point2=point_ray)
+
+ if callback.hit and callback.fixture.body.userData['name'] != self.object.box2d_body.userData['name']:
+ self.ray_distance_list.append(sim.utils.measure_center_distance(self.position, np.array(callback.point)+ noise))
+ self.coord_point_rays.append(np.array(callback.point)+ noise)
+ if self.draw :
+ self.point11 = self.simulation.box2d_world.renderer.to_screen(self.position)
+ if not ray_angle_list_from_agent_viewpoint[id]:
+ color = b2Color(1, 0, 0)
+ else :
+ color = b2Color(0.8, 0, 0.8)
+ self.draw_hit(
+ cb_point=np.array(callback.point)+ noise,
+ cb_normal=callback.normal,
+ point1=self.point11,
+ color=color
+ )
+ else:
+ self.ray_distance_list.append(sim.utils.measure_center_distance(self.position, np.array(point_ray)+noise))
+ self.coord_point_rays.append(np.array(point_ray)+noise)
+ if self.draw :
+ if not ray_angle_list_from_agent_viewpoint[id]:
+ color = b2Color(0.9, 0, 0)
+ else :
+ color = b2Color(0.8, 0, 0.8)
+ self.point11 = self.simulation.box2d_world.renderer.to_screen(self.position)
+ self.point22 = self.simulation.box2d_world.renderer.to_screen(np.array(point_ray)+noise)
+ self.simulation.box2d_world.renderer.DrawSegment(self.point11, self.point22, color)
+ self.simulation.box2d_world.renderer.DrawPoint(self.point22, 5.0, color)
+
+ if self.draw :
+ pygame.display.flip()
+
+ return np.round(self.ray_distance_list, 4)
+
+
+ def _check(self):
+ if not self.fov :
+ self.compute_raycast = False
+ if not self.delta_angle :
+ raise Warning('The \'delta_angle\' variable is has been set to {}. Please change that parameter to a non zero value'.format(self.delta_angle))
+
+ def _step(self):
+ if not self.check_done :
+ self._check()
+ self.check_done = True
+ if self.update_raycast_every_step :
+ self.update_raycast()
+
+
+ def draw_hit(self, cb_point, cb_normal, point1, color):
+ cb_point = self.simulation.box2d_world.renderer.to_screen(cb_point)
+ head = b2Vec2(cb_point) + 0.5 * cb_normal
+
+ self.simulation.box2d_world.renderer.DrawPoint(cb_point, 5.0, color)
+ self.simulation.box2d_world.renderer.DrawSegment(point1, cb_point, color)
+ self.simulation.box2d_world.renderer.DrawSegment(cb_point, head, color)
+
+
+
+
+
diff --git a/mpi_sim/components/social_force_navigation.py b/mpi_sim/components/social_force_navigation.py
index ee22ccfbf091c422a90974531184cc0605ef0d4c..e034e11cbcbaec7d34acb778d547dbc8f90355b6 100644
--- a/mpi_sim/components/social_force_navigation.py
+++ b/mpi_sim/components/social_force_navigation.py
@@ -13,7 +13,8 @@ class SocialForceNavigation(GeneratorComponent):
dc.max_movement_force = 0.25
dc.max_rotation_force = 6.
dc.goal_distance_threshold = 0.2
- dc.step_length_coeff = 1.0
+ dc.step_length_coeff = 1
+ dc.avoid_ari = True
return dc
###############################################################################
@@ -62,7 +63,6 @@ class SocialForceNavigation(GeneratorComponent):
# print('stats : ', self.box2d_body.inertia, self.box2d_body.awake, self.box2d_body.active, self.box2d_body.transform)
# print(self.box2d_body.localCenter, self.box2d_body.worldCenter, self.box2d_body.linearVelocity, self.box2d_body.angularVelocity)
-
# if goal position is given use it to update the human
if self.state.goal_position is not None:
@@ -92,7 +92,6 @@ class SocialForceNavigation(GeneratorComponent):
next_orientation = self.object.orientation + f_orientation * self.config.step_length_coeff * self.simulation.step_length
self.object.orientation = next_orientation
-
# TODO: include manual control if wanted
# if self.state.goal_position is not None or self.state.goal_orientation is not None:
@@ -113,7 +112,10 @@ class SocialForceNavigation(GeneratorComponent):
f_repulsion = position * 0.0
# identify other agents inside the personal distance of the agent
- agents = self.simulation.get_nearby_objects(self.object, self.config.personal_distance, (mpi_sim.objects.Human, mpi_sim.objects.ARIRobot))
+ if self.config.avoid_ari :
+ agents = self.simulation.get_nearby_objects(self.object, self.config.personal_distance, (mpi_sim.objects.Human, mpi_sim.objects.ARIRobot))
+ else :
+ agents = self.simulation.get_nearby_objects(self.object, self.config.personal_distance, mpi_sim.objects.Human)
if agents:
other_agents_pos = np.array([agent.position for agent in agents]) # [n_agent * 2 (x and y pos)] array
@@ -137,7 +139,7 @@ class SocialForceNavigation(GeneratorComponent):
goal_direction = self.goal_position - position
goal_distance = np.linalg.norm(goal_direction)
-
+ np.seterr(invalid='ignore')
f_goal = min(1.0, goal_distance / self.config.goal_distance_threshold) * goal_direction/goal_distance
return f_goal
diff --git a/mpi_sim/components/social_mapping.py b/mpi_sim/components/social_mapping.py
new file mode 100644
index 0000000000000000000000000000000000000000..27cce60b7990549f2a6e1b7c4d33da19948e6dba
--- /dev/null
+++ b/mpi_sim/components/social_mapping.py
@@ -0,0 +1,213 @@
+import mpi_sim
+from mpi_sim.core.sensor_component import SensorComponent
+import numpy as np
+from mpi_sim.utils.box2d_utils import constraint_angle
+
+
+import cv2
+
+# TODO: if there are several agents that do mapping, use a common mapping process to generate the global map to save resources
+
+
+class SocialMapping(SensorComponent):
+ r"""Component that creates a social map centered and oriented around its object, for example a robot agent."""
+
+ @staticmethod
+ def default_config():
+ dc = SensorComponent.default_config()
+ dc.name = 'social_mapping'
+ dc.area = None # area of global map ((x_min, x_max), (y_min, y_max)), if none it is simulation.visible_area
+ dc.depth = 5.0 # depth if local in meters
+ dc.object_filter = None # filter for the elements that should be mapped
+ dc.resolution = 0.05 # resolution of the maps in m per cell
+ dc.update_global_map_automatically = True # should the global map be automatically updated each step
+ dc.update_local_map_automatically = False # update local map each time the smulation is step
+ dc.imported_global_map_path = False # Import Global map - path
+ dc.imported_global_map_resolution = False # Import Global map - resolution
+ dc.imported_global_map_area = False # Import Global map - area (can either be a tuple of a function is the area is not squared)
+ dc.imported_global_map_px_coord_center = None
+ dc.transform_position_to_map_coordinate = None
+ dc.transform_real_to_sim_orientation = None
+ return dc
+
+
+ @property
+ def resolution(self):
+ r"""Get the tesolution of the maps in m per cell."""
+ return self._resolution
+
+
+ @property
+ def global_map(self):
+ r"""Get the global social map from which the local map is generated."""
+ if self.state.global_map is None:
+ return None
+ else:
+ return self.update_global_map()
+
+
+ @property
+ def global_map_area(self):
+ r"""The area that is spanned by the global map defined as ((x_min, x_max), (y_min, y_max))"""
+ return self._global_map_area
+
+
+ @property
+ def global_map_shape(self):
+ r"""The shape of the global map defined as (width, height)."""
+ return self._global_map_shape
+
+
+ @property
+ def local_map(self):
+ r"""Get the local social map that is centered and oriented around the object of the component."""
+ if self.state.local_map is None:
+ return None
+ else:
+ return self.update_local_map()
+
+
+ @property
+ def local_map_area(self):
+ r"""The area that is spanned by the local map defined as ((x_min, x_max), (y_min, y_max)) in reference frame of the local map."""
+ return self._local_map_area
+
+
+ @property
+ def local_map_shape(self):
+ r"""The shape of the local map defined as (width, height)."""
+ return self._local_map_shape
+
+ def set_human_velocities(self, human_velocities):
+ r""" Set the velocities of the human """
+ # Necessary when the humans are navigating with the navigation component
+ self.human_velocities= human_velocities
+
+
+ def set_global_map(self, global_map):
+ self.state.global_map = mpi_sim.utils.SocialMap(
+ map=global_map,
+ area=self._global_map_area,
+ resolution=self._resolution
+ )
+
+ def __init__(self, config=None, **kwargs):
+ super().__init__(config=config, **kwargs)
+ self.state.global_map = None
+
+ if not self.config.imported_global_map_path :
+ self._resolution = self.config.resolution
+ self._global_map_area = self.config.area
+
+ self._global_map_shape = None
+
+ # is the global map initially created, if not do it before creating the local map
+ self.state.is_global_map_initialized = False
+
+ # initialize the global map if possible
+ if self._global_map_area is not None:
+ self._global_map_shape = mpi_sim.utils.get_social_map_shape(area=self._global_map_area, resolution=self._resolution)
+ self.state.global_map = mpi_sim.utils.SocialMap(
+ map=np.zeros(self._global_map_shape),
+ area=self._global_map_area,
+ resolution=self._resolution
+ )
+
+
+
+
+ else :
+ self._resolution = self.config.imported_global_map_resolution
+ self._global_map_area = self.config.imported_global_map_area
+ self.state.is_global_map_initialized = True #The global map is initialized and doesn't need to be updated
+
+ if isinstance(self.config.imported_global_map_path, str):
+
+ imported_global_map = np.load(file=self.config.imported_global_map_path)
+ self.shape_imported_global_map = np.shape(imported_global_map)
+ self.state.global_map = mpi_sim.utils.SocialMap(
+ map=np.zeros_like(imported_global_map),
+ area=self._global_map_area,
+ resolution=self._resolution
+ )
+
+ self._global_map_shape = np.shape(imported_global_map)
+
+ else :
+ raise UserWarning('The parameter \'imported_global_map_path\' should be a type str but it is {}'.format(type(self.config.imported_global_map_path)))
+
+ self.human_velocities = None
+ self._local_map_area = ((-self.config.depth, self.config.depth), (-self.config.depth, self.config.depth))
+ local_map_length = int((self.config.depth / self._resolution) * 2)
+ self._local_map_shape = (local_map_length, local_map_length)
+ self.state.local_map = mpi_sim.utils.SocialMap(
+ map=np.zeros(self._local_map_shape),
+ area=None,
+ resolution=self._resolution
+ )
+
+ def _add_to_simulation(self):
+
+ # if the global map area should span the whole simulation (config.area=None), then read visible_area from the simulation when added to it
+ if self._global_map_area is None:
+ self._global_map_area = self.simulation.visible_area
+ self._global_map_shape = mpi_sim.utils.get_social_map_shape(area=self._global_map_area, resolution=self._resolution)
+ self.state.global_map = mpi_sim.utils.SocialMap(
+ map=np.zeros(self._global_map_shape),
+ area=self._global_map_area,
+ resolution=self._resolution
+ )
+
+
+ def update_global_map(self):
+
+ if not self.config.imported_global_map_path :
+
+ self.state.global_map = mpi_sim.utils.create_social_map(
+ self.simulation,
+ area=self._global_map_area,
+ resolution=self._resolution,
+ human_velocities=self.human_velocities
+ )
+ else :
+ self.state.global_map = mpi_sim.utils.create_social_map(
+ self.simulation,
+ area=self._global_map_area,
+ resolution=self._resolution,
+ human_velocities=self.human_velocities,
+ flag_imported_global_map=True
+ )
+
+ return self.state.global_map.map
+
+
+ def update_local_map(self):
+ if self.state.global_map is None:
+ self.update_global_map()
+ if not self.config.imported_global_map_path :
+ self.state.local_map = mpi_sim.utils.create_local_perspective_social_map(
+ global_map=self.state.global_map,
+ position=self.object.position,
+ orientation=constraint_angle(self.object.orientation),
+ depth=self.config.depth,
+ )
+ else :
+ # Same but we need to relocate the position from the center of the imported room
+ self.state.local_map = mpi_sim.utils.create_local_perspective_social_map(
+ global_map=self.state.global_map,
+ # find a way to explain this
+ position=self.object.position,
+ orientation=constraint_angle(self.config.transform_real_to_sim_orientation(self.object.orientation)),
+ depth=self.config.depth,
+ from_given_center = self.config.imported_global_map_px_coord_center,
+ transform_position_to_map_coordinate = self.config.transform_position_to_map_coordinate
+ )
+ return self.state.local_map.map
+
+
+ def _step(self):
+ if self.config.update_global_map_automatically or not self.state.is_global_map_initialized:
+ self.update_global_map()
+ self.state.is_global_map_initialized = True
+ if self.config.update_local_map_automatically :
+ self.update_local_map()
diff --git a/mpi_sim/core/simulation.py b/mpi_sim/core/simulation.py
index 68d49bdcea42cc64e35a2ceb78b3b5434f1f2ec6..43f7140cf5818fef38a596e2674bf16c236e6b9f 100644
--- a/mpi_sim/core/simulation.py
+++ b/mpi_sim/core/simulation.py
@@ -814,6 +814,21 @@ class Simulation:
# box2d simulation
self._box2d_simulation.close()
+ def get_objects_in_simulation(self, object_to_avoid=None):
+ """Returns the objects in the simulation"""
+ objects = []
+ # go through objects
+ for obj in self.objects.values():
+ if object_to_avoid is None :
+ # only include objects that have a position property
+ if obj.position is not None:
+ objects.append(obj)
+ else :
+ if type(obj) != type(object_to_avoid):
+ if obj.position is not None:
+ objects.append(obj)
+ return objects
+
def get_nearby_objects(self, point, radius, object_types=None):
"""Returns objects that are within a specified radius of the given point (x,y) or object (obj.position).
diff --git a/mpi_sim/objects/ari_robot.py b/mpi_sim/objects/ari_robot.py
index 45f6cc2730cc74a0ec89e1204ca3c3929b1bb06e..854461ccd5abfe558b41fc80c9c38e6e20df4e1c 100644
--- a/mpi_sim/objects/ari_robot.py
+++ b/mpi_sim/objects/ari_robot.py
@@ -45,7 +45,7 @@ class ARIRobot(Object):
dc.base_footprint_radius = 0.3
dc.total_mass = 54.893
dc.wheel_mass = 1.65651
- dc.enable_pan = True
+ dc.enable_pan = False
# dc.enable_raycast = False
dc.base_link2head_front_camera_optical_frame = [-0.120, -0.000, 1.466]
dc.head_cam_fov_h = 62.
@@ -57,6 +57,9 @@ class ARIRobot(Object):
dc.max_movement_force = 0.25
dc.max_rotation_force = 6.
+ dc.linearDamping = 2
+ dc.angularDamping = 2
+
return dc
@@ -129,6 +132,7 @@ class ARIRobot(Object):
position = (0.0, 0.0)
self.box2d_body.position = (float(position[0]), float(position[1]))
+ self.pan.body.position = np.array((float(position[0]), float(position[1]))) + np.array(self.pan_anchors)
@property
@@ -261,8 +265,8 @@ class ARIRobot(Object):
)
self.box2d_body.userData = dict(obj=self)
self.box2d_body.userData['name'] = self.config.id
- self.box2d_body.linearDamping = 2
- self.box2d_body.angularDamping = 2
+ self.box2d_body.linearDamping = self.config.linearDamping
+ self.box2d_body.angularDamping = self.config.angularDamping
self.box2d_body.CreatePolygonFixture(
box=(*wheel_dim, b2Vec2(tire_anchors[0]), 0),
@@ -297,7 +301,7 @@ class ARIRobot(Object):
localAnchorB=(0, 0),
referenceAngle=0
)
- self.pan.body.position = self.box2d_body.worldCenter + self.pan_anchors
+ self.pan.body.position = np.array(self.config.position) + np.array(self.pan_anchors)
self.joints.append(j)
return self.box2d_body, self.pan.body
@@ -331,8 +335,8 @@ class ARIRobot(Object):
#############################
- if self.pan.body.awake:
- self.pan.update_turn(None, self.joints[-1], self.pan_vel) # keys = None
+ # if self.pan.body.awake:
+ # self.pan.update_turn(None, self.joints[-1], self.pan_vel) # keys = None
############################
diff --git a/mpi_sim/objects/human.py b/mpi_sim/objects/human.py
index 0470f49bb5a97de0f8fb05d50526b9891b04c903..6e4bc2517fa2eb2719e7ec4e7bd76e3d231e014b 100644
--- a/mpi_sim/objects/human.py
+++ b/mpi_sim/objects/human.py
@@ -46,6 +46,7 @@ class Human(Object):
return mpi_sim.AttrDict(
type=mpi_sim.components.SocialForceNavigation,
name='navigation',
+ step_length_coeff = 0.2,
)
@@ -116,6 +117,11 @@ class Human(Object):
position_velocity = (0.0, 0.0)
self.box2d_body.linearVelocity = (float(position_velocity[0]), position_velocity[1])
+ # self.box2d_body.linearVelocity = calc_linear_velocity_from_forward_velocity(
+ # forward_velocity,
+ # self.box2d_body
+ # )
+
@property
@@ -139,6 +145,8 @@ class Human(Object):
###############################################################################
+
+
@property
def group(self):
"""The group the agent is part of. None if he is not part of the group."""
@@ -157,6 +165,7 @@ class Human(Object):
self.box2d_body = None
+
# maybe used in future for key control
# self.keys = None
diff --git a/mpi_sim/processes/gui.py b/mpi_sim/processes/gui.py
index 2f1e7ed526720dae30cce69fa9e0d251414f8ec5..024d2a24dca8b200949b0da509ea1b25c32509e7 100644
--- a/mpi_sim/processes/gui.py
+++ b/mpi_sim/processes/gui.py
@@ -105,11 +105,11 @@ class GUI(Process):
#('Time of Impact', 'enable_continuous'),
#('Sub-Stepping', 'enable_sub_stepping'),
('Draw', None),
- ('Agent Info', 'draw_agent_information'),
- ('Object Info', 'draw_object_information'),
+ # ('Agent Info', 'draw_agent_information'),
+ # ('Object Info', 'draw_object_information'),
('Dialogue', 'draw_dialogue'),
- ('Debug Mode', 'draw_debug_information'),
- ('Statistics', 'draw_stats'),
+ # ('Debug Mode', 'draw_debug_information'),
+ # ('Statistics', 'draw_stats'),
('RTF', 'draw_rtf'),
#('Control', None),
#('Pause', 'pause'),
diff --git a/mpi_sim/utils/__init__.py b/mpi_sim/utils/__init__.py
index 43c4fdecaffe98b7a5c5e6ec9825f851be1b6034..250982463fd5dcabfde2d63ba8257a5738736e26 100644
--- a/mpi_sim/utils/__init__.py
+++ b/mpi_sim/utils/__init__.py
@@ -39,4 +39,14 @@ from mpi_sim.utils.occupancy_grid_map import get_occupancy_grid_map_shape
from mpi_sim.utils.occupancy_grid_map import transform_position_to_map_coordinate
from mpi_sim.utils.occupancy_grid_map import transform_map_coordinate_to_position
from mpi_sim.utils.occupancy_grid_map import OccupancyGridMap
+from mpi_sim.utils.occupancy_grid_transform import RTABMAPlikeTransform
+# social map
+
+# occupancy grid
+from mpi_sim.utils.social_map import create_social_map
+from mpi_sim.utils.social_map import create_local_perspective_social_map
+from mpi_sim.utils.social_map import get_social_map_shape
+from mpi_sim.utils.social_map import transform_position_to_map_coordinate
+from mpi_sim.utils.social_map import transform_map_coordinate_to_position
+from mpi_sim.utils.social_map import SocialMap
diff --git a/mpi_sim/utils/measurements.py b/mpi_sim/utils/measurements.py
index 8c9417b63cd2de7c8c7426cc3ee32cc63f39e6b5..61df664ae9c11cc57111701f9f6bab0cbc1913b7 100644
--- a/mpi_sim/utils/measurements.py
+++ b/mpi_sim/utils/measurements.py
@@ -2,6 +2,7 @@ import numpy as np
import mpi_sim
import collections
import Box2D
+from Box2D import b2CircleShape
def measure_center_distance(p1, p2):
@@ -23,7 +24,7 @@ def measure_center_distance(p1, p2):
DistanceInfo = collections.namedtuple('DistanceInfo', ['distance', 'point_a', 'point_b'])
-def measure_distance(p1, p2):
+def measure_distance(p1, p2, simulation=None):
"""Measures the minimum distance between two points or objects (their bodies and shapes) in meters."""
if not isinstance(p1, mpi_sim.Object) and not isinstance(p2, mpi_sim.Object):
@@ -65,7 +66,63 @@ def measure_distance(p1, p2):
point_b = np.array(b2_dist_info.pointB)
else:
- raise NotImplementedError('Calculating the distance between a point and an object is currently not supported!')
+ # To measure the distance between an object and a position we create a temporary object
+ # Then we measure the distance between the new temporary object and the original object
+ if simulation is None:
+ raise UserWarning('The distance measurement between an object and a position needs the simulation')
+
+ # In case the object is given in firt or second position
+ if isinstance(p1, mpi_sim.Object) :
+ _object = p1
+ _point = p2
+ else :
+ _object = p2
+ _point = p1
+
+ distance = np.inf
+ point_a = None
+ point_b = None
+
+ # Creating an object to measure the distance from
+ fake_dot_obj = mpi_sim.objects.RoundTable(
+ position=_point,
+ orientation=0,
+ radius = 0.000001,
+ )
+ simulation.add_object(fake_dot_obj)
+
+ # Measure the min distance for each bodies and each fixture
+ for body_a in fake_dot_obj.box2d_bodies:
+ for fixture_a in body_a.fixtures:
+
+ # fixture must have a shape
+ if not fixture_a.shape:
+ continue
+
+ for body_b in _object.box2d_bodies:
+ for fixture_b in body_b.fixtures:
+
+ # fixture must have a shape
+ if not fixture_b.shape:
+ continue
+
+ b2_dist_info = Box2D.b2Distance(
+ shapeA=fixture_a.shape,
+ shapeB=fixture_b.shape,
+ transformA=body_a.transform,
+ transformB=body_b.transform
+ )
+
+ if b2_dist_info.distance < distance:
+ distance = b2_dist_info.distance
+ point_a = np.array(b2_dist_info.pointA)
+ point_b = np.array(b2_dist_info.pointB)
+
+ # Remove the fake object from simulation
+ simulation.remove_object(fake_dot_obj)
+
+ # Correct the distance measurement w.r.t to its radius
+ distance += 0.000001
return DistanceInfo(distance, point_a, point_b)
diff --git a/mpi_sim/utils/occupancy_grid_map.py b/mpi_sim/utils/occupancy_grid_map.py
index fce115f2f1b80e789431e52a2fcd3d03628f1559..ef72ab265ccdcd6a2776d426f861f5bace686f4d 100644
--- a/mpi_sim/utils/occupancy_grid_map.py
+++ b/mpi_sim/utils/occupancy_grid_map.py
@@ -9,7 +9,6 @@ from collections import namedtuple
OccupancyGridMap = namedtuple('OccupancyGridMap', ['map', 'area', 'resolution'])
-
def get_occupancy_grid_map_shape(area, resolution):
r"""Calculates the size of the occupancy grid map that can be created via create_occupancy_grid_map().
@@ -30,6 +29,27 @@ def get_occupancy_grid_map_shape(area, resolution):
return shape
+def get_occupancy_grid_map_resolution(area, shape):
+ r"""Calculates the resolution of the occupancy grid map that can be created via create_occupancy_grid_map().
+
+ Args:
+ area (tuple of floats): Area of the simulation that should be mapped in ((x_min, x_max), (y_min, y_max)).
+ shape (tuple): Shape of the map as a tuple: (width, height)
+
+ Returns:
+ Resolution in meter per cell. (float)
+ """
+
+ x_width = area[0][1] - area[0][0]
+ #y_width = area[1][1] - area[1][0]
+
+ (width, height) = shape
+
+ resolution = int(x_width / width)
+ #resolution = int(y_width / height)
+
+ return resolution
+
def transform_position_to_map_coordinate(position, map_area, map_resolution):
r"""Transform a position (x, y) in the simulation to a coordinate (x, y) of a map of the simulation.
@@ -37,7 +57,7 @@ def transform_position_to_map_coordinate(position, map_area, map_resolution):
Args:
position (x,y): x,y - Position that should be transformed.
map_area (tuple): Area ((x_min, x_max), (y_min, y_max)) of the simulation that the map is depicting.
- map_area (float): Resolution of the map in m per cells.
+ map_resolution (float): Resolution of the map in m per cells.
Returns: Map coordinate as a tuple (x, y).
"""
@@ -65,7 +85,7 @@ def transform_map_coordinate_to_position(map_coordinate, map_area, map_resolutio
return x, y
-def create_occupancy_grid_map(simulation, area=None, resolution=0.05, object_filter=None, default_occupancy_value=1.0, occupancy_value_depth=32):
+def create_occupancy_grid_map(simulation, area=None, resolution=0.05, object_filter=None, default_occupancy_value=1.0, occupancy_value_depth=16, transform=None):
r"""Creates an occupancy grid map.
The size of the map depends on the visible_map_area of the simulation and the resolution of the map.
@@ -145,13 +165,22 @@ def create_occupancy_grid_map(simulation, area=None, resolution=0.05, object_fil
radius = math.ceil(fixture.shape.radius / resolution)
pygame.draw.circle(surface, color, center, radius, 0)
- occupancy_grid = np.array(pixel_array)
- occupancy_grid = occupancy_grid / max_color_value
+ occupancy_grid = np.array(pixel_array) / max_color_value
+
+ map = OccupancyGridMap(map=occupancy_grid, area=area, resolution=resolution)
+
+ if transform :
- return OccupancyGridMap(map=occupancy_grid, area=area, resolution=resolution)
+ if not isinstance(transform, (list, tuple)):
+ transform = [transform]
+ for t in transform :
+ map = t(map, simulation, bodies, default_occupancy_value, occupancy_value_depth)
-def create_local_perspective_occupancy_grid_map(global_map, depth=5.0, position=(0,0), orientation=0.0):
+ return map
+
+
+def create_local_perspective_occupancy_grid_map(global_map, depth=5.0, position=(0,0), orientation=0.0, from_given_center=None, transform_position_to_map_coordinate=transform_position_to_map_coordinate):
r"""Creates a local perspective of an occupancy grid map.
Args:
@@ -172,12 +201,21 @@ def create_local_perspective_occupancy_grid_map(global_map, depth=5.0, position=
# TODO: there should be a way to avoid the transpose operations, by choosing a different points that should be
local_map_length = int(depth * 2 / global_map.resolution)
+
+ if from_given_center is None :
+ pos_in_map = transform_position_to_map_coordinate(
+ position,
+ map_area=global_map.area,
+ map_resolution=global_map.resolution
+ )
+ else :
+ pos_in_map = transform_position_to_map_coordinate(
+ center_position=from_given_center,
+ position=position,
+ map_resolution=global_map.resolution,
+ )
+
- pos_in_map = transform_position_to_map_coordinate(
- position,
- map_area=global_map.area,
- map_resolution=global_map.resolution
- )
orientation = mpi_sim.utils.constraint_angle(orientation)
# rotation in for cv operator is in opposite direction
@@ -192,7 +230,6 @@ def create_local_perspective_occupancy_grid_map(global_map, depth=5.0, position=
rect_ur = pos_in_map + 0.5 * hvect * local_map_length + 0.5 * vvect * local_map_length
rect_lr = pos_in_map + 0.5 * hvect * local_map_length - 0.5 * vvect * local_map_length
src_pts = np.vstack([rect_ll, rect_ul, rect_ur, rect_lr]).astype(np.float32)
- src_pts = src_pts
# print('src_pts', src_pts)
# destination points in pixel space
diff --git a/mpi_sim/utils/occupancy_grid_transform.py b/mpi_sim/utils/occupancy_grid_transform.py
new file mode 100644
index 0000000000000000000000000000000000000000..462d54b375a8db5ebedd9c4592672cceccba102f
--- /dev/null
+++ b/mpi_sim/utils/occupancy_grid_transform.py
@@ -0,0 +1,56 @@
+import mpi_sim
+import numpy as np
+import cv2
+
+class MapTransform():
+ @staticmethod
+ def default_config():
+ dc = mpi_sim.AttrDict()
+ return dc
+
+ def __init__(self, config=None, **kwargs):
+ super(MapTransform, self).__init__(config=config, **kwargs)
+
+
+ def __call__(self, map, simulation, bodies, default_occupancy_value, occupancy_value_depth):
+ raise Warning('Not Implemented')
+
+
+class RTABMAPlikeTransform(MapTransform):
+
+ @staticmethod
+ def default_config():
+ dc = mpi_sim.AttrDict()
+ return dc
+
+
+ def __init__(self, config=None, **kwargs):
+ self.config = mpi_sim.combine_dicts(kwargs, config, self.default_config())
+
+
+ def __call__(self, map, simulation, bodies, default_occupancy_value, occupancy_value_depth):
+ """Transforms the map to make it looking like a 2D inflated RTAB-Map."""
+
+ new_map = np.copy(map.map)
+ # Inflate the original map
+ k = 3 #np.random.randint(1,3)
+ kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (k, k))
+ inflate_map = cv2.dilate(new_map, kernel, iterations=1)
+ # Complete the noisy_map when there is a 1 on the inflate map with a given probability of 4%
+ sampled_map = np.zeros(np.shape(inflate_map))
+ for x in range(len(inflate_map[0])):
+ for y in range(len(inflate_map[1])):
+ if inflate_map[x][y] == 1 and np.random.random() > 0.98:
+ sampled_map[x][y] = 1
+
+ # Now dilate the sparse 1 on the new noisy map with a circular kernel
+ k = 20 #np.random.randint(15,20)
+ kernel_noisy_map = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (k,k))
+ k = 10 #np.random.randint(5,10)
+ final_noisy_map = np.clip(
+ cv2.dilate(sampled_map, kernel_noisy_map, iterations=1) + cv2.dilate(new_map, kernel=cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (k, k)), iterations=1),
+ 0, default_occupancy_value
+ )
+ new_occupancy_grid = np.where(final_noisy_map > 0.1, default_occupancy_value, np.min(map.map))
+
+ return mpi_sim.utils.OccupancyGridMap(map=new_occupancy_grid, area=map.area, resolution=map.resolution)
\ No newline at end of file
diff --git a/mpi_sim/utils/social_map.py b/mpi_sim/utils/social_map.py
new file mode 100644
index 0000000000000000000000000000000000000000..621feeebec73b71346ee57136c7b321e78819d93
--- /dev/null
+++ b/mpi_sim/utils/social_map.py
@@ -0,0 +1,220 @@
+import mpi_sim
+import numpy as np
+import cv2
+from collections import namedtuple
+import mpi_sim.utils.truong_2018 as ssn
+
+
+SocialMap = namedtuple('SocialMap', ['map', 'area', 'resolution'])
+
+def get_social_map_shape(area, resolution):
+ r"""Calculates the size of the social map that can be created via create_social_map().
+
+ Args:
+ area (tuple of floats): Area of the simulation that should be mapped in ((x_min, x_max), (y_min, y_max)).
+ resolution (float): Resolution in meter per cell.
+
+ Returns:
+ Shape of the map as a tuple: (width, height).
+ """
+
+ x_width = area[0][1] - area[0][0]
+ y_width = area[1][1] - area[1][0]
+
+ width = int(x_width / resolution)
+ height = int(y_width / resolution)
+ shape = (width, height)
+
+ return shape
+
+def get_social_map_resolution(area, shape):
+ r"""Calculates the resolution of the social map that can be created via create_social_map().
+
+ Args:
+ area (tuple of floats): Area of the simulation that should be mapped in ((x_min, x_max), (y_min, y_max)).
+ shape (tuple): Shape of the map as a tuple: (width, height)
+
+ Returns:
+ Resolution in meter per cell. (float)
+ """
+
+ x_width = area[0][1] - area[0][0]
+ #y_width = area[1][1] - area[1][0]
+
+ (width, height) = shape
+
+ resolution = int(x_width / width)
+ #resolution = int(y_width / height)
+
+ return resolution
+
+
+def transform_position_to_map_coordinate(position, map_area, map_resolution):
+ r"""Transform a position (x, y) in the simulation to a coordinate (x, y) of a map of the simulation.
+
+ Args:
+ position (x,y): x,y - Position that should be transformed.
+ map_area (tuple): Area ((x_min, x_max), (y_min, y_max)) of the simulation that the map is depicting.
+ map_resolution (float): Resolution of the map in m per cells.
+
+ Returns: Map coordinate as a tuple (x, y).
+ """
+ x_min = map_area[0][0]
+ y_min = map_area[1][0]
+ x = int((position[0] - x_min) / map_resolution)
+ y = int((position[1] - y_min) / map_resolution)
+ return x, y
+
+
+def transform_map_coordinate_to_position(map_coordinate, map_area, map_resolution):
+ r"""Transform a map coordinate (x, y) of a map to a position (x, y) in the simulation.
+
+ Args:
+ map_coordinate (x,y): x,y - Map coordinate that should be transformed.
+ map_area (tuple): Area ((x_min, x_max), (y_min, y_max)) of the simulation that the map is depicting.
+ map_area (float): Resolution of the map in m per cells.
+
+ Returns: Position as a tuple (x, y).
+ """
+ x_min = map_area[0][0]
+ y_min = map_area[1][0]
+ x = map_coordinate[0] * map_resolution + x_min
+ y = map_coordinate[1] * map_resolution + y_min
+ return x, y
+
+
+def create_social_map(simulation, area=None, resolution=0.05, human_velocities=None, flag_imported_global_map=False):
+ r"""Creates an social map.
+
+ The size of the map depends on the visible_map_area of the simulation and the resolution of the map.
+ The map encodes the x-y position via map[x,y].
+
+ Args:
+ simulation (mpi_sim.Simulation): Simulation for which a map should be created.
+ area (tuple): Area of the simulation that should be mapped in ((x_min, x_max), (y_min, y_max)). (Default: None)
+ If None, it uses the simulation.visible_area.
+ resolution (float): Resolution in meter per cell (Default: 0.05).
+ human_velocities (list) : List of [x,y] velocities of humans
+
+ Returns:
+ social map in form of a SocialMap (namedtuple with map, area, and resolution).
+ """
+
+ if area is None:
+ area = simulation.visible_area
+
+ # the size of the map depends on the world size and the resolution
+
+ # visible_area: ((min_x, max_x), (min_y, max_y))
+ x_width = area[0][1] - area[0][0]
+ y_width = area[1][1] - area[1][0]
+ width = x_width / resolution
+ height = y_width / resolution
+
+
+
+ object_filter = dict(types=mpi_sim.objects.Human)
+
+ bodies = simulation.box2d_simulation.get_bodies(
+ object_types=object_filter.get('types', None),
+ object_ids=object_filter.get('ids', None),
+ object_properties= object_filter.get('properties', None),
+ )
+
+ persons = []
+ for id, bodie in enumerate(bodies) :
+ x, y = np.array(bodie.position)
+ theta = np.pi/2 + bodie.angle
+ vx,vy = 0,0
+ if human_velocities is not None:
+ if len(human_velocities) > 0 :
+ vx = human_velocities[id][0]
+ vy = human_velocities[id][1]
+ v = np.sqrt(vx**2 + vy**2)
+ persons.append((x ,y, theta, v, False))
+
+
+ x_coordinates = np.linspace(area[0][1], area[0][0], int(width))
+ y_coordinates = np.linspace(area[1][1], area[1][0], int(height))
+ x_mesh, y_mesh = np.meshgrid(x_coordinates, y_coordinates)
+
+ if not flag_imported_global_map:
+ global_social_map = np.flip(np.flip(ssn.calc_eps(x_mesh, y_mesh, persons), axis=1), axis=0).T
+ else :
+ global_social_map = np.flip(ssn.calc_eps(x_mesh, y_mesh, persons), axis=1)
+
+ map = SocialMap(map=global_social_map, area=area, resolution=resolution)
+
+ return map
+
+
+def create_local_perspective_social_map(global_map, depth=5.0, position=(0,0), orientation=0.0, from_given_center=None, transform_position_to_map_coordinate=transform_position_to_map_coordinate):
+ r"""Creates a local perspective of an social map.
+
+ Args:
+ global_map (SocialMap): Global social map which is the source for the local perspective.
+ depth (float): Viewing depth of the map in meters. The map size is 2 * depth. (Default: 5)
+ position (tuple of floats): Position (x,y) of the local map center in the global map. (Default: (0,0))
+ orientation (float): Orientation of the map in rad. (Default: 0)
+ 0 for north, -pi/2 for east, pi/2 for west, pi for south
+
+ Returns: Local perspective of the social map in form of a SocialMap (named tuple with map, area, and resolution)
+
+ """
+
+ # see https://theailearner.com/tag/cv2-warpperspective/
+ # Attention: opencv uses a different coordinate frame to us, thus the code additionally transforms the coordinate frames by
+ # having the opposite rotation direction, and using transpose operations
+
+ # TODO: there should be a way to avoid the transpose operations, by choosing a different points that should be
+
+ local_map_length = int(depth * 2 / global_map.resolution)
+
+ if from_given_center is None :
+ pos_in_map = transform_position_to_map_coordinate(
+ position,
+ map_area=global_map.area,
+ map_resolution=global_map.resolution
+ )
+ else :
+ pos_in_map = transform_position_to_map_coordinate(
+ center_position=from_given_center,
+ position=position,
+ map_resolution=global_map.resolution,
+ )
+
+ orientation = mpi_sim.utils.constraint_angle(orientation)
+
+ # rotation in for cv operator is in opposite direction
+ hvect = np.array([np.cos(-orientation), -np.sin(-orientation)])
+ vvect = np.array([np.sin(-orientation), np.cos(-orientation)])
+ # print('hvect', hvect)
+ # print('vvect', vvect)
+
+ # source points in pixel space
+ rect_ll = pos_in_map - 0.5 * hvect * local_map_length - 0.5 * vvect * local_map_length
+ rect_ul = pos_in_map - 0.5 * hvect * local_map_length + 0.5 * vvect * local_map_length
+ rect_ur = pos_in_map + 0.5 * hvect * local_map_length + 0.5 * vvect * local_map_length
+ rect_lr = pos_in_map + 0.5 * hvect * local_map_length - 0.5 * vvect * local_map_length
+ src_pts = np.vstack([rect_ll, rect_ul, rect_ur, rect_lr]).astype(np.float32)
+ # print('src_pts', src_pts)
+
+ # destination points in pixel space
+ dst_pts = np.array(
+ [[0, 0],
+ [0, local_map_length],
+ [local_map_length, local_map_length],
+ [local_map_length, 0]],
+ dtype=np.float32
+ )
+ # print('dst_pts', dst_pts)
+
+ m = cv2.getPerspectiveTransform(src_pts, dst_pts)
+ # use transpose operations to bring image into coordinate frame of cv and then back to our coordinate system
+ local_map = cv2.warpPerspective(
+ global_map.map.T,
+ m,
+ (local_map_length, local_map_length)
+ ).T
+
+ return SocialMap(map=local_map, resolution=global_map.resolution, area=None)
diff --git a/mpi_sim/utils/truong_2018.py b/mpi_sim/utils/truong_2018.py
new file mode 100644
index 0000000000000000000000000000000000000000..bf2b5914bf1e1bf8d6c3bda02b37d260e283b90f
--- /dev/null
+++ b/mpi_sim/utils/truong_2018.py
@@ -0,0 +1,853 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.patches
+from scipy import optimize
+from scipy import ndimage
+import scipy.interpolate
+import skfmm
+
+
+def calc_group_circle(persons):
+ persons_coordinates = [[person[0], person[1]] for person in persons]
+ x_g, y_g, r_g, _ = least_squares_circle(persons_coordinates)
+ return x_g, y_g, r_g
+
+
+def least_squares_circle(coords):
+ """
+ Circle fit using least-squares solver.
+ Inputs:
+
+ - coords, list or numpy array with len>2 of the form:
+ [
+ [x_coord, y_coord],
+ ...,
+ [x_coord, y_coord]
+ ]
+
+ Outputs:
+
+ - xc : x-coordinate of solution center (float)
+ - yc : y-coordinate of solution center (float)
+ - R : Radius of solution (float)
+ - residu : MSE of solution against training data (float)
+
+ Taken from package circle-fit.
+ """
+
+ def calc_R(x, y, xc, yc):
+ """
+ calculate the distance of each 2D points from the center (xc, yc)
+ """
+ return np.sqrt((x - xc) ** 2 + (y - yc) ** 2)
+
+ def f(c, x, y):
+ """
+ calculate the algebraic distance between the data points
+ and the mean circle centered at c=(xc, yc)
+ """
+ Ri = calc_R(x, y, *c)
+ return Ri - Ri.mean()
+
+ # coordinates of the barycenter
+ x = np.array([x[0] for x in coords])
+ y = np.array([x[1] for x in coords])
+ x_m = np.mean(x)
+ y_m = np.mean(y)
+ center_estimate = x_m, y_m
+ center, ier = optimize.leastsq(f, center_estimate, args=(x,y))
+ xc, yc = center
+ Ri = calc_R(x, y, *center)
+ R = Ri.mean()
+ residu = np.sum((Ri - R)**2)
+ return xc, yc, R, residu
+
+
+def calc_default_hand_positions(person, **parameters):
+ '''Calculates the default postion for the hands of a person, so that they are on the side of og'''
+
+ default_hand_distance = parameters.get('default_hand_distance', 0.2)
+
+ x_p, y_p, theta_p, _, _ = person
+
+ # define position as if person has a 0 angle orientation at position [0,0]
+ pos_lh = np.array([[0, default_hand_distance]]).transpose()
+ pos_rh = np.array([[0, -default_hand_distance]]).transpose()
+
+ # rotate the positions according
+ rotation_mat = np.array([[np.cos(theta_p), -np.sin(theta_p)],
+ [np.sin(theta_p), np.cos(theta_p)]])
+
+ pos_lh = np.matmul(rotation_mat, pos_lh).transpose()[0]
+ pos_rh = np.matmul(rotation_mat, pos_rh).transpose()[0]
+
+ # translate according to position of person
+ x_lh = pos_lh[0] + x_p
+ y_lh = pos_lh[1] + y_p
+ x_rh = pos_rh[0] + x_p
+ y_rh = pos_rh[1] + y_p
+
+ return x_lh, y_lh, x_rh, y_rh
+
+
+def _calc_single_eps(x, y, person, d_po=None, **parameters):
+
+ A_p = parameters.get('A_p', 1.0)
+ sigma_p_x = parameters.get('sigma_p_x', 0.2) #0.45
+ sigma_p_y = parameters.get('sigma_p_y', 0.2) #0.45
+ alpha = parameters.get('alpha', np.pi / 2)
+ fov_angle = parameters.get('fov_angle', 60 * np.pi / 180)
+ f_v = parameters.get('f_v', 0.8)
+ f_front = parameters.get('f_front', 0.2)
+ f_fov = parameters.get('f_fov', 0)
+ F_sit = parameters.get('F_sit', 2.5)
+
+ f_h = parameters.get('f_h', 0.0)
+ sigma_h_x = parameters.get('sigma_h_x', 0.28)
+ sigma_h_y = parameters.get('sigma_h_y', 0.28)
+
+ w_1 = parameters.get('w_1', 1.0)
+ w_2 = parameters.get('w_2', 1.0)
+
+ if len(person) == 5:
+ x_p, y_p, theta_p, v_p, is_sitting = person
+ x_lh, y_lh, x_rh, y_rh = calc_default_hand_positions(person)
+ else:
+ x_p, y_p, theta_p, v_p, is_sitting, x_lh, y_lh, x_rh, y_rh = person
+
+ f_sit = F_sit if is_sitting else 1.0
+
+ sigma_p_y_i = sigma_p_y
+
+ # personal space f_p
+ d = np.sqrt((x - x_p) ** 2 + (y - y_p) ** 2)
+
+ # Algorithm 1
+ sigma_p_x_i = np.full_like(x, sigma_p_x)
+
+ # angle according to the perspective of the person
+ theta = np.arctan2(y - y_p, x - x_p)
+ theta_diff = np.abs(np.arctan2(np.sin(theta - theta_p), np.cos(theta - theta_p)))
+
+ # check if an object needs to be considered
+ if d_po is None:
+ in_field_of_view_of_p_inds = theta_diff <= fov_angle
+ in_frontal_area_of_p_inds = (theta_diff <= alpha) & ~in_field_of_view_of_p_inds
+
+ sigma_p_x_i[in_field_of_view_of_p_inds] = (1 + v_p * f_v + f_front * f_sit + f_fov) * sigma_p_x
+ sigma_p_x_i[in_frontal_area_of_p_inds] = (1 + v_p * f_v + f_front * f_sit) * sigma_p_x
+ else:
+ in_frontal_area_of_p_inds = (theta_diff <= alpha)
+
+ sigma_p_x_i[in_frontal_area_of_p_inds] = d_po / 2.0
+
+ f_p = A_p * np.exp(-(d * np.cos(theta_diff) / (np.sqrt(2) * sigma_p_x_i)) ** 2 -
+ (d * np.sin(theta_diff) / (np.sqrt(2) * sigma_p_y_i)) ** 2)
+
+ # left hand space
+ d_lh = np.sqrt((x_p - x_lh) ** 2 + (y_p - y_lh) ** 2)
+ theta_lh = np.arctan2(y_lh - y_p, x_lh - x_p)
+ sigma_lh_x = (1 + f_h * d_lh) * sigma_h_x
+
+ f_lh = A_p * np.exp(-(d * np.cos(theta - theta_lh) / (np.sqrt(2) * sigma_lh_x)) ** 2 -
+ (d * np.sin(theta - theta_lh) / (np.sqrt(2) * sigma_h_y)) ** 2)
+
+ # right hand space
+ d_rh = np.sqrt((x_p - x_rh) ** 2 + (y_p - y_rh) ** 2)
+ theta_rh = np.arctan2(y_rh - y_p, x_rh - x_p)
+ sigma_rh_x = (1 + f_h * d_rh) * sigma_h_x
+
+ f_rh = A_p * np.exp(-(d * np.cos(theta - theta_rh) / (np.sqrt(2) * sigma_rh_x)) ** 2 -
+ (d * np.sin(theta - theta_rh) / (np.sqrt(2) * sigma_h_y)) ** 2)
+
+ # combine hand and person spaces
+ f_hands = np.maximum(f_lh, f_rh)
+ f_cur_eps = np.maximum(w_1 * f_p, w_2 * f_hands)
+
+ return f_cur_eps
+
+
+def calc_eps(x, y, persons, objects=None, **parameters):
+ '''Calculates the extended personal space of a human.'''
+
+ if not isinstance(persons, list): persons = [persons]
+ if objects is not None and not isinstance(objects, list): objects = [objects]
+
+ T_dis = parameters.get('T_dis', 3.6)
+ T_ang = parameters.get('T_ang', 10 * np.pi / 180)
+
+ f_eps = np.zeros_like(x)
+
+ for person in persons:
+ x_p, y_p, theta_p, v_p, is_sitting = person[0:5]
+
+ is_incorporated_object = False
+
+ if objects is not None:
+ for object in objects:
+
+ # check if the object should be considered
+ x_ob, y_ob = object
+ # check if object is in interaction with the person
+ d_po = np.sqrt((x_ob - x_p) ** 2 + (y_ob - y_p) ** 2)
+ theta_po = np.arctan2((y_ob - y_p), (x_ob - x_p))
+ theta_diff = np.abs(np.arctan2(np.sin(theta_po - theta_p), np.cos(theta_po - theta_p)))
+
+ # check if the object should be incorporated into eps
+ if d_po < T_dis and theta_diff < T_ang:
+ cur_f_eps = _calc_single_eps(x, y, person, d_po=d_po, **parameters)
+ f_eps = np.maximum(f_eps, cur_f_eps)
+
+ is_incorporated_object = True
+
+ # if no object was incorporated into the eps, then calculate one without an object
+ if not is_incorporated_object:
+ cur_f_eps = _calc_single_eps(x, y, person, **parameters)
+ f_eps = np.maximum(f_eps, cur_f_eps)
+
+ return f_eps
+
+
+def calc_sis(x, y, persons, theta_g=0.0, v_g= 0.0, **parameters):
+
+ if not isinstance(persons, list): persons = [persons]
+
+ if len(persons) == 1:
+ f_group = np.zeros_like(x)
+ else:
+ A_p = parameters.get('A_p', 1.0)
+ f_g = parameters.get('f_g', -0.5) # 0.0
+ f_v = parameters.get('f_v', 0.8)
+
+ # use a simple circle approximation if the center and radius of the group are not given
+ x_g, y_g, r_g = calc_group_circle(persons)
+
+ sigma_g_x = (r_g / 2) * (1 + f_g)
+ sigma_g_y = (r_g / 2) * (1 + f_g)
+
+ # if the group moves then differentiate between area in movement direction and area oposite to the mvoement direction
+ if v_g > 0.0:
+ sigma_g_x = np.full_like(x, sigma_g_x)
+
+ theta = np.arctan2(y - y_g, x - x_g)
+ theta_diff = np.abs(np.arctan2(np.sin(theta - theta_g), np.cos(theta - theta_g)))
+
+ sigma_g_x[theta_diff < np.pi/2] = (r_g / 2) * (1 + f_g + f_v * v_g)
+
+ d = np.sqrt((x - x_g) ** 2 + (y - y_g) ** 2)
+ theta = np.arctan2(y - y_g, x - x_g)
+
+ f_group = A_p * np.exp(-(d * np.cos(theta - theta_g) / (np.sqrt(2) * sigma_g_x)) ** 2 -
+ (d * np.sin(theta - theta_g) / (np.sqrt(2) * sigma_g_y)) ** 2)
+
+ return f_group
+
+
+def calc_dsz(x, y, persons, groups=None, objects=None, **parameters):
+
+ if groups is not None and not isinstance(groups, list): groups = [groups]
+
+ w_3 = parameters.get('w_3', 1.0)
+ w_4 = parameters.get('w_4', 1.0)
+
+ # calc eps
+ f_eps = calc_eps(x, y, persons, objects, **parameters)
+
+ # calc sis for all groups
+ f_sis = np.zeros_like(x)
+ if groups is not None:
+ for group in groups:
+
+ # get persons of the group and the direction and velocity of the group
+ person_idxs, theta_g, v_g = group
+ cur_persons = [persons[p_idx] for p_idx in person_idxs]
+
+ cur_f_sis = calc_sis(x, y, cur_persons, theta_g, v_g, **parameters)
+ f_sis = np.maximum(f_sis, cur_f_sis)
+
+ # combine both
+ f_dsz = np.maximum(w_3 * f_eps, w_4 * f_sis)
+
+ return f_dsz
+
+
+def calc_goal_pos(x_mesh, y_mesh, f_dsz, robot, persons, **parameters):
+
+ if not isinstance(persons, list): persons = [persons]
+
+ fov_angle = parameters.get('fov_angle', 60 * np.pi / 180)
+ r_step = parameters.get('r_step', 0.1)
+ r_p_0 = parameters.get('r_p_0', 0.1)
+ dsz_border = parameters.get('dsz_border', 0.001) #0.03
+
+ # helper function to define the points on a circle
+ def calc_circle_points(x_mesh, y_mesh, x_c, y_c, r_c):
+ return (np.abs(np.hypot(x_c - x_mesh, y_c - y_mesh) - r_c) < 0.04)
+
+ if len(persons) == 1:
+ # single person
+ x_c, y_c = persons[0][0], persons[0][1]
+ r_c = r_p_0
+ else:
+ # group of people
+ x_c, y_c, r_c = calc_group_circle(persons)
+
+ A_out = np.full_like(x_mesh, False, dtype=bool)
+
+ while True:
+ r_c += r_step
+ A_in = calc_circle_points(x_mesh, y_mesh, x_c, y_c, r_c)
+
+ A_in_1 = A_in.copy()
+ A_out_1 = np.full_like(x_mesh, False, dtype=bool)
+
+ # check for circle points that are within all persons viewing point
+ for person in persons:
+
+ x_p, y_p, theta_p = person[0], person[1], person[2]
+
+ # calculate the angle of each point on the circle in relation to the person
+ theta = np.arctan2(y_mesh[A_in_1] - y_p, x_mesh[A_in_1] - x_p)
+ # angle according to the perspective of the person
+ theta_diff = np.abs(np.arctan2(np.sin(theta - theta_p), np.cos(theta - theta_p)))
+
+ # check if circle points are in the viewing angle of the person
+ A_out_1[A_in_1] = (theta_diff <= fov_angle)
+
+ # if there is no point that is viewed stop
+ if not np.any(A_out_1):
+ break
+ else:
+ # only check further points that are viewed by all people
+ A_in_1 = A_out_1.copy()
+
+ if np.any(A_out_1):
+ # check if the points are outside the DMZ
+ points_outside_DSZ_inds = f_dsz[A_out_1] <= dsz_border
+ A_out[A_out_1] = points_outside_DSZ_inds
+ else:
+ # there are no points on the circle that can be viewed by all people
+ # --> just take points into account that are inside the DSZ
+ points_outside_DSZ_inds = f_dsz[A_in] <= dsz_border
+ A_out[A_in] = points_outside_DSZ_inds
+
+ if np.any(A_out):
+ break
+
+ A_out, goal = _filter_approaching_areas(x_mesh, y_mesh, A_out, robot, (x_c, y_c))
+
+ return goal, A_out
+
+
+def _filter_approaching_areas(x_mesh, y_mesh, A_out, robot, target):
+
+ A_out = A_out.astype(np.float)
+
+ x_r, y_r = robot[0], robot[1]
+ x_t, y_t = target[0], target[1]
+
+ # identify connected areas in A_out
+ labeled_A_out, n_areas = ndimage.measurements.label(A_out, np.ones((3,3), dtype=np.int))
+
+ min_d_r = np.inf
+ x_q, y_q = np.nan, np.nan
+
+ # ignore area 1
+ for area_id in range(1, n_areas+1):
+
+ label_inds = (labeled_A_out == area_id)
+
+ # center point of area (true center of mass)
+ center_x, center_y = ndimage.measurements.center_of_mass(label_inds)
+
+ # identify the point in the 2d matrix that is the closest to the true center
+ grid_x, grid_y = np.indices(A_out.shape).astype(np.float)
+ d = np.sqrt((grid_x - center_x) ** 2 + (grid_y - center_y) ** 2)
+
+ # replace A_out with the distance
+ A_out[label_inds] = d[label_inds]
+
+ # find coordinates with the shortest distance
+ min_d_grid_idx = np.where(d == np.min(d))
+ x_a_idx = min_d_grid_idx[0][0]
+ y_a_idx = min_d_grid_idx[1][0]
+
+ x_a = x_mesh[x_a_idx, y_a_idx]
+ y_a = y_mesh[x_a_idx, y_a_idx]
+
+ # distance to the robot
+ d_r = np.sqrt((x_a - x_r) ** 2 + (y_a - y_r) ** 2)
+
+ if d_r < min_d_r:
+ min_d_r = d_r
+ x_q, y_q = x_a, y_a
+
+ # calculate the goal orientation
+ theta_q = np.arctan2(y_t - y_q, x_t - x_q)
+
+ goal = (x_q, y_q, theta_q)
+
+ return A_out, goal
+
+
+def calc_path(x_mesh, y_mesh, velocity_map, robot, goal, **parameters):
+
+ max_steps = parameters.get('max_steps', 100)
+ position_step_width = parameters.get('position_step_width', 0.1) # in meter
+ rotation_step_width = parameters.get('rotation_step_width', np.pi*0.1) # in rad
+ min_goal_position_dist = parameters.get('min_goal_position_dist', 0.05) # in meter
+ min_goal_rotation_dist = parameters.get('min_goal_rotation_dist', 0.05) # in rad
+
+ x_goal, y_goal, theta_goal = goal
+ x_robot, y_robot = robot[0], robot[1]
+
+ # TODO: also update the rotation of the robot
+ theta_robot = theta_goal
+
+ # compute the shortest travel time map based on the velocity map
+ # TODO: if the goal location is not reachable, then select the most nearby point that is reachable
+
+ # define the goal location to the point in the world grid that is closest to the goal location
+ # phi there is -1, otherwise it is 1
+ distance_to_goal = np.sqrt((x_mesh - x_goal) ** 2 + (y_mesh - y_goal) ** 2)
+ phi = np.ones_like(x_mesh)
+ phi[np.unravel_index(np.argmin(distance_to_goal), phi.shape)] = -1
+
+ travel_time_map = skfmm.travel_time(phi, velocity_map, order=1)
+
+ # calc gradient iver travel_time map and create interpolation functions
+ grad_t_y, grad_t_x = np.gradient(travel_time_map)
+
+ # interpolate the gradients and velocities
+ gradx_interp = scipy.interpolate.RectBivariateSpline(x_mesh[0, :], y_mesh[:, 0], grad_t_x)
+ grady_interp = scipy.interpolate.RectBivariateSpline(x_mesh[0, :], y_mesh[:, 0], grad_t_y)
+ velocity_interp = scipy.interpolate.RectBivariateSpline(x_mesh[0, :], y_mesh[:, 0], velocity_map)
+
+ path = []
+ path.append((x_robot, y_robot, theta_robot))
+
+ # create path by following the gradient of the travel time
+ for step in range(max_steps):
+
+ position_goal_distance = np.sqrt((x_robot - x_goal) ** 2 + (y_robot - y_goal) ** 2)
+ rotation_goal_distance = np.abs(np.arctan2(np.sin(theta_robot - theta_goal), np.cos(theta_robot - theta_goal)))
+
+ if position_goal_distance <= min_goal_position_dist and rotation_goal_distance <= min_goal_rotation_dist:
+ break
+
+ movement_direction = np.array([-gradx_interp(y_robot, x_robot)[0][0],
+ -grady_interp(y_robot, x_robot)[0][0]])
+
+ movement_speed = velocity_interp(y_robot, x_robot)[0][0]
+
+ # create normalized movement direction vector and change its size according to the movement speed
+ f_position = movement_direction / np.linalg.norm(movement_direction) * movement_speed
+
+ # update the position of the robot
+ x_robot += f_position[0] * position_step_width
+ y_robot += f_position[1] * position_step_width
+
+ f_orientation = 0.0
+ # TODO: adapt the rotation of the robot
+ path.append((x_robot, y_robot, theta_robot))
+
+ return path
+
+
+def occupancy_map_to_velocity_map(x_mesh, y_mesh, occupancy_map, **parameters):
+ '''Create a velocity map for a given occupancy map.'''
+
+ sigma = parameters.get('sigma', 0.15)
+
+ d_x = [x_mesh[0, 1] - x_mesh[0, 0], y_mesh[1, 0] - y_mesh[0, 0]]
+
+ # use fast marching method to compute the min distance from free to occupied areas
+ phi = np.ones_like(occupancy_map, dtype=int)
+ phi[occupancy_map == 1] = -1
+
+ distance_map = skfmm.distance(phi, d_x, order=1)
+
+ # compute velocity based on distance
+ velocity_map = 1.0 - np.exp(-distance_map/(np.sqrt(2)*sigma))
+ velocity_map[occupancy_map == 1] = 0
+
+ return velocity_map
+
+
+def get_default_occupany_map(x_mesh, y_mesh):
+
+ occupancy_map = np.full_like(x_mesh, False, dtype=bool)
+
+ # borders
+ occupancy_map[0, :] = True
+ occupancy_map[-1, :] = True
+ occupancy_map[:, 0] = True
+ occupancy_map[:, -1] = True
+
+ return occupancy_map
+
+
+def add_box_to_occupancy_map(x_mesh, y_mesh, occupancy_map, box):
+
+ occupancy_map = np.copy(occupancy_map)
+
+ x_box, y_box, width, height = box
+
+ # identify the starting in x
+ x_start = x_box
+ x_end = x_start + width
+ y_start = y_box
+ y_end = y_box + height
+
+ d_1_start_index = np.argmin(np.abs(x_mesh[0, :] - x_start))
+ d_1_end_index = np.argmin(np.abs(x_mesh[0, :] - x_end))
+
+ d_0_start_index = np.argmin(np.abs(y_mesh[:, 0] - y_start))
+ d_0_end_index = np.argmin(np.abs(y_mesh[:, 0] - y_end))
+
+ occupancy_map[d_0_start_index:d_0_end_index, d_1_start_index:d_1_end_index] = True
+
+ return occupancy_map
+
+
+def get_default_occupany_map(x_mesh, y_mesh):
+
+ occupancy_map = np.full_like(x_mesh, False, dtype=bool)
+
+ # borders
+ occupancy_map[0, :] = True
+ occupancy_map[-1, :] = True
+ occupancy_map[:, 0] = True
+ occupancy_map[:, -1] = True
+
+ return occupancy_map
+
+
+def plot_2d(x_mesh, y_mesh, space_model, occupancy_map=None, approaching_areas=None, persons=None, objects=None, goal=None, robot=None, path=None, title='', is_contur_lines=False):
+
+ if persons is None:
+ persons = []
+ elif not isinstance(persons, list):
+ persons = [persons]
+
+ if objects is None:
+ objects = []
+ elif not isinstance(objects, list):
+ objects = [objects]
+
+ origin = 'lower'
+
+ fig, ax = plt.subplots(constrained_layout=True)
+
+ CS = ax.contourf(x_mesh, y_mesh, space_model, 20, cmap=plt.cm.binary, origin=origin)
+
+ # Make a colorbar for the ContourSet returned by the contourf call.
+ cbar = fig.colorbar(CS)
+ cbar.ax.set_ylabel('space model value')
+ # Add the contour line levels to the colorbar
+ if is_contur_lines:
+ # Note that in the following, we explicitly pass in a subset of
+ # the contour levels used for the filled contours. Alternatively,
+ # We could pass in additional levels to provide extra resolution,
+ # or leave out the levels kwarg to use all of the original levels.
+ CS2 = ax.contour(CS, levels=CS.levels[::2], colors='r', origin=origin)
+ cbar.add_lines(CS2)
+
+ if approaching_areas is not None:
+ inds = approaching_areas > 0
+ ax.scatter(x_mesh[inds], y_mesh[inds], s=1, c=-approaching_areas[inds])
+
+ if robot is not None:
+ ax.scatter(robot[0], robot[1], s=150, c='g')
+
+ if goal is not None:
+ # print goal position and orientation
+ ax.scatter(goal[0], goal[1], c='r')
+ plt.arrow(goal[0], goal[1], np.cos(goal[2]) * 0.1, np.sin(goal[2]) * 0.1, edgecolor='r')
+
+ # draw persons
+ for person in persons:
+ if len(person) == 5:
+ x_p, y_p, theta_p, v_p, is_sitting = person
+ x_lh, y_lh, x_rh, y_rh = calc_default_hand_positions(person)
+ else:
+ x_p, y_p, theta_p, v_p, is_sitting, x_lh, y_lh, x_rh, y_rh = person
+
+ # person
+ e_p = matplotlib.patches.Ellipse((x_p, y_p), 0.15, 0.25,
+ angle=theta_p*180/np.pi, linewidth=2, fill=False, zorder=2, edgecolor='b')
+ ax.add_patch(e_p)
+
+ # left hand
+ e_lh = matplotlib.patches.Ellipse((x_lh, y_lh), 0.05, 0.05, linewidth=1, fill=False, zorder=2, edgecolor='b')
+ ax.add_patch(e_lh)
+
+ # right hand
+ e_rh = matplotlib.patches.Ellipse((x_rh, y_rh), 0.05, 0.05, linewidth=1, fill=False, zorder=2, edgecolor='b')
+ ax.add_patch(e_rh)
+
+ plt.arrow(x_p, y_p, np.cos(theta_p) * 0.1, np.sin(theta_p) * 0.1, edgecolor='b')
+
+ # draw objects
+ for object in objects:
+ x_ob, y_ob = object
+ e = matplotlib.patches.Rectangle((x_ob-0.1, y_ob-0.1), 0.2, 0.2, edgecolor='r')
+ ax.add_patch(e)
+
+ if occupancy_map is not None:
+ ax.scatter(x_mesh[occupancy_map == 1], y_mesh[occupancy_map == 1], c='k')
+
+ if path is not None:
+ path_position = np.array([[point[0], point[1]] for point in path])
+ ax.plot(path_position[:,0], path_position[:,1])
+
+ ax.set_title(title)
+ ax.set_xlabel('x')
+ ax.set_ylabel('y')
+
+ plt.show()
+
+
+if __name__ == '__main__':
+
+ # TODO: the current group identification algorithm does not take into account the orientation of people
+
+ # ###############################
+ # # EPS
+ #
+ # x_coordinates = np.linspace(0, 4, 400)
+ # y_coordinates = np.linspace(0, 4, 400)
+ # x_mesh, y_mesh = np.meshgrid(x_coordinates, y_coordinates)
+ #
+ # # FIGURE 2. b
+ # person = (2.0, 2.0, -np.pi/2, 0.0, False) # (x, y, theta, v, is_sitting)
+ # f_eps = calc_eps(x_mesh, y_mesh, person, f_fov=0.4, f_front=0.0)
+ # plot_2d(x_mesh, y_mesh, f_eps, persons=person, title='FOV based EPS (Fig. 2, b)')
+ #
+ # # FIGURE 2. c
+ # person = (2.0, 2.0, -np.pi/2, 0.0, False) # (x, y, theta, v, is_sitting)
+ # f_eps = calc_eps(x_mesh, y_mesh, person)
+ # plot_2d(x_mesh, y_mesh, f_eps, persons=person, title='Human frontal area-based EPS (Fig. 2, c)')
+ #
+ # # FIGURE 2. d
+ # person = (2.0, 2.0, -np.pi/2, 0.0, True) # (x, y, theta, v, is_sitting)
+ # #person = (2.0, 2.0, 0, 0.0, True) # (x, y, theta, v, is_sitting)
+ # f_eps = calc_eps(x_mesh, y_mesh, person)
+ # plot_2d(x_mesh, y_mesh, f_eps, persons=person, title='EPS of sitting person (Fig. 2, d)')
+ #
+ # # FIGURE 2. e
+ # person = (2.0, 2.0, -np.pi/2, 0.6, False) # (x, y, theta, v, is_sitting)
+ # f_eps = calc_eps(x_mesh, y_mesh, person)
+ # plot_2d(x_mesh, y_mesh, f_eps, persons=person, title='EPS of moving person (Fig. 2, e)')
+ #
+ # # FIGURE 2. f
+ # person = (2.0, 2.0, -np.pi/2, 0, False) # (x, y, theta, v, is_sitting)
+ # object = (2.0, 0.5)
+ # f_eps = calc_eps(x_mesh, y_mesh, person, object)
+ # plot_2d(x_mesh, y_mesh, f_eps, persons=person, objects=object, title='EPS of human with object (Fig. 2, f)')
+
+
+ # ###############################
+ # # SIS
+ #
+ # x_coordinates = np.linspace(0, 4, 400)
+ # y_coordinates = np.linspace(0, 4, 400)
+ # x_mesh, y_mesh = np.meshgrid(x_coordinates, y_coordinates)
+ #
+ # # # FIGURE 3. a
+ # persons = [(1.0, 2.0, 0.0, 0, False), # (x, y, theta, v, is_sitting)
+ # (3.0, 2.0, np.pi, 0, False)]
+ # theta_g, v_g = 0.0, 0.0
+ # f_sis = calc_sis(x_mesh, y_mesh, persons, theta_g=theta_g, v_g=v_g)
+ # plot_2d(x_mesh, y_mesh, f_sis, persons=persons, title='SIS vis-a-vis (Fig. 3, a)')
+ #
+ # # FIGURE 3. b
+ # persons = [(1.5, 1.5, np.pi*0.4, 0, False), # (x, y, theta, v, is_sitting)
+ # (2.5, 1.5, np.pi*0.6, 0, False)]
+ # theta_g, v_g = 0.0, 0.0
+ # f_sis = calc_sis(x_mesh, y_mesh, persons, theta_g=theta_g, v_g=v_g)
+ # plot_2d(x_mesh, y_mesh, f_sis, persons=persons, title='SIS v-shape (Fig. 3, b)')
+ #
+ # # FIGURE 3. c
+ # persons = [(1.5, 1.0, np.pi*0.4, 0, False), # (x, y, theta, v, is_sitting)
+ # (2.5, 1.0, np.pi*0.6, 0, False),
+ # (2.0, 3.0, np.pi*-0.5, 0, False)]
+ # theta_g, v_g = 0.0, 0.0
+ # f_sis = calc_sis(x_mesh, y_mesh, persons, theta_g=theta_g, v_g=v_g)
+ # plot_2d(x_mesh, y_mesh, f_sis, persons=persons, title='SIS close group (Fig. 3, c)')
+ #
+ # # FIGURE 3. d
+ # persons = [(1.75, 1.3, np.pi*0.45, 0.5, False), # (x, y, theta, v, is_sitting)
+ # (2.25, 1.3, np.pi*0.55, 0.5, False)]
+ # theta_g, v_g = np.pi*0.5, 0.5
+ # f_sis = calc_sis(x_mesh, y_mesh, persons, theta_g=theta_g, v_g=v_g)
+ # plot_2d(x_mesh, y_mesh, f_sis, persons=persons, title='SIS - Two people moving in abreast-shape (Fig. 3, d)')
+ #
+ # # FIGURE 3. e
+ # persons = [(1.5, 1.8, np.pi*0.45, 0, False), # (x, y, theta, v, is_sitting)
+ # (2.0, 1.2, np.pi*0.5, 0, False),
+ # (2.5, 1.8, np.pi*0.55, 0, False)]
+ # theta_g, v_g = np.pi * 0.5, 0.5
+ # f_sis = calc_sis(x_mesh, y_mesh, persons, theta_g=theta_g, v_g=v_g)
+ # plot_2d(x_mesh, y_mesh, f_sis, persons=persons, title='SIS - Three people moving in v-shape (Fig. 3, e)')
+
+
+ # ###############################
+ # # DSZ
+ #
+ # x_coordinates = np.linspace(0, 6, 700)
+ # y_coordinates = np.linspace(0, 6, 700)
+ # x_mesh, y_mesh = np.meshgrid(x_coordinates, y_coordinates)
+ #
+ # # Fig 4 (a)
+ # persons = [(2.5, 1.8, np.pi * 0.45, 0.0, False),
+ # (3.5, 1.8, np.pi * 0.55, 0.0, False)]
+ # object = (3.0, 5.0)
+ # group = ([0, 1], 0.0, 0.0)
+ # f_dsz = calc_dsz(x_mesh, y_mesh, persons, groups=group, objects=object)
+ # plot_2d(x_mesh, y_mesh, f_dsz, persons=persons, objects=object, title='DSZ (Fig. 4, a)')
+ #
+ # # Fig 4 (b)
+ # persons = [(2.5, 1.9, np.pi * 0.25, 0.0, False),
+ # (3.0, 3.8, np.pi * -0.5, 0.0, False),
+ # (3.5, 1.9, np.pi * 0.75, 0.0, False)]
+ # group = ([0, 1, 2], 0.0, 0.0)
+ # f_dsz = calc_dsz(x_mesh, y_mesh, persons, groups=group)
+ # plot_2d(x_mesh, y_mesh, f_dsz, persons=persons, title='DSZ (Fig. 4, b)')
+ #
+ #
+ # # Fig 4 (c)
+ # persons = [(2.5, 3.0, np.pi * 0.5, 0.8, False),
+ # (3.0, 2.5, np.pi * 0.5, 0.8, False),
+ # (3.5, 3.0, np.pi * 0.5, 0.8, False)]
+ # group = ([0, 1, 2], np.pi * 0.5, 0.8)
+ # f_dsz = calc_dsz(x_mesh, y_mesh, persons, groups=group)
+ # plot_2d(x_mesh, y_mesh, f_dsz, persons=persons, title='DSZ (Fig. 4, c)')
+ #
+ #
+ # # Fig 4 (d)
+ # persons = [(2.5, 2.5, np.pi * 0.25, 0.0, False),
+ # (3.5, 2.5, np.pi * 0.75, 0.0, True)]
+ # group = ([0, 1], 0.0, 0.0)
+ # f_dsz = calc_dsz(x_mesh, y_mesh, persons, groups=group)
+ # plot_2d(x_mesh, y_mesh, f_dsz, persons=persons, title='DSZ (Fig. 4, d)')
+
+
+ #################################
+ # Figure 5 - Approach areas
+
+ # x_coordinates = np.linspace(0, 6, 700)
+ # y_coordinates = np.linspace(0, 6, 700)
+ # x_mesh, y_mesh = np.meshgrid(x_coordinates, y_coordinates)
+ #
+ # # Fig 5 d top - right
+ # robot = (5.0, 5.0)
+ # persons = [(2.5, 1.8, np.pi * 0.45, 0.0, False),
+ # (3.5, 1.8, np.pi * 0.55, 0.0, False)]
+ # object = (3.0, 5.0)
+ # group = ([0, 1], 0.0, 0.0)
+ # f_dsz = calc_dsz(x_mesh, y_mesh, persons, groups=group, objects=object)
+ # goal, A_out = calc_goal_pos(x_mesh, y_mesh, f_dsz, robot, [persons[p_idx] for p_idx in group[0]])
+ # plot_2d(x_mesh, y_mesh, f_dsz, robot=robot, persons=persons, objects=object, approaching_areas=A_out, goal=goal,
+ # title='Approach area (Fig. 5, d, top right)')
+ #
+ # # Fig 5 d bottom right
+ # robot = (5.0, 5.0)
+ # persons = [(2.5, 1.9, np.pi * 0.25, 0.0, False),
+ # (3.0, 3.8, np.pi * -0.5, 0.0, False),
+ # (3.5, 1.9, np.pi * 0.75, 0.0, False)]
+ # group = ([0, 1, 2], 0.0, 0.0)
+ # f_dsz = calc_dsz(x_mesh, y_mesh, persons, groups=group)
+ # goal, A_out = calc_goal_pos(x_mesh, y_mesh, f_dsz, robot, [persons[p_idx] for p_idx in group[0]])
+ # plot_2d(x_mesh, y_mesh, f_dsz, robot=robot, persons=persons, approaching_areas=A_out, goal=goal,
+ # title='Approach area (Fig. 5, d, bottom right)')
+ #
+ # # Fig 5 d top left
+ # robot = (5.0, 5.0)
+ # persons = [(2.5, 3.0, np.pi * 0.5, 0.8, False),
+ # (3.0, 2.5, np.pi * 0.5, 0.8, False),
+ # (3.5, 3.0, np.pi * 0.5, 0.8, False)]
+ # group = ([0, 1, 2], np.pi * 0.5, 0.8)
+ # f_dsz = calc_dsz(x_mesh, y_mesh, persons, groups=group)
+ # goal, A_out = calc_goal_pos(x_mesh, y_mesh, f_dsz, robot, [persons[p_idx] for p_idx in group[0]])
+ # plot_2d(x_mesh, y_mesh, f_dsz, robot=robot, persons=persons, approaching_areas=A_out, goal=goal,
+ # title='Approach area (Fig. 5, d, top left)')
+ #
+ # # Fig 5 d bottom left
+ # robot = (5.0, 5.0)
+ # persons = [(2.5, 2.5, np.pi * 0.25, 0.0, False),
+ # (3.5, 2.5, np.pi * 0.75, 0.0, True)]
+ # group = ([0, 1], 0.0, 0.0)
+ # f_dsz = calc_dsz(x_mesh, y_mesh, persons, groups=group)
+ # goal, A_out = calc_goal_pos(x_mesh, y_mesh, f_dsz, robot, [persons[p_idx] for p_idx in group[0]])
+ # plot_2d(x_mesh, y_mesh, f_dsz, robot=robot, persons=persons, approaching_areas=A_out, goal=goal,
+ # title='Approach area (Fig. 5, d, bottom left)')
+
+
+
+ # ##########################################
+ # # PATH PLANNER
+ #
+ # x_coordinates = np.linspace(0, 6, 700)
+ # y_coordinates = np.linspace(0, 6, 700)
+ #
+ # x_mesh, y_mesh = np.meshgrid(x_coordinates, y_coordinates)
+ #
+ # # Fig 5 d bottom left
+ # robot = (3.0, 1.0, 0.0)
+ # persons = [(2.5, 2.5, np.pi * 0.25, 0.0, False),
+ # (3.5, 2.5, np.pi * 0.75, 0.0, True)]
+ # group = ([0, 1], 0.0, 0.0)
+ # f_dsz = calc_dsz(x_mesh, y_mesh, persons, groups=group)
+ # goal, A_out = calc_goal_pos(x_mesh, y_mesh, f_dsz, robot, [persons[p_idx] for p_idx in group[0]])
+ # # define velocity map
+ # velocity_map = 1.0 - f_dsz
+ # path = calc_path(x_mesh,y_mesh, velocity_map, robot, goal)
+ # plot_2d(x_mesh, y_mesh, f_dsz, robot=robot, persons=persons, approaching_areas=A_out, goal=goal, path=path,
+ # title='Robot path')
+
+
+ ##########################################
+ # OBSTACLES
+
+ x_coordinates = np.linspace(0, 6, 600)
+ y_coordinates = np.linspace(0, 6, 600)
+ x_mesh, y_mesh = np.meshgrid(x_coordinates, y_coordinates)
+
+ # default occupancy map
+ occupancy_map = get_default_occupany_map(x_mesh, y_mesh)
+ occupancy_map = add_box_to_occupancy_map(x_mesh, y_mesh, occupancy_map, (1.0, 1, 300, 0.2))
+ velocity_map = occupancy_map_to_velocity_map(x_mesh, y_mesh, occupancy_map)
+
+ robot = (3.0, 0.5, 0.0)
+ persons = [(2.5, 3.5, np.pi * 0.25, 0.0, False),
+ (3.5, 3.5, np.pi * 0.75, 0.0, True),
+ (1.0, 3.0, np.pi * -0.5, 0.0, False)]
+ group = ([0, 1], 0.0, 0.0)
+
+ f_dsz = calc_dsz(x_mesh, y_mesh, persons, groups=group)
+
+ goal, A_out = calc_goal_pos(x_mesh, y_mesh, f_dsz, robot, [persons[p_idx] for p_idx in group[0]])
+
+ # define velocity map
+ velocity_map = np.minimum(velocity_map, 1.0 - f_dsz)
+
+
+ path = calc_path(x_mesh,y_mesh, velocity_map, robot, goal)
+
+ plot_2d(x_mesh, y_mesh, f_dsz, robot=robot, persons=persons, approaching_areas=A_out, goal=goal, path=path,
+ occupancy_map=occupancy_map, title='Robot path')
+
+
+ # TODO:
+ # allow different groups
+ # define which person or group to join
+
+
+ # TODO: group identification
+
+ # TODO: Simulation
+
+ # TODO: incorporate movement
+
+ # TODO: compute the travel time to different possible goals to select one the with the optimal travel time and not the one that is closest to the robot
diff --git a/requirements.txt b/requirements.txt
index 20308afc4be1f0b071cf37464acf366c5ab446b1..550d4c7f2692ab606969cc05c6a220856c523433 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -8,7 +8,7 @@ attrs==21.4.0
backcall==0.2.0
beautifulsoup4==4.11.1
bleach==5.0.1
-Box2D==2.3.10
+Box2D==2.3.10 -f https://github.com/pybox2d/pybox2d.git
cachetools==5.2.0
certifi==2022.5.18.1
cffi==1.15.1
@@ -43,7 +43,7 @@ ipython-genutils==0.2.0
ipywidgets==7.6.5
isort==5.10.1
jax==0.2.20
-jaxlib==0.1.71
+jaxlib==0.1.71 -f https://storage.googleapis.com/jax-releases/jax_releases.html
jedi==0.18.1
Jinja2==3.1.2
joblib==1.1.0
@@ -61,7 +61,6 @@ matplotlib==3.5.2
matplotlib-inline==0.1.3
mccabe==0.7.0
mistune==2.0.4
--e git+https://gitlab.inria.fr/spring/wp6_robot_behavior/multiparty_interaction_simulator.git@f38e0e6373b38aeab5fbe41e78732d4aa3f1cdf8#egg=mpi_sim
nbclassic==0.4.8
nbclient==0.7.0
nbconvert==7.2.5
diff --git a/setup.cfg b/setup.cfg
index a72605bb5443865562a8a86216acb4c74ade6a8e..214125efafb54e1eaa020cf44549253f13c27ee1 100644
--- a/setup.cfg
+++ b/setup.cfg
@@ -11,7 +11,7 @@ install_requires =
six >= 1.16.0
scipy >= 1.8.1
opencv-python >= 4.6.0.66
-python_requires =
- >=3.8, <3.9
+#python_requires =
+# >=3.8, <=3.9.16
# required packages are in the setup.py
\ No newline at end of file
diff --git a/test/components/test_raycast_sensor.py b/test/components/test_raycast_sensor.py
new file mode 100644
index 0000000000000000000000000000000000000000..805d393215717805ff826218801ab8ec150be1ed
--- /dev/null
+++ b/test/components/test_raycast_sensor.py
@@ -0,0 +1,116 @@
+import mpi_sim
+import numpy.testing as np_test
+from Box2D import b2PolygonShape, b2CircleShape
+import numpy as np
+
+
+def manual_test():
+
+ # create the simulation
+ sim = mpi_sim.Simulation(
+ visible_area = ((-6,6),(-6,6)),
+
+ objects=[
+ {'type': 'Wall', 'position': [-5.1, 0], 'orientation': np.pi, 'length': 10.2},
+ {'type': 'Wall', 'position': [5.1, 0], 'orientation': 0., 'length': 10.2},
+ {'type': 'Wall', 'position': [0., 5.1], 'orientation': np.pi / 2, 'length': 10.2},
+ {'type': 'Wall', 'position': [0., -5.1], 'orientation': -np.pi / 2, 'length': 10.2},
+ #{'type': 'RoundTable', 'id': 10, 'position': [1., 0], 'radius': 0.5}
+ ],
+ processes=[{'type': 'GUI'}]
+ )
+
+
+ agent = mpi_sim.objects.ARIRobot( # type: ignore
+ components=[dict(
+ type=mpi_sim.components.RayCastSensor,
+ name = 'raycast',
+ field_of_view = 360,
+ delta_angle = 3,
+ ray_max_range = 3,
+ noise = False,
+ noise_ratio = 0.5,
+ draw_raycast = True
+ )],
+ position = (-2, 0),
+ orientation = -np.pi/2
+ )
+
+ sim.add_object(agent)
+ sim.set_reset_point()
+ sim.reset()
+ for _ in range(30):
+ for _ in range(22):
+ agent.forward_velocity = 0.1
+ agent.orientation_velocity = 0.1
+ sim.step()
+ agent.raycast.raycast_distances
+
+
+def test_3_rays_with_walls():
+
+ # create the simulation
+ sim = mpi_sim.Simulation(
+ visible_area = ((-6,6),(-6,6)),
+
+ objects=[
+ {'type': 'Wall', 'position': [-5.1, 0], 'orientation': np.pi, 'length': 10.2},
+ {'type': 'Wall', 'position': [5.1, 0], 'orientation': 0., 'length': 10.2},
+ {'type': 'Wall', 'position': [0., 5.1], 'orientation': np.pi / 2, 'length': 10.2},
+ {'type': 'Wall', 'position': [0., -5.1], 'orientation': -np.pi / 2, 'length': 10.2},
+ ],
+ )
+
+
+ agent = mpi_sim.objects.ARIRobot( # type: ignore
+ components=[dict(
+ type=mpi_sim.components.RayCastSensor,
+ name = 'raycast',
+ field_of_view = 180,
+ delta_angle = 90,
+
+ draw_raycast = False
+ )],
+ position = (0,0),
+ orientation = 0
+ )
+
+ sim.add_object(agent)
+ sim.set_reset_point()
+ sim.reset()
+ sim.step()
+ np.testing.assert_almost_equal(agent.raycast.raycast_distances, [5,5,5], decimal = 2)
+ np.testing.assert_almost_equal(agent.raycast.raycast_angle_degrees, [90,0,-90], decimal = 2)
+
+def test_3_rays_without_walls():
+
+ # create the simulation
+ sim = mpi_sim.Simulation(
+ visible_area = ((-6,6),(-6,6)),
+ )
+
+
+ agent = mpi_sim.objects.ARIRobot( # type: ignore
+ components=[dict(
+ type=mpi_sim.components.RayCastSensor,
+ name = 'raycast',
+ field_of_view = 180,
+ delta_angle = 90,
+ ray_max_range = 20,
+ draw_raycast = False
+ )],
+ position = (0,0),
+ orientation = 0
+ )
+
+ sim.add_object(agent)
+ sim.set_reset_point()
+ sim.reset()
+ sim.step()
+ np.testing.assert_almost_equal(agent.raycast.raycast_distances, [20,20,20], decimal = 2)
+ np.testing.assert_almost_equal(agent.raycast.raycast_angle_degrees, [90,0,-90], decimal = 2)
+
+if __name__ == "__main__":
+# test_3_rays_with_walls()
+# test_3_rays_without_walls()
+ manual_test()
\ No newline at end of file
diff --git a/test/components/test_social_mapping.py b/test/components/test_social_mapping.py
new file mode 100644
index 0000000000000000000000000000000000000000..4dd17710721345b47fc201ba088e4a8afd1a44c6
--- /dev/null
+++ b/test/components/test_social_mapping.py
@@ -0,0 +1,284 @@
+
+#TODO
+
+# import mpi_sim
+# import numpy.testing as np_test
+# from Box2D import b2PolygonShape, b2CircleShape
+# import numpy as np
+
+
+# class DummyAgentObject(mpi_sim.Object):
+
+# @staticmethod
+# def default_config():
+# dc = mpi_sim.Object.default_config()
+# dc.position = (0, 0)
+# dc.orientation = 1
+# dc.properties.append('dynamic')
+# return dc
+
+
+# @property
+# def position(self):
+# return self.box2d_body.position
+
+
+# @property
+# def orientation(self):
+# return self.box2d_body.angle
+
+
+# def _create_in_box2d_world(self, box2d_world):
+# self.box2d_body = box2d_world.CreateStaticBody(
+# position=self.config.position,
+# angle=self.config.orientation,
+# shapes=b2CircleShape(radius=.5),
+# )
+# return self.box2d_body
+
+
+# class PolygonObject(mpi_sim.Object):
+
+# @staticmethod
+# def default_config():
+# dc = mpi_sim.Object.default_config()
+# dc.position = (0, 0)
+# dc.width = 1
+# dc.height = 1
+# dc.properties.append('static')
+# return dc
+
+
+# def _create_in_box2d_world(self, box2d_world):
+# return box2d_world.CreateStaticBody(
+# position=self.config.position,
+# shapes=b2PolygonShape(box=(self.config.width, self.config.height)),
+# )
+
+
+# def test_no_rotation():
+
+# # create the simulation
+# sim = mpi_sim.Simulation(visible_area=((0, 10), (0, 5)))
+
+# agent = DummyAgentObject(
+# position=(0, 0),
+# orientation=0,
+# components=[dict(
+# type=mpi_sim.components.OccupancyGridMapping,
+# name='mapping',
+# object_filter=dict(properties='static'),
+# depth=3.,
+# resolution=1.,
+# )]
+# )
+
+# sim.add_object(agent)
+# sim.add_object(PolygonObject(position=(1,1), width=1.5, height=1.5))
+
+# # check default values before doing the mapping
+# expected_global = np.array([
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# ])
+# expected_global = np.flip(expected_global, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_global, agent.mapping.global_map)
+# assert agent.mapping.global_map_shape == agent.mapping.global_map.shape
+# assert agent.mapping.global_map_area == ((0, 10), (0, 5))
+
+# expected_local = np.array([
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# ])
+# expected_local = np.flip(expected_local, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_local, agent.mapping.local_map)
+# assert agent.mapping.local_map_shape == agent.mapping.local_map.shape
+# assert agent.mapping.local_map_area == ((-3, 3), (-3, 3))
+
+# #######################################
+
+# # update both maps
+# agent.mapping.update_global_map()
+# agent.mapping.update_local_map()
+
+# expected_global = np.array([
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# ])
+# expected_global = np.flip(expected_global, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_global, agent.mapping.global_map)
+# assert agent.mapping.global_map_shape == agent.mapping.global_map.shape
+# assert agent.mapping.global_map_area == ((0, 10), (0, 5))
+
+# expected_local = np.array([
+# [0., 0., 0., 1., 1., 1.],
+# [0., 0., 0., 1., 1., 1.],
+# [0., 0., 0., 1., 1., 1.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# ])
+# expected_local = np.flip(expected_local, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_local, agent.mapping.local_map)
+# assert agent.mapping.local_map_shape == agent.mapping.local_map.shape
+# assert agent.mapping.local_map_area == ((-3, 3), (-3, 3))
+
+
+# def test_east_rotation():
+
+# # create the simulation
+# sim = mpi_sim.Simulation(visible_area=((0, 10), (0, 5)))
+
+# agent = DummyAgentObject(
+# position=(0, 0),
+# orientation=-np.pi / 2,
+# components=[dict(
+# type=mpi_sim.components.OccupancyGridMapping,
+# name='mapping',
+# object_filter=dict(properties='static'),
+# depth=3.,
+# resolution=1.,
+# )]
+# )
+
+# sim.add_object(agent)
+# sim.add_object(PolygonObject(position=(1,1), width=1.5, height=1.5))
+
+# # update both maps
+# agent.mapping.update_global_map()
+# agent.mapping.update_local_map()
+
+# expected_global = np.array([
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# ])
+# expected_global = np.flip(expected_global, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_global, agent.mapping.global_map)
+# assert agent.mapping.global_map_shape == agent.mapping.global_map.shape
+# assert agent.mapping.global_map_area == ((0, 10), (0, 5))
+
+# expected_local = np.array([
+# [0., 1., 1., 1., 0., 0.],
+# [0., 1., 1., 1., 0., 0.],
+# [0., 1., 1., 1., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# ])
+# expected_local = np.flip(expected_local, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_local, agent.mapping.local_map)
+# assert agent.mapping.local_map_shape == agent.mapping.local_map.shape
+# assert agent.mapping.local_map_area == ((-3, 3), (-3, 3))
+
+
+# def test_west_rotation():
+
+# # create the simulation
+# sim = mpi_sim.Simulation(visible_area=((0, 10), (0, 5)))
+
+# agent = DummyAgentObject(
+# position=(0, 0),
+# orientation=np.pi / 2,
+# components=[dict(
+# type=mpi_sim.components.OccupancyGridMapping,
+# name='mapping',
+# object_filter=dict(properties='static'),
+# depth=3.,
+# resolution=1.,
+# )]
+# )
+
+# sim.add_object(agent)
+# sim.add_object(PolygonObject(position=(1,1), width=1.5, height=1.5))
+
+# # update both maps
+# agent.mapping.update_global_map()
+# agent.mapping.update_local_map()
+
+# expected_global = np.array([
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# ])
+# expected_global = np.flip(expected_global, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_global, agent.mapping.global_map)
+# assert agent.mapping.global_map_shape == agent.mapping.global_map.shape
+# assert agent.mapping.global_map_area == ((0, 10), (0, 5))
+
+# expected_local = np.array([
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 1., 1., 1.],
+# [0., 0., 0., 1., 1., 1.],
+# [0., 0., 0., 1., 1., 1.],
+# [0., 0., 0., 0., 0., 0.],
+# ])
+# expected_local = np.flip(expected_local, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_local, agent.mapping.local_map)
+# assert agent.mapping.local_map_shape == agent.mapping.local_map.shape
+# assert agent.mapping.local_map_area == ((-3, 3), (-3, 3))
+
+
+# def test_south_rotation():
+
+# # create the simulation
+# sim = mpi_sim.Simulation(visible_area=((0, 10), (0, 5)))
+
+# agent = DummyAgentObject(
+# position=(0, 0),
+# orientation=np.pi,
+# components=[dict(
+# type=mpi_sim.components.OccupancyGridMapping,
+# name='mapping',
+# object_filter=dict(properties='static'),
+# depth=3.,
+# resolution=1.,
+# )]
+# )
+
+# sim.add_object(agent)
+# sim.add_object(PolygonObject(position=(1,1), width=1.5, height=1.5))
+
+# # update both maps
+# agent.mapping.update_global_map()
+# agent.mapping.update_local_map()
+
+# expected_global = np.array([
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# [1., 1., 1., 0., 0., 0., 0., 0., 0., 0.],
+# ])
+# expected_global = np.flip(expected_global, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_global, agent.mapping.global_map)
+# assert agent.mapping.global_map_shape == agent.mapping.global_map.shape
+# assert agent.mapping.global_map_area == ((0, 10), (0, 5))
+
+# expected_local = np.array([
+# [0., 0., 0., 0., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# [0., 1., 1., 1., 0., 0.],
+# [0., 1., 1., 1., 0., 0.],
+# [0., 1., 1., 1., 0., 0.],
+# [0., 0., 0., 0., 0., 0.],
+# ])
+# expected_local = np.flip(expected_local, axis=0).transpose()
+# np_test.assert_array_almost_equal(expected_local, agent.mapping.local_map)
+# assert agent.mapping.local_map_shape == agent.mapping.local_map.shape
+# assert agent.mapping.local_map_area == ((-3, 3), (-3, 3))