예제 #1
0
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
예제 #2
0
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
예제 #3
0
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