Skip to content

Commit

Permalink
demo refactoring
Browse files Browse the repository at this point in the history
  • Loading branch information
@rohun33
rohun33 committed Jan 26, 2021
1 parent 6f6eb15 commit 81be478
Show file tree
Hide file tree
Showing 9 changed files with 1,062 additions and 209 deletions.
94 changes: 94 additions & 0 deletions demos/README.md
View file
Original file line number Diff line number Diff line change
@@ -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_<demo_name>.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):

92 changes: 5 additions & 87 deletions demos/demo_control_cfg.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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'
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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:
Expand All @@ -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:
111 changes: 59 additions & 52 deletions demos/demo_envs.py
View file
Original file line number Diff line number Diff line change
@@ -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.
Expand All @@ -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
Expand All @@ -55,14 +58,26 @@ 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
"""
def __init__(self,
config,
use_visualizer=False,
name=None,
name=None,
test_fn="set_joint_delta",
ctrl_type="JointImpedance"):
"""Initialize.
Expand All @@ -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
}
Loading

0 comments on commit 81be478

Please sign in to comment.