Helper class to compute and draw C-Space images from arbitrary robots. More...

#include <cspace.h>

Classes
struct	Chunk
	Container class for image chunk information to worker threads. More...

struct	GridCoords
	Grid coordinates. More...

struct	GridLine
	Line on the grid. More...

struct	RobotCoords
	Coordinates for the robot. More...

Public Types
enum	Mode { COLLISION, COLOR, DISTANCE, DISTANCE_COLOR }
	Drawing mode for the grid. More...

using	GridCallback = std::function< void(ompl::PPM::Color &, const StatePtr &, bool)>
	Function to callback for grid computation. Gives a pixel to color, the state, and whether the state is in collision. More...

Public Member Functions
	CSpaceGrid (const RobotPtr &robot, unsigned int jointA, unsigned int jointB, unsigned int width=500, unsigned int height=500, Mode mode=COLLISION)
	Constructor. More...

	CSpaceGrid (const RobotPtr &robot, const std::string &jointA, const std::string &jointB, unsigned int width=500, unsigned int height=500, Mode mode=COLLISION)
	Constructor. More...

	~CSpaceGrid ()
	Destructor. More...

void	compute (bool block=true, const StatePtr &base=nullptr, const GridCallback &callback={})
	Compute a PPM image of the configuration space of two joints, jointA and jointB. More...

void	waitToComplete ()
	Wait for all grid computation to complete. More...

bool	isComputing () const
	Checks if any computation is happening. More...

void	cancel ()
	Cancels any remaining computation. More...

ompl::PPM::Color &	getPixel (const StatePtr &state)
	Get the pixel coordinate for a configuration. More...

ompl::PPM::Color &	getPixel (double i, double j)
	Get the pixel coordinate for a configuration of i for jointA and j for jointB. More...

GridCoords	getCoords (const StatePtr &state)
	Get the pixel coordinate for a configuration. More...

GridCoords	getCoords (double i, double j)
	Get the pixel coordinate for a configuration. More...

RobotCoords	getCoords (unsigned int i, unsigned int j)
	Get the associated robot configuration from image coordinates. More...

std::vector< GridLine >	getLines (const StatePtr &a, const StatePtr &b)
	Get the line segments between two points on the grid. Can have two due to wrapping. More...

std::shared_ptr< ompl::PPM >	getImage () const
	Get the current computed PPM image. More...

double	distanceToSlice (const StatePtr &state)
	Returns the distance along the dimensions of the robot not captured by the current C-Space slice. More...

double	progress () const
	Returns a value in [0, 1] of the progress of the current computation. More...

unsigned int	getWidth () const
	Return width of grid. More...

unsigned int	getHeight () const
	Return height of grid. More...

Private Member Functions
void	initialize ()
	Initialize all internal constructs. More...

void	emplaceChunks ()
	Emplaces all images chunks into work queue. More...

Eigen::Vector4d	getFrameColor (const std::string &frame) const
	Gets the average color of a frame. More...

Private Attributes
const RobotPtr	robot_
	Robot to use. More...

const unsigned int	xChunk_ {25}
	Size of X chunk for threading. More...

const unsigned int	yChunk_ {25}
	Size of Y chunk for threading. More...

const double	minDepth_ {-1.}
	Minimum value for signed distance color map. More...

const double	maxDepth_ {1.}
	Maximum value for signed distance color map. More...

const unsigned int	indexA_
	Joint index for X-axis. More...

const unsigned int	indexB_
	Joint index for Y-axis. More...

const Eigen::Vector2d	limitA_
	Joint limits for first joint (1-DoF) More...

const Eigen::Vector2d	limitB_
	Joint limits for second joint (1-DoF) More...

const bool	contA_
	Is joint A continuous? More...

const bool	contB_
	Is joint B continuous? More...

const unsigned int	width_
	Width of image. More...

const unsigned int	height_
	Height of image. More...

const Mode	mode_
	Render mode. More...

std::shared_ptr< ompl::PPM >	image_ {nullptr}
	Image itself. More...

unsigned int	maxChunks_ {0}
	Maximum number of chunks in queue. More...

StatePtr	state_ {nullptr}
	State provided through compute(). More...

StatePtr	temp_ {nullptr}
	Temporary state. More...

GridCallback	callback_ {}
	Callback function to use for extra drawing. More...

bool	done_ {false}
	Notify threads object is to be destroyed. More...

std::mutex	lock_
	Mutex. More...

std::mutex	statelock_
	Mutex for state. More...

std::condition_variable	work_
	Condition variable that there is work to do. More...

std::condition_variable	complete_
	Condition variable to notify when complete. More...

std::queue< Chunk >	chunks_
	Queue of chunks. More...

std::vector< bool >	newState_
	Has each thread updated to new base state for computation? More...

std::vector< bool >	working_
	Which threads are currently working? More...

std::vector< std::thread * >	threads_
	Threads. More...

Static Private Attributes
static const unsigned int	cores_ {std::thread::hardware_concurrency()}
	Number of cores on the machine. More...

Detailed Description

Helper class to compute and draw C-Space images from arbitrary robots.

Definition at line 34 of file cspace.h.

Member Typedef Documentation

◆ GridCallback

using se2ez::CSpaceGrid::GridCallback = std::function<void(ompl::PPM::Color &, const StatePtr &, bool)>

Function to callback for grid computation. Gives a pixel to color, the state, and whether the state is in collision.

Definition at line 74 of file cspace.h.

Member Enumeration Documentation

◆ Mode

enum se2ez::CSpaceGrid::Mode

Drawing mode for the grid.

Enumerator
COLLISION	Draw collisions as black pixels.
COLOR	Color obstacles by geometry color.
DISTANCE	Use `viridis` color palette for signed distance.
DISTANCE_COLOR	Signed distance, but penetrations are colored by geometry.

Definition at line 63 of file cspace.h.

Constructor & Destructor Documentation

◆ CSpaceGrid() [1/2]

CSpaceGrid::CSpaceGrid	(	const RobotPtr &	robot,
		unsigned int	jointA,
		unsigned int	jointB,
		unsigned int	width = `500`,
		unsigned int	height = `500`,
		Mode	mode = `COLLISION`
	)

Constructor.

Parameters

[in]	robot	Robot to compute grid for.
[in]	jointA	index of configuration to vary.
[in]	jointB	other index of configuration to vary.
[in]	width	Width of image to compute.
[in]	height	Height of image to compute.
[in]	mode	What mode to use for rendering.

Definition at line 21 of file cspace.cpp.

◆ CSpaceGrid() [2/2]

CSpaceGrid::CSpaceGrid	(	const RobotPtr &	robot,
		const std::string &	jointA,
		const std::string &	jointB,
		unsigned int	width = `500`,
		unsigned int	height = `500`,
		Mode	mode = `COLLISION`
	)

Constructor.

Parameters

[in]	robot	Robot to compute grid for.
[in]	jointA	1-DoF joint to vary.
[in]	jointB	other 1-DoF joint to vary.
[in]	width	Width of image to compute.
[in]	height	Height of image to compute.
[in]	mode	What mode to use for rendering.

Definition at line 43 of file cspace.cpp.

◆ ~CSpaceGrid()

CSpaceGrid::~CSpaceGrid ( )

Destructor.

Definition at line 196 of file cspace.cpp.

Member Function Documentation

◆ cancel()

void CSpaceGrid::cancel ( )

Cancels any remaining computation.

Definition at line 394 of file cspace.cpp.

◆ compute()

void CSpaceGrid::compute	(	bool	block = `true`,
		const StatePtr &	base = `nullptr`,
		const GridCallback &	callback = `{}`
	)

Compute a PPM image of the configuration space of two joints, jointA and jointB.

Parameters

[in]	block	Wait for computation to finish. If false, returns immediately.
[in]	base	For robots with more than 2 DoF, set other DoF to this state.
[in]	callback	Extra callback function to draw extra information.

Definition at line 231 of file cspace.cpp.

◆ distanceToSlice()

double CSpaceGrid::distanceToSlice ( const StatePtr & state )

Returns the distance along the dimensions of the robot not captured by the current C-Space slice.

Parameters

[in] state State to measure distance to.

Returns: The distance of the state to the slice.

Definition at line 403 of file cspace.cpp.

◆ emplaceChunks()

void CSpaceGrid::emplaceChunks ( )

private

Emplaces all images chunks into work queue.

Definition at line 209 of file cspace.cpp.

◆ getCoords() [1/3]

CSpaceGrid::GridCoords CSpaceGrid::getCoords ( const StatePtr & state )

Get the pixel coordinate for a configuration.

Parameters

[in] state State to use for configuration values.

Returns: Pixel coordinates.

Definition at line 272 of file cspace.cpp.

◆ getCoords() [2/3]

CSpaceGrid::GridCoords CSpaceGrid::getCoords	(	double	i,
		double	j
	)

Get the pixel coordinate for a configuration.

Parameters

[in]	i	Configuration for first joint.
[in]	j	Configuration for second joint.

Returns: Pixel coordinates.

Definition at line 283 of file cspace.cpp.

◆ getCoords() [3/3]

CSpaceGrid::RobotCoords CSpaceGrid::getCoords	(	unsigned int	i,
		unsigned int	j
	)

Get the associated robot configuration from image coordinates.

Parameters

[in]	i	X-coordinate
[in]	j	Y-coordinate

Returns: Robot configuration.

Definition at line 291 of file cspace.cpp.

◆ getFrameColor()

Eigen::Vector4d CSpaceGrid::getFrameColor ( const std::string & frame ) const

private

Gets the average color of a frame.

Parameters

[in] frame Frame to get color of.

Returns: The average color of the frame.

Definition at line 420 of file cspace.cpp.

◆ getHeight()

unsigned int CSpaceGrid::getHeight ( ) const

Return height of grid.

Returns: The height of the grid.

Definition at line 436 of file cspace.cpp.

◆ getImage()

std::shared_ptr< ompl::PPM > CSpaceGrid::getImage ( ) const

Get the current computed PPM image.

Returns: A PPM image with black meaning the robot is in collision, white if it is not in collision.

Definition at line 381 of file cspace.cpp.

◆ getLines()

std::vector< CSpaceGrid::GridLine > CSpaceGrid::getLines	(	const StatePtr &	a,
		const StatePtr &	b
	)

Get the line segments between two points on the grid. Can have two due to wrapping.

Parameters

[in]	a	Endpoint a
[in]	b	Endpoint b

Returns: Grid lines.

Definition at line 299 of file cspace.cpp.

◆ getPixel() [1/2]

ompl::PPM::Color & CSpaceGrid::getPixel ( const StatePtr & state )

Get the pixel coordinate for a configuration.

Parameters

[in] state State to use for configuration values.

Returns: Reference to pixel to set.

Definition at line 267 of file cspace.cpp.

◆ getPixel() [2/2]

ompl::PPM::Color & CSpaceGrid::getPixel	(	double	i,
		double	j
	)

Get the pixel coordinate for a configuration of i for jointA and j for jointB.

Parameters

[in]	i	Value for jointA.
[in]	j	Value for jointB.

Returns: Reference to pixel to set.

Definition at line 277 of file cspace.cpp.

◆ getWidth()

unsigned int CSpaceGrid::getWidth ( ) const

Return width of grid.

Returns: The width of the grid.

Definition at line 431 of file cspace.cpp.

◆ initialize()

void CSpaceGrid::initialize ( )

private

Initialize all internal constructs.

Definition at line 50 of file cspace.cpp.

◆ isComputing()

bool CSpaceGrid::isComputing ( ) const

Checks if any computation is happening.

Definition at line 386 of file cspace.cpp.

◆ progress()

double CSpaceGrid::progress ( ) const

Returns a value in [0, 1] of the progress of the current computation.

Returns: Current progress value.

Definition at line 412 of file cspace.cpp.

◆ waitToComplete()

void CSpaceGrid::waitToComplete ( )

Wait for all grid computation to complete.

Definition at line 255 of file cspace.cpp.

Member Data Documentation

◆ callback_

GridCallback se2ez::CSpaceGrid::callback_ {}

private

Callback function to use for extra drawing.

Definition at line 250 of file cspace.h.

◆ chunks_

std::queue<Chunk> se2ez::CSpaceGrid::chunks_

private

Queue of chunks.

Definition at line 257 of file cspace.h.

◆ complete_

std::condition_variable se2ez::CSpaceGrid::complete_

private

Condition variable to notify when complete.

Definition at line 256 of file cspace.h.

◆ contA_

const bool se2ez::CSpaceGrid::contA_

private

Is joint A continuous?

Definition at line 215 of file cspace.h.

◆ contB_

const bool se2ez::CSpaceGrid::contB_

private

Is joint B continuous?

Definition at line 216 of file cspace.h.

◆ cores_

const unsigned int CSpaceGrid::cores_ {std::thread::hardware_concurrency()}

staticprivate

Number of cores on the machine.

CSpaceGrid

Definition at line 210 of file cspace.h.

◆ done_

bool se2ez::CSpaceGrid::done_ {false}

private

Notify threads object is to be destroyed.

Definition at line 252 of file cspace.h.

◆ height_

const unsigned int se2ez::CSpaceGrid::height_

private

Height of image.

Definition at line 219 of file cspace.h.

◆ image_

std::shared_ptr<ompl::PPM> se2ez::CSpaceGrid::image_ {nullptr}

private

Image itself.

Definition at line 221 of file cspace.h.

◆ indexA_

const unsigned int se2ez::CSpaceGrid::indexA_

private

Joint index for X-axis.

Definition at line 211 of file cspace.h.

◆ indexB_

const unsigned int se2ez::CSpaceGrid::indexB_

private

Joint index for Y-axis.

Definition at line 212 of file cspace.h.

◆ limitA_

const Eigen::Vector2d se2ez::CSpaceGrid::limitA_

private

Joint limits for first joint (1-DoF)

Definition at line 213 of file cspace.h.

◆ limitB_

const Eigen::Vector2d se2ez::CSpaceGrid::limitB_

private

Joint limits for second joint (1-DoF)

Definition at line 214 of file cspace.h.

◆ lock_

std::mutex se2ez::CSpaceGrid::lock_

private

Mutex.

Definition at line 253 of file cspace.h.

◆ maxChunks_

unsigned int se2ez::CSpaceGrid::maxChunks_ {0}

private

Maximum number of chunks in queue.

Definition at line 247 of file cspace.h.

◆ maxDepth_

const double se2ez::CSpaceGrid::maxDepth_ {1.}

private

Maximum value for signed distance color map.

Definition at line 208 of file cspace.h.

◆ minDepth_

const double se2ez::CSpaceGrid::minDepth_ {-1.}

private

Minimum value for signed distance color map.

Definition at line 207 of file cspace.h.

◆ mode_

const Mode se2ez::CSpaceGrid::mode_

private

Render mode.

Definition at line 220 of file cspace.h.

◆ newState_

std::vector<bool> se2ez::CSpaceGrid::newState_

private

Has each thread updated to new base state for computation?

Definition at line 259 of file cspace.h.

◆ robot_

const RobotPtr se2ez::CSpaceGrid::robot_

private

Robot to use.

Definition at line 204 of file cspace.h.

◆ state_

StatePtr se2ez::CSpaceGrid::state_ {nullptr}

private

State provided through compute().

Definition at line 248 of file cspace.h.

◆ statelock_

std::mutex se2ez::CSpaceGrid::statelock_

private

Mutex for state.

Definition at line 254 of file cspace.h.

◆ temp_

StatePtr se2ez::CSpaceGrid::temp_ {nullptr}

private

Temporary state.

Definition at line 249 of file cspace.h.

◆ threads_

std::vector<std::thread *> se2ez::CSpaceGrid::threads_

private

Threads.

Definition at line 261 of file cspace.h.

◆ width_

const unsigned int se2ez::CSpaceGrid::width_

private

Width of image.

Definition at line 218 of file cspace.h.

◆ work_

std::condition_variable se2ez::CSpaceGrid::work_

private

Condition variable that there is work to do.

Definition at line 255 of file cspace.h.

◆ working_

std::vector<bool> se2ez::CSpaceGrid::working_

private

Which threads are currently working?

Definition at line 260 of file cspace.h.

◆ xChunk_

const unsigned int se2ez::CSpaceGrid::xChunk_ {25}

private

Size of X chunk for threading.

Definition at line 205 of file cspace.h.

◆ yChunk_

const unsigned int se2ez::CSpaceGrid::yChunk_ {25}

private

Size of Y chunk for threading.

Definition at line 206 of file cspace.h.

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

/home/zak/code/se2ez_work/include/se2ez/core/cspace.h
/home/zak/code/se2ez_work/src/core/cspace.cpp

Classes

Public Types

Public Member Functions

Private Member Functions

Private Attributes

Static Private Attributes

Detailed Description

Member Typedef Documentation

◆ GridCallback

Member Enumeration Documentation

◆ Mode

Constructor & Destructor Documentation

◆ CSpaceGrid() [1/2]

◆ CSpaceGrid() [2/2]

◆ ~CSpaceGrid()

Member Function Documentation

◆ cancel()

◆ compute()

◆ distanceToSlice()

◆ emplaceChunks()

◆ getCoords() [1/3]

◆ getCoords() [2/3]

◆ getCoords() [3/3]

◆ getFrameColor()

◆ getHeight()

◆ getImage()

◆ getLines()

◆ getPixel() [1/2]

◆ getPixel() [2/2]

◆ getWidth()

◆ initialize()

◆ isComputing()

◆ progress()

◆ waitToComplete()

Member Data Documentation

◆ callback_

◆ chunks_

◆ complete_

◆ contA_

◆ contB_

◆ cores_

◆ done_

◆ height_

◆ image_

◆ indexA_

◆ indexB_

◆ limitA_

◆ limitB_

◆ lock_

◆ maxChunks_

◆ maxDepth_

◆ minDepth_

◆ mode_

◆ newState_

◆ robot_

◆ state_

◆ statelock_

◆ temp_

◆ threads_

◆ width_

◆ work_

◆ working_

◆ xChunk_

◆ yChunk_