def _joust(wkc: AckermannChassis, bkc: AckermannChassis, steps, throttle=1): collisions = ([], []) for _ in range(steps): wkc.control(throttle) bkc.control(throttle) bkc._client.stepSimulation() collisions[0].extend(wkc.contact_points) collisions[1].extend(bkc.contact_points) return collisions
def step_with_vehicle_commands( bv: AckermannChassis, steps, throttle=0, brake=0, steering=0 ): collisions = [] for _ in range(steps): bv.control(throttle, brake, steering) bv._client.stepSimulation() collisions.extend(bv.contact_points) return collisions
def test_collision_joust(bullet_client: bc.BulletClient): """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 ) black_knight_chassis = AckermannChassis( Pose.from_center([-10, 0, 0], Heading(-math.pi * 0.5)), bullet_client ) wkc_collisions, bkc_collisions = _joust( white_knight_chassis, black_knight_chassis, steps=10000 ) assert len(wkc_collisions) > 0 assert len(bkc_collisions) > 0 assert white_knight_chassis.bullet_id in [c.bullet_id for c in bkc_collisions] assert black_knight_chassis.bullet_id in [c.bullet_id for c in wkc_collisions]
def vehicle(bullet_client, TIMESTEP_SEC=time_step): pose = Pose.from_center((0, 0, 0), Heading(0)) vehicle1 = Vehicle( id="vehicle", pose=pose, showbase=mock.MagicMock(), chassis=AckermannChassis( pose=pose, bullet_client=bullet_client, ), ) return vehicle1
def vehicle(bullet_client, vehicle_controller_file, timestep_sec=time_step): pose = Pose.from_center((0, 0, 0), Heading(0)) vehicle1 = Vehicle( id="vehicle", pose=pose, chassis=AckermannChassis( pose=pose, bullet_client=bullet_client, vehicle_filepath=vehicle_controller_file[0], controller_parameters=vehicle_controller_file[1], ), ) return vehicle1
def test_collision_collide_with_standing_vehicle(bullet_client: bc.BulletClient): """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 ) b_chassis = BoxChassis( Pose.from_center([0, 0, 0], Heading(0)), speed=0, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, bullet_client=chassis._client, ) collisions = step_with_vehicle_commands(chassis, steps=1000, throttle=1, steering=0) assert len(collisions) > 0 assert b_chassis.bullet_id in [c.bullet_id for c in collisions]
def test_collision(bullet_client: bc.BulletClient): """Spawn overlap to check for the most basic collision""" chassis = AckermannChassis( Pose.from_center([0, 0, 0], Heading(-math.pi * 0.5)), bullet_client) b_chassis = BoxChassis( Pose.from_center([0, 0, 0], Heading(0)), speed=0, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, bullet_client=chassis._client, ) collisions = step_with_vehicle_commands(chassis, steps=1) assert b_chassis.bullet_id in [c.bullet_id for c in collisions] assert len(collisions) > 0
def test_non_collision(bullet_client: bc.BulletClient): """Spawn without overlap to check for the most basic collision""" GROUND_ID = 0 chassis = AckermannChassis( Pose.from_center([0, 0, 0], Heading(-math.pi * 0.5)), bullet_client) b_chassis = BoxChassis( Pose.from_center([0, 10, 0], Heading(0)), speed=0, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, bullet_client=chassis._client, ) collisions = step_with_vehicle_commands(chassis, steps=1) collided_bullet_ids = [c.bullet_id for c in collisions] assert b_chassis.bullet_id not in collided_bullet_ids assert (len(collisions) == 0 or len(collided_bullet_ids) == 1 and GROUND_ID in set(collided_bullet_ids))
# erp=0.1, # contactERP=0.1, # frictionERP=0.1, ) path = Path(__file__).parent / "../smarts/core/models/plane.urdf" path = str(path.absolute()) plane_body_id = client.loadURDF(path, useFixedBase=True) client.changeDynamics(plane_body_id, -1, **frictions(sliders)) pose = pose = Pose.from_center((0, 0, 0), Heading(0)) vehicle = Vehicle( id="vehicle", chassis=AckermannChassis( pose=pose, bullet_client=client, ), ) run( client, vehicle, plane_body_id, sliders, n_steps=int(1e6), ) except Exception as e: print(e) # Uncomment for calculating the acceleration # ax=[(x-vel[i-1])/TIMESTEP_SEC for i, x in enumerate(vel)][1:] plt.figure(1)
# contactERP=0.1, # frictionERP=0.1, ) path = Path(__file__).parent / "../smarts/core/models/plane.urdf" path = str(path.absolute()) plane_body_id = client.loadURDF(path, useFixedBase=True) client.changeDynamics(plane_body_id, -1, **frictions(sliders)) pose = pose = Pose.from_center((0, 0, 0), Heading(0)) vehicle = Vehicle( "hello", chassis=AckermannChassis( pose=pose, bullet_client=client, tire_parameters_filepath="../../smarts/core/models/tire_parameters.yaml", ), ) run( client, vehicle, plane_body_id, sliders, n_steps=int(1e6), ) except Exception as e: print(e) # plt.plot(xx,yy) # plt.plot(time, speed)
def test_ackerman_chassis_size_unchanged(bullet_client: bc.BulletClient): """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) separation_for_collision_error = 0.05 original_vehicle_dimensions = VEHICLE_CONFIGS["passenger"].dimensions shared_heading = Heading(0) chassis = AckermannChassis(Pose.from_center([0, 0, 0], shared_heading), bullet_client) chassis_n = BoxChassis( Pose.from_center( [ 0, (original_vehicle_dimensions.length + separation_for_collision_error), 0, ], shared_heading, ), speed=0, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, bullet_client=bullet_client, ) chassis_e = BoxChassis( Pose.from_center( [ (original_vehicle_dimensions.width + separation_for_collision_error), 0, 0, ], shared_heading, ), speed=0, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, bullet_client=bullet_client, ) chassis_s = BoxChassis( Pose.from_center( [ 0, -(original_vehicle_dimensions.length + separation_for_collision_error), 0, ], shared_heading, ), speed=0, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, bullet_client=bullet_client, ) chassis_w = BoxChassis( Pose.from_center( [ -(original_vehicle_dimensions.width + separation_for_collision_error), 0, 0, ], shared_heading, ), speed=0, dimensions=VEHICLE_CONFIGS["passenger"].dimensions, bullet_client=bullet_client, ) collisions = step_with_vehicle_commands(chassis, steps=10) assert len(collisions) < 1
) path = Path(__file__).parent / "../smarts/core/models/plane.urdf" path = str(path.absolute()) plane_body_id = client.loadURDF(path, useFixedBase=True) client.changeDynamics(plane_body_id, -1, **frictions(sliders)) pose = pose = Pose.from_center((0, 0, 0), Heading(0)) vehicle = Vehicle( "hello", pose=pose, showbase=showbase, chassis=AckermannChassis( pose=pose, bullet_client=client, tire_parameters_filepath= "/home/kyber/MainProjectSMARTS/SMARTS/tools/tire_parameters.yaml", ), ) run( showbase, client, vehicle, plane_body_id, sliders, n_steps=int(1e6), ) except Exception as e: print(e) # plt.plot(xx,yy)
def chassis(bullet_client): return AckermannChassis( Pose.from_center([0, 0, 0], Heading(math.pi * 0.5)), bullet_client)