From 4a9124f084a87d53e43ca323d98226b75a558a9c Mon Sep 17 00:00:00 2001 From: Danfei Xu Date: Mon, 1 Mar 2021 11:21:06 -0800 Subject: [PATCH] Safenet boundary example added --- cfg/default_panda_controller.yaml | 4 +- demos/__init__.py | 0 demos/demo_control_cfg.yaml | 2 +- examples/safenet/run_safenet_example.py | 63 +++++++++++++++---------- examples/safenet/safenet.yaml | 4 +- examples/safenet/safenet_example.yaml | 14 +++--- perls2/robots/real_robot_interface.py | 25 +++++++++- 7 files changed, 76 insertions(+), 36 deletions(-) create mode 100644 demos/__init__.py diff --git a/cfg/default_panda_controller.yaml b/cfg/default_panda_controller.yaml index d05aa169..6b94f071 100644 --- a/cfg/default_panda_controller.yaml +++ b/cfg/default_panda_controller.yaml @@ -8,8 +8,8 @@ panda_controller: damping: 1.0 #1 #0.5 #1 #0.5 # Damping factor [0,1] input_max: 1.0 input_min: -1.0 - output_max: 0.1 - output_min: -0.1 + output_max: 1.0 + output_min: -1.0 JointVelocity: kv: 10.0 #0.5 input_max: 1.0 diff --git a/demos/__init__.py b/demos/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/demos/demo_control_cfg.yaml b/demos/demo_control_cfg.yaml index 1118b159..1948f811 100644 --- a/demos/demo_control_cfg.yaml +++ b/demos/demo_control_cfg.yaml @@ -1,7 +1,7 @@ # Cfg file for Demo control environment world: type: 'Bullet' #'Bullet' - robot: 'panda' + robot: 'sawyer' controlType: 'EEImpedance' data_dir: 'data' diff --git a/examples/safenet/run_safenet_example.py b/examples/safenet/run_safenet_example.py index d92c37de..41fb80cd 100644 --- a/examples/safenet/run_safenet_example.py +++ b/examples/safenet/run_safenet_example.py @@ -5,64 +5,79 @@ from safenet_env import SafenetEnv import logging import numpy as np +from demos.demo_path import Line # logging.basicConfig(level=logging.DEBUG) +DELTA_STEP_SIZE = 0.015 -def get_action(observation): - """Run your policy to produce an action. +def get_next_action(curr_pose, path, step_num): + """Get action as delta to next waypoint on path. """ - action = [0, 0, 0, 0, 0, 0] + action = np.subtract(path[step_num][:3], curr_pose[:3]) + action = action / np.linalg.norm(action) * DELTA_STEP_SIZE return action -def get_next_action(curr_pose, axis): - """Set next ee_pose fixed amount in x direction. - """ - action = np.zeros(3) - action[axis] = 0.01 - return action +# def get_next_action(curr_pose, axis): +# """Set next ee_pose fixed amount in x direction. +# """ +# action = np.zeros(3) +# action[axis] = 0.015 +# return action def within_tol(current, goal): return np.any(np.abs(current - goal) > 0.01) env = SafenetEnv('safenet_example.yaml', True, "SafenetEnv") - step = 0 observation = env.reset() initial_ee_pos = observation['ee_position'] -upper_limit = np.add(initial_ee_pos, [0.1, 0.1, 0.1]) -lower_limit = np.add(initial_ee_pos, [-0.1, -0.1, -0.1]) -# Set safenet boundaries -env.robot_interface.set_safenet_boundaries(lower_limit, upper_limit) +if env.robot_interface.use_safenet: + upper_limit = env.robot_interface.safenet_ee_pos_upper + lower_limit = env.robot_interface.safenet_ee_pos_lower +else: + upper_limit = np.add(initial_ee_pos, [0.1, 0.1, 0.1]) + lower_limit = np.add(initial_ee_pos, [-0.05, -0.05, -0.05]) + env.robot_interface.set_safenet_boundaries(lower_limit, upper_limit) print("#############################################") print("Safenet Example") print("Current ee position: \t{}".format(initial_ee_pos)) -print("Safenet upper limit: \t{}".format(upper_limit)) -print("Safenet lower limit: \t{}".format(lower_limit)) +print("Safenet upper limit: \t{}".format(env.robot_interface.safenet_ee_pos_upper)) +print("Safenet lower limit: \t{}".format(env.robot_interface.safenet_ee_pos_lower)) print("#############################################") axis_names = ["X", "Y", "Z"] for axis_num, axis in enumerate(axis_names): print("Stepping forward along {} axis".format(axis)) env.reset() + step_num = 0 + path = Line(env.robot_interface.ee_pose, + num_pts=(0.12 / DELTA_STEP_SIZE), + delta_val=DELTA_STEP_SIZE, + dim_num=axis) done = False while not done: start = time.time() - action = get_next_action(observation['ee_position'], axis_num) + action = get_next_action(observation['ee_position'], path, step_num) + step_num += 1 + print("Waypoint goal {}".format(path[step_num])) # Pass the start time to enforce policy frequency. observation, reward, termination, info = env.step(action, start=start) + # if not env.world.is_sim: + # input("press enter to continue") # print("Curr ee_pos: \t{}".format(observation['ee_position'])) if (np.any(np.subtract(observation['ee_position'], upper_limit) > 0.01) or np.any(np.subtract(lower_limit, observation['ee_position']) > 0.01)) : - print("Safenet boundary exceeded!") + print("Safenet boundary exceeded! Stopping.") done = True - # input("press enter to step") - done = termination + else: # input("press enter to step") + done = termination - # In the real robot we have to use a ROS interface. Disconnect the interface - # after completing the experiment. - if (not env.world.is_sim): - env.robot_interface.disconnect() +env.reset() +# In the real robot we have to use a ROS interface. Disconnect the interface +# after completing the experiment. +if (not env.world.is_sim): + env.robot_interface.disconnect() diff --git a/examples/safenet/safenet.yaml b/examples/safenet/safenet.yaml index e8c71db9..70bf8c4a 100644 --- a/examples/safenet/safenet.yaml +++ b/examples/safenet/safenet.yaml @@ -2,5 +2,5 @@ safenet: use_safenet: False - lower: null - upper: null \ No newline at end of file + lower: [0.35, 0.06, 0.15] + upper: [0.55, 0.26, 0.31] diff --git a/examples/safenet/safenet_example.yaml b/examples/safenet/safenet_example.yaml index 0da46546..e6184d0c 100644 --- a/examples/safenet/safenet_example.yaml +++ b/examples/safenet/safenet_example.yaml @@ -1,7 +1,7 @@ # perls2 Template Project Config world: type: 'Bullet' # Choose from 'Bullet' or 'Real' - robot: 'sawyer' # Choose from 'sawyer' or 'panda' + robot: 'panda' # Choose from 'sawyer' or 'panda' controlType: "EEImpedance" # Choose from "EEImpedance", "EEPosture", "JointImpedance", "JointVelocity" or "Joint Torque" # Uncomment to use your own data directory for urdfs, objects etc. @@ -10,18 +10,20 @@ data_dir: 'data' # Simulation parameters sim_params: time_step: 0.004166 # 1./240. default for PyBullet. Changing not recommended. - MAX_STEPS: 100 + MAX_STEPS: 50 policy_freq: 20 # Frequency at which policy sends actions (Hz) control_freq: 500 # Frequency at which torque commands are sent to robot (Hz) # Robots are specified by types and urdf locations # also may include intial setup like poses and orientations -!include cfg/sawyer.yaml -!include cfg/panda.yaml +# Robots are specified by types and urdf locations +# also may include intial setup like poses and orientations +!include ../../cfg/sawyer.yaml +!include ../../cfg/panda.yaml +!include ../../cfg/table_and_bin.yaml +!include ../../cfg/redis.yaml -# Some basic tables and objects pre-configured. -!include cfg/table_and_bin.yaml # Specify which basic objects to include. scene_objects: ['table', 'bin'] # Choose from ['table', 'bin'] diff --git a/perls2/robots/real_robot_interface.py b/perls2/robots/real_robot_interface.py index 81ed0d9b..c130fe33 100644 --- a/perls2/robots/real_robot_interface.py +++ b/perls2/robots/real_robot_interface.py @@ -89,6 +89,8 @@ def set_control_params(self): """Set the control parameters key on redis """ control_config = self.controller_cfg['Real'][self.control_type] + if self.use_safenet: + control_config['position_limits'] = np.array([self.safenet_ee_pos_lower, self.safenet_ee_pos_upper]) self.redisClient.set(CONTROLLER_CONTROL_PARAMS_KEY, json.dumps(control_config)) def reset(self): @@ -96,12 +98,31 @@ def reset(self): """ raise NotImplementedError + def set_safenet_boundaries(self, lower, upper): + """Set position limits for existing controller. + + Controller will clip all end-effector goals to these limits. + + Args: + lower (ndarray): 3f lower boundary position limits for EE + upper (ndarray): 3f upper boundary position limits for EE + + Return: + None + """ + assert (np.all(lower <= upper)) + self.safenet_ee_pos_upper = upper.tolist() + self.safenet_ee_pos_lower = lower.tolist() + self.use_safenet = True + self.set_controller_params_from_config() + def set_controller_params_from_config(self): """Set controller parameters from config file to redis. """ selected_type = self.config['world']['controlType'] self.control_config = self.controller_cfg['Real'][selected_type] - + if self.use_safenet: + self.control_config['position_limits'] = [self.safenet_ee_pos_lower, self.safenet_ee_pos_upper] self.redisClient.mset({CONTROLLER_CONTROL_PARAMS_KEY: json.dumps(self.control_config), CONTROLLER_CONTROL_TYPE_KEY: selected_type}) @@ -224,6 +245,8 @@ def move_ee_delta(self, delta, set_pos=None, set_ori=None): raise ValueError("delta cannot be none.") kwargs = {'cmd_type': MOVE_EE_DELTA, 'delta': delta, 'set_pos': set_pos, 'set_ori': set_ori} + + print(kwargs) self.set_controller_goal(**kwargs) def set_ee_pose(self, set_pos, set_ori, **kwargs):