Skip to content

Commit

Permalink
Added support for SE3 in terms of quaternions with Exp map
Browse files Browse the repository at this point in the history
  • Loading branch information
@taylorpool
taylorpool committed Apr 12, 2023
1 parent 3632576 commit daf5336
Show file tree
Hide file tree
Showing 6 changed files with 231 additions and 1 deletion.
1 change: 1 addition & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,3 @@
build/
.cache/
*/.ipynb_checkpoints/
11 changes: 10 additions & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,15 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_STANDARD 20)

include(FetchContent)

FetchContent_Declare(
Sabai
GIT_REPOSITORY [email protected]:taylorpool/sabai.git
GIT_TAG constexpr
)
FetchContent_MakeAvailable(Sabai)

add_library(onogawa
src/onogawa.cpp
)
Expand All @@ -13,7 +22,7 @@ target_include_directories(onogawa
include
)
target_link_libraries(onogawa
PUBLIC
sabai
)

option(BUILD_Onogawa_TESTS "Build Onogawa Tests" OFF)
Expand Down
20 changes: 20 additions & 0 deletions include/onogawa/pose3.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

#include "onogawa/point3.hpp"
#include "onogawa/quaternion.hpp"
#include "sabai/sabai.hpp"

namespace onogawa {

Expand All @@ -21,4 +22,23 @@ constexpr Pose3<T> operator*(const Pose3<T> &a, const Pose3<T> &b) {

using Pose3d = Pose3<double>;

template <typename T>
constexpr Pose3<T> Exp(const sabai::StaticVector<T, 3> &angularVelocity,
const sabai::StaticVector<T, 3> &linearVelocity) {
const T theta;
if (sabai::norm(angularVelocity) == 0.0) {

theta = sabai::norm(linearVelocity);
} else {
theta = sabai::norm(angularVelocity);
}

const sabai::StaticVector<T, 3> hatv;
const sabai::StaticVector<T, 3> hathatv;

return Pose3<T>(Exp(angularVelocity),
theta * linearVelocity + (1.0 - std::cos(theta)) * hatv +
(theta - std::sin(theta) * hathatv));
}

} // namespace onogawa
20 changes: 20 additions & 0 deletions include/onogawa/quaternion.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
#pragma once

#include "sabai/sabai.hpp"

namespace onogawa {

template <typename T> struct Quaternion {
Expand Down Expand Up @@ -27,4 +29,22 @@ constexpr Quaternion<T> operator*(const Quaternion<T> &a,

using Quaterniond = Quaternion<double>;

template <typename T>
constexpr Quaternion<T> Exp(const sabai::StaticVector<T, 3> &omega) {
const auto theta = sabai::norm(omega);
const auto omegaNormalized = omega / theta;
const auto thetaHalf = theta / 2.0;
const auto sinThetaHalf = std::sin(thetaHalf);
return Quaternion<T>(std::cos(thetaHalf), sinThetaHalf * omegaNormalized(0),
sinThetaHalf * omegaNormalized(1),
sinThetaHalf * omegaNormalized(2));
}

template <typename T>
constexpr sabai::StaticVector<T, 3> Log(const Quaternion<T> &quaternion) {
const auto thetaHalf = std::acos(quaternion.w);
const auto sinThetaHalf = std::sin(thetaHalf);
const auto temp = 2.0 * thetaHalf / sinThetaHalf;
return {temp * quaternion.x, temp * quaternion.y, temp * quaternion.z};
}
} // namespace onogawa
171 changes: 171 additions & 0 deletions notebooks/quaternion.ipynb
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,171 @@
{
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"id": "f47e81ef-a415-4f7e-9897-407c6f9ef0f0",
"metadata": {},
"outputs": [],
"source": [
"import sympy as sy"
]
},
{
"cell_type": "code",
"execution_count": 2,
"id": "7623509f-fccb-4a91-ae2f-6c6b57538268",
"metadata": {},
"outputs": [],
"source": [
"wx, wy, wz, theta, k = sy.symbols('omega_x omega_y omega_z theta k')"
]
},
{
"cell_type": "code",
"execution_count": 4,
"id": "01f138f4-cf23-4267-bda9-c05d819c370c",
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}0 & - \\omega_{x} & - \\omega_{y} & - \\omega_{z}\\\\\\omega_{x} & 0 & \\omega_{z} & - \\omega_{y}\\\\\\omega_{y} & - \\omega_{z} & 0 & \\omega_{x}\\\\\\omega_{z} & \\omega_{y} & - \\omega_{x} & 0\\end{matrix}\\right]$"
],
"text/plain": [
"Matrix([\n",
"[ 0, -omega_x, -omega_y, -omega_z],\n",
"[omega_x, 0, omega_z, -omega_y],\n",
"[omega_y, -omega_z, 0, omega_x],\n",
"[omega_z, omega_y, -omega_x, 0]])"
]
},
"execution_count": 4,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"what = sy.Matrix([\n",
" [0, -wx, -wy, -wz],\n",
" [wx, 0, wz, -wy],\n",
" [wy, -wz, 0, wx],\n",
" [wz, wy, -wx, 0]\n",
"])\n",
"what"
]
},
{
"cell_type": "code",
"execution_count": 6,
"id": "1eac4770-a0c0-4ba4-90dd-c960e4f4a6e7",
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2} & 0 & 0 & 0\\\\0 & - \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2} & 0 & 0\\\\0 & 0 & - \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2} & 0\\\\0 & 0 & 0 & - \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\end{matrix}\\right]$"
],
"text/plain": [
"Matrix([\n",
"[-omega_x**2 - omega_y**2 - omega_z**2, 0, 0, 0],\n",
"[ 0, -omega_x**2 - omega_y**2 - omega_z**2, 0, 0],\n",
"[ 0, 0, -omega_x**2 - omega_y**2 - omega_z**2, 0],\n",
"[ 0, 0, 0, -omega_x**2 - omega_y**2 - omega_z**2]])"
]
},
"execution_count": 6,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"what@what"
]
},
{
"cell_type": "code",
"execution_count": 7,
"id": "f731e190-ce6f-43b9-9aeb-e09fcdf75237",
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}0 & - \\omega_{x} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) & - \\omega_{y} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) & - \\omega_{z} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right)\\\\\\omega_{x} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) & 0 & \\omega_{z} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) & - \\omega_{y} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right)\\\\\\omega_{y} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) & - \\omega_{z} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) & 0 & \\omega_{x} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right)\\\\\\omega_{z} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) & \\omega_{y} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) & - \\omega_{x} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) & 0\\end{matrix}\\right]$"
],
"text/plain": [
"Matrix([\n",
"[ 0, -omega_x*(-omega_x**2 - omega_y**2 - omega_z**2), -omega_y*(-omega_x**2 - omega_y**2 - omega_z**2), -omega_z*(-omega_x**2 - omega_y**2 - omega_z**2)],\n",
"[omega_x*(-omega_x**2 - omega_y**2 - omega_z**2), 0, omega_z*(-omega_x**2 - omega_y**2 - omega_z**2), -omega_y*(-omega_x**2 - omega_y**2 - omega_z**2)],\n",
"[omega_y*(-omega_x**2 - omega_y**2 - omega_z**2), -omega_z*(-omega_x**2 - omega_y**2 - omega_z**2), 0, omega_x*(-omega_x**2 - omega_y**2 - omega_z**2)],\n",
"[omega_z*(-omega_x**2 - omega_y**2 - omega_z**2), omega_y*(-omega_x**2 - omega_y**2 - omega_z**2), -omega_x*(-omega_x**2 - omega_y**2 - omega_z**2), 0]])"
]
},
"execution_count": 7,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"what@what@what"
]
},
{
"cell_type": "code",
"execution_count": 8,
"id": "44b5c045-b7dd-4284-89dd-d0d9d32b6519",
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
"$\\displaystyle \\left[\\begin{matrix}- \\omega_{x}^{2} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) - \\omega_{y}^{2} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) - \\omega_{z}^{2} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) & 0 & 0 & 0\\\\0 & - \\omega_{x}^{2} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) - \\omega_{y}^{2} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) - \\omega_{z}^{2} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) & 0 & 0\\\\0 & 0 & - \\omega_{x}^{2} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) - \\omega_{y}^{2} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) - \\omega_{z}^{2} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) & 0\\\\0 & 0 & 0 & - \\omega_{x}^{2} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) - \\omega_{y}^{2} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right) - \\omega_{z}^{2} \\left(- \\omega_{x}^{2} - \\omega_{y}^{2} - \\omega_{z}^{2}\\right)\\end{matrix}\\right]$"
],
"text/plain": [
"Matrix([\n",
"[-omega_x**2*(-omega_x**2 - omega_y**2 - omega_z**2) - omega_y**2*(-omega_x**2 - omega_y**2 - omega_z**2) - omega_z**2*(-omega_x**2 - omega_y**2 - omega_z**2), 0, 0, 0],\n",
"[ 0, -omega_x**2*(-omega_x**2 - omega_y**2 - omega_z**2) - omega_y**2*(-omega_x**2 - omega_y**2 - omega_z**2) - omega_z**2*(-omega_x**2 - omega_y**2 - omega_z**2), 0, 0],\n",
"[ 0, 0, -omega_x**2*(-omega_x**2 - omega_y**2 - omega_z**2) - omega_y**2*(-omega_x**2 - omega_y**2 - omega_z**2) - omega_z**2*(-omega_x**2 - omega_y**2 - omega_z**2), 0],\n",
"[ 0, 0, 0, -omega_x**2*(-omega_x**2 - omega_y**2 - omega_z**2) - omega_y**2*(-omega_x**2 - omega_y**2 - omega_z**2) - omega_z**2*(-omega_x**2 - omega_y**2 - omega_z**2)]])"
]
},
"execution_count": 8,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"what@what@what@what"
]
},
{
"cell_type": "code",
"execution_count": null,
"id": "4bc41fef-2aad-44a0-a334-5e4f7768093d",
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3 (ipykernel)",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.8.10"
}
},
"nbformat": 4,
"nbformat_minor": 5
}
9 changes: 9 additions & 0 deletions test/test_quaternion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -387,4 +387,13 @@ TEST_F(QuaternionMultiplicationEzEz_NegEw, CheckY) {
}
TEST_F(QuaternionMultiplicationEzEz_NegEw, CheckZ) {
ASSERT_NEAR(c.z, -ew.z, 1e-14);
}

TEST(Quaternion, Exponential) {
sabai::StaticVectord<3> omega({2 * M_PI, 0, 0});
const auto q = onogawa::Exp(omega);
std::cout << q.w << "," << q.x << "," << q.y << "," << q.z << std::endl;
const auto w = onogawa::Log(q);
std::cout << w(0) << "," << w(1) << "," << w(2) << std::endl;
ASSERT_NEAR(q.w, 1.0, 1e-14);
}

0 comments on commit daf5336

Please sign in to comment.