def RediscretizeTrajectory(self, traj, num_between_points):
or_points = len(traj)
if (num_between_points < 1):
return None
new_traj = []
for i in range(0, or_points - 1):
cur_traj_point = traj[i]
next_traj_point = traj[i + 1]
old_pos = cur_traj_point[0:3, 3]
next_pos = next_traj_point[0:3, 3]
pos_diff = (next_pos - old_pos)
or_quat = transformations.quaternion_from_matrix(cur_traj_point)
next_quat = transformations.quaternion_from_matrix(next_traj_point)
new_traj.append(np.copy(cur_traj_point))
for j in range(0, num_between_points):
fraction = float(j + 1) / float(num_between_points + 1)
interp_quat = transformations.quaternion_slerp(
or_quat, next_quat, fraction)
interp_pos = old_pos + (fraction * pos_diff)
interp_trans = transformations.quaternion_translation_matrix(
interp_quat, interp_pos)
new_traj.append(np.copy(interp_trans))
new_traj.append(np.copy(traj[len(traj) - 1])) #append last traj point
return new_traj
def update_all_transforms(self, all_frames):
# positions = frames[0]
# rotations = frames[1]
positions = []
rotations = []
for f in all_frames:
for i, p in enumerate(f[0]):
positions.append(f[0][i])
rotations.append(f[1][i])
for c in self.collision_objects:
if c.type == 'robot_link':
name = c.name
name_arr = name.split('_')
arm_id = int(name_arr[1])
link_id = int(name_arr[2])
ptA = all_frames[arm_id][0][link_id]
ptB = all_frames[arm_id][0][link_id + 1]
midPt = ptA + 0.5 * (ptB - ptA)
final_pos = midPt
rot_mat = np.zeros((3, 3))
z = ptB - ptA
norm = max(np.linalg.norm(z), 0.000001)
z = (1.0 / norm) * z
up = np.array([0, 0, 1])
if np.dot(z, up) == 1.0:
up = np.array([1, 0, 0])
x = np.cross(up, z)
y = np.cross(z, x)
rot_mat[:, 0] = x
rot_mat[:, 1] = y
rot_mat[:, 2] = z
final_quat = T.quaternion_from_matrix(rot_mat)
else:
coordinate_frame = c.coordinate_frame
# first, do local transforms
frame_len = len(positions)
if coordinate_frame == 0:
rot_mat = rotations[0]
final_pos = positions[0]
elif coordinate_frame >= frame_len:
rot_mat = rotations[frame_len - 1]
final_pos = positions[frame_len - 1]
else:
rot_mat = rotations[coordinate_frame]
final_pos = positions[coordinate_frame - 1]
final_quat = T.quaternion_from_matrix(rot_mat)
local_translation = np.array(c.translation)
rotated_local_translation = np.dot(rot_mat, local_translation)
final_pos = final_pos + rotated_local_translation
local_rotation = c.quaternion
final_quat = T.quaternion_multiply(local_rotation, final_quat)
c.update_transform(final_pos, final_quat)
def evaluate_R_t(R_gt, t_gt, R, t, q_gt=None):
# from Utils.transformations import quaternion_from_matrix
t = t.flatten()
t_gt = t_gt.flatten()
eps = 1e-15
if q_gt is None:
q_gt = quaternion_from_matrix(R_gt)
q = quaternion_from_matrix(R)
q = q / (np.linalg.norm(q) + eps)
q_gt = q_gt / (np.linalg.norm(q_gt) + eps)
loss_q = np.maximum(eps, (1.0 - np.sum(q * q_gt)**2))
err_q = np.arccos(1 - 2 * loss_q)
# dR = np.dot(R, R_gt.T)
# dt = t - np.dot(dR, t_gt)
# dR = np.dot(R, R_gt.T)
# dt = t - t_gt
t = t / (np.linalg.norm(t) + eps)
t_gt = t_gt / (np.linalg.norm(t_gt) + eps)
loss_t = np.maximum(eps, (1.0 - np.sum(t * t_gt)**2))
err_t = np.arccos(np.sqrt(1 - loss_t))
if np.sum(np.isnan(err_q)) or np.sum(np.isnan(err_t)):
# This should never happen! Debug here
import IPython
IPython.embed()
return err_q, err_t
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, )
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)
def save_trajectory_to_file(time,
t_world_body,
R_world_body,
body_w_world_body_measured,
body_a_measured,
gyro_bias,
acc_bia,
world_v,
output_dir='/tmp'):
# Trace groundtruth poses.
file_out = open(os.path.join(output_dir, 'groundtruth.csv'), 'w')
file_out.write(
'# timestamp [ns], t_w_b(x) [m], t_w_b(y) [m], t_w_b(z) [m], q_w_b(x) [rad], q_w_b(y) [rad], q_w_b(z) [rad], q_w_b(w) [rad]\n'
)
for i in range(len(time)):
T = np.eye(4)
T[:3, :3] = np.reshape(R_world_body[i, :], (3, 3))
q_world_body = transformations.quaternion_from_matrix(T)
q_world_body = q_world_body / np.linalg.norm(q_world_body)
#q_world_body = q_world_body / np.linalg.norm(q_world_body)
file_out.write('%d, %.8f, %.8f, %.8f, %.8f, %.8f, %.8f, %.8f\n' %
(time[i], t_world_body[i, 0], t_world_body[i, 1],
t_world_body[i, 2], q_world_body[0], q_world_body[1],
q_world_body[2], q_world_body[3]))
file_out.close()
# Trace all groundtruth states (bias, velocity, pose)
file_out = open(os.path.join(output_dir, 'groundtruth_states.csv'), 'w')
file_out.write(
'# timestamp [ns], p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z [], v_RS_R_x [m s^-1], v_RS_R_y [m s^-1], v_RS_R_z [m s^-1], b_w_RS_S_x [rad s^-1], b_w_RS_S_y [rad s^-1], b_w_RS_S_z [rad s^-1], b_a_RS_S_x [m s^-2], b_a_RS_S_y [m s^-2], b_a_RS_S_z [m s^-2]\n'
)
for i in range(len(time)):
T = np.eye(4)
T[:3, :3] = np.reshape(R_world_body[i, :], (3, 3))
q_world_body = transformations.quaternion_from_matrix(T)
q_world_body = q_world_body / np.linalg.norm(q_world_body)
#q_world_body = q_world_body / np.linalg.norm(q_world_body)
file_out.write(
'%d, %.8f, %.8f, %.8f, %.8f, %.8f, %.8f, %.8f, %.8f, %.8f, %.8f, %.8f, %.8f, %.8f, %.8f, %.8f, %.8f\n'
% (time[i], t_world_body[i, 0], t_world_body[i, 1],
t_world_body[i, 2], q_world_body[3], q_world_body[0],
q_world_body[1], q_world_body[2], world_v[i, 0], world_v[i, 1],
world_v[i, 2], gyro_bias[i, 0], gyro_bias[i, 1],
gyro_bias[i, 2], acc_bia[i, 0], acc_bia[i, 1], acc_bia[i, 2]))
file_out.close()
# Trace IMU Measurements
file_out = open(os.path.join(output_dir, 'imu0_data.csv'), 'w')
file_out.write('# timestamp, gx, gy, gz, ax, ay, az\n')
for i in range(len(time)):
file_out.write(
'%d, %.8f, %.8f, %.8f, %.8f, %.8f, %.8f\n' %
(time[i], body_w_world_body_measured[i, 0],
body_w_world_body_measured[i, 1],
body_w_world_body_measured[i, 2], body_a_measured[i, 0],
body_a_measured[i, 1], body_a_measured[i, 2]))
file_out.close()
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
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 RotationDeviationCost(x, n, waypoint, manip):
tool_pose = manip.GetEndEffector().GetTransform()
way_point_quat = transformations.quaternion_from_matrix(waypoint[0:3,
0:3])
quat = transformations.quaternion_from_matrix(tool_pose[0:3, 0:3])
adjustment_quat = self.utils.QuaternionFromTo(way_point_quat, quat)
axis_diff_mag = np.fabs(
sum(self.utils.SubVec(way_point_quat[1:4], adjustment_quat[1:4])))
return (0.25 * axis_mag + 0.75 * adjustment_quat[0]
) #25% of cost from different in axis
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)
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
def interpolate(cameras, n_inter):
'''
Interpolate camera parameters to create virtual cameras
Args:
cams: list of cameras. Each entry is a tuple with camera params RTfckp
n_inter: number of interpolations to make between each camera pair
Returns:
vcams: a list with all the interpolated virtual cameras
'''
from transformations import quaternion_from_matrix, quaternion_slerp, quaternion_matrix, interpolate_spherical
ncams = len(cameras)
inter_cameras = []
for i in range(ncams):
inter_cameras.append([])
fractions = np.linspace(0, 1, n_inter + 2)[1:-1]
inter_idx = 1
for inter in range(n_inter):
# Interpolate rotation matrices (R) in quaternion space.
q0 = quaternion_from_matrix(cameras[i][0])
q1 = quaternion_from_matrix(cameras[(i + 1) % ncams][0])
q_inter = quaternion_slerp(q0, q1, fractions[inter])
cam_inter = [quaternion_matrix(q_inter)[:3, :3]]
# Interpolate translations (T) in spherical coords around the center
# probably the ideal thing would be to use dual quaternion slerp.
T_mid = interpolate_spherical(
cameras[i][1].reshape(1, -1),
cameras[(i + 1) % ncams][1].reshape(1, -1), fractions[inter]).T
cam_inter.append(T_mid)
# Linear interpolation for the rest of numeric params (f, c, k, p)
for j in range(2, 6):
cam_inter.append(
(cameras[i][j] + cameras[(i + 1) % ncams][j]) / 2.)
# Give it a dummy name (name) - v for virtual
cam_inter.append(cameras[i][6] + ".v{0}".format(inter_idx))
inter_idx = inter_idx + 1
inter_cameras[-1].append(cam_inter)
# Put everything into one big list
#allcams = sum([[cam]+intercam for cam,intercam in zip(cameras, inter_cameras)], [])
vcams = list(itertools.chain(*inter_cameras))
return vcams
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
def calculatePoseError(tf0, tf1):
numDimMat = 16
numDimEul = 6
numDimQuat = 7
numData = len(tf0) # should be the same for tf1
matrixPoseError = np.empty((numData, numDimMat))
translationPoseError = np.empty((numData, numDimMat))
rotationPoseError = np.empty((numData, numDimMat))
for i_data in range(numData):
subtractedTau = tf0 - tf1
deltaTau = np.lin_alg.inverse(tf0[i_data]).dot(tf1[i_data])
diffTranslation = deltaTau[:3, 3]
diffRotation = np.eye(4, 4)
diffRotation[:3, :3] = deltaTau[:3, :3]
diffQuat = quaternion_from_matrix(diffRotation)
diffEuler = euler_from_matrix(diffRotation)
# flip quaternions on the wrong side of the hypersphere
if diffQuat[3] < 0:
diffQuat = -diffQuat
pose_error[i_data, :] = np.r_[diff_translation, diff_rot_rep]
return pose_error
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 getRotation(self):
"""
Get rotation matrix of rotation of this bone in local space.
"""
qw,qx,qy,qz = tm.quaternion_from_matrix(self.matPose)
ax,ay,az = tm.euler_from_matrix(self.matPose, axes='sxyz')
return (1000*qw,1000*qx,1000*qy,1000*qz, ax/D,ay/D,az/D)
def getRotation(self):
"""
Get rotation matrix of rotation of this bone in local space.
"""
qw,qx,qy,qz = tm.quaternion_from_matrix(self.matPose)
ax,ay,az = tm.euler_from_matrix(self.matPose, axes='sxyz')
return (1000*qw,1000*qx,1000*qy,1000*qz, ax/D,ay/D,az/D)
def back_front_thread():
global published_transform
rate = rospy.Rate(10)
while True:
time = rospy.Time.now()
num = 0
sum_mat = None
#print("Time diff = %s" %(str(time.to_sec() - front_transform["time"].to_sec()) if front_transform["time"] is not None else "??"))
if front_transform["transform"] is not None and time.to_sec(
) - front_transform["time"].to_sec() < 1:
sum_mat = front_transform["transform"]
num = 1.0
# BRS Let's not use averages - seems to cause normailization errors?
if back_transform["transform"] is not None and time.to_sec(
) - back_transform["time"].to_sec() < 1:
sum_mat = back_transform[
"transform"] if sum_mat is None else sum_mat + back_transform[
"transform"]
num = num + 1.0
if num > 0:
transform = sum_mat / num
published_transform = transform
trans = tr.translation_from_matrix(transform)
rot = tr.quaternion_from_matrix(transform)
r, p, y = tr.euler_from_quaternion(rot)
rot = tr.quaternion_from_euler(r, p, y)
br.sendTransform(trans, rot, time, "odom", "map")
# elif published_transform is not None:
# transform = published_transform
# trans = tr.translation_from_matrix(transform)
# rot = tr.quaternion_from_matrix(transform)
# br.sendTransform(trans, rot, time, "odom", "map")
rate.sleep()
def get_quaternion(z_axis, world_up):
"""
z_axis = numpy.ndarray(n_pts, 3)
world_up = axis representing the y axis
"""
world_up = np.tile(world_up, len(z_axis)).reshape(len(z_axis), 3)
side_axis = np.cross(world_up, z_axis)
side_axis = side_axis / np.linalg.norm(side_axis, axis=1).reshape(-1, 1)
cam_locs_to_remove = np.where(np.isnan(np.linalg.norm(side_axis, axis=1)))
cam_locs_to_take = np.ones(len(world_up)).astype(np.int)
cam_locs_to_take[cam_locs_to_remove] = 0
world_up = world_up[cam_locs_to_take.astype(np.bool)]
side_axis = side_axis[cam_locs_to_take.astype(np.bool)]
z_axis = z_axis[cam_locs_to_take.astype(np.bool)]
up_axis = np.cross(z_axis, side_axis)
# TODO: find a better way to do this
rot_mat = np.zeros((len(z_axis), 4, 4))
quats = list()
for i in range(len(rot_mat)):
rot_mat[i, :3, 0] = side_axis[i]
rot_mat[i, :3, 1] = up_axis[i]
rot_mat[i, :3, 2] = z_axis[i]
rot_mat[i, 3, 3] = 1
if np.isnan(np.sum(rot_mat)):
print('in the nan of utils while generating quaternions')
from IPython import embed
embed()
rot_mat[i] = sym(rot_mat[i])
quats.append(transformations.quaternion_from_matrix(rot_mat[i]))
return cam_locs_to_take, np.stack(quats)
def transform_to_robot_direct(pos, quaternion):
pkl_file = open('pkl_files/moCap_robot_calib.pkl', 'rb')
ret_pos = np.zeros((len(pos), 3))
ret_quat = np.zeros((len(quaternion), 4))
# ret_axis_angle = np.zeros((len(pos), 3))
MVR = np.zeros((3, 3))
PMR = np.array([])
MVR_226 = pickle.load(pkl_file)
pos = np.transpose(pos)
CC = np.dot(MVR_226, pos)
CC = np.transpose(CC)
CC_x = [a[0] for a in CC]
CC_y = [a[1] for a in CC]
CC_z = [a[2] for a in CC]
ret_pos = np.vstack((CC_x, CC_y, CC_z)).T
for r_idx in range(len(MVR_226) - 1):
row = MVR_226[r_idx]
MVR[r_idx] = row[0:3]
PMR = np.append(PMR, row[3])
for q_idx in range(len(quaternion)):
q_for_tran = quaternion[q_idx]
t_for_tran = transformations.quaternion_matrix_rev(q_for_tran)
tmp_M_1 = np.dot(MVR, t_for_tran)
transformed_q = transformations.quaternion_from_matrix(tmp_M_1)
ret_quat[q_idx] = transformed_q
return ret_pos, ret_quat
def generate_ankle_constraint_from_toe(skeleton,
frames,
frame_idx,
ankle_joint_name,
heel_joint,
toe_joint_name,
target_ground_height,
toe_pos=None):
""" create a constraint on the ankle position based on the toe constraint position"""
#print "add toe constraint"
if toe_pos is None:
ct = skeleton.nodes[toe_joint_name].get_global_position(
frames[frame_idx])
ct[1] = target_ground_height # set toe constraint on the ground
else:
ct = toe_pos
a = skeleton.nodes[ankle_joint_name].get_global_position(frames[frame_idx])
t = skeleton.nodes[toe_joint_name].get_global_position(frames[frame_idx])
target_toe_offset = a - t # difference between unmodified toe and ankle at the frame
ca = ct + target_toe_offset # move ankle so toe is on the ground
m = skeleton.nodes[heel_joint].get_global_matrix(frames[frame_idx])
m[:3, 3] = [0, 0, 0]
oq = quaternion_from_matrix(m)
oq = normalize(oq)
return MotionGroundingConstraint(frame_idx, ankle_joint_name, ca, None, oq)
def to_local_cos2(self, joint_name, frame, q):
# bring into parent coordinate system
parent_joint = self.skeleton.nodes[joint_name].parent.node_name
pm = self.skeleton.nodes[parent_joint].get_global_matrix(frame)#[:3, :3]
inv_p = quaternion_inverse(quaternion_from_matrix(pm))
normalize(inv_p)
return quaternion_multiply(inv_p, q)
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
])
def _boneToHash(self, boneHierarchy, bone, recursionLevel=1):
out = {}
out["name"] = bone.name
out["headPos"] = bone.headPos
out["tailPos"] = bone.tailPos
restMatrix = bone.matRestGlobal
matrix = np.array(
(restMatrix[0], -restMatrix[2], restMatrix[1], restMatrix[3]))
qw, qx, qy, qz = quaternion_from_matrix(matrix)
if qw < 1e-4:
roll = 0
else:
roll = math.pi - 2 * math.atan2(qy, qw)
if roll < -math.pi:
roll += 2 * math.pi
elif roll > math.pi:
roll -= 2 * math.pi
out["matrix"] = [
list(restMatrix[0, :]),
list(restMatrix[1, :]),
list(restMatrix[2, :]),
list(restMatrix[3, :])
]
out["roll"] = roll
out["children"] = []
boneHierarchy.append(out)
# Just a security measure.
if recursionLevel < 30:
for child in bone.children:
self._boneToHash(out["children"], child, recursionLevel + 1)
def Init(self,
pose_target,
constraint_target,
env,
robot,
utils,
a_manip,
goggles,
goggles_robot,
parent=None,
child=None):
self.pose_target = pose_target
self.current_pose = pose_target
self.constraint_target = constraint_target
self.env = env
self.robot = robot
self.utils = utils
self.a_manip = a_manip
self.parent_node = parent
self.child_node = child
if (goggles != None):
self.goggles = goggles
self.g_pose = self.goggles.GetTransform()
self.g_quat = self.utils.ArrayToQuat(
transformations.quaternion_from_matrix(self.g_pose[0:3, 0:3]))
self.goggles_robot = goggles_robot
def get_roll_to(head, tail, normal):
"""
Compute the roll angle for a bone to make the bone's local x axis align with
the specified normal.
"""
p1 = toZisUp3(head)
p2 = toZisUp3(tail)
xvec = normal
pvec = matrix.normalize(p2-p1)
xy = np.dot(xvec,pvec)
yvec = matrix.normalize(pvec-xy*xvec)
zvec = matrix.normalize(np.cross(xvec, yvec))
mat = np.asarray((xvec,yvec,zvec), dtype=np.float32)
try:
assertOrthogonal(mat)
except Exception as e:
log.warning("Calculated matrix is not orthogonal (%s)" % e)
quat = tm.quaternion_from_matrix(mat)
if abs(quat[0]) < 1e-4:
return 0
else:
roll = math.pi - 2*math.atan(quat[2]/quat[0])
if roll < -math.pi:
roll += 2*math.pi
elif roll > math.pi:
roll -= 2*math.pi
return roll
def inframe(self, pose, frame):
""" Transform a pose from one frame to another one.
Uses transformation matrices. Could be refactored to use directly
quaternions.
"""
pose = self.get(pose)
if pose['frame'] == "map":
orig = numpy.identity(4)
else:
orig = self._to_mat4(self.get(pose["frame"]))
if frame == "map":
dest = numpy.identity(4)
else:
dest = numpy.linalg.inv(self._to_mat4(self.get(frame)))
poseMatrix = self._to_mat4(pose)
transf = numpy.dot(dest, orig)
transformedPose = numpy.dot(transf, poseMatrix)
qx,qy,qz,qw = transformations.quaternion_from_matrix(transformedPose)
x,y,z = transformations.translation_from_matrix(transformedPose)
return {"x":x,
"y":y,
"z":z,
"qx":qx,
"qy":qy,
"qz":qz,
"qw":qw,
"frame": frame}
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 euler_to_quaternion(euler_angles,
rotation_order=DEFAULT_ROTATION_ORDER,
filter_values=True):
"""Convert euler angles to quaternion vector [qw, qx, qy, qz]
Parameters
----------
* euler_angles: list of floats
\tA list of ordered euler angles in degress
* rotation_order: Iteratable
\t a list that specifies the rotation axis corresponding to the values in euler_angles
* filter_values: Bool
\t enforce a unique rotation representation
"""
assert len(euler_angles) == 3, ('The length of euler angles should be 3!')
euler_angles = np.deg2rad(euler_angles)
rotmat = euler_matrix(*euler_angles,
rotation_order_to_string(rotation_order))
# convert rotation matrix R into quaternion vector (qw, qx, qy, qz)
quat = quaternion_from_matrix(rotmat)
# filter the quaternion see
# http://physicsforgames.blogspot.de/2010/02/quaternions.html
if filter_values:
dot = np.sum(quat)
if dot < 0:
quat = -quat
return [quat[0], quat[1], quat[2], quat[3]]
def quaternions_from_trajectory(trajectory):
"""
Returns rotation quaternions from a sequence of transformation.
"""
return np.array(
[transformations.quaternion_from_matrix(tr[:3, :3]) for tr in trajectory]
)
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
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)
def compute_grasping_direction(self):
#gripper_xquat = transformations.quaternion_from_matrix(h_gripper)
# at 1, 0, gripper is pointing toward -y, gripper right is pointing -x,
ori_gripper_xmat = np.array([[-1, 0, 0], [0, 1, 0], [0, 0, -1]])
d_rim_pos = self.get_grasp_point_to_center()
d_xy = d_rim_pos[:2]
if d_xy[0] == 0 and d_xy[1] == 0:
d_xy[1] = 0.00000001
d_xy = d_xy / np.linalg.norm(d_xy)
cos_theta = d_xy[0]
sin_theta = d_xy[1]
elevation = self.get_grasp_point_elevation()
roll = self.get_grasp_point_roll()
#ori_gripper_quat = transformations.quaternion_from_matrix(ori_gripper_xmat)
# add rotation on the xy plane
xy_rot_xmat = np.array([[cos_theta, -sin_theta, 0],
[sin_theta, cos_theta, 0], [0, 0, 1]])
# add elevation: elevation higher means camera looking more downwards
roll_xmat = np.array(
[[1, 0, 0],
[0, np.cos(np.deg2rad(-roll)), -np.sin(np.deg2rad(-roll))],
[0, np.sin(np.deg2rad(-roll)),
np.cos(np.deg2rad(-roll))]])
ele_xmat = np.array([[
np.cos(np.deg2rad(-elevation)), 0,
np.sin(np.deg2rad(-elevation))
], [0, 1, 0],
[
-np.sin(np.deg2rad(-elevation)), 0,
np.cos(np.deg2rad(-elevation))
]])
final_rot = np.matmul(
np.matmul(xy_rot_xmat, np.matmul(ele_xmat, ori_gripper_xmat)),
roll_xmat)
# making the "thumb" of the gripper to point to y+
# if not: rotate gripper with 180 degree
if final_rot[1, 1] < 0:
rot = 180
xmat = np.array(
[[1, 0, 0],
[0, np.cos(np.deg2rad(-rot)), -np.sin(np.deg2rad(-rot))],
[0, np.sin(np.deg2rad(-rot)),
np.cos(np.deg2rad(-rot))]])
final_rot = np.matmul(final_rot, xmat)
final_rot_4x4 = np.eye(4)
final_rot_4x4[:3, :3] = final_rot
gripper_xquat = transformations.quaternion_from_matrix(final_rot_4x4)
return gripper_xquat
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 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 calculatePoseError(tf0, tf1):
numDimMat = 16
numDimEul = 6
numDimQuat = 7
numData = len(tf0) # should be the same for tf1
matrixPoseError = np.empty((numData, numDimMat))
translationPoseError = np.empty((numData, numDimMat))
rotationPoseError = np.empty((numData, numDimMat))
for i_data in range(numData):
subtractedTau = tf0 - tf1
deltaTau = np.lin_alg.inverse(tf0[i_data]).dot(tf1[i_data])
diffTranslation = deltaTau[:3,3]
diffRotation = np.eye(4,4)
diffRotation[:3,:3] = deltaTau[:3,:3]
diffQuat = quaternion_from_matrix(diffRotation)
diffEuler = euler_from_matrix(diffRotation)
# flip quaternions on the wrong side of the hypersphere
if diffQuat[3] < 0:
diffQuat = -diffQuat
pose_error[i_data,:] = np.r_[diff_translation, diff_rot_rep]
return pose_error
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 fromMatrix(self, matrix):
q0 = tm.quaternion_from_matrix(matrix)
t = matrix[:3,3]
self.even = q0
q1 = self.odd
q1[0] = -0.5*(t[0]*q0[1] + t[1]*q0[2] + t[2]*q0[3])
q1[1] = 0.5*( t[0]*q0[0] + t[1]*q0[3] - t[2]*q0[2])
q1[2] = 0.5*(-t[0]*q0[3] + t[1]*q0[0] + t[2]*q0[1])
q1[3] = 0.5*( t[0]*q0[2] - t[1]*q0[1] + t[2]*q0[0])
def rvec2quat(rvec, cam2body):
Rned2cam, jac = cv2.Rodrigues(rvec)
Rned2body = cam2body.dot(Rned2cam)
Rbody2ned = np.matrix(Rned2body).T
# make 3x3 rotation matrix into 4x4 homogeneous matrix
hIR = np.concatenate( (np.concatenate( (Rbody2ned, np.zeros((3,1))),1),
np.mat([0,0,0,1])) )
quat = transformations.quaternion_from_matrix(hIR)
return quat
def _plot_img(img, K, T, z=0.1):
obj = mlab.imshow(img.T)
quat = tf.quaternion_from_matrix(T)
angle, axis = angle_axis_from_quaternion(quat)
obj.actor.rotate_wxyz(angle * 180 / np.pi, *axis)
h, w = img.shape
xmax_pixel, ymax_pixel = w, h
point3d = projectionto3d(K, (xmax_pixel, ymax_pixel))[0]
origin3d = projectionto3d(K, (0, 0))[0]
point3d = point3d * z / point3d[2]
origin3d = origin3d * z / origin3d[2]
center3d = (point3d + origin3d) / 2.
xmax, ymax, _ = point3d - origin3d
obj.actor.scale = np.array([xmax / xmax_pixel, ymax / ymax_pixel, 1.0])
obj.actor.position = utils.apply_transform(T, center3d)
mlab.view(distance=20 * z)
return obj
def computeRoll(head, tail, normal, bone=None):
if normal is None:
return 0
p1 = m2b(head)
p2 = m2b(tail)
xvec = normal
pvec = getUnitVector(p2-p1)
xy = np.dot(xvec,pvec)
yvec = getUnitVector(pvec-xy*xvec)
zvec = getUnitVector(np.cross(xvec, yvec))
if zvec is None:
return 0
else:
mat = np.array((xvec,yvec,zvec))
checkOrthogonal(mat)
quat = tm.quaternion_from_matrix(mat)
if abs(quat[0]) < 1e-4:
return 0
else:
roll = math.pi - 2*math.atan(quat[2]/quat[0])
if roll < -math.pi:
roll += 2*math.pi
elif roll > math.pi:
roll -= 2*math.pi
return roll
if bone and bone.name in ["forearm.L", "forearm.R"]:
log.debug("B %s" % bone.name)
log.debug(" H %.4g %.4g %.4g" % tuple(head))
log.debug(" T %.4g %.4g %.4g" % tuple(tail))
log.debug(" N %.4g %.4g %.4g" % tuple(normal))
log.debug(" P %.4g %.4g %.4g" % tuple(pvec))
log.debug(" X %.4g %.4g %.4g" % tuple(xvec))
log.debug(" Y %.4g %.4g %.4g" % tuple(yvec))
log.debug(" Z %.4g %.4g %.4g" % tuple(zvec))
log.debug(" Q %.4g %.4g %.4g %.4g" % tuple(quat))
log.debug(" R %.4g" % roll)
return roll
def contacts_to_baxter_hand_pose(contact1, contact2):
c1 = np.array(contact1)
c2 = np.array(contact2)
# compute gripper center and axis
center = 0.5 * (c1 + c2)
y_axis = c2 - c1
y_axis = y_axis / np.linalg.norm(y_axis)
z_axis = np.array([y_axis[1], -y_axis[0], 0]) # the z axis will always be in the table plane for now
z_axis = z_axis / np.linalg.norm(z_axis)
x_axis = np.cross(y_axis, z_axis)
# convert to hand pose
R_obj_gripper = np.array([x_axis, y_axis, z_axis]).T
t_obj_gripper = center
T_obj_gripper = np.eye(4)
T_obj_gripper[:3, :3] = R_obj_gripper
T_obj_gripper[:3, 3] = t_obj_gripper
q_obj_gripper = transformations.quaternion_from_matrix(T_obj_gripper)
return t_obj_gripper, q_obj_gripper
def prepair_data(self, image_list, matches_list, K):
# iterate through the image list and build the camera pose dictionary
# (and a simple list of camera locations for plotting)
f = open( self.root + '/sba-cams.txt', 'w' )
for image in image_list:
body2cam = image.get_body2cam()
ned2body = image.get_ned2body()
Rtotal = body2cam.dot( ned2body )
q = transformations.quaternion_from_matrix(Rtotal)
rvec, tvec = image.get_proj()
s = "%.8f %.8f %.8f %.8f %.8f %.8f %.8f\n" % (q[0], q[1], q[2], q[3],
tvec[0,0], tvec[1,0], tvec[2,0])
f.write(s)
f.close()
# iterate through the matches dictionary to produce a list of matches
f = open( self.root + '/sba-points.txt', 'w' )
for match in matches_list:
ned = np.array(match[0])
s = "%.4f %.4f %.4f " % (ned[0], ned[1], ned[2])
f.write(s)
s = "%d " % (len(match[1:]))
f.write(s)
for p in match[1:]:
image_num = p[0]
# kp = image_list[image_num].kp_list[p[1]].pt # undistorted
kp = image_list[image_num].uv_list[p[1]] # distorted
s = "%d %.2f %.2f " % (image_num, kp[0], kp[1])
f.write(s)
f.write('\n')
f.close()
# generate the calibration matrix (K) file
f = open( self.root + '/sba-calib.txt', 'w' )
s = "%.4f %.4f %.4f\n" % (K[0,0], K[0,1], K[0,2])
f.write(s)
s = "%.4f %.4f %.4f\n" % (K[1,0], K[1,1], K[1,2])
f.write(s)
s = "%.4f %.4f %.4f\n" % (K[2,0], K[2,1], K[2,2])
f.write(s)
def get_a2b(a, b, rep_out='rotmat'):
"""Returns an SO3 quantity that will align vector a with vector b.
The output will be in the representation selected by rep_out
('rotmat', 'quaternion', or 'rotvec').
>>> a = trns.random_vector(3)
>>> b = trns.random_vector(3)
>>> R = get_a2b(a, b)
>>> p1 = R.dot(a)
>>> print(np.allclose(np.cross(p1, b), [0, 0, 0]))
True
>>> p1.dot(b) > 0
True
>>> q = get_a2b(a, b, 'quaternion')
>>> p2 = apply_to_points(q, a)
>>> print(np.allclose(np.cross(p2, b), [0, 0, 0]))
True
>>> r = get_a2b(a, b, 'rotvec')
>>> p3 = apply_to_points(r, a)
>>> print(np.allclose(np.cross(p3, b), [0, 0, 0]))
True
"""
a = normalize(a)
b = normalize(b)
cosine = a.dot(b)
if np.isclose(abs(cosine), 1):
R = np.eye(3)
else:
axis = np.cross(a, b)
angle = np.arctan2(npl.norm(axis), cosine)
R = trns.rotation_matrix(angle, axis)
if rep_out == 'rotmat':
return R[:3, :3]
elif rep_out == 'quaternion':
return trns.quaternion_from_matrix(R)
elif rep_out == 'rotvec':
return get_rotvec(R)
else:
raise ValueError("Invalid rep_out. Choose 'rotmat', 'quaternion', or 'rotvec'.")
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 contacts_to_baxter_hand_pose(c1, c2):
# compute gripper center and axis
center = 0.5 * (c1 + c2)
y_axis = c2 - c1
print y_axis
y_axis = y_axis / np.linalg.norm(y_axis)
print y_axis
x = np.array([y_axis[1], -y_axis[0], 0]) # the x axis will always be in the table plane for now
x = x / np.linalg.norm(x)
z = np.cross(x, y_axis)
if z[2] < 0:
x = -x
z = np.cross(x, y_axis)
# convert to hand pose
R_obj_gripper = np.array([x, y_axis, z]).T
t_obj_gripper = center
T_obj_gripper = np.eye(4)
T_obj_gripper[:3,:3] = R_obj_gripper
T_obj_gripper[:3,3] = t_obj_gripper
q_obj_gripper = transformations.quaternion_from_matrix(T_obj_gripper)
return t_obj_gripper, q_obj_gripper
def listPose(self):
for bone in self.boneList:
quat = tm.quaternion_from_matrix(bone.matrixPose)
log.debug(" %s %s", bone.name, quat)
def solvePnP2( pts_dict ):
# build a new cam_dict that is a copy of the current one
cam_dict = {}
for i1 in proj.image_list:
cam_dict[i1.name] = {}
rvec, tvec = i1.get_proj()
ned, ypr, quat = i1.get_camera_pose()
cam_dict[i1.name]['rvec'] = rvec
cam_dict[i1.name]['tvec'] = tvec
cam_dict[i1.name]['ned'] = ned
camw, camh = proj.cam.get_image_params()
for i, i1 in enumerate(proj.image_list):
print i1.name
scale = float(i1.width) / float(camw)
K = proj.cam.get_K(scale)
rvec1, tvec1 = i1.get_proj()
cam2body = i1.get_cam2body()
body2cam = i1.get_body2cam()
# include our own position in the average
quat_start_weight = 100
ned_start_weight = 2
count = 0
sum_quat = np.array(i1.camera_pose['quat']) * quat_start_weight
sum_ned = np.array(i1.camera_pose['ned']) * ned_start_weight
for j, i2 in enumerate(proj.image_list):
matches = i1.match_list[j]
if len(matches) < 8:
continue
count += 1
rvec2, tvec2 = i2.get_proj()
# build obj_pts and img_pts to position i1 relative to the
# matches in i2.
img1_pts = []
img2_pts = []
obj_pts = []
for pair in matches:
kp1 = i1.kp_list[ pair[0] ]
img1_pts.append( kp1.pt )
kp2 = i2.kp_list[ pair[1] ]
img2_pts.append( kp2.pt )
key = "%d-%d" % (i, pair[0])
if not key in pts_dict:
key = "%d-%d" % (j, pair[1])
# print key, pts_dict[key]
obj_pts.append(pts_dict[key])
# compute the centroid of obj_pts
sum = np.zeros(3)
for p in obj_pts:
sum += p
obj_center = sum / len(obj_pts)
print "obj_pts center =", obj_center
# given the previously computed triangulations (and
# averages of point 3d locations if they are involved in
# multiple triangulations), then compute an estimate for
# both matching camera poses. The relative positioning of
# these camera poses should be pretty accurate.
(result, rvec1, tvec1) = cv2.solvePnP(np.float32(obj_pts),
np.float32(img1_pts),
K, None, rvec1, tvec1,
useExtrinsicGuess=True)
(result, rvec2, tvec2) = cv2.solvePnP(np.float32(obj_pts),
np.float32(img2_pts),
K, None, rvec2, tvec2,
useExtrinsicGuess=True)
Rned2cam1, jac = cv2.Rodrigues(rvec1)
Rned2cam2, jac = cv2.Rodrigues(rvec2)
ned1 = -np.matrix(Rned2cam1[:3,:3]).T * np.matrix(tvec1)
ned2 = -np.matrix(Rned2cam2[:3,:3]).T * np.matrix(tvec2)
print "cam1 orig=%s new=%s" % (i1.camera_pose['ned'], ned1)
print "cam2 orig=%s new=%s" % (i2.camera_pose['ned'], ned2)
# compute a rigid transform (rotation + translation) to
# align the estimated camera locations (projected from the
# triangulation) back with the original camera points, and
# roughly rotated around the centroid of the object
# points.
src = np.zeros( (3,3) ) # current camera locations
dst = np.zeros( (3,3) ) # original camera locations
src[0,:] = np.squeeze(ned1)
src[1,:] = np.squeeze(ned2)
src[2,:] = obj_center
dst[0,:] = i1.camera_pose['ned']
dst[1,:] = i2.camera_pose['ned']
dst[2,:] = obj_center
print "src:\n", src
print "dst:\n", dst
M = transformations.superimposition_matrix(src, dst)
print "M:\n", M
# Our (i1) Rned2cam matrix is body2cam * Rned, so solvePnP
# returns this combination. We can extract Rbody2ned by
# premultiplying by cam2body aka inv(body2cam).
Rned2body = cam2body.dot(Rned2cam1)
# print "Rned2body:\n", Rned2body
Rbody2ned = np.matrix(Rned2body).T # IR
# print "Rbody2ned:\n", Rbody2ned
# print "R (after M * R):\n", R
# now transform by the earlier "M" affine transform to
# rotate the work space closer to the actual camera points
Rot = M[:3,:3].dot(Rbody2ned)
# make 3x3 rotation matrix into 4x4 homogeneous matrix
hRot = np.concatenate( (np.concatenate( (Rot, np.zeros((3,1))),1),
np.mat([0,0,0,1])) )
# print "hRbody2ned:\n", hRbody2ned
quat = transformations.quaternion_from_matrix(hRot)
sum_quat += quat
sum_ned += np.squeeze(np.asarray(ned1))
print "count = ", count
newned = sum_ned / (ned_start_weight + count)
print "orig ned =", i1.camera_pose['ned']
print "new ned =", newned
newquat = sum_quat / (quat_start_weight + count)
newIR = transformations.quaternion_matrix(newquat)
print "orig ypr = ", i1.camera_pose['ypr']
(yaw, pitch, roll) = transformations.euler_from_quaternion(newquat, 'rzyx')
print "new ypr =", [yaw/d2r, pitch/d2r, roll/d2r]
newR = np.transpose(newIR[:3,:3]) # equivalent to inverting
newRned2cam = body2cam.dot(newR[:3,:3])
rvec, jac = cv2.Rodrigues(newRned2cam)
tvec = -np.matrix(newRned2cam) * np.matrix(newned).T
#print "orig pose:\n", cam_dict[i1.name]
cam_dict[i1.name]['rvec'] = rvec
cam_dict[i1.name]['tvec'] = tvec
cam_dict[i1.name]['ned'] = newned
#print "new pose:\n", cam_dict[i1.name]
return cam_dict
def getPoseQuaternion(self):
return tm.quaternion_from_matrix(self.matrixPose)
def set_matrix( self , mb , me ) : self.qb = tr.quaternion_from_matrix( mb ) self.qe = tr.quaternion_from_matrix( me )
for trial in range(0,5):
H_file = "/Users/isa/Dropbox/MultiModalRegPaper/ground_truth/rand_t/H_" + str(trial) + ".txt"
max_angle = (np.pi/18.0) #max angular error 10 degrees
max_t = 5 #max translation 5 meters
# H = np.identity(4)
R_rand = np.identity(3)
scale = 1.0;
#generate a random rotation angle smaller than max_angle
rand_angle = rotations.random_rot_angle(max_angle)
print "Random Angle: "
print rand_angle
#generate a random rotation matrix of fixed angle
rotations.R_random_axis(R_rand, rand_angle)
Q_rand = tf.quaternion_from_matrix(R_rand);
Q_vxl = np.array([float(Q_rand[1]),
float(Q_rand[2]),
float(Q_rand[3]),
float(Q_rand[0])])
# H[:3, :3]= R_rand
print "Rotation Matrix: "
print R_rand
print Q_vxl
#generate random translation
T = (np.random.random(3) - 0.5)* 2.0 * max_t
print "Translation : "
print T
def solvePnP1( pts_dict ):
# start with a clean slate
for i1 in proj.image_list:
i1.img_pts = []
i1.obj_pts = []
# build a new cam_dict that is a copy of the current one
cam_dict = {}
for i1 in proj.image_list:
cam_dict[i1.name] = {}
rvec, tvec = i1.get_proj()
ned, ypr, quat = i1.get_camera_pose()
cam_dict[i1.name]['rvec'] = rvec
cam_dict[i1.name]['tvec'] = tvec
cam_dict[i1.name]['ned'] = ned
camw, camh = proj.cam.get_image_params()
for i, i1 in enumerate(proj.image_list):
print i1.name
scale = float(i1.width) / float(camw)
K = proj.cam.get_K(scale)
rvec, tvec = i1.get_proj()
cam2body = i1.get_cam2body()
body2cam = i1.get_body2cam()
# include our own position in the average
count = 1
sum_quat = np.array(i1.camera_pose['quat'])
sum_ned = np.array(i1.camera_pose['ned'])
for j, i2 in enumerate(proj.image_list):
matches = i1.match_list[j]
if len(matches) < 8:
continue
count += 1
# build obj_pts and img_pts to position i1 relative to the
# matches in i2
obj_pts = []
img_pts = []
for pair in matches:
kp = i1.kp_list[ pair[0] ]
img_pts.append( kp.pt )
key = "%d-%d" % (i, pair[0])
if not key in pts_dict:
key = "%d-%d" % (j, pair[1])
# print key, pts_dict[key]
obj_pts.append(pts_dict[key])
#if key in pts_dict:
# sum_dict[key] += p
#else:
# sum_dict[key] = p
(result, rvec, tvec) = cv2.solvePnP(np.float32(obj_pts),
np.float32(img_pts),
K, None, rvec, tvec,
useExtrinsicGuess=True)
#print "rvec=%s tvec=%s" % (rvec, tvec)
Rned2cam, jac = cv2.Rodrigues(rvec)
#print "Rned2cam (from SolvePNP):\n", Rned2cam
# Our Rcam matrix (in our ned coordinate system) is
# body2cam * Rned, so solvePnP returns this combination.
# We can extract Rned by premultiplying by cam2body aka
# inv(body2cam).
Rned2body = cam2body.dot(Rned2cam)
# print "Rned2body:\n", Rned2body
Rbody2ned = np.matrix(Rned2body).T # IR
# print "Rbody2ned:\n", Rbody2ned
# print "R (after M * R):\n", R
# make 3x3 rotation matrix into 4x4 homogeneous matrix
hRbody2ned = np.concatenate( (np.concatenate( (Rbody2ned, np.zeros((3,1))),1), np.mat([0,0,0,1])) )
# print "hRbody2ned:\n", hRbody2ned
quat = transformations.quaternion_from_matrix(hRbody2ned)
sum_quat += quat
pos = -np.matrix(Rned2cam[:3,:3]).T * np.matrix(tvec)
sum_ned += np.squeeze(np.asarray(pos))
print "ned =", np.squeeze(np.asarray(pos))
print "count = ", count
newned = sum_ned / float(count)
print "orig ned =", i1.camera_pose['ned']
print "new ned =", newned
newquat = sum_quat / float(count)
newIR = transformations.quaternion_matrix(newquat)
print "orig ypr = ", i1.camera_pose['ypr']
(yaw, pitch, roll) = transformations.euler_from_quaternion(newquat, 'rzyx')
print "new ypr =", [yaw/d2r, pitch/d2r, roll/d2r]
newR = np.transpose(newIR[:3,:3]) # equivalent to inverting
newRned2cam = body2cam.dot(newR[:3,:3])
rvec, jac = cv2.Rodrigues(newRned2cam)
tvec = -np.matrix(newRned2cam) * np.matrix(newned).T
#print "orig pose:\n", cam_dict[i1.name]
cam_dict[i1.name]['rvec'] = rvec
cam_dict[i1.name]['tvec'] = tvec
cam_dict[i1.name]['ned'] = newned
#print "new pose:\n", cam_dict[i1.name]
return cam_dict
def getRotation(self):
qw,qx,qy,qz = tm.quaternion_from_matrix(self.matrixPose)
ax,ay,az = tm.euler_from_matrix(self.matrixPose, axes='sxyz')
return (1000*qw,1000*qx,1000*qy,1000*qz, ax/D,ay/D,az/D)
def compute_error(trial, descriptor_type, niter, percentile=99):
src_scene_root = "/Users/isa/Experiments/reg3d_eval/downtown_dan/trial_" +str(trial);
src_features_dir = "/Users/isa/Experiments/reg3d_eval/downtown_dan/trial_" +str(trial)+ "/" + descriptor_type + "_30"
#read "geo-transformatiom"
#************GEO**************"
Tfile = "/data/lidar_providence/downtown_offset-1-financial-Hs.txt"
Tfis = open(Tfile, '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)
#************Hs**************#
#read source to target "Ground Truth" Transformation
Tfile = src_scene_root + "/Hs.txt";
Tfis = open(Tfile, 'r')
lines=[];
lines = Tfis.readlines();
scale = float(lines[0])
Ss = tf.scale_matrix(scale);
quat_line = lines[1].split(" ");
quat = tf.unit_vector(np.array([float(quat_line[3]), float(quat_line[0]), float(quat_line[1]), float(quat_line[2])]))
Hs = tf.quaternion_matrix(quat);
trans_line = lines[2].split(" ");
Ts = np.array([float(trans_line[0]), float(trans_line[1]), float(trans_line[2])]);
Tfis.close();
Rs = Hs.copy()[:3,:3];
Hs[:3, 3] = Ts[:3]
Hs=Ss.dot(Hs)
Rs = Rs;
Ts = Ts;
LOG.debug( "\n************Hs************** \n R:\n%s \nT:\n%s \nH:\n%s", Rs, Ts, Hs)
#************Hs IA**************
#read source to target "Initial Alignment" Transformation
Tfile = src_features_dir + "/ia_transformation_" + str(percentile) + "_" + str(niter) + ".txt";
Tfis = open(Tfile, 'r')
Hs_ia = np.genfromtxt(Tfis, skip_header=1, usecols={0,1,2,3} );
Tfis.close()
Tfis = open(Tfile, 'r')
Ss_ia=np.genfromtxt(Tfis, skip_footer=4, usecols={0} );
Tfis.close()
Rs_ia = Hs_ia[:3,:3]*(1.0/Ss_ia)
Ts_ia = Hs_ia[:3,3]*(1.0/Ss_ia)
LOG.debug( "\n************Hs IA************** \n R:\n%s \nT:\n%s \nH:\n%s", Rs_ia, Ts_ia, Hs_ia)
#Initial Aligment errors
#Rotation error - half angle between the normalized quaterions
quat_ia = tf.unit_vector(tf.quaternion_from_matrix(Rs_ia));
Rs_error_ia_norm = math.acos(abs(np.dot(quat_ia, quat)));
#Translation error
# x = Rs_ia*x_ia + Ts_ia = Rs_ia(x_ia + np.dot(Rs_ia.T(), Ts_ia)
# np.dot(Rs_ia.T(), Ts_ia) correspond to trans on ia coordinate system
Ts_error_ia = (Rs_ia.T).dot(Ts_ia) - (Rs.T).dot(Ts)
Ts_error_ia_norm = scale_geo*scale*LA.norm(Ts_error_ia)
LOG.debug( "Error (R,T) %s , %s ", Rs_error_ia_norm , Ts_error_ia_norm )
#read source to target "Initial Alignment" Transformation
#************Hs ICP**************
Tfile = src_features_dir + "/icp_transformation_" + str(percentile) + "_" + str(niter) + ".txt";
Tfis = open(Tfile, 'r')
Hs_icp = np.genfromtxt(Tfis, usecols={0,1,2,3});
Tfis.close()
Hs_icp = Hs_icp.dot(Hs_ia)
Rs_icp = Hs_icp[:3,:3]*(1.0/Ss_ia)
Ts_icp = Hs_icp[:3,3]*(1.0/Ss_ia)
LOG.debug( "\n************Hs ICP************** \n R:\n%s \nT:\n%s \nH:\n%s", Rs_icp, Ts_icp, Hs_icp)
#ICP errors
#Rotation error - half angle between the normalized quaterions
quat_icp = tf.unit_vector(tf.quaternion_from_matrix(Rs_icp));
Rs_error_icp_norm = math.acos(abs(np.dot(quat_icp, quat)));
#Translation error
# x = Rs_ia*x_ia + Ts_ia = Rs_ia(x_ia + np.dot(Rs_ia.T(), Ts_ia)
# np.dot(Rs_ia.T(), Ts_ia) correspond to trans on ia coordinate system
Ts_error_icp = (Rs_icp.T).dot(Ts_icp) - (Rs.T).dot(Ts)
Ts_error_icp_norm = scale_geo*scale*LA.norm(Ts_error_icp)
LOG.debug( "Error (R,T) %s , %s ", Rs_error_icp_norm , Ts_error_icp_norm )
#read source to target "Initial Alignment" Transformation
#************Hs ICP Nprmals**************
Tfile = src_features_dir + "/icp_transformation_" + str(percentile) + "_" + str(niter) + "_n.txt";
Tfis = open(Tfile, 'r')
Hs_icn = np.genfromtxt(Tfis, usecols={0,1,2,3});
Tfis.close()
Hs_icn = Hs_icn.dot(Hs_ia)
Rs_icn = Hs_icn[:3,:3]*(1.0/Ss_ia)
Ts_icn = Hs_icn[:3,3]*(1.0/Ss_ia)
LOG.debug( "\n************Hs ICP Normals************** \n R:\n%s \nT:\n%s \nH:\n%s", Rs_icn, Ts_icn, Hs_icn)
#ICP errors
#Rotation error - half angle between the normalized quaterions
quat_icn = tf.unit_vector(tf.quaternion_from_matrix(Rs_icn));
Rs_error_icn_norm = math.acos(abs(np.dot(quat_icn, quat)));
#Translation error
# x = Rs_ia*x_ia + Ts_ia = Rs_ia(x_ia + np.dot(Rs_ia.T(), Ts_ia)
# np.dot(Rs_ia.T(), Ts_ia) correspond to trans on ia coordinate system
Ts_error_icn = (Rs_icn.T).dot(Ts_icn) - (Rs.T).dot(Ts)
Ts_error_icn_norm = scale_geo*scale*LA.norm(Ts_error_icn)
LOG.debug( "Error (R,T) %s , %s ", Rs_error_icn_norm , Ts_error_icn_norm )
ICP_error = np.array([Rs_error_icp_norm, Ts_error_icp_norm])
ICN_error = np.array([Rs_error_icn_norm, Ts_error_icn_norm]);
# import code; code.interact(local=locals())
return ICP_error,ICN_error
def writeAnimation(filepath, human, humanBBox, config, animTrack):
log.message("Exporting animation %s.", animTrack.name)
numJoints = len(human.getSkeleton().getBones()) + 1
animfilename = os.path.splitext(os.path.basename(filepath))[0]
animfilename = animfilename + "_%s.md5anim" % (animTrack.name)
foldername = os.path.dirname(filepath)
animfilepath = os.path.join(foldername, animfilename)
f = codecs.open(animfilepath, 'w', encoding="utf-8")
f.write('MD5Version 10\n')
f.write('commandline ""\n\n')
f.write('numFrames %d\n' % animTrack.nFrames)
f.write('numJoints %d\n' % numJoints)
f.write('frameRate %d\n' % int(animTrack.frameRate))
f.write('numAnimatedComponents %d\n\n' % (numJoints * 6))
skel = human.getSkeleton()
flags = 63
f.write('hierarchy {\n')
f.write('\t"origin" -1 %d 0\n' % flags)
arrayIdx = 6
for bIdx, bone in enumerate(skel.getBones()):
#<string:jointName> <int:parentIndex> <int:flags> <int:startIndex>
if bone.parent:
f.write('\t"%s" %d %d %d\n' % (bone.name, bone.parent.index+1, flags, arrayIdx))
else:
f.write('\t"%s" %d %d %d\n' % (bone.name, 0, flags, arrayIdx))
arrayIdx = arrayIdx + 6
f.write('}\n\n')
f.write('bounds {\n')
bounds = humanBBox
if config.feetOnGround:
bounds[:][1] += getFeetOnGroundOffset(human)
if config.zUp:
bounds[0] = bounds[0][[0,2,1]] * [1,-1,1]
bounds[1] = bounds[1][[0,2,1]] * [1,-1,1]
bounds = bounds * scale
# TODO use bounds calculated for every frame
for frameIdx in xrange(animTrack.nFrames):
#( vec3:boundMin ) ( vec3:boundMax )
f.write('\t( %f %f %f ) ( %f %f %f )\n' % (bounds[0][0], bounds[0][1], bounds[0][2], bounds[1][0], bounds[1][1], bounds[1][2]))
f.write('}\n\n')
f.write('baseframe {\n')
f.write('\t( %f %f %f ) ( %f %f %f )\n' % (0.0, 0.0, 0.0, 0.0, 0.0, 0.0))
bases = []
for bone in skel.getBones():
pos = bone.getRestOffset() * scale
if config.feetOnGround and not bone.parent:
pos[1] += (getFeetOnGroundOffset(human) * scale)
transformationMat = bone.matRestRelative.copy()
if config.zUp:
transformationMat = np.dot(ZYRotation, np.dot(transformationMat,la.inv(ZYRotation)))
pos = pos[[0,2,1]] * [1,-1,1]
orientationQuat = tm.quaternion_from_matrix(transformationMat)
qx = orientationQuat[0]
qy = orientationQuat[1]
qz = orientationQuat[2]
w = orientationQuat[3]
#( vec3:position ) ( vec3:orientation )
f.write('\t( %f %f %f ) ( %f %f %f )\n' % (pos[0], pos[1], pos[2], qx, qy, qz))
bases.append((pos, [qx, qy, qz, w]))
f.write('}\n\n')
for frameIdx in xrange(animTrack.nFrames):
frame = animTrack.getAtFramePos(frameIdx)
f.write('frame %d {\n' % frameIdx)
f.write('\t%f %f %f %f %f %f\n' % (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)) # Transformation for origin joint
for bIdx in xrange(numJoints-1):
transformationMat = frame[bIdx].copy()
pos = transformationMat[:3,3] * scale
transformationMat[:3,3] = [0.0, 0.0, 0.0]
if config.zUp:
transformationMat = np.dot(ZYRotation, np.dot(transformationMat,la.inv(ZYRotation)))
pos = pos[[0,2,1]] * [1,-1,1]
#baseRot = aljabr.quaternion2Matrix(bases[bIdx][1])
#transformationMat = np.dot(transformationMat[:3,:3], baseRot)
pos += bases[bIdx][0]
orientationQuat = tm.quaternion_from_matrix(transformationMat)
qx = orientationQuat[0]
qy = orientationQuat[1]
qz = orientationQuat[2]
w = orientationQuat[3]
if w > 0:
qx = -qx
qy = -qy
qz = -qz
# vec3:position vec3:orientation
f.write('\t%f %f %f %f %f %f\n' % (pos[0], pos[1], pos[2], qx, qy, qz))
f.write('}\n\n')
f.close()
def createAnimationTrack(self, skel=None, name=None):
"""
Create an animation track from the motion stored in this BHV file.
"""
def _bvhJointName(boneName):
# Remove the tail from duplicate bone names (added by the BVH parser)
import re
if not boneName:
return boneName
r = re.search("(.*)_\d+$", boneName)
if r:
return r.group(1)
return boneName
def _createAnimation(jointsData, name, frameTime, nFrames):
nJoints = len(jointsData)
#nFrames = len(jointsData[0])
# Interweave joints animation data, per frame with joints in breadth-first order
animData = np.hstack(jointsData).reshape(nJoints*nFrames,3,4)
framerate = 1.0/frameTime
return animation.AnimationTrack(name, animData, nFrames, framerate)
if name is None:
name = self.name
if skel is None:
jointsData = [joint.matrixPoses for joint in self.getJoints() if not joint.isEndConnector()]
# We leave out end effectors as they should not have animation data
return _createAnimation(jointsData, name, self.frameTime, self.frameCount)
elif isinstance(skel, list):
# skel parameter is a list of joint or bone names
jointsOrder = skel
jointsData = []
for jointName in jointsOrder:
jointName = _bvhJointName(jointName)
if jointName and self.getJointByCanonicalName(jointName) is not None:
poseMats = self.getJointByCanonicalName(jointName).matrixPoses.copy()
jointsData.append(poseMats)
else:
jointsData.append(animation.emptyTrack(self.frameCount))
return _createAnimation(jointsData, name, self.frameTime, self.frameCount)
else:
# skel parameter is a Skeleton
jointsData = []
for bone in skel.getBones():
if len(bone.reference_bones) > 0:
# Combine the rotations of reference bones to influence this bone
bvhJoints = []
for bonename in bone.reference_bones:
jointname = _bvhJointName(bonename)
joint = self.getJointByCanonicalName(jointname)
if joint:
bvhJoints.append(joint)
if len(bvhJoints) == 0:
poseMats = animation.emptyTrack(self.frameCount)
elif len(bvhJoints) == 1:
poseMats = bvhJoints[0].matrixPoses.copy()
else: # len(bvhJoints) >= 2:
# Combine the rotations using quaternions to simplify math and normalizing (rotations only)
poseMats = animation.emptyTrack(self.frameCount)
m = np.identity(4, dtype=np.float32)
for f_idx in xrange(self.frameCount):
m[:3,:4] = bvhJoints[0].matrixPoses[f_idx]
q1 = tm.quaternion_from_matrix(m, True)
m[:3,:4] = bvhJoints[1].matrixPoses[f_idx]
q2 = tm.quaternion_from_matrix(m, True)
quat = tm.quaternion_multiply(q2, q1)
for bvhJoint in bvhJoints[2:]:
m[:3,:4] = bvhJoint.matrixPoses[f_idx]
q = tm.quaternion_from_matrix(m, True)
quat = tm.quaternion_multiply(q, quat)
poseMats[f_idx] = tm.quaternion_matrix( quat )[:3,:4]
jointsData.append(poseMats)
else:
# Map bone to joint by bone name
jointName = _bvhJointName(bone.name)
joint = self.getJointByCanonicalName(jointName)
if joint:
jointsData.append( joint.matrixPoses.copy() )
else:
jointsData.append(animation.emptyTrack(self.frameCount))
return _createAnimation(jointsData, name, self.frameTime, self.frameCount)
def quatswap(quat):
mat = transformations.quaternion_matrix(quat)
mat_new = np.c_[mat[:,2],mat[:,1],-mat[:,0],mat[:,3]]
return transformations.quaternion_from_matrix(mat_new)
def mat2quat(mat33):
mat44 = np.eye(4)
mat44[:3,:3] = mat33
return transformations.quaternion_from_matrix(mat44)