Running tranimate with sequences of angles... about spatialmath-python HOT 15 OPEN

Not sure it's possible. I think you need to render a set of frames and play them back. For many use cases that's probably not a big disadvantage. Alternatively, programmatically change the backend and have the animation figure "pop out" of the notebook.

from spatialmath-python.

We found a way:

import matplotlib; matplotlib.use("TkAgg") #THIS IS THE MAGIC
import matplotlib.pyplot as plt

# TAke the first and last configurations
qr1 = out.q[0]
qrf = out.q[-1]

# Get homogenous transform representations
R1 = rpy2tr(qr1);
Rf = rpy2tr(qrf);

# Pass them to tranmiate via the @ operator
tranimate(R1@Rf, frame='A', arrow=False, nframes=200);

The question is now: what is a good way to pack a set of matrix rotations and pass them to tranimate in python the way it was done in matlab... Especially for interpolation of rotations... I can get starting and ending poses, convert them to tr's, and then pass them to tranimate. But this is not exactly the same as passing the set of matrices into tranimate directly from all of the interpolation.

The use of jtraj and ctraj do not seem to have examples of how to use them to pass their results to tranimate as before.
Any thoughts

from spatialmath-python.

from spatialmath-python.

I see.

Could possibly iterate over a loop doing something like this:

# b. Animate each frame
fig = plt.figure()
axes = plt.axes( xlim=(-5,5), ylim=(-5,5) )

nth = 10
dims = [-5,5]

fig = plt.figure()
for i in range(0,len(T),nth):
    T[i+1].animate(start=T[i],frame=str(i))
    #print(i)
    fig.clear()

I guess animating from a the first to the last frame with tranimate via the @ operator amounts to doing this no?

from spatialmath-python.

@rojas70 I looked into the function definition. I believe currently the A@B would be evaulated first before passing the result (a single SE3/SO3 object) to the tranimate function.

def tranimate(T, **kwargs):
    """
    Animate a 3D coordinate frame
    :param R: SE(3) or SO(3) matrix
    :type R: ndarray(4,4) or ndarray(3,3)

from spatialmath-python.

This is the code I use to animate the sequence of transformations with the cube (I did this in HW01)

Kenson

import matplotlib; matplotlib.use("TkAgg")
import numpy as np
import matplotlib.pyplot as plt
from spatialmath.base import *
from spatialmath import *
from spatialmath.base import animate
import matplotlib.animation as animation
import roboticstoolbox.tools.trajectory as tr

def plot_cube(ax, T=SE3()):
    P = np.array([
            [-1, 1, 1, -1, -1, 1, 1, -1],
            [-1, -1, 1, 1, -1, -1, 1, 1],
            [-1, -1, -1, -1, 1, 1, 1, 1]])
    Q = T*P
    ax.set_xlim3d(-2, 2);ax.set_ylim3d(-2, 2);ax.set_zlim3d(-2, 2);
    ax.set_xlabel('X');ax.set_ylabel('Y');ax.set_zlabel('Z');
    lines = [[0, 1, 5, 6], [1, 2, 6, 7], [2, 3, 7, 4], [3, 0, 4, 5]]
    ret = []
    for line in lines:
        o=ax.plot([Q[0, i] for i in line], [Q[1, i] for i in line], [Q[2, i] for i in line])
        ret.append(o[0])
    return ret


def update_frame(i):
    global out
    return plot_cube(ax, SO3(rpy2r(out.q[i])))

P = np.array([
        [-1, 1, 1, -1, -1, 1, 1, -1],
        [-1, -1, 1, 1, -1, -1, 1, 1],
        [-1, -1, -1, -1, 1, 1, 1, 1]])

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

q0=[0, 0, 0]
qf=[-np.pi/2, np.pi/2, np.pi/4]

out = tr.jtraj(q0, qf, tv=100)
number_of_steps = len(out.q)
anim = animation.FuncAnimation(fig, update_frame,
                              frames=number_of_steps,
                              interval=20,
                              blit=True,
                              repeat=True)
plt.show()

from spatialmath-python.

Thank you @mfkenson!

@petercorke, compariang outputs of carateian interpolation ctraj vs those of joint angle interpolation jtraj is an interesting test case scenario. The former yields straigtline motions that maintain an orientation while the latter follow an orbital path....

Additionally, many aspects of the previous version of the course was based on easily displaying jtraj's and ctraj's... it is very nice to see the evolution of the transformed coordinate frame. I think it would be good to include it in the toolbox here.

from spatialmath-python.

@rojas70 let me try to work on the code. hopefully will make a pull request after lunar new year. See you in next lecture!

from spatialmath-python.

@petercorke @rojas70 this PR would make trplot and tranmiate accepts list of T (SE3.A 4x4 ndarray) cheers!
(The PR page shows the example of tranimate)

from spatialmath-python.

Thanks. Maybe a bit before that I pushed a change to trplot() that takes an iterable. Unlike tranimate, it leaves all the frames showing.

from spatialmath-python.

yes I could see your changes in base.trplot (transforms3d.py). Thats why I decided to make the change accordingly to animate.trplot (animate.py) where tranimiate depends on it.

I think I should implement the same change in class Animate2 so that tranimate2 could take advantage as well. I will make another new PR for both changes soon.

from spatialmath-python.

just submitted a new PR. Hopefully this could help the students migrating from the matlab toolbox.

from spatialmath-python.

I've just pushed some changes to GH that allow you to pass a generator

def attitude():
   J = np.array([[2, -1, 0], [-1, 4, 0], [0, 0, 3]])
   attitude = UnitQuaternion()
   w = 0.2 * np.r_[1, 2, 2].T
   dt = 0.05

   for t in np.arange(0, 10, dt):
      wd =  -np.linalg.inv(J) @ (np.cross(w, J @ w))
      w += wd * dt
      attitude.increment(w * dt)
      yield r2t(attitude.R)

tranimate(attitude())

The animate framework could be extended to points. At the moment the only entities it supports are lines, arrows and text but the framework is quite general. Also need to generalise it allow multiple entities to be individually moved around.

from spatialmath-python.

Nice. BTW I've just seen your latest commit and really like the try_except way.

Maybe I could make an example demonstrating the use of tranmiate<-generator. Such as visualizing the pose of realsense t265 in real world.

from spatialmath-python.

from spatialmath-python.