Esempio n. 1
0
def alignPositionYawSingleExpm(est, gt):
    '''
    calcualte the 4DOF transformation: yaw R and translation t so that:
        gt = R * est + t
    '''

    _assertDim(est)
    _assertDim(gt)
    est_pos, est_quat, est_vel = tp.parseSingle(est[0, :])
    g_pos, g_quat, g_vel = tp.parseSingle(gt[0, :])
    g_rot = tfs.quaternion_matrix(g_quat)
    g_rot = g_rot[0:3, 0:3]
    est_rot = tfs.quaternion_matrix(est_quat)
    est_rot = est_rot[0:3, 0:3]

    R_full = np.dot(g_rot, np.transpose(est_rot))
    log_R = log_so3(R_full)
    yaw_R = np.array([0, 0, log_R[2]])
    if np.linalg.norm(yaw_R) < 1e-7:
        R = np.identity(3)
    else:
        R = exp_so3(yaw_R)
    t = g_pos - np.dot(R, est_pos)

    return R, t
Esempio n. 2
0
    def set_pose(self, simulationState):
        pose = simulationState.Current.PostureData
        # set rotation
        rot = [-pose[3], -pose[4], pose[5], pose[6]]
        rot = normalize(rot)
        m = quaternion_matrix(rot)[:3, :3]
        local_root_axes = copy(ROOT_COS_MAP)
        target_x_vec = np.dot(m, [1, 0, 0])
        twist_q, axes = align_axis(local_root_axes, "x", target_x_vec)
        twist_q = normalize(twist_q)
        swing_q = self.skeleton.reference_frame[3:7]
        q = quaternion_multiply(twist_q, swing_q)
        q = normalize(q)
        self.mg_state_machine.set_global_orientation(q)

        # set position
        target_pose = self.retargeting.RetargetToTarget(
            simulationState.Current)
        start_pos = array_from_mosim_t(
            target_pose.Joints[0].Position) * self.target_to_mmu_scale
        start_pos[1] = 0

        #start_pos += self.static_offset
        twist_m = quaternion_matrix(twist_q)[:3, :3]
        start_pos += np.dot(twist_m, self.static_offset)
        print("set start position", start_pos, target_pose.Joints[0].Position)
        self.mg_state_machine.set_global_position(start_pos)
        self.mg_state_machine.update(self.frame_time)
    def transform_to_object_part_frame(self, part_frame):
        assert not self.in_part_frame

        trans = part_frame[:3]
        rot_quat = part_frame[3:]

        rot_quat_mat = quaternion_matrix(rot_quat)

        rot_quat_inv = quaternion_inverse(rot_quat)
        rot_quat_inv_mat = quaternion_matrix(rot_quat_inv)

        # translate and rotate
        new_waypoint_array = []
        for i in range(len(self.waypoint_array)):
            waypoint = self.waypoint_array[i]
            way_pos, way_ori = pose_to_arrays(waypoint)

            way_pos_trans = np.asarray(way_pos) - np.asarray(trans)

            way_pos_rotated = np.dot(rot_quat_inv_mat,
                                     pos_array_to_column_vector(way_pos_trans))
            way_ori_rotated = quaternion_multiply(rot_quat_inv, way_ori)

            way_pos_rotated = np.transpose(way_pos_rotated)[0][:-1]
            assert way_ori_rotated.ndim == 1 and way_pos_rotated.ndim == 1
            assert way_ori_rotated.shape[0] == 4 and way_pos_rotated.shape[
                0] == 3

            new_waypoint = array_to_pose(
                np.hstack((way_pos_rotated, way_ori_rotated)))
            new_waypoint_array.append(new_waypoint)

        self.waypoint_array = new_waypoint_array
        self.update_interpolation()
        self.in_part_frame = False
Esempio n. 4
0
    def __predict_nominal_state(self, imu_measurement: np.array):
        p = self.nominal_state[:3].reshape(-1, 1)
        q = self.nominal_state[3:7]
        v = self.nominal_state[7:10].reshape(-1, 1)
        a_b = self.nominal_state[10:13].reshape(-1, 1)
        w_b = self.nominal_state[13:16]
        g = self.nominal_state[16:19].reshape(-1, 1)

        w_m = imu_measurement[1:4].copy()
        a_m = imu_measurement[4:7].reshape(-1, 1).copy()
        dt = imu_measurement[0] - self.last_predict_time
        """
        dp/dt = v
        dv/dt = R(a_m - a_b) + g
        dq/dt = 0.5 * q x(quaternion product) (w_m - w_b)
        
        a_m and w_m are the measurements of IMU.
        a_b and w_b are biases of acc and gyro, respectively.
        R = R{q}, which bring the point from local coordinate to global coordinate.
        """
        w_m -= w_b
        a_m -= a_b

        # use the zero-order integration to integrate the quaternion.
        # q_{n+1} = q_n x q{(w_m - w_b) * dt}
        angle = la.norm(w_m)
        axis = w_m / angle
        R_w = tr.rotation_matrix(0.5 * dt * angle, axis)
        q_w = tr.quaternion_from_matrix(R_w, True)
        q_half_next = tr.quaternion_multiply(q, q_w)

        R_w = tr.rotation_matrix(dt * angle, axis)
        q_w = tr.quaternion_from_matrix(R_w, True)
        q_next = tr.quaternion_multiply(q, q_w)
        if q_next[0] < 0:  # force the quaternion has a positive real part.
            q_next *= -1

        # use RK4 method to integration velocity and position.
        # integrate velocity first.
        R = tr.quaternion_matrix(q)[:3, :3]
        R_half_next = tr.quaternion_matrix(q_half_next)[:3, :3]
        R_next = tr.quaternion_matrix(q_next)[:3, :3]
        v_k1 = R @ a_m + g
        v_k2 = R_half_next @ a_m + g
        # v_k3 = R_half_next @ a_m + g  # yes. v_k2 = v_k3.
        v_k3 = v_k2
        v_k4 = R_next @ a_m + g
        v_next = v + dt * (v_k1 + 2 * v_k2 + 2 * v_k3 + v_k4) / 6

        # integrate position
        p_k1 = v
        p_k2 = v + 0.5 * dt * v_k1  # k2 = v_next_half = v + 0.5 * dt * v' = v + 0.5 * dt * v_k1(evaluate at t0)
        p_k3 = v + 0.5 * dt * v_k2  # v_k2 is evaluated at t0 + 0.5*delta
        p_k4 = v + dt * v_k3
        p_next = p + dt * (p_k1 + 2 * p_k2 + 2 * p_k3 + p_k4) / 6

        self.nominal_state[:3] = p_next.reshape(3, )
        self.nominal_state[3:7] = q_next
        self.nominal_state[7:10] = v_next.reshape(3, )
Esempio n. 5
0
    def getBlendedPose(self, poses, weights, additiveBlending=True):
        import transformations as tm

        REST_QUAT = np.asarray([1, 0, 0, 0], dtype=np.float32)

        if isinstance(poses[0], basestring):
            f_idxs = [self.getPoseNames().index(pname) for pname in poses]
        else:
            f_idxs = poses

        if not additiveBlending:
            # normalize weights
            if not isinstance(weights, np.ndarray):
                weights = np.asarray(weights, dtype=np.float32)
            t = sum(weights)
            if t < 1:
                # Fill up rest with neutral pose (neutral pose is assumed to be first frame)
                weights = np.asarray(weights + [1.0 - t], dtype=np.float32)
                f_idxs.append(0)
            weights /= t

        #print zip([self.getPoseNames()[_f] for _f in f_idxs],weights)

        result = emptyPose(self.nBones)
        m = np.identity(4, dtype=np.float32)
        m1 = np.identity(4, dtype=np.float32)
        m2 = np.identity(4, dtype=np.float32)

        if len(f_idxs) == 1:
            for b_idx in xrange(self.nBones):
                m[:3, :4] = self.getAtFramePos(f_idxs[0], True)[b_idx]
                q = tm.quaternion_slerp(REST_QUAT,
                                        tm.quaternion_from_matrix(m, True),
                                        float(weights[0]))
                result[b_idx] = tm.quaternion_matrix(q)[:3, :4]
        else:
            for b_idx in xrange(self.nBones):
                m1[:3, :4] = self.getAtFramePos(f_idxs[0], True)[b_idx]
                m2[:3, :4] = self.getAtFramePos(f_idxs[1], True)[b_idx]
                q1 = tm.quaternion_slerp(REST_QUAT,
                                         tm.quaternion_from_matrix(m1, True),
                                         float(weights[0]))
                q2 = tm.quaternion_slerp(REST_QUAT,
                                         tm.quaternion_from_matrix(m2, True),
                                         float(weights[1]))
                quat = tm.quaternion_multiply(q2, q1)

                for i, f_idx in enumerate(f_idxs[2:]):
                    i += 2
                    m[:3, :4] = self.getAtFramePos(f_idx, True)[b_idx]
                    q = tm.quaternion_slerp(REST_QUAT,
                                            tm.quaternion_from_matrix(m, True),
                                            float(weights[i]))
                    quat = tm.quaternion_multiply(q, quat)

                result[b_idx] = tm.quaternion_matrix(quat)[:3, :4]

        return Pose(self.name + '-blended', result)
Esempio n. 6
0
 def get_local_matrix(self, quaternion_frame):
     if not self.fixed:
         frame_index = self.quaternion_frame_index * 4 + 3
         m = quaternion_matrix(quaternion_frame[frame_index:frame_index +
                                                4])
     else:
         m = quaternion_matrix(self.rotation)
     m[:3, 3] = self.offset
     return m
Esempio n. 7
0
def rotate_frames(frames, q):
    new_frames = []
    m = quaternion_matrix(q)
    for f in frames:
        new_frame = copy(f)
        new_frame[:3] = np.dot(m[:3, :3], f[:3])
        #new_frame[3:7] = quaternion_multiply(q, f[3:7])
        new_frame[3:7] = quaternion_from_matrix(
            np.dot(m, quaternion_matrix(f[3:7])))
        new_frames.append(new_frame)
    return np.array(new_frames)
 def set_joint_orientation(self, joint_name, target_q):
     global_q = self.skeleton.nodes[
         joint_name].get_global_orientation_quaternion(self.pose_parameters,
                                                       use_cache=False)
     global_m = quaternion_matrix(global_q)
     target_m = quaternion_matrix(target_q)
     delta_m = np.linalg.inv(global_m) * target_m
     local_m = self.skeleton.nodes[joint_name].get_local_matrix(
         self.pose_parameters)
     new_local_m = np.dot(delta_m, local_m)
     new_local_q = quaternion_from_matrix(new_local_m)
     self.set_channel_values(new_local_q, [joint_name])
Esempio n. 9
0
def quat_distance(quat_a, quat_b):
    # normalize quaternion vector first
    quat_a = np.asarray(quat_a)
    quat_b = np.asarray(quat_b)
    quat_a /= np.linalg.norm(quat_a)
    quat_b /= np.linalg.norm(quat_b)
    rotmat_a = quaternion_matrix(quat_a)
    rotmat_b = quaternion_matrix(quat_b)
    diff_mat = rotmat_a - rotmat_b
    tmp = np.ravel(diff_mat)
    diff = np.linalg.norm(tmp)
    return diff
Esempio n. 10
0
    def get_six_dof_act(self, transform_params, heightmap, pose0, pose1):
        """Adjust SE(3) poses via the in-plane SE(2) augmentation transform."""
        p1_position, p1_rotation = pose1[0], pose1[1]
        p0_position, p0_rotation = pose0[0], pose0[1]

        if transform_params is not None:
            t_world_center, t_world_centernew = utils.get_se3_from_image_transform(
                *transform_params, heightmap, self.bounds, self.pixel_size)
            t_worldnew_world = t_world_centernew @ np.linalg.inv(
                t_world_center)
        else:
            t_worldnew_world = np.eye(4)

        p1_quat_wxyz = (p1_rotation[3], p1_rotation[0], p1_rotation[1],
                        p1_rotation[2])
        t_world_p1 = transformations.quaternion_matrix(p1_quat_wxyz)
        t_world_p1[0:3, 3] = np.array(p1_position)

        t_worldnew_p1 = t_worldnew_world @ t_world_p1

        p0_quat_wxyz = (p0_rotation[3], p0_rotation[0], p0_rotation[1],
                        p0_rotation[2])
        t_world_p0 = transformations.quaternion_matrix(p0_quat_wxyz)
        t_world_p0[0:3, 3] = np.array(p0_position)
        t_worldnew_p0 = t_worldnew_world @ t_world_p0

        t_worldnew_p0theta0 = t_worldnew_p0 * 1.0
        t_worldnew_p0theta0[0:3, 0:3] = np.eye(3)

        # PLACE FRAME, adjusted for this 0 rotation on pick
        t_p0_p0theta0 = np.linalg.inv(t_worldnew_p0) @ t_worldnew_p0theta0
        t_worldnew_p1theta0 = t_worldnew_p1 @ t_p0_p0theta0

        # convert the above rotation to euler
        quatwxyz_worldnew_p1theta0 = transformations.quaternion_from_matrix(
            t_worldnew_p1theta0)
        q = quatwxyz_worldnew_p1theta0
        quatxyzw_worldnew_p1theta0 = (q[1], q[2], q[3], q[0])
        p1_rotation = quatxyzw_worldnew_p1theta0
        p1_euler = utils.quatXYZW_to_eulerXYZ(p1_rotation)

        roll_scaled = p1_euler[0] / self.theta_scale
        pitch_scaled = p1_euler[1] / self.theta_scale
        p1_theta_scaled = -p1_euler[2] / self.theta_scale

        x = p1_position[0]
        y = p1_position[1]
        z = p1_position[2]

        return np.array([x, y, p1_theta_scaled, roll_scaled, pitch_scaled, z])
Esempio n. 11
0
    def look(self):
        #code for visual transformation
        #glLoadIdentity()  
        # create a 4x4 rot matrix based on the quaternion orientation. nb, transpose required for opengl multmatrix
        t = transformations.quaternion_matrix(self.orientation)
        selfrot = np.ascontiguousarray(np.transpose(transformations.quaternion_matrix(self.orientation)))
        #glMultMatrixd(selfrot)
        #glTranslatef(*-self.position)
        #tt = glGetFloatv(GL_MODELVIEW_MATRIX)

        #NEW CODE MODERN
        R = selfrot
        T = transformations.translation_matrix(-self.position)
        self.view = np.dot(R,T).T
Esempio n. 12
0
 def get_transform(self):
     """ Copied from ThinMatrix shadow tutorial
     src: https://www.youtube.com/watch?v=o6zDfDkOFIc
     https://www.dropbox.com/sh/g9vnfiubdglojuh/AACpq1KDpdmB8ZInYxhsKj2Ma/shadows
     """
     yaw = math.radians(self.yaw)
     pitch = math.radians(self.pitch)
     rot_y = trans.quaternion_about_axis(-yaw, np.array([0, 1, 0]))
     rot_y = trans.quaternion_matrix(rot_y)
     rot_x = trans.quaternion_about_axis(-pitch, np.array([1, 0, 0]))
     rot_x = trans.quaternion_matrix(rot_x)
     transform = np.dot(rot_y, rot_x)
     transform[3, :3] = -self.get_world_position()
     return transform
Esempio n. 13
0
    def getBlendedPose(self, poses, weights, additiveBlending=True):
        import transformations as tm

        REST_QUAT = np.asarray([1,0,0,0], dtype=np.float32)

        if isinstance(poses[0], basestring):
            f_idxs = [self.getPoseNames().index(pname) for pname in poses]
        else:
            f_idxs = poses

        if not additiveBlending:
            # normalize weights
            if not isinstance(weights, np.ndarray):
                weights = np.asarray(weights, dtype=np.float32)
            t = sum(weights)
            if t < 1:
                # Fill up rest with neutral pose (neutral pose is assumed to be first frame)
                weights = np.asarray(weights + [1.0-t], dtype=np.float32)
                f_idxs.append(0)
            weights /= t

        #print zip([self.getPoseNames()[_f] for _f in f_idxs],weights)

        result = emptyPose(self.nBones)
        m = np.identity(4, dtype=np.float32)
        m1 = np.identity(4, dtype=np.float32)
        m2 = np.identity(4, dtype=np.float32)

        if len(f_idxs) == 1:
            for b_idx in xrange(self.nBones):
                m[:3, :4] = self.getAtFramePos(f_idxs[0], True)[b_idx]
                q = tm.quaternion_slerp(REST_QUAT, tm.quaternion_from_matrix(m, True), float(weights[0]))
                result[b_idx] = tm.quaternion_matrix( q )[:3,:4]
        else:
            for b_idx in xrange(self.nBones):
                m1[:3, :4] = self.getAtFramePos(f_idxs[0], True)[b_idx]
                m2[:3, :4] = self.getAtFramePos(f_idxs[1], True)[b_idx]
                q1 = tm.quaternion_slerp(REST_QUAT, tm.quaternion_from_matrix(m1, True), float(weights[0]))
                q2 = tm.quaternion_slerp(REST_QUAT, tm.quaternion_from_matrix(m2, True), float(weights[1]))
                quat = tm.quaternion_multiply(q2, q1)

                for i,f_idx in enumerate(f_idxs[2:]):
                    i += 2
                    m[:3, :4] = self.getAtFramePos(f_idx, True)[b_idx]
                    q = tm.quaternion_slerp(REST_QUAT, tm.quaternion_from_matrix(m, True), float(weights[i]))
                    quat = tm.quaternion_multiply(q, quat)

                result[b_idx] = tm.quaternion_matrix( quat )[:3,:4]

        return Pose(self.name+'-blended', result)
Esempio n. 14
0
def flip_custom_coordinate_system_legs(q):
    conv_m = np.array([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 1, 0], [0, 0, 0,
                                                                   1]])

    m = quaternion_matrix(q)
    new_m = np.dot(conv_m, np.dot(m, conv_m))
    q = quaternion_from_matrix(new_m)

    conv_m = np.array([[-1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0,
                                                                   1]])

    m = quaternion_matrix(q)
    new_m = np.dot(conv_m, np.dot(m, conv_m))
    q = quaternion_from_matrix(new_m)
    return q
Esempio n. 15
0
def get_object_goal_pose(object_pose, goal_rel_pose):
    assert len(object_pose) == len(goal_rel_pose) == 7
    
    Ms = tf.quaternion_matrix(object_pose[3:])
    Ms[:3,3] = object_pose[:3]

    M_rel = tf.quaternion_matrix(goal_rel_pose[3:])
    M_rel[:3,3] = goal_rel_pose[:3]

    M = Ms.dot(M_rel)

    quat_M = tf.quaternion_from_matrix(M)
    pose_M = np.concatenate([M[:3,3], quat_M])

    return pose_M
Esempio n. 16
0
def writeAnimation(fp, bone, action, config):
    aname = "Action_%s" % goodBoneName(bone.name)
    points = action[bone.name]
    npoints = len(points)

    fp.write(
        '    <animation id="%s_pose_matrix">\n' % aname +
        '      <source id="%s_pose_matrix-input">\n' % aname +
        '        <float_array id="%s_pose_matrix-input-array" count="%d">' % (aname, npoints))

    fp.write(''.join([" %g" % (0.04*(t+1)) for t in range(npoints)]))

    fp.write(
        '</float_array>\n' +
        '        <technique_common>\n' +
        '          <accessor source="#%s_pose_matrix-input-array" count="%d" stride="1">\n' % (aname, npoints) +
        '            <param name="TIME" type="float"/>\n' +
        '          </accessor>\n' +
        '        </technique_common>\n' +
        '      </source>\n' +
        '      <source id="%s_pose_matrix-output">\n' % aname +
        '        <float_array id="%s_pose_matrix-output-array" count="%d">\n' % (aname, 16*npoints))

    string = '          '
    relmat = bone.getRelativeMatrix(config.meshOrientation, config.localBoneAxis, config.offset)
    for quat in points:
        mat0 = tm.quaternion_matrix(quat)
        mat = np.dot(relmat, mat0)
        string += ''.join(['%g %g %g %g  ' % tuple(mat[i,:]) for i in range(4)])
        string += '\n          '
    fp.write(string)

    fp.write(
        '</float_array>\n' +
        '        <technique_common>\n' +
        '          <accessor source="#%s_pose_matrix-output-array" count="%d" stride="16">\n' % (aname, npoints) +
        '            <param name="TRANSFORM" type="float4x4"/>\n' +
        '          </accessor>\n' +
        '        </technique_common>\n' +
        '      </source>\n' +
        '      <source id="%s_pose_matrix-interpolation">\n' % aname +
        '        <Name_array id="%s_pose_matrix-interpolation-array" count="%d">' % (aname, npoints))

    fp.write(npoints * 'LINEAR ')

    fp.write(
        '</Name_array>\n' +
        '        <technique_common>\n' +
        '          <accessor source="#%s_pose_matrix-interpolation-array" count="%d" stride="1">\n' % (aname, npoints) +
        '            <param name="INTERPOLATION" type="name"/>\n' +
        '          </accessor>\n' +
        '        </technique_common>\n' +
        '      </source>\n' +
        '      <sampler id="%s_pose_matrix-sampler">\n' % aname +
        '        <input semantic="INPUT" source="#%s_pose_matrix-input"/>\n' % aname +
        '        <input semantic="OUTPUT" source="#%s_pose_matrix-output"/>\n' % aname +
        '        <input semantic="INTERPOLATION" source="#%s_pose_matrix-interpolation"/>\n' % aname +
        '      </sampler>\n' +
        '      <channel source="#%s_pose_matrix-sampler" target="%s/transform"/>\n' % (aname, goodBoneName(bone.name)) +
        '    </animation>\n')
Esempio n. 17
0
    def draw_trajectory(self, pose_array, angles, ns="default_ns"):
        decimation = max(len(pose_array.poses)//6, 1)
        ps = gm.PoseStamped()
        ps.header.frame_id = pose_array.header.frame_id        
        ps.header.stamp = rospy.Time.now()
        handles = []
 
        multiplier = 5.79 
        
        for (pose,angle) in zip(pose_array.poses,angles)[::decimation]:

            ps.pose = deepcopy(pose)
            pointing_axis = transformations.quaternion_matrix([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])[:3,0]
            ps.pose.position.x -= .18*pointing_axis[0]
            ps.pose.position.y -= .18*pointing_axis[1]
            ps.pose.position.z -= .18*pointing_axis[2]
            
            
            root_link = "r_wrist_roll_link"
            valuedict = {"r_gripper_l_finger_joint":angle*multiplier,
                         "r_gripper_r_finger_joint":angle*multiplier,
                         "r_gripper_l_finger_tip_joint":angle*multiplier,
                         "r_gripper_r_finger_tip_joint":angle*multiplier,
                         "r_gripper_joint":angle}
            handles.extend(self.place_kin_tree_from_link(ps, root_link, valuedict, ns=ns))
        return handles
Esempio n. 18
0
    def loadPoseFromMhpFile(self, filepath):
        """
        Load a MHP pose file that contains a static pose. Posing data is defined
        with quaternions to indicate rotation angles.
        Sets current pose to
        """
        log.message("Mhp %s", filepath)
        fp = open(filepath, "rU", encoding="utf-8")

        boneMap = self.getBoneToIdxMapping()
        nBones = len(boneMap.keys())
        poseMats = np.zeros((nBones,4,4),dtype=np.float32)
        poseMats[:] = np.identity(4, dtype=np.float32)

        for line in fp:
            words = line.split()
            if len(words) < 5:
                continue
            elif words[1] in ["quat", "gquat"]:
                boneIdx = boneMap[words[0]]
                quat = float(words[2]),float(words[3]),float(words[4]),float(words[5])
                mat = tm.quaternion_matrix(quat)
                if words[1] == "gquat":
                    bone = self.bones[boneIdx]
                    mat = np.dot(la.inv(bone.matRestRelative), mat)
                poseMats[boneIdx] = mat[:3,:3]

        fp.close()
        self.setPose(poseMats)
Esempio n. 19
0
 def readMhpFile(self, filepath):
     log.message("Loading MHP file %s", filepath)
     amt = self.armature
     fp = open(filepath, "rU")
     bname = None
     mat = np.identity(4, float)
     for line in fp:
         words = line.split()
         if len(words) < 4:
             continue
         if words[0] != bname:
             self.setMatrixPose(bname, mat)
             bname = words[0]
             mat = np.identity(4, float)
         if words[1] in ["quat", "gquat"]:
             quat = float(words[2]),float(words[3]),float(words[4]),float(words[5])
             mat = tm.quaternion_matrix(quat)
             if words[1] == "gquat":
                 pb = self.posebones[words[0]]
                 mat = np.dot(la.inv(pb.bone.matrixRelative), mat)
         elif words[1] == "scale":
             scale = 1+float(words[2]), 1+float(words[3]), 1+float(words[4])
             smat = tm.compose_matrix(scale=scale)
             mat = np.dot(smat, mat)
         elif words[1] == "matrix":
             rows = []
             n = 2
             for i in range(4):
                 rows.append((float(words[n]), float(words[n+1]), float(words[n+2]), float(words[n+3])))
                 n += 4
             mat = np.array(rows)
     self.setMatrixPose(bname, mat)
     fp.close()
     self.update()
Esempio n. 20
0
 def readMhpFile(self, filepath):
     log.message("Loading MHP file %s", filepath)
     amt = self.armature
     fp = open(filepath, "rU")
     bname = None
     mat = np.identity(4, float)
     for line in fp:
         words = line.split()
         if len(words) < 4:
             continue
         if words[0] != bname:
             self.setMatrixPose(bname, mat)
             bname = words[0]
             mat = np.identity(4, float)
         if words[1] in ["quat", "gquat"]:
             quat = float(words[2]),float(words[3]),float(words[4]),float(words[5])
             mat = tm.quaternion_matrix(quat)
             if words[1] == "gquat":
                 pb = self.posebones[words[0]]
                 mat = np.dot(la.inv(pb.bone.matrixRelative), mat)
         elif words[1] == "scale":
             scale = 1+float(words[2]), 1+float(words[3]), 1+float(words[4])
             smat = tm.compose_matrix(scale=scale)
             mat = np.dot(smat, mat)
         elif words[1] == "matrix":
             rows = []
             n = 2
             for i in range(4):
                 rows.append((float(words[n]), float(words[n+1]), float(words[n+2]), float(words[n+3])))
                 n += 4
             mat = np.array(rows)
     self.setMatrixPose(bname, mat)
     fp.close()
     self.update()
Esempio n. 21
0
def apply_to_matrix(x, A):
    """Applies the change of basis given by the SO3 quantity
    x to the 3by3 matrix M.

    To be clear, this performs:
    R * A * transpose(R)
    where R is the rotation matrix form of x.

    >>> A = np.random.rand(3, 3)
    >>> q = trns.random_quaternion()
    >>> R = trns.quaternion_matrix(q)
    >>> B1 = R[:3, :3].dot(A).dot(R[:3, :3].T)
    >>> B2 = apply_to_matrix(q, A)
    >>> print(np.allclose(B1, B2))
    True

    """
    A = np.array(A)
    if A.shape != (3, 3):
        raise ValueError("A must be 3 by 3.")
    xrep = rep(x)
    # Get rotation matrix if not rotmat:
    if xrep == 'rotvec':
        x = matrix_from_rotvec(x)
    elif xrep == 'quaternion':
        x = trns.quaternion_matrix(x)[:3, :3]
    elif xrep == 'tfmat':
        x = x[:3, :3]
    elif xrep != 'rotmat':
        raise ValueError("Quantity to apply is not on SO3.")
    # Apply change of basis to A:
    return x.dot(A).dot(x.T)
Esempio n. 22
0
    def draw_trajectory(self, pose_array, angles, ns="default_ns"):
        decimation = max(len(pose_array.poses) // 6, 1)
        ps = gm.PoseStamped()
        ps.header.frame_id = pose_array.header.frame_id
        ps.header.stamp = rospy.Time.now()
        handles = []

        multiplier = 5.79

        for (pose, angle) in zip(pose_array.poses, angles)[::decimation]:

            ps.pose = deepcopy(pose)
            pointing_axis = transformations.quaternion_matrix([
                pose.orientation.x, pose.orientation.y, pose.orientation.z,
                pose.orientation.w
            ])[:3, 0]
            ps.pose.position.x -= .18 * pointing_axis[0]
            ps.pose.position.y -= .18 * pointing_axis[1]
            ps.pose.position.z -= .18 * pointing_axis[2]

            root_link = "r_wrist_roll_link"
            valuedict = {
                "r_gripper_l_finger_joint": angle * multiplier,
                "r_gripper_r_finger_joint": angle * multiplier,
                "r_gripper_l_finger_tip_joint": angle * multiplier,
                "r_gripper_r_finger_tip_joint": angle * multiplier,
                "r_gripper_joint": angle
            }
            handles.extend(
                self.place_kin_tree_from_link(ps, root_link, valuedict, ns=ns))
        return handles
Esempio n. 23
0
    def build_bone_shape(cls, offset_vector, size, material=None):
        REF_VECTOR = [0, 1, 0]
        array_type = GL_QUADS
        uv_pos = -1
        normal_pos = 12
        weight_pos = -1
        color_pos = -1
        bone_id_pos = -1
        offset = 24
        rotation = np.eye(4)
        length = np.linalg.norm(offset_vector)
        offset_vector /= length
        q = quaternion_from_vector_to_vector(REF_VECTOR, offset_vector)
        q = quaternion_multiply(q, quaternion_about_axis(np.radians(180), [0, 1, 0]))
        rotation[:3, :3] = quaternion_matrix(q)[:3, :3]
        vertex_list = construct_quad_box_based_on_height(size, length, size)
        index_list = None

        m = Mesh(vertex_list, array_type, normal_pos=normal_pos, color_pos=color_pos,
                 uv_pos=uv_pos, weight_pos=weight_pos,
                 bone_id_pos=bone_id_pos, stride=offset,
                 index_list=index_list, material=material)

        m.transform = rotation
        return m
Esempio n. 24
0
  def extract_x_y_theta(self,
                        object_info,
                        t_worldaug_world=None,
                        preserve_theta=False):
    """Extract in-plane theta."""
    object_position = object_info[0]
    object_quat_xyzw = object_info[1]

    if t_worldaug_world is not None:
      object_quat_wxyz = (object_quat_xyzw[3], object_quat_xyzw[0],
                          object_quat_xyzw[1], object_quat_xyzw[2])
      t_world_object = transformations.quaternion_matrix(object_quat_wxyz)
      t_world_object[0:3, 3] = np.array(object_position)
      t_worldaug_object = t_worldaug_world @ t_world_object

      object_quat_wxyz = transformations.quaternion_from_matrix(
          t_worldaug_object)
      if not preserve_theta:
        object_quat_xyzw = (object_quat_wxyz[1], object_quat_wxyz[2],
                            object_quat_wxyz[3], object_quat_wxyz[0])
      object_position = t_worldaug_object[0:3, 3]

    object_xy = object_position[0:2]
    object_theta = -np.float32(
        utils.get_rot_from_pybullet_quaternion(object_quat_xyzw)
        [2]) / self.theta_scale
    return np.hstack(
        (object_xy,
         object_theta)).astype(np.float32), object_position, object_quat_xyzw
Esempio n. 25
0
def ur2np(ur_pose):
    # ROS pose
    ros_pose = []

    # ROS position
    ros_pose.append(ur_pose[0])
    ros_pose.append(ur_pose[1])
    ros_pose.append(ur_pose[2])

    # Ros orientation
    angle = sqrt(ur_pose[3]**2 + ur_pose[4]**2 + ur_pose[5]**2)
    direction = [i / angle for i in ur_pose[3:6]]
    np_T = tf.rotation_matrix(angle, direction)
    np_q = tf.quaternion_from_matrix(np_T)
    ros_pose.append(np_q[0])
    ros_pose.append(np_q[1])
    ros_pose.append(np_q[2])
    ros_pose.append(np_q[3])

    # orientation
    np_pose = tf.quaternion_matrix(
        [ros_pose[3], ros_pose[4], ros_pose[5], ros_pose[6]])

    # position
    np_pose[0][3] = ros_pose[0]
    np_pose[1][3] = ros_pose[1]
    np_pose[2][3] = ros_pose[2]

    return np_pose
Esempio n. 26
0
 def _log_data_att(self, timestamp, data, logconf):
     # NOTE q0 is real part of Kalman state's quaternion, but
     # transformations.py wants it as last dimension.
     self.attq = np.r_[data['kalman.q1'], data['kalman.q2'],
                       data['kalman.q3'], data['kalman.q0']]
     # Extract 3x3 rotation matrix from 4x4 transformation matrix
     self.R = trans.quaternion_matrix(self.attq)[:3, :3]
Esempio n. 27
0
def _WorldFromCameraFromViewDict(view_json):
    """Fills the world from camera transform from the view_json.

  Args:
    view_json: A dictionary of view parameters.

  Returns:
     A 4x4 transform matrix representing the world from camera transform.
  """

    # The camera model transforms the 3d point X into a ray u in the local
    # coordinate system:
    #
    #  u = R * (X[0:2] - X[3] * c)
    #
    # Meaning the world from camera transform is [inv(R), c]

    transform = np.identity(4)
    position = view_json['position']
    transform[0:3, 3] = (position[0], position[1], position[2])
    orientation = view_json['orientation']
    angle_axis = np.array([orientation[0], orientation[1], orientation[2]])
    angle = np.linalg.norm(angle_axis)
    epsilon = 1e-7
    if abs(angle) < epsilon:
        # No rotation
        return np.matrix(transform)

    axis = angle_axis / angle
    rot_mat = transformations.quaternion_matrix(
        transformations.quaternion_about_axis(-angle, axis))
    transform[0:3, 0:3] = rot_mat[0:3, 0:3]
    return np.matrix(transform)
Esempio n. 28
0
def writeAnimation(fp, bone, action, config):
    aname = "Action%s" % bone.name
    points = action[bone.name]

    for channel,default in [
            ("T", 0),
            ("R", 0),
            ("S", 1)
        ]:
        writeAnimationCurveNode(fp, bone, channel, default)

    relmat = bone.getRelativeMatrix(config.meshOrientation, config.localBoneAxis, config.offset)
    translations = []
    eulers = []
    R = 180/math.pi
    for quat in points:
        mat = tm.quaternion_matrix(quat)
        mat = np.dot(relmat, mat)
        translations.append(mat[:3,3])
        eul = tm.euler_from_matrix(mat, axes='sxyz')
        eulers.append((eul[0]*R, eul[1]*R, eul[2]*R))
    scales = len(points)*[(1,1,1)]

    for channel,data in [
            ("T", translations),
            ("R", eulers),
            ("S", scales)
        ]:
        for idx,coord in enumerate(["X","Y","Z"]):
            writeAnimationCurve(fp, idx, coord, bone, channel, data)
Esempio n. 29
0
def writeAnimation(fp, bone, action, config):
    aname = "Action%s" % bone.name
    points = action[bone.name]

    for channel,default in [
            ("T", 0),
            ("R", 0),
            ("S", 1)
        ]:
        writeAnimationCurveNode(fp, bone, channel, default)

    relmat = bone.getRelativeMatrix(config.meshOrientation, config.localBoneAxis, config.offset)
    translations = []
    eulers = []
    R = 180/math.pi
    for quat in points:
        mat = tm.quaternion_matrix(quat)
        mat = np.dot(relmat, mat)
        translations.append(mat[:3,3])
        eul = tm.euler_from_matrix(mat, axes='sxyz')
        eulers.append((eul[0]*R, eul[1]*R, eul[2]*R))
    scales = len(points)*[(1,1,1)]

    for channel,data in [
            ("T", translations),
            ("R", eulers),
            ("S", scales)
        ]:
        for idx,coord in enumerate(["X","Y","Z"]):
            writeAnimationCurve(fp, idx, coord, bone, channel, data)
Esempio n. 30
0
def regenerate_ankle_constraint_with_new_orientation2(position, joint_offset,
                                                      delta):
    """ move ankle so joint is on the ground using the new orientation"""
    m = quaternion_matrix(delta)[:3, :3]
    target_offset = np.dot(m, joint_offset)
    ca = position + target_offset
    return ca
Esempio n. 31
0
def recebe(msg):
    global x
    global y
    global z
    global id
    for marker in msg.markers:
        x = round(marker.pose.pose.position.x, 2)
        y = round(marker.pose.pose.position.y, 2)
        z = round(marker.pose.pose.position.z, 2)
        #print(x)
        id = marker.id
        #print(marker.pose.pose)
        if id == 100:
            print(
                buffer.can_transform("base_link", "ar_marker_100",
                                     rospy.Time(0)))
            header = Header(frame_id="ar_marker_100")
            trans = buffer.lookup_transform("base_link", "ar_marker_100",
                                            rospy.Time(0))
            t = transformations.translation_matrix([
                trans.transform.translation.x, trans.transform.translation.y,
                trans.transform.translation.z
            ])
            r = transformations.quaternion_matrix([
                trans.transform.rotation.x, trans.transform.rotation.y,
                trans.transform.rotation.z, trans.transform.rotation.w
            ])
            m = numpy.dot(r, t)
            v2 = numpy.dot(m, [0, 0, 1, 0])
            v2_n = v2[0:-1]
            n2 = v2_n / linalg.norm(v2_n)
            cosa = numpy.dot(n2, [1, 0, 0])
            print("ang", math.degrees(math.acos(cosa)))
Esempio n. 32
0
    def get_six_dof_object(self, object_info, t_worldaug_world=None):
        """Calculate the pose of 6DOF object."""
        object_position = object_info[0]
        object_quat_xyzw = object_info[1]

        if t_worldaug_world is not None:
            object_quat_wxyz = (object_quat_xyzw[3], object_quat_xyzw[0],
                                object_quat_xyzw[1], object_quat_xyzw[2])
            t_world_object = transformations.quaternion_matrix(
                object_quat_wxyz)
            t_world_object[0:3, 3] = np.array(object_position)
            t_worldaug_object = t_worldaug_world @ t_world_object

            object_quat_wxyz = transformations.quaternion_from_matrix(
                t_worldaug_object)
            object_quat_xyzw = (object_quat_wxyz[1], object_quat_wxyz[2],
                                object_quat_wxyz[3], object_quat_wxyz[0])
            object_position = t_worldaug_object[0:3, 3]

        euler = utils.quatXYZW_to_eulerXYZ(object_quat_xyzw)
        roll = euler[0]
        pitch = euler[1]
        theta = -euler[2]

        return np.asarray([
            object_position[0], object_position[1], object_position[2], roll,
            pitch, theta
        ])
Esempio n. 33
0
    def update(self):
        # Wait for the next set of frames from the camera
        frames = self.pipe.wait_for_frames()
        # Fetch pose frame
        pose = frames.get_pose_frame()
        if pose:
            # Print some of the pose data to the terminal
            data = pose.get_pose_data()
            H_T265Ref_T265body = tf.quaternion_matrix(
                [
                    data.rotation.w, data.rotation.x, data.rotation.y,
                    data.rotation.z
                ]
            )  # in transformations, Quaternions w+ix+jy+kz are represented as [w, x, y, z]!

            # transform to aeronautic coordinates (body AND reference frame!)
            H_aeroRef_aeroBody = self.H_aeroRef_T265Ref.dot(
                H_T265Ref_T265body.dot(self.H_T265body_aeroBody))

            rpy_rad = np.array(
                tf.euler_from_matrix(H_aeroRef_aeroBody, 'sxyz')
            )  # Rz(yaw)*Ry(pitch)*Rx(roll) body w.r.t. reference frame
            cor = [
                data.translation.z, data.translation.x, data.translation.z,
                rpy_rad[2], rpy_rad[1], rpy_rad[0]
            ]
        else:
            cor = [None, None, None, None, None, None]

        y, x, z, ry, rx, rz = cor

        scaled_x = x * 100
        scaled_y = y * 100
        self.pos = (scaled_x, scaled_y, ry)
def rotate_axes2(cos, q):
    m = quaternion_matrix(q)[:3, :3]
    aligned_axes = dict()
    for key, a in list(cos.items()):
        aligned_axes[key] = np.dot(m, a)
        aligned_axes[key] = normalize(aligned_axes[key])
    return aligned_axes
Esempio n. 35
0
 def setRotationIndex(self, index, angle, useQuat):
     """
     Set the rotation for one of the three rotation channels, either as
     quaternion or euler matrix. index should be 1,2 or 3 and represents
     x, y and z axis accordingly
     """
     if useQuat:
         quat = tm.quaternion_from_matrix(self.matPose)
         log.debug("%s", str(quat))
         quat[index] = angle/1000
         log.debug("%s", str(quat))
         _normalizeQuaternion(quat)
         log.debug("%s", str(quat))
         self.matPose = tm.quaternion_matrix(quat)
         return quat[0]*1000
     else:
         angle = angle*D
         ax,ay,az = tm.euler_from_matrix(self.matPose, axes='sxyz')
         if index == 1:
             ax = angle
         elif index == 2:
             ay = angle
         elif index == 3:
             az = angle
         mat = tm.euler_matrix(ax, ay, az, axes='sxyz')
         self.matPose[:3,:3] = mat[:3,:3]
         return 1000.0
def orient_end_effector_to_target(skeleton, root, end_effector, frame,
                                  constraint):
    """ find angle between the vectors end_effector - root and target- root """

    # align vectors
    root_pos = skeleton.nodes[root].get_global_position(frame)
    if constraint.offset is not None:
        m = skeleton.nodes[end_effector].get_global_matrix(frame)
        end_effector_pos = np.dot(m, constraint.offset)[:3]
    else:
        end_effector_pos = skeleton.nodes[end_effector].get_global_position(
            frame)

    src_delta = end_effector_pos - root_pos
    src_dir = src_delta / np.linalg.norm(src_delta)

    target_delta = constraint.position - root_pos
    target_dir = target_delta / np.linalg.norm(target_delta)

    root_delta_q = quaternion_from_vector_to_vector(src_dir, target_dir)
    root_delta_q = normalize(root_delta_q)

    if skeleton.nodes[root].stiffness > 0:
        t = 1 - skeleton.nodes[root].stiffness
        root_delta_q = quaternion_slerp([1, 0, 0, 0], root_delta_q, t)
        root_delta_q = normalize(root_delta_q)

    global_m = quaternion_matrix(root_delta_q)
    parent_joint = skeleton.nodes[root].parent.node_name
    parent_m = skeleton.nodes[parent_joint].get_global_matrix(frame,
                                                              use_cache=True)
    old_global = np.dot(parent_m, skeleton.nodes[root].get_local_matrix(frame))
    new_global = np.dot(global_m, old_global)
    q = quaternion_from_matrix(new_global)
    return normalize(q)
def look_at_target_projected(skeleton,
                             root,
                             end_effector,
                             frame,
                             position,
                             local_dir=LOOK_AT_DIR):
    """ find angle between the look direction and direction from end effector to target projected on the xz plane"""
    #direction of endeffector
    m = skeleton.nodes[root].get_global_matrix(frame)
    end_effector_dir = np.dot(m[:3, :3], local_dir)
    end_effector_dir[1] = 0
    end_effector_dir = end_effector_dir / np.linalg.norm(end_effector_dir)

    # direction from endeffector to target
    end_effector_pos = m[:3, 3]
    target_delta = position - end_effector_pos
    target_delta[1] = 0
    target_dir = target_delta / np.linalg.norm(target_delta)

    # find rotation to align vectors
    root_delta_q = quaternion_from_vector_to_vector(end_effector_dir,
                                                    target_dir)
    root_delta_q = normalize(root_delta_q)
    #apply global delta to get new global matrix of joint
    global_m = quaternion_matrix(root_delta_q)
    old_global = skeleton.nodes[root].get_global_matrix(frame)
    new_global = np.dot(global_m, old_global)
    q = quaternion_from_matrix(new_global)
    return normalize(q)
def look_at_target(skeleton,
                   root,
                   end_effector,
                   frame,
                   position,
                   local_dir=LOOK_AT_DIR):
    """ find angle between the look direction and direction between end effector and target"""
    #direction of endeffector
    m = skeleton.nodes[end_effector].get_global_matrix(frame)
    #offset = skeleton.nodes[end_effector].offset
    end_effector_dir = np.dot(m[:3, :3], local_dir)
    end_effector_dir = end_effector_dir / np.linalg.norm(end_effector_dir)

    # direction from endeffector to target
    end_effector_pos = m[:3, 3]
    target_delta = position - end_effector_pos
    target_dir = target_delta / np.linalg.norm(target_delta)

    # find rotation to align vectors
    root_delta_q = quaternion_from_vector_to_vector(end_effector_dir,
                                                    target_dir)
    root_delta_q = normalize(root_delta_q)

    #apply global delta to get new global matrix of joint
    global_m = quaternion_matrix(root_delta_q)
    parent_joint = skeleton.nodes[root].parent.node_name
    parent_m = skeleton.nodes[parent_joint].get_global_matrix(frame,
                                                              use_cache=True)
    old_global = np.dot(parent_m, skeleton.nodes[root].get_local_matrix(frame))
    new_global = np.dot(global_m, old_global)
    q = quaternion_from_matrix(new_global)
    return normalize(q)
Esempio n. 39
0
 def toMatrix(self):
     q0 = self.even
     mat = tm.quaternion_matrix(q0)
     q1 = self.odd        
     mat[0,3] = 2.0*(-q1[0]*q0[1] + q1[1]*q0[0] - q1[2]*q0[3] + q1[3]*q0[2])
     mat[1,3] = 2.0*(-q1[0]*q0[2] + q1[1]*q0[3] + q1[2]*q0[0] - q1[3]*q0[1])
     mat[2,3] = 2.0*(-q1[0]*q0[3] - q1[1]*q0[2] + q1[2]*q0[1] + q1[3]*q0[0])
     return mat
Esempio n. 40
0
	def step( self , dt ) :
		if not self.R.successful() : return
		self.Q = self.R.integrate(self.R.t+dt)
		if len(self.trace) > self.trace_len :
			self.trace.pop(0)
		if len(self.trace) < self.trace_len+1 :
			qm = tr.quaternion_matrix( self.Q[3:] )
			self.trace.append( np.dot( qm , self.x[-1] ) )
Esempio n. 41
0
	def __modify_quaternion( self , q , d ) :
		glPushMatrix()
		glLoadMatrixf( tr.quaternion_matrix( q ) )
		glRotatef( d[0] , 0 , 1 , 0 )
		glRotatef( d[1] , 1 , 0 , 0 )
		q = tr.quaternion_from_matrix( glGetDoublev(GL_MODELVIEW_MATRIX) )
		glPopMatrix()
		return q
def compute_ref_accuracy(fid_path, original_corrs_path,
                         geo_tform):

    #Load fiducial .ply
    fid = open(fid_path, 'r')
    fid_points = np.genfromtxt(fid, dtype=float, delimiter=' ',
                                skip_header=9)
    fid.close()
    #Load original corrs .ply
    fid = open(original_corrs_path, 'r')
    original_corrs = np.genfromtxt(fid, dtype=float,
                                    delimiter=' ', skip_header=9)
    fid.close()

    #Load transformation
    #************GEO**************"
    Tfis = open(geo_tform, 'r')
    lines = []
    lines = Tfis.readlines()
    scale_geo = float(lines[0])
    Ss_geo = tf.scale_matrix(scale_geo)
    quat_line = lines[1].split(" ")
    quat_geo = np.array([float(quat_line[3]), float(quat_line[0]),
                         float(quat_line[1]), float(quat_line[2])])
    Rs_geo = tf.quaternion_matrix(quat_geo)
    trans_line = lines[2].split(" ")
    trans_geo = np.array([float(trans_line[0]), float(trans_line[1]),
                          float(trans_line[2])])
    Tfis.close()
    Hs_geo = Rs_geo.copy()
    Hs_geo[:3, 3] = trans_geo[:3]
    Hs_geo = Ss_geo.dot(Hs_geo)

    LOG.debug("\n******Geo***** \n Scale: \n%s \nR:\n%s \nT:\n%s \nH:\n%s",
        Ss_geo, Rs_geo, trans_geo, Hs_geo)

    #Compute the "reference error"
    #i.e. fiducial points - geo registered correspondances
    npoints, c = fid_points.shape
    if npoints != 30:
        LOG.warn("Number of fiducial point is NOT 30")
    if c != 3:
        LOG.error("Fiducial points has the wrong number of dimensions")
    # import code; code.interact(local=locals())

    fid_points_hom = np.hstack((fid_points, np.ones([npoints, 1]))).T
    original_corrs_hom = np.hstack((original_corrs, np.ones([npoints, 1]))).T
    geo_corrs_hom = Hs_geo.dot(original_corrs_hom)
    geo_ref_diff = geo_corrs_hom - fid_points_hom

    # import pdb; pdb.set_trace()

    delta_z = np.sqrt(geo_ref_diff[2, :] * geo_ref_diff[2, :])
    delta_r = np.sqrt(geo_ref_diff[0, :] * geo_ref_diff[0, :] +
                      geo_ref_diff[1, :] * geo_ref_diff[1, :])

    return delta_z, delta_r
Esempio n. 43
0
def interp_transforms(T1, T2, alpha):
    assert alpha <= 1
    T_avg = alpha * T1 + (1 - alpha) * T2
    q1 = quaternion_from_matrix(T1)
    q2 = quaternion_from_matrix(T2)
    q3 = quaternion_slerp(q1, q2, alpha)
    R = quaternion_matrix(q3)
    T_avg[0:3, 0:3] = R[0:3, 0:3]
    return T_avg
Esempio n. 44
0
	def _draw_scene( self ) :
		glPushMatrix()
		glTranslatef( *self.b )
		glPushMatrix()
		glMultMatrixd( tr.quaternion_matrix( self.qb ) )
		self.frame.draw()
		glPopMatrix()

		glTranslatef( *self.c )
		glMultMatrixd( tr.quaternion_matrix( self.qc ) )
		self.frame.draw()
		glPopMatrix()

		glPushMatrix()
		glTranslatef( *self.e )
		glMultMatrixd( tr.quaternion_matrix( self.qe ) )
		self.frame.draw()
		glPopMatrix()
Esempio n. 45
0
 def on_timer(self, event):
     # auto rotating
     qy = tr.quaternion_about_axis(0.005, [0,0,1])
     qx = tr.quaternion_about_axis(0.005, [0,1,0])
     q = tr.quaternion_multiply(qx,qy)
     self.rot = tr.quaternion_multiply(self.rot, q)
     self.model = tr.quaternion_matrix(self.rot)
     self.program_data['u_model'] = self.model
     self.program_axis['u_model'] = self.model
     self.program_plane['u_model'] = self.model
     self.update()
Esempio n. 46
0
	def newPose(self, poseMsg):
		self.publish()
		q = [poseMsg.orientation.x, poseMsg.orientation.y, poseMsg.orientation.z, poseMsg.orientation.w]
		rq = transformations.quaternion_matrix(q)
		al, be, ga = transformations.euler_from_matrix(rq, 'rxyz')
		self.ga = ga
		x = poseMsg.position.x
		y = poseMsg.position.y
		z = poseMsg.position.z
		self.dist = math.sqrt(x*x + y*y + z*z)
		print al, be, ga
Esempio n. 47
0
 def matrix_slerp(matA, matB, alpha=0.6):
     if matA is None:
         return matB
     import transformations
     qA = transformations.quaternion_from_matrix(matA)
     qB = transformations.quaternion_from_matrix(matB)
     qC =transformations.quaternion_slerp(qA, qB, alpha)
     mat = matB.copy()
     mat[:3,3] = (alpha)*matA[:3,3] + (1-alpha)*matB[:3,3]
     mat[:3,:3] = transformations.quaternion_matrix(qC)[:3,:3]
     return mat
def apply_pose_error(tf1s, errors, euler_representation=True):
    tf0s = []
    for (tf1, error) in zip(tf1s, errors):
        tf0 = np.eye(4)
        tf0[:3,3] = tf1[:3,3] + error[:3]
        #tf0[:3,:3] = rave.rotationMatrixFromAxisAngle(error[3:]).dot(tf1[:3,:3])
        if euler_representation:
            tf0[:3,:3] = euler_matrix(*error[3:])[:3,:3].dot(tf1[:3,:3])
        else:
            tf0[:3,:3] = quaternion_matrix(*error[3:])[:3,:3].dot(tf1[:3,:3])
        tf0s.append(tf0)
    return tf0s
    def __init__(self, T):

        if ( type(T) == str):
            Tfile = T
            #Load transformation
            Tfis = open(Tfile, 'r')
            lines = []
            lines = Tfis.readlines()
            self.scale = float(lines[0])
            self.Ss = tf.scale_matrix(self.scale)
            quat_line = lines[1].split(" ")
            self.quat = tf.unit_vector(np.array([float(quat_line[3]),
                                            float(quat_line[0]),
                                            float(quat_line[1]),
                                            float(quat_line[2])]))
            self.Hs = tf.quaternion_matrix(self.quat)
            trans_line = lines[2].split(" ")
            self.Ts = np.array([float(trans_line[0]),
                           float(trans_line[1]),
                           float(trans_line[2])])
            Tfis.close()
            self.Rs = self.Hs.copy()[:3, :3]
            self.Hs[:3, 3] = self.Ts[:3]

            self.Hs = self.Ss.dot(self.Hs)  # to add again

        elif (type(T) == np.ndarray):
            self.Hs = T
            scale, shear, angles, trans, persp = tf.decompose_matrix(T)
            self.quat = tf.quaternion_from_euler(angles[0], angles[1], angles[2])
            self.Rs = tf.quaternion_matrix(self.quat)
            self.scale =scale[0]
            self.Ts = trans / self.scale


        print "Loaded Ground Truth Transformation: "
        print self.Hs
Esempio n. 50
0
    def calc_joint_local_matrix(self):
        self.L = np.eye(4)

        # Add in rotation
        q = np.r_[0.0, 0.0, 0.0, 1.0]
        for i in range(3):
            rot_vec = np.r_[0.0, 0.0, 0.0]
            rot_vec[i] = 1.0
            q = tr.quaternion_about_axis(np.radians(-self.rotation[i]), rot_vec)
            # q = tr.quaternion_multiply(q, qi)
            Q = np.matrix(tr.quaternion_matrix(q))
            self.L = np.dot(self.L, Q)

            # Add in translation on the first three columns of bottom row
        self.L[3, :3] = self.translation
Esempio n. 51
0
 def readMhpFile(self, filepath):
     log.message("Mhp %s", filepath)
     fp = open(filepath, "rU")
     for line in fp:
         words = line.split()
         if len(words) < 5:
             continue
         elif words[1] in ["quat", "gquat"]:
             bone = self.bones[words[0]]
             quat = float(words[2]),float(words[3]),float(words[4]),float(words[5])
             mat = tm.quaternion_matrix(quat)
             if words[1] == "gquat":
                 mat = dot(inv(bone.matrixRelative), mat)
             bone.matrixPose[:3,:3] = mat[:3,:3]
     fp.close()
     self.update()                    
Esempio n. 52
0
def trans_rot_to_hmat(trans, rot): 
    ''' 
    Converts a rotation and translation to a homogeneous transform. 

    **Args:** 

        **trans (np.array):** Translation (x, y, z). 

        **rot (np.array):** Quaternion (x, y, z, w). 

    **Returns:** 
        H (np.array): 4x4 homogenous transform matrix. 
    ''' 
    H = transformations.quaternion_matrix(rot) 
    H[0:3, 3] = trans 
    return H
def set_wbs_transf():
    global workspace, slocations, inv_trans
    inv_trans = []

    for work_loc in workspace:
        for slocation in slocations:
            if work_loc == slocation['name']:
                translation = copy.deepcopy(slocation['position'])
                translation = np.array(translation)
                quaternion = copy.deepcopy(slocation['orientation'])
                rot_mat = quaternion_matrix(quaternion)
                inv_trans.append({'name':slocation['name'], 
                                  'translation': translation, 
                                  'rotation': rot_mat})
                break

    return
Esempio n. 54
0
 def readMhpFile(self, filepath):
     log.message("Mhp %s", filepath)
     amt = self.armature
     print((list(self.posebones.keys())))
     fp = open(filepath, "rU")
     for line in fp:
         words = line.split()
         if len(words) < 5:
             continue
         elif words[1] in ["quat", "gquat"]:
             pb = self.posebones[words[0]]
             quat = float(words[2]),float(words[3]),float(words[4]),float(words[5])
             mat = tm.quaternion_matrix(quat)
             if words[1] == "gquat":
                 mat = np.dot(la.inv(pb.bone.matrixRelative), mat)
             pb.matrixPose[:3,:3] = mat[:3,:3]
     fp.close()
     self.update()
Esempio n. 55
0
    def calcBindMatrices(self):
        import io_json
        self.bindMatrix = tm.rotation_matrix(math.pi/2, XUnit)
        self.bindInverse = la.inv(self.bindMatrix)

        if self.options.useTPose:
            filepath = "tools/blender26x/mh_mocap_tool/t_pose.json"
            blist = io_json.loadJson(filepath)
            for bname,quat in blist:
                pmat = tm.quaternion_matrix(quat)
                log.debug("TPose %s %s" % ( bname, pmat))
                self.bones[bname].matrixTPose = pmat
        else:
            for bone in list(self.bones.values()):
                bone.matrixTPose = None

        for bone in list(self.bones.values()):
            bone.calcBindMatrix()
Esempio n. 56
0
	def draw( self ) :
		qm = tr.quaternion_matrix( self.Q[3:] )
		if self.drawstate & MESH :
			glPushMatrix()
			glMultTransposeMatrixf( qm )
			Mesh.draw( self )
			glPopMatrix()

		if self.drawstate & WIREFRAME :
			glPushMatrix()
			glMultTransposeMatrixf( qm )
			glDisable(GL_LIGHTING)
			glPolygonMode(GL_FRONT_AND_BACK, GL_LINE)
			glDisable(GL_CULL_FACE)
			Mesh.draw( self )
			glBegin(GL_LINES)
			glVertex3f(0,0,0)
			glVertex3f( self.x[-1,0] , self.x[-1,1] , self.x[-1,2] )
			glEnd()
			glEnable(GL_CULL_FACE)
			glPolygonMode(GL_FRONT_AND_BACK, GL_FILL)
			glEnable(GL_LIGHTING)
			glPopMatrix()

		if self.drawstate & TRACE :
			glDisable(GL_LIGHTING)
			glBegin(GL_POINTS)
			for p in self.trace : glVertex3f( *p[:3] )
			glEnd()
			glEnable(GL_LIGHTING)

		if self.drawstate & GRAVITY :
			glPushMatrix()
			glDisable(GL_LIGHTING)
			glTranslatef( 2 , 2 , 0 )
			glScalef(.1,.1,.1)
			glMultTransposeMatrixf( qm )
			glColor3f(1,.5,0)
			glBegin(GL_LINES)
			glVertex3f( 0 , 0 , 0 )
			glVertex3f( *self.G )
			glEnd()
			glEnable(GL_LIGHTING)
			glPopMatrix()
Esempio n. 57
0
def to_rotation_matrix(op):
    if op is None: #shorthand for identity
        op = identity_rotation.copy()
    else:
        op = np.matrix(op)
        if op.shape == (1,4): #quaternion
            op = transformations.quaternion_matrix(np.array(op)[0, ...])
        elif op.shape == (1,3): #euler angles
            op = transformations.euler_matrix(*np.array(op).reshape(3))
        if op.shape == (4,4): #universal transformation matrix
            #ensure all transformations are rotational
            def check_slice(slc):
                assert all_epsilon_eq(slc, 0, 1e-10), 'bad slice %r' % (slc,)
            check_slice(op[-1, :3:])
            check_slice(op[:3:, -1])
            assert epsilon_eq(op[3,3], 1, 1e-10)
            op = np.asmatrix(op[:3, :3])
    assert op.shape == (3,3)
    return op
Esempio n. 58
0
def qapply_matrix(q, A):
    """Applies the change of basis given by the quaternion
    q to the 3by3 matrix A.

    Performs
    R * A * transpose(R)
    where R is the rotation matrix form of q.

    >>> A = np.random.rand(3, 3)
    >>> q = trns.random_quaternion()
    >>> R = trns.quaternion_matrix(q)
    >>> B1 = R[:3, :3].dot(A).dot(R[:3, :3].T)
    >>> B2 = qapply_matrix(q, A)
    >>> print(np.allclose(B1, B2))
    True

    """
    R = trns.quaternion_matrix(q)[:3, :3]
    return R.dot(A).dot(R.T)
Esempio n. 59
0
 def setRotationIndex(self, index, angle, useQuat):
     if useQuat:
         quat = tm.quaternion_from_matrix(self.matrixPose)
         log.debug("%s", str(quat))
         quat[index] = angle/1000
         log.debug("%s", str(quat))
         normalizeQuaternion(quat)
         log.debug("%s", str(quat))
         self.matrixPose = tm.quaternion_matrix(quat)
         return quat[0]*1000    
     else:
         angle = angle*D
         ax,ay,az = tm.euler_from_matrix(self.matrixPose, axes='sxyz')
         if index == 1:
             ax = angle
         elif index == 2:
             ay = angle
         elif index == 3:
             az = angle
         mat = tm.euler_matrix(ax, ay, az, axes='sxyz')
         self.matrixPose[:3,:3] = mat[:3,:3]
         return 1000.0
    def __init__(self, geo_tform):

        #Load transformation
        Tfis = open(geo_tform, 'r')
        lines = []
        lines = Tfis.readlines()
        self.scale_geo = float(lines[0])
        self.Ss_geo = tf.scale_matrix(self.scale_geo)
        quat_line = lines[1].split(" ")
        quat_geo = np.array([float(quat_line[3]), float(quat_line[0]),
                             float(quat_line[1]), float(quat_line[2])])
        self.Rs_geo = tf.quaternion_matrix(quat_geo)
        trans_line = lines[2].split(" ")
        self.trans_geo = np.array([float(trans_line[0]), float(trans_line[1]),
                              float(trans_line[2])])
        Tfis.close()
        self.Hs_geo = self.Rs_geo.copy()
        self.Hs_geo[:3, 3] = self.trans_geo[:3]
        self.Hs_geo = self.Ss_geo.dot(self.Hs_geo)

        print "Loaded GeoTransformation: "
        print self.Hs_geo