Drake
FrameKinematicsVector< KinematicsValue > Class Template Reference

A FrameKinematicsVector is used to report kinematics data for registered frames (identified by unique FrameId values) to SceneGraph. More...

#include <drake/geometry/frame_kinematics_vector.h>

Public Member Functions

 FrameKinematicsVector (SourceId source_id, const std::vector< FrameId > &ids)
 Initializes the vector on the owned ids. More...
 
void clear ()
 Initializes the vector to start setting kinematics values. More...
 
void set_value (FrameId id, const KinematicsValue &value)
 Sets the kinematics value for the frame indicated by the given id. More...
 
SourceId source_id () const
 
int size () const
 Returns the constructed size of this vector – the number of FrameId values it was constructed with. More...
 
const KinematicsValue & value (FrameId id) const
 Returns the value associated with the given id. More...
 
bool has_id (FrameId id) const
 Reports true if the given id is a member of this data. More...
 
Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable
 FrameKinematicsVector (const FrameKinematicsVector &)=default
 
FrameKinematicsVectoroperator= (const FrameKinematicsVector &)=default
 
 FrameKinematicsVector (FrameKinematicsVector &&)=default
 
FrameKinematicsVectoroperator= (FrameKinematicsVector &&)=default
 

Detailed Description

template<class KinematicsValue>
class drake::geometry::FrameKinematicsVector< KinematicsValue >

A FrameKinematicsVector is used to report kinematics data for registered frames (identified by unique FrameId values) to SceneGraph.

It serves as the basis of FramePoseVector, FrameVelocityVector, and FrameAccelerationVector.

The FrameKinematicsVector must be constructed with the source's SourceId and the ids of the frames owned by the source. Once constructed, it cannot be resized. Typically, this will be done in the allocation method of the LeafSystem which serves as a geometry source.

Populating the vector with values is a two-step process: clear and set. Before writing any kinematics data the vector should be cleared (see clear()). After clearing, each registered frame will have a kinematics value assigned to it by calling set_value().

Only those frame ids provided in the constructor can be set. Attempting to set the value for any other frame id is considered an error. Attempting to write more frames than the vector was constructed for is considered an error and will throw an exception. Failing to set data for every registered frame will be considered an error when the FrameKinematicsVector is consumed by SceneGraph.

The usage of this method would be in the allocation and calculation of a LeafSystem's output port. The nature of the allocation depends on whether the source id and number of frames are available at construction or not. The first example shows the case where source id and frame count are known. The second shows the alternate, deferred case.

template <typename T>
class AllocInConstructorSystem : public LeafSystem<T> {
public:
explicit AllocInConstructorSystem(SourceId source_id)
: source_id_(source_id) {
...
// Register frames, storing ids in frame_ids_
this->DeclareAbstractOutputPort(
FramePoseVector<T>(source_id, frame_ids_),
&AllocInConstructorSystem::CalcFramePoseOutput);
...
}
private:
void CalcFramePoseOutput(const MyContext& context,
geometry::FramePoseVector<T>* poses) const {
DRAKE_DEMAND(poses->source_id() == source_id_);
DRAKE_DEMAND(poses->size() == static_cast<int>(frame_ids_.size()));
poses->clear();
for (int i = 0; i < static_cast<int>(frame_ids_.size()); ++i) {
poses->set_value(frame_ids_[i], poses_[i]);
}
}
std::vector<FrameId> frame_ids_;
std::vector<Isometry3<T>> poses_;
};

Example 1: Known source id and frame count in constructor.

/// Definition of FramePoseVector deferred to define number of frames. However,
/// it must be defined prior to call to `AllocateContext()`.
template <typename T>
class DeferredAllocationSystem : public LeafSystem<T> {
public:
DeferredAllocationSystem() {
...
this->DeclareAbstractOutputPort(
&DeferredAllocationSystem::AllocateFramePoseOutput,
&DeferredAllocationSystem::CalcFramePoseOutput);
}
private:
geometry::FramePoseVector<T> AllocateFramePoseOutput() const {
// Assume that source_id_ has been assigned and the frames have been
// registered.
DRAKE_DEMAND(source_id_.is_valid());
return geometry::FramePoseVector<T>(source_id_, frame_ids_);
}
void CalcFramePoseOutput(const MyContext& context,
geometry::FramePoseVector<T>* poses) const {
DRAKE_DEMAND(poses->source_id() == source_id_);
DRAKE_DEMAND(poses->size() == static_cast<int>(frame_ids_.size()));
poses->clear();
for (int i = 0; i < static_cast<int>(frame_ids_.size()); ++i) {
poses->set_value(frame_ids_[i], poses_[i]);
}
}
std::vector<FrameId> frame_ids_;
std::vector<Isometry3<T>> poses_;
};

Example 2: Deferred pose vector allocation.

Template Parameters
KinematicsValueThe underlying data type of for the order of kinematics data (e.g., pose, velocity, or acceleration).

One should never interact with the FrameKinematicsVector class directly. Instead, the FramePoseVector, FrameVelocityVector, and FrameAccelerationVector classes are aliases of the FrameKinematicsVector instantiated on specific data types (Isometry3, SpatialVector, and SpatialAcceleration, respectively). Each of these data types are templated on Eigen scalars. All supported combinations of data type and scalar type are already available to link against in the containing library. No other values for KinematicsValue are supported.

Currently, the following data types with the following scalar types are supported:

Alias Instantiation Scalar types
FramePoseVector FrameKinematicsVector<Isometry3<Scalar>> double
FramePoseVector FrameKinematicsVector<Isometry3<Scalar>> AutoDiffXd
FramePoseVector FrameKinematicsVector<Isometry3<Scalar>> Expression

Constructor & Destructor Documentation

FrameKinematicsVector ( const FrameKinematicsVector< KinematicsValue > &  )
default
FrameKinematicsVector ( FrameKinematicsVector< KinematicsValue > &&  )
default
FrameKinematicsVector ( SourceId  source_id,
const std::vector< FrameId > &  ids 
)

Initializes the vector on the owned ids.

Parameters
source_idThe source id of the owning geometry source.
idsThe set of all frames owned by this geometry source. All of these ids must have values provided in the output port calculation and only these ids. SceneGraph will validate the ids to confirm that they are all owned by the source with the given source_id.

Member Function Documentation

void clear ( )

Initializes the vector to start setting kinematics values.

Here is the caller graph for this function:

bool has_id ( FrameId  id) const
inline

Reports true if the given id is a member of this data.

Here is the caller graph for this function:

FrameKinematicsVector& operator= ( FrameKinematicsVector< KinematicsValue > &&  )
default
FrameKinematicsVector& operator= ( const FrameKinematicsVector< KinematicsValue > &  )
default
void set_value ( FrameId  id,
const KinematicsValue &  value 
)

Sets the kinematics value for the frame indicated by the given id.

There are various error conditions which will lead to an exception being thrown:

  1. the id provided is not one of the frame ids provided in the constructor.
  2. clear() hasn't been called.
  3. the value for a particular id is set multiple times between clear() invocations.

If this isn't invoked for every frame id provided at construction, it will lead to a subsequent exception when SceneGraph consumes the data.

Here is the call graph for this function:

Here is the caller graph for this function:

int size ( ) const
inline

Returns the constructed size of this vector – the number of FrameId values it was constructed with.

Here is the caller graph for this function:

SourceId source_id ( ) const
inline

Here is the caller graph for this function:

const KinematicsValue & value ( FrameId  id) const

Returns the value associated with the given id.

Exceptions
std::runtime_errorif id is not in the specified set of ids.

Here is the call graph for this function:

Here is the caller graph for this function:


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