From c54d1d92591a853b4a70310f7ea9cc74275155f8 Mon Sep 17 00:00:00 2001 From: Ydr Date: Fri, 17 Jan 2020 13:48:25 +0800 Subject: [PATCH] =?UTF-8?q?=E5=88=A9=E7=94=A8=E5=9B=9B=E5=85=83=E6=95=B0?= =?UTF-8?q?=E5=AE=9E=E7=8E=B0=E4=B8=89=E7=BB=B4=E9=87=8C=E7=A8=8B=E7=9A=84?= =?UTF-8?q?=E5=8A=A0=E5=87=8F=EF=BC=8C=E5=B9=B6=E5=9C=A8=20xy=20=E9=9D=A2?= =?UTF-8?q?=E4=B8=8A=E6=B5=8B=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../geometry/transformation/Builder.kt | 9 +++-- .../geometry/transformation/Pose3D.kt | 18 ++++++++- .../geometry/transformation/Quaternion.kt | 3 ++ .../geometry/transformation/TestPose2D.kt | 3 +- .../geometry/transformation/TestPose3D.kt | 40 +++++++++++++++++++ 5 files changed, 67 insertions(+), 6 deletions(-) create mode 100644 src/test/kotlin/org/mechdancer/geometry/transformation/TestPose3D.kt diff --git a/src/main/kotlin/org/mechdancer/geometry/transformation/Builder.kt b/src/main/kotlin/org/mechdancer/geometry/transformation/Builder.kt index ad5b44e..eb949dd 100644 --- a/src/main/kotlin/org/mechdancer/geometry/transformation/Builder.kt +++ b/src/main/kotlin/org/mechdancer/geometry/transformation/Builder.kt @@ -9,12 +9,15 @@ import org.mechdancer.geometry.angle.toVector import kotlin.math.cos import kotlin.math.sin -fun pose(x: Number = 0, y: Number = 0, theta: Number = 0) = +fun pose2D(x: Number = 0, y: Number = 0, theta: Number = 0) = Pose2D(vector2DOf(x, y), theta.toRad()) fun odometry(x: Number = 0, y: Number = 0, theta: Number = 0) = Pose2D(vector2DOf(x, y), theta.toRad()) +fun pose3D(x: Number = 0, y: Number = 0, z: Number = 0, d: Vector3D = vector3DOfZero()) = + Pose3D(vector3DOf(x, y, z), d) + fun quaternion(a: Number = 0, b: Number = 0, c: Number = 0, d: Number = 0) = Quaternion(a.toDouble(), b.toDouble(), c.toDouble(), d.toDouble()) @@ -71,6 +74,6 @@ fun Pose3D.toTransformation(): Transformation { }) } -fun Pose2D.toPose3D(): Pose3D = +fun Pose2D.to3D(): Pose3D = Pose3D(p = vector3DOf(p.x, p.y, 0), - d = vector3DOf(0, 0, d.asRadian())) + d = vector3DOf(0, 0, d.asRadian() / 2)) diff --git a/src/main/kotlin/org/mechdancer/geometry/transformation/Pose3D.kt b/src/main/kotlin/org/mechdancer/geometry/transformation/Pose3D.kt index 0b67018..3b90c51 100644 --- a/src/main/kotlin/org/mechdancer/geometry/transformation/Pose3D.kt +++ b/src/main/kotlin/org/mechdancer/geometry/transformation/Pose3D.kt @@ -38,7 +38,23 @@ data class Pose3D( val (v0, q0) = toQuaternions() val (v1, q1) = delta.toQuaternions() return pose3D(v0 + q0 * v1 * q0.conjugate, - q0 * q1) + q1 * q0) + } + + /** 里程回滚到增量 [delta] 之前 */ + infix fun minusDelta(delta: Pose3D): Pose3D { + val (v0, q0) = toQuaternions() + val (v1, q1) = delta.toQuaternions() + val qi = q1.conjugate * q0 + return pose3D(v0 - qi * v1 * qi.conjugate, qi) + } + + /** 计算里程从标记 [mark] 到当前状态的增量 */ + infix fun minusState(mark: Pose3D): Pose3D { + val (v0, q0) = mark.toQuaternions() + val (v1, q1) = toQuaternions() + return pose3D(q0.conjugate * (v1 - v0) * q0, + q1 * q0.conjugate) } override fun toString() = "p = ${p.simpleString()}, u = ${u.simpleString()}, θ = $theta" diff --git a/src/main/kotlin/org/mechdancer/geometry/transformation/Quaternion.kt b/src/main/kotlin/org/mechdancer/geometry/transformation/Quaternion.kt index 337dbb3..85c8a73 100644 --- a/src/main/kotlin/org/mechdancer/geometry/transformation/Quaternion.kt +++ b/src/main/kotlin/org/mechdancer/geometry/transformation/Quaternion.kt @@ -28,6 +28,9 @@ data class Quaternion( /** 求逆 */ val inverse get() = conjugate / square + operator fun unaryMinus() = + Quaternion(-a, -b, -c, -d) + operator fun plus(others: Quaternion) = Quaternion(a + others.a, b + others.b, diff --git a/src/test/kotlin/org/mechdancer/geometry/transformation/TestPose2D.kt b/src/test/kotlin/org/mechdancer/geometry/transformation/TestPose2D.kt index 66caf85..eb01d49 100644 --- a/src/test/kotlin/org/mechdancer/geometry/transformation/TestPose2D.kt +++ b/src/test/kotlin/org/mechdancer/geometry/transformation/TestPose2D.kt @@ -4,7 +4,6 @@ import org.junit.Test import org.mechdancer.algebra.implement.vector.vector2DOf import org.mechdancer.algebra.implement.vector.vector2DOfZero import org.mechdancer.geometry.angle.toRad -import org.mechdancer.geometry.transformation.Pose2D.Companion.pose import kotlin.math.PI class TestPose2D { @@ -13,7 +12,7 @@ class TestPose2D { */ @Test fun test() { - val step0 = pose() + val step0 = pose2D() assert(step0 == Pose2D(vector2DOfZero(), .0.toRad())) { "里程计初始化错误：$step0" } diff --git a/src/test/kotlin/org/mechdancer/geometry/transformation/TestPose3D.kt b/src/test/kotlin/org/mechdancer/geometry/transformation/TestPose3D.kt new file mode 100644 index 0000000..d75142d --- /dev/null +++ b/src/test/kotlin/org/mechdancer/geometry/transformation/TestPose3D.kt @@ -0,0 +1,40 @@ +package org.mechdancer.geometry.transformation + +import org.junit.Test +import org.mechdancer.algebra.implement.vector.vector2DOf +import org.mechdancer.algebra.implement.vector.vector3DOfZero +import org.mechdancer.geometry.angle.toRad +import kotlin.math.PI + +class TestPose3D { + /** + * 测试里程计算法 + */ + @Test + fun test() { + val step0 = pose3D() + assert(step0 == Pose3D(vector3DOfZero(), vector3DOfZero())) { + "里程计初始化错误：$step0" + } + + val delta1 = Pose2D(vector2DOf(3.0, 4.0), (PI / 2).toRad()).to3D() + + val step1 = step0 plusDelta delta1 + assert(step1 == delta1) { + "里程计累加错误：$step1 ≠ $delta1" + } + + val step2 = step1 minusDelta delta1 + assert(step2 == step0) { + "里程计回滚错误：$step2 ≠ $step0" + } + + val delta2 = Pose2D(vector2DOf(1.0, 1.0), (-PI / 4).toRad()).to3D() + + val step3 = step2 plusDelta delta1 + val step4 = step3 plusDelta delta2 minusState step3 + assert(step4 == delta2) { + "里程计标记错误：$step4 ≠ $delta2" + } + } +}