def orthogonalize_gripper(axis_0, axis_2):
# axis_2 is the main axis
# axis_0 is the secondary axis
# assuming axis_0 cross axis_1 should be axis_2
# axis_2 stays axis_2
# axis_1 = axis_2 cross axis_0
# axis_0 = axis_1 cross axis_2
rotation_mat = np.zeros((3, 3))
rotation_mat[:, 2] = axis_2
axis_1 = math_utils.normalize(np.cross(axis_2, axis_0))
rotation_mat[:, 1] = axis_1
rotation_mat[:, 0] = math_utils.normalize(np.cross(axis_1, axis_2))
return rotation_mat
def orthogonalize_gripper(axis_0, axis_2):
#axis_2 is the main axis
#axis_0 is the secondary axis
#assuming axis_0 cross axis_1 should be axis_2
#axis_2 stays axis_2
#axis_1 = axis_2 cross axis_0
#axis_0 = axis_1 cross axis_2
rotation_mat = np.zeros((3,3))
rotation_mat[:,2] = axis_2
axis_1 = math_utils.normalize(np.cross(axis_2, axis_0))
rotation_mat[:,1] = axis_1
rotation_mat[:,0] = math_utils.normalize(np.cross(axis_1, axis_2))
return rotation_mat
def compute_rotation(self, segments):
# segments is a list of tuples
# returns the normalized vector of the transformed second point - the transformed first point
starts = [seg[0] for seg in segments]
ends = [seg[1] for seg in segments]
transformed_starts = self.transform_points(starts)
transformed_ends = self.transform_points(ends)
vectors = [math_utils.normalize(end - start) for start, end in zip(transformed_starts, transformed_ends)]
return vectors
def compute_rotation(self, segments):
#segments is a list of tuples
#returns the normalized vector of the transformed second point - the transformed first point
starts = [seg[0] for seg in segments]
ends = [seg[1] for seg in segments]
transformed_starts = self.transform_points(starts)
transformed_ends = self.transform_points(ends)
vectors = [math_utils.normalize(end - start) for start, end in zip(transformed_starts, transformed_ends)]
return vectors