Payload Identification and Tools
Identification of the tool attached to the flange.
The robot compensates the gravity of the arm and of the payload: the end effector configured in Desk plus the load set with RobotInterface.set_load(). If the payload does not match the tool, the arm drifts in torque control, e.g. with zero torques. identify_payload() estimates the mass and center of mass of the tool from the joint torques at rest in a set of poses, starting from any payload setting, e.g. none.
At rest, the measured joint torques minus the gravity torques of the robot model, tau_ext_hat_filtered, are the gravity torques of the part of the load the robot does not know about, plus stiction. Some joint friction acts past the torque sensors: at rest, they also see part of the torque with which the controller holds the joint against stiction, which on an FR3 was a few tenths of a Nm, flipping sign with the side the joint came from. So identify_payload() approaches each pose from both sides and averages. The gravity torques are linear in the mass m and the first moment m * c of the unknown part:
tau = g * [J_v^T e_z | -J_w^T [e_z]x R] @ [m, m * c]
where J_v and J_w are the linear and angular Jacobian and R the orientation of the flange in the base frame, and c is the center of mass in the flange frame. With poses in which the flange points in different directions, least squares gives m and c. The inertia does not change the gravity torques, so it cannot be identified at rest. It does not matter for the robot’s gravity compensation either.
- aiofranka.payload.payload_regressor(qpos, kinematics=None)[source]
Regressor of the gravity torques of a load on the flange.
- Parameters:
qpos (array-like) – Joint positions [rad] (7,)
kinematics (tuple | None) – MuJoCo model, data and flange site id to compute it with (default: a model of the FR3)
- Returns:
- Matrix Y (7, 4) such that Y @ [m, m * cx, m * cy, m * cz] are
the joint torques [Nm] that hold a load of mass m [kg] with center of mass c [m] in the flange frame against gravity.
- Return type:
np.ndarray
- class aiofranka.payload.PayloadEstimate(mass, com, mass_std, com_std, previous, bias, rms_before, rms_after, condition, qpos, residuals)[source]
Tool on the flange estimated by identify_payload() or fit_payload().
The estimate is the whole tool: the payload that was set during the measurements plus the correction the measurements found.
- Parameters:
- com
Center of mass in the flange frame [m] (3,)
- Type:
np.ndarray
- com_std
Standard error of the center of mass [m] (3,)
- Type:
np.ndarray
- bias
Torque offsets per joint [Nm] (7,), e.g. from the torque sensors. The payload cannot compensate them.
- Type:
np.ndarray
- rms_before
RMS of the residual torques at rest with the previous payload [Nm] (7,)
- Type:
np.ndarray
- rms_after
RMS of the residual torques at rest predicted with this tool [Nm] (7,). It includes the offsets in bias.
- Type:
np.ndarray
- condition
Condition number of the least-squares problem. Large values, e.g. > 100, mean the poses do not tell the parameters apart.
- Type:
- qpos
Joint positions of the measurements [rad] (n, 7)
- Type:
np.ndarray
- residuals
Measured residual torques at rest [Nm] (n, 7)
- Type:
np.ndarray
- __init__(mass, com, mass_std, com_std, previous, bias, rms_before, rms_after, condition, qpos, residuals)
- aiofranka.payload.fit_payload(qpos, residuals, previous=None, fit_bias=True, kinematics=None)[source]
Estimate the tool on the flange from residual torques at rest.
Solves for the mass and first moment of the unmodeled part of the tool, and with fit_bias a constant torque offset per joint, by least squares. A second pass weights the joints by their residual noise, so that model errors of the shoulder joints do not swamp the wrist joints, which see the center of mass.
- Parameters:
qpos (array-like) – Joint positions [rad] (n, 7)
residuals (array-like) – Residual torques at rest [Nm] (n, 7): the measured joint torques minus the gravity torques of the arm and the previous payload, e.g. tau_ext_hat_filtered
previous (dict | None) – Payload that was set during the measurements, with “mass” [kg] and “com” [m] (default: none)
fit_bias (bool) – Estimate a torque offset per joint (default: True)
kinematics (tuple | None) – See payload_regressor()
- Returns:
Estimated tool, i.e. previous plus the correction
- Return type:
- Raises:
ValueError – If there are too few poses for the parameters
- async aiofranka.payload.identify_payload(controller, tool_length=0.2, tool_radius=0.1, floor=0.0, clearance=0.05, n_poses=16, speed=0.5, settle=1.5, duration=1.0, fit_bias=True, verbose=True)[source]
Identify the mass and center of mass of the tool on the flange.
Plans n_poses poses around the current joint positions, in which the flange points in different directions, and moves through them in joint impedance control, approaching each from both sides. It measures the residual joint torques at rest in each and fits the tool with fit_payload(). It only measures: save the result as an end-effector profile in Desk with aiofranka.save_tool() and activate it with aiofranka.load_tool(). It starts from the payload the robot compensates, e.g. none, so running it again with the new profile active checks the result: the correction should be close to zero.
The poses and the paths between them are checked for collisions of the arm, a cylinder around the tool and the floor. Planning computes for a few tenths of a second, which would stall a running 1 kHz control loop and make the robot abort, so it plans while no control loop runs: a running controller is stopped for it, while the robot holds still, and started again.
- Parameters:
controller (FrankaController) – Controller, started or not. One that was not started is started for the identification and stopped afterwards.
tool_length (float) – Length of the tool from the flange along its z-axis [m] (default: 0.2)
tool_radius (float) – Radius of the tool around the flange z-axis [m] (default: 0.1)
floor (float) – Height of the floor or table in the base frame [m] (default: 0.0, the mounting plane of the robot)
clearance (float) – Minimum distance of the tool from the arm, and of the arm and tool from the floor [m] (default: 0.05)
n_poses (int) – Number of poses (default: 16)
speed (float) – Peak joint speed of the motions [rad/s] (default: 0.5)
settle (float) – Time to wait in each pose before measuring [s] (default: 1.5). tau_ext_hat_filtered lags by about 0.3 s.
duration (float) – Time to average the torques over in each pose [s] (default: 1)
fit_bias (bool) – Estimate a torque offset per joint (default: True)
verbose (bool) – Show a progress bar and the result (default: True)
- Returns:
Estimated tool
- Return type:
- Raises:
ValueError – If too few poses around the current joint positions are clear; move the arm to an open pose
- Caveats:
Takes about 3 minutes. Start in an open pose, keep other obstacles out of reach, and stay close to the e-stop.
Afterwards, a running controller is in impedance mode at the start pose.
Tools on the flange, kept as end-effector profiles in Desk.
Desk keeps named end-effector profiles (Settings > End Effector): the mass, center of mass and inertia of the tool on the flange, and its tool center point. The robot compensates the gravity of the active profile, and RobotInterface merges it into the MuJoCo model when it connects. These functions manage the profiles through the admin API that the Desk web UI uses, so Desk stays the one place that knows which tool is mounted.
Example
>>> estimate = await controller.identify_payload()
>>> await controller.stop()
>>> aiofranka.save_tool("gripper", estimate.mass, estimate.com)
>>> aiofranka.load_tool("gripper")
- class aiofranka.tools.Tool(name, mass, com, inertia, translation, rotation, device='ee-no-gripper', active=False, id=None)[source]
An end-effector profile in Desk.
- Parameters:
- com
Center of mass in the flange frame [m] (3,)
- Type:
np.ndarray
- inertia
Inertia about the center of mass in the flange frame [kg m^2] (3, 3)
- Type:
np.ndarray
- translation
Translation from the flange to the tool center point [m] (3,)
- Type:
np.ndarray
- rotation
Rotation from the flange to the tool center point as roll, pitch, yaw [rad] (3,)
- Type:
np.ndarray
- aiofranka.tools.list_tools(ip=None, username=None, password=None, protocol='https')[source]
The end-effector profiles in Desk.
- aiofranka.tools.active_tool(ip=None, username=None, password=None, protocol='https', timeout=None)[source]
The end-effector profile active in Desk, the tool the robot compensates.
Only reads: it never takes the control token.
- Parameters:
- Returns:
The active profile
- Return type:
- aiofranka.tools.save_tool(name, mass, com, inertia=None, translation=None, rotation=None, ip=None, username=None, password=None, protocol='https')[source]
Save a tool as an end-effector profile in Desk.
Updates the profile with that name, keeping what is not given, or creates one for a Generic Device. Does not activate it; see load_tool().
- Parameters:
name (str) – Name of the profile
mass (float) – Mass [kg]
com (array-like) – Center of mass in the flange frame [m] (3,)
inertia (array-like | None) – Inertia about the center of mass in the flange frame [kg m^2], as a (3, 3) tensor or its diagonal (3,). Default: keep the profile’s, or for a new one that of a solid sphere of radius DEFAULT_RADIUS, since the robot needs some inertia.
translation (array-like | None) – Translation from the flange to the tool center point [m] (3,) (default: keep the profile’s, or zero)
rotation (array-like | None) – Rotation from the flange to the tool center point as roll, pitch, yaw [rad] (3,) (default: keep the profile’s, or zero)
ip (str | None) – Robot IP (default: from config or 172.16.0.2)
username – Desk credentials (default: from config)
password – Desk credentials (default: from config)
protocol – Desk credentials (default: from config)
- Returns:
The saved tool
- Return type:
- Raises:
ValueError – If Desk or the robot would reject the tool
- aiofranka.tools.load_tool(name, ip=None, username=None, password=None, protocol='https')[source]
Activate an end-effector profile in Desk, so the robot compensates that tool.
Desk keeps it active across connections and reboots. RobotInterface merges it into the MuJoCo model when it connects, so connect after loading.
- Parameters:
- Returns:
The loaded tool
- Return type:
- Raises:
KeyError – If Desk has no profile, or several, with that name
- aiofranka.tools.unload_tool(ip=None, username=None, password=None, protocol='https')[source]
Activate Desk’s built-in profile without an end effector (“No End Effector”).
- aiofranka.tools.remove_tool(name, ip=None, username=None, password=None, protocol='https')[source]
Delete an end-effector profile from Desk.
- Raises:
KeyError – If Desk has no profile, or several, with that name
ValueError – If the profile is built in or active