コード例 #1
0
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])]
コード例 #2
0
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
コード例 #3
0
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
コード例 #4
0
def  get_way_points(inp):
	xo=float(inp[0]); 
	yo=float(inp[1]); 
	xf=float(inp[2]);  
	yf=float(inp[3]);  
	xdoto=float(inp[4]);  
	xddoto=float(inp[5]); 
	xdotf=float(inp[6]);  
	ko=float(inp[7]);   
	kdoto=float(inp[8]); 
	kddoto=float(inp[9]);
	kf=float(inp[10]);     
	kdotf=float(inp[11]);  
	to=float(inp[12]);      
	tf=float(inp[13]);  
	xc1=float(inp[14]) ; 
	yc1=float(inp[15]); 
	tc=float(inp[16]); 
	Ax = array([xo,xf,xdoto,xdotf,xddoto,xc1]);

	h1 = compute_x_coeff(Ax,to,tf);
	xo = h1[0];
	x1 = h1[1];
	x2 = h1[2];
	x3 = h1[3];
	x4 = h1[4];
	x5 = h1[5];


	t = tf;
	[coefotf,coef1tf,coef2tf,coef3tf,coef4tf,coef5tf]=get_integral_coeff(xo,x1,x2,x3,x4,x5,to,t,tf);
	t = tc;
	[coefotc1,coef1tc1,coef2tc1,coef3tc1,coef4tc1,coef5tc1 ]=get_integral_coeff(xo,x1,x2,x3,x4,x5,to,t,tf);

	t=to;
	[Bodoto,B1doto,B2doto,B3doto,B4doto,B5doto,Boddoto,B1ddoto,B2ddoto,B3ddoto,B4ddoto,B5ddoto]=get_bernstein_differentials(to,t,tf);
	t=tf;
	[Bodotf,B1dotf,B2dotf,B3dotf,B4dotf,B5dotf,Boddotf,B1ddotf,B2ddotf,B3ddotf,B4ddotf,B5ddotf]=get_bernstein_differentials(to,t,tf);


	A1 = array([[B1doto,B2doto,B3doto,B4doto],[B1dotf,B2dotf,B3dotf,B4dotf],[coef1tf,coef2tf,coef3tf,coef4tf],[coef1tc1,coef2tc1,coef3tc1,coef4tc1]]) 

	C1 = kdoto-Bodoto*ko-B5doto*kf;
	C2 = kdotf-Bodotf*ko-B5dotf*kf; 
	C3 = yf-ko*coefotf-kf*coef5tf-yo; 
	C4 = yc1-ko*coefotc1-kf*coef5tc1;

	
	C5 =  kddoto-Boddoto*ko-B5ddoto*kf;
	C = array([[C1],[C2],[C3],[C4]]);
	XX = dot(linalg.matrix_power(A1,-1),C)
	k1 = XX[0]
	k2 = XX[1];
	k3 = XX[2];
	k4 = XX[3];
#	f1 = [xo;x1;x2;x3;x4;xf];
#	f2 = [ko;k1;k2;k3;k4;kf];

#	f = [f1;f2];

	return  [xo,x1,x2,x3,x4,x5,array(ko),k1,k2,k3,k4,array(kf)]
コード例 #5
0
	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()
コード例 #6
0
    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()
コード例 #7
0
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])];
コード例 #8
0
def get_way_points(inp):
    xo = float(inp[0])
    yo = float(inp[1])
    xf = float(inp[2])
    yf = float(inp[3])
    xdoto = float(inp[4])
    xddoto = float(inp[5])
    xdotf = float(inp[6])
    ko = float(inp[7])
    kdoto = float(inp[8])
    kddoto = float(inp[9])
    kf = float(inp[10])
    kdotf = float(inp[11])
    to = float(inp[12])
    tf = float(inp[13])
    xc1 = float(inp[14])
    yc1 = float(inp[15])
    tc = float(inp[16])
    Ax = array([xo, xf, xdoto, xdotf, xddoto, xc1])

    h1 = compute_x_coeff(Ax, to, tf)
    xo = h1[0]
    x1 = h1[1]
    x2 = h1[2]
    x3 = h1[3]
    x4 = h1[4]
    x5 = h1[5]

    t = tf
    [coefotf, coef1tf, coef2tf, coef3tf, coef4tf,
     coef5tf] = get_integral_coeff(xo, x1, x2, x3, x4, x5, to, t, tf)
    t = tc
    [coefotc1, coef1tc1, coef2tc1, coef3tc1, coef4tc1,
     coef5tc1] = get_integral_coeff(xo, x1, x2, x3, x4, x5, to, t, tf)

    t = to
    [
        Bodoto, B1doto, B2doto, B3doto, B4doto, B5doto, Boddoto, B1ddoto,
        B2ddoto, B3ddoto, B4ddoto, B5ddoto
    ] = get_bernstein_differentials(to, t, tf)
    t = tf
    [
        Bodotf, B1dotf, B2dotf, B3dotf, B4dotf, B5dotf, Boddotf, B1ddotf,
        B2ddotf, B3ddotf, B4ddotf, B5ddotf
    ] = get_bernstein_differentials(to, t, tf)

    A1 = array([[B1doto, B2doto, B3doto, B4doto],
                [B1dotf, B2dotf, B3dotf, B4dotf],
                [coef1tf, coef2tf, coef3tf, coef4tf],
                [coef1tc1, coef2tc1, coef3tc1, coef4tc1]])

    C1 = kdoto - Bodoto * ko - B5doto * kf
    C2 = kdotf - Bodotf * ko - B5dotf * kf
    C3 = yf - ko * coefotf - kf * coef5tf - yo
    C4 = yc1 - ko * coefotc1 - kf * coef5tc1

    C5 = kddoto - Boddoto * ko - B5ddoto * kf
    C = array([[C1], [C2], [C3], [C4]])
    XX = dot(linalg.matrix_power(A1, -1), C)
    k1 = XX[0]
    k2 = XX[1]
    k3 = XX[2]
    k4 = XX[3]
    #	f1 = [xo;x1;x2;x3;x4;xf];
    #	f2 = [ko;k1;k2;k3;k4;kf];

    #	f = [f1;f2];

    return [xo, x1, x2, x3, x4, x5, array(ko), k1, k2, k3, k4, array(kf)]