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()
#!/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]
#!/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 ]
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()