All the code should be run under matlab console.

addpath ./utils/ ./pose/ ./traj/ ./collision_detection/
mex ./utils/generatePointsCloud.cpp % -largeArrayDims % This make mxSize size_t
mex ./utils/myTraverser.cpp

Run the following scripts, this will return v, f, n. readSTLModel function have a parameter func, which is a function handle used for preprocessing, the default is preprocessStlVFN, you can do some other preprocess by change the function.

[v, f, n] = readStlModel("/Users/junr/Documents/Works/graduation-project/code/planning/123.stl");

The following code will return robot as robot arm model.

robot, q0, speed_limit, qlimit = getRobotModel();

Divide into small faces will need less memory space, but also make the path not global-optimal. So, the path will be generated according to different faces.

clusters = divideIntoFaces(v, f ,n); % clusters have shape [1, #clusters]

[pointsPath, pointsPathIdx] = generatePathFromClusters(clusters, v, f, n, 0.5, 0);

The function generatePath will run following code:

[pointsCloud, pointsCloudFaceIdx] = generatePointsCloud(clusters{cluIdx}, v, f, n, gap);
[orderedPointsCloud, orderedPointsCloudIdx] = myTraverser(pointsCloud, pointsCloudFaceIdx, method);

which use graph traverse algorithm, see myTraverser.cpp for detail.

Normal vectors generate before are path normal vectors. As for points, we need add some smoothness between different points in different faces. And you can add more modification to make it fit your project, your robot by changing the function.

normalVecsM = modifyNormalVec(normalVecs);

The connectPaths function transfer points and normal vectors into homogeneous transformation matrix. TODO: according to their clustersIdx, add transition path.

Ts = connectPaths(pointsPath, normalVecsM, clustersIdx);

The Ts2q function transfer homogeneous transformation matrix into joint space coordinates, qs, which is angle of six axis of the robot.

qs = Ts2q(myRobot, q0, 2, Ts);

Save result, name it.

array2txt(qs, '2018-5-16_t1');