def callback(self, msg): if self.pose_0 == None: self.pose_0 = [ msg.position.x, msg.position.y, msg.position.z, msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w ] self.pose_1 = [ msg.position.x, msg.position.y, msg.position.z, msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w ] else: self.pose_0 = self.pose_1 self.pose_1 = [ msg.position.x, msg.position.y, msg.position.z, msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w ] if np.sum( np.abs(np.array(self.pose_0) - np.array(self.pose_1))) > 0.01: print('Moving') self.traj.append(self.pose_1) else: if len(self.traj) > 5: # Fit DMP to motion segment path = np.array(self.traj) dmp = DMP(path[0, :], path[-1, :], Nb=500, dt=0.01, d=path.shape[1], jnames=self.name) params = dmp.imitate_path( path + 1e-5 * np.random.randn(path.shape[0], path.shape[1])) #print(params) dmp.reset_state() self.dmp_count += 1 with open("./dmp%05d.npy" % self.dmp_count, "w") as f: pickle.dump(dmp, f, pickle.HIGHEST_PROTOCOL) y_r, dy_r, ddy_r = dmp.rollout() plt.subplot(1, 2, 1) plt.cla() plt.plot(y_r) plt.subplot(1, 2, 2) plt.cla() plt.plot(path) plt.draw() plt.pause(0.1) self.traj = [] print('Stationary')
def callback(self, msg): if self.js_prev is not None: self.js_prev = msg self.js_current = msg else: self.js_prev = self.js_current self.js_current = msg # If your joints have changed by more than a small threshold amount, then you are moving! Start recording joint angles if np.sum( np.abs( np.array(self.js_prev.position) - np.array(self.js_current.position))) > 0.01: print('Moving') j_list = [ msg.position[msg.name.index(j)] for j in self.joint_names ] self.traj.append(j_list) elif len(self.traj) > 0: # Fit DMP to motion segment path = np.array(self.traj) dmp = DMP(path[0, :], path[-1, :], num_basis_funcs=500, dt=0.01, d=path.shape[1], jnames=self.joint_names) _, weights = imitate_path( path + 1e-5 * np.random.randn(path.shape[0], path.shape[1]), dmp) print("Learned weights: {}".format(weights)) dmp.weights = weights self.dmp_count += 1 # Save it with pickle with open("./dmp{}.npy".format(self.dmp_count), "w") as f: pickle.dump(dmp, f, pickle.HIGHEST_PROTOCOL) # Plot the path imitated by the learned dmp (rolled out) against the actual trajectory path... y_r, dy_r, _ = dmp.rollout() plot_rollout(y_r, path) jt = build_jt(msg.header, self.joint_names, y_r, dy_r) self.traj_pub.publish(jt) self.traj = [] else: print("Stationary")
def callback(self, msg): if self.pose_prev is not None: self.pose_prev = [msg.position.x, msg.position.y, msg.position.z, msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w] self.pose_current = [msg.position.x, msg.position.y, msg.position.z, msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w] else: self.pose_prev = self.pose_current self.pose_current = [msg.position.x, msg.position.y, msg.position.z, msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w] if np.sum(np.abs(np.array(self.pose_prev) - np.array(self.pose_current))) > 0.01: print('Moving') self.traj.append(self.pose_current) else: if len(self.traj) > 5: # Fit DMP to motion segment path = np.array(self.traj) dmp = DMP(path[0, :], path[-1, :], num_basis_funcs=500, dt=0.01, d=path.shape[1], jnames=self.name) params = dmp.imitate_path(path+1e-5*np.random.randn(path.shape[0], path.shape[1])) print(params) self.dmp_count += 1 with open("./dmp%05d.npy" % self.dmp_count, "w") as f: pickle.dump(dmp, f, pickle.HIGHEST_PROTOCOL) y_r, _, _ = dmp.rollout() plot_rollout(y_r, path) self.traj = [] print('Stationary')
def callback(self, msg): if self.js_0 == None: self.js_0 = msg self.js_1 = msg else: self.js_0 = self.js_1 self.js_1 = msg if np.sum( np.abs( np.array(self.js_0.position) - np.array(self.js_1.position))) > 0.01: print('Moving') j_list = [] for j, joint in enumerate(self.joint_names): i = msg.name.index(joint) if i: j_list.append(msg.position[i]) self.traj.append(j_list) else: if len(self.traj) > 0: # Fit DMP to motion segment path = np.array(self.traj) dmp = DMP(path[0, :], path[-1, :], Nb=500, dt=0.01, d=path.shape[1], jnames=self.joint_names) params = dmp.imitate_path( path + 1e-5 * np.random.randn(path.shape[0], path.shape[1])) #print(params) dmp.reset_state() self.dmp_count += 1 with open("./dmp%05d.npy" % self.dmp_count, "w") as f: pickle.dump(dmp, f, pickle.HIGHEST_PROTOCOL) y_r, dy_r, ddy_r = dmp.rollout() plt.subplot(1, 2, 1) plt.cla() plt.plot(y_r) plt.subplot(1, 2, 2) plt.cla() plt.plot(path) plt.draw() plt.pause(0.1) self.jt.header = msg.header self.jt.joint_names = self.joint_names jtp = JointTrajectoryPoint() for i in range(y_r.shape[0]): jtp.positions = y_r[i, :].tolist() jtp.velocities = dy_r[i, :].tolist() jtp.time_from_start = rospy.Duration(1.0) self.jt.points.append(jtp) self.traj_pub.publish(self.jt) self.traj = [] print('Stationary') self.jt = JointTrajectory()