Force controlled scan xPC
This page documents an xPC program that generates a raster trajectory scan pattern, executes(sends) the trajectory interpolation in real time, and calculates compatible (decomposed) positions in force-controlled direction and motion controlled direction (hybrid force position admittance control). The program design diagram is shown as below:
- There are two parts needed for this repository to work. The first part is the Simulink Real-Time code (this repository.) The second part is the cisst repository part.
- For the Simulink Real-Time part, run the Matlab script MAKE_PSMCMD.m to compile the Simulink design to loadable program to xPC.
- The cisst-saw-nri repository needs to be installed and the code script to use is PSM xPC at VU (or PSM xPC at CMU)
If a force sensing plate is used (as in the following example figure), the frame registration between the force sensor frame and the robot base frame is needed. This may be done by using the correspondence points in both frames. In other words, we can digitize some known land mark locations in force sensor frame using the robot.
As shown in the figure, the locations of the screw heads, A, B, C, D, are given according to the force sensor specification. Using the digitized coordinates in robot base frame, we can easily get the transformation using frame calculation if these points construct a frame or using Arun's method.
In the current code, to calculate this transformation:
- Run CollectForceSensorFrameData.m, and following the command window to save the land mark points A, B, C, D.
- Run CalculateFrameTF_PSM.m, and the result is saved in the default location afterwards at "slrt-dVRK-commander/Model/Config_Mat/"
- roslaunch dvrk_nri_robot vu_udp_xpc on the Linux machine to start the PSM program that will accept UDP command from xPC.
- Make sure PSM is ready to accept command from xPC. This can be tested by running Initialize_PSM_CMD.m
- Run DefineExplorationMapCorners.m in the folder Model\Config_Func, and by following the command line instructions, you can select multiple points that can be used to define a polygon.
- For each of the exploration map corner references saved as MapCornersRefNameX.mat, the script will automatically run GenRasterScanPath('MapCornersReferenceNameX'), then save the raster scan following the command line as rasterScanNameX.
- Then you should expect to have the 2D raster scan pattern as below: (a) is the 2D raster scan and (b) is the executed exploration by the robot to give you an idea of the reality environment.
Main_Surface_Explore.m is the main script to run once you have generated a raster scan pattern for a defined region of interest. Run in the command line as one of the followings:
- Main_Surface_Explore('rasterScanNameX')
- Main_Surface_Explore('rasterScanNameX','log name',dataSavedName)
If without specifying options, the default options would apply.
The expected result is a point cloud collection of the environment. The point cloud shown below is an example using PSM. The entire silicone phantom was partitioned into four zones to explore for surface points.
The data is logged in Matlab using a class dvrk_logger. All of the data samples are logged using struct format, associated with time, force measurement, position and orientation (quaternion representation). To see the exploration result, one need to first load the saved data to Matlab workspace, and then run the following in command line:
- logger.compute_contact;
- logger.plot_explr_map;
To generate a video of the exploration, named "Hamlyn", run the following command line:
- logger.gen_explr_video('video name','Hamlyn')
The fiducial locations are needed if the exploration data is expected to be used in another setup, i.e. the deformation of the model w.r.t. to its model frame does not change but the model frame in robot frame changes.
There are four fiducials labeled in the figure of the setup, as A, B, C, D.
Run DefineFiducialLocations('FiducialHamlyn2017VU')
