From 81be478b313b55c34d38e0dba6ed3bd7427f6112 Mon Sep 17 00:00:00 2001 From: rohun-kulkarni Date: Mon, 25 Jan 2021 19:02:31 -0800 Subject: [PATCH] demo refactoring --- demos/README.md | 94 +++++ demos/demo_control_cfg.yaml | 92 +---- demos/demo_envs.py | 111 +++--- demos/demo_path.py | 122 ++++--- demos/joint_demo.py | 44 ++- demos/osc_demo.py | 673 +++++++++++++++++++++++++++++++++++ demos/run_osc_line_demo.py | 46 +++ demos/run_osc_rot_demo.py | 46 +++ demos/run_osc_square_demo.py | 43 +++ 9 files changed, 1062 insertions(+), 209 deletions(-) create mode 100644 demos/README.md create mode 100644 demos/osc_demo.py create mode 100644 demos/run_osc_line_demo.py create mode 100644 demos/run_osc_rot_demo.py create mode 100644 demos/run_osc_square_demo.py diff --git a/demos/README.md b/demos/README.md new file mode 100644 index 00000000..bd89912b --- /dev/null +++ b/demos/README.md @@ -0,0 +1,94 @@ +# Controller Demos + +The Demos module provides scripts to demonstrate and validate perls2 controller performance. +Each demo creates a perls2.env and can be used to test perls2 controllers for several +different trajectories and robot interface commands. They can be configured in the command line and +can also be used to run on custom configuration files. This is helpful when debugging controller +behavior vs policy behavior. + +Demos are organized into the two action spaces: OpSpace (Operational Space Control) and Joint Space + + +## Quickstart: +The following demos may be run out-of-the-box to verify perls2 controller performance. + +The default config file for all demos is found in `perls2/demos/demo_control_cfg.yaml`. +You may modify the robot and world types in this config file as needed. + + +OpSpace Demos: + * Line (x/y/z): Move end-effector in a straight line using EEImpedance or EEPosture controller, while + maintaining orientation. + `python run_osc_line_demo.py` + * Square : Move end-effector in a square shape in the XY plane, while maintaining orientation. + `python run_osc_square_demo.py` + * Rotation (x/y/z): Rotate end-effector about initial ee-frame axis a fixed amount while maintaining position. + `python run_osc_rot_demo.py` + + +JointSpace Demos: + * SequentialJoint : Individually rotate each joint using JointImpedance controller. Resets robot + after each joint test. + `python run_jointimp_demo.py` + + + + + +### Command-line specifications +Demonstrations may be customized with several command-line arguments. This is helpful in tuning gains, +or verifying a controller or robot is working properly. + +You can specify the following command-line arguments for all demos. These arguments have different meanings based on the type of demo, so it's best to check documentation for the specific demo type. + +For example, you can't run a JointSpace Demo with an EEImpedance controller. + +You can see the valid command line args for a demo with: +`python run_.py -h` + +The parameters general to all demos are listed here: + + --ctrl_type (str): Controller type for the demo. + --demo_type (str): Type of demo to perform. + --test_fn (str): RobotInterface function environment will use to perform actions. Specific to controller type. + --delta_val (float): Step size between consecutive steps in demo. Best to keep small (~0.001) + --num_steps (int) : Number of steps to perform in demonstration. + +The following optional command line arguments are also available: + + --plot_pos (flag): Plot actualrobot states vs goal joint state at each step using matplotlib + --plot_error (flag): Plot error at each step using matplotlib + --save_fig (flag): save position and error plots as png. + --config_file (str): config file to use for controller gains and parameters. + +## Joint Space Demos + +Joint space demos provide a demonstration of controller behavior for the following controllers: +- Joint Impedance +- Joint Velocity +- Joint Torque + +### Run a joint space demo. + +To run a joint space demo, use the run_joint_space_demo.py script. This demo rotates an individiually +specified joint using the controller and robot interface command specified. + +### Command-line specifications +You can specify the following command-line arguments: + + --ctrl_type (str): Controller type for the demo. Choose from: "JointImpedance", "JointVelocity" or "JointTorque" + --demo_type (str): Type of demo to perform. Currently only SequentialJoint available. + --test_fn (str): RobotInterface function environment will use to perform actions. Specific to controller type. + For JointImpedance controllers you may choose from "set_joint_positions" or "set_joint_delta" + For JointVelocity controllers you may choose from "set_joint_velocities" + For JointTorque controllers you may choose "set_joint_torques" + --joint_num (int): Joint number (0-6) to perform demo with. 0 is base joint, 6 is end-effector to gripper joint. + --delta_val (float): Step size [rad] between consecutive steps in demo. Best to keep small (~0.001) + +The following optional command line arguments are also available: + + --plot_pos (flag): Plot joint states with goal joint state at each step using matplotlib + --plot_error (flag): Plot error at each step using matplotlib + --save_fig (flag): save position and error plots as png. + --config_file (str): + diff --git a/demos/demo_control_cfg.yaml b/demos/demo_control_cfg.yaml index 662c29b4..70a01d63 100644 --- a/demos/demo_control_cfg.yaml +++ b/demos/demo_control_cfg.yaml @@ -3,6 +3,7 @@ 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. @@ -19,64 +20,14 @@ redis: # host: localhost # port: 6379 -#policy_freq : 10 # Hz -policy_freq: 20 # For gazebo? +policy_freq: 20 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' @@ -107,9 +58,9 @@ controller: output_max: 5.0 output_min: -5.0 EEPosture: - kp: 80 #70 #50 #200 #75 #200 # P Gain for Impedance Control + 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] - posture_gain: 60 + posture_gain: [5.0, 5.0, 5.0, 4.0, 3.0, 3.0, 3.0] posture: [0,-1.18,0.00,2.18,0.00,0.57,3.3161] input_max: 1.0 input_min: -1.0 @@ -163,35 +114,9 @@ controller: # 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 +# Not used. env: observation_space: low: @@ -202,10 +127,3 @@ env: low: [-0.2, -0.2, -0.2] high: [0.2, 0.2, 0.2] -learning_parms: - hyperparameters: - learning_rate: - - -vision_params: - segmentation: diff --git a/demos/demo_envs.py b/demos/demo_envs.py index 228ccaa1..52ee0247 100644 --- a/demos/demo_envs.py +++ b/demos/demo_envs.py @@ -1,32 +1,35 @@ """Environment for Controller Demonstrations -These environments step actions as specified by a test function, +These environments step actions as specified by a test function, with the appropriate keyword arguments for said test function. """ from perls2.envs.env import Env import time -VALID_JOINTSPACE_FNS = ["set_joint_delta", - "set_joint_positions", - "set_joint_velocities", - "set_joint_torques"] -VALID_JOINTSPACE_CTLRS = ["JointImpedance", - "JointVelocity", - "JointTorque"] +VALID_JOINTSPACE_CTRL_FNS = { + "JointImpedance" : ["set_joint_delta", "set_joint_positions"], + "JointVelocity" : ["set_joint_velocities"], + "JointTorque" : ["set_joint_torques"] + } + +VALID_OSC_CTRL_FNS = { + "EEImpedance": ["move_ee_delta", "set_ee_pose"], + "EEPosture" : ["move_ee_delta", "set_ee_pose"] + } -VALID_OSC_FNS = ["move_ee_delta", "set_ee_pose"] -VALID_OSC_CTLRS = ["EEImpedance", "EEPosture"] class DemoEnv(Env): def __init__(self, config, use_visualizer, name, - test_fn): + test_fn, + ctrl_type): super().__init__(config, use_visualizer, name) self.test_fn = test_fn + self.ctrl_type = ctrl_type def reset(self): """ Reset world and robot and return observation. @@ -36,7 +39,7 @@ def reset(self): self.world.reset() self.robot_interface.reset() - observation = self.get_observation() + observation = self.get_observation() def get_observation(self): """Return observation as dict @@ -55,6 +58,18 @@ def get_observation(self): def rewardFunction(self): return None + def _exec_action(self, action_kw): + """Applies the given action to the simulation. + + Args: action_kw (dict): dictionary of commands specific to test_fn to + execute action. + + actions are usually specified as a list, but in for demo purposes it's + easier to add the relevant kwargs directly for the controller. + """ + self.robot_command_fns[self.test_fn](**action_kw) + self.action_set = True + class JointDemoEnv(DemoEnv): """Class for Joint Space Controller Demonstrations @@ -62,7 +77,7 @@ class JointDemoEnv(DemoEnv): def __init__(self, config, use_visualizer=False, - name=None, + name=None, test_fn="set_joint_delta", ctrl_type="JointImpedance"): """Initialize. @@ -76,53 +91,45 @@ def __init__(self, test_fn (str): name of robot interface function to use for demo. See documentation for more details about config files. - """ - super().__init__(config, use_visualizer, name, test_fn) - - if not test_fn in VALID_JOINTSPACE_FNS: - raise ValueError("Invalid test function for Joint space demo") + """ + super().__init__(config, use_visualizer, name, test_fn, ctrl_type) - if not ctrl_type in VALID_JOINTSPACE_CTLRS: + if not self.ctrl_type in VALID_JOINTSPACE_CTRL_FNS.keys(): raise ValueError("Invalid controller for Joint Space Demo.") - self.ctrl_type = ctrl_type - self.robot_interface.change_controller(self.ctrl_type) - - def _exec_action(self, action_kw): - """Applies the given action to the simulation. + if not self.test_fn in VALID_JOINTSPACE_CTRL_FNS[ctrl_type]: + raise ValueError("Controller mismatch with test_fn") - Args: action_kw (dict): dictionary of commands specific to test_fn to - execute action. - actions are usually specified as a list, but in for demo purposes it's - easier to add the relevant kwargs directly for the controller. - """ - if self.robot_interface.controlType == 'JointVelocity': - self.robot_interface.set_joint_velocities(**action_kw) - elif self.robot_interface.controlType == 'JointImpedance': - if self.test_fn == 'set_joint_delta': - self.robot_interface.set_joint_delta(**action_kw) - elif self.test_fn == 'set_joint_positions': - self.robot_interface.set_joint_positions(**action_kw) - else: - raise ValueError("invalid test function.") - elif self.robot_interface.controlType == 'JointTorque': - if self.test_fn == 'set_joint_torques': - self.robot_interface.set_joint_torques(**action_kw) - else: - raise ValueError("invalid test function.") - else: - raise ValueError("Invalid controller.") - self.robot_interface.action_set = True - - -class OSCDemoEnv(DemoEnv): + self.robot_interface.change_controller(self.ctrl_type) + self.robot_command_fns = { + 'set_joint_velocities' : self.robot_interface.set_joint_velocities, + 'set_joint_delta' : self.robot_interface.set_joint_delta, + 'set_joint_torques' : self.robot_interface.set_joint_torques, + 'set_joint_positions' : self.robot_interface.set_joint_positions + } + + +class OpSpaceDemoEnv(DemoEnv): """Environment for Operational Space Control Demos. """ - def __init__(self, + def __init__(self, + config, use_visualizer=False, - name=None, + name=None, test_fn="set_ee_pose", ctrl_type="EEImpedance"): - super().__init__(config, use_visualizer, name) + super().__init__(config, use_visualizer, name, test_fn, ctrl_type) + + if not self.ctrl_type in VALID_OSC_CTRL_FNS.keys(): + raise ValueError("Invalid controller for Operational Space Demo.") + + if not self.test_fn in VALID_OSC_CTRL_FNS[ctrl_type]: + raise ValueError("Controller mismatch with test_fn") + + self.robot_interface.change_controller(self.ctrl_type) + self.robot_command_fns = { + 'set_ee_pose': self.robot_interface.set_ee_pose, + 'move_ee_delta': self.robot_interface.move_ee_delta + } diff --git a/demos/demo_path.py b/demos/demo_path.py index 8ea2c793..891a8798 100644 --- a/demos/demo_path.py +++ b/demos/demo_path.py @@ -2,11 +2,39 @@ """ import numpy as np import logging +import perls2.controllers.utils.transform_utils as T logger = logging.getLogger(__name__) +def apply_delta(pose, delta): + """ Applies delta to pose to obtain new 7f pose. + Args: pose (7f): x, y, z, qx, qy, qz, w . Position and quaternion + delta (6f): dx, dy, dz, ax, ay, az. delta position and axis-angle + + """ + if len(delta) != 6: + raise ValueError("delta should be [x, y, z, ax, ay, az]. Orientation should be axis-angle") + if len(pose) != 7: + raise ValueError("pose should be [x, y, z, qx, qy, qz, w] Orientation should be quaternion.") + pos = pose[:3] + dpos = delta[:3] + + # Get current orientation and delta as matrices to apply 3d rotation. + ori_mat = T.quat2mat(pose[3:]) + delta_quat = T.axisangle2quat(delta[3:]) + delta_mat = T.quat2mat(delta_quat) + new_ori = np.dot(delta_mat.T, ori_mat) + + # convert new orientation to quaternion. + new_ori_quat = T.mat2quat(new_ori) + + # add dpos to current position. + new_pos = np.add(pos, dpos) + + return np.hstack((new_pos, new_ori_quat)) + class Path(object): """A path is a sequence of goal states for the robot to achieve that - follow a specified pattern. + follow a specified pattern. The path may be either in joint space or in cartesian space as end-effector poses. @@ -28,20 +56,20 @@ def make_path(self): """ self.path = [self.start_pose] - for delta in self._deltas: - new_pose = np.add(self.path[-1], delta) + for delta in self.deltas: + new_pose = apply_delta(self.path[-1], delta) self.path.append(new_pose) class SequentialJoint(Path): - """ Series of joint positions in which a single joint is gradually incremented, - held at desired position, and then gradually decremented back to intial state. + """ Series of joint positions in which a single joint is gradually incremented, + held at desired position, and then gradually decremented back to intial state. i.e.(Move a single joint back and forth) - Attributes: + Attributes: start_pose (list): initial joint states - delta_val (double): amount to increment/decrement + delta_val (double): amount to increment/decrement """ def __init__(self, start_pose, delta_val=0.001, num_steps=30, joint_num=0): @@ -51,7 +79,7 @@ def __init__(self, start_pose, delta_val=0.001, num_steps=30, joint_num=0): self.delta_val = delta_val self.num_steps = num_steps self.joint_num = joint_num - self._deltas = self._get_deltas() + self.deltas = self._get_deltas() self.path = [] self.make_path() @@ -92,16 +120,16 @@ def make_path(self): self.path = [self.start_pose] - for delta in self._deltas: - new_pose = np.add(self.path[-1], delta) + for delta in self.deltas: + new_pose = apply_delta(self.path[-1], delta) self.path.append(new_pose) class Line(Path): """Class definition for straight line in given direction. """ - def __init__(self, start_pose, num_pts, path_length, - delta_val=None, dim=0, end_pos=None): + def __init__(self, start_pose, num_pts, + delta_val, dim=0): """ Initialize Line class Args: @@ -116,22 +144,21 @@ def __init__(self, start_pose, num_pts, path_length, """ self.start_pose = start_pose self.num_pts = num_pts - self.path_length = path_length - if delta_val is None: - delta_val = np.divide(self.path_length, self.num_pts) self.delta_val = delta_val self.dim = dim - self._deltas = [] - self.get_deltas() + self.deltas = self.get_line_deltas() self.path = [] self.make_path() - def get_deltas(self): + def get_line_deltas(self): + """Return list of 6f poses where dim_num=delta_val, all other dims are 0. + Poses are specified as [x, y, z, qx, qy, qz, w] with orientation as quaternion + """ delta = np.zeros(6) delta[self.dim] = self.delta_val - self._deltas = [delta]*self.num_pts - # self._deltas[0] = np.zeros(7) + return [delta]* self.num_pts + class Square(Path): """Class def for square path. @@ -158,60 +185,63 @@ def __init__(self, start_pose, side_num_pts, delta_val): self.start_pose = start_pose self.num_pts = side_num_pts self.delta_val = delta_val - self._deltas = [] - self.get_deltas() + self.deltas = [] + self.make_square_deltas() self.path = [] self.make_path() - def get_deltas(self): + def make_square_deltas(self): """ Get a series of steps from current position that produce a square shape. Travel starts with bottom side in positive direction, then proceeds clockwise (left, top, right.) """ - self._deltas = [[0, 0, 0, 0, 0, 0]] + self.deltas = [np.zeros(6)] + delta_x = np.array([self.delta_val, 0.0, 0.0, 0.0, 0.0, 0.0]) + delta_y = np.array([0.0, self.delta_val, 0.0, 0.0, 0.0, 0.0]) # Bottom side. for pt in range(self.num_pts): - self._deltas.append([self.delta_val, 0.0, 0.0, 0.0, 0.0, 0.0]) + self.deltas.append(delta_x) # Left Side for pt in range(self.num_pts): - self._deltas.append([0.0, -self.delta_val, 0.0, 0.0, 0.0, 0.0]) + self.deltas.append(-delta_y) # Top side for pt in range(self.num_pts): - self._deltas.append([-self.delta_val, 0.0, 0.0, 0.0, 0.0, 0.0]) + self.deltas.append(-delta_x) # Right side for pt in range(self.num_pts): - self._deltas.append([0.0, self.delta_val, 0.0, 0.0, 0.0, 0.0]) + self.deltas.append(delta_y) class Rotation(Path): """ Class definition for path that rotating end effector in place. Start and end orientation should be in euler angles. """ - def __init__(self, start_pose, num_pts, - rotation_rad=np.pi/4, delta_val=None, dim=2, end_ori=None): + def __init__(self, start_pose, num_pts=100, + delta_val=np.pi/400, dim=2): logger.debug("Making Rotation Path") self.start_pose = start_pose - self.end_ori = end_ori self.num_pts = num_pts - self.rotation_rad = rotation_rad - if delta_val is None: - if num_pts == 0: - delta_val = 0 - else: - delta_val = np.divide(rotation_rad, num_pts) self.dim = dim self.delta_val = delta_val - self.get_deltas() + self.deltas = self.get_rotation_deltas() self.path = [] self.make_path() - def get_deltas(self): - """Convert euler angle rotation with magnitude delta in the direction - specified by dim. + def get_rotation_deltas(self): + """Get set of deltas applying axis-angle rotation about specified axis. """ - delta = np.zeros(3) - delta[self.dim] = self.delta_val - # pad with position deltas= 0 - delta = np.hstack(([0, 0, 0], delta)) - self._deltas = [delta]*self.num_pts \ No newline at end of file + delta = np.zeros(6) + delta[self.dim + 3] = self.delta_val + return [delta]*self.num_pts + + def make_path(self): + """Create path by sequentially adding deltas to initial state + """ + self.path = [self.start_pose] + + for delta in self.deltas: + new_pose = apply_delta(self.path[-1], delta) + self.path.append(new_pose) + + diff --git a/demos/joint_demo.py b/demos/joint_demo.py index e063dffe..c9de131a 100644 --- a/demos/joint_demo.py +++ b/demos/joint_demo.py @@ -7,7 +7,7 @@ import numpy as np import logging logger = logging.getLogger(__name__) -import time +import time MAX_JOINT_DELTA = 0.005 @@ -30,26 +30,28 @@ class JointSpaceDemo(Demo): joint position in the demo. kwargs (dict): catch all other keyword arguments. """ - def __init__(self, - ctrl_type, - demo_type, + def __init__(self, + ctrl_type, + demo_type, config_file="demo_control_cfg.yaml", - delta_val=0.005, num_steps=30, + delta_val=0.005, num_steps=30, test_fn='set_joint_delta', **kwargs): """ Initial Joint Space demo and calculate joint trajectory to take. - + Args: - config_file (str): filepath of the config file for the environment. + config_file (str): filepath of the config file for the environment. """ super().__init__(ctrl_type=ctrl_type, demo_type=demo_type, test_fn=test_fn, **kwargs) - + self.env = JointDemoEnv(config=config_file, use_visualizer=True, - name=None) + name=None, + test_fn=self.test_fn, + ctrl_type=self.ctrl_type) self.joint_num = kwargs['joint_num'] @@ -58,26 +60,26 @@ def __init__(self, self.init_joint_state = [0]*7; else: self.init_joint_state = self.get_state() - + if (delta_val > MAX_JOINT_DELTA): logger.warn("Specified joint delta_val exceeds limit, clipping to {} rad".format(MAX_JOINT_DELTA)) delta_val = MAX_JOINT_DELTA - self.delta_val = delta_val + self.delta_val = delta_val self.num_steps = num_steps self.path = SequentialJoint(start_pose=self.init_joint_state, delta_val=self.delta_val, - joint_num=self.joint_num, + joint_num=self.joint_num, num_steps=self.num_steps) - + self.goal_poses = self.path.path self.step_num = 0 self.connect_and_reset_robot() - + def get_state(self): if self.ctrl_type == "JointTorque": return self.env.robot_interface.tau[:7] @@ -110,7 +112,6 @@ def run(self): self.states.append(new_state) self.errors.append( self.compute_error(goal_pose, new_state)) - #logger.debug("Errors:\t{}".format(self.errors[-1])) logger.debug("Final (actual) joint pose \n{}".format(self.env.robot_interface.q)) @@ -138,7 +139,7 @@ def get_action_kwargs(self, action): action_kwargs['set_qpos'] = None if self.test_fn == "set_joint_positions": action_kwargs['delta'] = None - action_kwargs['pose'] = action + action_kwargs['set_qpos'] = action if self.test_fn == "set_joint_torques": action_kwargs['torques'] = action if self.test_fn == "set_joint_velocities": @@ -158,14 +159,9 @@ def get_action(self, goal_state, current_state): """ if self.test_fn == "set_joint_delta": action = np.subtract(goal_state, current_state) - elif self.test_fn == "set_joint_positions": - action = goal_state - elif self.test_fn == "set_joint_torques": - action = goal_state - elif self.test_fn == "set_joint_velocities": - action = goal_state else: - raise ValueError("Invalid test function") + action = goal_state + return action @@ -253,7 +249,7 @@ def plot_errors(self): parser = argparse.ArgumentParser( description="Test controllers and measure errors.") parser.add_argument('--ctrl_type', - default=None, + default="JointImpedance", help='Type of controller to test') parser.add_argument('--demo_type', default=None, diff --git a/demos/osc_demo.py b/demos/osc_demo.py new file mode 100644 index 00000000..760a28c7 --- /dev/null +++ b/demos/osc_demo.py @@ -0,0 +1,673 @@ +"""Class definition for OpSpace Controller demonstration +""" + +from demo_envs import OpSpaceDemoEnv +from demo import Demo +from demo_path import Square, Line, Rotation +import numpy as np +import time +import logging +logger = logging.getLogger(__name__) + +import matplotlib.pyplot as plt +import logging +mpl_logger = logging.getLogger('matplotlib') +mpl_logger.setLevel(logging.WARNING) + +import perls2.controllers.utils.control_utils as C +import perls2.controllers.utils.transform_utils as T + +AXIS_DIM_NUM = {'x': 0, 'y': 1, 'z': 2} + +DEFAULT_ROTATION_DELTA_VAL = np.pi / 400.0 + + +def compute_error(goal_state, new_state): + """ Compute the error between current state and goal state. + For OpSpace Demos, this is position and orientation error. + """ + goal_pos = goal_state[:3] + new_pos = new_state[:3] + + # Check or convert to correct orientation + goal_ori = T.convert_euler_quat_2mat(goal_state[3:]) + new_ori = T.convert_euler_quat_2mat(new_state[3:]) + + pos_error = np.subtract(goal_pos, new_pos) + ori_error = C.orientation_error(goal_ori, new_ori) + + return np.hstack((pos_error, ori_error)) + + +def get_delta(goal_pose, current_pose): + """Get delta between goal pose and current_pose. + + Args: goal_pose (list): 7f pose [x, y, z, qx, qy, qz, w] . Position and quaternion of goal pose. + current_pose (list): 7f Position and quaternion of current pose. + + Returns: delta (list) 6f [dx, dy, dz, ax, ay, az] delta position and delta orientation + as axis-angle. + """ + + if len(goal_pose) != 7: + raise ValueError("Goal pose incorrect dimension should be 7f") + if len(current_pose) !=7: + raise ValueError("Current pose incorrect dimension, should be 7f") + + dpos = np.subtract(goal_pose[:3], current_pose[:3]) + goal_mat = T.quat2mat(goal_pose[3:]) + current_mat = T.quat2mat(current_pose[3:]) + delta_mat_T = np.dot(goal_mat, current_mat.T) + delta_quat = T.mat2quat(np.transpose(delta_mat_T)) + delta_aa = T.quat2axisangle(delta_quat) + + return np.hstack((dpos, delta_aa)).tolist() + +class OpSpaceDemo(Demo): + """Operational space demo for testing EEImpedance and EEPosture control. + + Operational space demo uses a cartesian action space defined from the end-effector + state. The demo generates an environment based on the config file provided. + It then steps the enviroment with a series of actions based on command-line arguments. + + OpSpace Demos can be used to demonstrate the controllers ability to follow several types + of paths. These include: + -Line (in x, y or z direction): straight line + -Square : square shape defined by delta_val and num_steps in fixed XY plane. + -Rotation: Holding end-effector position constant while rotating a fixed amount in the + x, y or z axis of the initial end-effector frame. + + Attributes: + env (OpSpaceDemoEnv) : environment to step with action generated by demo. + ctrl_type (str): string identifying type of controller: + EEImpedance or EEPosture. + demo_type (str): type of demo to perform: + Line, Square, Rotation + test_fn (str): Robot Interface function to perform action specified by demo. + delta_val (float): delta between consecutive goal states in the demo. [rad] for Rotation + demos only. For all others [m] + axis (str): Specifies axis path follows or rotates about for Line or Rotation Demos respectively + + + """ + def __init__(self, + ctrl_type, + demo_type, + test_fn, + config_file="demo_control_cfg.yaml", + **kwargs): + """Initialize OpSpaceDemo. + + Args: + ctrl_type (str): string identifying type of controller: + EEImpedance or EEPosture. + demo_type (str): type of demo to perform: + Line, Square, Rotation + test_fn (str): Robot Interface function to perform action specified by demo. + config_file (str): filepath for config file to test with demo + + """ + super().__init__(ctrl_type=ctrl_type, + demo_type=demo_type, + test_fn=test_fn, + config_file=config_file, + **kwargs) + + self.env = OpSpaceDemoEnv(config=config_file, + use_visualizer=True, + name=None, + test_fn=self.test_fn, + ctrl_type=self.ctrl_type) + + # Connect and reset robot so we can get the correct initial state. + self.connect_and_reset_robot() + + def make_demo(**kwargs): + if (kwargs['demo_type'] == "Line"): + return OpSpaceLineDemo(**kwargs) + elif (kwargs['demo_type'] == "Square"): + return OpSpaceSquareDemo(**kwargs) + elif (kwargs['demo_type'] == "Rotation"): + return OpSpaceRotationDemo(**kwargs) + + + def get_action_kwargs(self, action): + """Return dict of kwargs specific to function being tested. + + Args: + action (list): action to be converted in kwargs for robot_interface functions. + Returns + action_kwargs (dict): dictionary with key-value pairs corresponding to + arguments for robot_interface command being tested. + """ + action_kwargs = {} + if self.test_fn =="move_ee_delta": + action_kwargs['delta'] = action + action_kwargs['set_pos'] = None + action_kwargs['set_ori'] = None + if self.fix_ori: + action_kwargs['set_ori'] = self.initial_pose[3:] + if self.fix_pos: + action_kwargs['set_pos'] = self.initial_pose[:3] + if self.test_fn=="set_ee_pose": + action_kwargs['delta'] = None + action_kwargs['set_pos'] = action[:3] + action_kwargs['set_ori'] = action[3:] + return action_kwargs + + def get_state(self): + """Get robot state for the OpSpace Demo. + """ + return self.env.robot_interface.ee_pose + + def get_action(self, goal_pose, current_pose): + """ Return action corresponding to goal and test_fn + + Args: + goal_pose (list): 7f position and quaternion of goal pose + current_pose (list): 7f position and quaternion of current + pose. + + Returns: + action (list): action corresponding to robot_interface + function being tested as a list. + + Notes: + - If test function is "move_ee_delta" returns + (list 6f) [dx, dy, dz, ax, ay, az] with + delta position and delta orientation as axis-angle. + - If test function is "set_ee_pose" returns (list 6f) + [x,y,z,qx,qy,qz, w] absolute position and orientation + as quaternion. + """ + if self.test_fn =="move_ee_delta": + action = get_delta(goal_pose, current_pose) + elif self.test_fn =="set_ee_pose": + action = goal_pose + else: + raise ValueError("invalid test fn") + return action + + def print_demo_banner(self): + print("\n**************************************************************************************") + print("\n*********************** perls2 Controller Demo **********************************") + + def print_demo_info(self): + print("\n\t Running {} demo \n\t with control type {}. \ + \n\t Test function {}".format( + self.ctrl_type, self.demo_type, self.test_fn)) + + + def run(self): + """Run the demo. Execute actions in sequence and calculate error. + """ + self.print_demo_banner() + self.print_demo_info() + logger.debug("EE Pose initial:\n{}\n".format(self.env.robot_interface.ee_pose)) + print("--------") + input("Press Enter to start sending commands.") + self.step_through_demo() + self.env.robot_interface.reset() + self.env.robot_interface.disconnect() + + if self.plot_error: + self.plot_errors() + if self.plot_pos: + self.plot_positions() + + if self.save: + self.save_data() + + def step_through_demo(self): + for i, goal_pose in enumerate(self.goal_poses): + + # Get action corresponding to test_fn and goal pose + action= self.get_action(goal_pose, self.get_state()) + action_kwargs = self.get_action_kwargs(action) + + # Step environment forward + obs, reward, done, info = self.env.step(action_kwargs, time.time()) + self.observations.append(obs) + self.actions.append(action) + new_state = self.get_state() + self.states.append(new_state) + self.errors.append( + get_delta(goal_pose, new_state)) + + def get_action_list(self): + if (self.test_fn == "move_ee_delta"): + return self.path.deltas + elif (self.test_fn == "set_ee_pose"): + return self.path.path + + def plot_sequence_arrows(self, axes, x, y, color): + """Plot a sequence of arrows given a list of x y points. + """ + for pt_idx in range(len(x)-1): + dx = np.subtract(x[pt_idx+1], x[pt_idx]) + dy = np.subtract(y[pt_idx+1], y[pt_idx]) + + axes.arrow(x[pt_idx], y[pt_idx], dx, dy, color=color) + + def plot_positions(self): + """ Plot 3 plots showing xy, xz, and yz position. + Helps for visualizing decoupling. + """ + + goal_x = [goal[0] for goal in self.goal_poses] + goal_y = [goal[1] for goal in self.goal_poses] + goal_z = [goal[2] for goal in self.goal_poses] + + state_x = [state[0] for state in self.states] + state_y = [state[1] for state in self.states] + state_z = [state[2] for state in self.states] + + fig, (ax_xy, ax_xz, ax_yz) = plt.subplots(1, 3) + + ax_xy.plot(goal_x, goal_y, 'or') + ax_xy.plot(state_x, state_y, '*b') + # self.plot_sequence_arrows(ax_xy, state_x, state_y, 'b') + ax_xy.set_xlabel("x position (m)") + ax_xy.set_ylabel("y position (m)") + ax_xy.set_ylim(bottom=-0.1, top=0.4) + ax_xy.set_xlim(left=0.35, right=0.75) + + ax_xz.plot(goal_x, goal_z, 'or') + ax_xz.plot(state_x, state_z, '*b') + ax_xz.set_xlabel("x position (m)") + ax_xz.set_ylabel("z position (m)") + ax_xz.set_ylim(bottom=-0.5, top=0.5) + ax_xz.set_xlim(left=0.35, right=0.75) + + ax_yz.plot(goal_y, goal_z, 'or') + ax_yz.plot(state_y, state_z, '*b') + ax_yz.set_xlabel("y position (m)") + ax_yz.set_ylabel("z position(m)") + ax_yz.set_ylim(bottom=0, top=2.0) + ax_yz.set_xlim(left=-0.5, right=0.5) + + fname = self.demo_name + '_pos.png' + if self.save_fig: + print("saving figure") + plt.savefig(fname) + plt.show() + + def plot_errors(self): + """ Plot 6 plots showing errors for each dimension. + x, y, z and qx, qy, qz euler angles (from C.orientation_error) + """ + errors_x = [error[0] for error in self.errors] + errors_y = [error[1] for error in self.errors] + errors_z = [error[2] for error in self.errors] + + errors_qx = [error[3] for error in self.errors] + errors_qy = [error[4] for error in self.errors] + errors_qz = [error[5] for error in self.errors] + + fig, ((e_x, e_y, e_z), (e_qx, e_qy, e_qz)) = plt.subplots(2, 3) + + e_x.plot(errors_x) + e_x.set_title("X error per step.") + e_x.set_ylabel("error (m)") + e_x.set_xlabel("step num") + + e_y.plot(errors_y) + e_y.set_ylabel("error (m)") + e_y.set_xlabel("step num") + e_y.set_title("y error per step") + + e_z.plot(errors_z) + e_z.set_title("z error per step.") + e_z.set_ylabel("error (m)") + e_z.set_xlabel("step num") + + e_qx.plot(errors_qx) + e_qx.set_title("qx error per step.") + e_qx.set_ylabel("error (rad)") + e_qx.set_xlabel("step num") + + e_qy.plot(errors_qy) + e_qy.set_title("qy error per step.") + e_qy.set_ylabel("error (rad)") + e_qy.set_xlabel("step num") + + e_qz.plot(errors_qz) + e_qz.set_title("qz error per step.") + e_qz.set_ylabel("error (rad)") + e_qz.set_xlabel("step num") + + fname = self.demo_name + '_error.png' + if self.save_fig: + plt.savefig(fname) + plt.show() + plt.show() + + def reset_log(self): + """Reset states, actions and observation logs between demos. + """ + self.states = [] + self.errors = [] + self.actions = [] + self.observations = [] + + +class OpSpaceLineDemo(OpSpaceDemo): + """Opspace demo for straight line path. + + End-effector is made to travel in a straight line in either x, y or z direction. + + Attributes: + env (OpSpaceDemoEnv) : environment to step with action generated by demo. + ctrl_type (str): string identifying type of controller: + EEImpedance or EEPosture. + demo_type (str): type of demo to perform: + Line, Square, Rotation + test_fn (str): Robot Interface function to perform action specified by demo. + delta_val (float): delta [m] between end-effector positions in consecutive goal states. + axis (str): Specifies axis along which end-effector moves in demo. + """ + def __init__(self, + ctrl_type="EEImpedance", + test_fn="move_ee_delta", + config_file="demo_control_cfg.yaml", + axis='x', + path_length=None, + delta_val=0.001, + num_steps=50, **kwargs): + + super().__init__(ctrl_type=ctrl_type, + test_fn=test_fn, + demo_type="Line", + config_file=config_file, + **kwargs) + + # if path_length is defined, use it. + self.path_length = path_length + self.axis = axis; + self.num_steps = num_steps + self.delta_val = delta_val + if path_length is not None: + self.delta_val = self.path_length / self.num_steps + + self.initial_pose = self.env.robot_interface.ee_pose + self.path = Line(start_pose=self.initial_pose, + num_pts=self.num_steps, + delta_val=self.delta_val, + dim=AXIS_DIM_NUM[self.axis]) + self.fix_ori = True + self.fix_pos = False + + self.goal_poses = self.path.path + self.action_list = self.get_action_list() + + +class OpSpaceLineXYZDemo(OpSpaceLineDemo): + """Combine OpSpaceLine Demos for x,y and z axes. + """ + def __init__(self, + ctrl_type="EEImpedance", + test_fn="move_ee_delta", + config_file="demo_control_cfg.yaml", + axis='x', + path_length=None, + delta_val=0.001, + num_steps=50, **kwargs): + super().__init__(ctrl_type=ctrl_type, + test_fn=test_fn, + config_file=config_file, + axis=axis, + path_length=path_length, + delta_val=delta_val, + num_steps=num_steps, + **kwargs) + + def print_demo_info(self): + print("\n\tRunning Line XYZ demo \nwith control type {}. \ + \nTest function {}".format( + self.ctrl_type, self.demo_type, self.test_fn)) + print("Robot will perform 3 demonstrations: \n moving end-effector in straight line in x, y and then z direction. \nRobot will reset before continuing to next demo.") + + def run(self): + """Run the demo. Execute actions in sequence and calculate error. + """ + self.print_demo_banner() + self.print_demo_info() + + # Run Line demo on x-axis + self.run_demo_axis('x') + self.run_demo_axis('y') + self.run_demo_axis('z') + + print("Demo complete...disconnecting robot.") + self.env.robot_interface.disconnect() + + def run_demo_axis(self, axis): + """Run the demo with path along specific axis. + + Execute actions in sequence and log states, observations, errors at each step. + """ + self.reset_log() + self.path = Line(start_pose=self.initial_pose, + num_pts=self.num_steps, + delta_val=self.delta_val, + dim=AXIS_DIM_NUM[axis]) + + self.goal_poses = self.path.path + self.action_list = self.get_action_list() + + print("------------------------") + print("Running Line {} demo \nwith control type {}. \ + \nTest function {}".format( + axis, self.ctrl_type, self.test_fn)) + logger.debug("EE Pose initial:\n{}\n".format(self.env.robot_interface.ee_pose)) + print("--------") + input("Press Enter to start sending commands.") + + self.step_through_demo() + self.env.robot_interface.reset() + if self.plot_error: + self.plot_errors() + if self.plot_pos: + self.plot_positions() + + if self.save: + self.save_data() + + + def reset_log(self): + """Reset states, actions and observation logs between demos. + """ + self.states = [] + self.errors = [] + self.actions = [] + self.observations = [] + +class OpSpaceSquareDemo(OpSpaceDemo): + """OpSpace Demo for square path in xy plane. + + End-effector holds fixed orientation and travels in a square specified by + delta_val and num_steps. + + """ + def __init__(self, + ctrl_type, + test_fn, + config_file="demo_control_cfg.yaml", + delta_val=0.001, + num_steps=400, + cycles=1, + **kwargs): + + kwargs['demo_type'] = "Square" + super().__init__(ctrl_type=ctrl_type, + test_fn=test_fn, + config_file=config_file, + **kwargs) + + self.delta_val = delta_val + self.num_steps = num_steps + self.cycles = cycles + self.initial_pose = self.env.robot_interface.ee_pose + self.path = Square(start_pose=self.initial_pose, + side_num_pts=int(self.num_steps/4), + delta_val=self.delta_val) + if cycles < 1: + cycles = 1 + + self.goal_poses = self.path.path * cycles + self.fix_ori = True + self.fix_pos = False + + self.action_list = self.get_action_list() + +class OpSpaceRotationDemo(OpSpaceDemo): + """OpSpace demo for rotating end-effector while maintaining position. + """ + def __init__(self, + ctrl_type, + test_fn, + config_file="demo_control_cfg.yaml", + axis='x', + delta_val=0.00785, + num_steps=100, + **kwargs): + + kwargs['demo_type'] = "Rotation" + super().__init__(ctrl_type=ctrl_type, + test_fn=test_fn, + config_file=config_file, + **kwargs) + + self.delta_val = delta_val + self.num_steps = num_steps + self.axis = axis + self.initial_pose = self.env.robot_interface.ee_pose + self.path = Rotation( + start_pose=self.initial_pose, + num_pts=self.num_steps, + delta_val=self.delta_val, + dim=AXIS_DIM_NUM[self.axis]) + + self.goal_poses = self.path.path + self.fix_pos = True + self.fix_ori = False + + self.action_list = self.get_action_list() + + +class OpSpaceRotationXYZDemo(OpSpaceRotationDemo): + def __init__(self, + ctrl_type, + test_fn, + config_file="demo_control_cfg.yaml", + delta_val=DEFAULT_ROTATION_DELTA_VAL, + axis='x', + num_steps=100, + **kwargs): + super().__init__(ctrl_type=ctrl_type, + test_fn=test_fn, + config_file=config_file, + axis=axis, + delta_val=delta_val, + num_steps=num_steps, + **kwargs) + + def print_demo_info(self): + print("\n\tRunning Rotation XYZ demo \nwith control type {}. \ + \nTest function {}".format( + self.ctrl_type, self.demo_type, self.test_fn)) + print("Robot will perform 3 demos: \n rotating in x, y and then z direction. \nRobot will reset before continuing to next demo.") + + def run(self): + """Run the demo. Execute actions in sequence and calculate error. + """ + self.print_demo_banner() + self.print_demo_info() + + # Run Line demo on x-axis + self.run_demo_axis('x') + self.run_demo_axis('y') + self.run_demo_axis('z') + + print("Demo complete...disconnecting robot.") + self.env.robot_interface.disconnect() + + def run_demo_axis(self, axis): + """Run the demo with path along specific axis. + + Execute actions in sequence and log states, observations, errors at each step. + """ + self.reset_log() + self.path = Rotation(start_pose=self.initial_pose, + num_pts=self.num_steps, + delta_val=self.delta_val, + dim=AXIS_DIM_NUM[axis]) + + self.goal_poses = self.path.path + self.action_list = self.get_action_list() + + print("------------------------") + print("Running Rotation {} demo \nwith control type {}. \ + \nTest function {}".format( + axis, self.ctrl_type, self.test_fn)) + logger.debug("EE Pose initial:\n{}\n".format(self.env.robot_interface.ee_pose)) + print("--------") + input("Press Enter to start sending commands.") + + self.step_through_demo() + self.env.robot_interface.reset() + if self.plot_error: + self.plot_errors() + if self.plot_pos: + self.plot_positions() + + if self.save: + self.save_data() + + +if __name__ == '__main__': + import argparse + + parser = argparse.ArgumentParser( + description="Test controllers and measure errors.") + parser.add_argument('--ctrl_type', + default="EEImpedance", + help='Type of controller to test') + parser.add_argument('--demo_type', + default="Line", + help='Type of menu to run.') + parser.add_argument('--test_fn', + default='set_ee_pose', + help='Function to test', + choices=['set_ee_pose', 'move_ee_delta', 'set_joint_delta', 'set_joint_positions', 'set_joint_torques', 'set_joint_velocities']) + parser.add_argument('--path_length', type=float, + default=None, help='length in m of path') + parser.add_argument('--delta_val', + default=0.001, type=float, + help="Max step size (m or rad) to take for demo.") + parser.add_argument('--axis', + default='x', type=str, + choices=['x', 'y', 'z'], + help='axis for demo. Position direction for Line or rotation axis for Rotation') + parser.add_argument('--num_steps', default=100, type=int, + help="max steps for demo.") + parser.add_argument('--plot_pos', action="store_true", + help="whether to plot positions of demo.") + parser.add_argument('--plot_error', action="store_true", + help="whether to plot errors.") + parser.add_argument('--save', action="store_true", + help="whether to store data to file") + parser.add_argument('--demo_name', default=None, + type=str, help="Valid filename for demo.") + parser.add_argument('--save_fig', action="store_true", + help="whether to save pngs of plots") + parser.add_argument('--fix_ori', action="store_true", + help="fix orientation for move_ee_delta") + parser.add_argument('--fix_pos', action="store_true", + help="fix position for move_ee_delta") + parser.add_argument('--config_file', default='demos/demo_control_cfg.yaml', help='absolute filepath for config file.') + parser.add_argument('--cycles', type=int, default=1, help="num times to cycle path (only for square)") + args = parser.parse_args() + kwargs = vars(args) + demo = OpSpaceDemo.make_demo(**kwargs) + demo.run() \ No newline at end of file diff --git a/demos/run_osc_line_demo.py b/demos/run_osc_line_demo.py new file mode 100644 index 00000000..f66232d1 --- /dev/null +++ b/demos/run_osc_line_demo.py @@ -0,0 +1,46 @@ +"""Script to run Consecutive Line Demos in X, Y, Z direction with OpSpace controller. +""" + +from osc_demo import OpSpaceLineXYZDemo + +import argparse + +parser = argparse.ArgumentParser( + description="Run OpSpace Line Demo in XYZ directions") +parser.add_argument('--ctrl_type', + default="EEImpedance", + help='Type of controller to test', + choices=["EEImpedance", "EEPosture"]) +parser.add_argument('--test_fn', + default='set_ee_pose', + help='Function to test', + choices=['set_ee_pose', 'move_ee_delta']) + +parser.add_argument('--path_length', type=float, + default=None, help='length in m of path') +parser.add_argument('--delta_val', + default=0.001, type=float, + help="Max step size [m] to take for demo.") +parser.add_argument('--axis', + default='x', type=str, + choices=['x', 'y', 'z'], + help='Axis about which end-effector rotates (from initial end-effector frame)') +parser.add_argument('--num_steps', default=100, type=int, + help="max steps for demo.") +parser.add_argument('--plot_pos', action="store_true", + help="whether to plot positions of demo.") +parser.add_argument('--plot_error', action="store_true", + help="whether to plot errors.") +parser.add_argument('--save', action="store_true", + help="whether to store data to file") +parser.add_argument('--demo_name', default=None, + type=str, help="Valid filename for demo.") +parser.add_argument('--save_fig', action="store_true", + help="whether to save pngs of plots") +parser.add_argument('--config_file', default='demos/demo_control_cfg.yaml', help='absolute filepath for config file.') + +args = parser.parse_args() +kwargs = vars(args) + +demo = OpSpaceLineXYZDemo(**kwargs) +demo.run() \ No newline at end of file diff --git a/demos/run_osc_rot_demo.py b/demos/run_osc_rot_demo.py new file mode 100644 index 00000000..1c628c3e --- /dev/null +++ b/demos/run_osc_rot_demo.py @@ -0,0 +1,46 @@ +"""Script to run consecutive rotation demos about x, y, z axis with OpSpace Controller. +""" +from osc_demo import OpSpaceRotationXYZDemo, DEFAULT_ROTATION_DELTA_VAL +import argparse + + + +parser = argparse.ArgumentParser( + description="Run OpSpace Line Demo in XYZ directions") +parser.add_argument('--ctrl_type', + default="EEImpedance", + help='Type of controller to test', + choices=["EEImpedance", "EEPosture"]) +parser.add_argument('--test_fn', + default='set_ee_pose', + help='Function to test', + choices=['set_ee_pose', 'move_ee_delta']) + +parser.add_argument('--path_length', type=float, + default=None, help='length in m of path') +parser.add_argument('--delta_val', + default=DEFAULT_ROTATION_DELTA_VAL, type=float, + help="Max step size rad to take for demo.") +parser.add_argument('--axis', + default='x', type=str, + choices=['x', 'y', 'z'], + help='Axis about which end-effector rotates (from initial end-effector frame)') +parser.add_argument('--num_steps', default=100, type=int, + help="max steps for demo.") +parser.add_argument('--plot_pos', action="store_true", + help="whether to plot positions of demo.") +parser.add_argument('--plot_error', action="store_true", + help="whether to plot errors.") +parser.add_argument('--save', action="store_true", + help="whether to store data to file") +parser.add_argument('--demo_name', default=None, + type=str, help="Valid filename for demo.") +parser.add_argument('--save_fig', action="store_true", + help="whether to save pngs of plots") +parser.add_argument('--config_file', default='demos/demo_control_cfg.yaml', help='absolute filepath for config file.') + +args = parser.parse_args() +kwargs = vars(args) + +demo = OpSpaceRotationXYZDemo(**kwargs) +demo.run() \ No newline at end of file diff --git a/demos/run_osc_square_demo.py b/demos/run_osc_square_demo.py new file mode 100644 index 00000000..02f5b693 --- /dev/null +++ b/demos/run_osc_square_demo.py @@ -0,0 +1,43 @@ +"""Script to run Square with OpSpace controller. +""" + +from osc_demo import OpSpaceSquareDemo +import argparse + +parser = argparse.ArgumentParser( + description="Run OpSpace Square Demo ") +parser.add_argument('--ctrl_type', + default="EEImpedance", + help='Type of controller to test', + choices=["EEImpedance", "EEPosture"]) +parser.add_argument('--test_fn', + default='set_ee_pose', + help='Function to test', + choices=['set_ee_pose', 'move_ee_delta']) +parser.add_argument('--path_length', type=float, + default=None, help='length in m of path') +parser.add_argument('--delta_val', + default=0.001, type=float, + help="Max step size (m) between consecutive steps in demo.") +parser.add_argument('--num_steps', default=400, type=int, + help="number of steps for demo.") +parser.add_argument('--plot_pos', action="store_true", + help="whether to plot positions of demo.") +parser.add_argument('--plot_error', action="store_true", + help="whether to plot errors.") +parser.add_argument('--save', action="store_true", + help="whether to store data to file") +parser.add_argument('--demo_name', default=None, + type=str, help="Valid filename for demo.") +parser.add_argument('--save_fig', action="store_true", + help="whether to save pngs of plots") +parser.add_argument('--config_file', default='demos/demo_control_cfg.yaml', help='filepath for config file relative to current directory') +parser.add_argument('--cycles', type=int, default=1, help="num times to cycle path (only for square)") + +args = parser.parse_args() +kwargs = vars(args) + +kwargs["demo_type"] = "Square" + +demo = OpSpaceSquareDemo(**kwargs) +demo.run() \ No newline at end of file