separate demo directory created

demos/demo.py

@@ -0,0 +1,97 @@
+"""Class definition for demonstration.
+"""
+from demo_control_env import DemoControlEnv
+import numpy as np
+import time
+from datetime import datetime
+
+class Demo(object):
+    """Abstract class definition for demonstration.
+    Demonstrations step an environment according to a series of actions
+    that follow a specified pattern and are of appropriate dimension for the controller.
+
+    Attributes:
+        env (DemoControlEnv): environment to step with action generated
+            by demo.
+        ctrl_type (str): string identifying type of controller:
+            EEPosture, EEImpedance, JointImpedance, JointTorque
+        demo_type (str): type of demo to perform (zero, line, sequential,
+            square.)
+        test_fn (str): RobotInterface function to test.
+        use_abs (bool): Use absolute values for poses or goal states.
+        demo_name (str): name of the demo control environment.
+        plot_pos (bool): flag to plot positions of states during demo.
+        plot_error (bool): flag to plot errors of controllers during demo.
+        save (bool): flag to save states, errors, actions of demo as npz.
+        world_type: Type of world Bullet or Real.
+        initial_pose (list): initial end-effector pose.
+
+    """
+    def __init__(self, 
+                 ctrl_type, 
+                 demo_type, 
+                 test_fn, 
+                 config_file="demo_control_cfg.yaml",
+                 **kwargs):
+        self.ctrl_type = ctrl_type
+
+        self.demo_type = demo_type
+        self.test_fn = test_fn
+
+        self.plot_pos = kwargs['plot_pos']
+        self.plot_error = kwargs['plot_error']
+        self.save_fig = kwargs['save_fig']
+
+        self.save = kwargs['save']
+
+        self.demo_name = kwargs['demo_name']
+        if self.demo_name is None:
+            now = datetime.now()
+            demo_name = now.strftime("%d%m%y%H%M%S")
+            self.demo_name = "{}_{}_{}".format(demo_name, self.ctrl_type, self.demo_type)
+
+        # self.axis = kwargs['axis']
+
+
+        # Initialize lists for storing data.
+        self.errors = []
+        self.actions = []
+        self.states = []
+        self.observations = []
+
+    @property
+    def world_type(self):
+        return self.env.config['world']['type']
+
+    def get_action_list(self):
+        raise NotImplementedError
+
+    def run(self):
+        raise NotImplementedError
+
+    def save_data(self):
+        fpath = "demos/results/{}.npz".format(self.demo_name)
+        np.savez(fpath, states=self.states,
+                 errors=self.errors, actions=self.actions,
+                 goals=self.goal_states, obs=self.observations, allow_pickle=True)
+
+    def get_goal_state(self, delta):
+        raise NotImplementedError
+
+    def connect_and_reset_robot(self):
+        # Connect to redis and reset robot.
+        if not self.env.world.is_sim:
+            logger.info("connecting perls2 Robot Interface to redis")
+            self.env.robot_interface.connect()
+
+        self.env.robot_interface.reset()
+
+    # def make_demo(**kwargs):
+    #     """Factory method for making the write demo per params.
+    #     """
+    #     if kwargs['ctrl_type'] in ["EEImpedance", "EEPosture"]:
+    #         return OpSpaceDemo(**kwargs)
+    #     elif kwargs['ctrl_type'] in ["JointImpedance", "JointTorque", "JointVelocity"]:
+    #         return JointSpaceDemo(**kwargs)
+    #     else:
+    #         raise ValueError("invalid demo / controller pairing.")

demos/demo_control_cfg.yaml

@@ -0,0 +1,211 @@
+# Cfg file for  Demo control environment
+world:
+  type: 'Bullet' #'Bullet'
+  robot: 'panda'
+data_dir: 'data'
+# Simulation parameters
+sim_params:
+  time_step: 0.004166  # in seconds #0.004166 #0.002 #0.004166  # 1./240.
+  MAX_STEPS: 500
+real_params:
+  nuc_hostname: "c3po-ctrl.stanford.edu" # Hostname for the NUC for real robot control.
+
+redis:
+  host: c3po-ctrl.stanford.edu
+  port: 6379
+  password: "/home/robot/redis_pw.txt"
+
+# redis:
+#   host: localhost
+#   port: 6379
+
+#policy_freq : 10 # Hz
+policy_freq: 20 # For gazebo?
+control_freq: 500 # Hz
+!include ../cfg/sawyer.yaml
+!include ../cfg/panda.yaml
+!include ../cfg/table_and_bin.yaml
+scene_objects:
+  'None'
+goal_position:
+  lower:
+    [0.3, -0.2, 0.0]  # CHANGED
+  upper:
+    [0.5, 0.5, 0.2]   # CHANGED
+
+# 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:
+#         image_height: 1080
+#         image_width: 1920
+#         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
+
+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'
+      fraction: 0.2
+    # max_dx: 0.01
+    # ramp_ratio: 0.2
+
+# object:
+#   'None'
+  # object_dict:
+  #   # To add objects via arenas, list them directly using
+  #   # key object_%i,
+  #   object_0:
+  #     name: '013_apple'
+  #     path:
+  #       'objects/ycb/013_apple/google_16k/textured.urdf'
+  #     pose: [[1.5, 0, 0], [0,0,0]]
+  #     is_static: True
+  #     default_position: [0.7, 0.1, 0.03] #z  = 0.1
+  #   object_1:
+  #     name: '006_mustard_bottle'
+  #     path:
+  #       'objects/ycb/006_mustard_bottle/google_16k/textured.urdf'
+  #     pose: [[0, 0, 0], [0, 0, 0]]
+  #     is_static: False
+  #     default_position: [0.2, 0.1, 0.03]
+
+  # random:
+  #   randomize: True
+  #   position:
+  #     lower:
+  #       [0.3, -0.2, 0.1]
+  #     upper:
+  #       [0.7, 0.2, 0.1]
+
+# Perception and Learning
+env:
+  observation_space:
+    low:
+      [-2.0, -2.0, -2.0]
+    high:
+      [2.0, 2.0, 2.0]
+  action_space:
+    low: [-0.2, -0.2, -0.2]
+    high: [0.2, 0.2, 0.2]
+
+learning_parms:
+  hyperparameters:
+  learning_rate:
+
+
+vision_params:
+  segmentation: