Exemplo n.º 1
0
def pert_traj(t_start_pert, tf, xrobo, yrobo, x5, y5, xrobodot, xroboddot, yrobodot, krobo, krobodot, kroboddot, npath):
    xo = xrobo
    yo = yrobo
    xdoto = xrobodot
    xddoto = xroboddot
    ydoto = yrobodot
    kf = 0
    ko = krobo
    kdoto = krobodot
    kddoto = kroboddot
    xdotf = 0
    kdotf = 0
    to = t_start_pert
    x_pert1 = (xo + x5) / 2.0
    if yo > y5:
        y_pert1 = (y5 - yo) / 2.0
    else:
        y_pert1 = (y5 - yo) / 2.0
    t_pert1 = to + 0.5 * (tf - to)
    t_pert2 = to + 0.5 * (tf - to)
    min_pert = -1.0
    max_pert = 1.0
    npath = 21
    x_pert2 = numpy.zeros(npath)
    y_pert2 = numpy.zeros(npath)
    xrc0 = numpy.zeros(npath)
    xrc1 = numpy.zeros(npath)
    xrc2 = numpy.zeros(npath)
    xrc3 = numpy.zeros(npath)
    xrc4 = numpy.zeros(npath)
    xrc5 = numpy.zeros(npath)
    krc0 = numpy.zeros(npath)
    krc1 = numpy.zeros(npath)
    krc2 = numpy.zeros(npath)
    krc3 = numpy.zeros(npath)
    krc4 = numpy.zeros(npath)
    krc5 = numpy.zeros(npath)
    # 	pert_listx = linspace(min_pert,max_pert,npath)
    # 	pert_listy = linspace(-4.0,4.0,npath)
    # 	pert_list = linspace(-1.0,1.0,npath)
    pert_listx = [0] * npath
    pert_listy = linspace(-1.0, 1.0, npath)
    j = 0
    # 	for i in range(0,npath):
    # 		x_pert2[j] = x_pert1+pert_listx[i]
    # 		y_pert2[j] = y_pert1+pert_listy[i]
    # 		j = j+1
    theta = math.atan2(y5 - yo, x5 - xo)
    for i in range(0, npath):
        # 		x_pert2[j] = x_pert1-pert_list[i]*math.sin(theta)
        # 		y_pert2[j] = y_pert1+pert_list[i]*math.cos(theta)
        x_pert2[j] = x_pert1 + x_pert2[j]
        y_pert2[j] = y_pert1 + pert_listy[i]
        # 		if yo < y5:
        # 			y_pert2[j] = abs(y_pert2[j])

        j = j + 1
    for i1 in range(0, npath):
        input_for_perturbed_trajectories = [
            xo,
            yo,
            x5,
            y5,
            xdoto,
            xddoto,
            xdotf,
            ko,
            kdoto,
            kddoto,
            kf,
            kdotf,
            to,
            tf,
            x_pert2[i1],
            y_pert2[i1],
            t_pert2,
        ]
        [
            xrc0[i1],
            xrc1[i1],
            xrc2[i1],
            xrc3[i1],
            xrc4[i1],
            xrc5[i1],
            krc0[i1],
            krc1[i1],
            krc2[i1],
            krc3[i1],
            krc4[i1],
            krc5[i1],
        ] = get_way_points(input_for_perturbed_trajectories)
    bernst_param = array([xrc0, xrc1, xrc2, xrc3, xrc4, xrc5, krc0, krc1, krc2, krc3, krc4, krc5])
    return bernst_param
	def move_robot(self):
		inp=[self.x0,self.y0,self.xf,self.yf,self.vx0,self.ax0,self.vxf,self.k0,self.kdot0,self.kddot0,self.kf,self.kdotf,self.t0,self.tf,self.xc,self.yc,self.tc];
		[xo,x1,x2,x3,x4,xf,ko,k1,k2,k3,k4,kf ] = get_way_points(inp);
		print  [xo,x1,x2,x3,x4,xf,ko,k1,k2,k3,k4,kf ]
		t=self.t0;
		i=0;
		i_pert=0; 
		npath=21; 
		perturbation_at_time=4.1;
		delt = self.planner_rate
		robot_end = 0;
		t_start_pert = 0;
		time = zeros((self.tf-self.t0)/delt + 1);
		xrobo = zeros((self.tf-self.t0)/delt + 1);
		yrobo = zeros((self.tf-self.t0)/delt + 1);
		wrobo = zeros((self.tf-self.t0)/delt + 1);
		krobo = zeros((self.tf-self.t0)/delt + 1);
		xrobodot =zeros((self.tf-self.t0)/delt + 1);
		yrobodot = zeros((self.tf-self.t0)/delt + 1);
		krobodot = zeros((self.tf-self.t0)/delt + 1);
		xroboddot = zeros((self.tf-self.t0)/delt + 1);
		kroboddot = zeros((self.tf-self.t0)/delt + 1);
		wrobo1 = zeros(((self.tf-perturbation_at_time)/delt + 1,npath));
		vrobo1 = zeros(((self.tf-perturbation_at_time)/delt + 1,npath));
		xrobo1 = zeros(((self.tf-perturbation_at_time)/delt + 1,npath));
		yrobo1 = zeros(((self.tf-perturbation_at_time)/delt + 1,npath));
		xrobodot1 = zeros(((self.tf- perturbation_at_time)/delt + 1,npath));
		xroboddot1 = zeros(((self.tf-perturbation_at_time)/delt + 1,npath));
		yrobodot1 = zeros(((self.tf-perturbation_at_time)/delt + 1,npath));
		krobo1 = zeros(((self.tf-perturbation_at_time)/delt + 1,npath));
		krobodot1 = zeros(((self.tf-perturbation_at_time)/delt + 1,npath));
		kroboddot1 = zeros(((self.tf-perturbation_at_time)/delt + 1,npath));
		time_perturb_duration = zeros((self.tf-perturbation_at_time)/delt + 1);
		l = []
		print "NOW STARTED"
		while(t <= self.tf):
			time[i] = t
			output_robo = get_robot_pose( self.t0,t,self.tf,xo,x1,x2,x3,x4,xf,ko,k1,k2,k3,k4,kf,self.y0);
			xrobo[i]=output_robo[0];yrobo[i]=output_robo[1];krobo[i]=output_robo[2];xrobodot[i]=output_robo[3];yrobodot[i]=output_robo[4];krobodot[i]=output_robo[5];xroboddot[i]=output_robo[6];kroboddot[i]=output_robo[7];
		        wrobo[i] = krobodot[i]/(1.0+krobo[i]**2)
			
			if(abs(t-perturbation_at_time)<0.00000111):
				t_start_pert=t; 
				robo_end=i; 
				bernp=pert_traj(t_start_pert,self.tf,xrobo[robo_end],yrobo[robo_end],self.xf,self.yf,xrobodot[robo_end],xroboddot[robo_end],yrobodot[robo_end],krobo[robo_end],krobodot[robo_end],kroboddot[robo_end],npath);
			if(t > perturbation_at_time ):
				time_perturb_duration[i_pert] = t			
			        output_robo_pert = numpy.zeros((npath,8))
			        for j in range(0,npath):
#	print "here"
					output_robo_pert[j] = get_robot_pose(t_start_pert,t,self.tf,bernp[0,j],bernp[1,j],bernp[2,j],bernp[3,j],bernp[4,j],bernp[5,j],bernp[6,j],bernp[7,j],bernp[8,j],bernp[9,j],bernp[10,j],bernp[11,j],yrobo[robo_end]);
#					print "done"
				xrobo1[i_pert]=output_robo_pert[:,0];yrobo1[i_pert]=output_robo_pert[:,1];krobo1[i_pert]=output_robo_pert[:,2];xrobodot1[i_pert]=output_robo_pert[:,3];yrobodot1[i_pert]=output_robo_pert[:,4];krobodot1[i_pert]=output_robo_pert[:,5];xroboddot1[i_pert]=output_robo_pert[:,6];kroboddot1[i_pert]=output_robo_pert[:,7];
#				xrobo1[i_pert] = l[1]
#				yrobo1[i_pert] = l[2]
#				xrobodot1[i_pert] = l[3]
#				xroboddot1[i_pert] = l[4]
#				yrobodot1[i_pert] = l[5]
#				krobo1[i_pert] = l[6]
#				krobodot1[i_pert] = l[7]
				wrobo1[i_pert]= [krobodot1[i_pert,j]/(1.0+krobo1[i_pert,j]**2) for j in range(0,npath)]
				vrobo1[i_pert]=[math.sqrt(xrobodot1[i_pert,j]**2+yrobodot1[i_pert,j]**2) for j in range(0,npath)]
				i_pert=i_pert+1;

			i = i+1
			t = t+delt
		r =rospy.Rate(self.rate)
#		print self.yc
	        temp_c = 0;
	        path_toBe_executed = 10;
		while not rospy.is_shutdown():

			for j in range(0,21):
				if j != 10:
					continue;
#			if j != path_toBe_executed:
#	continue;
#	l = calc_scale_single(xrobo1[perturbation_at_time,j],yrobo1[perturbation_at_time,j],xrobodot1[perturbation_at_time,j],yrobodot1[perturbation_at_time,j],3,3,-1,0,2.0);
				if temp_c == 0:
					l = calc_scale_single(xrobo1[2,j],yrobo1[2,j],xrobodot1[2,j],yrobodot1[2,j],1,8,0,1,2.0);
					print l
				path = Path()
				path.header.frame_id ='map'
                        	path.poses = [PoseStamped]*int((self.tf-self.t0)/delt+1)
	                        s=0
				for k in range(0,int((perturbation_at_time-self.t0)/delt)+1):
					temp = PoseStamped()
#                       temp.position.x = xrobo1[k,j];
#                                       temp.position.y = yrobo1[k,j];
					temp.header.frame_id ='map'
					temp.pose.position.x = xrobo[k];
					temp.pose.position.y = yrobo[k];
					temp.pose.position.z = 0.1
					path.poses[k] = temp
					s=s+1

				for k in range(0,i_pert):
#	print 'hi'
					temp = PoseStamped()
#			temp.position.x = xrobo1[k,j];
#					temp.position.y = yrobo1[k,j];
					temp.header.frame_id ='map'
					temp.pose.position.x = xrobo1[k,j];
					temp.pose.position.y = yrobo1[k,j];
					temp.pose.position.z = 0.1
					path.poses[k+s] = temp
			
				self.paths_publisher[j].publish(path)
			temp_c = temp_c +1
			break;
			r.sleep()
		r2 = rospy.Rate(1.0/delt)
	        print rospy.Time.now().secs
		for i in  range (0,int((perturbation_at_time-self.t0)/delt)+1):
			vel_command = Twist()
			vel_command.linear.x = math.sqrt(xrobodot[i]**2+yrobodot[i]**2)
#	vel_command.linear.y = yrobodot[i]
			vel_command.linear.z = 0
			vel_command.angular.x = 0
			vel_command.angular.y = 0
			vel_command.angular.z = wrobo[i]
			self.vel_publisher.publish(vel_command)
			r2.sleep()
	        path_index = 5;
		path = Path()
		path.header.frame_id ='map'
		path.poses = [PoseStamped]*int((self.tf-self.t0)/delt+1)
		s=0
		for k in range(0,int((perturbation_at_time-self.t0)/delt)+1):
			temp = PoseStamped()
#                       temp.position.x = xrobo1[k,j];
#                                       temp.position.y = yrobo1[k,j];
			temp.header.frame_id ='map'
			temp.pose.position.x = xrobo[k];
			temp.pose.position.y = yrobo[k];
			temp.pose.position.z = 0.1
			path.poses[k] = temp
			s=s+1

		for k in range(0,i_pert):
#       print 'hi'
			temp = PoseStamped()
#                       temp.position.x = xrobo1[k,j];
#                                       temp.position.y = yrobo1[k,j];
			temp.header.frame_id ='map'
			temp.pose.position.x = xrobo1[k,path_index];
			temp.pose.position.y = yrobo1[k,path_index];
			temp.pose.position.z = 0.1
			path.poses[k+s] = temp
		self.paths_publisher[path_index].publish(path)
		for i in  range (0,i_pert):
			vel_command = Twist()
			vel_command.linear.x = math.sqrt(xrobodot1[i,path_index]**2+yrobodot1[i,path_index]**2)
#	vel_command.linear.y = yrobodot[i]
			vel_command.linear.z = 0
			vel_command.angular.x = 0
			vel_command.angular.y = 0
			vel_command.angular.z = wrobo1[i,path_index]
			self.vel_publisher.publish(vel_command)
			r2.sleep()


	        print rospy.Time.now().secs
    def move_robot(self):
        inp = [
            self.x0, self.y0, self.xf, self.yf, self.vx0, self.ax0, self.vxf,
            self.k0, self.kdot0, self.kddot0, self.kf, self.kdotf, self.t0,
            self.tf, self.xc, self.yc, self.tc
        ]
        [xo, x1, x2, x3, x4, xf, ko, k1, k2, k3, k4, kf] = get_way_points(inp)
        print[xo, x1, x2, x3, x4, xf, ko, k1, k2, k3, k4, kf]
        t = self.t0
        i = 0
        i_pert = 0
        npath = 21
        perturbation_at_time = 4.1
        delt = self.planner_rate
        robot_end = 0
        t_start_pert = 0
        time = zeros((self.tf - self.t0) / delt + 1)
        xrobo = zeros((self.tf - self.t0) / delt + 1)
        yrobo = zeros((self.tf - self.t0) / delt + 1)
        wrobo = zeros((self.tf - self.t0) / delt + 1)
        krobo = zeros((self.tf - self.t0) / delt + 1)
        xrobodot = zeros((self.tf - self.t0) / delt + 1)
        yrobodot = zeros((self.tf - self.t0) / delt + 1)
        krobodot = zeros((self.tf - self.t0) / delt + 1)
        xroboddot = zeros((self.tf - self.t0) / delt + 1)
        kroboddot = zeros((self.tf - self.t0) / delt + 1)
        wrobo1 = zeros(((self.tf - perturbation_at_time) / delt + 1, npath))
        vrobo1 = zeros(((self.tf - perturbation_at_time) / delt + 1, npath))
        xrobo1 = zeros(((self.tf - perturbation_at_time) / delt + 1, npath))
        yrobo1 = zeros(((self.tf - perturbation_at_time) / delt + 1, npath))
        xrobodot1 = zeros(((self.tf - perturbation_at_time) / delt + 1, npath))
        xroboddot1 = zeros(
            ((self.tf - perturbation_at_time) / delt + 1, npath))
        yrobodot1 = zeros(((self.tf - perturbation_at_time) / delt + 1, npath))
        krobo1 = zeros(((self.tf - perturbation_at_time) / delt + 1, npath))
        krobodot1 = zeros(((self.tf - perturbation_at_time) / delt + 1, npath))
        kroboddot1 = zeros(
            ((self.tf - perturbation_at_time) / delt + 1, npath))
        time_perturb_duration = zeros((self.tf - perturbation_at_time) / delt +
                                      1)
        l = []
        print "NOW STARTED"
        while (t <= self.tf):
            time[i] = t
            output_robo = get_robot_pose(self.t0, t, self.tf, xo, x1, x2, x3,
                                         x4, xf, ko, k1, k2, k3, k4, kf,
                                         self.y0)
            xrobo[i] = output_robo[0]
            yrobo[i] = output_robo[1]
            krobo[i] = output_robo[2]
            xrobodot[i] = output_robo[3]
            yrobodot[i] = output_robo[4]
            krobodot[i] = output_robo[5]
            xroboddot[i] = output_robo[6]
            kroboddot[i] = output_robo[7]
            wrobo[i] = krobodot[i] / (1.0 + krobo[i]**2)

            if (abs(t - perturbation_at_time) < 0.00000111):
                t_start_pert = t
                robo_end = i
                bernp = pert_traj(t_start_pert, self.tf, xrobo[robo_end],
                                  yrobo[robo_end], self.xf, self.yf,
                                  xrobodot[robo_end], xroboddot[robo_end],
                                  yrobodot[robo_end], krobo[robo_end],
                                  krobodot[robo_end], kroboddot[robo_end],
                                  npath)
            if (t > perturbation_at_time):
                time_perturb_duration[i_pert] = t
                output_robo_pert = numpy.zeros((npath, 8))
                for j in range(0, npath):
                    #	print "here"
                    output_robo_pert[j] = get_robot_pose(
                        t_start_pert, t, self.tf, bernp[0, j], bernp[1, j],
                        bernp[2, j], bernp[3, j], bernp[4, j], bernp[5, j],
                        bernp[6, j], bernp[7, j], bernp[8, j], bernp[9, j],
                        bernp[10, j], bernp[11, j], yrobo[robo_end])
#					print "done"
                xrobo1[i_pert] = output_robo_pert[:, 0]
                yrobo1[i_pert] = output_robo_pert[:, 1]
                krobo1[i_pert] = output_robo_pert[:, 2]
                xrobodot1[i_pert] = output_robo_pert[:, 3]
                yrobodot1[i_pert] = output_robo_pert[:, 4]
                krobodot1[i_pert] = output_robo_pert[:, 5]
                xroboddot1[i_pert] = output_robo_pert[:, 6]
                kroboddot1[i_pert] = output_robo_pert[:, 7]
                #				xrobo1[i_pert] = l[1]
                #				yrobo1[i_pert] = l[2]
                #				xrobodot1[i_pert] = l[3]
                #				xroboddot1[i_pert] = l[4]
                #				yrobodot1[i_pert] = l[5]
                #				krobo1[i_pert] = l[6]
                #				krobodot1[i_pert] = l[7]
                wrobo1[i_pert] = [
                    krobodot1[i_pert, j] / (1.0 + krobo1[i_pert, j]**2)
                    for j in range(0, npath)
                ]
                vrobo1[i_pert] = [
                    math.sqrt(xrobodot1[i_pert, j]**2 +
                              yrobodot1[i_pert, j]**2)
                    for j in range(0, npath)
                ]
                i_pert = i_pert + 1

            i = i + 1
            t = t + delt
        r = rospy.Rate(self.rate)
        #		print self.yc
        temp_c = 0
        path_toBe_executed = 10
        while not rospy.is_shutdown():

            for j in range(0, 21):
                if j != 10:
                    continue
#			if j != path_toBe_executed:
#	continue;
#	l = calc_scale_single(xrobo1[perturbation_at_time,j],yrobo1[perturbation_at_time,j],xrobodot1[perturbation_at_time,j],yrobodot1[perturbation_at_time,j],3,3,-1,0,2.0);
                if temp_c == 0:
                    l = calc_scale_single(xrobo1[2, j], yrobo1[2, j],
                                          xrobodot1[2, j], yrobodot1[2, j], 1,
                                          8, 0, 1, 2.0)
                    print l
                path = Path()
                path.header.frame_id = 'map'
                path.poses = [PoseStamped
                              ] * int((self.tf - self.t0) / delt + 1)
                s = 0
                for k in range(
                        0,
                        int((perturbation_at_time - self.t0) / delt) + 1):
                    temp = PoseStamped()
                    #                       temp.position.x = xrobo1[k,j];
                    #                                       temp.position.y = yrobo1[k,j];
                    temp.header.frame_id = 'map'
                    temp.pose.position.x = xrobo[k]
                    temp.pose.position.y = yrobo[k]
                    temp.pose.position.z = 0.1
                    path.poses[k] = temp
                    s = s + 1

                for k in range(0, i_pert):
                    #	print 'hi'
                    temp = PoseStamped()
                    #			temp.position.x = xrobo1[k,j];
                    #					temp.position.y = yrobo1[k,j];
                    temp.header.frame_id = 'map'
                    temp.pose.position.x = xrobo1[k, j]
                    temp.pose.position.y = yrobo1[k, j]
                    temp.pose.position.z = 0.1
                    path.poses[k + s] = temp

                self.paths_publisher[j].publish(path)
            temp_c = temp_c + 1
            break
            r.sleep()
        r2 = rospy.Rate(1.0 / delt)
        print rospy.Time.now().secs
        for i in range(0, int((perturbation_at_time - self.t0) / delt) + 1):
            vel_command = Twist()
            vel_command.linear.x = math.sqrt(xrobodot[i]**2 + yrobodot[i]**2)
            #	vel_command.linear.y = yrobodot[i]
            vel_command.linear.z = 0
            vel_command.angular.x = 0
            vel_command.angular.y = 0
            vel_command.angular.z = wrobo[i]
            self.vel_publisher.publish(vel_command)
            r2.sleep()
        path_index = 5
        path = Path()
        path.header.frame_id = 'map'
        path.poses = [PoseStamped] * int((self.tf - self.t0) / delt + 1)
        s = 0
        for k in range(0, int((perturbation_at_time - self.t0) / delt) + 1):
            temp = PoseStamped()
            #                       temp.position.x = xrobo1[k,j];
            #                                       temp.position.y = yrobo1[k,j];
            temp.header.frame_id = 'map'
            temp.pose.position.x = xrobo[k]
            temp.pose.position.y = yrobo[k]
            temp.pose.position.z = 0.1
            path.poses[k] = temp
            s = s + 1

        for k in range(0, i_pert):
            #       print 'hi'
            temp = PoseStamped()
            #                       temp.position.x = xrobo1[k,j];
            #                                       temp.position.y = yrobo1[k,j];
            temp.header.frame_id = 'map'
            temp.pose.position.x = xrobo1[k, path_index]
            temp.pose.position.y = yrobo1[k, path_index]
            temp.pose.position.z = 0.1
            path.poses[k + s] = temp
        self.paths_publisher[path_index].publish(path)
        for i in range(0, i_pert):
            vel_command = Twist()
            vel_command.linear.x = math.sqrt(xrobodot1[i, path_index]**2 +
                                             yrobodot1[i, path_index]**2)
            #	vel_command.linear.y = yrobodot[i]
            vel_command.linear.z = 0
            vel_command.angular.x = 0
            vel_command.angular.y = 0
            vel_command.angular.z = wrobo1[i, path_index]
            self.vel_publisher.publish(vel_command)
            r2.sleep()

        print rospy.Time.now().secs
	def move_robot(self):
		inp=[self.x0,self.y0,self.xf,self.yf,self.vx0,self.ax0,self.vxf,self.k0,self.kdot0,self.kddot0,self.kf,self.kdotf,self.t0,self.tf,self.xc,self.yc,self.tc];
		[xo,x1,x2,x3,x4,xf,ko,k1,k2,k3,k4,kf ] = get_way_points(inp);
		t=self.t0;
		i=0;
		i_pert=0; 
		npath=21; 
		perturbation_at_time=8.1;
		delt = 0.1
		robot_end = 0;
		t_start_pert = 0;
		time = zeros((self.tf-self.t0)/delt + 1);
		xrobo = zeros((self.tf-self.t0)/delt + 1);
		yrobo = zeros((self.tf-self.t0)/delt + 1);
		krobo = zeros((self.tf-self.t0)/delt + 1);
		xrobodot =zeros((self.tf-self.t0)/delt + 1);
		yrobodot = zeros((self.tf-self.t0)/delt + 1);
		krobodot = zeros((self.tf-self.t0)/delt + 1);
		xroboddot = zeros((self.tf-self.t0)/delt + 1);
		kroboddot = zeros((self.tf-self.t0)/delt + 1);
		wrobo1 = zeros(((self.tf-self.t0)/delt + 1,npath));
		vrobo1 = zeros(((self.tf-self.t0)/delt + 1,npath));
		xrobo1 = zeros(((self.tf-self.t0)/delt + 1,npath));
		yrobo1 = zeros(((self.tf-self.t0)/delt + 1,npath));
		xrobodot1 = zeros(((self.tf-self.t0)/delt + 1,npath));
		xroboddot1 = zeros(((self.tf-self.t0)/delt + 1,npath));
		yrobodot1 = zeros(((self.tf-self.t0)/delt + 1,npath));
		krobo1 = zeros(((self.tf-self.t0)/delt + 1,npath));
		krobodot1 = zeros(((self.tf-self.t0)/delt + 1,npath));
		kroboddot1 = zeros(((self.tf-self.t0)/delt + 1,npath));
		time_perturb_duration = zeros((self.tf-self.t0)/delt + 1);
		l = []
		while(t <= self.tf):
			time[i] = t
			output_robo = get_robot_pose( self.t0,t,self.tf,xo,x1,x2,x3,x4,xf,ko,k1,k2,k3,k4,kf,self.y0);
			xrobo[i]=output_robo[0];yrobo[i]=output_robo[1];krobo[i]=output_robo[2];xrobodot[i]=output_robo[3];yrobodot[i]=output_robo[4];krobodot[i]=output_robo[5];xroboddot[i]=output_robo[6];kroboddot[i]=output_robo[7];
			
			if(abs(t-perturbation_at_time)<0.00000111):
				t_start_pert=t; 
				robo_end=i;  
				bernp=pert_traj(t_start_pert,self.tf,xrobo[robo_end],yrobo[robo_end],self.xf,self.yf,xrobodot[robo_end],xroboddot[robo_end],yrobodot[robo_end],krobo[robo_end],krobodot[robo_end],kroboddot[robo_end],npath);

			if(t > perturbation_at_time ):
				time_perturb_duration[i_pert] = t			
			        output_robo_pert = numpy.zeros((npath,8))
			        for j in range(0,npath):
					output_robo_pert[j] = get_robot_pose(t_start_pert,t,self.tf,bernp[0,j],bernp[1,j],bernp[2,j],bernp[3,j],bernp[4,j],bernp[5,j],bernp[6,j],bernp[7,j],bernp[8,j],bernp[9,j],bernp[10,j],bernp[11,j],self.y0);
				xrobo1[i_pert]=output_robo_pert[:,0];yrobo1[i_pert]=output_robo_pert[:,1];krobo1[i_pert]=output_robo_pert[:,2];xrobodot1[i_pert]=output_robo_pert[:,3];yrobodot1[i_pert]=output_robo_pert[:,4];krobodot1[i_pert]=output_robo_pert[:,5];xroboddot1[i_pert]=output_robo_pert[:,6];kroboddot1[i_pert]=output_robo_pert[:,7];
#				xrobo1[i_pert] = l[1]
#				yrobo1[i_pert] = l[2]
#				xrobodot1[i_pert] = l[3]
#				xroboddot1[i_pert] = l[4]
#				yrobodot1[i_pert] = l[5]
#				krobo1[i_pert] = l[6]
#				krobodot1[i_pert] = l[7]
				wrobo1[i_pert]= [krobodot1[i_pert,j]/(1.0+krobo1[i_pert,j]**2) for j in range(0,npath)]
				vrobo1[i_pert]=[math.sqrt(xrobodot1[i_pert,j]**2+yrobodot1[i_pert,j]**2) for j in range(0,npath)] 
				i_pert=i_pert+1;

			i = i+1
			t = t+0.1
		fig1 = plt.figure()
	        plt.plot(xrobo,yrobo,'r+')
		fig2 = plt.figure()
		plt.plot(xrobo1,yrobo1)
		plt.show()
Exemplo n.º 5
0
#!/usr/bin/env python
from get_way_points import *
[xo, x1, x2, x3, x4, xf, ko, k1, k2, k3, k4,
 kf] = get_way_points([2, 2, 8, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 25, 5, 5, 12.5])
print[xo, x1, x2, x3, x4, xf, ko, k1, k2, k3, k4, kf]
Exemplo n.º 6
0
#!/usr/bin/env python 
from get_way_points import *
[ xo,x1,x2,x3,x4,xf,ko,k1,k2,k3,k4,kf ] = get_way_points([2,2,8,8,0,0,0,0,0,0,0,0,0,25,5,5,12.5])
print [ xo,x1,x2,x3,x4,xf,ko,k1,k2,k3,k4,kf ]
Exemplo n.º 7
0
def pert_traj(t_start_pert, tf, xrobo, yrobo, x5, y5, xrobodot, xroboddot,
              yrobodot, krobo, krobodot, kroboddot, npath):
    xo = xrobo
    yo = yrobo
    xdoto = ((xrobodot))
    xddoto = xroboddot
    ydoto = yrobodot
    kf = 0
    ko = krobo
    kdoto = krobodot
    kddoto = kroboddot
    xdotf = 0
    kdotf = 0
    to = t_start_pert
    x_pert1 = ((xo + x5) / 2.0)
    if yo > y5:
        y_pert1 = ((y5 - yo) / 2.0)
    else:
        y_pert1 = (((y5 - yo) / 2.0))
    t_pert1 = to + 0.5 * (tf - to)
    t_pert2 = to + 0.5 * (tf - to)
    min_pert = -1.0
    max_pert = 1.0
    npath = 21
    x_pert2 = numpy.zeros(npath)
    y_pert2 = numpy.zeros(npath)
    xrc0 = numpy.zeros(npath)
    xrc1 = numpy.zeros(npath)
    xrc2 = numpy.zeros(npath)
    xrc3 = numpy.zeros(npath)
    xrc4 = numpy.zeros(npath)
    xrc5 = numpy.zeros(npath)
    krc0 = numpy.zeros(npath)
    krc1 = numpy.zeros(npath)
    krc2 = numpy.zeros(npath)
    krc3 = numpy.zeros(npath)
    krc4 = numpy.zeros(npath)
    krc5 = numpy.zeros(npath)
    #	pert_listx = linspace(min_pert,max_pert,npath)
    #	pert_listy = linspace(-4.0,4.0,npath)
    #	pert_list = linspace(-1.0,1.0,npath)
    pert_listx = [0] * npath
    pert_listy = linspace(-1.0, 1.0, npath)
    j = 0
    #	for i in range(0,npath):
    #		x_pert2[j] = x_pert1+pert_listx[i]
    #		y_pert2[j] = y_pert1+pert_listy[i]
    #		j = j+1
    theta = math.atan2(y5 - yo, x5 - xo)
    for i in range(0, npath):
        #		x_pert2[j] = x_pert1-pert_list[i]*math.sin(theta)
        #		y_pert2[j] = y_pert1+pert_list[i]*math.cos(theta)
        x_pert2[j] = x_pert1 + x_pert2[j]
        y_pert2[j] = y_pert1 + pert_listy[i]
        #		if yo < y5:
        #			y_pert2[j] = abs(y_pert2[j])

        j = j + 1
    for i1 in range(0, npath):
        input_for_perturbed_trajectories = [
            xo, yo, x5, y5, xdoto, xddoto, xdotf, ko, kdoto, kddoto, kf, kdotf,
            to, tf, x_pert2[i1], y_pert2[i1], t_pert2
        ]
        [
            xrc0[i1], xrc1[i1], xrc2[i1], xrc3[i1], xrc4[i1], xrc5[i1],
            krc0[i1], krc1[i1], krc2[i1], krc3[i1], krc4[i1], krc5[i1]
        ] = get_way_points(input_for_perturbed_trajectories)
    bernst_param = array([
        xrc0, xrc1, xrc2, xrc3, xrc4, xrc5, krc0, krc1, krc2, krc3, krc4, krc5
    ])
    return bernst_param
    def move_robot(self):
        inp = [
            self.x0,
            self.y0,
            self.xf,
            self.yf,
            self.vx0,
            self.ax0,
            self.vxf,
            self.k0,
            self.kdot0,
            self.kddot0,
            self.kf,
            self.kdotf,
            self.t0,
            self.tf,
            self.xc,
            self.yc,
            self.tc,
        ]
        [xo, x1, x2, x3, x4, xf, ko, k1, k2, k3, k4, kf] = get_way_points(inp)
        t = self.t0
        i = 0
        i_pert = 0
        npath = 21
        perturbation_at_time = 8.1
        delt = 0.1
        robot_end = 0
        t_start_pert = 0
        time = zeros((self.tf - self.t0) / delt + 1)
        xrobo = zeros((self.tf - self.t0) / delt + 1)
        yrobo = zeros((self.tf - self.t0) / delt + 1)
        krobo = zeros((self.tf - self.t0) / delt + 1)
        xrobodot = zeros((self.tf - self.t0) / delt + 1)
        yrobodot = zeros((self.tf - self.t0) / delt + 1)
        krobodot = zeros((self.tf - self.t0) / delt + 1)
        xroboddot = zeros((self.tf - self.t0) / delt + 1)
        kroboddot = zeros((self.tf - self.t0) / delt + 1)
        wrobo1 = zeros(((self.tf - self.t0) / delt + 1, npath))
        vrobo1 = zeros(((self.tf - self.t0) / delt + 1, npath))
        xrobo1 = zeros(((self.tf - self.t0) / delt + 1, npath))
        yrobo1 = zeros(((self.tf - self.t0) / delt + 1, npath))
        xrobodot1 = zeros(((self.tf - self.t0) / delt + 1, npath))
        xroboddot1 = zeros(((self.tf - self.t0) / delt + 1, npath))
        yrobodot1 = zeros(((self.tf - self.t0) / delt + 1, npath))
        krobo1 = zeros(((self.tf - self.t0) / delt + 1, npath))
        krobodot1 = zeros(((self.tf - self.t0) / delt + 1, npath))
        kroboddot1 = zeros(((self.tf - self.t0) / delt + 1, npath))
        time_perturb_duration = zeros((self.tf - self.t0) / delt + 1)
        l = []
        while t <= self.tf:
            time[i] = t
            output_robo = get_robot_pose(self.t0, t, self.tf, xo, x1, x2, x3, x4, xf, ko, k1, k2, k3, k4, kf, self.y0)
            xrobo[i] = output_robo[0]
            yrobo[i] = output_robo[1]
            krobo[i] = output_robo[2]
            xrobodot[i] = output_robo[3]
            yrobodot[i] = output_robo[4]
            krobodot[i] = output_robo[5]
            xroboddot[i] = output_robo[6]
            kroboddot[i] = output_robo[7]

            if abs(t - perturbation_at_time) < 0.00000111:
                t_start_pert = t
                robo_end = i
                bernp = pert_traj(
                    t_start_pert,
                    self.tf,
                    xrobo[robo_end],
                    yrobo[robo_end],
                    self.xf,
                    self.yf,
                    xrobodot[robo_end],
                    xroboddot[robo_end],
                    yrobodot[robo_end],
                    krobo[robo_end],
                    krobodot[robo_end],
                    kroboddot[robo_end],
                    npath,
                )

            if t > perturbation_at_time:
                time_perturb_duration[i_pert] = t
                output_robo_pert = numpy.zeros((npath, 8))
                for j in range(0, npath):
                    output_robo_pert[j] = get_robot_pose(
                        t_start_pert,
                        t,
                        self.tf,
                        bernp[0, j],
                        bernp[1, j],
                        bernp[2, j],
                        bernp[3, j],
                        bernp[4, j],
                        bernp[5, j],
                        bernp[6, j],
                        bernp[7, j],
                        bernp[8, j],
                        bernp[9, j],
                        bernp[10, j],
                        bernp[11, j],
                        self.y0,
                    )
                xrobo1[i_pert] = output_robo_pert[:, 0]
                yrobo1[i_pert] = output_robo_pert[:, 1]
                krobo1[i_pert] = output_robo_pert[:, 2]
                xrobodot1[i_pert] = output_robo_pert[:, 3]
                yrobodot1[i_pert] = output_robo_pert[:, 4]
                krobodot1[i_pert] = output_robo_pert[:, 5]
                xroboddot1[i_pert] = output_robo_pert[:, 6]
                kroboddot1[i_pert] = output_robo_pert[:, 7]
                # 				xrobo1[i_pert] = l[1]
                # 				yrobo1[i_pert] = l[2]
                # 				xrobodot1[i_pert] = l[3]
                # 				xroboddot1[i_pert] = l[4]
                # 				yrobodot1[i_pert] = l[5]
                # 				krobo1[i_pert] = l[6]
                # 				krobodot1[i_pert] = l[7]
                wrobo1[i_pert] = [krobodot1[i_pert, j] / (1.0 + krobo1[i_pert, j] ** 2) for j in range(0, npath)]
                vrobo1[i_pert] = [
                    math.sqrt(xrobodot1[i_pert, j] ** 2 + yrobodot1[i_pert, j] ** 2) for j in range(0, npath)
                ]
                i_pert = i_pert + 1

            i = i + 1
            t = t + 0.1
        fig1 = plt.figure()
        plt.plot(xrobo, yrobo, "r+")
        fig2 = plt.figure()
        plt.plot(xrobo1, yrobo1)
        plt.show()