navlie.lib.states.SE3State¶

class navlie.lib.states.SE3State(value: ndarray, stamp: float | None = None, state_id=None, direction='right')¶

Bases: MatrixLieGroupState

A state object for 3D rigid body transformations. The value of this state is stored as a 4x4 numpy array representing a direct element of the SE3 group. I.e.,

\[\mathbf{T} = egin{bmatrix} \mathbf{C} & \mathbf{r} \ \mathbf{0} & 1 \end{bmatrix}, \quad \mathbf{C} \in SO(3), \quad \mathbf{r} \in \mathbb{R}^3.\]

Parameters:

value (np.ndarray) – Value of of the state. If the value has as many elements as the DOF of the group, then it is assumed to be a vector of exponential coordinates. Otherwise, the value must be a 2D numpy array representing a direct element of the group in matrix form.
group (MatrixLieGroup) – A pymlg.MatrixLieGroup class, such as pymlg.SE2 or pymlg.SO3.
stamp (float, optional) – timestamp, by default None
state_id (Any, optional) – optional state ID, by default None
direction (str, optional) –
either “left” or “right”, by default “right”. Defines the perturbation \(\delta \mathbf{x}\) as either

\[ \begin{align}\begin{aligned}\mathbf{X} = \mathbf{X} \exp(\delta \mathbf{x}^\wedge) ext{ (right)}\\\mathbf{X} = \exp(\delta \mathbf{x}^\wedge) \mathbf{X} ext{ (left)}\end{aligned}\end{align} \]

group¶

property attitude: ndarray¶

property position: ndarray¶

static jacobian_from_blocks(attitude: ndarray | None = None, position: ndarray | None = None)¶

static from_ros(msg: PoseStamped, state_id: Any = None, direction='right') → SE3State¶

Convert a ROS PoseStamped message to a SE3State.

Parameters:

msg (PoseStamped) – ROS PoseStamped message
state_id (Any, optional) – If not provided, the frame_id of the message will be used
direction (str, optional) – perturbation direction, by default “right”

Returns:

a new instance of SE3State

Return type:

SE3State

to_ros(frame_id: str = None) → PoseStamped¶

Convert a SE3State to a ROS PoseStamped message.

Parameters:: frame_id (str, optional) – If not provided, the state_id will be used.
Returns:: ROS PoseStamped message
Return type:: PoseStamped

static random(stamp: float | None = None, state_id=None, direction='right')¶

direction¶

copy() → MatrixLieGroupState¶: Returns a copy of this State instance.

dof¶

Degree of freedom of the state

Type:: int

dot(other: MatrixLieGroupState) → MatrixLieGroupState¶

minus(x: MatrixLieGroupState) → ndarray¶: A generic “subtraction” operation given another State object of the same type, always returning a numpy array.

minus_jacobian(x: MatrixLieGroupState) → ndarray¶

Jacobian of the minus operator with respect to self.

\[\mathbf{J} = \frac{D (\mathcal{Y} \ominus \mathcal{X})}{D \mathcal{Y}}\]

That is, if dx = y.minus(x) then this is the Jacobian of dx with respect to y. For Lie groups, this is the inverse of the group Jacobian evaluated at dx = x1.minus(x2).

minus_jacobian_fd(x: State, step_size=1e-08) → ndarray¶: Calculates the minus jacobian with finite difference.

plus(dx: ndarray) → MatrixLieGroupState¶: A generic “addition” operation given a dx numpy array with as many elements as the dof of this state.

plus_jacobian(dx: ndarray) → ndarray¶

Jacobian of the plus operator. That is, using Lie derivative notation,

\[\mathbf{J} = \frac{D (\mathcal{X} \oplus \delta \mathbf{x})}{D \delta \mathbf{x}}\]

For Lie groups, this is known as the group Jacobian.

plus_jacobian_fd(dx, step_size=1e-08) → ndarray¶: Calculates the plus jacobian with finite difference.

stamp¶

Timestamp

Type:: float

state_id¶

value: ndarray¶

State value

Type:: Any

property velocity: ndarray¶

navlie.lib.states.SE2State

navlie.lib.states.SL3State