Diff of /envs/target/test_v_tgt.py [000000] .. [077a87]

Switch to side-by-side view

--- 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))
+