Velocity projection between s-domain and joint domain

[mahmoud-a-ali]

with the current implementation of s-variable, there are some issues when it comes to the non-zero velocity at waypoint:

• all the joints are linear functions in the s-variable (e.g. q_i(s)=m_i * s+ c_i for any joint i), so q`(s) for each joint has its own value which is m_i, and q_dot for each joint is (q_dot(s) = q`(s) * s_dot = m_i * s_dot). the current function to project q_dot on s-domain project_limits_onto_s calculates s_dot for each joint and then takes the min s_dot as the dominant one, and as a consequence:
• as the s_dot is the minimum value ( in our case), all the joint velocities will be related to each other with ratio (m_i/m_j) e.g m_j is the joint with minimum s_dot and m_i any other joint.

Example with numbers [ when we convert joint velocity to s-domain]:

• linear functions of s-variable are q(s)=[0.6s, 0.8s], starting joint velocity= [0.0, 0.1] ==> [0.0, 0.125] in s-domain , as 0.0 is the dominant s_dot, then both joints start with joint velocity equal to (m_is_dot) = 0.0. ending joint velocity=[0.3, 0.4] ==> [.33, .625] in s-domain, as 0.33 is the dominant s_dot. then the ending joint velocity equal to (m_is_dot) = [0.19, .26]. as q`(s) is constant (m_i for i joint) then all the velocities are related to each other.

I was wondering why the topp-ra method does not have the same issues, I found that they using s-variable in such a way that q(s) is a cubicspline [q(s)= as^3 + bs^2 + cs + d] , so [q`(s) = 3as^2 + 2bs + c], which mean that with the same rule (q_dot(s) = q`(s) * s_dot ), a single value of s_dot combined with the three variables (a, b, c) can result in non-related velocities in joint-domain. they are using piecewise cubic functions instead of piecewise linear functions.

• the second issue: when calculating s_dot ( instead of converting joint velocities to s-domani and choose minimum between of them ), the velocity in s-domain (s_dot) will be continuous but when it is converted to the joint domain it won't be continuous anymore as each segment have its own function (or its own slope m_i)

• the third issue: if any joint velocity becomes zero (change direction), then s_dot for that joint will be zero, and as we take minimum s-dot, then all the other joints will have zero velocity as well.

[jonbinney]

Thanks for the detailed explanation! If I understand correctly the 3 points are:

1. The joints velocities are all related to each other by some constant ratio at each segment in the path. I'm not sure why this is a problem - in fact it must be true or else the arm would be leaving the chosen path, which could result in collisions.

2. There are discontinuities where linear segments meet - this also is always going to be true for piecewise linear paths. Any S-parameterization of the path will have the same problem; fundamentally it is impossible for the robot to change direction in joint space instantly without having infinite instantaneous acceleration and jerk. The solution is that we must create smooth blends in the path before doing time parameterization - that's what I'm working on now.

3. I don't think this will be a problem after we add blending to smooth transitions between segments. The joint whose velocity passes through zero will have dj/ds = 0 for the point where it changes direction. So ds/dt can still be nonzero while keeping dj/dt for that joint equal to zero.

[mahmoud-a-ali]

yes, you got the three points I want to discuss. I agree that the joints' velocities will be related to each other for each segment. but as you mentioned that "There are discontinuities where linear segments meet - this also is always going to be true for piecewise linear paths", I think this discontinuity can be removed if the joints are represented by a higher-order polynomial of S.

[jonbinney]

If the higher level polynomial follows the same geometric path as the piecewise linear path, then the only way it would avoid velocity discontinuities would be to have dj/ds = 0 for all joints at the points where the arm changes directions. That would mean no matter how you did the time parameterization ( ds/dt ), all joints will stop at every waypoint - so we'd be just as well off keeping the piecewise linear function of s for the joints, and using a time parameterization that stops the robot at every waypoint.

I'm pretty convinced that moving with the joints with non-zero velocity through the waypoints will require geometric change to the path, not just a change in the form of the parameterization.

[mahmoud-a-ali]

update after blending:
A. Before:

• As the main source of velocity discontinuity which happened between any two adjacent segments is that we are using linear (1st order) piecewise s-variable [ where qi(s)=mi * s+ ci for each joint i] ... check the following fig. for a two-segment path/trajectory:
  
  

• Explanation: consider "dqsj" is the velocity of segment s and joint j
  for any value of s_dot at the end of the first segment, we have two values for joint velocities according to the relation [ dq1=m1s_dot] and [ dq2=m2s_dot] lets called them dq11 and dq12.
  the same way the begging of the second segment, for any value of s_dot we have dq21 and dq22.
  as m1m2 during the second segment, there is no way to have dq11=dq21 and dq12=dq22 except of setting s_dot=0, which mean dq11=dq12=dq21=dq22=0, which stop at each waypoint.

• Note: the acceleration looks continuous because we fit each segment separately using the fit_seven_segment function and we always start and end with zero acceleration. and for each segment, we start and end with one velocity in s-domain which generates discontinuous velocity in joint domain [ dqi=mi*s_dot for each joint i].

B. After:

• by adding a blending segment (with a higher order of s or based on sinusoidal function), we have [dqi = qi'(s)*s_dot for each joint i ]. as qi'(s) is not linear anymore and it has more degree of freedom, it servers as a transition between the velocities of the two segments, where the blending segment starts with (q11, q12) and ends with (q21, q22) which keeps the velocity of the two joints continuous, check the following figures path/trajectory for same waypoints:
  
  

• I was trying to transfer the original segment to be represented with a higher order of s-polynomial but it did not work.

• as Jon suggested we can search for another method for calculating the motion profile for the blending segment which grantee the limits along the curvature.

[mahmoud-a-ali]

@jonbinney
we might need to change the project_limits_onto_s, this function takes only the first derivative of q(s) into account which is true for the original segments (as 2nd and 3rd derivative are zeros because it is linear in s), but it is not true for the blending segment. .... as the blending 2nd and 3rd dervative are not zeros, look at the following equations from paper:

we were using equation number (2) to project velocity, acceleration, and jerk limits which is not right in the blending segment case

[jonbinney]

Yes, @mahmoud-a-ali you're right. project_limits_onto_s assumes a linear segment path. If we want to compute constant jerk/acceleration/velocity limits for the blending section, we'll have to take use the equations you listed and take the minimum (the limits are different at each point in the blending section).

[jonbinney]

Hmm... this is slightly trickier than i thought. This paper extends the paper you cited to include jerk constraints using sequential convex programming. They use the chain rule to get the jerk depending on q(s) and s(t):

In the rest of this comment, I'm going to use the following nomenclature so that i can write in plain text:
sd means ,
sdd means
sdd means

Looking at these, it becomes clear that the constraints on sd, sdd, and sdd are not independent - meaning that the constraint on sdd depends on choice velocity and jerk, the constraint on sddd depends on the acceleration and velocity chosen, etc.

Put another way, if you plotted the region of valid values for sd, sdd, and sddd on the axes sd, sdd and sddd, that region would not be an axis-aligned rectangular prism. So we can't just return upper and lower limits on sd, sdd, and sddd for curved parts of the path - we have to return something more complicated. In the paper, they use the following representation for the jerk constraint:

where:

For their SCO approach, that is fine as long as the can use tricks to decompose the constraints into a set of convex constraints. For the kinds of approaches we've been pursuing this is more complex. My original thought was that we could require sd to be constant within the blending regions. This greatly simplifies the limits - you know that sdd and sddd are zero, so the upper and lower limits on sd only depend on s (where you are in the blended region). My hunch is that for a blend region that has constant curvature in joint space, the limits on sd will actually be constant across the entire blend region (the limit on sd only depends on the curvature). I still need to think through this though.

[jonbinney]

@mahmoud-a-ali @gavanderhoorn what do you think about me moving this repository to gitlab so that we can use latex math in issues, pull requests, and readme files? https://docs.gitlab.com/ee/user/markdown.html#math

[jonbinney]

Update: I tried out gitlab's markdown, and while it is almost exactly what we need, it is missing a couple of key features. Specifically, it doesn't have \dddot to denote the third derivative. I was able to create a def for it:

\def\dddot#1{#1^{\mathclap{\dotsb}}}
\dot{s} + \ddot{s} + \dddot{s}

But since each math block has its own scope, we would have to copy and paste that first line into every one. Overall I think it isn't worth the move. I'll look into latex to image renderers that we can use to create math images we can copy to github issues as a workaround.

[gavanderhoorn]

What about using d''' notation?

[jonbinney]

The TOPPRA authors use apostrophes to indicate the derivative with respect to s - so they have equations with a mix of dots and apostrophes:

I'd suggest that in plain text we use d for dot so we could write the above equation as

qddd = q'''*sddd^3 + 3*q''*sdd*sd + q'*sddd