Safenet boundary example added

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

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'
 

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()
 

examples/safenet/safenet.yaml

@@ -2,5 +2,5 @@
 
 safenet: 
   use_safenet: False
-  lower: null
-  upper: null
+  lower: [0.35, 0.06, 0.15]
+  upper: [0.55, 0.26, 0.31]

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']

perls2/robots/real_robot_interface.py

@@ -89,19 +89,40 @@ 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):
         """ Reset robot to neutral positions.
         """
         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):