|
| 1 | +Example: A simple 2D Matlab example |
| 2 | +=================================================== |
| 3 | +We start with a very simple Matlab example, with a 3-link planer arm avoiding obstacles. |
| 4 | +Make sure you have properly installed gpmp2 with Matlab toolbox, and added the toolbox path to Matlab paths. |
| 5 | +The full example is available at [matlab/Arm3PlannerExample.m](../matlab/Arm3PlannerExample.m). |
| 6 | + |
| 7 | +Dataset |
| 8 | +----- |
| 9 | +The generate2Ddataset utility function will generate a dataset object that includes the ground truth 2D occupancy grid, |
| 10 | +cell size of the occupancy grid, and the occupancy grid origin (in gtsam.Point2 object) in world coordinate frame. |
| 11 | + |
| 12 | +```matlab |
| 13 | +%% small dataset |
| 14 | +dataset = generate2Ddataset('TwoObstaclesDataset'); |
| 15 | +``` |
| 16 | + |
| 17 | +Signed Distance Field |
| 18 | +----- |
| 19 | +After generating datasets, the signed distance field can be calculated from the ground truth occupancy grid. |
| 20 | + |
| 21 | +In 2D cases, the signed distance field is just a matrix. Matrix column index represents X direction, |
| 22 | +and row index represents Y direction. |
| 23 | +The origin is the position of cell (1,1) in the matrix. |
| 24 | +A [PlanarSDF](../gpmp2/obstacle/PlanarSDF.h) object which stores the signed distance field is initialized as follows: |
| 25 | + |
| 26 | +```matlab |
| 27 | +% signed distance field |
| 28 | +field = signedDistanceField2D(dataset.map, cell_size); |
| 29 | +sdf = PlanarSDF(origin_point2, cell_size, field); |
| 30 | +``` |
| 31 | + |
| 32 | +Robot Arm |
| 33 | +----- |
| 34 | +A simple 3-link arm in an ArmModel object can be generated from Matlab generateArm utility function for convenience: |
| 35 | + |
| 36 | +```matlab |
| 37 | +% arm model |
| 38 | +arm = generateArm('SimpleThreeLinksArm'); |
| 39 | +``` |
| 40 | + |
| 41 | +[ArmModel](../gpmp2/kinematics/ArmModel.h) class consists of a forward kinematics model (DH parameter arm implemented by [Arm](../gpmp2/kinematics/Arm.h) class) |
| 42 | +and a physical arm represented by a series of spheres (implemented by [BodySphereVector](../gpmp2/kinematics/RobotModel.h) class). |
| 43 | +Details can be found in [generateArm.m](../matlab/+gpmp2/generateArm.m). |
| 44 | + |
| 45 | +We can now visualize the dataset setting and start/goal configurations in Matlab |
| 46 | + |
| 47 | +|**Figure 1:** WAM arm dataset visualized in Matlab, with start (blue) and end (red) configuration.| |
| 48 | +|:-----------| |
| 49 | +|| |
| 50 | + |
| 51 | +Initialize the Trajectory |
| 52 | +----- |
| 53 | + |
| 54 | +To start the iterative non-linear least square optimization, we need to give an initial trajectory. |
| 55 | +Here we initialize the trajectory as straight line in configuration space. The initial trajectory should be stored in a gtsam.Values object. |
| 56 | +GPMP2 provides a utility function [initArmTrajStraightLine](../gpmp2/planner/TrajUtils.h) to initialize straight line given start/end configuration, |
| 57 | +and total number of states/waypoints for optimization. |
| 58 | + |
| 59 | +```matlab |
| 60 | +%% init values |
| 61 | +init_values = initArmTrajStraightLine(start_conf, end_conf, total_time_step); |
| 62 | +``` |
| 63 | + |
| 64 | +Parameters |
| 65 | +----- |
| 66 | +All the trajectory optimization related parameters are stored in [TrajOptimizerSetting](../gpmp2/planner/TrajOptimizerSetting.h) class. |
| 67 | +Please check [parameters](Parameters.md) page for details on tuning/setting these parameters. |
| 68 | + |
| 69 | +```matlab |
| 70 | +%% optimization settings |
| 71 | +opt_setting = TrajOptimizerSetting(3); % initialization with DOF |
| 72 | +opt_setting.set_total_step(total_time_step); % total number of states/waypoints for optimization |
| 73 | +opt_setting.set_total_time(total_time_sec); % total runtime (second) of the trajectory |
| 74 | +opt_setting.set_epsilon(epsilon_dist); % \epsilon for hingeloss function, see the paper |
| 75 | +opt_setting.set_cost_sigma(cost_sigma); % \sigma_obs for obstacle cost, see the paper |
| 76 | +opt_setting.set_obs_check_inter(check_inter); % number of waypoints are checked by GP interpolation, 0 means GP interpolation is not used |
| 77 | +opt_setting.set_conf_prior_model(pose_fix_sigma); % start/end pose prior sigma, leave as default (0.0001) for most cases |
| 78 | +opt_setting.set_vel_prior_model(vel_fix_sigma); % start/end velocity prior sigma, leave as default (0.0001) for most cases |
| 79 | +opt_setting.set_Qc_model(Qc); % GP hyperparameter |
| 80 | +opt_setting.setGaussNewton(); % optimization method |
| 81 | +``` |
| 82 | + |
| 83 | +Optimization |
| 84 | +----- |
| 85 | +Factor graph generation and optimization are performed in [BatchTrajOptimize2DArm](../gpmp2/planner/BatchTrajOptimizer.h) function. The BatchTrajOptimize2DArm needs input of arm model, signed distance field, start/end configuration+velocity, |
| 86 | +initial trajectory values, and parameters. |
| 87 | + |
| 88 | +```matlab |
| 89 | +% optimize! |
| 90 | +result = BatchTrajOptimize2DArm(arm, sdf, start_conf, start_vel, end_conf, end_vel, init_values, opt_setting); |
| 91 | +``` |
| 92 | + |
| 93 | +Densify the Trajectory by GP Interpolation (Optional) |
| 94 | +----- |
| 95 | +If you need dense trajectory for robot execution, you can densify the trajectory by GP Interpolation. |
| 96 | +GPMP2 provides a utility function [interpolateArmTraj](../gpmp2/planner/TrajUtils.h) to densify the trajectory using GP Interpolation and by providing optimized values, GP hyperparameter, time interval between states, and number of points you would like to interpolate between each two states (0 means not interpolation). |
| 97 | + |
| 98 | +```matlab |
| 99 | +plot_values = interpolateArmTraj(result, Qc_model, delta_t, plot_inter-1); |
| 100 | +``` |
| 101 | + |
| 102 | +Display |
| 103 | +----- |
| 104 | +Once optimized trajectory values are available, we can query the configuration and plot the arm. |
| 105 | +The configuration vector can be queried from a gtsam.Values object by ```plot_values.atVector(symbol('x', i));```, |
| 106 | +where ```i``` is the index of the state you query (starting from 0 and ending at ```opt_setting.total_step```). |
| 107 | +Then [plotPlanarArm](../matlab/+gpmp2/plotPlanarArm.m) is used to plot the arm |
| 108 | + |
| 109 | +```matlab |
| 110 | +% plot arm |
| 111 | +conf = plot_values.atVector(symbol('x', i)); |
| 112 | +plotPlanarArm(arm.fk_model(), conf, 'b', 2); |
| 113 | +``` |
| 114 | + |
| 115 | +Here's the output results of non-densified and densified trajectory |
| 116 | + |
| 117 | +|**Figure 2:** Non-densified trajectory for 2D 3-link arm example.| |
| 118 | +|:-----------| |
| 119 | +|| |
| 120 | + |
| 121 | +|**Figure 3:** Densified trajectory for 2D 3-link arm example.| |
| 122 | +|:-----------| |
| 123 | +|| |
0 commit comments