This example demonstrates the haptic path (Constrain to Path). First, the user records a 3D trajectory and later the robot constrains the user into that path with a force-field mechanism. Also, the user can adjust the tangential forces for custom assistance or resistance along the path.
Steps for implementing the haptic path algorithm:
Record Cartesian position data from the robot while performing a 3D trajectory
Sample this data to be ("almost") equal-distant ( COARSE_STEP = 0.01; ) ==> (output) coarsePath
Calculate the cubic spline from the coarsePath points.
At each robot's control cycle, get its tool position (cp) and search for the closest point in the coarsePath. (This step has been modified slightly, as it is searching around the previous segment area for the detection of the closest distance point. That happened for the avoidance of intersecting-and-curved paths, where jerky situations are caused from the closest segment, far away from the current one.)
Then search the area around the closest coarse point by calculating the minimum distance of the tool position with the spline points between [ - COARSE_STEP , + COARSE_STEP ] iterating with a tiny FINE_STEP.
Finally, calculate the direction from the closest point into the spline.
What to focus on
- Focus on the force/haptic rendering when you deviate from the recorded path.
- Focus on the two different examples (Joint space and Tool space)
What issues does this fix?
- Fixes #117