The SimoxCSpace contains a set of stationary obstacles and an agent. More...

#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>

Inheritance diagram for SimoxCSpace:

Classes
struct	AgentData
	Contains information about an agent. More...

Public Member Functions
virtual void	addObjectsFromWorkingMemory (memoryx::WorkingMemoryInterfacePrx workingMemoryPrx, const std::vector< std::string > &excludedInstancesIds=std::vector< std::string >())
	Adds all objects except the ones with ids specified in the parameter excludedInstancesIds from the working memory to the cspace. More...

virtual void	addPlanarObject (const std::vector< Eigen::Vector3f > &convexHull)

void	addStationaryObject (const StationaryObject &obj, const Ice::Current &=Ice::emptyCurrent) override
	Adds a stationary object to the cspace. More...

virtual CSpaceBasePtr	clone (bool loadVisualizationModel)

CSpaceBasePtr	clone (const Ice::Current &=Ice::emptyCurrent) override

virtual Saba::CSpaceSampledPtr	createSimoxCSpace () const

const AgentPlanningInformation &	getAgent () const

virtual Ice::StringSeq	getAgentJointNames () const

const VirtualRobot::RobotPtr &	getAgentSceneObj () const

VirtualRobot::CDManager &	getCD ()

Ice::Long	getDimensionality (const Ice::Current &=Ice::emptyCurrent) const override

FloatRangeSeq	getDimensionsBounds (const Ice::Current &=Ice::emptyCurrent) const override

float	getStationaryObjectMargin () const

const StationaryObjectList &	getStationaryObjects () const

const VirtualRobot::SceneObjectSetPtr &	getStationaryObjectSet () const

std::vector< armarx::DebugDrawerLineSet >	getTraceVisu (const Path &p, const std::vector< std::string > &nodeNames, float traceStepSize=std::numeric_limits< float >::infinity())

std::vector< armarx::DebugDrawerLineSet >	getTraceVisu (const PathWithCost &p, const std::vector< std::string > &nodeNames, float traceStepSize=std::numeric_limits< float >::infinity())

std::vector< armarx::DebugDrawerLineSet >	getTraceVisu (const VectorXfSeq &vecs, const std::vector< std::string > &nodeNames, float traceStepSize=std::numeric_limits< float >::infinity())

void	ice_postUnmarshal () override
	Initializes basic structures after transmission through ice. More...

void	initCollisionTest (const Ice::Current &=Ice::emptyCurrent) override
	Initializes the collision test. More...

bool	isCollisionFree (const ::std::pair< const Ice::Float , const Ice::Float > &cfg, const Ice::Current &=Ice::emptyCurrent) override

bool	isCollisionFree (const ::std::pair< const Ice::Float , const Ice::Float > &config, const Ice::Current &=Ice::emptyCurrent) override=0

virtual bool	isCollisionFree (const Ice::Float first, const Ice::Float last)

virtual bool	isCollisionFree (const VectorXf &config)

VectorXf	jointMapToVector (const std::map< std::string, float > &jointMap, bool checkForNonexistenJointsInCspace=false) const

virtual TrajectoryPtr	pathToTrajectory (const Path &p) const

virtual TrajectoryPtr	pathToTrajectory (const PathWithCost &p) const

virtual TrajectoryPtr	pathToTrajectory (const VectorXfSeq &p) const

void	setAgent (const AgentPlanningInformation &agentInfo, const Ice::Current &=Ice::emptyCurrent) override
	Adds a stationary agent to the cspace. More...

virtual void	setConfig (const float *&&it)

virtual void	setConfig (const float *&it)

virtual void	setConfig (const std::vector< float > cfg)
	Sets a configuration to check for. More...

void	setStationaryObjectMargin (float stationaryObjectMargin)

virtual void	setStationaryObjects (const StationaryObjectList &objList)

	SimoxCSpace (memoryx::CommonStorageInterfacePrx commonStoragePrx, bool loadVisualizationModel=false, float stationaryObjectMargin=0.0f)
	ctor More...

Public Member Functions inherited from CSpace
void	initCollisionTest (const Ice::Current &=Ice::emptyCurrent) override
	Initializes collision checking. More...

virtual bool	isCollisionFree (const Ice::Float first, const Ice::Float last)

virtual bool	isCollisionFree (const VectorXf &config)

bool	isValidConfiguration (const ::std::pair< const Ice::Float , const Ice::Float > &config, const Ice::Current &=Ice::emptyCurrent) const override

virtual bool	isValidConfiguration (const VectorXf &config) const

virtual bool	usesInternalScaling ()

Static Public Member Functions
static SimoxCSpacePtr	PrefetchWorkingMemoryObjects (memoryx::WorkingMemoryInterfacePrx workingMemoryPrx, memoryx::CommonStorageInterfacePrx commonStoragePrx, RobotStateComponentInterfacePrx rsc)
	Load objects from WorkingMemory and puts them into the mesh cache. More...

Protected Member Functions
AgentData	getAgentDataAndLoadRobot () const

AgentData	getAgentDataFromRobot (VirtualRobot::RobotPtr robotPtr) const

const std::vector< VirtualRobot::RobotNodePtr > &	getAgentJoints () const

VirtualRobot::ManipulationObjectPtr	getMovedSimoxManipulatorObject (const memoryx::ObjectClassBasePtr &object, const armarx::PoseBasePtr &pose, memoryx::GridFileManagerPtr &fileManager) const

VirtualRobot::ManipulationObjectPtr	getSimoxManipulatorObject (const memoryx::ObjectClassBasePtr &object, const memoryx::GridFileManagerPtr &fileManager, bool inflate=true, VirtualRobot::CollisionCheckerPtr const &colChecker=VirtualRobot::CollisionCheckerPtr {}) const

void	initAgent ()

void	initAgent (AgentData agentData)
	Initializes an agent. More...

void	initAgent (VirtualRobot::RobotPtr robotPtr)

void	initStationaryObjects ()
	Initializes stationary objects for collision checking. More...

	SimoxCSpace ()=default
	Default ctor. More...

Protected Attributes
std::vector< VirtualRobot::RobotNodePtr >	agentJoints

VirtualRobot::RobotPtr	agentSceneObj

VirtualRobot::CDManager	cd
	The collision checker. More...

memoryx::GridFileManagerPtr	fileManager
	The file manager used to load objects. More...

bool	isInitialized {false}
	Whether the collision check is initialized. More...

bool	loadVisualizationModel {false}
	Whether the visualization model of objects/agents sould be loaded. More...

GlobalCache< VirtualRobot::ManipulationObjectPtr, std::pair< int, std::string > >	meshCache

GlobalCache< RobotPoolPtr, std::pair< VirtualRobot::RobotIO::RobotDescription, std::string > >	robotCache

VirtualRobot::SceneObjectSetPtr	stationaryObjectSet {new VirtualRobot::SceneObjectSet{"StationaryObjectSet"}}
	The scene objects for stationary objects. More...

std::vector< std::vector< Eigen::Vector3f > >	stationaryPlanes

Friends
template<class IceBaseClass , class DerivedClass >
class	armarx::GenericFactory

Detailed Description

The SimoxCSpace contains a set of stationary obstacles and an agent.

The agent can have attached objects. The objects are loaded from a memoryx::CommonStorageInterfacePrx. The number of dimensions is determined by the agent's kinematic chains.

See also: armarx::SimoxCSpaceBase; RobotComponents-Tutorial-SimoxCSpace

Definition at line 67 of file SimoxCSpace.h.

Constructor & Destructor Documentation

◆ SimoxCSpace() [1/2]

SimoxCSpace	(	memoryx::CommonStorageInterfacePrx	commonStoragePrx,
		bool	loadVisualizationModel = `false`,
		float	stationaryObjectMargin = `0.0f`
	)

ctor

Parameters

commonStoragePrx	The common storage containing the objects.
loadVisualizationModel	Whether to load the visualization model. (true, if you want to display the object. false if you want to plan)

Definition at line 81 of file SimoxCSpace.cpp.

◆ SimoxCSpace() [2/2]

SimoxCSpace ( )

protecteddefault

Default ctor.

Used for ice factories.

Here is the caller graph for this function:

Member Function Documentation

◆ addObjectsFromWorkingMemory()

void addObjectsFromWorkingMemory	(	memoryx::WorkingMemoryInterfacePrx	workingMemoryPrx,
		const std::vector< std::string > &	excludedInstancesIds = `std::vector<std::string>()`
	)

virtual

Adds all objects except the ones with ids specified in the parameter excludedInstancesIds from the working memory to the cspace.

Parameters

workingMemoryPrx	The working memory.
excludedInstancesIds	the ids of the instances in teh working memory that should not be added

Definition at line 628 of file SimoxCSpace.cpp.

Here is the call graph for this function:

◆ addPlanarObject()

void addPlanarObject ( const std::vector< Eigen::Vector3f > & convexHull )

virtual

Definition at line 688 of file SimoxCSpace.cpp.

Here is the call graph for this function:

◆ addStationaryObject()

void addStationaryObject	(	const StationaryObject &	obj,
		const Ice::Current &	= `Ice::emptyCurrent`
	)

override

Adds a stationary object to the cspace.

Parameters

obj	The object to add.

Definition at line 145 of file SimoxCSpace.cpp.

Here is the caller graph for this function:

◆ clone() [1/2]

CSpaceBasePtr clone ( bool loadVisualizationModel )

virtual

Parameters

loadVisualizationModel Whether the clone should load the visualization model.

Returns: A clone of this cspace.

Reimplemented in SimoxCSpaceWith3DPose, SimoxCSpaceWith2DPose, and VoxelGridCSpace.

Definition at line 113 of file SimoxCSpace.cpp.

◆ clone() [2/2]

CSpaceBasePtr clone ( const Ice::Current & = Ice::emptyCurrent )

inlineoverridevirtual

Returns: A clone of this cspace.

Implements CSpace.

Reimplemented in VoxelGridCSpace.

Definition at line 118 of file SimoxCSpace.h.

◆ createSimoxCSpace()

Saba::CSpaceSampledPtr createSimoxCSpace ( ) const

virtual

Definition at line 129 of file SimoxCSpace.cpp.

◆ getAgent()

const AgentPlanningInformation& getAgent ( ) const

inline

Definition at line 173 of file SimoxCSpace.h.

◆ getAgentDataAndLoadRobot()

SimoxCSpace::AgentData getAgentDataAndLoadRobot ( ) const

protected

Definition at line 287 of file SimoxCSpace.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getAgentDataFromRobot()

SimoxCSpace::AgentData getAgentDataFromRobot ( VirtualRobot::RobotPtr robotPtr ) const

protected

Definition at line 370 of file SimoxCSpace.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getAgentJointNames()

Ice::StringSeq getAgentJointNames ( ) const

virtual

Reimplemented in SimoxCSpaceWith3DPose, and SimoxCSpaceWith2DPose.

Definition at line 792 of file SimoxCSpace.cpp.

Here is the caller graph for this function:

◆ getAgentJoints()

const std::vector<VirtualRobot::RobotNodePtr>& getAgentJoints ( ) const

inlineprotected

Definition at line 250 of file SimoxCSpace.h.

Here is the caller graph for this function:

◆ getAgentSceneObj()

const VirtualRobot::RobotPtr& getAgentSceneObj ( ) const

inline

Definition at line 186 of file SimoxCSpace.h.

◆ getCD()

VirtualRobot::CDManager& getCD ( )

inline

Returns: The collision checker.

Definition at line 194 of file SimoxCSpace.h.

◆ getDimensionality()

Ice::Long getDimensionality ( const Ice::Current & = Ice::emptyCurrent ) const

inlineoverride

Returns: The cspace's dimensionality.

Definition at line 144 of file SimoxCSpace.h.

Here is the caller graph for this function:

◆ getDimensionsBounds()

FloatRangeSeq getDimensionsBounds ( const Ice::Current & = Ice::emptyCurrent ) const

override

Returns: The cspace's dimension bounds.

Definition at line 778 of file SimoxCSpace.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getMovedSimoxManipulatorObject()

VirtualRobot::ManipulationObjectPtr getMovedSimoxManipulatorObject	(	const memoryx::ObjectClassBasePtr &	object,
		const armarx::PoseBasePtr &	pose,
		memoryx::GridFileManagerPtr &	fileManager
	)		const

protected

Parameters

object	The object.
pose	The pose to use.

Returns: The given objects manipulation object moved to the given pose.

Definition at line 609 of file SimoxCSpace.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getSimoxManipulatorObject()

VirtualRobot::ManipulationObjectPtr getSimoxManipulatorObject	(	const memoryx::ObjectClassBasePtr &	object,
		const memoryx::GridFileManagerPtr &	fileManager,
		bool	inflate = `true`,
		VirtualRobot::CollisionCheckerPtr const &	colChecker = `VirtualRobot::CollisionCheckerPtr {}`
	)		const

protected

Parameters

object The object.

Returns: The given objects manipulation object.

Definition at line 550 of file SimoxCSpace.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getStationaryObjectMargin()

float getStationaryObjectMargin ( ) const

inline

Definition at line 224 of file SimoxCSpace.h.

◆ getStationaryObjects()

const StationaryObjectList& getStationaryObjects ( ) const

inline

Returns: The stationary objects.

Definition at line 168 of file SimoxCSpace.h.

◆ getStationaryObjectSet()

const VirtualRobot::SceneObjectSetPtr& getStationaryObjectSet ( ) const

inline

Returns: The set of stationary objects' scene objects.

Definition at line 181 of file SimoxCSpace.h.

◆ getTraceVisu() [1/3]

std::vector<armarx::DebugDrawerLineSet> getTraceVisu	(	const Path &	p,
		const std::vector< std::string > &	nodeNames,
		float	traceStepSize = `std::numeric_limits<float>::infinity()`
	)

inline

Definition at line 214 of file SimoxCSpace.h.

Here is the caller graph for this function:

◆ getTraceVisu() [2/3]

std::vector<armarx::DebugDrawerLineSet> getTraceVisu	(	const PathWithCost &	p,
		const std::vector< std::string > &	nodeNames,
		float	traceStepSize = `std::numeric_limits<float>::infinity()`
	)

inline

Definition at line 218 of file SimoxCSpace.h.

Here is the call graph for this function:

◆ getTraceVisu() [3/3]

std::vector<armarx::DebugDrawerLineSet> getTraceVisu	(	const VectorXfSeq &	vecs,
		const std::vector< std::string > &	nodeNames,
		float	traceStepSize = `std::numeric_limits< float >::infinity()`
	)

◆ ice_postUnmarshal()

void ice_postUnmarshal ( )

override

Initializes basic structures after transmission through ice.

Definition at line 693 of file SimoxCSpace.cpp.

Here is the call graph for this function:

◆ initAgent() [1/3]

void initAgent ( )

inlineprotected

Definition at line 332 of file SimoxCSpace.h.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ initAgent() [2/3]

void initAgent ( AgentData agentData )

protected

Initializes an agent.

(Does not need to read from the file system if loadAnyModel is false)

Parameters

agentData	Data about the agent.
loadAnyModel	if unset only the robot structure is loaded. If set the robot's and objects' collision model are loaded. (depending on loadVisualizationModel the robot's visualisation model is loaded)

Definition at line 259 of file SimoxCSpace.cpp.

◆ initAgent() [3/3]

void initAgent ( VirtualRobot::RobotPtr robotPtr )

inlineprotected

Definition at line 337 of file SimoxCSpace.h.

Here is the call graph for this function:

◆ initCollisionTest()

void initCollisionTest ( const Ice::Current & = Ice::emptyCurrent )

override

Initializes the collision test.

Definition at line 192 of file SimoxCSpace.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ initStationaryObjects()

void initStationaryObjects ( )

protected

Initializes stationary objects for collision checking.

Definition at line 221 of file SimoxCSpace.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isCollisionFree() [1/4]

bool isCollisionFree	(	const ::std::pair< const Ice::Float , const Ice::Float > &	cfg,
		const Ice::Current &	= `Ice::emptyCurrent`
	)

overridevirtual

Parameters

cfg	The configuration to check.

Returns: Whether the given configuration is collision free.

Implements CSpace.

Definition at line 176 of file SimoxCSpace.cpp.

Here is the call graph for this function:

◆ isCollisionFree() [2/4]

bool isCollisionFree

override

Parameters

config The config to check.

Returns: Whether the given configuration is collision free.

◆ isCollisionFree() [3/4]

virtual bool isCollisionFree

inline

Parameters

first	The configuration's first element.
last	The configuration's last element.

Returns: Whether the given configuration [first,last) is collision free.

Definition at line 87 of file CSpace.h.

◆ isCollisionFree() [4/4]

virtual bool isCollisionFree

inline

Parameters

config The config to check. (as armarx::VectorXf)

Returns: Whether the given configuration is collision free.

Definition at line 105 of file CSpace.h.

◆ jointMapToVector()

VectorXf jointMapToVector	(	const std::map< std::string, float > &	jointMap,
		bool	checkForNonexistenJointsInCspace = `false`
	)		const

Definition at line 803 of file SimoxCSpace.cpp.

◆ pathToTrajectory() [1/3]

virtual TrajectoryPtr pathToTrajectory ( const Path & p ) const

inlinevirtual

Definition at line 204 of file SimoxCSpace.h.

Here is the caller graph for this function:

◆ pathToTrajectory() [2/3]

virtual TrajectoryPtr pathToTrajectory ( const PathWithCost & p ) const

inlinevirtual

Definition at line 208 of file SimoxCSpace.h.

Here is the call graph for this function:

◆ pathToTrajectory() [3/3]

TrajectoryPtr pathToTrajectory ( const VectorXfSeq & p ) const

virtual

Definition at line 746 of file SimoxCSpace.cpp.

Here is the call graph for this function:

◆ PrefetchWorkingMemoryObjects()

SimoxCSpacePtr PrefetchWorkingMemoryObjects	(	memoryx::WorkingMemoryInterfacePrx	workingMemoryPrx,
		memoryx::CommonStorageInterfacePrx	commonStoragePrx,
		armarx::RobotStateComponentInterfacePrx	rsc
	)

static

Load objects from WorkingMemory and puts them into the mesh cache.

Parameters

workingMemoryPrx

Definition at line 673 of file SimoxCSpace.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setAgent()

void setAgent	(	const AgentPlanningInformation &	agentInfo,
		const Ice::Current &	= `Ice::emptyCurrent`
	)

override

Adds a stationary agent to the cspace.

Parameters

obj	The object to add.

Definition at line 157 of file SimoxCSpace.cpp.

Here is the call graph for this function:

◆ setConfig() [1/3]

virtual void setConfig ( const float *&& it )

inlinevirtual

Definition at line 159 of file SimoxCSpace.h.

Here is the call graph for this function:

◆ setConfig() [2/3]

void setConfig ( const float *& it )

virtual

Reimplemented in SimoxCSpaceWith3DPose, and SimoxCSpaceWith2DPose.

Definition at line 163 of file SimoxCSpace.cpp.

◆ setConfig() [3/3]

virtual void setConfig ( const std::vector< float > cfg )

inlinevirtual

Sets a configuration to check for.

(can be used to display a configuration)

Parameters

it	A pointer to the configuration.

Definition at line 153 of file SimoxCSpace.h.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setStationaryObjectMargin()

void setStationaryObjectMargin ( float stationaryObjectMargin )

inline

Definition at line 229 of file SimoxCSpace.h.

◆ setStationaryObjects()

void setStationaryObjects ( const StationaryObjectList & objList )

virtual

Definition at line 152 of file SimoxCSpace.cpp.

Friends And Related Function Documentation

◆ armarx::GenericFactory

friend class armarx::GenericFactory

friend

Definition at line 248 of file SimoxCSpace.h.

Member Data Documentation

◆ agentJoints

std::vector<VirtualRobot::RobotNodePtr> agentJoints

protected

Definition at line 287 of file SimoxCSpace.h.

◆ agentSceneObj

VirtualRobot::RobotPtr agentSceneObj

protected

Definition at line 289 of file SimoxCSpace.h.

◆ cd

VirtualRobot::CDManager cd

protected

The collision checker.

Definition at line 280 of file SimoxCSpace.h.

◆ fileManager

memoryx::GridFileManagerPtr fileManager

protected

The file manager used to load objects.

Definition at line 274 of file SimoxCSpace.h.

◆ isInitialized

bool isInitialized {false}

protected

Whether the collision check is initialized.

Definition at line 264 of file SimoxCSpace.h.

◆ loadVisualizationModel

bool loadVisualizationModel {false}

protected

Whether the visualization model of objects/agents sould be loaded.

Definition at line 268 of file SimoxCSpace.h.

◆ meshCache

GlobalCache<VirtualRobot::ManipulationObjectPtr, std::pair<int, std::string> > meshCache

mutableprotected

Definition at line 293 of file SimoxCSpace.h.

◆ robotCache

GlobalCache<RobotPoolPtr, std::pair<VirtualRobot::RobotIO::RobotDescription, std::string> > robotCache

mutableprotected

Definition at line 294 of file SimoxCSpace.h.

◆ stationaryObjectSet

VirtualRobot::SceneObjectSetPtr stationaryObjectSet {new VirtualRobot::SceneObjectSet{"StationaryObjectSet"}}

protected

The scene objects for stationary objects.

Definition at line 285 of file SimoxCSpace.h.

◆ stationaryPlanes

std::vector<std::vector<Eigen::Vector3f> > stationaryPlanes

protected

Definition at line 291 of file SimoxCSpace.h.

The documentation for this class was generated from the following files:

RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h
RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.cpp

Classes

Public Member Functions

Static Public Member Functions

Protected Member Functions

Protected Attributes

Friends

Detailed Description

Constructor & Destructor Documentation

◆ SimoxCSpace() [1/2]

◆ SimoxCSpace() [2/2]

Member Function Documentation

◆ addObjectsFromWorkingMemory()

◆ addPlanarObject()

◆ addStationaryObject()

◆ clone() [1/2]

◆ clone() [2/2]

◆ createSimoxCSpace()

◆ getAgent()

◆ getAgentDataAndLoadRobot()

◆ getAgentDataFromRobot()

◆ getAgentJointNames()

◆ getAgentJoints()

◆ getAgentSceneObj()

◆ getCD()

◆ getDimensionality()

◆ getDimensionsBounds()

◆ getMovedSimoxManipulatorObject()

◆ getSimoxManipulatorObject()

◆ getStationaryObjectMargin()

◆ getStationaryObjects()

◆ getStationaryObjectSet()

◆ getTraceVisu() [1/3]

◆ getTraceVisu() [2/3]

◆ getTraceVisu() [3/3]

◆ ice_postUnmarshal()

◆ initAgent() [1/3]

◆ initAgent() [2/3]

◆ initAgent() [3/3]

◆ initCollisionTest()

◆ initStationaryObjects()

◆ isCollisionFree() [1/4]

◆ isCollisionFree() [2/4]

◆ isCollisionFree() [3/4]

◆ isCollisionFree() [4/4]

◆ jointMapToVector()

◆ pathToTrajectory() [1/3]

◆ pathToTrajectory() [2/3]

◆ pathToTrajectory() [3/3]

◆ PrefetchWorkingMemoryObjects()

◆ setAgent()

◆ setConfig() [1/3]

◆ setConfig() [2/3]

◆ setConfig() [3/3]

◆ setStationaryObjectMargin()

◆ setStationaryObjects()

Friends And Related Function Documentation

◆ armarx::GenericFactory

Member Data Documentation

◆ agentJoints

◆ agentSceneObj

◆ cd

◆ fileManager

◆ isInitialized

◆ loadVisualizationModel

◆ meshCache

◆ robotCache

◆ stationaryObjectSet

◆ stationaryPlanes