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
	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 66 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 84 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 685 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 752 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 159 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 119 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 125 of file SimoxCSpace.h.

◆ createSimoxCSpace()

Saba::CSpaceSampledPtr createSimoxCSpace ( ) const

virtual

Definition at line 137 of file SimoxCSpace.cpp.

◆ getAgent()

const AgentPlanningInformation& getAgent ( ) const

inline

Definition at line 189 of file SimoxCSpace.h.

◆ getAgentDataAndLoadRobot()

SimoxCSpace::AgentData getAgentDataAndLoadRobot ( ) const

protected

Definition at line 316 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 398 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 858 of file SimoxCSpace.cpp.

Here is the caller graph for this function:

◆ getAgentJoints()

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

inlineprotected

Definition at line 290 of file SimoxCSpace.h.

Here is the caller graph for this function:

◆ getAgentSceneObj()

const VirtualRobot::RobotPtr& getAgentSceneObj ( ) const

inline

Definition at line 204 of file SimoxCSpace.h.

◆ getCD()

VirtualRobot::CDManager& getCD ( )

inline

Returns: The collision checker.

Definition at line 213 of file SimoxCSpace.h.

◆ getDimensionality()

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

inlineoverride

Returns: The cspace's dimensionality.

Definition at line 154 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 844 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 658 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 591 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 260 of file SimoxCSpace.h.

◆ getStationaryObjects()

const StationaryObjectList& getStationaryObjects ( ) const

inline

Returns: The stationary objects.

Definition at line 183 of file SimoxCSpace.h.

◆ getStationaryObjectSet()

const VirtualRobot::SceneObjectSetPtr& getStationaryObjectSet ( ) const

inline

Returns: The set of stationary objects' scene objects.

Definition at line 198 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 239 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 247 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 758 of file SimoxCSpace.cpp.

Here is the call graph for this function:

◆ initAgent() [1/3]

void initAgent ( )

inlineprotected

Definition at line 377 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 285 of file SimoxCSpace.cpp.

◆ initAgent() [3/3]

void initAgent ( VirtualRobot::RobotPtr robotPtr )

inlineprotected

Definition at line 383 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 209 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 239 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 194 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 90 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 111 of file CSpace.h.

◆ jointMapToVector()

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

Definition at line 870 of file SimoxCSpace.cpp.

◆ pathToTrajectory() [1/3]

virtual TrajectoryPtr pathToTrajectory ( const Path & p ) const

inlinevirtual

Definition at line 225 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 231 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 811 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 734 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 173 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 173 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 180 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 164 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 266 of file SimoxCSpace.h.

◆ setStationaryObjects()

void setStationaryObjects ( const StationaryObjectList & objList )

virtual

Definition at line 167 of file SimoxCSpace.cpp.

Friends And Related Function Documentation

◆ armarx::GenericFactory

friend class armarx::GenericFactory

friend

Definition at line 287 of file SimoxCSpace.h.

Member Data Documentation

◆ agentJoints

std::vector<VirtualRobot::RobotNodePtr> agentJoints

protected

Definition at line 328 of file SimoxCSpace.h.

◆ agentSceneObj

VirtualRobot::RobotPtr agentSceneObj

protected

Definition at line 330 of file SimoxCSpace.h.

◆ cd

VirtualRobot::CDManager cd

protected

The collision checker.

Definition at line 320 of file SimoxCSpace.h.

◆ fileManager

memoryx::GridFileManagerPtr fileManager

protected

The file manager used to load objects.

Definition at line 314 of file SimoxCSpace.h.

◆ isInitialized

bool isInitialized {false}

protected

Whether the collision check is initialized.

Definition at line 304 of file SimoxCSpace.h.

◆ loadVisualizationModel

bool loadVisualizationModel {false}

protected

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

Definition at line 308 of file SimoxCSpace.h.

◆ meshCache

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

mutableprotected

Definition at line 335 of file SimoxCSpace.h.

◆ robotCache

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

mutableprotected

Definition at line 338 of file SimoxCSpace.h.

◆ stationaryObjectSet

VirtualRobot::SceneObjectSetPtr stationaryObjectSet

protected

Initial value:

{

new VirtualRobot::SceneObjectSet{"StationaryObjectSet"}}

The scene objects for stationary objects.

Definition at line 325 of file SimoxCSpace.h.

◆ stationaryPlanes

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

protected

Definition at line 332 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