def test_quaternions_from_matrices_no_batch():
random_state = np.random.RandomState(85)
for _ in range(5):
q = pr.random_quaternion(random_state)
R = pr.matrix_from_quaternion(q)
q2 = pbr.quaternions_from_matrices(R)
pr.assert_quaternion_equal(q, q2)
a = np.array([1.0, 0.0, 0.0, np.pi])
q = pr.quaternion_from_axis_angle(a)
R = pr.matrix_from_axis_angle(a)
q_from_R = pbr.quaternions_from_matrices(R)
assert_array_almost_equal(q, q_from_R)
a = np.array([0.0, 1.0, 0.0, np.pi])
q = pr.quaternion_from_axis_angle(a)
R = pr.matrix_from_axis_angle(a)
q_from_R = pbr.quaternions_from_matrices(R)
assert_array_almost_equal(q, q_from_R)
a = np.array([0.0, 0.0, 1.0, np.pi])
q = pr.quaternion_from_axis_angle(a)
R = pr.matrix_from_axis_angle(a)
q_from_R = pbr.quaternions_from_matrices(R)
assert_array_almost_equal(q, q_from_R)
def transformCM(ptlist, ftlist, ftlist_trimmed, params):
nT = params['nT']
nF = params['nF']
AC = params['AC']
ftlist2 = copy.deepcopy(ftlist)
ptlist2 = copy.deepcopy(ptlist)
world_origin = np.array([0, 0, 0])
world_vec = np.identity(3)
for it in range(nT):
obj_origin = AC.COM(ftlist_trimmed[it]).transpose()
w, obj_vec = AC.GetBasisVectors(ftlist_trimmed[it])
T = (world_origin - obj_origin)
# Get vector perp to z-axis and nematic director
vperp = pyro.perpendicular_to_vectors(world_vec[:, -1], obj_vec[:, -1])
if np.linalg.norm(vperp) > 0.00001:
# rotation needed
vperp = vperp / np.linalg.norm(vperp)
theta = pyro.angle_between_vectors(world_vec[:, -1], obj_vec[:,
-1])
axis_angle = np.concatenate((vperp, -np.array([theta])))
q = pyro.quaternion_from_axis_angle(axis_angle)
# transfrom filaments
for fil in ftlist2[it]:
fil.pos0 = pyro.q_prod_vector(q, fil.pos0 + T.transpose())
fil.pos1 = pyro.q_prod_vector(q, fil.pos1 + T.transpose())
# transfrom proteins
# for prot in ptlist2[it]:
# prot.pos0 = pyro.q_prod_vector( q, prot.pos0 +T.transpose() )
# prot.pos1 = pyro.q_prod_vector( q, prot.pos0 +T.transpose() )
else:
# transfrom filaments
for fil in ftlist2[it]:
fil.pos0 = fil.pos0 + T.transpose()
fil.pos1 = fil.pos1 + T.transpose()
# transfrom proteins
# for prot in ptlist2[it]:
# prot.pos0 = prot.pos0 +T.transpose()
# prot.pos1 = prot.pos0 +T.transpose()
return ftlist2, ptlist2
def transform_coords(coords, origin_new, axis_new):
# transform coordinates to a new frame of reference with the orivided
# new origin and the the new basis matrix
# Translate origin
coords = coords - origin_new
# Get vector perp to z-axis and the new z-axis
vperp = pyro.perpendicular_to_vectors(np.array([0, 0, 1]), axis_new)
# Rotate if needed
if np.linalg.norm(vperp) > 0.00001:
vperp = vperp / np.linalg.norm(vperp)
theta = pyro.angle_between_vectors(np.array([0, 0, 1]), axis_new)
axis_angle = np.concatenate((vperp, -np.array([theta])))
q = pyro.quaternion_from_axis_angle(axis_angle)
# transfrom coordinates
coords = q_prod_coords(coords, q)
return coords
p = np.array([1.0, 1.0, 1.0]) euler = [0, 0, 0] euler[axis] = angle R = pr.active_matrix_from_intrinsic_euler_xyz(euler) pr.plot_basis(ax, R, p) p = np.array([-1.0, 1.0, 1.0]) euler = [0, 0, 0] euler[2 - axis] = angle R = pr.active_matrix_from_intrinsic_euler_zyx(euler) pr.plot_basis(ax, R, p) p = np.array([1.0, 1.0, -1.0]) R = pr.active_matrix_from_angle(axis, angle) pr.plot_basis(ax, R, p) p = np.array([1.0, -1.0, -1.0]) e = [pr.unitx, pr.unity, pr.unitz][axis] a = np.hstack((e, (angle, ))) R = pr.matrix_from_axis_angle(a) pr.plot_basis(ax, R, p) pr.plot_axis_angle(ax, a, p) p = np.array([-1.0, -1.0, -1.0]) q = pr.quaternion_from_axis_angle(a) R = pr.matrix_from_quaternion(q) pr.plot_basis(ax, R, p) plt.show()
def calc_inverse_kinematics(self,
guess,
target_position,
target_orientation,
tcp_config=None,
orientation_type=None):
"""
End-effector orientation (target_orientation) can be provided in several standard formats,
by specifying the orinetation_type (default is None):
- 'matrix': rotation matrix -> 3x3 array, in row major order
- 'axis_angle': axis angle -> {'angle': float, 'x': float, 'y': float, 'z': float}
- 'euler_zyx': {yaw, pitch, roll} Euler (Tait-Bryan) angles -> {'yaw': float, 'pitch': float, 'roll': float}
- 'quat': quaternion -> {'w': float, 'x': float, 'y': float, 'z': float}
- None: defaults to quaternion
Conversion relies on pytransform3d library
"""
quat_not_normed = None
if orientation_type == 'matrix':
self.__ensure_pyt3d()
quat_not_normed = pyrot.quaternion_from_matrix(
pyrot.check_matrix(target_orientation))
elif orientation_type == 'axis_angle':
self.__ensure_pyt3d()
axis_angle = [
target_orientation['x'], target_orientation['y'],
target_orientation['z'], target_orientation['angle']
]
quat_not_normed = pyrot.quaternion_from_axis_angle(
pyrot.check_axis_angle(axis_angle))
elif orientation_type == 'euler_zyx':
self.__ensure_pyt3d()
euler_zyx = [
target_orientation['yaw'], target_orientation['pitch'],
target_orientation['roll']
]
matrix = pyrot.matrix_from_euler_zyx(euler_zyx)
quat_not_normed = pyrot.quaternion_from_matrix(
pyrot.check_matrix(matrix))
elif orientation_type == 'quat' or orientation_type is None:
quat_not_normed = [
target_orientation['w'], target_orientation['x'],
target_orientation['y'], target_orientation['z']
]
else:
eva_error(
f'calc_inverse_kinematics invalid "{orientation_type}" orientation_type'
)
quat_normed = self.__normalize(quat_not_normed)
quaternion = {
'w': quat_normed[0],
'x': quat_normed[1],
'y': quat_normed[2],
'z': quat_normed[3]
}
body = {
'guess': guess,
'position': target_position,
'orientation': quaternion
}
if tcp_config is not None:
body['tcp_config'] = tcp_config
r = self.api_call_with_auth('PUT', 'calc/inverse_kinematics',
json.dumps(body))
if r.status_code != 200:
eva_error('inverse_kinematics error', r)
return self.__check_calculation(r, 'calc_inverse_kinematics',
'ik')['joints']
[test[1] / 2.0, test[1]])
rot[3].set_3d_properties([test[2] / 2.0, test[2]])
velocity.append(np.linalg.norm(R - last_R))
last_R = R
profile.set_data(np.linspace(0, 1, n_frames)[:len(velocity)], velocity)
return rot
if __name__ == "__main__":
# Generate random start and goal
np.random.seed(3)
start = np.array([0, 0, 0, np.pi])
start[:3] = np.random.randn(3)
start = pr.quaternion_from_axis_angle(start)
end = np.array([0, 0, 0, np.pi])
end[:3] = np.random.randn(3)
end = pr.quaternion_from_axis_angle(end)
n_frames = 200
fig = plt.figure(figsize=(12, 5))
ax = fig.add_subplot(121, projection="3d")
ax.set_xlim((-1, 1))
ax.set_ylim((-1, 1))
ax.set_zlim((-1, 1))
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
