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