PF400 Interface Improvements: TCS Kinematics, Force Compliance, Height Detection, and Motion Enhancements

[Dozgulbas]

PF400 Interface Improvements: TCS Kinematics, Force Compliance, Height Detection, and Motion Enhancements

Overview

This PR covers a series of improvements to the PF400 robot interface spanning the TCS server custom command layer, the Python driver, and motion handling logic. The changes replace the old local kinematics implementation with accurate robot-side calculations, add new hardware features, and improve the reliability and flexibility of pick and place operations.

---

TCS Server: Custom GPL Commands (Custom.gpl)

A custom GPL module (RPL Custom Module 2.1) has been added to the robot's TCS server project (Tcp_cmd_server_pa). This module exposes five new TCP commands callable over port 10100.

Kinematics Commands

The existing local Python kinematics implementation (pf400_kinematics.py) had known inaccuracies and has been fully replaced by calls to the robot's internal KineSol method via these commands:

JointToCart j1 j2 j3 j4 j5 rail
Forward kinematics (FK): converts joint angles to Cartesian coordinates. Rail offset is added to X internally so the result matches wherec world coordinates.

CartToJoint X Y Z yaw pitch roll rail
Inverse kinematics (IK): converts Cartesian coordinates to joint angles. Rail is passed explicitly, subtracted from X before IK, and returned unchanged as j6.

RotateLoc j1 j2 j3 j4 j5 rail rotation_deg
Rotates the end effector yaw by rotation_deg degrees at a given joint location. Internally runs FK, applies the rotation, then IK. Used to switch between narrow and wide microplate orientations without saving duplicate locations.

Force Compliance Commands

Requires XY Compliance license on the controller.

EnableCompliance bias_torque_pct
Enables horizontal force compliance via AC_HorizCompliance2. The bias_torque_pct parameter (0-100) sets a bias torque as a percentage of the last used position control torque to prevent axis drift. 0 = fully free, typical values 0-20.

DisableCompliance
Disables horizontal force compliance and returns to normal position control. Always call after EnableCompliance.

---

Python interface (pf400.py)

Kinematics Module Removed

pf400_kinematics.py and the KINEMATICS parent class have been removed entirely. PF400 is now a standalone class. All kinematics calls now go through the TCS server.

New Kinematics Methods

pf400.joint_to_cart(joint_states)        # FK via JointToCart TCS command
pf400.cart_to_joint(cartesian, rail)     # IK via CartToJoint TCS command
pf400.rotate_yaw(joint_states, deg)      # FK + rotate + IK via RotateLoc TCS command
pf400.move_with_rotation(joints, deg)    # FK + rotate yaw + MoveC (avoids IK ambiguity)

New Force Compliance Methods

pf400.enable_compliance(bias_torque_pct=95)   # Enable horizontal compliance
pf400.disable_compliance()                     # Disable and return to position control

default_bias_torque_pct = 95 is set as a class attribute and can be overridden per call or at the class level.

New Height Detection Method

Requires Z Height Detection license on the controller.

pf400.height_detect(search_limit_mm=-400, max_force_n=-15, thorough=True)

Returns the detected Z height in mm. The gripper must be positioned at least 20mm above the surface before calling.

Updated Motion Methods

set_plate_rotation now calls rotate_yaw instead of the old local kinematics. check_incorrect_plate_orientation now uses joint_to_cart to check yaw and rotate_yaw to correct orientation.

---

Previous Motion Improvements

The following changes were made in earlier work and are included here for completeness:

Height and Grab Offsets

Pick, place, transfer, remove lid, and replace lid operations now support:

• grab_offset: adds a custom height offset to the default plate grab height, sourced from plate resource attributes
• approach_height_offset: adds an additional height offset to the approach waypoint, specified per location in the LocationArgument dictionary

Source and target locations can have independent approach height offsets in transfer operations.

Height Limit Validation

An optional height_limit parameter can be set per location in LocationArgument. If provided, pick_plate and place_plate validate the calculated above position against this limit before moving. If the approach height would exceed the limit an ActionFailed is returned without moving the robot.

Straight Motion Profile

A third motion profile (straight_motion_profile = 3) has been added for straight-line joint moves. The approach sequence now uses this profile for the critical segments:

• Moving from the above approach position down to the target
• Retreating from the target back up to the above approach position

Fast motion profile is used for all other approach waypoints.

Multiple Approach Locations

source_approach and target_approach now support both single and multiple approach waypoints. When a list of locations is provided the robot sequences through them in order on approach and in reverse on return, using straight motion for the waypoint closest to the target and fast motion for the rest.

Resource Validation

Pick and place operations now validate resource state before moving:

• Pick checks that a plate exists at the source before attempting to grab
• Place checks that the target is not already occupied

Validation failures return ActionFailed without moving the robot, keeping the node alive for subsequent commands.

---

Files Changed

• src/pf400_interface/pf400_driver.py — main driver, kinematics removed, new methods added
• src/pf400_interface/pf400_kinematics.py — deleted
• robot/Tcp_cmd_server_pa/Custom.gpl — new TCS custom commands
• docs/DEPLOY_CUSTOM_GPL.md — deployment instructions for Custom.gpl