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

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

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

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

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        Zivid::Motion::PathRequest pathRequest{};
52        pathRequest.goals = Zivid::Motion::Goals::fromConfigurations({ 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 (
 4    Application,
 5    Configuration,
 6    Goals,
 7    InitialState,
 8    PathRequest,
 9    Planner,
10    PlannerSettings,
11    Profile,
12    Visualizer,
13)
14
15USE_VISUALIZER = True
16
17
18def _start_planner(app: Application) -> Planner:
19    planner_settings = PlannerSettings(cell_name="demo_cell", profile=Profile.testing)
20    return app.create_planner(planner_settings)
21
22
23def _main() -> None:
24    app = Application()
25
26    print("Starting planner")
27    planner = _start_planner(app)
28    visualizer: Optional[Visualizer] = Visualizer.view_planner(planner) if USE_VISUALIZER else None
29
30    start_configuration = Configuration([0.0, 0.0, 0.0, 0.0, 1.57, 0.0])
31    goal_configuration = Configuration([1.57, 0.0, 0.0, 0.0, 1.57, 0.0])
32
33    result = planner.path(
34        InitialState(start_configuration),
35        PathRequest(
36            goals=Goals.from_configurations([goal_configuration]),
37            description="Path with joint goal",
38        ),
39    )
40    if not result:
41        raise RuntimeError(f"Planning failed with error: {result.error}")
42
43    print(f"Path result: \n{result}\n")
44
45    if visualizer is not None:
46        print("Close the window to exit.")
47        visualizer.wait()
48
49
50if __name__ == "__main__":
51    _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        const Zivid::Motion::Pose poseGoal{ Zivid::Motion::Matrix4x4{
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 jointGoal = planner.computeInverseKinematics({ poseGoal }, startConfiguration);
56        if(jointGoal.noneValid())
57        {
58            throw std::runtime_error("No valid inverse kinematics solution found");
59        }
60
61        Zivid::Motion::PathRequest pathRequest{};
62        pathRequest.goals = jointGoal;
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
 3from zividmotion import (
 4    Application,
 5    Configuration,
 6    InitialState,
 7    PathRequest,
 8    Planner,
 9    PlannerSettings,
10    Pose,
11    Profile,
12    Visualizer,
13)
14
15USE_VISUALIZER = True
16
17
18def _start_planner(app: Application) -> Planner:
19    planner_settings = PlannerSettings(cell_name="demo_cell", profile=Profile.testing)
20    return app.create_planner(planner_settings)
21
22
23def _main() -> None:
24    app = Application()
25
26    print("Starting planner")
27    planner = _start_planner(app)
28    visualizer: Optional[Visualizer] = Visualizer.view_planner(planner) if USE_VISUALIZER else None
29
30    start_configuration = Configuration([0.0, 0.0, 0.0, 0.0, 1.57, 0.0])
31    pose_goal = Pose(
32        [
33            [0.0, -1.0, 0.0, 0.0],
34            [-1.0, 0.0, 0.0, 1.45],
35            [0.0, 0.0, -1.0, 1.63],
36            [0.0, 0.0, 0.0, 1.0],
37        ]
38    )
39
40    joint_goal = planner.compute_inverse_kinematics(
41        poses=[pose_goal],
42        reference_configuration=start_configuration,
43    )
44    if joint_goal.none_valid():
45        raise RuntimeError("No valid inverse kinematics solution found")
46
47    result = planner.path(
48        InitialState(start_configuration),
49        PathRequest(
50            goals=joint_goal,
51            description="Path with goal from pose",
52        ),
53    )
54    if not result:
55        raise RuntimeError(f"Planning failed with error: {result.error}")
56
57    print(f"Path result: \n{result}\n")
58
59    if visualizer is not None:
60        print("Close the window to exit.")
61        visualizer.wait()
62
63
64if __name__ == "__main__":
65    _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 auto goals = Zivid::Motion::Goals::fromConfigurations({ goalFarAway, goalCloser, goalClosest });
55
56        Zivid::Motion::PathRequest shortestPathRequest{};
57        shortestPathRequest.goals = 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.goals = 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 (
 4    Application,
 5    Configuration,
 6    Goals,
 7    InitialState,
 8    PathRequest,
 9    Planner,
10    PlannerSettings,
11    Profile,
12    Visualizer,
13)
14
15USE_VISUALIZER = True
16
17
18def _start_planner(app: Application) -> Planner:
19    planner_settings = PlannerSettings(cell_name="demo_cell", profile=Profile.testing)
20    return app.create_planner(planner_settings)
21
22
23def _main() -> None:
24    app = Application()
25
26    print("Starting planner")
27    planner = _start_planner(app)
28    visualizer: Optional[Visualizer] = Visualizer.view_planner(planner) if USE_VISUALIZER else None
29
30    start_configuration = Configuration([0.0, 0.0, 0.0, 0.0, 1.57, 0.0])
31    initial_state = InitialState(start_configuration)
32
33    goal_far_away = Configuration([3.14, 0.0, 0.0, 0.0, 1.57, 0.0])
34    goal_closer = Configuration([1.57, 0.1, -0.1, 0.0, 1.57, 0.0])
35    goal_closest = Configuration([1.57, 0.0, 0.0, 0.0, 1.57, 0.0])
36    goals = Goals.from_configurations([goal_far_away, goal_closer, goal_closest])
37
38    result_shortest_path = planner.path(
39        initial_state,
40        PathRequest(
41            goals=goals,
42            description="Multiple goals - Default (ShortestPath) prioritization",
43        ),
44    )
45    if not result_shortest_path:
46        raise RuntimeError(f"Planning with ShortestPath failed with error: {result_shortest_path.error}")
47    assert result_shortest_path.selected_goal_idx == 2
48    print(f"ShortestPath selected goal index: {result_shortest_path.selected_goal_idx}")
49
50    result_list_order = planner.path(
51        initial_state,
52        PathRequest(
53            goal_prioritization_method=PathRequest.GoalPrioritizationMethod.listOrder,
54            goals=goals,
55            description="Multiple goals - ListOrder prioritization",
56        ),
57    )
58    if not result_list_order:
59        raise RuntimeError(f"Planning with ListOrder failed with error: {result_list_order.error}")
60    assert result_list_order.selected_goal_idx == 0
61    print(f"ListOrder selected goal index: {result_list_order.selected_goal_idx}")
62
63    if visualizer is not None:
64        print("Close the window to exit.")
65        visualizer.wait()
66
67
68if __name__ == "__main__":
69    _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} // 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        const Zivid::Motion::InitialState initialState{ startConfiguration };
 51        const auto goal = Zivid::Motion::Goals::fromConfigurations({ goalConfiguration });
 52
 53        Zivid::Motion::PathRequest withoutObstacleRequest{};
 54        withoutObstacleRequest.goals = goal;
 55        withoutObstacleRequest.description = "Without obstacle";
 56        const auto resultWithoutObstacle = planner.path(initialState, withoutObstacleRequest);
 57        if(!resultWithoutObstacle)
 58        {
 59            throw std::runtime_error(
 60                "Planning before obstacle is set failed with result: " + resultWithoutObstacle.toString());
 61        }
 62        assert(resultWithoutObstacle.path().size() == 1);
 63        std::cout << "Path before obstacle is set: " << resultWithoutObstacle.path().size() << " waypoint\n";
 64
 65        // Set an obstacle that obstructs the direct path
 66        const auto boxMesh = Zivid::Motion::Mesh::createBox(Zivid::Motion::Vector3f{ 0.2f, 0.2f, 0.2f })
 67                                 .transformInPlace(
 68                                     Zivid::Motion::Pose{ Zivid::Motion::Matrix4x4{
 69                                         { 1.f, 0.f, 0.f, 1.1f },
 70                                         { 0.f, 1.f, 0.f, 1.0f },
 71                                         { 0.f, 0.f, 1.f, 1.6f },
 72                                         { 0.f, 0.f, 0.f, 1.f },
 73                                     } });
 74        planner.setObstacles({ Zivid::Motion::Obstacle::fromMesh("box_obstacle", boxMesh) });
 75
 76        Zivid::Motion::PathRequest withObstacleRequest{};
 77        withObstacleRequest.goals = goal;
 78        withObstacleRequest.description = "With obstacle";
 79        const auto resultWithObstacle = planner.path(initialState, withObstacleRequest);
 80        if(!resultWithObstacle)
 81        {
 82            throw std::runtime_error(
 83                "Planning after obstacle is set failed with result: " + resultWithObstacle.toString());
 84        }
 85        assert(resultWithObstacle.path().size() > 1);
 86        std::cout << "Path after obstacle is set: " << resultWithObstacle.path().size() << " waypoints\n";
 87
 88        if(useVisualizer)
 89        {
 90            std::cout << "Close the window to exit.\n";
 91            visualizer->wait();
 92        }
 93    }
 94    catch(const std::exception &exception)
 95    {
 96        printException(exception);
 97        std::cout << "Press enter to exit." << std::endl;
 98        std::cin.get();
 99        return EXIT_FAILURE;
100    }
101    return EXIT_SUCCESS;
102}

 1from typing import Optional
 2
 3from zividmotion import (
 4    Application,
 5    Configuration,
 6    Goals,
 7    InitialState,
 8    Mesh,
 9    Obstacle,
10    PathRequest,
11    Planner,
12    PlannerSettings,
13    Pose,
14    Profile,
15    Vector3f,
16    Visualizer,
17)
18
19USE_VISUALIZER = True
20
21
22def _start_planner(app: Application) -> Planner:
23    planner_settings = PlannerSettings(cell_name="demo_cell", profile=Profile.testing)
24    return app.create_planner(planner_settings)
25
26
27def _main() -> None:
28    app = Application()
29
30    print("Starting planner")
31    planner = _start_planner(app)
32    visualizer: Optional[Visualizer] = Visualizer.view_planner(planner) if USE_VISUALIZER else None
33
34    start_configuration = Configuration([0.0, 0.0, 0.0, 0.0, 1.57, 0.0])
35    goal_configuration = Configuration([1.57, 0.0, 0.0, 0.0, 1.57, 0.0])
36    initial_state = InitialState(start_configuration=start_configuration)
37    goal = Goals.from_configurations([goal_configuration])
38
39    result_without_obstacle = planner.path(
40        initial_state=initial_state,
41        request=PathRequest(
42            goals=goal,
43            description="Without obstacle",
44        ),
45    )
46    if not result_without_obstacle:
47        raise RuntimeError(f"Planning before obstacle is set failed with error: {result_without_obstacle.error}")
48    assert len(result_without_obstacle.path) == 1
49    print(f"Path before obstacle is set: {len(result_without_obstacle.path)} waypoint")
50
51    # Set an obstacle that obstructs the direct path
52    box_mesh = Mesh.create_box(Vector3f(0.2, 0.2, 0.2)).transform_in_place(
53        Pose(
54            [
55                [1.0, 0.0, 0.0, 1.1],
56                [0.0, 1.0, 0.0, 1.0],
57                [0.0, 0.0, 1.0, 1.6],
58                [0.0, 0.0, 0.0, 1.0],
59            ]
60        )
61    )
62    planner.set_obstacles(
63        obstacles=[
64            Obstacle.from_mesh(name="box_obstacle", mesh=box_mesh),
65        ]
66    )
67
68    result_with_obstacle = planner.path(
69        initial_state=initial_state,
70        request=PathRequest(
71            goals=goal,
72            description="With obstacle",
73        ),
74    )
75    if not result_with_obstacle:
76        raise RuntimeError(f"Planning after obstacle is set failed with error: {result_with_obstacle.error}")
77    assert len(result_with_obstacle.path) > 1
78    print(f"Path after obstacle is set: {len(result_with_obstacle.path)} waypoints")
79
80    if visualizer is not None:
81        print("Close the window to exit.")
82        visualizer.wait()
83
84
85if __name__ == "__main__":
86    _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::Vector3f{ 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(
 97            { Zivid::Motion::Obstacle::fromMesh("cad_obstacle", Zivid::Motion::Mesh::fromTriangles(triangles)) });
 98
 99        if(useVisualizer)
100        {
101            std::cout << "Close the window to exit.\n";
102            visualizer->wait();
103        }
104    }
105    catch(const std::exception &exception)
106    {
107        printException(exception);
108        std::cout << "Press enter to exit." << std::endl;
109        std::cin.get();
110        return EXIT_FAILURE;
111    }
112    return EXIT_SUCCESS;
113}

Install additional dependencies with:

pip install trimesh

 1from pathlib import Path
 2from typing import Optional
 3
 4import numpy as np
 5import trimesh
 6from zividmotion import (
 7    Application,
 8    Mesh,
 9    Obstacle,
10    Planner,
11    PlannerSettings,
12    Profile,
13    Triangles,
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
25def _load_triangles_from_cad_file(cad_file_path: Path) -> Triangles:
26    mesh = trimesh.load(cad_file_path)
27    return Triangles(mesh.triangles.astype(np.float32))
28
29
30def _main() -> None:
31    app = Application()
32
33    print("Starting planner")
34    planner = _start_planner(app)
35    visualizer: Optional[Visualizer] = Visualizer.view_planner(planner) if USE_VISUALIZER else None
36
37    # Make sure the CAD uses meters as unit, not millimeters
38    cad_file_path = Path("path/to/obstacle.stl")
39
40    print("Loading CAD file from:", cad_file_path)
41    triangles = _load_triangles_from_cad_file(cad_file_path)
42
43    planner.set_obstacles(
44        obstacles=[
45            Obstacle.from_mesh(name="cad_obstacle", mesh=Mesh.from_triangles(triangles)),
46        ]
47    )
48
49    if visualizer is not None:
50        print("Close the window to exit.")
51        visualizer.wait()
52
53
54if __name__ == "__main__":
55    _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        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::Vector3f> 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::Vector3f{ 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 = Obstacle.PointCloud(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    colors = Obstacle.Colors(unorganized_point_cloud.copy_data("rgba"))
69    colored_obstacle = Obstacle.from_colored_point_cloud(name="colored_obstacle", points=points, colors=colors)
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()

Interaction Planning

Simple Touch Approach

This example shows how to use the Touch functionality with a compliant replaceable tool to plan an approach to an object to be interacted with in a simplified setup.

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

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    Configuration,
  8    InitialState,
  9    Matrix4x4,
 10    Mesh,
 11    Obstacle,
 12    PathRequest,
 13    PathResult,
 14    Planner,
 15    PlannerSettings,
 16    Pose,
 17    Profile,
 18    Tcp,
 19    Vector3f,
 20    Visualizer,
 21)
 22
 23USE_VISUALIZER = True
 24
 25
 26def _start_planner(app: Application) -> Planner:
 27    planner_settings = PlannerSettings(cell_name="demo_cell", profile=Profile.testing)
 28    return app.create_planner(planner_settings)
 29
 30
 31# Utility method for creating a dummy obstacle
 32def _generate_sphere_points(center_point: list[float], radius: float, num_points: int) -> Obstacle.PointCloud:
 33    np.random.seed(42)  # Set a fixed random seed for determinism
 34    angles = np.random.uniform(0.0, 1.0, size=(num_points, 2))
 35    theta = angles[:, 0] * 2 * np.pi
 36    phi = angles[:, 1] * np.pi
 37    points = np.empty((num_points, 3), dtype=np.float32)
 38    points[:, 0] = center_point[0] + radius * np.sin(phi) * np.cos(theta)
 39    points[:, 1] = center_point[1] + radius * np.sin(phi) * np.sin(theta)
 40    points[:, 2] = center_point[2] + radius * np.cos(phi)
 41    return Obstacle.PointCloud(points)
 42
 43
 44def _get_pick_pose(pick_point: list[float]) -> Pose:
 45    # Create a simple pick pose with the end-effector pointing downwards at the pick point
 46    pick_pose = np.eye(4, dtype=np.float32)
 47    pick_pose[:3, 3] = pick_point
 48    # Rotate 180 degrees around Y-axis to point downwards
 49    pick_pose[:3, :3] = Rotation.from_rotvec([0, np.pi, 0]).as_matrix()
 50    return Pose(pick_pose)
 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    start_configuration = Configuration([0.0, 0.0, 0.0, 0.0, 1.57, 0.0])
 61
 62    # Set a dummy obstacle to be interacted with
 63    center_point = [1.1, 1.0, 0.9]
 64    radius = 0.2
 65    planner.set_obstacles(
 66        obstacles=[
 67            Obstacle.from_point_cloud(
 68                name="interaction_object",
 69                points=_generate_sphere_points(center_point=center_point, radius=radius, num_points=1000),
 70            )
 71        ]
 72    )
 73
 74    # Set a tool that simulates a vacuum gripper with a suction cup with 2 cm compliance
 75    tool_base_dimensions = Vector3f(0.15, 0.15, 0.15)
 76    suction_cup_matrix = Matrix4x4.identity()
 77    suction_cup_matrix[2, 3] = tool_base_dimensions.z
 78    suction_cup_dimensions = Vector3f(0.05, 0.05, 0.02)
 79    rigid_tool = Mesh.create_box(tool_base_dimensions).bottom_center_transform_in_place()
 80    compliant_tool = (
 81        Mesh.create_box(suction_cup_dimensions)
 82        .bottom_center_transform_in_place()
 83        .transform_in_place(Pose(suction_cup_matrix))
 84    )
 85    planner.set_replaceable_tool(replaceable_tool=rigid_tool, compliant_section=compliant_tool)
 86
 87    # Set the new tcp to be at the center tip of the gripper, when the suction cup is compressed 1 cm for good contact
 88    tcp_matrix = Matrix4x4.identity()
 89    tcp_matrix[2, 3] += tool_base_dimensions.z + suction_cup_dimensions.z - 0.01
 90    planner.set_tcp(tcp=Tcp(transform=Pose(tcp_matrix), tool_direction=Vector3f(0.0, 0.0, 1.0)))
 91
 92    # Find the pick joint configuration, gripping the top of the object with the new TCP
 93    pick_point = [center_point[0], center_point[1], center_point[2] + radius]
 94    pick_pose = _get_pick_pose(pick_point)
 95
 96    pick_goal = planner.compute_inverse_kinematics(poses=[pick_pose], reference_configuration=start_configuration)
 97    if pick_goal.none_valid():
 98        raise RuntimeError("No valid inverse kinematics solution found for pick pose")
 99
100    initial_state = InitialState(start_configuration=start_configuration)
101    # Path type defaults to Free
102    result_free = planner.path(
103        initial_state=initial_state,
104        request=PathRequest(
105            goals=pick_goal,
106            description="Free path to pick goal",
107        ),
108    )
109    # Expect blocked end with path type Free, since the compliant part of the tool has to enter the obstacle for the
110    # TCP to reach the goal
111    assert result_free.error == PathResult.Error.blockedEnd
112
113    # Specify type touch
114    result_touch = planner.path(
115        initial_state=initial_state,
116        request=PathRequest(
117            type=PathRequest.Type.touch,
118            goals=pick_goal,
119            description="Touch path to pick goal",
120        ),
121    )
122    if not result_touch:
123        raise RuntimeError(f"Planning with path type Touch failed with error: {result_touch.error}")
124
125    print("\nSuccessful touch path:", result_touch.path)
126
127    if visualizer is not None:
128        print("Close the window to exit.")
129        visualizer.wait()
130
131
132if __name__ == "__main__":
133    _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/Mesh.h>
  3#include <Zivid/Motion/Planner.h>
  4#include <Zivid/Motion/Visualizer.h>
  5
  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::Vector3f>
 40    generateSpherePoints(const Zivid::Motion::Vector3f &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::Vector3f> 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    std::vector<Zivid::Motion::Obstacle> getDummyObstacles(const Zivid::Motion::Vector3f &center, const float radius)
 64    {
 65        std::vector<Zivid::Motion::Obstacle> obstacles;
 66        for(const float dx : { -2.f * radius, 0.f, 2.f * radius })
 67        {
 68            for(const float dy : { -2.f * radius, 0.f, 2.f * radius })
 69            {
 70                const Zivid::Motion::Vector3f c{ center.x + dx, center.y + dy, center.z };
 71                const auto name = "obstacle_" + std::to_string(obstacles.size());
 72                obstacles.push_back(
 73                    Zivid::Motion::Obstacle::fromPointCloud(
 74                        name, Zivid::Motion::Obstacle::PointCloud{ generateSpherePoints(c, radius, 400) }));
 75            }
 76        }
 77        return obstacles;
 78    }
 79} // namespace
 80
 81int main()
 82{
 83    try
 84    {
 85        const Zivid::Motion::Application app;
 86
 87        std::cout << "Starting planner\n";
 88        auto planner = startPlanner(app);
 89        auto visualizer =
 90            useVisualizer ? std::optional{ Zivid::Motion::Visualizer::viewPlanner(planner) } : std::nullopt;
 91
 92        constexpr Zivid::Motion::Vector3f obstacleCenter{ 1.1f, 1.0f, 0.2f };
 93        constexpr float obstacleRadius = 0.15f;
 94        planner.setObstacles(getDummyObstacles(obstacleCenter, obstacleRadius));
 95
 96        // For example purposes, simulate a tool mounted asymmetrically at 45 degrees with a compliant tip
 97        // (e.g. a vacuum gripper with compliant suction cups)
 98        constexpr float angle = M_PI_4;
 99        const float cosA = std::cos(angle);
100        const float sinA = std::sin(angle);
101        const Zivid::Motion::Pose toolTransform{ Zivid::Motion::Matrix4x4{
102            { cosA, -sinA, 0.f, 0.f },
103            { sinA, cosA, 0.f, 0.f },
104            { 0.f, 0.f, 1.f, 0.05f },
105            { 0.f, 0.f, 0.f, 1.f },
106        } };
107        constexpr Zivid::Motion::Vector3f rigidToolDimensions{ 0.35f, 0.35f, 0.04f };
108        // Specify the last 2cm of the tool as the compliant area
109        const Zivid::Motion::Pose compliantToolTransform{ Zivid::Motion::Matrix4x4{
110            { 1.f, 0.f, 0.f, 0.f },
111            { 0.f, 1.f, 0.f, 0.f },
112            { 0.f, 0.f, 1.f, rigidToolDimensions.z },
113            { 0.f, 0.f, 0.f, 1.f },
114        } };
115        constexpr Zivid::Motion::Vector3f compliantToolDimensions{ 0.4f, 0.4f, 0.02f };
116        auto rigidTool = Zivid::Motion::Mesh::createBox(rigidToolDimensions)
117                             .bottomCenterTransformInPlace()
118                             .transformInPlace(toolTransform);
119        auto compliantTool = Zivid::Motion::Mesh::createBox(compliantToolDimensions)
120                                 .bottomCenterTransformInPlace()
121                                 .transformInPlace(compliantToolTransform)
122                                 .transformInPlace(toolTransform);
123        planner.setReplaceableTool(rigidTool, compliantTool);
124
125        // Set the new tcp to be at the center tip of the tool, with the compliant area compressed by 1cm for good contact
126        auto tcpTransform = toolTransform.toMatrix();
127        tcpTransform(2, 3) += rigidToolDimensions.z + compliantToolDimensions.z - 0.01f;
128        planner.setTcp(Zivid::Motion::Tcp{ Zivid::Motion::Pose{ tcpTransform }, { 0.f, 0.f, 1.f } });
129
130        const Zivid::Motion::Configuration startConfiguration{ 0.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
131
132        // Define a pick pose on the top center of the object with the end-effector rotated 180 degrees around Y to point downwards
133        const Zivid::Motion::Pose pickPose{ Zivid::Motion::Matrix4x4{
134            { -1.f, 0.f, 0.f, obstacleCenter.x },
135            { 0.f, 1.f, 0.f, obstacleCenter.y },
136            { 0.f, 0.f, -1.f, obstacleCenter.z + obstacleRadius },
137            { 0.f, 0.f, 0.f, 1.f },
138        } };
139        // Compute the pick joint configuration with the new tcp
140        const auto pickGoal = planner.computeInverseKinematics({ pickPose }, startConfiguration);
141        if(pickGoal.noneValid())
142        {
143            throw std::runtime_error("No valid inverse kinematics solution found for pick pose");
144        }
145
146        // Compute a pick approach path with the new tool
147        Zivid::Motion::PathRequest approachRequest{};
148        approachRequest.type = Zivid::Motion::PathRequest::Type::touch;
149        approachRequest.goals = pickGoal;
150        approachRequest.description = "Pick approach";
151        const auto approachResult = planner.path(Zivid::Motion::InitialState{ startConfiguration }, approachRequest);
152        if(!approachResult)
153        {
154            throw std::runtime_error("Planning pick approach failed with result: " + approachResult.toString());
155        }
156
157        // Set carried object (bounding box around the picked obstacle)
158        constexpr float objectSize = obstacleRadius * 2.f;
159        planner.setCarriedObject(
160            Zivid::Motion::Mesh::createBox(Zivid::Motion::Vector3f{ objectSize, objectSize, objectSize })
161                .bottomCenterTransformInPlace());
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        Zivid::Motion::PathRequest retractRequest{};
168        retractRequest.retractDirection = { 0.f, 0.f, 1.f };
169        retractRequest.goals = Zivid::Motion::Goals::fromConfigurations({ 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}

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    Configuration,
  8    Goals,
  9    InitialState,
 10    Matrix4x4,
 11    Mesh,
 12    Obstacle,
 13    PathRequest,
 14    Planner,
 15    PlannerSettings,
 16    Pose,
 17    Profile,
 18    Tcp,
 19    Vector3f,
 20    Visualizer,
 21)
 22
 23USE_VISUALIZER = True
 24
 25
 26def _start_planner(app: Application) -> Planner:
 27    planner_settings = PlannerSettings(cell_name="demo_cell", profile=Profile.testing)
 28    return app.create_planner(planner_settings)
 29
 30
 31# Utility method for creating a dummy obstacle
 32def _generate_sphere_points(center_point: list[float], radius: float, num_points: int) -> Obstacle.PointCloud:
 33    np.random.seed(42)  # Set a fixed random seed for determinism
 34    angles = np.random.uniform(0.0, 1.0, size=(num_points, 2))
 35    theta = angles[:, 0] * 2 * np.pi
 36    phi = angles[:, 1] * np.pi
 37    points = np.empty((num_points, 3), dtype=np.float32)
 38    points[:, 0] = center_point[0] + radius * np.sin(phi) * np.cos(theta)
 39    points[:, 1] = center_point[1] + radius * np.sin(phi) * np.sin(theta)
 40    points[:, 2] = center_point[2] + radius * np.cos(phi)
 41    return Obstacle.PointCloud(points)
 42
 43
 44def _get_dummy_obstacles(center_point: list[float], radius: float) -> list[Obstacle]:
 45    obstacles: list[Obstacle] = []
 46    for x in [center_point[0] - 2 * radius, center_point[0], center_point[0] + 2 * radius]:
 47        for y in [center_point[1] - 2 * radius, center_point[1], center_point[1] + 2 * radius]:
 48            center = [x, y, center_point[2]]
 49            points = _generate_sphere_points(center_point=center, radius=radius, num_points=400)
 50            obstacles.append(Obstacle.from_point_cloud(name=f"obstacle_{len(obstacles)}", points=points))
 51    return obstacles
 52
 53
 54def _main() -> None:
 55    app = Application()
 56
 57    print("Starting planner")
 58    planner = _start_planner(app)
 59    visualizer: Optional[Visualizer] = Visualizer.view_planner(planner) if USE_VISUALIZER else None
 60
 61    obstacle_center = [1.1, 1.0, 0.2]
 62    obstacle_radius = 0.15
 63    planner.set_obstacles(obstacles=_get_dummy_obstacles(center_point=obstacle_center, radius=obstacle_radius))
 64
 65    # For example purposes, simulate a tool mounted asymmetrically at 45 degrees with a compliant tip
 66    # (e.g. a vacuum gripper with compliant suction cups)
 67    tool_matrix = np.eye(4)
 68    tool_matrix[1, 3] = 0.05
 69    tool_matrix[:3, :3] = Rotation.from_rotvec([0, 0, np.pi / 4]).as_matrix()
 70    rigid_tool_dimensions = Vector3f(0.35, 0.35, 0.04)
 71    # Specify the last 2cm of the tool as the compliant area
 72    compliant_matrix = Matrix4x4.identity()
 73    compliant_matrix[2, 3] = rigid_tool_dimensions.z
 74    compliant_tool_dimensions = Vector3f(0.35, 0.35, 0.02)
 75    rigid_tool = (
 76        Mesh.create_box(rigid_tool_dimensions).bottom_center_transform_in_place().transform_in_place(Pose(tool_matrix))
 77    )
 78    compliant_tool = (
 79        Mesh.create_box(compliant_tool_dimensions)
 80        .bottom_center_transform_in_place()
 81        .transform_in_place(Pose(tool_matrix @ compliant_matrix))
 82    )
 83    planner.set_replaceable_tool(replaceable_tool=rigid_tool, compliant_section=compliant_tool)
 84
 85    # Set the new tcp to be at the center tip of the tool, with the compliant area compressed by 1cm for good contact
 86    tcp_matrix = tool_matrix.copy()
 87    tcp_matrix[2, 3] += rigid_tool_dimensions.z + compliant_tool_dimensions.z - 0.01
 88    planner.set_tcp(tcp=Tcp(transform=Pose(tcp_matrix), tool_direction=Vector3f(0.0, 0.0, 1.0)))
 89
 90    start_configuration = Configuration([0.0, 0.0, 0.0, 0.0, 1.57, 0.0])
 91
 92    # Define a pick pose on the top center of the object with the end-effector rotated 180 degrees around Y to point downwards
 93    pick_position = [obstacle_center[0], obstacle_center[1], obstacle_center[2] + obstacle_radius]
 94    pick_matrix = np.eye(4)
 95    pick_matrix[:3, 3] = pick_position
 96    pick_matrix[:3, :3] = Rotation.from_rotvec([0, np.pi, 0]).as_matrix()
 97    pick_pose = Pose(pick_matrix)
 98
 99    # Compute the pick joint configuration with the new tcp
100    pick_goal = planner.compute_inverse_kinematics(poses=[pick_pose], reference_configuration=start_configuration)
101    if pick_goal.none_valid():
102        raise RuntimeError("No valid inverse kinematics solution found for pick pose")
103
104    # Compute a pick approach path with the new tool
105    approach_result = planner.path(
106        initial_state=InitialState(start_configuration=start_configuration),
107        request=PathRequest(
108            type=PathRequest.Type.touch,
109            goals=pick_goal,
110            description="Pick approach",
111        ),
112    )
113    if not approach_result:
114        raise RuntimeError(f"Planning pick approach failed with error: {approach_result.error}")
115
116    # Set carried object (bounding box around the picked obstacle)
117    obstacle_size = obstacle_radius * 2
118    planner.set_carried_object(
119        carried_object=Mesh.create_box(
120            Vector3f(obstacle_size, obstacle_size, obstacle_size)
121        ).bottom_center_transform_in_place()
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            retract_direction=Vector3f(0.0, 0.0, 1.0),
132            goals=Goals.from_configurations([start_configuration]),
133            description="Pick retract",
134        ),
135    )
136    if not retract_result:
137        raise RuntimeError(f"Planning pick retract failed with error: {retract_result.error}")
138
139    print(retract_result)
140
141    if visualizer is not None:
142        print("Close the window to exit.")
143        visualizer.wait()
144
145
146if __name__ == "__main__":
147    _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::Vector3f>
 40    generateSpherePoints(const Zivid::Motion::Vector3f &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::Vector3f> 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        const auto goal = Zivid::Motion::Goals::fromConfigurations({ goalConfiguration });
 88
 89        Zivid::Motion::PathRequest withoutAttachmentRequest{};
 90        withoutAttachmentRequest.goals = goal;
 91        withoutAttachmentRequest.description = "Path without attachment";
 92        const auto resultWithoutAttachment = planner.path(initialState, withoutAttachmentRequest);
 93        if(!resultWithoutAttachment)
 94        {
 95            throw std::runtime_error(
 96                "Planning without active attachment failed with result: " + resultWithoutAttachment.toString());
 97        }
 98        assert(resultWithoutAttachment.path().size() == 1);
 99        std::cout << "Path without attachment: " << resultWithoutAttachment.path().size() << " waypoints\n";
100
101        std::cout << "Setting attachment " << attachmentName << "\n";
102        planner.setAttachments({ attachmentName });
103
104        Zivid::Motion::PathRequest withAttachmentRequest{};
105        withAttachmentRequest.goals = goal;
106        withAttachmentRequest.description = "Path with attachment";
107        const auto resultWithAttachment = planner.path(initialState, withAttachmentRequest);
108        if(!resultWithAttachment)
109        {
110            std::cout << "Planning with active attachment failed with result: " << resultWithAttachment.toString()
111                      << "\n";
112        }
113        assert(resultWithAttachment.path().size() > 1);
114        std::cout << "With attachment: " << resultWithAttachment.path().size() << " waypoints\n";
115
116        if(useVisualizer)
117        {
118            std::cout << "Close the window to exit.\n";
119            visualizer->wait();
120        }
121    }
122    catch(const std::exception &exception)
123    {
124        printException(exception);
125        std::cout << "Press enter to exit." << std::endl;
126        std::cin.get();
127        return EXIT_FAILURE;
128    }
129    return EXIT_SUCCESS;
130}

 1from typing import Optional
 2
 3import numpy as np
 4from zividmotion import (
 5    Application,
 6    Configuration,
 7    Goals,
 8    InitialState,
 9    Obstacle,
10    PathRequest,
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) -> Obstacle.PointCloud:
27    np.random.seed(42)  # Set a fixed random seed for determinism
28    angles = np.random.uniform(0.0, 1.0, size=(num_points, 2))
29    theta = angles[:, 0] * 2 * np.pi
30    phi = angles[:, 1] * np.pi
31    points = np.empty((num_points, 3), dtype=np.float32)
32    points[:, 0] = center_point[0] + radius * np.sin(phi) * np.cos(theta)
33    points[:, 1] = center_point[1] + radius * np.sin(phi) * np.sin(theta)
34    points[:, 2] = center_point[2] + radius * np.cos(phi)
35    return Obstacle.PointCloud(points)
36
37
38def _main() -> None:
39    app = Application()
40
41    print("Starting planner")
42    planner = _start_planner(app)
43    visualizer: Optional[Visualizer] = Visualizer.view_planner(planner) if USE_VISUALIZER else None
44
45    start_configuration = Configuration([0.0, 0.0, 0.0, 0.0, 1.57, 0.0])
46    goal_configuration = Configuration([1.57, 0.0, 0.0, 0.0, 1.57, 0.0])
47    attachment_name = "demo_attachment"  # Must match a name in the attachments definition in the planner config file
48
49    # Set an obstacle that obstructs the direct path for the attachment
50    planner.set_obstacles(
51        obstacles=[
52            Obstacle.from_point_cloud(
53                name="sphere_obstacle",
54                points=_generate_sphere_points(center_point=[1.1, 1.0, 1.2], radius=0.2, num_points=1000),
55            )
56        ]
57    )
58
59    initial_state = InitialState(start_configuration=start_configuration)
60    goal = Goals.from_configurations([goal_configuration])
61
62    result_without_attachment = planner.path(
63        initial_state=initial_state,
64        request=PathRequest(
65            goals=goal,
66            description="Path without attachment",
67        ),
68    )
69    if not result_without_attachment:
70        raise RuntimeError(f"Planning without active attachment failed with error: {result_without_attachment.error}")
71    assert len(result_without_attachment.path) == 1
72    print(f"Path without attachment: {len(result_without_attachment.path)} waypoints")
73
74    print("Setting attachment", attachment_name)
75    planner.set_attachments(attachments=[attachment_name])
76
77    result_with_attachment = planner.path(
78        initial_state=initial_state,
79        request=PathRequest(
80            goals=goal,
81            description="Path with attachment",
82        ),
83    )
84    if not result_with_attachment:
85        print(f"Planning with active attachment failed with error: {result_with_attachment.error}")
86    assert len(result_with_attachment.path) > 1
87    print(f"Path with attachment: {len(result_with_attachment.path)} waypoints")
88
89    if visualizer is not None:
90        print("Close the window to exit.")
91        visualizer.wait()
92
93
94if __name__ == "__main__":
95    _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        Zivid::Motion::PathRequest unreachableRequest{};
48        unreachableRequest.goals = Zivid::Motion::Goals::fromConfigurations({ 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.goals = Zivid::Motion::Goals::fromConfigurations({ 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 (
 4    Application,
 5    Configuration,
 6    Goals,
 7    InitialState,
 8    PathRequest,
 9    Planner,
10    PlannerSettings,
11    Profile,
12    package_api_log,
13)
14
15
16def _start_planner(app: Application) -> Planner:
17    planner_settings = PlannerSettings(cell_name="demo_cell", profile=Profile.testing)
18    return app.create_planner(planner_settings)
19
20
21def _main() -> None:
22    app = Application()
23
24    print("Starting planner")
25    planner = _start_planner(app)
26
27    start_configuration = Configuration([0, 0, 0, 0, 1.57, 0])
28    invalid_goal_configuration = Configuration([100, 0, 0, 0, 1.57, 0])
29
30    unsuccessful_result = planner.path(
31        initial_state=InitialState(start_configuration=start_configuration),
32        request=PathRequest(
33            goals=Goals.from_configurations([invalid_goal_configuration]),
34            description="Path with unreachable goal",
35        ),
36    )
37    assert unsuccessful_result.error is not None
38    print("Path result has error: ", unsuccessful_result.error)
39
40    try:
41        # Some operation that fails, like creating an InitialState from an unsuccessful path result
42        initial_state = InitialState(previous_result=unsuccessful_result)
43        result = planner.path(
44            initial_state=initial_state,
45            request=PathRequest(
46                goals=Goals.from_configurations([start_configuration]),
47                description="Path with invalid initial state",
48            ),
49        )
50        print(result)
51
52    except Exception:
53        output_folder = Path("/tmp")
54
55        log_path = planner.export_api_log(output_directory=output_folder)
56        package_path = package_api_log(application=app, api_log_path=log_path)
57
58        print(f"Planner failed, debug package available at: {package_path.resolve()}")
59
60
61if __name__ == "__main__":
62    _main()