--- a +++ b/envs/target/test_v_tgt.py @@ -0,0 +1,39 @@ +#from v_tgt_field import VTgtField +from envs.target import VTgtField +import sys +import numpy as np +import matplotlib.pyplot as plt +# -------------------------------------------------------------------- + +#... + +# -------------------------------------------------------------------- +version = 3 +# 0: constant forward velocities +# 1: consecutive sinks forward for walking +# 2: consecutive sinks for walking (-90 < th < 90) (Round 1) +# 3: consecutive sinks for walking (-180 < th < 180) (Round 2) + +dt = .01 +#dt = .5 +pose_agent = np.array([0, 0, 0]) # [x, y] + +vtgt_v1 = VTgtField(version=version, dt=dt) +vtgt_v1.reset(version=version, seed=0) + +t_sim = 25; +x = 0; y = 0; th = 0 +pose_t = np.array([[x], [y], [th]]) + +for t in np.arange(0, t_sim, dt): + pose = np.array([x, y, th]) # [x, y, theta] + vtgt_field_local, flag_new_target = vtgt_v1.update(pose) + + vtgt = vtgt_v1.get_vtgt(pose[0:2]) + + x += np.asscalar(vtgt[0])*dt + y += np.asscalar(vtgt[1])*dt + + if flag_new_target: + print('Target achieved at {} sec'.format(t)) +