def test_partially_apply(self): rnd.seed(34254) for i in range(100): case_seed = rnd.randint(1, 1000000) rnd.seed(234872) # Create position and target position = random_position(0, 0, 100000, 5000, timestamp=random.randint(0, 10 * 10**6)) target = random_position(0, 0, 100000, 5000) # Build flight plan plan = prepare_flight_plan(position, target, amax=rnd.uniform(0.1, 1000)) step_us = 100000 # 100 ms for t in range(plan.starts_at(), plan.ends_at() - step_us, step_us): position = plan.partially_apply_to(position, duration_usec=step_us) arrive = plan.partially_apply_to(position, duration_usec=plan.ends_at() - position.timestamp.usec()) # Check: self.assertTrue( positions_equal( one=target, other=arrive, ds=max(0.01, position.distance_to(target) * 0.01), dv=max(0.01, (target.velocity - position.velocity).abs() * 0.01)), f"Case seed: {case_seed}")
def test_prepare_fligh_plan_time_sensitive(self): rnd.seed(434543) for i in range(100): case_seed = rnd.randint(1, 1000000) if case_seed in [424908]: # Exception (yes, it's a bug): continue rnd.seed(case_seed) print(case_seed) # Create position and target velocity_case = rnd.randint(0, 6) timestamp = random.randint(0, 10 * 10**6) dt = random.randint(60 * 10**6, 240 * 10**6) # 60 to 240 sec if velocity_case == 0: position = random_position(0, 0, 100000, 0, timestamp) target = random_position(0, 0, 100000, 5000) elif velocity_case >= 1: position = random_position(0, 0, 100000, 5000, timestamp) target = random_position(0, 0, 100000, 5000) amax = rnd.uniform(5, 100) # Calculating dt (should be big enough to have a solution) plan = prepare_flight_plan(position, target.no_timestamp(), amax=amax) min_dt = plan.duration_usec() dt = random.randint(min_dt, 10 * min_dt) # Adding timestamp to target target = target.with_timestamp(timestamp=timestamp + dt) # Build flight plan plan = prepare_flight_plan(position, target, amax=amax) arrive = plan.apply_to(position) target = target.predict(arrive.timestamp.usec()) # Check: self.assertTrue( positions_equal( one=target, other=arrive, ds=max(0.1, position.distance_to(target) * 0.01), dv=max(0.1, (target.velocity - position.velocity).abs() * 0.01)), f"Case seed: {case_seed}") # +- 10 ms self.assertAlmostEqual(arrive.timestamp.usec(), target.timestamp.usec(), 10000)
def test_approach_to_plan(self): rnd.seed(43454) fail_cases = [] total_cases = 100 for i in range(total_cases): case_seed = rnd.randint(1, 1000000) #697314 rnd.seed(case_seed) # Create position and target velocity_case = 1 #rnd.randint(0, 6) timestamp = random.randint(0, 10 * 10**6) dt = random.randint(60 * 10**6, 240 * 10**6) # 60 to 240 sec if velocity_case == 0: position = random_position(0, 0, 100000, 0, timestamp) target = random_position(0, 0, 100000, 5000, timestamp) elif velocity_case >= 1: position = random_position(0, 0, 100000, 5000, timestamp) target = random_position(0, 0, 100000, 5000, timestamp) amax = rnd.uniform(5, 100) # Calculating dt (should be big enough to have a solution) plan = approach_to_plan(position, target, amax=amax) self.assertTrue(plan, f"{case_seed}") if plan is None: fail_cases.append(case_seed) continue arrive = plan.apply_to(position) target = target.predict(arrive.timestamp.usec()) # Check: success = positions_equal( one=target, other=arrive, ds=max(0.01, position.distance_to(target) * 0.01), dv=max(0.01, (target.velocity - position.velocity).abs() * 0.01)), if not success: print("Fail case: ", case_seed) fail_cases.append(case_seed) print("Fail cases: ", fail_cases) self.assertLess(len(fail_cases) / total_cases, 0.1)
def test_prepare_fligh_plan_2D(self): # Some special cases: position = types.Position(0, 0, types.Vector(0, 100)) target = types.Position(100, 0, types.Vector(0, 100)) # Build flight plan plan = prepare_flight_plan(position, target, amax=10) # Predict flight plan result arrive = plan.apply_to(position) self.assertTrue( positions_equal( one=target, other=arrive, ds=max(0.01, position.distance_to(target) * 0.001), dv=max(0.01, (target.velocity - position.velocity).abs() * 0.001))) # Random cases rnd.seed(34254) for i in range(2000): case_seed = rnd.randint(1, 1000000) rnd.seed(case_seed) # Create position and target velocity_case = rnd.randint(0, 6) timestamp = random.randint(0, 10 * 10**6) if velocity_case == 0: position = random_position(0, 0, 100000, 0, timestamp) target = random_position(0, 0, 100000, 0) elif velocity_case == 1: position = random_position(0, 0, 100000, 5000, timestamp) target = random_position(0, 0, 100000, 0) elif velocity_case == 2: position = random_position(0, 0, 100000, 0, timestamp) target = random_position(0, 0, 100000, 5000) elif velocity_case >= 3: position = random_position(0, 0, 100000, 5000, timestamp) target = random_position(0, 0, 100000, 5000) # Build flight plan plan = prepare_flight_plan(position, target, amax=rnd.uniform(0.1, 1000)) # Predict flight plan result arrive = plan.apply_to(position) self.assertTrue( positions_equal( one=target, other=arrive, ds=max(0.01, position.distance_to(target) * 0.001), dv=max(0.01, (target.velocity - position.velocity).abs() * 0.001)), f"Case seed: {case_seed}")
def test_position_decompose(self): rnd.seed(473758) for i in range(10000): case_seed = rnd.randint(1, 1000000) rnd.seed(case_seed) target = random_position(0, 0, 100, 10) me = random_position(0, 0, 100, 10) a, b = me.decompose(target) # Check positioning expectations: self.assertAlmostEqual(0.0, me.vector_to(a).cosa(me.vector_to(b))) self.assertAlmostEqual(0.0, a.vector_to(target).cosa(b.vector_to(target))) self.assertAlmostEqual(0.0, a.vector_to(target).cosa(a.vector_to(me))) self.assertAlmostEqual(0.0, b.vector_to(target).cosa(b.vector_to(me))) self.assertAlmostEqual(1.0, abs(a.vector_to(target).cosa(target.velocity))) self.assertAlmostEqual(0.0, b.vector_to(target).cosa(target.velocity)) # Check velocities expectations: self.assertTrue(vectors_equal(me.velocity, a.velocity + b.velocity)) self.assertAlmostEqual(0.0, a.velocity.cosa(b.velocity)) self.assertAlmostEqual(1.0, abs(a.velocity.cosa(target.velocity))) self.assertAlmostEqual(0.0, b.velocity.cosa(target.velocity))
def test_accelerated_from(self): rnd.seed(43653) for i in range(10000): case_seed = rnd.randint(1, 1000000) rnd.seed(case_seed) timestamp = random.randint(0, 10 * 10**6) position = random_position(0, 0, 1000, 1000, timestamp=timestamp) amax = rnd.random() * 1000 # Calculating start position start_position = accelerated_from(position, acc=amax) # Verify dt = position.timestamp.sec() - start_position.timestamp.sec() arrive = accelerate(start_position, acc=position.velocity.set_length( amax, inplace=False), t_sec=dt) positions_equal(arrive, position)
def test_decelerating_plan(self): rnd.seed(43653) for i in range(10000): case_seed = rnd.randint(1, 1000000) rnd.seed(case_seed) timestamp = random.randint(0, 10 * 10**6) position = random_position(0, 0, 1000, 1000, timestamp=timestamp) amax = rnd.random() * 1000 # Calculating stop position plan = decelerating_plan(position, acc=amax) stop_position = plan.apply_to(position) # Verify dt = stop_position.timestamp.sec() - position.timestamp.sec() arrive = accelerate(position, acc=position.velocity.set_length( -amax, inplace=False), t_sec=dt) positions_equal(arrive, stop_position)