/home/slemaign/softs-local/BioMove3D-git/collision/p3d_col_traj.c File Reference

dynamic collision checker More...

#include "Util-pkg.h"
#include "P3d-pkg.h"
#include "Collision-pkg.h"
#include "Graphic-pkg.h"
#include "Localpath-pkg.h"

Classes
struct	type_interval
	Structure used to describe intervals. More...
Defines
#define	DEBUG_COL_TRAJ   0
#define	INIT_SIZE_INTERVAL   200
	Initial size for the array of interval use dichotomy test.
#define	INC_SIZE_INTERVAL   100
	Increment to increase the size of the array of interval when it is needed.
Functions
void	p3d_col_set_microcollision (int value)
int	p3d_col_get_microcollision ()
int	change_position_robot_without_obj (p3d_rob *robotPt, p3d_localpath *localpathPt, double l)
	Place the robot at position describe by localpathPt and the parameter l without object (only jnts)
int	change_position_robot_multisol (p3d_rob *robotPt, p3d_localpath *localpathPt, double l, double dl, configPt qp)
	Place the robot at position describe by localpathPt and the parameter l When several solutions for cntrt are possible, one is chosen depending on previous config in localpath.
int	change_position_robot_without_obj_multisol (p3d_rob *robotPt, p3d_localpath *localpathPt, double l, double dl, configPt qp)
	Place the robot at position describe by localpathPt and the parameter l without object (only jnts) When several solutions for cntrt are possible, one is chosen depending on previous config in localpath.
void	p3d_reset_current_q_inv (p3d_rob *robotPt)
void	p3d_set_current_q_inv (p3d_rob *robotPt, p3d_localpath *localpathPt, configPt q_inv)
int	p3d_get_current_q_inv (p3d_rob *robotPt, configPt q_invPt)
int	p3d_col_test_localpath_classic (p3d_rob *robotPt, p3d_localpath *localpathPt, int *ntest, double *Kpath, configPt *q_atKpath)
int	p3d_col_test_localpath_classic_multisol (p3d_rob *robotPt, p3d_localpath *localpathPt, int *ntest, double *Kpath, configPt *q_atKpath)
int	p3d_col_and_cntrts_test_localpath_classic (p3d_rob *robotPt, p3d_localpath *localpathPt, int *ntest, double *Kpath, configPt *q_atKpath)
	Collision and Constraints checking of a local path.
int	p3d_col_and_cntrts_test_localpath_classic_multisol (p3d_rob *robotPt, p3d_localpath *localpathPt, int *ntest, double *Kpath, configPt *q_atKpath)
int	p3d_test_config_continuity (p3d_rob *robotPt, configPt qprev, configPt qcur)
	Tests the continuity between two configurations (that are supposed to be two successive configurations along a path)
int	p3d_test_localpath_pb_continuity (p3d_rob *robotPt, p3d_localpath *localpathPt)
int	p3d_onlycol_test_localpath_classic (p3d_rob *robotPt, p3d_localpath *localpathPt, int *ntest, double *Kpath, configPt *q_atKpath)
	Collision checking of a local path.
void	p3d_col_env_free_memory_traj_col_tab (void)
	Release memory allocated to work on the trajectory.
void	p3d_col_env_set_traj_method (p3d_traj_test_type type)
	Choose the path collision checking method.
p3d_traj_test_type	p3d_col_env_get_traj_method (void)
	Get the choice of path collision checking method.
int	p3d_col_test_localpath (p3d_rob *robotPt, p3d_localpath *localpathPt, int *ntest)
	Collision checking of a local path.
int	p3d_col_test_traj (p3d_rob *robotPt, p3d_localpath *localpathPt, int *ntest)
	Collision checking of a trajectory.
int	p3d_test_visibility_edge (p3d_rob *robotPt, configPt q0, configPt q_edge1, configPt q_edge2)
Variables
std::vector< double >	aveBBDist
	Main function of dichotomy process.

---

Detailed Description

dynamic collision checker

---

Function Documentation

int change_position_robot_multisol	(	p3d_rob *	robotPt,
		p3d_localpath *	localpathPt,
		double	l,
		double	dl,
		configPt	qp
	)

Place the robot at position describe by localpathPt and the parameter l When several solutions for cntrt are possible, one is chosen depending on previous config in localpath.

Parameters:

robotPt,:	the robot
localpathPt,:	the robot
l,:	the parameter
dl,:	the increment from previous config.

Returns:

Whether robot respect(FALSE) kinematics constraints and joints limits or not(TRUE)

int change_position_robot_without_obj	(	p3d_rob *	robotPt,
		p3d_localpath *	localpathPt,
		double	l
	)

Place the robot at position describe by localpathPt and the parameter l without object (only jnts)

Parameters:

robotPt,:	the robot
localpathPt,:	the robot
l,:	the parameter

Returns:

Whether robot respect kinematics constraints and joints limits or not

int change_position_robot_without_obj_multisol	(	p3d_rob *	robotPt,
		p3d_localpath *	localpathPt,
		double	l,
		double	dl,
		configPt	qp
	)

Place the robot at position describe by localpathPt and the parameter l without object (only jnts) When several solutions for cntrt are possible, one is chosen depending on previous config in localpath.

Parameters:

robotPt,:	the robot
localpathPt,:	the robot
l,:	the parameter
dl,:	the increment from previous config.

Returns:

Whether robot respect kinematics constraints and joints limits or not

int p3d_col_and_cntrts_test_localpath_classic	(	p3d_rob *	robotPt,
		p3d_localpath *	localpathPt,
		int *	ntest,
		double *	Kpath,
		configPt *	q_atKpath
	)

Collision and Constraints checking of a local path.

Parameters:

robotPt,:	the robot
localpathPt,:	the local path

Return values:

ntest,:	the sum of collision checking tests
Kpath,:	the configuration at the parameter Kpath * localpathPt::range_param is the last collision free or valid (for cntrts) tested configuration on the path.

Returns:

Whether valid or not

Description: Compute the distances between the bounding boxes of each obstacle and the bounding boxes of the environment. If these distances are positive, go forward along the path in such a way that each point of each body does not move by more than the distance between the body and the environment. If one of these distance is zero, go forward along the in such a way that no point of the robot moves more than 2*dmax. Constraints are systematically tested at least each 2*dmax

void p3d_col_env_free_memory_traj_col_tab

(

void

)

Release memory allocated to work on the trajectory.

Note:

Release the memory of ::intervals.

p3d_traj_test_type p3d_col_env_get_traj_method

(

void

)

Get the choice of path collision checking method.

Returns:

the collision checking method type

void p3d_col_env_set_traj_method

(

p3d_traj_test_type

type

)

Choose the path collision checking method.

Parameters:

type	the collision checking method type

int p3d_col_test_localpath	(	p3d_rob *	robotPt,
		p3d_localpath *	localpathPt,
		int *	ntest
	)

Collision checking of a local path.

Parameters:

robotPt,:	the robot
localpathPt,:	the local path

Return values:

ntest,:

the sum of collision checking tests

Returns:

Whether collision or not

Description: test a local path according to the type of test selected by p3d_col_env_set_traj_method()

<- modif JPS: be careful with this

int p3d_col_test_traj	(	p3d_rob *	robotPt,
		p3d_localpath *	localpathPt,
		int *	ntest
	)

Collision checking of a trajectory.

Parameters:

robotPt,:	the robot
localpathPt,:	the first local path

Return values:

ntest,:

the sum of collision checking tests

Returns:

Whether collision or not

Description: test a trajectory according to the type of test selected by p3d_col_env_set_traj_method()

int p3d_onlycol_test_localpath_classic	(	p3d_rob *	robotPt,
		p3d_localpath *	localpathPt,
		int *	ntest,
		double *	Kpath,
		configPt *	q_atKpath
	)

Collision checking of a local path.

Parameters:

robotPt,:	the robot
localpathPt,:	the local path

Return values:

ntest,:	the sum of collision checking tests
Kpath,:	the configuration at the parameter Kpath * localpathPt::range_param is the last collision free tested configuration on the path.

Returns:

Whether collision or not

Description: Compute the distances between the bounding boxes of each obstacle and the bounding boxes of the environment. If these distances are positive, go forward along the path in such a way that each point of each body does not move by more than the distance between the body and the environment. If one of these distance is zero, go forward along the in such a way that no point of the robot moves more than 2*dmax.

FREE

---

Variable Documentation

std::vector<double> aveBBDist

Main function of dichotomy process.

This function work only on one localpath.

Parameters:

robotPt,:	the robot
dmax,:	the minimal distance between two tests hidden param : global variable intervals (array of intervals)

Return values:

ntest,:

the number of collision tests.

Returns:

True : collision along the localpath (in the hidden intervals)

Note:

The local path informations are passed by global variable ::intervals