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