def rule_branch(self):
		'''
		@brief Create a branch
		'''	
		# Translate
		frame = self.current_node.frame	
		frame = np.dot(frame, tr.translation_matrix([0, 0, self.current_node.size]))
		
		# Add noise
		if self.noise != 0:
			self.current_phi_angle += random.uniform(-self.noise, self.noise)
			self.current_rho_angle += random.uniform(-self.noise, self.noise)
		
		# Phi Rotation
		phi_matrix = tr.rotation_matrix(self.current_phi_angle, [0, 0, 1])
		frame[:3,:3] = np.dot(frame[:3,:3], phi_matrix[:3,:3])
		
		# Rho rotation
		rho_matrix = tr.rotation_matrix(self.current_rho_angle, [1, 0, 0])	
		frame[:3,:3] = np.dot(frame[:3,:3], rho_matrix[:3,:3])
				
		# Create a new node
		node = Node(frame, self.current_node.size * self.scale_factor, self.current_node.diameter * self.current_subdivision_ratio * self.scale_factor)	
		
		self.current_node.add_node(node)
		self.current_node = node
		self.current_rho_angle = 0
		self.current_phi_angle = 0
		self.current_subdivision_ratio = 1

	def inverse_kinematics( self , pos ,  normal ) :
		l1 , l2 , l3 = .91 , .81 , .33
		dy , dz  = .27 , .26 

		normal = normal / la.norm( normal )

		pos1 = pos + normal * l3

		e = m.sqrt(pos1[2]*pos1[2]+pos1[0]*pos1[0]-dz*dz)

		a1 = m.atan2(pos1[2], -pos1[0]) + m.atan2(dz, e)

		pos2 = np.array( [ e , pos1[1]-dy , .0 ] );
		a3 = -m.acos(min(1.0,(pos2[0]*pos2[0]+pos2[1]*pos2[1]-l1*l1-l2*l2)/(2.0*l1*l2)))
		k = l1 + l2 * m.cos(a3)
		l = l2 * m.sin(a3)
		a2 = -m.atan2(pos2[1],m.sqrt(pos2[0]*pos2[0]+pos2[2]*pos2[2])) - m.atan2(l,k);

		rotmat = tr.rotation_matrix( -a1 , (0,1,0) ) 
		normal1 = np.resize( np.dot( rotmat , np.resize( normal , 4 ) ) , 3 )

		rotmat = tr.rotation_matrix( -a2-a3 , (0,0,1) ) 
		normal1 = np.resize( np.dot( rotmat , np.resize( normal1, 4 ) ) , 3 )

		a5 = m.acos( normal1[0] )
		a4 = m.atan2(normal1[2], normal1[1])

		return a1 , a2 , a3 , a4 , a5

def _getMatrix(head, tail, roll):
    """
    Calculate an orientation (rest) matrix for a bone between specified head
    and tail positions with given bone roll angle.
    Returns length of the bone and rest orientation matrix in global coordinates.
    """
    # TODO validate, or replace

    vector = toZisUp3(tail - head)
    length = math.sqrt(np.dot(vector, vector))
    if length == 0:
        vector = [0,0,1]
    else:
        vector = vector/length
    yproj = np.dot(vector, YUnit)

    if yproj > 1-1e-6:
        axis = YUnit
        angle = 0
    elif yproj < -1+1e-6:
        axis = YUnit
        angle = pi
    else:
        axis = np.cross(YUnit, vector)
        axis = axis / math.sqrt(np.dot(axis,axis))
        angle = math.acos(yproj)
    mat = tm.rotation_matrix(angle, axis)
    if roll:
        mat = np.dot(mat, tm.rotation_matrix(roll, YUnit))
    mat = fromZisUp4(mat)
    mat[:3,3] = head
    return length, mat

        def setup_particle_orientation(invert, theta, phi):
            # spin about z-axis vector to create phi spin
            phi_mat = transformations.rotation_matrix(phi + (pi if invert else 0), vz)
            # rotate z-x axis to create orientation vector
            axis_rotation = transformations.rotation_matrix((-1 if invert else 1) * -(theta - pi/2), vy)

            return transformations.concatenate_matrices(axis_rotation, phi_mat)

def compute_transform(rx, rz, tx, ty, tz):
	origin, xaxis, yaxis, zaxis = [0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]

	Rx = tf.rotation_matrix(rx, xaxis)
	Rz = tf.rotation_matrix(rz, zaxis)
	T = tf.translation_matrix([tx, ty, tz])
	
	return T.dot(Rz).dot(Rx)	

    def createAnimationTrack(self, jointsOrder = None, name="BVHMotion"):
        """
        Create an animation track from the motion stored in this BHV file.
        """
        if jointsOrder == None:
            jointsData = [joint.matrixPoses for joint in self.getJoints() if not joint.isEndConnector()]
            # We leave out end effectors as they should not have animation data
        else:
            nFrames = self.frameCount
            import re
            # Remove the tail from duplicate bone names
            for idx,jName in enumerate(jointsOrder):
                # Joint mappings can contain a rotation compensation
                if isinstance(jName, tuple):
                    jName, _ = jName
                if not jName:
                    continue
                r = re.search("(.*)_\d+$",jName)
                if r:
                    jointsOrder[idx] = r.group(1)

            jointsData = []
            for jointName in jointsOrder:
                if isinstance(jointName, tuple):
                    jointName, angle = jointName
                else:
                    angle = 0.0
                if jointName:
                    poseMats = self.getJointByCanonicalName(jointName).matrixPoses.copy()
                    if isinstance(angle, float):
                        if angle != 0.0:
                            # Rotate around global Z axis
                            rot = tm.rotation_matrix(-angle*D, [0,0,1])
                            # Roll around global Y axis (this is a limitation)
                            roll = tm.rotation_matrix(angle*D, [0,1,0])
                            for i in range(nFrames):
                                # TODO make into numpy loop
                                poseMats[i] = np.dot(poseMats[i], rot)
                                poseMats[i] = np.dot(poseMats[i], roll)
                    else:   # Compensation (angle) is a transformation matrix
                        # Compensate animation frames
                        for i in range(nFrames):
                            # TODO make into numpy loop
                            poseMats[i] = np.mat(poseMats[i]) * np.mat(angle)
                            #poseMats[i] = np.mat(angle) # Test compensated rest pose
                    jointsData.append(poseMats)
                else:
                    jointsData.append(animation.emptyTrack(nFrames))

        nJoints = len(jointsData)
        nFrames = len(jointsData[0])

        # Interweave joints animation data, per frame with joints in breadth-first order
        animData = np.hstack(jointsData).reshape(nJoints*nFrames,4,4)
        framerate = 1.0/self.frameTime
        return animation.AnimationTrack(name, animData, nFrames, framerate)

    def update_sagital_rot(self, robot):

        # ok we assume a symetric configuration with both foot on the ground

        self.LAnkleRotY = tf.rotation_matrix(
            radians(robot.l_ankle_y.present_position), self.yaxis)
        self.RAnkleRotY = tf.rotation_matrix(
            radians(robot.r_ankle_y.present_position), self.yaxis)

        self.LKneeRotY = tf.rotation_matrix(
            radians(- robot.l_knee_y.present_position), self.yaxis)
        self.RKneeRotY = tf.rotation_matrix(
            radians(- robot.r_knee_y.present_position), self.yaxis)

        self.LHipRotY = tf.rotation_matrix(
            radians(- robot.l_hip_y.present_position), self.yaxis)
        self.RHipRotY = tf.rotation_matrix(
            radians(- robot.r_hip_y.present_position), self.yaxis)

        self.AbsRotY = tf.rotation_matrix(
            radians(robot.abs_y.present_position), self.yaxis)
        self.BustRotY = tf.rotation_matrix(
            radians(robot.bust_y.present_position), self.yaxis)

        self.roty = array([tf.rotation_matrix(0, self.yaxis), (self.LAnkleRotY + self.RAnkleRotY) / 2.0,
                           (self.LKneeRotY + self.RKneeRotY) / 2.0, (self.LHipRotY + self.RHipRotY) / 2.0, self.AbsRotY, self.BustRotY])

        # self.transformy =array([tf.concatenate_matrices(self.roty[i], self.SegCom[i]) for i in range(len(self.roty))])
        # self.transformy =array([self.roty[i].dot( self.SegCom[i]) for i in range(len(self.roty))])

        self.transformy = array([identity(4),
                                 self.FootT,
                                 tf.concatenate_matrices(
            (self.LAnkleRotY + self.RAnkleRotY) / 2.0, self.ThighT),
            tf.concatenate_matrices(
            (self.LKneeRotY + self.RKneeRotY) / 2.0, self.LegT),
            tf.concatenate_matrices(
            (self.LHipRotY + self.RHipRotY) / 2.0, self.HipT),
            tf.concatenate_matrices(
            self.AbsRotY, self.AbsT),
            tf.concatenate_matrices(
            self.BustRotY, self.BustT)
        ])

        a = dot(self.transformy[1], self.transformy[2])
        b = dot(a, self.transformy[3])
        c = dot(b, self.transformy[4])
        d = dot(c, self.transformy[5])
        e = dot(d, self.transformy[6])

        self.comtrans = array([self.transformy[1],
                               a,
                               b,
                               c,
                               d,
                               e
                               ])

def align(atoms, init_vec, align_vec):
    import transformations
    import numpy as np

    if len(init_vec) == 2:
        orig_atom_ind = init_vec[0]
        final_atom_ind = init_vec[1]
        init_vec = atoms[final_atom_ind].position - atoms[orig_atom_ind].position

    if len(align_vec) == 2:
        orig_atom_ind = align_vec[0]
        final_atom_ind = align_vec[1]
        align_vec = atoms[final_atom_ind].position - atoms[orig_atom_ind].position

    rot_axis = np.cross(init_vec, align_vec)
    nrot_axis = rot_axis/np.linalg.norm(rot_axis)

    angle = transformations.angle_between_vectors(init_vec, align_vec)

    rot_M = transformations.rotation_matrix(angle, nrot_axis)[:3,:3]

    for a in atoms:
        a.position = rot_M.dot(a.position)

    return atoms

 def poleTargetCorrect(self, head, goal, pole, angle):
     yvec = goal-head
     xvec = pole-head
     xy = dot(xvec, yvec)/dot(yvec,yvec)
     xvec = xvec - xy * yvec
     xlen = math.sqrt(dot(xvec,xvec))
     if xlen > 1e-6:
         xvec = xvec / xlen
         zvec = self.matrixGlobal[:3,2]
         zlen = math.sqrt(dot(zvec,zvec))
         zvec = zvec / zlen
         angle0 = math.asin( dot(xvec,zvec) )
         rot = tm.rotation_matrix(angle - angle0, CBone.Axes[1])
         #m0 = self.matrixGlobal.copy()
         self.matrixGlobal[:3,:3] = dot(self.matrixGlobal[:3,:3], rot[:3,:3])
         
         if 0 and self.name == "DfmUpArm2_L":
             log.debug("")
             log.debug("IK %s", self.name)
             log.debug("X %s", xvec)
             log.debug("Y %s", yvec)
             log.debug("Z %s", zvec)
             log.debug("A0 %s", angle0)
             log.debug("A %s", angle)
             log.debug("R %s", rot)
             log.debug("M0 %s", m0)
             log.debug("M %s", self.matrixGlobal)

    def init_local_transformation(self):
        """
        compute local transformation w.r.t for the first time (compute everything)
        """
        # rotation part
        self.localTransformation = numpy.eye(4)
        # print type(self), id(self), self.rotation
        try:
            angle = self.rotation[3]
        except IndexError:
            logger.exception("Failed on {0}, rotation={1}".format(type(self),self.rotation))
            raise
        direction = self.rotation[:3]
        self.localTransformation[0:3,0:3] = tf.rotation_matrix(angle, direction)[:3,:3]
        self.rpy = tf.euler_from_matrix(self.localTransformation)

        # last column
        scale = [1,1,1]
        if self.parent:
            scale = self.cumul_scale()
        scale_translation = [self.translation[i]*scale[i] for i in range(3)]

        self.localTransformation[0:3,3] = numpy.array(scale_translation)+\
            numpy.dot(numpy.eye(3)-self.localTransformation[:3,:3],
                      numpy.array(self.center))

        # last line
        self.localTransformation[3,0:4]=[0,0,0,1]

	def make_pts( self , corners ) :
		dx = (corners[3] - corners[0])
		dx = dx / la.norm(dx)
		dy = (corners[1] - corners[0]) / (self.size[1]+3-1)
		dz = np.cross( dx , dy )
		dz = dz / la.norm(dz)
		ax = np.cross( dx , dz )
		ax = ax / la.norm(ax)

		an = 2.0 * m.pi / float(self.size[0]) 
		rot = tr.rotation_matrix( an , ax )

		ox = (corners[3] + corners[0]) / 2.0
		rx = np.resize( ox - corners[0] , 4 )

		self.axis   = ax
		self.center = ox

		del self.pts[:]

		for y in range(self.size[1]+3) :
			drx = copy(rx)
			for x in range(self.size[0]) :
				pt = ox + np.resize(drx,3) 
				self.pts.append(     pt )
				drx = np.dot( drx , rot )
			self.pts.append( self.pts[-self.size[0]] )
			self.pts.append( self.pts[-self.size[0]] )
			self.pts.append( self.pts[-self.size[0]] )
			ox += dy 

 def Rotation(self, angle, dim, axis):
     self.size = dim
     mat = tm.rotation_matrix(angle, Matrix.Axis[axis])
     if dim == 3:
         self.matrix = mat[:3,:3]
     else:
         self.matrix = mat

	def __init__( self , fovy , ratio , near , far , robot_files ) :
		self.fovy = fovy
		self.near = near 
		self.far = far
		self.ratio = ratio

		self.camera = None
		self.plane  = Plane( (2,2) )

		self.wall = Plane( (20,10) )
		self.mw = tr.rotation_matrix( -m.pi / 2.0 , (1,0,0) )
		self.mw = np.dot( self.mw , tr.translation_matrix( (0,3,0) ) )

		self.robot = Robot( robot_files )

		self.x = 0.0

		self.last_time = timer()

		self.plane_alpha = 65.0 / 180.0 * m.pi

		self.lpos = [ 1 ,-1 , 0 ]

		self._make_plane_matrix()

		self.draw_robot = True
		self.draw_sparks = True 
		self.draw_front = False
		self.draw_back = False

    def update_local_transformation(self):
        """
        update local transformation w.r.t element's parent

        .. seealso:: VRML transform calculation http://www.web3d.org/x3d/specifications/vrml/ISO-IEC-14772-VRML97/part1/nodesRef.html#Transform
        """
        if self.jointType in ["free", "freeflyer"]:
            self.localTransformation = tf.euler_matrix(self.rpy[0], self.rpy[1], self.rpy[2])

            scale = [1,1,1]
            if self.parent:
                scale = self.cumul_scale()
            scale_translation = [self.translation[i]*scale[i] for i in range(3)]
            self.localTransformation[0:3,3]=numpy.array(scale_translation)+\
                numpy.dot(numpy.eye(3)-self.localTransformation[0:3,:3],numpy.array(self.center))

        elif ( type(self) == Joint and self.jointType in [ "rotate", "rotation", "revolute"]
               and self.jointId >= 0):
            if self.jointAxis in ["x","X"]:
                axis = [1, 0, 0]
            elif self.jointAxis in ["y","Y"]:
                axis = [0, 1, 0]
            elif self.jointAxis in ["z","Z"]:
                axis = [0, 0, 1]
            else:
                raise RuntimeError("Invalid joint axis {0}".format(self.jointAxis))
            self.localR2= tf.rotation_matrix(self.angle, axis)[:3,:3]
            self.localTransformation[0:3,0:3] = numpy.dot(self.localR1, self.localR2)

def make_kin_tree_from_joint(ps,jointname, ns='default_ns',valuedict=None):
    rospy.logdebug("joint: %s"%jointname)
    U = get_pr2_urdf()
        
    joint = U.joints[jointname]
    joint.origin = joint.origin or urdf.Pose()
    if not joint.origin.position: joint.origin.position = [0,0,0]
    if not joint.origin.rotation: joint.origin.rotation = [0,0,0]
   
    parentFromChild = conversions.trans_rot_to_hmat(joint.origin.position, transformations.quaternion_from_euler(*joint.origin.rotation))

    childFromRotated = np.eye(4)
    if valuedict and jointname in valuedict:        
        
        if joint.joint_type == 'revolute' or joint.joint_type == 'continuous':
            childFromRotated = transformations.rotation_matrix(valuedict[jointname], joint.axis)
        elif joint.joint_type == 'prismatic':
            childFromRotated = transformations.translation_matrix(np.array(joint.axis)* valuedict[jointname]) 
            
    parentFromRotated = np.dot(parentFromChild, childFromRotated)            
    originFromParent = conversions.pose_to_hmat(ps.pose)
    originFromRotated = np.dot(originFromParent, parentFromRotated)
    
    newps = gm.PoseStamped()
    newps.header = ps.header
    newps.pose = conversions.hmat_to_pose(originFromRotated)
    
    return make_kin_tree_from_link(newps,joint.child,ns=ns,valuedict=valuedict)

	def _make_plane_matrix( self ) :
		r = tr.rotation_matrix( self.plane_alpha , (0,0,1) )
		s = tr.scale_matrix( 1 )
		t = tr.translation_matrix( (-1.25,.7,.05) )

		self.m = np.dot( np.dot( t , s ) , r )
		self.im = la.inv( self.m )
		self.im[3] = [ 0 , 0 , 0 , 1 ]

 def fixCSysBone(self, bname, infix, mat, index, axis, angle):
     csys = csysBoneName(bname, infix)
     bone = self.armature.bones[csys]
     rot = tm.rotation_matrix(angle, axis)
     cmat = np.dot(mat, rot)
     bone.tail = bone.head + self.armature.bones[bname].length * cmat[:3,1]
     normal = getUnitVector(mat[:3,index])
     bone.roll = computeRoll(bone.head, bone.tail, normal)

	def step( self , dt ) :
		rot = tr.rotation_matrix( dt * self.w , (0,0,1) )

		self.v1 = np.resize( np.dot( rot , np.resize( self.v1 , 4 ) ) , 2 )
		R = self.v1 * self.l1
		self.v2[1] = -R[1]
		L = self.l2 + rand.normalvariate(self.mu,self.sg) 
		self.v2[0] = m.sqrt(L**2-self.v2[1]**2)

 def rotate(self, angle, axis, rotWorld):
     mat = tm.rotation_matrix(angle*D, CBone.Axes[axis])
     if rotWorld:
         mat = dot(mat, self.matrixGlobal)        
         self.matrixGlobal[:3,:3] = mat[:3,:3]
         self.matrixPose = self.getPoseFromGlobal()
     else:
         mat = dot(mat, self.matrixPose)
         self.matrixPose[:3,:3] = mat[:3,:3]

    def __init__(self,dis=[0,0,0],angle=0,axis=[0,0,1],unit='deg'):
        """initialization of object, dis = displacement, angle is the angle of
        rotation around the vector axis, unit is the unit of the angle."""
        self.__dis = np.array(dis).reshape((3,1))
        if unit == 'deg': angle = angle*np.pi/180.
        # otherwise, angle is assumed to be in radians
        self.__rot = tf.rotation_matrix(angle,axis)[0:3,0:3]

        self.__hom     = np.vstack( (np.hstack((self.__rot,self.__dis)), np.array([0,0,0,1]))) 
        self.__hom_inv = np.vstack( (np.hstack((self.__rot.T,-np.dot(self.__rot.T,self.__dis))), np.array([0,0,0,1])))