diff --git a/examples/tools/pybullet_linear_tire_example.py b/examples/tools/pybullet_linear_tire_example.py index 959fd22a28..643ed38d9f 100644 --- a/examples/tools/pybullet_linear_tire_example.py +++ b/examples/tools/pybullet_linear_tire_example.py @@ -4,14 +4,14 @@ import matplotlib.pyplot as plt import numpy as np -from smarts.core.chassis import AckermannChassis +from smarts.bullet import pybullet +from smarts.bullet.chassis import BulletAckermannChassis +from smarts.bullet.pybullet import bullet_client as bc from smarts.core.controllers.actuator_dynamic_controller import ( ActuatorDynamicController, ActuatorDynamicControllerState, ) from smarts.core.coordinates import Heading, Pose -from smarts.core.utils import pybullet -from smarts.core.utils.pybullet import bullet_client as bc from smarts.core.vehicle import Vehicle TIMESTEP_SEC = 1 / 240 @@ -165,7 +165,7 @@ def run(client, vehicle, plane_body_id, sliders, n_steps=1e6): client.stepSimulation() - frictions_ = frictions(sliders) + frictions_ = frictions(client, sliders) if prev_friction_sum is not None and not math.isclose( sum(frictions_.values()), prev_friction_sum @@ -188,7 +188,7 @@ def run(client, vehicle, plane_body_id, sliders, n_steps=1e6): ) -def frictions(sliders): +def frictions(client, sliders): return dict( lateralFriction=client.readUserDebugParameter(sliders["lateral_friction"]), spinningFriction=client.readUserDebugParameter(sliders["spinning_friction"]), @@ -201,7 +201,7 @@ def frictions(sliders): ) -if __name__ == "__main__": +def main(): # https://turtlemonvh.github.io/python-multiprocessing-and-corefoundation-libraries.html # mp.set_start_method('spawn', force=True) @@ -242,12 +242,12 @@ def frictions(sliders): path = str(path.absolute()) plane_body_id = client.loadURDF(path, useFixedBase=True) - client.changeDynamics(plane_body_id, -1, **frictions(sliders)) + client.changeDynamics(plane_body_id, -1, **frictions(client, sliders)) pose = pose = Pose.from_center((0, 0, 0), Heading(0)) vehicle = Vehicle( "hello", - chassis=AckermannChassis( + chassis=BulletAckermannChassis( pose=pose, bullet_client=client, tire_parameters_filepath="../../smarts/core/models/tire_parameters.yaml", @@ -311,3 +311,7 @@ def frictions(sliders): plt.plot(time, yaw) plt.show() + + +if __name__ == "__main__": + main() diff --git a/examples/tools/pybullet_sumo_orientation_example.py b/examples/tools/pybullet_sumo_orientation_example.py index 0c3ebd8750..3dabf5ad6a 100644 --- a/examples/tools/pybullet_sumo_orientation_example.py +++ b/examples/tools/pybullet_sumo_orientation_example.py @@ -4,13 +4,13 @@ import numpy as np +from smarts.bullet import pybullet +from smarts.bullet.chassis import BulletBoxChassis +from smarts.bullet.pybullet import bullet_client as bc from smarts.core.actor import ActorRole -from smarts.core.chassis import BoxChassis from smarts.core.coordinates import Heading, Pose from smarts.core.scenario import Scenario from smarts.core.sumo_traffic_simulation import SumoTrafficSimulation -from smarts.core.utils import pybullet -from smarts.core.utils.pybullet import bullet_client as bc from smarts.core.vehicle import VEHICLE_CONFIGS, Vehicle from smarts.core.vehicle_state import VehicleState @@ -96,7 +96,7 @@ def run( current_provider_state = traffic_sim.step(None, 0.01, step * 0.01) for pose, i in zip(injected_poses, range(len(injected_poses))): converted_to_provider = VehicleState( - vehicle_id=f"EGO{i}", + actor_id=f"EGO{i}", vehicle_config_type="passenger", pose=pose, dimensions=passenger_dimen, @@ -115,7 +115,7 @@ def run( pose = Pose.from_center([0, 0, 0], Heading(0)) vehicles[v_id] = Vehicle( id=v_id, - chassis=BoxChassis( + chassis=BulletBoxChassis( pose=pose, speed=0, dimensions=vehicle_config.dimensions, @@ -144,7 +144,7 @@ def run( # pytype: enable=name-error -if __name__ == "__main__": +def main(): # https://turtlemonvh.github.io/python-multiprocessing-and-corefoundation-libraries.html # mp.set_start_method('spawn', force=True) @@ -181,3 +181,7 @@ def run( plane_body_id, n_steps=int(1e6), ) + + +if __name__ == "__main__": + main() diff --git a/examples/tools/pybullet_trajectory_example.py b/examples/tools/pybullet_trajectory_example.py index 0f648325f9..9abe8e16ab 100644 --- a/examples/tools/pybullet_trajectory_example.py +++ b/examples/tools/pybullet_trajectory_example.py @@ -3,14 +3,14 @@ import matplotlib.pyplot as plt -from smarts.core.chassis import AckermannChassis +from smarts.bullet import pybullet +from smarts.bullet.chassis import BulletAckermannChassis +from smarts.bullet.pybullet import bullet_client as bc from smarts.core.controllers import ( TrajectoryTrackingController, TrajectoryTrackingControllerState, ) from smarts.core.coordinates import Heading, Pose -from smarts.core.utils import pybullet -from smarts.core.utils.pybullet import bullet_client as bc from smarts.core.vehicle import Vehicle TIMESTEP_SEC = 1 / 240 @@ -79,18 +79,12 @@ def run(client, vehicle, plane_body_id, sliders, n_steps=1e6): vehicle, controller_state, dt_sec=TIMESTEP_SEC, - heading_gain=0.05, - lateral_gain=0.65, - velocity_gain=1.8, - traction_gain=2, - derivative_activation=False, - speed_reduction_activation=False, ) client.stepSimulation() vehicle.sync_chassis() - frictions_ = frictions(sliders) + frictions_ = frictions(client, sliders) if prev_friction_sum is not None and not math.isclose( sum(frictions_.values()), prev_friction_sum @@ -112,7 +106,7 @@ def run(client, vehicle, plane_body_id, sliders, n_steps=1e6): ydes.append(trajectory[1][0]) -def frictions(sliders): +def frictions(client, sliders): return dict( throttle=client.addUserDebugParameter("Throttle", 0, 1, 0.0), brake=client.addUserDebugParameter("Brake", 0, 1, 0), @@ -128,7 +122,7 @@ def frictions(sliders): ) -if __name__ == "__main__": +def main(): # https://turtlemonvh.github.io/python-multiprocessing-and-corefoundation-libraries.html # mp.set_start_method('spawn', force=True) @@ -166,12 +160,12 @@ def frictions(sliders): path = str(path.absolute()) plane_body_id = client.loadURDF(path, useFixedBase=True) - client.changeDynamics(plane_body_id, -1, **frictions(sliders)) + client.changeDynamics(plane_body_id, -1, **frictions(client, sliders)) pose = pose = Pose.from_center((0, 0, 0), Heading(0)) vehicle = Vehicle( id="vehicle", - chassis=AckermannChassis( + chassis=BulletAckermannChassis( pose=pose, bullet_client=client, ), @@ -200,3 +194,7 @@ def frictions(sliders): plt.plot(xdes, ydes) plt.plot(xx, yy) plt.show() + + +if __name__ == "__main__": + main() diff --git a/examples/tools/pybullet_vehicle_example.py b/examples/tools/pybullet_vehicle_example.py index 5b6fa2e9ad..c04e3003b7 100644 --- a/examples/tools/pybullet_vehicle_example.py +++ b/examples/tools/pybullet_vehicle_example.py @@ -1,14 +1,14 @@ import math from pathlib import Path -from smarts.core.chassis import AckermannChassis +from smarts.bullet import pybullet +from smarts.bullet.chassis import BulletAckermannChassis +from smarts.bullet.pybullet import bullet_client as bc from smarts.core.controllers.actuator_dynamic_controller import ( ActuatorDynamicController, ActuatorDynamicControllerState, ) from smarts.core.coordinates import Heading, Pose -from smarts.core.utils import pybullet -from smarts.core.utils.pybullet import bullet_client as bc from smarts.core.vehicle import Vehicle TIMESTEP_SEC = 1 / 240 @@ -90,7 +90,7 @@ def frictions(sliders, client): ) -if __name__ == "__main__": +def main(): # https://turtlemonvh.github.io/python-multiprocessing-and-corefoundation-libraries.html # mp.set_start_method('spawn', force=True) @@ -136,7 +136,7 @@ def frictions(sliders, client): pose = pose = Pose.from_center((0, 0, 0), Heading(0)) vehicle = Vehicle( id="vehicle", - chassis=AckermannChassis( + chassis=BulletAckermannChassis( pose=pose, bullet_client=client, ), @@ -149,3 +149,7 @@ def frictions(sliders, client): sliders, n_steps=int(1e6), ) + + +if __name__ == "__main__": + main() diff --git a/smarts/bullet/__init__.py b/smarts/bullet/__init__.py new file mode 100644 index 0000000000..2c65923417 --- /dev/null +++ b/smarts/bullet/__init__.py @@ -0,0 +1,21 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. diff --git a/smarts/core/utils/bullet.py b/smarts/bullet/bullet.py similarity index 98% rename from smarts/core/utils/bullet.py rename to smarts/bullet/bullet.py index 29cab194c4..caab096a01 100644 --- a/smarts/core/utils/bullet.py +++ b/smarts/bullet/bullet.py @@ -23,9 +23,9 @@ import numpy as np +from smarts.bullet import pybullet +from smarts.bullet.pybullet import bullet_client as bc from smarts.core.coordinates import Pose -from smarts.core.utils import pybullet -from smarts.core.utils.pybullet import bullet_client as bc class BulletClient: diff --git a/smarts/bullet/bullet_simulation.py b/smarts/bullet/bullet_simulation.py new file mode 100644 index 0000000000..27a4aeea86 --- /dev/null +++ b/smarts/bullet/bullet_simulation.py @@ -0,0 +1,159 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +import importlib.resources as pkg_resources +import os +from typing import Any, Optional + +from smarts.bullet import pybullet +from smarts.bullet.pybullet import bullet_client as bc +from smarts.core import config, models +from smarts.core.coordinates import BoundingBox +from smarts.core.physics.physics_simulation import PhysicsSimulation +from smarts.core.simulation_frame import SimulationFrame +from smarts.core.vehicle_index import VehicleIndex + +MAX_PYBULLET_FREQ = 240 + + +class BulletSimulation(PhysicsSimulation): + """The pybullet implementation of the physics simulation. + + This can configure GUI with SMARTS_BULLET_GUI=[MAC|LINUX|OFF]. + """ + + def __init__(self) -> None: + gui: str = config()("bullet", "gui", default="OFF", cast=lambda s: s.upper()) + + self._bullet_mode = pybullet.DIRECT # pylint: disable=no-member + if gui == "LINUX": + self._bullet_mode = pybullet.GUI # pylint: disable=no-member + # For macOS GUI. See our `BulletClient` docstring for details. + # from .utils.bullet import BulletClient + # self._bullet_client = BulletClient(pybullet.GUI) + # For macOS GUI. See our `BulletClient` docstring for details. + # from .utils.bullet import BulletClient + # self._bullet_client = BulletClient(pybullet.GUI) + if gui == "MAC": + from smarts.bullet.bullet import BulletClient + + self._bullet_client = BulletClient( + pybullet.GUI # pylint: disable=no-member + ) + else: + self._bullet_client = pybullet.SafeBulletClient( + self._bullet_mode + ) # pylint: disable=no-member + self._pybullet_period = 0.1 + self._map_bb = None + self._ground_bullet_id = None + self._max_pybullet_freq: int = config()( + "physics", "max_pybullet_freq", default=MAX_PYBULLET_FREQ, cast=int + ) + + def initialize(self, fixed_timestep_sec: float): + self._bullet_client.resetSimulation() + self._bullet_client.configureDebugVisualizer( + pybullet.COV_ENABLE_GUI, 0 # pylint: disable=no-member + ) + # PyBullet defaults the timestep to 240Hz. Several parameters are tuned with + # this value in mind. For example the number of solver iterations and the error + # reduction parameters (erp) for contact, friction and non-contact joints. + # Attempting to get around this we set the number of substeps so that + # timestep * substeps = 240Hz. Bullet (C++) does something to this effect as + # well (https://git.io/Jvf0M), but PyBullet does not expose it. + # But if our timestep is variable (due to being externally driven) + # then we will step pybullet multiple times ourselves as necessary + # to account for the time delta on each SMARTS step. + self._pybullet_period = ( + fixed_timestep_sec if fixed_timestep_sec else 1 / self._max_pybullet_freq + ) + self._bullet_client.setPhysicsEngineParameter( + fixedTimeStep=self._pybullet_period, + numSubSteps=int(self._pybullet_period * self._max_pybullet_freq), + numSolverIterations=10, + solverResidualThreshold=0.001, + # warmStartingFactor=0.99 + ) + + self._bullet_client.setGravity(0, 0, -9.8) + + def initialize_ground(self, resource_path, map_bb): + plane_path = resource_path # self._scenario.plane_filepath + if resource_path is None or not os.path.exists(plane_path): + with pkg_resources.path(models, "plane.urdf") as path: + plane_path = str(path.absolute()) + + self._map_bb = map_bb + + if self._map_bb: + # 1e6 is the default value for plane length and width in smarts/models/plane.urdf. + DEFAULT_PLANE_DIM = 1e6 + ground_plane_scale = ( + 2.2 * max(self._map_bb.length, self._map_bb.width) / DEFAULT_PLANE_DIM + ) + ground_plane_center = self._map_bb.center + else: + # first step on undefined map, just use a big scale (1e6). + # it should get updated as soon as vehicles are added... + ground_plane_scale = 1.0 + ground_plane_center = (0, 0, 0) + + if self._ground_bullet_id is not None: + self._bullet_client.removeBody(self._ground_bullet_id) + self._ground_bullet_id = None + + self._ground_bullet_id = self._bullet_client.loadURDF( + plane_path, + useFixedBase=True, + basePosition=ground_plane_center, + globalScaling=ground_plane_scale, + ) + + def step( + self, + dt: float, + simulation_frame: Optional[SimulationFrame], + vehicle_index: VehicleIndex, + ): + self._bullet_client.stepSimulation() + pybullet_substeps = max(1, round(dt / self._pybullet_period)) - 1 + for _ in range(pybullet_substeps): + for vehicle in vehicle_index.vehicles: + vehicle.chassis.reapply_last_control() + self._bullet_client.stepSimulation() + + def reset_simulation(self): + self._bullet_client.resetSimulation() + + def teardown(self): + self._bullet_client.disconnect() + + def collider_by_id(self, entity_id: Any): + pass + + @property + def simulation_bounding_box(self) -> BoundingBox: + return self._map_bb + + @property + def client(self): + return self._bullet_client diff --git a/smarts/core/chassis.py b/smarts/bullet/chassis.py similarity index 89% rename from smarts/core/chassis.py rename to smarts/bullet/chassis.py index 14c613d3c4..1c1ef6b61a 100644 --- a/smarts/core/chassis.py +++ b/smarts/bullet/chassis.py @@ -30,24 +30,25 @@ from shapely.geometry import Polygon from shapely.geometry import box as shapely_box -from smarts.core import models -from smarts.core.coordinates import Dimensions, Heading, Pose -from smarts.core.tire_models import TireForces -from smarts.core.utils import pybullet -from smarts.core.utils.bullet import ( +from smarts.bullet import pybullet +from smarts.bullet.bullet import ( BulletBoxShape, BulletPositionConstraint, ContactPoint, JointInfo, JointState, ) +from smarts.bullet.pybullet import bullet_client as bc +from smarts.core import models +from smarts.core.coordinates import Dimensions, Heading, Pose +from smarts.core.physics.chassis import AckermannChassis, BoxChassis, Chassis +from smarts.core.tire_models import TireForces from smarts.core.utils.math import ( min_angles_difference_signed, radians_to_vec, vec_to_radians, yaw_from_quaternion, ) -from smarts.core.utils.pybullet import bullet_client as bc with pkg_resources.path(models, "vehicle.urdf") as path: DEFAULT_VEHICLE_FILEPATH = str(path.absolute()) @@ -85,111 +86,7 @@ def _query_bullet_contact_points(bullet_client, bullet_id, link_index): return contact_points -class Chassis: - """Represents a vehicle chassis.""" - - def control(self, *args, **kwargs): - """Apply control values to the chassis.""" - raise NotImplementedError - - def reapply_last_control(self): - """Re-apply the last given control given to the chassis.""" - raise NotImplementedError - - def teardown(self): - """Clean up resources.""" - raise NotImplementedError - - @property - def dimensions(self) -> Dimensions: - """The fitted front aligned dimensions of the chassis.""" - raise NotImplementedError - - @property - def contact_points(self) -> Sequence: - """The contact point of the chassis.""" - raise NotImplementedError - - @property - def bullet_id(self) -> str: - """The physics id of the chassis physics body.""" - raise NotImplementedError - - @property - def velocity_vectors(self) -> Tuple[np.ndarray, np.ndarray]: - """Returns linear velocity vector in m/s and angular velocity in rad/sec.""" - raise NotImplementedError - - @property - def speed(self) -> float: - """The speed of the chassis in the facing direction of the chassis.""" - raise NotImplementedError - - def set_pose(self, pose: Pose): - """Use with caution since it disrupts the physics simulation. Sets the pose of the - chassis. - """ - raise NotImplementedError - - @speed.setter - def speed(self, speed: float): - """Apply GCD from front-end.""" - raise NotImplementedError - - @property - def pose(self) -> Pose: - """The pose of the chassis.""" - raise NotImplementedError - - @property - def steering(self) -> float: - """The steering value of the chassis in radians [-math.pi, math.pi].""" - raise NotImplementedError - - @property - def yaw_rate(self) -> float: - """The turning rate of the chassis in radians.""" - raise NotImplementedError - - def inherit_physical_values(self, other: "Chassis"): - """Apply GCD between the two chassis.""" - raise NotImplementedError - - @property - def to_polygon(self) -> Polygon: - """Convert the chassis to a 2D shape.""" - p = self.pose.as_position2d() - d = self.dimensions - poly = shapely_box( - p[0] - d.width * 0.5, - p[1] - d.length * 0.5, - p[0] + d.width * 0.5, - p[1] + d.length * 0.5, - ) - return shapely_rotate(poly, self.pose.heading, use_radians=True) - - def step(self, current_simulation_time): - """Update the chassis state.""" - raise NotImplementedError - - def state_override( - self, - dt: float, - force_pose: Pose, - linear_velocity: Optional[np.ndarray] = None, - angular_velocity: Optional[np.ndarray] = None, - ): - """Use with care! In essence, this is tinkering with the physics of the world, - and may have unintended behavioral or performance consequences.""" - raise NotImplementedError - - def set_pose(self, pose: Pose): - """Use with caution since it disrupts the physics simulation. Sets the pose of the - chassis.""" - raise NotImplementedError - - -class BoxChassis(Chassis): +class BulletBoxChassis(BoxChassis): """Control a vehicle by setting its absolute position and heading. The collision shape of the vehicle is a box of the provided dimensions. """ @@ -325,7 +222,7 @@ def teardown(self): self._bullet_body.teardown() -class AckermannChassis(Chassis): +class BulletAckermannChassis(AckermannChassis): """Control a vehicle by applying forces on its joints. The joints and links are defined by a URDF file. """ diff --git a/smarts/core/utils/pybullet.py b/smarts/bullet/pybullet.py similarity index 100% rename from smarts/core/utils/pybullet.py rename to smarts/bullet/pybullet.py diff --git a/smarts/core/agents_provider.py b/smarts/core/agents_provider.py index 2c3d0186c7..340250a3fc 100644 --- a/smarts/core/agents_provider.py +++ b/smarts/core/agents_provider.py @@ -92,7 +92,7 @@ def teardown(self): self._my_agent_actors = dict() def sync(self, provider_state): - # Here we don't care what other providers are doing (pybullet cares for us). + # Here we don't care what other providers are doing (physics simulation cares for us). # The removal of missing actors is done in perform_agent_actions() instead of here # b/c these are agent actors and so get removed when they are done. pass diff --git a/smarts/core/controllers/direct_controller.py b/smarts/core/controllers/direct_controller.py index a1c0f13b93..f190883fe5 100644 --- a/smarts/core/controllers/direct_controller.py +++ b/smarts/core/controllers/direct_controller.py @@ -22,8 +22,8 @@ import numpy as np -from smarts.core.chassis import AckermannChassis, BoxChassis from smarts.core.coordinates import Pose +from smarts.core.physics.chassis import AckermannChassis, BoxChassis from smarts.core.utils.math import fast_quaternion_from_angle, radians_to_vec @@ -67,7 +67,7 @@ def perform_action( target_heading = (vehicle.heading + angular_velocity * dt) % (2 * math.pi) if isinstance(chassis, BoxChassis): - # Since BoxChassis does not use pybullet for force-to-motion computations (only collision detection), + # Since BoxChassis does not use the physics simulation for force-to-motion computations (only collision detection), # we have to update the position and other state here (instead of pybullet.stepSimulation()). heading_vec = radians_to_vec(vehicle.heading) dpos = heading_vec * vehicle.speed * dt diff --git a/smarts/core/controllers/lane_following_controller.py b/smarts/core/controllers/lane_following_controller.py index f92d3c3791..7b98b0a6b6 100644 --- a/smarts/core/controllers/lane_following_controller.py +++ b/smarts/core/controllers/lane_following_controller.py @@ -22,10 +22,10 @@ import numpy as np from scipy import signal -from smarts.core.chassis import AckermannChassis from smarts.core.controllers.trajectory_tracking_controller import ( TrajectoryTrackingController, ) +from smarts.core.physics.chassis import AckermannChassis from smarts.core.utils.math import lerp, low_pass_filter, min_angles_difference_signed METER_PER_SECOND_TO_KM_PER_HR = 3.6 @@ -194,7 +194,7 @@ def perform_lane_following( velocity_error_damping_term = (speed_error - state.speed_error) / sim.last_dt # 5.5 is the gain of feedforward term for throttle. This term is # directly related to the steering angle, this is added to further - # enhance the speed tracking performance. TODO: currently, the bullet + # enhance the speed tracking performance. TODO: currently, the bullet model # does not provide the lateral acceleration which is needed for # calculating the front lateral force. we need to replace the coefficient # with better approximation of the front lateral forces using explicit diff --git a/smarts/core/controllers/motion_planner_controller.py b/smarts/core/controllers/motion_planner_controller.py index 5275cb4920..2d06bbd0cc 100644 --- a/smarts/core/controllers/motion_planner_controller.py +++ b/smarts/core/controllers/motion_planner_controller.py @@ -22,8 +22,8 @@ import numpy as np from smarts.core.bezier_motion_planner import BezierMotionPlanner -from smarts.core.chassis import BoxChassis from smarts.core.coordinates import Heading, Pose +from smarts.core.physics.chassis import BoxChassis class MotionPlannerControllerState: diff --git a/smarts/core/controllers/trajectory_interpolation_controller.py b/smarts/core/controllers/trajectory_interpolation_controller.py index 4046767fd5..01586065e6 100644 --- a/smarts/core/controllers/trajectory_interpolation_controller.py +++ b/smarts/core/controllers/trajectory_interpolation_controller.py @@ -23,8 +23,8 @@ import numpy as np -from smarts.core.chassis import BoxChassis from smarts.core.coordinates import Heading, Pose +from smarts.core.physics.chassis import BoxChassis class TrajectoryField(IntEnum): diff --git a/smarts/core/lidar.py b/smarts/core/lidar.py index 84687a999f..91912ebbf6 100644 --- a/smarts/core/lidar.py +++ b/smarts/core/lidar.py @@ -19,15 +19,14 @@ # THE SOFTWARE. import itertools import random -from typing import List, Sequence, Tuple +from typing import List, Tuple import numpy as np import psutil +from ..bullet import pybullet from .lidar_sensor_params import SensorParams -from .utils import pybullet from .utils.math import batches, rotate_quat -from .utils.pybullet import bullet_client as bc class Lidar: diff --git a/smarts/core/physics/__init__.py b/smarts/core/physics/__init__.py new file mode 100644 index 0000000000..2c65923417 --- /dev/null +++ b/smarts/core/physics/__init__.py @@ -0,0 +1,21 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. diff --git a/smarts/core/physics/chassis.py b/smarts/core/physics/chassis.py new file mode 100644 index 0000000000..908be34fcc --- /dev/null +++ b/smarts/core/physics/chassis.py @@ -0,0 +1,296 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +from typing import Optional, Sequence + +import numpy as np +from cached_property import cached_property +from shapely.affinity import rotate as shapely_rotate +from shapely.geometry import Polygon +from shapely.geometry import box as shapely_box + +from smarts.core.coordinates import Dimensions, Pose +from smarts.core.physics.collider import ColliderBase + + +class Chassis(ColliderBase): + """Represents a vehicle chassis.""" + + def control(self, *args, **kwargs): + """Apply control values to the chassis.""" + raise NotImplementedError + + def reapply_last_control(self): + """Re-apply the last given control given to the chassis.""" + raise NotImplementedError + + def teardown(self): + """Clean up resources.""" + raise NotImplementedError + + @property + def bullet_id(self) -> str: + """The physics id of the chassis physics body.""" + raise NotImplementedError + + def set_pose(self, pose: Pose): + """Use with caution since it disrupts the physics simulation. Sets the pose of the + chassis. + """ + raise NotImplementedError + + @property + def steering(self) -> Optional[float]: + """The steering value of the chassis in radians [-math.pi:math.pi].""" + raise NotImplementedError + + def inherit_physical_values(self, other: "Chassis"): + """Apply GCD between the two chassis.""" + raise NotImplementedError + + @property + def to_polygon(self) -> Polygon: + """Convert the chassis to a 2D shape.""" + p = self.pose.as_position2d() + d = self.dimensions + poly = shapely_box( + p[0] - d.width * 0.5, + p[1] - d.length * 0.5, + p[0] + d.width * 0.5, + p[1] + d.length * 0.5, + ) + return shapely_rotate(poly, self.pose.heading, use_radians=True) + + def step(self, current_simulation_time): + """Update the chassis state.""" + raise NotImplementedError + + +class BoxChassis(Chassis): + """Control a vehicle by setting its absolute position and heading. The collision + shape of the vehicle is a box of the provided dimensions. + """ + + def control(self, pose: Pose, speed: float, dt: float = 0): + raise NotImplementedError + + def reapply_last_control(self): + # no need to do anything here since we're not applying forces + raise NotImplementedError + + def state_override( + self, + dt: float, + force_pose: Pose, + linear_velocity: Optional[np.ndarray] = None, + angular_velocity: Optional[np.ndarray] = None, + ): + """Use with care! In essence, this is tinkering with the physics of the world, + and may have unintended behavioral or performance consequences.""" + raise NotImplementedError + + def set_pose(self, pose: Pose): + raise NotImplementedError + + @property + def dimensions(self) -> Dimensions: + raise NotImplementedError + + @property + def contact_points(self) -> Sequence: + raise NotImplementedError + + @property + def bullet_id(self) -> str: + raise NotImplementedError + + @property + def speed(self) -> float: + raise NotImplementedError + + @property + def velocity_vectors(self): + raise NotImplementedError + + @speed.setter + def speed(self, speed: Optional[float] = None): + raise NotImplementedError + + @property + def pose(self) -> Pose: + raise NotImplementedError + + @property + def steering(self) -> Optional[float]: + raise NotImplementedError + + @property + def yaw_rate(self) -> Optional[float]: + raise NotImplementedError + + def inherit_physical_values(self, other: Chassis): + raise NotImplementedError + + def step(self, current_simulation_time): + raise NotImplementedError + + def teardown(self): + raise NotImplementedError + + +class AckermannChassis(Chassis): + """Control a vehicle by applying forces on its joints. The joints and links are + defined by a URDF file. + """ + + @property + def pose(self) -> Pose: + raise NotImplementedError + + def set_pose(self, pose: Pose): + raise NotImplementedError + + @property + def steering(self): + raise NotImplementedError + + @property + def speed(self) -> float: + raise NotImplementedError + + @property + def velocity_vectors(self): + raise NotImplementedError + + @speed.setter + def speed(self, speed: Optional[float] = None): + raise NotImplementedError + + @property + def yaw_rate(self) -> float: + """Returns 2-D rotational speed in rad/sec.""" + raise NotImplementedError + + @cached_property + def longitudinal_lateral_speed(self): + """Returns speed in m/s.""" + raise NotImplementedError + + @property + def front_rear_stiffness(self): + """The front and rear stiffness values of the tires on this chassis.""" + raise NotImplementedError + + @property + def approx_max_speed(self): + """This is the scientifically discovered maximum speed of this vehicle model""" + raise NotImplementedError + + @property + def contact_points(self): + raise NotImplementedError + + @cached_property + def mass_and_inertia(self): + """The mass and inertia values of this chassis.""" + raise NotImplementedError + + @property + def controller_parameters(self): + """The current controller parameters for this chassis.""" + raise NotImplementedError + + @property + def dimensions(self): + raise NotImplementedError + + @property + def max_steering_wheel(self): + """Maximum steering output for the current gear ratio.""" + raise NotImplementedError + + @property + def wheel_radius(self): + """The wheel radius of the wheels on the chassis.""" + raise NotImplementedError + + @property + def front_rear_axle_CG_distance(self): + """The axle offsets from the vehicle base.""" + raise NotImplementedError + + @property + def front_track_width(self): + """The track width between the front wheels.""" + raise NotImplementedError + + @property + def rear_track_width(self): + """The track width between the back wheels.""" + raise NotImplementedError + + @property + def max_torque(self): + """The maximum throttle torque.""" + raise NotImplementedError + + @property + def max_btorque(self): + """The maximum break torque.""" + raise NotImplementedError + + @property + def steering_ratio(self): + """The steering gear ratio""" + raise NotImplementedError + + @property + def bullet_id(self) -> str: + raise NotImplementedError + + @property + def bullet_client(self): + """The bullet physics simulator.""" + raise NotImplementedError + + def step(self, current_simulation_time): + raise NotImplementedError + + def inherit_physical_values(self, other: BoxChassis): + raise NotImplementedError + + def teardown(self): + raise NotImplementedError + + def control(self, throttle=0, brake=0, steering=0): + raise NotImplementedError + + def reapply_last_control(self): + raise NotImplementedError + + def state_override( + self, + dt: float, + force_pose: Pose, + linear_velocity: Optional[np.ndarray] = None, + angular_velocity: Optional[np.ndarray] = None, + ): + raise NotImplementedError diff --git a/smarts/core/physics/collider.py b/smarts/core/physics/collider.py new file mode 100644 index 0000000000..347281b9e8 --- /dev/null +++ b/smarts/core/physics/collider.py @@ -0,0 +1,87 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + + +from typing import Optional, Sequence, Tuple + +import numpy as np + +from smarts.core.coordinates import Dimensions, Pose + + +class ColliderBase: + """The base collider""" + + @property + def dimensions(self) -> Dimensions: + """The fitted front aligned dimensions of the chassis.""" + raise NotImplementedError + + @property + def contact_points(self) -> Sequence: + """The contact point of the chassis.""" + raise NotImplementedError + + @property + def speed(self) -> float: + """The speed of the chassis in the facing direction of the chassis.""" + raise NotImplementedError + + @speed.setter + def speed(self, speed: float): + """Apply GCD from front-end.""" + raise NotImplementedError + + @property + def pose(self) -> Pose: + """The pose of the chassis.""" + raise NotImplementedError + + def set_pose(self, pose: Pose): + """Use with caution since it disrupts the physics simulation. Sets the pose of the + chassis.""" + raise NotImplementedError + + @property + def velocity_vectors(self) -> Tuple[np.ndarray, np.ndarray]: + """Returns linear velocity vector in m/s and angular velocity in rad/sec.""" + raise NotImplementedError + + @property + def yaw_rate(self) -> float: + """The turning rate of the chassis in radians.""" + raise NotImplementedError + + def step(self, current_simulation_time): + """Update the chassis state.""" + raise NotImplementedError + + def state_override( + self, + dt: float, + force_pose: Pose, + linear_velocity: Optional[np.ndarray] = None, + angular_velocity: Optional[np.ndarray] = None, + ): + """Use with care! In essence, this is tinkering with the physics of the world, + and may have unintended behavioral or performance consequences.""" + raise NotImplementedError diff --git a/smarts/core/physics/physics_simulation.py b/smarts/core/physics/physics_simulation.py new file mode 100644 index 0000000000..b6204ac003 --- /dev/null +++ b/smarts/core/physics/physics_simulation.py @@ -0,0 +1,88 @@ +# MIT License +# +# Copyright (C) 2023. Huawei Technologies Co., Ltd. All rights reserved. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +from typing import Any, Optional + +from smarts.core.coordinates import BoundingBox +from smarts.core.simulation_frame import SimulationFrame +from smarts.core.vehicle_index import VehicleIndex + + +class PhysicsSimulation: + """The physics engine base interface.""" + + def initialize(self, fixed_timestep_sec: float): + """Initializes the physics simulation. + + Args: + fixed_timestep_sec (float): The step size for the simulation. + """ + raise NotImplementedError + + def initialize_ground(self, resource_path: str, map_bb): + """Initializes the ground for the simulation. + + Args: + resource (str): The ground shape resource id. + """ + raise NotImplementedError + + def reset_simulation(self): + """Restores the physics simulation back to base state.""" + raise NotImplementedError + + def teardown(self): + """Close down all open resources relating this physics simulation holds. + This permanently invalidates and destroys the physics simulation. + """ + raise NotImplementedError + + def step( + self, + dt: float, + simulation_frame: Optional[SimulationFrame], + vehicle_index: VehicleIndex, + ): + """Step the current physics simulation. + + Args: + simulation_frame (SimulationFrame): The simulation frame state. + vehicle_index (VehicleIndex): The vehicle index current state. + """ + raise NotImplementedError + + def collider_by_id(self, entity_id: Any): + """Gets a collider by its given id. + + Args: + entity_id (Any): The id of an entity to + """ + raise NotImplementedError + + @property + def simulation_bounding_box(self) -> BoundingBox: + """Get the current bounding box of the simulation.""" + raise NotImplementedError + + @property + def client(self): + """Returns the underlying physics client.""" + raise NotImplementedError diff --git a/smarts/core/simulation_frame.py b/smarts/core/simulation_frame.py index 6a5c3fdb98..4b0cee8883 100644 --- a/smarts/core/simulation_frame.py +++ b/smarts/core/simulation_frame.py @@ -23,7 +23,7 @@ import re from dataclasses import dataclass from functools import cached_property, lru_cache -from typing import Any, Dict, List, Optional, Set +from typing import Any, Dict, FrozenSet, List, Optional, Set from smarts.core.actor import ActorState from smarts.core.agent_interface import AgentInterface @@ -58,7 +58,7 @@ class SimulationFrame: interest_filter: re.Pattern # TODO MTA: renderer can be allowed here as long as it is only type information # renderer_type: Any = None - _ground_bullet_id: Optional[str] = None + _collision_filter: FrozenSet[str] = frozenset() @cached_property def agent_ids(self) -> Set[str]: @@ -116,7 +116,7 @@ def vehicle_did_collide(self, vehicle_id) -> bool: """Test if the given vehicle had any collisions in the last physics update.""" vehicle_collisions = self.vehicle_collisions.get(vehicle_id, []) for c in vehicle_collisions: - if c.collidee_id != self._ground_bullet_id: + if c.collidee_id not in self._collision_filter: return True return False @@ -126,7 +126,7 @@ def filtered_vehicle_collisions(self, vehicle_id) -> List[Collision]: """ vehicle_collisions = self.vehicle_collisions.get(vehicle_id, []) return [ - c for c in vehicle_collisions if c.collidee_id != self._ground_bullet_id + c for c in vehicle_collisions if c.collidee_id != self._collision_filter ] @cached_property diff --git a/smarts/core/smarts.py b/smarts/core/smarts.py index b80f34eecc..5bfb12964c 100644 --- a/smarts/core/smarts.py +++ b/smarts/core/smarts.py @@ -71,10 +71,8 @@ from .traffic_history_provider import TrafficHistoryProvider from .traffic_provider import TrafficProvider from .trap_manager import TrapManager -from .utils import pybullet from .utils.id import Id from .utils.math import rounder_for_dt -from .utils.pybullet import bullet_client as bc from .vehicle import Vehicle from .vehicle_index import VehicleIndex from .vehicle_state import Collision, VehicleState, neighborhood_vehicles_around_vehicle @@ -201,12 +199,9 @@ def __init__( self._last_provider_state = None self._reset_agents_only = reset_agents_only # a.k.a "teleportation" - # For macOS GUI. See our `BulletClient` docstring for details. - # from .utils.bullet import BulletClient - # self._bullet_client = BulletClient(pybullet.GUI) - self._bullet_client = pybullet.SafeBulletClient( - pybullet.DIRECT - ) # pylint: disable=no-member + from smarts.bullet.bullet_simulation import BulletSimulation + + self._physics_simulation = BulletSimulation() # Set up indices self._sensor_manager = SensorManager() @@ -224,9 +219,6 @@ def __init__( IdActorCaptureManager(), ] - self._ground_bullet_id = None - self._map_bb = None - def step( self, agent_actions: Dict[str, Any], @@ -525,7 +517,10 @@ def setup(self, scenario: Scenario): if self._renderer: self._renderer.setup(scenario) - self._setup_bullet_client(self._bullet_client) + self._physics_simulation.initialize(self._fixed_timestep_sec) + self._physics_simulation.initialize_ground( + self._scenario.plane_filepath, self.road_map.bounding_box + ) provider_state = self._setup_providers(self._scenario) self._vehicle_index.load_controller_params( scenario.controller_parameters_filepath @@ -789,73 +784,6 @@ def provider_removing_actor(self, provider: Optional[Provider], actor_id: str): # when it notices a social vehicle has exited the simulation. self._teardown_vehicles([actor_id]) - def _setup_bullet_client(self, client: bc.BulletClient): - client.resetSimulation() - client.configureDebugVisualizer( - pybullet.COV_ENABLE_GUI, 0 - ) # pylint: disable=no-member - max_pybullet_freq = config()( - "physics", "max_pybullet_freq", default=MAX_PYBULLET_FREQ, cast=int - ) - # PyBullet defaults the timestep to 240Hz. Several parameters are tuned with - # this value in mind. For example the number of solver iterations and the error - # reduction parameters (erp) for contact, friction and non-contact joints. - # Attempting to get around this we set the number of substeps so that - # timestep * substeps = 240Hz. Bullet (C++) does something to this effect as - # well (https://git.io/Jvf0M), but PyBullet does not expose it. - # But if our timestep is variable (due to being externally driven) - # then we will step pybullet multiple times ourselves as necessary - # to account for the time delta on each SMARTS step. - self._pybullet_period = ( - self._fixed_timestep_sec - if self._fixed_timestep_sec - else 1 / max_pybullet_freq - ) - client.setPhysicsEngineParameter( - fixedTimeStep=self._pybullet_period, - numSubSteps=int(self._pybullet_period * max_pybullet_freq), - numSolverIterations=10, - solverResidualThreshold=0.001, - # warmStartingFactor=0.99 - ) - - client.setGravity(0, 0, -9.8) - self._map_bb = None - self._setup_pybullet_ground_plane(client) - - def _setup_pybullet_ground_plane(self, client: bc.BulletClient): - plane_path = self._scenario.plane_filepath - if not os.path.exists(plane_path): - with pkg_resources.path(models, "plane.urdf") as path: - plane_path = str(path.absolute()) - - if not self._map_bb: - self._map_bb = self.road_map.bounding_box - - if self._map_bb: - # 1e6 is the default value for plane length and width in smarts/models/plane.urdf. - DEFAULT_PLANE_DIM = 1e6 - ground_plane_scale = ( - 2.2 * max(self._map_bb.length, self._map_bb.width) / DEFAULT_PLANE_DIM - ) - ground_plane_center = self._map_bb.center - else: - # first step on undefined map, just use a big scale (1e6). - # it should get updated as soon as vehicles are added... - ground_plane_scale = 1.0 - ground_plane_center = (0, 0, 0) - - if self._ground_bullet_id is not None: - client.removeBody(self._ground_bullet_id) - self._ground_bullet_id = None - - self._ground_bullet_id = client.loadURDF( - plane_path, - useFixedBase=True, - basePosition=ground_plane_center, - globalScaling=ground_plane_scale, - ) - def teardown(self): """Clean up episode resources.""" if self._agent_manager is not None: @@ -865,8 +793,8 @@ def teardown(self): if self._sensor_manager is not None: self._sensor_manager.teardown(self.renderer_ref) - if self._bullet_client is not None: - self._bullet_client.resetSimulation() + if self._physics_simulation is not None: + self._physics_simulation.reset_simulation() if self._renderer is not None: self._renderer.teardown() self._teardown_providers() @@ -879,7 +807,6 @@ def teardown(self): if self._trap_manager is not None: self._trap_manager = None - self._ground_bullet_id = None self._is_setup = False def destroy(self): @@ -906,9 +833,9 @@ def destroy(self): if self._renderer is not None: self._renderer.destroy() self._renderer = None - if self._bullet_client is not None: - self._bullet_client.disconnect() - self._bullet_client = None + if self._physics_simulation is not None: + self._physics_simulation.teardown() + self._physics_simulation = None self._is_destroyed = True def _check_valid(self): @@ -1009,7 +936,9 @@ def is_rendering(self) -> bool: @property def road_stiffness(self) -> Any: """The stiffness of the road.""" - return self._bullet_client.getDynamicsInfo(self._ground_bullet_id, -1)[9] + return self._physics_simulation.client.getDynamicsInfo( + self._physics_simulation._ground_bullet_id, -1 + )[9] @property def dynamic_action_spaces(self) -> Set[ActionSpaceType]: @@ -1039,7 +968,7 @@ def external_provider(self) -> ExternalProvider: @property def bc(self): """The bullet physics client instance.""" - return self._bullet_client + return self._physics_simulation.client @property def envision(self) -> Optional[EnvisionClient]: @@ -1125,13 +1054,13 @@ def _teardown_vehicles_and_agents(self, vehicle_ids): # XXX: don't remove vehicle from its (traffic) Provider here, as it may be being teleported # (and needs to remain registered in Traci during this step). - def _pybullet_provider_sync(self, provider_state: ProviderState): + def _provider_sync(self, provider_state: ProviderState): current_actor_ids = {v.actor_id for v in provider_state.actors} previous_sv_ids = self._vehicle_index.social_vehicle_ids() exited_actors = previous_sv_ids - current_actor_ids self._teardown_vehicles_and_agents(exited_actors) - # Update our pybullet world given this provider state + # Update our simulation given this provider state dt = provider_state.dt or self._last_dt for vehicle in provider_state.actors: if not isinstance(vehicle, VehicleState): @@ -1155,19 +1084,9 @@ def _pybullet_provider_sync(self, provider_state: ProviderState): ) if not vehicle.updated: - # Note: update_state() happens *after* pybullet has been stepped. + # Note: update_state() happens *after* physics has been stepped. social_vehicle.update_state(vehicle, dt=dt) - def _step_pybullet(self): - self._bullet_client.stepSimulation() - pybullet_substeps = max(1, round(self._last_dt / self._pybullet_period)) - 1 - for _ in range(pybullet_substeps): - for vehicle in self._vehicle_index.vehicles: - vehicle.chassis.reapply_last_control() - self._bullet_client.stepSimulation() - for vehicle in self._vehicle_index.vehicles: - vehicle.step(self._elapsed_sim_time) - @property def vehicle_index(self) -> VehicleIndex: """The vehicle index for direct vehicle manipulation.""" @@ -1232,7 +1151,7 @@ def _harmonize_providers(self, provider_state: ProviderState): provider.sync(provider_state) except Exception as provider_error: self._handle_provider(provider, provider_error) - self._pybullet_provider_sync(provider_state) + self._provider_sync(provider_state) def _reset_providers(self): for provider in self.providers: @@ -1323,7 +1242,11 @@ def _step_providers(self, actions) -> ProviderState: provider.perform_agent_actions(agent_actions) self._check_ground_plane() - self._step_pybullet() + self._physics_simulation.step( + self._last_dt, self.cached_frame, self._vehicle_index + ) + for vehicle in self._vehicle_index.vehicles: + vehicle.step(self._elapsed_sim_time) self._process_collisions() accumulated_provider_state = ProviderState() @@ -1399,7 +1322,7 @@ def fixed_timestep_sec(self, fixed_timestep_sec: float): else: self._rounder = rounder_for_dt(fixed_timestep_sec) self._fixed_timestep_sec = fixed_timestep_sec - self._is_setup = False # need to re-setup pybullet + self._is_setup = False # need to re-initialize @property def last_dt(self) -> float: @@ -1428,7 +1351,7 @@ def _get_pybullet_collisions(self, vehicle_id: str) -> Set[str]: return { p.bullet_id for p in vehicle.chassis.contact_points - if p.bullet_id != self._ground_bullet_id + if p.bullet_id != self._physics_simulation._ground_bullet_id } def _process_collisions(self): @@ -1465,11 +1388,12 @@ def _bullet_id_to_vehicle(self, bullet_id): def _check_ground_plane(self): rescale_plane = False - map_min = np.array(self._map_bb.min_pt)[:2] if self._map_bb else np.array([]) - map_max = np.array(self._map_bb.max_pt)[:2] if self._map_bb else np.array([]) + map_bb = self._physics_simulation.simulation_bounding_box + map_min = np.array(map_bb.min_pt)[:2] if map_bb else np.array([]) + map_max = np.array(map_bb.max_pt)[:2] if map_bb else np.array([]) for vehicle_id in self._vehicle_index.agent_vehicle_ids(): vehicle = self._vehicle_index.vehicle_by_id(vehicle_id) - map_spot = vehicle.pose.point.as_np_array[:2] + map_spot = vehicle.pose.point.as_np_array[:2].copy() if len(map_min) == 0: map_min = map_spot rescale_plane = True @@ -1490,11 +1414,15 @@ def _check_ground_plane(self): if map_max[1] - map_min[1] < MIN_DIM: map_min[1] -= MIN_DIM map_max[1] += MIN_DIM - self._map_bb = BoundingBox(Point(*map_min), Point(*map_max)) + map_bb = BoundingBox(Point(*map_min), Point(*map_max)) self._log.info( - f"rescaling pybullet ground plane to at least {map_min} and {map_max}" + "rescaling pybullet ground plane to at least %s and %s", + map_min, + map_max, + ) + self._physics_simulation.initialize_ground( + self._scenario.plane_filepath, map_bb ) - self._setup_pybullet_ground_plane(self._bullet_client) def _try_emit_envision_state(self, provider_state: ProviderState, obs, scores): if not self._envision: @@ -1716,4 +1644,5 @@ def cached_frame(self) -> SimulationFrame: interest_filter=re.compile( self.scenario.metadata.get("actor_of_interest_re_filter", "") ), + _collision_filter=frozenset((self._physics_simulation._ground_bullet_id,)), ) diff --git a/smarts/core/tests/test_collision.py b/smarts/core/tests/test_collision.py index 8efdef0137..3ffd7babf1 100644 --- a/smarts/core/tests/test_collision.py +++ b/smarts/core/tests/test_collision.py @@ -27,77 +27,80 @@ import pytest from helpers.scenario import temp_scenario +from smarts.bullet.bullet_simulation import BulletSimulation +from smarts.bullet.chassis import BulletAckermannChassis, BulletBoxChassis from smarts.core import models from smarts.core.agent_interface import ActionSpaceType, AgentInterface -from smarts.core.chassis import AckermannChassis, BoxChassis from smarts.core.coordinates import Heading, Pose +from smarts.core.physics.chassis import AckermannChassis, BoxChassis from smarts.core.scenario import Scenario from smarts.core.smarts import SMARTS from smarts.core.sumo_traffic_simulation import SumoTrafficSimulation -from smarts.core.utils import pybullet -from smarts.core.utils.pybullet import bullet_client as bc from smarts.core.vehicle import VEHICLE_CONFIGS +from smarts.core.vehicle_index import VehicleIndex from smarts.sstudio import gen_scenario from smarts.sstudio import types as t +time_step = 0.1 -@pytest.fixture -def bullet_client(): - client = bc.BulletClient(pybullet.DIRECT) - client.setGravity(0, 0, -9.8) - - path = Path(__file__).parent / "../smarts/core/models/plane.urdf" - with pkg_resources.path(models, "plane.urdf") as path: - plane_path = str(path.absolute()) - # create the ground plane to be big enough that the vehicles can potentially contact the ground too - client.loadURDF( - plane_path, - useFixedBase=True, - basePosition=(0, 0, 0), - globalScaling=1000.0 / 1e6, - ) - yield client - client.disconnect() +@pytest.fixture +def bullet_simulation(fixed_timestep_sec=time_step): + simulation = BulletSimulation() + simulation.initialize(fixed_timestep_sec) + simulation.initialize_ground(None, None) + yield simulation + simulation.teardown() def step_with_vehicle_commands( - bv: AckermannChassis, steps, throttle=0, brake=0, steering=0 + bv: AckermannChassis, + bullet_simulation: BulletSimulation, + steps, + throttle=0, + brake=0, + steering=0, ): collisions = [] for _ in range(steps): bv.control(throttle, brake, steering) - bv._client.stepSimulation() + bullet_simulation.step(time_step, None, vehicle_index=VehicleIndex.identity()) collisions.extend(bv.contact_points) return collisions -def step_with_pose_delta(bv: BoxChassis, steps, pose_delta: np.ndarray, speed: float): +def step_with_pose_delta( + bv: BoxChassis, + bullet_simulation: BulletSimulation, + steps, + pose_delta: np.ndarray, + speed: float, +): collisions = [] for _ in range(steps): cur_pose = bv.pose new_pose = Pose.from_center(cur_pose.position + pose_delta, cur_pose.heading) bv.control(new_pose, speed) - bv._client.stepSimulation() + bullet_simulation.step(time_step, None, VehicleIndex.identity()) collisions.extend(bv.contact_points) return collisions -def test_collision(bullet_client: bc.BulletClient): +def test_collision(bullet_simulation: BulletSimulation): """Spawn overlap to check for the most basic collision""" - chassis = AckermannChassis( - Pose.from_center([0, 0, 0], Heading(-math.pi * 0.5)), bullet_client + chassis = BulletAckermannChassis( + Pose.from_center([0, 0, 0], Heading(-math.pi * 0.5)), bullet_simulation.client ) - b_chassis = BoxChassis( + b_chassis = BulletBoxChassis( Pose.from_center([0, 0, 0], Heading(0)), speed=0, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, - bullet_client=chassis._client, + bullet_client=bullet_simulation.client, ) - collisions = step_with_vehicle_commands(chassis, steps=2) + collisions = step_with_vehicle_commands(chassis, bullet_simulation, steps=2) assert len(collisions) > 0 collided_bullet_ids = set([c.bullet_id for c in collisions]) GROUND_ID = 0 @@ -106,39 +109,41 @@ def test_collision(bullet_client: bc.BulletClient): assert GROUND_ID not in collided_bullet_ids -def test_non_collision(bullet_client: bc.BulletClient): +def test_non_collision(bullet_simulation: BulletSimulation): """Spawn without overlap to check for the most basic collision""" - chassis = AckermannChassis( - Pose.from_center([0, 0, 0], Heading(-math.pi * 0.5)), bullet_client + chassis = BulletAckermannChassis( + Pose.from_center([0, 0, 0], Heading(-math.pi * 0.5)), bullet_simulation.client ) - b_chassis = BoxChassis( + b_chassis = BulletBoxChassis( Pose.from_center([0, 10, 0], Heading(0)), speed=0, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, - bullet_client=chassis._client, + bullet_client=bullet_simulation.client, ) - collisions = step_with_vehicle_commands(chassis, steps=1) + collisions = step_with_vehicle_commands(chassis, bullet_simulation, steps=1) collided_bullet_ids = set([c.bullet_id for c in collisions]) assert b_chassis.bullet_id not in collided_bullet_ids assert len(collisions) == 0 -def test_collision_collide_with_standing_vehicle(bullet_client: bc.BulletClient): +def test_collision_collide_with_standing_vehicle(bullet_simulation: BulletSimulation): """Run a vehicle at a standing vehicle as fast as possible.""" - chassis = AckermannChassis( - Pose.from_center([10, 0, 0], Heading(math.pi * 0.5)), bullet_client + chassis = BulletAckermannChassis( + Pose.from_center([10, 0, 0], Heading(math.pi * 0.5)), bullet_simulation.client ) - b_chassis = BoxChassis( + b_chassis = BulletBoxChassis( Pose.from_center([0, 0, 0], Heading(0)), speed=0, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, - bullet_client=chassis._client, + bullet_client=bullet_simulation.client, + ) + collisions = step_with_vehicle_commands( + chassis, bullet_simulation, steps=1000, throttle=1, steering=0 ) - collisions = step_with_vehicle_commands(chassis, steps=1000, throttle=1, steering=0) collided_bullet_ids = set([c.bullet_id for c in collisions]) GROUND_ID = 0 assert len(collisions) > 0 @@ -147,34 +152,34 @@ def test_collision_collide_with_standing_vehicle(bullet_client: bc.BulletClient) assert GROUND_ID not in collided_bullet_ids -def test_box_chassis_collision(bullet_client: bc.BulletClient): +def test_box_chassis_collision(bullet_simulation: BulletSimulation): """Spawn overlap to check for the most basic BoxChassis collision""" # This is required to ensure that the bullet move_to constraint # actually moves the vehicle the correct amount - bullet_client.setPhysicsEngineParameter( + bullet_simulation.client.setPhysicsEngineParameter( fixedTimeStep=0.1, numSubSteps=24, numSolverIterations=10, solverResidualThreshold=0.001, ) - chassis = BoxChassis( + chassis = BulletBoxChassis( Pose.from_center([0, 0, 0], Heading(-0.5 * math.pi)), speed=10, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, - bullet_client=bullet_client, + bullet_client=bullet_simulation.client, ) - b_chassis = BoxChassis( + b_chassis = BulletBoxChassis( Pose.from_center([0, 0, 0], Heading(0.5 * math.pi)), speed=0, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, - bullet_client=chassis._client, + bullet_client=bullet_simulation.client, ) collisions = step_with_pose_delta( - chassis, steps=10, pose_delta=np.array((1.0, 0, 0)), speed=10 + chassis, bullet_simulation, steps=10, pose_delta=np.array((1.0, 0, 0)), speed=10 ) assert len(collisions) == 3 collided_bullet_ids = set([c.bullet_id for c in collisions]) @@ -184,28 +189,34 @@ def test_box_chassis_collision(bullet_client: bc.BulletClient): assert GROUND_ID not in collided_bullet_ids -def _joust(wkc: AckermannChassis, bkc: AckermannChassis, steps, throttle=1): +def _joust( + wkc: AckermannChassis, + bkc: AckermannChassis, + bullet_simulation: BulletSimulation, + steps, + throttle=1, +): collisions = ([], []) for _ in range(steps): wkc.control(throttle) bkc.control(throttle) - bkc._client.stepSimulation() + bullet_simulation.step(time_step, None, VehicleIndex.identity()) collisions[0].extend(wkc.contact_points) collisions[1].extend(bkc.contact_points) return collisions -def test_collision_joust(bullet_client: bc.BulletClient): +def test_collision_joust(bullet_simulation: BulletSimulation): """Run two agents at each other to test for clipping.""" - white_knight_chassis = AckermannChassis( - Pose.from_center([10, 0, 0], Heading(math.pi * 0.5)), bullet_client + white_knight_chassis = BulletAckermannChassis( + Pose.from_center([10, 0, 0], Heading(math.pi * 0.5)), bullet_simulation.client ) - black_knight_chassis = AckermannChassis( - Pose.from_center([-10, 0, 0], Heading(-math.pi * 0.5)), bullet_client + black_knight_chassis = BulletAckermannChassis( + Pose.from_center([-10, 0, 0], Heading(-math.pi * 0.5)), bullet_simulation.client ) wkc_collisions, bkc_collisions = _joust( - white_knight_chassis, black_knight_chassis, steps=10000 + white_knight_chassis, black_knight_chassis, bullet_simulation, steps=10000 ) assert len(wkc_collisions) > 0 @@ -215,18 +226,18 @@ def test_collision_joust(bullet_client: bc.BulletClient): assert black_knight_chassis.bullet_id in [c.bullet_id for c in wkc_collisions] -def test_ackerman_chassis_size_unchanged(bullet_client: bc.BulletClient): +def test_ackerman_chassis_size_unchanged(bullet_simulation: BulletSimulation): """Test that the ackerman chassis size has not changed accidentally by packing it around itself with no forces and then check for collisions after a few steps.""" - bullet_client.setGravity(0, 0, 0) + bullet_simulation.client.setGravity(0, 0, 0) separation_for_collision_error = 0.0501 original_vehicle_dimensions = VEHICLE_CONFIGS["passenger"].dimensions shared_heading = Heading(0) - chassis = AckermannChassis( - Pose.from_center([0, 0, 0], shared_heading), bullet_client + chassis = BulletAckermannChassis( + Pose.from_center([0, 0, 0], shared_heading), bullet_simulation.client ) - chassis_n = BoxChassis( + chassis_n = BulletBoxChassis( Pose.from_center( [ 0, @@ -237,9 +248,9 @@ def test_ackerman_chassis_size_unchanged(bullet_client: bc.BulletClient): ), speed=0, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, - bullet_client=bullet_client, + bullet_client=bullet_simulation.client, ) - chassis_e = BoxChassis( + chassis_e = BulletBoxChassis( Pose.from_center( [ (original_vehicle_dimensions.width + separation_for_collision_error), @@ -250,9 +261,9 @@ def test_ackerman_chassis_size_unchanged(bullet_client: bc.BulletClient): ), speed=0, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, - bullet_client=bullet_client, + bullet_client=bullet_simulation.client, ) - chassis_s = BoxChassis( + chassis_s = BulletBoxChassis( Pose.from_center( [ 0, @@ -263,9 +274,9 @@ def test_ackerman_chassis_size_unchanged(bullet_client: bc.BulletClient): ), speed=0, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, - bullet_client=bullet_client, + bullet_client=bullet_simulation.client, ) - chassis_w = BoxChassis( + chassis_w = BulletBoxChassis( Pose.from_center( [ -(original_vehicle_dimensions.width + separation_for_collision_error), @@ -276,9 +287,9 @@ def test_ackerman_chassis_size_unchanged(bullet_client: bc.BulletClient): ), speed=0, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, - bullet_client=bullet_client, + bullet_client=bullet_simulation.client, ) - collisions = step_with_vehicle_commands(chassis, steps=10) + collisions = step_with_vehicle_commands(chassis, bullet_simulation, steps=10) assert len(collisions) == 0 @@ -325,14 +336,14 @@ def smarts(): action=ActionSpaceType.Lane, ) agents = {AGENT_1: laner, AGENT_2: buddha} - smarts = SMARTS( + smarts_ = SMARTS( agents, traffic_sims=[SumoTrafficSimulation(headless=True)], envision=None, ) - yield smarts - smarts.destroy() + yield smarts_ + smarts_.destroy() def test_sim_level_collision(smarts, scenarios): diff --git a/smarts/core/tests/test_dynamics_backend.py b/smarts/core/tests/test_dynamics_backend.py index 4cfc35335e..36cdf147df 100644 --- a/smarts/core/tests/test_dynamics_backend.py +++ b/smarts/core/tests/test_dynamics_backend.py @@ -24,23 +24,22 @@ import numpy as np import pytest -from smarts.core.chassis import AckermannChassis +from smarts.bullet.bullet_simulation import BulletSimulation +from smarts.bullet.chassis import BulletAckermannChassis from smarts.core.coordinates import Heading, Pose -from smarts.core.utils import pybullet -from smarts.core.utils.pybullet import bullet_client as bc @pytest.fixture -def bullet_client(): - client = bc.BulletClient(pybullet.DIRECT) - yield client - client.disconnect() +def bullet_simulation(): + simulation = BulletSimulation() + yield simulation + simulation.teardown() @pytest.fixture -def chassis(bullet_client): - return AckermannChassis( - Pose.from_center([0, 0, 0], Heading(math.pi * 0.5)), bullet_client +def chassis(bullet_simulation: BulletSimulation): + return BulletAckermannChassis( + Pose.from_center([0, 0, 0], Heading(math.pi * 0.5)), bullet_simulation.client ) diff --git a/smarts/core/tests/test_trajectory_controller.py b/smarts/core/tests/test_trajectory_controller.py index 10a913b237..bd9ccdbd43 100644 --- a/smarts/core/tests/test_trajectory_controller.py +++ b/smarts/core/tests/test_trajectory_controller.py @@ -27,15 +27,14 @@ import pytest import yaml +from smarts.bullet.bullet_simulation import BulletSimulation +from smarts.bullet.chassis import BulletAckermannChassis from smarts.core import models -from smarts.core.chassis import AckermannChassis from smarts.core.controllers import ( TrajectoryTrackingController, TrajectoryTrackingControllerState, ) from smarts.core.coordinates import Heading, Pose -from smarts.core.utils import pybullet -from smarts.core.utils.pybullet import bullet_client as bc from smarts.core.vehicle import Vehicle time_step = 0.1 @@ -58,29 +57,26 @@ def vehicle_controller_file(request): @pytest.fixture -def bullet_client(fixed_timestep_sec=time_step): - client = bc.BulletClient(pybullet.DIRECT) - client.resetSimulation() - client.setGravity(0, 0, -9.8) - client.setPhysicsEngineParameter( - fixedTimeStep=fixed_timestep_sec, - numSubSteps=int(fixed_timestep_sec / (1 / 240)), - ) - path = Path(__file__).parent / "../models/plane.urdf" - path = str(path.absolute()) - plane_body_id = client.loadURDF(path, useFixedBase=True) - yield client - client.disconnect() +def bullet_simulation(fixed_timestep_sec=time_step): + simulation = BulletSimulation() + simulation.initialize(fixed_timestep_sec) + simulation.initialize_ground(None, None) + yield simulation + simulation.teardown() @pytest.fixture -def vehicle(bullet_client, vehicle_controller_file, fixed_timestep_sec=time_step): +def vehicle( + bullet_simulation: BulletSimulation, + vehicle_controller_file, + fixed_timestep_sec=time_step, +): pose = Pose.from_center((0, 0, 0), Heading(0)) vehicle1 = Vehicle( id="vehicle", - chassis=AckermannChassis( + chassis=BulletAckermannChassis( pose=pose, - bullet_client=bullet_client, + bullet_client=bullet_simulation.client, vehicle_filepath=vehicle_controller_file[0], controller_parameters=vehicle_controller_file[1], ), @@ -132,7 +128,11 @@ def build_trajectory(radius, omega, step_num, fixed_timestep_sec=time_step): def step_with_vehicle_commands( - bullet_client, vehicle, radius, omega, fixed_timestep_sec=time_step + bullet_simulation: BulletSimulation, + vehicle, + radius, + omega, + fixed_timestep_sec=time_step, ): prev_friction_sum = None # Proceed till the end of half of the circle. @@ -150,7 +150,7 @@ def step_with_vehicle_commands( dt_sec=fixed_timestep_sec, ) - bullet_client.stepSimulation() + bullet_simulation.client.stepSimulation() final_error = math.sqrt( (vehicle.position[0] - desired_trajectory[0][0]) ** 2 @@ -159,6 +159,8 @@ def step_with_vehicle_commands( return final_error -def test_trajectory_tracking(bullet_client, vehicle, radius, omega): - final_error = step_with_vehicle_commands(bullet_client, vehicle, radius, omega) +def test_trajectory_tracking( + bullet_simulation: BulletSimulation, vehicle, radius, omega +): + final_error = step_with_vehicle_commands(bullet_simulation, vehicle, radius, omega) assert final_error <= 10 diff --git a/smarts/core/tests/test_trajectory_interpolation_provider.py b/smarts/core/tests/test_trajectory_interpolation_provider.py index 530e029af9..1765104acd 100644 --- a/smarts/core/tests/test_trajectory_interpolation_provider.py +++ b/smarts/core/tests/test_trajectory_interpolation_provider.py @@ -25,9 +25,10 @@ import pytest import smarts.sstudio.types as t +from smarts.bullet.bullet_simulation import BulletSimulation +from smarts.bullet.chassis import BulletBoxChassis from smarts.core.agent import Agent from smarts.core.agent_interface import AgentInterface, AgentType -from smarts.core.chassis import BoxChassis from smarts.core.controllers.trajectory_interpolation_controller import ( TrajectoryField, TrajectoryInterpolationController, @@ -36,8 +37,6 @@ from smarts.core.scenario import Scenario from smarts.core.smarts import SMARTS from smarts.core.tests.helpers.scenario import temp_scenario -from smarts.core.utils import pybullet -from smarts.core.utils.pybullet import bullet_client as bc from smarts.core.vehicle import VEHICLE_CONFIGS, Vehicle from smarts.sstudio import gen_scenario from smarts.zoo.agent_spec import AgentSpec @@ -151,13 +150,15 @@ def scenario(): @pytest.fixture -def bullet_client(): - client = bc.BulletClient(pybullet.DIRECT) - yield client - client.disconnect() +def bullet_simulation(): + simulation = BulletSimulation() + yield simulation + simulation.teardown() -def test_trajectory_interpolation_controller(controller_actions, bullet_client): +def test_trajectory_interpolation_controller( + controller_actions, bullet_simulation: BulletSimulation +): dt = 0.1 i, j = np.ix_([TrajectoryField.X_INDEX, TrajectoryField.Y_INDEX], [0]) @@ -165,11 +166,11 @@ def test_trajectory_interpolation_controller(controller_actions, bullet_client): original_position = trajectory[i, j].reshape(2) original_heading = Heading(trajectory[TrajectoryField.THETA_INDEX][0]) initial_speed = trajectory[TrajectoryField.VEL_INDEX][0] - chassis = BoxChassis( + chassis = BulletBoxChassis( pose=Pose.from_center(original_position, original_heading), speed=initial_speed, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, - bullet_client=bullet_client, + bullet_client=bullet_simulation.client, ) vehicle = Vehicle(vehicle_id, chassis) diff --git a/smarts/core/tests/test_vehicle.py b/smarts/core/tests/test_vehicle.py index 630744d150..83a50c93c9 100644 --- a/smarts/core/tests/test_vehicle.py +++ b/smarts/core/tests/test_vehicle.py @@ -24,18 +24,17 @@ import numpy as np import pytest -from smarts.core.chassis import BoxChassis +from smarts.bullet.bullet_simulation import BulletSimulation +from smarts.bullet.chassis import BulletBoxChassis from smarts.core.coordinates import Dimensions, Heading, Pose -from smarts.core.utils import pybullet -from smarts.core.utils.pybullet import bullet_client as bc from smarts.core.vehicle import VEHICLE_CONFIGS, Vehicle, VehicleState @pytest.fixture -def bullet_client(): - client = bc.BulletClient(pybullet.DIRECT) - yield client - client.disconnect() +def bullet_simulation(): + simulation = BulletSimulation() + yield simulation + simulation.teardown() # TODO: Clean up these tests and fixtures @@ -55,13 +54,13 @@ def speed(): @pytest.fixture -def social_vehicle(position, heading, speed, bullet_client): +def social_vehicle(position, heading, speed, bullet_simulation: BulletSimulation): pose = Pose.from_center(position, heading) - chassis = BoxChassis( + chassis = BulletBoxChassis( pose=pose, speed=speed, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, - bullet_client=bullet_client, + bullet_client=bullet_simulation.client, ) return Vehicle(id="sv-132", chassis=chassis) @@ -100,12 +99,12 @@ def test_update_from_traffic_sim(social_vehicle, provider_vehicle): assert social_vehicle.speed == provider_vehicle.speed -def test_create_social_vehicle(bullet_client): - chassis = BoxChassis( +def test_create_social_vehicle(bullet_simulation: BulletSimulation): + chassis = BulletBoxChassis( pose=Pose.from_center((0, 0, 0), Heading(0)), speed=0, dimensions=Dimensions(length=3, width=1, height=1), - bullet_client=bullet_client, + bullet_client=bullet_simulation.client, ) car = Vehicle( @@ -123,13 +122,13 @@ def test_create_social_vehicle(bullet_client): assert truck.vehicle_type == "truck" -def test_vehicle_bounding_box(bullet_client): +def test_vehicle_bounding_box(bullet_simulation: BulletSimulation): pose = Pose.from_center((1, 1, 0), Heading(0)) - chassis = BoxChassis( + chassis = BulletBoxChassis( pose=pose, speed=0, dimensions=Dimensions(length=3, width=1, height=1), - bullet_client=bullet_client, + bullet_client=bullet_simulation.client, ) vehicle = Vehicle( diff --git a/smarts/core/vehicle.py b/smarts/core/vehicle.py index 449ad9cdee..d03a40406e 100644 --- a/smarts/core/vehicle.py +++ b/smarts/core/vehicle.py @@ -26,12 +26,13 @@ import numpy as np +from smarts.bullet.chassis import BulletAckermannChassis, BulletBoxChassis from smarts.core.agent_interface import AgentInterface +from smarts.core.physics.chassis import AckermannChassis, BoxChassis, Chassis from smarts.core.plan import Mission, Plan from . import models from .actor import ActorRole -from .chassis import AckermannChassis, BoxChassis, Chassis from .colors import SceneColors from .coordinates import Dimensions, Heading, Pose from .sensors import ( @@ -319,7 +320,7 @@ def build_agent_vehicle( logger.warning( "setting vehicle dimensions on a AckermannChassis not yet supported" ) - chassis = AckermannChassis( + chassis = BulletAckermannChassis( pose=start_pose, bullet_client=sim.bc, vehicle_filepath=vehicle_filepath, @@ -329,7 +330,7 @@ def build_agent_vehicle( initial_speed=initial_speed, ) else: - chassis = BoxChassis( + chassis = BulletBoxChassis( pose=start_pose, speed=initial_speed, dimensions=chassis_dims, @@ -351,7 +352,7 @@ def build_social_vehicle(sim, vehicle_id, vehicle_state: VehicleState) -> "Vehic vehicle_state.dimensions, VEHICLE_CONFIGS[vehicle_state.vehicle_config_type].dimensions, ) - chassis = BoxChassis( + chassis = BulletBoxChassis( pose=vehicle_state.pose, speed=vehicle_state.speed, dimensions=dims, diff --git a/smarts/core/vehicle_index.py b/smarts/core/vehicle_index.py index eb454ca739..0f54ba622f 100644 --- a/smarts/core/vehicle_index.py +++ b/smarts/core/vehicle_index.py @@ -41,13 +41,13 @@ import numpy as np import tableprint as tp +from smarts.bullet.chassis import BulletAckermannChassis, BulletBoxChassis from smarts.core import gen_id from smarts.core.utils import resources from smarts.core.utils.cache import cache, clear_cache from smarts.core.utils.string import truncate from .actor import ActorRole -from .chassis import AckermannChassis, BoxChassis from .controllers import ControllerState from .road_map import RoadMap from .sensors import SensorState @@ -459,9 +459,9 @@ def switch_control_to_agent( vehicle = self._vehicles[vehicle_id] chassis = None if agent_interface and agent_interface.action in sim.dynamic_action_spaces: - chassis = AckermannChassis(pose=vehicle.pose, bullet_client=sim.bc) + chassis = BulletAckermannChassis(pose=vehicle.pose, bullet_client=sim.bc) else: - chassis = BoxChassis( + chassis = BulletBoxChassis( pose=vehicle.pose, speed=vehicle.speed, dimensions=vehicle.state.dimensions, @@ -537,7 +537,7 @@ def relinquish_agent_control( # pytype: enable=attribute-error vehicle = self._vehicles[v_id] - box_chassis = BoxChassis( + box_chassis = BulletBoxChassis( pose=vehicle.chassis.pose, speed=vehicle.chassis.speed, dimensions=vehicle.chassis.dimensions,