From 1ad7a4fe525f6a0b95d76b1c2269c8484c6cb799 Mon Sep 17 00:00:00 2001 From: Sriram S K Date: Tue, 9 Apr 2024 21:16:24 +0530 Subject: [PATCH 1/5] fix normalize for "vel" mode of robot --- robohive/robot/robot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index 4daa9a5d..1b910efe 100644 --- a/robohive/robot/robot.py +++ b/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)) From f256763bec5914592a495389a5e6efccae93b60c Mon Sep 17 00:00:00 2001 From: Sriram S K Date: Tue, 9 Apr 2024 21:24:13 +0530 Subject: [PATCH 2/5] support teleop with "vel" mode environments --- .../common/kitchen/franka_kitchen.xml | 1 + robohive/tutorials/ee_teleop.py | 28 ++++++++++++++----- 2 files changed, 22 insertions(+), 7 deletions(-) diff --git a/robohive/envs/multi_task/common/kitchen/franka_kitchen.xml b/robohive/envs/multi_task/common/kitchen/franka_kitchen.xml index abc6490e..a3c58daf 100644 --- a/robohive/envs/multi_task/common/kitchen/franka_kitchen.xml +++ b/robohive/envs/multi_task/common/kitchen/franka_kitchen.xml @@ -54,6 +54,7 @@ + diff --git a/robohive/tutorials/ee_teleop.py b/robohive/tutorials/ee_teleop.py index 2808e775..4d76bbc0 100644 --- a/robohive/tutorials/ee_teleop.py +++ b/robohive/tutorials/ee_teleop.py @@ -9,6 +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 """ # TODO: (1) Enforce pos/rot/grip limits (b) move gripper to delta commands @@ -251,12 +252,25 @@ 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 - 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) + if env.env.robot._act_mode == "pos": + act[:7] = ik_result.qpos[:7] + 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) + 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)) # nan actions for last log entry act = np.nan*np.ones(env.action_space.shape) if i_step == horizon else act @@ -294,4 +308,4 @@ def main(env_name, env_args, reset_noise, action_noise, input_device, output, ho if __name__ == '__main__': - main() \ No newline at end of file + main() From ccd8ede3f400549700ecdf49cfc80731a78f047f Mon Sep 17 00:00:00 2001 From: Sriram S K Date: Mon, 29 Apr 2024 19:22:43 +0530 Subject: [PATCH 3/5] fix qvel calculation --- robohive/tutorials/ee_teleop.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/robohive/tutorials/ee_teleop.py b/robohive/tutorials/ee_teleop.py index 4d76bbc0..b4b47e08 100644 --- a/robohive/tutorials/ee_teleop.py +++ b/robohive/tutorials/ee_teleop.py @@ -262,13 +262,13 @@ def main(env_name, env_args, reset_noise, action_noise, input_device, output, ho 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) + 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) + if env.normalize_act: + act = env.env.robot.normalize_actions(act) else: raise TypeError("Unknown act mode: {}".format(env.env.robot._act_mode)) From 7381aa4b2e67f963823662503fbba681124c0b43 Mon Sep 17 00:00:00 2001 From: Sriram S K Date: Wed, 1 May 2024 17:47:54 +0530 Subject: [PATCH 4/5] refactor calculation of action for vel/pos modes --- .../common/kitchen/franka_kitchen.xml | 3 +-- robohive/tutorials/ee_teleop.py | 18 +++++++----------- 2 files changed, 8 insertions(+), 13 deletions(-) diff --git a/robohive/envs/multi_task/common/kitchen/franka_kitchen.xml b/robohive/envs/multi_task/common/kitchen/franka_kitchen.xml index a3c58daf..0c39aafc 100644 --- a/robohive/envs/multi_task/common/kitchen/franka_kitchen.xml +++ b/robohive/envs/multi_task/common/kitchen/franka_kitchen.xml @@ -47,14 +47,13 @@ - + - diff --git a/robohive/tutorials/ee_teleop.py b/robohive/tutorials/ee_teleop.py index b4b47e08..24ca79a9 100644 --- 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 From 32619e5c11798e6ca50c4b74290d5f7461d7e67c Mon Sep 17 00:00:00 2001 From: Sriram S K Date: Wed, 1 May 2024 20:03:34 +0530 Subject: [PATCH 5/5] move goal site to end effector on init --- .../common/kitchen/franka_kitchen.xml | 2 +- robohive/tutorials/ee_teleop.py | 20 ++++++++++++++++++- 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/robohive/envs/multi_task/common/kitchen/franka_kitchen.xml b/robohive/envs/multi_task/common/kitchen/franka_kitchen.xml index 0c39aafc..70ea1b6f 100644 --- a/robohive/envs/multi_task/common/kitchen/franka_kitchen.xml +++ b/robohive/envs/multi_task/common/kitchen/franka_kitchen.xml @@ -47,7 +47,7 @@ - + diff --git a/robohive/tutorials/ee_teleop.py b/robohive/tutorials/ee_teleop.py index 24ca79a9..640d4c65 100644 --- a/robohive/tutorials/ee_teleop.py +++ b/robohive/tutorials/ee_teleop.py @@ -13,7 +13,7 @@ """ # 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 @@ -148,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') @@ -216,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):