This AssemblyCondition specifies a correspondence between orientation sensors fixed on mobilized bodies ("osensors") and Ground-relative orientation sensor readings ("observations"). More...

Inheritance diagram for SimTK::OrientationSensors:

Public Member Functions
	SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE (OrientationSensors, OSensorIx)
	Define the OSensorIx type which is just a uniquely-typed int. More...

	SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE (OrientationSensors, ObservationIx)
	Define the ObservationIx type which is just a uniquely-typed int. More...

Construction and setup
These methods are used as an extended construction phase for OrientationSensors objects, defining the osensors and observations that will be used in the subsequent tracking steps.
	OrientationSensors ()
	The default constructor creates an empty OrientationSensors AssemblyCondition object that should be filled in with calls to addOSensor() and optionally defineObservationOrder(). More...

OSensorIx	addOSensor (const String &name, MobilizedBodyIndex bodyB, const Rotation &orientationInB, Real weight=1)
	Define a new orientation sensor (osensor) attached to a particular MobilizedBody. More...

OSensorIx	addOSensor (MobilizedBodyIndex bodyB, const Rotation &orientationInB, Real weight=1)
	Define an unnamed osensor. More...

void	defineObservationOrder (const Array_< OSensorIx > &observationOrder)
	Define the meaning of the observation data by giving the OSensorIx associated with each observation. More...

void	defineObservationOrder (const Array_< String > &observationOrder)
	Define the meaning of the observations by giving the osensor name corresponding to each observation, as a SimTK::Array_<String>. More...

void	defineObservationOrder (const std::vector< String > &observationOrder)
	Define observation order using an std::vector of SimTK::String. More...

void	defineObservationOrder (const Array_< std::string > &observationOrder)
	Define observation order using an Array_ of std::string. More...

void	defineObservationOrder (const std::vector< std::string > &observationOrder)
	Define observation order using an std::vector of std::string. More...

void	defineObservationOrder (int n, const char *const observationOrder[])
	Define observation order using a C array of const char* names. More...

Retrieve setup information
These methods are used to query information associated with the construction and setup of this OrientationSensors object. This information does not normally change during an osensor-tracking study, although osensor weights may be changed by some inverse kinematics methods.
int	getNumOSensors () const
	Return a count n of the number of currently-defined osensors. More...

const String &	getOSensorName (OSensorIx ix)
	Return the unique osensor name assigned to the osensor whose index is provided. More...

const OSensorIx	getOSensorIx (const String &name)
	Return the osensor index associated with the given osensor name. More...

Real	getOSensorWeight (OSensorIx mx)
	Get the weight currently in use for the specified osensor; this can be changed dynamically via changeOSensorWeight(). More...

MobilizedBodyIndex	getOSensorBody (OSensorIx mx) const
	Get the MobilizedBodyIndex of the body associated with this osensor. More...

const Rotation &	getOSensorStation (OSensorIx mx) const
	Get the orientation (coordinate axes fixed in its body frame) of the given osensor. More...

int	getNumObservations () const
	Return the number of observations that were defined via the last call to defineObservationOrder(). More...

ObservationIx	getObservationIxForOSensor (OSensorIx mx) const
	Return the ObservationIx of the observation that is currently associated with the given osensor, or an invalid index if the osensor doesn't have any corresponding observation (in which case it is being ignored). More...

bool	hasObservation (OSensorIx mx) const
	Return true if the supplied osensor is currently associated with an observation. More...

OSensorIx	getOSensorIxForObservation (ObservationIx ox) const
	Return the OSensorIx of the osensor that is associated with the given observation, or an invalid index if the observation doesn't correspond to any osensor (in which case it is being ignored). More...

bool	hasOSensor (ObservationIx ox) const
	Return true if the supplied observation is currently associated with a osensor. More...

const Array_< OSensorIx > &	getOSensorsOnBody (MobilizedBodyIndex mbx)
	The OSensors assembly condition organizes the osensors by body after initialization; call this to get the list of osensors on any particular body. More...

Execution methods
These methods can be called between tracking steps to make step-to-step changes without reinitialization, and to access the current values of step-to-step data including the resulting osensor errors.
void	moveOneObservation (ObservationIx ox, const Rotation &observation)
	Move a single osensor's observed orientation without moving any of the others. More...

void	moveAllObservations (const Array_< Rotation > &observations)
	Set the observed osensor orientations for a new observation frame. More...

void	changeOSensorWeight (OSensorIx mx, Real weight)
	Change the weight associated with a particular osensor. More...

const Rotation &	getObservation (ObservationIx ox) const
	Return the current value of the orientation for this observation. More...

const Array_< Rotation, ObservationIx > &	getAllObservations () const
	Return the current values of all the observed orientations. More...

Rotation	findCurrentOSensorOrientation (OSensorIx mx) const
	Using the current value of the internal state, calculate the ground frame orientation of a particular osensor. More...

Real	findCurrentOSensorError (OSensorIx mx) const
	Using the current value of the internal state, calculate the error between the given osensor's current orientation and its corresponding observed orientation (unweighted), as a nonnegative angle in radians between 0 and Pi. More...

AssemblyCondition virtuals
These methods are the implementations of the AssemblyCondition virtuals.
int	initializeCondition () const override
	This is called whenever the Assembler is initialized in case this assembly condition wants to do some internal work before getting started. More...

void	uninitializeCondition () const override
	This is called whenever the containing Assembler is uninitialized in case this assembly condition has some cleanup to do. More...

int	calcErrors (const State &state, Vector &err) const override
	Calculate the amount by which this assembly condition is violated by the q values in the given state, with one scalar error per assembly equation returned in err. More...

int	calcErrorJacobian (const State &state, Matrix &jacobian) const override
	Override to supply an analytic Jacobian for the assembly errors returned by calcErrors(). More...

int	getNumErrors (const State &state) const override
	Override to supply an efficient method for determining how many errors will be returned by calcErrors(). More...

int	calcGoal (const State &state, Real &goal) const override
	Calculate the current contribution (>= 0) of this assembly condition to the goal value that is being minimized. More...

int	calcGoalGradient (const State &state, Vector &grad) const override
	Override to supply an analytic gradient for this assembly condition's goal. More...

Public Member Functions inherited from SimTK::AssemblyCondition
	AssemblyCondition (const String &name)
	Base class constructor just takes the assembly condition name and saves it. More...

virtual	~AssemblyCondition ()
	Destructor is virtual for use by derived classes. More...

const char *	getName () const
	Return the name assigned to this AssemblyCondition on construction. More...

bool	isInAssembler () const
	Test whether this AssemblyCondition has already been adopted by an Assembler. More...

const Assembler &	getAssembler () const
	Return the Assembler that has adopted this AssemblyCondition. More...

AssemblyConditionIndex	getAssemblyConditionIndex () const
	Return the AssemblyConditionIndex of this concrete AssemblyCondition within the Assembler that has adopted it. More...

Additional Inherited Members
Protected Member Functions inherited from SimTK::AssemblyCondition
int	getNumFreeQs () const
	Ask the assembler how many free q's there are; only valid after initialization but does not invoke initialization. More...

QIndex	getQIndexOfFreeQ (Assembler::FreeQIndex fx) const
	Ask the assembler where to find the actual q in the State that corresponds to a given free q; only valid after initialization but does not invoke initialization. More...

Assembler::FreeQIndex	getFreeQIndexOfQ (QIndex qx) const
	Ask the assembler where to find the free q (if any) that corresponds to a given q in the State; only valid after initialization but does not invoke initialization. More...

const MultibodySystem &	getMultibodySystem () const
	Ask the assembler for the MultibodySystem with which it is associated. More...

const SimbodyMatterSubsystem &	getMatterSubsystem () const
	Ask the assembler for the MultibodySystem with which it is associated and extract the SimbodyMatterSubsystem contained therein. More...

void	initializeAssembler () const
	Call this method before doing anything that logically requires the Assembler, or at least this AssemblyCondition, to have been initialized. More...

void	uninitializeAssembler () const
	Call this when modifying any parameter of the concrete AssemblyCondition that would require reinitialization of the Assembler or the AssemblyCondition. More...

Detailed Description

This AssemblyCondition specifies a correspondence between orientation sensors fixed on mobilized bodies ("osensors") and Ground-relative orientation sensor readings ("observations").

Each osensor represents the three orthogonal axis directions of a frame fixed to a mobilized body; only orientation and not location is represented. Orientation sensing is commonly performed by Inertial Measurements Units (IMUs). The behavior here is designed to be as similar as possible to position sensors handled by the Markers AssemblyCondition. You can combine OrientationSensors and Markers on the same body.

The idea is to adjust the q's so that each osensor is oriented similarly to its corresponding observation. This is normally used as a goal since we don't expect to be able to obtain a perfect match, but you can use these as a set of assembly error conditions if there are enough degrees of freedom to achieve a near-perfect solution.

Osensors are defined one at a time and assigned sequential index values of type OrientationSensors::OSensorIx. They may optionally be given unique, case-sensitive names, and we will keep a map from name to OSensorIx. A default name will be assigned if none is given. A weight is assigned to every osensor, with default weight=1. We do not expect that all the osensors will be used; osensors with weights of zero will not be included in the study, nor will osensors for which no observation is given.

Once specified, the osensor definitions do not change during a series of inverse kinematic (tracking) steps. The observations, on the other hand, are expected to come from a time series of experimental measurements of osensor orientations and will be different at every step. They typically come from a file organized by "frame", meaning an observation time and a set of observed orientations, one per sensor, corresponding to that time. During initial setup, the number of observations per frame and their correspondence to the defined osensors is specified. They can be in any order, may skip some osensors, and may include data for osensors that are not defined. However, once initialized each frame must supply the same information in the same order. Data for an unobserved osensor can be provided as NaN in which case it will be ignored in that frame. The frame time is supplied to the track() method which initiates assembly for a frame.

Observation-osensor correspondence maps an ObservationIx to a unique OSensorIx. By default, we'll expect to get an observation for each osensor and that the observation order and the osensor order are the same, i.e. ObservationIx==OSensorIx for every osensor. However, you can instead define observation/osensor correspondence yourself, (after all osensors have been defined), via one of the defineObservationOrder() methods. This is done by supplying an array of OSensorIx values, or an array of osensor names, with the array elements ordered by ObservationIx. Any invalid osensor index or unrecognized osensor name means we will ignore values provide for that observation; similarly, any osensors whose index or name is not specified at all will be ignored.

Constructor & Destructor Documentation

SimTK::OrientationSensors::OrientationSensors ( )

inline

The default constructor creates an empty OrientationSensors AssemblyCondition object that should be filled in with calls to addOSensor() and optionally defineObservationOrder().

Member Function Documentation

SimTK::OrientationSensors::SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE	(	OrientationSensors	,
		OSensorIx
	)

Define the OSensorIx type which is just a uniquely-typed int.

SimTK::OrientationSensors::SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE	(	OrientationSensors	,
		ObservationIx
	)

Define the ObservationIx type which is just a uniquely-typed int.

OSensorIx SimTK::OrientationSensors::addOSensor	(	const String &	name,
		MobilizedBodyIndex	bodyB,
		const Rotation &	orientationInB,
		Real	weight = `1`
	)

inline

Define a new orientation sensor (osensor) attached to a particular MobilizedBody.

Note that an osensor will be ignored unless an observation is provided for it.

Parameters

[in]	name	A unique name to be used to identify this osensor. If the name is empty or blank, a default name will be supplied.
[in]	bodyB	The MobilizedBody to which this osensor is fixed. OSensors on Ground are allowed but will be ignored.
[in]	orientationInB	This is the orientation of the osensor in bodyB's local frame.
[in]	weight	An optional weight for use in defining the objective function, which combines errors in this osensor's orientation with errors in other osensors' orientation. If the weight is zero this osensor is ignored.

Returns: The unique osensor index number assigned to this osensor. These are assigned sequentially as the osensors are added.

Note: Adding an osensor invalidates any observation/osensor correspondence; be sure to call defineObservationOrder() after defining all your osensors.

OSensorIx SimTK::OrientationSensors::addOSensor	(	MobilizedBodyIndex	bodyB,
		const Rotation &	orientationInB,
		Real	weight = `1`
	)

inline

Define an unnamed osensor.

A default name will be assigned; that name will be "_UNNAMED_XX" where XX is the OSensorIx assigned to that osensor (don't use names of that form yourself).

See also: addOSensor(name,...) for more information.

void SimTK::OrientationSensors::defineObservationOrder ( const Array_< OSensorIx > & observationOrder )

inline

Define the meaning of the observation data by giving the OSensorIx associated with each observation.

The length of the array of osensor indices defines the expected number of observations to be provided for each observation frame. Any osensor index that is supplied with an invalid value means that the corresponding observation will be present in the supplied data but should be ignored.

Parameters

[in] observationOrder This is an array of osensor index values, one per observation, that defines both the number of expected observations and the osensor corresponding to each observation. OSensors can be in any order; an invalid osensor index means that observation will be provided but should be ignored; osensors whose indices are never listed are ignored. If observationOrder is supplied as a zero-length array, then we'll assume there are as many observations as osensors and that their indices match.

Note: If you don't call this method at all, a default correspondence will be defined as described for a zero-length observationOrder array (that is, same number of observations and osensors with matching indices). Whenever you add a new osensor, any previously defined observation order is forgotten so the default correspondence will be used unless you call this again.

void SimTK::OrientationSensors::defineObservationOrder ( const Array_< String > & observationOrder )

inline

Define the meaning of the observations by giving the osensor name corresponding to each observation, as a SimTK::Array_<String>.

The length of the array of osensor indices defines the expected number of observations. Any osensor name that is unrecognized or empty means that the corresponding observation will be present in the supplied data but should be ignored.

void SimTK::OrientationSensors::defineObservationOrder ( const std::vector< String > & observationOrder )

inline

Define observation order using an std::vector of SimTK::String.

void SimTK::OrientationSensors::defineObservationOrder ( const Array_< std::string > & observationOrder )

inline

Define observation order using an Array_ of std::string.

void SimTK::OrientationSensors::defineObservationOrder ( const std::vector< std::string > & observationOrder )

inline

Define observation order using an std::vector of std::string.

void SimTK::OrientationSensors::defineObservationOrder	(	int	n,
		const char *const	observationOrder[]
	)

inline

Define observation order using a C array of const char* names.

int SimTK::OrientationSensors::getNumOSensors ( ) const

inline

Return a count n of the number of currently-defined osensors.

Valid osensor index values (of type OrientationSensors::OSensorIx) are 0..n-1.

const String& SimTK::OrientationSensors::getOSensorName ( OSensorIx ix )

inline

Return the unique osensor name assigned to the osensor whose index is provided.

If the osensor was defined without a name, this will return the default name that was assigned to it.

const OSensorIx SimTK::OrientationSensors::getOSensorIx ( const String & name )

inline

Return the osensor index associated with the given osensor name.

If the name is not recognized the returned index will be invalid (test with index.isValid()).

Real SimTK::OrientationSensors::getOSensorWeight ( OSensorIx mx )

inline

Get the weight currently in use for the specified osensor; this can be changed dynamically via changeOSensorWeight().

MobilizedBodyIndex SimTK::OrientationSensors::getOSensorBody ( OSensorIx mx ) const

inline

Get the MobilizedBodyIndex of the body associated with this osensor.

const Rotation& SimTK::OrientationSensors::getOSensorStation ( OSensorIx mx ) const

inline

Get the orientation (coordinate axes fixed in its body frame) of the given osensor.

int SimTK::OrientationSensors::getNumObservations ( ) const

inline

Return the number of observations that were defined via the last call to defineObservationOrder().

These are not necessarily all being used. If defineObservationOrder() was never called, we'll expect the same number of observations as osensors although that won't be set up until the Assembler has been initialized.

ObservationIx SimTK::OrientationSensors::getObservationIxForOSensor ( OSensorIx mx ) const

inline

Return the ObservationIx of the observation that is currently associated with the given osensor, or an invalid index if the osensor doesn't have any corresponding observation (in which case it is being ignored).

An exception will be thrown if the given OSensorIx is not in the range 0..getNumOSensors()-1.

bool SimTK::OrientationSensors::hasObservation ( OSensorIx mx ) const

inline

Return true if the supplied osensor is currently associated with an observation.

See also: getObservationIxForOSensor()

OSensorIx SimTK::OrientationSensors::getOSensorIxForObservation ( ObservationIx ox ) const

inline

Return the OSensorIx of the osensor that is associated with the given observation, or an invalid index if the observation doesn't correspond to any osensor (in which case it is being ignored).

An exception will be thrown if the given ObservationIx is not in the range 0..getNumObservations()-1.

bool SimTK::OrientationSensors::hasOSensor ( ObservationIx ox ) const

inline

Return true if the supplied observation is currently associated with a osensor.

See also: getOSensorIxForObservation()

const Array_<OSensorIx>& SimTK::OrientationSensors::getOSensorsOnBody ( MobilizedBodyIndex mbx )

inline

The OSensors assembly condition organizes the osensors by body after initialization; call this to get the list of osensors on any particular body.

If necessary the Assembler will be initialized. It is an error if this assembly condition has not yet been adopted by an Assembler.

void SimTK::OrientationSensors::moveOneObservation	(	ObservationIx	ox,
		const Rotation &	observation
	)

inline

Move a single osensor's observed orientation without moving any of the others.

If the value contains a NaN, this osensor/observation pair will be ignored the next time the assembly goal cost function is calculated.

void SimTK::OrientationSensors::moveAllObservations ( const Array_< Rotation > & observations )

inline

Set the observed osensor orientations for a new observation frame.

These are the orientations to which we will next attempt to rotate all the corresponding osensors. Note that not all observations necessarily have corresponding osensors defined; orientations of those osensors must still be provided here but they will be ignored. The length of the allObservations array must be the same as the number of defined observations; you can obtain that using getNumObservations(). Any observations that contain a NaN will be ignored; that osensor/observation pair will not be used in the next calculation of the assembly goal cost function.

void SimTK::OrientationSensors::changeOSensorWeight	(	OSensorIx	mx,
		Real	weight
	)

inline

Change the weight associated with a particular osensor.

If this is just a quantitative change (e.g., weight was 0.3 now it is 0.4) then this does not require any reinitialization and will affect the goal calculation next time it is done. If the weight changes to or from zero (a qualitative change) then this will uninitialize the Assembler and all the internal data structures will be changed to remove or add this osensor from the list of active osensors. If you want to temporarily ignore an osensor without reinitializing, you can set its corresponding observation to NaN in which case it will simply be skipped when the goal value is calculated.

const Rotation& SimTK::OrientationSensors::getObservation ( ObservationIx ox ) const

inline

Return the current value of the orientation for this observation.

This is the orientation to which we will try to rotate the corresponding osensor if there is one. The result might be NaN if there is no current value for this observation; you can check using Rotation's isFinite() method.

const Array_<Rotation,ObservationIx>& SimTK::OrientationSensors::getAllObservations ( ) const

inline

Return the current values of all the observed orientations.

These are the orientations to which we will try to rotate the corresponding osensors, for those observations that have corresponding osensors defined. Some of the values may be NaN if there is currently no corresponding observation. Note that these are indexed by ObservationIx; use getObservationIxForOSensor() to map a OSensorIx to its corresponding ObservationIx.

Rotation SimTK::OrientationSensors::findCurrentOSensorOrientation ( OSensorIx mx ) const

Using the current value of the internal state, calculate the ground frame orientation of a particular osensor.

The difference between this orientation and the corresponding observation is the current error for this osensor.

Real SimTK::OrientationSensors::findCurrentOSensorError ( OSensorIx mx ) const

inline

Using the current value of the internal state, calculate the error between the given osensor's current orientation and its corresponding observed orientation (unweighted), as a nonnegative angle in radians between 0 and Pi.

If the osensor is not associated with an observation, or if the observed location is missing (indicated by a NaN value), then the error is reported as zero.

int SimTK::OrientationSensors::initializeCondition ( ) const

overridevirtual

This is called whenever the Assembler is initialized in case this assembly condition wants to do some internal work before getting started.

None of the other virtual methods will be called until this one has been, except possibly the destructor. The set of free q's and the internal State are valid at this point and can be retrieved from the Assembler stored in the base class.

Reimplemented from SimTK::AssemblyCondition.

void SimTK::OrientationSensors::uninitializeCondition ( ) const

overridevirtual

This is called whenever the containing Assembler is uninitialized in case this assembly condition has some cleanup to do.

Reimplemented from SimTK::AssemblyCondition.

int SimTK::OrientationSensors::calcErrors	(	const State &	state,
		Vector &	err
	)		const

overridevirtual

Calculate the amount by which this assembly condition is violated by the q values in the given state, with one scalar error per assembly equation returned in err.

The functional return should be zero if successful; negative values are reserved with -1 meaning "not implemented"; return a positive value if your implementation is unable to evaluate the error at the current state. If this method is not implemented then you must implement calcGoal() and this assembly condition may only be used as a goal, not a requirement.

Reimplemented from SimTK::AssemblyCondition.

int SimTK::OrientationSensors::calcErrorJacobian	(	const State &	state,
		Matrix &	jacobian
	)		const

overridevirtual

Override to supply an analytic Jacobian for the assembly errors returned by calcErrors().

The returned Jacobian must be nErr X nFreeQs; that is, if there is only one assembly error equation the returned matrix is a single row (that's the transpose of the gradient). The functional return should be zero if this succeeds; negative values are reserved with the default implementation returning -1 which indicates that the Jacobian must be calculated numerically using the calcErrors() method. Return a positive value if your implementation is unable to evaluate the Jacobian at the current state.

Reimplemented from SimTK::AssemblyCondition.

int SimTK::OrientationSensors::getNumErrors ( const State & state ) const

overridevirtual

Override to supply an efficient method for determining how many errors will be returned by calcErrors().

Otherwise the default implementation determines this by making a call to calcErrors() and returning the size of the returned error vector. The functional return should be zero if this succeeds; negative values are reserved; return a positive value if your implementation of this method can't determine the number of errors with the given state (unlikely!).

Reimplemented from SimTK::AssemblyCondition.

int SimTK::OrientationSensors::calcGoal	(	const State &	state,
		Real &	goal
	)		const

overridevirtual

Calculate the current contribution (>= 0) of this assembly condition to the goal value that is being minimized.

If this isn't overridden we'll generate it by combining the m errors returned by calcErrors() in a mean sum of squares: goal = err^2/m.

Reimplemented from SimTK::AssemblyCondition.

int SimTK::OrientationSensors::calcGoalGradient	(	const State &	state,
		Vector &	gradient
	)		const

overridevirtual

Override to supply an analytic gradient for this assembly condition's goal.

The returned gradient must be nFreeQ X 1; that is, it is a column vector giving the partial derivative of the goal with respect to each of the free q's in order. The functional return should be zero if this succeeds. The default implementation return -1 which indicates that the gradient must be calculated numerically using the calcGoal() method.

Reimplemented from SimTK::AssemblyCondition.

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

AssemblyCondition_OrientationSensors.h

Public Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation