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
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
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 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
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])
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
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])
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
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
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 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
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')
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
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)
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()
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()
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)
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
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
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 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 _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]
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)
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)
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)
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
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)))
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 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
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)
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
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] ) )
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
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 _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()
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()
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
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
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
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()
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
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()
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()
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()
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
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)
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