示例

设置

生成规划数据

本示例展示了如何为机器人单元生成规划数据，这是在启动运动规划器之前必须执行的操作。

 1#include <Zivid/Motion/Application.h>
 2#include <Zivid/Motion/Generation.h>
 3
 4#include <iostream>
 5
 6namespace
 7{
 8    void printException(const std::exception &e, const int level = 0)
 9    {
10        std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
11        try
12        {
13            std::rethrow_if_nested(e);
14        }
15        catch(const std::exception &nestedException)
16        {
17            printException(nestedException, level + 1);
18        }
19        catch(...)
20        {}
21    }
22} // namespace
23
24int main()
25{
26    try
27    {
28        const Zivid::Motion::Application app;
29
30        // Specify which cell you want to generate data for. This is referenced later
31        // when using the data to instantiate a planner.
32        constexpr auto cellName = "demo_cell";
33
34        // Specify which profile you want to generate data for (testing or production)
35        constexpr auto profile = Zivid::Motion::Profile::testing;
36
37        std::cout << "Generating data for " << cellName << "...\n";
38        Zivid::Motion::generate(
39            app,
40            Zivid::Motion::PlannerSettings{ cellName, profile },
41            [](const double progress, const std::string &step) {
42                std::cout << "Generation progress on step " << step << ": " << progress << "% complete\n";
43            });
44        std::cout << "Generation complete. Results stored on drive under the specified cell name.\n";
45    }
46    catch(const std::exception &exception)
47    {
48        printException(exception);
49        return EXIT_FAILURE;
50    }
51    return EXIT_SUCCESS;
52}

 1from zividmotion import Application, PlannerSettings, Profile, generate
 2
 3
 4def _main() -> None:
 5    app = Application()
 6
 7    # Specify which cell you want to generate data for. This is referenced later
 8    # when using the data to instantiate a planner.
 9    cell_name = "demo_cell"
10
11    # Specify which profile you want to generate data for (testing or production)
12    profile = Profile.testing
13
14    print(f"Generating {profile} data for {cell_name}...")
15    generate(
16        app,
17        PlannerSettings(cell_name, profile),
18        progress_callback=lambda progress, step: print(f"Generation progress on step {step}: {progress:.2f}% complete"),
19    )
20    print("Generation complete. Results stored on drive under the specified cell name.")
21
22
23if __name__ == "__main__":
24    _main()

规划器软件包设置

本示例展示了如何将运行运动规划器所需的所有文件打包到一个 zip 压缩包中，该压缩包可以轻松分发并在其他机器上解压缩。

在已完成规划器设置的工作站上运行以下代码片段：

 1#include <Zivid/Motion/Application.h>
 2#include <Zivid/Motion/Packaging.h>
 3
 4#include <filesystem>
 5#include <iostream>
 6
 7namespace
 8{
 9    void printException(const std::exception &e, const int level = 0)
10    {
11        std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
12        try
13        {
14            std::rethrow_if_nested(e);
15        }
16        catch(const std::exception &nestedException)
17        {
18            printException(nestedException, level + 1);
19        }
20        catch(...)
21        {}
22    }
23} // namespace
24
25int main()
26{
27    try
28    {
29        const Zivid::Motion::Application app;
30        const std::filesystem::path outputPath = "demo_cell.zip";
31
32        // In this example, we package not only the configuration files for the cell, but also the generated data
33        // for the testing setup.
34        std::cout << "Packaging data...\n";
35        Zivid::Motion::packageCell(app, "demo_cell", outputPath, { Zivid::Motion::Profile::testing });
36
37        std::cout << "Data package stored at: " << std::filesystem::canonical(outputPath) << "\n";
38    }
39    catch(const std::exception &exception)
40    {
41        printException(exception);
42        return EXIT_FAILURE;
43    }
44    return EXIT_SUCCESS;
45}

 1from pathlib import Path
 2
 3from zividmotion import Application, Profile, package_cell
 4
 5
 6def _main() -> None:
 7    app = Application()
 8
 9    output_path = Path("demo_cell.zip")
10
11    # In this example, we package not only the configuration files for the cell, but also the generated data
12    # for the testing setup.
13    print("Packaging data...")
14    package_cell(app, cell_name="demo_cell", output_path=output_path, include_generated_data=[Profile.testing])
15    print(f"Data package stored at: {output_path.resolve()}")
16
17
18if __name__ == "__main__":
19    _main()

收到压缩包的用户随后可以运行以下代码片段来解压安装程序：

 1#include <Zivid/Motion/Application.h>
 2#include <Zivid/Motion/Packaging.h>
 3
 4#include <filesystem>
 5#include <iostream>
 6
 7namespace
 8{
 9    void printException(const std::exception &e, const int level = 0)
10    {
11        std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
12        try
13        {
14            std::rethrow_if_nested(e);
15        }
16        catch(const std::exception &nestedException)
17        {
18            printException(nestedException, level + 1);
19        }
20        catch(...)
21        {}
22    }
23} // namespace
24
25int main()
26{
27    try
28    {
29        const Zivid::Motion::Application app;
30        const std::filesystem::path packagePath = "demo_cell.zip";
31
32        std::cout << "Installing package from " << std::filesystem::canonical(packagePath) << "...\n";
33        Zivid::Motion::installPackage(app, packagePath);
34
35        std::cout << "Package installed.\n";
36    }
37    catch(const std::exception &exception)
38    {
39        printException(exception);
40        return EXIT_FAILURE;
41    }
42    return EXIT_SUCCESS;
43}

 1from pathlib import Path
 2
 3from zividmotion import Application, install_package
 4
 5
 6def _main() -> None:
 7    app = Application()
 8
 9    package_path = Path("demo_cell.zip")
10
11    print(f"Installing package from {package_path.resolve()}...")
12    install_package(app, package_path)
13
14    print("Package installed.")
15
16
17if __name__ == "__main__":
18    _main()

可视化工作站

本示例展示了如何为已配置的机器人工作单元打开 3D 可视化窗口。在生成或运行运动规划器之前，这有助于检查您的工作单元设置。

 1#include <Zivid/Motion/Application.h>
 2#include <Zivid/Motion/Visualizer.h>
 3
 4#include <iostream>
 5
 6namespace
 7{
 8    void printException(const std::exception &e, const int level = 0)
 9    {
10        std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
11        try
12        {
13            std::rethrow_if_nested(e);
14        }
15        catch(const std::exception &nestedException)
16        {
17            printException(nestedException, level + 1);
18        }
19        catch(...)
20        {}
21    }
22} // namespace
23
24int main()
25{
26    try
27    {
28        const Zivid::Motion::Application app;
29
30        // Specify the cell you want to visualize.
31        constexpr auto cellName = "demo_cell";
32
33        std::cout << "Opening visualizer for cell: " << cellName << "\n";
34        std::cout << "Close the window to exit.\n";
35
36        auto visualizer = Zivid::Motion::Visualizer::viewCell(app, cellName);
37        visualizer.wait();
38    }
39    catch(const std::exception &exception)
40    {
41        printException(exception);
42        return EXIT_FAILURE;
43    }
44    return EXIT_SUCCESS;
45}

 1from zividmotion import Application, Visualizer
 2
 3
 4def _main() -> None:
 5    app = Application()
 6
 7    # Specify the cell you want to visualize.
 8    cell_name = "demo_cell"
 9
10    print(f"Opening visualizer for cell: {cell_name}")
11    print("Close the window to exit.")
12
13    visualizer = Visualizer.view_cell(app, cell_name)
14    visualizer.wait()
15
16
17if __name__ == "__main__":
18    _main()

规划器

基本路径调用

关节目标

 1#include <Zivid/Motion/Application.h>
 2#include <Zivid/Motion/Planner.h>
 3#include <Zivid/Motion/Visualizer.h>
 4
 5#include <iostream>
 6#include <stdexcept>
 7
 8namespace
 9{
10    constexpr auto useVisualizer = true;
11
12    void printException(const std::exception &e, const int level = 0)
13    {
14        std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
15        try
16        {
17            std::rethrow_if_nested(e);
18        }
19        catch(const std::exception &nestedException)
20        {
21            printException(nestedException, level + 1);
22        }
23        catch(...)
24        {}
25    }
26
27    Zivid::Motion::Planner startPlanner(const Zivid::Motion::Application &app)
28    {
29        return app.createPlanner(
30            Zivid::Motion::PlannerSettings{
31                "demo_cell",
32                Zivid::Motion::Profile::testing,
33            });
34    }
35} // namespace
36
37int main()
38{
39    try
40    {
41        const Zivid::Motion::Application app;
42
43        std::cout << "Starting planner\n";
44        auto planner = startPlanner(app);
45        auto visualizer =
46            useVisualizer ? std::optional{ Zivid::Motion::Visualizer::viewPlanner(planner) } : std::nullopt;
47
48        const Zivid::Motion::Configuration startConfiguration = { 0.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
49        const Zivid::Motion::Configuration goalConfiguration = { 1.57f, 0.f, 0.f, 0.f, 1.57f, 0.f };
50
51        Zivid::Motion::PathRequest pathRequest{};
52        pathRequest.goalConfigurations = { goalConfiguration };
53        pathRequest.description = "Path with joint goal";
54        const auto result = planner.path(Zivid::Motion::InitialState{ startConfiguration }, pathRequest);
55        if(!result)
56        {
57            throw std::runtime_error("Planning failed with result: " + result.toString());
58        }
59
60        std::cout << "Path result:\n" << result << "\n";
61
62        if(useVisualizer)
63        {
64            std::cout << "Close the window to exit.\n";
65            visualizer->wait();
66        }
67    }
68    catch(const std::exception &exception)
69    {
70        printException(exception);
71        std::cout << "Press enter to exit." << std::endl;
72        std::cin.get();
73        return EXIT_FAILURE;
74    }
75    return EXIT_SUCCESS;
76}

 1from typing import Optional
 2
 3from zividmotion import Application, InitialState, PathRequest, Planner, PlannerSettings, Profile, Visualizer
 4
 5USE_VISUALIZER = True
 6
 7
 8def _start_planner(app: Application) -> Planner:
 9    planner_settings = PlannerSettings(cell_name="demo_cell", profile=Profile.testing)
10    return app.create_planner(planner_settings)
11
12
13def _main() -> None:
14    app = Application()
15
16    print("Starting planner")
17    planner = _start_planner(app)
18    visualizer: Optional[Visualizer] = Visualizer.view_planner(planner) if USE_VISUALIZER else None
19
20    start_configuration = [0.0, 0.0, 0.0, 0.0, 1.57, 0.0]
21    goal_configuration = [1.57, 0.0, 0.0, 0.0, 1.57, 0.0]
22
23    result = planner.path(
24        InitialState(start_configuration),
25        PathRequest(
26            goal_configurations=[goal_configuration],
27            description="Path with joint goal",
28        ),
29    )
30    if not result:
31        raise RuntimeError(f"Planning failed with error: {result.error}")
32
33    print(f"Path result: \n{result}\n")
34
35    if visualizer is not None:
36        print("Close the window to exit.")
37        visualizer.wait()
38
39
40if __name__ == "__main__":
41    _main()

位姿目标

 1#include <Zivid/Motion/Application.h>
 2#include <Zivid/Motion/Planner.h>
 3#include <Zivid/Motion/Visualizer.h>
 4
 5#include <iostream>
 6
 7namespace
 8{
 9    constexpr auto useVisualizer = true;
10
11    void printException(const std::exception &e, const int level = 0)
12    {
13        std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
14        try
15        {
16            std::rethrow_if_nested(e);
17        }
18        catch(const std::exception &nestedException)
19        {
20            printException(nestedException, level + 1);
21        }
22        catch(...)
23        {}
24    }
25
26    Zivid::Motion::Planner startPlanner(const Zivid::Motion::Application &app)
27    {
28        return app.createPlanner(
29            Zivid::Motion::PlannerSettings{
30                "demo_cell",
31                Zivid::Motion::Profile::testing,
32            });
33    }
34} // namespace
35
36int main()
37{
38    try
39    {
40        const Zivid::Motion::Application app;
41
42        std::cout << "Starting planner\n";
43        auto planner = startPlanner(app);
44        auto visualizer =
45            useVisualizer ? std::optional{ Zivid::Motion::Visualizer::viewPlanner(planner) } : std::nullopt;
46
47        const Zivid::Motion::Configuration startConfiguration = { 0.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
48        constexpr Zivid::Motion::Pose poseGoal = { {
49            { 0.f, -1.f, 0.f, 0.f },
50            { -1.f, 0.f, 0.f, 1.45f },
51            { 0.f, 0.f, -1.f, 1.63f },
52            { 0.f, 0.f, 0.f, 1.f },
53        } };
54
55        const auto poseGoalConfiguration = planner.computeInverseKinematics(poseGoal, startConfiguration);
56        if(!poseGoalConfiguration.has_value())
57        {
58            throw std::runtime_error("No valid inverse kinematics solution found");
59        }
60
61        Zivid::Motion::PathRequest pathRequest{};
62        pathRequest.goalConfigurations = { poseGoalConfiguration.value() };
63        pathRequest.description = "Path with goal from pose";
64        const auto result = planner.path(Zivid::Motion::InitialState{ startConfiguration }, pathRequest);
65        if(!result)
66        {
67            throw std::runtime_error("Planning failed with result: " + result.toString());
68        }
69
70        std::cout << "Path result:\n" << result << "\n";
71
72        if(useVisualizer)
73        {
74            std::cout << "Close the window to exit.\n";
75            visualizer->wait();
76        }
77    }
78    catch(const std::exception &exception)
79    {
80        printException(exception);
81        std::cout << "Press enter to exit." << std::endl;
82        std::cin.get();
83        return EXIT_FAILURE;
84    }
85    return EXIT_SUCCESS;
86}

 1from typing import Optional
 2
 3import numpy as np
 4from zividmotion import Application, InitialState, PathRequest, Planner, PlannerSettings, Profile, Visualizer
 5
 6USE_VISUALIZER = True
 7
 8
 9def _start_planner(app: Application) -> Planner:
10    planner_settings = PlannerSettings(cell_name="demo_cell", profile=Profile.testing)
11    return app.create_planner(planner_settings)
12
13
14def _main() -> None:
15    app = Application()
16
17    print("Starting planner")
18    planner = _start_planner(app)
19    visualizer: Optional[Visualizer] = Visualizer.view_planner(planner) if USE_VISUALIZER else None
20
21    start_configuration = [0.0, 0.0, 0.0, 0.0, 1.57, 0.0]
22    pose_goal = np.array(
23        [
24            [0.0, -1.0, 0.0, 0.0],
25            [-1.0, 0.0, 0.0, 1.45],
26            [0.0, 0.0, -1.0, 1.63],
27            [0.0, 0.0, 0.0, 1.0],
28        ]
29    )
30
31    pose_goal_configuration = planner.compute_inverse_kinematics(
32        pose=pose_goal,
33        reference_configuration=start_configuration,
34    )
35    if pose_goal_configuration is None:
36        raise RuntimeError("No valid inverse kinematics solution found")
37
38    result = planner.path(
39        InitialState(start_configuration),
40        PathRequest(
41            goal_configurations=[pose_goal_configuration],
42            description="Path with goal from pose",
43        ),
44    )
45    if not result:
46        raise RuntimeError(f"Planning failed with error: {result.error}")
47
48    print(f"Path result: \n{result}\n")
49
50    if visualizer is not None:
51        print("Close the window to exit.")
52        visualizer.wait()
53
54
55if __name__ == "__main__":
56    _main()

多重目标和优先级排序

 1#include <Zivid/Motion/Application.h>
 2#include <Zivid/Motion/Planner.h>
 3#include <Zivid/Motion/Visualizer.h>
 4
 5#include <cassert>
 6#include <iostream>
 7
 8namespace
 9{
10    constexpr auto useVisualizer = true;
11
12    void printException(const std::exception &e, const int level = 0)
13    {
14        std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
15        try
16        {
17            std::rethrow_if_nested(e);
18        }
19        catch(const std::exception &nestedException)
20        {
21            printException(nestedException, level + 1);
22        }
23        catch(...)
24        {}
25    }
26
27    Zivid::Motion::Planner startPlanner(const Zivid::Motion::Application &app)
28    {
29        return app.createPlanner(
30            Zivid::Motion::PlannerSettings{
31                "demo_cell",
32                Zivid::Motion::Profile::testing,
33            });
34    }
35} // namespace
36
37int main()
38{
39    try
40    {
41        const Zivid::Motion::Application app;
42
43        std::cout << "Starting planner\n";
44        auto planner = startPlanner(app);
45        auto visualizer =
46            useVisualizer ? std::optional{ Zivid::Motion::Visualizer::viewPlanner(planner) } : std::nullopt;
47
48        const Zivid::Motion::Configuration startConfiguration = { 0.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
49        const Zivid::Motion::InitialState initialState{ startConfiguration };
50
51        const Zivid::Motion::Configuration goalFarAway = { 3.14f, 0.f, 0.f, 0.f, 1.57f, 0.f };
52        const Zivid::Motion::Configuration goalCloser = { 1.57f, 0.1f, -0.1f, 0.f, 1.57f, 0.f };
53        const Zivid::Motion::Configuration goalClosest = { 1.57f, 0.f, 0.f, 0.f, 1.57f, 0.f };
54        const std::vector<Zivid::Motion::Configuration> goals = { goalFarAway, goalCloser, goalClosest };
55
56        Zivid::Motion::PathRequest shortestPathRequest{};
57        shortestPathRequest.goalConfigurations = goals;
58        shortestPathRequest.description = "Multiple goals - Default (ShortestPath) prioritization";
59        const auto resultShortestPath = planner.path(initialState, shortestPathRequest);
60        if(!resultShortestPath)
61        {
62            throw std::runtime_error("Planning with ShortestPath failed with result: " + resultShortestPath.toString());
63        }
64        assert(resultShortestPath.selectedGoalIdx == 2u);
65        std::cout << "ShortestPath selected goal index: " << resultShortestPath.selectedGoalIdx.value() << "\n";
66
67        Zivid::Motion::PathRequest listOrderRequest{};
68        listOrderRequest.goalPrioritizationMethod = Zivid::Motion::PathRequest::GoalPrioritizationMethod::listOrder;
69        listOrderRequest.goalConfigurations = goals;
70        listOrderRequest.description = "Multiple goals - ListOrder prioritization";
71        const auto resultListOrder = planner.path(initialState, listOrderRequest);
72        if(!resultListOrder)
73        {
74            throw std::runtime_error("Planning with ListOrder failed with result: " + resultListOrder.toString());
75        }
76        assert(resultListOrder.selectedGoalIdx == 0u);
77        std::cout << "ListOrder selected goal index: " << resultListOrder.selectedGoalIdx.value() << "\n";
78
79        if(useVisualizer)
80        {
81            std::cout << "Close the window to exit.\n";
82            visualizer->wait();
83        }
84    }
85    catch(const std::exception &exception)
86    {
87        printException(exception);
88        std::cout << "Press enter to exit." << std::endl;
89        std::cin.get();
90        return EXIT_FAILURE;
91    }
92    return EXIT_SUCCESS;
93}

 1from typing import Optional
 2
 3from zividmotion import Application, InitialState, PathRequest, Planner, PlannerSettings, Profile, Visualizer
 4
 5USE_VISUALIZER = True
 6
 7
 8def _start_planner(app: Application) -> Planner:
 9    planner_settings = PlannerSettings(cell_name="demo_cell", profile=Profile.testing)
10    return app.create_planner(planner_settings)
11
12
13def _main() -> None:
14    app = Application()
15
16    print("Starting planner")
17    planner = _start_planner(app)
18    visualizer: Optional[Visualizer] = Visualizer.view_planner(planner) if USE_VISUALIZER else None
19
20    start_configuration = [0.0, 0.0, 0.0, 0.0, 1.57, 0.0]
21    initial_state = InitialState(start_configuration)
22
23    goal_far_away = [3.14, 0.0, 0.0, 0.0, 1.57, 0.0]
24    goal_closer = [1.57, 0.1, -0.1, 0.0, 1.57, 0.0]
25    goal_closest = [1.57, 0.0, 0.0, 0.0, 1.57, 0.0]
26    goals = [goal_far_away, goal_closer, goal_closest]
27
28    result_shortest_path = planner.path(
29        initial_state,
30        PathRequest(
31            goal_configurations=goals,
32            description="Multiple goals - Default (ShortestPath) prioritization",
33        ),
34    )
35    if not result_shortest_path:
36        raise RuntimeError(f"Planning with ShortestPath failed with error: {result_shortest_path.error}")
37    assert result_shortest_path.selected_goal_idx == 2
38    print(f"ShortestPath selected goal index: {result_shortest_path.selected_goal_idx}")
39
40    result_list_order = planner.path(
41        initial_state,
42        PathRequest(
43            goal_prioritization_method=PathRequest.GoalPrioritizationMethod.listOrder,
44            goal_configurations=goals,
45            description="Multiple goals - ListOrder prioritization",
46        ),
47    )
48    if not result_list_order:
49        raise RuntimeError(f"Planning with ListOrder failed with error: {result_list_order.error}")
50    assert result_list_order.selected_goal_idx == 0
51    print(f"ListOrder selected goal index: {result_list_order.selected_goal_idx}")
52
53    if visualizer is not None:
54        print("Close the window to exit.")
55        visualizer.wait()
56
57
58if __name__ == "__main__":
59    _main()

设置运行时障碍物

简单障碍物几何体

本示例展示了如何向运动规划器的动态环境模型中添加简单的障碍物（例如长方体）。

  1#include <Zivid/Motion/Application.h>
  2#include <Zivid/Motion/Planner.h>
  3#include <Zivid/Motion/Visualizer.h>
  4
  5#include <cassert>
  6#include <iostream>
  7
  8namespace
  9{
 10    constexpr auto useVisualizer = true;
 11
 12    void printException(const std::exception &e, const int level = 0)
 13    {
 14        std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
 15        try
 16        {
 17            std::rethrow_if_nested(e);
 18        }
 19        catch(const std::exception &nestedException)
 20        {
 21            printException(nestedException, level + 1);
 22        }
 23        catch(...)
 24        {}
 25    }
 26
 27    Zivid::Motion::Planner startPlanner(const Zivid::Motion::Application &app)
 28    {
 29        return app.createPlanner(
 30            Zivid::Motion::PlannerSettings{
 31                "demo_cell",
 32                Zivid::Motion::Profile::testing,
 33            });
 34    }
 35
 36    Zivid::Motion::Triangles createBoxMesh(
 37        const Zivid::Motion::PointXYZ &center,
 38        const Zivid::Motion::VectorXYZ &dimensions)
 39    {
 40        const float hx = dimensions.x / 2.f;
 41        const float hy = dimensions.y / 2.f;
 42        const float hz = dimensions.z / 2.f;
 43
 44        // 8 vertices of the box
 45        const Zivid::Motion::PointXYZ v000{ center.x - hx, center.y - hy, center.z - hz };
 46        const Zivid::Motion::PointXYZ v100{ center.x + hx, center.y - hy, center.z - hz };
 47        const Zivid::Motion::PointXYZ v010{ center.x - hx, center.y + hy, center.z - hz };
 48        const Zivid::Motion::PointXYZ v110{ center.x + hx, center.y + hy, center.z - hz };
 49        const Zivid::Motion::PointXYZ v001{ center.x - hx, center.y - hy, center.z + hz };
 50        const Zivid::Motion::PointXYZ v101{ center.x + hx, center.y - hy, center.z + hz };
 51        const Zivid::Motion::PointXYZ v011{ center.x - hx, center.y + hy, center.z + hz };
 52        const Zivid::Motion::PointXYZ v111{ center.x + hx, center.y + hy, center.z + hz };
 53
 54        // 6 faces × 2 triangles each = 12 triangles
 55        return {
 56            // -Z face
 57            { v000, v100, v110 },
 58            { v000, v110, v010 },
 59            // +Z face
 60            { v001, v111, v101 },
 61            { v001, v011, v111 },
 62            // -Y face
 63            { v000, v101, v100 },
 64            { v000, v001, v101 },
 65            // +Y face
 66            { v010, v110, v111 },
 67            { v010, v111, v011 },
 68            // -X face
 69            { v000, v010, v011 },
 70            { v000, v011, v001 },
 71            // +X face
 72            { v100, v101, v111 },
 73            { v100, v111, v110 },
 74        };
 75    }
 76} // namespace
 77
 78int main()
 79{
 80    try
 81    {
 82        const Zivid::Motion::Application app;
 83
 84        std::cout << "Starting planner\n";
 85        auto planner = startPlanner(app);
 86        auto visualizer =
 87            useVisualizer ? std::optional{ Zivid::Motion::Visualizer::viewPlanner(planner) } : std::nullopt;
 88
 89        const Zivid::Motion::Configuration startConfiguration = { 0.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
 90        const Zivid::Motion::Configuration goalConfiguration = { 1.57f, 0.f, 0.f, 0.f, 1.57f, 0.f };
 91        const Zivid::Motion::InitialState initialState{ startConfiguration };
 92
 93        Zivid::Motion::PathRequest withoutObstacleRequest{};
 94        withoutObstacleRequest.goalConfigurations = { goalConfiguration };
 95        withoutObstacleRequest.description = "Without obstacle";
 96        const auto resultWithoutObstacle = planner.path(initialState, withoutObstacleRequest);
 97        if(!resultWithoutObstacle)
 98        {
 99            throw std::runtime_error(
100                "Planning before obstacle is set failed with result: " + resultWithoutObstacle.toString());
101        }
102        assert(resultWithoutObstacle.path().size() == 1);
103        std::cout << "Path before obstacle is set: " << resultWithoutObstacle.path().size() << " waypoint\n";
104
105        // Set an obstacle that obstructs the direct path
106        const auto boxTriangles = createBoxMesh({ 1.1f, 1.0f, 1.6f }, { 0.2f, 0.2f, 0.2f });
107        planner.setObstacles({ Zivid::Motion::Obstacle::fromMesh("box_obstacle", boxTriangles) });
108
109        Zivid::Motion::PathRequest withObstacleRequest{};
110        withObstacleRequest.goalConfigurations = { goalConfiguration };
111        withObstacleRequest.description = "With obstacle";
112        const auto resultWithObstacle = planner.path(initialState, withObstacleRequest);
113        if(!resultWithObstacle)
114        {
115            throw std::runtime_error(
116                "Planning after obstacle is set failed with result: " + resultWithObstacle.toString());
117        }
118        assert(resultWithObstacle.path().size() > 1);
119        std::cout << "Path after obstacle is set: " << resultWithObstacle.path().size() << " waypoints\n";
120
121        if(useVisualizer)
122        {
123            std::cout << "Close the window to exit.\n";
124            visualizer->wait();
125        }
126    }
127    catch(const std::exception &exception)
128    {
129        printException(exception);
130        std::cout << "Press enter to exit." << std::endl;
131        std::cin.get();
132        return EXIT_FAILURE;
133    }
134    return EXIT_SUCCESS;
135}

使用以下命令安装其他依赖项：

pip install trimesh

 1from typing import Optional
 2
 3import trimesh
 4from zividmotion import Application, InitialState, Obstacle, PathRequest, Planner, PlannerSettings, Profile, Visualizer
 5
 6USE_VISUALIZER = True
 7
 8
 9def _start_planner(app: Application) -> Planner:
10    planner_settings = PlannerSettings(cell_name="demo_cell", profile=Profile.testing)
11    return app.create_planner(planner_settings)
12
13
14def _create_box_mesh(center_point, size):
15    box = trimesh.creation.box(extents=size)
16    box.apply_translation(center_point)
17    return box.triangles.tolist()
18
19
20def _main() -> None:
21    app = Application()
22
23    print("Starting planner")
24    planner = _start_planner(app)
25    visualizer: Optional[Visualizer] = Visualizer.view_planner(planner) if USE_VISUALIZER else None
26
27    start_configuration = [0.0, 0.0, 0.0, 0.0, 1.57, 0.0]
28    goal_configuration = [1.57, 0.0, 0.0, 0.0, 1.57, 0.0]
29    initial_state = InitialState(start_configuration=start_configuration)
30
31    result_without_obstacle = planner.path(
32        initial_state=initial_state,
33        request=PathRequest(
34            goal_configurations=[goal_configuration],
35            description="Without obstacle",
36        ),
37    )
38    if not result_without_obstacle:
39        raise RuntimeError(f"Planning before obstacle is set failed with error: {result_without_obstacle.error}")
40    assert len(result_without_obstacle.path) == 1
41    print(f"Path before obstacle is set: {len(result_without_obstacle.path)} waypoint")
42
43    # Set an obstacle that obstructs the direct path
44    box_triangles = _create_box_mesh(center_point=[1.1, 1.0, 1.6], size=[0.2, 0.2, 0.2])
45    planner.set_obstacles(
46        obstacles=[
47            Obstacle.from_mesh(name="box_obstacle", mesh=box_triangles),
48        ]
49    )
50
51    result_with_obstacle = planner.path(
52        initial_state=initial_state,
53        request=PathRequest(
54            goal_configurations=[goal_configuration],
55            description="With obstacle",
56        ),
57    )
58    if not result_with_obstacle:
59        raise RuntimeError(f"Planning after obstacle is set failed with error: {result_with_obstacle.error}")
60    assert len(result_with_obstacle.path) > 1
61    print(f"Path after obstacle is set: {len(result_with_obstacle.path)} waypoints")
62
63    if visualizer is not None:
64        print("Close the window to exit.")
65        visualizer.wait()
66
67
68if __name__ == "__main__":
69    _main()

基于 CAD 文件的障碍物

本示例展示了如何向运动规划器的动态环境模型中添加自定义 CAD 文件。需要注意的是，对于代表静态障碍物的 CAD 文件，如果在 URDF 文件中将其作为静态障碍物添加，其处理效率会比在运行时作为动态障碍物添加更高。

  1#include <Zivid/Motion/Application.h>
  2#include <Zivid/Motion/Planner.h>
  3#include <Zivid/Motion/Visualizer.h>
  4
  5#include <assimp/Importer.hpp>
  6#include <assimp/postprocess.h>
  7#include <assimp/scene.h>
  8
  9#include <filesystem>
 10#include <iostream>
 11
 12namespace
 13{
 14    constexpr auto useVisualizer = true;
 15
 16    void printException(const std::exception &e, const int level = 0)
 17    {
 18        std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
 19        try
 20        {
 21            std::rethrow_if_nested(e);
 22        }
 23        catch(const std::exception &nestedException)
 24        {
 25            printException(nestedException, level + 1);
 26        }
 27        catch(...)
 28        {}
 29    }
 30
 31    Zivid::Motion::Planner startPlanner(const Zivid::Motion::Application &app)
 32    {
 33        return app.createPlanner(
 34            Zivid::Motion::PlannerSettings{
 35                "demo_cell",
 36                Zivid::Motion::Profile::testing,
 37            });
 38    }
 39
 40    Zivid::Motion::Triangles loadTrianglesFromCad(const std::filesystem::path &cadFilePath)
 41    {
 42        Assimp::Importer importer;
 43        const aiScene *scene = importer.ReadFile(cadFilePath, aiProcess_Triangulate | aiProcess_JoinIdenticalVertices);
 44
 45        if(!scene || !scene->HasMeshes())
 46        {
 47            throw std::runtime_error("Failed to load CAD file: " + cadFilePath.string());
 48        }
 49
 50        Zivid::Motion::Triangles triangles;
 51        for(unsigned int m = 0; m < scene->mNumMeshes; ++m)
 52        {
 53            const aiMesh *mesh = scene->mMeshes[m];
 54            for(unsigned int f = 0; f < mesh->mNumFaces; ++f)
 55            {
 56                const aiFace &face = mesh->mFaces[f];
 57                if(face.mNumIndices != 3)
 58                {
 59                    continue; // skip non-triangles (shouldn't happen with aiProcess_Triangulate)
 60                }
 61
 62                const auto toPoint = [&](unsigned int idx) {
 63                    const aiVector3D &v = mesh->mVertices[face.mIndices[idx]];
 64                    return Zivid::Motion::PointXYZ{ v.x, v.y, v.z };
 65                };
 66
 67                triangles.push_back({ toPoint(0), toPoint(1), toPoint(2) });
 68            }
 69        }
 70
 71        if(triangles.empty())
 72        {
 73            throw std::runtime_error("No triangles found in CAD file: " + cadFilePath.filename().string());
 74        }
 75        return triangles;
 76    }
 77} // namespace
 78
 79int main()
 80{
 81    try
 82    {
 83        const Zivid::Motion::Application app;
 84
 85        std::cout << "Starting planner\n";
 86        auto planner = startPlanner(app);
 87        auto visualizer =
 88            useVisualizer ? std::optional{ Zivid::Motion::Visualizer::viewPlanner(planner) } : std::nullopt;
 89
 90        // Make sure the CAD uses meters as unit, not millimeters
 91        const std::filesystem::path cadFilePath = "path/to/obstacle.stl";
 92
 93        std::cout << "Loading CAD file from: " << cadFilePath << "\n";
 94        const auto triangles = loadTrianglesFromCad(cadFilePath);
 95
 96        planner.setObstacles({ Zivid::Motion::Obstacle::fromMesh("cad_obstacle", triangles) });
 97
 98        if(useVisualizer)
 99        {
100            std::cout << "Close the window to exit.\n";
101            visualizer->wait();
102        }
103    }
104    catch(const std::exception &exception)
105    {
106        printException(exception);
107        std::cout << "Press enter to exit." << std::endl;
108        std::cin.get();
109        return EXIT_FAILURE;
110    }
111    return EXIT_SUCCESS;
112}

使用以下命令安装其他依赖项：

pip install trimesh

 1from pathlib import Path
 2from typing import Optional
 3
 4import trimesh
 5from zividmotion import Application, Obstacle, Planner, PlannerSettings, Profile, Visualizer
 6
 7USE_VISUALIZER = True
 8
 9
10def _start_planner(app: Application) -> Planner:
11    planner_settings = PlannerSettings(cell_name="demo_cell", profile=Profile.testing)
12    return app.create_planner(planner_settings)
13
14
15def _load_triangles_from_cad_file(cad_file_path: Path) -> list:
16    mesh = trimesh.load(cad_file_path)
17    return mesh.triangles.tolist()
18
19
20def _main() -> None:
21    app = Application()
22
23    print("Starting planner")
24    planner = _start_planner(app)
25    visualizer: Optional[Visualizer] = Visualizer.view_planner(planner) if USE_VISUALIZER else None
26
27    # Make sure the CAD uses meters as unit, not millimeters
28    cad_file_path = Path("path/to/obstacle.stl")
29
30    print("Loading CAD file from:", cad_file_path)
31    triangles = _load_triangles_from_cad_file(cad_file_path)
32
33    planner.set_obstacles(
34        obstacles=[
35            Obstacle.from_mesh(name="cad_obstacle", mesh=triangles),
36        ]
37    )
38
39    if visualizer is not None:
40        print("Close the window to exit.")
41        visualizer.wait()
42
43
44if __name__ == "__main__":
45    _main()

基于 Zivid 点云的障碍物

本示例展示了如何将 Zivid 相机采集的点云添加到运动规划器的动态环境模型中。使用此功能需要安装 Zivid SDK，并确保已连接到 Zivid 相机。

 1#include <Zivid/Motion/Application.h>
 2#include <Zivid/Motion/Planner.h>
 3#include <Zivid/Motion/Visualizer.h>
 4#include <Zivid/Zivid.h>
 5
 6#include <iostream>
 7
 8namespace
 9{
10    constexpr auto useVisualizer = true;
11    constexpr auto useFileCamera = true;
12
13    Zivid::Motion::Planner startPlanner(const Zivid::Motion::Application &app)
14    {
15        return app.createPlanner(
16            Zivid::Motion::PlannerSettings{
17                "demo_cell",
18                Zivid::Motion::Profile::testing,
19            });
20    }
21} // namespace
22
23int main()
24{
25    try
26    {
27        Zivid::Application cameraApp;
28        const Zivid::Motion::Application app;
29
30        std::cout << "Connecting to camera\n";
31        auto camera = useFileCamera ? cameraApp.createFileCamera("/path/to/FileCamera.zfc") : cameraApp.connectCamera();
32
33        std::cout << "Starting planner\n";
34        auto planner = startPlanner(app);
35        auto visualizer =
36            useVisualizer ? std::optional{ Zivid::Motion::Visualizer::viewPlanner(planner) } : std::nullopt;
37
38        // Retrieved from hand-eye calibration of your setup.
39        // In an eye-in-hand setup, this would be the handeye_transform * robot_capture_pose.
40        // In an eye-to-hand setup, this would simply be the handeye_transform.
41        const Zivid::Matrix4x4 cameraToBaseTransform{ {
42            { 1.f, 0.f, 0.f, 0.f },
43            { 0.f, 1.f, 0.f, 0.f },
44            { 0.f, 0.f, 1.f, 0.f },
45            { 0.f, 0.f, 0.f, 1.f },
46        } };
47
48        const Zivid::Matrix4x4 millimetersToMetersTransform{ {
49            { 0.001f, 0.f, 0.f, 0.f },
50            { 0.f, 0.001f, 0.f, 0.f },
51            { 0.f, 0.f, 0.001f, 0.f },
52            { 0.f, 0.f, 0.f, 1.f },
53        } };
54
55        std::cout << "Capturing with default capture settings\n";
56        const auto settings =
57            Zivid::Settings{ Zivid::Settings::Acquisitions{ Zivid::Settings::Acquisition{} },
58                             Zivid::Settings::Color{ Zivid::Settings2D{
59                                 Zivid::Settings2D::Acquisitions{ Zivid::Settings2D::Acquisition{} } } } };
60        const auto frame = camera.capture(settings);
61
62        // Downsampling improves speed, if you don't need the extra resolution
63        frame.pointCloud().downsample(Zivid::PointCloud::Downsampling::by4x4);
64
65        // Convert point cloud to unorganized point cloud
66        auto unorganizedPointCloud = frame.pointCloud().toUnorganizedPointCloud();
67
68        // Transform the point cloud from the camera frame to the base frame of the planner
69        unorganizedPointCloud.transform(cameraToBaseTransform);
70
71        // Transform the point cloud from millimeters to meters, which is the expected unit in the planner
72        unorganizedPointCloud.transform(millimetersToMetersTransform);
73
74        const auto xyzData = unorganizedPointCloud.copyPointsXYZ();
75        std::vector<Zivid::Motion::PointXYZ> points;
76        points.reserve(xyzData.size());
77        for(const auto &p : xyzData)
78        {
79            constexpr auto mmToM = 1.f / 1000.f; // Convert from millimeters to meters
80            points.emplace_back(Zivid::Motion::PointXYZ{ p.x * mmToM, p.y * mmToM, p.z * mmToM });
81        }
82        planner.setObstacles({ Zivid::Motion::Obstacle::fromPointCloud("point_cloud_obstacle", points) });
83
84        if(useVisualizer)
85        {
86            std::cout << "Close the window to exit.\n";
87            visualizer->wait();
88        }
89    }
90    catch(const std::exception &e)
91    {
92        std::cerr << "Error: " << e.what() << std::endl;
93        std::cout << "Press enter to exit." << std::endl;
94        std::cin.get();
95        return EXIT_FAILURE;
96    }
97    return EXIT_SUCCESS;
98}

 1from typing import Optional
 2
 3import numpy as np
 4import zivid
 5from zividmotion import Application, Obstacle, Planner, PlannerSettings, Profile, Visualizer
 6
 7USE_VISUALIZER = True
 8USE_FILE_CAMERA = True
 9
10
11def _start_planner(app: Application) -> Planner:
12    planner_settings = PlannerSettings(cell_name="demo_cell", profile=Profile.testing)
13    return app.create_planner(planner_settings)
14
15
16def _main() -> None:
17    camera_app = zivid.Application()
18    motion_app = Application()
19
20    print("Connecting to camera")
21    camera = (
22        camera_app.create_file_camera("/path/to/FileCamera.zfc") if USE_FILE_CAMERA else camera_app.connect_camera()
23    )
24
25    print("Starting planner")
26    planner = _start_planner(motion_app)
27    visualizer: Optional[Visualizer] = Visualizer.view_planner(planner) if USE_VISUALIZER else None
28
29    # Retrieved from hand-eye calibration of your setup.
30    # In an eye-in-hand setup, this would be the handeye_transform * robot_capture_pose.
31    # In an eye-to-hand setup, this would simply be the handeye_transform.
32    camera_to_base_transform = np.eye(4)
33
34    millimeters_to_meters_transform = np.diag([0.001, 0.001, 0.001, 1])
35
36    print("Capturing with default capture settings")
37    # Replace with your own capture settings for better results
38    capture_settings = zivid.Settings(
39        acquisitions=[zivid.Settings.Acquisition()],
40        color=zivid.Settings2D(acquisitions=[zivid.Settings2D.Acquisition()]),
41    )
42
43    frame = camera.capture(capture_settings)
44
45    # Downsampling improves speed, if you don't need the extra resolution
46    frame.point_cloud().downsample(zivid.PointCloud.Downsampling.by4x4)
47
48    # Convert point cloud to unorganized point cloud
49    unorganized_point_cloud = frame.point_cloud().to_unorganized_point_cloud()
50
51    # Transform the point cloud from the camera frame to the base frame of the planner
52    unorganized_point_cloud.transform(camera_to_base_transform)
53
54    # Transform the point cloud from millimeters to meters, which is the expected unit in the planner
55    unorganized_point_cloud.transform(millimeters_to_meters_transform)
56
57    points = unorganized_point_cloud.copy_data("xyz")
58    obstacle = Obstacle.from_point_cloud(name="point_cloud_obstacle", points=points)
59    planner.set_obstacles(obstacles=[obstacle])
60
61    if USE_VISUALIZER:
62        input("Press enter to continue:")
63
64    # Set the obstacle again, but this time with color.
65    # Note that this has a slight performance impact, not recommended in production code.
66    planner.clear_obstacles()
67
68    rgba = unorganized_point_cloud.copy_data("rgba")
69    colored_obstacle = Obstacle.from_colored_point_cloud(name="colored_obstacle", points=points, colors=rgba)
70    planner.set_obstacles(obstacles=[colored_obstacle])
71
72    if visualizer is not None:
73        print("Close the window to exit.")
74        visualizer.wait()
75
76
77if __name__ == "__main__":
78    _main()

交互规划

简单触碰接近

本示例展示了在简化的设置中，如何利用触碰（Touch）功能来规划接近待抓取物体的动作。

  1#include <Zivid/Motion/Application.h>
  2#include <Zivid/Motion/Planner.h>
  3#include <Zivid/Motion/Visualizer.h>
  4
  5#include <cassert>
  6#include <cmath>
  7#include <iostream>
  8#include <random>
  9
 10namespace
 11{
 12    constexpr auto useVisualizer = true;
 13
 14    void printException(const std::exception &e, const int level = 0)
 15    {
 16        std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
 17        try
 18        {
 19            std::rethrow_if_nested(e);
 20        }
 21        catch(const std::exception &nestedException)
 22        {
 23            printException(nestedException, level + 1);
 24        }
 25        catch(...)
 26        {}
 27    }
 28
 29    Zivid::Motion::Planner startPlanner(const Zivid::Motion::Application &app)
 30    {
 31        return app.createPlanner(
 32            Zivid::Motion::PlannerSettings{
 33                "demo_cell",
 34                Zivid::Motion::Profile::testing,
 35            });
 36    }
 37
 38    // Utility function for creating a dummy point-cloud obstacle
 39    std::vector<Zivid::Motion::PointXYZ>
 40    generateSpherePoints(const Zivid::Motion::PointXYZ &center, const float radius, const int numPoints)
 41    {
 42        // Fixed seed for determinism
 43        std::mt19937 rng(42);
 44        std::uniform_real_distribution<float> thetaDist(0, 2 * M_PI);
 45        std::uniform_real_distribution<float> phiDist(0, M_PI);
 46
 47        std::vector<Zivid::Motion::PointXYZ> points;
 48        points.reserve(numPoints);
 49        for(int i = 0; i < numPoints; ++i)
 50        {
 51            const float theta = thetaDist(rng);
 52            const float phi = phiDist(rng);
 53            points.push_back(
 54                {
 55                    center.x + radius * std::sin(phi) * std::cos(theta),
 56                    center.y + radius * std::sin(phi) * std::sin(theta),
 57                    center.z + radius * std::cos(phi),
 58                });
 59        }
 60        return points;
 61    }
 62
 63    // Create a pick pose with the end-effector pointing downwards at the given point
 64    // (180-degree rotation around Y-axis)
 65    Zivid::Motion::Pose getPickPose(const Zivid::Motion::PointXYZ &pickPoint)
 66    {
 67        return { {
 68            { -1.f, 0.f, 0.f, pickPoint.x },
 69            { 0.f, 1.f, 0.f, pickPoint.y },
 70            { 0.f, 0.f, -1.f, pickPoint.z },
 71            { 0.f, 0.f, 0.f, 1.f },
 72        } };
 73    }
 74} // namespace
 75
 76int main()
 77{
 78    try
 79    {
 80        const Zivid::Motion::Application app;
 81
 82        std::cout << "Starting planner\n";
 83        auto planner = startPlanner(app);
 84        auto visualizer =
 85            useVisualizer ? std::optional{ Zivid::Motion::Visualizer::viewPlanner(planner) } : std::nullopt;
 86
 87        const Zivid::Motion::Configuration startConfiguration = { 0.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
 88
 89        // Set a dummy obstacle to be interacted with
 90        constexpr Zivid::Motion::PointXYZ obstacleCenter{ 1.1f, 1.0f, 0.9f };
 91        constexpr float obstacleRadius = 0.2f;
 92        planner.setObstacles(
 93            { Zivid::Motion::Obstacle::fromPointCloud(
 94                "interaction_object",
 95                Zivid::Motion::Obstacle::PointCloud{ generateSpherePoints(obstacleCenter, obstacleRadius, 1000) }) });
 96
 97        // Find the pick joint configuration
 98        constexpr Zivid::Motion::PointXYZ pickPoint{
 99            obstacleCenter.x,
100            obstacleCenter.y,
101            obstacleCenter.z + obstacleRadius
102                - 0.01f, // Put goal 1cm into the object for good contact with gripper compliance
103        };
104        const auto pickConfiguration = planner.computeInverseKinematics(getPickPose(pickPoint), startConfiguration);
105        if(!pickConfiguration.has_value())
106        {
107            throw std::runtime_error("No valid inverse kinematics solution found for pick pose");
108        }
109
110        const Zivid::Motion::InitialState initialState{ startConfiguration };
111
112        // Path type defaults to Free
113        Zivid::Motion::PathRequest freeRequest{};
114        freeRequest.goalConfigurations = { pickConfiguration.value() };
115        freeRequest.description = "Free path to pick goal";
116        const auto resultFree = planner.path(initialState, freeRequest);
117        // Expect blocked end with path type Free, since the goal is inside the obstacle
118        assert(resultFree.status == Zivid::Motion::PlanResult::Status::blockedEnd);
119
120        Zivid::Motion::PathRequest touchRequest{};
121        touchRequest.type = Zivid::Motion::PathRequest::Type::touch;
122        touchRequest.goalConfigurations = { pickConfiguration.value() };
123        touchRequest.description = "Touch path to pick goal";
124        const auto resultTouch = planner.path(initialState, touchRequest);
125        if(!resultTouch)
126        {
127            throw std::runtime_error("Planning with path type Touch failed with result: " + resultTouch.toString());
128        }
129
130        std::cout << "\nSuccessful touch path: " << resultTouch << "\n";
131
132        if(useVisualizer)
133        {
134            std::cout << "Close the window to exit.\n";
135            visualizer->wait();
136        }
137    }
138    catch(const std::exception &exception)
139    {
140        printException(exception);
141        std::cout << "Press enter to exit." << std::endl;
142        std::cin.get();
143        return EXIT_FAILURE;
144    }
145    return EXIT_SUCCESS;
146}

使用以下命令安装其他依赖项：

pip install scipy

  1from typing import Optional
  2
  3import numpy as np
  4from scipy.spatial.transform import Rotation
  5from zividmotion import (
  6    Application,
  7    InitialState,
  8    Obstacle,
  9    PathRequest,
 10    PathResult,
 11    Planner,
 12    PlannerSettings,
 13    Profile,
 14    Visualizer,
 15)
 16
 17USE_VISUALIZER = True
 18
 19
 20def _start_planner(app: Application) -> Planner:
 21    planner_settings = PlannerSettings(cell_name="demo_cell", profile=Profile.testing)
 22    return app.create_planner(planner_settings)
 23
 24
 25# Utility method for creating a dummy obstacle
 26def _generate_sphere_points(center_point: list[float], radius: float, num_points: int) -> list[list[float]]:
 27    np.random.seed(42)  # Set a fixed random seed for determinism
 28    points = []
 29    for _ in range(num_points):
 30        theta = np.random.uniform(0, 2 * np.pi)
 31        phi = np.random.uniform(0, np.pi)
 32        x = center_point[0] + radius * np.sin(phi) * np.cos(theta)
 33        y = center_point[1] + radius * np.sin(phi) * np.sin(theta)
 34        z = center_point[2] + radius * np.cos(phi)
 35        points.append([x, y, z])
 36    return points
 37
 38
 39def _get_pick_pose(pick_point: list[float]) -> np.ndarray:
 40    # Create a simple pick pose with the end-effector pointing downwards at the pick point
 41    pick_pose = np.eye(4, dtype=np.float32)
 42    pick_pose[:3, 3] = pick_point
 43    # Rotate 180 degrees around Y-axis to point downwards
 44    pick_pose[:3, :3] = Rotation.from_rotvec([0, np.pi, 0]).as_matrix()
 45    return pick_pose
 46
 47
 48def _main() -> None:
 49    app = Application()
 50
 51    print("Starting planner")
 52    planner = _start_planner(app)
 53    visualizer: Optional[Visualizer] = Visualizer.view_planner(planner) if USE_VISUALIZER else None
 54
 55    start_configuration = [0.0, 0.0, 0.0, 0.0, 1.57, 0.0]
 56
 57    # Set a dummy obstacle to be interacted with
 58    center_point = [1.1, 1.0, 0.9]
 59    radius = 0.2
 60    planner.set_obstacles(
 61        obstacles=[
 62            Obstacle.from_point_cloud(
 63                name="interaction_object",
 64                points=_generate_sphere_points(center_point=center_point, radius=radius, num_points=1000),
 65            )
 66        ]
 67    )
 68
 69    # Find the pick joint configuration
 70    pick_point = [center_point[0], center_point[1], center_point[2] + radius]
 71    pick_point[2] -= 0.01  # Put goal 1cm into the object, for instance for good contact with gripper compliance
 72    pick_pose = _get_pick_pose(pick_point)
 73    pick_configuration = planner.compute_inverse_kinematics(pose=pick_pose, reference_configuration=start_configuration)
 74    if pick_configuration is None:
 75        raise RuntimeError("No valid inverse kinematics solution found for pick pose")
 76
 77    initial_state = InitialState(start_configuration=start_configuration)
 78    # Path type defaults to Free
 79    result_free = planner.path(
 80        initial_state=initial_state,
 81        request=PathRequest(
 82            goal_configurations=[pick_configuration],
 83            description="Free path to pick goal",
 84        ),
 85    )
 86    # Expect blocked end with path type Free, since the goal is inside the obstacle
 87    assert result_free.error == PathResult.Error.blockedEnd
 88
 89    # Specify type touch
 90    result_touch = planner.path(
 91        initial_state=initial_state,
 92        request=PathRequest(
 93            type=PathRequest.Type.touch,
 94            goal_configurations=[pick_configuration],
 95            description="Touch path to pick goal",
 96        ),
 97    )
 98    if not result_touch:
 99        raise RuntimeError(f"Planning with path type Touch failed with error: {result_touch.error}")
100
101    print("\nSuccessful touch path:", result_touch.path)
102
103    if visualizer is not None:
104        print("Close the window to exit.")
105        visualizer.wait()
106
107
108if __name__ == "__main__":
109    _main()

可替换工具的抓取接近与回撤

本示例展示了在更换工具并携带物体的情况下，如何执行简化的物体抓取操作（包括接近和回撤路径）。

  1#include <Zivid/Motion/Application.h>
  2#include <Zivid/Motion/Planner.h>
  3#include <Zivid/Motion/Visualizer.h>
  4
  5#include <cmath>
  6#include <iostream>
  7#include <random>
  8
  9namespace
 10{
 11    constexpr auto useVisualizer = true;
 12
 13    void printException(const std::exception &e, const int level = 0)
 14    {
 15        std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
 16        try
 17        {
 18            std::rethrow_if_nested(e);
 19        }
 20        catch(const std::exception &nestedException)
 21        {
 22            printException(nestedException, level + 1);
 23        }
 24        catch(...)
 25        {}
 26    }
 27
 28    Zivid::Motion::Planner startPlanner(const Zivid::Motion::Application &app)
 29    {
 30        return app.createPlanner(
 31            Zivid::Motion::PlannerSettings{
 32                "demo_cell",
 33                Zivid::Motion::Profile::testing,
 34            });
 35    }
 36
 37    // Utility function for creating a dummy point-cloud obstacle
 38    std::vector<Zivid::Motion::PointXYZ>
 39    generateSpherePoints(const Zivid::Motion::PointXYZ &center, const float radius, const int numPoints)
 40    {
 41        // Fixed seed for determinism
 42        std::mt19937 rng(42);
 43        std::uniform_real_distribution<float> thetaDist(0, 2 * M_PI);
 44        std::uniform_real_distribution<float> phiDist(0, M_PI);
 45
 46        std::vector<Zivid::Motion::PointXYZ> points;
 47        points.reserve(numPoints);
 48        for(int i = 0; i < numPoints; ++i)
 49        {
 50            const float theta = thetaDist(rng);
 51            const float phi = phiDist(rng);
 52            points.push_back(
 53                {
 54                    center.x + radius * std::sin(phi) * std::cos(theta),
 55                    center.y + radius * std::sin(phi) * std::sin(theta),
 56                    center.z + radius * std::cos(phi),
 57                });
 58        }
 59        return points;
 60    }
 61
 62    std::vector<Zivid::Motion::Obstacle> getDummyObstacles(const Zivid::Motion::PointXYZ &center, const float radius)
 63    {
 64        std::vector<Zivid::Motion::Obstacle> obstacles;
 65        for(const float dx : { -2.f * radius, 0.f, 2.f * radius })
 66        {
 67            for(const float dy : { -2.f * radius, 0.f, 2.f * radius })
 68            {
 69                const Zivid::Motion::PointXYZ c{ center.x + dx, center.y + dy, center.z };
 70                const auto name = "obstacle_" + std::to_string(obstacles.size());
 71                obstacles.push_back(
 72                    Zivid::Motion::Obstacle::fromPointCloud(
 73                        name, Zivid::Motion::Obstacle::PointCloud{ generateSpherePoints(c, radius, 400) }));
 74            }
 75        }
 76        return obstacles;
 77    }
 78} // namespace
 79
 80int main()
 81{
 82    try
 83    {
 84        const Zivid::Motion::Application app;
 85
 86        std::cout << "Starting planner\n";
 87        auto planner = startPlanner(app);
 88        auto visualizer =
 89            useVisualizer ? std::optional{ Zivid::Motion::Visualizer::viewPlanner(planner) } : std::nullopt;
 90
 91        constexpr Zivid::Motion::PointXYZ obstacleCenter{ 1.1f, 1.0f, 0.2f };
 92        constexpr float obstacleRadius = 0.15f;
 93        planner.setObstacles(getDummyObstacles(obstacleCenter, obstacleRadius));
 94
 95        // For example purposes, simulate a suction cup gripper that is mounted asymmetrically at a 45 degree angle
 96        constexpr float angle = M_PI_4;
 97        const float cosA = std::cos(angle);
 98        const float sinA = std::sin(angle);
 99        const Zivid::Motion::Pose toolTransform{ {
100            { cosA, -sinA, 0.f, 0.f },
101            { sinA, cosA, 0.f, 0.f },
102            { 0.f, 0.f, 1.f, 0.05f },
103            { 0.f, 0.f, 0.f, 1.f },
104        } };
105        constexpr Zivid::Motion::VectorXYZ toolDimensions{ 0.4f, 0.4f, 0.06f };
106        // Specify the last 2cm of the gripper as the compliant suction cup area
107        constexpr Zivid::Motion::Pose compliantTransform{ {
108            { 1.f, 0.f, 0.f, 0.f },
109            { 0.f, 1.f, 0.f, 0.f },
110            { 0.f, 0.f, 1.f, toolDimensions.z - 0.02f },
111            { 0.f, 0.f, 0.f, 1.f },
112        } };
113        constexpr Zivid::Motion::VectorXYZ compliantDimensions{ 0.4f, 0.4f, 0.02f };
114        Zivid::Motion::ToolBoxElement toolElement{};
115        toolElement.transform = toolTransform;
116        toolElement.rigidBoxDimensions = toolDimensions;
117        toolElement.compliantBox = Zivid::Motion::TransformedBox{ compliantTransform, compliantDimensions };
118        planner.setReplaceableTool(Zivid::Motion::ReplaceableTool::fromBoxElements({ toolElement }));
119
120        // Set the new tcp to be at the center tip of the gripper, compressed 10 mm
121        Zivid::Motion::Pose tcpTransform = toolTransform;
122        tcpTransform[2][3] += toolDimensions.z - 0.01f;
123        planner.setTcp(Zivid::Motion::Tcp{ tcpTransform, { 0.f, 0.f, 1.f } });
124
125        const Zivid::Motion::Configuration startConfiguration = { 0.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
126
127        // Define a pick pose: above the obstacle center, end-effector rotated 180 degrees around Y to point downwards
128        constexpr Zivid::Motion::Pose pickPose{ {
129            { -1.f, 0.f, 0.f, obstacleCenter.x },
130            { 0.f, 1.f, 0.f, obstacleCenter.y },
131            { 0.f, 0.f, -1.f, obstacleCenter.z + obstacleRadius },
132            { 0.f, 0.f, 0.f, 1.f },
133        } };
134        const auto pickConfiguration = planner.computeInverseKinematics(pickPose, startConfiguration);
135        if(!pickConfiguration.has_value())
136        {
137            throw std::runtime_error("No valid inverse kinematics solution found for pick pose");
138        }
139
140        // Compute a pick approach path with the new tool
141        Zivid::Motion::PathRequest approachRequest{};
142        approachRequest.type = Zivid::Motion::PathRequest::Type::touch;
143        approachRequest.goalConfigurations = { pickConfiguration.value() };
144        approachRequest.description = "Pick approach";
145        const auto approachResult = planner.path(Zivid::Motion::InitialState{ startConfiguration }, approachRequest);
146        if(!approachResult)
147        {
148            throw std::runtime_error("Planning pick approach failed with result: " + approachResult.toString());
149        }
150
151        // Set carried object (bounding box around the picked obstacle)
152        constexpr float objectSize = obstacleRadius * 2.f;
153        constexpr Zivid::Motion::Pose identityPose{ {
154            { 1.f, 0.f, 0.f, 0.f },
155            { 0.f, 1.f, 0.f, 0.f },
156            { 0.f, 0.f, 1.f, 0.f },
157            { 0.f, 0.f, 0.f, 1.f },
158        } };
159        planner.setCarriedObject(
160            Zivid::Motion::CarriedObject::fromBox({ identityPose, { objectSize, objectSize, objectSize } }));
161
162        // Compute a pick retract path with custom retract direction
163        // This retract direction is the same as the negative tcp tool direction expressed in the base frame when the robot
164        // is in the start configuration for this path call. Meaning it in this case is a redundant specification, just to
165        // show the signature for example purposes.
166        Zivid::Motion::PathRequest retractRequest{};
167        retractRequest.type = Zivid::Motion::PathRequest::Type::touch;
168        retractRequest.retractDirection = Zivid::Motion::VectorXYZ{ 0.f, 0.f, 1.f };
169        retractRequest.goalConfigurations = { startConfiguration };
170        retractRequest.description = "Pick retract";
171        const auto retractResult = planner.path(Zivid::Motion::InitialState{ approachResult }, retractRequest);
172        if(!retractResult)
173        {
174            throw std::runtime_error("Planning pick retract failed with result: " + retractResult.toString());
175        }
176
177        std::cout << retractResult << "\n";
178
179        if(useVisualizer)
180        {
181            std::cout << "Close the window to exit.\n";
182            visualizer->wait();
183        }
184    }
185    catch(const std::exception &exception)
186    {
187        printException(exception);
188        std::cout << "Press enter to exit." << std::endl;
189        std::cin.get();
190        return EXIT_FAILURE;
191    }
192    return EXIT_SUCCESS;
193}

使用以下命令安装其他依赖项：

pip install scipy

  1from typing import Optional
  2
  3import numpy as np
  4from scipy.spatial.transform import Rotation
  5from zividmotion import (
  6    Application,
  7    CarriedObject,
  8    InitialState,
  9    Obstacle,
 10    PathRequest,
 11    Planner,
 12    PlannerSettings,
 13    Profile,
 14    ReplaceableTool,
 15    Tcp,
 16    ToolBoxElement,
 17    TransformedBox,
 18    Visualizer,
 19)
 20
 21USE_VISUALIZER = True
 22
 23
 24def _start_planner(app: Application) -> Planner:
 25    planner_settings = PlannerSettings(cell_name="demo_cell", profile=Profile.testing)
 26    return app.create_planner(planner_settings)
 27
 28
 29# Utility method for creating a dummy obstacle
 30def _generate_sphere_points(center_point: list[float], radius: float, num_points: int) -> list[list[float]]:
 31    np.random.seed(42)  # Set a fixed random seed for determinism
 32    points = []
 33    for _ in range(num_points):
 34        theta = np.random.uniform(0, 2 * np.pi)
 35        phi = np.random.uniform(0, np.pi)
 36        x = center_point[0] + radius * np.sin(phi) * np.cos(theta)
 37        y = center_point[1] + radius * np.sin(phi) * np.sin(theta)
 38        z = center_point[2] + radius * np.cos(phi)
 39        points.append([x, y, z])
 40    return points
 41
 42
 43def _get_dummy_obstacles(center_point: list[float], radius: float) -> list[Obstacle]:
 44    obstacles: list[Obstacle] = []
 45    for x in [center_point[0] - 2 * radius, center_point[0], center_point[0] + 2 * radius]:
 46        for y in [center_point[1] - 2 * radius, center_point[1], center_point[1] + 2 * radius]:
 47            center = [x, y, center_point[2]]
 48            points = _generate_sphere_points(center_point=center, radius=radius, num_points=400)
 49            obstacles.append(Obstacle.from_point_cloud(name=f"obstacle_{len(obstacles)}", points=points))
 50    return obstacles
 51
 52
 53def _main() -> None:
 54    app = Application()
 55
 56    print("Starting planner")
 57    planner = _start_planner(app)
 58    visualizer: Optional[Visualizer] = Visualizer.view_planner(planner) if USE_VISUALIZER else None
 59
 60    obstacle_center = [1.1, 1.0, 0.2]
 61    obstacle_radius = 0.15
 62    planner.set_obstacles(obstacles=_get_dummy_obstacles(center_point=obstacle_center, radius=obstacle_radius))
 63
 64    # For example purposes, simulate a suction cup gripper that is mounted asymmetrically at a 45 degree angle
 65    tool_transform = np.eye(4)
 66    tool_transform[1, 3] = 0.05
 67    tool_transform[:3, :3] = Rotation.from_rotvec([0, 0, np.pi / 4]).as_matrix()
 68    tool_dimensions = [0.4, 0.4, 0.06]
 69    # Specify the last 2cm of the gripper as the compliant suction cup area
 70    compliant_transform = tool_transform.copy()
 71    compliant_transform[1, 3] = tool_dimensions[2] - 0.02
 72    compliant_dimensions = [0.4, 0.4, 0.02]
 73    planner.set_replaceable_tool(
 74        replaceable_tool=ReplaceableTool.from_box_elements(
 75            tool_elements=[
 76                ToolBoxElement(
 77                    tool_transform,
 78                    tool_dimensions,
 79                    compliant_box=TransformedBox(compliant_transform, compliant_dimensions),
 80                )
 81            ]
 82        )
 83    )
 84
 85    # Set the new tcp to be at the center tip of the gripper, when the suction cup is compressed 1 cm.
 86    tcp_transform = tool_transform.copy()
 87    tcp_transform[2, 3] += tool_dimensions[2] - 0.01
 88    planner.set_tcp(tcp=Tcp(transform=tcp_transform, tool_direction=[0.0, 0.0, 1.0]))
 89
 90    start_configuration = [0.0, 0.0, 0.0, 0.0, 1.57, 0.0]
 91
 92    # Define a pick pose and compute the pick joint configuration with the new tcp
 93    pick_position = [obstacle_center[0], obstacle_center[1], obstacle_center[2] + obstacle_radius]
 94    pick_pose = np.eye(4)
 95    pick_pose[:3, 3] = pick_position
 96    # Rotate 180 degrees around Y-axis to pick perpendicular to the obstacles
 97    pick_pose[:3, :3] = Rotation.from_rotvec([0, np.pi, 0]).as_matrix()
 98    pick_configuration = planner.compute_inverse_kinematics(pose=pick_pose, reference_configuration=start_configuration)
 99    if pick_configuration is None:
100        raise RuntimeError("No valid inverse kinematics solution found for pick pose")
101
102    # Compute a pick approach path with the new tool
103    approach_result = planner.path(
104        initial_state=InitialState(start_configuration=start_configuration),
105        request=PathRequest(
106            type=PathRequest.Type.touch,
107            goal_configurations=[pick_configuration],
108            description="Pick approach",
109        ),
110    )
111    if not approach_result:
112        raise RuntimeError(f"Planning pick approach failed with error: {approach_result.error}")
113
114    # Set carried object
115    obstacle_size = obstacle_radius * 2
116    planner.set_carried_object(
117        carried_object=CarriedObject.from_box(
118            transformed_box=TransformedBox(
119                bottom_centered_transform=np.eye(4), box_dimensions=[obstacle_size, obstacle_size, obstacle_size]
120            )
121        )
122    )
123
124    # Compute a pick retract path with custom retract direction
125    # This retract direction is the same as the negative tcp tool direction expressed in the base frame when the robot
126    # is in the start configuration for this path call. Meaning it in this case is a redundant specification, just to
127    # show the signature for example purposes.
128    retract_result = planner.path(
129        initial_state=InitialState(previous_result=approach_result),
130        request=PathRequest(
131            type=PathRequest.Type.touch,
132            retract_direction=[0.0, 0.0, 1.0],
133            goal_configurations=[start_configuration],
134            description="Pick retract",
135        ),
136    )
137    if not retract_result:
138        raise RuntimeError(f"Planning pick retract failed with error: {retract_result.error}")
139
140    print(retract_result)
141
142    if visualizer is not None:
143        print("Close the window to exit.")
144        visualizer.wait()
145
146
147if __name__ == "__main__":
148    _main()

机器人附件

带附件的路径规划

  1#include <Zivid/Motion/Application.h>
  2#include <Zivid/Motion/Planner.h>
  3#include <Zivid/Motion/Visualizer.h>
  4
  5#include <cassert>
  6#include <cmath>
  7#include <iostream>
  8#include <random>
  9
 10namespace
 11{
 12    constexpr auto useVisualizer = true;
 13
 14    void printException(const std::exception &e, const int level = 0)
 15    {
 16        std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
 17        try
 18        {
 19            std::rethrow_if_nested(e);
 20        }
 21        catch(const std::exception &nestedException)
 22        {
 23            printException(nestedException, level + 1);
 24        }
 25        catch(...)
 26        {}
 27    }
 28
 29    Zivid::Motion::Planner startPlanner(const Zivid::Motion::Application &app)
 30    {
 31        return app.createPlanner(
 32            Zivid::Motion::PlannerSettings{
 33                "demo_cell",
 34                Zivid::Motion::Profile::testing,
 35            });
 36    }
 37
 38    // Utility function for creating a dummy point-cloud obstacle
 39    std::vector<Zivid::Motion::PointXYZ>
 40    generateSpherePoints(const Zivid::Motion::PointXYZ &center, const float radius, const int numPoints)
 41    {
 42        // Fixed seed for determinism
 43        std::mt19937 rng(42);
 44        std::uniform_real_distribution<float> thetaDist(0, 2 * M_PI);
 45        std::uniform_real_distribution<float> phiDist(0, M_PI);
 46
 47        std::vector<Zivid::Motion::PointXYZ> points;
 48        points.reserve(numPoints);
 49        for(int i = 0; i < numPoints; ++i)
 50        {
 51            const float theta = thetaDist(rng);
 52            const float phi = phiDist(rng);
 53            points.push_back(
 54                {
 55                    center.x + radius * std::sin(phi) * std::cos(theta),
 56                    center.y + radius * std::sin(phi) * std::sin(theta),
 57                    center.z + radius * std::cos(phi),
 58                });
 59        }
 60        return points;
 61    }
 62} // namespace
 63
 64int main()
 65{
 66    try
 67    {
 68        const Zivid::Motion::Application app;
 69
 70        std::cout << "Starting planner\n";
 71        auto planner = startPlanner(app);
 72        auto visualizer =
 73            useVisualizer ? std::optional{ Zivid::Motion::Visualizer::viewPlanner(planner) } : std::nullopt;
 74
 75        const Zivid::Motion::Configuration startConfiguration = { 0.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
 76        const Zivid::Motion::Configuration goalConfiguration = { 1.57f, 0.f, 0.f, 0.f, 1.57f, 0.f };
 77
 78        // Must match a name in the attachments definition in the planner config file
 79        const std::string attachmentName = "demo_attachment";
 80
 81        // Set an obstacle that obstructs the direct path for the attachment
 82        planner.setObstacles(
 83            { Zivid::Motion::Obstacle::fromPointCloud(
 84                "sphere_obstacle", generateSpherePoints({ 1.1f, 1.0f, 1.2f }, 0.2f, 1000)) });
 85
 86        const Zivid::Motion::InitialState initialState{ startConfiguration };
 87
 88        Zivid::Motion::PathRequest withoutAttachmentRequest{};
 89        withoutAttachmentRequest.goalConfigurations = { goalConfiguration };
 90        withoutAttachmentRequest.description = "Path without attachment";
 91        const auto resultWithoutAttachment = planner.path(initialState, withoutAttachmentRequest);
 92        if(!resultWithoutAttachment)
 93        {
 94            throw std::runtime_error(
 95                "Planning without active attachment failed with result: " + resultWithoutAttachment.toString());
 96        }
 97        assert(resultWithoutAttachment.path().size() == 1);
 98        std::cout << "Path without attachment: " << resultWithoutAttachment.path().size() << " waypoints\n";
 99
100        std::cout << "Setting attachment " << attachmentName << "\n";
101        planner.setAttachments({ attachmentName });
102
103        Zivid::Motion::PathRequest withAttachmentRequest{};
104        withAttachmentRequest.goalConfigurations = { goalConfiguration };
105        withAttachmentRequest.description = "Path with attachment";
106        const auto resultWithAttachment = planner.path(initialState, withAttachmentRequest);
107        if(!resultWithAttachment)
108        {
109            std::cout << "Planning with active attachment failed with result: " << resultWithAttachment.toString()
110                      << "\n";
111        }
112        assert(resultWithAttachment.path().size() > 1);
113        std::cout << "With attachment: " << resultWithAttachment.path().size() << " waypoints\n";
114
115        if(useVisualizer)
116        {
117            std::cout << "Close the window to exit.\n";
118            visualizer->wait();
119        }
120    }
121    catch(const std::exception &exception)
122    {
123        printException(exception);
124        std::cout << "Press enter to exit." << std::endl;
125        std::cin.get();
126        return EXIT_FAILURE;
127    }
128    return EXIT_SUCCESS;
129}

 1from typing import Optional
 2
 3import numpy as np
 4from zividmotion import Application, InitialState, Obstacle, PathRequest, Planner, PlannerSettings, Profile, Visualizer
 5
 6USE_VISUALIZER = True
 7
 8
 9def _start_planner(app: Application) -> Planner:
10    planner_settings = PlannerSettings(cell_name="demo_cell", profile=Profile.testing)
11    return app.create_planner(planner_settings)
12
13
14# Utility method for creating a dummy obstacle
15def _generate_sphere_points(center_point: list[float], radius: float, num_points: int) -> list[list[float]]:
16    np.random.seed(42)  # Set a fixed random seed for determinism
17    points = []
18    for _ in range(num_points):
19        theta = np.random.uniform(0, 2 * np.pi)
20        phi = np.random.uniform(0, np.pi)
21        x = center_point[0] + radius * np.sin(phi) * np.cos(theta)
22        y = center_point[1] + radius * np.sin(phi) * np.sin(theta)
23        z = center_point[2] + radius * np.cos(phi)
24        points.append([x, y, z])
25    return points
26
27
28def _main() -> None:
29    app = Application()
30
31    print("Starting planner")
32    planner = _start_planner(app)
33    visualizer: Optional[Visualizer] = Visualizer.view_planner(planner) if USE_VISUALIZER else None
34
35    start_configuration = [0.0, 0.0, 0.0, 0.0, 1.57, 0.0]
36    goal_configuration = [1.57, 0.0, 0.0, 0.0, 1.57, 0.0]
37    attachment_name = "demo_attachment"  # Must match a name in the attachments definition in the planner config file
38
39    # Set an obstacle that obstructs the direct path for the attachment
40    planner.set_obstacles(
41        obstacles=[
42            Obstacle.from_point_cloud(
43                name="sphere_obstacle",
44                points=_generate_sphere_points(center_point=[1.1, 1.0, 1.2], radius=0.2, num_points=1000),
45            )
46        ]
47    )
48
49    initial_state = InitialState(start_configuration=start_configuration)
50    result_without_attachment = planner.path(
51        initial_state=initial_state,
52        request=PathRequest(
53            goal_configurations=[goal_configuration],
54            description="Path without attachment",
55        ),
56    )
57    if not result_without_attachment:
58        raise RuntimeError(f"Planning without active attachment failed with error: {result_without_attachment.error}")
59    assert len(result_without_attachment.path) == 1
60    print(f"Path without attachment: {len(result_without_attachment.path)} waypoints")
61
62    print("Setting attachment", attachment_name)
63    planner.set_attachments(attachments=[attachment_name])
64
65    result_with_attachment = planner.path(
66        initial_state=initial_state,
67        request=PathRequest(
68            goal_configurations=[goal_configuration],
69            description="Path with attachment",
70        ),
71    )
72    if not result_with_attachment:
73        print(f"Planning with active attachment failed with error: {result_with_attachment.error}")
74    assert len(result_with_attachment.path) > 1
75    print(f"Path with attachment: {len(result_with_attachment.path)} waypoints")
76
77    if visualizer is not None:
78        print("Close the window to exit.")
79        visualizer.wait()
80
81
82if __name__ == "__main__":
83    _main()

调试

导出和打包 API 日志

本示例展示了如何创建一个 zip 压缩包，其中包含重现您的规划器设置以及从同一规划器状态重新运行失败的 API 调用所需的所有内容。请注意，此示例预期会抛出错误。

 1#include <Zivid/Motion/Application.h>
 2#include <Zivid/Motion/Packaging.h>
 3#include <Zivid/Motion/Planner.h>
 4
 5#include <filesystem>
 6#include <iostream>
 7
 8namespace
 9{
10    void printException(const std::exception &e, const int level = 0)
11    {
12        std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
13        try
14        {
15            std::rethrow_if_nested(e);
16        }
17        catch(const std::exception &nestedException)
18        {
19            printException(nestedException, level + 1);
20        }
21        catch(...)
22        {}
23    }
24
25    Zivid::Motion::Planner startPlanner(const Zivid::Motion::Application &app)
26    {
27        return app.createPlanner(
28            Zivid::Motion::PlannerSettings{
29                "demo_cell",
30                Zivid::Motion::Profile::testing,
31            });
32    }
33} // namespace
34
35int main()
36{
37    const Zivid::Motion::Application app;
38
39    std::cout << "Starting planner\n";
40    auto planner = startPlanner(app);
41
42    try
43    {
44        const Zivid::Motion::Configuration startConfiguration = { 0.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
45        const Zivid::Motion::Configuration invalidGoalConfiguration = { 100.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
46
47        Zivid::Motion::PathRequest unreachableRequest{};
48        unreachableRequest.goalConfigurations = { invalidGoalConfiguration };
49        unreachableRequest.description = "Path with unreachable goal";
50        const auto unsuccessfulResult =
51            planner.path(Zivid::Motion::InitialState{ startConfiguration }, unreachableRequest);
52        std::cout << "Path result has status: " << unsuccessfulResult.toString() << "\n";
53
54        // Some operation that fails, like creating an InitialState from an unsuccessful path result
55        const Zivid::Motion::InitialState initialState{ unsuccessfulResult };
56        Zivid::Motion::PathRequest invalidStateRequest{};
57        invalidStateRequest.goalConfigurations = { startConfiguration };
58        invalidStateRequest.description = "Path with invalid initial state";
59        const auto result = planner.path(Zivid::Motion::InitialState{ startConfiguration }, invalidStateRequest);
60        std::cout << result << "\n";
61    }
62    catch(const std::exception &exception)
63    {
64        const std::filesystem::path outputFolder = "/tmp";
65
66        const auto logPath = planner.exportApiLog(outputFolder);
67        const auto packagePath = Zivid::Motion::packageApiLog(app, logPath);
68        std::cerr << "Planner failed, debug package available at: " + std::filesystem::canonical(packagePath).string()
69                  << std::endl;
70
71        printException(exception);
72        return EXIT_SUCCESS; // for the purpose of this sample, saving a debug package is success
73    }
74    return EXIT_FAILURE;
75}

 1from pathlib import Path
 2
 3from zividmotion import Application, InitialState, PathRequest, Planner, PlannerSettings, Profile, package_api_log
 4
 5
 6def _start_planner(app: Application) -> Planner:
 7    planner_settings = PlannerSettings(cell_name="demo_cell", profile=Profile.testing)
 8    return app.create_planner(planner_settings)
 9
10
11def _main() -> None:
12    app = Application()
13
14    print("Starting planner")
15    planner = _start_planner(app)
16
17    start_configuration = [0, 0, 0, 0, 1.57, 0]
18    invalid_goal_configuration = [100, 0, 0, 0, 1.57, 0]
19
20    unsuccessful_result = planner.path(
21        initial_state=InitialState(start_configuration=start_configuration),
22        request=PathRequest(
23            goal_configurations=[invalid_goal_configuration],
24            description="Path with unreachable goal",
25        ),
26    )
27    assert unsuccessful_result.error is not None
28    print("Path result has error: ", unsuccessful_result.error)
29
30    try:
31        # Some operation that fails, like creating an InitialState from an unsuccessful path result
32        initial_state = InitialState(previous_result=unsuccessful_result)
33        result = planner.path(
34            initial_state=initial_state,
35            request=PathRequest(
36                goal_configurations=[start_configuration],
37                description="Path with invalid initial state",
38            ),
39        )
40        print(result)
41
42    except Exception:
43        output_folder = Path("/tmp")
44
45        log_path = planner.export_api_log(output_directory=output_folder)
46        package_path = package_api_log(application=app, api_log_path=log_path)
47
48        print(f"Planner failed, debug package available at: {package_path.resolve()}")
49
50
51if __name__ == "__main__":
52    _main()