######################################################################## # Spatial ALgebra #################### """ 1 The scene, and later the movement and forces in the scene are modeled following Featherstone's Spatial Algebra. 2 Placement, i.e. rotation and translation of frames (and bodies) are stored in objects of the class SE3. 3 Rigid velocities and acceleration are stored in the class Motion, 4 Forces are stored in the class Forces 5 Masses/Inertias can be found in the class Inertias. An important point to note is that these objects store linear and angular part separately, but we often have to come back to a plain vector/matrix representation. In that case, contrary to Featherstone, we rather store linear part first and angular second. """ M = SE3.Random() nu = Motion.Random() phi = Force.Random() Y = Inertia.Random() print(M, nu, phi, Y) # As mentioned before, the linear and the angular components are stored separately. However we can easily get back to a vector representation. print(nu, nu.vector.T) # Pinocchio strictly separates the constant model element in the Model class, and all the buffers for storing algorithmic quantities # We can create a data buffer through... rdata = model.createData() # By default the RobotWrapper already createsa data buffer through.... robot.data # The idea is that the same model can be used by different part of the algorithm to compute different values from different argument. # For example, in a optimal-control implementation of Pinocchio, you likely want to have a single robot model for all your problem, # but several data for each node of your optimal control solver.
def _lerp(p0, p1, t):
return (1 - t) * p0 + t * p1
def slerp(q0, q1, t):
assert (t >= 0 and t <= 1)
a = AngleAxis(q0.inverse() * q1)
return Quaternion(AngleAxis(a.angle * t, a.axis))
def nlerp(q0, q1, t):
q0 = q0.coeffs()
q1 = q1.coeffs()
l = _lerp(q0, q1, t)
l /= norm(l)
return Quaternion(l[3, 0], *l[:3].T.tolist()[0])
q0 = Quaternion(SE3.Random().rotation)
q1 = Quaternion(SE3.Random().rotation)
gv.applyConfiguration('world/box', [0, 0, 0] + q0.coeffs().T.tolist()[0])
time.sleep(.1)
for t in np.arange(0, 1, .01):
q = nlerp(q0, q1, t)
gv.applyConfiguration('world/box', [0, 0, 0] + q.coeffs().T.tolist()[0])
gv.refresh()
time.sleep(.01)
time.sleep(.1)
gv.applyConfiguration('world/box', [0, 0, 0] + q1.coeffs().T.tolist()[0])
