計画経路エンジン More...

#include <PathPlanner.h>

Public Member Functions
void	boundingBoxMode (bool mode)
bool	calcPath ()
	経路計画を行う
bool	checkCollision ()
	現在の状態で干渉検出を行う
bool	checkCollision (const Configuration &pos)
	干渉検出を行う
bool	checkCollision (const std::vector< Configuration > &path)
	パスの干渉検出を行う
const std::pair< std::string, std::string > &	collidingPair ()
unsigned int	countCollisionCheck () const
	干渉チェックを呼び出した回数を取得する
Algorithm *	getAlgorithm ()
	計画アルゴリズムを取得する
void	getAlgorithmNames (std::vector< std::string > &algorithms)
	経路計画アルゴリズム名の一覧を取得する
ConfigurationSpace *	getConfigurationSpace ()
	コンフィギュレーション空間設定を取得する
Mobility *	getMobility ()
	移動能力を取得する
void	getMobilityNames (std::vector< std::string > &mobilitys)
	移動能力名の一覧を取得する
void	getOptimizerNames (std::vector< std::string > &optimizers)
	経路最適化アルゴリズム名の一覧を取得する
std::vector< Configuration >	getPath ()
	計画された経路の補間されたものを取得する
bool	getProperties (const std::string &algorithm, std::vector< std::string > &names, std::vector< std::string > &values)
	経路計画アルゴリズムのプロパティ名一覧を取得する
RoadmapPtr	getRoadmap ()
	ロードマップを取得する
std::vector< Configuration > &	getWayPoints ()
	計画された経路を取得する
void	getWorldState (OpenHRP::WorldState_out wstate)
	物理世界の状況を取得する
void	initPlanner (const std::string &nameServer)
	初期化。NameServer, ModelLoaderとの接続を行う。
void	initSimulation ()
	動力学シミュレータを初期化する
bool	optimize (const std::string &optimizer)
	指定した方法で経路を最適化する
	PathPlanner (unsigned int dim, WorldPtr world=WorldPtr(), bool isDebugMode=false)
	コンストラクタ
void	registerAlgorithm (const std::string &algorithmName, AlgorithmNewFunc newFunc, AlgorithmDeleteFunc deleteFunc)
	経路計画アルゴリズムの登録
hrp::BodyPtr	registerCharacter (const char *name, OpenHRP::BodyInfo_ptr cInfo)
	キャラクタを動力学シミュレータに登録する。
hrp::BodyPtr	registerCharacter (const char *name, hrp::BodyPtr i_body)
	キャラクタを動力学シミュレータに登録する。
hrp::BodyPtr	registerCharacterByURL (const char name, const char url)
	キャラクタを動力学シミュレータに登録する。
void	registerIntersectionCheckPair (const char char1, const char name1, const char char2, const char name2, CORBA::Double tolerance)
	衝突検出ペアを設定する
void	registerMobility (const std::string &mobilityName, MobilityNewFunc newFunc, MobilityDeleteFunc deleteFunc)
	移動能力の登録
void	registerOptimizer (const std::string &optimizerName, OptimizerNewFunc newFunc, OptimizerDeleteFunc deleteFunc)
	経路最適化アルゴリズムの登録
hrp::BodyPtr	robot ()
	ロボットを取得する
void	setAlgorithmName (const std::string &algorithm)
	使用する経路計画アルゴリズム名を指定する
void	setApplyConfigFunc (applyConfigFunc i_func)
	コンフィギュレーションベクトルからロボットの姿勢をセットする関数をセットする
void	setCharacterPosition (const char *character, const OpenHRP::DblSequence &pos)
	位置を設定する
void	setCollisionDetector (CollisionDetector *i_cd)
bool	setConfiguration (const Configuration &pos)
	コンフィギュレーションをセットする
void	setDebug (bool debug)
	デバッグモードの変更
void	setGoalConfiguration (const Configuration &pos)
	ゴール位置を設定する
bool	setMobilityName (const std::string &mobility)
	使用する移動能力を設定する
void	setPointCloud (const std::vector< hrp::Vector3 > &i_cloud, double i_radius)
	干渉チェック対象となるポイントクラウドを設定する
void	setProperties (const std::map< std::string, std::string > &properties)
	アルゴリズムに対して各種情報を設定する
void	setRobotName (const std::string &model)
	移動動作の設計対象にするモデルを設定する
void	setStartConfiguration (const Configuration &pos)
	スタート位置を設定する
void	stopPlanning ()
	計算を中止する
double	timeCollisionCheck () const
	干渉チェックに使用した時間[s]を取得する
double	timeForwardKinematics () const
WorldPtr	world ()
	物理世界を取得する
	~PathPlanner ()
	デストラクタ
Private Types
typedef std::map< const std::string, std::pair < AlgorithmNewFunc, AlgorithmDeleteFunc > >	AlgorithmFactory
typedef AlgorithmFactory::value_type	AlgorithmFactoryValueType
typedef std::map< std::string, std::pair< MobilityNewFunc, MobilityDeleteFunc > >	MobilityFactory
typedef MobilityFactory::value_type	MobilityFactoryValueType
typedef std::map< std::string, std::pair< OptimizerNewFunc, OptimizerDeleteFunc > >	OptimizerFactory
typedef OptimizerFactory::value_type	OptimizerFactoryValueType
Private Member Functions
void	_setupCharacterData ()
void	_updateCharacterPositions ()
template<typename X , typename X_ptr >
X_ptr	checkCorbaServer (const std::string &n, CosNaming::NamingContext_var &cxt)
	CORBAサーバ取得
bool	defaultCheckCollision ()
Private Attributes
Algorithm *	algorithm_
	使用する経路計画アルゴリズム
AlgorithmFactory	algorithmFactory_
	経路計画アルゴリズムのファクトリ
std::string	algorithmName_
	使用する経路計画アルゴリズム名
OpenHRP::CharacterPositionSequence_var	allCharacterPositions_
bool	bboxMode_
std::vector< hrp::ColdetModelPair >	checkPairs_
std::pair< std::string, std::string >	collidingPair_
OpenHRP::CollisionDetector_var	collisionDetector_
unsigned int	countCollisionCheck_
	干渉チェック呼び出し回数
ConfigurationSpace	cspace_
	コンフィギュレーション空間
CollisionDetector *	customCollisionDetector_
bool	debug_
	デバッグモード
double	dt_
	デバッグモード時に使用する現在時刻
applyConfigFunc	m_applyConfigFunc
Mobility *	mobility_
	使用する移動能力
MobilityFactory	mobilityFactory_
	移動能力のファクトリ
std::string	mobilityName_
	使用する移動能力名
hrp::BodyPtr	model_
	経路計画の対象とするロボット
OpenHRP::ModelLoader_var	modelLoader_
	ModelLoader への参照
OpenHRP::OnlineViewer_var	onlineViewer_
	デバッグモード時に使用する OnlineViewer への参照
OptimizerFactory	optimizerFactory_
	経路最適化アルゴリズムのファクトリ
CORBA::ORB_var	orb_
std::vector< Configuration >	path_
	経路
std::vector< hrp::Vector3 >	pointCloud_
double	radius_
	radius of spheres assigned to points
TimeMeasure	timeCollisionCheck_
	干渉チェックに使用したクロック数
TimeMeasure	timeForwardKinematics_
WorldPtr	world_

Detailed Description

計画経路エンジン

経路計画を行うプログラムはこのクラスを使用する。干渉検出などを簡易化したメソッドを持つ。

Definition at line 44 of file hrplib/hrpPlanner/PathPlanner.h.

Member Typedef Documentation

typedef std::map<const std::string, std::pair<AlgorithmNewFunc, AlgorithmDeleteFunc> > PathEngine::PathPlanner::AlgorithmFactory [private]

Definition at line 48 of file hrplib/hrpPlanner/PathPlanner.h.

typedef AlgorithmFactory::value_type PathEngine::PathPlanner::AlgorithmFactoryValueType [private]

Definition at line 49 of file hrplib/hrpPlanner/PathPlanner.h.

typedef std::map<std::string, std::pair<MobilityNewFunc, MobilityDeleteFunc> > PathEngine::PathPlanner::MobilityFactory [private]

Definition at line 55 of file hrplib/hrpPlanner/PathPlanner.h.

typedef MobilityFactory::value_type PathEngine::PathPlanner::MobilityFactoryValueType [private]

Definition at line 56 of file hrplib/hrpPlanner/PathPlanner.h.

typedef std::map<std::string, std::pair<OptimizerNewFunc, OptimizerDeleteFunc> > PathEngine::PathPlanner::OptimizerFactory [private]

Definition at line 62 of file hrplib/hrpPlanner/PathPlanner.h.

typedef OptimizerFactory::value_type PathEngine::PathPlanner::OptimizerFactoryValueType [private]

Definition at line 63 of file hrplib/hrpPlanner/PathPlanner.h.

Constructor & Destructor Documentation

PathPlanner::PathPlanner	(	unsigned int	dim,
		WorldPtr	world = `WorldPtr()`,
		bool	isDebugMode = `false`
	)

コンストラクタ

Parameters:

dim	コンフィギュレーション空間の次元
world	物理世界
isDebugMode	デバッグモードにするか否か

Definition at line 82 of file hrplib/hrpPlanner/PathPlanner.cpp.

PathPlanner::~PathPlanner ( )

デストラクタ

Definition at line 123 of file hrplib/hrpPlanner/PathPlanner.cpp.

Member Function Documentation

void PathPlanner::_setupCharacterData ( ) [private]

Definition at line 802 of file hrplib/hrpPlanner/PathPlanner.cpp.

void PathPlanner::_updateCharacterPositions ( ) [private]

Definition at line 831 of file hrplib/hrpPlanner/PathPlanner.cpp.

void PathEngine::PathPlanner::boundingBoxMode ( bool mode ) [inline]

Definition at line 465 of file hrplib/hrpPlanner/PathPlanner.h.

bool PathPlanner::calcPath ( )

経路計画を行う

Returns:: 計画が正常に終了した場合true、それ以外はfalseを返す

Definition at line 737 of file hrplib/hrpPlanner/PathPlanner.cpp.

bool PathPlanner::checkCollision ( )

現在の状態で干渉検出を行う

Returns:: 干渉している場合true, それ以外false

Definition at line 623 of file hrplib/hrpPlanner/PathPlanner.cpp.

bool PathPlanner::checkCollision ( const Configuration & pos )

干渉検出を行う

Parameters:

pos ロボットの位置

Returns:: 干渉している場合true, それ以外false

Definition at line 580 of file hrplib/hrpPlanner/PathPlanner.cpp.

bool PathPlanner::checkCollision ( const std::vector< Configuration > & path )

パスの干渉検出を行う

Parameters:

path パス

Returns:: 一点でも干渉しているとtrue。ただし視点と終点はチェックしない

Definition at line 706 of file hrplib/hrpPlanner/PathPlanner.cpp.

template<typename X , typename X_ptr >

X_ptr PathPlanner::checkCorbaServer	(	const std::string &	n,
		CosNaming::NamingContext_var &	cxt
	)		`[private]`

CORBAサーバ取得

CORBAサーバをネームサーバから取得する

Parameters:

n	サーバ名
cxt	ネーミングコンテキスト

Definition at line 31 of file hrplib/hrpPlanner/PathPlanner.cpp.

const std::pair<std::string, std::string>& PathEngine::PathPlanner::collidingPair ( ) [inline]

Definition at line 467 of file hrplib/hrpPlanner/PathPlanner.h.

unsigned int PathEngine::PathPlanner::countCollisionCheck ( ) const [inline]

干渉チェックを呼び出した回数を取得する

Returns:: 干渉チェックを呼び出した回数

Definition at line 455 of file hrplib/hrpPlanner/PathPlanner.h.

bool PathPlanner::defaultCheckCollision ( ) [private]

Definition at line 639 of file hrplib/hrpPlanner/PathPlanner.cpp.

Algorithm* PathEngine::PathPlanner::getAlgorithm ( ) [inline]

計画アルゴリズムを取得する

Returns:: 計画アルゴリズム

Definition at line 411 of file hrplib/hrpPlanner/PathPlanner.h.

void PathPlanner::getAlgorithmNames ( std::vector< std::string > & algorithms )

経路計画アルゴリズム名の一覧を取得する

Returns:: 経路計画アルゴリズム名文字列の配列

Definition at line 157 of file hrplib/hrpPlanner/PathPlanner.cpp.

ConfigurationSpace* PathEngine::PathPlanner::getConfigurationSpace ( ) [inline]

コンフィギュレーション空間設定を取得する

Returns:: コンフィギュレーション空間設定

Definition at line 417 of file hrplib/hrpPlanner/PathPlanner.h.

Mobility* PathEngine::PathPlanner::getMobility ( ) [inline]

移動能力を取得する

Returns:: 移動能力

Definition at line 405 of file hrplib/hrpPlanner/PathPlanner.h.

void PathPlanner::getMobilityNames ( std::vector< std::string > & mobilitys )

移動能力名の一覧を取得する

Returns:: 移動能力名の配列

Definition at line 145 of file hrplib/hrpPlanner/PathPlanner.cpp.

void PathPlanner::getOptimizerNames ( std::vector< std::string > & optimizers )

経路最適化アルゴリズム名の一覧を取得する

Returns:: 経路最適化アルゴリズム名文字列の配列

Definition at line 136 of file hrplib/hrpPlanner/PathPlanner.cpp.

std::vector< Configuration > PathPlanner::getPath ( )

計画された経路の補間されたものを取得する

Returns:: 補間された経路

Definition at line 787 of file hrplib/hrpPlanner/PathPlanner.cpp.

bool PathPlanner::getProperties	(	const std::string &	algorithm,
		std::vector< std::string > &	names,
		std::vector< std::string > &	values
	)

経路計画アルゴリズムのプロパティ名一覧を取得する

Parameters:

algorithm	経路計画アルゴリズムの名称
names	プロパティ名文字列の配列
values	プロパティ値文字列の配列

Returns:: 取得に成功した場合true、指定したアルゴリズムが見つからなかった場合falseを返す

Definition at line 169 of file hrplib/hrpPlanner/PathPlanner.cpp.

RoadmapPtr PathEngine::PathPlanner::getRoadmap ( ) [inline]

ロードマップを取得する

Returns:: ロードマップ

Definition at line 387 of file hrplib/hrpPlanner/PathPlanner.h.

std::vector< Configuration > & PathPlanner::getWayPoints ( )

計画された経路を取得する

Returns:: 計画された経路

Definition at line 783 of file hrplib/hrpPlanner/PathPlanner.cpp.

void PathPlanner::getWorldState ( OpenHRP::WorldState_out wstate )

物理世界の状況を取得する

Parameters:

wstate 物理世界の状況

Definition at line 605 of file hrplib/hrpPlanner/PathPlanner.cpp.

void PathPlanner::initPlanner ( const std::string & nameServer )

初期化。NameServer, ModelLoaderとの接続を行う。

Parameters:

nameServer CORBAネームサーバの位置をホスト名:ポート番号の形式で指定る。

Definition at line 191 of file hrplib/hrpPlanner/PathPlanner.cpp.

void PathPlanner::initSimulation ( )

動力学シミュレータを初期化する

Definition at line 555 of file hrplib/hrpPlanner/PathPlanner.cpp.

bool PathPlanner::optimize ( const std::string & optimizer )

指定した方法で経路を最適化する

Parameters:

optimizer 最適化法の名称

Returns:: 指定した最適化法が見つからなかった場合false、それ以外true

Definition at line 771 of file hrplib/hrpPlanner/PathPlanner.cpp.

void PathPlanner::registerAlgorithm	(	const std::string &	algorithmName,
		AlgorithmNewFunc	newFunc,
		AlgorithmDeleteFunc	deleteFunc
	)

経路計画アルゴリズムの登録

Parameters:

algorithmName	経路計画アルゴリズム名
newFunc
deleteFunc

Definition at line 696 of file hrplib/hrpPlanner/PathPlanner.cpp.

BodyPtr PathPlanner::registerCharacter	(	const char *	name,
		OpenHRP::BodyInfo_ptr	cInfo
	)

キャラクタを動力学シミュレータに登録する。

Parameters:

name	モデル名
cInfo	モデルデータ

Definition at line 411 of file hrplib/hrpPlanner/PathPlanner.cpp.

BodyPtr PathPlanner::registerCharacter	(	const char *	name,
		hrp::BodyPtr	i_body
	)

キャラクタを動力学シミュレータに登録する。

Parameters:

name	モデル名
i_body	モデルデータ

Definition at line 437 of file hrplib/hrpPlanner/PathPlanner.cpp.

BodyPtr PathPlanner::registerCharacterByURL	(	const char *	name,
		const char *	url
	)

キャラクタを動力学シミュレータに登録する。

Parameters:

name	モデル名
url	モデルURL

Definition at line 292 of file hrplib/hrpPlanner/PathPlanner.cpp.

void PathPlanner::registerIntersectionCheckPair	(	const char *	char1,
		const char *	name1,
		const char *	char2,
		const char *	name2,
		CORBA::Double	tolerance
	)

衝突検出ペアを設定する

Parameters:

char1	キャラクタ1
name1	キャラクタ1のリンク
char2	キャラクタ2
name2	キャラクタ2のリンク
tolerance	リンク間の距離がこの値以下になると干渉と見なされる