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)
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