示例#1
0
	def __init__( self , maxt , fovy , ratio , near , far ) :
		Scene.__init__(self,fovy,ratio,near,far)

		self.maxt = maxt

		self.frame = Frame()

		self.b = np.array((-5,0,0) , np.float64 )
		self.c = np.array(( 0,0,0) , np.float64 )
		self.e = np.array(( 5,0,0) , np.float64 )

		self.qb = tr.random_quaternion()
		self.qc = self.qb
		self.qe = tr.random_quaternion()
def averageTransforms(l_transforms):
    N = len(l_transforms)
    print("Computing the average of " + str(N) + " transforms")

    # Get a list of all the translations l_t
    l_t = [i[0] for i in l_transforms]
    # print(l_t)

    # compute the average translation
    tmp1 = sum(v[0] for v in l_t) / N
    tmp2 = sum(v[1] for v in l_t) / N
    tmp3 = sum(v[2] for v in l_t) / N
    avg_t = (tmp1, tmp2, tmp3)
    # print(avg_t)

    # Get a list of the rotation quaternions
    l_q = [i[1] for i in l_transforms]
    #print l_q

    # Average the quaterions using an incremental slerp approach
    acc = 1.0  # accumulator, counts the number of observations inserted already
    avg_q = random_quaternion(
    )  # can be random for start, since it will not be used
    for q in l_q:
        # How to deduce the ratio on each iteration
        # w_q = 1 / (acc + 1)
        # w_qavg = acc  / (acc + 1)
        # ratio = w_q / w_qavg <=> 1 / acc
        avg_q = quaternion_slerp(avg_q, q, 1.0 / acc)  # run pairwise slerp
        acc = acc + 1  # increment acc

    avg_q = tuple(avg_q)
    # print(avg_q)

    return (avg_t, avg_q)
示例#3
0
    def test_euler_init(self):
        random_e = ttf.euler_from_quaternion(ttf.random_quaternion())
        ornt = Orientation(random_e)
        euler = ornt.euler
        quat = ornt.quaternion
        matrix = ornt.matrix3

        self.assertTrue(np.allclose(euler, random_e))
        self.assertTrue(np.allclose(
            matrix, ttf.euler_matrix(*random_e)[:3, :3]))
        self.assertTrue(np.allclose(
            quat, ttf.quaternion_from_euler(*random_e)))
示例#4
0
    def test_quaternion_init(self):
        random_q = ttf.random_quaternion()
        ornt = Orientation(random_q)
        euler = ornt.euler
        quat = ornt.quaternion
        matrix = ornt.matrix3

        self.assertTrue(np.allclose(euler,
                                    ttf.euler_from_quaternion(random_q)))
        self.assertTrue(np.allclose(matrix,
                                    ttf.quaternion_matrix(random_q)[:3, :3]))
        self.assertTrue(np.allclose(quat, random_q))
################################################# SETUP

# Define time:
dt = 0.01  # global time step (s)
T = 10  # simulation duration (s)
t_arr = np.arange(0, T, dt)  # time values array (s)

# Define body inertia:
I = np.array([[ 1   , 0.01  , 0.02],
              [0.01 ,  2    , 0.03],
              [0.02 , 0.03  ,  3 ]])  # inertia matrix in body frame (kg*m^2)
# I = np.diag([1, 2, 3])  # cool to compare to diagonal inertia matrix case (which would imply body frame is principal frame)
invI = npl.inv(I)  # store inverse for future use

# Define state and initial conditions:
q = trns.random_quaternion()  # orientation state quaternion representing a conversion ***from body frame to world frame***
w = 10 * (np.random.rand(3) - 0.5)  # angular velocity state (rad/s) in world frame
torque = np.array([0, 0, 0]) # initial control input torque, need only be initialized for programming purposes
print('\nInitial Orientation: {}'.format(q))
print('Initial Ang. Velocity: {}'.format(np.rad2deg(w)))

# Controller setup (set gains to 0 if you want to test torque-free precession):
q_des = trns.random_quaternion()  # desired orientation state
w_des = np.array([0, 0, 0])  # desired angular velocity state ()
kp = np.array([150, 150, 150])  # proportional gain (body frame roll, pitch, yaw)
kd = np.array([170, 170, 170])  # derivative gain (body frame rolling, pitching, yawing)
print('Desired Orientation: {}'.format(q_des))
print('Desired Ang. Velocity: {}'.format(np.rad2deg(w_des)))
print('Proportional Gains: {}'.format(kp))
print('Derivative Gains: {}'.format(kd))
示例#6
0
def random_quaternion(rand=None):
    trans_quat = trans.random_quaternion(rand)
    return to_pyquat(trans_quat)
def _random_quaternion(dtype):
    return t.random_quaternion().astype(dtype)
示例#8
0
def random_pose(radius):
    """Returns:
        Quaternion shape==(4,), Transformation shape==(3,)
    """
    return transformations.random_quaternion(), (np.random.rand(3) * 2 - 1) * radius
示例#9
0
from playbackUtils import *
import transformations as t
# from tongsCenter import *
from tongsCenter import *
# from usingRosBag_linear_version0 import *
from usingRosBag_linear_version2 import *
import numpy as np

rq = t.random_quaternion()

utils = PlaybackUtils(None)
utils.transformQuat(rq)
quatRot = t.quaternion_from_euler(0, 0, 90)
print quatRot

tc = GetTongsTransform()
'''
rbp = RosBagParser(resampleRate=1000)
rbp.parseTongsBag('/home/danny/Desktop/bagFiles/bagFiles_1_13_17/assemblingLegos.bag')
print len(rbp.vivePos_interpolated)
print len(rbp.viveQuat_interpolated)
print rbp.resample_time_stamp[12]
'''

A = np.array([1.0, 5.0, 6.0, 7.0, 11.0])
print tc.getTongsDistance([0, 0, 0], [1, 0, 0, 0], 6.5)

a = np.zeros((4, 4))
a[:, 0] = np.array([0, 1, 2, 3])
print a