def get_rot_vector(frame):
qx, qy, qz, _ = real_quat_from_matrix(frame)
axis = vector3(qx, qy, qz)
sin_a2 = norm(axis)
a = asin(sin_a2) * 2
return axis * (a / sin_a2)
def encode_rotation(data):
quat = real_quat_from_matrix(data)
out = QuaternionMsg()
out.w = quat[3]
out.x = quat[0]
out.y = quat[1]
out.z = quat[2]
return out
for x, (pose_name,
noise_pose) in enumerate(zip(pose_names, noise_poses)):
try:
trans = tf_buffer.lookup_transform(reference_frame, pose_name,
rospy.Time(0))
stamp = trans.header.stamp
translation = trans.transform.translation
rotation = trans.transform.rotation
clean_pose = np_frame3_quaternion(translation.x, translation.y,
translation.z, rotation.x,
rotation.y, rotation.z,
rotation.w)
noisy_obs = clean_pose.dot(noise_pose)
msg.transforms[x] = trans
quat = real_quat_from_matrix(noisy_obs)
trans.header.stamp = rospy.Time.now()
trans.transform.translation.x = noisy_obs[0, 3]
trans.transform.translation.y = noisy_obs[1, 3]
trans.transform.translation.z = noisy_obs[2, 3]
trans.transform.rotation.x = quat[0]
trans.transform.rotation.y = quat[1]
trans.transform.rotation.z = quat[2]
trans.transform.rotation.w = quat[3]
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
print(
f'Exception raised while looking up {pose_name} -> {reference_frame}:\n{e}'
)
break
else:
def _generate_pose_constraints(self, str_path, model):
with self.lock:
if str_path in self.tracked_poses:
align_rotation = '{} align rotation'.format(str_path)
align_position = '{} align position'.format(str_path)
if model is not None:
m_free_symbols = cm.free_symbols(model.pose)
if len(m_free_symbols) > 0:
te = self.tracked_poses[str_path]
self.joints |= m_free_symbols
self.joint_aliases = {s: str(s) for s in self.joints}
r_dist = norm(
cm.rot_of(model.pose) - cm.rot_of(te.pose))
self.soft_constraints[align_rotation] = SC(
-r_dist, -r_dist, 1, r_dist)
# print(align_position)
# print(model.pose)
dist = norm(cm.pos_of(model.pose) - cm.pos_of(te.pose))
# print('Distance expression:\n{}'.format(dist))
# print('Distance expression symbol overlap:\n{}'.format(m_free_symbols.intersection(cm.free_symbols(dist))))
# for s in m_free_symbols:
# print('Diff w.r.t {}:\n{}'.format(s, cm.diff(dist, s)))
self.soft_constraints[align_position] = SC(
-dist, -dist, 1, dist)
self.generate_opt_problem()
# Avoid crashes due to insufficient perception data. This is not fully correct.
if str_path in self._unintialized_poses:
state = self.integrator.state.copy(
) if self.integrator is not None else {}
state.update({
s: 0.0
for s in m_free_symbols if s not in state
})
null_pose = cm.subs(model.pose, state)
pos = cm.pos_of(null_pose)
quat = real_quat_from_matrix(cm.rot_of(null_pose))
msg = PoseMsg()
msg.position.x = pos[0]
msg.position.y = pos[1]
msg.position.z = pos[2]
msg.orientation.x = quat[0]
msg.orientation.y = quat[1]
msg.orientation.z = quat[2]
msg.orientation.w = quat[3]
te.update_state(msg, self.integrator.state)
else:
regenerate_problem = False
if align_position in self.soft_constraints:
del self.soft_constraints[align_position]
regenerate_problem = True
if align_rotation in self.soft_constraints:
del self.soft_constraints[align_rotation]
regenerate_problem = True
if regenerate_problem:
self.generate_opt_problem()
random_rot_normal(len(frames), 0, angular_std))
]
# Calculate forward kinematics of frames
obs_frames = {
k: subs(f, joint_state).dot(n)
for (k, f), n in zip(frames.items(), noise)
}
for x, (k, f) in enumerate(obs_frames.items()):
update_msg.poses[x].header.frame_id = k
update_msg.poses[x].pose.position.x = float(f[0, 3])
update_msg.poses[x].pose.position.y = float(f[1, 3])
update_msg.poses[x].pose.position.z = float(f[2, 3])
qx, qy, qz, qw = real_quat_from_matrix(f)
update_msg.poses[x].pose.orientation.x = qx
update_msg.poses[x].pose.orientation.y = qy
update_msg.poses[x].pose.orientation.z = qz
update_msg.poses[x].pose.orientation.w = qw
try:
tracker.cb_process_obs(update_msg)
time_start = Time.now()
tracker.cb_tick(None)
iter_times.append(
(Time.now() - time_start).to_sec() /
(tracker.integrator.current_iteration + 1))
n_iter.append(tracker.integrator.current_iteration + 1)
def matrix_to_transform(matrix):
quat = real_quat_from_matrix(matrix)
pos = matrix[:3, 3]
return pb.Transform(pb.Quaternion(*quat), pb.Vector3(*pos))