class StraightLineGen(Trajectory):
done = False
a_max = 9.81/3.
def __init__(self,trajectory_node,start,end):
Trajectory.__init__(self,trajectory_node)
self.start_point = start
self.end_point = end
self.tg = TrajectoryGenerator()
self.dist = self.tg.get_distance(self.start_point, self.end_point)
n = [0.,0.,0.]
for i in range(0,3):
n[i] = self.end_point[i] - self.start_point[i]
self.e_t = self.tg.get_direction(n)
self.t_f = math.sqrt(6*self.dist/0.9*self.a_max)
self.constant = -2.0/self.t_f**3.0 * self.dist
def begin(self):
self.__set_done(False)
def loop(self,start_time):
r = 10.0
rate = rospy.Rate(10)
time = start_time
while not rospy.is_shutdown() and not self.is_done():
outpos = self.get_position(time)
outpos.append(0)
outvelo = self.get_velocity(time)
outvelo.append(0)
outacc = self.get_acceleration(time)
outacc.append(0)
outmsg = self.tg.get_message(outpos, outvelo, outacc)
self.trajectory_node.send_msg(outmsg)
self.trajectory_node.send_permission(False)
rate.sleep()
time += 1/r
if time >= self.t_f:
self.__set_done(True)
end = self.end_point
end.append(0)
outmsg = self.tg.get_message(self.end_point, [0.0,0.0,0.0,0.0], [0.0,0.0,0.0,0.0])
self.trajectory_node.send_msg(outmsg)
self.trajectory_node.send_permission(False)
def __set_done(self,boolean):
self.done = boolean
def get_position(self,t):
outpos = [0.0,0.0,0.0]
s = self.get_s(t)
for i in range(0,3):
outpos[i] = self.start_point[i] + s * self.e_t[i]
return outpos
def get_s(self,t):
return t**2.0 * self.constant * (t-1.5*self.t_f)
def get_velocity(self,t):
outvelo = [0.0,0.0,0.0]
s_d = self.get_s_d(t)
for i in range(0,3):
outvelo[i] = s_d * self.e_t[i]
return outvelo
def get_s_d (self,t):
return self.constant * (3 * t**2.0 - 3 * self.t_f * t)
def get_acceleration(self,t):
outacc = [0.0,0.0,0.0]
s_dd = self.get_s_dd(t)
for i in range(0,3):
outacc[i] = s_dd * self.e_t[i]
return outacc
def get_s_dd (self,t):
return self.constant * (6 * t - 3 * self.t_f )
def is_done(self):
return self.done
class AccGen(Trajectory):
done = False
a_max = 0.6**2.0 / 0.8
t_f = 0
def __init__(self, trajectory_node, mid, start, velo, psi):
Trajectory.__init__(self, trajectory_node)
self.tg = TrajectoryGenerator()
self.midpoint = mid
self.start = start
n = [
self.start[0] - self.midpoint[0], self.start[1] - self.midpoint[1],
self.start[0] - self.midpoint[2]
]
self.radius = self.tg.get_distance(self.start, self.midpoint)
self.initial_velo = velo
self.velo = self.tg.get_norm(self.initial_velo)
#define new coordinates
self.e_n = self.tg.get_direction(n)
self.yp = self.tg.get_direction(self.initial_velo)
self.zp = numpy.cross(self.e_n, self.yp)
self.psi = psi #angle of rotation about initial e_n direction
self.w = self.radius * self.velo
self.theta_z = self.tg.get_projection([0, 0, 1], self.e_n)
def begin(self):
v_max = (self.radius * self.a_max)**(0.5)
if self.velo > v_max:
self.velo = v_max
self.__set_done(False)
def loop(self, start_time):
self.__set_t_f(
self.velo /
math.sqrt(self.a_max**2.0 - self.velo**4.0 / self.radius**2.0))
time = start_time
r = 10.0
rate = rospy.Rate(r)
while not rospy.is_shutdown() and not is_done():
theta = self.velo * time**2.0 / (2 * self.radius * t_f)
w = self.velo * time / (self.radius * t_f)
alpha = self.velo / (self.radius * t_f)
outpos = self.tg.get_circle_point(self.radius, theta)
outpos = self.tg.offset(outpos, self.midpoint)
outpos.append(self.tg.adjust_yaw([1, 0, 0]))
outvelo = self.tg.get_circle_velocity(self.radius, theta, omega)
outvelo.append(0)
outacc = self.tg.get_circle_acc(self.radius, theta, omega, alpha)
outacc.append(0)
outmsg = self.tg.get_message(outpos, outvelo, outacc)
self.trajectory_node.send_msg(outmsg)
self.trajectory_node.send_permission(False)
rate.sleep()
time += 1 / r
if time >= t_f:
self.__set_done(True)
def is_done(self):
return self.done
def __set_done(self, boolean):
self.done = boolean
def __set_t_f(self, t):
self.t_f = t
def get_t_f(self):
return self.t_f
class ArcGen(Trajectory):
done = False
a_max = 0.6**2.0/0.8
def __init__(self,trajectory_node,mid,start,velo,psi):
Trajectory.__init__(self,trajectory_node)
self.tg = TrajectoryGenerator()
self.midpoint = mid
self.start = start
n = [self.start[0]-self.midpoint[0],self.start[1]-self.midpoint[1],self.start[0]-self.midpoint[2]]
self.radius = self.tg.get_distance(self.start,self.midpoint)
self.initial_velo = velo
self.velo = self.tg.get_norm(self.initial_velo)
#define new coordinates
self.e_n = self.tg.get_direction(n)
self.yp = self.tg.get_direction(self.initial_velo)
self.zp = numpy.cross(self.e_n,self.yp)
self.psi = psi #angle of rotation about initial e_n direction
self.w = self.radius*self.velo
self.theta_z = self.tg.get_projection([0,0,1],self.e_n)
def begin(self):
v_max = (self.radius*self.a_max)**(0.5)
if self.velo > v_max:
self.velo = v_max
self.__set_done(False)
def loop(self, start_time):
time = start_time
r = 10.0
rate = rospy.Rate(r)
while not rospy.is_shutdown() and not self.is_done():
outpos = self.tg.get_circle_point(self.radius,self.w*time)
#outpos = self.tg.rotate_vector(outpos,[0,0,self.theta_z])
#outpos = self.tg.vector_to_list(outpos)
outpos = self.tg.offset(outpos,self.midpoint)
outpos.append(self.tg.adjust_yaw([1,0,0]))
outvelo = self.tg.get_circle_velocity(self.radius,self.w*time,self.w)
#outvelo = self.tg.rotate_vector(outvelo,[0,0,self.theta_z])
#outvelo = self.tg.vector_to_list(outvelo)
outvelo.append(0)
outacc = self.tg.get_circle_acc(self.radius,self.w*time,self.w,0)
#outacc = self.tg.rotate_vector(outacc,[0,0,self.theta_z])
#outacc = self.tg.vector_to_list(outacc)
outacc.append(0)
outmsg = self.tg.get_message(outpos,outvelo,outacc)
self.trajectory_node.send_msg(outmsg)
self.trajectory_node.send_permission(False)
rate.sleep()
time += 1/r
if self.w*time >= self.psi:
outpos = self.tg.get_circle_point(self.radius,self.psi)
#outpos = self.tg.rotate_vector(outpos,[0,0,self.theta_z])
#outpos = self.tg.vector_to_list(outpos)
outpos = self.tg.offset(outpos,self.midpoint)
outpos.append(self.tg.adjust_yaw([1,0,0]))
outvelo = self.tg.get_circle_velocity(self.radius,self.psi,self.w)
#outvelo = self.tg.rotate_vector(outvelo,[0,0,self.theta_z])
#outvelo = self.tg.vector_to_list(outvelo)
outvelo.append(0)
outacc = self.tg.get_circle_acc(self.radius,self.psi,self.w,0)
#outacc = self.tg.rotate_vector(outacc,[0,0,self.theta_z])
#outacc = self.tg.vector_to_list(outacc)
outacc.append(0)
outmsg = self.tg.get_message(outpos,outvelo,outacc)
self.trajectory_node.send_msg(outmsg)
self.trajectory_node.send_permission(False)
rate.sleep()
time += 1/r
self.__set_done(True)
def is_done(self):
return self.done
def __set_done(self,boolean):
self.done = boolean
class StepPlugin(Plugin):
def __init__(self, context):
super(StepPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('StepPlugin')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q",
"--quiet",
action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)),
'Step.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('StepPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.tg = TrajectoryGenerator()
self._widget.bUp.clicked.connect(self.Up)
self._widget.bDown.clicked.connect(self.Down)
self._widget.bLeft.clicked.connect(self.Left)
self._widget.bRight.clicked.connect(self.Right)
self._widget.bFront.clicked.connect(self.Front)
self._widget.bBack.clicked.connect(self.Back)
self._widget.bStart.clicked.connect(self.Start)
self._widget.IrisInputBox.insertItems(
0, ['iris1', 'iris2', 'iris3', 'iris4'])
self.point = []
def Start(self):
abspath = "/" + self._widget.IrisInputBox.currentText() + "/"
self.pub = rospy.Publisher(abspath + 'trajectory_gen/target',
QuadPositionDerived,
queue_size=10)
self.security_pub = rospy.Publisher(abspath + 'trajectory_gen/done',
Permission,
queue_size=10)
rospy.Subscriber(abspath + 'security_guard/data_forward',
QuadPositionDerived, self.get_position)
def get_position(self, data):
if self.point == []:
self.point = [data.x, data.y, data.z]
utils.logwarn("Initial position: %s" % (str(self.point)))
def Up(self):
self.Goto([0.0, 0.0, 1.0])
def Down(self):
self.Goto([0.0, 0.0, 0.5])
def Left(self):
self.Goto([-1.0, 0.0, 0.5])
def Right(self):
self.Goto([1.0, 0.0, 0.5])
def Back(self):
self.Goto([0.0, -1.0, 0.5])
def Front(self):
self.Goto([0.0, 1.0, 0.5])
def Goto(self, dest):
utils.logwarn(str(dest))
outpos = [sum(x) for x in zip(self.point, dest)]
utils.logwarn(str(outpos))
outpos.append(0.0)
outvelo = [0.0] * 3
outvelo.append(0.0)
outacc = [0.0] * 3
outacc.append(0.0)
outmsg = self.tg.get_message(outpos, outvelo, outacc)
self.pub.publish(outmsg)
self.security_pub.publish(False)
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
instance_settings.set_value("irisindex",
self._widget.IrisInputBox.currentIndex())
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
index = instance_settings.value("irisindex", 0)
self._widget.IrisInputBox.setCurrentIndex(int(index))
class AccGen(Trajectory):
done = False
a_max = 0.6**2.0/0.8
t_f = 0
def __init__(self,trajectory_node,mid,start,velo,psi):
Trajectory.__init__(self,trajectory_node)
self.tg = TrajectoryGenerator()
self.midpoint = mid
self.start = start
n = [self.start[0]-self.midpoint[0],self.start[1]-self.midpoint[1],self.start[0]-self.midpoint[2]]
self.radius = self.tg.get_distance(self.start,self.midpoint)
self.initial_velo = velo
self.velo = self.tg.get_norm(self.initial_velo)
#define new coordinates
self.e_n = self.tg.get_direction(n)
self.yp = self.tg.get_direction(self.initial_velo)
self.zp = numpy.cross(self.e_n,self.yp)
self.psi = psi #angle of rotation about initial e_n direction
self.w = self.radius*self.velo
self.theta_z = self.tg.get_projection([0,0,1],self.e_n)
def begin(self):
v_max = (self.radius*self.a_max)**(0.5)
if self.velo > v_max:
self.velo = v_max
self.__set_done(False)
def loop(self, start_time):
self.__set_t_f(self.velo/math.sqrt(self.a_max**2.0 - self.velo**4.0/self.radius**2.0))
time = start_time
r = 10.0
rate = rospy.Rate(r)
while not rospy.is_shutdown() and not is_done():
theta = self.velo*time**2.0/(2*self.radius*t_f)
w = self.velo*time/(self.radius*t_f)
alpha = self.velo/(self.radius*t_f)
outpos = self.tg.get_circle_point(self.radius, theta)
outpos = self.tg.offset(outpos,self.midpoint)
outpos.append(self.tg.adjust_yaw([1,0,0]))
outvelo = self.tg.get_circle_velocity(self.radius,theta,omega)
outvelo.append(0)
outacc = self.tg.get_circle_acc(self.radius,theta,omega,alpha)
outacc.append(0)
outmsg = self.tg.get_message(outpos,outvelo,outacc)
self.trajectory_node.send_msg(outmsg)
self.trajectory_node.send_permission(False)
rate.sleep()
time += 1/r
if time >= t_f:
self.__set_done(True)
def is_done(self):
return self.done
def __set_done(self,boolean):
self.done = boolean
def __set_t_f(self,t):
self.t_f = t
def get_t_f(self):
return self.t_f
class StraightLineGen(Trajectory):
done = False
a_max = 9.81 / 3.
def __init__(self, trajectory_node, start, end):
Trajectory.__init__(self, trajectory_node)
self.start_point = start
self.end_point = end
self.tg = TrajectoryGenerator()
self.dist = self.tg.get_distance(self.start_point, self.end_point)
n = [0., 0., 0.]
for i in range(0, 3):
n[i] = self.end_point[i] - self.start_point[i]
self.e_t = self.tg.get_direction(n)
self.t_f = math.sqrt(6 * self.dist / 0.9 * self.a_max)
self.constant = -2.0 / self.t_f**3.0 * self.dist
def begin(self):
self.__set_done(False)
def loop(self, start_time):
r = 10.0
rate = rospy.Rate(10)
time = start_time
while not rospy.is_shutdown() and not self.is_done():
outpos = self.get_position(time)
outpos.append(0)
outvelo = self.get_velocity(time)
outvelo.append(0)
outacc = self.get_acceleration(time)
outacc.append(0)
outmsg = self.tg.get_message(outpos, outvelo, outacc)
self.trajectory_node.send_msg(outmsg)
self.trajectory_node.send_permission(False)
rate.sleep()
time += 1 / r
if time >= self.t_f:
self.__set_done(True)
end = self.end_point
end.append(0)
outmsg = self.tg.get_message(self.end_point,
[0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0])
self.trajectory_node.send_msg(outmsg)
self.trajectory_node.send_permission(False)
def __set_done(self, boolean):
self.done = boolean
def get_position(self, t):
outpos = [0.0, 0.0, 0.0]
s = self.get_s(t)
for i in range(0, 3):
outpos[i] = self.start_point[i] + s * self.e_t[i]
return outpos
def get_s(self, t):
return t**2.0 * self.constant * (t - 1.5 * self.t_f)
def get_velocity(self, t):
outvelo = [0.0, 0.0, 0.0]
s_d = self.get_s_d(t)
for i in range(0, 3):
outvelo[i] = s_d * self.e_t[i]
return outvelo
def get_s_d(self, t):
return self.constant * (3 * t**2.0 - 3 * self.t_f * t)
def get_acceleration(self, t):
outacc = [0.0, 0.0, 0.0]
s_dd = self.get_s_dd(t)
for i in range(0, 3):
outacc[i] = s_dd * self.e_t[i]
return outacc
def get_s_dd(self, t):
return self.constant * (6 * t - 3 * self.t_f)
def is_done(self):
return self.done
class StepPlugin(Plugin):
def __init__(self, context):
super(StepPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('StepPlugin')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'Step.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('StepPluginUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.tg = TrajectoryGenerator()
self._widget.bUp.clicked.connect(self.Up)
self._widget.bDown.clicked.connect(self.Down)
self._widget.bLeft.clicked.connect(self.Left)
self._widget.bRight.clicked.connect(self.Right)
self._widget.bFront.clicked.connect(self.Front)
self._widget.bBack.clicked.connect(self.Back)
self._widget.bStart.clicked.connect(self.Start)
self._widget.IrisInputBox.insertItems(0,['iris1','iris2','iris3','iris4'])
self.point = []
def Start(self):
abspath = "/"+self._widget.IrisInputBox.currentText()+"/"
self.pub = rospy.Publisher(abspath+'trajectory_gen/target',QuadPositionDerived, queue_size=10)
self.security_pub = rospy.Publisher(abspath+'trajectory_gen/done', Permission, queue_size=10)
rospy.Subscriber(abspath+'security_guard/data_forward',QuadPositionDerived,self.get_position)
def get_position(self,data):
if self.point == []:
self.point = [data.x,data.y,data.z]
utils.logwarn("Initial position: %s"%(str(self.point)))
def Up(self):
self.Goto([0.0,0.0,1.0])
def Down(self):
self.Goto([0.0,0.0,0.5])
def Left(self):
self.Goto([-1.0,0.0,0.5])
def Right(self):
self.Goto([1.0,0.0,0.5])
def Back(self):
self.Goto([0.0,-1.0,0.5])
def Front(self):
self.Goto([0.0,1.0,0.5])
def Goto(self, dest):
utils.logwarn(str(dest))
outpos = [sum(x) for x in zip(self.point, dest)]
utils.logwarn(str(outpos))
outpos.append(0.0)
outvelo = [0.0]*3
outvelo.append(0.0)
outacc = [0.0]*3
outacc.append(0.0)
outmsg = self.tg.get_message(outpos, outvelo, outacc)
self.pub.publish(outmsg)
self.security_pub.publish(False)
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
instance_settings.set_value("irisindex", self._widget.IrisInputBox.currentIndex())
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
index = instance_settings.value("irisindex",0)
self._widget.IrisInputBox.setCurrentIndex(int(index))
