def rotate_midsegment(self, rot_axis, rot_angle): """ Rotates the midsegment. Because the midsegment is constrained to point in the x direction, we implement this rotation by rotating the other joints. Parameters: rot_axis (numpy.ndarray): Cartesian coordinates of the rotation axis rot_angle (float): Rotation amount in degrees """ if not self.can_rotate_midsegment(rot_axis, rot_angle): raise ValueError("Cannot rotate midsegment. Rotation leads to a too short segment.") old_midsegment = self.joint_positions[self.mid_segment_id] new_midsegment = geom_3d.rotate_axis_angle(old_midsegment, rot_axis, rot_angle) # rotate every joint (except the midsegment joints) for i in range(self.joint_count): if i != self.mid_segment_id and i != self.mid_segment_id + 1: # note that we are rotating such that the new midsegment becomes the old one because the joints # rotate in the opposite direction to the midsegment new_joint_position = geom_3d.rotate_vector_by_vector(self.joint_positions[i], old_z=new_midsegment, new_z=old_midsegment) # update joint position self.joint_positions[i] = new_joint_position
def paperclip_shape_rotate_midsegment(h, params): """ This move rotates the midsegment of h, keeping the other segments fixed. Because we constrain midsegment to be aligned with the x axis, this rotation is implemented by rotating the other segments. Crucially, we want it to look like only the midsegment is rotated; so we rotate the viewpoint to do that. """ hp = h.copy() # if the object has only a midsegment if hp.joint_count == 2: return hp, 1.0, 1.0 # get random rotation axis and angle theta = np.random.rand() * 360.0 - 180.0 phi = np.random.rand() * 180.0 rotation_axis = geom_3d.spherical_to_cartesian((1.0, theta, phi)) kappa = 1 / (params['ROTATE_MIDSEGMENT_VARIANCE'] * np.pi**2 / 180**2) rotation_angle = np.random.vonmises(0.0, kappa) * 180.0 / np.pi # rotate midsegment try: # we might not be able to rotate midsegment by rotation_angle around rotation_axis hp.rotate_midsegment(rotation_axis, rotation_angle) except ValueError: return hp, 1.0, 1.0 # rotate each viewpoint to correct for the rotation of the joints (we want only the midsegment to change how it # looks) for i in range(len(hp.viewpoint)): vp_cartesian = geom_3d.spherical_to_cartesian(hp.viewpoint[i]) new_vp_cartesian = geom_3d.rotate_axis_angle(vp_cartesian, rotation_axis, -rotation_angle) hp.viewpoint[i] = geom_3d.cartesian_to_spherical(new_vp_cartesian) return hp, 1.0, 1.0
def can_rotate_midsegment(self, rot_axis, rot_angle): """ Check if we can rotate the midsegment by rot_angle around rot_axis. Parameters: rot_axis (numpy.ndarray): Cartesian coordinates of the rotation axis rot_angle (float): Rotation amount in degrees Returns: bool """ old_midsegment = self.joint_positions[self.mid_segment_id] new_midsegment = geom_3d.rotate_axis_angle(old_midsegment, rot_axis, rot_angle) # we need to check if rotating creates a segment that is too short. Note that this could happen for # neighboring segments of the midsegment if self.mid_segment_id > 0: new_joint_position = geom_3d.rotate_vector_by_vector(self.joint_positions[self.mid_segment_id-1], old_z=new_midsegment, new_z=old_midsegment) new_length = np.sqrt(np.sum(np.square(new_joint_position - self.joint_positions[self.mid_segment_id]))) if new_length < MIN_SEGMENT_LENGTH: return False if self.mid_segment_id < self.joint_count - 2: new_joint_position = geom_3d.rotate_vector_by_vector(self.joint_positions[self.mid_segment_id+2], old_z=new_midsegment, new_z=old_midsegment) new_length = np.sqrt(np.sum(np.square(new_joint_position - self.joint_positions[self.mid_segment_id+1]))) if new_length < MIN_SEGMENT_LENGTH: return False return True