Optimize a Demonstration
The optimization process refines the original demonstration trajectory to produce a noise-free, smooth, and efficient trajectory.
The optimization occurs in two stages. At the end of each stage, a plot of the resulting trajectory will be displayed. Close the first plot to proceed to the second stage. The optimized demonstration will be saved with the prefix "smooth" (e.g., if the original demonstration is named "pick", the optimized version will be named "smoothpick").
The result can be visualized through Drake's MeshCat, accessible by default at localhost:7000.
Configuration
The configuration file for optimization is located in lfd_smoothing/config. The general configuration file for FR3 looks like this:
smoother_config: {
demo_filter_threshold: 0.01,
pkg_xml: $(find lfd_smoothing)/drake/franka_drake/package.xml,
urdf_path: package://franka_drake/urdf/fr3_nohand.urdf,
config_hierarchy: ["$(find lfd_smoothing)/config/opt/initial.yaml", "$(find lfd_smoothing)/config/opt/main.yaml"],
robot_type: "fr3"
}
- demo_filter_threshold: Determines the minimum distance between waypoints extracted from the original path. A higher value reduces the number of waypoints, leading to faster optimization.
- pkg_xml and urdf_path: Paths for the robot's URDF packages, used by Drake to model and visualize the robot.
- config_hierarchy: Paths to configuration files for the two stages of optimization.
Optimization Stages
The configuration files for each stage of the optimization are in lfd_smoothing/config/opt.
First Optimization Stage Configuration (FR3)
num_cps: 4
bspline_order: 4
velocity_scaling: 1
duration_bound: [0.01, 5]
coeff_duration: 1
tol_joint: 0
- num_cps: Number of control points per path segment.
- bspline_order: Order of the B-spline used for trajectory modeling.
- velocity_scaling: Scaling factor for velocity limits (default is 1).
- duration_bound: Minimum and maximum duration bounds for the trajectory.
- coeff_duration: Coefficient for the duration term in the cost function.
- tol_joint: Tolerance for deviation from the original joint configuration.
Second Optimization Stage Configuration (FR3)
bspline_order: 4
velocity_scaling: 1
acceleration_scaling: 1
jerk_scaling: 1
duration_bound: [0.01, 5]
coeff_duration: 1
coeff_jerk: 0.04
coeff_joint_cp_error: 1
tol_translation: 0.02
tol_rotation: 0.05
- acceleration_scaling and jerk_scaling: Similar to velocity scaling, used to adjust limits for acceleration and jerk.
- coeff_jerk: Coefficient for the jerk term in the cost function.
- coeff_joint_cp_error: Coefficient for penalizing deviations from the original joint configuration.
- tol_translation: Tolerance for end-effector translational deviation.
- tol_rotation: Tolerance for end-effector rotational deviation.
Tunable Parameters
Most configuration parameters are suitable for general use, but the following can be adjusted for specific demonstrations:
- demo_filter_threshold: Increasing this value allows for a smoother, faster trajectory by reducing the number of waypoints.
- tol_translation and tol_rotation: Adjust these values based on the required accuracy. Higher tolerances provide more freedom for smoother, faster optimization.