From 99acfc1783e07f60822e2e93883e1bc9c5dc3035 Mon Sep 17 00:00:00 2001
From: ottmar-zittlau <74417452+ottmar-zittlau@users.noreply.github.com>
Date: Thu, 24 Oct 2024 15:13:18 +0200
Subject: [PATCH] fix: Unwrapped rotation angles to avoid sign flips (#23)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Co-authored-by: Torsten Knüppel <torsten.knueppel@wandelbots.com>
---
 src/lib/MotionStreamConnection.ts | 58 +++++++++++++++++++++++++++++--
 1 file changed, 56 insertions(+), 2 deletions(-)

diff --git a/src/lib/MotionStreamConnection.ts b/src/lib/MotionStreamConnection.ts
index d76e619..4489745 100644
--- a/src/lib/MotionStreamConnection.ts
+++ b/src/lib/MotionStreamConnection.ts
@@ -2,8 +2,10 @@ import type {
   ControllerInstance,
   MotionGroupPhysical,
   MotionGroupStateResponse,
+  Vector3d,
 } from "@wandelbots/wandelbots-api-client"
 import { makeAutoObservable, runInAction } from "mobx"
+import { Vector3 } from "three"
 import type { NovaClient } from "../NovaClient"
 import type { AutoReconnectingWebsocket } from "./AutoReconnectingWebsocket"
 import { tryParseJson } from "./converters"
@@ -11,6 +13,45 @@ import { jointValuesEqual, tcpPoseEqual } from "./motionStateUpdate"
 
 const MOTION_DELTA_THRESHOLD = 0.0001
 
+function unwrapRotationVector(
+  newRotationVectorApi: Vector3d,
+  currentRotationVectorApi: Vector3d,
+): Vector3d {
+  const currentRotationVector = new Vector3(
+    currentRotationVectorApi.x,
+    currentRotationVectorApi.y,
+    currentRotationVectorApi.z,
+  )
+
+  const newRotationVector = new Vector3(
+    newRotationVectorApi.x,
+    newRotationVectorApi.y,
+    newRotationVectorApi.z,
+  )
+
+  const currentAngle = currentRotationVector.length()
+  const currentAxis = currentRotationVector.normalize()
+
+  let newAngle = newRotationVector.length()
+  let newAxis = newRotationVector.normalize()
+
+  // Align rotation axes
+  if (newAxis.dot(currentAxis) < 0) {
+    newAngle = -newAngle
+    newAxis = newAxis.multiplyScalar(-1.0)
+  }
+
+  // Shift rotation angle close to previous one to extend domain of rotation angles beyond [0, pi]
+  // - this simplifies interpolation and prevents abruptly changing signs of the rotation angles
+  let angleDifference = newAngle - currentAngle
+  angleDifference -=
+    2.0 * Math.PI * Math.floor((angleDifference + Math.PI) / (2.0 * Math.PI))
+
+  newAngle = currentAngle + angleDifference
+
+  return newAxis.multiplyScalar(newAngle)
+}
+
 /**
  * Store representing the current state of a connected motion group.
  */
@@ -109,8 +150,21 @@ export class MotionStreamConnection {
         )
       ) {
         runInAction(() => {
-          this.rapidlyChangingMotionState.tcp_pose =
-            motionStateResponse.tcp_pose
+          if (this.rapidlyChangingMotionState.tcp_pose == undefined) {
+            this.rapidlyChangingMotionState.tcp_pose =
+              motionStateResponse.tcp_pose
+          } else {
+            this.rapidlyChangingMotionState.tcp_pose = {
+              position: motionStateResponse.tcp_pose!.position,
+              orientation: unwrapRotationVector(
+                motionStateResponse.tcp_pose!.orientation,
+                this.rapidlyChangingMotionState.tcp_pose!.orientation,
+              ),
+              tcp: motionStateResponse.tcp_pose!.tcp,
+              coordinate_system:
+                motionStateResponse.tcp_pose!.coordinate_system,
+            }
+          }
         })
       }
     })