aikido
Classes | Public Types | Public Member Functions | List of all members
aikido::statespace::SO2 Class Reference

The two-dimensional special orthogonal group SO(2), i.e. More...

#include <SO2.hpp>

Inheritance diagram for aikido::statespace::SO2:
aikido::statespace::StateSpace aikido::statespace::dart::SO2Joint

Classes

class  State
 

Public Types

using StateHandle = SO2StateHandle< State >
 
using StateHandleConst = SO2StateHandle< 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

 SO2 ()=default
 Constructs a state space representing SO(2).
 
ScopedState createState () const
 Helper function to create a ScopedState. More...
 
ScopedState cloneState (const StateSpace::State *stateIn) const
 Creates an identical clone of stateIn.
 
double toAngle (const State *state) const
 Returns state as a rotation angle in (-pi, pi]. More...
 
void fromAngle (State *state, double angle) const
 Sets state from a rotation angle in (-inf, inf). More...
 
Eigen::Rotation2Dd toRotation (const State *state) const
 Returns state as an Eigen rotation. More...
 
void fromRotation (State *state, const Eigen::Rotation2Dd &rotation) const
 Sets state from an Eigen rotation. 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 angle represented by the state.
 
- 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...
 

Detailed Description

The two-dimensional special orthogonal group SO(2), i.e.

the space of planar rigid body rotations.

Member Function Documentation

§ allocateStateInBuffer()

StateSpace::State * aikido::statespace::SO2::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()

void aikido::statespace::SO2::copyState ( const StateSpace::State _source,
StateSpace::State _destination 
) const
overridevirtual

Copy a state.

Parameters
_sourceinput state
[out]_destinationoutput state

Implements aikido::statespace::StateSpace.

§ createState()

auto aikido::statespace::SO2::createState ( ) const

Helper function to create a ScopedState.

Returns
new ScopedState.

§ expMap()

void aikido::statespace::SO2::expMap ( const Eigen::VectorXd &  tangent,
StateSpace::State out 
) const
override

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

The tangent space is parameterized a rotation angle.

Parameters
[in]tangentElement of the tangent space.
[out]outCorresponding element of the Lie group.

§ fromAngle()

void aikido::statespace::SO2::fromAngle ( State state,
double  angle 
) const

Sets state from a rotation angle in (-inf, inf).

Parameters
[out]stateState corresponding to angle.
[in]angleRotation angle in (-inf, inf).

§ fromRotation()

void aikido::statespace::SO2::fromRotation ( State state,
const Eigen::Rotation2Dd &  rotation 
) const

Sets state from an Eigen rotation.

Parameters
[out]stateState corresponding to rotation.
[in]rotationEigen rotation.

§ getDimension()

std::size_t aikido::statespace::SO2::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()

std::size_t aikido::statespace::SO2::getStateSizeInBytes ( ) const
overridevirtual

Gets the size of a State, in bytes.

Returns
size, in bytes, requires to store a State

Implements aikido::statespace::StateSpace.

§ logMap()

void aikido::statespace::SO2::logMap ( const StateSpace::State in,
Eigen::VectorXd &  tangent 
) const
override

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

The tangent space is parameterized as a rotation angle.

Parameters
[in]inElement of this Lie group.
[out]tangentCorresponding element of the tangent space.

§ toAngle()

double aikido::statespace::SO2::toAngle ( const State state) const

Returns state as a rotation angle in (-pi, pi].

Parameters
[in]stateState.

§ toRotation()

Eigen::Rotation2Dd aikido::statespace::SO2::toRotation ( const State state) const

Returns state as an Eigen rotation.

Parameters
[in]stateState.

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