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
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 _get_random_vector_along(z_vector, min_angle=30.0, max_angle=180.0): """ Get a random vector that makes more than min_angles and less than max_angles degrees with the `z_vector`. This method is used by ``paperclip_shape_add_remove_joint`` move. Note that the angle between the returned vector and the -z_vector (NEGATIVE z_vector, not the z_vector) will be in (min_angle, max_angle). If we add such a vector to z_vector, the angle between z_vector and the new vector will be in (min_angle, max_angle). """ if max_angle < min_angle: raise ValueError("Maximum angle cannot be smaller than minimum angle.") max_phi = 180.0 - min_angle min_phi = 180.0 - max_angle phi = min_phi + (np.random.rand() * (max_phi - min_phi)) theta = np.random.rand() * 360.0 coords = geom_3d.spherical_to_cartesian((1.0, theta, phi)) v = geom_3d.rotate_vector_by_vector(coords, old_z=np.array([0., 0., 1.]), new_z=z_vector) return v