def main():
# Load cleaned trajectories and respective targets
# Y = [y1_x y1_y y1_z, ..., y2_x, y2_y, y2_z, ...]
# O = [o1_x, o1_y, o1_z, ...]
Y, O, _, _ = load_trajs()
init_ros_vars()
neutral()
rospy.loginfo("Creating COPMP")
copmp = CoProMP(O, Y, 3, 7, o_dt=1, dt=0.001, Sigma_y=None)
copmp.build(linear_phase,
lambda z, dt: normalized_gaussian_basis(2, z, dt),
linear_phase,
lambda z, dt: normalized_gaussian_basis(10, z, dt))
rospy.loginfo("COPMP created")
# Use state extractor to obtain new target
# new_o = ...
raw_input()
new_o = np.vstack(_target)
rospy.loginfo("Getting COPMP trajectory")
copmp.condition(new_o, 1)
Ymp = copmp.most_probable()
# Use trajectory executor to execute the trajectory
# You shall turn on jtas before running this
time = Ymp[:, 0][:, np.newaxis]
right_traj = Ymp[:, 1:7 + 1]
rospy.loginfo("Executing COPMP")
te = TrajectoryExecutor(time, 10, None, right_traj)
te.execute()
def legible_traj_srv_handler(self, req):
result = 1
target = [req.x_pos, req.y_pos, req.z_pos]
new_o = np.vstack(target)
rospy.loginfo("Creating COPMP")
copmp = CoProMP(self._O, self._Y, 3, 7, o_dt=1, dt=0.0001, Sigma_y=None)
copmp.build(linear_phase, lambda z, dt: normalized_gaussian_basis(2, z, dt),
linear_phase, lambda z, dt: normalized_gaussian_basis(10, z, dt))
rospy.loginfo("COPMP created")
rospy.loginfo("Conditioning COPMP to target")
copmp.condition(new_o, 1)
ymp = copmp.most_probable()
time = ymp[:, 0][:, np.newaxis]
right_traj = ymp[:, 1:7 + 1]
rospy.loginfo("Executing legible trajectory")
te = TrajectoryExecutor(time, 10, None, right_traj)
te.execute()
rospy.loginfo("Legible trajectory executed")
return MovementResponse(result)
def legible_traj_srv_handler(self, req):
result = 1
target = [req.x_pos, req.y_pos, req.z_pos]
new_o = np.vstack(target)
rospy.loginfo("Creating COPMP")
copmp = CoProMP(self._O, self._Y, 3, 7, o_dt=1, dt=0.0001, Sigma_y=None)
copmp.build(linear_phase, lambda z, dt: normalized_gaussian_basis(2, z, dt),
linear_phase, lambda z, dt: normalized_gaussian_basis(10, z, dt))
rospy.loginfo("COPMP created")
rospy.loginfo("Conditioning COPMP to target")
copmp.condition(new_o, 1)
ymp = copmp.most_probable()
time = ymp[:, 0][:, np.newaxis]
right_traj = ymp[:, 1:7 + 1]
rospy.loginfo("Executing legible trajectory")
te = TrajectoryExecutor(time, 10, None, right_traj)
te.execute()
rospy.loginfo("Legible trajectory executed")
return MovementResponse(result)
def main():
# Load cleaned trajectories and respective targets
# Y = [y1_x y1_y y1_z, ..., y2_x, y2_y, y2_z, ...]
# O = [o1_x, o1_y, o1_z, ...]
Y, O, _, _ = load_trajs()
init_ros_vars()
neutral()
rospy.loginfo("Creating COPMP")
copmp = CoProMP(O, Y, 3, 7, o_dt=1, dt=0.001, Sigma_y=None)
copmp.build(linear_phase, lambda z, dt: normalized_gaussian_basis(2, z, dt),
linear_phase, lambda z, dt: normalized_gaussian_basis(10, z, dt))
rospy.loginfo("COPMP created")
# Use state extractor to obtain new target
# new_o = ...
raw_input()
new_o = np.vstack(_target)
rospy.loginfo("Getting COPMP trajectory")
copmp.condition(new_o, 1)
Ymp = copmp.most_probable()
# Use trajectory executor to execute the trajectory
# You shall turn on jtas before running this
time = Ymp[:, 0][:, np.newaxis]
right_traj = Ymp[:, 1:7 + 1]
rospy.loginfo("Executing COPMP")
te = TrajectoryExecutor(time, 10, None, right_traj)
te.execute()