Exemplo n.º 1
0
    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')
Exemplo n.º 2
0
    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')
Exemplo n.º 3
0
    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()