（4-4-02） 轨迹规划算法和优化：多无人机路径规划系统（2）

于 2024-04-14 16:18:18 发布
阅读量370 收藏 6
点赞数 1
分类专栏： 《运动控制算法--Python篇》 文章标签： 算法 无人机 python 路径规划 运动控制
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/asd343442/article/details/137747737
版权

4.10.3  刚体路径规划

刚体路径规划（Rigid Body Path Planning）是指在给定刚体（即不可变形的物体）的情况下，寻找从初始状态到目标状态的合适路径的过程。在机器人学、自动化、计算机图形学等领域，刚体路径规划是一个重要的问题。

（1）文件src/RigidBodyPlanners/fcl_checker.py定义了类Fcl_checker，用于使用FCL（Flexible Collision Library）检查两个三维网格模型（环境和机器人）之间的碰撞。它包括一个Fcl_mesh类，用于加载STL文件、创建FCL模型，并提供了检查碰撞和设置变换的方法。在__main__部分，使用了该类来检查给定机器人姿态是否与环境发生碰撞，并通过可视化展示环境和机器人的三维网格模型。

class PlannerSepCollision:
    def __init__(self, env_mesh_name, robot_mesh_name) -> None:
        self.time_sum = 0
        self.states_tried = 0
        # env_mesh_name and robot_mesh_name are of type "env-scene-hole.stl"
        try:
            env_mesh = "ros_ws/src/drone_path_planning/resources/stl/{}".format(
                env_mesh_name)
            robot_mesh = "ros_ws/src/drone_path_planning/resources/stl/{}".format(
                robot_mesh_name)
            self.checker = Fcl_checker(env_mesh, robot_mesh)
        except:
            print("cwd:", os.getcwd())
            env_mesh = r"/home/marios/thesis_ws/src/drone_path_planning/resources/stl/{}".format(
                env_mesh_name)
            robot_mesh = r"/home/marios/thesis_ws/src/drone_path_planning/resources/stl/{}".format(
                robot_mesh_name)
            self.checker = Fcl_checker(env_mesh, robot_mesh)
        self.space = ob.RealVectorStateSpace(4)
        # set lower and upper bounds
        self.set_bounds()
        self.ss = og.SimpleSetup(self.space)
        # set State Validity Checker function
        self.ss.setStateValidityChecker( ob.StateValidityCheckerFn(self.isStateValid))
        self.ss.getSpaceInformation().setStateValidityCheckingResolution(0.001)
        # set problem optimization objective
        self.set_optim_objective()

        print("Space Bounds High:", self.space.getBounds(
        ).high[0], self.space.getBounds().high[1], self.space.getBounds().high[2])
        print("Space Bounds Low:", self.space.getBounds(
        ).low[0], self.space.getBounds().low[1], self.space.getBounds().low[2])

    def set_optim_objective(self, objective_class=ob.MechanicalWorkOptimizationObjective):
        self.ss.setOptimizationObjective(
            objective_class(self.ss.getSpaceInformation()))

    def set_planner(self, planner_class=og.RRT):
        # choose planner
        planner = planner_class(self.ss.getSpaceInformation())
        self.ss.setPlanner(planner)
        self.ss.setup()

    def set_bounds(self):
        bounds = ob.RealVectorBounds(4)
        # set bounds for x, y, z , rotation
        bounds.low[0] = -2.2
        bounds.low[1] = 2.8
        bounds.low[2] = 0.5
        bounds.low[3] = -pi
        # set bounds for x, y, z, rotation
        bounds.high[0] = 2.2
        bounds.high[1] = 5.0
        bounds.high[2] = 2.5
        bounds.high[3] = pi
        # bounds.setLow(-10)
        # bounds.setHigh(10)
        self.space.setBounds(bounds)
        return bounds

    def save_path(self, file_name="path.txt"):
        # save the path
        print("Saving path to %s" % file_name)
        text_file = open(file_name, "w")
        n = text_file.write(self.path.printAsMatrix())
        text_file.close()

    def set_start_goal(self, start_pose: Pose, goal_pose: Pose, transform=False):
        # define start state
        start = ob.State(self.space)
        start[0] = start_pose.position.x
        start[1] = start_pose.position.y
        start[2] = start_pose.position.z
        start[3] = tf.transformations.euler_from_quaternion([start_pose.orientation.x, start_pose.orientation.y, start_pose.orientation.z, start_pose.orientation.w])[2]
        goal = ob.State(self.space)
        goal[0] = goal_pose.position.x
        goal[1] = goal_pose.position.y
        goal[2] = goal_pose.position.z
        goal[3] = tf.transformations.euler_from_quaternion([goal_pose.orientation.x, goal_pose.orientation.y, goal_pose.orientation.z, goal_pose.orientation.w])[2]
        print("start:", start)
        print("goal:", goal)
        self.ss.setStartAndGoalStates(start, goal)
        # return the start & goal states
        return start, goal

    def solve(self, timeout=15.0):
        print(f"Solving with timeout: {timeout} sec...")
        solved = self.ss.solve(timeout)
        if solved:
            print("Found solution...")
            # try to shorten the path
            self.ss.simplifySolution()
            # print the simplified path
            path = self.ss.getSolutionPath()
            path.interpolate(50)
            self.path = path
            self.save_path()
        else:
            print("No solution found")
        print("Tried {} states --> average time: {} msec".format(self.states_tried,self.time_sum / self.states_tried*1000))
        return solved

    def visualize_path(self, path_file="path.txt"):
        try:
            data = np.loadtxt(path_file)
        except Exception as e:
            print("No path file found")
        fig = plt.figure()
        ax = fig.gca(projection='3d')
        ax.plot(data[:, 3], data[:, 2], data[:, 1], '.-')
        # Load the STL files and add the vectors to the plot
        env_mesh = mesh.Mesh.from_file(env_mesh_name)
        ax.add_collection3d(mplot3d.art3d.Poly3DCollection(env_mesh.vectors))
        # Auto scale to the mesh size
        scale = env_mesh.points.flatten()
        ax.auto_scale_xyz(scale, scale, scale)
        # set axes limits
        ax.set_xlim3d(-2, 5, 2.5)
        ax.set_ylim3d(-2, 5, 2.5)
        ax.set_zlim3d(-2, 5, 2.5)
        ax.set_xlabel('X axis')
        ax.set_ylabel('Y axis')
        ax.set_zlabel('Z axis')
        plt.show()

    def isStateValid(self, state):
        t0 = rospy.get_time()
        pos = [state[0], state[1], state[2]]
        q = tf.transformations.quaternion_from_euler(0, 0, state[3])
        self.checker.set_robot_transform(pos, q)
        no_collision = not self.checker.check_collision()
        dt = rospy.get_time()-t0
        self.time_sum += dt
        self.states_tried += 1
        if (SHOW_VALID_STATES_CNTR and self.states_tried % 1000) == 0:
            print("Tried {} states --> average time: {} msec".format(self.states_tried,
                  self.time_sum / self.states_tried*1000), end="")
            print("\r", end="")
        return no_collision

def isBetween(x, min, max):
    return x >= min and x <= max

if __name__ == "__main__":
    # checker.visualize()
    env_mesh_name = "env-scene-hole.stl"
    robot_mesh_name = "robot-scene-triangle.stl"
    planner = PlannerSepCollision(env_mesh_name, robot_mesh_name)

    start_pos = [-4, -2, 2]
    goal_pos = [4, 2, 2]
    # start
    start_pose = PoseStamped()
    start_pose.pose.position = Point(start_pos[0], start_pos[1], start_pos[2])
    # start_pose.pose.orientation = Quaternion(-0.7071067811865475, 0, 0, 0.7071067811865476)
    start_pose.pose.orientation = Quaternion(0, 0, 0, 1)
    # goal
    goal_pose = PoseStamped()
    goal_pose.pose.position = Point(goal_pos[0], goal_pos[1], goal_pos[2])
    goal_pose.pose.orientation = Quaternion(0, 0, 0, 1)
    planner.set_start_goal(start_pose.pose, goal_pose.pose)
    planner.set_planner()
    solved = planner.solve(timeout=80.0)
    if solved:
        planner.visualize_path()

（2）文件src/RigidBodyPlanners/RB_planning_sep_coll_check.py实现了基于OMPL（Open Motion Planning Library）的路径规划工作，使用FCL（Flexible Collision Library）进行碰撞检测。通过定义PlannerSepCollision类初始化模拟环境和机器人的3D模型，设置状态空间和规划优化目标，然后执行路径规划工作，具体包括路径规划器的初始化、状态空间设置、碰撞检测、规划目标设定、路径求解以及路径可视化等功能。

class PlannerSepCollision:
    def __init__(self, env_mesh_name, robot_mesh_name) -> None:
        self.time_sum = 0
        self.states_tried = 0
        # env_mesh_name and robot_mesh_name are of type "env-scene-hole.stl"
        try:
            env_mesh = "ros_ws/src/drone_path_planning/resources/stl/{}".format(
                env_mesh_name)
            robot_mesh = "ros_ws/src/drone_path_planning/resources/stl/{}".format(
                robot_mesh_name)
            self.checker = Fcl_checker(env_mesh, robot_mesh)
        except:
            print("cwd:", os.getcwd())
            env_mesh = r"/home/marios/thesis_ws/src/drone_path_planning/resources/stl/{}".format(
                env_mesh_name)
            robot_mesh = r"/home/marios/thesis_ws/src/drone_path_planning/resources/stl/{}".format(
                robot_mesh_name)
            self.checker = Fcl_checker(env_mesh, robot_mesh)
        self.space = ob.RealVectorStateSpace(4)
        # set lower and upper bounds
        self.set_bounds()
        self.ss = og.SimpleSetup(self.space)
        # set State Validity Checker function
        self.ss.setStateValidityChecker( ob.StateValidityCheckerFn(self.isStateValid))
        self.ss.getSpaceInformation().setStateValidityCheckingResolution(0.001)
        # set problem optimization objective
        self.set_optim_objective()

        print("Space Bounds High:", self.space.getBounds(
        ).high[0], self.space.getBounds().high[1], self.space.getBounds().high[2])
        print("Space Bounds Low:", self.space.getBounds(
        ).low[0], self.space.getBounds().low[1], self.space.getBounds().low[2])

    def set_optim_objective(self, objective_class=ob.MechanicalWorkOptimizationObjective):
        self.ss.setOptimizationObjective(
            objective_class(self.ss.getSpaceInformation()))

    def set_planner(self, planner_class=og.RRT):
        # choose planner
        planner = planner_class(self.ss.getSpaceInformation())
        self.ss.setPlanner(planner)
        self.ss.setup()

    def set_bounds(self):
        bounds = ob.RealVectorBounds(4)
        # set bounds for x, y, z , rotation
        bounds.low[0] = -2.2
        bounds.low[1] = 2.8
        bounds.low[2] = 0.5
        bounds.low[3] = -pi
        # set bounds for x, y, z, rotation
        bounds.high[0] = 2.2
        bounds.high[1] = 5.0
        bounds.high[2] = 2.5
        bounds.high[3] = pi
        # bounds.setLow(-10)
        # bounds.setHigh(10)
        self.space.setBounds(bounds)
        return bounds

    def save_path(self, file_name="path.txt"):
        # save the path
        print("Saving path to %s" % file_name)
        text_file = open(file_name, "w")
        n = text_file.write(self.path.printAsMatrix())
        text_file.close()

    def set_start_goal(self, start_pose: Pose, goal_pose: Pose, transform=False):
        # define start state
        start = ob.State(self.space)
        start[0] = start_pose.position.x
        start[1] = start_pose.position.y
        start[2] = start_pose.position.z
        start[3] = tf.transformations.euler_from_quaternion([start_pose.orientation.x, start_pose.orientation.y, start_pose.orientation.z, start_pose.orientation.w])[2]
        goal = ob.State(self.space)
        goal[0] = goal_pose.position.x
        goal[1] = goal_pose.position.y
        goal[2] = goal_pose.position.z
        goal[3] = tf.transformations.euler_from_quaternion([goal_pose.orientation.x, goal_pose.orientation.y, goal_pose.orientation.z, goal_pose.orientation.w])[2]
        print("start:", start)
        print("goal:", goal)
        self.ss.setStartAndGoalStates(start, goal)
        # return the start & goal states
        return start, goal

    def solve(self, timeout=15.0):
        print(f"Solving with timeout: {timeout} sec...")
        solved = self.ss.solve(timeout)
        if solved:
            print("Found solution...")
            # try to shorten the path
            self.ss.simplifySolution()
            # print the simplified path
            path = self.ss.getSolutionPath()
            path.interpolate(50)
            self.path = path
            self.save_path()
        else:
            print("No solution found")
        print("Tried {} states --> average time: {} msec".format(self.states_tried,self.time_sum / self.states_tried*1000))
        return solved

    def visualize_path(self, path_file="path.txt"):
        try:
            data = np.loadtxt(path_file)
        except Exception as e:
            print("No path file found")
        fig = plt.figure()
        ax = fig.gca(projection='3d')
        ax.plot(data[:, 3], data[:, 2], data[:, 1], '.-')
        # Load the STL files and add the vectors to the plot
        env_mesh = mesh.Mesh.from_file(env_mesh_name)
        ax.add_collection3d(mplot3d.art3d.Poly3DCollection(env_mesh.vectors))
        # Auto scale to the mesh size
        scale = env_mesh.points.flatten()
        ax.auto_scale_xyz(scale, scale, scale)
        # set axes limits
        ax.set_xlim3d(-2, 5, 2.5)
        ax.set_ylim3d(-2, 5, 2.5)
        ax.set_zlim3d(-2, 5, 2.5)
        ax.set_xlabel('X axis')
        ax.set_ylabel('Y axis')
        ax.set_zlabel('Z axis')
        plt.show()

    def isStateValid(self, state):
        t0 = rospy.get_time()
        pos = [state[0], state[1], state[2]]
        q = tf.transformations.quaternion_from_euler(0, 0, state[3])
        self.checker.set_robot_transform(pos, q)
        no_collision = not self.checker.check_collision()
        dt = rospy.get_time()-t0
        self.time_sum += dt
        self.states_tried += 1
        if (SHOW_VALID_STATES_CNTR and self.states_tried % 1000) == 0:
            print("Tried {} states --> average time: {} msec".format(self.states_tried,
                  self.time_sum / self.states_tried*1000), end="")
            print("\r", end="")
        return no_collision

def isBetween(x, min, max):
    return x >= min and x <= max

if __name__ == "__main__":
    # checker.visualize()
    env_mesh_name = "env-scene-hole.stl"
    robot_mesh_name = "robot-scene-triangle.stl"
    planner = PlannerSepCollision(env_mesh_name, robot_mesh_name)

    start_pos = [-4, -2, 2]
    goal_pos = [4, 2, 2]
    # start
    start_pose = PoseStamped()
    start_pose.pose.position = Point(start_pos[0], start_pos[1], start_pos[2])
    # start_pose.pose.orientation = Quaternion(-0.7071067811865475, 0, 0, 0.7071067811865476)
    start_pose.pose.orientation = Quaternion(0, 0, 0, 1)
    # goal
    goal_pose = PoseStamped()
    goal_pose.pose.position = Point(goal_pos[0], goal_pos[1], goal_pos[2])
    goal_pose.pose.orientation = Quaternion(0, 0, 0, 1)
    planner.set_start_goal(start_pose.pose, goal_pose.pose)
    planner.set_planner()
    solved = planner.solve(timeout=80.0)
    if solved:
        planner.visualize_path()

未完待续

码农三叔
关注
  • 1
    点赞
  • 踩
  • 6
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
专栏目录
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

码农三叔

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值