Package: s-curve-beta
This file is also available asREADME.nbMathematicanotebook that replicates all derivations and plots.s-curve-beta: Efficient Python implementation of the smoothest S-curve robot motion planner everIt's 2022 and you're still using a trapezoid motion profile?Your robot dog is vibrating like a washing machine?Tired of the mess in the code because of the multiple piecewise time regions?Want a simple single-formula smooth motion profile?Want an S-curve implementation that literally fits in 20 lines of code?If you answer "yes" to any of the above, read on.Based on the answer of Cye Waldman (https://math.stackexchange.com/a/2403818) I present this Python package to calculate the S-curve for the robot motion planning with a given maximum velocity and acceleration with a single formula.Here is the complete motion formula:$$f\left( t, p \right)=\frac{1}{2}\left[ 1+\text{sgn} (x)\cdot \frac{B\left( {1}/{2}, p+1,{t^2} \right)}{B\left( {1}/{2}, p+1 \right)} \right],$$$$motionTime(robotVmax, robotAmax,motionRange)=\
max\left[\frac{2\cdot3^{3/4}}{\sqrt{\pi}}\cdot\sqrt\frac{motionRange}{robotAmax},\frac{32}{5\pi}\cdot\frac{motionRange}{robotVmax}\right],$$$$position(t,robotVmax,robotAmax,motionRange)=\
motionRange\cdot f\left(\frac{2 t}{motionTime}-1,2.5\right),$$wheresgn(x)is Sign function, numerator and denominator𝐵sare the incomplete and complete beta functions, respectively (that's wherebetacomes from in the name),robotVmaxis the maximum velocity,robotAmaxis the maximum acceleration andmotionRangeis the absolute value of the motion (from start to end).Here are 2 examples of motion planning with $motionRange=2$:The motion curve always has the same shape, while horizontal and vertical scales change.This property has the following benefits:The curve (function $f(t,2.5)$ for $-1\le t \le 1$) can be precomputed, so instead of calculating Beta functions each time it's possible to do the linear interpolation of the precomputed points. They can be programmed in a microcontroller.The code is much simpler (<20 lines).The movements appear more natural and human-like.The disadvantages are:The movements may take longer time to complete for large motions.You can't simply put a linear area in the middle of the curve, that would introduce discontinuity and possibly large values in jerk.InstallationUsing pip (recommended):pip install s-curve-betaFrom source:git clone https://github.com/hidoba/s-curve-beta.git
cd s-curve-beta
python setup.py installExamplesCalculating robot position at given moments of time:importscurvebetaasscb# motion parametersmax_velocity=12max_acceleration=3x0=-3x1=10# Calculate motion timemotionTime=scb.motionTime(max_velocity,max_acceleration,abs(x0-x1))print("motionTime = ",motionTime,"seconds")# Calculate position at a given timet=2.3print("position(",t,") = ",scb.sCurve(t,motionTime,x0,x1))# Calculate multiple positions at given moments of time (faster, most recommended)importnumpyasnpt=np.array([-1,0,1,2,3,motionTime,10])print("position(",t,") = ",scb.sCurve(t,motionTime,x0,x1))Making plots:importscurvebetaasscbimportmatplotlib.pyplotaspltimportnumpyasnpdefplotMotion(max_velocity,max_acceleration,x0,x1):motionTime=scb.motionTime(max_velocity,max_acceleration,abs(x0-x1))dt=0.08t=np.arange(0,motionTime,dt)pos=scb.sCurve(t,motionTime,x0,x1)vel=np.diff(pos)/dtacc=np.diff(vel)/dtjerk=np.diff(acc)/dtfig,axs=plt.subplots(2,1)axs[0].plot(t,pos)axs[0].set_ylabel('Position')axs[0].grid(True)axs[1].plot(t[:-1],vel,label='velocity')axs[1].plot(t[:-2],acc,label='acceleration')axs[1].plot(t[:-3],jerk,label='jerk')axs[1].axhline(y=max_acceleration,color='orange',linestyle='--')axs[1].axhline(y=max_velocity,color='blue',linestyle='--')axs[1].text(0,max_acceleration+0.15,'max_acceleration='+str(max_acceleration))axs[1].text(0,max_velocity+0.15,'max_velocity='+str(max_velocity))axs[1].grid(True)axs[1].set_xlabel('Time')axs[1].legend(handlelength=4)fig.tight_layout()plt.show()# Please note that if you need more accurate velocity / acceleration# you should better use the 'true' version of the function instead of the 'interpolated' one.# Pay attention at the jerk on the second plot, the sawteeth are because of the imprecision# of very small changes of the third derivative. If you just need the position,# interpolated function should be just fine.plotMotion(6,3,-3,10)plotMotion(2,3,-3,10)Interpolated vs True functionBy default s-curve-beta uses an interpolated version of theffunction (using 801 points interpolation). It's very fast if used on the arrays and it can be easily adapted for microcontrollers.You can use the true function (requires scipy) by importing an optionalscurvebetatruemodule:fromscurvebetaimportscurvebetatrueprint(scurvebetatrue.sCurve_true(2.3,15,-1,5))importscurvebetaasscbprint(scb.sCurve(2.3,15,-1,5))-0.8849490453964555
-0.8849435436031998Compare the execution speed:importtimeitfromscurvebetaimportscurvebetatrueimportscurvebetaasscbimportnumpyasnpdeftrueFunction():returnscurvebetatrue.sCurve_true(2.3,15,-1,5)deftrueFunctionArray():returnscurvebetatrue.sCurve_true(np.arange(0,15,15/100000),15,-1,5)definterpolatedFunction():returnscb.sCurve(2.3,15,-1,5)definterpolatedFunctionArray():returnscb.sCurve(np.arange(0,15,15/100000),15,-1,5)print(timeit.timeit('trueFunction()',number=100000,setup="from __main__ import trueFunction"))# 0.4408080680000239print(timeit.timeit('trueFunctionArray()',number=1,setup="from __main__ import trueFunctionArray"))# 0.4047970910000913print(timeit.timeit('interpolatedFunction()',number=100000,setup="from __main__ import interpolatedFunction"))# 0.381616509999958print(timeit.timeit('interpolatedFunctionArray()',number=1,setup="from __main__ import interpolatedFunctionArray"))# 0.0019440340000755896Plots with "True" function:importscurvebetaasscbfromscurvebetaimportscurvebetatrueimportmatplotlib.pyplotaspltimportnumpyasnpdefplotMotionTrue(max_velocity,max_acceleration,x0,x1):motionTime=scb.motionTime(max_velocity,max_acceleration,abs(x0-x1))dt=0.005t=np.arange(0,motionTime,dt)pos=scurvebetatrue.sCurve_true(t,motionTime,x0,x1)vel=np.diff(pos)/dtacc=np.diff(vel)/dtjerk=np.diff(acc)/dtfig,axs=plt.subplots(2,1)axs[0].plot(t,pos)axs[0].set_ylabel('Position')axs[0].grid(True)axs[1].plot(t[:-1],vel,label='velocity')axs[1].plot(t[:-2],acc,label='acceleration')axs[1].plot(t[:-3],jerk,label='jerk')axs[1].axhline(y=max_acceleration,color='orange',linestyle='--')axs[1].axhline(y=max_velocity,color='blue',linestyle='--')axs[1].text(0,max_acceleration+0.15,'max_acceleration='+str(max_acceleration))axs[1].text(0,max_velocity+0.15,'max_velocity='+str(max_velocity))axs[1].grid(True)axs[1].set_xlabel('Time')axs[1].legend(handlelength=4)fig.tight_layout()plt.show()plotMotionTrue(6,3,-3,10)plotMotionTrue(2,3,-3,10)Multiple axes synchronizationTo synchronize multiple axes simply calculate the maximum motion time of all axes and use it for every axis. This will minimize the maximum jerk and acceleration of the initially faster axes, decreasing the vibrations of the robot.DerivationsBelow derivations includeMathematicacode that can be used to replicate them.This file is also available asREADME.nbMathematicanotebook that replicates all derivations and plots.The originalffunction comes from the integration of thesuperparabola,doneby Cye Waldman. We can prove that his integration is correct:f[x_,p_]:=1/2(1+RealSign[x]*Beta[x^2,1/2,p+1]/Beta[1/2,p+1])FullSimplify[Integrate[(1-x^2)^p/Beta[1/2,1+p],{x,-1,t}]-f[t,p],Element[p,Reals]&&Element[t,Reals]&&0<=t<1&&p>-1](* 0 *)Proof that the most optimal $p$ is 2.5My suggested value of $p$ is 2.5 which gives the lowest possible absolute jerk, let's see why.Visualizing derivatives with different values of $p$:Animate[Show[Plot[Evaluate@Table[D[f[x,2.5],{x,i}],{i,0,3}],{x,-1,1},PlotLegends->{"position f(t,2.5)","velocity f'(t,2.5)","acceleration f''(t,2.5)","jerk f'''(t,2.5)"},PlotRange->{-11,8},PlotLabel->"p = "~~ToString[param]],Plot[Evaluate@Table[D[f[x,param],{x,i}],{i,0,3}],{x,-1,1},PlotLegends->(StringJoin[#,"(t,"~~ToString[param]~~")"]&/@{"position f","velocity f'","acceleration f''","jerk f'''"}),PlotRange->All,PlotStyle->Dashed]],{param,2.00001,5,0.15}]Calculating largest jerks for different values of the parameter $p$:jerkValues[param_]:={Limit[D[f[x,param],{x,3}],x->0],Maximize[{D[f[x,param],{x,3}],-1<=x<=1},x][[1]]}}paramData=Table[{p,jerkValues[p]},{p,2.001,3,0.01}];ListLinePlot[Abs[Transpose[(Outer[List,{#1},#2][[1]]&@@@paramData)]],PlotLegends->{"|max negative jerk| (at x=0)","max positive jerk"},Epilog->{Dashed,InfiniteLine[{2.5,0},{0,1}]},AxesLabel->{"p","max jerk"}]The best value of the parameter $p$ happens to be 2.5, where the max absolute jerk is the smallest.Discussion on $2\le p<2.5$It's possible to achieveslightly lower maximum robot accelerationsat $2\le p<2.5$:param=2;Show[Plot[Evaluate@Table[D[f[x,2.5],{x,i}],{i,0,3}],{x,-1,1},PlotLegends->{"position f(t,2.5)","velocity f'(t,2.5)","acceleration f''(t,2.5)","jerk f'''(t,2.5)"},PlotRange->All],Plot[Evaluate@Table[D[f[x,param],{x,i}],{i,0,3}],{x,-1,1},PlotLegends->(StringJoin[#,"(t,"~~ToString[param]~~")"]&/@{"position f","velocity f'","acceleration f''","jerk f'''"}),PlotRange->All,PlotStyle->Dashed]]Values of $2\le p<2.5$ can shorten the robot motion time a little bit at the cost of increased maximum jerk hence increased robot vibrations at the beginning and at the end of the motion. I have decided not to implement this at the moment.Rescaling functionffor the particular motion parameters1. Motion time from the maximum velocity constraintMax velocity at $t=0$:maxVelocity=Limit[D[f[x,5/2],x],x->0]$$\frac{16}{5 \pi}$$With the given maximum robot velocityrobotVmaxthe shortest motion time can be derived from the equation:$$robotVmax=\frac{2\frac{16}{5\pi}motionRange}{time}$$Hence,$$time = \frac{32}{5 \pi}\cdot\frac{motionRange}{robotVmax}$$2. Motion time from the maximum acceleration constraintCalculate max acceleration at $p=2.5$:FindMaximum[{D[f[x,5/2],{x,2}],-1<x<0},x](*{1.65399, {x -> -0.5}}*)Maximum acceleration happens to be at $t=-\frac{1}{2}$D[f[x,5/2],{x,2}]/.x->-1/2$$\frac{3\sqrt3}{\pi}$$With the given maximum robot accelerationrobotAmaxthe shortest motion time can be derived from the equation:$$\text{robotAmax}=\frac{4\frac{3\sqrt{3}}{\pi }\text{motionRange}}{time^2}$$Hence,$$time=\frac{2\cdot3^{3/4}}{\sqrt{\pi}}\cdot\sqrt\frac{motionRange}{robotAmax}$$3. Motion time with both constraintsTo consider both maximum velocity and maximum acceleration constraints we have to take the maximum of the above motion times:$$motionTime=
max\left[\frac{2\cdot3^{3/4}}{\sqrt{\pi}}\cdot\sqrt\frac{motionRange}{robotAmax},\frac{32}{5\pi}\cdot\frac{motionRange}{robotVmax}\right],$$In the future I may add a maximum jerk constraint.4. Final motion position formulaWe have to rescaletin $f(t,2.5)$ in such a way that the motion would start at $t=0$ and end at $t=motionTime$. Additionally we have to rescale the value offto go from 0 tomotionRange. After rescaling we get:$$position(t,robotAmax,motionRange)=\
motionRange\cdot f \left(\frac{2 t}{motionTime}-1,2.5 \right)$$Example robot motions limited by accelerationLicenseCopyright (c) 2022 Vladimir Grankovsky at Hidoba Research. This work is licensed under an Apache 2.0 license.THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.