Samples

Setup

Generate Planning Data

This example shows how to generate planning data for your robot cell, which is necessary to do before you can start the motion planner.

 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        std::cout << "Press enter to exit." << std::endl;
50        std::cin.get();
51        return EXIT_FAILURE;
52    }
53    return EXIT_SUCCESS;
54}

 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()

Package Planner Setup

This example shows how to package all the files necessary for running the motion planner into a zip archive, which can be easily distributed and unpacked on other machines.

Run this snippet on the workstation where you have completed the planner setup:

 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 = "desired/path/output.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        std::cout << "Press enter to exit." << std::endl;
43        std::cin.get();
44        return EXIT_FAILURE;
45    }
46    return EXIT_SUCCESS;
47}

 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("desired/path/output.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()

The recipients of the zip archive can then run this snippet to unpack the setup:

 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 = "path/to/packaged/data.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        std::cout << "Press enter to exit." << std::endl;
41        std::cin.get();
42        return EXIT_FAILURE;
43    }
44    return EXIT_SUCCESS;
45}

 1from pathlib import Path
 2
 3from zividmotion import Application, install_package
 4
 5
 6def _main() -> None:
 7    app = Application()
 8
 9    package_path = Path("path/to/packaged/data.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()

Visualize Cell

This example shows how to open a 3D visualization window for a configured robot cell. This is useful for inspecting your cell setup before generating or running the motion planner.

 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        std::cout << "Press enter to exit." << std::endl;
43        std::cin.get();
44        return EXIT_FAILURE;
45    }
46    return EXIT_SUCCESS;
47}

 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()

Planner

Basic Path Calls

Joint Goal

 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        const auto result = planner.path(
52            Zivid::Motion::InitialState{ startConfiguration },
53            Zivid::Motion::PathRequest{ .goalConfigurations = { goalConfiguration },
54                                        .description = "Path with joint goal" });
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()

Pose Goal

 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        const auto result = planner.path(
62            Zivid::Motion::InitialState{ startConfiguration },
63            Zivid::Motion::PathRequest{ .goalConfigurations = { poseGoalConfiguration.value() },
64                                        .description = "Path with goal from pose" });
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()

Multiple Goals and Prioritization

 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        const auto resultShortestPath = planner.path(
57            initialState,
58            Zivid::Motion::PathRequest{ .goalConfigurations = goals,
59                                        .description = "Multiple goals - Default (ShortestPath) prioritization" });
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        const auto resultListOrder = planner.path(
68            initialState,
69            Zivid::Motion::PathRequest{ .goalPrioritizationMethod =
70                                            Zivid::Motion::PathRequest::GoalPrioritizationMethod::listOrder,
71                                        .goalConfigurations = goals,
72                                        .description = "Multiple goals - ListOrder prioritization" });
73        if(!resultListOrder)
74        {
75            throw std::runtime_error("Planning with ListOrder failed with result: " + resultListOrder.toString());
76        }
77        assert(resultListOrder.selectedGoalIdx == 0u);
78        std::cout << "ListOrder selected goal index: " << resultListOrder.selectedGoalIdx.value() << "\n";
79
80        if(useVisualizer)
81        {
82            std::cout << "Close the window to exit.\n";
83            visualizer->wait();
84        }
85    }
86    catch(const std::exception &exception)
87    {
88        printException(exception);
89        std::cout << "Press enter to exit." << std::endl;
90        std::cin.get();
91        return EXIT_FAILURE;
92    }
93    return EXIT_SUCCESS;
94}

 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()

Setting Runtime Obstacles

Simple Obstacle Geometries

This example shows how you can add simple obstacles such as boxes to the motion planner’s dynamic environment model.

  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        const auto resultWithoutObstacle = planner.path(
 94            initialState,
 95            Zivid::Motion::PathRequest{ .goalConfigurations = { goalConfiguration },
 96                                        .description = "Without obstacle" });
 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        const auto resultWithObstacle = planner.path(
110            initialState,
111            Zivid::Motion::PathRequest{ .goalConfigurations = { goalConfiguration }, .description = "With obstacle" });
112        if(!resultWithObstacle)
113        {
114            throw std::runtime_error(
115                "Planning after obstacle is set failed with result: " + resultWithObstacle.toString());
116        }
117        assert(resultWithObstacle.path().size() > 1);
118        std::cout << "Path after obstacle is set: " << resultWithObstacle.path().size() << " waypoints\n";
119
120        if(useVisualizer)
121        {
122            std::cout << "Close the window to exit.\n";
123            visualizer->wait();
124        }
125    }
126    catch(const std::exception &exception)
127    {
128        printException(exception);
129        std::cout << "Press enter to exit." << std::endl;
130        std::cin.get();
131        return EXIT_FAILURE;
132    }
133    return EXIT_SUCCESS;
134}

Install additional dependencies with:

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()

Obstacle from CAD File

This example shows how you can add custom cad files to the motion planner’s dynamic environment model. Note that cad files that represent static obstacles are more efficiently handled if added as static obstacles in the urdf files, rather than as dynamic obstacles in runtime.

  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}

Install additional dependencies with:

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()

Obstacle from Zivid Point Cloud

This example shows how to add a point cloud from a Zivid camera to the motion planner’s dynamic environment model. This requires having the Zivid SDK installed, and being connected to a Zivid camera.

 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        std::cout << "Capturing with default capture settings\n";
49        const auto settings =
50            Zivid::Settings{ Zivid::Settings::Acquisitions{ Zivid::Settings::Acquisition{} },
51                             Zivid::Settings::Color{ Zivid::Settings2D{
52                                 Zivid::Settings2D::Acquisitions{ Zivid::Settings2D::Acquisition{} } } } };
53        const auto frame = camera.capture(settings);
54
55        // Downsampling improves speed, if you don't need the extra resolution
56        frame.pointCloud().downsample(Zivid::PointCloud::Downsampling::by4x4);
57
58        // Transform the point cloud from the camera frame to the base frame of the planner
59        frame.pointCloud().transform(cameraToBaseTransform);
60
61        const auto xyzData = frame.pointCloud().toUnorganizedPointCloud().copyPointsXYZ();
62        std::vector<Zivid::Motion::PointXYZ> points;
63        points.reserve(xyzData.size());
64        for(const auto &p : xyzData)
65        {
66            constexpr auto mmToM = 1.f / 1000.f; // Convert from millimeters to meters
67            points.emplace_back(Zivid::Motion::PointXYZ{ p.x * mmToM, p.y * mmToM, p.z * mmToM });
68        }
69        planner.setObstacles({ Zivid::Motion::Obstacle::fromPointCloud("point_cloud_obstacle", points) });
70
71        if(useVisualizer)
72        {
73            std::cout << "Close the window to exit.\n";
74            visualizer->wait();
75        }
76    }
77    catch(const std::exception &e)
78    {
79        std::cerr << "Error: " << e.what() << std::endl;
80        std::cout << "Press enter to exit." << std::endl;
81        std::cin.get();
82        return EXIT_FAILURE;
83    }
84    return EXIT_SUCCESS;
85}

 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    print("Capturing with default capture settings")
35    # Replace with your own capture settings for better results
36    capture_settings = zivid.Settings(
37        acquisitions=[zivid.Settings.Acquisition()],
38        color=zivid.Settings2D(acquisitions=[zivid.Settings2D.Acquisition()]),
39    )
40
41    frame = camera.capture(capture_settings)
42
43    # Downsampling improves speed, if you don't need the extra resolution
44    frame.point_cloud().downsample(zivid.PointCloud.Downsampling.by4x4)
45
46    # Transform the point cloud from the camera frame to the base frame of the planner
47    frame.point_cloud().transform(camera_to_base_transform)
48
49    point_cloud = frame.point_cloud().to_unorganized_point_cloud()
50    points = point_cloud.copy_data("xyz") / 1000.0  # Convert from millimeters to meters
51    obstacle = Obstacle.from_point_cloud(name="point_cloud_obstacle", points=points)
52    planner.set_obstacles(obstacles=[obstacle])
53
54    if USE_VISUALIZER:
55        input("Press enter to continue:")
56
57    # Set the obstacle again, but this time with color.
58    # Note that this has a slight performance impact, not recommended in production code.
59    planner.clear_obstacles()
60
61    rgba = point_cloud.copy_data("rgba")
62    colored_obstacle = Obstacle.from_colored_point_cloud(name="colored_obstacle", points=points, colors=rgba)
63    planner.set_obstacles(obstacles=[colored_obstacle])
64
65    if visualizer is not None:
66        print("Close the window to exit.")
67        visualizer.wait()
68
69
70if __name__ == "__main__":
71    _main()

Interaction Planning

Simple Touch Approach

This example shows how to use the Touch functionality to plan an approach to an object to be picked in a simplified setup.

  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        const auto resultFree = planner.path(
114            initialState,
115            Zivid::Motion::PathRequest{ .goalConfigurations = { pickConfiguration.value() },
116                                        .description = "Free path to pick goal" });
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        const auto resultTouch = planner.path(
121            initialState,
122            Zivid::Motion::PathRequest{ .type = Zivid::Motion::PathRequest::Type::touch,
123                                        .goalConfigurations = { pickConfiguration.value() },
124                                        .description = "Touch path to pick goal" });
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}

Install additional dependencies with:

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()

Pick Approach and Retract with Replaceable Tool

This example shows how to do a simplified object pick, including approach and retract paths, with a replaceable tool and carried object.

  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        planner.setReplaceableTool(
115            Zivid::Motion::ReplaceableTool::fromBoxElements(
116                { Zivid::Motion::ToolBoxElement{
117                    .transform = toolTransform,
118                    .rigidBoxDimensions = toolDimensions,
119                    .compliantBox = Zivid::Motion::TransformedBox{ compliantTransform, compliantDimensions } } }));
120
121        // Set the new tcp to be at the center tip of the gripper, compressed 10 mm
122        Zivid::Motion::Pose tcpTransform = toolTransform;
123        tcpTransform[2][3] += toolDimensions.z - 0.01f;
124        planner.setTcp(Zivid::Motion::Tcp{ tcpTransform, { 0.f, 0.f, 1.f } });
125
126        const Zivid::Motion::Configuration startConfiguration = { 0.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
127
128        // Define a pick pose: above the obstacle center, end-effector rotated 180 degrees around Y to point downwards
129        constexpr Zivid::Motion::Pose pickPose{ {
130            { -1.f, 0.f, 0.f, obstacleCenter.x },
131            { 0.f, 1.f, 0.f, obstacleCenter.y },
132            { 0.f, 0.f, -1.f, obstacleCenter.z + obstacleRadius },
133            { 0.f, 0.f, 0.f, 1.f },
134        } };
135        const auto pickConfiguration = planner.computeInverseKinematics(pickPose, startConfiguration);
136        if(!pickConfiguration.has_value())
137        {
138            throw std::runtime_error("No valid inverse kinematics solution found for pick pose");
139        }
140
141        // Compute a pick approach path with the new tool
142        const auto approachResult = planner.path(
143            Zivid::Motion::InitialState{ startConfiguration },
144            Zivid::Motion::PathRequest{ .type = Zivid::Motion::PathRequest::Type::touch,
145                                        .goalConfigurations = { pickConfiguration.value() },
146                                        .description = "Pick approach" });
147        if(!approachResult)
148        {
149            throw std::runtime_error("Planning pick approach failed with result: " + approachResult.toString());
150        }
151
152        // Set carried object (bounding box around the picked obstacle)
153        constexpr float objectSize = obstacleRadius * 2.f;
154        constexpr Zivid::Motion::Pose identityPose{ {
155            { 1.f, 0.f, 0.f, 0.f },
156            { 0.f, 1.f, 0.f, 0.f },
157            { 0.f, 0.f, 1.f, 0.f },
158            { 0.f, 0.f, 0.f, 1.f },
159        } };
160        planner.setCarriedObject(
161            Zivid::Motion::CarriedObject::fromBox({ identityPose, { objectSize, objectSize, objectSize } }));
162
163        // Compute a pick retract path with custom retract direction
164        // This retract direction is the same as the negative tcp tool direction expressed in the base frame when the robot
165        // is in the start configuration for this path call. Meaning it in this case is a redundant specification, just to
166        // show the signature for example purposes.
167        const auto retractResult = planner.path(
168            Zivid::Motion::InitialState{ approachResult },
169            Zivid::Motion::PathRequest{ .type = Zivid::Motion::PathRequest::Type::touch,
170                                        .retractDirection = Zivid::Motion::VectorXYZ{ 0.f, 0.f, 1.f },
171                                        .goalConfigurations = { startConfiguration },
172                                        .description = "Pick retract" });
173        if(!retractResult)
174        {
175            throw std::runtime_error("Planning pick retract failed with result: " + retractResult.toString());
176        }
177
178        std::cout << retractResult << "\n";
179
180        if(useVisualizer)
181        {
182            std::cout << "Close the window to exit.\n";
183            visualizer->wait();
184        }
185    }
186    catch(const std::exception &exception)
187    {
188        printException(exception);
189        std::cout << "Press enter to exit." << std::endl;
190        std::cin.get();
191        return EXIT_FAILURE;
192    }
193    return EXIT_SUCCESS;
194}

Install additional dependencies with:

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()

Robot Attachments

Path Planning with Attachment

  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        const auto resultWithoutAttachment = planner.path(
 89            initialState,
 90            Zivid::Motion::PathRequest{ .goalConfigurations = { goalConfiguration },
 91                                        .description = "Path without attachment" });
 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        const auto resultWithAttachment = planner.path(
104            initialState,
105            Zivid::Motion::PathRequest{ .goalConfigurations = { goalConfiguration },
106                                        .description = "Path with attachment" });
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()

Debugging

Export and Package API Log

This example shows how to create a zip archive containing everything needed to recreate your planner setup and re-run a failing API call from the same planner state. Note that this example is expected to throw an error.

 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        const auto unsuccessfulResult = planner.path(
48            Zivid::Motion::InitialState{ startConfiguration },
49            Zivid::Motion::PathRequest{ .goalConfigurations = { invalidGoalConfiguration },
50                                        .description = "Path with unreachable goal" });
51        std::cout << "Path result has status: " << unsuccessfulResult.toString() << "\n";
52
53        // Some operation that fails, like creating an InitialState from an unsuccessful path result
54        const Zivid::Motion::InitialState initialState{ unsuccessfulResult };
55        const auto result = planner.path(
56            Zivid::Motion::InitialState{ startConfiguration },
57            Zivid::Motion::PathRequest{ .goalConfigurations = { startConfiguration },
58                                        .description = "Path with invalid initial state" });
59        std::cout << result << "\n";
60    }
61    catch(const std::exception &exception)
62    {
63        const std::filesystem::path outputFolder = "my/desired/output/folder";
64
65        const auto logPath = planner.exportApiLog(outputFolder);
66        const auto packagePath = Zivid::Motion::packageApiLog(app, logPath);
67        std::cerr << "Planner failed, debug package available at: " + std::filesystem::canonical(packagePath).string()
68                  << std::endl;
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 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 as e:
43        output_folder = Path("my/desired/output/folder")
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        raise RuntimeError(f"Planner failed, debug package available at: {package_path.resolve()}") from e
49
50
51if __name__ == "__main__":
52    _main()