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