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 __init__(
self,
pose: Pose,
speed: float,
dimensions: BoundingBox,
bullet_client: bc.BulletClient,
):
self._dimensions = dimensions
self._bullet_body = BulletBoxShape(self._dimensions.as_lwh,
bullet_client)
self._bullet_constraint = BulletPositionConstraint(
self._bullet_body, bullet_client)
bullet_client.setCollisionFilterGroupMask(self._bullet_body._bullet_id,
-1, 0x0, 0x0)
self.control(pose, speed)
def control(self, pose: Pose, speed: float):
self._pose = pose
self._speed = speed
self._bullet_constraint.move_to(pose)
@property
def dimensions(self) -> BoundingBox:
return self._dimensions
@property
def contact_points(self) -> Sequence:
contact_points = _query_bullet_contact_points(self._client,
self._bullet_body, -1)
return [
ContactPoint(bullet_id=p[2],
contact_point=p[5],
contact_point_other=p[6]) for p in contact_points
]
@property
def bullet_id(self) -> str:
return self._bullet_body._bullet_id
@property
def speed(self):
return self._speed
@property
def velocity_vectors(self):
return (None, None)
@speed.setter
def speed(self, speed: float = None):
self._speed = speed
@property
def pose(self) -> Pose:
return self._pose
@property
def steering(self):
return None
@property
def yaw_rate(self):
return None
def inherit_physical_values(self, other: Chassis):
self._pose = other.pose
self.speed = other.speed
# ignore physics
def teardown(self):
self._bullet_constraint.teardown()
self._bullet_body.teardown()
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 __init__(
self,
pose: Pose,
speed: float,
dimensions: Dimensions,
bullet_client: bc.BulletClient,
):
self._dimensions = dimensions
self._bullet_body = BulletBoxShape(self._dimensions.as_lwh,
bullet_client)
self._bullet_constraint = BulletPositionConstraint(
self._bullet_body, bullet_client)
bullet_client.setCollisionFilterGroupMask(self._bullet_body._bullet_id,
-1, 0x0, 0x0)
self._pose = None
self.control(pose, speed)
self._client = bullet_client
def control(self, pose: Pose, speed: float, dt: float = 0):
if self._pose:
self._last_heading = self._pose.heading
self._last_dt = dt
self._pose = pose
self._speed = speed
self._bullet_constraint.move_to(pose)
def reapply_last_control(self):
# no need to do anything here since we're not applying forces
pass
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."""
if self._pose:
self._last_heading = self._pose.heading
self._last_dt = dt
self._pose = force_pose
if linear_velocity is not None or angular_velocity is not None:
assert linear_velocity is not None
assert angular_velocity is not None
self._speed = np.linalg.norm(linear_velocity)
self._client.resetBaseVelocity(
self.bullet_id,
linearVelocity=linear_velocity,
angularVelocity=angular_velocity,
)
self._bullet_constraint.move_to(force_pose)
@property
def dimensions(self) -> Dimensions:
return self._dimensions
@property
def contact_points(self) -> Sequence:
contact_points = _query_bullet_contact_points(self._client,
self.bullet_id, -1)
return [
ContactPoint(bullet_id=p[2],
contact_point=p[5],
contact_point_other=p[6]) for p in contact_points
]
@property
def bullet_id(self) -> str:
return self._bullet_body._bullet_id
@property
def speed(self) -> float:
# in m/s
return self._speed
@property
def velocity_vectors(self):
# linear_velocity in m/s, angular_velocity in rad/s
vh = radians_to_vec(self.pose.heading)
if self._speed is not None:
linear_velocity = np.array((vh[0], vh[1], 0.0)) * self._speed
else:
linear_velocity = None
if self._last_dt and self._last_dt > 0:
av = (vh - radians_to_vec(self._last_heading)) / self._last_dt
angular_velocity = np.array((av[0], av[1], 0.0))
else:
angular_velocity = np.array((0.0, 0.0, 0.0))
return (linear_velocity, angular_velocity)
@speed.setter
def speed(self, speed: Optional[float] = None):
self._speed = speed
@property
def pose(self) -> Pose:
return self._pose
@property
def steering(self):
return None
@property
def yaw_rate(self) -> Optional[float]:
# in rad/s
if self._last_dt and self._last_dt > 0:
delta = min_angles_difference_signed(self._pose.heading,
self._last_heading)
return delta / self._last_dt
return None
def inherit_physical_values(self, other: Chassis):
self._pose = other.pose
self.speed = other.speed
# ignore physics
def step(self, current_simulation_time):
pass
def teardown(self):
self._bullet_constraint.teardown()
self._bullet_body.teardown()
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 __init__(
self,
pose: Pose,
speed: float,
dimensions: BoundingBox,
bullet_client: bc.BulletClient,
):
self._dimensions = dimensions
self._bullet_body = BulletBoxShape(self._dimensions.as_lwh, bullet_client)
self._bullet_constraint = BulletPositionConstraint(
self._bullet_body, bullet_client
)
bullet_client.setCollisionFilterGroupMask(
self._bullet_body._bullet_id, -1, 0x0, 0x0
)
self._pose = None
self.control(pose, speed)
self._client = bullet_client
def control(self, pose: Pose, speed: float, dt: float = 0):
if self._pose:
self._last_heading = self._pose.heading
self._last_dt = dt
self._pose = pose
self._speed = speed
self._bullet_constraint.move_to(pose)
@property
def dimensions(self) -> BoundingBox:
return self._dimensions
@property
def contact_points(self) -> Sequence:
contact_points = _query_bullet_contact_points(self._client, self.bullet_id, -1)
return [
ContactPoint(bullet_id=p[2], contact_point=p[5], contact_point_other=p[6])
for p in contact_points
]
@property
def bullet_id(self) -> str:
return self._bullet_body._bullet_id
@property
def speed(self) -> float:
# in m/s
return self._speed
@property
def velocity_vectors(self):
# linear_velocity in m/s, angular_velocity in rad/s
vh = radians_to_vec(self.pose.heading)
if self._speed is not None:
linear_velocity = np.array((vh[0], vh[1], 0.0)) * self._speed
else:
linear_velocity = None
angular_velocity = np.array((0.0, 0.0, 0.0))
if self._last_dt > 0:
angular_velocity = vh - radians_to_vec(self._last_heading)
angular_velocity = np.append(angular_velocity / self._last_dt, 0.0)
return (linear_velocity, angular_velocity)
@speed.setter
def speed(self, speed: float = None):
self._speed = speed
@property
def pose(self) -> Pose:
return self._pose
@property
def steering(self):
return None
@property
def yaw_rate(self) -> float:
# in rad/s
if self._last_dt > 0:
delta = min_angles_difference_signed(self._pose.heading, self._last_heading)
return delta / self._last_dt
return None
def inherit_physical_values(self, other: Chassis):
self._pose = other.pose
self.speed = other.speed
# ignore physics
def step(self, current_simulation_time):
pass
def teardown(self):
self._bullet_constraint.teardown()
self._bullet_body.teardown()