def compute_x_coeff(inputs,t0,tf): x0 = float(inputs[0]) xf = float(inputs[1]) xdot0 = float(inputs[2]) xdotf = float(inputs[3]) xddot0 = float(inputs[4]) xc1 = float(inputs[5]) t0 = float(t0) tf = float(tf) tc1 = t0+0.5*(tf-t0) t=tc1; [Botc1,B1tc1,B2tc1,B3tc1,B4tc1,B5tc1] = get_bernstein_coeff(t0,t,tf) t=t0 [Bodoto,B1doto,B2doto,B3doto,B4doto,B5doto,Boddoto,B1ddoto,B2ddoto,B3ddoto,B4ddoto,B5ddoto]=get_bernstein_differentials(t0,t,tf) t=tf [Bodotf,B1dotf,B2dotf,B3dotf,B4dotf,B5dotf,Boddotf,B1ddotf,B2ddotf,B3ddotf,B4ddotf,B5ddotf]=get_bernstein_differentials(t0,t,tf) A1 = [[B1tc1,B2tc1,B3tc1,B4tc1],[B1doto,B2doto,B3doto,B4doto],[B1ddoto,B2ddoto,B3ddoto,B4ddoto],[B1dotf,B2dotf,B3dotf,B4dotf]] C1 = xc1-x0*Botc1-xf*B5tc1 C2 = xdot0-Bodoto*x0-B5doto*xf C3 = xddot0-Boddoto*x0-B5ddoto*xf C4 = xdotf-Bodotf*x0-B5dotf*xf C = [[C1],[C2],[C3],[C4]] XX = dot(linalg.matrix_power(A1,-1),C) x1 = XX[0] x2 = XX[1] x3 = XX[2] x4 = XX[3] return [array([x0]),x1,x2,x3,x4,array([xf])];
def get_robot_pose(to,t,tf,xo,x1,x2,x3,x4,x5,ko,k1,k2,k3,k4,k5,yo): [ B0,B1,B2,B3,B4,B5] = get_bernstein_coeff(to,t,tf) [Bodot,B1dot,B2dot,B3dot,B4dot,B5dot,Boddot,B1ddot,B2ddot,B3ddot,B4ddot,B5ddot]=get_bernstein_differentials(to,t,tf) [coefot, coef1t, coef2t, coef3t, coef4t,coef5t ]=get_integral_coeff(xo,x1,x2,x3,x4,x5,to,t,tf) xrobo = B0*xo+B1*x1+B2*x2+B3*x3+B4*x4+B5*x5 xrobodot=Bodot*xo+B1dot*x1+B2dot*x2+B3dot*x3+B4dot*x4+B5dot*x5 xroboddot=Boddot*xo+B1ddot*x1+B2ddot*x2+B3ddot*x3+B4ddot*x4+B5ddot*x5 krobo=B0*ko+B1*k1+B2*k2+B3*k3+B4*k4+B5*k5 krobodot=Bodot*ko+B1dot*k1+B2dot*k2+B3dot*k3+B4dot*k4+B5dot*k5 kroboddot=Boddot*ko+B1ddot*k1+B2ddot*k2+B3ddot*k3+B4ddot*k4+B5ddot*k5 yrobo = yo+ko*coefot+k1*coef1t+k2*coef2t+k3*coef3t+k4*coef4t+k5*coef5t yrobodot=xrobodot* krobo omega_robo= krobodot/(1+krobo**2) velocity_robo=math.sqrt(xrobodot**2+yrobodot**2) output_robo=array([xrobo,yrobo,krobo,xrobodot,yrobodot,krobodot,xroboddot,kroboddot]) return output_robo end
def compute_x_coeff(inputs, t0, tf):
x0 = float(inputs[0])
xf = float(inputs[1])
xdot0 = float(inputs[2])
xdotf = float(inputs[3])
xddot0 = float(inputs[4])
xc1 = float(inputs[5])
t0 = float(t0)
tf = float(tf)
tc1 = t0 + 0.5 * (tf - t0)
t = tc1
[Botc1, B1tc1, B2tc1, B3tc1, B4tc1, B5tc1] = get_bernstein_coeff(t0, t, tf)
t = t0
[
Bodoto, B1doto, B2doto, B3doto, B4doto, B5doto, Boddoto, B1ddoto,
B2ddoto, B3ddoto, B4ddoto, B5ddoto
] = get_bernstein_differentials(t0, t, tf)
t = tf
[
Bodotf, B1dotf, B2dotf, B3dotf, B4dotf, B5dotf, Boddotf, B1ddotf,
B2ddotf, B3ddotf, B4ddotf, B5ddotf
] = get_bernstein_differentials(t0, t, tf)
A1 = [[B1tc1, B2tc1, B3tc1, B4tc1], [B1doto, B2doto, B3doto, B4doto],
[B1ddoto, B2ddoto, B3ddoto, B4ddoto],
[B1dotf, B2dotf, B3dotf, B4dotf]]
C1 = xc1 - x0 * Botc1 - xf * B5tc1
C2 = xdot0 - Bodoto * x0 - B5doto * xf
C3 = xddot0 - Boddoto * x0 - B5ddoto * xf
C4 = xdotf - Bodotf * x0 - B5dotf * xf
C = [[C1], [C2], [C3], [C4]]
XX = dot(linalg.matrix_power(A1, -1), C)
x1 = XX[0]
x2 = XX[1]
x3 = XX[2]
x4 = XX[3]
return [array([x0]), x1, x2, x3, x4,
array([xf])]
def get_robot_pose(to, t, tf, xo, x1, x2, x3, x4, x5, ko, k1, k2, k3, k4, k5,
yo):
[B0, B1, B2, B3, B4, B5] = get_bernstein_coeff(to, t, tf)
[
Bodot, B1dot, B2dot, B3dot, B4dot, B5dot, Boddot, B1ddot, B2ddot,
B3ddot, B4ddot, B5ddot
] = get_bernstein_differentials(to, t, tf)
[coefot, coef1t, coef2t, coef3t, coef4t,
coef5t] = get_integral_coeff(xo, x1, x2, x3, x4, x5, to, t, tf)
xrobo = B0 * xo + B1 * x1 + B2 * x2 + B3 * x3 + B4 * x4 + B5 * x5
xrobodot = Bodot * xo + B1dot * x1 + B2dot * x2 + B3dot * x3 + B4dot * x4 + B5dot * x5
xroboddot = Boddot * xo + B1ddot * x1 + B2ddot * x2 + B3ddot * x3 + B4ddot * x4 + B5ddot * x5
krobo = B0 * ko + B1 * k1 + B2 * k2 + B3 * k3 + B4 * k4 + B5 * k5
krobodot = Bodot * ko + B1dot * k1 + B2dot * k2 + B3dot * k3 + B4dot * k4 + B5dot * k5
kroboddot = Boddot * ko + B1ddot * k1 + B2ddot * k2 + B3ddot * k3 + B4ddot * k4 + B5ddot * k5
yrobo = yo + ko * coefot + k1 * coef1t + k2 * coef2t + k3 * coef3t + k4 * coef4t + k5 * coef5t
yrobodot = xrobodot * krobo
omega_robo = krobodot / (1 + krobo**2)
velocity_robo = math.sqrt(xrobodot**2 + yrobodot**2)
output_robo = array([
xrobo, yrobo, krobo, xrobodot, yrobodot, krobodot, xroboddot, kroboddot
])
return output_robo
end
def move_robot(self):
Ax = array([self.x0,self.xf,self.vx0,self.vxf,self.ax0,self.xc])
xcoeff = compute_x_coeff(Ax,self.t0,self.tf)
x0 = xcoeff[0]
x1 = xcoeff[1]
x2 = xcoeff[2]
x3 = xcoeff[3]
x4 = xcoeff[4]
x5 = xcoeff[5]
#get the coefficients to calculate y at t=tf;
t = self.tf
[coef0tf,coef1tf,coef2tf,coef3tf,coef4tf,coef5tf]=get_integral_coeff(x0,x1,x2,x3,x4,x5,self.t0,t,self.tf);
# get the coefficients to calculate y at t=tc;
t = self.tc
[coef0tc,coef1tc,coef2tc,coef3tc,coef4tc,coef5tc]=get_integral_coeff(x0,x1,x2,x3,x4,x5,self.t0,t,self.tf);
# get differentials of bernstein coeffs at t=t0:
t=self.t0
[B0doto,B1doto,B2doto,B3doto,B4doto,B5doto,Boddoto,B1ddoto,B2ddoto,B3ddoto,B4ddoto,B5ddoto]=get_bernstein_differentials(self.t0,t,self.tf);
#get differentials of bernstein coeffs at t=tf:
t=self.tf
[B0dotf,B1dotf,B2dotf,B3dotf,B4dotf,B5dotf,Boddotf,B1ddotf,B2ddotf,B3ddotf,B4ddotf,B5ddotf]=get_bernstein_differentials(self.t0,t,self.tf);
A1 = array([[B1doto,B2doto,B3doto,B4doto],[B1dotf,B2dotf,B3dotf,B4dotf],[coef1tf,coef2tf,coef3tf,coef4tf],[coef1tc,coef2tc,coef3tc,coef4tc]])
C1 = self.kdot0-B0doto*self.k0-B5doto*self.kf
C2 = self.kdotf-B0dotf*self.k0-B5dotf*self.kf
C3 = self.yf-self.k0*coef0tf-self.kf*coef5tf-self.y0
C4 = self.yc-self.k0*coef0tc-self.kf*coef5tc
C = array([[C1],[C2],[C3],[C4]])
thcoeff = dot(linalg.matrix_power(A1,-1),C)
num_points = int((self.tf - self.t0)/self.planner_rate+1)
k0 = self.k0
k1 = thcoeff[0]
k2 = thcoeff[1]
k3 = thcoeff[2]
k4 = thcoeff[3]
k5 = self.kf
xt = [None] * num_points
vxt = [None] * num_points
kt = [None] * num_points
kdott = [None] * num_points
yt = [None] * num_points
vyt = [None] * num_points
wt = [None] * num_points
velt = [None] * num_points
t = self.t0
i = 0
r = rospy.Rate(10);
while (t <= self.tf):
[ B0,B1,B2,B3,B4,B5] = get_bernstein_coeff(self.t0,t,self.tf);
[Bodot,B1dot,B2dot,B3dot,B4dot,B5dot,Boddot,B1ddot,B2ddot,B3ddot,B4ddot,B5ddot]=get_bernstein_differentials(self.t0,t,self.tf);
[coefot, coef1t, coef2t, coef3t, coef4t,coef5t ]=get_integral_coeff(x0,x1,x2,x3,x4,x5,self.t0,t,self.tf);
xt[i] = B0*x0+B1*x1+B2*x2+B3*x3+B4*x4+B5*x5
vxt[i] = Bodot*x0+B1dot*x1+B2dot*x2+B3dot*x3+B4dot*x4+B5dot*x5
kt[i] = B0*k0+B1*k1+B2*k2+B3*k3+B4*k4+B5*k5
kdott[i]= Bodot*k0+B1dot*k1+B2dot*k2+B3dot*k3+B4dot*k4+B5dot*k5
yt[i] = self.y0+k0*coefot+k1*coef1t+k2*coef2t+k3*coef3t+k4*coef4t+k5*coef5t
vyt[i]=vxt[i]*kt[i]
wt[i] = kdott[i]/(1+kt[i]**2);
velt[i]=sqrt(vxt[i]**2+vyt[i]**2);
v_command = Twist()
v_command.linear.x = vxt[i]
v_command.linear.y = vyt[i]
v_command.linear.z = 0.0
v_command.angular.x = 0.0
v_command.angular.y = 0.0
v_command.angular.z = wt[i]
self.vel_publisher.publish(v_command)
t = t+0.1
i = i+1
r.sleep()
fig1 = plt.figure()
plt.plot(xt,yt,'r-')
fig2 = plt.figure()
plt.plot(linspace(self.t0,self.tf,num_points),velt,'g-')
fig3 = plt.figure()
plt.plot(linspace(self.t0,self.tf,num_points),wt,'b-')
plt.show()
def move_robot(self):
Ax = array([self.x0, self.xf, self.vx0, self.vxf, self.ax0, self.xc])
xcoeff = compute_x_coeff(Ax, self.t0, self.tf)
x0 = xcoeff[0]
x1 = xcoeff[1]
x2 = xcoeff[2]
x3 = xcoeff[3]
x4 = xcoeff[4]
x5 = xcoeff[5]
#get the coefficients to calculate y at t=tf;
t = self.tf
[coef0tf, coef1tf, coef2tf, coef3tf, coef4tf,
coef5tf] = get_integral_coeff(x0, x1, x2, x3, x4, x5, self.t0, t,
self.tf)
# get the coefficients to calculate y at t=tc;
t = self.tc
[coef0tc, coef1tc, coef2tc, coef3tc, coef4tc,
coef5tc] = get_integral_coeff(x0, x1, x2, x3, x4, x5, self.t0, t,
self.tf)
# get differentials of bernstein coeffs at t=t0:
t = self.t0
[
B0doto, B1doto, B2doto, B3doto, B4doto, B5doto, Boddoto, B1ddoto,
B2ddoto, B3ddoto, B4ddoto, B5ddoto
] = get_bernstein_differentials(self.t0, t, self.tf)
#get differentials of bernstein coeffs at t=tf:
t = self.tf
[
B0dotf, B1dotf, B2dotf, B3dotf, B4dotf, B5dotf, Boddotf, B1ddotf,
B2ddotf, B3ddotf, B4ddotf, B5ddotf
] = get_bernstein_differentials(self.t0, t, self.tf)
A1 = array([[B1doto, B2doto, B3doto, B4doto],
[B1dotf, B2dotf, B3dotf, B4dotf],
[coef1tf, coef2tf, coef3tf, coef4tf],
[coef1tc, coef2tc, coef3tc, coef4tc]])
C1 = self.kdot0 - B0doto * self.k0 - B5doto * self.kf
C2 = self.kdotf - B0dotf * self.k0 - B5dotf * self.kf
C3 = self.yf - self.k0 * coef0tf - self.kf * coef5tf - self.y0
C4 = self.yc - self.k0 * coef0tc - self.kf * coef5tc
C = array([[C1], [C2], [C3], [C4]])
thcoeff = dot(linalg.matrix_power(A1, -1), C)
num_points = int((self.tf - self.t0) / self.planner_rate + 1)
k0 = self.k0
k1 = thcoeff[0]
k2 = thcoeff[1]
k3 = thcoeff[2]
k4 = thcoeff[3]
k5 = self.kf
xt = [None] * num_points
vxt = [None] * num_points
kt = [None] * num_points
kdott = [None] * num_points
yt = [None] * num_points
vyt = [None] * num_points
wt = [None] * num_points
velt = [None] * num_points
t = self.t0
i = 0
r = rospy.Rate(10)
while (t <= self.tf):
[B0, B1, B2, B3, B4,
B5] = get_bernstein_coeff(self.t0, t, self.tf)
[
Bodot, B1dot, B2dot, B3dot, B4dot, B5dot, Boddot, B1ddot,
B2ddot, B3ddot, B4ddot, B5ddot
] = get_bernstein_differentials(self.t0, t, self.tf)
[coefot, coef1t, coef2t, coef3t, coef4t,
coef5t] = get_integral_coeff(x0, x1, x2, x3, x4, x5, self.t0, t,
self.tf)
xt[i] = B0 * x0 + B1 * x1 + B2 * x2 + B3 * x3 + B4 * x4 + B5 * x5
vxt[i] = Bodot * x0 + B1dot * x1 + B2dot * x2 + B3dot * x3 + B4dot * x4 + B5dot * x5
kt[i] = B0 * k0 + B1 * k1 + B2 * k2 + B3 * k3 + B4 * k4 + B5 * k5
kdott[
i] = Bodot * k0 + B1dot * k1 + B2dot * k2 + B3dot * k3 + B4dot * k4 + B5dot * k5
yt[i] = self.y0 + k0 * coefot + k1 * coef1t + k2 * coef2t + k3 * coef3t + k4 * coef4t + k5 * coef5t
vyt[i] = vxt[i] * kt[i]
wt[i] = kdott[i] / (1 + kt[i]**2)
velt[i] = sqrt(vxt[i]**2 + vyt[i]**2)
v_command = Twist()
v_command.linear.x = vxt[i]
v_command.linear.y = vyt[i]
v_command.linear.z = 0.0
v_command.angular.x = 0.0
v_command.angular.y = 0.0
v_command.angular.z = wt[i]
self.vel_publisher.publish(v_command)
t = t + 0.1
i = i + 1
r.sleep()
fig1 = plt.figure()
plt.plot(xt, yt, 'r-')
fig2 = plt.figure()
plt.plot(linspace(self.t0, self.tf, num_points), velt, 'g-')
fig3 = plt.figure()
plt.plot(linspace(self.t0, self.tf, num_points), wt, 'b-')
plt.show()
