move test_yopo_ros.py and yopo_planner_node.cpp to single script

flightpolicy/control_msg/_PositionCommand.py

@@ -0,0 +1,312 @@
+# This Python file uses the following encoding: utf-8
+"""autogenerated by genpy from quadrotor_msgs/PositionCommand.msg. Do not edit."""
+import codecs
+import sys
+python3 = True if sys.hexversion > 0x03000000 else False
+import genpy
+import struct
+
+import geometry_msgs.msg
+import std_msgs.msg
+
+class PositionCommand(genpy.Message):
+  _md5sum = "4712f0609ca29a79af79a35ca3e3967a"
+  _type = "quadrotor_msgs/PositionCommand"
+  _has_header = True  # flag to mark the presence of a Header object
+  _full_text = """Header header
+geometry_msgs/Point position
+geometry_msgs/Vector3 velocity
+geometry_msgs/Vector3 acceleration
+float64 yaw
+float64 yaw_dot
+float64[3] kx
+float64[3] kv 
+
+uint32 trajectory_id
+
+uint8 TRAJECTORY_STATUS_EMPTY = 0
+uint8 TRAJECTORY_STATUS_READY = 1
+uint8 TRAJECTORY_STATUS_COMPLETED = 3
+uint8 TRAJECTROY_STATUS_ABORT = 4
+uint8 TRAJECTORY_STATUS_ILLEGAL_START = 5
+uint8 TRAJECTORY_STATUS_ILLEGAL_FINAL = 6
+uint8 TRAJECTORY_STATUS_IMPOSSIBLE = 7
+
+# Its ID number will start from 1, allowing you comparing it with 0.
+uint8 trajectory_flag
+
+================================================================================
+MSG: std_msgs/Header
+# Standard metadata for higher-level stamped data types.
+# This is generally used to communicate timestamped data 
+# in a particular coordinate frame.
+# 
+# sequence ID: consecutively increasing ID 
+uint32 seq
+#Two-integer timestamp that is expressed as:
+# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
+# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
+# time-handling sugar is provided by the client library
+time stamp
+#Frame this data is associated with
+string frame_id
+
+================================================================================
+MSG: geometry_msgs/Point
+# This contains the position of a point in free space
+float64 x
+float64 y
+float64 z
+
+================================================================================
+MSG: geometry_msgs/Vector3
+# This represents a vector in free space. 
+# It is only meant to represent a direction. Therefore, it does not
+# make sense to apply a translation to it (e.g., when applying a 
+# generic rigid transformation to a Vector3, tf2 will only apply the
+# rotation). If you want your data to be translatable too, use the
+# geometry_msgs/Point message instead.
+
+float64 x
+float64 y
+float64 z"""
+  # Pseudo-constants
+  TRAJECTORY_STATUS_EMPTY = 0
+  TRAJECTORY_STATUS_READY = 1
+  TRAJECTORY_STATUS_COMPLETED = 3
+  TRAJECTROY_STATUS_ABORT = 4
+  TRAJECTORY_STATUS_ILLEGAL_START = 5
+  TRAJECTORY_STATUS_ILLEGAL_FINAL = 6
+  TRAJECTORY_STATUS_IMPOSSIBLE = 7
+
+  __slots__ = ['header','position','velocity','acceleration','yaw','yaw_dot','kx','kv','trajectory_id','trajectory_flag']
+  _slot_types = ['std_msgs/Header','geometry_msgs/Point','geometry_msgs/Vector3','geometry_msgs/Vector3','float64','float64','float64[3]','float64[3]','uint32','uint8']
+
+  def __init__(self, *args, **kwds):
+    """
+    Constructor. Any message fields that are implicitly/explicitly
+    set to None will be assigned a default value. The recommend
+    use is keyword arguments as this is more robust to future message
+    changes.  You cannot mix in-order arguments and keyword arguments.
+
+    The available fields are:
+       header,position,velocity,acceleration,yaw,yaw_dot,kx,kv,trajectory_id,trajectory_flag
+
+    :param args: complete set of field values, in .msg order
+    :param kwds: use keyword arguments corresponding to message field names
+    to set specific fields.
+    """
+    if args or kwds:
+      super(PositionCommand, self).__init__(*args, **kwds)
+      # message fields cannot be None, assign default values for those that are
+      if self.header is None:
+        self.header = std_msgs.msg.Header()
+      if self.position is None:
+        self.position = geometry_msgs.msg.Point()
+      if self.velocity is None:
+        self.velocity = geometry_msgs.msg.Vector3()
+      if self.acceleration is None:
+        self.acceleration = geometry_msgs.msg.Vector3()
+      if self.yaw is None:
+        self.yaw = 0.
+      if self.yaw_dot is None:
+        self.yaw_dot = 0.
+      if self.kx is None:
+        self.kx = [0.] * 3
+      if self.kv is None:
+        self.kv = [0.] * 3
+      if self.trajectory_id is None:
+        self.trajectory_id = 0
+      if self.trajectory_flag is None:
+        self.trajectory_flag = 0
+    else:
+      self.header = std_msgs.msg.Header()
+      self.position = geometry_msgs.msg.Point()
+      self.velocity = geometry_msgs.msg.Vector3()
+      self.acceleration = geometry_msgs.msg.Vector3()
+      self.yaw = 0.
+      self.yaw_dot = 0.
+      self.kx = [0.] * 3
+      self.kv = [0.] * 3
+      self.trajectory_id = 0
+      self.trajectory_flag = 0
+
+  def _get_types(self):
+    """
+    internal API method
+    """
+    return self._slot_types
+
+  def serialize(self, buff):
+    """
+    serialize message into buffer
+    :param buff: buffer, ``StringIO``
+    """
+    try:
+      _x = self
+      buff.write(_get_struct_3I().pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
+      _x = self.header.frame_id
+      length = len(_x)
+      if python3 or type(_x) == unicode:
+        _x = _x.encode('utf-8')
+        length = len(_x)
+      buff.write(struct.Struct('<I%ss'%length).pack(length, _x))
+      _x = self
+      buff.write(_get_struct_11d().pack(_x.position.x, _x.position.y, _x.position.z, _x.velocity.x, _x.velocity.y, _x.velocity.z, _x.acceleration.x, _x.acceleration.y, _x.acceleration.z, _x.yaw, _x.yaw_dot))
+      buff.write(_get_struct_3d().pack(*self.kx))
+      buff.write(_get_struct_3d().pack(*self.kv))
+      _x = self
+      buff.write(_get_struct_IB().pack(_x.trajectory_id, _x.trajectory_flag))
+    except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self)))))
+    except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self)))))
+
+  def deserialize(self, str):
+    """
+    unpack serialized message in str into this message instance
+    :param str: byte array of serialized message, ``str``
+    """
+    if python3:
+      codecs.lookup_error("rosmsg").msg_type = self._type
+    try:
+      if self.header is None:
+        self.header = std_msgs.msg.Header()
+      if self.position is None:
+        self.position = geometry_msgs.msg.Point()
+      if self.velocity is None:
+        self.velocity = geometry_msgs.msg.Vector3()
+      if self.acceleration is None:
+        self.acceleration = geometry_msgs.msg.Vector3()
+      end = 0
+      _x = self
+      start = end
+      end += 12
+      (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _get_struct_3I().unpack(str[start:end])
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      start = end
+      end += length
+      if python3:
+        self.header.frame_id = str[start:end].decode('utf-8', 'rosmsg')
+      else:
+        self.header.frame_id = str[start:end]
+      _x = self
+      start = end
+      end += 88
+      (_x.position.x, _x.position.y, _x.position.z, _x.velocity.x, _x.velocity.y, _x.velocity.z, _x.acceleration.x, _x.acceleration.y, _x.acceleration.z, _x.yaw, _x.yaw_dot,) = _get_struct_11d().unpack(str[start:end])
+      start = end
+      end += 24
+      self.kx = _get_struct_3d().unpack(str[start:end])
+      start = end
+      end += 24
+      self.kv = _get_struct_3d().unpack(str[start:end])
+      _x = self
+      start = end
+      end += 5
+      (_x.trajectory_id, _x.trajectory_flag,) = _get_struct_IB().unpack(str[start:end])
+      return self
+    except struct.error as e:
+      raise genpy.DeserializationError(e)  # most likely buffer underfill
+
+
+  def serialize_numpy(self, buff, numpy):
+    """
+    serialize message with numpy array types into buffer
+    :param buff: buffer, ``StringIO``
+    :param numpy: numpy python module
+    """
+    try:
+      _x = self
+      buff.write(_get_struct_3I().pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
+      _x = self.header.frame_id
+      length = len(_x)
+      if python3 or type(_x) == unicode:
+        _x = _x.encode('utf-8')
+        length = len(_x)
+      buff.write(struct.Struct('<I%ss'%length).pack(length, _x))
+      _x = self
+      buff.write(_get_struct_11d().pack(_x.position.x, _x.position.y, _x.position.z, _x.velocity.x, _x.velocity.y, _x.velocity.z, _x.acceleration.x, _x.acceleration.y, _x.acceleration.z, _x.yaw, _x.yaw_dot))
+      buff.write(self.kx.tostring())
+      buff.write(self.kv.tostring())
+      _x = self
+      buff.write(_get_struct_IB().pack(_x.trajectory_id, _x.trajectory_flag))
+    except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self)))))
+    except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(locals().get('_x', self)))))
+
+  def deserialize_numpy(self, str, numpy):
+    """
+    unpack serialized message in str into this message instance using numpy for array types
+    :param str: byte array of serialized message, ``str``
+    :param numpy: numpy python module
+    """
+    if python3:
+      codecs.lookup_error("rosmsg").msg_type = self._type
+    try:
+      if self.header is None:
+        self.header = std_msgs.msg.Header()
+      if self.position is None:
+        self.position = geometry_msgs.msg.Point()
+      if self.velocity is None:
+        self.velocity = geometry_msgs.msg.Vector3()
+      if self.acceleration is None:
+        self.acceleration = geometry_msgs.msg.Vector3()
+      end = 0
+      _x = self
+      start = end
+      end += 12
+      (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _get_struct_3I().unpack(str[start:end])
+      start = end
+      end += 4
+      (length,) = _struct_I.unpack(str[start:end])
+      start = end
+      end += length
+      if python3:
+        self.header.frame_id = str[start:end].decode('utf-8', 'rosmsg')
+      else:
+        self.header.frame_id = str[start:end]
+      _x = self
+      start = end
+      end += 88
+      (_x.position.x, _x.position.y, _x.position.z, _x.velocity.x, _x.velocity.y, _x.velocity.z, _x.acceleration.x, _x.acceleration.y, _x.acceleration.z, _x.yaw, _x.yaw_dot,) = _get_struct_11d().unpack(str[start:end])
+      start = end
+      end += 24
+      self.kx = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=3)
+      start = end
+      end += 24
+      self.kv = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=3)
+      _x = self
+      start = end
+      end += 5
+      (_x.trajectory_id, _x.trajectory_flag,) = _get_struct_IB().unpack(str[start:end])
+      return self
+    except struct.error as e:
+      raise genpy.DeserializationError(e)  # most likely buffer underfill
+
+_struct_I = genpy.struct_I
+def _get_struct_I():
+    global _struct_I
+    return _struct_I
+_struct_11d = None
+def _get_struct_11d():
+    global _struct_11d
+    if _struct_11d is None:
+        _struct_11d = struct.Struct("<11d")
+    return _struct_11d
+_struct_3I = None
+def _get_struct_3I():
+    global _struct_3I
+    if _struct_3I is None:
+        _struct_3I = struct.Struct("<3I")
+    return _struct_3I
+_struct_3d = None
+def _get_struct_3d():
+    global _struct_3d
+    if _struct_3d is None:
+        _struct_3d = struct.Struct("<3d")
+    return _struct_3d
+_struct_IB = None
+def _get_struct_IB():
+    global _struct_IB
+    if _struct_IB is None:
+        _struct_IB = struct.Struct("<IB")
+    return _struct_IB

flightpolicy/control_msg/__init__.py

@@ -0,0 +1 @@
+from ._PositionCommand import *

flightpolicy/yopo/primitive_utils.py

@@ -5,7 +5,7 @@ from scipy.spatial.transform import Rotation as R
 class LatticeParam():
     def __init__(self, cfg):
         self.vel_max = cfg["vel_max"]
-        segment_time = 2 * cfg["radio_range"] / self.vel_max
+        self.segment_time = 2 * cfg["radio_range"] / self.vel_max
         self.horizon_num = cfg["horizon_num"]
         self.vertical_num = cfg["vertical_num"]
         self.radio_num = cfg["radio_num"]
@@ -17,10 +17,10 @@ class LatticeParam():
         self.radio_range = cfg["radio_range"]
         self.vel_fov = cfg["vel_fov"]
         self.vel_prefile = cfg["vel_prefile"]
-        self.acc_max = self.vel_max / segment_time
+        self.acc_max = self.vel_max / self.segment_time
         print("---------------------")
         print("| max speed = ", round(self.vel_max, 1), " |")
-        print("| traj time = ", round(segment_time, 1), " |")
+        print("| traj time = ", round(self.segment_time, 1), " |")
         print("| max radio = ", round(2 * self.radio_range, 1), " |")
         print("---------------------")
 
@@ -95,6 +95,63 @@ class LatticePrimitive():
         return self.lattice_Rbp_list[id]
 
 
+class Poly5Solver:
+    def __init__(self, pos0, vel0, acc0, pos1, vel1, acc1, Tf):
+        """ 5-th order polynomial at each Axis """
+        State_Mat = np.array([pos0, vel0, acc0, pos1, vel1, acc1])
+        t = Tf
+        Coef_inv = np.array([[1, 0, 0, 0, 0, 0],
+                             [0, 1, 0, 0, 0, 0],
+                             [0, 0, 1 / 2, 0, 0, 0],
+                             [-10 / t ** 3, -6 / t ** 2, -3 / (2 * t), 10 / t ** 3, -4 / t ** 2, 1 / (2 * t)],
+                             [15 / t ** 4, 8 / t ** 3, 3 / (2 * t ** 2), -15 / t ** 4, 7 / t ** 3, -1 / t ** 2],
+                             [-6 / t ** 5, -3 / t ** 4, -1 / (2 * t ** 3), 6 / t ** 5, -3 / t ** 4, 1 / (2 * t ** 3)]])
+        self.A = np.dot(Coef_inv, State_Mat)
+
+    def get_snap(self, t):
+        """Return the scalar jerk at time t."""
+        return 24 * self.A[4] + 120 * self.A[5] * t
+
+    def get_jerk(self, t):
+        """Return the scalar jerk at time t."""
+        return 6 * self.A[3] + 24 * self.A[4] * t + 60 * self.A[5] * t * t
+
+    def get_acceleration(self, t):
+        """Return the scalar acceleration at time t."""
+        return 2 * self.A[2] + 6 * self.A[3] * t + 12 * self.A[4] * t * t + 20 * self.A[5] * t * t * t
+
+    def get_velocity(self, t):
+        """Return the scalar velocity at time t."""
+        return self.A[1] + 2 * self.A[2] * t + 3 * self.A[3] * t * t + 4 * self.A[4] * t * t * t + \
+            5 * self.A[5] * t * t * t * t
+
+    def get_position(self, t):
+        """Return the scalar position at time t."""
+        return self.A[0] + self.A[1] * t + self.A[2] * t * t + self.A[3] * t * t * t + self.A[4] * t * t * t * t + \
+            self.A[5] * t * t * t * t * t
+
+
+class Polys5Solver:
+    def __init__(self, pos0, vel0, acc0, pos1, vel1, acc1, Tf):
+        """ multiple 5-th order polynomials at each Axis (only used for visualization of multiple trajectories) """
+        N = len(pos1)
+        State_Mat = np.array([[pos0] * N, [vel0] * N, [acc0] * N, pos1, vel1, acc1])
+        t = Tf
+        Coef_inv = np.array([[1, 0, 0, 0, 0, 0],
+                             [0, 1, 0, 0, 0, 0],
+                             [0, 0, 1 / 2, 0, 0, 0],
+                             [-10 / t ** 3, -6 / t ** 2, -3 / (2 * t), 10 / t ** 3, -4 / t ** 2, 1 / (2 * t)],
+                             [15 / t ** 4, 8 / t ** 3, 3 / (2 * t ** 2), -15 / t ** 4, 7 / t ** 3, -1 / t ** 2],
+                             [-6 / t ** 5, -3 / t ** 4, -1 / (2 * t ** 3), 6 / t ** 5, -3 / t ** 4, 1 / (2 * t ** 3)]])
+        self.A = np.dot(Coef_inv, State_Mat)
+
+    def get_position(self, t):
+        """Return the position array at time t."""
+        t = np.atleast_1d(t)
+        result = (self.A[0][:, np.newaxis] + self.A[1][:, np.newaxis] * t + self.A[2][:, np.newaxis] * t ** 2 +
+                  self.A[3][:, np.newaxis] * t ** 3 + self.A[4][:, np.newaxis] * t ** 4 + self.A[5][:, np.newaxis] * t ** 5 )
+        return result.flatten()
+
 """
 From body to world
 p_w = Rwb * p_b + t_w
@@ -135,3 +192,71 @@ def rotate_inv(q_wb, pos_w):  # quat: wxzy
 def transform_inv(q_wb, tw, pos_w):
     pos_b = rotate_inv(q_wb, pos_w - tw)
     return pos_b
+
+
+def calculate_yaw(vel_dir, goal_dir, last_yaw_, dt, max_yaw_rate=0.3):
+    YAW_DOT_MAX_PER_SEC = max_yaw_rate * np.pi
+    # Normalize direction of velocity
+    vel_dir = vel_dir / (np.linalg.norm(vel_dir) + 1e-5)
+
+    # Direction of goal
+    goal_dist = np.linalg.norm(goal_dir)
+    goal_dir = goal_dir / (goal_dist + 1e-5)  # Prevent division by zero
+
+    # Desired direction
+    dir_des = vel_dir + goal_dir
+
+    # Temporary yaw calculation
+    yaw_temp = np.arctan2(dir_des[1], dir_des[0]) if goal_dist > 0.2 else last_yaw_
+    max_yaw_change = YAW_DOT_MAX_PER_SEC * dt
+
+    # Initialize yaw and yawdot
+    yaw = last_yaw_
+    yawdot = 0
+
+    # Logic for yaw adjustment
+    if yaw_temp - last_yaw_ > np.pi:
+        if yaw_temp - last_yaw_ - 2 * np.pi < -max_yaw_change:
+            yaw = last_yaw_ - max_yaw_change
+            if yaw < -np.pi:
+                yaw += 2 * np.pi
+            yawdot = -YAW_DOT_MAX_PER_SEC
+        else:
+            yaw = yaw_temp
+            if yaw - last_yaw_ > np.pi:
+                yawdot = -YAW_DOT_MAX_PER_SEC
+            else:
+                yawdot = (yaw_temp - last_yaw_) / dt
+    elif yaw_temp - last_yaw_ < -np.pi:
+        if yaw_temp - last_yaw_ + 2 * np.pi > max_yaw_change:
+            yaw = last_yaw_ + max_yaw_change
+            if yaw > np.pi:
+                yaw -= 2 * np.pi
+            yawdot = YAW_DOT_MAX_PER_SEC
+        else:
+            yaw = yaw_temp
+            if yaw - last_yaw_ < -np.pi:
+                yawdot = YAW_DOT_MAX_PER_SEC
+            else:
+                yawdot = (yaw_temp - last_yaw_) / dt
+    else:
+        if yaw_temp - last_yaw_ < -max_yaw_change:
+            yaw = last_yaw_ - max_yaw_change
+            if yaw < -np.pi:
+                yaw += 2 * np.pi
+            yawdot = -YAW_DOT_MAX_PER_SEC
+        elif yaw_temp - last_yaw_ > max_yaw_change:
+            yaw = last_yaw_ + max_yaw_change
+            if yaw > np.pi:
+                yaw -= 2 * np.pi
+            yawdot = YAW_DOT_MAX_PER_SEC
+        else:
+            yaw = yaw_temp
+            if yaw - last_yaw_ > np.pi:
+                yawdot = -YAW_DOT_MAX_PER_SEC
+            elif yaw - last_yaw_ < -np.pi:
+                yawdot = YAW_DOT_MAX_PER_SEC
+            else:
+                yawdot = (yaw_temp - last_yaw_) / dt
+
+    return yaw, yawdot