|
| 1 | +#!/usr/bin/env python |
| 2 | +# Software License Agreement (BSD License) |
| 3 | +# |
| 4 | +# Copyright (c) 2016, Kei Okada |
| 5 | +# All rights reserved. |
| 6 | +# |
| 7 | +# Redistribution and use in source and binary forms, with or without |
| 8 | +# modification, are permitted provided that the following conditions |
| 9 | +# are met: |
| 10 | +# |
| 11 | +# * Redistributions of source code must retain the above copyright |
| 12 | +# notice, this list of conditions and the following disclaimer. |
| 13 | +# * Redistributions in binary form must reproduce the above |
| 14 | +# copyright notice, this list of conditions and the following |
| 15 | +# disclaimer in the documentation and/or other materials provided |
| 16 | +# with the distribution. |
| 17 | +# * Neither the name of Kei Okada nor the names of its |
| 18 | +# contributors may be used to endorse or promote products derived |
| 19 | +# from this software without specific prior written permission. |
| 20 | +# |
| 21 | +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
| 22 | +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
| 23 | +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
| 24 | +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
| 25 | +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
| 26 | +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
| 27 | +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
| 28 | +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
| 29 | +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
| 30 | +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
| 31 | +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 32 | +# POSSIBILITY OF SUCH DAMAGE. |
| 33 | +# |
| 34 | + |
| 35 | +""" |
| 36 | +This library implement Minimum Jerk trajectory generation, a.k.a Hoff |
| 37 | +& Arbib, described in the documents |
| 38 | +http://mplab.ucsd.edu/tutorials/minimumJerk.pdf (you can find copy of |
| 39 | +this at http://www.shadmehrlab.org/book/minimumjerk.pdf) |
| 40 | +
|
| 41 | +Hoff B, Arbib MA (1992) A model of the effects of speed, accuracy, and |
| 42 | +perturbation on visually guided reaching. In: Control of arm movement |
| 43 | +in space: neurophysiological and computational approaches |
| 44 | +(Caminiti R, Johnson PB, Burnod Y, eds), pp 285-306. |
| 45 | +
|
| 46 | +~~~~~~~~~~~~~~~~~~~~~~~~ Min Jerk ~~~~~~~~~~~~~~~~~~~~~~~~ |
| 47 | +A library for computing minimum jerk trajectory for an arbitrary |
| 48 | +set of control points in R2, R3, up to RN space. |
| 49 | +
|
| 50 | + x(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5 |
| 51 | + x'(t) = a1 + 2*a2*t + 3*a3*t^2 + 4*a4*t^3 + 5*a5*t^4 |
| 52 | +x''(t) = 2*a2 + 6*a3*t + 12*a4*t^2 + 20*a5*t^3 |
| 53 | +
|
| 54 | +Solve this problem from boundary conditions of x(0), x'(0), x''(0) and |
| 55 | +x(T), x'(T), x''(T). |
| 56 | +
|
| 57 | +ex. usage: |
| 58 | +
|
| 59 | +import numpy |
| 60 | +import minjerk |
| 61 | +points_array = numpy.array([[1, 2, 3], [4, 4, 4], |
| 62 | + [6, 4, 6], [2, 5, 6], |
| 63 | + [5, 6, 7]]) |
| 64 | +m_coeffs = minjerk.minjerk_coefficients(points_array) |
| 65 | +m_curve = minjerk.minjerk_trajectory(m_coeffs, 10) |
| 66 | +# plotting example |
| 67 | +import matplotlib.pyplot as plt |
| 68 | +from mpl_toolkits.mplot3d import Axes3D |
| 69 | +fig = plt.figure() |
| 70 | +ax = fig.gca(projection='3d') |
| 71 | +#plot bezier curve |
| 72 | +ax.plot(m_curve[:,0], m_curve[:,1], m_curve[:,2], 'r') |
| 73 | +#plot specified points |
| 74 | +ax.plot(points_array[:,0], points_array[:,1], points_array[:,2], 'g*') |
| 75 | +ax.set_title("Minimum Jerk Trajectory") |
| 76 | +ax.set_xlabel("X") |
| 77 | +ax.set_ylabel("Y") |
| 78 | +ax.set_zlabel("Z") |
| 79 | +ax.legend(["Minimum Jerk", "Control Points"], loc=2) |
| 80 | +plt.show() |
| 81 | +""" |
| 82 | + |
| 83 | +import numpy as np |
| 84 | + |
| 85 | + |
| 86 | +def minjerk_coefficients(points_array, duration_array=None): |
| 87 | + """ |
| 88 | + Compute the min-jerk coefficients for a given set for user-supplied control pts |
| 89 | + |
| 90 | + params: |
| 91 | + points_array: array of user-supplied control points |
| 92 | + numpy.array of size N by k |
| 93 | + N is the number of control points |
| 94 | + k is the number of dimensions for each point |
| 95 | + duration_array: array of user-supplied control duration of ech segment |
| 96 | + numpy.array of size N-1 |
| 97 | + N is the number of control points |
| 98 | +
|
| 99 | + returns: |
| 100 | + m_coeffs: k-dimensional array of N-1 x (6 coefficients + 1 duration of each segment) |
| 101 | + numpy.array of size N-1 by (6+1) by k |
| 102 | + """ |
| 103 | + (rows, k) = np.shape(points_array) |
| 104 | + N = rows - 1 # N minus 1 because points array includes x_0 |
| 105 | + m_coeffs = np.zeros(shape=(k, N, 7)) |
| 106 | + x = points_array[0] |
| 107 | + v = np.zeros(k) |
| 108 | + a = np.zeros(k) |
| 109 | + if duration_array == None: |
| 110 | + duration_array = np.array([1.0]*N) |
| 111 | + assert len(duration_array) == N,\ |
| 112 | + "Invalid number of intervals chosen (must be equal to N+1={})".format(N) |
| 113 | + for i in range(0, N): |
| 114 | + gx = points_array[i+1]; |
| 115 | + t = duration_array[i] |
| 116 | + if i == N-1: |
| 117 | + gv = np.zeros(k) |
| 118 | + else: |
| 119 | + t0 = t |
| 120 | + t1 = duration_array[i+1] |
| 121 | + d0 = points_array[i+1] - points_array[i] |
| 122 | + d1 = points_array[i+2] - points_array[i+1] |
| 123 | + v0 = d0 / t0 |
| 124 | + v1 = d1 / t1 |
| 125 | + gv = np.where(np.multiply(v0, v1)>=1e-10, 0.5 * ( v0 + v1 ), np.zeros(k)) # 0 + eps |
| 126 | + ga = np.zeros(k) |
| 127 | + |
| 128 | + A=(gx-(x+v*t+(a/2.0)*t*t))/(t*t*t); |
| 129 | + B=(gv-(v+a*t))/(t*t); |
| 130 | + C=(ga-a)/t; |
| 131 | + |
| 132 | + a0=x; |
| 133 | + a1=v; |
| 134 | + a2=a/2.0; |
| 135 | + a3=10*A-4*B+0.5*C; |
| 136 | + a4=(-15*A+7*B-C)/t; |
| 137 | + a5=(6*A-3*B+0.5*C)/(t*t); |
| 138 | + |
| 139 | + x = gx |
| 140 | + v = gv |
| 141 | + |
| 142 | + m_coeffs[:,i,0] = a0 |
| 143 | + m_coeffs[:,i,1] = a1 |
| 144 | + m_coeffs[:,i,2] = a2 |
| 145 | + m_coeffs[:,i,3] = a3 |
| 146 | + m_coeffs[:,i,4] = a4 |
| 147 | + m_coeffs[:,i,5] = a5 |
| 148 | + m_coeffs[:,i,6] = t |
| 149 | + return m_coeffs |
| 150 | + |
| 151 | +def minjerk_trajectory(m_coeffs, num_intervals, duration_array=None): |
| 152 | + """ |
| 153 | + Iterpolation of the entire minimum jerk trajectory at once, |
| 154 | + using a specified number of intervals between |
| 155 | + control points (encapsulated by m_coeffs). |
| 156 | +
|
| 157 | + params: |
| 158 | + m_coeffs: N-dimensional array of (6+1) x k coefficients |
| 159 | + for every control point |
| 160 | + numpy.array of size N by (6 + 1) by k |
| 161 | + N is the number of control points |
| 162 | + k is the number of dimensions for each point |
| 163 | + num_intervals: the number of intervals between |
| 164 | + control points |
| 165 | + int > 0 |
| 166 | + duration_array: array of user-supplied control duration of segment |
| 167 | + numpy.array of size N-1 |
| 168 | + N is the number of control points |
| 169 | +
|
| 170 | + returns: |
| 171 | + m_curve: positions along the minimum trajectory in k-dimensions |
| 172 | + numpy.array of size N*num_interval+1 by k |
| 173 | + (the +1 is to include the start position on the curve) |
| 174 | + """ |
| 175 | + assert num_intervals > 0,\ |
| 176 | + "Invalid number of intervals chosen (must be greater than 0)" |
| 177 | + interval = 1.0 / num_intervals |
| 178 | + (num_axes, num_mpts, _) = np.shape(m_coeffs) |
| 179 | + m_curve = np.zeros((num_mpts*num_intervals+1, num_axes)) |
| 180 | + # Copy out initial point |
| 181 | + m_curve[0, :] = m_coeffs[:, 0, 0] |
| 182 | + if duration_array == None: |
| 183 | + duration_array = np.array([1.0]*num_mpts) |
| 184 | + assert len(duration_array) == num_mpts,\ |
| 185 | + "Invalid number of intervals chosen (must be equal to N={})".format(num_mpts) |
| 186 | + for current_mpt in range(num_mpts): |
| 187 | + m_coeff_set = m_coeffs[:, current_mpt, range(7)] |
| 188 | + for iteration, t in enumerate(np.linspace(interval, 1, |
| 189 | + num_intervals)): |
| 190 | + m_curve[(current_mpt * |
| 191 | + num_intervals + |
| 192 | + iteration+1), :] = _minjerk_trajectory_point(m_coeff_set, t * duration_array[current_mpt]) |
| 193 | + return m_curve |
| 194 | + |
| 195 | +def _minjerk_trajectory_point(m_coeff, t): |
| 196 | + """ |
| 197 | + Internal convenience function for calculating |
| 198 | + a k-dimensional point defined by the supplied |
| 199 | + minimum jerk coefficients. Finds the point that |
| 200 | + describes the current position along the minimum |
| 201 | + trajectory segment for k dimensions. |
| 202 | +
|
| 203 | + params: |
| 204 | + m_coeff => m0...m3: Four k-dimensional minimum jerk |
| 205 | + coefficients each one is a numpy.array |
| 206 | + of size k by 1, so |
| 207 | + m_coeff is a numpy array of size k by (6+1) |
| 208 | + k is the number of dimensions for each |
| 209 | + coefficient |
| 210 | + t: percentage of time elapsed for this segment |
| 211 | + 0 <= int <= 1.0 |
| 212 | +
|
| 213 | + returns: |
| 214 | + current position in k dimensions |
| 215 | + numpy.array of size 1 by k |
| 216 | + """ |
| 217 | + a0 = m_coeff[:,0] |
| 218 | + a1 = m_coeff[:,1] |
| 219 | + a2 = m_coeff[:,2] |
| 220 | + a3 = m_coeff[:,3] |
| 221 | + a4 = m_coeff[:,4] |
| 222 | + a5 = m_coeff[:,5] |
| 223 | + tm = m_coeff[:,6] |
| 224 | + |
| 225 | + t = t * tm # input t is percentage of time elapsed for this segment, tm is the duration of this segment and to calculate x, v, a , t is the time[s] elapsed for this segment |
| 226 | + |
| 227 | + # calculate x, v, z at the time percentage t |
| 228 | + # x=a0+a1*t+a2*t*t+a3*t*t*t+a4*t*t*t*t+a5*t*t*t*t*t; |
| 229 | + x=a0+a1*t+a2*np.power(t,2)+a3*np.power(t,3)+a4*np.power(t,4)+a5*np.power(t,5); |
| 230 | + # v=a1+2*a2*t+3*a3*t*t+4*a4*t*t*t+5*a5*t*t*t*t; |
| 231 | + v=a1+2*a2*t+3*a3*np.power(t,2)+4*a4*np.power(t,3)+5*a5*np.power(t,4); |
| 232 | + # a=2*a2+6*a3*t+12*a4*t*t+20*a5*t*t*t; |
| 233 | + a=2*a2+6*a3*t+12*a4*np.power(t,2)+20*a5*np.power(t,3); |
| 234 | + |
| 235 | + return x |
| 236 | + |
| 237 | +def minjerk_point(m_coeffs, m_index, t): |
| 238 | + """ |
| 239 | + Finds the k values that describe the current |
| 240 | + position along the minjerk trajectory for k dimensions. |
| 241 | +
|
| 242 | + params: |
| 243 | + m_coeffs: k-dimensional array |
| 244 | + for every control point with 6 Minimum Jerk coefficients and a segument duration |
| 245 | + numpy.array of size k by N by 7 |
| 246 | + N is the number of control points |
| 247 | + k is the number of dimensions for each point |
| 248 | + m_index: index position out between two of |
| 249 | + the N b_coeffs for this point in time |
| 250 | + int |
| 251 | + t: percentage of time that has passed between |
| 252 | + the two control points |
| 253 | + 0 <= int <= 1.0 |
| 254 | +
|
| 255 | + returns: |
| 256 | + m_point: current position in k dimensions |
| 257 | + numpy.array of size 1 by k |
| 258 | + """ |
| 259 | + if m_index <= 0: |
| 260 | + m_point = m_coeffs[:, 0, 0] |
| 261 | + elif m_index > m_coeffs.shape[1]: |
| 262 | + t = 1 |
| 263 | + m_coeff_set = m_coeffs[:,m_coeffs.shape[1]-1, range(7)] |
| 264 | + m_point = _minjerk_trajectory_point(m_coeff_set, t) |
| 265 | + else: |
| 266 | + t = 0.0 if t < 0.0 else t |
| 267 | + t = 1.0 if t > 1.0 else t |
| 268 | + m_coeff_set = m_coeffs[:,m_index-1, range(7)] |
| 269 | + m_point = _minjerk_trajectory_point(m_coeff_set, t) |
| 270 | + return m_point |
0 commit comments