def interpolationQuatData(time_array, data_array, new_time_array):
'''
We have to use SLERP for start-goal quaternion interpolation.
But, I cound not find any good library for quaternion array interpolation.
time_array: N - length array
data_array: 4 x N - length array
'''
from scipy import interpolate
n, m = np.shape(data_array)
if len(time_array) > m:
time_array = time_array[0:m]
new_data_array = None
l = len(time_array)
new_l = len(new_time_array)
idx_list = np.linspace(0, l - 1, new_l)
for idx in idx_list:
if new_data_array is None:
new_data_array = qt.slerp(data_array[:, int(idx)],
data_array[:, int(np.ceil(idx))],
idx - int(idx))
else:
new_data_array = np.vstack([new_data_array,
qt.slerp( data_array[:,int(idx)], data_array[:,int(np.ceil(idx))], \
idx-int(idx) )])
return new_data_array.T
def updateFrames(self, tag_id, frame):
with self.bundle_dict_lock:
tag_flag = False
for key in self.bundle_dict.keys():
if key == str(tag_id):
tag_flag = True
self.bundle_dict[key].p = (self.bundle_dict[key].p + frame.p)/2.0
pre_quat = geometry_msgs.msg.Quaternion()
pre_quat.x = self.bundle_dict[key].M.GetQuaternion()[0]
pre_quat.y = self.bundle_dict[key].M.GetQuaternion()[1]
pre_quat.z = self.bundle_dict[key].M.GetQuaternion()[2]
pre_quat.w = self.bundle_dict[key].M.GetQuaternion()[3]
cur_quat = geometry_msgs.msg.Quaternion()
cur_quat.x = frame.M.GetQuaternion()[0]
cur_quat.y = frame.M.GetQuaternion()[1]
cur_quat.z = frame.M.GetQuaternion()[2]
cur_quat.w = frame.M.GetQuaternion()[3]
quat = qt.slerp(pre_quat, cur_quat, 0.5)
self.bundle_dict[key].M = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
# New tag
if tag_flag == False:
self.bundle_list.append(tag_id)
self.bundle_dict[str(tag_id)] = frame
print "Detected tags: ", self.bundle_list
def updateFrames(self, tag_id, frame):
with self.bundle_dict_lock:
tag_flag = False
for key in self.bundle_dict.keys():
if key == str(tag_id):
tag_flag = True
self.bundle_dict[key].p = (self.bundle_dict[key].p +
frame.p) / 2.0
pre_quat = geometry_msgs.msg.Quaternion()
pre_quat.x = self.bundle_dict[key].M.GetQuaternion()[0]
pre_quat.y = self.bundle_dict[key].M.GetQuaternion()[1]
pre_quat.z = self.bundle_dict[key].M.GetQuaternion()[2]
pre_quat.w = self.bundle_dict[key].M.GetQuaternion()[3]
cur_quat = geometry_msgs.msg.Quaternion()
cur_quat.x = frame.M.GetQuaternion()[0]
cur_quat.y = frame.M.GetQuaternion()[1]
cur_quat.z = frame.M.GetQuaternion()[2]
cur_quat.w = frame.M.GetQuaternion()[3]
quat = qt.slerp(pre_quat, cur_quat, 0.5)
self.bundle_dict[key].M = PyKDL.Rotation.Quaternion(
quat.x, quat.y, quat.z, quat.w)
# New tag
if tag_flag == False:
self.bundle_list.append(tag_id)
self.bundle_dict[str(tag_id)] = frame
print "Detected tags: ", self.bundle_list
def updateBowlcenFrames(self, bowl_frame):
bowl_cen_frame = copy.deepcopy(bowl_frame)
## Rotation
rot = bowl_cen_frame.M
## bowl_z = np.array([rot.UnitX()[0], rot.UnitX()[1], rot.UnitX()[2]])
tx = PyKDL.Vector(1.0, 0.0, 0.0)
ty = PyKDL.Vector(0.0, 1.0, 0.0)
# Projection to xy plane
px = PyKDL.dot(tx, rot.UnitZ())
py = PyKDL.dot(ty, rot.UnitZ())
bowl_cen_y = rot.UnitY()
bowl_cen_z = PyKDL.Vector(px, py, 0.0)
bowl_cen_z.Normalize()
bowl_cen_x = bowl_cen_y * bowl_cen_z
bowl_cen_y = bowl_cen_z * bowl_cen_x
bowl_cen_rot = PyKDL.Rotation(bowl_cen_x, bowl_cen_y, bowl_cen_z)
bowl_cen_frame.M = bowl_cen_rot
## Position
bowl_cen_frame.p[2] -= self.bowl_z_offset
bowl_cen_frame.p += bowl_cen_z * self.bowl_forward_offset
if bowl_cen_y[2] > bowl_cen_x[2]:
# -90 x axis rotation
bowl_cen_frame = bowl_cen_frame * self.x_neg90_frame
else:
# 90 y axis rotation
bowl_cen_frame = bowl_cen_frame * self.y_90_frame
bowl_cen_frame_off = bowl_frame.Inverse() * bowl_cen_frame
if self.bowl_cen_frame_off == None:
self.bowl_cen_frame_off = bowl_cen_frame_off
else:
self.bowl_cen_frame_off.p = (self.bowl_cen_frame_off.p +
bowl_cen_frame_off.p) / 2.0
pre_quat = geometry_msgs.msg.Quaternion()
pre_quat.x = self.bowl_cen_frame_off.M.GetQuaternion()[0]
pre_quat.y = self.bowl_cen_frame_off.M.GetQuaternion()[1]
pre_quat.z = self.bowl_cen_frame_off.M.GetQuaternion()[2]
pre_quat.w = self.bowl_cen_frame_off.M.GetQuaternion()[3]
cur_quat = geometry_msgs.msg.Quaternion()
cur_quat.x = bowl_cen_frame_off.M.GetQuaternion()[0]
cur_quat.y = bowl_cen_frame_off.M.GetQuaternion()[1]
cur_quat.z = bowl_cen_frame_off.M.GetQuaternion()[2]
cur_quat.w = bowl_cen_frame_off.M.GetQuaternion()[3]
quat = qt.slerp(pre_quat, cur_quat, 0.5)
self.bowl_cen_frame_off.M = PyKDL.Rotation.Quaternion(
quat.x, quat.y, quat.z, quat.w)
def updateBowlcenFrames(self, bowl_frame):
bowl_cen_frame = copy.deepcopy(bowl_frame)
## Rotation
rot = bowl_cen_frame.M
## bowl_z = np.array([rot.UnitX()[0], rot.UnitX()[1], rot.UnitX()[2]])
tx = PyKDL.Vector(1.0, 0.0, 0.0)
ty = PyKDL.Vector(0.0, 1.0, 0.0)
# Projection to xy plane
px = PyKDL.dot(tx, rot.UnitZ())
py = PyKDL.dot(ty, rot.UnitZ())
bowl_cen_y = rot.UnitY()
bowl_cen_z = PyKDL.Vector(px, py, 0.0)
bowl_cen_z.Normalize()
bowl_cen_x = bowl_cen_y * bowl_cen_z
bowl_cen_y = bowl_cen_z * bowl_cen_x
bowl_cen_rot = PyKDL.Rotation(bowl_cen_x, bowl_cen_y, bowl_cen_z)
bowl_cen_frame.M = bowl_cen_rot
## Position
bowl_cen_frame.p[2] -= self.bowl_z_offset
bowl_cen_frame.p += bowl_cen_z * self.bowl_forward_offset
if bowl_cen_y[2] > bowl_cen_x[2]:
# -90 x axis rotation
bowl_cen_frame = bowl_cen_frame * self.x_neg90_frame
else:
# 90 y axis rotation
bowl_cen_frame = bowl_cen_frame * self.y_90_frame
bowl_cen_frame_off = bowl_frame.Inverse()*bowl_cen_frame
if self.bowl_cen_frame_off == None:
self.bowl_cen_frame_off = bowl_cen_frame_off
else:
self.bowl_cen_frame_off.p = (self.bowl_cen_frame_off.p + bowl_cen_frame_off.p)/2.0
pre_quat = geometry_msgs.msg.Quaternion()
pre_quat.x = self.bowl_cen_frame_off.M.GetQuaternion()[0]
pre_quat.y = self.bowl_cen_frame_off.M.GetQuaternion()[1]
pre_quat.z = self.bowl_cen_frame_off.M.GetQuaternion()[2]
pre_quat.w = self.bowl_cen_frame_off.M.GetQuaternion()[3]
cur_quat = geometry_msgs.msg.Quaternion()
cur_quat.x = bowl_cen_frame_off.M.GetQuaternion()[0]
cur_quat.y = bowl_cen_frame_off.M.GetQuaternion()[1]
cur_quat.z = bowl_cen_frame_off.M.GetQuaternion()[2]
cur_quat.w = bowl_cen_frame_off.M.GetQuaternion()[3]
quat = qt.slerp(pre_quat, cur_quat, 0.5)
self.bowl_cen_frame_off.M = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
def updateMouthFrames(self, head_frame):
mouth_frame = copy.deepcopy(head_frame)
## Position
mouth_frame.p[2] -= self.head_z_offset
## Rotation
rot = mouth_frame.M
## head_z = np.array([rot.UnitX()[0], rot.UnitX()[1], rot.UnitX()[2]])
tx = PyKDL.Vector(1.0, 0.0, 0.0)
ty = PyKDL.Vector(0.0, 1.0, 0.0)
# Projection to xy plane
px = PyKDL.dot(tx, rot.UnitZ())
py = PyKDL.dot(ty, rot.UnitZ())
mouth_y = rot.UnitY()
mouth_z = PyKDL.Vector(px, py, 0.0)
mouth_z.Normalize()
mouth_x = mouth_y * mouth_z
mouth_y = mouth_z * mouth_x
mouth_rot = PyKDL.Rotation(mouth_x, mouth_y, mouth_z)
mouth_frame.M = mouth_rot
mouth_frame_off = head_frame.Inverse()*mouth_frame
if self.mouth_frame_off == None:
self.mouth_frame_off = mouth_frame_off
else:
self.mouth_frame_off.p = (self.mouth_frame_off.p + mouth_frame_off.p)/2.0
pre_quat = geometry_msgs.msg.Quaternion()
pre_quat.x = self.mouth_frame_off.M.GetQuaternion()[0]
pre_quat.y = self.mouth_frame_off.M.GetQuaternion()[1]
pre_quat.z = self.mouth_frame_off.M.GetQuaternion()[2]
pre_quat.w = self.mouth_frame_off.M.GetQuaternion()[3]
cur_quat = geometry_msgs.msg.Quaternion()
cur_quat.x = mouth_frame_off.M.GetQuaternion()[0]
cur_quat.y = mouth_frame_off.M.GetQuaternion()[1]
cur_quat.z = mouth_frame_off.M.GetQuaternion()[2]
cur_quat.w = mouth_frame_off.M.GetQuaternion()[3]
# check close quaternion and inverse
if np.dot(self.mouth_frame_off.M.GetQuaternion(), mouth_frame_off.M.GetQuaternion()) < 0.0:
cur_quat.x *= -1.
cur_quat.y *= -1.
cur_quat.z *= -1.
cur_quat.w *= -1.
quat = qt.slerp(pre_quat, cur_quat, 0.5)
self.mouth_frame_off.M = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)