def H(self, X, q0, q1):
if self.ElementaryType is 'linear':
rvec = 1 - X
return Quaternion.get_slerp_traj_(q0, q1, rvec)
if self.ElementaryType is 'minjerk':
rvec = 6 * np.power(1-X, 5) - 15 * np.power(1-X, 4) + 10 * np.power(1-X,3)
return Quaternion.get_slerp_traj_(q0, q1, rvec)
def get_target(self, t):
ts = self.qs[:,0]
ind = np.searchsorted(ts, t)
if ind != 0:
q0 = self.qs[ind - 1, :]
q1 = self.qs[ind, :]
else:
q0 = self.qs[0, :]
q1 = self.qs[1, :]
tq = (t - q0[0]) / (q1[0] - q0[0])
h_q = self.h(tq, q0[1:], q1[1:])
f_q = self.get_position(t)
y_q = Quaternion.qmulti(h_q, f_q)
y_q = Quaternion.normalize(y_q)
return y_q
def control(self, sim_duration):
if self.low_ctrl is None:
print("Error: low level controller cannot be None ")
return True
done = False
if self.low_ctrl.restart_high_ctrl:
self.last_period_end_time = self.low_ctrl.sim.data.time
start = self.low_ctrl.tcp_pose.copy()
goal = self.low_ctrl.tcp_pose.copy()
print(start, goal)
goal[0] = goal[0] + 0.040
self.ts_vmp.set_start_goal(start, goal)
normalized_time = (sim_duration -
self.last_period_end_time) / self.motion_duration
if normalized_time > 1:
if self.periodic:
self.last_period_end_time = self.low_ctrl.sim.data.time
normalized_time = 1
# for wiping
if self.change_wipe_location_counter == 0:
print("change start and goal to: ")
start = self.ts_vmp.goal.copy()
goal = self.ts_vmp.goal.copy()
print(start, goal)
goal[0] = goal[0] + 0.040
self.ts_vmp.set_start_goal(start, goal)
else:
self.change_wipe_location_counter -= 1
else:
done = True
return done
if self.js_vmp is None:
desired_joints = None
else:
desired_joints = np.squeeze(
np.asarray(self.js_vmp.get_target(normalized_time)[:, 0]))
ts_target = self.get_target_func(normalized_time)
target_xyz = np.asarray(ts_target[0]).flatten()
ts_quat = ts_target[1].flatten()
target_quat = Quaternion.normalize(ts_quat)
# print("vmp: ", target_xyz, target_quat, normalizedTime)
if self.velocity_mode:
targets = self.pack(target_xyz / self.motion_duration, target_quat,
desired_joints, normalized_time)
else:
targets = self.pack(target_xyz, target_quat, desired_joints,
normalized_time)
self.low_ctrl.control(targets)
return done
def roll(self, X=None):
if X is None:
X = self.linearDecayCanonicalSystem(1, 0, self.n_samples)
Xi = np.zeros(shape=(len(X),4))
for i in range(len(X)):
x = X[i]
t = 1 - x
Xi[i,:] = self.get_target(t)
t = 1 - np.expand_dims(X,1)
traj = np.concatenate([t, Xi], axis=1)
return Quaternion.normalize_traj(traj)
def train(self, trajectory):
self.n_samples = np.shape(trajectory)[0]
tvec = trajectory[:,0]
X = self.linearDecay(tvec)
self.q0 = trajectory[0,1:]
self.qg = trajectory[-1,1:]
Psi = self.__Psi__(X)
H_q = self.H(X, self.q0, self.qg)
F_q = Quaternion.qtraj_diff(H_q, trajectory[:,1:])
pseudo_inv = np.linalg.inv(np.matmul(np.transpose(Psi), Psi) + self.lamb * np.eye(self.kernel_num))
self.muW = np.matmul(np.matmul(pseudo_inv, np.transpose(Psi)), F_q)
ts = np.array([0, 1])
ts = np.expand_dims(ts, axis=1)
q = np.stack([self.q0, self.qg], axis=0)
self.qs = np.concatenate([ts, q], axis=1)
return F_q
def h(self, t, q0, q1):
if self.ElementaryType is 'linear':
return Quaternion.slerp(t, q0, q1)
if self.ElementaryType is 'minjerk':
ratio = 6 * np.power(t, 5) - 15 * np.power(t,4) + 10 * np.power(t,3)
return Quaternion.slerp(ratio, q0, q1)
queries = []
env.is_data_collection = True
for i in progressbar.progressbar(range(EXP_NUM)):
is_failed = True
while is_failed:
start, _ = env.reset(init_ball_pos=INIT_BALL_POS)
q0 = start[3:]
q1 = start[3:]
ax1 = np.array([0,1,0])
if np.random.uniform(0, 1) < 0.5:
ang1 = np.random.uniform(low=-40 * np.pi/180, high=-30 * np.pi/180)
else:
ang1 = np.random.uniform(low=20 * np.pi/180, high=30 * np.pi/180)
qrot1 = Quaternion.from_axis_angle(ax1, ang1)
ax2 = np.array([1,0,0])
ang2 = np.random.uniform(low=-30 * np.pi/180, high=-20 * np.pi/180)
qrot2 = Quaternion.from_axis_angle(ax2, ang2)
ax3 = np.random.uniform(low=-1, high=1, size=3)
if np.random.uniform(0, 1) < 0.5:
ang3 = np.random.uniform(low=-30 * np.pi / 180, high=-20 * np.pi / 180)
else:
ang3 = np.random.uniform(low=20 * np.pi / 180, high=30 * np.pi / 180)
ax3 = ax3 / np.linalg.norm(ax3)
qrot3 = Quaternion.from_axis_angle(ax3, ang3)
qv1 = Quaternion.qmulti(qrot1, q0)