Add Safenet example

Add example to show safenet functionality by setting ee position limits
in x, y and z direction.

examples/safenet/README.md

@@ -0,0 +1,10 @@
+# Project Template ReadME
+
+This is a template folder you can use to start building your project in perls2. Simply copy the `project_template` folder over and get started!
+
+## Custom data directory
+If you want to use your own data directory for object meshes and urdfs, be sure to specify it with the absolute filepath in the config file.
+
+Robot urdfs and meshes will always be loaded from the perls2/data directory.
+If you want to change those, you should do it within a local copy of the perls2 library.
+

examples/safenet/cfg/default_camera.yaml

@@ -0,0 +1,43 @@
+# Camera parameters.
+sensor:
+  camera:
+    name:
+        'camera'
+    image:
+      height: 224
+      width: 224
+    extrinsics:
+        eye_position:
+          [0.6, 0.0, 1.0]
+        target_position:
+          [0.6, 0., 0]
+        up_vector:
+          [1., 0., 1.]
+    intrinsics:
+        fov: 60
+        near_plane: 0.02
+        far_plane: 100
+    # Parameters for randomization
+    random:
+      randomize: False
+      extrinsics:
+        eye_position:
+          lower:
+            [0.6, 0., 1.75]
+          upper:
+            [0.6, 0., 1.75]
+        target_position:
+          lower:
+            [0.6, 0., 0]
+          upper:
+            [0.6, 0., 0]
+      intrinsics:
+        fov:
+          lower: 50
+          upper: 80
+        near_plane:
+          lower: 0.01
+          upper: 0.05
+        far_plane:
+          lower: 10
+          upper: 150

examples/safenet/cfg/default_controllers.yaml

@@ -0,0 +1,81 @@
+controller:
+  selected_type: 'JointTorque' # Choose between 'EEImpedance', 'JointVelocity'
+  Bullet:
+    EEImpedance:
+      kp: [65.0, 65.0, 65.0, 10., 10., 10.] #70 #50 #200 #75 #200 # P Gain for Impedance Control
+      damping: 1.0 #1 #0.5 #1 #0.5 # Damping factor [0,1]
+      input_max: 1.0
+      input_min: -1.0
+      output_max: 1.0
+      output_min: -1.0
+    JointVelocity:
+      kv: 2.0 #0.5
+      input_max: 1.0
+      input_min: -1.0
+      output_max: 1.0 #1.0
+      output_min: -1.0 #-1.0
+    JointImpedance:
+      kp: 250 #100
+      damping: 1.0
+      input_max: 1.0
+      input_min: -1.0
+      output_max: 1.0
+      output_min: -1.0
+    JointTorque:
+      input_max: 1.0
+      input_min: -1.0
+      output_max: 5.0
+      output_min: -5.0
+    EEPosture:
+      kp: 80 #70 #50 #200 #75 #200 # P Gain for Impedance Control
+      damping: 1.0 #1 #0.5 #1 #0.5 # Damping factor [0,1]
+      posture_gain: 60
+      posture: [0,-1.18,0.00,2.18,0.00,0.57,3.3161]
+      input_max: 1.0
+      input_min: -1.0
+      output_max: 1.0
+      output_min: -1.0
+  Real:
+    EEImpedance:
+      kp:  [40, 40, 40, 5.0, 5.0, 3.0] #70 #50 #200 #75 #200 # P Gain for Impedance Control
+      kv: [10.0, 10.0, 10.0, 1.0, 1.0, 1.7]
+      damping: 1.0 #1 #0.5 #1 #0.5 # Damping factor [0,1]
+      input_max: 1.0
+      input_min: -1.0
+      output_max: 1.0
+      output_min: -1.0
+    JointVelocity:
+      kv: 10.0 #0.5
+      input_max: 1.0
+      input_min: -1.0
+      output_max: 1.0 #1.0
+      output_min: -1.0 #-1.0
+    JointImpedance:
+      kp: [20.0, 40.0, 20.0, 40.0, 10.0, 12.0, 4.0] #[70.0, 70.0, 60.0, 60.0, 50.0, 50.0, 20.0] #[100.0, 100.0, 80.0, 80.0, 70.0, 70.0, 50.0]
+      kv: [2.0, 3.0, 2.0, 2.0, 2.0, 2.0, 0.2]
+      damping: 1.0
+      input_max: 1.0
+      input_min: -1.0
+      output_max: 1.0
+      output_min: -1.0
+    JointTorque:
+      input_max: 1.0
+      input_min: -1.0
+      output_max: 5.0
+      output_min: -5.0
+    EEPosture:
+      kp:  [40, 40, 40, 5.0, 5.0, 3.0] #70 #50 #200 #75 #200 # P Gain for Impedance Control
+      kv: [10.0, 10.0, 10.0, 1.0, 1.0, 1.7]
+      damping: 1.0 #1 #0.5 #1 #0.5 # Damping factor [0,1]
+      posture_gain: [5.0, 5.0, 5.0, 4.0, 3.0, 3.0, 3.0]  #[7.0, 7.0, 7.0, 6.0, 5.0, 5.0, 5.0] #0.7
+      input_max: 1.0
+      input_min: -1.0
+      output_max: 1.0
+      output_min: -1.0
+  interpolator_pos:
+      type: 'linear'
+      order: 1
+      max_dx: 0.2
+      ramp_ratio: 0.2
+  interpolator_ori:
+      type: 'linear'

examples/safenet/cfg/panda.yaml

@@ -0,0 +1,53 @@
+#Cfg file specifying FRANKA PANDA ARM
+panda:
+  arm:
+    path:
+      #'robot/franka_description/robots/panda_arm_hand.urdf'
+      'robot/franka_panda/panda.urdf'
+    pose:
+      [0, 0, 0]
+    orn:
+      [0, 0, 0]
+    is_static:
+      True
+  base: 'None'
+  neutral_joint_angles:
+   [0., -0.3135, 0., -2.515, 0., 2.226, 0.87]
+
+
+  limb_joint_names: [
+  'panda_link0',
+  'panda_link1',
+  'panda_link2',
+  'panda_link3',
+  'panda_link4',
+  'panda_link5',
+  'panda_link6',
+  ]
+
+  # TODO: Change these to actual values. with gripper.
+  # Height of the limb (installed on the base) from the ground, for computing IK.
+  limb_height: 0.9
+  # Link name of the end effector, for computing IK.
+  end_effector_name: 'right_hand'
+  # Joint name of the left finger of the gripper.
+  l_finger_name: 'panda_leftfinger'
+  # Joint name of the right finger of the gripper.
+  r_finger_name: 'panda_rightfinger'
+  # Link name of the left finger tip of the gripper.
+  l_finger_tip_name: 'r_gripper_l_finger_tip'
+  # Link name of the right finger tip of the gripper.
+  r_finger_tip_name: 'r_gripper_r_finger_tip'
+
+  # Default maximum joint velocity ratio, chosen from (0, 1].
+  # Set it to 1.0 for data collection, 0.3 for demo.
+  limb_max_velocity_ratio: 0.01
+
+  # Threshold (in rads) for each joint for position control.
+  limb_position_threshold: 0.008726640
+
+  # Threshold (in rads) for each joint for position control.
+  limb_velocity_threshold: 0.75
+
+  # Step size of a gripper trajectory.
+  end_effector_step: 0.02

examples/safenet/cfg/redis.yaml

@@ -0,0 +1,7 @@
+# Redis config for perls2 installation on this machine. 
+
+# Redis-server hosted by NUC for 
+redis:
+  host: 172.16.0.1
+  port: 6379
+  password: '/home/robot/.redis/redis_passfile.txt'

examples/safenet/cfg/sawyer.yaml

@@ -0,0 +1,57 @@
+# Cfg file for sawyer arm
+sawyer:
+  arm:
+    path:
+      'robot/rethink/sawyer_description/urdf/sawyer_arm.urdf'
+      # 'robot/rethink/sawyer_description/urdf/sawyer_urdf.urdf'
+    pose:
+      [0, 0, 0]
+    orn:
+      [0, 0, 0]
+    is_static:
+      True
+  base:
+    path:
+      'robot/rethink/sawyer_description/urdf/sawyer_base.urdf'
+    pose:
+      [0, 0, 0.01]
+    orn:
+      [0, 0, 0]
+    is_static:
+      True
+  neutral_joint_angles:
+    [0.00, -1.18, 0.00, 2.18, 0.00, 0.57, 1.5708]
+  limb_joint_names: [
+  'right_j0',
+  'right_j1',
+  'right_j2',
+  'right_j3',
+  'right_j4',
+  'right_j5',
+  'right_j6',
+  ]
+  # Height of the limb (installed on the base) from the ground, for computing IK.
+  limb_height: 0.9
+  # Link name of the end effector, for computing IK.
+  end_effector_name: 'right_hand'
+  # Joint name of the left finger of the gripper.
+  l_finger_name: 'r_gripper_l_finger'
+  # Joint name of the right finger of the gripper.
+  r_finger_name: 'r_gripper_r_finger'
+  # Link name of the left finger tip of the gripper.
+  l_finger_tip_name: 'r_gripper_l_finger_tip'
+  # Link name of the right finger tip of the gripper.
+  r_finger_tip_name: 'r_gripper_r_finger_tip'
+
+  # Default maximum joint velocity ratio, chosen from (0, 1].
+  # Set it to 1.0 for data collection, 0.3 for demo.
+  limb_max_velocity_ratio: 0.01
+
+  # Threshold (in rads) for each joint for position control.
+  limb_position_threshold: 0.008726640
+
+  # Threshold (in rads) for each joint for position control.
+  limb_velocity_threshold: 0.75
+
+  # Step size of a gripper trajectory.
+  end_effector_step: 0.02

examples/safenet/cfg/table_and_bin.yaml

@@ -0,0 +1,22 @@
+ground:
+  path: 'scene_objects/plane/plane.urdf'
+  pose: [[0, 0, -0.9], [0, 0, 0]]
+  is_static: True
+
+# path: 'tables/table_svl/table.urdf'
+table:
+  path: 'scene_objects/table_svl/table.urdf' #'scene_objects/table_svl/table_grasp.urdf'
+  pose: [[0.6, 0, -0.05], [0, 0, 0.0]]
+  height:
+    range: [0.8, 0.8]
+  is_static: True
+
+bin:
+  path: 'scene_objects/beveled_bin/beveled_bin.urdf'
+  pose: [[0.6, 0, 0], [0, 0, 0]]
+  is_static: True
+
+ikea:
+  path: 'ikea/config/ikea.urdf'
+  pose: [[0.75, 0.3, 0.5], [1.57079, 0, -0.157079]]
+  is_static: True

examples/safenet/run_safenet_example.py

@@ -0,0 +1,68 @@
+"""Project template example running the environment.
+"""
+from __future__ import division
+import time
+from safenet_env import SafenetEnv
+import logging
+import numpy as np
+# logging.basicConfig(level=logging.DEBUG)
+
+
+def get_action(observation):
+    """Run your policy to produce an action.
+    """
+    action = [0, 0, 0, 0, 0, 0]
+    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 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)
+
+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("#############################################")
+
+axis_names = ["X", "Y", "Z"]
+for axis_num, axis in enumerate(axis_names):
+    print("Stepping forward along {} axis".format(axis))
+    env.reset()
+    done = False
+    while not done:
+        start = time.time()
+        action = get_next_action(observation['ee_position'], axis_num)
+        # Pass the start time to enforce policy frequency.
+        observation, reward, termination, info = env.step(action, start=start)
+        # 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!")
+            done = True
+        # 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()
+

examples/safenet/safenet.yaml

@@ -0,0 +1,6 @@
+# Config file for safenet
+
+safenet: 
+  use_safenet: False
+  lower: null
+  upper: null