Ejemplo n.º 1
0
def num_euler_to_quat(euler):
	'''Converts 3 euler angles XYZ to a quaternion (YXZ order)'''
	assert(True not in [(math.isnan(x) or math.isinf(x)) for x in euler])
	rx, ry, rz = euler
	q_xrot = cgtypes.quat().fromAngleAxis(rx, [1, 0, 0])
	q_yrot = cgtypes.quat().fromAngleAxis(ry, [0, 1, 0])
	q_zrot = cgtypes.quat().fromAngleAxis(rz, [0, 0, 1])
	q = (q_yrot * q_xrot * q_zrot).normalize() #YXZ order
	return q
Ejemplo n.º 2
0
def num_euler_to_quat(euler):
    '''Converts 3 euler angles XYZ to a quaternion (YXZ order)'''
    assert (True not in [(math.isnan(x) or math.isinf(x)) for x in euler])
    rx, ry, rz = euler
    q_xrot = cgtypes.quat().fromAngleAxis(rx, [1, 0, 0])
    q_yrot = cgtypes.quat().fromAngleAxis(ry, [0, 1, 0])
    q_zrot = cgtypes.quat().fromAngleAxis(rz, [0, 0, 1])
    q = (q_yrot * q_xrot * q_zrot).normalize()  #YXZ order
    return q
Ejemplo n.º 3
0
def rotate_quat(q, ax, ang):
    """
    Rotate quaternion orientation by given axis and angle

    q = input quaternion
    ax rotation axis
    ang = rotation angle

    """

    # Convert quaternion to cgtypes quaternion
    q1 = cgtypes.quat(q)
    q_rot = cgtypes.quat(ang,ax)
    q2 = q_rot*q1
    return quat2tuple(q2)
Ejemplo n.º 4
0
def qrotate_vec(v,q):
    """
    Rotate vector using a quaternion.

    v = input vector (tuple)
    q = quaternion (tuple)

    """
    # Get cgtypes rotation quaternion and its inverse
    rot_q = cgtypes.quat(q)
    rot_q_inv = rot_q.inverse()
    # Rotate vector
    vq = cgtypes.quat(0.0, v[0], v[1], v[2])
    vq_new = rot_q*vq*rot_q_inv
    return vq_new.x, vq_new.y, vq_new.z
Ejemplo n.º 5
0
    def _integrateAngularVelocities(self, angularVelocity, timeElapsed):

        angularVelocityQuat = quat(0.0, angularVelocity.x, angularVelocity.y,
                                   angularVelocity.z)

        self.orientationBody = self.orientationBody * (angularVelocityQuat *
                                                       timeElapsed / 2).exp()
Ejemplo n.º 6
0
    def calculate_quaternion(self):
        from cgkit.cgtypes import quat, mat3, slerp
        q = quat()
        mat = mat3([num for row in self.transform[0:3] for num in row[0:3]])
        try:
            mat = mat.decompose()[0]
            q.fromMat(mat)
        except ZeroDivisionError:
            q = quat(0,0,0,0)
            self.do_not_use = True

        a = 2*math.acos(q.w)
        t = np.dot(np.array([q.x,q.y,q.z]),a)
        self.rot_axis = t.tolist()

        self.q = np.dot(np.array([q.x,q.y,q.z]),q.w).tolist()
Ejemplo n.º 7
0
    def measure(self, magneto):

        measurementJacobian = sp.zeros((3, 13))

        magnetoJacobian = scipy_utils.d_inverse_rotation_d_quaternion_at(
            cgkit_to_scipy.convert(self.orientationBody),
            cgkit_to_scipy.convert(magneto))

        scipy_utils.load_submatrix(measurementJacobian, (0, 6),
                                   magnetoJacobian)

        kalmanGain = sp.dot(
            sp.dot(self._errorCovariance, measurementJacobian.transpose()),
            linalg.inv(
                sp.dot(
                    measurementJacobian,
                    sp.dot(self._errorCovariance,
                           measurementJacobian.transpose())) +
                self._measurementCovariance))

        stateCorrection = sp.dot(
            kalmanGain,
            (cgkit_to_scipy.convert(magneto - self._predictMagnetoBody())))

        self.orientationBody = self.orientationBody + quat(
            list(stateCorrection[6:10]))

        self._errorCovariance = sp.dot(
            (sp.identity(13) - sp.dot(kalmanGain, measurementJacobian)),
            self._errorCovariance)
Ejemplo n.º 8
0
def rotate_vec(v, ax, ang):
    """
    Rotate a vector by a given angle about a given axis.

    v = input vector
    ax = rotation axis
    ang = rotation angle (radians)

    """
    # Get rotation quaterion and its inverse
    rot_q = cgtypes.quat(ang, ax)
    rot_q_inv = rot_q.inverse()
    # Rotate vector
    vq = cgtypes.quat(0.0,v[0],v[1],v[2])
    vq_new = rot_q*vq*rot_q_inv
    return vq_new.x, vq_new.y, vq_new.z
Ejemplo n.º 9
0
    def testquaternion_derivative(self):
        
        quat = scipy_utils.to_quaternion(*self.YPRAngles)
        
        print scipy_utils.quaternion_norm(quat)
        
        d_quat = scipy_utils.quaternion_derivative(2*sp.pi, 0.0, 0.0, quat)
        
        print scipy_utils.quaternion_norm(d_quat)
        
        quat_1s_later = quat + 0.01 * d_quat
        
        cgquat = cgtypes.quat(quat_1s_later)
        
        print cgquat
        
        print cgquat.normalize()
        

        
        quat_1s_later = quat_1s_later / sp.sqrt(scipy_utils.quaternion_norm(quat_1s_later))
        
        print quat_1s_later
        
        print scipy_utils.quaternion_norm(quat_1s_later)
        
        roll, pitch, yaw  = scipy_utils.to_YPR(quat_1s_later)
        
        testing.assert_almost_equal(roll, self.YPRAngles[0] + .01)
Ejemplo n.º 10
0
 def calculate_quaternion(self):
     from cgkit.cgtypes import quat, mat3, slerp
     q = quat()
     mat = mat3([num for row in self.transform[0:3] for num in row[0:3]])
     mat = mat.decompose()[0]
     q.fromMat(mat)
     self.q = np.dot(np.array([q.x,q.y,q.z]),q.w).tolist()
Ejemplo n.º 11
0
    def updateFromJoint(self):
        # TODO: I think this has been replaced by the transformVertices function
        if self.joint is None:
            return
        if self.updateOnlyFromGrandparentJoints:
            if self.joint.parentJoint is None:
                q = quat(1, 0, 0, 0)
            else:
                q = self.joint.parentJoint.orientation
        else:
            q = self.joint.orientation
        # update vertex
        for i in xrange(len(self.OriginalVertexList)):
            scale = self.scale * self.joint.scale  # scale is combination of model scale factor and joint scale factor
            v = scale * (
                self.OriginalVertexList[i] - self.initialRotationCenter
            )  # get scaled vector based on initial rotation center
            v = numpy.array(q.rotateVec(v))  # apply current orientation
            self.VertexList[
                i] = v + self.joint.location  # move vector to joint location

        # update normals (orientation only, not scaled or moved)
        for i in xrange(len(self.OriginalNormVectors)):
            n = self.OriginalNormVectors[i]
            n = numpy.array(q.rotateVec(n))
            self.NormVectors[i] = n
Ejemplo n.º 12
0
    def __init__(self):

        self._processCovariance = sp.identity(13)

        self._measurementCovariance = sp.identity(3)

        self._errorCovariance = sp.identity(13)

        self._translationRotorFromBody = vec3(0)

        self._gravity = vec3([0.0, 0.0, 1.0])

        self._magneticField = mat3.fromEulerXYZ(0.0, -1.22, 0.0) * vec3(
            [1.0, 0.0, 0.0])
        # the magnetic field vector points to the magnetic north, with a -70 degrees (-1.22 radians) pitch (towards the ground)

        self._declinationAngle = 0.0

        self.timestamp = 0.0

        self.positionNED = vec3(0)

        self.velocityBody = vec3(0)

        self.orientationBody = quat(1)

        self.gyroBiases = vec3(0)

        self.gravityBody = self._gravity
Ejemplo n.º 13
0
 def testDavailus(self):
     
     q_w = cgtypes.quat(0.0, 2*sp.pi, 0.0, 0.0)
     
     d_t = 1.0
     
     q_w = q_w * d_t * 0.5
     
     e_dav = q_w.exp()
     
     quat_t0 = cgtypes.quat(scipy_utils.to_quaternion(*self.YPRAngles))
     
     quat_t1 = quat_t0 * e_dav
     
     roll, pitch, yaw  = scipy_utils.to_YPR((quat_t1.w, quat_t1.x, quat_t1.y, quat_t1.z))
     
     testing.assert_almost_equal(roll, self.YPRAngles[0])
Ejemplo n.º 14
0
    def rotateCamera(self,camera,eyePose):
        """
        This function rotates the Gluskap camera based on the Oculus Rift sensor data in eyePose parameter
        """

        # Convert the OculusRift rotation quaternion to a cgkit quat object
        rotationQuaternion = quat(eyePose.Orientation.toList())

        # Fix the quaternion because of incompatible coordinate system
        rotationQuaternion=quat(rotationQuaternion.w,rotationQuaternion.z,rotationQuaternion.y,-rotationQuaternion.x)

        # Normalize the quaternion
        rotationQuaternion=rotationQuaternion.normalize()

        # Apply the rotation to the basevpn to achieve the new view plane normal vector of the camera
        vpn=rotationQuaternion.rotateVec(self.basevpn)

        # Apply the rotation to baseup to acheive the up vector of the camera
        up=rotationQuaternion.rotateVec(self.baseup)

        # Apply the gamepad yaw rotation to the view plane normal and up vector of camera
        gamePadYawRotation=quat().fromAngleAxis(self.gamePadYaw,self.baseup)
        vpn=gamePadYawRotation.rotateVec(vpn)
        up=gamePadYawRotation.rotateVec(up)

        # Apply the gamepad pitch to the view plane normal and up vector of camera
        rightAxis=up.cross(vpn).normalize()
        gamePadPitchRotation=quat().fromAngleAxis(self.gamePadPitch,rightAxis)
        vpn=gamePadPitchRotation.rotateVec(vpn)
        up=gamePadPitchRotation.rotateVec(up)

        # Compute the angle between the new view plane normal and view plane normal of previous frame
        angle=vpn.angle(self.prevpn)

        # Store the prevpn for the next frame
        self.prevpn=vec3(vpn)

        # If angle is below certain threshold do not apply the rotation
        # This is to remove image vibration caused by unstable Oculus Rift orientation
        if math.fabs(angle) < 0.005:
            return

        # update the camera on Gluskap
        camera.vpn=(vpn.x,vpn.y,vpn.z)
        camera.up=(up.x,up.y,up.z)
Ejemplo n.º 15
0
 def rotateAroundAxis(self, angle, axis):
     # create rotation quat based on local axis
     q = quat(angle, self.cameraQuat.rotateVec(axis))
     # rotate camera around focus point
     self.camLoc = numpy.array(q.rotateVec(self.camLoc - self.camFocus)) + self.camFocus
     # update camera orientation
     self.cameraQuat = q * self.cameraQuat
     # calculate up direction from new orientation
     self.camUp = numpy.array(self.cameraQuat.rotateVec([0.0, 1.0, 0.0]))
Ejemplo n.º 16
0
 def __init__(self,
              pos_0=cgtypes.vec3(0,0,0),
              ori_0=cgtypes.quat(1,0,0,0),
              vel_0=cgtypes.vec3(0,0,0),
              ):
     self.pos_0 = pos_0
     self.ori_0 = ori_0
     self.vel_0 = vel_0
     self.reset()
Ejemplo n.º 17
0
 def rotateAroundAxis(self, angle, axis):
     # create rotation quat based on local axis
     q = quat(angle, self.cameraQuat.rotateVec(axis))
     # rotate camera around focus point
     self.camLoc = numpy.array(
         q.rotateVec(self.camLoc - self.camFocus)) + self.camFocus
     # update camera orientation
     self.cameraQuat = q * self.cameraQuat
     # calculate up direction from new orientation
     self.camUp = numpy.array(self.cameraQuat.rotateVec([0.0, 1.0, 0.0]))
Ejemplo n.º 18
0
    def walkTo(self, X, Y, Theta):
        cur_position = vec3(self.nao.position)
        curTheta = self.nao.orientation

        movement = vec3(X, Y, 0)

        rotation = quat(curTheta, AXIS_Z)
        movement = rotation.rotateVec(movement)

        self.nao.position = list(cur_position + movement)
        self.nao.orientation = (curTheta + Theta) % (math.pi * 2)
Ejemplo n.º 19
0
    def walkTo(self, X, Y, Theta):
        cur_position = vec3(self.nao.position)
        curTheta = self.nao.orientation

        movement = vec3(X, Y, 0)

        rotation = quat(curTheta, AXIS_Z)
        movement = rotation.rotateVec(movement)

        self.nao.position = list(cur_position + movement)
        self.nao.orientation = (curTheta + Theta) % (math.pi * 2)
Ejemplo n.º 20
0
def rbm_to_dualquat(rbm):
    import cgkit.cgtypes as cg
    q0 = cg.quat().fromMat(cg.mat3(rbm[:3, :3].T.tolist()))
    q0 = q0.normalize()
    q0 = np.array([q0.w, q0.x, q0.y, q0.z])
    t = rbm[:3, 3]
    q1 = np.array([
        -0.5 * (t[0] * q0[1] + t[1] * q0[2] + t[2] * q0[3]),
        0.5 * (t[0] * q0[0] + t[1] * q0[3] - t[2] * q0[2]),
        0.5 * (-t[0] * q0[3] + t[1] * q0[0] + t[2] * q0[1]),
        0.5 * (t[0] * q0[2] - t[1] * q0[1] + t[2] * q0[0])
    ])
    return np.array(q0.tolist() + q1.tolist())
Ejemplo n.º 21
0
  def render_frame(self):
    self.frame += 1

    # Fetch the head pose
    poses = self.hmd.get_eye_poses(self.frame, self.eyeOffsets)

    self.hmd.begin_frame(self.frame)
    for i in range(0, 2):
      eye = self.hmdDesc.EyeRenderOrder[i]

      glMatrixMode(GL_PROJECTION)
      glLoadMatrixf(self.projections[eye])

      self.eyeview = mat4(1.0)


      # Apply the head orientation
      rot = poses[eye].Orientation
      # Convert the OVR orientation (a quaternion
      # structure) to a cgkit quaternion class, and
      # from there to a mat4  Coordinates are camera
      # coordinates
      rot = quat(rot.toList())
      rot = rot.toMat4()

      # Apply the head position
      pos = poses[eye].Position
      # Convert the OVR position (a vector3 structure)
      # to a cgcit vector3 class. Position is in camera /
      # Rift coordinates
      pos = vec3(pos.toList())
      pos = mat4(1.0).translate(pos)
      
      pose = pos * rot

      # apply it to the eyeview matrix
      self.eyeview = pose;

      # The subclass is responsible for taking eyeview
      # and applying it to whatever camera or modelview
      # coordinate system it uses before rendering the
      # scene

      # Active the offscreen framebuffer and render the scene
      glBindFramebuffer(GL_FRAMEBUFFER, self.fbo[eye])
      size = self.eyeTextures[eye].Texture.Header.RenderViewport.Size
      glViewport(0, 0, size.w, size.h)
      self.render_scene()
      glBindFramebuffer(GL_FRAMEBUFFER, 0)
    self.hmd.end_frame(poses, self.eyeTextures)
    glGetError()
Ejemplo n.º 22
0
    def _rotVector_(self, v, angle, axis):
        """
        Rotate a vector by an angle around an axis
        INPUTS:    
            v:    3-dim float array
            angle:    float, the rotation angle in radians
            axis: string, 'x', 'y', or 'z'
        
        RETURNS:    
            float array(3):  the rotated vector
        """
        # axisd = {'x':[1,0,0], 'y':[0,1,0], 'z':[0,0,1]}

        # construct quaternion and rotate...
        rot = cgt.quat()
        rot.fromAngleAxis(angle, axis)
        return list(rot.rotateVec(v))
Ejemplo n.º 23
0
def ovrPoseToMat4(pose):
  # Apply the head orientation
  rot = pose.Orientation
  # Convert the OVR orientation (a quaternion structure) to a cgkit quaternion 
  # class, and from there to a mat4  Coordinates are camera coordinates
  rot = quat(rot[-1:]+rot[:-1]) # reorder xyzw -> wxyz
  rot = rot.toMat4()

  # Apply the head position
  pos = pose.Position
  # Convert the OVR position (a vector3 structure) to a cgcit vector3 class. 
  # Position is in camera / Rift coordinates
  pos = vec3(pos)
  pos = mat4(1.0).translate(pos)
  
  pose = pos * rot
  return pose
Ejemplo n.º 24
0
def ovrPoseToMat4(pose):
  # Apply the head orientation
  rot = pose.Orientation
  # Convert the OVR orientation (a quaternion structure) to a cgkit quaternion 
  # class, and from there to a mat4  Coordinates are camera coordinates
  rot = quat(rot[-1:]+rot[:-1]) # reorder xyzw -> wxyz
  rot = rot.toMat4()

  # Apply the head position
  pos = pose.Position
  # Convert the OVR position (a vector3 structure) to a cgcit vector3 class. 
  # Position is in camera / Rift coordinates
  pos = vec3(pos)
  pos = mat4(1.0).translate(pos)
  
  pose = pos * rot
  return pose
Ejemplo n.º 25
0
def residual(parameters, motion, statisticsByLabelBeforeMotion, samplesByLabelInMotion, statisticsByLabelAfterMotion, acceleroMisalignmentsAndScales, acceleroBias):
    
#    gravityVectorBefore = sp.array([statisticsByLabelBeforeMotion['x'][0], statisticsByLabelBeforeMotion['y'][0], statisticsByLabelBeforeMotion['z'][0]])
#
#    gravityVectorBefore = utils.correct_measures(acceleroMisalignmentsAndScales, acceleroBias, gravityVectorBefore.reshape((3,1)))
#    
#    gravityVectorAfter = sp.array([statisticsByLabelAfterMotion['x'][0], statisticsByLabelAfterMotion['y'][0], statisticsByLabelAfterMotion['z'][0]])
#  
#    gravityVectorAfter = utils.correct_measures(acceleroMisalignmentsAndScales, acceleroBias, gravityVectorAfter.reshape((3,1)))
    
    gyroMisalignmentsAndScales = sp.reshape(parameters, (3, 3))
    
    correctedMeasures = utils.extract_and_correct_gyro_motion(gyroMisalignmentsAndScales, statisticsByLabelBeforeMotion, samplesByLabelInMotion)
    
    aSamples, bSamples, cSamples = correctedMeasures[0,:], correctedMeasures[1,:], correctedMeasures[2,:]
    
#    gravityVector = vec3(*sp.reshape(gravityVectorBefore, 3))

    fromAngularPosition, toAngularPosition, feedrate, endTime, beginTime = motion
    
    gravityVector = utils.gravityVector_IMU_coords(math.radians(fromAngularPosition[0]), math.radians(fromAngularPosition[1]))
    
    gravityVectorAfter = utils.gravityVector_IMU_coords(math.radians(toAngularPosition[0]), math.radians(toAngularPosition[1]))
    
    timestamps = samplesByLabelInMotion['timestamps']
    
    previousTimestamp = timestamps[0]
    
    for index in range(len(timestamps)) :
        
        angularVelocityQuat = quat(0.0, aSamples[index], bSamples[index], cSamples[index])
        
        timeElapsed = timestamps[index] - previousTimestamp
            
        previousTimestamp = timestamps[index]
        
        gravityVector = (angularVelocityQuat * timeElapsed / 2).exp().rotateVec(gravityVector)
    
    residualGavity = gravityVector - gravityVectorAfter
    
    residualGavity = sp.array([residualGavity.x, residualGavity.y, residualGavity.z])
    
    return sp.sqrt(sp.dot(residualGavity, residualGavity))
Ejemplo n.º 26
0
    def updateFromJoint(self):
        # TODO: I think this has been replaced by the transformVertices function
        if self.joint is None:
            return
        if self.updateOnlyFromGrandparentJoints:
            if self.joint.parentJoint is None:
                q = quat(1, 0, 0, 0)
            else:
                q = self.joint.parentJoint.orientation
        else:
            q = self.joint.orientation
        # update vertex
        for i in xrange(len(self.OriginalVertexList)):
            scale = self.scale * self.joint.scale  # scale is combination of model scale factor and joint scale factor
            v = scale * (self.OriginalVertexList[i] - self.initialRotationCenter)  # get scaled vector based on initial rotation center
            v = numpy.array(q.rotateVec(v))  # apply current orientation
            self.VertexList[i] = v + self.joint.location  # move vector to joint location

        # update normals (orientation only, not scaled or moved)
        for i in xrange(len(self.OriginalNormVectors)):
            n = self.OriginalNormVectors[i]
            n = numpy.array(q.rotateVec(n))
            self.NormVectors[i] = n
Ejemplo n.º 27
0
    def calculate_plane_to_xy_transform(self):
        """
            Calculate the transform to rotate the plane of the circle into the yz plane.
        """
        def chunks(l, n):
            """ Yield successive n-sized chunks from l.
            """
            for i in xrange(0, len(l), n):
                yield l[i:i+n]

        x = self.best_fit_plane()
        normal = [x[0],x[1],-1]
        self.normal = normal

        from cgkit.cgtypes import quat, mat3, slerp
        axis = np.cross(np.array(normal),np.array([0,0,1]))
        angle = math.acos(np.dot(np.array(normal),np.array([0,0,1]))/np.linalg.norm(normal))
        q = quat()
        q = q.fromAngleAxis(angle,axis.tolist())
        transform = [i for i in chunks(q.toMat4().toList(rowmajor=True),4)]
        self.plane_to_xy_transform = transform

        return transform
Ejemplo n.º 28
0
def createCone(*args, **kwargs):
    '''
    Function Signatures:
    createCone(...)
    createCone(radius, ...)
    createCone(radius, height, ...)
    createCone(radius, height, offset, ...)

    Arguments {default value}:

    radius
        radius of cone {1.0}
    height
        height of cone {twice value as radius}
    offset
        offsets the center of the bottom cylinder cap.
        Given in form [x,y,z]. { [0.0,0.0,0.0] }

    Keywords:
    joint
        Joint object that the model will rotate around. {None}
    ngon
        number of edges used to approximate the circle bottom {10}
    axis
        axis this cone is parallel to, 'x', 'y', or 'z'
    '''
    if len(args) > 0:
        r = float(args[0])
    else:
        r = 1.0
    if len(args) > 1:
        height = float(args[1])
    else:
        height = r * 2.0
    if 'ngon' in kwargs:
        N = int(kwargs['ngon'])
    elif 'Ngon' in kwargs:
        N = int(kwargs['Ngon'])
    else:
        N = 10
    if N < 3:
        N = 3
    if 'axis' in kwargs:
        if kwargs['axis'].lower() == 'x':
            initialOrintation = quat(math.pi / 2, [0, 1, 0])
        elif kwargs['axis'].lower() == 'y':
            initialOrintation = quat(-math.pi / 2, [1, 0, 0])
        else:
            initialOrintation = quat(1)
    else:
        initialOrintation = quat(1)
    vList = []
    normList = []

    for n in xrange(N):
        x1 = r * math.cos(2 * math.pi * n / N)
        y1 = r * math.sin(2 * math.pi * n / N)
        x2 = r * math.cos(2 * math.pi * (n + 1) / N)
        y2 = r * math.sin(2 * math.pi * (n + 1) / N)
        # draw cone
        f = [[0.0, 0.0, height],
             [x2, y2, 0.0],
             [x1, y1, 0.0]]
        vList.extend(f)
        f = numpy.array(f)
        norm = numpy.cross(f[2] - f[0], f[1] - f[0])
        normList.append(norm)
        # draw circle
        f = [[0.0, 0.0, 0.0],
             [x1, y1, 0.0],
             [x2, y2, 0.0]]
        vList.extend(f)
        f = numpy.array(f)
        norm = numpy.cross(f[2] - f[0], f[1] - f[0])
        normList.append(norm)

    vList = numpy.array(vList)
    normList = numpy.array(normList)

    # change vertex to new orientation
    for i in xrange(len(vList)):
        vList[i] = numpy.array(initialOrintation.rotateVec(vList[i]))

    # add offset
    if len(args) > 2:
        vList += numpy.array(args[2], dtype=float)
    triVert = numpy.array(range(len(vList)))
    triVert = triVert.reshape([len(vList) / 3, 3])
    if 'joint' in kwargs:
        rect = TriModel(vList, triVert, kwargs['joint'], normalVectors=normList, **kwargs)
    else:
        rect = TriModel(vList, triVert, normalVectors=normList, **kwargs)
    return rect
Ejemplo n.º 29
0
def createCone(*args, **kwargs):
    '''
    Function Signatures:
    createCone(...)
    createCone(radius, ...)
    createCone(radius, height, ...)
    createCone(radius, height, offset, ...)

    Arguments {default value}:

    radius
        radius of cone {1.0}
    height
        height of cone {twice value as radius}
    offset
        offsets the center of the bottom cylinder cap.
        Given in form [x,y,z]. { [0.0,0.0,0.0] }

    Keywords:
    joint
        Joint object that the model will rotate around. {None}
    ngon
        number of edges used to approximate the circle bottom {10}
    axis
        axis this cone is parallel to, 'x', 'y', or 'z'
    '''
    if len(args) > 0:
        r = float(args[0])
    else:
        r = 1.0
    if len(args) > 1:
        height = float(args[1])
    else:
        height = r * 2.0
    if 'ngon' in kwargs:
        N = int(kwargs['ngon'])
    elif 'Ngon' in kwargs:
        N = int(kwargs['Ngon'])
    else:
        N = 10
    if N < 3:
        N = 3
    if 'axis' in kwargs:
        if kwargs['axis'].lower() == 'x':
            initialOrintation = quat(math.pi / 2, [0, 1, 0])
        elif kwargs['axis'].lower() == 'y':
            initialOrintation = quat(-math.pi / 2, [1, 0, 0])
        else:
            initialOrintation = quat(1)
    else:
        initialOrintation = quat(1)
    vList = []
    normList = []

    for n in xrange(N):
        x1 = r * math.cos(2 * math.pi * n / N)
        y1 = r * math.sin(2 * math.pi * n / N)
        x2 = r * math.cos(2 * math.pi * (n + 1) / N)
        y2 = r * math.sin(2 * math.pi * (n + 1) / N)
        # draw cone
        f = [[0.0, 0.0, height], [x2, y2, 0.0], [x1, y1, 0.0]]
        vList.extend(f)
        f = numpy.array(f)
        norm = numpy.cross(f[2] - f[0], f[1] - f[0])
        normList.append(norm)
        # draw circle
        f = [[0.0, 0.0, 0.0], [x1, y1, 0.0], [x2, y2, 0.0]]
        vList.extend(f)
        f = numpy.array(f)
        norm = numpy.cross(f[2] - f[0], f[1] - f[0])
        normList.append(norm)

    vList = numpy.array(vList)
    normList = numpy.array(normList)

    # change vertex to new orientation
    for i in xrange(len(vList)):
        vList[i] = numpy.array(initialOrintation.rotateVec(vList[i]))

    # add offset
    if len(args) > 2:
        vList += numpy.array(args[2], dtype=float)
    triVert = numpy.array(range(len(vList)))
    triVert = triVert.reshape([len(vList) / 3, 3])
    if 'joint' in kwargs:
        rect = TriModel(vList,
                        triVert,
                        kwargs['joint'],
                        normalVectors=normList,
                        **kwargs)
    else:
        rect = TriModel(vList, triVert, normalVectors=normList, **kwargs)
    return rect
Ejemplo n.º 30
0
def quaternionForAxisAngle(angle, axis):
    return quat().fromAngleAxis(angle, axis)
Ejemplo n.º 31
0
    def __init__(self, *args, **kwargs):
        '''
        Function Signatures:
        Joint()
        Joint(location, ...)

        Arguments {default value}:

        location
            array like object of form [x,y,z] containing x, y, and z coordinates
            of this Joint. { [0,0,0] }

        Keywords:
        parentJoint
            Joint object of which this Joint is a child. {None}
        models
            TriModel object of list of TriModel objects that represent bones
            that are attached to this joint. { [] }
        name
            Name of joint
        initialOrientation
            quaternion (type cgkit.cgtypes.quat) representing the initial
            orientation. { quat(1,0,0,0) }
        showAxis
            Boolean that determines whether or not a 3D representation of the
            Joint is visible. { False }
        axisScale
            Number that determines the size of the drawn axis. Must be greater
            than 0. { 1.0 }
        '''
#        self.translateMat = cgtypes.mat4.identity()
        self.scaleMat = numpy.matrix(numpy.identity(4))
        self.rotateMat = numpy.matrix(numpy.identity(4))

        if len(args) > 0:
            self.location = numpy.array(args[0], dtype=float)
        else:
            self.location = numpy.array([0.0, 0.0, 0.0], dtype=float)
        self.initialLocationMat = numpy.matrix([[1.0, 0.0, 0.0, self.location[0]],
                                                [0.0, 1.0, 0.0, self.location[1]],
                                                [0.0, 0.0, 1.0, self.location[2]],
                                                [0.0, 0.0, 0.0, 1.0]])
        self.translateMat = numpy.matrix([[1.0, 0.0, 0.0, self.location[0]],
                                          [0.0, 1.0, 0.0, self.location[1]],
                                          [0.0, 0.0, 1.0, self.location[2]],
                                          [0.0, 0.0, 0.0, 1.0]])
        self.transformICP = numpy.matrix(numpy.identity(4))

        self.childJoints = []
#        self.scale = 1.0
        self.locationUnityScale = self.location.copy()
        self.type = 'ball'
        self.length = 10.0
        self.proximodistalVec = numpy.array([1.0, 0.0, 0.0])
        self.secondaryVec = numpy.array([0.0, 1.0, 0.0])
        self.tertiaryVec = numpy.array([0.0, 0.0, 1.0])
        self.proximodistalVecTransformed = numpy.array([1.0, 0.0, 0.0])
        self.secondaryVecTransformed = numpy.array([0.0, 1.0, 0.0])
        self.tertiaryVecTransformed = numpy.array([0.0, 0.0, 1.0])
        self.proximodistalVecScaled = self.length * self.proximodistalVec
        self.proximodistalVecTransformedScaled = self.length * self.proximodistalVecTransformed
        self.DOFvec = numpy.array([0, 0, 10.0])
        self.DOFangle = math.radians(45.0)
        self.DOFtrans = 5.0

        if 'parentJoint' in kwargs and isinstance(kwargs['parentJoint'], Joint):
            self.parentJoint = kwargs['parentJoint']
            self.parentJoint.childJoints.append(self)
        else:
            self.parentJoint = None

        self.models = []
        if 'models' in kwargs:
            try:
                for model in kwargs['models']:
                    if isinstance(model, TriModel):
                        self.models.append(model)
                        self.models[-1].setJoint(self)
            except TypeError:
                if isinstance(kwargs['models'], TriModel):
                    self.models.append(kwargs['models'])
                    self.models[-1].setJoint(self)

        if 'initialOrientation' in kwargs and isinstance(kwargs['initialOrientation'], quat):
            self.orientation = kwargs['initialOrientation']
        else:
            self.orientation = quat(1, 0, 0, 0)
        self.xAngle = 0.0
        self.yAngle = 0.0
        self.zAngle = 0.0

        if 'showAxis' in kwargs and isinstance(kwargs['showAxis'], bool):
            self.showAxis = kwargs['showAxis']
        else:
            self.showAxis = False
        if 'axisScale' in kwargs and kwargs['axisScale'] > 0.0:
            self.axisScale = kwargs['axisScale']
        else:
            self.axisScale = 1.0
        if 'name' in kwargs:
            self.name = kwargs['name']
        else:
            self.name = 'Joint'

        for model in self.models:
            model.initialRotationCenter = self.location.copy()

        if self.parentJoint is None:
            self.initalLocationRelativeToParentJoint = self.location.copy()
            self.initialRelativeOrientationFromParent = self.orientation * quat(1, 0, 0, 0).inverse()
        else:
            self.initalLocationRelativeToParentJoint = self.location - self.parentJoint.location
            self.initialRelativeOrientationFromParent = self.orientation * self.parentJoint.orientation.inverse()
        self.relativeOrientationFromParent = quat(self.initialRelativeOrientationFromParent)

        self.createAxis(self.showAxis)
Ejemplo n.º 32
0
    def rotate(self, *args, **kwargs):
        '''
        Function Signatures:
        rotate(q, ...)
        rotate(mat, ...)
        rotate(angle, axis, ...)
        rotate(xAngle, yAngle, zAngle, ...)
        rotate(elevation, azimuth, spin, sphericalCoord=True, ...)

        Arguments {default value}:
        q
            quaternion (cgkit.cgtypes.quat) that defines joint orientation
            (relative or absolute)
        mat
            4x4 rotation matrix (cgkit.cgtypes.mat4) that defines joint orientation
        angle
            angle (degrees or radians, radians default) which to rotate around
            axis
        axis
            vector [i,j,k] which defines axis to rotate around
        xAngle
            angle (degrees or radians, radians default) which to rotate around
            x axis
        yAngle
            angle (degrees or radians, radians default) which to rotate around
            y axis
        zAngle
            angle (degrees or radians, radians default) which to rotate around
            z axis
        elevation
            elevation angle (spherical coordinate system)
        azimuth
            azimuth angle (spherical coordinate system)
        spin
            spin angle around vector defined by elevation and azimuth

        Keywords:
        relative
            defines whether the change in orientation is relative or absolute
            {False}
        updateModels
            flag that determines if child models should be updated {True}
        angleOrder
            string that determines the order that Euler angle rotations are
            applied. {'xyz'}
        unitsDegrees
            flag that indicates what units the passed angles are in, degrees or
            radians. {False}
        sphericalCoord
            flag that indicates passed angles are spherical coordinates. In the
            spherical coordinate system, elevation rotates around the x axis
            first, then azimuth rotates around the y axis, finally spin rotates
            around the vector created by elevation and azimuth {False}
        '''

        if 'relative' in kwargs:
            relative = kwargs['relative']
        else:
            relative = False
        if 'updateModels' in kwargs:
            updateModels = kwargs['updateModels']
        else:
            updateModels = True
        if 'sphericalCoord' in kwargs:
            sphericalCoord = kwargs['sphericalCoord']
        else:
            sphericalCoord = False

        # the baseOrientation of the joint is either its current orientation, relativeAngle=True
        # or the baseOrientation is the initial relative orientation from the parent joint, relativeAngle=False
        if relative:
            baseOrientation = quat(self.orientation)
        # change by original orientation difference, to regain original orientation, relative to parent
        elif self.parentJoint is None:
            baseOrientation = self.initialRelativeOrientationFromParent * quat(1, 0, 0, 0)
        else:
            baseOrientation = self.initialRelativeOrientationFromParent * self.parentJoint.orientation

        if len(args) == 1:  # Quaternion rotation
            if isinstance(args[0], quat):
                rotateMat = args[0].toMat4()
            else:
                rotateMat = args[0]
        elif len(args) == 2:  # angle and axis rotation
            angle = args[0]
            axis = args[1]

            # convert angle units to radians if required
            if 'unitsDegrees' in kwargs and kwargs['unitsDegrees']:
                angle = math.radians(angle)
            angle = NormalizeAngleRad(angle)

            # create rotate matrix
            rotateMat = numpyTransform.rotation(angle, axis, N=4)

        elif len(args) == 3:  # Euler angle rotation
            xAngle = args[0]
            yAngle = args[1]
            zAngle = args[2]
            if 'angleOrder' in kwargs and not sphericalCoord:
                angleOrder = kwargs['angleOrder'].lower()
                if len(angleOrder) != 3 or angleOrder.find('x') < 0 or angleOrder.find('y') < 0 or angleOrder.find('z') < 0:
                    raise Exception('invalid angle order string')
            else:
                angleOrder = 'xyz'
            if 'unitsDegrees' in kwargs and kwargs['unitsDegrees']:
                xAngle = math.radians(xAngle)
                yAngle = math.radians(yAngle)
                zAngle = math.radians(zAngle)
            if 'relative' in kwargs and kwargs['relative']:
                self.xAngle += xAngle
                self.yAngle += yAngle
                self.zAngle += zAngle
            else:
                self.xAngle = xAngle
                self.yAngle = yAngle
                self.zAngle = zAngle
            if sphericalCoord:
                self.xAngle = NormalizeAngleRad(self.xAngle, -math.pi / 2, math.pi / 2, math.pi)
                self.yAngle = NormalizeAngleRad(self.yAngle)
                self.zAngle = NormalizeAngleRad(self.zAngle)
            else:
                self.xAngle = NormalizeAngleRad(self.xAngle)
                self.yAngle = NormalizeAngleRad(self.yAngle)
                self.zAngle = NormalizeAngleRad(self.zAngle)

            rotateMat = numpy.matrix(numpy.identity(4))
            # create orientation quaternion by multiplying rotations around local
            # x,y, and z axis
            # TODO: maybe flip the order of this rotation application?
            # FIXME: Spherical rotation not working
            for i in xrange(3):
                if angleOrder[i] == 'x':
                    rotateMat *= numpyTransform.rotation(self.xAngle, [1.0, 0.0, 0.0], N=4)
                if angleOrder[i] == 'y':
                    rotateMat *= numpyTransform.rotation(self.yAngle, [0.0, 1.0, 0.0], N=4)
                if angleOrder[i] == 'z':
                    rotateMat *= numpyTransform.rotation(self.zAngle, [0.0, 0.0, 1.0], N=4)

        else:  # invalid signature
            raise Exception("Invalid Function Signature")

        if relative:
            self.rotateMat *= rotateMat
        else:
            self.rotateMat = rotateMat
Ejemplo n.º 33
0
 def get_quaternion_from_matrix(matrix):
     q = quat()
     mat = mat3([num for row in matrix for num in row])
     mat = mat.decompose()[0]
     q.fromMat(mat)
     return q
Ejemplo n.º 34
0
import os

import cgkit.cgtypes as cgtypes

import fsee
import fsee.Observer
import fsee.plot_utils
import pylab
import sys

pos_vec3 = cgtypes.vec3( 600, 400, 200)
ori_quat = cgtypes.quat( 1, 0, 0, 0)

model_path = os.path.join(fsee.data_dir,"models/mamarama_checkerboard/mamarama_checkerboard.osg")
optics = 'buchner71'
vision = fsee.Observer.Observer(model_path=model_path,
                                scale=1000.0, # convert model from meters to mm
                                hz=200.0,
                                skybox_basename=None,
                                full_spectrum=True,
                                optics=optics,
                                do_luminance_adaptation=False,
                                )
vision.step(pos_vec3,ori_quat)
#vision.save_last_environment_map('simple_save_envmap.png')

R=vision.get_last_retinal_imageR()
G=vision.get_last_retinal_imageG()
B=vision.get_last_retinal_imageB()
emds = vision.get_last_emd_outputs()
for fname in ['optics.png',
Ejemplo n.º 35
0
 def __init__(self):
     self.cameraQuat = quat(1)  # identity quaternion
     self.camLoc = numpy.array([0.0, 0.0, 2.0])
     self.camFocus = numpy.array([0.0, 0.0, 0.0])
     self.camUp = numpy.array([0.0, 1.0, 0.0])
Ejemplo n.º 36
0
    def setView(self, *args, **kwargs):
        '''
        function signatures:
        setView(self, vMin, vMax, view='ortho1')
        setView(self, orientation, position, focusPoint):
        '''
        if isinstance(args[0], quat) and len(args) >= 3:
            self.cameraQuat = args[0]
            self.camLoc = numpy.array(args[1])
            self.camFocus = numpy.array(args[2])
            self.camUp = numpy.array(self.cameraQuat.rotateVec([0.0, 1.0, 0.0]))
        elif len(args) >= 2:
            vMin = numpy.array(args[0])
            vMax = numpy.array(args[1])
            view = 'ortho1'
            if 'view' in kwargs:
                view = kwargs['view']

            yAngle = None
            xAngle = None
            diameter = 2.0 * numpy.sqrt(numpy.sum((vMax - vMin) ** 2))
            self.camFocus = (vMax + vMin) / 2.0
            if view == 'top':
                yAngle = 0.0
                xAngle = -0.5 * math.pi
            elif view == 'bottom':
                yAngle = 0.0
                xAngle = 0.5 * math.pi
            elif view == 'right':
                yAngle = 0.5 * math.pi
                xAngle = 0.0
            elif view == 'left':
                yAngle = -0.5 * math.pi
                xAngle = 0.0
            elif view == 'front':
                yAngle = 0.0
                xAngle = 0.0
            elif view == 'back':
                yAngle = math.pi
                xAngle = 0.0
            elif view == 'ortho1':
                yAngle = 0.25 * math.pi
                xAngle = -0.25 * math.pi
            elif view == 'ortho2':
                yAngle = 0.75 * math.pi
                xAngle = -0.25 * math.pi
            elif view == 'ortho3':
                yAngle = 1.25 * math.pi
                xAngle = -0.25 * math.pi
            elif view == 'ortho4':
                yAngle = 1.75 * math.pi
                xAngle = -0.25 * math.pi
            elif view == 'ortho5':
                yAngle = 0.75 * math.pi
                xAngle = 0.25 * math.pi
            elif view == 'ortho6':
                yAngle = 1.25 * math.pi
                xAngle = 0.25 * math.pi
            elif view == 'ortho7':
                yAngle = 1.75 * math.pi
                xAngle = 0.25 * math.pi
            elif view == 'ortho8':
                yAngle = 0.25 * math.pi
                xAngle = 0.25 * math.pi

            if yAngle is not None and xAngle is not None:
                # create quat as rotation around y then around (local)x
                startQuat = quat(yAngle, [0, 1, 0])
#                startQuat = quat(numpy.pi, startQuat.rotateVec([0,1,0]))*startQuat
                self.cameraQuat = quat(xAngle, startQuat.rotateVec([1, 0, 0])) * startQuat
                self.camLoc = self.cameraQuat.rotateVec([0.0, 0.0, diameter])
                self.camLoc += self.camFocus
                self.camUp = numpy.array(self.cameraQuat.rotateVec([0.0, 1.0, 0.0]))
Ejemplo n.º 37
0
def quaternionForAxisAngle( angle, axis ):
    return quat().fromAngleAxis( angle, axis )
Ejemplo n.º 38
0
    def addBP(self, bp, parent=None, joint_end=None):
        """Recursively add BodyParts to the simulation.

        bp -- current BodyPart to process
        parent -- parent BP to add to
        joint_end -- which end of the parent ccylinder we should add to"""

        log.debug('addBP')
        body = ode.Body(self.world)
        mass = ode.Mass()
        if not parent:
            # if this is the root we have an absolute length,
            # root scale is relative to midpoint of min..max
            bp.length = bp.scale * (
                bpg.BP_MIN_LENGTH +
                (bpg.BP_MAX_LENGTH - bpg.BP_MIN_LENGTH) / 2)
            bp.isRoot = 1
        else:
            # otherwise child scale is relative to parent
            bp.length = parent.length * bp.scale
            bp.isRoot = 0
    # limit the bp length
        bp.length = min(bp.length, bpg.BP_MAX_LENGTH)
        # mass along x axis, with length without caps==bp.length
        # arg 3 means aligned along z-axis - must be same in renderer and Geoms
        mass.setCappedCylinder(CYLINDER_DENSITY, 3, CYLINDER_RADIUS, bp.length)
        # attach mass to body
        body.setMass(mass)
        # create Geom
        # aligned along z-axis by default!!
        geom = ode.GeomCCylinder(self.space, CYLINDER_RADIUS, bp.length)
        self.geoms.append(geom)
        self.geom_contact[geom] = 0
        # remember parent for collison detection
        if not parent:
            geom.parent = None
        else:
            geom.parent = parent.geom
    # attach geom to body
        geom.setBody(body)
        log.debug('created CappedCylinder(radius=%f, len=%f)', CYLINDER_RADIUS,
                  bp.length)
        # assert(not in a loop)
        assert not hasattr(bp, 'geom')
        # ref geom from bodypart (used above to find parent geom)
        bp.geom = geom

        # set rotation
        (radians, v) = bp.rotation
        log.debug('radians,v = %f,%s', radians, str(v))
        q = quat(radians, vec3(v))
        rotmat = q.toMat3()
        if parent:
            # rotate relative to parent
            p_r = mat3(parent.geom.getRotation())  # joint_end *
            log.debug('parent rotation = %s', str(p_r))
            rotmat = p_r * rotmat
        geom.setRotation(rotmat.toList(rowmajor=1))
        log.debug('r=%s', str(rotmat))
        geom_axis = rotmat * vec3(0, 0, 1)
        log.debug('set geom axis to %s', str(geom_axis))
        (x, y, z) = geom.getBody().getRelPointPos((0, 0, bp.length / 2.0))
        log.debug('real position of joint is %f,%f,%f', x, y, z)
        # set position
        if not parent:
            # root  - initially located at 0,0,0
            # (once the model is constructed we translate it until all
            # bodies have z>0)
            geom.setPosition((0, 0, 0))
            log.debug('set root geom x,y,z = 0,0,0')
        else:
            # child - located relative to the parent. from the
            # parents position move along their axis of orientation to
            # the joint position, then pick a random angle within the
            # joint limits, move along that vector by half the length
            # of the child cylinder, and we have the position of the
            # child.
            # vector from parent xyz is half of parent length along
            # +/- x axis rotated by r
            p_v = vec3(parent.geom.getPosition())
            p_r = mat3(parent.geom.getRotation())
            p_hl = parent.geom.getParams()[1] / 2.0  # half len of par
            j_v = p_v + p_r * vec3(0, 0, p_hl * joint_end)  # joint vector
            # rotation is relative to parent
            c_v = j_v + rotmat * vec3(0, 0, bp.length / 2.0)
            geom.setPosition(tuple(c_v))
            log.debug('set geom x,y,z = %f,%f,%f', c_v[0], c_v[1], c_v[2])

            jointclass = {
                'hinge': ode.HingeJoint,
                'universal': ode.UniversalJoint,
                'ball': ode.BallJoint
            }
            j = jointclass[bp.joint](self.world)

            self.joints.append(j)
            # attach bodies to joint
            j.attach(parent.geom.getBody(), body)
            # set joint position
            j.setAnchor(j_v)
            geom.parent_joint = j

            # create motor and attach to this geom
            motor = Motor(self.world, None)
            motor.setJoint(j)
            self.joints.append(motor)
            bp.motor = motor
            geom.motor = motor
            motor.attach(parent.geom.getBody(), body)
            motor.setMode(ode.AMotorEuler)

            if bp.joint == 'hinge':
                # we have 3 points - parent body, joint, child body
                # find the normal to these points
                # (hinge only has 1 valid axis!)
                try:
                    axis1 = ((j_v - p_v).cross(j_v - c_v)).normalize()
                except ZeroDivisionError:
                    v = (j_v - p_v).cross(j_v - c_v)
                    v.z = 1**-10
                    axis1 = v.normalize()
                log.debug('setting hinge joint axis to %s', axis1)
                log.debug('hinge axis = %s', j.getAxis())
                axis_inv = rotmat.inverse() * axis1
                axis2 = vec3((0, 0, 1)).cross(axis_inv)
                log.debug('hinge axis2 = %s', axis2)
                j.setAxis(tuple(axis1))
                # some anomaly here.. if we change the order of axis2 and axis1,
                # it should make no difference. instead there appears to be an
                # instability when the angle switches from -pi to +pi
                # so.. use parameter3 to control the hinge
                # (maybe this is only a problem in the test case with perfect axis alignment?)
                motor.setAxes(axis2, rotmat * axis1)
            elif bp.joint == 'universal':
                # bp.axis1/2 is relative to bp rotation, so rotate axes
                axis1 = rotmat * vec3(bp.axis1)
                axis2 = rotmat * vec3((0, 0, 1)).cross(vec3(bp.axis1))
                j.setAxis1(tuple(axis1))
                j.setAxis2(tuple(axis2))
                motor.setAxes(axis1, axis2)
            elif bp.joint == 'ball':
                axis1 = rotmat * vec3(bp.axis1)
                axis2 = rotmat * vec3((0, 0, 1)).cross(vec3(bp.axis1))
                motor.setAxes(axis1, axis2)

            log.debug('created joint with parent at %f,%f,%f', j_v[0], j_v[1],
                      j_v[2])

    # recurse on children
        geom.child_joint_ends = set([e.joint_end for e in bp.edges])
        geom.parent_joint_end = joint_end
        if joint_end == None:
            # root
            if -1 in geom.child_joint_ends:
                geom.left = 'internal'
            else:
                geom.left = 'external'
            if 1 in geom.child_joint_ends:
                geom.right = 'internal'
            else:
                geom.right = 'external'
        else:
            # not root
            geom.left = 'internal'
            if 1 in geom.child_joint_ends:
                geom.right = 'internal'
            else:
                geom.right = 'external'

        for e in bp.edges:
            self.addBP(e.child, bp, e.joint_end)
Ejemplo n.º 39
0
    data_scale = 1
    xgain *= data_scale
    ygain *= data_scale

    X = (X - xoffset) * xgain
    Y = (Y - yoffset) * ygain

    if 0:
        import pylab
        pylab.plot(X[:, 0], Y[:, 0], '.')
        pylab.show()
        #sys.exit(0)

    if 0:
        pos_vec3, ori_quat = cgtypes.vec3(nums[0:3]), cgtypes.quat(nums[3:7])
        M = cgtypes.mat4().identity().translate(pos_vec3)
        M = M * ori_quat.toMat4()
        # grr, I hate cgtypes to numpy conversions!
        M = np.array((M[0, 0], M[1, 0], M[2, 0], M[3, 0], M[0, 1], M[1, 1],
                      M[2, 1], M[3, 1], M[0, 2], M[1, 2], M[2, 2], M[3, 2],
                      M[0, 3], M[1, 3], M[2, 3], M[3, 3]))
        M.shape = (4, 4)

    fly_model_node_filename = os.path.join(fsee.data_dir,
                                           'models/fly/body.osg')
    model_path = os.path.join(fsee.data_dir,
                              "models/alice_cylinder/alice_cylinder.osg")
    model_path = os.path.join(fsee.data_dir,
                              "models/alice_cylinder/white_cylinder.osg")
    #model_path = None
Ejemplo n.º 40
0
 def get_quaternion_from_matrix(matrix):
     q = quat()
     mat = mat3([num for row in matrix for num in row])
     mat = mat.decompose()[0]
     q.fromMat(mat)
     return q
Ejemplo n.º 41
0
def run():
    f_fts = open(sys.argv[1], 'r')
    f_tfs = open(sys.argv[2], 'r')
    fn_output = sys.argv[3]

    DL = '\t'

    tfs = []
    tfs_file = ""
    for l in f_tfs:
        s = l.split(DL)
        if len(s) > 0: s[len(s) - 1] = s[len(s) - 1][0:len(s[len(s) - 1]) - 1]
        if len(s) > 0 and s[0] == "tf": tfs.append(s[1:])
        if len(s) > 0 and s[0] == "file": tfs_file = s[1]

    fts_file = ""
    T = None
    keypoints = []
    KPstart = 0
    frames = 0
    took = {}
    for l in f_fts:
        s = l.split(DL)
        if len(s) > 0: s[len(s) - 1] = s[len(s) - 1][0:len(s[len(s) - 1]) - 1]

        if len(s) > 0 and s[0] == "file": fts_file = s[1]
        if len(s) > 0 and s[0] == "header":
            T = None
            ts = float(s[1])
            best = None
            mi = 0.075  #minimum
            for t in tfs:
                if abs(float(t[0]) - ts) < mi:
                    mi = abs(float(t[0]) - ts)
                    best = t
            if best == None:
                #print "skipping frame as no valid tf was found"
                continue
            #print mi

            T = [
                vec4(float(best[1]), float(best[2]), float(best[3]), 1.),
                quat(float(v) for v in best[4:])
            ]
            KPstart = len(keypoints)
            KPtype = ""
            frames += 1
        if len(s) > 0 and s[0] == "took":
            if not s[1] in took: took[s[1]] = []
            took[s[1]].append(reduce(lambda x, y: float(x) + float(y), s[2:]))

        if len(s) == 4 and s[0] == "keypoint" and T != None:
            pt = vec4(float(s[1]), float(s[2]), float(s[3]), 1.)
            TT = T[1].toMat4().setColumn(3, T[0])
            #pt = TT.inverse()*pt
            pt = TT * pt
            keypoints.append({"pt": pt})
            #print pt
        if len(s) > 4 and s[0] == "keypoint" and T != None:
            if KPtype != s[1]:
                KPtype = s[1]
                KP = KPstart
            keypoints[KP][s[1]] = [float(v) for v in s[2:]]
            KP += 1

    print "read input file ", len(keypoints)

    f_out = file(fn_output + "_timing.csv", "w")
    i = 1
    for ft in took:
        aa, bb, cc = mquantiles(took[ft])
        f_out.write(str(i) + "\t")
        f_out.write(str(min(took[ft])) + "\t")
        f_out.write(str(aa) + "\t")
        f_out.write(str(bb) + "\t")
        f_out.write(str(cc) + "\t")
        f_out.write(str(max(took[ft])) + "\t")
        f_out.write(
            str(reduce(lambda x, y: x + y, took[ft]) / len(took[ft])) + "\t")
        f_out.write(ft)
        f_out.write("\n")
        i += 1
    f_out.close()
    print "written timing output"

    borders = [x / 20. for x in range(1, 31)]
    avg = {}
    fts = []
    for i in xrange(len(keypoints)):
        if len(keypoints) > 1000:
            if (i) % (len(keypoints) / 1000) == 0:
                print "#",
                sys.stdout.flush()
        #if i==3: exit()

        kp1 = keypoints[i]
        for j in range(i + 1, len(keypoints)):
            kp2 = keypoints[j]

            L = (kp1["pt"] - kp2["pt"]).length()
            #print L,"\t",
            for ft in kp1:
                if ft == "pt" or not ft in kp2: continue
                #D=dist(kp1[ft], kp2[ft], ft)
                #if D==0:
                #	#print i,j, kp1["pt"], kp2["pt"]
                #	if i+1==j: continue
                #print D,"\t",
                if not ft in avg:
                    avg[ft] = 0.
                    avg[ft + "num"] = 0
                    avg[ft + "b"] = 0.
                    avg[ft + "bnum"] = 0
                    #avg[ft+"ar"]=[]
                    #avg[ft+"bar"]=[]
                    avg[ft + "border"] = [[] for x in xrange(len(borders))]
                    #avg[ft+"bborder"]=[[] for x in xrange(len(borders))]
                    fts.append(ft)
                #p=""
                #if L>0.7/3: p="b"
                #avg[ft+p]+=D
                #avg[ft+p+"num"]+=1
                #avg[ft+p+"ar"].append(D)
                D = False
                for b in range(int(L / 0.05),
                               len(borders)):  #for b in xrange(len(borders)):
                    pp = ""
                    if L > borders[b]: continue  #pp="b"
                    if D == False: D = dist(kp1[ft], kp2[ft], ft)
                    avg[ft + pp + "border"][b].append(D)
            #print ""

    print "frames ", frames
    for ft in fts:
        #if avg[ft+"bnum"]>0 and avg[ft+"bnum"] and avg[ft]>0:
        #	print ft, avg[ft+"num"], avg[ft+"bnum"], avg[ft]/avg[ft+"num"], "  ", avg[ft+"b"]/avg[ft+"bnum"], "   ->  ", (avg[ft+"b"]/avg[ft+"bnum"])/(avg[ft]/avg[ft+"num"])
        #else:
        #	print ft," is emtpy"
        print ft
        #print min(avg[ft+"ar"]),  mquantiles(avg[ft+"ar"]),  max(avg[ft+"ar"])
        #print min(avg[ft+"bar"]), mquantiles(avg[ft+"bar"]), max(avg[ft+"bar"])
        f_out = file(fn_output + "_" + ft + ".csv", "w")
        aa, bb, cc = mquantiles(avg[ft + "border"][len(borders) - 1])
        FACT = 1 / bb
        for pp in [""]:  #["", "b"]:
            for b in xrange(len(borders)):
                if len(avg[ft + pp + "border"][b]) < 1: continue
                f_out.write(str(borders[b]) + "\t")
                aa, bb, cc = mquantiles(avg[ft + pp + "border"][b])
                f_out.write(str(min(avg[ft + pp + "border"][b]) * FACT) + "\t")
                f_out.write(str(aa * FACT) + "\t")
                f_out.write(str(bb * FACT) + "\t")
                f_out.write(str(cc * FACT) + "\t")
                f_out.write(str(max(avg[ft + pp + "border"][b]) * FACT) + "\t")
                f_out.write(
                    str(
                        reduce(lambda x, y: x + y, avg[ft + pp + "border"][b])
                        / len(avg[ft + pp + "border"][b]) * FACT))
                f_out.write("\n")
            #print ""
        f_out.close()
    assert (tfs_file == fts_file)
def run():
    f_fts = open(sys.argv[1], 'r')
    f_tfs = open(sys.argv[2], 'r')
    fn_output = sys.argv[3]

    DL = '\t'

    tfs = []
    tfs_file = ""
    for l in f_tfs:
        s = l.split(DL)
        if len(s) > 0: s[len(s) - 1] = s[len(s) - 1][0:len(s[len(s) - 1]) - 1]
        if len(s) > 0 and s[0] == "tf": tfs.append(s[1:])
        if len(s) > 0 and s[0] == "file": tfs_file = s[1]

    fts_file = ""
    T = None
    frames = []
    took = {}
    radius = 0
    for l in f_fts:
        s = l.split(DL)
        if len(s) > 0: s[len(s) - 1] = s[len(s) - 1][0:len(s[len(s) - 1]) - 1]

        if len(s) > 0 and s[0] == "file": fts_file = s[1]
        if len(s) > 0 and s[0] == "header":
            T = None
            ts = float(s[1])
            radius = None
            best = None
            mi = 0.075  #minimum
            for t in tfs:
                if abs(float(t[0]) - ts) < mi:
                    mi = abs(float(t[0]) - ts)
                    best = t
            if best == None:
                #print "skipping frame as no valid tf was found"
                continue

            T = [
                vec4(float(best[1]), float(best[2]), float(best[3]), 1.),
                quat(float(v) for v in best[4:])
            ]
            frames.append({})
        elif radius == None:
            radius = float(s[1])

        if len(s) > 0 and s[0] == "took":
            name = '_'.join(s[1:4])
            if not name in took: took[name] = []
            if not name in frames[len(frames) - 1]:
                frames[len(frames) - 1][name] = []
            took[name].append(
                float(reduce(lambda x, y: float(x) + float(y), s[4:])))

        if len(s) == 7 and s[0] == "eval_keypoint" and T != None:
            name = '_'.join(s[1:4])
            pt = vec4(float(s[4]), float(s[5]), float(s[6]), 1.)
            TT = T[1].toMat4().setColumn(3, T[0])
            #pt = TT.inverse()*pt
            pt = TT * pt
            frames[len(frames) - 1][name].append({"pt": pt})

    print "read input file "

    f_out = file(fn_output + "_timing.csv", "w")
    i = 1
    for ft in took:
        aa, bb, cc = mquantiles(took[ft])
        f_out.write(str(i) + "\t")
        f_out.write(str(min(took[ft])) + "\t")
        f_out.write(str(aa) + "\t")
        f_out.write(str(bb) + "\t")
        f_out.write(str(cc) + "\t")
        f_out.write(str(max(took[ft])) + "\t")
        f_out.write(
            str(reduce(lambda x, y: x + y, took[ft]) / len(took[ft])) + "\t")
        f_out.write(ft)
        f_out.write("\n")
        i += 1
    f_out.close()
    print "written timing output"

    print "frames ", len(frames)

    names = [n for n in frames[0]]

    f_out = file(fn_output + ".csv", "a")
    for name in names:
        print name
        dist = []
        gtp = 0
        gfp = 0
        ges = 0
        for i in xrange(len(frames)):
            ges += len(frames[i][name])
            for ii in xrange(len(frames[i][name])):
                kp1 = frames[i][name][ii]
                for j in range(i + 1, len(frames)):
                    tp = 0
                    fp = 0
                    mi = radius
                    for jj in xrange(len(frames[j][name])):
                        kp2 = frames[j][name][jj]

                        L2 = (kp1["pt"] - kp2["pt"]).length()
                        if L2 <= radius / 4:
                            if tp > 0:
                                fp += 1
                            else:
                                tp += 1
                            mi = min(mi, L2)
                    if tp > 0: dist.append(mi)
                    gtp += tp
                    gfp += fp
        print ""

        aa, bb, cc = mquantiles(dist)

        f_out.write(str(name) + "\t")
        f_out.write(str(gtp) + "\t")
        f_out.write(str(gfp) + "\t")
        f_out.write(str(ges) + "\t")
        f_out.write(str(len(frames)) + "\t")
        f_out.write(str(min(dist)) + "\t")
        f_out.write(str(aa) + "\t")
        f_out.write(str(bb) + "\t")
        f_out.write(str(cc) + "\t")
        f_out.write(str(max(dist)) + "\t")
        f_out.write(str(reduce(lambda x, y: x + y, dist) / len(dist)) + "\t")
        f_out.write(str(max(dist)) + "\n")
        f_out.flush()

    assert (tfs_file == fts_file)
Ejemplo n.º 43
0
def run():
    global num_radii, num_angles
    f_fts = open(sys.argv[1], 'r')
    f_tfs = open(sys.argv[2], 'r')
    fn_output = sys.argv[3]

    DL = '\t'

    tfs = []
    tfs_file = ""
    for l in f_tfs:
        s = l.split(DL)
        if len(s) > 0: s[len(s) - 1] = s[len(s) - 1][0:len(s[len(s) - 1]) - 1]
        if len(s) > 0 and s[0] == "tf": tfs.append(s[1:])
        if len(s) > 0 and s[0] == "file": tfs_file = s[1]

    fts_file = ""
    T = None
    keypoints = []
    KPstart = 0
    frames = 0
    took = {}
    for l in f_fts:
        s = l.split(DL)
        if len(s) > 0: s[len(s) - 1] = s[len(s) - 1][0:len(s[len(s) - 1]) - 1]

        if len(s) > 0 and s[0] == "file": fts_file = s[1]
        if len(s) > 0 and s[0] == "num_radii": num_radii = int(s[1])
        if len(s) > 0 and s[0] == "num_angles": num_angles = int(s[1])
        if len(s) > 0 and s[0] == "header":
            T = None
            ts = float(s[1])
            best = None
            mi = 0.075  #minimum
            for t in tfs:
                if abs(float(t[0]) - ts) < mi:
                    mi = abs(float(t[0]) - ts)
                    best = t
            if best == None:
                #print "skipping frame as no valid tf was found"
                continue
            #print mi

            T = [
                vec4(float(best[1]), float(best[2]), float(best[3]), 1.),
                quat(float(v) for v in best[4:])
            ]
            KPstart = len(keypoints)
            KPtype = ""
            frames += 1
        if len(s) > 0 and s[0] == "took":
            if not s[1] in took: took[s[1]] = []
            took[s[1]].append(reduce(lambda x, y: float(x) + float(y), s[2:]))

        if len(s) == 4 and s[0] == "keypoint" and T != None:
            pt = vec4(float(s[1]), float(s[2]), float(s[3]), 1.)
            TT = T[1].toMat4().setColumn(3, T[0])
            #pt = TT.inverse()*pt
            pt = TT * pt
            keypoints.append({"pt": pt})
            #print pt
        if len(s) > 4 and s[0] == "keypoint" and T != None:
            if KPtype != s[1]:
                KPtype = s[1]
                KP = KPstart
            keypoints[KP][s[1]] = [float(v) for v in s[2:]]
            KP += 1

    print "read input file ", len(keypoints)

    borders = [x / 20. for x in range(1, 31)]
    avg = {}
    avgDBG = {}
    for i in xrange(len(keypoints)):
        if len(keypoints) > 1000:
            if (i) % (len(keypoints) / 1000) == 0:
                print "#",
                sys.stdout.flush()
                if i > 10: break
        #if i==3: exit()

        fts = []
        md = {}
        mL = {}
        mF = {}
        mdDBG = {}
        mLDBG = {}
        mFDBG = {}
        kp1 = keypoints[i]
        NNNN = 0
        for j in range(i + 1, len(keypoints)):
            kp2 = keypoints[j]

            L = (kp1["pt"] - kp2["pt"]).length()
            #print L,"\t",
            for ft in kp1:
                if ft == "pt" or not ft in kp2: continue
                d = dist(kp1[ft], kp2[ft], ft)

                if not ft in md:
                    md[ft] = None
                    mL[ft] = None
                    mdDBG[ft] = None
                    mLDBG[ft] = None
                    fts.append(ft)
                if md[ft] == None or d < md[ft]:
                    md[ft] = d
                    mL[ft] = L
                    mF[ft] = [kp1[ft], kp2[ft]]
                if mdDBG[ft] == None or L < mLDBG[ft]:
                    mdDBG[ft] = d
                    mLDBG[ft] = L
                    mFDBG[ft] = [kp1[ft], kp2[ft]]
            '''for ft in kp1:
				if ft=="pt" or not ft in kp2: continue
				d=dist(kp1[ft], kp2[ft], ft)
				if d<=mdDBG[ft]:
					NNNN+=1
					print L'''
        for ft in fts:
            if md[ft] == None: continue

            if not ft in avg:
                avg[ft] = []
                avgDBG[ft] = []

            avg[ft].append([md[ft], mL[ft], mF[ft]])
            avgDBG[ft].append([mdDBG[ft], mLDBG[ft], mFDBG[ft]])

            print ft, NNNN
            print md[ft], mL[ft]
            print mdDBG[ft], mLDBG[ft]
            print mFDBG[ft][0]
            print mFDBG[ft][1]
            print mF[ft][1]

    print "frames ", frames
    print avg
    print avgDBG

    for b in xrange(len(borders)):
        for ft in avg:
            f1 = file(fn_output + "_" + ft + "_pr_" + str(borders[b]) + ".csv",
                      "w")
            R = calc_pr(avg[ft], borders[b])
            for r in R:
                f1.write(str(r[0]))
                for i in range(1, len(r)):
                    f1.write("\t" + str(r[i]))
                f1.write("\n")
            f1.close()

    assert (tfs_file == fts_file)
Ejemplo n.º 44
0
 def __init__(self):
     self.cameraQuat = quat(1)  # identity quaternion
     self.camLoc = numpy.array([0.0, 0.0, 2.0])
     self.camFocus = numpy.array([0.0, 0.0, 0.0])
     self.camUp = numpy.array([0.0, 1.0, 0.0])
Ejemplo n.º 45
0
    def setView(self, *args, **kwargs):
        '''
        function signatures:
        setView(self, vMin, vMax, view='ortho1')
        setView(self, orientation, position, focusPoint):
        '''
        if isinstance(args[0], quat) and len(args) >= 3:
            self.cameraQuat = args[0]
            self.camLoc = numpy.array(args[1])
            self.camFocus = numpy.array(args[2])
            self.camUp = numpy.array(self.cameraQuat.rotateVec([0.0, 1.0,
                                                                0.0]))
        elif len(args) >= 2:
            vMin = numpy.array(args[0])
            vMax = numpy.array(args[1])
            view = 'ortho1'
            if 'view' in kwargs:
                view = kwargs['view']

            yAngle = None
            xAngle = None
            diameter = 2.0 * numpy.sqrt(numpy.sum((vMax - vMin)**2))
            self.camFocus = (vMax + vMin) / 2.0
            if view == 'top':
                yAngle = 0.0
                xAngle = -0.5 * math.pi
            elif view == 'bottom':
                yAngle = 0.0
                xAngle = 0.5 * math.pi
            elif view == 'right':
                yAngle = 0.5 * math.pi
                xAngle = 0.0
            elif view == 'left':
                yAngle = -0.5 * math.pi
                xAngle = 0.0
            elif view == 'front':
                yAngle = 0.0
                xAngle = 0.0
            elif view == 'back':
                yAngle = math.pi
                xAngle = 0.0
            elif view == 'ortho1':
                yAngle = 0.25 * math.pi
                xAngle = -0.25 * math.pi
            elif view == 'ortho2':
                yAngle = 0.75 * math.pi
                xAngle = -0.25 * math.pi
            elif view == 'ortho3':
                yAngle = 1.25 * math.pi
                xAngle = -0.25 * math.pi
            elif view == 'ortho4':
                yAngle = 1.75 * math.pi
                xAngle = -0.25 * math.pi
            elif view == 'ortho5':
                yAngle = 0.75 * math.pi
                xAngle = 0.25 * math.pi
            elif view == 'ortho6':
                yAngle = 1.25 * math.pi
                xAngle = 0.25 * math.pi
            elif view == 'ortho7':
                yAngle = 1.75 * math.pi
                xAngle = 0.25 * math.pi
            elif view == 'ortho8':
                yAngle = 0.25 * math.pi
                xAngle = 0.25 * math.pi

            if yAngle is not None and xAngle is not None:
                # create quat as rotation around y then around (local)x
                startQuat = quat(yAngle, [0, 1, 0])
                #                startQuat = quat(numpy.pi, startQuat.rotateVec([0,1,0]))*startQuat
                self.cameraQuat = quat(xAngle, startQuat.rotateVec(
                    [1, 0, 0])) * startQuat
                self.camLoc = self.cameraQuat.rotateVec([0.0, 0.0, diameter])
                self.camLoc += self.camFocus
                self.camUp = numpy.array(
                    self.cameraQuat.rotateVec([0.0, 1.0, 0.0]))
Ejemplo n.º 46
0
    def rotate(self, *args, **kwargs):
        '''
        Function Signatures:
        rotate(q, ...)
        rotate(mat, ...)
        rotate(angle, axis, ...)
        rotate(xAngle, yAngle, zAngle, ...)
        rotate(elevation, azimuth, spin, sphericalCoord=True, ...)

        Arguments {default value}:
        q
            quaternion (cgkit.cgtypes.quat) that defines joint orientation
            (relative or absolute)
        mat
            4x4 rotation matrix (cgkit.cgtypes.mat4) that defines joint orientation
        angle
            angle (degrees or radians, radians default) which to rotate around
            axis
        axis
            vector [i,j,k] which defines axis to rotate around
        xAngle
            angle (degrees or radians, radians default) which to rotate around
            x axis
        yAngle
            angle (degrees or radians, radians default) which to rotate around
            y axis
        zAngle
            angle (degrees or radians, radians default) which to rotate around
            z axis
        elevation
            elevation angle (spherical coordinate system)
        azimuth
            azimuth angle (spherical coordinate system)
        spin
            spin angle around vector defined by elevation and azimuth

        Keywords:
        relative
            defines whether the change in orientation is relative or absolute
            {False}
        updateModels
            flag that determines if child models should be updated {True}
        angleOrder
            string that determines the order that Euler angle rotations are
            applied. {'xyz'}
        unitsDegrees
            flag that indicates what units the passed angles are in, degrees or
            radians. {False}
        sphericalCoord
            flag that indicates passed angles are spherical coordinates. In the
            spherical coordinate system, elevation rotates around the x axis
            first, then azimuth rotates around the y axis, finally spin rotates
            around the vector created by elevation and azimuth {False}
        '''

        if 'relative' in kwargs:
            relative = kwargs['relative']
        else:
            relative = False
        if 'updateModels' in kwargs:
            updateModels = kwargs['updateModels']
        else:
            updateModels = True
        if 'sphericalCoord' in kwargs:
            sphericalCoord = kwargs['sphericalCoord']
        else:
            sphericalCoord = False

        # the baseOrientation of the joint is either its current orientation, relativeAngle=True
        # or the baseOrientation is the initial relative orientation from the parent joint, relativeAngle=False
        if relative:
            baseOrientation = quat(self.orientation)
        # change by original orientation difference, to regain original orientation, relative to parent
        elif self.parentJoint is None:
            baseOrientation = self.initialRelativeOrientationFromParent * quat(
                1, 0, 0, 0)
        else:
            baseOrientation = self.initialRelativeOrientationFromParent * self.parentJoint.orientation

        if len(args) == 1:  # Quaternion rotation
            if isinstance(args[0], quat):
                rotateMat = args[0].toMat4()
            else:
                rotateMat = args[0]
        elif len(args) == 2:  # angle and axis rotation
            angle = args[0]
            axis = args[1]

            # convert angle units to radians if required
            if 'unitsDegrees' in kwargs and kwargs['unitsDegrees']:
                angle = math.radians(angle)
            angle = NormalizeAngleRad(angle)

            # create rotate matrix
            rotateMat = numpyTransform.rotation(angle, axis, N=4)

        elif len(args) == 3:  # Euler angle rotation
            xAngle = args[0]
            yAngle = args[1]
            zAngle = args[2]
            if 'angleOrder' in kwargs and not sphericalCoord:
                angleOrder = kwargs['angleOrder'].lower()
                if len(angleOrder) != 3 or angleOrder.find(
                        'x') < 0 or angleOrder.find(
                            'y') < 0 or angleOrder.find('z') < 0:
                    raise Exception('invalid angle order string')
            else:
                angleOrder = 'xyz'
            if 'unitsDegrees' in kwargs and kwargs['unitsDegrees']:
                xAngle = math.radians(xAngle)
                yAngle = math.radians(yAngle)
                zAngle = math.radians(zAngle)
            if 'relative' in kwargs and kwargs['relative']:
                self.xAngle += xAngle
                self.yAngle += yAngle
                self.zAngle += zAngle
            else:
                self.xAngle = xAngle
                self.yAngle = yAngle
                self.zAngle = zAngle
            if sphericalCoord:
                self.xAngle = NormalizeAngleRad(self.xAngle, -math.pi / 2,
                                                math.pi / 2, math.pi)
                self.yAngle = NormalizeAngleRad(self.yAngle)
                self.zAngle = NormalizeAngleRad(self.zAngle)
            else:
                self.xAngle = NormalizeAngleRad(self.xAngle)
                self.yAngle = NormalizeAngleRad(self.yAngle)
                self.zAngle = NormalizeAngleRad(self.zAngle)

            rotateMat = numpy.matrix(numpy.identity(4))
            # create orientation quaternion by multiplying rotations around local
            # x,y, and z axis
            # TODO: maybe flip the order of this rotation application?
            # FIXME: Spherical rotation not working
            for i in xrange(3):
                if angleOrder[i] == 'x':
                    rotateMat *= numpyTransform.rotation(self.xAngle,
                                                         [1.0, 0.0, 0.0],
                                                         N=4)
                if angleOrder[i] == 'y':
                    rotateMat *= numpyTransform.rotation(self.yAngle,
                                                         [0.0, 1.0, 0.0],
                                                         N=4)
                if angleOrder[i] == 'z':
                    rotateMat *= numpyTransform.rotation(self.zAngle,
                                                         [0.0, 0.0, 1.0],
                                                         N=4)

        else:  # invalid signature
            raise Exception("Invalid Function Signature")

        if relative:
            self.rotateMat *= rotateMat
        else:
            self.rotateMat = rotateMat
Ejemplo n.º 47
0
    def __init__(self, *args, **kwargs):
        '''
        Function Signatures:
        Joint()
        Joint(location, ...)

        Arguments {default value}:

        location
            array like object of form [x,y,z] containing x, y, and z coordinates
            of this Joint. { [0,0,0] }

        Keywords:
        parentJoint
            Joint object of which this Joint is a child. {None}
        models
            TriModel object of list of TriModel objects that represent bones
            that are attached to this joint. { [] }
        name
            Name of joint
        initialOrientation
            quaternion (type cgkit.cgtypes.quat) representing the initial
            orientation. { quat(1,0,0,0) }
        showAxis
            Boolean that determines whether or not a 3D representation of the
            Joint is visible. { False }
        axisScale
            Number that determines the size of the drawn axis. Must be greater
            than 0. { 1.0 }
        '''
        #        self.translateMat = cgtypes.mat4.identity()
        self.scaleMat = numpy.matrix(numpy.identity(4))
        self.rotateMat = numpy.matrix(numpy.identity(4))

        if len(args) > 0:
            self.location = numpy.array(args[0], dtype=float)
        else:
            self.location = numpy.array([0.0, 0.0, 0.0], dtype=float)
        self.initialLocationMat = numpy.matrix(
            [[1.0, 0.0, 0.0, self.location[0]],
             [0.0, 1.0, 0.0, self.location[1]],
             [0.0, 0.0, 1.0, self.location[2]], [0.0, 0.0, 0.0, 1.0]])
        self.translateMat = numpy.matrix([[1.0, 0.0, 0.0, self.location[0]],
                                          [0.0, 1.0, 0.0, self.location[1]],
                                          [0.0, 0.0, 1.0, self.location[2]],
                                          [0.0, 0.0, 0.0, 1.0]])
        self.transformICP = numpy.matrix(numpy.identity(4))

        self.childJoints = []
        #        self.scale = 1.0
        self.locationUnityScale = self.location.copy()
        self.type = 'ball'
        self.length = 10.0
        self.proximodistalVec = numpy.array([1.0, 0.0, 0.0])
        self.secondaryVec = numpy.array([0.0, 1.0, 0.0])
        self.tertiaryVec = numpy.array([0.0, 0.0, 1.0])
        self.proximodistalVecTransformed = numpy.array([1.0, 0.0, 0.0])
        self.secondaryVecTransformed = numpy.array([0.0, 1.0, 0.0])
        self.tertiaryVecTransformed = numpy.array([0.0, 0.0, 1.0])
        self.proximodistalVecScaled = self.length * self.proximodistalVec
        self.proximodistalVecTransformedScaled = self.length * self.proximodistalVecTransformed
        self.DOFvec = numpy.array([0, 0, 10.0])
        self.DOFangle = math.radians(45.0)
        self.DOFtrans = 5.0

        if 'parentJoint' in kwargs and isinstance(kwargs['parentJoint'],
                                                  Joint):
            self.parentJoint = kwargs['parentJoint']
            self.parentJoint.childJoints.append(self)
        else:
            self.parentJoint = None

        self.models = []
        if 'models' in kwargs:
            try:
                for model in kwargs['models']:
                    if isinstance(model, TriModel):
                        self.models.append(model)
                        self.models[-1].setJoint(self)
            except TypeError:
                if isinstance(kwargs['models'], TriModel):
                    self.models.append(kwargs['models'])
                    self.models[-1].setJoint(self)

        if 'initialOrientation' in kwargs and isinstance(
                kwargs['initialOrientation'], quat):
            self.orientation = kwargs['initialOrientation']
        else:
            self.orientation = quat(1, 0, 0, 0)
        self.xAngle = 0.0
        self.yAngle = 0.0
        self.zAngle = 0.0

        if 'showAxis' in kwargs and isinstance(kwargs['showAxis'], bool):
            self.showAxis = kwargs['showAxis']
        else:
            self.showAxis = False
        if 'axisScale' in kwargs and kwargs['axisScale'] > 0.0:
            self.axisScale = kwargs['axisScale']
        else:
            self.axisScale = 1.0
        if 'name' in kwargs:
            self.name = kwargs['name']
        else:
            self.name = 'Joint'

        for model in self.models:
            model.initialRotationCenter = self.location.copy()

        if self.parentJoint is None:
            self.initalLocationRelativeToParentJoint = self.location.copy()
            self.initialRelativeOrientationFromParent = self.orientation * quat(
                1, 0, 0, 0).inverse()
        else:
            self.initalLocationRelativeToParentJoint = self.location - self.parentJoint.location
            self.initialRelativeOrientationFromParent = self.orientation * self.parentJoint.orientation.inverse(
            )
        self.relativeOrientationFromParent = quat(
            self.initialRelativeOrientationFromParent)

        self.createAxis(self.showAxis)
Ejemplo n.º 48
0
    data_scale = 1
    xgain *= data_scale
    ygain *= data_scale

    X=(X-xoffset)*xgain
    Y=(Y-yoffset)*ygain

    if 0:
        import pylab
        pylab.plot(X[:,0],Y[:,0],'.')
        pylab.show()
        #sys.exit(0)

    if 0:
        pos_vec3,ori_quat = cgtypes.vec3(nums[0:3]),cgtypes.quat(nums[3:7])
        M = cgtypes.mat4().identity().translate(pos_vec3)
        M = M*ori_quat.toMat4()
        # grr, I hate cgtypes to numpy conversions!
        M = np.array((M[0,0],M[1,0],M[2,0],M[3,0],
                      M[0,1],M[1,1],M[2,1],M[3,1],
                      M[0,2],M[1,2],M[2,2],M[3,2],
                      M[0,3],M[1,3],M[2,3],M[3,3]))
        M.shape=(4,4)

    fly_model_node_filename = os.path.join(fsee.data_dir,'models/fly/body.osg')
    model_path = os.path.join(fsee.data_dir,"models/alice_cylinder/alice_cylinder.osg")
    model_path = os.path.join(fsee.data_dir,"models/alice_cylinder/white_cylinder.osg")
    #model_path = None
    z = 2 # 2 mm
    #for j,id in enumerate(ids):