SimpleGraspPlanner.c File Reference

#include "SimpleGraspPlanner.h"
#include <math.h>
#include <stdio.h>

Include dependency graph for SimpleGraspPlanner.c:

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 Documentation

#define max	(	A,
		B		)	(((A) > (B)) ? (A) : (B))

Definition at line 47 of file SimpleGraspPlanner.c.

#define MAX_HAND_POINTS   100

Definition at line 201 of file SimpleGraspPlanner.c.

#define min	(	A,
		B		)	(((A) < (B)) ? (A) : (B))

Definition at line 46 of file SimpleGraspPlanner.c.

---

Function Documentation

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)

• -------------------------------------| 4 + |- ------------------------------------- 2 1.504695 / \ 2

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.

int InitSGP	(	Point3D *	hand_points,
		int	num,
		double	table_z
	)

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.

---

Variable Documentation

Dataset g_data

Definition at line 45 of file SimpleGraspPlanner.c.