Mentions légales du service

Skip to content
Snippets Groups Projects
Commit 953aecc1 authored by SANCHEZ Victor's avatar SANCHEZ Victor
Browse files

adding Social Map Component

parent cd2a6628
Branches
No related tags found
No related merge requests found
[submodule "modules/Motor_Controller"]
path = modules/Motor_Controller
url = git@gitlab.inria.fr:spring/wp6_robot_behavior/Motor_Controller.git
[submodule "modules/social_space_navigation_models"]
path = modules/social_space_navigation_models
url = git@gitlab.inria.fr:creinke/social-space-navigation-models.git
......@@ -3,9 +3,13 @@ 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
......
......@@ -9,7 +9,7 @@ import yaml
from collections import namedtuple
from mpi_sim.utils.box2d_utils import constraint_angle
import cv2
import os
# TODO: need group identification for SocialMPC
......@@ -69,6 +69,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 +147,14 @@ 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)
# 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)
self.robot_config = RobotConfig()
# initialize the robot state
self.state.robot_state = RobotState()
......
......@@ -2,7 +2,7 @@ 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
......@@ -75,6 +75,13 @@ class OccupancyGridMapping(SensorComponent):
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)
......@@ -107,6 +114,7 @@ class OccupancyGridMapping(SensorComponent):
if isinstance(self.config.imported_global_map_path, str):
imported_global_map = np.load(file=self.config.imported_global_map_path)
self.state.global_map = mpi_sim.utils.OccupancyGridMap(
map=imported_global_map,
area=self._global_map_area,
......@@ -151,7 +159,8 @@ class OccupancyGridMapping(SensorComponent):
transform=self.config.transform
)
else :
imported_global_map = np.load(file=self.config.imported_global_map_path)
imported_global_map = np.load(file=self.config.imported_global_map_path)
#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:
imported_global_map=self.config.transform(imported_global_map)
self.state.global_map = mpi_sim.utils.OccupancyGridMap(
......@@ -180,7 +189,7 @@ class OccupancyGridMapping(SensorComponent):
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),
orientation=constraint_angle(np.pi/2+self.object.orientation),
depth=self.config.depth,
from_given_center = self.config.imported_global_map_px_coord_center
)
......
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
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_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._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_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
)
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):
self.state.global_map = mpi_sim.utils.create_social_map(
self.simulation,
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()
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,
)
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()
......@@ -40,3 +40,13 @@ from mpi_sim.utils.occupancy_grid_map import transform_position_to_map_coordinat
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
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):
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).
object_filter: Defines which objects should be on the map. (Default: None)
If None, then all objects.
Otherwise, a dictionary that can have the following fields: properties, ids, types
properties: List of object properties (strings). See Object.properties.
ids: List of object ids (ints).
types: List of object types (classes).
operator: Either 'or' or 'and'. Defines how the different filters are combined. (Default: 'or')
All objects that fulfill these requirements are added.
default_occupancy_value (float): Occupancy value of objects (Default: 1.0)
occupancy_value_depth (integer): Number of bits used to encode the occupancy value (Default: 32).
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 bodie in bodies :
x, y = np.array(bodie.position)
theta = bodie.angle
# TODO : Study why the speed is zero
v = 2 + np.sqrt(np.array(bodie.linearVelocity)[0]**2 + np.array(bodie.linearVelocity)[1]**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)
global_social_map = ssn.calc_eps(x_mesh, y_mesh, persons)
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):
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 :
center_x, center_y = from_given_center
position_px = position/global_map.resolution
pos_in_map = np.array([center_x - np.array(position_px)[1], center_y + np.array(position_px)[0]])
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)
This diff is collapsed.
......@@ -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
......
......@@ -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.16
#python_requires =
# >=3.8, <=3.9.16
# required packages are in the setup.py
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment