RobotStateStream Class Reference

#include <RoboticsPlugin/Include/ImFusion/Robotics/RobotStateStream.h>

A Stream class for interfacing with robots in the ImFusion SDK. More...

Inheritance diagram for RobotStateStream:

Detailed Description

A Stream class for interfacing with robots in the ImFusion SDK.

RobotStateStream is the central class for integrating robotic devices and interacting with them within the ImFusion SDK. It acts as gateway to interact with a simulated or real robot, and holds a RobotInstance class to store its abstract representation. It supports configuration through Properties to set its parameters via an ImFusion workspace file.

The state of the robot is published at regular intervals through the signalNewData Signal (see the RobotStateStreamData documentation for more details).

Commands can be given to the robot through the robot control interfaces implemented by the particular robot integration (see RobotControlInterfacesHolder and ControlInterfaceBase). Different kinds of robots may implement different subsets of the various robot control interfaces. In general, robots supporting asynchronous commands will implement "point to point" (PTP) control interfaces. On the other hand, robots with an interface based on a real-time control loop will implement "streaming" control interfaces. Some may implement both. To send the robot to a given Cartesian position (a target world_T_effector transformation), use CartesianPTPControlInterface or CartesianStreamingControlInterface by passing a CartesianInterpolationMotionGenerator. To send the robot to a given joint position, use CartesianPTPControlInterface or JointStreamingControlInterface by passing a JointInterpolationMotionGenerator.

The PlanningFrameInterface allows to query and set the position of the robot in space, as well as the TCP/end-effector transform. The former is defined in the framework as the world_T_base transformation, and it is analogous to the registration matrix of other kinds of Data (

See also

Data::matrix()). The latter is called flange_T_effector transformation, and it will always be considered to determine the planning frame of the robot. All Cartesian commands to RobotStream will be considered to be in world coordinates, and as targets for the final position of the end effector frame. RobotInstance has the same behavior. RobotModel is not aware of these transformations, and always works in base coordinates. Both transforms can be queried and employed to switch between reference frames. For example, if the pose of an object is known in base or world coordinates, it is possible to change reference frame as follows:

isom3 world_T_base = robotStream.world_T_base(); // position of the base in world coordinates, i.e. transformation from base to world coordinates
isom3 base_T_world = world_T_base.inverse(); // position of the world in base coordinates, i.e. transformation from world to base coordinates
 
isom3 base_T_myObject; // pose of some object in base coordinates
isom3 world_T_myObject; // pose of some object in world coordinates
 
 
isom3 world_T_myObject = world_T_base * base_T_myObject; // pose of the object in world coordinates
isom3 base_T_myObject = base_T_world * world_T_myObject; // pose of the object in base coordinates

Public Member Functions
	RobotStateStream ()
	Create robot state stream with an empty robot name, a simulated stream type, and a custom robot state stream tag.
	RobotStateStream (RobotType robotType, RobotStreamType streamType, const std::string &name="Robot State Stream")
	Initialize a robot state stream with particular parameters.
bool	init (std::shared_ptr< RobotInstance > instance)
	Initialize the stream with a robot instance by.
	~RobotStateStream () override
	Virtual destructor.
RobotStreamType	robotStreamType () const
	Gets the stream's operational mode (simulated, state, control) indicating the type of connection \Return the current type of the robot stream.
RobotType	robotType ()
	Gets the robot identifier as defined in the ImFusion SDK \Return the current type (name) of the robot as string(e.g "Franka Emika Panda")
std::shared_ptr< const RobotStateStreamData >	lastData () const
	Get the most recent robot state data or nullptr if not available.
Data::Kind	kind () const override
	The functions must be overridden to inherit from the Data class.
Modality	modality () const override
	The data type is primarily used for particular algorithms in the SDK, while the robotics plugin has its own dedicated implementation.
void	configure (const Properties *p) override
	Configure the Robot Instance.
void	configuration (Properties *p) const override
	Retrieve the properties of the RobotInstance.
std::shared_ptr< const RobotInstance >	robotInstance ()
	Access to the robot instance and its configuration.
const RobotInstance *	robotInstanceRef () const
	Read-only access to the robot instance and its configuration.
std::shared_ptr< const RobotModel >	robotModel () const
	Access to the robot's kinematic and dynamic model via the robot instance.
const RobotModel *	robotModelRef () const
	Read-only access to the robot's kinematic and dynamic model via the robot instance.
isom3	flange_T_effector () const override
	Retrieve the effector-to-flange transformation.
std::optional< isom3 >	base_T_effector_current () const override
	Return the current-effector-to-base transformation.
std::optional< isom3 >	flange_T_effector_nominal () const override
	Get the nominal Tool Center Point (TCP)-to-flange transformation.
bool	set_flange_T_effector (const isom3 &newValue, bool setNominal) override
	Update the end-effector-to-flange (TCP) transformation and optionally the nominal configuration.
bool	set_flange_T_effector_nominal (const ImFusion::isom3 &newValue) override
	Set the nominal TCP transformation.
void	set_world_T_base (const isom3 &newValue)
	Updates the robot's pose in world coordinates.
isom3	world_T_base () const
	Returns the base-to-world transformation.
Public Member Functions inherited from Stream
	Stream (const std::string &name="")
	Constructor with the name for this stream.
virtual ThreadingMode	threadingMode () const
	Configures threading mode of stream.
virtual bool	supportsAsyncOperation ()
	Returns true if xxxAsync() methods support execution in a background thread.
std::shared_future< bool >	openAsync ()
	Open stream in an asynchronous fashion (wait on the returned future before taking further actions). Returns whether the operation was successful.
std::shared_future< bool >	closeAsync ()
	Close stream in an asynchronous fashion (wait on the returned future before taking further actions). Returns whether the operation was successful.
std::shared_future< bool >	startAsync ()
	Start stream in an asynchronous fashion (wait on the returned future before taking further actions). Returns whether the operation was successful.
std::shared_future< bool >	stopAsync ()
	Stop stream in an asynchronous fashion (wait on the returned future before taking further actions). Returns whether the operation was successful.
std::shared_future< bool >	pauseAsync ()
	Pause stream in an asynchronous fashion (wait on the returned future before taking further actions). Returns whether the operation was successful.
std::shared_future< bool >	resumeAsync ()
	Resume stream in an asynchronous fashion (wait on the returned future before taking further actions). Returns whether the operation was successful.
bool	open ()
	Open stream and wait for the operation to complete. Returns whether the operation was successful.
bool	close ()
	Close stream and wait for the operation to complete. Returns whether the operation was successful.
bool	start ()
	Start stream and wait for the operation to complete. Returns whether the operation was successful.
bool	stop ()
	Stop stream and wait for the operation to complete. Returns whether the operation was successful.
bool	pause ()
	Pause stream and wait for the operation to complete. Returns whether the operation was successful.
bool	resume ()
	Resume stream and wait for the operation to complete. Returns whether the operation was successful.
virtual bool	supportsPausing () const
	Override and return true if this stream implements the pausing and resume operations.
bool	restart ()
	Stops, closes, opens and starts the stream. Returns whether the operation was successful.
virtual bool	reset ()
	Reverts stream to first frame, if possible. Returns whether the operation was successful.
virtual bool	isRunning () const
	Return whether stream is currently running.
virtual std::string	uuid ()=0
	Unique identifier for stream.
State	currentState () const
bool	isStateOneOf (const std::vector< State > &states) const
Geometry::AlignedBox	bounds () const override
	Per default a Stream has empty/invalid bounds.
Pose::TransformationConvention	matrixConvention () const override
	Matrices of streams by default map to the world coordinate system.
virtual bool	createDefaultStreamController ()
	Indicates whether a StreamController should be created for the stream (by default returns true).
double	updateRate () const
	Average update rate in Hz of the signalStreamData.
std::string	describe () const override
	Short description of the stream including latest state and update rate information.
Public Member Functions inherited from Data
	Data (const std::string &name="")
	Data (const Data &other)
Data &	operator= (const Data &other)
virtual	~Data ()
	Mandatory virtual destructor.
const std::string &	name () const
	Return the name of this data.
void	setName (const std::string &name)
	Sets the name of this data.
virtual bool	isAnnotationType () const
	Return whether this data type is visualized through an annotation (e.g. mesh)
const DataComponentList &	components () const
	Returns the list of DataComponents for this data.
DataComponentList &	components ()
virtual void	setMatrixFromWorld (const mat4 &m)
	Set matrix mapping from the world coordinate system to the data coordinate system.
virtual void	setMatrixToWorld (const mat4 &m)
	Set matrix mapping from the data coordinate system to the world coordinate system.
virtual mat4	matrixFromWorld () const
	Get matrix mapping from the world coordinate system to the data coordinate system.
virtual mat4	matrixToWorld () const
	Get matrix mapping from the data coordinate system to the world coordinate system.
virtual mat4	matrix () const
	Return the transformation matrix.
virtual void	setMatrix (const mat4 &m)
	Set the transformation matrix.
Public Member Functions inherited from Configurable
virtual void	configureDefaults ()
	Retrieve the properties of this object, replaces values with their defaults and sets it again.
void	registerParameter (ParameterBase *param)
	Register the given Parameter or SubProperty, so that it will be configured during configure()/configuration().
void	unregisterParameter (const ParameterBase *param)
	Remove the given Parameter or SubProperty from the list of registered parameters.
	Configurable (const Configurable &rhs)
	Configurable (Configurable &&rhs) noexcept
Configurable &	operator= (const Configurable &)
Configurable &	operator= (Configurable &&) noexcept
Public Member Functions inherited from RobotControlInterfacesHolder
template<typename T>
bool	hasControlInterface () const
template<typename T>
void	registerControlInterface (T *newInterface)
template<typename T>
bool	deRegisterControlInterface ()
template<typename T>
T *	getControlInterface () const
Public Member Functions inherited from PlanningFrameInterface
virtual bool	supportsSetting_flange_T_effector_nominal () const =0
	if true, the TCP transform can be set in the robot firmware
virtual bool	set_flange_T_effector_nominal (const isom3 &newValue)=0
	sets the TCP transform in the robot controller RobotStateStream has a dummy implementation that will log an error if a Streams declaring support does not override this function

Protected Member Functions
void	publishNewData (std::shared_ptr< const RobotState > newData)
	Emit new robot state data to all the stream listeners, with data containing RobotState.
Protected Member Functions inherited from Stream
virtual bool	openImpl ()=0
	Open stream.
virtual bool	closeImpl ()=0
	Close stream.
virtual bool	startImpl ()=0
	Start stream.
virtual bool	stopImpl ()=0
	Stop stream.
virtual bool	pauseImpl ()
	Pause stream (not required to be supported).
virtual bool	resumeImpl ()
	Resume stream from pause (only required to be supported when pauseImpl() is supported).
virtual bool	workerThreadRequiresOpenGl () const
	Override and return true if worker thread should create a GL::Context.
virtual std::optional< WorkContinuation >	doWork ()=0
	Create one StreamData and publish it (this function will be called at the desired frame rate) If this is not desired, you can do the following: return std::nullopt;.
virtual bool	worksWhilePaused () const
	Override if doWork() should be called also in the Paused state.
void	changeState (State newState)
std::shared_future< bool >	attemptStateChange (std::function< bool()> work, State stateWorking, State stateSuccess, State stateError)
Protected Member Functions inherited from Data
void	swapWith (Data &other)
	Swaps the data and emits a matrix and name changed signal for both.

Protected Attributes
std::shared_ptr< RobotInstance >	m_robotInstance
	Active robot instance.
RobotStreamType	m_robotStreamType
	Stream type (Simulated, Control, State)
RobotType	m_robotType
	Robot name in the SDK.
std::shared_ptr< const RobotStateStreamData >	m_lastData
	A copy of most recent stream data.
std::mutex	m_lastDataMtx
	Synchronizes access to m_lastData.
Protected Attributes inherited from Data
mat4	m_matrix
	Transformation matrix.
std::recursive_mutex *	m_matrixMutex
	Used to internally synchronize access to the matrix of data.
DataComponentList	m_dataComponentList
	The list of DataComponents for this data.
Protected Attributes inherited from Configurable
std::vector< Param >	m_params
	List of all registered Parameter and SubProperty instances.

Additional Inherited Members
Public Types inherited from Stream
enum class	ThreadingMode { BaseClassRunsWorkerThread , DerivedClassHandlesThreading }
	This flag represents the two supported threading modes of a Stream. More...
enum class	State {   Closed , Opening , Open , Starting ,   Running , Pausing , Paused , Resuming ,   Stopping , Closing }
Public Types inherited from Data
enum	Kind {   UNKNOWN = 0 , IMAGE = 1 , VOLUME = 2 , IMAGESET = 3 ,   VOLUMESET = 4 , IMAGESTREAM = 5 , VOLUMESTREAM = 6 , POINTSET = 7 ,   SURFACE = 8 , TRACKINGSTREAM = 9 , LIVETRACKINGSTREAM = TRACKINGSTREAM , TRACKINGDATA = 10 ,   TREE = 11 , TENSOR = 12 , POLYDATASTREAM = 13 , STEREOIMAGESET = 14 ,   STEREOIMAGESTREAM = 15 , VOLUMETRICMESH = 16 }
	Kind of data. More...
enum	Modality {   NA = 0 , XRAY = 1 , CT = 2 , MRI = 3 ,   ULTRASOUND = 4 , VIDEO = 5 , NM = 6 , OCT = 7 ,   LABEL = 8 , DISTANCE = 9 }
	Image modality of the data. More...
Static Public Member Functions inherited from Stream
static std::string	stateToString (State state)
Static Public Member Functions inherited from Data
static std::string	modalityString (Data::Modality m)
	Return the name of an image modality.
static Data::Modality	stringToModality (const std::string &s)
	Returns the modality corresponding to a modality string or NA if nothing matches.
Static Public Member Functions inherited from PlanningFrameInterface
static std::string	id ()
Public Attributes inherited from Stream
ProtectedSignal< StateChange >	signalStateChanged
	Emitted after a state change has been completed with the old and new state.
ProtectedSignal	signalEnded
	Indicates that the stream reached the end of its data, e.g. when a video stream has played to the end.
ProtectedSignal< std::shared_ptr< const StreamData > >	signalStreamData
	Signal that is emitted by the stream when a new StreamData is available.
DeprecatedSignal< ProtectedSignal< std::shared_ptr< const StreamData > >, const StreamData & >	signalNewData
	Signal that is emitted by the stream when a new StreamData is available.
Public Attributes inherited from Data
Signal< const Data * >	signalDeleted
	Signal emitted when this instance is deleted.
Signal< const Data * >	signalMatrixChanged
	Signal emitted when the transformation of this Data has changed.
Signal< std::string >	signalNameChanged
	Signal emitted when the name changed.
Public Attributes inherited from Configurable
Signal	signalParametersChanged
	Emitted whenever one of the registered Parameters' or SubPropertys' signalValueChanged signal was emitted.

Constructor & Destructor Documentation

RobotStateStream()

RobotStateStream ( RobotType robotType, RobotStreamType streamType, const std::string & name = "Robot State Stream" )

explicit

Initialize a robot state stream with particular parameters.

Parameters

robotType	string encoding the names of the supported robots within the ImFusion SDK
streamType	The stream type defines the data flow and can be Simulated, Control, or State. A Simulated stream runs in a virtual environment, useful for testing without a physical robot. The Control stream sends commands to the robot, like joint positions or velocities. The State stream receives feedback, providing real-time information on sensor data and system status.
name	identifier for the stream that appears in the SDK Data List widget

Member Function Documentation

init()

bool init

(

std::shared_ptr< RobotInstance >

instance

)

Initialize the stream with a robot instance by.

• Set up the robot instance for the stream
• Establish bidirectional synchronization between the stream's matrix and the base-to-world transformation
• Connect signal handlers to maintain consistency

  Parameters

  instance

lastData()

std::shared_ptr< const RobotStateStreamData > lastData ( ) const

inline

Get the most recent robot state data or nullptr if not available.

Returns

shared pointer to the recent robot state stream data The retrieved data is read-only and cannot be modified.

kind()

Data::Kind kind ( ) const

inlineoverridevirtual

The functions must be overridden to inherit from the Data class.

Returns

(always) UNKNOWN data as no robot-specific type exist. The data type is primarily used for visualization within the SDK, while the robotics plugin has its own dedicated visualization.

Implements Data.

modality()

Modality modality ( ) const

inlineoverridevirtual

The data type is primarily used for particular algorithms in the SDK, while the robotics plugin has its own dedicated implementation.

The functions must be overridden to inherit from the Data class.

Returns

(always) NA data as no robot-specific modality exist.

Reimplemented from Data.

configure()

void configure ( const Properties * p )

overridevirtual

Configure the Robot Instance.

Reimplemented from Configurable.

configuration()

void configuration ( Properties * p ) const

overridevirtual

Retrieve the properties of the RobotInstance.

Reimplemented from Configurable.

robotInstance()

std::shared_ptr< const RobotInstance > robotInstance

(

)

Access to the robot instance and its configuration.

Returns

- Valid pointer if robot instance exists

• nullptr if no robot instance not defined

robotInstanceRef()

const RobotInstance * robotInstanceRef

(

)

const

Read-only access to the robot instance and its configuration.

Returns

non-owning pointer to the robot instance

• Valid pointer if robot instance exists
• nullptr if no robot instance not defined

robotModel()

std::shared_ptr< const RobotModel > robotModel

(

)

const

Access to the robot's kinematic and dynamic model via the robot instance.

Returns

- Valid pointer if robot instance exists

• nullptr if no robot instance not defined

robotModelRef()

const RobotModel * robotModelRef

(

)

const

Read-only access to the robot's kinematic and dynamic model via the robot instance.

Returns

non-owning pointer to the robot model

• Valid pointer if robot instance exists
• nullptr if no robot instance is available

flange_T_effector()

isom3 flange_T_effector ( ) const

overridevirtual

Retrieve the effector-to-flange transformation.

Returns

Eigen SE(3) isometric transformation

Implements PlanningFrameInterface.

base_T_effector_current()

std::optional< isom3 > base_T_effector_current ( ) const

overridevirtual

Return the current-effector-to-base transformation.

Returns

Eigen SE(3) isometric transformation if available, otherwise std::nullopt

Implements PlanningFrameInterface.

flange_T_effector_nominal()

std::optional< isom3 > flange_T_effector_nominal ( ) const

overridevirtual

Get the nominal Tool Center Point (TCP)-to-flange transformation.

Returns

Eigen SE(3) isometric transformation if available, otherwise std::nullopt

Note

- The base implementation returns std::nullopt. For robots that support setting the TCP transformation via the API, the Control and State streams should override this function

Implements PlanningFrameInterface.

set_flange_T_effector()

bool set_flange_T_effector ( const isom3 & newValue, bool setNominal )

overridevirtual

Update the end-effector-to-flange (TCP) transformation and optionally the nominal configuration.

Parameters

newValue	isometric end-effector-to-flange transformation
setNominal	If true, also updates the nominal (configured) transformation

Returns

true if successful, false if:

• No robot instance is available
• Nominal setting is requested but not supported

Implements PlanningFrameInterface.

set_flange_T_effector_nominal()

bool set_flange_T_effector_nominal ( const ImFusion::isom3 & newValue )

override

Set the nominal TCP transformation.

Parameters

newValue

isometric end-effector-to-flange transformation matrix

Returns

false (base implementation)

Note

Logs error if called without proper override in derived class Must be implemented if the robot supports setting the nominal-effector-to-flange via the API

set_world_T_base()

void set_world_T_base ( const isom3 & newValue )

inline

Updates the robot's pose in world coordinates.

Parameters

newValue

isometric base-to-world transformation

Note

- Synchronizes stream matrix (Data::matrix) with robot instance (RobotInstance::world_T_base)

• Only performs update if transformation changes
• Maintains consistency between stream and robot state

world_T_base()

isom3 world_T_base ( ) const

inline

Returns the base-to-world transformation.

Returns

Eigen SE(3) isometric transformation

---

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

• RoboticsPlugin/Include/ImFusion/Robotics/RobotStateStream.h