If optimization is only needed in the null space, for instance, when the Jacobian controller generate the entire path fromξ1toxf, then the optimization problem is reformulated. A Jacobian controller
generated path will have low acceleration costs with a low overall task space path length cost. Omitting these optimization costs, only costs in the null space of the task space are optimized without introducing significant acceleration and task space length costs. The null space matrix has ranknq−nx, so the joint space path,ξ1:nt ∈R
nt×nq, is reformulated into a lower dimensional path
λ1:nt ∈R
nt×(nq−nx).
First, the null space components of the trajectory are identified in Equation (13).
ξtNULL=Nq(ξt+1−ξt) (13)
The lower dimensional coordinatesλt are found by performing a Singular Value Decomposition
to find an orthogonal basis, similar to techniques that analysis Jacobian singularities [131]. With
Nq=U SVT,λt is defined in Equation (14) and its reconstruction toξt in Equation (15).
λt=StVtT(ξt+1−ξt) (14)
ξtNULL=Utλt (15)
As noted, a side effect of using the Jacobian pseudo-inverse in Equation (10) is that its pseudo null space is full rank. Instead of choosingnq−nx coordinates, we are afforded the possibility of
using more if we wish, even in more dimensions mean more incursion on the task space trajectory. We projectξt and reconstructξtNULLusing only thenq−nx basis vectors with the highest singular
values so thatλt ∈Rnq−nx.
When an optimal subspace trajectoryλ1:∗nt is found, we determine the resulting joint trajectory
as in Equation (16). Only the pseudo null space is updated.
Figure 4.3: Some initial trajectories require joint interpolation since the Jacobian controller times out. The straight lines at the end of the trajectories show how the controller switches.
We want to still ensure a smooth trajectory, but in the reformulated space that omits some information onξt, we can only use an approximate cost to represent accelerations. Asλ is a velocity
coordinate because N operates on a joint difference, we mimic the cost of joint acceleration in Equation (17). Gacceleration(λ1:nt) = nt−1
∑
t=2 ||λt−λt−1||2 (17)The same boundary conditions also are used: λ∗f =λˆf, λ1∗=λˆ1. However, the human pref-
erence costs cannot be reformulated to equivalent costs in the λ1:,nt subspace, but since the final
configuration was achieved, we just need to find the shortest path to the achieved human preference. We append the cost in Equation (18) to represent the shortest path to the human preference.
Gpath= nt−1
∑
t=1 ||λt||2 (18) 4.3.1 Basis FilteringSingular value decomposition includes ambiguity with respect to basis vector directions, which for a series of decompositions, need to be resolved [15]. We set the sign of the jth basis vector,stj
Figure 4.4: The optimization routine is able to smooth out the trajectory, even if two different controllers were used in generation.
accordingly to Equation (19). stj= 1 ifVtj·V j t−1>φ −1 ifVtj·Vt−j 1<−φ (19)
Additionally, when tracking the basis vectors in time, the singular values may “flip” their order [22], [108]. We reorder the basis vectors accordingly, whennx−nq>1, so that each coordinate
λj can refer to the same basis vectorVtj, even if that direction is no longer associated with the jth
largest singular value att. We reorder according to Equation (20). We only consider swapping j
with j+1 and j−1, which through empirical testing, works well.
k∗=argmaxk V k t ·V j t−1 (20) Vt−j1←Vtk−∗1 (21)
If the dot product is small at time t, then we cannot continue to track λt:nt being consistent
withλ1:t−1, and so we must remove the acceleration constraint of Equation (17) at timet for the
4.3.2 Singularities
Even with properly tracked basis vectors, changes inλtcan cause unpredictable changes inξt when
two basis vectors inNthave very similar singular values. We condition the optimization formulation
bounds based on this insight, with a modified linearized region bound, shown in Equation (22).
||λt∗−λˆ
∗
1||<ctελ (22)
The scaling factorct is the minimum difference between large enough singular values at timet:
ct=minj|σj>µ|σj−σj+1|.
Figure 4.5: Above, the THOR-RD robot plans a path with the elbow protruding out with high manipulability in the middle of the joint range. Below, the robot plans a path with the elbow tucked in for maneuvering in tight spaces. The target transform for both paths is the same.