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)
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)))
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))
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)
def random_pose(radius):
"""Returns:
Quaternion shape==(4,), Transformation shape==(3,)
"""
return transformations.random_quaternion(), (np.random.rand(3) * 2 - 1) * radius
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
