armarx::navigation::global_planning::optimization Namespace Reference

This file is part of ArmarX. More...

Classes
class	OrientationOptimizer
struct	OrientationOptimizerParams
struct	OrientationPriorCostFunctor
struct	SmoothOrientationCostFunctor
struct	SmoothOrientationFixedNextCostFunctor
struct	SmoothOrientationFixedPreCostFunctor

Enumerations
enum class	RotationDirection { Any = 10 , Clockwise = 1 , CounterClockwise = 0 }

Functions
double	angleDiff (const double a, const double b)
auto	periodicDiff (const auto a, const auto b)
	Signed, wrapped difference b - a, in (-pi, pi].
std::array< double, 2 >	periodicDiffJacobian (const auto, const auto)
	Partial derivatives of periodicDiff(a, b) with respect to a and b.

Detailed Description

This file is part of ArmarX.

ArmarX is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License version 2 as published by the Free Software Foundation.

ArmarX is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with this program. If not, see http://www.gnu.org/licenses/.

Author

Fabian Reister ( fabian dot reister at kit dot edu )

Date

2021

Copyright

http://www.gnu.org/licenses/gpl-2.0.txt GNU General Public License

Enumeration Type Documentation

RotationDirection

enum class RotationDirection

strong

Enumerator
Any
Clockwise
CounterClockwise

Definition at line 31 of file OrientationOptimizer.h.

Function Documentation

angleDiff()

double angleDiff ( const double a, const double b )

inline

Definition at line 38 of file OrientationOptimizer.cpp.

Here is the caller graph for this function:

periodicDiff()

auto periodicDiff	(	const auto	a,
		const auto	b )

Signed, wrapped difference b - a, in (-pi, pi].

atan2 of the cross and dot products of the two unit vectors. The previous formulation was acos of the dot product alone, which has two problems this one does not have:

• acos is defined on [-1, 1] and the dot product of two unit vectors leaves it by up to one ulp whenever the angles agree – which is every straight stretch of a path – so it returned NaN for 3.7 % of equal-angle pairs and Ceres rejected the step;
• its derivative is infinite exactly where the angles agree, so the analytic jacobian had to guard the singularity by returning zero there. That is the wrong value: a set of perfectly aligned waypoints is where the gradient was needed and where it was suppressed.

atan2 is well conditioned for every pair and its derivative is exactly -1 with respect to a and +1 with respect to b, everywhere.

The residual is now signed where it used to be a magnitude. The objective is unchanged: Ceres squares residuals, and |b - a|^2 == (b - a)^2 for the wrapped difference.

Definition at line 53 of file math.h.

Here is the call graph for this function:

Here is the caller graph for this function:

periodicDiffJacobian()

std::array< double, 2 > periodicDiffJacobian ( const auto , const auto  )

inline

Partial derivatives of periodicDiff(a, b) with respect to a and b.

Constant, and no longer a guarded singularity: the wrapped difference is locally just b - a. The wrap itself is not differentiable at +-pi, where the shortest way round swaps – a measure-zero point the minimizer has no reason to sit on, and one the old formulation could not represent either.

Definition at line 67 of file math.h.

Here is the caller graph for this function: