refactor calculation of action for vel/pos modes

vikashplus · May 1, 2024 · 7381aa4 · 7381aa4
1 parent ccd8ede
commit 7381aa4
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 13 deletions.
diff --git a/robohive/envs/multi_task/common/kitchen/franka_kitchen.xml b/robohive/envs/multi_task/common/kitchen/franka_kitchen.xml
@@ -47,14 +47,13 @@
             <geom type="box" group="3" pos='0 0 .142' size="0.02 0.10 0.03" contype="0" conaffinity="0" rgba=".9 .7 .95 1" euler="0 0 -.785"/>
         </body>
 
-        <site name='target' pos='0 0 0' size='0.1' rgba='0 2 0 .2'/>
+        <site name='target' type='box' size='.03 .07 .04' pos='-0.4 0.4 2.1' group='1' rgba='0 1 .4 0' euler="0 3.14 3.14"/>
         <camera name='left_cam' pos='-1.4 -0.75 3' quat='0.78 0.49 -0.22 -0.32' />
         <camera name='right_cam' pos='1.4 -0.75 3' quat='0.76 0.5 0.21 0.35'/>
         <camera name='top_cam' pos='-.3 -.95 3.5' euler='0.785 0 0'/>
 
 
         <!-- Robot -->
-        <site name='ee_target' type='box' size='.03 .07 .04' pos='-0.4 0.4 2.1' group='1' rgba='0 1 .4 0' euler="0 3.14 3.14"/>
         <body name="chef" pos='0. 0 1.8' euler='0 0 1.57'>
             <geom type='cylinder' size='.120 .90' pos='-.04 0 -0.90' class='panda_viz'/>
             <include file="../../../../simhive/franka_sim/assets/chain0.xml"/>

diff --git a/robohive/tutorials/ee_teleop.py b/robohive/tutorials/ee_teleop.py
@@ -9,7 +9,7 @@
     - NOTE: Tutorial is written for franka arm and robotiq gripper. This demo is a tutorial, not a generic functionality for any any environment
 EXAMPLE:\n
     - python tutorials/ee_teleop.py -e rpFrankaRobotiqData-v0\n
-    - python tutorials/ee_teleop.py -e FK1_LightOnFixed-v4\n
+    - python tutorials/ee_teleop.py -e FK1_LightOnFixed-v4 --goal_site target\n
 """
 # TODO: (1) Enforce pos/rot/grip limits (b) move gripper to delta commands
 
@@ -252,26 +252,22 @@ def main(env_name, env_args, reset_noise, action_noise, input_device, output, ho
             if ik_result.success==False:
                 print(f"IK(t:{i_step}):: Status:{ik_result.success}, total steps:{ik_result.steps}, err_norm:{ik_result.err_norm}")
             else:
+                target_qpos = ik_result.qpos[:7]
                 if env.env.robot._act_mode == "pos":
-                    act[:7] = ik_result.qpos[:7]
+                    act[:7] = target_qpos
                     act[7:] = gripper_state
-                    if action_noise:
-                        act = act + env.env.np_random.uniform(high=action_noise, low=-action_noise, size=len(act)).astype(act.dtype)
-                    if env.normalize_act:
-                        act = env.env.robot.normalize_actions(act)
                 elif env.env.robot._act_mode == "vel":
                     curr_qpos = env.sim.get_state()['qpos'][:7]
-                    target_qpos = ik_result.qpos[:7]
                     qvel = (target_qpos - curr_qpos) / env.dt
                     act[:7] = qvel
                     act[7:] = gripper_state
-                    if action_noise:
-                        act = act + env.env.np_random.uniform(high=action_noise, low=-action_noise, size=len(act)).astype(act.dtype)
-                    if env.normalize_act:
-                        act = env.env.robot.normalize_actions(act)
                 else:
                     raise TypeError("Unknown act mode: {}".format(env.env.robot._act_mode))
 
+                if action_noise:
+                    act = act + env.env.np_random.uniform(high=action_noise, low=-action_noise, size=len(act)).astype(act.dtype)
+                if env.normalize_act:
+                    act = env.env.robot.normalize_actions(act)
             # nan actions for last log entry
             act = np.nan*np.ones(env.action_space.shape) if i_step == horizon else act