#include <BipedalLocomotion/Planners/SO3Planner.h>
template<LieGroupTrivialization trivialization>
BipedalLocomotion::Planners::SO3Planner class

SO3Planner implements a minimum jerk trajectory planner for object belonging to SO(3).

The planner assumes initial and final angular acceleration and velocity proportional to the error between the initial and final rotation. Where the error formulation depends on the trivialization used. In details, for left trivialized - body frame

\[ \epsilon = \text{log} \left(R_0^\top R_f \right) \]

for right trivialized - inertial frame

\[ \epsilon = \text{log} \left(R_f^\top R_0 \right) \]

Base classes

template<class Output>
class BipedalLocomotion::System::Source<SO3PlannerState>
Source is a template specialization of Advanceable and represents a block that does not contains input.

Public static functions

static auto projectTangentVector(const manif::SO3d& initialRotation, const manif::SO3d& finalRotation, const manif::SO3d::Tangent& vector) -> manif::SO3d::Tangent
Project the tangent vector on the vector that represents the distance between the two rotations.

Public functions

auto setRotations(const manif::SO3d& initialRotation, const manif::SO3d& finalRotation, const std::chrono::nanoseconds& duration) -> bool
Set the rotation.
auto evaluatePoint(const std::chrono::nanoseconds& time, SO3PlannerState& state) -> bool
Get the trajectory at a given time.
template<class Derived>
auto evaluatePoint(const std::chrono::nanoseconds& time, manif::SO3d& rotation, manif::SO3TangentBase<Derived>& velocity, manif::SO3TangentBase<Derived>& acceleration) -> bool
Get the trajectory at a given time.
auto setAdvanceTimeStep(const std::chrono::nanoseconds& dt) -> bool
Set the time step of the advance interface.
auto setInitialConditions(const manif::SO3d::Tangent& velocity, const manif::SO3d::Tangent& acceleration) -> bool
Set the initial condition of the spline.
auto setFinalConditions(const manif::SO3d::Tangent& velocity, const manif::SO3d::Tangent& acceleration) -> bool
Set the final condition of the trajectory generator.
auto getOutput() const -> const SO3PlannerState& final
Get the state of the system.
auto isOutputValid() const -> bool final
Determines the validity of the object retrieved with get()
auto advance() -> bool final
Advance the internal state.

Function documentation

template<LieGroupTrivialization trivialization>
static manif::SO3d::Tangent BipedalLocomotion::Planners::SO3Planner<trivialization>::projectTangentVector(const manif::SO3d& initialRotation, const manif::SO3d& finalRotation, const manif::SO3d::Tangent& vector)

Project the tangent vector on the vector that represents the distance between the two rotations.

Parameters
initialRotation initial rotation ${}^{\mathcal{I}}R_{\mathcal{B}_0}$ .
finalRotation final rotation ${}^{\mathcal{I}}R_{\mathcal{B}_T}$ .
vector Tangent vector that needs to be projected.
Returns The projected tangent vector.

template<LieGroupTrivialization trivialization>
bool BipedalLocomotion::Planners::SO3Planner<trivialization>::setRotations(const manif::SO3d& initialRotation, const manif::SO3d& finalRotation, const std::chrono::nanoseconds& duration)

Set the rotation.

Parameters
initialRotation initial rotation ${}^{\mathcal{I}}R_{\mathcal{B}_0}$ .
finalRotation final rotation ${}^{\mathcal{I}}R_{\mathcal{B}_T}$ .
duration Trajectory duration in seconds.
Returns True in case of success/false otherwise.

template<LieGroupTrivialization trivialization>
bool BipedalLocomotion::Planners::SO3Planner<trivialization>::evaluatePoint(const std::chrono::nanoseconds& time, SO3PlannerState& state)

Get the trajectory at a given time.

Parameters
time time, in seconds, at which the trajectory should be computed.
state state of the planner.
Returns True in case of success/false otherwise.

template<LieGroupTrivialization trivialization> template<class Derived>
bool BipedalLocomotion::Planners::SO3Planner<trivialization>::evaluatePoint(const std::chrono::nanoseconds& time, manif::SO3d& rotation, manif::SO3TangentBase<Derived>& velocity, manif::SO3TangentBase<Derived>& acceleration)

Get the trajectory at a given time.

Parameters
time time, in seconds, at which the trajectory should be computed.
rotation ${}^{\mathcal{I}}R_{\mathcal{B}}$ .
velocity angular velocity expressed in inertial or body fixed frame (accordingly to the trivialization used).
acceleration angular acceleration expressed in inertial or body fixed frame (accordingly to the trivialization used).
Returns True in case of success/false otherwise.

template<LieGroupTrivialization trivialization>
bool BipedalLocomotion::Planners::SO3Planner<trivialization>::setAdvanceTimeStep(const std::chrono::nanoseconds& dt)

Set the time step of the advance interface.

Parameters
dt the time step of the advance block.
Returns True in case of success, false otherwise.

template<LieGroupTrivialization trivialization>
bool BipedalLocomotion::Planners::SO3Planner<trivialization>::setInitialConditions(const manif::SO3d::Tangent& velocity, const manif::SO3d::Tangent& acceleration)

Set the initial condition of the spline.

Parameters
velocity initial angular velocity.
acceleration initial angular acceleration.
Returns True in case of success, false otherwise.

template<LieGroupTrivialization trivialization>
bool BipedalLocomotion::Planners::SO3Planner<trivialization>::setFinalConditions(const manif::SO3d::Tangent& velocity, const manif::SO3d::Tangent& acceleration)

Set the final condition of the trajectory generator.

Parameters
velocity final angular velocity.
acceleration final angular acceleration.
Returns True in case of success, false otherwise.

template<LieGroupTrivialization trivialization>
const SO3PlannerState& BipedalLocomotion::Planners::SO3Planner<trivialization>::getOutput() const final

Get the state of the system.

Returns a const reference of the requested object.

template<LieGroupTrivialization trivialization>
bool BipedalLocomotion::Planners::SO3Planner<trivialization>::isOutputValid() const final

Determines the validity of the object retrieved with get()

Returns True if the object is valid, false otherwise.

template<LieGroupTrivialization trivialization>
bool BipedalLocomotion::Planners::SO3Planner<trivialization>::advance() final

Advance the internal state.

Returns True if the advance is successfull.

This may change the value retrievable from get().