def trinterp(T0, T1, r):
"""
Interpolate homogeneous transformations.
Compute a homogeneous transform interpolation between C{T0} and C{T1} as
C{r} varies from 0 to 1 such that::
trinterp(T0, T1, 0) = T0
trinterp(T0, T1, 1) = T1
Rotation is interpolated using quaternion spherical linear interpolation.
@type T0: 4x4 homogeneous transform
@param T0: Initial value
@type T1: 4x4 homogeneous transform
@param T1: Final value
@type r: number
@param r: Interpolation index, in the range 0 to 1 inclusive
@rtype: 4x4 homogeneous transform
@return: Interpolated value
@see: L{quaternion}, L{ctraj}
"""
q0 = Quaternion(T0)
q1 = Quaternion(T1)
p0 = transl(T0)
p1 = transl(T1)
qr = q0.interp(q1, r)
pr = p0 * (1 - r) + r * p1
return vstack((concatenate((qr.R(), pr), 1), mat([0, 0, 0, 1])))
def getFInv():
q=Quaternion.Quaternion()
q1=sympy.symbols('v3_q1')
q2=sympy.symbols('v3_q2')
q3=sympy.symbols('v3_q3')
q4=sympy.symbols('v3_q4')
q.setValues(q1,q2,q3,q4)
pt3=Quaternion.Quaternion()
X=sympy.symbols('v2_X')
Y=sympy.symbols('v2_Y')
Z=sympy.symbols('v2_Z')
d=sympy.symbols('v2_d')
pt3.setValues(0,X/d,Y/d,Z/d)
CX,CY,CZ=sympy.symbols('v3_CX,v3_CY,v3_CZ')
pt3c=q.conj()*pt3*q
pt3c[1]=pt3c[1]+CX
pt3c[2]=pt3c[2]+CY
pt3c[3]=pt3c[3]+CZ
CX2,CY2,CZ2=sympy.symbols('v4_CX,v4_CY,v4_CZ')
qq2=Quaternion.Quaternion()
q12=sympy.symbols('v4_q1')
q22=sympy.symbols('v4_q2')
q32=sympy.symbols('v4_q3')
q42=sympy.symbols('v4_q4')
qq2.setValues(q12,q22,q32,q42)
pt3c[1]=pt3c[1]-CX2
pt3c[2]=pt3c[2]-CY2
pt3c[3]=pt3c[3]-CZ2
pt3c2=qq2*pt3c*qq2.conj()
fx,fy,u0,v0,s,k1=sympy.symbols('v5_fx,v5_fy,v5_u0,v5_v0,v5_s,v5_k1')
ptx=pt3c2[1]/pt3c2[3]
pty=pt3c2[2]/pt3c2[3]
ptx2=ptx*fx+u0
pty2=pty*fy+v0
pt=sympy.Matrix([ptx2,pty2])
ptxd,ptyd=sympy.symbols('v1_x, v1_y')
r2=k1*(ptxd*ptx+ptyd*ptyd)
ptx=(1.0+r2)*ptxd
pty=(1.0+r2)*ptyd
pt[0]=((ptx-pt[0]))
pt[1]=((pty-pt[1]))
return pt
def doRotation(vector):
target.facing.normalize()
rotq = Quaternion()
rotq.fromAxis(-1.0, vector.x, vector.y, vector.z)
rotq.normalize()
target.facing = target.facing.mult(rotq)
target.facing.normalize()
def __init__(s):
s.pos = Vector3D()
s.facing = Quaternion()
s.heading = Vector3D()
s.hits = 1
s.mass = 1
s.radius = 1.0
class Transform: '''This is the transform class with components position, rotation and scale''' position = Vector3(0.0, 0.0, 0.0) rotation = Quaternion(0.0, 0.0, 0.0, 1.0) scale = Vector3(0.0, 0.0, 0.0) def Rotate(self, pitch, yaw, roll): self.rotation.EulerRotation(pitch, yaw, roll)
def dis4DoF(gripperOrientation, simpleL1=True):
gripperQ = Quaternion()
gripperQ.rotationMatrixInit(gripperOrientation)
if simpleL1:
return abs(gripperQ.x) + abs(gripperQ.y)
else:
# return sqrt(dof4Q.x * dof4Q.x + dof4Q.y * dof4Q.y)
# return acos(sqrt(dof4Q.w * dof4Q.w + dof4Q.z * dof4Q.z))
return 1 - sqrt(gripperQ.w * gripperQ.w + gripperQ.z * gripperQ.z)
def getCosAngle(gripperOrientation):
rotation = Quaternion()
rotation.listInit(gripperOrientation)
gripperX = np.array(rotation.applyRotation([1, 0, 0]))
gripperX = getUniqueGripperX(gripperX)
gripperZ = np.array(rotation.applyRotation([0, 0, 1]))
gripperZ = gripperZ / getNorm(gripperZ)
tangentY = - np.cross(gripperX, (0, 0, 1))
tangentY = tangentY / getNorm(tangentY)
cosAngle = np.dot(gripperZ, tangentY)
return cosAngle # return cos of elevation
def getFQuat():
X,Y,Z,q1,q2,q3,q4,s=sympy.symbols('v1_CX,v1_CY,v1_CZ,v1_q1,v1_q2,v1_q3,v1_q4,v1_s')
q=Quaternion.Quaternion()
f=(1-(q1**2+q2**2+q3**2+q4**2))**2
fm=sympy.Matrix([f])
return fm
def getFSimTrans():
XS,YS,ZS=sympy.symbols('v1_X,v1_Y,v1_Z')
XD,YD,ZD=sympy.symbols('v2_X,v2_Y,v2_Z')
X,Y,Z,q1,q2,q3,q4,s=sympy.symbols('v3_CX,v3_CY,v3_CZ,v3_q1,v3_q2,v3_q3,v3_q4,v3_s')
q=Quaternion.Quaternion()
q.setValues(q1,q2,q3,q4)
pt3=Quaternion.Quaternion()
pt3.setValues(0,XS,YS,ZS)
pt3d=s*q*pt3*q.conj()
f=sympy.Matrix([pt3d[1]-XD+X,pt3d[2]-YD+Y,pt3d[3]-ZD+Z])
return f
def getContactPoint(gripperLength, gripperCenter, gripperOrientation):
rotation = Quaternion()
rotation.listInit(gripperOrientation)
# print(rotation.getMatrix())
contact1Relative = [- gripperLength / 2, 0, 0]
contact2Relative = [gripperLength / 2, 0, 0]
contact1Relative = rotation.applyRotation(contact1Relative)
contact2Relative = rotation.applyRotation(contact2Relative)
contact1 = [c + r for c, r in zip(gripperCenter, contact1Relative)]
contact2 = [c + r for c, r in zip(gripperCenter, contact2Relative)]
return contact1, contact2
def getFProj():
q=Quaternion.Quaternion()
q1=sympy.symbols('v3_q1')
q2=sympy.symbols('v3_q2')
q3=sympy.symbols('v3_q3')
q4=sympy.symbols('v3_q4')
q.setValues(q1,q2,q3,q4)
pt3=Quaternion.Quaternion()
X=sympy.symbols('v2_X')
Y=sympy.symbols('v2_Y')
Z=sympy.symbols('v2_Z')
pt3.setValues(0.,X,Y,Z)
CX,CY,CZ=sympy.symbols('v3_CX,v3_CY,v3_CZ')
pt3[1]=pt3[1]-CX
pt3[2]=pt3[2]-CY
pt3[3]=pt3[3]-CZ
pt3c2=q*pt3*q.conj()
fx,fy,u0,v0,s,k1,k2,k3=sympy.symbols('v4_fx,v4_fy,v4_u0,v4_v0,v4_s,v4_k1,v4_k2,v4_k3')
ptx=pt3c2[1]/pt3c2[3]
pty=pt3c2[2]/pt3c2[3]
ptx2=ptx*fx+u0
pty2=pty*fy+v0
pt=sympy.Matrix([ptx2,pty2])
ptx,pty=sympy.symbols('v1_x, v1_y')
pt[0]=((ptx-pt[0]))
pt[1]=((pty-pt[1]))
return pt
def getPRY(self):
self.q = Quaternion(100, 0.002, 1 / self.frq)
self.pitch, self.yaw, self.roll = [], [], []
self.q4 = []
for index in range(self.num):
r, p, y, qur = self.q.getAngle(self.ax[index], self.ay[index],
self.az[index], self.gx[index],
self.gy[index], self.gz[index])
self.roll.append(r)
self.pitch.append(p)
self.yaw.append(y)
self.q4.append(qur)
def rotate(self, angle, axis): # fold>>
atom_list = []
q = Quaternion.Quaternion(axis, angle)
m = numpy.array(q.toRotationMatrix())
arr = numpy.array(
[self.value()[0],
self.value()[1],
self.value()[2], 1.0])
rotated = numpy.dot(m, arr)
self.__value[0] = rotated[0]
self.__value[1] = rotated[1]
self.__value[2] = rotated[2]
def getFQuat():
q = Quaternion.Quaternion()
q1 = sympy.symbols('v1_q1')
q2 = sympy.symbols('v1_q2')
q3 = sympy.symbols('v1_q3')
q4 = sympy.symbols('v1_q4')
CX, CY, CZ = sympy.symbols('v1_CX v1_CY v1_CZ')
f = (1 - (q1**2 + q2**2 + q3**2 + q4**2))**2
fm = sympy.Matrix([f])
return fm
def e2q(self):
quat = Quaternion((1, 0, 0, 0))
chz = np.cos(self.z / 2.0)
shz = np.sin(self.z / 2.0)
chy = np.cos(self.y / 2.0)
shy = np.sin(self.y / 2.0)
chx = np.cos(self.x / 2.0)
shx = np.sin(self.x / 2.0)
quat.w = chz * chy * chx + shz * shy * shx
quat.x = shz * chy * chx - chz * shy * shx
quat.y = chz * shy * chx + shz * chy * shx
quat.z = chz * chy * shx - shz * shy * chx
return quat
def trplot(r, name=''):
'''
Plot a rotation as a set of axes aligned with the x, y and z
directions of a frame rotated by C{r}.
'''
if type(r) is matrix:
q = Quaternion(r)
elif isinstance(r, Quaternion):
q = r
else:
raise ValueError
x = q * mat([1, 0, 0])
y = q * mat([0, 1, 0])
z = q * mat([0, 0, 1])
fig = p.figure()
ax = p3.Axes3D(fig)
ax.plot([0, x[0, 0]], [0, x[0, 1]], [0, x[0, 2]], color='red')
ax.plot([0, y[0, 0]], [0, y[0, 1]], [0, y[0, 2]], color='green')
ax.plot([0, z[0, 0]], [0, z[0, 1]], [0, z[0, 2]], color='blue')
p.show()
t_plot = np.reshape(x['ts'], (-1, 1)) tv_plot = np.reshape(v['ts'], (-1, 1)) # plt.figure(1) # plt.subplot(311) # plt.plot(t_plot, accel_orientation[0, :], 'r', t_plot, gyro_orientation[0, :], 'b', tv_plot, vicon_orientation[0, :], 'g') # plt.subplot(312) # plt.plot(t_plot, accel_orientation[1, :], 'r', t_plot, gyro_orientation[1, :], 'b', tv_plot, vicon_orientation[1, :], 'g') # plt.subplot(313) # plt.plot(t_plot, accel_orientation[2, :], 'r', t_plot, gyro_orientation[2, :], 'b', tv_plot,vicon_orientation[2, :], 'g') # plt.show() ############################################################################## ############# FILTER and real time plots ##################################### n = 3 g = Quaternion(0, np.array([0, 0, 1])) # initial conditions q_k = Quaternion.from_euler(accel_orientation[:, 0]) # mean # TODO tune P_k = 1e-6 * np.eye(3) # cov # noise parameters Q = 1e-8 * np.eye(3) # cov R = 5e-2 * np.eye(3) # cov # animated plot # fig = plt.figure() # ax = fig.gca(projection='3d') # plt.ion() # plt.show()
