예제 #1
0
 def plot_npendulum(self, ax, m, time=999):
     func = simulation.step(m, 1000 * self.dt)
     s = simulation.simulate(self.n, self.x0, self.dt, func)
     s_cartesian = simulation.cartesian(self.n, s[time])
     x = s_cartesian[:len(s_cartesian) / 2]
     y = s_cartesian[len(s_cartesian) / 2:]
     ax.plot(x, y)
     ax.plot(x[0], y[0], 'o', ms=6)
     ax.plot(x[-1], y[-1], 's', ms=6)
예제 #2
0
    def compute_sensori_effect(self, m):
        """ This function generates the end effector position at the end of the movement.

        .. note:: The duration of the movement is  1000*self.dt
        .. note:: To use basis functions rather than step functions, set :use_basis_functions: to 1
        """
        if self.use_basis_functions:
            func = simulation.step(m, 1000 * self.dt)
        else:
            traj = self.bf.trajectory(m).flatten()
            func = lambda t: traj[int(1000 * self.dt * t)]

        states = simulation.simulate(self.n, self.x0, self.dt, func)
        last_state_cartesian = simulation.cartesian(self.n, states[-1])

        end_effector_pos = array(
            [last_state_cartesian[self.n], last_state_cartesian[2 * self.n]])
        end_effector_pos += self.noise * random.randn(len(end_effector_pos))
        return end_effector_pos