def __init__(self, bfs, gain, trajectory, tau,
add_to_goals=None, **kwargs):
"""
bfs int: the number of basis functions per DMP
gain float: the PD gain while following a DMP trajectory
trajectory np.array: the time series of points to follow
[DOFs, time], with a column of None
wherever the pen should be lifted
tau float: the time scaling term
add_to_goals np.array: floats to add to the DMP goals
used to scale the DMPs spatially
"""
Control_OSC.__init__(self, **kwargs)
self.bfs = bfs
self.gain = gain
self.gen_dmps(trajectory)
self.tau = tau
self.target,_,_ = self.dmps.step(tau=self.tau)
if add_to_goals is not None:
for ii, dmp in enumerate(self.dmp_sets):
dmp.goal[0] += add_to_goals[ii*2]
dmp.goal[1] += add_to_goals[ii*2+1]
self.num_seq = 0
self.done = False
self.not_at_start = True
def __init__(self, dt, gain, trajectory, **kwargs):
Control_OSC.__init__(self, **kwargs)
self.dt = dt
self.gain = gain
self.gen_trajectories(trajectory)
self.target = np.array([self.y_des[0](0.0), self.y_des[1](0.0)])
self.num_seq = 0
self.time = 0.0
self.done = False
self.not_at_start = True
def __init__(self, dt, gain, trajectory, **kwargs):
Control_OSC.__init__(self, **kwargs)
self.dt = dt
self.gain = gain
self.gen_trajectories(trajectory)
self.target = np.array([self.y_des[0](0.0), self.y_des[1](0.0)])
self.num_seq = 0
self.time = 0.0
self.done = False
self.not_at_start = True
def control(self, arm):
"""Drive the end-effector through a series
of (x,y) points"""
if np.sum(abs(arm.x - self.target)) < .01:
self.not_at_start = False
if self.not_at_start or self.done:
u = Control_OSC.control(self, arm)
else:
y = np.array([self.y_des[d](self.time) for d in range(2)])
self.time += self.dt
# check to see if it's pen up time
if self.time >= 1:
self.pen_down = False
self.time = 0.0
if self.num_seq >= len(self.seqs_x) - 1:
# if we're finished the last sequence
self.done = True
self.target = [.3, 0]
else:
# else move on to the next sequence
self.not_at_start = True
self.num_seq += 1
self.y_des = [
self.seqs_x[self.num_seq], self.seqs_y[self.num_seq]
]
self.target = [self.y_des[0](0.0), self.y_des[1](0.0)]
else:
self.pen_down = True
self.x = arm.position(ee_only=True)
x_des = self.gain * (y - self.x)
u = Control_OSC.control(self, arm, x_des=x_des)
return u
def control(self, arm):
"""Apply a given control signal in (x,y)
space to the arm"""
if np.sum(abs(arm.x - self.target)) < .01:
self.not_at_start = False
if self.not_at_start or self.done:
u = Control_OSC.control(self, arm)
else:
y,_,_ = self.dmps.step(tau=self.tau, state_fb=self.x)
# check to see if it's pen up time
if self.dmps.cs.x < \
np.exp(-self.dmps.cs.ax * self.dmps.cs.run_time):
self.pen_down = False
if self.num_seq >= len(self.dmp_sets) - 1:
# if we're finished the last DMP
self.done = True
self.target = [.3, 0]
else:
# else move on to the next DMP
self.not_at_start = True
self.num_seq += 1
self.dmps = self.dmp_sets[self.num_seq]
self.target,_,_ = self.dmps.step(tau=self.tau)
else:
self.pen_down = True
self.x = arm.position(ee_only=True)
x_des = self.gain * (y - self.x)
u = Control_OSC.control(self, arm, x_des=x_des)
return u
def control(self, arm):
"""Drive the end-effector through a series
of (x,y) points"""
if np.sum(abs(arm.x - self.target)) < .01:
self.not_at_start = False
if self.not_at_start or self.done:
u = Control_OSC.control(self, arm)
else:
y = np.array([self.y_des[d](self.time) for d in range(2)])
self.time += self.dt
# check to see if it's pen up time
if self.time >= 1:
self.pen_down = False
self.time = 0.0
if self.num_seq >= len(self.seqs_x) - 1:
# if we're finished the last sequence
self.done = True
self.target = [.3, 0]
else:
# else move on to the next sequence
self.not_at_start = True
self.num_seq += 1
self.y_des = [self.seqs_x[self.num_seq],
self.seqs_y[self.num_seq]]
self.target = [self.y_des[0](0.0), self.y_des[1](0.0)]
else:
self.pen_down = True
self.x = arm.position(ee_only=True)
x_des = self.gain * (y - self.x)
u = Control_OSC.control(self, arm, x_des=x_des)
return u