diff --git a/MATH.md b/MATH.md index de997e87..22e72ad4 100644 --- a/MATH.md +++ b/MATH.md @@ -1,265 +1,329 @@ # Potential Fields Mathematical Overview -These equations were obtained from the [Principles of Robot Motion: Theory, Algorithms, and Implementations](https://ieeexplore.ieee.org/book/6267238) textbook [2] and [Real-Time Obstacle Avoidance for Manipulators and Mobile Robots](https://ieeexplore.ieee.org/stamp/stampCaCancaCccCfaksdfojas;kfjd;ksldadjf.jsp?tp=&arnumber=1087247) [1], which describes the derivation of the potential equations, how to obtain the gradients, and how to obtain a velocity from the gradients. -In this library, the potential field produces a task-space wrench over the robot’s end-effector pose: +These equations are drawn from *Principles of Robot Motion* (Choset et al.) [2] and *Real-Time Obstacle Avoidance for Manipulators and Mobile Robots* (Khatib) [1], which derive the potential functions, their gradients, and how to obtain a velocity from those gradients. -- Linear force $\mathbf{F}(q)$ [N] from the translational potential -- Torque $\boldsymbol{\tau}(q)$ [Nm] from the rotational potential +The potential field produces a **task-space wrench** over the robot's end-effector pose, a linear force $\mathbf{F}(q)$ from the translational potential and a torque $\boldsymbol{\tau}(q)$ from the rotational potential. This wrench is converted to a task-space twist using constant admittance gains: -This wrench is mapped to a task-space twist $T(q)$ using constant gains: +$$ +\mathbf{v}(q) = k_{lin}\,\mathbf{F}(q), \qquad \boldsymbol{\omega}(q) = k_{ang}\,\boldsymbol{\tau}(q) +$$ -$$\begin{align} -\mathbf{v}(q) &= k_{lin}\ \cdot \mathbf{F}(q) \\ -\boldsymbol{\omega}(q) &= k_{ang} \cdot \boldsymbol{\tau}(q) \\ -T(q) &= \begin{bmatrix}\mathbf{v}(q) \\ \boldsymbol{\omega}(q) \end{bmatrix} -\end{align}$$ +with defaults $k_{lin} = 1.0\,[(\mathrm{m/s})/\mathrm{N}]$ and $k_{ang} = 1.0\,[(\mathrm{rad/s})/\mathrm{Nm}]$. -with defaults of $k_{lin} = 1.0\,[(\mathrm{m/s})/\mathrm{N}]$ and $k_{ang} = 1.0\,[(\mathrm{rad/s})/\mathrm{Nm}]$. +The potential functions are scalar fields in units of energy [Nm]. Their gradients are forces [N]. The gains $\zeta$ (attractive) and $\eta$ (repulsive) act as inverse damping coefficients [Ns/m], converting forces into velocities [m/s]. -The potential functions are scalar fields representing potential energy (Newton-meters [Nm]). The gradient of the potential is represented as a force (Newtons [N]). To obtain velocity vectors, we use some parameter ($\zeta$ and $\eta$) that acts as an inverse damping coefficient (Newton-seconds / meter [Ns/m]) to convert the force into a velocity (meters / second [m/s]). +--- ## Attractive Potential -Attractive Potential is computed using a combined Conical and Quadratic potential function. This approach ensures a constant attractive force at large distances (Conical) to prevent high velocities, while switching to a quadratic behavior near the goal to ensure smooth convergence without chattering. -The transition occurs at a distance $\Gamma$, which can be dynamic based on the environment (e.g., clearance from obstacles) or a fixed parameter. + +The attractive potential combines a **quadratic** near the goal (for smooth convergence) with a **conical** far from the goal (for a constant-magnitude pull that avoids high velocities). The transition occurs at distance $\Gamma$. $$ U_{att}(q) = \begin{cases} -\frac{1}{2}\zeta D(q, q_{goal})^2 & D(q, q_{goal}) \le \Gamma \\ -\Gamma \zeta D(q, q_{goal}) - \frac{1}{2}\zeta \Gamma^2 & D(q, q_{goal}) > \Gamma +\dfrac{1}{2}\zeta\, D(q, q_{goal})^2 & D(q, q_{goal}) \le \Gamma \\[6pt] +\Gamma\zeta\, D(q, q_{goal}) - \dfrac{1}{2}\zeta\Gamma^2 & D(q, q_{goal}) > \Gamma \end{cases} $$ -The gradient (force) is: +The gradient gives the attractive force directed toward the goal: $$ \mathbf{F}_{att}(q) = -\nabla U_{att}(q) = \begin{cases} --\zeta (q - q_{goal}) & D(q, q_{goal}) \le \Gamma \\ --\frac{\Gamma \zeta}{D(q, q_{goal})} (q - q_{goal}) & D(q, q_{goal}) > \Gamma +-\zeta\,(q - q_{goal}) & D(q, q_{goal}) \le \Gamma \\[6pt] +-\dfrac{\Gamma\zeta}{D(q, q_{goal})}\,(q - q_{goal}) & D(q, q_{goal}) > \Gamma \end{cases} $$ -Where: -- $\zeta$ is the *attractive gain* parameter -- $D(q, q_{goal})$ is the Euclidean distance between $q$ and $q_{goal}$ -- $\Gamma$ is the quadratic threshold distance +| Symbol | Description | Units | +|--------|-------------|-------| +| $\zeta$ | Attractive gain | Ns/m | +| $q_{goal}$ | Goal pose | | +| $D(q, q_{goal})$ | Euclidean distance from current pose to goal | m | +| $\Gamma$ | Threshold distance between quadratic and conical regimes | m | + +> **Implementation note:** The conical branch is currently disabled in `computeAttractiveForceLinear` (`pfield.cpp`). The function returns the pure quadratic force $-\zeta(q - q_{goal})$ at all distances, because the conical branch was observed to cause oscillations. As a result, $\Gamma$ has no effect at runtime. See [Issue #44](https://github.com/argallab/potential_fields/issues/44) for the ongoing investigation. + +--- ## Rotational Attraction -Let $q_c$ be the current unit quaternion and $q_g$ the goal orientation. The geodesic distance $\theta \in [0,\pi]$ is the shortest rotation aligning $q_c$ to $q_g$. Define the quaternion difference -$$q_{diff} = q_c^{*} \otimes q_g$$ +Let $q_c$ be the current unit quaternion and $q_g$ the goal orientation. The geodesic distance $\theta \in [0, \pi]$ is the shortest rotation aligning $q_c$ to $q_g$. Define the quaternion difference and its rotation axis: + +$$ +q_{diff} = q_c^{*} \otimes q_g, \qquad \mathbf{u} = \frac{\vec{q}_{diff}}{\lVert\vec{q}_{diff}\rVert} +$$ + +The attractive rotational torque is proportional to the geodesic angle, applied only when $\theta$ exceeds a small threshold: -and the corresponding unit rotation axis $\mathbf{u} = \frac{\vec{q_{diff}}}{\lVert\vec{q_{diff}}\rVert}$. The attractive rotational torque is proportional to the geodesic angle and acts about $\mathbf{u}$: +$$ +\boldsymbol{\tau}_{att}(q) = -\,\omega\,\theta\,\mathbf{u} +$$ -$$\boldsymbol{\tau}_{att}(q) = -\,\omega\,\theta\,\mathbf{u}$$ +| Symbol | Description | Units | +|--------|-------------|-------| +| $\omega$ | Rotational attractive gain | Ns/rad | +| $\theta$ | Geodesic angle between current and goal orientations | rad | +| $\mathbf{u}$ | Unit rotation axis from $q_c$ toward $q_g$ | | -applied only when $\theta$ exceeds a small threshold. Here $\omega$ is the rotational attractive gain. +--- ## Repulsive Potential -Repulsion increases with proximity to each obstacle and is summed across obstacles. We use the obstacle surface’s signed distance $d$ and outward normal $\mathbf{n}_{out}$ at the closest point. Outside the obstacle $d \ge 0$; inside $d < 0$ is treated as $d \approx \varepsilon$ to produce a strong outward push. The gradient of the potential yields the repulsive force + +Repulsion increases with proximity to each obstacle and is zero beyond the influence distance $Q$. The signed distance $d$ to the obstacle surface is positive outside and negative inside; a value inside the obstacle is clamped to a small $\varepsilon > 0$ to produce a strong outward push. The repulsive force points along the outward surface normal $\mathbf{n}_{out}$: $$ \mathbf{F}_{rep}(q) = \begin{cases} -\eta\,\Big(\frac{1}{D(q)} - \frac{1}{Q}\Big)\,\frac{1}{D(q)^2}\,\mathbf{n}_{out} & 0 < D(q) < Q \\ +\eta\,\left(\dfrac{1}{D(q)} - \dfrac{1}{Q}\right)\dfrac{1}{D(q)^2}\,\mathbf{n}_{out} & 0 < D(q) < Q \\[8pt] \mathbf{0} & D(q) \ge Q \end{cases} $$ -Where: -- $\eta$ is the repulsive gain -- $Q$ is the influence distance -- $D(q)$ is the signed distance magnitude to the obstacle surface -- $\mathbf{n}_{out}$ is the outward surface normal at the closest point +This is summed over all obstacles in the environment. + +| Symbol | Description | Units | +|--------|-------------|-------| +| $\eta$ | Repulsive gain | Ns/m | +| $Q$ | Influence distance (repulsion is zero beyond this) | m | +| $D(q)$ | Signed distance from the query point to the obstacle surface | m | +| $\mathbf{n}_{out}$ | Outward unit normal of the obstacle surface at the closest point | | + +--- ## Total Potential, Wrench, and Twist +The total potential is the sum of attractive and all repulsive contributions. Its negative gradient gives the net force, which is combined with the rotational torque to form the wrench, then mapped to a twist: + $$ \begin{align} -U(q) &= U_{att}(q) + \sum_i U_{rep,i}(q) \\ -\mathbf{F}(q) &= -\nabla U(q) \quad\text{(linear force)} \\ -\boldsymbol{\tau}_{att}(q) &= -\,\omega\,\theta\,\mathbf{u} \quad\text{(rotational attraction)} \\ -\mathbf{v}(q) &= k_{lin}\,\mathbf{F}(q) \\ -\boldsymbol{\omega}(q) &= k_{ang}\,\boldsymbol{\tau}_{att}(q) +U(q) &= U_{att}(q) + \sum_i U_{rep,i}(q) \\[4pt] +\mathbf{F}(q) &= -\nabla U(q) \\[4pt] +\mathbf{v}(q) &= k_{lin}\,\mathbf{F}(q), \qquad \boldsymbol{\omega}(q) = k_{ang}\,\boldsymbol{\tau}_{att}(q) \end{align} $$ -Where: -- $q$ is the pose (position and orientation) -- $\mathbf{F}$ is the net linear force, the negative gradient of the scalar potential -- $\boldsymbol{\tau}_{att}$ is the attractive torque toward the goal orientation -- $\mathbf{v}$ and $\boldsymbol{\omega}$ are the linear and angular velocities after mapping with $(k_{lin}, k_{ang})$ +--- + +## Mitigating Local Minima (Planning-only) -### Integration and Motion Strategy -We conceptually utilize gradient descent to move down the potential’s gradient, but the implementation uses a velocity-constrained Runge–Kutta (RK4) step over the twist for better stability and constraint handling. In pseudocode, gradient descent looks like: +A local minimum occurs when the attractive and repulsive forces exactly cancel, causing the planner to stall, most commonly when the robot is directly between a goal and an obstacle. To reduce sticking, the planner strips the repulsive component that directly opposes the attractive direction while keeping tangential components that encourage the robot to slide around the obstacle: $$ -\begin{align*} -&q[0] = q_{start} \\ -&i = 0 \\ -&\text{while }\Vert\nabla U(q_i)\Vert > \epsilon \\ -& q_{i+1} = q_i - \alpha_i \nabla U(q_i) \\ -& i = i + 1 -\end{align*} +\mathbf{F}_{rep}^{\text{filtered}} = \mathbf{F}_{rep} - \min\big(0,\, \mathbf{F}_{rep}\cdot \hat{\mathbf{u}}_{att}\big)\,\hat{\mathbf{u}}_{att}, \qquad \hat{\mathbf{u}}_{att} = \frac{\mathbf{F}_{att}}{\lVert\mathbf{F}_{att}\rVert} $$ -Where: -- $\epsilon$ parameterizes when to stop -- $\alpha_i$ is a step-size (learning rate) +The $\min(0, \cdot)$ ensures only the opposing (negative) projection is removed; repulsion that is already tangential or aligned with attraction is left unchanged. This filtering is applied only during planning, the unfiltered field is used for real-time control and visualization. -#### Runge-Kutta 4 (RK4) Integration -We integrate over the twist (linear and angular velocities) using a constrained RK4 step. Each stage evaluates a constrained twist at an intermediate pose and time step, then averages the four stages to advance the pose. +--- + +## Path Planning Algorithm + +This library supports two planning methods with different trade-offs between computational cost and collision-avoidance coverage. Both follow the same conceptual strategy: at each step, evaluate the potential field, convert the gradient to a velocity, and integrate to advance the robot's state. The planner stops when the robot reaches the goal within a positional tolerance. + +### Task-Space Wrench Path Planning + +Forces and torques are evaluated at the **end-effector** in Cartesian space. The resulting twist is integrated directly in task space, and IK solves for the joint configuration at each waypoint. + +**Best suited for:** uncluttered environments where precise end-effector path control matters more than link-level collision avoidance. -Given current pose $q_i = (\mathbf{x}_i, \mathbf{R}_i)$ and previous applied twist $T_{i-1}$, with step size $\Delta t$: +**Steps:** -1) Stage 1 +1. **Wrench**: sum attractive and repulsive contributions at the end-effector: $$ -\begin{aligned} -\mathbf{k}_1 &= T_c\big(q_i,\, T_{i-1},\, \Delta t\big) \\ -q_{i}^{(1/2)} &= \Big(\, \mathbf{x}_i + \tfrac{\Delta t}{2}\,\mathbf{v}_1\,,\; \mathbf{R}_i\;\exp\big( [\boldsymbol{\omega}_1]_\times \tfrac{\Delta t}{2} \big) \Big) -\end{aligned} +\mathcal{W}_{task} = \begin{bmatrix} \mathbf{F}_{att} + \mathbf{F}_{rep} \\ \boldsymbol{\tau}_{att} \end{bmatrix} $$ -2) Stage 2 +2. **Twist**: apply admittance gains: $$ -\begin{aligned} -\mathbf{k}_2 &= T_c\big(q_{i}^{(1/2)},\, \mathbf{k}_1,\, \tfrac{\Delta t}{2}\big) \\ -q_{i}^{(1/2)'} &= \Big(\, \mathbf{x}_i + \tfrac{\Delta t}{2}\,\mathbf{v}_2\,,\; \mathbf{R}_i\;\exp\big( [\boldsymbol{\omega}_2]_\times \tfrac{\Delta t}{2} \big) \Big) -\end{aligned} +\mathcal{V}_{task} = \begin{bmatrix} k_{lin} & \mathbf{0} \\ \mathbf{0} & k_{ang} \end{bmatrix} \mathcal{W}_{task} $$ -3) Stage 3 +3. **Integrate**: advance the end-effector pose with RK4 (see [RK4 Integration](#runge-kutta-4-rk4-integration)): $$ -\begin{aligned} -\mathbf{k}_3 &= T_c\big(q_{i}^{(1/2)'},\, \mathbf{k}_2,\, \tfrac{\Delta t}{2}\big) \\ -q_{i}^{(1)} &= \Big(\, \mathbf{x}_i + \Delta t\,\mathbf{v}_3\,,\; \mathbf{R}_i\;\exp\big( [\boldsymbol{\omega}_3]_\times \Delta t \big) \Big) -\end{aligned} +x_{next} = \mathrm{RK4}(x_{curr},\, \mathcal{V}_{task},\, \Delta t) $$ -4) Stage 4 +#### Runge-Kutta 4 (RK4) Integration + +Rather than a simple Euler step, the planner uses a constrained RK4 scheme for better accuracy and stability. Each of the four stages evaluates a constrained twist at an intermediate pose, applies velocity and acceleration limits, then the four stages are averaged for the final update. +Given current pose $q_i = (\mathbf{x}_i, \mathbf{R}_i)$, previous applied twist $T_{i-1}$, and step size $\Delta t$: + +**Stage 1**: evaluate at the current pose: $$ -\mathbf{k}_4 = T_c\big(q_{i}^{(1)},\, \mathbf{k}_3,\, \Delta t\big) +\mathbf{k}_1 = T_c\big(q_i,\, T_{i-1},\, \Delta t\big), \qquad +q_{i}^{(1/2)} = \Big(\mathbf{x}_i + \tfrac{\Delta t}{2}\mathbf{v}_1,\; \mathbf{R}_i\exp\!\big([\boldsymbol{\omega}_1]_\times \tfrac{\Delta t}{2}\big)\Big) +$$ + +**Stage 2**: evaluate at the half-step pose from stage 1: +$$ +\mathbf{k}_2 = T_c\big(q_{i}^{(1/2)},\, \mathbf{k}_1,\, \tfrac{\Delta t}{2}\big), \qquad +q_{i}^{(1/2)'} = \Big(\mathbf{x}_i + \tfrac{\Delta t}{2}\mathbf{v}_2,\; \mathbf{R}_i\exp\!\big([\boldsymbol{\omega}_2]_\times \tfrac{\Delta t}{2}\big)\Big) $$ -Weighted average twist (component-wise for linear and angular parts): +**Stage 3**: evaluate at the half-step pose from stage 2: +$$ +\mathbf{k}_3 = T_c\big(q_{i}^{(1/2)'},\, \mathbf{k}_2,\, \tfrac{\Delta t}{2}\big), \qquad +q_{i}^{(1)} = \Big(\mathbf{x}_i + \Delta t\,\mathbf{v}_3,\; \mathbf{R}_i\exp\!\big([\boldsymbol{\omega}_3]_\times \Delta t\big)\Big) +$$ +**Stage 4**: evaluate at the full-step pose from stage 3: $$ -\bar{\mathbf{v}} = \frac{\mathbf{v}_1 + 2\mathbf{v}_2 + 2\mathbf{v}_3 + \mathbf{v}_4}{6},\quad -\bar{\boldsymbol{\omega}} = \frac{\boldsymbol{\omega}_1 + 2\boldsymbol{\omega}_2 + 2\boldsymbol{\omega}_3 + \boldsymbol{\omega}_4}{6}. +\mathbf{k}_4 = T_c\big(q_{i}^{(1)},\, \mathbf{k}_3,\, \Delta t\big) $$ -The averaged twist is re-limited (soft saturation and rate limits) to ensure it respects the velocity/acceleration bounds. The pose is then advanced once: +**Weighted average** of the four stage twists: +$$ +\bar{\mathbf{v}} = \frac{\mathbf{v}_1 + 2\mathbf{v}_2 + 2\mathbf{v}_3 + \mathbf{v}_4}{6}, \qquad +\bar{\boldsymbol{\omega}} = \frac{\boldsymbol{\omega}_1 + 2\boldsymbol{\omega}_2 + 2\boldsymbol{\omega}_3 + \boldsymbol{\omega}_4}{6} +$$ +The averaged twist is re-limited (soft saturation and rate limits) before the final pose update: $$ -\mathbf{x}_{i+1} = \mathbf{x}_i + \bar{\mathbf{v}}\,\Delta t,\qquad -\mathbf{R}_{i+1} = \mathbf{R}_i\;\exp\big( [\bar{\boldsymbol{\omega}}]_\times\, \Delta t \big), +\mathbf{x}_{i+1} = \mathbf{x}_i + \bar{\mathbf{v}}\,\Delta t, \qquad +\mathbf{R}_{i+1} = \mathbf{R}_i\;\exp\!\big([\bar{\boldsymbol{\omega}}]_\times\,\Delta t\big) $$ -where $\exp([\boldsymbol{\omega}]_\times\, \Delta t)$ is implemented via an angle–axis exponential map (i.e., $\mathrm{AngleAxis}(|\boldsymbol{\omega}|\,\Delta t,\, \boldsymbol{\omega}/|\boldsymbol{\omega}|)$) and the resulting quaternion is normalized. +where $\exp([\boldsymbol{\omega}]_\times\,\Delta t)$ is the angle–axis exponential map $\mathrm{AngleAxis}(|\boldsymbol{\omega}|\Delta t,\, \boldsymbol{\omega}/|\boldsymbol{\omega}|)$, with the resulting quaternion normalized. -Notes: -- $T_c(\cdot)$ applies planning-only opposing-force removal, maps wrench to twist, and enforces motion constraints for each stage. -- Per-stage rate limits use the stage’s effective step size ($\Delta t$ for stages 1 and 4, $\Delta t/2$ for stages 2–3) by supplying the previous stage’s twist to the constraint function. -- After averaging, constraints are applied again before the final integration step (as in the implementation). +| Symbol | Description | Units | +|--------|-------------|-------| +| $T_c(\cdot)$ | Constrained twist function: applies local-minima filtering, maps wrench to twist, enforces velocity and acceleration limits | | +| $[\boldsymbol{\omega}]_\times$ | Skew-symmetric matrix of $\boldsymbol{\omega}$ (so(3) element for the exponential map) | | +| $\Delta t$ | Integration timestep | s | -### Velocity and Acceleration Limits -Before integrating the twist, linear and angular speeds are soft-saturated by norm and then rate-limited using maximum linear/angular accelerations over the step size $\Delta t$. The library enforces: +> **Note:** Per-stage rate limits use the stage's effective step size ($\Delta t$ for stages 1 and 4, $\Delta t/2$ for stages 2–3). After averaging, constraints are applied once more before the final integration step. -- Max linear speed $\lVert\mathbf{v}\rVert \leq v_{max}$ -- Max angular speed $\lVert\boldsymbol{\omega}\rVert \leq \omega_{max}$ -- Linear/Angular acceleration limits over $\Delta t$ +#### Velocity and Acceleration Limits -This produces smooth, feasible motions while following the potential field. +Before integrating each twist, linear and angular velocities are constrained in two passes: -#### Soft Saturation -Soft saturation smoothly caps a vector’s norm while preserving its direction. For a vector $\mathbf{v}$, maximum allowed norm $v_{max}$, and softness parameter $\beta$: +**Soft saturation**: smoothly caps a vector's norm to $v_{max}$ while preserving its direction, using the $\tanh$ function to avoid hard clipping: $$ -\mathbf{v}_{sat} \;=\; \mathbf{v} \cdot \frac{v_{max}\,\tanh\!\left(\beta\,\lVert\mathbf{v}\rVert / v_{max}\right)}{\lVert\mathbf{v}\rVert}. +\mathbf{v}_{sat} = \mathbf{v}\cdot\frac{v_{max}\,\tanh\!\left(\beta\,\lVert\mathbf{v}\rVert / v_{max}\right)}{\lVert\mathbf{v}\rVert} $$ -- If $\lVert\mathbf{v}\rVert \ll v_{max}$, $\tanh(x) \approx x$ and the scaling is nearly linear (no abrupt clipping). -- As $\lVert\mathbf{v}\rVert \to \infty$, the saturated norm approaches $v_{max}$ asymptotically. -- Higher $\beta$ produces a steeper transition near $v_{max}$; lower $\beta$ makes it gentler. - -This is applied to both linear and angular velocities before rate limiting. +When $\lVert\mathbf{v}\rVert \ll v_{max}$ the scaling is nearly linear; as $\lVert\mathbf{v}\rVert \to \infty$ the saturated norm asymptotes to $v_{max}$. Higher $\beta$ gives a sharper transition near the cap. -#### Rate Limiting -Rate limiting bounds the change from the previous vector $\mathbf{v}_{prev}$ to the current target $\mathbf{v}_{curr}$ by a maximum step $\Delta_{max}$ (e.g., from acceleration limits over $\Delta t$): +**Rate limiting**: bounds the change from the previous vector $\mathbf{v}_{prev}$ to the current target by a maximum step $\Delta_{max}$ (derived from acceleration limits over $\Delta t$): $$ -\mathbf{d} = \mathbf{v}_{curr} - \mathbf{v}_{prev},\quad -\mathbf{v}_{rl} = -\begin{cases} -\mathbf{v}_{curr}, & \lVert\mathbf{d}\rVert \le \Delta_{max} \\ -\mathbf{v}_{prev} + \mathbf{d}\,\dfrac{\Delta_{max}}{\lVert\mathbf{d}\rVert}, & \lVert\mathbf{d}\rVert > \Delta_{max} +\mathbf{d} = \mathbf{v}_{curr} - \mathbf{v}_{prev}, \qquad +\mathbf{v}_{rl} = \begin{cases} +\mathbf{v}_{curr} & \lVert\mathbf{d}\rVert \le \Delta_{max} \\[4pt] +\mathbf{v}_{prev} + \mathbf{d}\,\dfrac{\Delta_{max}}{\lVert\mathbf{d}\rVert} & \lVert\mathbf{d}\rVert > \Delta_{max} \end{cases} $$ -This guarantees the step size is never larger than $\Delta_{max}$ while preserving the intended direction of change. After rate limiting, soft saturation is re-applied to ensure the final vector also respects the velocity caps. +Soft saturation is re-applied after rate limiting to ensure the final vector satisfies both constraints. + +| Symbol | Description | Units | +|--------|-------------|-------| +| $v_{max}$ | Maximum linear speed | m/s | +| $\omega_{max}$ | Maximum angular speed | rad/s | +| $\beta$ | Soft-saturation steepness parameter | | +| $\Delta_{max}$ | Maximum allowed velocity change per step (acceleration limit $\times\,\Delta t$) | m/s | -### Mitigating Local Minima (Planning-only) -To reduce sticking on broad obstacle faces, the planner removes only the repulsive component that directly opposes the attractive force and keeps tangential components that encourage sliding: +--- + +### Whole-Body Velocity Path Planning + +This method extends potential field planning to the full robot body by placing repulsive control points on every link, not just the end-effector. The approach follows §4.7.2–4.7.3 of Choset et al. [2]. + +**Best suited for:** cluttered environments where links and elbows risk colliding with obstacles. Every link of the arm contributes a repulsive force, so the robot naturally folds or repositions itself while driving the end-effector toward the goal. + +#### Workspace Forces → Configuration Space Forces (§4.7.1) + +Potential functions are easiest to define in the workspace ($\mathbb{R}^3$), but planning requires forces in **configuration space** (joint torques). We bridge these using the **principle of virtual work**: power is coordinate-independent, so $f^T\dot{x} = u^T\dot{q}$. With $\dot{x} = J(q)\dot{q}$, this gives: $$ -\mathbf{F}_{rep}^{\text{filtered}} = \mathbf{F}_{rep} - \min\big(0,\, \mathbf{F}_{rep}\cdot \mathbf{u}_{att}\big)\,\mathbf{u}_{att},\quad \mathbf{u}_{att} = \frac{\mathbf{F}_{att}}{\lVert\mathbf{F}_{att}\rVert}. +u = J^T(q)\,f $$ -This filtering is applied only during planning; the unfiltered field is kept for visualization and testing semantics. +A workspace force $f$ acting at any point $x = \phi(q)$ maps to a generalized joint-space force $u$ via the transpose of the Jacobian at that point. -## Path Planning Algorithm -This library supports two distinct path planning methods, each with different trade-offs regarding computational complexity and collision avoidance coverage. +#### Control Points (§4.7.2) -### Task-Space Wrench Path Planning -This method calculates forces and torques (wrench) directly at the end-effector in the task space (Cartesian space). The wrench is converted into a twist (velocity) and integrated to update the end-effector's pose. Inverse Kinematics (IK) is then used to solve for the joint angles that achieve this pose. +Rather than integrating forces over the robot's volume, a finite set of **control points** $\{r_j\}$ is selected on the robot body. Each control point has its own workspace potential; the resulting workspace forces are mapped to joint torques via $J^T$ and summed in configuration space. -**Main Benefit:** Computationally efficient and provides precise control over the end-effector's trajectory. Best suited when the primary concern is the end-effector's path and the robot arm is relatively unobstructed. +**Floating control point:** Fixed points at link origins or vertices may miss the true closest approach between a link and an obstacle. For example, the midpoint of a long link edge can be far closer to an obstacle than either endpoint. The textbook therefore introduces a *floating* control point $r_\text{float}$: the point on the robot boundary closest to any obstacle at the current configuration $q$. Its repulsive force is computed identically to any other control point. -**Key Evaluations:** -1. **Task-Space Wrench:** Sum of attractive and repulsive forces/torques at the end-effector. +**In this implementation**, each link is approximated as a **capsule** fitted to the URDF mesh via PCA. The capsule's two endcap centers and shaft midpoint are the fixed candidate control points per link. For each link–obstacle pair, the candidate with the smallest clearance is selected as the floating control point, giving a smooth and geometrically tight clearance estimate as the configuration changes. -$$ \mathcal{W}_{task} = \begin{bmatrix} \mathbf{F}_{att} + \mathbf{F}_{rep} \\ \boldsymbol{\tau}_{att} \end{bmatrix} $$ +#### Repulsive Potential at a Control Point (§4.7.2, eq. 4.24–4.25) -2. **Task-Space Twist:** Wrench converted to velocity using admittance gains ($k_{lin} = k_{ang} = 1.0$). +For control point $r_j$ and obstacle $\mathcal{WO}_i$, the repulsive potential and its gradient (the workspace repulsive force) are: -$$ \mathcal{V}_{task} = \begin{bmatrix} k_{lin} & \mathbf{0} \\ \mathbf{0} & k_{ang} \end{bmatrix} \mathcal{W}_{task} $$ +$$ +U_{\text{rep},i,j}(q) = +\begin{cases} +\dfrac{1}{2}\eta_j \left(\dfrac{1}{d_i(r_j(q))} - \dfrac{1}{Q^*_i}\right)^2 & d_i(r_j(q)) \le Q^*_i \\[8pt] +0 & d_i(r_j(q)) > Q^*_i +\end{cases} +$$ -3. **Integration:** The twist is integrated (using RK4) to find the next end-effector pose $x_{next}$. +$$ +\nabla U_{\text{rep},i,j}(q) = +\begin{cases} +\eta_j \left(\dfrac{1}{Q^*_i} - \dfrac{1}{d_i(r_j(q))}\right)\dfrac{1}{d_i^2(r_j(q))}\,\nabla d_i(r_j(q)) & d_i(r_j(q)) \le Q^*_i \\[8pt] +0 & d_i(r_j(q)) > Q^*_i +\end{cases} +$$ -$$ x_{next} = \text{RK4}(x_{curr}, \mathcal{V}_{task}, \Delta t) $$ +$\nabla d_i(r_j(q))$ is the outward unit normal of the obstacle surface at the point closest to $r_j$. -### Whole-Body Velocity Path Planning -This method calculates virtual forces acting on *every* link of the robot body, not just the end-effector. These forces are mapped to joint torques using the Jacobian transpose of each link. The total joint torques are then converted to joint velocities, which are integrated to update the robot's configuration directly. +| Symbol | Description | Units | +|--------|-------------|-------| +| $r_j$ | Control point $j$ on the robot body | | +| $d_i(r_j(q))$ | Shortest distance from control point $r_j$ to obstacle $\mathcal{WO}_i$ | m | +| $Q^*_i$ | Influence distance for obstacle $i$ | m | +| $\eta_j$ | Repulsive gain for control point $r_j$ | Ns/m | +| $\nabla d_i(r_j(q))$ | Outward unit normal of obstacle $i$'s surface at the closest point to $r_j$ | | + +#### Total Configuration Space Force (§4.7.2, eq. 4.26) -**Main Benefit:** Provides whole-body collision avoidance. The robot "feels" obstacles along its entire arm and will naturally fold or move its elbows to avoid them while trying to reach the goal. Ideal for cluttered environments. +The total generalized force is the sum of end-effector attraction and whole-body repulsion, accumulated over all links and all obstacles. Crucially, this summation happens in **configuration space**, summing workspace forces directly would be incorrect because the same workspace force has different joint-torque implications depending on which link it acts on. -**Key Evaluations:** -1. **Joint Torques:** Sum of end-effector attraction and whole-body repulsion mapped to joint space. +$$ +u(q) = \underbrace{J_{ee}^T(q)\,\mathbf{F}_{att}}_{\text{EE attraction}} \;+\; \underbrace{\sum_{j \in \text{links}}\;\sum_{i \in \text{obstacles}} J_j^T(q)\,\nabla U_{\text{rep},i,j}(q)}_{\text{whole-body repulsion}} +$$ -$$ \boldsymbol{\tau}_{joints} = J_{ee}^T(\mathbf{q}) \mathcal{W}_{att} + \sum_{i \in \text{links}} J_{i}^T(\mathbf{q}) \mathbf{F}_{rep, i} $$ +| Symbol | Description | Units | +|--------|-------------|-------| +| $J_{ee}(q)$ | Jacobian at the end-effector | | +| $\mathbf{F}_{att}$ | Attractive force toward the goal, evaluated at the end-effector | N | +| $J_j(q)$ | Jacobian evaluated at the floating control point on link $j$ | | -2. **Joint Velocities:** Torques converted to velocities using an admittance gain. +#### Joint Velocity Integration -$$ \dot{\mathbf{q}} = k_{adm} \boldsymbol{\tau}_{joints} $$ +The total configuration space force $u(q)$ is treated as a joint velocity command via an admittance gain, then integrated with an Euler step: + +$$ +\dot{\mathbf{q}} = k_{\text{adm}}\,u(q), \qquad \mathbf{q}_{next} = \mathbf{q}_{curr} + \dot{\mathbf{q}}\,\Delta t +$$ -_Can be replaced with the [Robot Dynamics Equation](https://github.com/argallab/pfields_2025/issues/23)_ +| Symbol | Description | Units | +|--------|-------------|-------| +| $k_{\text{adm}}$ | Admittance gain (scales configuration-space force to joint velocity) | | +| $\dot{\mathbf{q}}$ | Joint velocity vector | rad/s | -3. **Integration:** Joint velocities are integrated (Euler) to find the next joint configuration. +> **Note:** The admittance mapping $\dot{\mathbf{q}} = k_{\text{adm}}\,u(q)$ can be replaced by the full robot dynamics equation (using the mass matrix and Coriolis terms) for more physically accurate joint velocity commands (see [Issue #23](https://github.com/argallab/potential_fields/issues/23)). -$$ \mathbf{q}_{next} = \mathbf{q}_{curr} + \dot{\mathbf{q}} \Delta t $$ +--- # References - [1] [Real-Time Obstacle Avoidance for Manipulators and Mobile Robots](https://ieeexplore.ieee.org/stamp/stampCaCancaCccCfaksdfojas;kfjd;ksldadjf.jsp?tp=&arnumber=1087247) +[1] [Real-Time Obstacle Avoidance for Manipulators and Mobile Robots](https://ieeexplore.ieee.org/document/1087247) - ``` - O. Khatib, “Real-time obstacle avoidance for manipulators and mobile robots,” in Proc. 1985 IEEE Int. Conf. Robotics and Automation, vol. 2, pp. 500–505, 1985, doi: 10.1109/ROBOT.1985.1087247. - ``` +``` +O. Khatib, "Real-time obstacle avoidance for manipulators and mobile robots," in Proc. 1985 IEEE Int. Conf. Robotics and Automation, vol. 2, pp. 500–505, 1985, doi: 10.1109/ROBOT.1985.1087247. +``` - [2] [Principles of Robot Motion: Theory, Algorithms, and Implementations](https://ieeexplore.ieee.org/book/6267238) +[2] [Principles of Robot Motion: Theory, Algorithms, and Implementations](https://ieeexplore.ieee.org/book/6267238) - ``` - H. Choset, K. M. Lynch, S. Hutchinson, G. A. Kantor, W. Burgard, L. E. Kavraki, and S. Thrun, Principles of Robot Motion: Theory, Algorithms, and Implementations. Cambridge, MA: MIT Press, 2005. - ``` +``` +H. Choset, K. M. Lynch, S. Hutchinson, G. A. Kantor, W. Burgard, L. E. Kavraki, and S. Thrun, Principles of Robot Motion: Theory, Algorithms, and Implementations. Cambridge, MA: MIT Press, 2005. +``` diff --git a/potential_fields/CHANGELOG.rst b/potential_fields/CHANGELOG.rst new file mode 100644 index 00000000..1e70baa9 --- /dev/null +++ b/potential_fields/CHANGELOG.rst @@ -0,0 +1,135 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package potential_fields +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.2 (2026-04-04) +------------------ +* Clarified Obstacle type's geometry fields +* Moved HSVtoRGB to pfield_manager instead +* Estimate Robot Geometry using Ellipsoids and Control Points for smooth WBV Repulsion (`#42 `_) + Co-authored-by: Claude Sonnet 4.6 +* 40 test main branch ruleset (testing Issue `#40 `_) (`#41 `_) +* README edits and CONTRIBUTING.md +* Created visualization method to visualize both TS and WBV +* Fixed CSV bugs and tested demo1 and demo2, TS planning has problems +* 1.0.1 +* Rename Packages (`#35 `_) +* Enabling Github Workflow for building and testing repo (`#33 `_) +* Successfully migrated all unit tests to `pfield_library` (`#32 `_) +* December Final Demos (`#26 `_) +* Use Robot Dynamics to convert Joint Torques to Joint Velocities (`#24 `_) +* Independent pfield C++ Library (`#22 `_) +* Updating ComputeAutonomyVector +* Collision Avoidance between robot links and environment obstacles (`#21 `_) +* XArm 7-DoF Arm Demo (`#19 `_) +* Dynamic D* Threshold (`#20 `_) +* Aligned AttractiveMoment Doc with Implementation +* Fixing Rotational Attraction (`#18 `_) +* Editing gains +* Fixing goal and query pose visualizations +* Fixing plotting functions and organizing code (`#16 `_) + Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> +* Adding Reference to Principles of Robot Motion and Updating PF Equations (`#15 `_) +* Updating repulsive force computation from Choset text +* Repulsive force piecewise equation includes equals to QStar +* Merged in extra fixes +* Comments cleanup +* Using RK4 for integrating position from PF Twist (`#13 `_) +* Tested another demo with plots +* Docstrings for new functions +* First pass at RK4 +* Fixing up tests +* Prototyping path planning (`#11 `_) +* Refactored influence scale and mesh collision (`#12 `_) +* Merge branch 'main' of github.com:argallab/pfields_2025 +* Merged RobotParser into PFieldManager (`#10 `_) +* Current TODO +* Pinocchio for Obstacles and Motion Constraints (`#9 `_) +* PlanPath Service Demo (`#7 `_) +* Planning "World" for path-planning with robot (`#6 `_) +* Motion Interface (`#5 `_) +* Update license to Apache 2.0 +* Updating email to northwestern email for packages +* Updating functions to write to folders +* Documentation +* Updating robot parser targets +* typo +* Custom service to plan path in PF +* Merge pull request `#4 `_ from argallab/demo-package + Example Package +* Changing error to warn +* Launch arguments and parsing raw URDF +* Created robot parser node to listen for the transforms and parse the URDF collision objects +* Typo +* RViz sets fixed frame from user +* Moving static transforms to demo repository +* Updating build info +* Removing extra forward slash +* Another delta robot typo +* Fixing typo with delta robot include dir +* Created interpolate and integration functions for PField linear velocity and angular velocity +* A bit of hacking to get obstacles where the URDF defines them +* Created TF from world +* Attempting to fix TF listener and collision objects +* Fixed ID incrementing +* Fixed parsing problem but collision objects aren't showing up +* Created URDF testing file +* Adding urdf parser +* Updating the transforms of every obstacle +* Added path trajectory and tf at world frame +* Merge pull request `#2 `_ from argallab/more-obstacles + More Obstacle Support +* Moved rotated object to new position +* Fixed frequency in timer +* Removed the Lerp +* Updated Obstacle class, tests, and visualizer +* Added default case to withinInfluenceZone +* Converted SphereObstacle to PFObstacle class +* Cleaned up file comment +* documentation and normalizePosition +* Deleted default constructor for sphereObstacle to force data entry +* Followed NL.16 and removed extra public: statements +* Adjusting units and adding notes +* Deleted influence_scale_radius param since it's unused +* Changing query point size +* Visualize a query point +* Visualize a query point +* Visualizing the orientation of the goal and setting the goal pose with subscriber to /goal_pose +* Created an interfaces package +* Merge pull request `#1 `_ from argallab/eigenify + Implementing Vector operations with Eigen3 +* Few more tests for angular distance +* Removed geodesic distance function and replaced with Eigen angularDistance function +* euler functions are now YPR and comments accordingly +* Added more tests and verified euler to quaternion method +* Collapsed implementation into header +* Fixed euler conversion +* All tests pass but need to write more +* Added normalizePosition function +* Fixed manager with new syntax and Eigen vectors +* Edited attraction and repulsion functions using Eigen and new SpatialVector formatting +* Changed tests to match new syntax +* Reimplemented position and orientation with eigen and removed operator overloads and changed floats to doubles +* Changed position to an Eigen::Vector3d and changed floats to doubles +* Adjusted language and changed floats to doubles +* Built PKG with Eigen dependency and with include statement +* Added rotational attractive gain and equation to README +* Wrote geodesic formulation in README and function as commented code +* Replaced geodesic distance with raw implementation inside Quaternion difference +* Wrote tests but orientation methods are failing +* Removing uncrustify tests +* Reverted formatting of uncrustify +* Reformated with uncrustify and created tests +* Attraction now includes orientation +* Vector3D -> SpatialVector +* Meeting notes +* Cleaning up docs in pfield vector markers +* Drew arrows in RViz +* Saved rviz config +* Included RViz and added config and launch folders/files +* Started writing node +* Edited math to match the python version that created graphs +* Updating force computation +* Docstrings +* Started working on implementation of C++ Potential Fields in a ROS2 pkg +* Contributors: Demiana Barsoum, Sharwin Patil, Sharwin24 diff --git a/potential_fields/include/potential_fields/ros/pfield_manager.hpp b/potential_fields/include/potential_fields/ros/pfield_manager.hpp index 17a3c972..1d805920 100644 --- a/potential_fields/include/potential_fields/ros/pfield_manager.hpp +++ b/potential_fields/include/potential_fields/ros/pfield_manager.hpp @@ -54,26 +54,38 @@ #include "rclcpp/rclcpp.hpp" -using Marker = visualization_msgs::msg::Marker; -using MarkerArray = visualization_msgs::msg::MarkerArray; -using Pose = geometry_msgs::msg::Pose; -using Point = geometry_msgs::msg::Point; -using Quaternion = geometry_msgs::msg::Quaternion; -using Path = nav_msgs::msg::Path; -using TransformStamped = geometry_msgs::msg::TransformStamped; -using Obstacle = potential_fields_interfaces::msg::Obstacle; -using ObstacleArray = potential_fields_interfaces::msg::ObstacleArray; -using PlanPath = potential_fields_interfaces::srv::PlanPath; -using ComputeAutonomyVector = potential_fields_interfaces::srv::ComputeAutonomyVector; -using JointState = sensor_msgs::msg::JointState; +// Short aliases for ROS message/service types used throughout the node. +// Scoped to pfield_manager_types to avoid polluting headers that include this file. +namespace pfield_manager_types { + using MarkerMsg = visualization_msgs::msg::Marker; + using MarkerArrayMsg = visualization_msgs::msg::MarkerArray; + using PoseMsg = geometry_msgs::msg::Pose; + using PoseStampedMsg = geometry_msgs::msg::PoseStamped; + using PointMsg = geometry_msgs::msg::Point; + using QuaternionMsg = geometry_msgs::msg::Quaternion; + using TwistMsg = geometry_msgs::msg::Twist; + using TwistStampedMsg = geometry_msgs::msg::TwistStamped; + using PathMsg = nav_msgs::msg::Path; + using TransformStampedMsg = geometry_msgs::msg::TransformStamped; + using ObstacleMsg = potential_fields_interfaces::msg::Obstacle; + using ObstacleArrayMsg = potential_fields_interfaces::msg::ObstacleArray; + using PlanPathSrv = potential_fields_interfaces::srv::PlanPath; + using ComputeAutonomyVectorSrv = potential_fields_interfaces::srv::ComputeAutonomyVector; + using JointStateMsg = sensor_msgs::msg::JointState; + using JointTrajectoryMsg = trajectory_msgs::msg::JointTrajectory; + using JointTrajectoryPointMsg = trajectory_msgs::msg::JointTrajectoryPoint; +} // namespace pfield_manager_types + +// Bring aliases into scope for this header and its includers. +using namespace pfield_manager_types; // NOLINT(build/namespaces) class PotentialFieldManager : public rclcpp::Node { public: PotentialFieldManager(); ~PotentialFieldManager() = default; - static Quaternion getQuaternionFromYaw(double yaw) { - Quaternion q; + [[nodiscard]] static QuaternionMsg getQuaternionFromYaw(double yaw) { + QuaternionMsg q; q.x = 0.0; q.y = 0.0; q.z = sin(yaw / 2.0); @@ -115,42 +127,43 @@ class PotentialFieldManager : public rclcpp::Node { const double TASK_SPACE_AXIS_HEAD_DIAMETER = 0.025; // Diameter of axis heads [m] rclcpp::TimerBase::SharedPtr timer; // Timer to periodically update the potential field - rclcpp::Publisher::SharedPtr pFieldMarkerPub; // Publisher for PF Markers - rclcpp::Publisher::SharedPtr planningJointStatePub; // Publisher for planning joint states - rclcpp::Publisher::SharedPtr plannedEndEffectorPathPub; // Publisher for planned end-effector path - rclcpp::Subscription::SharedPtr goalPoseSub; // Subscriber for the goal pose - rclcpp::Subscription::SharedPtr queryPoseSub; // Subscriber for query poses - rclcpp::Subscription::SharedPtr obstacleSub; // Subscriber for obstacles - rclcpp::Service::SharedPtr pathPlanningService; // Now hosted here - rclcpp::Service::SharedPtr autonomyVectorService; // Service to compute velocity vector at a given pose + rclcpp::Publisher::SharedPtr pFieldMarkerPub; // Publisher for PF Markers + rclcpp::Publisher::SharedPtr planningJointStatePub; // Publisher for planning joint states + rclcpp::Publisher::SharedPtr plannedEndEffectorPathPub; // Publisher for planned end-effector path + rclcpp::Subscription::SharedPtr goalPoseSub; // Subscriber for the goal pose + rclcpp::Subscription::SharedPtr queryPoseSub; // Subscriber for query poses + rclcpp::Subscription::SharedPtr obstacleSub; // Subscriber for obstacles + rclcpp::Service::SharedPtr pathPlanningService; // Now hosted here + // Service to compute velocity vector at a given pose + rclcpp::Service::SharedPtr autonomyVectorService; void timerCallback(); // Service callbacks - void handlePlanPath(const PlanPath::Request::SharedPtr request, PlanPath::Response::SharedPtr response); + void handlePlanPath(const PlanPathSrv::Request::SharedPtr request, PlanPathSrv::Response::SharedPtr response); void handleComputeAutonomyVector( - const ComputeAutonomyVector::Request::SharedPtr request, ComputeAutonomyVector::Response::SharedPtr response); + const ComputeAutonomyVectorSrv::Request::SharedPtr request, ComputeAutonomyVectorSrv::Response::SharedPtr response); /** * @brief Given a potential field, generate all visualization markers * (goal, obstacles, obstacle influence zones, velocity vectors) * * @param pf The potential field instance - * @return MarkerArray The marker array containing all visualization markers + * @return MarkerArrayMsg The marker array containing all visualization markers */ - MarkerArray visualizePF(std::shared_ptr pf); + MarkerArrayMsg visualizePF(std::shared_ptr pf); - MarkerArray createQueryPoseMarker(); + MarkerArrayMsg createQueryPoseMarker(); - MarkerArray createThresholdMarkers(std::shared_ptr pf); + MarkerArrayMsg createThresholdMarkers(std::shared_ptr pf); /** * @brief Get Obstacle and Obstacle Influence Zone markers from a potential field * * @param pf The potential field instance - * @return MarkerArray The marker array containing all obstacle markers + * @return MarkerArrayMsg The marker array containing all obstacle markers */ - MarkerArray createObstacleMarkers(std::shared_ptr pf); + MarkerArrayMsg createObstacleMarkers(std::shared_ptr pf); /** * @brief Build obstacle shape markers and their semi-transparent influence-zone overlays @@ -158,10 +171,10 @@ class PotentialFieldManager : public rclcpp::Node { * * @param obstacles Combined list of environment and robot obstacles. * @param influenceDistance Repulsive influence radius [m]. - * @return MarkerArray Markers in namespaces "robot_obstacles", "environment_obstacles", + * @return MarkerArrayMsg Markers in namespaces "robot_obstacles", "environment_obstacles", * and "environment_influence_zones". */ - MarkerArray createObstaclesWithInfluenceZoneMarkerArray( + MarkerArrayMsg createObstaclesWithInfluenceZoneMarkerArray( const std::vector& obstacles, double influenceDistance); @@ -171,28 +184,26 @@ class PotentialFieldManager : public rclcpp::Node { * approximation in RViz. * * @param robotObstacles Robot link obstacles. - * @return MarkerArray Markers in namespace "robot_mesh_ghost". + * @return MarkerArrayMsg Markers in namespace "robot_mesh_ghost". */ - MarkerArray createGhostMeshOverlayMarkerArray( - const std::vector& robotObstacles); + MarkerArrayMsg createGhostMeshOverlayMarkerArray(const std::vector& robotObstacles); /** * @brief Build small sphere markers at each robot link control point used by the * whole-body velocity repulsion, for visualization in RViz. * * @param robotObstacles Robot link obstacles. - * @return MarkerArray Markers in namespace "robot_control_points". + * @return MarkerArrayMsg Markers in namespace "robot_control_points". */ - MarkerArray createRobotLinkControlPointsMarkerArray( - const std::vector& robotObstacles); + MarkerArrayMsg createRobotLinkControlPointsMarkerArray(const std::vector& robotObstacles); /** * @brief Create a marker for the goal position in the potential field * * @param pf The potential field instance - * @return MarkerArray The marker array containing the goal marker and orientation indicator + * @return MarkerArrayMsg The marker array containing the goal marker and orientation indicator */ - MarkerArray createGoalMarker(std::shared_ptr pf); + MarkerArrayMsg createGoalMarker(std::shared_ptr pf); /** * @brief Create velocity vector markers for visualization in RViz @@ -201,9 +212,9 @@ class PotentialFieldManager : public rclcpp::Node { * and the vector sizes are normalized but the color intensity represents the magnitude * * @param pf The potential field instance - * @return MarkerArray The marker array containing all velocity vector markers + * @return MarkerArrayMsg The marker array containing all velocity vector markers */ - MarkerArray createPotentialVectorMarkers(std::shared_ptr pf); + MarkerArrayMsg createPotentialVectorMarkers(std::shared_ptr pf); /** * @brief Fuses the two twists using a weighted alpha parameter @@ -213,21 +224,18 @@ class PotentialFieldManager : public rclcpp::Node { * @param twist1 The first twist to fuse * @param twist2 The second twist to fuse * @param alpha The weight for the second twist [0.0, 1.0] - * @return geometry_msgs::msg::Twist The fused twist + * @return TwistMsg The fused twist */ - geometry_msgs::msg::Twist fuseTwists( - const geometry_msgs::msg::Twist::SharedPtr twist1, - const geometry_msgs::msg::Twist::SharedPtr twist2, - const double alpha); + TwistMsg fuseTwists(const TwistMsg::SharedPtr twist1, const TwistMsg::SharedPtr twist2, const double alpha); /** * @brief Clamps the twist to the specified limits. * * @param twist The twist to clamp * @param limits A twist message where linear and angular components represent the maximum allowed values - * @return geometry_msgs::msg::Twist The clamped twist as a new message + * @return TwistMsg The clamped twist as a new message */ - geometry_msgs::msg::Twist clampTwist(const geometry_msgs::msg::Twist& twist, const geometry_msgs::msg::Twist& limits); + TwistMsg clampTwist(const TwistMsg& twist, const TwistMsg& limits); /** * @brief Exports the potential field data to a CSV file for external analysis or visualization. diff --git a/potential_fields/package.xml b/potential_fields/package.xml index 8f823fe1..ec205966 100644 --- a/potential_fields/package.xml +++ b/potential_fields/package.xml @@ -2,7 +2,7 @@ potential_fields - 1.0.1 + 1.0.2 Potential Fields Motion Planner Sharwin Patil Apache 2.0 diff --git a/potential_fields/src/ros/pfield_manager.cpp b/potential_fields/src/ros/pfield_manager.cpp index 58c15ef8..81879961 100644 --- a/potential_fields/src/ros/pfield_manager.cpp +++ b/potential_fields/src/ros/pfield_manager.cpp @@ -52,6 +52,27 @@ /// pfield/planned_path (nav_msgs::msg::Path): Planned end-effector path #include "ros/pfield_manager.hpp" + + +/// @brief Converts an HSV color to RGB format. +/// @param hue Hue [0, 360) degrees. +/// @param saturation Saturation in [0, 1]. +/// @param value Value (brightness) in [0, 1]. +/// @return RGB components in [0, 1]. +static std::array convertHSVToRGB(double hue, double saturation, double value) { + const double c = value * saturation; + const double x = c * (1.0 - std::fabs(std::fmod(hue / 60.0, 2.0) - 1.0)); + const double m = value - c; + double rp{}, gp{}, bp{}; + if (hue < 60.0) { rp = c; gp = x; bp = 0; } + else if (hue < 120.0) { rp = x; gp = c; bp = 0; } + else if (hue < 180.0) { rp = 0; gp = c; bp = x; } + else if (hue < 240.0) { rp = 0; gp = x; bp = c; } + else if (hue < 300.0) { rp = x; gp = 0; bp = c; } + else { rp = c; gp = 0; bp = x; } + return {rp + m, gp + m, bp + m}; +} + #include "robot_plugins/null_motion_plugin.hpp" #include "robot_plugins/franka_plugin.hpp" #include "robot_plugins/xarm_plugin.hpp" @@ -207,18 +228,18 @@ PotentialFieldManager::PotentialFieldManager() : Node("potential_field_manager") } // Setup marker publisher - // Use reliable and transient_local QoS for RViz MarkerArray publisher + // Use reliable and transient_local QoS for RViz MarkerArrayMsg publisher auto markerPubQos = rclcpp::QoS(rclcpp::KeepLast(200)).reliable().transient_local(); - this->pFieldMarkerPub = this->create_publisher("pfield/markers", markerPubQos); + this->pFieldMarkerPub = this->create_publisher("pfield/markers", markerPubQos); RCLCPP_INFO(this->get_logger(), "PF Markers publishing on '%s' at %.1f Hz", this->pFieldMarkerPub->get_topic_name(), this->visualizerFrequency ); // Setup goal pose subscriber auto goalPoseQos = rclcpp::QoS(rclcpp::KeepLast(10)).reliable(); - this->goalPoseSub = this->create_subscription("pfield/planning_goal_pose", + this->goalPoseSub = this->create_subscription("pfield/planning_goal_pose", goalPoseQos, - [this](const geometry_msgs::msg::Pose::SharedPtr msg) { + [this](const PoseMsg::SharedPtr msg) { const pfield::SpatialVector goalPose( Eigen::Vector3d( msg->position.x, @@ -238,8 +259,8 @@ PotentialFieldManager::PotentialFieldManager() : Node("potential_field_manager") // Setup obstacle subscriber for external obstacles auto obstacleSubQos = rclcpp::QoS(rclcpp::KeepLast(100)).best_effort().durability_volatile(); - this->obstacleSub = this->create_subscription("pfield/obstacles", obstacleSubQos, - [this](const ObstacleArray::SharedPtr msg) { + this->obstacleSub = this->create_subscription("pfield/obstacles", obstacleSubQos, + [this](const ObstacleArrayMsg::SharedPtr msg) { const auto& obstacles = msg->obstacles; for (const auto& obst : obstacles) { // Build the typed geometry from the message's flat parameters @@ -256,7 +277,7 @@ PotentialFieldManager::PotentialFieldManager() : Node("potential_field_manager") geom = std::make_unique(obst.radius, obst.height); break; case pfield::ObstacleType::ELLIPSOID: - geom = std::make_unique(obst.radius, obst.length, obst.width); + geom = std::make_unique(obst.semi_x, obst.semi_y, obst.semi_z); break; case pfield::ObstacleType::CAPSULE: geom = std::make_unique(obst.radius, obst.height); @@ -285,9 +306,9 @@ PotentialFieldManager::PotentialFieldManager() : Node("potential_field_manager") // Setup query pose subscriber (for visualizing paths in "real-time") auto queryPoseQos = rclcpp::QoS(rclcpp::KeepLast(10)).reliable(); - this->queryPoseSub = this->create_subscription("pfield/query_pose", + this->queryPoseSub = this->create_subscription("pfield/query_pose", queryPoseQos, - [this](const geometry_msgs::msg::Pose::SharedPtr msg) { + [this](const PoseMsg::SharedPtr msg) { const pfield::SpatialVector newQuery( Eigen::Vector3d( msg->position.x, @@ -316,17 +337,17 @@ PotentialFieldManager::PotentialFieldManager() : Node("potential_field_manager") ); // Setup planned end-effector path publisher - this->plannedEndEffectorPathPub = this->create_publisher("pfield/planned_path", 10); + this->plannedEndEffectorPathPub = this->create_publisher("pfield/planned_path", 10); RCLCPP_INFO(this->get_logger(), "Planned EE path publishing on: %s", this->plannedEndEffectorPathPub->get_topic_name()); // Create service to compute the autonomy vector at a given pose - this->autonomyVectorService = this->create_service( + this->autonomyVectorService = this->create_service( "pfield/compute_autonomy_vector", std::bind(&PotentialFieldManager::handleComputeAutonomyVector, this, std::placeholders::_1, std::placeholders::_2) ); // Create the PlanPath service - this->pathPlanningService = this->create_service( + this->pathPlanningService = this->create_service( "pfield/plan_path", std::bind(&PotentialFieldManager::handlePlanPath, this, std::placeholders::_1, std::placeholders::_2) ); @@ -349,7 +370,7 @@ PotentialFieldManager::PotentialFieldManager() : Node("potential_field_manager") void PotentialFieldManager::timerCallback() { // Update the query pose position based on integrating the PF velocity this->integrateQueryPoseFromField(); - MarkerArray pfieldMarkers = this->visualizePF(this->pField); + MarkerArrayMsg pfieldMarkers = this->visualizePF(this->pField); this->pFieldMarkerPub->publish(pfieldMarkers); } @@ -376,7 +397,7 @@ void PotentialFieldManager::integrateQueryPoseFromField() { } void PotentialFieldManager::handleComputeAutonomyVector( - const ComputeAutonomyVector::Request::SharedPtr request, ComputeAutonomyVector::Response::SharedPtr response) { + const ComputeAutonomyVectorSrv::Request::SharedPtr request, ComputeAutonomyVectorSrv::Response::SharedPtr response) { // Compute the autonomy vector at the given pose pfield::SpatialVector queryPose( Eigen::Vector3d( @@ -391,7 +412,7 @@ void PotentialFieldManager::handleComputeAutonomyVector( ); pfield::TaskSpaceTwist autonomyVector; - if (request->planning_method == PlanPath::Request::PLANNING_METHOD_WHOLE_BODY) { + if (request->planning_method == PlanPathSrv::Request::PLANNING_METHOD_WHOLE_BODY) { // Update obstacles first if joint angles are provided std::vector jointAngles; if (!request->joint_angles.empty()) { @@ -438,7 +459,8 @@ void PotentialFieldManager::handleComputeAutonomyVector( ); } -void PotentialFieldManager::handlePlanPath(const PlanPath::Request::SharedPtr request, PlanPath::Response::SharedPtr response) { +void PotentialFieldManager::handlePlanPath( + const PlanPathSrv::Request::SharedPtr request, PlanPathSrv::Response::SharedPtr response) { RCLCPP_INFO(this->get_logger(), "PlanPath request: start=(%.3f, %.3f, %.3f) goal=(%.3f, %.3f, %.3f) delta_time=%.4f goal_tolerance=%.6f max_steps=%d", request->start.position.x, request->start.position.y, request->start.position.z, @@ -496,7 +518,7 @@ void PotentialFieldManager::handlePlanPath(const PlanPath::Request::SharedPtr re pfield::PlannedPath planningResult; try { // Plan a path using the request parameters and store the result - if (request->planning_method == PlanPath::Request::PLANNING_METHOD_TASK_SPACE) { + if (request->planning_method == PlanPathSrv::Request::PLANNING_METHOD_TASK_SPACE) { RCLCPP_INFO(this->get_logger(), "Using Task-Space Wrench Planning"); planningResult = this->pField->planPathFromTaskSpaceWrench( /*startPose=*/startSV, @@ -506,7 +528,7 @@ void PotentialFieldManager::handlePlanPath(const PlanPath::Request::SharedPtr re /*maxIterations=*/request->max_iterations ); } - else if (request->planning_method == PlanPath::Request::PLANNING_METHOD_WHOLE_BODY) { + else if (request->planning_method == PlanPathSrv::Request::PLANNING_METHOD_WHOLE_BODY) { RCLCPP_INFO(this->get_logger(), "Using Whole-Body Joint Velocity Planning"); planningResult = this->pField->planPathFromWholeBodyJointVelocities( /*startJointAngles=*/startJointAnglesDouble, @@ -517,7 +539,7 @@ void PotentialFieldManager::handlePlanPath(const PlanPath::Request::SharedPtr re } else { // Default to Task Space Wrench - if (!request->planning_method.empty() && request->planning_method != PlanPath::Request::PLANNING_METHOD_TASK_SPACE) { + if (!request->planning_method.empty() && request->planning_method != PlanPathSrv::Request::PLANNING_METHOD_TASK_SPACE) { RCLCPP_WARN(this->get_logger(), "Unknown planning method '%s'", request->planning_method.c_str()); } RCLCPP_INFO(this->get_logger(), "Defaulting to Task-Space Wrench Planning"); @@ -542,12 +564,12 @@ void PotentialFieldManager::handlePlanPath(const PlanPath::Request::SharedPtr re const rclcpp::Time t0 = this->now(); // Create Path msg from EE Poses, stamp each pose at t0 + i*dt - nav_msgs::msg::Path path; + PathMsg path; path.header.frame_id = this->fixedFrame; path.header.stamp = t0; for (size_t i = 0; i < planningResult.poses.size(); ++i) { const auto& pose = planningResult.poses[i]; - geometry_msgs::msg::PoseStamped poseStamped; + PoseStampedMsg poseStamped; poseStamped.header.frame_id = this->fixedFrame; poseStamped.header.stamp = t0 + rclcpp::Duration::from_seconds(static_cast(i) * stepDt); poseStamped.pose.position.x = pose.getPosition().x(); @@ -561,10 +583,10 @@ void PotentialFieldManager::handlePlanPath(const PlanPath::Request::SharedPtr re } // Create JointTrajectory from vector of joint angles - trajectory_msgs::msg::JointTrajectory jointTrajectory; + JointTrajectoryMsg jointTrajectory; jointTrajectory.header.stamp = t0; for (size_t i = 0; i < planningResult.jointAngles.size(); ++i) { - trajectory_msgs::msg::JointTrajectoryPoint jtp; + JointTrajectoryPointMsg jtp; jtp.positions = planningResult.jointAngles[i]; jtp.velocities = planningResult.jointVelocities[i]; jtp.time_from_start = rclcpp::Duration::from_seconds(static_cast(i) * stepDt); @@ -572,11 +594,11 @@ void PotentialFieldManager::handlePlanPath(const PlanPath::Request::SharedPtr re } // Create EE Velocity Trajectory - std::vector eeVelocityTrajectory; + std::vector eeVelocityTrajectory; eeVelocityTrajectory.reserve(planningResult.twists.size()); for (size_t i = 0; i < planningResult.twists.size(); ++i) { const auto& twist = planningResult.twists[i]; - geometry_msgs::msg::TwistStamped eeVel; + TwistStampedMsg eeVel; eeVel.header.frame_id = this->fixedFrame; eeVel.header.stamp = t0 + rclcpp::Duration::from_seconds(static_cast(i) * stepDt); eeVel.twist.linear.x = twist.getLinearVelocity().x(); @@ -650,25 +672,25 @@ void PotentialFieldManager::handlePlanPath(const PlanPath::Request::SharedPtr re } -MarkerArray PotentialFieldManager::visualizePF(std::shared_ptr pf) { - MarkerArray markerArray; +MarkerArrayMsg PotentialFieldManager::visualizePF(std::shared_ptr pf) { + MarkerArrayMsg markerArray; auto start = this->now(); - MarkerArray goalMarkerArray = this->createGoalMarker(pf); - markerArray.markers.insert(markerArray.markers.cend(), goalMarkerArray.markers.cbegin(), goalMarkerArray.markers.cend()); + MarkerArrayMsg goalMarkerArrayMsg = this->createGoalMarker(pf); + markerArray.markers.insert(markerArray.markers.cend(), goalMarkerArrayMsg.markers.cbegin(), goalMarkerArrayMsg.markers.cend()); auto endGoal = this->now(); - MarkerArray obstacleMarkers = this->createObstacleMarkers(pf); + MarkerArrayMsg obstacleMarkers = this->createObstacleMarkers(pf); markerArray.markers.insert(markerArray.markers.cend(), obstacleMarkers.markers.cbegin(), obstacleMarkers.markers.cend()); auto endObstacles = this->now(); - MarkerArray queryPoseMarkerArray = this->createQueryPoseMarker(); - markerArray.markers.insert(markerArray.markers.cend(), queryPoseMarkerArray.markers.cbegin(), - queryPoseMarkerArray.markers.cend()); + MarkerArrayMsg queryPoseMarkerArrayMsg = this->createQueryPoseMarker(); + markerArray.markers.insert(markerArray.markers.cend(), queryPoseMarkerArrayMsg.markers.cbegin(), + queryPoseMarkerArrayMsg.markers.cend()); auto endQueryPose = this->now(); - MarkerArray thresholdMarkers = this->createThresholdMarkers(pf); + MarkerArrayMsg thresholdMarkers = this->createThresholdMarkers(pf); // markerArray.markers.insert(markerArray.markers.cend(), thresholdMarkers.markers.cbegin(), // thresholdMarkers.markers.cend()); auto endThreshold = this->now(); if (this->visualizeFieldVectors) { - MarkerArray potentialVectorMarkers = this->createPotentialVectorMarkers(pf); + MarkerArrayMsg potentialVectorMarkers = this->createPotentialVectorMarkers(pf); markerArray.markers.insert(markerArray.markers.cend(), potentialVectorMarkers.markers.cbegin(), potentialVectorMarkers.markers.cend()); auto endVectors = this->now(); @@ -693,19 +715,19 @@ MarkerArray PotentialFieldManager::visualizePF(std::shared_ptrqueryPose; // Center sphere (blue) - Marker qpSphere; + MarkerMsg qpSphere; qpSphere.header.frame_id = this->fixedFrame; qpSphere.header.stamp = this->now(); qpSphere.frame_locked = true; qpSphere.ns = "query_pose"; qpSphere.id = 0; - qpSphere.type = Marker::SPHERE; - qpSphere.action = Marker::ADD; + qpSphere.type = MarkerMsg::SPHERE; + qpSphere.action = MarkerMsg::ADD; qpSphere.pose.position.x = qp.getPosition().x(); qpSphere.pose.position.y = qp.getPosition().y(); qpSphere.pose.position.z = qp.getPosition().z(); @@ -725,13 +747,13 @@ MarkerArray PotentialFieldManager::createQueryPoseMarker() { // RGB unit arrows aligned with query orientation for (int i = 0; i < 3; ++i) { - Marker axis; + MarkerMsg axis; axis.header.frame_id = this->fixedFrame; axis.header.stamp = this->now(); axis.ns = "query_pose"; axis.id = i + 1; - axis.type = Marker::ARROW; - axis.action = Marker::ADD; + axis.type = MarkerMsg::ARROW; + axis.action = MarkerMsg::ADD; axis.frame_locked = true; axis.pose.position.x = qp.getPosition().x(); axis.pose.position.y = qp.getPosition().y(); @@ -766,18 +788,18 @@ MarkerArray PotentialFieldManager::createQueryPoseMarker() { return markerArray; } -MarkerArray PotentialFieldManager::createThresholdMarkers(std::shared_ptr pf) { - MarkerArray markerArray; +MarkerArrayMsg PotentialFieldManager::createThresholdMarkers(std::shared_ptr pf) { + MarkerArrayMsg markerArray; if (!pf->isGoalSet()) { return markerArray; } // Create a translucent sphere representing the dStarThreshold around the goal const pfield::SpatialVector goalPose = pf->getGoalPose(); - Marker thresholdMarker; + MarkerMsg thresholdMarker; thresholdMarker.header.frame_id = this->fixedFrame; thresholdMarker.header.stamp = this->now(); thresholdMarker.ns = "dstar_threshold"; thresholdMarker.id = 0; - thresholdMarker.type = Marker::SPHERE; - thresholdMarker.action = Marker::ADD; + thresholdMarker.type = MarkerMsg::SPHERE; + thresholdMarker.action = MarkerMsg::ADD; thresholdMarker.pose.position.x = goalPose.getPosition().x(); thresholdMarker.pose.position.y = goalPose.getPosition().y(); thresholdMarker.pose.position.z = goalPose.getPosition().z(); @@ -800,14 +822,14 @@ MarkerArray PotentialFieldManager::createThresholdMarkers(std::shared_ptr pf) { - MarkerArray markerArray; +MarkerArrayMsg PotentialFieldManager::createObstacleMarkers(std::shared_ptr pf) { + MarkerArrayMsg markerArray; // Clear previous markers to prevent trails for (const auto& ns : {"robot_obstacles", "environment_obstacles", "environment_influence_zones", "robot_control_points"}) { - Marker del; - del.action = Marker::DELETEALL; + MarkerMsg del; + del.action = MarkerMsg::DELETEALL; del.ns = ns; markerArray.markers.push_back(del); } @@ -819,26 +841,26 @@ MarkerArray PotentialFieldManager::createObstacleMarkers(std::shared_ptrcreateObstaclesWithInfluenceZoneMarkerArray( + const MarkerArrayMsg obstacleMarkers = this->createObstaclesWithInfluenceZoneMarkerArray( allObstacles, pf->getInfluenceDistance()); markerArray.markers.insert(markerArray.markers.cend(), obstacleMarkers.markers.begin(), obstacleMarkers.markers.end()); - const MarkerArray ghostMarkers = this->createGhostMeshOverlayMarkerArray(robotObstacles); + const MarkerArrayMsg ghostMarkers = this->createGhostMeshOverlayMarkerArray(robotObstacles); markerArray.markers.insert(markerArray.markers.cend(), ghostMarkers.markers.begin(), ghostMarkers.markers.end()); - const MarkerArray cpMarkers = this->createRobotLinkControlPointsMarkerArray(robotObstacles); + const MarkerArrayMsg cpMarkers = this->createRobotLinkControlPointsMarkerArray(robotObstacles); markerArray.markers.insert(markerArray.markers.cend(), cpMarkers.markers.begin(), cpMarkers.markers.end()); return markerArray; } -MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( +MarkerArrayMsg PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( const std::vector& obstacles, double influenceDistance) { - MarkerArray markerArray; + MarkerArrayMsg markerArray; std::unordered_set usedIDs; for (const auto& obstacle : obstacles) { @@ -849,7 +871,7 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( } usedIDs.insert(hashID); - Marker obstacleMarker; + MarkerMsg obstacleMarker; obstacleMarker.header.frame_id = this->fixedFrame; obstacleMarker.header.stamp = this->now(); obstacleMarker.frame_locked = true; @@ -860,7 +882,7 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( obstacleMarker.ns = "environment_obstacles"; } obstacleMarker.id = hashID; - obstacleMarker.action = Marker::ADD; + obstacleMarker.action = MarkerMsg::ADD; auto position = obstacle.getPosition(); auto orientation = obstacle.getOrientation(); obstacleMarker.pose.position.x = position.x(); @@ -874,7 +896,7 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( case pfield::ObstacleType::SPHERE: { // Scale is the Diameter of the Sphere const auto& sg = static_cast(obstacle.getGeometry()); - obstacleMarker.type = Marker::SPHERE; + obstacleMarker.type = MarkerMsg::SPHERE; obstacleMarker.scale.x = sg.radius * 2.0f; obstacleMarker.scale.y = sg.radius * 2.0f; obstacleMarker.scale.z = sg.radius * 2.0f; @@ -882,7 +904,7 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( } case pfield::ObstacleType::BOX: { const auto& bg = static_cast(obstacle.getGeometry()); - obstacleMarker.type = Marker::CUBE; + obstacleMarker.type = MarkerMsg::CUBE; obstacleMarker.scale.x = bg.length; obstacleMarker.scale.y = bg.width; obstacleMarker.scale.z = bg.height; @@ -907,7 +929,7 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( } case pfield::ObstacleType::CYLINDER: { const auto& cg = static_cast(obstacle.getGeometry()); - obstacleMarker.type = Marker::CYLINDER; + obstacleMarker.type = MarkerMsg::CYLINDER; obstacleMarker.scale.x = cg.radius * 2.0f; // Diameter obstacleMarker.scale.y = cg.radius * 2.0f; // Diameter obstacleMarker.scale.z = cg.height; // Height @@ -915,7 +937,7 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( } case pfield::ObstacleType::MESH: { if (!obstacle.getMeshResource().empty()) { - obstacleMarker.type = Marker::MESH_RESOURCE; + obstacleMarker.type = MarkerMsg::MESH_RESOURCE; obstacleMarker.mesh_resource = obstacle.getMeshResource(); obstacleMarker.mesh_use_embedded_materials = true; Eigen::Vector3d scale = obstacle.getMeshScale(); @@ -925,7 +947,7 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( } else { // Fallback to inflated AABB cube - obstacleMarker.type = Marker::CUBE; + obstacleMarker.type = MarkerMsg::CUBE; const Eigen::Vector3d halfDims = obstacle.halfDimensions(); obstacleMarker.scale.x = halfDims.x() * 2.0; obstacleMarker.scale.y = halfDims.y() * 2.0; @@ -938,7 +960,7 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( // Compose the obstacle orientation with the PCA rotation so the marker aligns with the // ellipsoid's principal axes in the world frame. const auto& eg = static_cast(obstacle.getGeometry()); - obstacleMarker.type = Marker::SPHERE; + obstacleMarker.type = MarkerMsg::SPHERE; obstacleMarker.scale.x = eg.semiX * 2.0; obstacleMarker.scale.y = eg.semiY * 2.0; obstacleMarker.scale.z = eg.semiZ * 2.0; @@ -982,13 +1004,13 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( // Guard against zero-scale warning in RViz: if the shaft is zero or near-zero, // render as a sphere (the capsule degenerates to a sphere). if (shaft < 1e-4) { - obstacleMarker.type = Marker::SPHERE; + obstacleMarker.type = MarkerMsg::SPHERE; obstacleMarker.scale.x = r * 2.0; obstacleMarker.scale.y = r * 2.0; obstacleMarker.scale.z = r * 2.0; } else { - obstacleMarker.type = Marker::CYLINDER; + obstacleMarker.type = MarkerMsg::CYLINDER; obstacleMarker.scale.x = r * 2.0; obstacleMarker.scale.y = r * 2.0; obstacleMarker.scale.z = shaft; @@ -1034,12 +1056,12 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( for (int cap = 0; cap < 2; ++cap) { const double sign = (cap == 0) ? 1.0 : -1.0; const Eigen::Vector3d capCenter = center + sign * halfShaft * capsuleAxisWorld; - Marker capMarker; + MarkerMsg capMarker; capMarker.header = obstacleMarker.header; capMarker.ns = obstacleMarker.ns; capMarker.id = hashID + 10000 + cap; // offset to avoid ID collision - capMarker.action = Marker::ADD; - capMarker.type = Marker::SPHERE; + capMarker.action = MarkerMsg::ADD; + capMarker.type = MarkerMsg::SPHERE; capMarker.pose.position.x = capCenter.x(); capMarker.pose.position.y = capCenter.y(); capMarker.pose.position.z = capCenter.z(); @@ -1058,13 +1080,13 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( continue; } // Create a transparent volume representing the influence zone - Marker influenceMarker; + MarkerMsg influenceMarker; influenceMarker.header.frame_id = this->fixedFrame; influenceMarker.header.stamp = this->now(); influenceMarker.frame_locked = true; influenceMarker.ns = "environment_influence_zones"; influenceMarker.id = hashID; // mirror id for influence volume - influenceMarker.action = Marker::ADD; + influenceMarker.action = MarkerMsg::ADD; influenceMarker.pose.position.x = position.x(); influenceMarker.pose.position.y = position.y(); influenceMarker.pose.position.z = position.z(); @@ -1081,7 +1103,7 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( case pfield::ObstacleType::SPHERE: { // Inflated sphere diameter = 2 * (radius + influenceDistance). const auto& sg2 = static_cast(obstacle.getGeometry()); - influenceMarker.type = Marker::SPHERE; + influenceMarker.type = MarkerMsg::SPHERE; const double influenceZoneDiameter = 2.0 * (sg2.radius + influenceDistance); influenceMarker.scale.x = influenceZoneDiameter; influenceMarker.scale.y = influenceZoneDiameter; @@ -1091,7 +1113,7 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( case pfield::ObstacleType::BOX: { // Inflated box dimensions = base dims + 2 * influenceDistance along each axis. const auto& bg2 = static_cast(obstacle.getGeometry()); - influenceMarker.type = Marker::CUBE; + influenceMarker.type = MarkerMsg::CUBE; influenceMarker.scale.x = bg2.length + 2.0 * influenceDistance; influenceMarker.scale.y = bg2.width + 2.0 * influenceDistance; influenceMarker.scale.z = bg2.height + 2.0 * influenceDistance; @@ -1110,7 +1132,7 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( case pfield::ObstacleType::CYLINDER: { // Inflated cylinder: diameter = 2 * (radius + d), height = height + 2 * d. const auto& cg2 = static_cast(obstacle.getGeometry()); - influenceMarker.type = Marker::CYLINDER; + influenceMarker.type = MarkerMsg::CYLINDER; influenceMarker.scale.x = 2.0 * (cg2.radius + influenceDistance); influenceMarker.scale.y = 2.0 * (cg2.radius + influenceDistance); influenceMarker.scale.z = cg2.height + 2.0 * influenceDistance; @@ -1121,7 +1143,7 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( // We approximate a Minkowski sum by scaling the mesh per-axis so its AABB grows by +2d. // This preserves the silhouette and is a better visual cue than a plain inflated AABB cube. if (!obstacle.getMeshResource().empty()) { - influenceMarker.type = Marker::MESH_RESOURCE; + influenceMarker.type = MarkerMsg::MESH_RESOURCE; influenceMarker.mesh_resource = obstacle.getMeshResource(); influenceMarker.mesh_use_embedded_materials = false; // keep our semi-transparent color // Base axis-aligned dimensions for the mesh in obstacle frame @@ -1142,7 +1164,7 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( } else { // Fallback: render an inflated AABB cube if no mesh resource provided - influenceMarker.type = Marker::CUBE; + influenceMarker.type = MarkerMsg::CUBE; const Eigen::Vector3d baseHalf = obstacle.halfDimensions(); const Eigen::Vector3d baseDims = 2.0 * baseHalf; // L, W, H influenceMarker.scale.x = baseDims.x() + 2.0 * influenceDistance; @@ -1155,7 +1177,7 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( // Inflated ellipsoid: each semi-axis grows by influenceDistance. // Apply PCA orientation so the marker matches the ellipsoid's principal axes. const auto& eg2 = static_cast(obstacle.getGeometry()); - influenceMarker.type = Marker::SPHERE; + influenceMarker.type = MarkerMsg::SPHERE; influenceMarker.scale.x = 2.0 * (eg2.semiX + influenceDistance); influenceMarker.scale.y = 2.0 * (eg2.semiY + influenceDistance); influenceMarker.scale.z = 2.0 * (eg2.semiZ + influenceDistance); @@ -1187,7 +1209,7 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( influenceMarker.pose.orientation.y = markerQ.y(); influenceMarker.pose.orientation.z = markerQ.z(); influenceMarker.pose.orientation.w = markerQ.w(); - influenceMarker.type = Marker::CYLINDER; + influenceMarker.type = MarkerMsg::CYLINDER; influenceMarker.scale.x = r * 2.0; influenceMarker.scale.y = r * 2.0; influenceMarker.scale.z = shaft; @@ -1200,9 +1222,9 @@ MarkerArray PotentialFieldManager::createObstaclesWithInfluenceZoneMarkerArray( return markerArray; } -MarkerArray PotentialFieldManager::createGhostMeshOverlayMarkerArray( +MarkerArrayMsg PotentialFieldManager::createGhostMeshOverlayMarkerArray( const std::vector& robotObstacles) { - MarkerArray markerArray; + MarkerArrayMsg markerArray; int ghostID = 0; for (const auto& obstacle : robotObstacles) { @@ -1211,14 +1233,14 @@ MarkerArray PotentialFieldManager::createGhostMeshOverlayMarkerArray( const std::string& meshURI = ghostCapg.sourceMeshResource; if (meshURI.empty()) { continue; } - Marker ghostMarker; + MarkerMsg ghostMarker; ghostMarker.header.frame_id = this->fixedFrame; ghostMarker.header.stamp = this->now(); ghostMarker.frame_locked = true; ghostMarker.ns = "robot_mesh_ghost"; ghostMarker.id = ghostID++; - ghostMarker.action = Marker::ADD; - ghostMarker.type = Marker::MESH_RESOURCE; + ghostMarker.action = MarkerMsg::ADD; + ghostMarker.type = MarkerMsg::MESH_RESOURCE; ghostMarker.mesh_resource = meshURI; ghostMarker.mesh_use_embedded_materials = false; @@ -1250,9 +1272,9 @@ MarkerArray PotentialFieldManager::createGhostMeshOverlayMarkerArray( return markerArray; } -MarkerArray PotentialFieldManager::createRobotLinkControlPointsMarkerArray( +MarkerArrayMsg PotentialFieldManager::createRobotLinkControlPointsMarkerArray( const std::vector& robotObstacles) { - MarkerArray markerArray; + MarkerArrayMsg markerArray; const double CP_RADIUS = 0.015; // sphere diameter = 3 cm for (const auto& link : robotObstacles) { @@ -1260,20 +1282,20 @@ MarkerArray PotentialFieldManager::createRobotLinkControlPointsMarkerArray( // Deterministic color per link: hash the frame_id to a hue in [0, 360) const std::size_t hashValue = std::hash{}(link.getFrameID()); const double hue = static_cast(hashValue % 360); - const auto& [r, g, b] = pfield::convertHSVToRGB(hue, 0.9, 0.9); + const auto& [r, g, b] = convertHSVToRGB(hue, 0.9, 0.9); // Stable per-link ID base: keeps IDs consistent across frames so RViz doesn't // see duplicate ns+id pairs when DELETEALL and ADD markers coexist in one message. const int linkIDBase = static_cast(hashValue & 0x7FFFFFFF); int slotIndex = 0; for (const auto& [cp, surfaceRadius] : controlPoints) { - Marker cpMarker; + MarkerMsg cpMarker; cpMarker.header.frame_id = this->fixedFrame; cpMarker.header.stamp = this->now(); cpMarker.frame_locked = true; cpMarker.ns = "robot_control_points"; cpMarker.id = linkIDBase + slotIndex++; - cpMarker.action = Marker::ADD; - cpMarker.type = Marker::SPHERE; + cpMarker.action = MarkerMsg::ADD; + cpMarker.type = MarkerMsg::SPHERE; cpMarker.pose.position.x = cp.x(); cpMarker.pose.position.y = cp.y(); cpMarker.pose.position.z = cp.z(); @@ -1293,18 +1315,18 @@ MarkerArray PotentialFieldManager::createRobotLinkControlPointsMarkerArray( return markerArray; } -MarkerArray PotentialFieldManager::createGoalMarker(std::shared_ptr pf) { +MarkerArrayMsg PotentialFieldManager::createGoalMarker(std::shared_ptr pf) { // Create a green sphere marker - MarkerArray markerArray; + MarkerArrayMsg markerArray; if (!pf->isGoalSet()) { return markerArray; } - Marker goalMarker; + MarkerMsg goalMarker; goalMarker.header.frame_id = this->fixedFrame; goalMarker.header.stamp = this->now(); goalMarker.frame_locked = true; goalMarker.ns = "goal"; goalMarker.id = 0; - goalMarker.type = Marker::SPHERE; - goalMarker.action = Marker::ADD; + goalMarker.type = MarkerMsg::SPHERE; + goalMarker.action = MarkerMsg::ADD; pfield::SpatialVector goalPose = pf->getGoalPose(); goalMarker.pose.position.x = goalPose.getPosition().x(); goalMarker.pose.position.y = goalPose.getPosition().y(); @@ -1321,16 +1343,16 @@ MarkerArray PotentialFieldManager::createGoalMarker(std::shared_ptr goalAxes; + std::vector goalAxes; goalAxes.reserve(3); for (int i = 0; i < 3; i++) { - Marker axis; + MarkerMsg axis; axis.header.frame_id = this->fixedFrame; axis.header.stamp = this->now(); axis.ns = "goal"; axis.id = i + 1; - axis.type = Marker::ARROW; - axis.action = Marker::ADD; + axis.type = MarkerMsg::ARROW; + axis.action = MarkerMsg::ADD; axis.frame_locked = true; axis.pose.position.x = goalPose.getPosition().x(); axis.pose.position.y = goalPose.getPosition().y(); @@ -1362,15 +1384,15 @@ MarkerArray PotentialFieldManager::createGoalMarker(std::shared_ptr pf) { - MarkerArray markerArray; +MarkerArrayMsg PotentialFieldManager::createPotentialVectorMarkers(std::shared_ptr pf) { + MarkerArrayMsg markerArray; int id = 0; const auto limits = pf->computeFieldBounds(this->queryPose, this->visualizerBufferArea); const double resolution = std::max(this->fieldResolution, 1.0); // Resolution must be at least 1.0 @@ -1380,7 +1402,7 @@ MarkerArray PotentialFieldManager::createPotentialVectorMarkers(std::shared_ptr< // Skip any points that are inside obstacle radius Eigen::Vector3d point(x, y, z); if (pf->isPointInsideObstacle(point)) { continue; } - Marker vectorMarker; + MarkerMsg vectorMarker; pfield::SpatialVector position{point}; pfield::TaskSpaceTwist velocity = pf->evaluateLimitedVelocityAtPose(position); const Eigen::Vector3d v = velocity.getLinearVelocity(); @@ -1389,8 +1411,8 @@ MarkerArray PotentialFieldManager::createPotentialVectorMarkers(std::shared_ptr< vectorMarker.header.stamp = this->now(); vectorMarker.ns = "potential_vectors"; vectorMarker.id = id++; - vectorMarker.type = Marker::ARROW; - vectorMarker.action = Marker::ADD; + vectorMarker.type = MarkerMsg::ARROW; + vectorMarker.action = MarkerMsg::ADD; vectorMarker.pose.position.x = position.getPosition().x(); vectorMarker.pose.position.y = position.getPosition().y(); vectorMarker.pose.position.z = position.getPosition().z(); @@ -1422,11 +1444,11 @@ MarkerArray PotentialFieldManager::createPotentialVectorMarkers(std::shared_ptr< return markerArray; } -geometry_msgs::msg::Twist PotentialFieldManager::fuseTwists( - const geometry_msgs::msg::Twist::SharedPtr twist1, - const geometry_msgs::msg::Twist::SharedPtr twist2, +TwistMsg PotentialFieldManager::fuseTwists( + const TwistMsg::SharedPtr twist1, + const TwistMsg::SharedPtr twist2, const double alpha) { - geometry_msgs::msg::Twist fusedTwist; + TwistMsg fusedTwist; // Alpha = 0 -> A, Alpha = 1 -> B auto fuseValue = [](double A, double B, double alpha) { @@ -1444,8 +1466,8 @@ geometry_msgs::msg::Twist PotentialFieldManager::fuseTwists( return fusedTwist; } -geometry_msgs::msg::Twist clampTwist(const geometry_msgs::msg::Twist& twist, const geometry_msgs::msg::Twist& limits) { - geometry_msgs::msg::Twist clampedTwist = twist; // Start with the input twist +TwistMsg clampTwist(const TwistMsg& twist, const TwistMsg& limits) { + TwistMsg clampedTwist = twist; // Start with the input twist // Clamp linear velocities clampedTwist.linear.x = std::clamp(clampedTwist.linear.x, -limits.linear.x, limits.linear.x); clampedTwist.linear.y = std::clamp(clampedTwist.linear.y, -limits.linear.y, limits.linear.y); diff --git a/potential_fields_demos/CHANGELOG.rst b/potential_fields_demos/CHANGELOG.rst new file mode 100644 index 00000000..460e18ea --- /dev/null +++ b/potential_fields_demos/CHANGELOG.rst @@ -0,0 +1,15 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package potential_fields_demos +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.2 (2026-04-04) +------------------ +* New READMEs for demos and interfaces +* Delete turtlebot urdf +* Estimate Robot Geometry using Ellipsoids and Control Points for smooth WBV Repulsion (`#42 `_) + Co-authored-by: Claude Sonnet 4.6 +* Latest Demo +* Fixed CSV bugs and tested demo1 and demo2, TS planning has problems +* 1.0.1 +* Rename Packages (`#35 `_) +* Contributors: Sharwin Patil, Sharwin24 diff --git a/potential_fields_demos/README.md b/potential_fields_demos/README.md new file mode 100644 index 00000000..e023729f --- /dev/null +++ b/potential_fields_demos/README.md @@ -0,0 +1,96 @@ +# `potential_fields_demos` + +Example ROS 2 package demonstrating how to use the `potential_fields` package. Provides a launch file, demo nodes, bundled URDFs, and a configuration file to get started quickly without any hardware. + +## Package Contents + +``` +potential_fields_demos/ +├── launch/ +│ └── pf_demo.launch.xml # Primary entry-point launch file +├── src/ +│ ├── pfield_demo.cpp # Demo node: publishes obstacles, calls plan_path service +│ └── pfield_teleop_demo.cpp # Teleop variant for interactive testing +├── urdf/ +│ ├── xarm7.urdf # UFactory XArm7 (primary reference robot) +│ ├── fer_franka_hand.urdf # Franka Emika FER with hand +│ └── sphere_robot.urdf # Minimal sphere robot for unit testing +└── config/ + └── demo.yaml # ROS parameters for the demo node +``` + +## Quick Start + +Build the workspace and source it, then launch the demo: + +```bash +colcon build && source install/setup.bash + +# Minimal demo (no robot URDF, no hardware) +ros2 launch potential_fields_demos pf_demo.launch.xml + +# XArm7 demo (no hardware required) +ros2 launch potential_fields_demos pf_demo.launch.xml \ + urdf_file_path:=xarm7.urdf \ + motion_plugin_type:=xarm \ + end_effector_frame:=link_eef # TF frame of the end-effector link +``` + +Once running, trigger the plan path demo via service call: + +```bash +ros2 service call /pfield_demo/run_plan_path_demo std_srvs/srv/Empty +``` + +## Launch Arguments + +| Argument | Default | Description | +|----------|---------|-------------| +| `urdf_file_path` | `""` | URDF filename relative to `urdf/` (e.g. `xarm7.urdf`) | +| `motion_plugin_type` | `""` | Robot plugin to load: `xarm`, `franka`, or empty for `null` | +| `end_effector_frame` | `link_eef` | Name of the end-effector link | +| `use_rviz` | `true` | Launch RViz alongside the demo | +| `rviz_fixed_frame` | `world` | RViz fixed frame | +| `base_frame` | `base` | Robot base link (child of fixed frame TF) | +| `base_x/y/z` | `0.0` | Translation of base frame relative to fixed frame | +| `base_roll/pitch/yaw` | `0.0` | Rotation of base frame relative to fixed frame | + +## Demo Nodes + +### `pfield_demo_node` +The primary demo node (`pfield_demo.cpp`). On startup it: +1. Publishes a set of static obstacles to `pfield/obstacles`. +2. Waits for the `pfield/plan_path` service to become available. +3. Exposes `/pfield_demo/run_plan_path_demo` (`std_srvs/Empty`) — call this service to trigger a planning request from the current robot pose to a pre-configured goal. + +**Parameters** + +| Parameter | Default | Description | +|-----------|---------|-------------| +| `fixed_frame` | `world` | World frame used for obstacle and goal poses | +| `ee_link_name` | `link_tcp` | End-effector link name passed to the planner | + +### `pfield_teleop_demo_node` +Interactive variant (`pfield_teleop_demo.cpp`) for testing the autonomy vector in a shared-control loop. Calls `pfield/compute_autonomy_vector` at a configurable rate and forwards the result as a velocity command. + +## Adding Your Own Robot + +1. Place your robot's URDF in the `urdf/` directory. +2. Launch with `urdf_file_path:=.urdf` and the appropriate `motion_plugin_type` and `end_effector_frame`. +3. If your robot requires a custom plugin, see the [top-level README](../README.md#supporting-your-own-robot) for instructions on implementing `MotionPlugin` and `IKSolver`. + +## Integrating into Your Own Package + +To reuse the core potential field launch without the demo node, include `pfield.launch.xml` from the `potential_fields` package directly: + +```xml + + + + + +``` + +--- + +Copyright Notice [Sharwin Patil](https://www.sharwinpatil.info/) 2025 diff --git a/potential_fields_demos/package.xml b/potential_fields_demos/package.xml index 571da469..c5ab1a02 100644 --- a/potential_fields_demos/package.xml +++ b/potential_fields_demos/package.xml @@ -2,7 +2,7 @@ potential_fields_demos - 1.0.1 + 1.0.2 Demo Package for potential_fields Sharwin Patil Apache 2.0 diff --git a/potential_fields_demos/urdf/robot.urdf b/potential_fields_demos/urdf/robot.urdf deleted file mode 100644 index 6b20ebef..00000000 --- a/potential_fields_demos/urdf/robot.urdf +++ /dev/null @@ -1,108 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/potential_fields_demos/urdf/robot.urdf.xacro b/potential_fields_demos/urdf/robot.urdf.xacro deleted file mode 100644 index 5ba47980..00000000 --- a/potential_fields_demos/urdf/robot.urdf.xacro +++ /dev/null @@ -1,123 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/potential_fields_interfaces/CHANGELOG.rst b/potential_fields_interfaces/CHANGELOG.rst new file mode 100644 index 00000000..0ff4b3fb --- /dev/null +++ b/potential_fields_interfaces/CHANGELOG.rst @@ -0,0 +1,29 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package potential_fields_interfaces +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.2 (2026-04-04) +------------------ +* Clarified Obstacle type's geometry fields +* New READMEs for demos and interfaces +* Estimate Robot Geometry using Ellipsoids and Control Points for smooth WBV Repulsion (`#42 `_) + Co-authored-by: Claude Sonnet 4.6 +* 1.0.1 +* Rename Packages (`#35 `_) +* December Final Demos (`#26 `_) +* Use Robot Dynamics to convert Joint Torques to Joint Velocities (`#24 `_) +* Updating ComputeAutonomyVector +* Collision Avoidance between robot links and environment obstacles (`#21 `_) +* Merge branch 'main' of github.com:argallab/pfields_2025 +* Merged RobotParser into PFieldManager (`#10 `_) +* PlanPath Service Demo (`#7 `_) +* Planning "World" for path-planning with robot (`#6 `_) +* Motion Interface (`#5 `_) +* Update license to Apache 2.0 +* Updating email to northwestern email for packages +* Custom service to plan path in PF +* Merge pull request `#4 `_ from argallab/demo-package + Example Package +* Created robot parser node to listen for the transforms and parse the URDF collision objects +* Created an interfaces package +* Contributors: Sharwin Patil, Sharwin24 diff --git a/potential_fields_interfaces/README.md b/potential_fields_interfaces/README.md new file mode 100644 index 00000000..b1f98ea3 --- /dev/null +++ b/potential_fields_interfaces/README.md @@ -0,0 +1,228 @@ +# `potential_fields_interfaces` + +ROS 2 message and service definitions for the `potential_fields` package. All nodes in this repository communicate using these types. + +## Messages + +### `Obstacle.msg` +Describes a single obstacle in the potential field environment. + +| Field | Type | Used by | Description | +|-------|------|---------|-------------| +| `frame_id` | `string` | all | Unique identifier for this obstacle (e.g. `"wall_1"`) | +| `type` | `string` | all | Shape: `"Sphere"`, `"Box"`, `"Cylinder"`, `"Ellipsoid"`, `"Capsule"`, or `"Mesh"` | +| `group` | `string` | all | Class: `"Static"`, `"Dynamic"`, or `"Robot"` | +| `pose` | `geometry_msgs/Pose` | all | World-frame center position and orientation | +| `radius` | `float32` | Sphere, Cylinder, Capsule | Radius of the shape | +| `height` | `float32` | Box, Cylinder, Capsule | Height / full extent along Z | +| `length` | `float32` | Box | Full extent along X | +| `width` | `float32` | Box | Full extent along Y | +| `semi_x` | `float32` | Ellipsoid | Semi-axis along X | +| `semi_y` | `float32` | Ellipsoid | Semi-axis along Y | +| `semi_z` | `float32` | Ellipsoid | Semi-axis along Z | +| `mesh_resource` | `string` | Mesh | URI to mesh file (e.g. `package://my_robot/meshes/link.dae`) | +| `scale_x/y/z` | `float32` | Mesh | Per-axis mesh scale | + +### `ObstacleArray.msg` +A stamped array of `Obstacle` messages, published to `pfield/obstacles`. + +| Field | Type | Description | +|-------|------|-------------| +| `header` | `std_msgs/Header` | Timestamp and frame | +| `obstacles` | `Obstacle[]` | List of obstacles | + +**CLI examples — one per shape type** + +```bash +# Sphere (radius = 0.1 m) +ros2 topic pub --once /pfield/obstacles potential_fields_interfaces/msg/ObstacleArray "{ + header: {frame_id: 'world'}, + obstacles: [{ + frame_id: 'ball', type: 'Sphere', group: 'Static', + pose: {position: {x: 0.4, y: 0.0, z: 0.1}, orientation: {w: 1.0}}, + radius: 0.1 + }] +}" + +# Box (15 cm × 70 cm × 60 cm) +ros2 topic pub --once /pfield/obstacles potential_fields_interfaces/msg/ObstacleArray "{ + header: {frame_id: 'world'}, + obstacles: [{ + frame_id: 'table', type: 'Box', group: 'Static', + pose: {position: {x: 0.66, y: 0.0, z: 0.3}, orientation: {w: 1.0}}, + length: 0.15, width: 0.7, height: 0.6 + }] +}" + +# Cylinder (radius = 9 cm, height = 30 cm) +ros2 topic pub --once /pfield/obstacles potential_fields_interfaces/msg/ObstacleArray "{ + header: {frame_id: 'world'}, + obstacles: [{ + frame_id: 'pitcher', type: 'Cylinder', group: 'Static', + pose: {position: {x: 0.51, y: 0.0, z: 0.15}, orientation: {w: 1.0}}, + radius: 0.09, height: 0.3 + }] +}" + +# Ellipsoid (semi-axes 8 cm × 5 cm × 12 cm) +ros2 topic pub --once /pfield/obstacles potential_fields_interfaces/msg/ObstacleArray "{ + header: {frame_id: 'world'}, + obstacles: [{ + frame_id: 'ellipsoid_obs', type: 'Ellipsoid', group: 'Static', + pose: {position: {x: 0.5, y: 0.1, z: 0.2}, orientation: {w: 1.0}}, + semi_x: 0.08, semi_y: 0.05, semi_z: 0.12 + }] +}" + +# Capsule (radius = 5 cm, shaft length = 20 cm) +ros2 topic pub --once /pfield/obstacles potential_fields_interfaces/msg/ObstacleArray "{ + header: {frame_id: 'world'}, + obstacles: [{ + frame_id: 'capsule_obs', type: 'Capsule', group: 'Static', + pose: {position: {x: 0.45, y: 0.0, z: 0.2}, orientation: {w: 1.0}}, + radius: 0.05, height: 0.2 + }] +}" + +# Mesh +ros2 topic pub --once /pfield/obstacles potential_fields_interfaces/msg/ObstacleArray "{ + header: {frame_id: 'world'}, + obstacles: [{ + frame_id: 'mesh_obs', type: 'Mesh', group: 'Static', + pose: {position: {x: 0.5, y: 0.0, z: 0.1}, orientation: {w: 1.0}}, + mesh_resource: 'package://my_robot_description/meshes/obstacle.dae', + scale_x: 1.0, scale_y: 1.0, scale_z: 1.0 + }] +}" +``` + +## Services + +### `PlanPath.srv` +Plans a collision-free path from a start pose to a goal pose using the potential field. + +**Request** + +| Field | Type | Description | +|-------|------|-------------| +| `start` | `geometry_msgs/Pose` | Start end-effector pose (task-space planning only) | +| `goal` | `geometry_msgs/Pose` | Goal end-effector pose | +| `starting_joint_angles` | `float32[]` | Initial joint configuration in radians (whole-body planning) | +| `delta_time` | `float32` | Integration timestep in seconds | +| `goal_tolerance` | `float32` | Distance threshold for reaching the goal in meters | +| `max_iterations` | `int32` | Maximum integration steps (default: 30000) | +| `planning_method` | `string` | `"task_space"` or `"whole_body"` (defaults to `"task_space"`) | + +**Response** + +| Field | Type | Description | +|-------|------|-------------| +| `success` | `bool` | Whether planning reached the goal within tolerance | +| `end_effector_path` | `nav_msgs/Path` | Planned end-effector trajectory | +| `joint_trajectory` | `trajectory_msgs/JointTrajectory` | Joint-space trajectory (whole-body only) | +| `end_effector_velocity_trajectory` | `geometry_msgs/TwistStamped[]` | EE velocity at each planning step | +| `error_message` | `string` | Populated on failure | + +**CLI example — task-space planning** + +Plan from a current pose to a goal 50 cm in front of the robot: + +```bash +ros2 service call /pfield/plan_path potential_fields_interfaces/srv/PlanPath "{ + start: { + position: {x: 0.227, y: 0.0, z: 0.294}, + orientation: {x: 0.707, y: 0.0, z: 0.707, w: 0.0} + }, + goal: { + position: {x: 0.5, y: 0.2, z: 0.4}, + orientation: {x: 0.707, y: 0.0, z: 0.707, w: 0.0} + }, + delta_time: 0.02, + goal_tolerance: 0.01, + max_iterations: 25000, + planning_method: 'task_space' +}" +``` + +**CLI example — whole-body planning** + +Plan using joint angles as the initial configuration (start pose is ignored): + +```bash +ros2 service call /pfield/plan_path potential_fields_interfaces/srv/PlanPath "{ + goal: { + position: {x: 0.5, y: 0.2, z: 0.4}, + orientation: {x: 0.707, y: 0.0, z: 0.707, w: 0.0} + }, + starting_joint_angles: [0.0, 0.0, 0.0, 0.0, 0.0, -1.5708, 0.0], + delta_time: 0.02, + goal_tolerance: 0.01, + max_iterations: 25000, + planning_method: 'whole_body' +}" +``` + +### `ComputeAutonomyVector.srv` +Returns the potential field's recommended velocity at a single query pose, used for shared-control applications. + +**Request** + +| Field | Type | Description | +|-------|------|-------------| +| `query_pose` | `geometry_msgs/PoseStamped` | Pose at which to evaluate the field | +| `joint_angles` | `float32[]` | Current joint configuration in radians | +| `prev_joint_velocities` | `float32[]` | Previous joint velocities (optional, for dynamic terms) | +| `delta_time` | `float32` | Timestep for dynamic calculations | +| `planning_method` | `string` | `"task_space"` or `"whole_body"` | + +**Response** + +| Field | Type | Description | +|-------|------|-------------| +| `autonomy_vector` | `geometry_msgs/TwistStamped` | Recommended velocity at the query pose | + +**CLI example — task-space autonomy vector** + +```bash +ros2 service call /pfield/compute_autonomy_vector potential_fields_interfaces/srv/ComputeAutonomyVector "{ + query_pose: { + header: {frame_id: 'world'}, + pose: { + position: {x: 0.3, y: 0.1, z: 0.35}, + orientation: {x: 0.707, y: 0.0, z: 0.707, w: 0.0} + } + }, + delta_time: 0.02, + planning_method: 'task_space' +}" +``` + +**CLI example — whole-body autonomy vector** + +```bash +ros2 service call /pfield/compute_autonomy_vector potential_fields_interfaces/srv/ComputeAutonomyVector "{ + query_pose: { + header: {frame_id: 'world'}, + pose: { + position: {x: 0.3, y: 0.1, z: 0.35}, + orientation: {x: 0.707, y: 0.0, z: 0.707, w: 0.0} + } + }, + joint_angles: [0.0, 0.0, 0.0, 0.0, 0.0, -1.5708, 0.0], + prev_joint_velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + delta_time: 0.02, + planning_method: 'whole_body' +}" +``` + +## Dependencies + +- `std_msgs` +- `geometry_msgs` +- `nav_msgs` +- `trajectory_msgs` +- `builtin_interfaces` + +--- + +Copyright Notice [Sharwin Patil](https://www.sharwinpatil.info/) 2025 diff --git a/potential_fields_interfaces/msg/Obstacle.msg b/potential_fields_interfaces/msg/Obstacle.msg index f6f4dbed..5d1f9095 100644 --- a/potential_fields_interfaces/msg/Obstacle.msg +++ b/potential_fields_interfaces/msg/Obstacle.msg @@ -1,11 +1,24 @@ string frame_id -string type # Either "Sphere", "Box", "Cylinder", or "Mesh" -string group # Either "Static", "Dynamic", or "Robot" +string type # "Sphere", "Box", "Cylinder", "Ellipsoid", "Capsule", or "Mesh" +string group # "Static", "Dynamic", or "Robot" geometry_msgs/Pose pose + +# Sphere: radius +# Cylinder: radius, height +# Capsule: radius, height float32 radius +float32 height + +# Box: length, width, height float32 length float32 width -float32 height + +# Ellipsoid: semi_x, semi_y, semi_z +float32 semi_x +float32 semi_y +float32 semi_z + +# Mesh: mesh_resource, scale_x, scale_y, scale_z string mesh_resource float32 scale_x float32 scale_y diff --git a/potential_fields_interfaces/package.xml b/potential_fields_interfaces/package.xml index dc18bdcf..c453a973 100644 --- a/potential_fields_interfaces/package.xml +++ b/potential_fields_interfaces/package.xml @@ -2,7 +2,7 @@ potential_fields_interfaces - 1.0.1 + 1.0.2 Custom Messages and Services for potential_fields package Sharwin Patil Apache 2.0 diff --git a/potential_fields_library/CHANGELOG.rst b/potential_fields_library/CHANGELOG.rst new file mode 100644 index 00000000..7435f279 --- /dev/null +++ b/potential_fields_library/CHANGELOG.rst @@ -0,0 +1,26 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package potential_fields_library +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.2 (2026-04-04) +------------------ +* Moved HSVtoRGB to pfield_manager instead +* Estimate Robot Geometry using Ellipsoids and Control Points for smooth WBV Repulsion (`#42 `_) + Co-authored-by: Claude Sonnet 4.6 +* Updated README to fix example +* 40 test main branch ruleset (testing Issue `#40 `_) (`#41 `_) +* README edits and CONTRIBUTING.md +* Reflecting new RK4 estimation in unit test +* pfields_2025 -> potential_fields in docs and example scripts +* Cleaned plotting with more clear plots and messaging for invalid data +* Don't approximate joint velocities since people might try to use them +* Created visualization method to visualize both TS and WBV +* Removed link clearances since already computed in CSV creation +* Removed link clearances since already computed in CSV creation +* Populating csv with attraction force, repulsive force, and clearance +* Fixed cylinder distance bug +* Linting +* Fixed CSV bugs and tested demo1 and demo2, TS planning has problems +* 1.0.1 +* Rename Packages (`#35 `_) +* Contributors: Demiana Barsoum, Sharwin Patil, Sharwin24 diff --git a/potential_fields_library/include/pfield/pf_kinematics.hpp b/potential_fields_library/include/pfield/pf_kinematics.hpp index ae3f0ea0..134073c5 100644 --- a/potential_fields_library/include/pfield/pf_kinematics.hpp +++ b/potential_fields_library/include/pfield/pf_kinematics.hpp @@ -35,10 +35,11 @@ namespace pfield { + /// An entry in the per-link collision catalog built from the URDF. struct CollisionCatalogEntry { - std::string id; // unique obstacle id (link::name or link::colN) - std::string linkName; // link this collision belongs to - urdf::CollisionSharedPtr col; // collision element + std::string id; ///< Unique obstacle ID (e.g. "link1::col0"). + std::string linkName; ///< Name of the URDF link this collision belongs to. + urdf::CollisionSharedPtr col; ///< URDF collision element (geometry + origin). }; class PFKinematics { @@ -92,11 +93,11 @@ namespace pfield { std::vector updateObstaclesFromJointAngles(const std::vector& jointAngles); - bool areCachesInitialized() const { return this->cachesReady; } - const std::vector& cachedJointNames() const { return this->jointNamesCache; } - const std::vector& cachedLinkNames() const { return this->linkNamesCache; } - size_t getNumJoints() const { return this->numJoints; } - size_t getNumLinks() const { return this->numLinks; } + [[nodiscard]] bool areCachesInitialized() const { return this->cachesReady; } + [[nodiscard]] const std::vector& cachedJointNames() const { return this->jointNamesCache; } + [[nodiscard]] const std::vector& cachedLinkNames() const { return this->linkNamesCache; } + [[nodiscard]] size_t getNumJoints() const { return this->numJoints; } + [[nodiscard]] size_t getNumLinks() const { return this->numLinks; } /** * @brief Estimate a conservative bounding-sphere radius of the robot from its URDF diff --git a/potential_fields_library/include/pfield/pf_obstacle.hpp b/potential_fields_library/include/pfield/pf_obstacle.hpp index 09de7bf1..a89edf0b 100644 --- a/potential_fields_library/include/pfield/pf_obstacle.hpp +++ b/potential_fields_library/include/pfield/pf_obstacle.hpp @@ -230,15 +230,15 @@ namespace pfield { } bool operator!=(const PotentialFieldObstacle& other) const { return !(*this == other); } - std::string getFrameID() const { return this->frameID; } - ObstacleGroup getGroup() const { return this->group; } - Eigen::Vector3d getPosition() const { return this->position; } - Eigen::Quaterniond getOrientation() const { return this->orientation; } - ObstacleType getType() const { return this->geometry->getType(); } - const ObstacleGeometry& getGeometry() const { return *this->geometry; } - const std::string& getMeshResource() const { return this->meshResource; } - Eigen::Vector3d getMeshScale() const { return this->meshScale; } - std::shared_ptr getCoalCollisionObject() const { return this->coalCollisionObject; } + [[nodiscard]] std::string getFrameID() const { return this->frameID; } + [[nodiscard]] ObstacleGroup getGroup() const { return this->group; } + [[nodiscard]] Eigen::Vector3d getPosition() const { return this->position; } + [[nodiscard]] Eigen::Quaterniond getOrientation() const { return this->orientation; } + [[nodiscard]] ObstacleType getType() const { return this->geometry->getType(); } + [[nodiscard]] const ObstacleGeometry& getGeometry() const { return *this->geometry; } + [[nodiscard]] const std::string& getMeshResource() const { return this->meshResource; } + [[nodiscard]] Eigen::Vector3d getMeshScale() const { return this->meshScale; } + [[nodiscard]] std::shared_ptr getCoalCollisionObject() const { return this->coalCollisionObject; } void setPose(const Eigen::Vector3d newPosition, const Eigen::Quaterniond newOrientation) { this->position = newPosition; @@ -343,7 +343,7 @@ namespace pfield { } }; - static int inline createHashID(const PotentialFieldObstacle& obstacle) { + inline int createHashID(const PotentialFieldObstacle& obstacle) { PotentialFieldObstacleHash hasher; return static_cast(hasher(obstacle) & 0x7FFFFFFF); // keep positive } diff --git a/potential_fields_library/include/pfield/pfield.hpp b/potential_fields_library/include/pfield/pfield.hpp index 01e2cfbd..6d4a5e5c 100644 --- a/potential_fields_library/include/pfield/pfield.hpp +++ b/potential_fields_library/include/pfield/pfield.hpp @@ -338,10 +338,10 @@ namespace pfield { size_t getNumJoints() const { return this->pfKinematics ? this->pfKinematics->getNumJoints() : 0; } size_t getNumLinks() const { return this->pfKinematics ? this->pfKinematics->getNumLinks() : 0; } bool isUsingDynamicQuadraticThreshold() const { return this->dynamicQuadraticThresholdEnabled; } - bool isGoalSet() const { return this->goalSet; } - SpatialVector getGoalPose() const { return this->goalPose; } - std::vector getEnvObstacles() const { return this->envObstacles; } - std::vector getRobotObstacles() const { return this->robotObstacles; } + [[nodiscard]] bool isGoalSet() const { return this->goalSet; } + [[nodiscard]] SpatialVector getGoalPose() const { return this->goalPose; } + [[nodiscard]] std::vector getEnvObstacles() const { return this->envObstacles; } + [[nodiscard]] std::vector getRobotObstacles() const { return this->robotObstacles; } /** * @brief Integrates the current pose forward in time based on the potential field. @@ -603,7 +603,23 @@ namespace pfield { const Eigen::Vector3d& angularVelocity, double deltaTime); - Eigen::VectorXd convertJointTorquesToJointVelocities( + /** + * @brief Converts joint torques to joint velocities utilizing the robot dynamics equation or a simple proportional gain. + * + * The full robot dynamics equation, where joint accelerations (q̈) are integrated to get joint velocities (q̇): + * M(q)·q̈ = τ − C(q,q̇)·q̇ − G(q) − D·q̇ + * If the robot dynamics equation is disabled, it uses a simple proportional gain: + * q̇ = torqueToVelocityGain · τ + * + * @note This function currently always disables the Robot Dynamics Equation due to bugs (see Issue #23). + * + * @param jointTorques Generalized joint torques [Nm]. + * @param jointAngles Current joint configuration [rad]. + * @param prevJointVelocities Previous joint velocities for Coriolis and damping [rad/s]. + * @param dt Integration timestep [s]. + * @return Eigen::VectorXd Joint velocities [rad/s]. + */ + [[nodiscard]] Eigen::VectorXd convertJointTorquesToJointVelocities( const Eigen::VectorXd& jointTorques, const std::vector& jointAngles, const std::vector& prevJointVelocities, const double dt) const; @@ -858,7 +874,7 @@ namespace pfield { std::string eeLinkName; // End-effector link name for kinematic model, IK, and planning std::unique_ptr pfKinematics; // Kinematics helper for obstacle updates via joint angles std::shared_ptr ikSolver; // Inverse kinematics solver for joint angle computation - // Constant Parameters that can be adjusted if neccessary + // Constant parameters that can be adjusted if necessary const double translationalTolerance = 1e-3; // Threshold for distances to the goal and obstacles [m] const double rotationalThreshold = 0.05; // Threshold for rotational geodesic distance [rad] const double softSatBeta = 1.0; // Soft-saturation parameter, higher = more aggressive curve diff --git a/potential_fields_library/include/pfield/pfield_common.hpp b/potential_fields_library/include/pfield/pfield_common.hpp index 1751899e..887623a5 100644 --- a/potential_fields_library/include/pfield/pfield_common.hpp +++ b/potential_fields_library/include/pfield/pfield_common.hpp @@ -23,7 +23,7 @@ namespace pfield { - inline static bool isPositiveFinite(double v) { return std::isfinite(v) && v > 1e-12; } + [[nodiscard]] inline bool isPositiveFinite(double v) { return std::isfinite(v) && v > 1e-12; } /** * @brief Given a vector (typically linear or angular velocity), a maximum norm, @@ -34,7 +34,7 @@ namespace pfield { * @param beta Softness parameter for the saturation, higher = more abrupt (default=1.0) * @return Eigen::Vector3d The saturated vector */ - inline static Eigen::Vector3d softSaturateNorm(const Eigen::Vector3d& v, double maxNorm, double beta = 1.0) { + [[nodiscard]] inline Eigen::Vector3d softSaturateNorm(const Eigen::Vector3d& v, double maxNorm, double beta = 1.0) { if (!isPositiveFinite(maxNorm)) return v; const double n = v.norm(); if (!isPositiveFinite(n)) return v; @@ -53,7 +53,7 @@ namespace pfield { * @param dMax The maximum allowed step size * @return Eigen::Vector3d The rate-limited vector */ - inline Eigen::Vector3d rateLimitStep(const Eigen::Vector3d& prev, const Eigen::Vector3d& curr, double dMax) { + [[nodiscard]] inline Eigen::Vector3d rateLimitStep(const Eigen::Vector3d& prev, const Eigen::Vector3d& curr, double dMax) { if (!isPositiveFinite(dMax) || dMax <= 0.0) return curr; Eigen::Vector3d d = curr - prev; const double dn = d.norm(); @@ -68,7 +68,7 @@ namespace pfield { * @param v The 3D Vector to be rotated * @return Eigen::Vector3d The rotated vector */ - static inline Eigen::Vector3d rotateVector(const Eigen::Quaterniond& q, const Eigen::Vector3d& v) { return q * v; } + [[nodiscard]] inline Eigen::Vector3d rotateVector(const Eigen::Quaterniond& q, const Eigen::Vector3d& v) { return q * v; } /** * @brief Convert a urdf::Pose to an Eigen::Affine3d @@ -76,7 +76,7 @@ namespace pfield { * The URDF pose stores position (x,y,z) and a quaternion (x,y,z,w). This helper * creates an Eigen isometry with the same translation and rotation. */ - static inline Eigen::Affine3d urdfPoseToEigen(const urdf::Pose& p) { + [[nodiscard]] inline Eigen::Affine3d urdfPoseToEigen(const urdf::Pose& p) { const Eigen::Quaterniond q(p.rotation.w, p.rotation.x, p.rotation.y, p.rotation.z); const Eigen::Translation3d t(p.position.x, p.position.y, p.position.z); Eigen::Affine3d T = Eigen::Affine3d::Identity(); @@ -94,7 +94,7 @@ namespace pfield { * @param lambda The damping factor for the least squares solution * @return Eigen::VectorXd The computed joint velocities */ - static inline Eigen::VectorXd dampedLeastSquares( + [[nodiscard]] inline Eigen::VectorXd dampedLeastSquares( const Eigen::MatrixXd& J, const Eigen::VectorXd& V, const double lambda = 1e-3) { @@ -119,7 +119,7 @@ namespace pfield { * @param dt The time difference between the current and previous joint angles [s] * @return std::vector The approximated joint velocities [rad/s] */ - static inline std::vector approximateJointVelocities( + inline std::vector approximateJointVelocities( const std::vector& currentJointAngles, const std::vector& previousJointAngles, const double dt) { @@ -134,34 +134,6 @@ namespace pfield { return jointVelocities; } - static std::array convertHSVToRGB(double hue, double saturation, double value) { - double c = value * saturation; - double x = c * (1 - fabs(fmod(hue / 60.0, 2) - 1)); - double m = value - c; - - double rPrime, gPrime, bPrime; - if (hue >= 0 && hue < 60) { - rPrime = c; gPrime = x; bPrime = 0; - } - else if (hue >= 60 && hue < 120) { - rPrime = x; gPrime = c; bPrime = 0; - } - else if (hue >= 120 && hue < 180) { - rPrime = 0; gPrime = c; bPrime = x; - } - else if (hue >= 180 && hue < 240) { - rPrime = 0; gPrime = x; bPrime = c; - } - else if (hue >= 240 && hue < 300) { - rPrime = x; gPrime = 0; bPrime = c; - } - else { - rPrime = c; gPrime = 0; bPrime = x; - } - - return {rPrime + m, gPrime + m, bPrime + m}; - } - constexpr double DEFAULT_ATTRACTIVE_GAIN = 1.0; // Gain for attractive force [Ns/m] constexpr double DEFAULT_ROTATIONAL_ATTRACTIVE_GAIN = 0.7; // Gain for rotational attractive force [Ns/m] constexpr double DEFAULT_MAX_LINEAR_VELOCITY = 5.0; // [m/s] diff --git a/potential_fields_library/include/solvers/ik_solver.hpp b/potential_fields_library/include/solvers/ik_solver.hpp index ba35f952..55f98c0a 100644 --- a/potential_fields_library/include/solvers/ik_solver.hpp +++ b/potential_fields_library/include/solvers/ik_solver.hpp @@ -41,11 +41,11 @@ namespace pfield { const std::vector& jointPositions, Eigen::Matrix& J) = 0; - virtual std::vector getHomeConfiguration() const = 0; + [[nodiscard]] virtual std::vector getHomeConfiguration() const = 0; - virtual std::vector getJointNames() const = 0; + [[nodiscard]] virtual std::vector getJointNames() const = 0; - virtual std::string getName() const { return this->name; } + [[nodiscard]] virtual std::string getName() const { return this->name; } protected: std::string name; diff --git a/potential_fields_library/include/solvers/xarm_ik_solver.hpp b/potential_fields_library/include/solvers/xarm_ik_solver.hpp index 69c1a39d..5e93d51c 100644 --- a/potential_fields_library/include/solvers/xarm_ik_solver.hpp +++ b/potential_fields_library/include/solvers/xarm_ik_solver.hpp @@ -57,7 +57,7 @@ namespace pfield { // Pinocchio members pinocchio::Model model; pinocchio::Data data; - bool initialized_ = false; + bool initialized = false; // Jparse parameters double gamma_ = 0.2; diff --git a/potential_fields_library/package.xml b/potential_fields_library/package.xml index 4b17a201..189d04d9 100644 --- a/potential_fields_library/package.xml +++ b/potential_fields_library/package.xml @@ -2,7 +2,7 @@ potential_fields_library - 1.0.1 + 1.0.2 Library for managing a vector field overlaid onto a robot's task-space Sharwin Patil Apache 2.0 diff --git a/potential_fields_library/src/solvers/xarm_ik_solver.cpp b/potential_fields_library/src/solvers/xarm_ik_solver.cpp index e2d93d7b..df5d187b 100644 --- a/potential_fields_library/src/solvers/xarm_ik_solver.cpp +++ b/potential_fields_library/src/solvers/xarm_ik_solver.cpp @@ -23,7 +23,7 @@ namespace pfield { } this->data = pinocchio::Data(this->model); - initialized_ = true; + initialized = true; } bool XArmIKSolver::solve( @@ -32,7 +32,7 @@ namespace pfield { std::vector& solution, Eigen::Matrix& J, std::string& errorMsg) { - if (!initialized_) { + if (!initialized) { errorMsg = "XArmIKSolver not initialized (failed to load URDF)"; return false; } @@ -168,9 +168,7 @@ namespace pfield { const std::vector& joint_positions, Eigen::Matrix& J) { - if (!initialized_) { - return false; - } + if (!initialized) { return false; } std::vector joint_names = getJointNames(); if (joint_positions.size() != 7) {