示例#1
0
    def __init__(self, joint):
        GenericObject.__init__(self)
        children_joints = joint.get_children_joints()
        for child in children_joints:
            child.init_local_transformation()
            p = child.localTransformation[:3, 3]
            translation = p * 0.5
            height = numpy.linalg.norm(p)
            if height < 1e-6:
                continue
            radius = 0.005
            new_y = p / numpy.linalg.norm(p)
            new_x = numpy.cross(new_y, numpy.array([1, 0, 0]))
            new_x_norm = numpy.linalg.norm(new_x)
            # if new_y is colinear to numpy.array([1,0,0])
            # recompute new_x
            if new_x_norm < 1e-6:
                new_x = numpy.cross(new_y, numpy.array([0, 1, 0]))
                new_x_norm = numpy.linalg.norm(new_x)
            new_x /= new_x_norm
            new_z = numpy.cross(new_x, new_y)
            trans_T = numpy.eye(4)
            trans_T[:3, 0] = new_x
            trans_T[:3, 1] = new_y
            trans_T[:3, 2] = new_z

            try:
                angle, direc, point = tf.rotation_from_matrix(trans_T)
            except:
                logger.exception("Error in creating link {0}-{1}".format(
                    joint.name, child.name))
            else:
                rotation = [direc[0], direc[1], direc[2], angle]
                link = Shape()
                link.enabled = False

                link.appearance.material.diffuseColor = [0, 0.5, 1]
                link.geometry = Cylinder()
                link.geometry.height = height
                link.geometry.radius = radius
                link.rotation = rotation
                link.translation = translation
                link.init()
                self.add_child(link)

        motor = Shape()
        motor.enabled = False
        self.add_child(motor)
        motor.appearance.material.diffuseColor = [1, 0, 0]
        if joint.jointAxis in ["X", "x", "Y", "y", "z", "Z"]:
            motor.geometry = Cylinder()
            motor.geometry.height = 0.005
            motor.geometry.radius = 0.02
            if joint.jointAxis in ["X", "x"]:
                motor.rotation = [0, 0, 1, 1.5708]
            elif joint.jointAxis in ["Z", "z"]:
                motor.rotation = [0, 1, 0, 1.5708]
        else:
            motor.geometry = Sphere()
            motor.geometry.radius = 0.02
    def updateData(self, feature, prop):
        r"""If a property of the handled feature has changed,
        we have the chance to handle this here
        
        See Also
        --------
        https://www.freecadweb.org/wiki/Scripted_objects

        """
        debug("ViewProviderAnchor/updateData")
        p = feature.getPropertyByName("p")
        u = feature.getPropertyByName("u")
        v = feature.getPropertyByName("v")

        self.transform.translation.setValue((p[0], p[1], p[2]))

        at = anchor_transformation(p0=(0, 0, 0),
                                   u0=(0, -1, 0),
                                   v0=(0, 0, 1),
                                   p1=(p[0], p[1], p[2]),
                                   u1=(u[0], u[1], u[2]),
                                   v1=(v[0], v[1], v[2]))

        t = translation_from_matrix(at)

        self.transform.translation.setValue((t[0], t[1], t[2]))

        angle, direction, point = rotation_from_matrix(at)
        # print("angle : %f" % angle)
        # print("direction : %s" % str(direction))
        # print("point : %s" % str(point))

        self.transform.rotation.setValue(coin.SbVec3f(direction), angle)
示例#3
0
def writeAnimation(human, linebuffer, animTrack, config):
    # TODO animations need to be adapted to rest pose and retargeted to user skeleton
    import numpy as np
    progress = Progress(len(human.getSkeleton().getBones()))
    log.message("Exporting animation %s.", animTrack.name)
    linebuffer.append('        <animation name="%s" length="%s">' % (animTrack.name, animTrack.getPlaytime()))
    linebuffer.append('            <tracks>')
    I = np.identity(4, dtype=np.float32)
    axis_ = np.asarray([0,0,0,1], dtype=np.float32)
    for bIdx, bone in enumerate(human.getSkeleton().getBones()):
        # Note: OgreXMLConverter will optimize out unused (not moving) animation tracks
        linebuffer.append('                <track bone="%s">' % bone.name)
        linebuffer.append('                    <keyframes>')
        frameTime = 1.0/float(animTrack.frameRate)
        for frameIdx in xrange(animTrack.nFrames):
            poseMat = animTrack.getAtFramePos(frameIdx)[bIdx]
            I[:3,:4] = poseMat[:3,:4]
            poseMat = I
            translation = poseMat[:3,3]
            angle, axis, _ = transformations.rotation_from_matrix(poseMat)
            axis_[:3] = axis[:3]
            axis = np.asarray(axis_ * np.matrix(bone.getRestMatrix(offsetVect=config.offset)))[0]
            linebuffer.append('                        <keyframe time="%s">' % (float(frameIdx) * frameTime))
            linebuffer.append('                            <translate x="%s" y="%s" z="%s" />' % (translation[0], translation[1], translation[2]))
            # TODO account for scale
            linebuffer.append('                            <rotate angle="%s">' % angle)
            linebuffer.append('                                <axis x="%s" y="%s" z="%s" />' % (axis[0], axis[1], axis[2]))
            linebuffer.append('                            </rotate>')
            linebuffer.append('                        </keyframe>')
        linebuffer.append('                    </keyframes>')
        linebuffer.append('                </track>')
        progress.step()
    linebuffer.append('            </tracks>')
    linebuffer.append('        </animation>')
示例#4
0
def writeAnimation(human, linebuffer, animTrack):
    progress = Progress(len(human.getSkeleton().getBones()))
    log.message("Exporting animation %s.", animTrack.name)
    linebuffer.append('        <animation name="%s" length="%s">' %
                      (animTrack.name, animTrack.getPlaytime()))
    linebuffer.append('            <tracks>')
    for bIdx, bone in enumerate(human.getSkeleton().getBones()):
        # Note: OgreXMLConverter will optimize out unused (not moving) animation tracks
        linebuffer.append('                <track bone="%s">' % bone.name)
        linebuffer.append('                    <keyframes>')
        frameTime = 1.0 / float(animTrack.frameRate)
        for frameIdx in xrange(animTrack.nFrames):
            poseMat = animTrack.getAtFramePos(frameIdx)[bIdx]
            translation = poseMat[:3, 3]
            angle, axis, _ = transformations.rotation_from_matrix(poseMat)
            linebuffer.append('                        <keyframe time="%s">' %
                              (float(frameIdx) * frameTime))
            linebuffer.append(
                '                            <translate x="%s" y="%s" z="%s" />'
                % (translation[0], translation[1], translation[2]))
            # TODO account for scale
            linebuffer.append(
                '                            <rotate angle="%s">' % angle)
            linebuffer.append(
                '                                <axis x="%s" y="%s" z="%s" />'
                % (axis[0], axis[1], axis[2]))
            linebuffer.append('                            </rotate>')
            linebuffer.append('                        </keyframe>')
        linebuffer.append('                    </keyframes>')
        linebuffer.append('                </track>')
        progress.step()
    linebuffer.append('            </tracks>')
    linebuffer.append('        </animation>')
示例#5
0
def writeAnimation(human, linebuffer, animTrack, config):
    import numpy as np
    progress = Progress(len(human.getSkeleton().getBones()))
    log.message("Exporting animation %s.", animTrack.name)
    linebuffer.append('        <animation name="%s" length="%s">' % (animTrack.name, animTrack.getPlaytime()))
    linebuffer.append('            <tracks>')
    for bIdx, bone in enumerate(human.getSkeleton().getBones()):
        # Note: OgreXMLConverter will optimize out unused (not moving) animation tracks
        linebuffer.append('                <track bone="%s">' % bone.name)
        linebuffer.append('                    <keyframes>')
        frameTime = 1.0/float(animTrack.frameRate)
        for frameIdx in xrange(animTrack.nFrames):
            poseMat = animTrack.getAtFramePos(frameIdx)[bIdx]
            translation = poseMat[:3,3]
            angle, axis, _ = transformations.rotation_from_matrix(poseMat)
            axis = np.asarray(axis * np.matrix(bone.getRestMatrix(offsetVect=config.offset)))[0]
            linebuffer.append('                        <keyframe time="%s">' % (float(frameIdx) * frameTime))
            linebuffer.append('                            <translate x="%s" y="%s" z="%s" />' % (translation[0], translation[1], translation[2]))
            # TODO account for scale
            linebuffer.append('                            <rotate angle="%s">' % angle)
            linebuffer.append('                                <axis x="%s" y="%s" z="%s" />' % (axis[0], axis[1], axis[2]))
            linebuffer.append('                            </rotate>')
            linebuffer.append('                        </keyframe>')
        linebuffer.append('                    </keyframes>')
        linebuffer.append('                </track>')
        progress.step()
    linebuffer.append('            </tracks>')
    linebuffer.append('        </animation>')
示例#6
0
    def __init__(self, joint):
        GenericObject.__init__(self)
        children_joints = joint.get_children_joints()
        for child in children_joints:
            child.init_local_transformation()
            p = child.localTransformation[:3,3]
            translation = p*0.5
            height = numpy.linalg.norm(p)
            if height < 1e-6:
                continue
            radius = 0.005
            new_y = p/numpy.linalg.norm(p)
            new_x = numpy.cross(new_y, numpy.array([1,0,0]))
            new_x_norm = numpy.linalg.norm(new_x)
            # if new_y is colinear to numpy.array([1,0,0])
            # recompute new_x
            if new_x_norm < 1e-6:
               new_x = numpy.cross(new_y, numpy.array([0,1,0]))
               new_x_norm = numpy.linalg.norm(new_x)
            new_x /= new_x_norm
            new_z = numpy.cross(new_x, new_y)
            trans_T = numpy.eye(4)
            trans_T[:3,0] = new_x
            trans_T[:3,1] = new_y
            trans_T[:3,2] = new_z

            try:
                angle, direc, point = tf.rotation_from_matrix(trans_T)
            except:
                logger.exception("Error in creating link {0}-{1}".format(joint.name,
                                                                         child.name))
            else:
                rotation = [direc[0], direc[1], direc[2], angle]
                link = Shape()
                link.enabled = False

                link.appearance.material.diffuseColor = [0,0.5,1]
                link.geometry = Cylinder()
                link.geometry.height = height
                link.geometry.radius = radius
                link.rotation = rotation
                link.translation = translation
                link.init()
                self.add_child(link)

        motor = Shape()
        motor.enabled = False
        self.add_child(motor)
        motor.appearance.material.diffuseColor = [1,0,0]
        if joint.jointAxis in ["X","x","Y","y","z","Z"]:
            motor.geometry = Cylinder()
            motor.geometry.height = 0.005
            motor.geometry.radius = 0.02
            if joint.jointAxis in ["X","x"]:
                motor.rotation = [0, 0, 1, 1.5708]
            elif joint.jointAxis in ["Z","z"]:
                motor.rotation = [0, 1, 0, 1.5708]
        else:
            motor.geometry = Sphere()
            motor.geometry.radius = 0.02
示例#7
0
def log_so3(R):
    assert R.shape == (3, 3)
    T = np.identity(4)
    T[0:3, 0:3] = R
    angle, axis, _ = tfs.rotation_from_matrix(T)

    return angle * axis
示例#8
0
 def remove_generic_objects(self):
     generic_children = [
         child for child in self.children
         if (type(child) == GenericObject and child.children[:])
     ]
     while generic_children[:]:
         for child in generic_children:
             for grandchild in child.children[:]:
                 grandchild.localTransformation = numpy.dot(
                     child.localTransformation,
                     grandchild.localTransformation)
                 grandchild.translation = grandchild.localTransformation[:3,
                                                                         3]
                 angle, direction, point = tf.rotation_from_matrix(
                     grandchild.localTransformation)
                 grandchild.rotation = list(direction) + [angle]
                 grandchild.rpy = tf.euler_from_matrix(
                     grandchild.localTransformation)
                 grandchild.scale = [
                     grandchild.scale[i] * child.scale[i] for i in range(3)
                 ]
                 self.add_child(grandchild)
             self.children.remove(child)
             del child
         generic_children = [
             child for child in self.children
             if (type(child) == GenericObject and child.children[:])
         ]
     for child in self.children:
         child.remove_generic_objects()
示例#9
0
 def update_config2(self, T, q):
     self.count += 1
     T = numpy.array(T)
     self.localTransformation = T
     self.translation = T[:3, 3]
     angle, direction, point = tf.rotation_from_matrix(T)
     self.rpy = tf.euler_from_matrix(T)
     self.rotation = [direction[0], direction[1], direction[2], angle]
     self.origin.update()
示例#10
0
 def update_config2(self, T, q):
     self.count += 1
     T = numpy.array(T)
     self.localTransformation = T
     self.translation = T[:3,3]
     angle, direction, point = tf.rotation_from_matrix(T)
     self.rpy = tf.euler_from_matrix(T)
     self.rotation = [ direction[0],direction[1],direction[2], angle ]
     self.origin.update()
示例#11
0
def transform_pad(padcoords, pad_length, pad_separation, pad_thickness,
                  drum_separation, drum_radius, totalAlpha):
    TranG = trn.translation_matrix((padcoords[0], padcoords[1], padcoords[2]))
    if padcoords[4] != 0:
        RotG = trn.rotation_matrix(padcoords[4], [0, 1, 0])
    else:
        RotG = trn.identity_matrix()
    TranJ = trn.translation_matrix(((pad_length + pad_separation), 0, 0))
    if (padcoords[0] + pad_separation + pad_length) > (drum_separation / 2):
        TranJ_Rot = trn.translation_matrix(
            (-(pad_length + pad_separation) / 2, 0, 0))
        alpha = -np.arctan((pad_length + pad_separation) / (drum_radius))

        totalAlpha[0] += alpha
        if totalAlpha[0] < -math.pi:
            alpha -= (totalAlpha[0] - math.pi)
            totalAlpha[0] = -math.pi
        RotJ = trn.rotation_matrix(
            alpha, [0, 1, 0],
            [TranJ_Rot[0][3], TranJ_Rot[1][3], TranJ_Rot[2][3]])
        Final = trn.concatenate_matrices(TranG, RotG, TranJ, RotJ)

        angle, direction, point = trn.rotation_from_matrix(Final)
    elif (padcoords[0] - pad_separation - pad_length) < -(drum_separation / 2):
        TranJ_Rot = trn.translation_matrix(
            (-(pad_length + pad_separation) / 2, 0, 0))
        alpha = -np.arctan((pad_length + pad_separation) / (drum_radius))

        totalAlpha[0] += alpha
        if totalAlpha[0] < -2 * math.pi:
            alpha -= (totalAlpha[0] - 2 * math.pi)
            totalAlpha[0] = -2 * math.pi
        RotJ = trn.rotation_matrix(
            alpha, [0, 1, 0],
            [TranJ_Rot[0][3], TranJ_Rot[1][3], TranJ_Rot[2][3]])
        Final = trn.concatenate_matrices(TranG, RotG, TranJ, RotJ)

        angle, direction, point = trn.rotation_from_matrix(Final)
    else:
        Final = trn.concatenate_matrices(TranG, RotG, TranJ)
        angle, direction, point = trn.rotation_from_matrix(Final)
    padcoords = [Final[0][3], Final[1][3], Final[2][3], 0, angle, 0]
    return padcoords
示例#12
0
    def __init__(self, server = None, width = 640, height = 480):
        logger.debug("Creating camera")
        kinematics.GenericObject.__init__(self)
        self.pixels = None
        self.draw_t = 0
        self.frame_seq = 0
        self.server = server
        self.frontClipDistance = 0.01
        self.backClipDistance = 1000
        self.width = width
        self.height = height
        self.cam_type = "COLOR"
        self.fieldOfView = 45*math.pi/180
        self.translation = [0, 0, 0]
        self.r = 3.5
        self.x0 = 0.
        self.y0 = 0.
        self.aspect = float(width)/float(height)

        # OpenCV params
        self.fx = 0.
        self.fy = 0.
        self.cx = float(width)/2
        self.cy = float(height)/2
        self.R = numpy.eye(3)

        self.localTransformation[:3,:3] = numpy.array([ [ 0 , 0 , 1],
                                                        [ 1 , 0 , 0],
                                                        [ 0 , 1 , 0],
                                                        ]
                                                      )
        angle, direction, point = tf.rotation_from_matrix(self.localTransformation)
        self.rotation = list(direction) + [angle]
        self.moved = True
        self.init()

        self.top    =  math.atan(self.fovy/2)*self.near
        self.bottom = -math.atan(self.fovy/2)*self.near

        self.right  = self.aspect*self.top
        self.left   = self.aspect*self.bottom

        self.compute_opencv_params()
        # print self.globalTransformation
        self.count = 0
        self.fbo = None
        self.texture = None
        self.render_buffer = None
        self.gl_error = None
        logger.debug("{0} GL vals: {1}".format(self.name,
                                              (self.x0, self.y0,
                                               self.fovy, self.aspect,
                                               self.left, self.right,
                                               self.bottom, self.top,
                                               self.near, self.far)))
示例#13
0
    def evaluate_frame(self, frame):
        head_position = self.skeleton.nodes[self.skeleton.head_joint].get_global_position(frame, use_cache=False)
        target_direction = self.target_position - head_position
        target_direction /= np.linalg.norm(target_direction)

        head_orientation = self.skeleton.nodes[self.skeleton.head_joint].get_global_orientation_quaternion(frame, use_cache=True)
        head_direction = self._get_direction_vector_from_orientation(head_orientation)

        delta_q = quaternion_from_vector_to_vector(head_direction, target_direction)
        angle, _, _ = rotation_from_matrix(quaternion_matrix(delta_q))
        return abs(angle)
示例#14
0
    def update_config(self,config):
        self.count += 1
        if not config:
            raise Exception("Failed to update_config for {0} with {1}".format(self.name, config))

        self.translation = config[:3]
        self.rpy = config[3:6]
        rot_mat = tf.euler_matrix(self.rpy[0], self.rpy[1], self.rpy[2])
        angle, direction, point = tf.rotation_from_matrix(rot_mat)
        self.rotation = [ direction[0],direction[1],direction[2], angle ]
        self.init_local_transformation()
        self.origin.update()
示例#15
0
def angle_axis(x):
    """Returns the extracted angle and axis from a valid SO3 quantity x.
    This is equivalent to separating the rotvec form of x into its
    magnitude and direction. The angle will always be between 0 and pi, 
    inclusive, and the axis will always be a unit vector.

    >>> yourAngle, yourAxis = -1337, [-2, 0, 7]
    >>> yourMat = trns.rotation_matrix(yourAngle, yourAxis)
    >>> myAngle, myAxis = angle_axis(yourMat)
    >>> print(np.isclose(myAngle, np.mod(yourAngle, 2*np.pi)))
    True
    >>> print(np.allclose(myAxis, normalize(yourAxis)))
    True
    >>> myAngle2, myAxis2 = angle_axis(trns.quaternion_from_matrix(yourMat))
    >>> print(np.allclose(myAngle, myAngle2), np.allclose(myAxis, myAxis2))
    (True, True)

    """
    xrep = rep(x)
    # In the matrix case, transformations.py already has a nice implementation:
    if xrep in ['rotmat', 'tfmat']:
        if xrep == 'rotmat':
            x = make_affine(x)
        angle, axis, point = trns.rotation_from_matrix(x)
        # But to be consistent with rotvecs, we only use positive angles:
        if angle < 0:
            angle, axis = -angle, -axis
        # And we map the identity rotation to an axis of [0, 0, 0]:
        elif np.isclose(angle, 0):
            axis = np.array([0, 0, 0])
    # In the quaternion case, we carry out the following routine:
    elif xrep == 'quaternion':
        # Renormalize for accuracy:
        x = normalize(x)
        # If "amount of rotation" is negative, flip quaternion:
        if x[0] < 0:
            x = -x
        # Extract axis:
        axis = normalize(x[1:])
        # Extract angle:
        angle = 2*np.arccos(x[0])
    # In the rotvec case, the routine is trivial:
    elif xrep == 'rotvec':
        angle = npl.norm(x)
        if not np.isclose(angle, 0):
            axis = x / angle
        else:
            axis = np.array([0, 0, 0])
    # If not SO3:
    else:
        raise ValueError("Quantity to extract angle and axis from must be on SO3.")
    # Finally:
    return (angle, axis)
示例#16
0
    def update_config(self, config):
        self.count += 1
        if not config:
            raise Exception("Failed to update_config for {0} with {1}".format(
                self.name, config))

        self.translation = config[:3]
        self.rpy = config[3:6]
        rot_mat = tf.euler_matrix(self.rpy[0], self.rpy[1], self.rpy[2])
        angle, direction, point = tf.rotation_from_matrix(rot_mat)
        self.rotation = [direction[0], direction[1], direction[2], angle]
        self.init_local_transformation()
        self.origin.update()
示例#17
0
def transform_pad(padcoords,pad_length,pad_separation,pad_thickness,drum_separation,drum_radius,totalAlpha):
    TranG = trn.translation_matrix(( padcoords[0],padcoords[1],padcoords[2] ))
    if padcoords[4]!=0:
        RotG = trn.rotation_matrix(padcoords[4],[0,1,0])
    else:
        RotG = trn.identity_matrix()
    TranJ = trn.translation_matrix(( (pad_length+pad_separation),0,0))
    if (padcoords[0]+pad_separation+pad_length) >(drum_separation/2):
        TranJ_Rot = trn.translation_matrix((-(pad_length+pad_separation)/2,0,0))
        alpha = - np.arctan((pad_length+pad_separation)/(drum_radius))
        
        totalAlpha[0] += alpha
        if totalAlpha[0]<-math.pi:
            alpha -= (totalAlpha[0] - math.pi)
            totalAlpha[0] = -math.pi
        RotJ = trn.rotation_matrix(alpha,[0,1,0],[TranJ_Rot[0][3],TranJ_Rot[1][3],TranJ_Rot[2][3]])
        Final = trn.concatenate_matrices(TranG,RotG,TranJ,RotJ)
        
        angle, direction, point = trn.rotation_from_matrix(Final)
    elif (padcoords[0]-pad_separation-pad_length)<-(drum_separation/2):
        TranJ_Rot = trn.translation_matrix((-(pad_length+pad_separation)/2,0,0))
        alpha = - np.arctan((pad_length+pad_separation)/(drum_radius))
        
        totalAlpha[0] += alpha
        if totalAlpha[0]<-2*math.pi:
            alpha -= (totalAlpha[0] - 2*math.pi)
            totalAlpha[0] = -2*math.pi
        RotJ = trn.rotation_matrix(alpha,[0,1,0],[TranJ_Rot[0][3],TranJ_Rot[1][3],TranJ_Rot[2][3]])
        Final = trn.concatenate_matrices(TranG,RotG,TranJ,RotJ)
        
        angle, direction, point = trn.rotation_from_matrix(Final)
    else :
        Final = trn.concatenate_matrices(TranG,RotG,TranJ)
        angle, direction, point = trn.rotation_from_matrix(Final)
    padcoords = [Final[0][3],Final[1][3],Final[2][3],0,angle,0]
    return padcoords
示例#18
0
def errors(reference, target):
	if len(reference) != len(target):
		raise ValueError("reference and target should be of same length! len(reference): %d, len(target): %d" % (len(reference), len(target)) )

	F_ref = to_homogeneous(reference)
	F_tar = to_homogeneous(target)

	F_ref_inv = inverse_matrix(F_ref)

	T_tar_ref = concatenate_matrices(F_ref_inv, F_tar)

	trans = translation_from_matrix(T_tar_ref)
	ang, foo, bar = rotation_from_matrix(T_tar_ref)

	return (numpy.sum( numpy.square(trans) ), ang*ang)
    def load(self, di_dict, predictor):
        di = DatasetItem(di_dict)
        self.df['subject'] = pd.Series(data=di.get_subject(), index=di.get_stamps())
        self.df['scenario'] = di.get_scenario()
        self.df['humanhash'] = di.get_humanhash()
        
        for stamp in di.get_stamps():
            T_camdriver_head = di.get_T_camdriver_head(stamp)
            
            assert T_camdriver_head is not None
            
            T_headfrontal_head = T_headfrontal_camdriver.dot(T_camdriver_head)
            self.df.at[stamp, 'gt_roll'], self.df.at[stamp, 'gt_pitch'], self.df.at[stamp, 'gt_yaw'] = tr.euler_from_matrix(T_headfrontal_head)
            self.df.at[stamp, 'gt_x'], self.df.at[stamp, 'gt_y'], self.df.at[stamp, 'gt_z'] = T_camdriver_head[0:3,3]
            
            gt_angle_from_zero, _, _ = tr.rotation_from_matrix(T_headfrontal_head)
            self.df.at[stamp, 'gt_angle_from_zero'] = abs(gt_angle_from_zero)

            self.df.at[stamp, 'occlusion_state'] = di.get_occlusion_state(stamp)
            
            hypo_T_headfrontal_head = predictor.get_T_headfrontal_head(stamp)
            if hypo_T_headfrontal_head is None:
                self.df.at[stamp, 'hypo_roll'] = None
                self.df.at[stamp, 'hypo_pitch'] = None
                self.df.at[stamp, 'hypo_yaw'] = None
                self.df.at[stamp, 'angle_diff'] = None
                self.df.at[stamp, 'hypo_x'] = None
                self.df.at[stamp, 'hypo_y'] = None
                self.df.at[stamp, 'hypo_z'] = None
            else:
                self.df.at[stamp, 'hypo_roll'], self.df.at[stamp, 'hypo_pitch'], self.df.at[stamp, 'hypo_yaw'] = tr.euler_from_matrix(hypo_T_headfrontal_head)

                angle_difference, _, _ = tr.rotation_from_matrix(tr.inverse_matrix(T_headfrontal_head).dot(hypo_T_headfrontal_head))
                self.df.at[stamp, 'angle_diff'] = abs(angle_difference)

                self.df.at[stamp, 'hypo_x'], self.df.at[stamp, 'hypo_y'], self.df.at[stamp, 'hypo_z'] = predictor.get_t_camdriver_head(stamp)
def scale_and_translate(Tlistgroundtruth, Tlistartoolkit):
    ground_truth_pointcloud = np.asarray([utils.apply_transform(T, np.zeros(3))
                                          for T in Tlistgroundtruth])
    artoolkit_pointcloud = np.asarray([utils.apply_transform(T, np.zeros(3))
                                    for T in Tlistartoolkit])
    Tscale = tf.affine_matrix_from_points(artoolkit_pointcloud.T,
                                          ground_truth_pointcloud.T,
                                          shear=False,
                                          scale=True)
    #print("Scaling by {0}".format(tf.scale_from_matrix(Tscale)[0]))
    try:
        print("translation by {0}".format(tf.translation_from_matrix(Tscale)))
        print("rotation by {0}".format(tf.rotation_from_matrix(Tscale)[0]))
    except ValueError:
        pass
    Tlistartoolkit = [tf.concatenate_matrices(Tscale, T) for T in Tlistartoolkit]
    return Tlistartoolkit
示例#21
0
def so3_log(r, return_angle_only=True, return_skew=False):
    """
    :param r: SO(3) rotation matrix
    :param return_angle_only: return only the angle (default)
    :param return_skew: return skew symmetric Lie algebra element
    :return: axis/angle
        or if skew:
             3x3 skew symmetric logarithmic map in so(3) (Ma, Soatto eq. 2.8)
    """
    if not is_so3(r):
        raise LieAlgebraException("matrix is not a valid SO(3) group element")
    if return_angle_only and not return_skew:
        return np.arccos(min(1, max(-1, (np.trace(r) - 1) / 2)))
    angle, axis, _ = tr.rotation_from_matrix(se3(r, [0, 0, 0]))
    if return_skew:
        return hat(axis * angle)
    else:
        return axis, angle
示例#22
0
def from_homogeneous(M,n):
	if n != 7 and n != 3:
		raise ValueError("Only poses with 3 or 7 elements supported! this one has %d" % n)

	trans = translation_from_matrix(M)
	if n == 7:
		quat = quaternion_from_matrix(M)
		out = list(trans)
		out.extend(quaternion_imag(quat))
		out.append(quaternion_real(quat))
	else:
		ang, foo, bar = rotation_from_matrix(M)
		out = list(trans[0:2])
		out.append(ang)

	if len(out) != n:
		raise ValueError("Wrong output length: %d should be %d" %(len(out), n))

	return out
示例#23
0
 def remove_generic_objects(self):
     generic_children = [child for child in self.children
                         if (type(child) == GenericObject and child.children[:])]
     while generic_children[:]:
         for child in generic_children:
             for grandchild in child.children[:]:
                 grandchild.localTransformation = numpy.dot(child.localTransformation,
                                                           grandchild.localTransformation)
                 grandchild.translation =  grandchild.localTransformation[:3,3]
                 angle, direction, point = tf.rotation_from_matrix(grandchild.localTransformation)
                 grandchild.rotation = list(direction) + [angle]
                 grandchild.rpy = tf.euler_from_matrix(grandchild.localTransformation)
                 grandchild.scale = [grandchild.scale[i]*child.scale[i] for i in range(3)]
                 self.add_child(grandchild)
             self.children.remove(child)
             del child
         generic_children = [child for child in self.children
                             if (type(child) == GenericObject and child.children[:])]
     for child in self.children:
         child.remove_generic_objects()
示例#24
0
    def _post_optimization(self, grasp_contacts):
        logging.info('[HFTSSampler::_post_optimization] Performing post optimization.')
        transform = self._robot.GetTransform()
        angle, axis, point = transformations.rotation_from_matrix(transform)
        # further optimize hand configuration and pose
        # TODO this is Robotiq hand specific
        transform_params = axis.tolist() + [angle] + transform[:3, 3].tolist()
        robot_dofs = self._robot.GetDOFValues().tolist()

        def joint_limits_constraint(x, *args):
            positions, normals, robot = args
            lower_limits, upper_limits = robot.GetDOFLimits()
            return -dist_in_range(x[0], [lower_limits[0], upper_limits[0]]) - \
                   dist_in_range(x[1], [lower_limits[1], upper_limits[1]])

        def collision_free_constraint(x, *args):
            positions, normals, robot = args
            config = [x[0], x[1]]
            robot.SetDOFValues(config)
            env = robot.GetEnv()
            links = robot.get_non_fingertip_links()
            for link in links:
                if env.CheckCollision(robot.GetLink(link)):
                    return -1.0
            return 0.0

        x_min = scipy.optimize.fmin_cobyla(self._post_optimization_obj_fn, robot_dofs + transform_params,
                                           [joint_limits_constraint, collision_free_constraint],
                                           rhobeg=.5, rhoend=1e-3,
                                           args=(grasp_contacts[:, :3], grasp_contacts[:, 3:], self._robot),
                                           maxfun=int(1e8), iprint=0)
        self._robot.SetDOFValues(x_min[:2])
        axis = x_min[2:5]
        angle = x_min[5]
        position = x_min[6:]
        transform = transformations.rotation_matrix(angle, axis)
        transform[:3, 3] = position
        self._robot.SetTransform(transform)
示例#25
0
def sketch(robot):
    res = ""
    res += "def robot {\n"
    oldT = numpy.eye(4)
    for j in robot.moving_joint_list[:20]:
        T = j.globalTransformation
        T1 = numpy.eye(4)
        if j.jointAxis in ['y','Y']:
            T1 = tf.rotation_matrix(math.pi/2, [0,0,1])
        if j.jointAxis in ['z','Z']:
            T1 = tf.rotation_matrix(math.pi/2, [0,1,0])
        T = numpy.dot(T,T1)
        relT = numpy.dot(numpy.linalg.inv(oldT),T)
        angle, direc, point = tf.rotation_from_matrix(relT)
        print T
        p = relT[:3,3]
        p = [w*10 for w in p]
        res += """put {rotate (%f,(%f, %f, %f),[%f, %f, %f]) then translate ([%f, %f, %f])} {joint}
"""%(angle*180/math.pi,0,0,0, direc[0], direc[1], direc[2], p[0],p[1],p[2])
        oldT = T

    res += "}\n"
    return res
示例#26
0
def writeAnimation(human, fp, animTrack):
    log.message("Exporting animation %s.", animTrack.name)
    fp.write('        <animation name="%s" length="%s">\n' % (animTrack.name, animTrack.getPlaytime()))
    fp.write('            <tracks>\n')
    for bIdx, bone in enumerate(human.getSkeleton().getBones()):
        # Note: OgreXMLConverter will optimize out unused (not moving) animation tracks
        fp.write('                <track bone="%s">\n' % bone.name)
        fp.write('                    <keyframes>\n')
        frameTime = 1.0/float(animTrack.frameRate)
        for frameIdx in xrange(animTrack.nFrames):
            poseMat = animTrack.getAtFramePos(frameIdx)[bIdx]
            translation = poseMat[:3,3]
            angle, axis, _ = transformations.rotation_from_matrix(poseMat)
            fp.write('                        <keyframe time="%s">\n' % (float(frameIdx) * frameTime))
            fp.write('                            <translate x="%s" y="%s" z="%s" />\n' % (translation[0], translation[1], translation[2]))
            # TODO account for scale
            fp.write('                            <rotate angle="%s">\n' % angle)
            fp.write('                                <axis x="%s" y="%s" z="%s" />\n' % (axis[0], axis[1], axis[2]))
            fp.write('                            </rotate>\n')
            fp.write('                        </keyframe>\n')
        fp.write('                    </keyframes>\n')
        fp.write('                </track>\n')
    fp.write('            </tracks>\n')
    fp.write('        </animation>\n')
示例#27
0
def sketch(robot):
    res = ""
    res += "def robot {\n"
    oldT = numpy.eye(4)
    for j in robot.moving_joint_list[:20]:
        T = j.globalTransformation
        T1 = numpy.eye(4)
        if j.jointAxis in ['y', 'Y']:
            T1 = tf.rotation_matrix(math.pi / 2, [0, 0, 1])
        if j.jointAxis in ['z', 'Z']:
            T1 = tf.rotation_matrix(math.pi / 2, [0, 1, 0])
        T = numpy.dot(T, T1)
        relT = numpy.dot(numpy.linalg.inv(oldT), T)
        angle, direc, point = tf.rotation_from_matrix(relT)
        print T
        p = relT[:3, 3]
        p = [w * 10 for w in p]
        res += """put {rotate (%f,(%f, %f, %f),[%f, %f, %f]) then translate ([%f, %f, %f])} {joint}
""" % (angle * 180 / math.pi, 0, 0, 0, direc[0], direc[1], direc[2], p[0],
        p[1], p[2])
        oldT = T

    res += "}\n"
    return res
示例#28
0
def elbow_angle(obj, light='L', heavy='H', limit_l=107, limit_h=113, draw=0):
    """

DESCRIPTION

    Calculates the integer elbow angle of an antibody Fab complex and
    optionally draws a graphical representation of the vectors used to
    determine the angle.

ARGUMENTS

    obj = string: object

    light/heavy = strings: chain ID of light and heavy chains, respectively

    limit_l/limit_h = integers: residue numbers of the last residue in the
    light and heavy chain variable domains, respectively

    draw = boolean: Choose whether or not to draw the angle visualization

REQUIRES: com.py, transformations.py, numpy (see above)


    """

    # store current view
    orig_view = cmd.get_view()

    limit_l = int(limit_l)
    limit_h = int(limit_h)
    draw = int(draw)

    # for temp object names
    tmp_prefix = "tmp_elbow_"

    prefix = tmp_prefix + obj + '_'

    # names
    vl = prefix + 'VL'
    vh = prefix + 'VH'
    cl = prefix + 'CL'
    ch = prefix + 'CH'

    # selections
    vl_sel = 'polymer and %s and chain %s and resi 1-%i' % (obj, light,
                                                            limit_l)
    vh_sel = 'polymer and %s and chain %s and resi 1-%i' % (obj, heavy,
                                                            limit_h)
    cl_sel = 'polymer and %s and chain %s and not resi 1-%i' % (obj, light,
                                                                limit_l)
    ch_sel = 'polymer and %s and chain %s and not resi 1-%i' % (obj, heavy,
                                                                limit_h)
    v_sel = '((' + vl_sel + ') or (' + vh_sel + '))'
    c_sel = '((' + cl_sel + ') or (' + ch_sel + '))'

    # create temp objects
    cmd.create(vl, vl_sel)
    cmd.create(vh, vh_sel)
    cmd.create(cl, cl_sel)
    cmd.create(ch, ch_sel)

    # superimpose vl onto vh, calculate axis and angle
    Rv = calc_super_matrix(vl, vh)
    angle_v, direction_v, point_v = transformations.rotation_from_matrix(Rv)

    # superimpose cl onto ch, calculate axis and angle
    Rc = calc_super_matrix(cl, ch)
    angle_c, direction_c, point_c = transformations.rotation_from_matrix(Rc)

    # delete temporary objects
    cmd.delete(vl)
    cmd.delete(vh)
    cmd.delete(cl)
    cmd.delete(ch)

    # if dot product is positive, angle is acute
    if (numpy.dot(direction_v, direction_c) > 0):
        direction_c = direction_c * -1  # ensure angle is > 90 (need to standardize this)

        # TODO: make both directions point away from the elbow axis.

    elbow = int(
        numpy.degrees(numpy.arccos(numpy.dot(direction_v, direction_c))))
    # while (elbow < 90):
    # elbow = 180 - elbow   # limit to physically reasonable range

    # compare the direction_v and direction_c axes to the vector defined by
    # the C-alpha atoms of limit_l and limit_h of the original fab
    hinge_l_sel = "%s//%s/%s/CA" % (obj, light, limit_l)
    hinge_h_sel = "%s//%s/%s/CA" % (obj, heavy, limit_h)

    try:
        hinge_l = cmd.get_atom_coords(hinge_l_sel)
        hinge_h = cmd.get_atom_coords(hinge_h_sel)
    except pymol.CmdException:
        # Either hinge_l_sel or hinge_h_sel atom did not exist.
        raise pymol.CmdException(
            'Unable to calculate elbow angle. Please check '
            'your limit and chain selections and try again.')
    hinge_vec = numpy.array(hinge_h) - numpy.array(hinge_l)

    test = numpy.dot(hinge_vec, numpy.cross(direction_v, direction_c))
    if (test > 0):
        elbow = 360 - elbow

    print("    Elbow angle: %i degrees" % elbow)

    if (draw == 1):
        # there is probably a more elegant way to do this, but
        # it works so I'm not going to mess with it for now

        pre = obj + '_elbow_'

        # draw hinge vector
        cmd.pseudoatom(pre + "hinge_l", pos=hinge_l)
        cmd.pseudoatom(pre + "hinge_h", pos=hinge_h)
        cmd.distance(pre + "hinge_vec", pre + "hinge_l", pre + "hinge_h")
        cmd.set("dash_gap", 0)

        # draw the variable domain axis
        com_v = COM(v_sel)
        start_v = [a - 10 * b for a, b in zip(com_v, direction_v)]
        end_v = [a + 10 * b for a, b in zip(com_v, direction_v)]
        cmd.pseudoatom(pre + "start_v", pos=start_v)
        cmd.pseudoatom(pre + "end_v", pos=end_v)
        cmd.distance(pre + "v_vec", pre + "start_v", pre + "end_v")

        # draw the constant domain axis
        com_c = COM(c_sel)
        start_c = [a - 10 * b for a, b in zip(com_c, direction_c)]
        end_c = [a + 10 * b for a, b in zip(com_c, direction_c)]
        cmd.pseudoatom(pre + "start_c", pos=start_c)
        cmd.pseudoatom(pre + "end_c", pos=end_c)
        cmd.distance(pre + "c_vec", pre + "start_c", pre + "end_c")

        # customize appearance
        cmd.hide("labels", pre + "hinge_vec")
        cmd.hide("labels", pre + "v_vec")
        cmd.hide("labels", pre + "c_vec")
        cmd.color("green", pre + "hinge_l")
        cmd.color("red", pre + "hinge_h")
        cmd.color("black", pre + "hinge_vec")
        cmd.color("black", pre + "start_v")
        cmd.color("black", pre + "end_v")
        cmd.color("black", pre + "v_vec")
        cmd.color("black", pre + "start_c")
        cmd.color("black", pre + "end_c")
        cmd.color("black", pre + "c_vec")
        # draw spheres
        cmd.show("spheres", pre + "hinge_l or " + pre + "hinge_h")
        cmd.show("spheres", pre + "start_v or " + pre + "start_c")
        cmd.show("spheres", pre + "end_v or " + pre + "end_c")
        cmd.set("sphere_scale", 2)
        cmd.set("dash_gap", 0, pre + "hinge_vec")
        cmd.set("dash_width", 5)
        cmd.set("dash_radius", 0.3)

        # group drawing objects
        cmd.group(pre, pre + "*")

    # restore original view
    cmd.set_view(orig_view)

    return 0
示例#29
0
def to_angle_axis(quaternion):
    rot_mat = transf.quaternion_matrix(quaternion)
    angle, direction, point = transf.rotation_from_matrix(rot_mat)
    return np.array([angle, direction[0], direction[1], direction[2]])
示例#30
0
def writeSkeletonFile(human, filepath, config):
    import transformations as tm
    Pprogress = Progress(3)  # Parent.
    filename = os.path.basename(filepath)
    filename = getbasefilename(filename)
    filename = filename + ".skeleton.xml"
    filepath = os.path.join(os.path.dirname(filepath), filename)

    skel = human.getSkeleton()
    if config.scale != 1:
        skel = skel.scaled(config.scale)

    f = io.open(filepath, 'w', encoding="utf-8")
    lines = []

    lines.append('<?xml version="1.0" encoding="UTF-8"?>')
    lines.append(
        '<!-- Exported from MakeHuman (www.makehumancommunity.org) -->')
    lines.append('<!-- Skeleton: %s -->' % skel.name)
    lines.append('<skeleton>')
    lines.append('    <bones>')
    progress = Progress(len(skel.getBones()))
    for bIdx, bone in enumerate(skel.getBones()):
        mat = bone.getRelativeMatrix(
            offsetVect=config.offset
        )  # TODO adapt offset if mesh orientation is different

        # Bone positions are in parent bone space
        pos = mat[:3, 3]

        angle, axis, _ = tm.rotation_from_matrix(mat)

        lines.append('        <bone id="%s" name="%s">' % (bIdx, bone.name))
        lines.append('            <position x="%s" y="%s" z="%s" />' %
                     (pos[0], pos[1], pos[2]))
        lines.append('            <rotation angle="%s">' % angle)
        lines.append('                <axis x="%s" y="%s" z="%s" />' %
                     (axis[0], axis[1], axis[2]))
        lines.append('            </rotation>')
        lines.append('        </bone>')
        progress.step()
    lines.append('    </bones>')
    Pprogress.step()

    lines.append('    <bonehierarchy>')
    progress = Progress(len(skel.getBones()))
    for bone in skel.getBones():
        if bone.parent:
            lines.append('        <boneparent bone="%s" parent="%s" />' %
                         (bone.name, bone.parent.name))
        progress.step()
    lines.append('    </bonehierarchy>')
    Pprogress.step()

    animations = [human.getAnimation(name) for name in human.getAnimations()]
    # TODO compensate animations for alternate rest pose
    if len(animations) > 0:
        lines.append('    <animations>')
        for anim in animations:
            # Use pose matrices, not skinning matrices
            anim.resetBaked()
            #anim = bvhanim.getAnimationTrack()
            writeAnimation(human, lines, anim, config)
        lines.append('    </animations>')

    lines.append('</skeleton>')

    f.write("\n".join(lines))
    f.close()
    Pprogress.finish()
def elbow_angle(obj, light='L', heavy='H', limit_l=107, limit_h=113, draw=0):
    """

DESCRIPTION

    Calculates the integer elbow angle of an antibody Fab complex and 
    optionally draws a graphical representation of the vectors used to 
    determine the angle.

ARGUMENTS

    obj = string: object

    light/heavy = strings: chain ID of light and heavy chains, respectively

    limit_l/limit_h = integers: residue numbers of the last residue in the 
    light and heavy chain variable domains, respectively    

    draw = boolean: Choose whether or not to draw the angle visualization

REQUIRES: com.py, transformations.py, numpy (see above)


    """

    # store current view
    orig_view = cmd.get_view()

    limit_l = int(limit_l)
    limit_h = int(limit_h)
    draw = int(draw)

    # for temp object names
    tmp_prefix = "tmp_elbow_"

    prefix = tmp_prefix + obj + '_'

    # names
    vl = prefix + 'VL'
    vh = prefix + 'VH'
    cl = prefix + 'CL'
    ch = prefix + 'CH'

    # selections
    vl_sel = 'polymer and %s and chain %s and resi 1-%i' % (obj, light, limit_l)
    vh_sel = 'polymer and %s and chain %s and resi 1-%i' % (obj, heavy, limit_h)
    cl_sel = 'polymer and %s and chain %s and not resi 1-%i' % (obj, light, limit_l)
    ch_sel = 'polymer and %s and chain %s and not resi 1-%i' % (obj, heavy, limit_h)
    v_sel = '((' + vl_sel + ') or (' + vh_sel + '))'
    c_sel = '((' + cl_sel + ') or (' + ch_sel + '))'

    # create temp objects
    cmd.create(vl, vl_sel)
    cmd.create(vh, vh_sel)
    cmd.create(cl, cl_sel)
    cmd.create(ch, ch_sel)

    # superimpose vl onto vh, calculate axis and angle
    Rv = calc_super_matrix(vl, vh)
    angle_v, direction_v, point_v = transformations.rotation_from_matrix(Rv)

    # superimpose cl onto ch, calculate axis and angle
    Rc = calc_super_matrix(cl, ch)
    angle_c, direction_c, point_c = transformations.rotation_from_matrix(Rc)

    # delete temporary objects
    cmd.delete(vl)
    cmd.delete(vh)
    cmd.delete(cl)
    cmd.delete(ch)

    # if dot product is positive, angle is acute
    if (numpy.dot(direction_v, direction_c) > 0):
        direction_c = direction_c * -1   # ensure angle is > 90 (need to standardize this)

        # TODO: make both directions point away from the elbow axis.

    elbow = int(numpy.degrees(numpy.arccos(numpy.dot(direction_v, direction_c))))
    # while (elbow < 90):
    # elbow = 180 - elbow   # limit to physically reasonable range

    # compare the direction_v and direction_c axes to the vector defined by
    # the C-alpha atoms of limit_l and limit_h of the original fab
    hinge_l_sel = "%s//%s/%s/CA" % (obj, light, limit_l)
    hinge_h_sel = "%s//%s/%s/CA" % (obj, heavy, limit_h)
    hinge_l = cmd.get_atom_coords(hinge_l_sel)
    hinge_h = cmd.get_atom_coords(hinge_h_sel)
    hinge_vec = numpy.array(hinge_h) - numpy.array(hinge_l)

    test = numpy.dot(hinge_vec, numpy.cross(direction_v, direction_c))
    if (test > 0):
        elbow = 360 - elbow

    print "    Elbow angle: %i degrees" % elbow

    if (draw == 1):
        # there is probably a more elegant way to do this, but
        # it works so I'm not going to mess with it for now

        pre = obj + '_elbow_'

        # draw hinge vector
        cmd.pseudoatom(pre + "hinge_l", pos=hinge_l)
        cmd.pseudoatom(pre + "hinge_h", pos=hinge_h)
        cmd.distance(pre + "hinge_vec", pre + "hinge_l", pre + "hinge_h")
        cmd.set("dash_gap", 0)

        # draw the variable domain axis
        com_v = com.COM(v_sel)
        start_v = [a - 10 * b for a, b in zip(com_v, direction_v)]
        end_v = [a + 10 * b for a, b in zip(com_v, direction_v)]
        cmd.pseudoatom(pre + "start_v", pos=start_v)
        cmd.pseudoatom(pre + "end_v", pos=end_v)
        cmd.distance(pre + "v_vec", pre + "start_v", pre + "end_v")

        # draw the constant domain axis
        com_c = com.COM(c_sel)
        start_c = [a - 10 * b for a, b in zip(com_c, direction_c)]
        end_c = [a + 10 * b for a, b in zip(com_c, direction_c)]
        cmd.pseudoatom(pre + "start_c", pos=start_c)
        cmd.pseudoatom(pre + "end_c", pos=end_c)
        cmd.distance(pre + "c_vec", pre + "start_c", pre + "end_c")

        # customize appearance
        cmd.hide("labels", pre + "hinge_vec")
        cmd.hide("labels", pre + "v_vec")
        cmd.hide("labels", pre + "c_vec")
        cmd.color("green", pre + "hinge_l")
        cmd.color("red", pre + "hinge_h")
        cmd.color("black", pre + "hinge_vec")
        cmd.color("black", pre + "start_v")
        cmd.color("black", pre + "end_v")
        cmd.color("black", pre + "v_vec")
        cmd.color("black", pre + "start_c")
        cmd.color("black", pre + "end_c")
        cmd.color("black", pre + "c_vec")
        # draw spheres
        cmd.show("spheres", pre + "hinge_l or " + pre + "hinge_h")
        cmd.show("spheres", pre + "start_v or " + pre + "start_c")
        cmd.show("spheres", pre + "end_v or " + pre + "end_c")
        cmd.set("sphere_scale", 2)
        cmd.set("dash_gap", 0, pre + "hinge_vec")
        cmd.set("dash_width", 5)
        cmd.set("dash_radius", 0.3)

        # group drawing objects
        cmd.group(pre, pre + "*")

    # restore original view
    cmd.set_view(orig_view)

    return 0
示例#32
0
def writeSkeletonFile(human, filepath, config):
    import transformations as tm
    Pprogress = Progress(3)  # Parent.
    filename = os.path.basename(filepath)
    filename = getbasefilename(filename)
    filename = filename + ".skeleton.xml"
    filepath = os.path.join(os.path.dirname(filepath), filename)

    skel = human.getSkeleton()
    if config.scale != 1:
        skel = skel.scaled(config.scale)
    if not skel.isInRestPose():
        # Export skeleton with the current pose as rest pose
        skel = skel.createFromPose()

    f = codecs.open(filepath, 'w', encoding="utf-8")
    lines = []

    lines.append('<?xml version="1.0" encoding="UTF-8"?>')
    lines.append('<!-- Exported from MakeHuman (www.makehuman.org) -->')
    lines.append('<!-- Skeleton: %s -->' % skel.name)
    lines.append('<skeleton>')
    lines.append('    <bones>')
    progress = Progress(len(skel.getBones()))
    for bIdx, bone in enumerate(skel.getBones()):
        mat = bone.getRelativeMatrix(offsetVect=config.offset)  # TODO adapt offset if mesh orientation is different

        # Bone positions are in parent bone space
        pos = mat[:3,3]

        angle, axis, _ = tm.rotation_from_matrix(mat)

        lines.append('        <bone id="%s" name="%s">' % (bIdx, bone.name))
        lines.append('            <position x="%s" y="%s" z="%s" />' % (pos[0], pos[1], pos[2]))
        lines.append('            <rotation angle="%s">' % angle)
        lines.append('                <axis x="%s" y="%s" z="%s" />' % (axis[0], axis[1], axis[2]))
        lines.append('            </rotation>')
        lines.append('        </bone>')
        progress.step()
    lines.append('    </bones>')
    Pprogress.step()

    lines.append('    <bonehierarchy>')
    progress = Progress(len(skel.getBones()))
    for bone in skel.getBones():
        if bone.parent:
            lines.append('        <boneparent bone="%s" parent="%s" />' % (bone.name, bone.parent.name))
        progress.step()
    lines.append('    </bonehierarchy>')
    Pprogress.step()

    if hasattr(human, 'animations'):
        lines.append('    <animations>')
        for anim in human.animations:
            writeAnimation(human, lines, anim.getAnimationTrack(), config)
        lines.append('    </animations>')

    lines.append('</skeleton>')

    f.write("\n".join(lines))
    f.close()
    Pprogress.finish()