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