predefined cartesian poses (as joint positions)

moveit trajectory planning in joint space

image capture and object detection in each pose

detected objects matching in 3D based on point cloud reading

target = closest object detected in at least 2 frames, with predefined threshold

from current position to 20 cm distance from the target pepper

TODO: orientation?

moveit inverse kinematics for target pose

moveit position trajectory planned in joint space

if moveit fails to plan, joint position control in small increments until plan succeeds or until reference joint position reached

from current position, in 1 cm increments towards last detected 3D position of pepper

if target pepper is lost for longer than x, abort and return home

TODO: details

orientation such that gripper is horizontal, with pepper center point at image center

TODO: schematic

moveit inverse kinematics for target pose

franka_position_controller at 1 kHz with additional filtering

TODO: speed up position control

TODO: joint impedance position control (collaborative!)

finished: when close enough to pepper (5cm?) or when pepper not visible any longer

from last servo position, approach pepper using last estimated pepper pose

pepper pose estimation:

position: based on depth of center of detected bounding box

orientation: opencv contour within bounding box + fit ellipse + longer axis incline (in image projection plane)

account for camera/knife displacements

gripper control - call grasp action

franka gripper is stupid