Box pushing IK algorithm

[dhimiter49]

To test the IK algorithm I implemented a test function in 03d651b. The function chooses a random desired point in a bounded space beneath the robot where the tcp should be positioned (calls set_tcp_pos(), however the same can be achieved by calling env.calculateOfflineIK(world_point, np.array([0, 1, 0, 0])) instead of set_tcp_pos(world_point, hard_set=True) in line 132).

When using the original implementation of IK that handles overshooting by limiting dt the failure rate was 30.6% for a failure threshold of 0.3 or 0.2. Commenting out the overshooting condition results in a failure rate of 2.8% for threshold of 0.2 and 0% for threshold of 0.3.

Testing setup in fancy_gym/examples/examples_general.py

diff --git a/fancy_gym/examples/examples_general.py b/fancy_gym/examples/examples_general.py
index d7d1bfc..8321cbc 100644
--- a/fancy_gym/examples/examples_general.py
+++ b/fancy_gym/examples/examples_general.py
@@ -31,18 +31,22 @@ def example_general(env_id="Pendulum-v1", seed=1, iterations=1000, render=Tru
e):
     print("Observation shape: ", env.observation_space.shape)
     print("Action shape: ", env.action_space.shape)
 
+    tcp_failur = 0
     # number of environment steps
     for i in range(iterations):
+        tcp_failur += set_tcp_testing(0, env, render)
         obs, reward, done, info = env.step(env.action_space.sample())
         rewards += reward
+        set_tcp_testing(1, env, render)
 
         if render:
-            env.render()
+            env.render(mode="human")
 
         if done:
             print(rewards)
             rewards = 0
             obs = env.reset()
+    print(tcp_failur)
 
 
 def example_async(env_id="HoleReacher-v0", n_cpu=4, seed=int('533D', 16), n_samples=800):
@@ -91,13 +95,13 @@ if __name__ == '__main__':
     render = True
 
     # Basic gym task
-    example_general("Pendulum-v1", seed=10, iterations=200, render=render)
+    example_general("BoxPushingBinSparse1ReplanProDMP-v0", seed=0, iterations=1000, render=rende
r)
 
     # Mujoco task from framework
-    example_general("Reacher5d-v0", seed=10, iterations=200, render=render)
+    # example_general("Reacher5d-v0", seed=10, iterations=200, render=render)
 
     # # OpenAI Mujoco task
-    example_general("HalfCheetah-v2", seed=10, render=render)
+    # example_general("HalfCheetah-v2", seed=10, render=render)
 
     # Vectorized multiprocessing environments
     # example_async(env_id="HoleReacher-v0", n_cpu=2, seed=int('533D', 16), n_samples=2 * 200)