aikido
Classes | Public Types | Public Member Functions | Static Public Attributes | List of all members
aikido::statespace::R< N > Class Template Reference

Represents a N-dimensional real vector space with vector addition as the group operation. More...

#include <Rn.hpp>

Inheritance diagram for aikido::statespace::R< N >:
aikido::statespace::StateSpace aikido::statespace::dart::RJoint< N > aikido::statespace::dart::WeldJoint

Classes

class  State
 Point in a R<N>. More...
 

Public Types

using VectorNd = Eigen::Matrix< double, N, 1 >
 
using StateHandle = RStateHandle< State >
 
using StateHandleConst = RStateHandle< const State >
 
using ScopedState = statespace::ScopedState< StateHandle >
 
using ScopedStateConst = statespace::ScopedState< StateHandleConst >
 
- Public Types inherited from aikido::statespace::StateSpace
using StateHandle = statespace::StateHandle< StateSpace, State >
 
using StateHandleConst = statespace::StateHandle< StateSpace, const State >
 
using ScopedState = statespace::ScopedState< StateHandle >
 
using ScopedStateConst = statespace::ScopedState< StateHandleConst >
 

Public Member Functions

 R ()
 Constructs a N dimensional real vector space only when the dimension is can be known in compile time. More...
 
 R (int dimension)
 Constructs a dimension dimensional real vector space. More...
 
ScopedState createState () const
 Helper function to create a ScopedState. More...
 
ScopedState cloneState (const StateSpace::State *stateIn) const
 Creates an identical clone of stateIn.
 
Eigen::Map< const VectorNd > getValue (const State *_state) const
 Gets the real vector stored in a State. More...
 
void setValue (State *_state, const VectorNd &_value) const
 Sets the real vector stored in a State. More...
 
std::size_t getStateSizeInBytes () const override
 Gets the size of a State, in bytes. More...
 
StateSpace::StateallocateStateInBuffer (void *_buffer) const override
 Create a new state in a pre-allocated buffer. More...
 
void freeStateInBuffer (StateSpace::State *_state) const override
 
void compose (const StateSpace::State *_state1, const StateSpace::State *_state2, StateSpace::State *_out) const override
 
void getIdentity (StateSpace::State *_out) const override
 
void getInverse (const StateSpace::State *_in, StateSpace::State *_out) const override
 
std::size_t getDimension () const override
 Get the dimension of this Lie group. More...
 
void copyState (const StateSpace::State *_source, StateSpace::State *_destination) const override
 Copy a state. More...
 
void expMap (const Eigen::VectorXd &_tangent, StateSpace::State *_out) const override
 Exponential mapping of Lie algebra element to a Lie group element. More...
 
void logMap (const StateSpace::State *_in, Eigen::VectorXd &_tangent) const override
 Log mapping of Lie group element to a Lie algebra element. More...
 
void print (const StateSpace::State *_state, std::ostream &_os) const override
 Print the n-dimensional vector represented by the state Format: [x_1, x_2, ..., x_n].
 
- Public Member Functions inherited from aikido::statespace::StateSpace
ScopedState createState () const
 Helper function to create a ScopedState. More...
 
ScopedState cloneState (const State *stateIn) const
 Creates an identical clone of stateIn.
 
virtual StateallocateState () const
 Allocate a new state. More...
 
virtual void freeState (State *_state) const
 Free a state previously created by allocateState. More...
 
virtual void freeStateInBuffer (State *_state) const =0
 Free a state previously created by allocateStateInBuffer. More...
 
virtual void compose (const State *_state1, const State *_state2, State *_out) const =0
 Lie group operation for this StateSpace. More...
 
virtual void compose (State *_state1, const State *_state2) const
 Lie group operation for this StateSpace. More...
 
virtual void getIdentity (State *_out) const =0
 Gets the identity element for this Lie group, such that: More...
 
virtual void getInverse (const State *_state, State *_out) const =0
 Gets the inverse of _in in this Lie group, such that: More...
 
virtual void getInverse (State *_state) const
 Gets the inverse of _in in this Lie group. More...
 
virtual void expMap (const Eigen::VectorXd &_tangent, State *_out) const =0
 Exponential mapping of Lie algebra element to a Lie group element. More...
 
virtual void logMap (const State *_in, Eigen::VectorXd &_tangent) const =0
 Log mapping of Lie group element to a Lie algebra element. More...
 
virtual void print (const State *_state, std::ostream &_os) const =0
 Print the state to the output stream. More...
 

Static Public Attributes

static constexpr int DimensionAtCompileTime = N
 Dimension of the space.
 

Detailed Description

template<int N>
class aikido::statespace::R< N >

Represents a N-dimensional real vector space with vector addition as the group operation.

N should be either non-negative or Eigen::Dynamic.

Constructor & Destructor Documentation

§ R() [1/2]

template<int N>
aikido::statespace::R< N >::R ( )

Constructs a N dimensional real vector space only when the dimension is can be known in compile time.

If the dimension is known in compile time, it is recommended to use this constructor over R(int) because this constructor uses fixed size Eigen objects to represent the state data internally, which is generally faster than using dynamic size Eigen objects.

N must be non-negative and not Eigen::Dynamic.

Exceptions
std::invalid_argumentwhen N is Eigen::Dynamic.
See also
R(int)

§ R() [2/2]

template<int N>
aikido::statespace::R< N >::R ( int  dimension)
explicit

Constructs a dimension dimensional real vector space.

This constructor must be used only when N is Eigen::Dynamic (i.e., -1). Otherwise, throws an std::invalid_argument exception.

N must be non-negative and not Eigen::Dynamic.

Exceptions
std::invalid_argumentwhen N is not Eigen::Dynamic and dimension is not the same with N.
See also
R()

Member Function Documentation

§ allocateStateInBuffer()

template<int N>
StateSpace::State * aikido::statespace::R< N >::allocateStateInBuffer ( void *  _buffer) const
overridevirtual

Create a new state in a pre-allocated buffer.

The input argument must contain at least getStateSizeInBytes() bytes of memory. This state must be freed with freeStateInBuffer before freeing _buffer.

Parameters
_buffermemory used to store the returned state
Returns
state object allocated in _buffer

Implements aikido::statespace::StateSpace.

§ copyState()

template<int N>
void aikido::statespace::R< N >::copyState ( const StateSpace::State _source,
StateSpace::State _destination 
) const
overridevirtual

Copy a state.

Parameters
_sourceinput state
[out]_destinationoutput state

Implements aikido::statespace::StateSpace.

§ createState()

template<int N>
auto aikido::statespace::R< N >::createState ( ) const

Helper function to create a ScopedState.

Returns
new ScopedState

§ expMap()

template<int N>
void aikido::statespace::R< N >::expMap ( const Eigen::VectorXd &  _tangent,
StateSpace::State _out 
) const
override

Exponential mapping of Lie algebra element to a Lie group element.

This is simply the identity transformation on a real vector space.

Parameters
_tangentelement of the tangent space
[out]_outcorresponding element of the Lie group

§ getDimension()

template<int N>
std::size_t aikido::statespace::R< N >::getDimension ( ) const
overridevirtual

Get the dimension of this Lie group.

This is also the dimension of the tangent space, i.e. the Lie algebra, associated with this group.

Returns
dimension of this state space

Implements aikido::statespace::StateSpace.

§ getStateSizeInBytes()

template<int N>
std::size_t aikido::statespace::R< N >::getStateSizeInBytes ( ) const
overridevirtual

Gets the size of a State, in bytes.

Returns
size, in bytes, requires to store a State

Implements aikido::statespace::StateSpace.

§ getValue()

template<int N>
auto aikido::statespace::R< N >::getValue ( const State _state) const

Gets the real vector stored in a State.

Parameters
_statea State in this state space
Returns
real vector represented by _state

§ logMap()

template<int N>
void aikido::statespace::R< N >::logMap ( const StateSpace::State _in,
Eigen::VectorXd &  _tangent 
) const
override

Log mapping of Lie group element to a Lie algebra element.

This is simply an identity transformation on a real vector space.

Parameters
_inelement of this Lie group
[out]_tangentcorresponding element of the tangent space

§ setValue()

template<int N>
void aikido::statespace::R< N >::setValue ( State _state,
const VectorNd &  _value 
) const

Sets the real vector stored in a State.

Parameters
_statea State in this state space
_valuereal vector to store in _state

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