This repository contains Python code used mostly for data processing, as well as a dynamic model using SymPy mirroring the Matlab version.
The model is functionally the same as the Matlab version, however is currently much slower so mostly only useable for kinematics. The most likely candidate for improving this is the computation of the Fresnel functions (noting that these are manually replaced with an approximation in the Matlab version).
Scripts defining the model are in scripts_model which outputs to generated_functions. In installable package version of the model equations is available here.
Several scripts process data collected in the experiments (manipulator EE state EE_pose.csv, force/torque readings EE_wrench.csv, RGB images images\ and associated extrinsic camera calibration transform TFs_adj.npz). Another repo is more related to collecting this data.
These are interactive scripts that generally have parameters that need to be adjusted while stepping through.
Uses the directory of image frames captured in the experiment and outputs the positions of the base, middle and end coloured markers on the object, in the XZ plane of the object model. The ROS timestamps are embedded as the image names.
Version where marker points can be manually selected in each frame instead of automatically by HSV filtering.
Processes the time series of manipulator EE state, FT measurements and marker positions extracted from the images. Resample data to consistent rate, extract curvature configurations of Theta from the marker positions, calculate derivatives. Output is saved to data_out\ and processed.npz. Also can generate animations of the state in XY plane or projected over the recorded image frames.
curvature_anim_FT_robot.mp4
overlay_anim_delay60.mp4
Version for processing a set of static equilibrium data points.