def Task(arm, controller_class, force=None, write_to_file=False, **kwargs): """ This task sets up the arm to follow the mouse with its end-effector. """ # check controller type ------------------ controller_name = controller_class.__name__.split('.')[1] if controller_name not in ('lqr', 'osc'): raise Exception('Cannot perform reaching task with this controller.') # set arm specific parameters ------------ if arm.DOF == 1: kp = 5 elif arm.DOF == 2: kp = 20 elif arm.DOF == 3: kp = 50 # generate control shell ----------------- additions = [] if force is not None: print 'applying joint velocity based forcefield...' additions.append(forcefield.Addition(scale=force)) task = 'arm%i/forcefield'%arm.DOF controller = controller_class.Control( additions=additions, kp=kp, kv=np.sqrt(kp), task='arm%i/follow_mouse'%arm.DOF, write_to_file=write_to_file) control_shell = shell.Shell(controller=controller) # generate runner parameters ----------- runner_pars = {'control_type':'osc', 'title':'Task: Follow mouse', 'mouse_control':True} return (control_shell, runner_pars)
def Task(arm, controller_class, x_bias=0., y_bias=2., dist=.4, force=None, write_to_file=False, sequence=None, **kwargs): """ This task sets up the arm to reach to 8 targets center out from (x_bias, y_bias) at a distance=dist. """ # check controller type ------------------ controller_name = controller_class.__name__.split('.')[1] if controller_name not in ('ilqr', 'lqr', 'osc', 'gradient_approximation', 'ahf'): raise Exception('Cannot perform reaching task with this controller.') # set arm specific parameters ------------ if arm.DOF == 2: dist = .15 kp = 20 # position error gain on the PD controller threshold = .01 y_bias = .2 elif arm.DOF == 3: kp = 100 threshold = .02 else: raise Exception('Cannot perform reaching task with this arm.') # generate the path to follow ------------- # set up the reaching trajectories, 8 points around unit circle targets_x = [dist * np.cos(theta) + x_bias \ for theta in np.linspace(0, np.pi*2, 9)][:-1] targets_y = [dist * np.sin(theta) + y_bias \ for theta in np.linspace(0, np.pi*2, 9)][:-1] trajectory = np.ones((3 * len(targets_x) + 3, 2)) * np.nan start = 0 if sequence is None else int(sequence) for ii in range(start, len(targets_x)): trajectory[ii * 3 + 1] = [0, y_bias] trajectory[ii * 3 + 2] = [targets_x[ii], targets_y[ii]] trajectory[-2] = [0, y_bias] # generate control shell ----------------- additions = [] if force is not None: print('applying joint velocity based forcefield...') additions.append(forcefield.Addition(scale=force)) task = 'arm%i/forcefield' % arm.DOF controller = controller_class.Control(additions=additions, kp=kp, kv=np.sqrt(kp), task='arm%i/reach' % arm.DOF, write_to_file=write_to_file) control_pars = { 'target_list': trajectory, 'threshold': threshold } # how close to get to each target} control_shell = target_list.Shell(controller=controller, **control_pars) # generate runner parameters ----------- runner_pars = { 'infinite_trail': True, 'title': 'Task: Reaching', 'trajectory': trajectory } return (control_shell, runner_pars)
def Task(arm, controller_class, sequence=None, scale=None, force=None, write_to_file=False, **kwargs): """ This task sets up the arm to write numbers inside a specified area (-x_bias, x_bias, -y_bias, y_bias). """ # check controller type ------------------ controller_name = controller_class.__name__.split('.')[1] if controller_name not in ('dmp', 'trace'): raise Exception('Cannot perform reaching task with this controller.') # set arm specific parameters ------------ if arm.DOF == 2: kp = 20 # position error gain on the PD controller threshold = .01 writebox = np.array([-.1, .1, .2, .25]) elif arm.DOF == 3: kp = 100 # position error gain on the PD controller threshold = .05 writebox = np.array([-.25, .25, 1.65, 2.]) # generate the path to follow ------------- sequence = 'hello' if sequence is None else sequence sequence = [c for c in sequence] if scale is None: scale = [1.0] * len(sequence) else: scale = [float(c) for c in scale] import pdb pdb.set_trace() trajectory = rp.get_sequence(sequence, writebox, spaces=True) # generate control shell ----------------- additions = [] if force is not None: print('applying joint velocity based forcefield...') additions.append(forcefield.Addition(scale=force)) control_pars = { 'additions': additions, 'gain': 1000, # pd gain for trajectory following 'pen_down': False, 'threshold': threshold, 'trajectory': trajectory.T } controller_name = controller_class.__name__.split('.')[1] if controller_name == 'dmp': # number of goals is the number of (NANs - 1) * number of DMPs num_goals = (np.sum(trajectory[:, 0] != trajectory[:, 0]) - 1) * 2 # respecify goals for spatial scaling by changing add_to_goals control_pars['add_to_goals'] = [0] * num_goals control_pars['bfs'] = 1000 # number of basis function per DMP control_pars['tau'] = .1 # how fast the trajectory rolls out elif controller_name == 'trace': control_pars['tau'] = .005 # how fast the trajectory rolls out print('Using operational space controller...') controller = osc.Control(kp=kp, kv=np.sqrt(kp), write_to_file=write_to_file) control_shell = controller_class.Shell(controller=controller, **control_pars) # generate runner parameters ----------- runner_pars = { 'infinite_trail': True, 'title': 'Task: Writing numbers', 'trajectory': trajectory } return (control_shell, runner_pars)