diff --git a/CMakeLists.txt b/CMakeLists.txt index 8f098f40a..48aa872db 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -361,6 +361,7 @@ set(VIAMCPPSDK_GRPC_VERSION_MINIMUM 1.30.2) set(VIAMCPPSDK_PROTOBUF_VERSION_MINIMUM 3.12.4) set(VIAMCPPSDK_XTL_VERSION_MINIMUM 0.7.2) set(VIAMCPPSDK_XTENSOR_VERSION_MINIMUM 0.24.3) +set(VIAMCPPSDK_EIGEN_VERSION_MINIMUM 3.3) # Time to find `BOOST`. if (VIAMCPPSDK_BUILD_TESTS) @@ -503,6 +504,8 @@ FetchContent_Declare( FetchContent_MakeAvailable(xtl xtensor) +find_package(Eigen3 ${VIAMCPPSDK_EIGEN_VERSION_MINIMUM} CONFIG REQUIRED) + # Pull in our subdirectories add_subdirectory(src/viam) diff --git a/conanfile.py b/conanfile.py index 0849616db..4dfd9f547 100644 --- a/conanfile.py +++ b/conanfile.py @@ -70,6 +70,7 @@ def requirements(self): self.requires(self._grpc_requires()) self.requires('protobuf/[>=3.17.1 <6.30.0]') self.requires(self._xtensor_requires(), transitive_headers=True) + self.requires('eigen/[>=3.3.0]') def build_requirements(self): if self.options.offline_proto_generation: diff --git a/src/viam/sdk/CMakeLists.txt b/src/viam/sdk/CMakeLists.txt index b5b129449..97bc7fb82 100644 --- a/src/viam/sdk/CMakeLists.txt +++ b/src/viam/sdk/CMakeLists.txt @@ -136,6 +136,7 @@ target_sources(viamsdk module/service.cpp module/private/data_consumer_query.cpp referenceframe/frame.cpp + referenceframe/urdf_to_dh_params.cpp registry/registry.cpp resource/reconfigurable.cpp resource/resource.cpp @@ -217,6 +218,7 @@ target_sources(viamsdk ../../viam/sdk/module/service.hpp ../../viam/sdk/module/signal_manager.hpp ../../viam/sdk/referenceframe/frame.hpp + ../../viam/sdk/referenceframe/urdf_to_dh_params.hpp ../../viam/sdk/registry/registry.hpp ../../viam/sdk/resource/reconfigurable.hpp ../../viam/sdk/resource/resource.hpp @@ -306,6 +308,7 @@ target_link_libraries(viamsdk PRIVATE ${VIAMCPPSDK_GRPC_LIBRARIES} PRIVATE ${VIAMCPPSDK_PROTOBUF_LIBRARIES} PRIVATE Threads::Threads + PRIVATE Eigen3::Eigen ) # if the `viam_rust_utils` target exists then we should use it. If not then diff --git a/src/viam/sdk/referenceframe/latex/dh.tex b/src/viam/sdk/referenceframe/latex/dh.tex new file mode 100644 index 000000000..944d9fc4d --- /dev/null +++ b/src/viam/sdk/referenceframe/latex/dh.tex @@ -0,0 +1,538 @@ +\documentclass[11pt]{article} + +\usepackage[margin=1in]{geometry} +\usepackage{amsmath, amssymb} +\usepackage{bm} +\usepackage{xcolor} +\usepackage{hyperref} +\usepackage{mdframed} +\usepackage{booktabs} + +\hypersetup{ + colorlinks=true, + linkcolor=blue!60!black, + urlcolor=blue!60!black, +} + +\newcommand{\vect}[1]{\bm{#1}} +\newcommand{\unit}[1]{\hat{\bm{#1}}} +\newcommand{\norm}[1]{\left\|#1\right\|} +\newcommand{\atantwo}{\operatorname{atan2}} + +\newmdenv[ + linecolor=blue!50!black, + linewidth=0.5pt, + backgroundcolor=blue!3, + roundcorner=4pt, + skipabove=8pt, + skipbelow=8pt, +]{keybox} + +\newmdenv[ + linecolor=orange!70!black, + linewidth=0.5pt, + backgroundcolor=orange!5, + roundcorner=4pt, + skipabove=8pt, + skipbelow=8pt, +]{warnbox} + +\title{Computing Classical DH Parameters from a URDF} +\author{} +\date{} + +\begin{document} +\maketitle + +\section{Overview} + +This document describes how to compute the classical (Spong) Denavit--Hartenberg +parameters $(a_i, \alpha_i, d_i, \theta_i)$ for each joint $i$ of a serial +manipulator, starting from a URDF file. + +Each joint's DH row describes the homogeneous transform from frame $i{-}1$ to +frame $i$: +\[ + T_i^{i-1} = R_z(\theta_i)\,T_z(d_i)\,T_x(a_i)\,R_x(\alpha_i). +\] + +The four parameters are: +\begin{itemize} + \item $a_i$: link length --- distance between consecutive joint axes along the + common normal. + \item $\alpha_i$: link twist --- signed angle between consecutive joint + axes, measured about $\unit{x}_i$. + \item $d_i$: joint offset --- signed distance along $\unit{z}_{i-1}$ from the + previous frame origin to frame $i$'s origin. + \item $\theta_i$: joint angle offset --- signed rotation about + $\unit{z}_{i-1}$ that aligns $\unit{x}_{i-1}$ with $\unit{x}_i$. For + revolute joints, the runtime angle is $\theta_i + q_i$. +\end{itemize} + +\begin{warnbox} +\textbf{Notation convention.} To avoid a common source of confusion, we use +two distinct symbols throughout this document: +\begin{itemize} + \item $\unit{a}_i$ denotes the direction of joint $i$'s axis as extracted + from the URDF (in the base frame). + \item $\unit{z}_i$ denotes the $z$-axis of DH frame $i$. +\end{itemize} +In classical DH, frame $i$ has its $z$-axis along \emph{joint $i{+}1$'s} axis, +so $\unit{z}_i = \unit{a}_{i+1}$. To compute joint $i$'s DH row, we need the +geometry between axes $i$ and $i{+}1$ --- this is the single most common +source of derivation errors. +\end{warnbox} + +\section{Step 1: Extract Axis Data from the URDF} + +To compute the DH row for joint $i$, we need four values, all expressed in +the base frame: $\vect{p}_i$, $\unit{a}_i$, $\vect{p}_{i+1}$, $\unit{a}_{i+1}$. + +As we walk the kinematic chain, we maintain two running quantities: +\begin{itemize} + \item $R_{\mathrm{cum}}$ --- cumulative rotation from base to the current + link's frame. + \item $\vect{p}_{\mathrm{cum}}$ --- cumulative position of the current + link's origin, in the base frame. +\end{itemize} +Initial values (at the base): $R_{\mathrm{cum}} = I$, +$\vect{p}_{\mathrm{cum}} = (0, 0, 0)$. + +Suppose the URDF gives us, for each joint $i$: +\begin{itemize} + \item \texttt{rpy} $= (r_i, \beta_i, \gamma_i)$ --- roll, pitch, yaw angles + (URDF uses fixed-axis / extrinsic convention: + $R = R_z(\gamma)\,R_y(\beta)\,R_x(r)$). + \item \texttt{xyz} $= \vect{t}_i$ --- translation of the joint's origin in + the parent link's frame. + \item \texttt{axis} $= \unit{a}_i^{\mathrm{local}}$ --- unit direction of + the joint's rotation axis, expressed in the child link's frame. +\end{itemize} + +\paragraph{Computing $\vect{p}_1$ and $\unit{a}_1$.} Process joint 1 in +three substeps, in order: + +\textbf{(a)} Update the position: rotate the joint's translation by the +\emph{current} $R_{\mathrm{cum}}$ (i.e., the parent's rotation), then add: +\[ + \vect{p}_1 = \vect{p}_{\mathrm{cum}} + R_{\mathrm{cum}} \cdot \vect{t}_1 + = (0, 0, 0) + I \cdot \vect{t}_1 = \vect{t}_1. +\] + +\textbf{(b)} Update the cumulative rotation: +\[ + R_{\mathrm{cum}} \leftarrow R_{\mathrm{cum}} \cdot R(r_1, \beta_1, \gamma_1) + = I \cdot R(r_1, \beta_1, \gamma_1) + = R(r_1, \beta_1, \gamma_1). +\] + +\textbf{(c)} Compute the joint axis direction, using the \emph{updated} +$R_{\mathrm{cum}}$: +\[ + \unit{a}_1 = R_{\mathrm{cum}} \cdot \unit{a}_1^{\mathrm{local}}. +\] + +\paragraph{Computing $\vect{p}_2$ and $\unit{a}_2$.} After processing joint 1, +the running state is: +\[ + R_{\mathrm{cum}} = R(r_1, \beta_1, \gamma_1), \qquad + \vect{p}_{\mathrm{cum}} = \vect{t}_1. +\] +Apply the same three substeps to joint 2. + +\textbf{(a)} Update the position: rotate joint 2's translation by the +\emph{current} $R_{\mathrm{cum}}$ (i.e., joint 1's rotation, since joint 1's +child link is the parent of joint 2), then add: +\[ + \vect{p}_2 = \vect{p}_{\mathrm{cum}} + R_{\mathrm{cum}} \cdot \vect{t}_2 + = \vect{t}_1 + R(r_1, \beta_1, \gamma_1) \cdot \vect{t}_2. +\] +Unlike joint 1, this rotation is not guaranteed to be the identity, so the +components of $\vect{t}_2$ may mix into $\vect{p}_2$. + +\textbf{(b)} Update the cumulative rotation: +\[ + R_{\mathrm{cum}} \leftarrow R_{\mathrm{cum}} \cdot R(r_2, \beta_2, \gamma_2) + = R(r_1, \beta_1, \gamma_1) \cdot R(r_2, \beta_2, \gamma_2). +\] + +\textbf{(c)} Compute the joint axis direction, using the \emph{updated} +$R_{\mathrm{cum}}$: +\[ + \unit{a}_2 = R_{\mathrm{cum}} \cdot \unit{a}_2^{\mathrm{local}}. +\] + +\paragraph{Iterating.} Continue this three-substep process for each subsequent +joint along the kinematic chain. The output of Step 1 is a list of +$(\vect{p}_i, \unit{a}_i)$ pairs, one per joint, all in the base frame. + +\begin{keybox} +\textbf{The key rule.} At each joint, the translation $\vect{t}_i$ is rotated +by the \emph{parent's} cumulative rotation (because $\vect{t}_i$ is expressed +in the parent's frame), while the axis $\unit{a}_i^{\mathrm{local}}$ is +rotated by the \emph{child's} cumulative rotation (because the axis is +expressed in the child's frame, which includes the joint's own rpy). +\end{keybox} + +\section{Step 2: Choose Frame 0} + +Before computing any DH row, we establish frame 0 --- the base frame of the +DH chain. Frame 0 has two constraints and one free choice: +\begin{itemize} + \item $\unit{z}_0 = \unit{a}_1$ (required --- frame 0's $z$-axis must lie + along joint 1's axis). + \item $\vect{o}_0$ must lie on joint 1's axis. Standard choice: + $\vect{o}_0 = (0, 0, 0)$ if the URDF's base is already at the origin. + \item $\unit{x}_0$ is free, as long as it is perpendicular to $\unit{z}_0$. + Standard choice: project world $+x$ onto the plane perpendicular to + $\unit{z}_0$, then normalize. If $\unit{z}_0 = (0, 0, 1)$, this gives + $\unit{x}_0 = (1, 0, 0)$. +\end{itemize} +Complete the right-handed triad with $\unit{y}_0 = \unit{z}_0 \times \unit{x}_0$. + +\section{Step 3: Build Frame $i$ from the Common Normal} + +Frame $i$ is constructed from the common normal between axis $i$ and axis +$i{+}1$. We describe the process for joint 1 below; the procedure is +identical for each subsequent joint with indices shifted. + +\paragraph{Step 3a: Classify the axis pair.} Compute +\[ + \vect{c} = \unit{a}_1 \times \unit{a}_2, + \qquad \vect{d}_{\mathrm{pp}} = \vect{p}_2 - \vect{p}_1. +\] +\begin{itemize} + \item If $\norm{\vect{c}} > \epsilon$: axes are skew or intersecting + (Case~A / Case~B). + \item If $\norm{\vect{c}} \le \epsilon$: axes are parallel (Case~C). +\end{itemize} +Use $\epsilon \approx 10^{-6}$ in practice. + +\paragraph{Step 3b: Find the feet of the common normal.} + +\textbf{Cases A and B (non-parallel).} We require the connecting vector +$\vect{f}_2 - \vect{f}_1$ to be perpendicular to both axes. Writing +$\vect{f}_1 = \vect{p}_1 + s\,\unit{a}_1$ and +$\vect{f}_2 = \vect{p}_2 + t\,\unit{a}_2$, the two perpendicularity conditions +\[ + (\vect{f}_2 - \vect{f}_1) \cdot \unit{a}_1 = 0, \qquad + (\vect{f}_2 - \vect{f}_1) \cdot \unit{a}_2 = 0 +\] +give the $2 \times 2$ system: +\[ + \begin{bmatrix} 1 & -\unit{a}_1 \cdot \unit{a}_2 \\ + \unit{a}_1 \cdot \unit{a}_2 & -1 \end{bmatrix} + \begin{bmatrix} s \\ t \end{bmatrix} + = \begin{bmatrix} \vect{d}_{\mathrm{pp}} \cdot \unit{a}_1 \\ + \vect{d}_{\mathrm{pp}} \cdot \unit{a}_2 \end{bmatrix}. +\] +Solve for $s, t$, then +\[ + \vect{f}_1 = \vect{p}_1 + s\,\unit{a}_1, \qquad + \vect{f}_2 = \vect{p}_2 + t\,\unit{a}_2. +\] + +\textbf{Case C (parallel).} The common normal is non-unique. By convention, +project $\vect{p}_2$ onto axis 1: +\[ + \vect{f}_1 = \vect{p}_1 + (\vect{d}_{\mathrm{pp}} \cdot \unit{a}_1)\,\unit{a}_1, + \qquad \vect{f}_2 = \vect{p}_2. +\] + +\paragraph{Step 3c: Determine $\unit{x}_1$.} + +\textbf{Case A (skew, $\norm{\vect{f}_2 - \vect{f}_1} > \epsilon$):} +\[ + \unit{x}_1 = \frac{\vect{f}_2 - \vect{f}_1}{\norm{\vect{f}_2 - \vect{f}_1}}. +\] + +\textbf{Case B (intersecting, feet coincide):} +\[ + \unit{x}_1 = \frac{\unit{a}_1 \times \unit{a}_2}{\norm{\unit{a}_1 \times \unit{a}_2}}. +\] +There is still a sign ambiguity; choose the sign so +$\unit{x}_1 \cdot \unit{x}_0 \ge 0$ (this tends to keep $\theta_1$ near zero). + +\textbf{Case C (parallel):} +\[ + \unit{x}_1 = \frac{\vect{p}_2 - \vect{f}_1}{\norm{\vect{p}_2 - \vect{f}_1}}. +\] +If $\vect{p}_2 = \vect{f}_1$ (the axes are coincident, not merely parallel), +this formula is degenerate. Fall back to $\unit{x}_0$ projected into the +plane perpendicular to $\unit{a}_2$, then normalized. + +\paragraph{Step 3d: Assemble frame 1.} +\[ + \vect{o}_1 = \vect{f}_1, \qquad + \unit{z}_1 = \unit{a}_2, \qquad + \unit{x}_1 = \text{as computed above}, \qquad + \unit{y}_1 = \unit{z}_1 \times \unit{x}_1. +\] +From this point forward, $\unit{z}_1$ refers to the DH frame axis (which +equals $\unit{a}_2$), not the joint 1 axis. + +\section{Step 4: Compute the DH Parameters for Joint 1} + +With frame 0 (Step 2) and frame 1 (Step 3) both fully defined, apply the +four formulas: +\begin{align} + a_1 &= (\vect{f}_2 - \vect{f}_1) \cdot \unit{x}_1 \\[4pt] + \alpha_1 &= \atantwo\!\Bigl(\bigl(\unit{z}_0 \times \unit{z}_1\bigr) \cdot \unit{x}_1,\;\; + \unit{z}_0 \cdot \unit{z}_1\Bigr) \\[4pt] + d_1 &= (\vect{o}_1 - \vect{o}_0) \cdot \unit{z}_0 \\[4pt] + \theta_1 &= \atantwo\!\Bigl(\bigl(\unit{x}_0 \times \unit{x}_1\bigr) \cdot \unit{z}_0,\;\; + \unit{x}_0 \cdot \unit{x}_1\Bigr) +\end{align} +For a revolute joint, $\theta_1$ is a constant offset; the runtime joint +angle is $\theta_1 + q_1$, where $q_1$ is the commanded joint variable. For +a prismatic joint, $d_1$ is the constant offset and the runtime distance is +$d_1 + q_1$. + +\subsection*{Validation} + +Before trusting the result, check: +\begin{itemize} + \item $a_1 \ge 0$ in Cases A and B (by construction, if $\unit{x}_1$ was + chosen as the foot-to-foot direction). + \item Compose $T_1^0 \, T_2^1 \cdots T_n^{n-1}$ at the zero configuration + and confirm the resulting end-effector pose matches the URDF forward + kinematics. Any mismatch signals a sign error somewhere in the chain. +\end{itemize} + +\section{Step 5: Iterate to the Next Joint} + +To compute joint 2's DH row, repeat Steps 3 and 4: +\begin{itemize} + \item Use frame 1 (just built) in place of frame 0. + \item Build frame 2 from the common normal between axis 2 ($\unit{a}_2$) + and axis 3 ($\unit{a}_3$). + \item Apply the four DH formulas with the indices shifted by one. +\end{itemize} +Continue until all $n$ joints have been processed. + +\begin{warnbox} +\textbf{The last joint.} For joint $n$, there is no axis $n{+}1$. The +$z$-axis of frame $n$ is still constrained to lie along joint $n$'s axis +($\unit{z}_n = \unit{a}_n$), but the origin placement along that axis and +the direction of $\unit{x}_n$ are free. Standard practice is to place +$\vect{o}_n$ at the tool center point and choose $\unit{x}_n$ to match the +tool frame's $x$-axis, both supplied by the URDF's \texttt{tool0} fixed +joint. +\end{warnbox} + +\section{Worked Example: Joint 1 of the Motoman GP12} + +This example illustrates \textbf{Case A (skew axes)}. + +The URDF declares: +\begin{verbatim} + + + + + + + + +\end{verbatim} + +\paragraph{Step 1 (axis data).} All \texttt{rpy} values are zero, so +$R_{\mathrm{cum}} = I$ throughout and axis directions pass through unchanged. +Cumulative positions simplify to the sum of \texttt{xyz} translations: +\begin{align*} + \vect{p}_1 &= (0, 0, 0.450), & \unit{a}_1 &= (0, 0, 1) \\ + \vect{p}_2 &= (0.155, 0, 0.450), & \unit{a}_2 &= (0, 1, 0) +\end{align*} + +\paragraph{Step 2 (frame 0).} +\[ + \vect{o}_0 = (0, 0, 0), \quad + \unit{z}_0 = (0, 0, 1), \quad + \unit{x}_0 = (1, 0, 0), \quad + \unit{y}_0 = (0, 1, 0). +\] + +\paragraph{Step 3a (classification).} +$\vect{c} = \unit{a}_1 \times \unit{a}_2 = (0,0,1) \times (0,1,0) = (-1, 0, 0)$, +with $\norm{\vect{c}} = 1 > \epsilon$. Case A (skew). + +\paragraph{Step 3b (feet of the common normal).} +$\vect{d}_{\mathrm{pp}} = (0.155, 0, 0)$, $\unit{a}_1 \cdot \unit{a}_2 = 0$, +so the system becomes: +\[ + \begin{bmatrix} 1 & 0 \\ 0 & -1 \end{bmatrix} + \begin{bmatrix} s \\ t \end{bmatrix} + = \begin{bmatrix} 0 \\ 0 \end{bmatrix} + \quad\Longrightarrow\quad s = 0,\; t = 0. +\] +\[ + \vect{f}_1 = (0, 0, 0.450), \qquad \vect{f}_2 = (0.155, 0, 0.450). +\] + +\paragraph{Step 3c ($\unit{x}_1$).} +$\unit{x}_1 = (0.155, 0, 0) / 0.155 = (1, 0, 0)$. + +\paragraph{Step 3d (assemble frame 1).} +\[ + \vect{o}_1 = (0, 0, 0.450), \quad + \unit{z}_1 = \unit{a}_2 = (0, 1, 0), \quad + \unit{x}_1 = (1, 0, 0), \quad + \unit{y}_1 = (0, 0, -1). +\] + +\paragraph{Step 4 (DH parameters).} +\begin{align*} + a_1 &= (\vect{f}_2 - \vect{f}_1) \cdot \unit{x}_1 + = (0.155, 0, 0) \cdot (1, 0, 0) = 0.155 \\[4pt] + \alpha_1 &= \atantwo\bigl((\unit{z}_0 \times \unit{z}_1) \cdot \unit{x}_1,\; + \unit{z}_0 \cdot \unit{z}_1\bigr) \\ + &= \atantwo\bigl((-1, 0, 0) \cdot (1, 0, 0),\; 0\bigr) + = \atantwo(-1,\, 0) = -\tfrac{\pi}{2} \\[4pt] + d_1 &= (\vect{o}_1 - \vect{o}_0) \cdot \unit{z}_0 + = (0, 0, 0.450) \cdot (0, 0, 1) = 0.450 \\[4pt] + \theta_1 &= \atantwo\bigl((\unit{x}_0 \times \unit{x}_1) \cdot \unit{z}_0,\; + \unit{x}_0 \cdot \unit{x}_1\bigr) + = \atantwo(0,\, 1) = 0 +\end{align*} + +\begin{keybox} +\textbf{Result for joint 1:} +\[ + a_1 = 0.155\,\text{m}, \quad + \alpha_1 = -\tfrac{\pi}{2}, \quad + d_1 = 0.450\,\text{m}, \quad + \theta_1 = 0. +\] +\end{keybox} + +\section{Worked Example: Joint 2 of the Motoman GP12} + +This example illustrates \textbf{Case C (parallel axes)} and reuses frame 1 +as computed in the previous example. + +The URDF declares: +\begin{verbatim} + + + + + + + + +\end{verbatim} + +\paragraph{Step 1 (axis data).} Continuing the cumulative sum: +\begin{align*} + \vect{p}_2 &= (0.155, 0, 0.450), & \unit{a}_2 &= (0, 1, 0) \\ + \vect{p}_3 &= (0.155, 0, 1.064), & \unit{a}_3 &= (0, -1, 0) +\end{align*} + +Note that $\unit{a}_3 = -\unit{a}_2$. The URDF deliberately assigns opposite +signs to the shoulder and elbow axis directions --- physically this doesn't +change anything (an axis line is symmetric under sign flip), but it does +affect the DH table. A positive $\unit{a}_3$ would produce $\alpha_2 = 0$ +instead of $\alpha_2 = \pi$; the final forward kinematics would still match. + +\paragraph{Frame 1 (from the previous example).} +\[ + \vect{o}_1 = (0, 0, 0.450), \quad + \unit{z}_1 = (0, 1, 0), \quad + \unit{x}_1 = (1, 0, 0), \quad + \unit{y}_1 = (0, 0, -1). +\] + +\paragraph{Step 3a (classification).} +\[ + \vect{c} = \unit{a}_2 \times \unit{a}_3 = (0,1,0) \times (0,-1,0) = (0,0,0), + \qquad \norm{\vect{c}} = 0. +\] +Case C (parallel). The axes are antiparallel but that is still ``parallel'' +for classification: lines in 3D have no inherent direction, so the common +normal is non-unique regardless of the sign of $\unit{a}_3$. + +\paragraph{Step 3b (feet of the common normal, parallel convention).} Set +$\vect{f}_3 = \vect{p}_3$ and project $\vect{p}_3$ onto axis 2: +\[ + \vect{d}_{\mathrm{pp}} = \vect{p}_3 - \vect{p}_2 = (0, 0, 0.614), + \qquad \vect{d}_{\mathrm{pp}} \cdot \unit{a}_2 = 0. +\] +\[ + \vect{f}_2 = \vect{p}_2 + 0 \cdot \unit{a}_2 = (0.155, 0, 0.450), + \qquad \vect{f}_3 = (0.155, 0, 1.064). +\] + +\paragraph{Step 3c ($\unit{x}_2$, parallel convention).} +\[ + \unit{x}_2 = \frac{\vect{p}_3 - \vect{f}_2}{\norm{\vect{p}_3 - \vect{f}_2}} + = \frac{(0, 0, 0.614)}{0.614} = (0, 0, 1). +\] + +\paragraph{Step 3d (assemble frame 2).} +\[ + \vect{o}_2 = (0.155, 0, 0.450), \quad + \unit{z}_2 = \unit{a}_3 = (0, -1, 0), \quad + \unit{x}_2 = (0, 0, 1), \quad + \unit{y}_2 = \unit{z}_2 \times \unit{x}_2 = (-1, 0, 0). +\] + +\paragraph{Step 4 (DH parameters).} +\begin{align*} + a_2 &= (\vect{f}_3 - \vect{f}_2) \cdot \unit{x}_2 + = (0, 0, 0.614) \cdot (0, 0, 1) = 0.614 \\[4pt] + \alpha_2 &= \atantwo\bigl((\unit{z}_1 \times \unit{z}_2) \cdot \unit{x}_2,\; + \unit{z}_1 \cdot \unit{z}_2\bigr) \\ + &= \atantwo\bigl(\,\vect{0} \cdot (0,0,1),\; -1\,\bigr) + = \atantwo(0,\, -1) = \pi \\[4pt] + d_2 &= (\vect{o}_2 - \vect{o}_1) \cdot \unit{z}_1 + = (0.155, 0, 0) \cdot (0, 1, 0) = 0 \\[4pt] + \theta_2 &= \atantwo\bigl((\unit{x}_1 \times \unit{x}_2) \cdot \unit{z}_1,\; + \unit{x}_1 \cdot \unit{x}_2\bigr) \\ + &= \atantwo\bigl((0, -1, 0) \cdot (0, 1, 0),\; 0\bigr) + = \atantwo(-1,\, 0) = -\tfrac{\pi}{2} +\end{align*} + +\begin{keybox} +\textbf{Result for joint 2:} +\[ + a_2 = 0.614\,\text{m}, \quad + \alpha_2 = \pi, \quad + d_2 = 0, \quad + \theta_2 = -\tfrac{\pi}{2}. +\] +\end{keybox} + +\paragraph{Interpretation of $\theta_2 = -\pi/2$.} The URDF's zero +configuration places joint 2 ``straight up'' (link 2 oriented along world +$+z$), but the DH frame convention puts $\unit{x}_2$ along the common +normal --- which also points along world $+z$. The $-\pi/2$ offset rotates +$\unit{x}_1$ (world $+x$) to $\unit{x}_2$ (world $+z$) about $\unit{z}_1$ +(world $+y$). At runtime, the commanded angle $q_2$ is added to this +offset: $\theta_2^{\mathrm{runtime}} = -\pi/2 + q_2$. + +This offset is \emph{not} a bug --- it is an artifact of the URDF and DH +conventions disagreeing about what the zero pose looks like. Published DH +tables for the GP12 typically bake this offset into the joint angle +definition, so the ``canonical'' and ``URDF'' zero poses differ by exactly +this quarter turn. + +\section{Sign-Convention Pitfalls} + +The most common errors in DH derivations come from sign ambiguities. Three +rules prevent most of them: + +\begin{enumerate} + \item \textbf{The four DH parameters are not independent scalars --- they + are the components of a rigid transform.} Flipping the sign of + $\unit{x}_i$ flips both $a_i$ and $\alpha_i$, and shifts $\theta_i$ by + $\pi$. You cannot flip one in isolation. + \item \textbf{Commit to a convention for $\unit{x}_i$ before computing.} + The ``common normal points from axis $i$ to axis $i{+}1$'' convention + (used above) gives $a_i \ge 0$ and tends to produce clean tables. + \item \textbf{Validate by composing the transforms.} At the zero + configuration, the product $T_1^0 \cdots T_n^{n-1}$ must match the + end-effector pose computed directly from the URDF. If it doesn't, there + is a sign error, and it is almost always in $\alpha_i$ or $\theta_i$. +\end{enumerate} + +\end{document} diff --git a/src/viam/sdk/referenceframe/latex/dh_params.pdf b/src/viam/sdk/referenceframe/latex/dh_params.pdf new file mode 100644 index 000000000..6e55ef2f4 Binary files /dev/null and b/src/viam/sdk/referenceframe/latex/dh_params.pdf differ diff --git a/src/viam/sdk/referenceframe/private/urdf_to_dh_internals.hpp b/src/viam/sdk/referenceframe/private/urdf_to_dh_internals.hpp new file mode 100644 index 000000000..71b08bdda --- /dev/null +++ b/src/viam/sdk/referenceframe/private/urdf_to_dh_internals.hpp @@ -0,0 +1,95 @@ +/// @file referenceframe/private/urdf_to_dh_internals.hpp +/// @brief Internal helpers for urdf_to_dh_params. Exposed only to the +/// test suite; not part of the installed public API. +#pragma once + +#include +#include + +#include +#include +#include + +#include +#include + +namespace viam { +namespace sdk { +namespace urdf_to_dh_internals { + +constexpr double k_axis_parallel_epsilon = 1e-9; +constexpr double k_dh_compat_epsilon = 1e-6; + +/// @brief Parsed joint element from a URDF (subset needed for DH extraction). +struct Joint { + std::string name; + std::string type; // "revolute", "continuous", "fixed", ... + std::string parent_link; + std::string child_link; + boost::optional origin; // none == identity + boost::optional axis; // none == URDF default (1,0,0) +}; + +/// @brief Result of walking joint axes at rest along a chain. +struct JointAxesAtRest { + std::vector axes; // revolute joint axes in world + std::vector origins; // revolute joint origins in world + Eigen::Isometry3d end_pose; // final link pose in world +}; + +/// @brief Result of build_dh_frames: frames 0..N, each length N+1. +struct DHFrames { + std::vector zs; + std::vector xs; + std::vector origins; +}; + +/// @brief Result of common_normal: direction, feet on each line, and +/// whether the two axes are (anti-)parallel. +struct CommonNormalResult { + Eigen::Vector3d x_dir; // zero vector if the two lines are coincident + Eigen::Vector3d foot0; + Eigen::Vector3d foot1; + bool parallel; +}; + +std::vector parse_urdf(const KinematicsDataURDF& urdf); + +std::vector walk_urdf_chain(const std::vector& joints); + +Eigen::Isometry3d pose_in_meters(const boost::optional& origin); + +Eigen::Vector3d axis_unit(const boost::optional& axis); + +JointAxesAtRest joint_axes_at_rest(const std::vector& joints); + +CommonNormalResult common_normal(const Eigen::Vector3d& z0, + const Eigen::Vector3d& p0, + const Eigen::Vector3d& z1, + const Eigen::Vector3d& p1); + +DHFrames build_dh_frames(const std::vector& axes, + const std::vector& origins, + const Eigen::Isometry3d& end_pose); + +void extract_dh_row(const Eigen::Vector3d& z_prev, + const Eigen::Vector3d& x_prev, + const Eigen::Vector3d& p_prev, + const Eigen::Vector3d& z_curr, + const Eigen::Vector3d& x_curr, + const Eigen::Vector3d& p_curr, + double& d, + double& theta, + double& a, + double& alpha); + +void validate_end_effector_dh(const Eigen::Vector3d& z_prev, + const Eigen::Vector3d& x_end, + const Eigen::Vector3d& origin_prev, + const Eigen::Vector3d& p_end); + +Eigen::Vector3d pick_base_x(const Eigen::Vector3d& z); + +} // namespace urdf_to_dh_internals +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/referenceframe/urdf_to_dh_params.cpp b/src/viam/sdk/referenceframe/urdf_to_dh_params.cpp new file mode 100644 index 000000000..f54e2e52c --- /dev/null +++ b/src/viam/sdk/referenceframe/urdf_to_dh_params.cpp @@ -0,0 +1,471 @@ +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace viam { +namespace sdk { +namespace urdf_to_dh_internals { + +namespace { + +// Parse "x y z" (space-delimited) to Vector3d. Throws if fewer than 3 tokens. +Eigen::Vector3d parse_triple(const std::string& s) { + std::istringstream iss(s); + double x = 0, y = 0, z = 0; + if (!(iss >> x >> y >> z)) { + throw Exception(ErrorCondition::k_general, + "URDFToDHParams: failed to parse space-delimited triple: '" + s + "'"); + } + return Eigen::Vector3d(x, y, z); +} + +// URDF RPY -> rotation matrix (R = Rz(yaw) * Ry(pitch) * Rx(roll)). +Eigen::Matrix3d rpy_to_rotation(double roll, double pitch, double yaw) { + return (Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())) + .toRotationMatrix(); +} + +} // namespace + +std::vector parse_urdf(const KinematicsDataURDF& urdf) { + namespace pt = boost::property_tree; + pt::ptree tree; + + std::string text(reinterpret_cast(urdf.bytes.data()), urdf.bytes.size()); + std::istringstream iss(text); + try { + pt::read_xml(iss, tree); + } catch (const pt::xml_parser_error& e) { + throw Exception(ErrorCondition::k_general, + std::string("URDFToDHParams: failed to parse URDF XML: ") + e.what()); + } + + const auto robot_it = tree.find("robot"); + if (robot_it == tree.not_found()) { + throw Exception(ErrorCondition::k_general, + "URDFToDHParams: no root element in URDF"); + } + const pt::ptree& robot = robot_it->second; + + std::vector joints; + for (const auto& child : robot) { + if (child.first != "joint") { + continue; + } + const pt::ptree& jnode = child.second; + + Joint j; + j.name = jnode.get(".name", ""); + j.type = jnode.get(".type", ""); + j.parent_link = jnode.get("parent..link", ""); + j.child_link = jnode.get("child..link", ""); + + const auto origin_child = jnode.get_child_optional("origin"); + if (origin_child) { + const std::string xyz = origin_child->get(".xyz", "0 0 0"); + const std::string rpy = origin_child->get(".rpy", "0 0 0"); + const Eigen::Vector3d t = parse_triple(xyz); + const Eigen::Vector3d r = parse_triple(rpy); + Eigen::Isometry3d iso = Eigen::Isometry3d::Identity(); + iso.linear() = rpy_to_rotation(r.x(), r.y(), r.z()); + iso.translation() = t; + j.origin = iso; + } + + const auto axis_child = jnode.get_child_optional("axis"); + if (axis_child) { + const std::string xyz = axis_child->get(".xyz", "1 0 0"); + j.axis = parse_triple(xyz); + } + joints.push_back(std::move(j)); + } + return joints; +} + +std::vector walk_urdf_chain(const std::vector& joints) { + // Index joints by parent-link name. + std::map> by_parent; + std::set all_parents; + std::set all_children; + for (const auto& j : joints) { + by_parent[j.parent_link].push_back(&j); + all_parents.insert(j.parent_link); + all_children.insert(j.child_link); + } + + // A root link is any parent that is never a child. + std::vector roots; + for (const auto& p : all_parents) { + if (all_children.find(p) == all_children.end()) { + roots.push_back(p); + } + } + if (roots.size() != 1) { + std::string list; + for (const auto& r : roots) { + list += "'" + r + "' "; + } + throw Exception(ErrorCondition::k_general, + "URDFToDHParams: expected exactly one root link, found " + + std::to_string(roots.size()) + ": " + list); + } + + std::vector ordered; + ordered.reserve(joints.size()); + std::string current = roots[0]; + while (true) { + auto it = by_parent.find(current); + if (it == by_parent.end() || it->second.empty()) { + break; // reached leaf + } + if (it->second.size() > 1) { + throw Exception(ErrorCondition::k_general, + "URDFToDHParams: branching topology at link '" + current + "' (has " + + std::to_string(it->second.size()) + " outgoing joints)"); + } + const Joint* j = it->second.front(); + ordered.push_back(*j); + current = j->child_link; + } + if (ordered.size() != joints.size()) { + throw Exception(ErrorCondition::k_general, + "URDFToDHParams: chain walk visited " + std::to_string(ordered.size()) + + " of " + std::to_string(joints.size()) + + " joints; URDF may be disconnected"); + } + return ordered; +} + +Eigen::Isometry3d pose_in_meters(const boost::optional& origin) { + return origin ? *origin : Eigen::Isometry3d::Identity(); +} + +Eigen::Vector3d axis_unit(const boost::optional& axis) { + if (!axis) { + return Eigen::Vector3d(1.0, 0.0, 0.0); // URDF default + } + const double norm = axis->norm(); + if (norm < k_axis_parallel_epsilon) { + throw Exception(ErrorCondition::k_general, "URDFToDHParams: joint axis has zero magnitude"); + } + return *axis / norm; +} + +// dh_params.pdf steps 3a, 3b, 3c for a single (axis i, axis i+1) pair +// Inputs: z0 = a_i, p0 = p_i, z1 = a_{i+1}, p1 = p_{i+1} +CommonNormalResult common_normal(const Eigen::Vector3d& z0, + const Eigen::Vector3d& p0, + const Eigen::Vector3d& z1, + const Eigen::Vector3d& p1) { + CommonNormalResult r{}; + // step 3a: c = a_i x a_{i+1}. Drives the classify branch below. + const Eigen::Vector3d cross = z0.cross(z1); + + if (cross.norm() < k_axis_parallel_epsilon) { + // ||c|| equiv to 0 --> case C + r.parallel = true; + // step 3b case C: d = d_pp = p_{i+1} - p_i + // perp = component of d_pp perpendicular to a_i (= p_{i+1} - f_i). + const Eigen::Vector3d d = p1 - p0; + const Eigen::Vector3d perp = d - z0 * d.dot(z0); + const double perp_norm = perp.norm(); + if (perp_norm < k_axis_parallel_epsilon) { + // degenerate case C: axes coincident, not merely parallel, x_i is undefined + // chose to signal via zero x_dir so the caller + // can inherit x from the previous frame. + r.x_dir = Eigen::Vector3d::Zero(); + r.foot0 = p0; + r.foot1 = p0; + return r; + } + // step 3c case C: x_i = (p_{i+1} - f_i) / ||p_{i+1} - f_i|| = perp / ||perp|| + r.x_dir = perp / perp_norm; + r.foot0 = p0; // f_i = p_i (d_pp dot a_i component lies on the axis itself) + r.foot1 = r.foot0 + r.x_dir * perp_norm; + return r; + } + + // ||c|| > eps --> cases A (skew) and B (intersecting) + // step 3b + const Eigen::Vector3d d = p1 - p0; // d_pp + const double a = z0.dot(z0); // = 1 (unit) + const double b = z0.dot(z1); // a_i dot a_{i+1} + const double c = z1.dot(z1); // = 1 + const double det = a * c - b * b; // = |cross|^2 > 0 + const double t0 = (d.dot(z0) * c - d.dot(z1) * b) / det; // = s + const double t1 = (d.dot(z0) * b - d.dot(z1) * a) / det; // = t + + // step 3b feet: f_i = p_i + s*a_i, f_{i+1} = p_{i+1} + t*a_{i+1} + r.foot0 = p0 + z0 * t0; + r.foot1 = p1 + z1 * t1; + const Eigen::Vector3d diff = r.foot1 - r.foot0; + const double diff_norm = diff.norm(); + if (diff_norm < k_axis_parallel_epsilon) { + // case B (intersecting): feet coincide, so foot-to-foot is undefined + // step 3c Case B: x_i = (a_i x a_{i+1}) / ||a_i x a_{i+1}|| + r.x_dir = cross / cross.norm(); + } else { + // case A (skew): step 3c case A: x_i = (f_{i+1} - f_i) / ||f_{i+1} - f_i|| + r.x_dir = diff / diff_norm; + } + r.parallel = false; + return r; +} + +// dh_params.pdf step 4: compute one DH row (d, theta, a, alpha) from two +// consecutive DH frames (i-1, i). All inputs in the base frame. +// Inputs: z_prev = z_{i-1}, x_prev = x_{i-1}, p_prev = o_{i-1} +// z_curr = z_i, x_curr = x_i, p_curr = o_i +// +// dh.tex writes Eq. (a) as (f_{i+1} - f_i).x_i. That equals (o_i - o_{i-1}).x_i +// used below: the offsets between f's and o's lie along z_{i-1}, which vanishes +// under the x_i projection (x_i is perpendicular to z_{i-1} by construction). +void extract_dh_row(const Eigen::Vector3d& z_prev, + const Eigen::Vector3d& x_prev, + const Eigen::Vector3d& p_prev, + const Eigen::Vector3d& z_curr, + const Eigen::Vector3d& x_curr, + const Eigen::Vector3d& p_curr, + double& d, + double& theta, + double& a, + double& alpha) { + const Eigen::Vector3d delta = p_curr - p_prev; // o_i - o_{i-1} + // d_i = (o_i - o_{i-1}) dot z_{i-1} + d = delta.dot(z_prev); + // a_i = (o_i - o_{i-1}) dot x_i + a = delta.dot(x_curr); + // theta_i = atan2((x_{i-1} x x_i) dot z_{i-1}, x_{i-1} dot x_i) + theta = std::atan2(x_prev.cross(x_curr).dot(z_prev), x_prev.dot(x_curr)); + // alpha_i = atan2((z_{i-1} x z_i) dot x_i, z_{i-1} dot z_i) + alpha = std::atan2(z_prev.cross(z_curr).dot(x_curr), z_prev.dot(z_curr)); +} + +// Fail-fast guard for the last DH row. +// Frames 0 through n-1 are built by the common-normal procedure, so they satisfy +// the DH constraints by construction. Frame n is overwritten with the URDF +// tool pose, which the URDF author chose freely so nothing forces it to be DH-compatible. +// +// A DH row T = Rz(theta)*Tz(d)*Tx(a)*Rx(alpha) has only 4 scalars but a rigid +// transform has 6 DoF. The 2 "missing" DoF correspond to two geometric +// constraints that must hold at the frame boundary, both checked here: +// +// (1) x_n perpendicular to z_{n-1} -- otherwise no (d,theta,a,alpha) row +// can represent the orientation step from frame n-1 to frame n. +// (2) (p_n - o_{n-1}) lies in the (z_{n-1}, x_n) plane -- the row can only +// translate along z_{n-1} and x_n, so a y-component is unrepresentable. +// +// Without this, a bad URDF produces a DH table that compiles, looks plausible +// but silently disagrees with FK. +void validate_end_effector_dh(const Eigen::Vector3d& z_prev, + const Eigen::Vector3d& x_end, + const Eigen::Vector3d& origin_prev, + const Eigen::Vector3d& p_end) { + // check (1): x_n . z_{n-1} == 0 + const double perp_dot = x_end.dot(z_prev); + if (std::abs(perp_dot) > k_dh_compat_epsilon) { + throw Exception(ErrorCondition::k_general, + "URDFToDHParams: end-effector X-axis not perpendicular to last joint axis " + "(residual dot = " + + std::to_string(perp_dot) + ")"); + } + // check (2): displacement to tool origin has no y_{n-1} component. + // y = z_{n-1} x x_n is the plane normal; delta . y must be ~0. + const Eigen::Vector3d delta = p_end - origin_prev; + const Eigen::Vector3d y_dir = z_prev.cross(x_end); + const double plane_residual = delta.dot(y_dir); + if (std::abs(plane_residual) > k_dh_compat_epsilon) { + throw Exception(ErrorCondition::k_general, + "URDFToDHParams: end-effector origin out of DH plane (residual along y = " + + std::to_string(plane_residual) + ")"); + } +} + +// Walk the joint list in chain order and maintain a base to currentl link transform. +// For all revolute or continuous joints record two values in the base frame: +// (1) the joint origin p_i and (2) the joint axis direction a_i +JointAxesAtRest joint_axes_at_rest(const std::vector& joints) { + JointAxesAtRest out; + // initialize the running transformation + // stores rotation in .linear() and translation in .translation() + Eigen::Isometry3d cumulative = Eigen::Isometry3d::Identity(); + + for (const auto& j : joints) { + // with reference to dh_params.pdf: Step 1a and 1b are done in one Isometry3d multiply: + // translation: p_cum_new = p_cum + R_cum * t_i + // rotation: R_cum_new = R_cum * R_i + cumulative = cumulative * pose_in_meters(j.origin); + + if (j.type == "fixed") { + } else if (j.type == "revolute" || j.type == "continuous") { + try { + // normalize as we cannot assume unit length + const Eigen::Vector3d local_axis = axis_unit(j.axis); + // step 1c: axis is expressed in the child's frame, so use the updated R_cum + const Eigen::Vector3d world_axis = cumulative.linear() * local_axis; + // record (p_i, a_i) in the base frame + out.axes.push_back(world_axis); + out.origins.push_back(cumulative.translation()); + } catch (const Exception& e) { + throw Exception(ErrorCondition::k_general, + "URDFToDHParams: joint '" + j.name + "': " + e.what()); + } + } else { + throw Exception(ErrorCondition::k_not_supported, + "URDFToDHParams: joint '" + j.name + "' has unsupported type '" + + j.type + "' (only revolute, continuous, and fixed are supported)"); + } + } + + if (out.axes.empty()) { + throw Exception(ErrorCondition::k_general, "URDFToDHParams: no revolute joints in chain"); + } + // full base-to-leaf transform, this includes any trailing fixed joints like tool0 + out.end_pose = cumulative; + return out; +} + +// Build all n+1 DH frames from the (axes, origins) list produced by +// joint_axes_at_rest. Frame 0 comes from a base-frame projection (dh_params.pdf step 2), +// frames 1 through n from successive common normals (dh_params.pdf step 3), and frame n is +// finally pinned to the URDF tool pose +DHFrames build_dh_frames(const std::vector& axes, + const std::vector& origins, + const Eigen::Isometry3d& end_pose) { + const std::size_t n = axes.size(); + + const Eigen::Vector3d end_z = end_pose.linear() * Eigen::Vector3d(0, 0, 1); + const Eigen::Vector3d end_x = end_pose.linear() * Eigen::Vector3d(1, 0, 0); + const Eigen::Vector3d end_p = end_pose.translation(); + + std::vector all_z(axes.begin(), axes.end()); + all_z.push_back(end_z); + std::vector all_p(origins.begin(), origins.end()); + all_p.push_back(end_p); + + // n revolute joints means n+1 DH frames + DHFrames out; + out.zs.resize(n + 1); + out.xs.resize(n + 1); + out.origins.resize(n + 1); + + // dh_params.pdf step 2: Choose Frame 0. + out.zs[0] = axes[0]; + const Eigen::Vector3d& p0 = origins[0]; + const double t = -p0.dot(axes[0]); // signed distance along axis from p0 toward (0,0,0) + out.origins[0] = p0 + axes[0] * t; + out.xs[0] = pick_base_x(axes[0]); + + // dh_params.pdf step 3: build frame i from the common normal of axis i and axis i+1. + for (std::size_t i = 1; i <= n; ++i) { + const Eigen::Vector3d& z_prev = out.zs[i - 1]; + const Eigen::Vector3d& z_curr = all_z[i]; + const Eigen::Vector3d& p_prev = out.origins[i - 1]; + const Eigen::Vector3d& p_curr = all_p[i]; + + // steps 3a/3b/3c: classify, find feet f_i and f_{i+1}, derive x_i + const auto cn = common_normal(z_prev, p_prev, z_curr, p_curr); + + if (cn.x_dir.isZero(k_axis_parallel_epsilon)) { + // coincident axes (step 3c parallel degenerate) + // common normal is undefined, so inherit x from the previous frame + out.xs[i] = out.xs[i - 1]; + out.origins[i] = p_curr; + out.zs[i] = z_curr; + continue; + } + + // sign disambiguation: keep x_i dot x_{i-1} >= 0 so theta_i stays near zero + Eigen::Vector3d x_dir = cn.x_dir; + if (x_dir.dot(out.xs[i - 1]) < 0) { + x_dir = -x_dir; + } + // step 3d: assemble frame i. + // x_i as above + // o_i = f_i (= cn.foot1) + // z_i = a_{i+1} + out.xs[i] = x_dir; + out.origins[i] = cn.foot1; + out.zs[i] = z_curr; + } + + out.origins[n] = end_p; + out.zs[n] = end_z; + out.xs[n] = end_x; + + return out; +} + +// dh_params.pdf step 2 helper: pick x_0 given z_0, x_0 perpendicular to z_0 +// Standard choice is world +x projected onto the plane perp to z, normalized +// Falls back to world +y if z is (anti)parallel to world +x (projection ~0) +Eigen::Vector3d pick_base_x(const Eigen::Vector3d& z) { + const Eigen::Vector3d world_x(1, 0, 0); + // remove the component of world_x along z, leaving the perpendicular part + Eigen::Vector3d proj = world_x - z * z.dot(world_x); + if (proj.norm() < k_axis_parallel_epsilon) { + // z parallel to world +x --> projection ~ 0. retry with world +y + const Eigen::Vector3d world_y(0, 1, 0); + proj = world_y - z * z.dot(world_y); + } + return proj.normalized(); +} + +} // namespace urdf_to_dh_internals + +std::vector urdf_to_dh_params(const KinematicsDataURDF& urdf) { + using namespace urdf_to_dh_internals; + + auto chain = walk_urdf_chain(parse_urdf(urdf)); + auto axes_origins = joint_axes_at_rest(chain); + const std::size_t n = axes_origins.axes.size(); + + auto frames = build_dh_frames(axes_origins.axes, axes_origins.origins, axes_origins.end_pose); + + validate_end_effector_dh( + frames.zs[n - 1], frames.xs[n], frames.origins[n - 1], frames.origins[n]); + + // Revolute joint names in chain order (matches joint_axes_at_rest filter). + std::vector revolute_names; + revolute_names.reserve(n); + for (const auto& j : chain) { + if (j.type == "revolute" || j.type == "continuous") { + revolute_names.push_back(j.name); + } + } + + std::vector result(n); + for (std::size_t i = 0; i < n; ++i) { + double d, theta, a, alpha; + extract_dh_row(frames.zs[i], + frames.xs[i], + frames.origins[i], + frames.zs[i + 1], + frames.xs[i + 1], + frames.origins[i + 1], + d, + theta, + a, + alpha); + result[i] = DHParam{revolute_names[i], d, theta, a, alpha}; + } + return result; +} + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/referenceframe/urdf_to_dh_params.hpp b/src/viam/sdk/referenceframe/urdf_to_dh_params.hpp new file mode 100644 index 000000000..b2d507be2 --- /dev/null +++ b/src/viam/sdk/referenceframe/urdf_to_dh_params.hpp @@ -0,0 +1,40 @@ +/// @file referenceframe/urdf_to_dh_params.hpp +/// @brief Convert a revolute-only URDF serial chain to Denavit-Hartenberg +/// parameters. +#pragma once + +#include +#include + +#include + +namespace viam { +namespace sdk { + +/// @brief One Denavit-Hartenberg row for a single revolute joint. +/// +/// Units: meters for `d` and `a`, radians for `theta` and `alpha`. +/// `theta` is the joint's angle at the URDF's zero configuration +/// (typically 0). The runtime joint angle is `theta + input`. +struct DHParam { + std::string name; ///< joint name copied from the URDF + double d; ///< translation along Z_{i-1} + double theta; ///< rotation about Z_{i-1} at zero config + double a; ///< translation along X_i + double alpha; ///< rotation about X_i +}; + +/// @brief Convert a revolute-only URDF serial chain to DH parameters. +/// +/// Returns one `DHParam` per revolute or continuous joint in chain order. +/// Fixed joints are folded into the cumulative transform. +/// +/// @throws viam::sdk::Exception if the URDF is not a serial revolute chain, +/// has an unsupported joint type, has a zero-magnitude axis, has an +/// end-effector frame that is not DH-compatible (X-axis not +/// perpendicular to the last joint axis, or origin out of the DH +/// plane), or fails to parse as XML. +std::vector urdf_to_dh_params(const KinematicsDataURDF& urdf); + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/tests/CMakeLists.txt b/src/viam/sdk/tests/CMakeLists.txt index fd0f27006..65a1432d8 100644 --- a/src/viam/sdk/tests/CMakeLists.txt +++ b/src/viam/sdk/tests/CMakeLists.txt @@ -56,6 +56,9 @@ target_link_libraries(viamsdk_test viamcppsdk_link_viam_api(viamsdk_test PUBLIC) +# Path to the urdf fixtures used by test_urdf_to_dh_params +set(VIAMCPPSDK_TEST_TESTFILES_DIR "${CMAKE_CURRENT_SOURCE_DIR}/testfiles") + viamcppsdk_add_boost_test(test_arm.cpp) viamcppsdk_add_boost_test(test_audio_in.cpp) viamcppsdk_add_boost_test(test_audio_out.cpp) @@ -84,4 +87,10 @@ viamcppsdk_add_boost_test(test_resource.cpp) viamcppsdk_add_boost_test(test_sensor.cpp) viamcppsdk_add_boost_test(test_servo.cpp) viamcppsdk_add_boost_test(test_switch.cpp) +viamcppsdk_add_boost_test(test_urdf_to_dh_params.cpp) viamcppsdk_add_boost_test(test_robot.cpp) + +target_compile_definitions(test_urdf_to_dh_params + PRIVATE VIAMCPPSDK_TEST_TESTFILES_DIR="${VIAMCPPSDK_TEST_TESTFILES_DIR}") + +target_link_libraries(test_urdf_to_dh_params Eigen3::Eigen) diff --git a/src/viam/sdk/tests/test_urdf_to_dh_params.cpp b/src/viam/sdk/tests/test_urdf_to_dh_params.cpp new file mode 100644 index 000000000..815a8bc06 --- /dev/null +++ b/src/viam/sdk/tests/test_urdf_to_dh_params.cpp @@ -0,0 +1,424 @@ +#define BOOST_TEST_MODULE test module test_urdf_to_dh_params + +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +using namespace viam::sdk::urdf_to_dh_internals; + +namespace { + +viam::sdk::KinematicsDataURDF kinematics_from_string(const std::string& xml) { + std::vector bytes(xml.begin(), xml.end()); + return viam::sdk::KinematicsDataURDF(std::move(bytes)); +} + +viam::sdk::KinematicsDataURDF load_urdf(const std::string& filename) { + std::ifstream f(std::string(VIAMCPPSDK_TEST_TESTFILES_DIR) + "/" + filename, std::ios::binary); + if (!f) { + throw std::runtime_error("failed to open " + filename); + } + std::ostringstream buf; + buf << f.rdbuf(); + return kinematics_from_string(buf.str()); +} + +} // namespace + +namespace viam { +namespace sdktests { + +using namespace viam::sdk; + +BOOST_AUTO_TEST_CASE(test_pose_in_meters) { + Eigen::Isometry3d origin = Eigen::Isometry3d::Identity(); + origin.translation() = Eigen::Vector3d(0.5, 0.0, 0.0); + auto got = pose_in_meters(origin); + BOOST_CHECK_SMALL(got.translation().x() - 0.5, 1e-12); + BOOST_CHECK_SMALL(got.translation().y() - 0.0, 1e-12); + BOOST_CHECK_SMALL(got.translation().z() - 0.0, 1e-12); + + // None -> identity + auto identity = pose_in_meters(boost::none); + BOOST_CHECK(identity.isApprox(Eigen::Isometry3d::Identity())); +} + +BOOST_AUTO_TEST_CASE(test_common_normal_perpendicular) { + // Z-axis through origin + Y-axis through (0, 0, 1): perpendicular intersecting lines. + Eigen::Vector3d z0(0, 0, 1), p0(0, 0, 0); + Eigen::Vector3d z1(0, 1, 0), p1(0, 0, 1); + auto r = common_normal(z0, p0, z1, p1); + BOOST_CHECK(!r.parallel); + // z0 x z1 = (0,0,1) x (0,1,0) = (-1, 0, 0) + BOOST_CHECK_SMALL(r.x_dir.x() - (-1.0), 1e-9); + BOOST_CHECK_SMALL(r.x_dir.y(), 1e-9); + BOOST_CHECK_SMALL(r.x_dir.z(), 1e-9); + BOOST_CHECK_SMALL(r.foot0.z() - 1.0, 1e-9); + BOOST_CHECK_SMALL(r.foot1.z() - 1.0, 1e-9); +} + +BOOST_AUTO_TEST_CASE(test_common_normal_parallel) { + // Two parallel Z-axes separated by 0.5 in X. + Eigen::Vector3d z0(0, 0, 1), p0(0, 0, 0); + Eigen::Vector3d z1(0, 0, 1), p1(0.5, 0, 2); + auto r = common_normal(z0, p0, z1, p1); + BOOST_CHECK(r.parallel); + // Perpendicular projected off Z: (0.5, 0, 0) normalized -> (1, 0, 0) + BOOST_CHECK_SMALL(r.x_dir.x() - 1.0, 1e-9); + BOOST_CHECK_SMALL(r.x_dir.y(), 1e-9); + BOOST_CHECK_SMALL(r.x_dir.z(), 1e-9); +} + +BOOST_AUTO_TEST_CASE(test_extract_dh_row_ur5e_row1) { + // Frame 0: origin (0,0,0), Z=(0,0,1), X=(1,0,0). + // Frame 1: origin (0,0,0.1625), Z=(0,-1,0), X=(1,0,0). + // Expected: d=0.1625, a=0, alpha=pi/2, theta=0. + Eigen::Vector3d z_prev(0, 0, 1), x_prev(1, 0, 0), p_prev(0, 0, 0); + Eigen::Vector3d z_curr(0, -1, 0), x_curr(1, 0, 0), p_curr(0, 0, 0.1625); + + double d, theta, a, alpha; + extract_dh_row(z_prev, x_prev, p_prev, z_curr, x_curr, p_curr, d, theta, a, alpha); + + BOOST_CHECK_SMALL(d - 0.1625, 1e-9); + BOOST_CHECK_SMALL(theta, 1e-9); + BOOST_CHECK_SMALL(a, 1e-9); + BOOST_CHECK_SMALL(alpha - M_PI / 2.0, 1e-9); +} + +BOOST_AUTO_TEST_CASE(test_validate_end_effector_dh_compatible) { + BOOST_CHECK_NO_THROW(validate_end_effector_dh(Eigen::Vector3d(0, 0, 1), + Eigen::Vector3d(1, 0, 0), + Eigen::Vector3d(0, 0, 0), + Eigen::Vector3d(0, 0, 1))); +} + +BOOST_AUTO_TEST_CASE(test_validate_end_effector_dh_x_not_perpendicular) { + auto substring_match = [](const Exception& e) { + return std::string(e.what()).find("not perpendicular") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(validate_end_effector_dh(Eigen::Vector3d(0, 0, 1), + Eigen::Vector3d(1, 0, 0.5), + Eigen::Vector3d(0, 0, 0), + Eigen::Vector3d(0, 0, 1)), + Exception, + substring_match); +} + +BOOST_AUTO_TEST_CASE(test_validate_end_effector_dh_origin_out_of_plane) { + auto substring_match = [](const Exception& e) { + return std::string(e.what()).find("out of DH plane") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(validate_end_effector_dh(Eigen::Vector3d(0, 0, 1), + Eigen::Vector3d(1, 0, 0), + Eigen::Vector3d(0, 0, 0), + Eigen::Vector3d(0.5, 0.5, 1)), + Exception, + substring_match); +} + +BOOST_AUTO_TEST_CASE(test_walk_urdf_chain_ur5e) { + auto parsed = parse_urdf(load_urdf("ur5e-real.urdf")); + auto ordered = walk_urdf_chain(parsed); + const std::vector expected = { + "base_joint", + "base_link-base_link_inertia", + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", + "wrist_3_link-ft_frame", + }; + BOOST_REQUIRE_EQUAL(ordered.size(), expected.size()); + for (std::size_t i = 0; i < expected.size(); ++i) { + BOOST_CHECK_EQUAL(ordered[i].name, expected[i]); + } +} + +BOOST_AUTO_TEST_CASE(test_walk_urdf_chain_branching) { + const std::string xml = R"( + + + + + + + + + + + + + + + + + +)"; + auto substring_match = [](const Exception& e) { + return std::string(e.what()).find("branching") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION( + walk_urdf_chain(parse_urdf(kinematics_from_string(xml))), Exception, substring_match); +} + +BOOST_AUTO_TEST_CASE(test_joint_axes_at_rest_ur5e) { + auto chain = walk_urdf_chain(parse_urdf(load_urdf("ur5e-real.urdf"))); + auto r = joint_axes_at_rest(chain); + BOOST_REQUIRE_EQUAL(r.axes.size(), 6u); + BOOST_REQUIRE_EQUAL(r.origins.size(), 6u); + + // Joint 1 (shoulder_pan): axis = world Z, origin = (0, 0, 0.1625). + BOOST_CHECK_SMALL(r.axes[0].x(), 1e-9); + BOOST_CHECK_SMALL(r.axes[0].y(), 1e-9); + BOOST_CHECK_SMALL(r.axes[0].z() - 1.0, 1e-9); + BOOST_CHECK_SMALL(r.origins[0].x(), 1e-9); + BOOST_CHECK_SMALL(r.origins[0].y(), 1e-9); + BOOST_CHECK_SMALL(r.origins[0].z() - 0.1625, 1e-9); + + // Joint 2 (shoulder_lift): axis in world = (0, -1, 0), origin = (0, 0, 0.1625). + BOOST_CHECK_SMALL(r.origins[1].x(), 1e-9); + BOOST_CHECK_SMALL(r.origins[1].y(), 1e-9); + BOOST_CHECK_SMALL(r.origins[1].z() - 0.1625, 1e-9); + BOOST_CHECK_SMALL(r.axes[1].x(), 1e-9); + BOOST_CHECK_SMALL(r.axes[1].y() - (-1.0), 1e-9); + BOOST_CHECK_SMALL(r.axes[1].z(), 1e-9); +} + +BOOST_AUTO_TEST_CASE(test_build_dh_frames_ur5e) { + auto chain = walk_urdf_chain(parse_urdf(load_urdf("ur5e-real.urdf"))); + auto axes_origins = joint_axes_at_rest(chain); + auto frames = build_dh_frames(axes_origins.axes, axes_origins.origins, axes_origins.end_pose); + + // 7 frames: base + 6 per joint. + BOOST_REQUIRE_EQUAL(frames.zs.size(), 7u); + BOOST_REQUIRE_EQUAL(frames.xs.size(), 7u); + BOOST_REQUIRE_EQUAL(frames.origins.size(), 7u); + + // Frame 0: world. + BOOST_CHECK_SMALL(frames.zs[0].z() - 1.0, 1e-9); + BOOST_CHECK_SMALL(frames.xs[0].x() - 1.0, 1e-9); + BOOST_CHECK_SMALL(frames.origins[0].x(), 1e-9); + + // Frame 1: Z along -Y, X along world X, origin at z=0.1625. + BOOST_CHECK_SMALL(frames.zs[1].y() - (-1.0), 1e-9); + BOOST_CHECK_SMALL(frames.xs[1].x() - 1.0, 1e-9); + BOOST_CHECK_SMALL(frames.origins[1].z() - 0.1625, 1e-9); +} + +BOOST_AUTO_TEST_CASE(test_urdf_to_dh_params_ur5e) { + struct Row { + const char* name; + double d, theta, a, alpha; + }; + const std::vector expected = { + {"shoulder_pan_joint", 0.1625, 0, 0, M_PI / 2}, + {"shoulder_lift_joint", 0, 0, -0.425, 0}, + {"elbow_joint", 0, 0, -0.3922, 0}, + {"wrist_1_joint", 0.1333, 0, 0, M_PI / 2}, + {"wrist_2_joint", 0.0997, 0, 0, -M_PI / 2}, + {"wrist_3_joint", 0.0996, 0, 0, 0}, + }; + auto got = urdf_to_dh_params(load_urdf("ur5e-real.urdf")); + BOOST_REQUIRE_EQUAL(got.size(), expected.size()); + const double tol = 1e-6; + for (std::size_t i = 0; i < expected.size(); ++i) { + BOOST_CHECK_EQUAL(got[i].name, expected[i].name); + BOOST_CHECK_SMALL(got[i].d - expected[i].d, tol); + BOOST_CHECK_SMALL(got[i].theta - expected[i].theta, tol); + BOOST_CHECK_SMALL(got[i].a - expected[i].a, tol); + BOOST_CHECK_SMALL(got[i].alpha - expected[i].alpha, tol); + } +} + +BOOST_AUTO_TEST_CASE(test_parse_urdf_inline) { + const std::string xml = R"( + + + + + + + +)"; + auto joints = parse_urdf(kinematics_from_string(xml)); + BOOST_REQUIRE_EQUAL(joints.size(), 1u); + BOOST_CHECK_EQUAL(joints[0].name, "j1"); + BOOST_CHECK_EQUAL(joints[0].type, "fixed"); + BOOST_CHECK_EQUAL(joints[0].parent_link, "a"); + BOOST_CHECK_EQUAL(joints[0].child_link, "b"); + BOOST_REQUIRE(joints[0].origin.has_value()); + BOOST_CHECK_SMALL(joints[0].origin->translation().x() - 1.0, 1e-12); + BOOST_CHECK_SMALL(joints[0].origin->translation().y() - 2.0, 1e-12); + BOOST_CHECK_SMALL(joints[0].origin->translation().z() - 3.0, 1e-12); +} + +BOOST_AUTO_TEST_CASE(test_urdf_to_dh_params_no_revolute_joints) { + const std::string xml = R"( + + + + + + + +)"; + auto match = [](const Exception& e) { + return std::string(e.what()).find("no revolute joints") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(urdf_to_dh_params(kinematics_from_string(xml)), Exception, match); +} + +BOOST_AUTO_TEST_CASE(test_urdf_to_dh_params_unsupported_prismatic) { + const std::string xml = R"( + + + + + + + + + +)"; + auto match = [](const Exception& e) { + return std::string(e.what()).find("unsupported type") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(urdf_to_dh_params(kinematics_from_string(xml)), Exception, match); +} + +BOOST_AUTO_TEST_CASE(test_urdf_to_dh_params_branching) { + const std::string xml = R"( + + + + + + + + + + + + + + + + + + + + + + + +)"; + auto match = [](const Exception& e) { + return std::string(e.what()).find("branching") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(urdf_to_dh_params(kinematics_from_string(xml)), Exception, match); +} + +namespace { + +// Forward-kinematics helper: compose each DH row as +// Rz(theta) * Tz(d) * Tx(a) * Rx(alpha). Matches the Go test helper. +Eigen::Isometry3d dh_forward_kinematics(const std::vector& params) { + Eigen::Isometry3d cumulative = Eigen::Isometry3d::Identity(); + for (const auto& p : params) { + Eigen::Isometry3d step = Eigen::Isometry3d::Identity(); + step.linear() = Eigen::AngleAxisd(p.theta, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + cumulative = cumulative * step; + + step = Eigen::Isometry3d::Identity(); + step.translation() = Eigen::Vector3d(0, 0, p.d); + cumulative = cumulative * step; + + step = Eigen::Isometry3d::Identity(); + step.translation() = Eigen::Vector3d(p.a, 0, 0); + cumulative = cumulative * step; + + step = Eigen::Isometry3d::Identity(); + step.linear() = Eigen::AngleAxisd(p.alpha, Eigen::Vector3d::UnitX()).toRotationMatrix(); + cumulative = cumulative * step; + } + return cumulative; +} + +// End-effector pose at zero configuration, computed by direct URDF composition. +Eigen::Isometry3d urdf_end_pose_at_rest(const viam::sdk::KinematicsDataURDF& urdf) { + using namespace viam::sdk::urdf_to_dh_internals; + auto chain = walk_urdf_chain(parse_urdf(urdf)); + Eigen::Isometry3d cumulative = Eigen::Isometry3d::Identity(); + for (const auto& j : chain) { + cumulative = cumulative * pose_in_meters(j.origin); + } + return cumulative; +} + +void check_fk_roundtrip(const std::string& fixture) { + auto urdf = load_urdf(fixture); + auto params = urdf_to_dh_params(urdf); + auto dh = dh_forward_kinematics(params); + auto ref = urdf_end_pose_at_rest(urdf); + + const double tol = 1e-6; + BOOST_CHECK_SMALL(dh.translation().x() - ref.translation().x(), tol); + BOOST_CHECK_SMALL(dh.translation().y() - ref.translation().y(), tol); + BOOST_CHECK_SMALL(dh.translation().z() - ref.translation().z(), tol); + + // Compare rotation matrices element-wise. + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + BOOST_CHECK_SMALL(dh.linear()(r, c) - ref.linear()(r, c), tol); + } + } +} + +} // namespace + +BOOST_AUTO_TEST_CASE(test_urdf_to_dh_params_ur5e_fk_round_trip) { + check_fk_roundtrip("ur5e-real.urdf"); +} + +BOOST_AUTO_TEST_CASE(test_urdf_to_dh_params_gp12_fk_round_trip) { + auto params = urdf_to_dh_params(load_urdf("gp12.urdf")); + BOOST_CHECK_EQUAL(params.size(), 6u); + check_fk_roundtrip("gp12.urdf"); +} + +BOOST_AUTO_TEST_CASE(test_urdf_to_dh_params_non_dh_compatible_end) { + const std::string xml = R"( + + + + + + + + + + + + + + +)"; + auto match = [](const Exception& e) { + return std::string(e.what()).find("not perpendicular") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(urdf_to_dh_params(kinematics_from_string(xml)), Exception, match); +} + +} // namespace sdktests +} // namespace viam diff --git a/src/viam/sdk/tests/testfiles/gp12.urdf b/src/viam/sdk/tests/testfiles/gp12.urdf new file mode 100644 index 000000000..a8f2bbd36 --- /dev/null +++ b/src/viam/sdk/tests/testfiles/gp12.urdf @@ -0,0 +1,115 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/viam/sdk/tests/testfiles/ur5e-real.urdf b/src/viam/sdk/tests/testfiles/ur5e-real.urdf new file mode 100644 index 000000000..9f71d808f --- /dev/null +++ b/src/viam/sdk/tests/testfiles/ur5e-real.urdf @@ -0,0 +1,270 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +