We will use the geometry
library to do operations with coordinate frames,
and the Numpy library. We will abbreviate them as g
and np
, respectively.
import geometry as g
import numpy as np
In code, angles are always represented in radians.
The only place where degrees might be acceptable is in configuration files that need to be edited by users.
Use the functions np.deg2rad
and np.rad2deg
to convert back and forth.
assert np.allclose(np.deg2rad(60), np.pi/3)
assert np.allclose(np.rad2deg(np.pi/3), 60)
The function np.allclose
allows to compare two numbers allowing some tolerance to
tolerate numerical errors.
To create a pose from translation, angle (in radians), use the function SE2_from_translation_angle(translation, angle)
:
translation = [0, 1]
angle = np.pi/3
q = g.SE2_from_translation_angle(translation, angle)
This returns a 3 by 3 matrix:
print(q)
Use the function g.SE2.friendly()
to print a nice representation of the matrix:
print(g.SE2.friendly(q))
To convert back to position, angle use the function translation_angle_from_SE2
position, direction = g.translation_angle_from_SE2(q)
print(position)
print(direction)
The object geometry.SE2
is a representation of the group SE(2).
It provides the operations multiply
, identity
, and inverse
:
q1 = g.SE2_from_translation_angle([1, 2], np.deg2rad(15))
q2 = g.SE2_from_translation_angle([2, 3], np.deg2rad(30))
# let's compute the relative pose of q2 wrt q1
q2_from_q1 = g.SE2.multiply( g.SE2.inverse(q1), q2)
# now let's re-find q2 by adding the relative pose to q1
q2b = g.SE2.multiply(q1, q2_from_q1)
# this must be equal to q2
assert np.allclose(q2, q2b)
Suppose you want to interpolate between two poses. This is the right way to do it.
First, compute the relative pose:
q2_from_q1 = g.SE2.multiply( g.SE2.inverse(q1), q2)
Now compute the equivalent "velocity".
The function algebra_from_group
computes
the element of the algebra of SE(2); that is, skew symmetric matrices that
represent velocities:
vel = g.SE2.algebra_from_group(q2_from_q1)
print(vel)
The set of velocities on $SE(2)$ is called the lie algebra $se(2)$.
It is represented by the object se2
:
g.se2.belongs(vel)
print(g.se2.friendly(vel))
The velocity can be converted to angular, linear components:
linear, angular = g.linear_angular_from_se2(vel)
print('linear: {}'.format(linear))
print('angular: {}'.format(angular))
Now, suppose that you want to interpolate smoothly according to a parameter alpha
,
such that alpha = 0
gives q1
and alpha = 1
gives q2
.
You can do that by interpolating linearly in the velocity space:
alpha = 0.5
rel = g.SE2.group_from_algebra(vel * alpha)
q_alpha = g.SE2.multiply( q1, rel)
print(g.SE2.friendly(q_alpha))
def relative_pose(q0, q1):
return g.SE2.multiply(g.SE2.inverse(q0), q1)
def interpolate(q0, q1, alpha):
q1_from_q0 = relative_pose(q0, q1)
vel = g.SE2.algebra_from_group(q1_from_q0)
rel = g.SE2.group_from_algebra(vel * alpha)
q = g.SE2.multiply(q0, rel)
return q
# sample two poses
q0 = g.SE2.sample_uniform()
q1 = g.SE2.sample_uniform()
# make sure that for alpha=0,1 we get q0,q1
assert np.allclose(q0, interpolate(q0, q1, 0))
assert np.allclose(q1, interpolate(q0, q1, 1))
We stand back to back.
I walk 3 meters forward; I rotate 20 deg to my right; I walk 7 meters forward.
You walk 2 meters forward, you rotate 30 deg to your left, you go 6 meters forward.
What is the distance between us?
At what angle I see you on my field of view? (0=front, pi/2 = left, -pi/2 = right, -pi=pi back)