Go to the source code of this file.
Classes | |
| struct | Dataset |
Defines | |
| #define | max(A, B) (((A) > (B)) ? (A) : (B)) |
| #define | MAX_HAND_POINTS 100 |
| #define | min(A, B) (((A) < (B)) ? (A) : (B)) |
Functions | |
| double | det (CovariancePoint cov) |
| double | f (Point3D point_test, Point3D mean, CovariancePoint cov) |
| HandConfList | GetGraspList (Point3D *obstacle_list, CovariancePoint *obstacle_covs, int num, double offset_rot_z_side, double offset_rot_z_top) |
| HandConfig | GetGraspLM (Point3D *obstacle_list, CovariancePoint *obstacle_covs, int num, double offset_rot_z_side, double offset_rot_z_top) |
| double | getTableHeight () |
| int | InitSGP (Point3D *hand_points, int num, double table_z) |
| void | setTableHeight (double table_z) |
| Point3D | TransformPoint (Point3D hand, Point3D target, double alpha, double beta, double delta) |
Variables | |
| Dataset | g_data |
| #define MAX_HAND_POINTS 100 |
Definition at line 201 of file SimpleGraspPlanner.c.
| double det | ( | CovariancePoint | cov | ) |
Determinant a b c double sx; double sxy; double sxz; d e f double syx; double sy; double szy; g h i double szx; double syz; double sz;
Definition at line 99 of file SimpleGraspPlanner.c.
| double f | ( | Point3D | point_test, |
| Point3D | mean, | ||
| CovariancePoint | cov | ||
| ) |
Approximation of the probability density Implementation of this formula:
/%4 (cov_sy cov_sz - cov_syz cov_szy) %3 (-cov_szx cov_syz + cov_syx cov_sz) |------------------------------------ - -------------------------------------- \ %2 %2
%1 (cov_szx cov_sy - cov_syx cov_szy)\ / %4 (cov_sxy cov_sz - cov_sxz cov_szy)
%3 (-cov_szx cov_sxz + cov_sx cov_sz) %1 (cov_szx cov_sxy - cov_sx cov_szy)\ / + ------------------------------------- + -------------------------------------| %3 + | %2 %2 / \
%4 (cov_sxy cov_syz - cov_sxz cov_sy) %3 (-cov_syx cov_sxz + cov_sx cov_syz) ------------------------------------- - -------------------------------------- %2 %2
%1 (-cov_syx cov_sxy + cov_sx cov_sy)\ + -------------------------------------| %1 %2 /
1% = dz := point_test_z - mean_z
2% = cov_bla := cov_szx cov_sxy cov_syz - cov_szx cov_sxz cov_sy - cov_syx cov_sxy cov_sz
+ cov_syx cov_sxz cov_szy + cov_sx cov_sy cov_sz - cov_sx cov_syz cov_szy
3% = dy := point_test_y - mean_y
4% = dx := point_test_x - mean_x
Definition at line 143 of file SimpleGraspPlanner.c.
| HandConfList GetGraspList | ( | Point3D * | obstacle_list, |
| CovariancePoint * | obstacle_covs, | ||
| int | num, | ||
| double | offset_rot_z_side, | ||
| double | offset_rot_z_top | ||
| ) |
Initialize hand_center
Initialize lists
Initialize search space + sampling
Consider Table collision
Initialize start score: careful for strange covariances, this value might be to low
generate configuration dependant hand points
Definition at line 340 of file SimpleGraspPlanner.c.
| HandConfig GetGraspLM | ( | Point3D * | obstacle_list, |
| CovariancePoint * | obstacle_covs, | ||
| int | num, | ||
| double | offset_rot_z_side, | ||
| double | offset_rot_z_top | ||
| ) |
Initialize hand_center
Initialize lists
Initialize search space + sampling
Consider Table collision
Initialize start score: careful for strange covariances, this value might be to low
generate configuration dependant hand points
Definition at line 202 of file SimpleGraspPlanner.c.
| double getTableHeight | ( | ) |
Definition at line 83 of file SimpleGraspPlanner.c.
Initialize the hand configuration
Get biggest hand z (first collision coming from top with a table)
Get smallest hand y (first collision point with a table coming from the side)
Definition at line 52 of file SimpleGraspPlanner.c.
| void setTableHeight | ( | double | table_z | ) |
Definition at line 77 of file SimpleGraspPlanner.c.
| Point3D TransformPoint | ( | Point3D | hand, |
| Point3D | target, | ||
| double | alpha, | ||
| double | beta, | ||
| double | delta | ||
| ) |
new poitions: Point P in object coordinates depending on the parameters alpha, beta and delta
Definition at line 187 of file SimpleGraspPlanner.c.
Definition at line 45 of file SimpleGraspPlanner.c.
