Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions docs/api/pylabrobot.formulatrix.rst
Original file line number Diff line number Diff line change
Expand Up @@ -18,17 +18,17 @@ Mantis
driver.master.MantisDriver
driver.simulator.MantisSimulationDriver
driver.simulator.MantisSimulationArm
driver.simulator.MantisSimulationV3Arm
driver.simulator.MantisSimulationPlateClamp
driver.simulator.MantisSimulationPneumaticPlateClamp
driver.simulator.MantisSimulationPump
driver.simulator.MantisSimulationRfid
driver.simulator.MantisSimulationLedBar
driver.features.arm.Arm
driver.features.arm_v3.V3Arm
driver.features.arm.MantisPose
driver.features.arm.MantisArmConfiguration
driver.arm_kinematics
driver.stage_transform
driver.sequence_files
driver.features.plate_clamp.PlateClamp
driver.features.pneumatic_plate_clamp.PneumaticPlateClamp
driver.features.pump.Pump
driver.features.rfid.Rfid
driver.features.led_bar.LedBar
Expand Down
168 changes: 140 additions & 28 deletions docs/user_guide/formulatrix/mantis/hello-world.ipynb

Large diffs are not rendered by default.

2 changes: 1 addition & 1 deletion pylabrobot/formulatrix/mantis/carousel.py
Original file line number Diff line number Diff line change
Expand Up @@ -277,7 +277,7 @@ async def move_to(
except BaseException:
await self._stop_after_failure()
raise
logger.info("[%s] carousel at %.3f degrees", self.configuration.device_id, position % 360.0)
logger.info("[%s] carousel at %.2f degrees", self.configuration.device_id, position % 360.0)

async def move_to_slot(
self, slot: int, *, speed: Optional[float] = None, acceleration: Optional[float] = None
Expand Down
335 changes: 254 additions & 81 deletions pylabrobot/formulatrix/mantis/design.md

Large diffs are not rendered by default.

66 changes: 41 additions & 25 deletions pylabrobot/formulatrix/mantis/device.py
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

import math
import os
from typing import Dict, Optional, Union
from typing import Dict, Optional, cast

from pylabrobot.io.ftdi import FTDI
from pylabrobot.resources import Coordinate, Deck, Resource
Expand All @@ -19,22 +19,25 @@
from .driver.features.arm import Arm
from .driver.features.led_bar import LedBar
from .driver.features.plate_clamp import PlateClamp
from .driver.features.pneumatic_plate_clamp import PneumaticPlateClamp
from .driver.features.pump import Pump
from .driver.features.rfid import Rfid
from .driver.master import MantisDriver
from .driver.simulator import MantisSimulationDriver
from .resource_model import MANIFEST, arm_link, chip_rack, z_column

# The main box's size, in mm, from the manufacturer's model: up to the top plate's surface.
SIZE_X, SIZE_Y, SIZE_Z = MANIFEST["size"]
# The plate holder on the deck: an SBS footprint pushed into the back-left of the pin nest, in mm.
PLATE_HOLDER_LOCATION = Coordinate(23.62, 15.02, 0.0)
PLATE_HOLDER_SIZE_X, PLATE_HOLDER_SIZE_Y = 127.76, 85.48
# Where the right motor turns, x and y in the device frame, and the arm's height, in mm; the arm
# frame is the device frame moved there and mirrored in y.
ARM_BASE_X, ARM_BASE_Y = MANIFEST["joints"]["base_right"]
ARM_Z = MANIFEST["joints"]["arm_z"]
from .resource_model import (
ARM_BASE_X,
ARM_BASE_Y,
ARM_Z,
MANIFEST,
PLATE_HOLDER_LOCATION,
PLATE_HOLDER_SIZE_X,
PLATE_HOLDER_SIZE_Y,
SIZE_X,
SIZE_Y,
SIZE_Z,
arm_link,
chip_rack,
z_column,
)


class MantisDevice(Resource):
Expand All @@ -44,22 +47,30 @@ class MantisDevice(Resource):
arm. The generation is read at setup (``driver.configuration.generation``).
"""

def _update_arm_model(self, *, theta_1: float, theta_2: float) -> None:
"""Turn the arm's links to two link angles, the geometry's own, in degrees.
def _update_arm_model(self, *, theta_1: float, theta_2: float, z: float) -> None:
"""Turn the arm's links to two link angles, the geometry's own, in degrees, and raise or lower
the Z column with the arm on it to a height.

Args:
theta_1: motor 0's link angle; with its offset, the angle the motor reads.
theta_2: motor 1's link angle; with its offset, the angle the motor reads.
z: Z's height, in mm above its home flag, up positive.
"""
column_x, column_y, column_z = MANIFEST["modules"]["z_column"]["measured_at"]
self.z_column.location = Coordinate(x=column_x, y=column_y, z=column_z + z)
right_elbow, left_elbow = compute_elbow_positions(theta_1=theta_1, theta_2=theta_2)
head = compute_head_position(theta_1=theta_1, theta_2=theta_2)
for side, base, elbow in (
("right", RIGHT_BASE, right_elbow),
("left", LEFT_BASE, left_elbow),
):
base_x, base_y = cast(float, base.x), cast(float, base.y)
elbow_x, elbow_y = cast(float, elbow.x), cast(float, elbow.y)
# The device frame mirrors the arm frame in y, so an angle there turns the other way.
short_angle = -math.degrees(math.atan2(elbow[1] - base[1], elbow[0] - base[0]))
long_angle = -math.degrees(math.atan2(head[1] - elbow[1], head[0] - elbow[0]))
short_angle = -math.degrees(math.atan2(elbow_y - base_y, elbow_x - base_x))
long_angle = -math.degrees(
math.atan2(cast(float, head.y) - elbow_y, cast(float, head.x) - elbow_x)
)
short, long = self.arm_links[f"{side}_short_arm"], self.arm_links[f"{side}_long_arm"]
short.rotate_to(z=short_angle, pivot_coordinate=short.proximal_joint)
long.rotate_to(z=long_angle - short_angle, pivot_coordinate=long.proximal_joint)
Expand Down Expand Up @@ -165,7 +176,7 @@ def __init__(
self.assign_child_resource(self.right_chip_rack, location=measured_at["right_chip_rack"])

# The Z column and the arm it carries, drawn at idle until setup reads where the arm is. The
# column is drawn where it stands with Z at 0: how far it travels with Z is not measured.
# column is drawn where it stands with Z at 0 and moves with Z, the arm on it.
self.z_column = z_column(name=f"{name}_z_column")
self.assign_child_resource(self.z_column, location=measured_at["z_column"])
self.arm_links: Dict[str, LinkBody] = {}
Expand All @@ -175,7 +186,9 @@ def __init__(
assert short.distal_joint is not None
self.z_column.assign_child_resource(
short,
location=Coordinate(ARM_BASE_X + base[0], ARM_BASE_Y - base[1], ARM_Z)
location=Coordinate(
ARM_BASE_X + cast(float, base.x), ARM_BASE_Y - cast(float, base.y), ARM_Z
)
- measured_at["z_column"]
- short.proximal_joint,
)
Expand All @@ -185,11 +198,12 @@ def __init__(
location=short.distal_joint - long.proximal_joint + Coordinate(0.0, 0.0, above),
)
self.arm_links[f"{side}_short_arm"], self.arm_links[f"{side}_long_arm"] = short, long
self._update_arm_model(theta_1=0.0, theta_2=0.0)
self._update_arm_model(theta_1=0.0, theta_2=0.0, z=0.0)

@property
def plate_clamp(self) -> Optional[Union[PlateClamp, PneumaticPlateClamp]]:
"""The discovered motor or pneumatic plate clamp; None before discovery or without one."""
def plate_clamp(self) -> Optional[PlateClamp]:
"""The plate clamp, on board 12 or on a pressure-board output; None before discovery or
without one."""
return self.driver.plate_clamp

@property
Expand Down Expand Up @@ -220,9 +234,11 @@ async def setup(self, *, skip_arm: bool = False) -> None:
"""
await self.driver.setup(skip_arm=skip_arm)
arm = self.driver.arm
if arm is not None and arm.is_initialized:
theta_1, theta_2 = await arm.request_link_angles()
self._update_arm_model(theta_1=theta_1, theta_2=theta_2)
if arm is not None:
# Every move from here on passes where it ended to the model; the bring-up's is read here.
arm._pose_observer = self._update_arm_model
if arm.is_initialized:
await arm._report_pose()

async def stop(self) -> None:
"""Stop the motors, park an arm brought up, release the clamp and disconnect."""
Expand Down
28 changes: 27 additions & 1 deletion pylabrobot/formulatrix/mantis/device_tests.py
Original file line number Diff line number Diff line change
@@ -1,10 +1,15 @@
"""Mantis device tests: the parts tree."""
"""Mantis device tests: the parts tree, and the arm model following the moves."""

import os
import unittest
from unittest import mock

from pylabrobot.formulatrix.mantis import Mantis, MantisDevice
from pylabrobot.formulatrix.mantis.driver.arm_kinematics import (
MantisArmCoordinate,
compute_joint_angles,
)
from pylabrobot.formulatrix.mantis.driver.features.led_bar import LedBar
from pylabrobot.io.ftdi import FTDI

RESOURCE_MODEL = os.path.join(os.path.dirname(__file__), "resource_model")
Expand All @@ -25,5 +30,26 @@ def test_simulation_refuses_a_link(self):
Mantis(simulation=True, io=mock.create_autospec(FTDI, instance=True))


class ArmModelTests(unittest.IsolatedAsyncioTestCase):
async def test_a_move_poses_the_links_and_moves_the_z_column(self):
device = Mantis(simulation=True)
with mock.patch.object(LedBar, "_sleep", autospec=True):
await device.setup()
self.addAsyncCleanup(device.stop)
assert device.arm is not None
column = device.z_column.location
assert column is not None
# The simulated arm has no offsets: its joint state is the links' own angles.
theta_1, theta_2 = compute_joint_angles(location=MantisArmCoordinate(x=15.0, y=60.0, z=None))
await device.arm.move_to_joint_state(joint_state={0: theta_1, 1: theta_2, 2: 10.0})
self.assertAlmostEqual(device.z_column.get_absolute_location().z, column.z - 10.0)
idle, posed = Mantis(simulation=True), Mantis(simulation=True)
posed._update_arm_model(theta_1=theta_1, theta_2=theta_2, z=-10.0)
for name, link in device.arm_links.items():
with self.subTest(link=name):
self.assertAlmostEqual(link.rotation.z, posed.arm_links[name].rotation.z)
self.assertNotAlmostEqual(link.rotation.z, idle.arm_links[name].rotation.z)


if __name__ == "__main__":
unittest.main()
96 changes: 62 additions & 34 deletions pylabrobot/formulatrix/mantis/driver/arm_kinematics.py
Original file line number Diff line number Diff line change
Expand Up @@ -10,10 +10,26 @@

import bisect
import math
from typing import Dict, Sequence, Tuple
from dataclasses import dataclass
from typing import Dict, Optional, Sequence, Tuple, cast

from .errors import MantisKinematicsError


@dataclass(frozen=True)
class MantisArmCoordinate:
"""A position in the arm frame, mm: the unit's files' frame, not PLR's deck frame.

``Coordinate`` has its origin at the deck's front-left-bottom with z up; here the origin is the
base of motor 0's link, and z is Z's position, down from its home flag. An axis is None where a
route line holds it, or where only x and y are meant.
"""

x: Optional[float]
y: Optional[float]
z: Optional[float]


# The link geometry, in mm and degrees; the same on a v3 and a v4.
RIGHT_SHORT_ARM_LENGTH = 63.0
RIGHT_LONG_ARM_LENGTH = 84.0
Expand All @@ -23,11 +39,12 @@
GAMMA = 0.0
MIN_THETA_1 = -93.96
MIN_THETA_2 = -141.786
# Where each link pair turns, (x, y) in the arm frame, mm: the right pair at the origin.
RIGHT_BASE = (0.0, 0.0)
LEFT_BASE = (
ARM_DISTANCE * math.cos(math.radians(GAMMA)),
ARM_DISTANCE * math.sin(math.radians(GAMMA)),
# Where each link pair turns, in the arm frame: the right pair at the origin.
RIGHT_BASE = MantisArmCoordinate(x=0.0, y=0.0, z=None)
LEFT_BASE = MantisArmCoordinate(
x=ARM_DISTANCE * math.cos(math.radians(GAMMA)),
y=ARM_DISTANCE * math.sin(math.radians(GAMMA)),
z=None,
)

# Each motor's travel per revolution: degrees on the arms, mm on Z.
Expand Down Expand Up @@ -88,31 +105,35 @@ def compute_wrapped_angle(*, angle: float, minimum: float) -> float:
return angle


def compute_joint_angles(*, x: float, y: float) -> Tuple[float, float]:
"""The two links' angles that put the head at (x, y): inverse kinematics.
def compute_joint_angles(*, location: MantisArmCoordinate) -> Tuple[float, float]:
"""The two links' angles that put the head at a location's x and y: inverse kinematics.

The angles are the geometry's own, before a unit's motor offsets.

Args:
x: the head's x in the arm frame, in mm.
y: the head's y in the arm frame, in mm.
location: the head's position in the arm frame; its z is not used.

Raises:
MantisKinematicsError: if (x, y) is out of either link pair's reach or on a base.
ValueError: if x or y is None.
"""
base_x, base_y = LEFT_BASE
x, y = location.x, location.y
if x is None or y is None:
raise ValueError("inverse kinematics needs the head's x and y")
# The bases are always given in x and y.
base_x, base_y = cast(float, LEFT_BASE.x), cast(float, LEFT_BASE.y)
distance = math.hypot(x, y)
distance_to_other_base = math.hypot(x - base_x, y - base_y)
if distance == 0 or distance_to_other_base == 0:
raise MantisKinematicsError(f"({x:.3f}, {y:.3f}) mm is on a motor's base")
raise MantisKinematicsError(f"({x:.2f}, {y:.2f}) mm is on a motor's base")
first = (RIGHT_SHORT_ARM_LENGTH**2 - RIGHT_LONG_ARM_LENGTH**2 + distance**2) / (
2 * RIGHT_SHORT_ARM_LENGTH * distance
)
second = (LEFT_SHORT_ARM_LENGTH**2 - LEFT_LONG_ARM_LENGTH**2 + distance_to_other_base**2) / (
2 * LEFT_SHORT_ARM_LENGTH * distance_to_other_base
)
if not -1.0 <= first <= 1.0 or not -1.0 <= second <= 1.0:
raise MantisKinematicsError(f"({x:.3f}, {y:.3f}) mm is out of the arm's reach")
raise MantisKinematicsError(f"({x:.2f}, {y:.2f}) mm is out of the arm's reach")
theta_1 = 90 + math.degrees(math.asin(first) - math.atan2(y, x))
theta_2 = 90 - math.degrees(math.pi - math.asin(second) - math.atan2(y - base_y, x - base_x))
return (
Expand All @@ -123,27 +144,36 @@ def compute_joint_angles(*, x: float, y: float) -> Tuple[float, float]:

def compute_elbow_positions(
*, theta_1: float, theta_2: float
) -> Tuple[Tuple[float, float], Tuple[float, float]]:
"""Where each short link ends, (x, y) in the arm frame in mm.
) -> Tuple[MantisArmCoordinate, MantisArmCoordinate]:
"""Where each short link ends in the arm frame, motor 0's first; z None.

Args:
theta_1: motor 0's link angle, in degrees, the geometry's own.
theta_2: motor 1's link angle, in degrees, the geometry's own.
"""
base_x, base_y = LEFT_BASE
# The bases are always given in x and y.
base_x, base_y = cast(float, LEFT_BASE.x), cast(float, LEFT_BASE.y)
first = math.radians(180.0 - theta_1)
second = math.radians(theta_2)
return (
(RIGHT_SHORT_ARM_LENGTH * math.cos(first), RIGHT_SHORT_ARM_LENGTH * math.sin(first)),
(
base_x + LEFT_SHORT_ARM_LENGTH * math.cos(second),
base_y + LEFT_SHORT_ARM_LENGTH * math.sin(second),
MantisArmCoordinate(
x=RIGHT_SHORT_ARM_LENGTH * math.cos(first),
y=RIGHT_SHORT_ARM_LENGTH * math.sin(first),
z=None,
),
MantisArmCoordinate(
x=base_x + LEFT_SHORT_ARM_LENGTH * math.cos(second),
y=base_y + LEFT_SHORT_ARM_LENGTH * math.sin(second),
z=None,
),
)


def compute_head_position(*, theta_1: float, theta_2: float) -> Tuple[float, float]:
"""Where the head is for two link angles: forward kinematics, the solution of greater y.
def compute_head_position(*, theta_1: float, theta_2: float) -> MantisArmCoordinate:
"""Where the head is in the arm frame for two link angles, z None: forward kinematics.

Of the two solutions, the head's is the one left of the line from motor 0's elbow to motor 1's:
the recorded recoveries route from it in every zone recorded, the back zones included.

Args:
theta_1: motor 0's link angle, in degrees, the geometry's own.
Expand All @@ -152,30 +182,28 @@ def compute_head_position(*, theta_1: float, theta_2: float) -> Tuple[float, flo
Raises:
MantisKinematicsError: if the long links cannot meet at these angles.
"""
(first_x, first_y), (second_x, second_y) = compute_elbow_positions(
theta_1=theta_1, theta_2=theta_2
)
first, second = compute_elbow_positions(theta_1=theta_1, theta_2=theta_2)
# The elbows are always given in x and y.
first_x, first_y = cast(float, first.x), cast(float, first.y)
second_x, second_y = cast(float, second.x), cast(float, second.y)
distance = math.hypot(second_x - first_x, second_y - first_y)
if (
distance == 0
or distance > RIGHT_LONG_ARM_LENGTH + LEFT_LONG_ARM_LENGTH
or distance < abs(RIGHT_LONG_ARM_LENGTH - LEFT_LONG_ARM_LENGTH)
):
raise MantisKinematicsError(
f"the links cannot meet at angles ({theta_1:.3f}, {theta_2:.3f}) degrees"
f"the links cannot meet at angles ({theta_1:.2f}, {theta_2:.2f}) degrees"
)
along = (RIGHT_LONG_ARM_LENGTH**2 - LEFT_LONG_ARM_LENGTH**2 + distance**2) / (2 * distance)
across = math.sqrt(max(0.0, RIGHT_LONG_ARM_LENGTH**2 - along**2))
middle_x = first_x + along * (second_x - first_x) / distance
middle_y = first_y + along * (second_y - first_y) / distance
solutions = [
(
middle_x + sign * across * (second_y - first_y) / distance,
middle_y - sign * across * (second_x - first_x) / distance,
)
for sign in (1.0, -1.0)
]
return max(solutions, key=lambda solution: solution[1])
return MantisArmCoordinate(
x=middle_x - across * (second_y - first_y) / distance,
y=middle_y + across * (second_x - first_x) / distance,
z=None,
)


def compute_corrected_encoder_reading(
Expand Down
Loading
Loading