Support tele-op for Franka Kitchen environments ("vel" mode)

robohive/envs/multi_task/common/kitchen/franka_kitchen.xml

@@ -47,7 +47,7 @@
             <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 0 0' 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'/>

robohive/robot/robot.py

@@ -574,7 +574,7 @@ def normalize_actions(self, controls, out_space='sim', unnormalize=False):
                         act_rng = (actuator['pos_range'][1]-actuator['pos_range'][0])/2.0
                     elif self._act_mode == "vel":
                         act_mid = (actuator['vel_range'][1]+actuator['vel_range'][0])/2.0
-                        act_rng = (actuator['vel_range'][1]-actuator['pos_range'][0])/2.0
+                        act_rng = (actuator['vel_range'][1]-actuator['vel_range'][0])/2.0
                     else:
                         raise TypeError("Unknown act mode: {}".format(self._act_mode))
 

robohive/tutorials/ee_teleop.py

@@ -9,10 +9,11 @@
     - 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 --goal_site target\n
 """
 # TODO: (1) Enforce pos/rot/grip limits (b) move gripper to delta commands
 
-from robohive.utils.quat_math import euler2quat, mulQuat
+from robohive.utils.quat_math import euler2quat, mulQuat, mat2quat
 from robohive.utils.inverse_kinematics import IKResult, qpos_from_site_pose
 from robohive.logger.roboset_logger import RoboSet_Trace
 from robohive.logger.grouped_datasets import Trace as RoboHive_Trace
@@ -147,6 +148,21 @@ def poll_gamepad(input_device):
 
     return delta_pos * scale_factor, delta_euler * scale_factor, delta_gripper, done
 
+def move_goal_site_to_end_effector(teleop_site, goal_site, physics):
+    """
+    Get the location of the teleop site (the end effector),
+    and place the goal site exactly at this location.
+
+    teleop_site: A string specifying the name of the teleoperation site.
+    goal_site: A string specifying the name of the goal site.
+    physics: A `mujoco.Physics` instance.
+    """
+    ee_sid = physics.model.site_name2id(teleop_site)
+    goal_sid = physics.model.site_name2id(goal_site)
+    ee_xpos = physics.data.site_xpos[ee_sid]
+    ee_xquat = mat2quat(physics.data.site_xmat[ee_sid].reshape(3,3))
+    physics.model.site_pos[goal_sid] = ee_xpos
+    physics.model.site_quat[goal_sid] = ee_xquat
 
 @click.command(help=DESC)
 @click.option('-e', '--env_name', type=str, help='environment to load', default='rpFrankaRobotiqData-v0')
@@ -215,6 +231,9 @@ def main(env_name, env_args, reset_noise, action_noise, input_device, output, ho
         act = np.zeros(env.action_space.shape)
         gripper_state = 0
 
+        # Position the goal site exactly at the init location of the end effector
+        move_goal_site_to_end_effector(teleop_site, goal_site, env.sim)
+
         # start rolling out
         for i_step in range(horizon+1):
 
@@ -251,13 +270,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:
-                act[:7] = ik_result.qpos[:7]
-                act[7:] = gripper_state
+                target_qpos = ik_result.qpos[:7]
+                if env.env.robot._act_mode == "pos":
+                    act[:7] = target_qpos
+                    act[7:] = gripper_state
+                elif env.env.robot._act_mode == "vel":
+                    curr_qpos = env.sim.get_state()['qpos'][:7]
+                    qvel = (target_qpos - curr_qpos) / env.dt
+                    act[:7] = qvel
+                    act[7:] = gripper_state
+                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
 
@@ -294,4 +322,4 @@ def main(env_name, env_args, reset_noise, action_noise, input_device, output, ho
 
 
 if __name__ == '__main__':
-    main()
+    main()