def get_rotated_target(self, target: np.array) -> np.array: """ Rotate the target by the avatar's forward directional vector. :param target: The target position. :return: The rotated position. """ angle = TDWUtils.get_angle_between(v1=FORWARD, v2=self.frame.get_forward()) return TDWUtils.rotate_position_around(position=target - self.frame.get_position(), angle=-angle)
def reach_for_target(self, arm: Arm, target: np.array, stop_on_mitten_collision: bool, target_orientation: np.array = None, precision: float = 0.05) -> List[dict]: """ Get an IK solution to move a mitten to a target position. :param arm: The arm (left or right). :param target: The target position for the mitten. :param target_orientation: Target IK orientation. Usually you should leave this as None (the default). :param stop_on_mitten_collision: If true, stop moving when the mitten collides with something. :param precision: The distance threshold to the target position. :return: A list of commands to begin bending the arm. """ # Get the IK solution. rotations, ik_target = self._get_ik( target=target, arm=arm, target_orientation=target_orientation) angle = TDWUtils.get_angle_between(v1=FORWARD, v2=self.frame.get_forward()) target = TDWUtils.rotate_position_around( position=ik_target, angle=angle) + self.frame.get_position() rotation_targets = dict() for c, r in zip(self._arms[arm].links[1:-1], rotations[1:-1]): rotation_targets[c.name] = r self._ik_goals[arm] = _IKGoal( target=target, stop_on_mitten_collision=stop_on_mitten_collision, rotations=rotation_targets, precision=precision) commands = [self.get_start_bend_sticky_mitten_profile(arm=arm)] # If the avatar is holding something, strengthen the wrist. held = self.frame.get_held_left( ) if arm == Arm.left else self.frame.get_held_right() if len(held) > 0: commands.append({ "$type": "set_joint_angular_drag", "joint": f"wrist_{arm.name}", "axis": "roll", "angular_drag": 50, "avatar_id": self.id }) if self._debug: print([np.rad2deg(r) for r in rotations]) self._plot_ik(target=ik_target, arm=arm) # Show the target. commands.extend([{ "$type": "remove_position_markers" }, { "$type": "add_position_marker", "position": TDWUtils.array_to_vector3(target) }]) a = arm.name for c in self._ik_goals[arm].rotations: j = c.split("_") # Apply the motion. commands.extend([{ "$type": "bend_arm_joint_to", "angle": np.rad2deg(self._ik_goals[arm].rotations[c]), "joint": f"{j[0]}_{a}", "axis": j[1], "avatar_id": self.id }]) return commands