The wrench transform pipeline doesn't handle angled contacts correctly. This should be fairly easy to solve with some transform math. The original wrench is already Stamped, so we should be able to do a DoTransform, or maybe convert the two vectors to Point objects and transform them individually.

Right now the two still live in one module, but best practice is to split them. This way it's easier to refer to the base class while developing your implementation.

We need to move as many configuration variable as possible to YAML files and document them clearly. Users should expect AssemblyTools to be a closed box and Algorithm Blocks and their Algorithm program to be their work site.

Every Algorithm Block uses speed and force "region checks" to move to the next state. These bound checks are very complicated invocations, going on for multiple lines. This kind of reasoning is critical to the re-usability of the system so this invocation needs to be much more streamlined. We should probably wrap the entire "situation sensing" operation, including a) conditions and b) confidence reasoning into its own method or maybe Class.

We discussed moving the state reasoning to a new node entirely, the "Assembly State Emitter", but I don't remember how that was supposed to work. Each state is designed with only a few sets of very specific exit conditions; either ASE would need to be reconfigured per state (so it's no better than a sub-class) or it would need to be constantly checking all possible collision conditions.

To check collision conditions, input force and position inputs have to be compared with the force-torque-position feedback from the environment; that is, we detect a surface by poking it, and we detect an edge by running over it. The ASE would need to be able to periodically "probe" in different directions, which would necessarily interrupt the algorithm's other motion commands, creating a very unpredictable behavior.

I think we should create a Class of short-lived (i.e. until state transition) "Collision Checks" and iterate them each loop. Starting a new State instantiates little "watchers" with internal "confidence" levels and condition probes they can "request". Surface-finding "watcher" will request a motion toward the expected surface position. "Hole insertion watchers" will require small circles to return a circling normal force to indicate collision in all directions about an axis. Maybe this is good enough? Need to discuss.

Referring to the data flow diagram: on the "front and back" ends, right inside the input from hardware, the FT reading from hardware needs to be transformed to the TCP. This required transformation is the one which reinterprets incoming forces about the wrist as torques about the TCP, and reinterprets torques about the wrist as forces through the TCP. This should cause no change to current program functionality; it just corrects the high torque-to-force ratio when operating in corner mode.

Wrench reinterpretation includes both ends of the wrench pipeline

• Incoming measurements from the load cell are reoriented to be relative to the goal frame.
• forces and torques are reinterpreted under the assumption that the forces occur at the position of the TCP.
• Commanded forces in world space are reoriented to match TCP orientation, maintaining consistency between inputs (mention above) and resultant outputs.

Also conducted some minor tweaks to plot visuals, and removed some irrelevant comments.

Added Conntact_Config.yaml; Organized AssemblyTools class member instantiation; all constants are now drawn from Conntact_Config; Renaming and cleanup of "spiral_search_motion" method, and changed to rely on yaml params. Tested on hardware; runs properly on Spiral Search or Corner Search.

Make the "Find Surface" state call configurable enough to command "Find Reference Surface" which makes the robot poke its own stand, which has a known location, and thereby measure precisely the tip position of the peg in its grip. That would be easy to do if we add the right level of configurability.

AssemblyTools is messy! There are like 50 functions with very niche uses. We should pare them down or organize them so that users can identify the most useful and most important functions. This probably involves the reinstatement of the Utils class/subclass/tag.

Incoming wrench readings (conntext.average_wrench_gripper ) and outgoing wrench commands (topic ccc/target_wrench) are now correctly reoriented into task space and reinterpreted so that torques are about the tool tip.

Find all instances of the scalar "1000" and delete them so we can ask the user to always enter data into the configuration yamls in meters. In conntext, get_pose_from_yaml is a big offender. Make sure to fix the data in the yamls as you fix the code!

There are several layers of config files:

• peg_in_hole_params for the example ROS nodes
• conntact_params for the conntact instance on this hardware
• connfig for the current task instance

There's a lot of junk DNA in each of these files, and they don't include inline comment instructions about how to complete them.

We need to

• Delete unused items, or comment why they might be enabled for an application
• Add guidance comments so these files work as templates for new users

No rotation speed parameter is exposed in conntext config. Actual speed of move/rotation achieved does not match conntext config file values. The robot moves much faster than the default ".03 m" max speed passed in. Make those values more accurate.

The robot currently can't receive an orientation command relative to the hole. Changing orientations is dangerous, causing fast unplanned motion of the wrist as it orbits around the TCP. To enable this feature while mitigating the risk of damaging collision, we need a reliable and transparent system for setting the desired position command and controlling the motion speed. A new motion command system is probably needed.