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 = false;
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{ std::vector{ goalConfiguration } };
52        pathRequest.description = "Path with joint goal";
53        const auto result = planner.path(Zivid::Motion::InitialState{ startConfiguration }, pathRequest);
54        if(!result)
55        {
56            throw std::runtime_error("Planning failed with result: " + result.toString());
57        }
58
59        std::cout << "Path result:\n" << result << "\n";
60
61        if(useVisualizer)
62        {
63            std::cout << "Close the window to exit.\n";
64            visualizer->wait();
65        }
66    }
67    catch(const std::exception &exception)
68    {
69        printException(exception);
70        return EXIT_FAILURE;
71    }
72    return EXIT_SUCCESS;
73}

 1from typing import Optional
 2
 3from zividmotion import (
 4    Application,
 5    Configuration,
 6    InitialState,
 7    PathRequest,
 8    Planner,
 9    PlannerSettings,
10    Profile,
11    Visualizer,
12)
13
14USE_VISUALIZER = False
15
16
17def _start_planner(app: Application) -> Planner:
18    planner_settings = PlannerSettings(cell_name="demo_cell", profile=Profile.testing)
19    return app.create_planner(planner_settings)
20
21
22def _main() -> None:
23    app = Application()
24
25    print("Starting planner")
26    planner = _start_planner(app)
27    visualizer: Optional[Visualizer] = Visualizer.view_planner(planner) if USE_VISUALIZER else None
28
29    start_configuration = Configuration([0.0, 0.0, 0.0, 0.0, 1.57, 0.0])
30    goal_configuration = Configuration([1.57, 0.0, 0.0, 0.0, 1.57, 0.0])
31
32    result = planner.path(
33        InitialState(start_configuration),
34        PathRequest(
35            goals=[goal_configuration],
36            description="Path with joint goal",
37        ),
38    )
39    if not result:
40        raise RuntimeError(f"Planning failed with error: {result.error}")
41
42    print(f"Path result: \n{result}\n")
43
44    if visualizer is not None:
45        print("Close the window to exit.")
46        visualizer.wait()
47
48
49if __name__ == "__main__":
50    _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 = false;
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(std::vector{ 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{ jointGoal };
62        pathRequest.description = "Path with goal from pose";
63        const auto result = planner.path(Zivid::Motion::InitialState{ startConfiguration }, pathRequest);
64        if(!result)
65        {
66            throw std::runtime_error("Planning failed with result: " + result.toString());
67        }
68
69        std::cout << "Path result:\n" << result << "\n";
70
71        if(useVisualizer)
72        {
73            std::cout << "Close the window to exit.\n";
74            visualizer->wait();
75        }
76    }
77    catch(const std::exception &exception)
78    {
79        printException(exception);
80        return EXIT_FAILURE;
81    }
82    return EXIT_SUCCESS;
83}

 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 = False
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 = false;
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 Zivid::Motion::ConfigurationGoals goals{ { goalFarAway, goalCloser, goalClosest } };
55
56        Zivid::Motion::PathRequest shortestPathRequest{ goals };
57        shortestPathRequest.description = "Multiple goals - Default (ShortestPath) prioritization";
58        const auto resultShortestPath = planner.path(initialState, shortestPathRequest);
59        if(!resultShortestPath)
60        {
61            throw std::runtime_error("Planning with ShortestPath failed with result: " + resultShortestPath.toString());
62        }
63        assert(resultShortestPath.selectedGoalIdx == 2u);
64        std::cout << "ShortestPath selected goal index: " << resultShortestPath.selectedGoalIdx.value() << "\n";
65
66        Zivid::Motion::PathRequest listOrderRequest{ goals };
67        listOrderRequest.goalPrioritizationMethod = Zivid::Motion::PathRequest::GoalPrioritizationMethod::listOrder;
68        listOrderRequest.description = "Multiple goals - ListOrder prioritization";
69        const auto resultListOrder = planner.path(initialState, listOrderRequest);
70        if(!resultListOrder)
71        {
72            throw std::runtime_error("Planning with ListOrder failed with result: " + resultListOrder.toString());
73        }
74        assert(resultListOrder.selectedGoalIdx == 0u);
75        std::cout << "ListOrder selected goal index: " << resultListOrder.selectedGoalIdx.value() << "\n";
76
77        if(useVisualizer)
78        {
79            std::cout << "Close the window to exit.\n";
80            visualizer->wait();
81        }
82    }
83    catch(const std::exception &exception)
84    {
85        printException(exception);
86        return EXIT_FAILURE;
87    }
88    return EXIT_SUCCESS;
89}

 1from typing import Optional
 2
 3from zividmotion import (
 4    Application,
 5    Configuration,
 6    ConfigurationGoals,
 7    InitialState,
 8    PathRequest,
 9    Planner,
10    PlannerSettings,
11    Profile,
12    Visualizer,
13)
14
15USE_VISUALIZER = False
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 = ConfigurationGoals([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 = false;
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 Zivid::Motion::ConfigurationGoals goal{ { goalConfiguration } };
52
53        Zivid::Motion::PathRequest withoutObstacleRequest{ goal };
54        withoutObstacleRequest.description = "Without obstacle";
55        const auto resultWithoutObstacle = planner.path(initialState, withoutObstacleRequest);
56        if(!resultWithoutObstacle)
57        {
58            throw std::runtime_error(
59                "Planning before obstacle is set failed with result: " + resultWithoutObstacle.toString());
60        }
61        assert(resultWithoutObstacle.path().size() == 1);
62        std::cout << "Path before obstacle is set: " << resultWithoutObstacle.path().size() << " waypoint\n";
63
64        // Set an obstacle that obstructs the direct path
65        const auto boxMesh = Zivid::Motion::Mesh::createBox(Zivid::Motion::Vector3f{ 0.2f, 0.2f, 0.2f })
66                                 .transform(
67                                     Zivid::Motion::Pose{ Zivid::Motion::Matrix4x4{
68                                         { 1.f, 0.f, 0.f, 1.1f },
69                                         { 0.f, 1.f, 0.f, 1.0f },
70                                         { 0.f, 0.f, 1.f, 1.6f },
71                                         { 0.f, 0.f, 0.f, 1.f },
72                                     } });
73        planner.setObstacles({ Zivid::Motion::Obstacle::fromMesh("box_obstacle", boxMesh) });
74
75        Zivid::Motion::PathRequest withObstacleRequest{ goal };
76        withObstacleRequest.description = "With obstacle";
77        const auto resultWithObstacle = planner.path(initialState, withObstacleRequest);
78        if(!resultWithObstacle)
79        {
80            throw std::runtime_error(
81                "Planning after obstacle is set failed with result: " + resultWithObstacle.toString());
82        }
83        assert(resultWithObstacle.path().size() > 1);
84        std::cout << "Path after obstacle is set: " << resultWithObstacle.path().size() << " waypoints\n";
85
86        if(useVisualizer)
87        {
88            std::cout << "Close the window to exit.\n";
89            visualizer->wait();
90        }
91    }
92    catch(const std::exception &exception)
93    {
94        printException(exception);
95        return EXIT_FAILURE;
96    }
97    return EXIT_SUCCESS;
98}

 1from typing import Optional
 2
 3from zividmotion import (
 4    Application,
 5    Configuration,
 6    ConfigurationGoals,
 7    InitialState,
 8    Mesh,
 9    Obstacle,
10    PathRequest,
11    Planner,
12    PlannerSettings,
13    Pose,
14    Profile,
15    Vector3f,
16    Visualizer,
17)
18
19USE_VISUALIZER = False
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 = ConfigurationGoals([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(
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 <iostream>
 6#include <string>
 7
 8namespace
 9{
10    constexpr auto useVisualizer = false;
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        // Make sure the CAD uses meters as unit, not millimeters
49        const std::string cadFilePath = "obstacle.stl";
50
51        std::cout << "Loading CAD file from: " << cadFilePath << "\n";
52        const auto mesh = Zivid::Motion::Experimental::loadMesh(cadFilePath);
53
54        planner.setObstacles({ Zivid::Motion::Obstacle::fromMesh("cad_obstacle", mesh) });
55
56        if(useVisualizer)
57        {
58            std::cout << "Close the window to exit.\n";
59            visualizer->wait();
60        }
61    }
62    catch(const std::exception &exception)
63    {
64        printException(exception);
65        return EXIT_FAILURE;
66    }
67    return EXIT_SUCCESS;
68}

 1from typing import Optional
 2
 3from zividmotion import Application, Obstacle, Planner, PlannerSettings, Profile, Visualizer, experimental
 4
 5USE_VISUALIZER = False
 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    # Make sure the CAD uses meters as unit, not millimeters
21    cad_file_path = "obstacle.stl"
22
23    print("Loading CAD file from:", cad_file_path)
24    mesh = experimental.load_mesh(cad_file_path)
25
26    planner.set_obstacles(
27        obstacles=[
28            Obstacle.from_mesh(name="cad_obstacle", mesh=mesh),
29        ]
30    )
31
32    if visualizer is not None:
33        print("Close the window to exit.")
34        visualizer.wait()
35
36
37if __name__ == "__main__":
38    _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 = false;
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("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            points.emplace_back(Zivid::Motion::Vector3f{ p.x, p.y, p.z });
80        }
81        planner.setObstacles({ Zivid::Motion::Obstacle::fromPointCloud("point_cloud_obstacle", points) });
82
83        if(useVisualizer)
84        {
85            std::cout << "Close the window to exit.\n";
86            visualizer->wait();
87        }
88    }
89    catch(const std::exception &e)
90    {
91        std::cerr << "Error: " << Zivid::toString(e) << std::endl;
92        return EXIT_FAILURE;
93    }
94    return EXIT_SUCCESS;
95}

 1from typing import Optional
 2
 3import numpy as np
 4import zivid
 5from zividmotion import Application, Obstacle, Planner, PlannerSettings, Profile, Visualizer
 6
 7USE_VISUALIZER = False
 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 = camera_app.create_file_camera("FileCamera.zfc") if USE_FILE_CAMERA else camera_app.connect_camera()
22
23    print("Starting planner")
24    planner = _start_planner(motion_app)
25    visualizer: Optional[Visualizer] = Visualizer.view_planner(planner) if USE_VISUALIZER else None
26
27    # Retrieved from hand-eye calibration of your setup.
28    # In an eye-in-hand setup, this would be the handeye_transform * robot_capture_pose.
29    # In an eye-to-hand setup, this would simply be the handeye_transform.
30    camera_to_base_transform = np.eye(4)
31
32    millimeters_to_meters_transform = np.diag([0.001, 0.001, 0.001, 1])
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    # Convert point cloud to unorganized point cloud
47    unorganized_point_cloud = frame.point_cloud().to_unorganized_point_cloud()
48
49    # Transform the point cloud from the camera frame to the base frame of the planner
50    unorganized_point_cloud.transform(camera_to_base_transform)
51
52    # Transform the point cloud from millimeters to meters, which is the expected unit in the planner
53    unorganized_point_cloud.transform(millimeters_to_meters_transform)
54
55    points = Obstacle.PointCloud(unorganized_point_cloud.copy_data("xyz"))
56    obstacle = Obstacle.from_point_cloud(name="point_cloud_obstacle", points=points)
57    planner.set_obstacles(obstacles=[obstacle])
58
59    if USE_VISUALIZER:
60        input("Press enter to continue:")
61
62    # Set the obstacle again, but this time with color.
63    # Note that this has a slight performance impact, not recommended in production code.
64    planner.clear_obstacles()
65
66    colors = Obstacle.Colors(unorganized_point_cloud.copy_data("rgba"))
67    colored_obstacle = Obstacle.from_colored_point_cloud(name="colored_obstacle", points=points, colors=colors)
68    planner.set_obstacles(obstacles=[colored_obstacle])
69
70    if visualizer is not None:
71        print("Close the window to exit.")
72        visualizer.wait()
73
74
75if __name__ == "__main__":
76    _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/ReplaceableTool.h>
  5#include <Zivid/Motion/Visualizer.h>
  6
  7#include <cassert>
  8#include <cmath>
  9#include <iostream>
 10#include <random>
 11
 12namespace
 13{
 14    constexpr auto useVisualizer = false;
 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    // Utility function for creating a dummy point-cloud obstacle
 41    std::vector<Zivid::Motion::Vector3f>
 42    generateSpherePoints(const Zivid::Motion::Vector3f &center, const float radius, const int numPoints)
 43    {
 44        // Fixed seed for determinism
 45        std::mt19937 rng(42);
 46        std::uniform_real_distribution<float> thetaDist(0, 2 * M_PI);
 47        std::uniform_real_distribution<float> phiDist(0, M_PI);
 48
 49        std::vector<Zivid::Motion::Vector3f> points;
 50        points.reserve(numPoints);
 51        for(int i = 0; i < numPoints; ++i)
 52        {
 53            const float theta = thetaDist(rng);
 54            const float phi = phiDist(rng);
 55            points.push_back(
 56                {
 57                    center.x + radius * std::sin(phi) * std::cos(theta),
 58                    center.y + radius * std::sin(phi) * std::sin(theta),
 59                    center.z + radius * std::cos(phi),
 60                });
 61        }
 62        return points;
 63    }
 64
 65    // Create a pick pose with the end-effector pointing downwards at the given point
 66    // (180-degree rotation around Y-axis)
 67    Zivid::Motion::Pose getPickPose(const Zivid::Motion::Vector3f &pickPoint)
 68    {
 69        return Zivid::Motion::Pose{ Zivid::Motion::Matrix4x4{
 70            { -1.f, 0.f, 0.f, pickPoint.x },
 71            { 0.f, 1.f, 0.f, pickPoint.y },
 72            { 0.f, 0.f, -1.f, pickPoint.z },
 73            { 0.f, 0.f, 0.f, 1.f },
 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
 91        // Set a dummy obstacle to be interacted with
 92        constexpr Zivid::Motion::Vector3f obstacleCenter{ 1.1f, 1.0f, 0.9f };
 93        constexpr float obstacleRadius = 0.2f;
 94        planner.setObstacles(
 95            { Zivid::Motion::Obstacle::fromPointCloud(
 96                "interaction_object",
 97                Zivid::Motion::Obstacle::PointCloud{ generateSpherePoints(obstacleCenter, obstacleRadius, 1000) }) });
 98
 99        // Set a tool that simulates a vacuum gripper with a suction cup with 2 cm compliance
100        constexpr Zivid::Motion::Vector3f toolBaseDimensions{ 0.15, 0.15, 0.15 };
101        auto suctionCupMatrix = Zivid::Motion::Matrix4x4::identity();
102        suctionCupMatrix(2, 3) = toolBaseDimensions.z;
103        constexpr Zivid::Motion::Vector3f suctionCupDimensions{ 0.05, 0.05, 0.02 };
104
105        auto rigidTool = Zivid::Motion::Mesh::createBox(toolBaseDimensions).bottomCenterTransform();
106        auto compliantTool = Zivid::Motion::Mesh::createBox(suctionCupDimensions)
107                                 .bottomCenterTransform()
108                                 .transform(Zivid::Motion::Pose{ suctionCupMatrix });
109        const Zivid::Motion::ToolGeometry toolGeometry{ rigidTool, compliantTool };
110        planner.setReplaceableTool(Zivid::Motion::ReplaceableTool{ "vacuum_gripper", toolGeometry });
111
112        // Set the new tcp to be at the center tip of the gripper, when the suction cup is compressed 1 cm for good contact
113        auto tcpMatrix = Zivid::Motion::Matrix4x4::identity();
114        tcpMatrix(2, 3) += toolBaseDimensions.z + suctionCupDimensions.z - 0.01f;
115        planner.setTcp(Zivid::Motion::Tcp{ Zivid::Motion::Pose{ tcpMatrix }, { 0.f, 0.f, 1.f } });
116
117        // Find the pick joint configuration, gripping the top of the object with the new TCP
118        constexpr Zivid::Motion::Vector3f pickPoint{ obstacleCenter.x,
119                                                     obstacleCenter.y,
120                                                     obstacleCenter.z + obstacleRadius };
121        const auto pickGoal =
122            planner.computeInverseKinematics(std::vector{ getPickPose(pickPoint) }, startConfiguration);
123        if(pickGoal.noneValid())
124        {
125            throw std::runtime_error("No valid inverse kinematics solution found for pick pose");
126        }
127
128        const Zivid::Motion::InitialState initialState{ startConfiguration };
129
130        // Path type defaults to Free
131        Zivid::Motion::PathRequest freeRequest{ pickGoal };
132        freeRequest.description = "Free path to pick goal";
133        const auto resultFree = planner.path(initialState, freeRequest);
134        // Expect blocked end with path type Free, since the compliant part of the tool has to enter the obstacle for the
135        // TCP to reach the goal
136        assert(resultFree.error == Zivid::Motion::PathResult::Error::blockedEnd);
137
138        Zivid::Motion::PathRequest touchRequest{ pickGoal };
139        touchRequest.type = Zivid::Motion::PathRequest::Type::touch;
140        touchRequest.description = "Touch path to pick goal";
141        const auto resultTouch = planner.path(initialState, touchRequest);
142        if(!resultTouch)
143        {
144            throw std::runtime_error("Planning with path type Touch failed with result: " + resultTouch.toString());
145        }
146
147        std::cout << "\nSuccessful touch path: " << resultTouch << "\n";
148
149        if(useVisualizer)
150        {
151            std::cout << "Close the window to exit.\n";
152            visualizer->wait();
153        }
154    }
155    catch(const std::exception &exception)
156    {
157        printException(exception);
158        return EXIT_FAILURE;
159    }
160    return EXIT_SUCCESS;
161}

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

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

 1from typing import Optional
 2
 3import numpy as np
 4from zividmotion import (
 5    Application,
 6    Configuration,
 7    ConfigurationGoals,
 8    InitialState,
 9    Obstacle,
10    PathRequest,
11    Planner,
12    PlannerSettings,
13    Profile,
14    Visualizer,
15)
16
17USE_VISUALIZER = False
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 = ConfigurationGoals([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{ std::vector{ invalidGoalConfiguration } };
48        unreachableRequest.description = "Path with unreachable goal";
49        const auto unsuccessfulResult =
50            planner.path(Zivid::Motion::InitialState{ startConfiguration }, unreachableRequest);
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    }
56    catch(const std::exception &exception)
57    {
58        const std::filesystem::path outputFolder = "/tmp";
59
60        const auto logPath = planner.exportApiLog(outputFolder);
61        const auto packagePath = Zivid::Motion::packageApiLog(app, logPath);
62        std::cerr << "Planner failed, debug package available at: " + std::filesystem::canonical(packagePath).string()
63                  << std::endl;
64
65        printException(exception);
66        return EXIT_SUCCESS; // for the purpose of this sample, saving a debug package is success
67    }
68    return EXIT_FAILURE;
69}

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