def gettraj(pts):
beziers = fitCurve(pts, 1)
pathi = path()
single = game()
bz = []
for b in beziers:
for t in range(0, 51):
bz.append(bezier.q(b, t / 50.0).tolist())
bz = np.array(bz)
pli = []
for i in range(len(bz)):
single = game()
single.kx = bz[i][0]
single.ky = bz[i][1]
pli.append(single)
pathi.points = pli
xdot = np.gradient(bz.T[0])
ydot = np.gradient(bz.T[1])
Xdot = [xdot, ydot]
Xdot = np.array(Xdot)
Xdot = Xdot.T
pti = []
for i in range(len(Xdot)):
single = game()
single.kx = Xdot[i][0]
single.ky = Xdot[i][1]
pti.append(single)
return pathi, Xdot, pti
def conv(rpath):
p = []
temp = [0, 0]
pt = rpath
for i in range(len(pt.points)):
s = game()
s = pt.points[i]
temp = [s.kx, s.ky]
p.append(temp)
return p
def getpts():
a = input('Enter number of points')
pts = []
robott = input('Enter team \n1\n2\n')
robotn = input('Enter bot number \n0\n1\n')
st = 'robot'+str(robott)+'n'+str(robotn)+'/goalpoints'
path_pub=rospy.Publisher(st,path,queue_size = 20)
for i in range(a):
x = input('Enter x value of point'+str(i))
y = input('Enter y value of point'+str(i))
single = game()
single.kx = x
single.ky = y
pts.append(single)
path_pub.publish(pts)
def get_traj_all(robot, rpath, ppathr, traj_publ):
global flag
r = game()
i = 0
if len(rpath.points) > 0:
p = conv(rpath)
p.reverse()
a = 1
pts = [robot.x, robot.y]
p.append(pts)
p.reverse()
p = np.array(p)
if len(p) > 1:
X, Xdot, pti = gettraj(p)
ppathr.publish(X)
traj_publ.publish(pti)
def hwcallback(msg):
global pctrl
global r1
global r2
global ball
R = 46.6 # radius of robot in pixels
r = 15.7 # radius of wheels in pixels
w1 = msg.w1
w2 = msg.w2
w3 = msg.w3
tag = msg.tag
kick = msg.kick
k = np.array([0, 0, 0]) #np.random.normal(0,0.01,3)
w1 = w1 + k[0]
w2 = w2 + k[1]
w3 = w3 + k[2]
#forward kinematics
vx = -r * m.sqrt(3) / 2 * (w3 - w1)
vy = -r * (w1 / 2 - w2 + w3 / 2)
wz = (r / (3 * R)) * (w1 + w2 + w3)
#local to global conversion
if tag == 0:
bcos = m.cos(r1[0].theta)
bsin = m.sin(r1[0].theta)
if tag == 1:
bcos = m.cos(r1[1].theta)
bsin = m.sin(r1[1].theta)
if tag == 2:
bcos = m.cos(r2[0].theta)
bsin = m.sin(r2[0].theta)
if tag == 3:
bcos = m.cos(r2[1].theta)
bsin = m.sin(r2[1].theta)
vxg = vx * bcos - vy * bsin
vyg = vy * bcos + vx * bsin
a = game(kx=vxg, ky=vyg, thetad=wz, tag=tag, kick=kick)
pctrl.publish(a)
from sbsim.msg import dribble from sbsim.msg import game """ if gmsg.status == 0 bot is stationary if gmsg.status == 1 bot moves to location in gmsg if gmsg.status == 2 ball kicked at target location if rxycs == 1 p2p control pass if rxycs == 2 trajectory command bypass if rxycs == 3 extremal control """ flag=0 ctrl=game() traj_ctrl = game() ctrl.kick=0 ball = p.ball(x = 0,y = 0) bpose = Pose() btwist = Twist() rpose = [Pose(),Pose(),Pose(),Pose()] rtwist =[Twist(),Twist(),Twist(),Twist()] r10msg = goalmsg() r11msg = goalmsg() r20msg = goalmsg() r21msg = goalmsg() r10cs = 1 r11cs = 1 r20cs = 1 r21cs = 1
def run():
global rpath
global rvects
global r10cs
global r11cs
global r20cs
global r21cs
global rpose
rospy.init_node('trajectory_tracking_controller', anonymous=True)
rospy.Subscriber('robot1n0/cselect', Int32, r10selcallback)
rospy.Subscriber('/ballpose', Pose, ballposecallback)
rospy.Subscriber('robot1n1/cselect', Int32, r11selcallback)
rospy.Subscriber('robot2n0/cselect', Int32, r20selcallback)
rospy.Subscriber('robot2n1/cselect', Int32, r21selcallback)
rospy.Subscriber('robot1n0/traj_vect_list', path, r10tcallback)
rospy.Subscriber('robot1n1/traj_vect_list', path, r11tcallback)
rospy.Subscriber('robot2n0/traj_vect_list', path, r20tcallback)
rospy.Subscriber('robot2n1/traj_vect_list', path, r21tcallback)
rospy.Subscriber('robot1n0/path', path, r1n0pathcbk)
rospy.Subscriber('robot1n1/path', path, r1n1pathcbk)
rospy.Subscriber('robot2n0/path', path, r2n0pathcbk)
rospy.Subscriber('robot2n1/path', path, r2n1pathcbk)
rospy.Subscriber('robot1n0/pose', Pose, r10posecallback)
rospy.Subscriber('robot1n1/pose', Pose, r11posecallback)
rospy.Subscriber('robot2n0/pose', Pose, r20posecallback)
rospy.Subscriber('robot2n1/pose', Pose, r21posecallback)
traj_pub_r10 = rospy.Publisher('robot1n0/traj_vect', game, queue_size=20)
traj_pub_r11 = rospy.Publisher('robot1n1/traj_vect', game, queue_size=20)
traj_pub_r20 = rospy.Publisher('robot2n0/traj_vect', game, queue_size=20)
traj_pub_r21 = rospy.Publisher('robot2n1/traj_vect', game, queue_size=20)
rate = rospy.Rate(30)
kpc = 0.0001
kpv = 0.013
kph1 = 0.1
kdh1 = 0.2
heading_error_1 = 0
while (True):
if r10cs == 2:
'''
this section sets the point for the bot to point at(heading control)
'''
r10hp = Pose()
r10hp.position.x = 0
r10hp.position.y = 0
r10vel = game()
r10vel.kx = 0
r10vel.ky = 0
#pd Controller for heading
thtg = m.atan2((r10hp.position.y - rpose[0].y),
(r10hp.position.x - rpose[0].x))
"""if(( thetaset_1 >=(m.pi)) and (thetaset_1 < (0.95*m.pi))):
thetaset_1= abs(thetaset_1)
diff_1 = (thetaset_1 - rpose[0].theta) - heading_error_1
heading_error_1 = (thetaset_1 - rpose[0].theta)
r10vel.thetad = kph1*heading_error_1 + kdh1*diff_1"""
while (thtg < 0):
if thtg < 0:
thtg = 6.28 + thtg
else:
break
while (thtg > 6.28):
if thtg > 6.28:
thtg = thtg - 6.28
else:
break
if (abs(thtg - rpose[0].theta) > 0.1 and thtg <= 6.28
and thtg >= 0):
if (abs(thtg - rpose[0].theta) <
(6.28 - abs(thtg - rpose[0].theta))):
f = (thtg - rpose[0].theta) / abs(thtg - rpose[0].theta)
e = abs(thtg - rpose[0].theta)
r10vel.thetad = (e * kph1) * (f)
else:
f = (thtg - rpose[0].theta) / abs(thtg - rpose[0].theta)
e = 6.28 - abs(thtg - rpose[0].theta)
r10vel.thetad = -((e * kph1)) * (f)
else:
r10vel.thetad = 0
if len(rpath[0]) > 0 and len(rvects[0]) > 0:
l = len(rpath[0])
cptr10, indexr10, mindistr10 = closestpt_search(
rpath[0], rpose[0])
"""
cptr10 closest point on trajectory from robot
indexr10 index of closest point on the trajectory
mindistr10 distance of closest point on the trajetory
vectr10 tangent of trajectory curve
comp1x component 1 x tangential component
comp2x component 2 x correctional component
comp1y component 1 y tangential component
comp2y component 2 y correctional component
"""
vectr10 = rvects[0][indexr10]
comp1x = findexv(indexr10, l) * kpv * vectr10[0]
comp1y = findexv(indexr10, l) * kpv * vectr10[1]
if (not (cptr10[0] - rpose[0].x) == 0
and not (cptr10[1] - rpose[0].y) == 0):
comp2x = kpc * findexc(
indexr10, l) * (cptr10[0] - rpose[0].x)**3 / abs(
(cptr10[0] - rpose[0].x) * 10)
comp2y = kpc * findexc(
indexr10, l) * (cptr10[1] - rpose[0].y)**3 / abs(
(cptr10[1] - rpose[0].y) * 10)
else:
comp2x = kpc * findexc(indexr10,
l) * (cptr10[0] - rpose[0].x)
comp2y = kpc * findexc(indexr10,
l) * (cptr10[1] - rpose[0].y)
r10vel.kx = comp1x + comp2x
r10vel.ky = comp1y + comp2y
norm = np.sqrt(r10vel.kx**2 + r10vel.ky**2)
#if not norm == 0:
# r10vel.kx /= 0.5*norm
# r10vel.ky /= 0.5*norm
# r10vel.tag = 0
#if mindistr10 < 1:
#rpath[0].pop(indexr10)
traj_pub_r10.publish(r10vel)
if r11cs == 2:
if len(rpath[1]) > 0 and len(rvects[1]) > 0:
r11vel = game()
l = len(rpath[1])
cptr11, indexr11, mindistr11 = closestpt_search(
rpath[1], rpose[1])
vectr11 = rvects[1][indexr11]
comp1x = findexv(indexr11, l) * kpv * vectr11[0]
comp2x = kpc * findexc(indexr11, l) * (cptr11[0] - rpose[1].x)
comp1y = findexv(indexr11, l) * kpv * vectr11[1]
comp2y = kpc * findexc(indexr11, l) * (cptr11[1] - rpose[1].y)
r11vel.kx = comp1x + comp2x
r11vel.ky = comp1y + comp2y
norm = np.sqrt(r11vel.kx**2 + r11vel.ky**2)
if not norm == 0:
r11vel.kx /= 0.5 * norm
r11vel.ky /= 0.5 * norm
r11vel.tag = 1
#if mindistr11 < 1:
# rpath[0].pop(indexr11)
traj_pub_r11.publish(r11vel)
if r20cs == 2:
if len(rpath[2]) > 0 and len(rvects[2]) > 0:
r20vel = game()
l = len(rpath[2])
cptr20, indexr20, mindistr20 = closestpt_search(
rpath[2], rpose[2])
vectr20 = rvects[2][indexr20]
comp1x = findexv(indexr20, l) * kpv * vectr20[0]
comp2x = kpc * findexc(indexr20, l) * (cptr20[0] - rpose[2].x)
comp1y = findexv(indexr20, l) * kpv * vectr20[1]
comp2y = kpc * findexc(indexr20, l) * (cptr20[1] - rpose[2].y)
r20vel.kx = comp1x + comp2x
r20vel.ky = comp1y + comp2y
norm = np.sqrt(r20vel.kx**2 + r20vel.ky**2)
if not norm == 0:
r20vel.kx /= 0.5 * norm
r20vel.ky /= 0.5 * norm
r20vel.tag = 2
#if mindistr20 < 1:
# rpath[2].pop(indexr20)
traj_pub_r20.publish(r20vel)
if r21cs == 2:
if len(rpath[3]) > 0 and len(rvects[3]) > 0:
r21vel = game()
l = len(rpath[3])
cptr21, indexr21, mindistr21 = closestpt_search(
rpath[3], rpose[3])
vectr21 = rvects[3][indexr21]
comp1x = findexv(indexr21, l) * kpv * vectr21[0]
comp2x = kpc * findexc(indexr21, l) * (cptr21[0] - rpose[3].x)
comp1y = findexv(indexr21, l) * kpv * vectr21[1]
comp2y = kpc * findexc(indexr21, l) * (cptr21[1] - rpose[3].y)
r21vel.kx = comp1x + comp2x
r21vel.ky = comp1y + comp2y
norm = np.sqrt(r21vel.kx**2 + r21vel.ky**2)
if not norm == 0:
r21vel.kx /= 0.5 * norm
r21vel.ky /= 0.5 * norm
r21vel.tag = 3
#if mindistr21 < 1:
# rpath[3].pop(indexr21)
traj_pub_r21.publish(r21vel)
rate.sleep()
def run():
global rpath
global rvects
global r10cs
global r11cs
global r20cs
global r21cs
global rpose
rospy.init_node('trajectory_tracking_controller',anonymous=True)
rospy.Subscriber('robot1n0/cselect',Int32,r10selcallback)
rospy.Subscriber('/ballpose',Pose,ballposecallback)
rospy.Subscriber('robot1n1/cselect',Int32,r11selcallback)
rospy.Subscriber('robot2n0/cselect',Int32,r20selcallback)
rospy.Subscriber('robot2n1/cselect',Int32,r21selcallback)
rospy.Subscriber('robot1n0/traj_vect_list',path,r10tcallback)
rospy.Subscriber('robot1n1/traj_vect_list',path,r11tcallback)
rospy.Subscriber('robot2n0/traj_vect_list',path,r20tcallback)
rospy.Subscriber('robot2n1/traj_vect_list',path,r21tcallback)
rospy.Subscriber('robot1n0/path',path,r1n0pathcbk)
rospy.Subscriber('robot1n1/path',path,r1n1pathcbk)
rospy.Subscriber('robot2n0/path',path,r2n0pathcbk)
rospy.Subscriber('robot2n1/path',path,r2n1pathcbk)
rospy.Subscriber('robot1n0/pose',Pose,r10posecallback)
rospy.Subscriber('robot1n1/pose',Pose,r11posecallback)
rospy.Subscriber('robot2n0/pose',Pose,r20posecallback)
rospy.Subscriber('robot2n1/pose',Pose,r21posecallback)
traj_pub_r10 = rospy.Publisher('robot1n0/traj_vect',game, queue_size = 20)
traj_pub_r11 = rospy.Publisher('robot1n1/traj_vect',game, queue_size = 20)
traj_pub_r20 = rospy.Publisher('robot2n0/traj_vect',game, queue_size = 20)
traj_pub_r21 = rospy.Publisher('robot2n1/traj_vect',game, queue_size = 20)
rate = rospy.Rate(30)
kpc = 1.0
kpv = 30
while(True):
if r10cs == 2:
'''
this section sets the point for the bot to point at(heading control)
'''
r10hp = Pose()
r10hp.position.x = 0
r10hp.position.y = 0
if len(rpath[0])> 0 and len(rvects[0]) > 0:
r10vel = game()
l = len(rpath[0])
cptr10,indexr10,mindistr10 = closestpt_search(rpath[0],rpose[0])
"""
cptr10 closest point on trajectory from robot
indexr10 index of closest point on the trajectory
mindistr10 distance of closest point on the trajetory
vectr10 tangent of trajectory curve
comp1x component 1 x tangential component
comp2x component 2 x correctional component
comp1y component 1 y tangential component
comp2y component 2 y correctional component
"""
vectr10 = rvects[0][indexr10]
comp1x = findexv(indexr10,l)*kpv*vectr10[0]
comp1y = findexv(indexr10,l)*kpv*vectr10[1]
if(not (cptr10[0] - rpose[0].x)==0 and not(cptr10[1] - rpose[0].y)==0):
comp2x = kpc*findexc(indexr10,l)*(cptr10[0] - rpose[0].x)**3/abs((cptr10[0] - rpose[0].x)*10)
comp2y = kpc*findexc(indexr10,l)*(cptr10[1] - rpose[0].y)**3/abs((cptr10[1] - rpose[0].y)*10)
else:
comp2x = kpc*findexc(indexr10,l)*(cptr10[0] - rpose[0].x)
comp2y = kpc*findexc(indexr10,l)*(cptr10[1] - rpose[0].y)
r10vel.kx = comp1x + comp2x
r10vel.ky = comp1y + comp2y
'''p Controller for heading'''
kph1 = 0.1
thetaset_1 = m.atan2((r10hp.position.y-rpose[0].y),(r10hp.position.x-rpose[0].x))
print(thetaset_1, rpose[0].theta,(thetaset_1 - rpose[0].theta))
r10vel.thetad = kph1*(thetaset_1 - rpose[0].theta)
norm = np.sqrt(r10vel.kx**2 + r10vel.ky**2)
if not norm == 0:
r10vel.kx /= 0.5*norm
r10vel.ky /= 0.5*norm
r10vel.tag = 0
#if mindistr10 < 1:
#rpath[0].pop(indexr10)
traj_pub_r10.publish(r10vel)
if r11cs == 2:
if len(rpath[1])> 0 and len(rvects[1]) > 0:
r11vel = game()
l = len(rpath[1])
cptr11,indexr11,mindistr11 = closestpt_search(rpath[1],rpose[1])
vectr11 = rvects[1][indexr11]
comp1x = findexv(indexr11,l)*kpv*vectr11[0]
comp2x = kpc*findexc(indexr11,l)*(cptr11[0] - rpose[1].x)
comp1y = findexv(indexr11,l)*kpv*vectr11[1]
comp2y = kpc*findexc(indexr11,l)*(cptr11[1] - rpose[1].y)
r11vel.kx = comp1x + comp2x
r11vel.ky = comp1y + comp2y
norm = np.sqrt(r11vel.kx**2 + r11vel.ky**2)
if not norm == 0:
r11vel.kx /= 0.5*norm
r11vel.ky /= 0.5*norm
r11vel.tag = 1
#if mindistr11 < 1:
# rpath[0].pop(indexr11)
traj_pub_r11.publish(r11vel)
if r20cs == 2:
if len(rpath[2])> 0 and len(rvects[2]) > 0:
r20vel = game()
l = len(rpath[2])
cptr20,indexr20,mindistr20 = closestpt_search(rpath[2],rpose[2])
vectr20 = rvects[2][indexr20]
comp1x = findexv(indexr20,l)*kpv*vectr20[0]
comp2x = kpc*findexc(indexr20,l)*(cptr20[0] - rpose[2].x)
comp1y = findexv(indexr20,l)*kpv*vectr20[1]
comp2y = kpc*findexc(indexr20,l)*(cptr20[1] - rpose[2].y)
r20vel.kx = comp1x + comp2x
r20vel.ky = comp1y + comp2y
norm = np.sqrt(r20vel.kx**2 + r20vel.ky**2)
if not norm == 0:
r20vel.kx /= 0.5*norm
r20vel.ky /= 0.5*norm
r20vel.tag = 2
#if mindistr20 < 1:
# rpath[2].pop(indexr20)
traj_pub_r20.publish(r20vel)
if r21cs == 2:
if len(rpath[3])> 0 and len(rvects[3]) > 0:
r21vel = game()
l = len(rpath[3])
cptr21,indexr21,mindistr21 = closestpt_search(rpath[3],rpose[3])
vectr21 = rvects[3][indexr21]
comp1x = findexv(indexr21,l)*kpv*vectr21[0]
comp2x = kpc*findexc(indexr21,l)*(cptr21[0] - rpose[3].x)
comp1y = findexv(indexr21,l)*kpv*vectr21[1]
comp2y = kpc*findexc(indexr21,l)*(cptr21[1] - rpose[3].y)
r21vel.kx = comp1x + comp2x
r21vel.ky = comp1y + comp2y
norm = np.sqrt(r21vel.kx**2 + r21vel.ky**2)
if not norm == 0:
r21vel.kx /= 0.5*norm
r21vel.ky /= 0.5*norm
r21vel.tag = 3
#if mindistr21 < 1:
# rpath[3].pop(indexr21)
traj_pub_r21.publish(r21vel)
rate.sleep()
from geometry_msgs.msg import Pose
from sbsim.msg import game
from sbsim.msg import path
b1sin = 0
b1cos = 1
b2sin = 0
b2cos = 1
b3sin = 0
b3cos = 1
b4sin = 0
b4cos = 1
tag = 0
kx = 0
ky = 0
thetad = 0
b1g = game()
b2g = game()
b3g = game()
b4g = game()
b1lt = Twist()
b2lt = Twist()
b3lt = Twist()
b4lt = Twist()
"""
def b1t(msg):
global b1sin
global b1cos
global b1lt
vx = -msg.linear.x
vy = -msg.linear.y
b1lt.linear.x = vx*b1cos + vy*b1sin