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)