diff --git a/mpi_sim/components/occupancy_grid_mapping.py b/mpi_sim/components/occupancy_grid_mapping.py index 69671fcc0cbe76e5201308f8f17226651a283355..aa43774bad537e3f487039c8e29bb25fa3d3b03b 100644 --- a/mpi_sim/components/occupancy_grid_mapping.py +++ b/mpi_sim/components/occupancy_grid_mapping.py @@ -25,6 +25,8 @@ class OccupancyGridMapping(SensorComponent): 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 @@ -205,10 +207,11 @@ class OccupancyGridMapping(SensorComponent): 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, # @ np.array([[0, 1],[-1, 0]]), - orientation=constraint_angle(np.pi/2+self.object.orientation), # TODO : define a function that adjust the angle depending on the map + 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 + 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 @@ -229,9 +232,10 @@ class OccupancyGridMapping(SensorComponent): 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(np.pi/2+self.object.orientation) + self.config.orientation_noise*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 + 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 diff --git a/mpi_sim/objects/ari_robot.py b/mpi_sim/objects/ari_robot.py index 40e68b0eefb570a534d8a2b3df30b973f36f0b79..854461ccd5abfe558b41fc80c9c38e6e20df4e1c 100644 --- a/mpi_sim/objects/ari_robot.py +++ b/mpi_sim/objects/ari_robot.py @@ -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 @@ -262,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), diff --git a/mpi_sim/utils/occupancy_grid_map.py b/mpi_sim/utils/occupancy_grid_map.py index 67a5870ce78219b9d5cc13feb12fb3db9fa56c9a..789dbb2676eeb58464c5dc924d4d71f62e6ddde0 100644 --- a/mpi_sim/utils/occupancy_grid_map.py +++ b/mpi_sim/utils/occupancy_grid_map.py @@ -180,7 +180,7 @@ def create_occupancy_grid_map(simulation, area=None, resolution=0.05, object_fil return map -def create_local_perspective_occupancy_grid_map(global_map, depth=5.0, position=(0,0), orientation=0.0, from_given_center=None): +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: @@ -209,9 +209,12 @@ def create_local_perspective_occupancy_grid_map(global_map, depth=5.0, position= 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]]) + 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)