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 data-tag you want to generate data for. This is referenced later
31 // when using the data to instantiate a planner.
32 constexpr auto cellName = "my_cell";
33
34 // Specify which configuration 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 data-tag.\n";
45 }
46 catch(const std::exception &exception)
47 {
48 printException(exception);
49 std::cout << "Press enter to exit." << std::endl;
50 std::cin.get();
51 return EXIT_FAILURE;
52 }
53 return EXIT_SUCCESS;
54}
1from zividmotion import Application, PlannerSettings, Profile, generate
2
3
4def _main() -> None:
5 app = Application()
6
7 # Specify which cell you want to generate data for. This is referenced later
8 # when using the data to instantiate a planner.
9 cell_name = "my_cell"
10
11 # Specify which configuration 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 data-tag.")
21
22
23if __name__ == "__main__":
24 _main()
Package Planner Setup
This example shows how to package all the files necessary for running the motion planner into a zip archive, which can be easily distributed and unpacked on other machines.
Run this snippet on the workstation where you have completed the planner setup:
1#include <Zivid/Motion/Application.h>
2#include <Zivid/Motion/Packaging.h>
3
4#include <filesystem>
5#include <iostream>
6
7namespace
8{
9 void printException(const std::exception &e, const int level = 0)
10 {
11 std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
12 try
13 {
14 std::rethrow_if_nested(e);
15 }
16 catch(const std::exception &nestedException)
17 {
18 printException(nestedException, level + 1);
19 }
20 catch(...)
21 {}
22 }
23} // namespace
24
25int main()
26{
27 try
28 {
29 const Zivid::Motion::Application app;
30 const std::filesystem::path outputPath = "desired/path/output.zip";
31
32 // In this example, we package not only the configuration files for "my_cell", but also the generated data
33 // for the testing setup.
34 std::cout << "Packaging data...\n";
35 Zivid::Motion::packageCell(app, "my_cell", outputPath, { Zivid::Motion::Profile::testing });
36
37 std::cout << "Data package stored at: " << std::filesystem::canonical(outputPath) << "\n";
38 }
39 catch(const std::exception &exception)
40 {
41 printException(exception);
42 std::cout << "Press enter to exit." << std::endl;
43 std::cin.get();
44 return EXIT_FAILURE;
45 }
46 return EXIT_SUCCESS;
47}
1from pathlib import Path
2
3from zividmotion import Application, Profile, package_cell
4
5
6def _main() -> None:
7 app = Application()
8
9 output_path = Path("desired/path/output.zip")
10
11 # In this example, we package not only the configuration files for the cell, but also the generated data
12 # for the testing setup.
13 print("Packaging data...")
14 package_cell(app, data_tag="my_cell", output_path=output_path, include_generated_data=[Profile.testing])
15 print(f"Data package stored at: {output_path.resolve()}")
16
17
18if __name__ == "__main__":
19 _main()
The recipients of the zip archive can then run this snippet to unpack the setup:
1#include <Zivid/Motion/Application.h>
2#include <Zivid/Motion/Packaging.h>
3
4#include <filesystem>
5#include <iostream>
6
7namespace
8{
9 void printException(const std::exception &e, const int level = 0)
10 {
11 std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
12 try
13 {
14 std::rethrow_if_nested(e);
15 }
16 catch(const std::exception &nestedException)
17 {
18 printException(nestedException, level + 1);
19 }
20 catch(...)
21 {}
22 }
23} // namespace
24
25int main()
26{
27 try
28 {
29 const Zivid::Motion::Application app;
30 const std::filesystem::path packagePath = "path/to/packaged/data.zip";
31
32 std::cout << "Installing package from " << std::filesystem::canonical(packagePath) << "...\n";
33 Zivid::Motion::installPackage(app, packagePath);
34
35 std::cout << "Package installed.\n";
36 }
37 catch(const std::exception &exception)
38 {
39 printException(exception);
40 std::cout << "Press enter to exit." << std::endl;
41 std::cin.get();
42 return EXIT_FAILURE;
43 }
44 return EXIT_SUCCESS;
45}
1from pathlib import Path
2
3from zividmotion import Application, install_package
4
5
6def _main() -> None:
7 app = Application()
8
9 package_path = Path("path/to/packaged/data.zip")
10
11 print(f"Installing package from {package_path.resolve()}...")
12 install_package(app, package_path)
13
14 print("Package installed.")
15
16
17if __name__ == "__main__":
18 _main()
Planner
Basic Path Calls
Joint Goal
1#include <Zivid/Motion/Application.h>
2#include <Zivid/Motion/Planner.h>
3
4#include <iostream>
5#include <stdexcept>
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 "my_cell",
31 Zivid::Motion::Profile::testing,
32 },
33 useVisualizer);
34 }
35} // namespace
36
37int main()
38{
39 try
40 {
41 const Zivid::Motion::Application app;
42
43 std::cout << "Starting planner\n";
44 const auto planner = startPlanner(app);
45
46 const Zivid::Motion::Configuration startConfiguration = { 0.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
47 const Zivid::Motion::Configuration goalConfiguration = { 1.57f, 0.f, 0.f, 0.f, 1.57f, 0.f };
48
49 const auto result = planner.path(
50 Zivid::Motion::InitialState{ startConfiguration },
51 Zivid::Motion::PathRequest{ .goalConfigurations = { goalConfiguration },
52 .description = "Path with joint goal" });
53 if(!result)
54 {
55 throw std::runtime_error("Planning failed with result: " + result.toString());
56 }
57
58 std::cout << "Path result:\n" << result << "\n";
59
60 if(useVisualizer)
61 {
62 std::cout << "Press enter to quit:";
63 std::cin.get();
64 }
65 }
66 catch(const std::exception &exception)
67 {
68 printException(exception);
69 std::cout << "Press enter to exit." << std::endl;
70 std::cin.get();
71 return EXIT_FAILURE;
72 }
73 return EXIT_SUCCESS;
74}
1from zividmotion import Application, InitialState, PathRequest, Planner, PlannerSettings, Profile
2
3USE_VISUALIZER = True
4
5
6def _start_planner(app: Application) -> Planner:
7 planner_settings = PlannerSettings(cell_name="my_cell", profile=Profile.testing)
8 return app.create_planner(planner_settings, use_visualizer=USE_VISUALIZER)
9
10
11def _main() -> None:
12 app = Application()
13
14 print("Starting planner")
15 planner = _start_planner(app)
16
17 start_configuration = [0.0, 0.0, 0.0, 0.0, 1.57, 0.0]
18 goal_configuration = [1.57, 0.0, 0.0, 0.0, 1.57, 0.0]
19
20 result = planner.path(
21 InitialState(start_configuration),
22 PathRequest(
23 goal_configurations=[goal_configuration],
24 description="Path with joint goal",
25 ),
26 )
27 if not result:
28 raise RuntimeError(f"Planning failed with error: {result.error}")
29
30 print(f"Path result: \n{result}\n")
31
32 if USE_VISUALIZER:
33 input("Press enter to quit:")
34
35
36if __name__ == "__main__":
37 _main()
Pose Goal
1#include <Zivid/Motion/Application.h>
2#include <Zivid/Motion/Planner.h>
3
4#include <iostream>
5
6namespace
7{
8 constexpr auto useVisualizer = true;
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 "my_cell",
30 Zivid::Motion::Profile::testing,
31 },
32 useVisualizer);
33 }
34} // namespace
35
36int main()
37{
38 try
39 {
40 const Zivid::Motion::Application app;
41
42 std::cout << "Starting planner\n";
43 const auto planner = startPlanner(app);
44
45 const Zivid::Motion::Configuration startConfiguration = { 0.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
46 const Zivid::Motion::Pose poseGoal = { {
47 { 0.f, -1.f, 0.f, 0.f },
48 { -1.f, 0.f, 0.f, 1.45f },
49 { 0.f, 0.f, -1.f, 1.63f },
50 { 0.f, 0.f, 0.f, 1.f },
51 } };
52
53 const auto poseGoalConfiguration = planner.computeInverseKinematics(poseGoal, startConfiguration);
54 if(!poseGoalConfiguration.has_value())
55 {
56 throw std::runtime_error("No valid inverse kinematics solution found");
57 }
58
59 const auto result = planner.path(
60 Zivid::Motion::InitialState{ startConfiguration },
61 Zivid::Motion::PathRequest{ .goalConfigurations = { poseGoalConfiguration.value() },
62 .description = "Path with goal from pose" });
63 if(!result)
64 {
65 throw std::runtime_error("Planning failed with result: " + result.toString());
66 }
67
68 std::cout << "Path result:\n" << result << "\n";
69
70 if(useVisualizer)
71 {
72 std::cout << "Press enter to quit:";
73 std::cin.get();
74 }
75 }
76 catch(const std::exception &exception)
77 {
78 printException(exception);
79 std::cout << "Press enter to exit." << std::endl;
80 std::cin.get();
81 return EXIT_FAILURE;
82 }
83 return EXIT_SUCCESS;
84}
1import numpy as np
2from zividmotion import Application, InitialState, PathRequest, Planner, PlannerSettings, Profile
3
4USE_VISUALIZER = True
5
6
7def _start_planner(app: Application) -> Planner:
8 planner_settings = PlannerSettings(cell_name="my_cell", profile=Profile.testing)
9 return app.create_planner(planner_settings, use_visualizer=USE_VISUALIZER)
10
11
12def _main() -> None:
13 app = Application()
14
15 print("Starting planner")
16 planner = _start_planner(app)
17
18 start_configuration = [0.0, 0.0, 0.0, 0.0, 1.57, 0.0]
19 pose_goal = np.array(
20 [
21 [0.0, -1.0, 0.0, 0.0],
22 [-1.0, 0.0, 0.0, 1.45],
23 [0.0, 0.0, -1.0, 1.63],
24 [0.0, 0.0, 0.0, 1.0],
25 ]
26 )
27
28 pose_goal_configuration = planner.compute_inverse_kinematics(
29 pose=pose_goal,
30 reference_configuration=start_configuration,
31 )
32 if pose_goal_configuration is None:
33 raise RuntimeError("No valid inverse kinematics solution found")
34
35 result = planner.path(
36 InitialState(start_configuration),
37 PathRequest(
38 goal_configurations=[pose_goal_configuration],
39 description="Path with goal from pose",
40 ),
41 )
42 if not result:
43 raise RuntimeError(f"Planning failed with error: {result.error}")
44
45 print(f"Path result: \n{result}\n")
46
47 if USE_VISUALIZER:
48 input("Press enter to quit:")
49
50
51if __name__ == "__main__":
52 _main()
Multiple Goals and Prioritization
1#include <Zivid/Motion/Application.h>
2#include <Zivid/Motion/Planner.h>
3
4#include <cassert>
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 "my_cell",
31 Zivid::Motion::Profile::testing,
32 },
33 useVisualizer);
34 }
35} // namespace
36
37int main()
38{
39 try
40 {
41 const Zivid::Motion::Application app;
42
43 std::cout << "Starting planner\n";
44 const auto planner = startPlanner(app);
45
46 const Zivid::Motion::Configuration startConfiguration = { 0.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
47 const Zivid::Motion::InitialState initialState{ startConfiguration };
48
49 const Zivid::Motion::Configuration goalFarAway = { 3.14f, 0.f, 0.f, 0.f, 1.57f, 0.f };
50 const Zivid::Motion::Configuration goalCloser = { 1.57f, 0.1f, -0.1f, 0.f, 1.57f, 0.f };
51 const Zivid::Motion::Configuration goalClosest = { 1.57f, 0.f, 0.f, 0.f, 1.57f, 0.f };
52 const std::vector<Zivid::Motion::Configuration> goals = { goalFarAway, goalCloser, goalClosest };
53
54 const auto resultShortestPath = planner.path(
55 initialState,
56 Zivid::Motion::PathRequest{ .goalConfigurations = goals,
57 .description = "Multiple goals - Default (ShortestPath) prioritization" });
58 if(!resultShortestPath)
59 {
60 throw std::runtime_error("Planning with ShortestPath failed with result: " + resultShortestPath.toString());
61 }
62 assert(resultShortestPath.selectedGoalIdx == 2u);
63 std::cout << "ShortestPath selected goal index: " << resultShortestPath.selectedGoalIdx.value() << "\n";
64
65 const auto resultListOrder = planner.path(
66 initialState,
67 Zivid::Motion::PathRequest{ .goalPrioritizationMethod =
68 Zivid::Motion::PathRequest::GoalPrioritizationMethod::listOrder,
69 .goalConfigurations = goals,
70 .description = "Multiple goals - ListOrder prioritization" });
71 if(!resultListOrder)
72 {
73 throw std::runtime_error("Planning with ListOrder failed with result: " + resultListOrder.toString());
74 }
75 assert(resultListOrder.selectedGoalIdx == 0u);
76 std::cout << "ListOrder selected goal index: " << resultListOrder.selectedGoalIdx.value() << "\n";
77
78 if(useVisualizer)
79 {
80 std::cout << "Press enter to quit:";
81 std::cin.get();
82 }
83 }
84 catch(const std::exception &exception)
85 {
86 printException(exception);
87 std::cout << "Press enter to exit." << std::endl;
88 std::cin.get();
89 return EXIT_FAILURE;
90 }
91 return EXIT_SUCCESS;
92}
1from zividmotion import Application, InitialState, PathRequest, Planner, PlannerSettings, Profile
2
3USE_VISUALIZER = True
4
5
6def _start_planner(app: Application) -> Planner:
7 planner_settings = PlannerSettings(cell_name="my_cell", profile=Profile.testing)
8 return app.create_planner(planner_settings, use_visualizer=USE_VISUALIZER)
9
10
11def _main() -> None:
12 app = Application()
13
14 print("Starting planner")
15 planner = _start_planner(app)
16
17 start_configuration = [0.0, 0.0, 0.0, 0.0, 1.57, 0.0]
18 initial_state = InitialState(start_configuration)
19
20 goal_far_away = [3.14, 0.0, 0.0, 0.0, 1.57, 0.0]
21 goal_closer = [1.57, 0.1, -0.1, 0.0, 1.57, 0.0]
22 goal_closest = [1.57, 0.0, 0.0, 0.0, 1.57, 0.0]
23 goals = [goal_far_away, goal_closer, goal_closest]
24
25 result_shortest_path = planner.path(
26 initial_state,
27 PathRequest(
28 goal_configurations=goals,
29 description="Multiple goals - Default (ShortestPath) prioritization",
30 ),
31 )
32 if not result_shortest_path:
33 raise RuntimeError(f"Planning with ShortestPath failed with error: {result_shortest_path.error}")
34 assert result_shortest_path.selected_goal_idx == 2
35 print(f"ShortestPath selected goal index: {result_shortest_path.selected_goal_idx}")
36
37 result_list_order = planner.path(
38 initial_state,
39 PathRequest(
40 goal_prioritization_method=PathRequest.GoalPrioritizationMethod.listOrder,
41 goal_configurations=goals,
42 description="Multiple goals - ListOrder prioritization",
43 ),
44 )
45 if not result_list_order:
46 raise RuntimeError(f"Planning with ListOrder failed with error: {result_list_order.error}")
47 assert result_list_order.selected_goal_idx == 0
48 print(f"ListOrder selected goal index: {result_list_order.selected_goal_idx}")
49
50 if USE_VISUALIZER:
51 input("Press enter to quit:")
52
53
54if __name__ == "__main__":
55 _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
4#include <cassert>
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 "my_cell",
31 Zivid::Motion::Profile::testing,
32 },
33 useVisualizer);
34 }
35
36 Zivid::Motion::Triangles createBoxMesh(
37 const Zivid::Motion::PointXYZ ¢er,
38 const Zivid::Motion::VectorXYZ &dimensions)
39 {
40 const float hx = dimensions.x / 2.f;
41 const float hy = dimensions.y / 2.f;
42 const float hz = dimensions.z / 2.f;
43
44 // 8 vertices of the box
45 const Zivid::Motion::PointXYZ v000{ center.x - hx, center.y - hy, center.z - hz };
46 const Zivid::Motion::PointXYZ v100{ center.x + hx, center.y - hy, center.z - hz };
47 const Zivid::Motion::PointXYZ v010{ center.x - hx, center.y + hy, center.z - hz };
48 const Zivid::Motion::PointXYZ v110{ center.x + hx, center.y + hy, center.z - hz };
49 const Zivid::Motion::PointXYZ v001{ center.x - hx, center.y - hy, center.z + hz };
50 const Zivid::Motion::PointXYZ v101{ center.x + hx, center.y - hy, center.z + hz };
51 const Zivid::Motion::PointXYZ v011{ center.x - hx, center.y + hy, center.z + hz };
52 const Zivid::Motion::PointXYZ v111{ center.x + hx, center.y + hy, center.z + hz };
53
54 // 6 faces × 2 triangles each = 12 triangles
55 return {
56 // -Z face
57 { v000, v100, v110 },
58 { v000, v110, v010 },
59 // +Z face
60 { v001, v111, v101 },
61 { v001, v011, v111 },
62 // -Y face
63 { v000, v101, v100 },
64 { v000, v001, v101 },
65 // +Y face
66 { v010, v110, v111 },
67 { v010, v111, v011 },
68 // -X face
69 { v000, v010, v011 },
70 { v000, v011, v001 },
71 // +X face
72 { v100, v101, v111 },
73 { v100, v111, v110 },
74 };
75 }
76} // namespace
77
78int main()
79{
80 try
81 {
82 const Zivid::Motion::Application app;
83
84 std::cout << "Starting planner\n";
85 const auto planner = startPlanner(app);
86
87 const Zivid::Motion::Configuration startConfiguration = { 0.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
88 const Zivid::Motion::Configuration goalConfiguration = { 1.57f, 0.f, 0.f, 0.f, 1.57f, 0.f };
89 const Zivid::Motion::InitialState initialState{ startConfiguration };
90
91 const auto resultWithoutObstacle = planner.path(
92 initialState,
93 Zivid::Motion::PathRequest{ .goalConfigurations = { goalConfiguration },
94 .description = "Without obstacle" });
95 if(!resultWithoutObstacle)
96 {
97 throw std::runtime_error(
98 "Planning before obstacle is set failed with result: " + resultWithoutObstacle.toString());
99 }
100 assert(resultWithoutObstacle.path().size() == 1);
101 std::cout << "Path before obstacle is set: " << resultWithoutObstacle.path().size() << " waypoint\n";
102
103 // Set an obstacle that obstructs the direct path
104 const auto boxTriangles = createBoxMesh({ 1.1f, 1.0f, 1.6f }, { 0.2f, 0.2f, 0.2f });
105 planner.setObstacles({ Zivid::Motion::Obstacle::fromMesh("box_obstacle", boxTriangles) });
106
107 const auto resultWithObstacle = planner.path(
108 initialState,
109 Zivid::Motion::PathRequest{ .goalConfigurations = { goalConfiguration }, .description = "With obstacle" });
110 if(!resultWithObstacle)
111 {
112 throw std::runtime_error(
113 "Planning after obstacle is set failed with result: " + resultWithObstacle.toString());
114 }
115 assert(resultWithObstacle.path().size() > 1);
116 std::cout << "Path after obstacle is set: " << resultWithObstacle.path().size() << " waypoints\n";
117
118 if(useVisualizer)
119 {
120 std::cout << "Press enter to quit:";
121 std::cin.get();
122 }
123 }
124 catch(const std::exception &exception)
125 {
126 printException(exception);
127 std::cout << "Press enter to exit." << std::endl;
128 std::cin.get();
129 return EXIT_FAILURE;
130 }
131 return EXIT_SUCCESS;
132}
Install additional dependencies with:
$ pip install trimesh
1import trimesh
2from zividmotion import Application, InitialState, Obstacle, PathRequest, Planner, PlannerSettings, Profile
3
4USE_VISUALIZER = True
5
6
7def _start_planner(app: Application) -> Planner:
8 planner_settings = PlannerSettings(cell_name="my_cell", profile=Profile.testing)
9 return app.create_planner(planner_settings, use_visualizer=USE_VISUALIZER)
10
11
12def _create_box_mesh(center_point, size):
13 box = trimesh.creation.box(extents=size)
14 box.apply_translation(center_point)
15 return box.triangles.tolist()
16
17
18def _main() -> None:
19 app = Application()
20
21 print("Starting planner")
22 planner = _start_planner(app)
23
24 start_configuration = [0.0, 0.0, 0.0, 0.0, 1.57, 0.0]
25 goal_configuration = [1.57, 0.0, 0.0, 0.0, 1.57, 0.0]
26 initial_state = InitialState(start_configuration=start_configuration)
27
28 result_without_obstacle = planner.path(
29 initial_state=initial_state,
30 request=PathRequest(
31 goal_configurations=[goal_configuration],
32 description="Without obstacle",
33 ),
34 )
35 if not result_without_obstacle:
36 raise RuntimeError(f"Planning before obstacle is set failed with error: {result_without_obstacle.error}")
37 assert len(result_without_obstacle.path) == 1
38 print(f"Path before obstacle is set: {len(result_without_obstacle.path)} waypoint")
39
40 # Set an obstacle that obstructs the direct path
41 box_triangles = _create_box_mesh(center_point=[1.1, 1.0, 1.6], size=[0.2, 0.2, 0.2])
42 planner.set_obstacles(
43 obstacles=[
44 Obstacle.from_mesh(name="box_obstacle", mesh=box_triangles),
45 ]
46 )
47
48 result_with_obstacle = planner.path(
49 initial_state=initial_state,
50 request=PathRequest(
51 goal_configurations=[goal_configuration],
52 description="With obstacle",
53 ),
54 )
55 if not result_with_obstacle:
56 raise RuntimeError(f"Planning after obstacle is set failed with error: {result_with_obstacle.error}")
57 assert len(result_with_obstacle.path) > 1
58 print(f"Path after obstacle is set: {len(result_with_obstacle.path)} waypoints")
59
60 if USE_VISUALIZER:
61 input("Press enter to quit:")
62
63
64if __name__ == "__main__":
65 _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
4#include <assimp/Importer.hpp>
5#include <assimp/postprocess.h>
6#include <assimp/scene.h>
7
8#include <filesystem>
9#include <iostream>
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 "my_cell",
35 Zivid::Motion::Profile::testing,
36 },
37 useVisualizer);
38 }
39
40 Zivid::Motion::Triangles loadTrianglesFromCad(const std::filesystem::path &cadFilePath)
41 {
42 Assimp::Importer importer;
43 const aiScene *scene = importer.ReadFile(cadFilePath, aiProcess_Triangulate | aiProcess_JoinIdenticalVertices);
44
45 if(!scene || !scene->HasMeshes())
46 {
47 throw std::runtime_error("Failed to load CAD file: " + cadFilePath.string());
48 }
49
50 Zivid::Motion::Triangles triangles;
51 for(unsigned int m = 0; m < scene->mNumMeshes; ++m)
52 {
53 const aiMesh *mesh = scene->mMeshes[m];
54 for(unsigned int f = 0; f < mesh->mNumFaces; ++f)
55 {
56 const aiFace &face = mesh->mFaces[f];
57 if(face.mNumIndices != 3)
58 {
59 continue; // skip non-triangles (shouldn't happen with aiProcess_Triangulate)
60 }
61
62 const auto toPoint = [&](unsigned int idx) {
63 const aiVector3D &v = mesh->mVertices[face.mIndices[idx]];
64 return Zivid::Motion::PointXYZ{ v.x, v.y, v.z };
65 };
66
67 triangles.push_back({ toPoint(0), toPoint(1), toPoint(2) });
68 }
69 }
70
71 if(triangles.empty())
72 {
73 throw std::runtime_error("No triangles found in CAD file: " + cadFilePath.filename().string());
74 }
75 return triangles;
76 }
77} // namespace
78
79int main()
80{
81 try
82 {
83 const Zivid::Motion::Application app;
84
85 std::cout << "Starting planner\n";
86 const auto planner = startPlanner(app);
87
88 // Make sure the CAD uses meters as unit, not millimeters
89 const std::filesystem::path cadFilePath = "path/to/obstacle.stl";
90
91 std::cout << "Loading CAD file from: " << cadFilePath << "\n";
92 const auto triangles = loadTrianglesFromCad(cadFilePath);
93
94 planner.setObstacles({ Zivid::Motion::Obstacle::fromMesh("cad_obstacle", triangles) });
95
96 if(useVisualizer)
97 {
98 std::cout << "Press enter to quit:";
99 std::cin.get();
100 }
101 }
102 catch(const std::exception &exception)
103 {
104 printException(exception);
105 std::cout << "Press enter to exit." << std::endl;
106 std::cin.get();
107 return EXIT_FAILURE;
108 }
109 return EXIT_SUCCESS;
110}
Install additional dependencies with:
$ pip install trimesh
1from pathlib import Path
2
3import trimesh
4from zividmotion import Application, Obstacle, Planner, PlannerSettings, Profile
5
6USE_VISUALIZER = True
7
8
9def _start_planner(app: Application) -> Planner:
10 planner_settings = PlannerSettings(cell_name="my_cell", profile=Profile.testing)
11 return app.create_planner(planner_settings, use_visualizer=USE_VISUALIZER)
12
13
14def _load_triangles_from_cad_file(cad_file_path: Path) -> list:
15 mesh = trimesh.load(cad_file_path)
16 return mesh.triangles.tolist()
17
18
19def _main() -> None:
20 app = Application()
21
22 print("Starting planner")
23 planner = _start_planner(app)
24
25 # Make sure the CAD uses meters as unit, not millimeters
26 cad_file_path = Path("path/to/obstacle.stl")
27
28 print("Loading CAD file from:", cad_file_path)
29 triangles = _load_triangles_from_cad_file(cad_file_path)
30
31 planner.set_obstacles(
32 obstacles=[
33 Obstacle.from_mesh(name="cad_obstacle", mesh=triangles),
34 ]
35 )
36
37 if USE_VISUALIZER:
38 input("Press enter to quit:")
39
40
41if __name__ == "__main__":
42 _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/Zivid.h>
4
5#include <iostream>
6
7namespace
8{
9 constexpr auto useVisualizer = true;
10 constexpr auto useFileCamera = true;
11
12 Zivid::Motion::Planner startPlanner(const Zivid::Motion::Application &app)
13 {
14 return app.createPlanner(
15 Zivid::Motion::PlannerSettings{
16 "my_cell",
17 Zivid::Motion::Profile::testing,
18 },
19 useVisualizer);
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 const auto planner = startPlanner(app);
35
36 // Retrieved from hand-eye calibration of your setup.
37 // In an eye-in-hand setup, this would be the handeye_transform * robot_capture_pose.
38 // In an eye-to-hand setup, this would simply be the handeye_transform.
39 const Zivid::Matrix4x4 cameraToBaseTransform{ {
40 { 1.f, 0.f, 0.f, 0.f },
41 { 0.f, 1.f, 0.f, 0.f },
42 { 0.f, 0.f, 1.f, 0.f },
43 { 0.f, 0.f, 0.f, 1.f },
44 } };
45
46 std::cout << "Capturing with default capture settings\n";
47 const auto settings =
48 Zivid::Settings{ Zivid::Settings::Acquisitions{ Zivid::Settings::Acquisition{} },
49 Zivid::Settings::Color{ Zivid::Settings2D{
50 Zivid::Settings2D::Acquisitions{ Zivid::Settings2D::Acquisition{} } } } };
51 const auto frame = camera.capture(settings);
52
53 // Downsampling improves speed, if you don't need the extra resolution
54 frame.pointCloud().downsample(Zivid::PointCloud::Downsampling::by4x4);
55
56 // Transform the point cloud from the camera frame to the base frame of the planner
57 frame.pointCloud().transform(cameraToBaseTransform);
58
59 const auto xyzData = frame.pointCloud().toUnorganizedPointCloud().copyPointsXYZ();
60 std::vector<Zivid::Motion::PointXYZ> points;
61 points.reserve(xyzData.size());
62 for(const auto &p : xyzData)
63 {
64 constexpr auto mmToM = 1.f / 1000.f; // Convert from millimeters to meters
65 points.emplace_back(Zivid::Motion::PointXYZ{ p.x * mmToM, p.y * mmToM, p.z * mmToM });
66 }
67 planner.setObstacles({ Zivid::Motion::Obstacle::fromPointCloud("point_cloud_obstacle", points) });
68
69 if(useVisualizer)
70 {
71 std::cout << "Press enter to quit:";
72 std::cin.get();
73 }
74 }
75 catch(const std::exception &e)
76 {
77 std::cerr << "Error: " << e.what() << std::endl;
78 std::cout << "Press enter to exit." << std::endl;
79 std::cin.get();
80 return EXIT_FAILURE;
81 }
82 return EXIT_SUCCESS;
83}
1import numpy as np
2import zivid
3from zividmotion import Application, Obstacle, Planner, PlannerSettings, Profile
4
5USE_VISUALIZER = True
6USE_FILE_CAMERA = True
7
8
9def _start_planner(app: Application) -> Planner:
10 planner_settings = PlannerSettings(cell_name="my_cell", profile=Profile.testing)
11 return app.create_planner(planner_settings, use_visualizer=USE_VISUALIZER)
12
13
14def _main() -> None:
15 camera_app = zivid.Application()
16 motion_app = Application()
17
18 print("Connecting to camera")
19 camera = (
20 camera_app.create_file_camera("/path/to/FileCamera.zfc") if USE_FILE_CAMERA else camera_app.connect_camera()
21 )
22
23 print("Starting planner")
24 planner = _start_planner(motion_app)
25
26 # Retrieved from hand-eye calibration of your setup.
27 # In an eye-in-hand setup, this would be the handeye_transform * robot_capture_pose.
28 # In an eye-to-hand setup, this would simply be the handeye_transform.
29 camera_to_base_transform = np.eye(4)
30
31 print("Capturing with default capture settings")
32 # Replace with your own capture settings for better results
33 capture_settings = zivid.Settings(
34 acquisitions=[zivid.Settings.Acquisition()],
35 color=zivid.Settings2D(acquisitions=[zivid.Settings2D.Acquisition()]),
36 )
37
38 frame = camera.capture(capture_settings)
39
40 # Downsampling improves speed, if you don't need the extra resolution
41 frame.point_cloud().downsample(zivid.PointCloud.Downsampling.by4x4)
42
43 # Transform the point cloud from the camera frame to the base frame of the planner
44 frame.point_cloud().transform(camera_to_base_transform)
45
46 point_cloud = frame.point_cloud().to_unorganized_point_cloud()
47 points = point_cloud.copy_data("xyz") / 1000.0 # Convert from millimeters to meters
48 obstacle = Obstacle.from_point_cloud(name="point_cloud_obstacle", points=points)
49 planner.set_obstacles(obstacles=[obstacle])
50
51 if USE_VISUALIZER:
52 input("Press enter to continue:")
53
54 # Set the obstacle again, but this time with color.
55 # Note that this has a slight performance impact, not recommended in production code.
56 planner.clear_obstacles()
57
58 rgba = point_cloud.copy_data("rgba")
59 colored_obstacle = Obstacle.from_colored_point_cloud(name="colored_obstacle", points=points, colors=rgba)
60 planner.set_obstacles(obstacles=[colored_obstacle])
61
62 if USE_VISUALIZER:
63 input("Press enter to quit:")
64
65
66if __name__ == "__main__":
67 _main()
Interaction Planning
Simple Touch Approach
This example shows how to use the Touch functionality to plan an approach to an object to be picked in a simplified setup.
1#include <Zivid/Motion/Application.h>
2#include <Zivid/Motion/Planner.h>
3
4#include <cassert>
5#include <cmath>
6#include <iostream>
7#include <random>
8
9namespace
10{
11 constexpr auto useVisualizer = true;
12
13 void printException(const std::exception &e, const int level = 0)
14 {
15 std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
16 try
17 {
18 std::rethrow_if_nested(e);
19 }
20 catch(const std::exception &nestedException)
21 {
22 printException(nestedException, level + 1);
23 }
24 catch(...)
25 {}
26 }
27
28 Zivid::Motion::Planner startPlanner(const Zivid::Motion::Application &app)
29 {
30 return app.createPlanner(
31 Zivid::Motion::PlannerSettings{
32 "my_cell",
33 Zivid::Motion::Profile::testing,
34 },
35 useVisualizer);
36 }
37
38 // Utility function for creating a dummy point-cloud obstacle
39 std::vector<Zivid::Motion::PointXYZ>
40 generateSpherePoints(const Zivid::Motion::PointXYZ ¢er, const float radius, const int numPoints)
41 {
42 // Fixed seed for determinism
43 std::mt19937 rng(42);
44 std::uniform_real_distribution<float> thetaDist(0, 2 * M_PI);
45 std::uniform_real_distribution<float> phiDist(0, M_PI);
46
47 std::vector<Zivid::Motion::PointXYZ> points;
48 points.reserve(numPoints);
49 for(int i = 0; i < numPoints; ++i)
50 {
51 const float theta = thetaDist(rng);
52 const float phi = phiDist(rng);
53 points.push_back(
54 {
55 center.x + radius * std::sin(phi) * std::cos(theta),
56 center.y + radius * std::sin(phi) * std::sin(theta),
57 center.z + radius * std::cos(phi),
58 });
59 }
60 return points;
61 }
62
63 // Create a pick pose with the end-effector pointing downwards at the given point
64 // (180-degree rotation around Y-axis)
65 Zivid::Motion::Pose getPickPose(const Zivid::Motion::PointXYZ &pickPoint)
66 {
67 return { {
68 { -1.f, 0.f, 0.f, pickPoint.x },
69 { 0.f, 1.f, 0.f, pickPoint.y },
70 { 0.f, 0.f, -1.f, pickPoint.z },
71 { 0.f, 0.f, 0.f, 1.f },
72 } };
73 }
74} // namespace
75
76int main()
77{
78 try
79 {
80 const Zivid::Motion::Application app;
81
82 std::cout << "Starting planner\n";
83 const auto planner = startPlanner(app);
84
85 const Zivid::Motion::Configuration startConfiguration = { 0.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
86
87 // Set a dummy obstacle to be interacted with
88 constexpr Zivid::Motion::PointXYZ obstacleCenter{ 1.1f, 1.0f, 0.9f };
89 constexpr float obstacleRadius = 0.2f;
90 planner.setObstacles(
91 { Zivid::Motion::Obstacle::fromPointCloud(
92 "interaction_object",
93 Zivid::Motion::Obstacle::PointCloud{ generateSpherePoints(obstacleCenter, obstacleRadius, 1000) }) });
94
95 // Find the pick joint configuration
96 constexpr Zivid::Motion::PointXYZ pickPoint{
97 obstacleCenter.x,
98 obstacleCenter.y,
99 obstacleCenter.z + obstacleRadius
100 - 0.01f, // Put goal 1cm into the object for good contact with gripper compliance
101 };
102 const auto pickConfiguration = planner.computeInverseKinematics(getPickPose(pickPoint), startConfiguration);
103 if(!pickConfiguration.has_value())
104 {
105 throw std::runtime_error("No valid inverse kinematics solution found for pick pose");
106 }
107
108 const Zivid::Motion::InitialState initialState{ startConfiguration };
109
110 // Path type defaults to Free
111 const auto resultFree = planner.path(
112 initialState,
113 Zivid::Motion::PathRequest{ .goalConfigurations = { pickConfiguration.value() },
114 .description = "Free path to pick goal" });
115 // Expect blocked end with path type Free, since the goal is inside the obstacle
116 assert(resultFree.status == Zivid::Motion::PlanResult::Status::blockedEnd);
117
118 const auto resultTouch = planner.path(
119 initialState,
120 Zivid::Motion::PathRequest{ .type = Zivid::Motion::PathRequest::Type::touch,
121 .goalConfigurations = { pickConfiguration.value() },
122 .description = "Touch path to pick goal" });
123 if(!resultTouch)
124 {
125 throw std::runtime_error("Planning with path type Touch failed with result: " + resultTouch.toString());
126 }
127
128 std::cout << "\nSuccessful touch path: " << resultTouch << "\n";
129
130 if(useVisualizer)
131 {
132 std::cout << "Press enter to quit:";
133 std::cin.get();
134 }
135 }
136 catch(const std::exception &exception)
137 {
138 printException(exception);
139 std::cout << "Press enter to exit." << std::endl;
140 std::cin.get();
141 return EXIT_FAILURE;
142 }
143 return EXIT_SUCCESS;
144}
Install additional dependencies with:
$ pip install scipy
1import numpy as np
2from scipy.spatial.transform import Rotation
3from zividmotion import Application, InitialState, Obstacle, PathRequest, PathResult, Planner, PlannerSettings, Profile
4
5USE_VISUALIZER = True
6
7
8def _start_planner(app: Application) -> Planner:
9 planner_settings = PlannerSettings(cell_name="my_cell", profile=Profile.testing)
10 return app.create_planner(planner_settings, use_visualizer=USE_VISUALIZER)
11
12
13# Utility method for creating a dummy obstacle
14def _generate_sphere_points(center_point: list[float], radius: float, num_points: int) -> list[list[float]]:
15 np.random.seed(42) # Set a fixed random seed for determinism
16 points = []
17 for _ in range(num_points):
18 theta = np.random.uniform(0, 2 * np.pi)
19 phi = np.random.uniform(0, np.pi)
20 x = center_point[0] + radius * np.sin(phi) * np.cos(theta)
21 y = center_point[1] + radius * np.sin(phi) * np.sin(theta)
22 z = center_point[2] + radius * np.cos(phi)
23 points.append([x, y, z])
24 return points
25
26
27def _get_pick_pose(pick_point: list[float]) -> np.ndarray:
28 # Create a simple pick pose with the end-effector pointing downwards at the pick point
29 pick_pose = np.eye(4, dtype=np.float32)
30 pick_pose[:3, 3] = pick_point
31 # Rotate 180 degrees around Y-axis to point downwards
32 pick_pose[:3, :3] = Rotation.from_rotvec([0, np.pi, 0]).as_matrix()
33 return pick_pose
34
35
36def _main() -> None:
37 app = Application()
38
39 print("Starting planner")
40 planner = _start_planner(app)
41
42 start_configuration = [0.0, 0.0, 0.0, 0.0, 1.57, 0.0]
43
44 # Set a dummy obstacle to be interacted with
45 center_point = [1.1, 1.0, 0.9]
46 radius = 0.2
47 planner.set_obstacles(
48 obstacles=[
49 Obstacle.from_point_cloud(
50 name="interaction_object",
51 points=_generate_sphere_points(center_point=center_point, radius=radius, num_points=1000),
52 )
53 ]
54 )
55
56 # Find the pick joint configuration
57 pick_point = [center_point[0], center_point[1], center_point[2] + radius]
58 pick_point[2] -= 0.01 # Put goal 1cm into the object, for instance for good contact with gripper compliance
59 pick_pose = _get_pick_pose(pick_point)
60 pick_configuration = planner.compute_inverse_kinematics(pose=pick_pose, reference_configuration=start_configuration)
61 if pick_configuration is None:
62 raise RuntimeError("No valid inverse kinematics solution found for pick pose")
63
64 initial_state = InitialState(start_configuration=start_configuration)
65 # Path type defaults to Free
66 result_free = planner.path(
67 initial_state=initial_state,
68 request=PathRequest(
69 goal_configurations=[pick_configuration],
70 description="Free path to pick goal",
71 ),
72 )
73 # Expect blocked end with path type Free, since the goal is inside the obstacle
74 assert result_free.error == PathResult.Error.blockedEnd
75
76 # Specify type touch
77 result_touch = planner.path(
78 initial_state=initial_state,
79 request=PathRequest(
80 type=PathRequest.Type.touch,
81 goal_configurations=[pick_configuration],
82 description="Touch path to pick goal",
83 ),
84 )
85 if not result_touch:
86 raise RuntimeError(f"Planning with path type Touch failed with error: {result_touch.error}")
87
88 print("\nSuccessful touch path:", result_touch.path)
89
90 if USE_VISUALIZER:
91 input("Press enter to quit:")
92
93
94if __name__ == "__main__":
95 _main()
Pick Approach and Retract with Replaceable Tool
This example shows how to do a simplified object pick, including approach and retract paths, with a replaceable tool and carried object.
1#include <Zivid/Motion/Application.h>
2#include <Zivid/Motion/Planner.h>
3
4#include <cmath>
5#include <iostream>
6#include <random>
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 "my_cell",
32 Zivid::Motion::Profile::testing,
33 },
34 useVisualizer);
35 }
36
37 // Utility function for creating a dummy point-cloud obstacle
38 std::vector<Zivid::Motion::PointXYZ>
39 generateSpherePoints(const Zivid::Motion::PointXYZ ¢er, const float radius, const int numPoints)
40 {
41 // Fixed seed for determinism
42 std::mt19937 rng(42);
43 std::uniform_real_distribution<float> thetaDist(0, 2 * M_PI);
44 std::uniform_real_distribution<float> phiDist(0, M_PI);
45
46 std::vector<Zivid::Motion::PointXYZ> points;
47 points.reserve(numPoints);
48 for(int i = 0; i < numPoints; ++i)
49 {
50 const float theta = thetaDist(rng);
51 const float phi = phiDist(rng);
52 points.push_back(
53 {
54 center.x + radius * std::sin(phi) * std::cos(theta),
55 center.y + radius * std::sin(phi) * std::sin(theta),
56 center.z + radius * std::cos(phi),
57 });
58 }
59 return points;
60 }
61
62 std::vector<Zivid::Motion::Obstacle> getDummyObstacles(const Zivid::Motion::PointXYZ ¢er, const float radius)
63 {
64 std::vector<Zivid::Motion::Obstacle> obstacles;
65 for(const float dx : { -2.f * radius, 0.f, 2.f * radius })
66 {
67 for(const float dy : { -2.f * radius, 0.f, 2.f * radius })
68 {
69 const Zivid::Motion::PointXYZ c{ center.x + dx, center.y + dy, center.z };
70 const auto name = "obstacle_" + std::to_string(obstacles.size());
71 obstacles.push_back(
72 Zivid::Motion::Obstacle::fromPointCloud(
73 name, Zivid::Motion::Obstacle::PointCloud{ generateSpherePoints(c, radius, 400) }));
74 }
75 }
76 return obstacles;
77 }
78} // namespace
79
80int main()
81{
82 try
83 {
84 const Zivid::Motion::Application app;
85
86 std::cout << "Starting planner\n";
87 const auto planner = startPlanner(app);
88
89 constexpr Zivid::Motion::PointXYZ obstacleCenter{ 1.1f, 1.0f, 0.2f };
90 constexpr float obstacleRadius = 0.15f;
91 planner.setObstacles(getDummyObstacles(obstacleCenter, obstacleRadius));
92
93 // For example purposes, simulate a suction cup gripper that is mounted asymmetrically at a 45 degree angle
94 constexpr float angle = M_PI_4;
95 const float cosA = std::cos(angle);
96 const float sinA = std::sin(angle);
97 const Zivid::Motion::Pose toolTransform{ {
98 { cosA, -sinA, 0.f, 0.f },
99 { sinA, cosA, 0.f, 0.f },
100 { 0.f, 0.f, 1.f, 0.05f },
101 { 0.f, 0.f, 0.f, 1.f },
102 } };
103 constexpr Zivid::Motion::VectorXYZ toolDimensions{ 0.4f, 0.4f, 0.06f };
104 // Specify the last 2cm of the gripper as the compliant suction cup area
105 const Zivid::Motion::Pose compliantTransform{ {
106 { 1.f, 0.f, 0.f, 0.f },
107 { 0.f, 1.f, 0.f, 0.f },
108 { 0.f, 0.f, 1.f, toolDimensions.z - 0.02f },
109 { 0.f, 0.f, 0.f, 1.f },
110 } };
111 constexpr Zivid::Motion::VectorXYZ compliantDimensions{ 0.4f, 0.4f, 0.02f };
112 planner.setReplaceableTool(
113 Zivid::Motion::ReplaceableTool::fromBoxElements(
114 { Zivid::Motion::ToolBoxElement{
115 .transform = toolTransform,
116 .rigidBoxDimensions = toolDimensions,
117 .compliantBox = Zivid::Motion::TransformedBox{ compliantTransform, compliantDimensions } } }));
118
119 // Set the new tcp to be at the center tip of the gripper, compressed 10 mm
120 Zivid::Motion::Pose tcpTransform = toolTransform;
121 tcpTransform[2][3] += toolDimensions.z - 0.01f;
122 planner.setTcp(Zivid::Motion::Tcp{ tcpTransform, { 0.f, 0.f, 1.f } });
123
124 const Zivid::Motion::Configuration startConfiguration = { 0.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
125
126 // Define a pick pose: above the obstacle center, end-effector rotated 180 degrees around Y to point downwards
127 constexpr Zivid::Motion::Pose pickPose{ {
128 { -1.f, 0.f, 0.f, obstacleCenter.x },
129 { 0.f, 1.f, 0.f, obstacleCenter.y },
130 { 0.f, 0.f, -1.f, obstacleCenter.z + obstacleRadius },
131 { 0.f, 0.f, 0.f, 1.f },
132 } };
133 const auto pickConfiguration = planner.computeInverseKinematics(pickPose, startConfiguration);
134 if(!pickConfiguration.has_value())
135 {
136 throw std::runtime_error("No valid inverse kinematics solution found for pick pose");
137 }
138
139 // Compute a pick approach path with the new tool
140 const auto approachResult = planner.path(
141 Zivid::Motion::InitialState{ startConfiguration },
142 Zivid::Motion::PathRequest{ .type = Zivid::Motion::PathRequest::Type::touch,
143 .goalConfigurations = { pickConfiguration.value() },
144 .description = "Pick approach" });
145 if(!approachResult)
146 {
147 std::cout << "Planning pick approach failed with result: " << approachResult.toString() << "\n";
148 std::cin.get();
149 }
150
151 // Set carried object (bounding box around the picked obstacle)
152 constexpr float objectSize = obstacleRadius * 2.f;
153 constexpr Zivid::Motion::Pose identityPose{ {
154 { 1.f, 0.f, 0.f, 0.f },
155 { 0.f, 1.f, 0.f, 0.f },
156 { 0.f, 0.f, 1.f, 0.f },
157 { 0.f, 0.f, 0.f, 1.f },
158 } };
159 planner.setCarriedObject(
160 Zivid::Motion::CarriedObject::fromBox({ identityPose, { objectSize, objectSize, objectSize } }));
161
162 // Compute a pick retract path with custom retract direction
163 // This retract direction is the same as the negative tcp tool direction expressed in the base frame when the robot
164 // is in the start configuration for this path call. Meaning it in this case is a redundant specification, just to
165 // show the signature for example purposes.
166 const auto retractResult = planner.path(
167 Zivid::Motion::InitialState{ approachResult },
168 Zivid::Motion::PathRequest{ .type = Zivid::Motion::PathRequest::Type::touch,
169 .retractDirection = Zivid::Motion::VectorXYZ{ 0.f, 0.f, 1.f },
170 .goalConfigurations = { startConfiguration },
171 .description = "Pick retract" });
172 if(!retractResult)
173 {
174 std::cout << "Planning pick retract failed with result: " << retractResult.toString() << "\n";
175 }
176
177 std::cout << retractResult << "\n";
178
179 if(useVisualizer)
180 {
181 std::cout << "Press enter to quit:";
182 std::cin.get();
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
1import numpy as np
2from scipy.spatial.transform import Rotation
3from zividmotion import (
4 Application,
5 CarriedObject,
6 InitialState,
7 Obstacle,
8 PathRequest,
9 Planner,
10 PlannerSettings,
11 Profile,
12 ReplaceableTool,
13 Tcp,
14 ToolBoxElement,
15 TransformedBox,
16)
17
18USE_VISUALIZER = True
19
20
21def _start_planner(app: Application) -> Planner:
22 planner_settings = PlannerSettings(cell_name="my_cell", profile=Profile.testing)
23 return app.create_planner(planner_settings, use_visualizer=USE_VISUALIZER)
24
25
26# Utility method for creating a dummy obstacle
27def _generate_sphere_points(center_point: list[float], radius: float, num_points: int) -> list[list[float]]:
28 np.random.seed(42) # Set a fixed random seed for determinism
29 points = []
30 for _ in range(num_points):
31 theta = np.random.uniform(0, 2 * np.pi)
32 phi = np.random.uniform(0, np.pi)
33 x = center_point[0] + radius * np.sin(phi) * np.cos(theta)
34 y = center_point[1] + radius * np.sin(phi) * np.sin(theta)
35 z = center_point[2] + radius * np.cos(phi)
36 points.append([x, y, z])
37 return points
38
39
40def _get_dummy_obstacles(center_point: list[float], radius: float) -> list[Obstacle]:
41 obstacles: list[Obstacle] = []
42 for x in [center_point[0] - 2 * radius, center_point[0], center_point[0] + 2 * radius]:
43 for y in [center_point[1] - 2 * radius, center_point[1], center_point[1] + 2 * radius]:
44 center = [x, y, center_point[2]]
45 points = _generate_sphere_points(center_point=center, radius=radius, num_points=400)
46 obstacles.append(Obstacle.from_point_cloud(name=f"obstacle_{len(obstacles)}", points=points))
47 return obstacles
48
49
50def _main() -> None:
51 app = Application()
52
53 print("Starting planner")
54 planner = _start_planner(app)
55
56 obstacle_center = [1.1, 1.0, 0.2]
57 obstacle_radius = 0.15
58 planner.set_obstacles(obstacles=_get_dummy_obstacles(center_point=obstacle_center, radius=obstacle_radius))
59
60 # For example purposes, simulate a suction cup gripper that is mounted asymmetrically at a 45 degree angle
61 tool_transform = np.eye(4)
62 tool_transform[1, 3] = 0.05
63 tool_transform[:3, :3] = Rotation.from_rotvec([0, 0, np.pi / 4]).as_matrix()
64 tool_dimensions = [0.4, 0.4, 0.06]
65 # Specify the last 2cm of the gripper as the compliant suction cup area
66 compliant_transform = tool_transform.copy()
67 compliant_transform[1, 3] = tool_dimensions[2] - 0.02
68 compliant_dimensions = [0.4, 0.4, 0.02]
69 planner.set_replaceable_tool(
70 replaceable_tool=ReplaceableTool.from_box_elements(
71 tool_elements=[
72 ToolBoxElement(
73 tool_transform,
74 tool_dimensions,
75 compliant_box=TransformedBox(compliant_transform, compliant_dimensions),
76 )
77 ]
78 )
79 )
80
81 # Set the new tcp to be at the center tip of the gripper, when the suction cup is compressed 1 cm.
82 tcp_transform = tool_transform.copy()
83 tcp_transform[2, 3] += tool_dimensions[2] - 0.01
84 planner.set_tcp(tcp=Tcp(transform=tcp_transform, tool_direction=[0.0, 0.0, 1.0]))
85
86 start_configuration = [0.0, 0.0, 0.0, 0.0, 1.57, 0.0]
87
88 # Define a pick pose and compute the pick joint configuration with the new tcp
89 pick_position = [obstacle_center[0], obstacle_center[1], obstacle_center[2] + obstacle_radius]
90 pick_pose = np.eye(4)
91 pick_pose[:3, 3] = pick_position
92 # Rotate 180 degrees around Y-axis to pick perpendicular to the obstacles
93 pick_pose[:3, :3] = Rotation.from_rotvec([0, np.pi, 0]).as_matrix()
94 pick_configuration = planner.compute_inverse_kinematics(pose=pick_pose, reference_configuration=start_configuration)
95 if pick_configuration is None:
96 raise RuntimeError("No valid inverse kinematics solution found for pick pose")
97
98 # Compute a pick approach path with the new tool
99 approach_result = planner.path(
100 initial_state=InitialState(start_configuration=start_configuration),
101 request=PathRequest(
102 type=PathRequest.Type.touch,
103 goal_configurations=[pick_configuration],
104 description="Pick approach",
105 ),
106 )
107 if not approach_result:
108 print(f"Planning pick approach failed with error: {approach_result.error}")
109
110 # Set carried object
111 obstacle_size = obstacle_radius * 2
112 planner.set_carried_object(
113 carried_object=CarriedObject.from_box(
114 transformed_box=TransformedBox(
115 bottom_centered_transform=np.eye(4), box_dimensions=[obstacle_size, obstacle_size, obstacle_size]
116 )
117 )
118 )
119
120 # Compute a pick retract path with custom retract direction
121 # This retract direction is the same as the negative tcp tool direction expressed in the base frame when the robot
122 # is in the start configuration for this path call. Meaning it in this case is a redundant specification, just to
123 # show the signature for example purposes.
124 retract_result = planner.path(
125 initial_state=InitialState(previous_result=approach_result),
126 request=PathRequest(
127 type=PathRequest.Type.touch,
128 retract_direction=[0.0, 0.0, 1.0],
129 goal_configurations=[start_configuration],
130 description="Pick retract",
131 ),
132 )
133 if not retract_result:
134 print(f"Planning pick retract failed with error: {retract_result.error}")
135
136 print(retract_result)
137
138 if USE_VISUALIZER:
139 input("Press enter to quit:")
140
141
142if __name__ == "__main__":
143 _main()
Robot Attachments
Path Planning with Attachment
1#include <Zivid/Motion/Application.h>
2#include <Zivid/Motion/Planner.h>
3
4#include <cassert>
5#include <cmath>
6#include <iostream>
7#include <random>
8
9namespace
10{
11 constexpr auto useVisualizer = true;
12
13 void printException(const std::exception &e, const int level = 0)
14 {
15 std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
16 try
17 {
18 std::rethrow_if_nested(e);
19 }
20 catch(const std::exception &nestedException)
21 {
22 printException(nestedException, level + 1);
23 }
24 catch(...)
25 {}
26 }
27
28 Zivid::Motion::Planner startPlanner(const Zivid::Motion::Application &app)
29 {
30 return app.createPlanner(
31 Zivid::Motion::PlannerSettings{
32 "my_cell",
33 Zivid::Motion::Profile::testing,
34 },
35 useVisualizer);
36 }
37
38 // Utility function for creating a dummy point-cloud obstacle
39 std::vector<Zivid::Motion::PointXYZ>
40 generateSpherePoints(const Zivid::Motion::PointXYZ ¢er, const float radius, const int numPoints)
41 {
42 // Fixed seed for determinism
43 std::mt19937 rng(42);
44 std::uniform_real_distribution<float> thetaDist(0, 2 * M_PI);
45 std::uniform_real_distribution<float> phiDist(0, M_PI);
46
47 std::vector<Zivid::Motion::PointXYZ> points;
48 points.reserve(numPoints);
49 for(int i = 0; i < numPoints; ++i)
50 {
51 const float theta = thetaDist(rng);
52 const float phi = phiDist(rng);
53 points.push_back(
54 {
55 center.x + radius * std::sin(phi) * std::cos(theta),
56 center.y + radius * std::sin(phi) * std::sin(theta),
57 center.z + radius * std::cos(phi),
58 });
59 }
60 return points;
61 }
62} // namespace
63
64int main()
65{
66 try
67 {
68 const Zivid::Motion::Application app;
69
70 std::cout << "Starting planner\n";
71 const auto planner = startPlanner(app);
72
73 const Zivid::Motion::Configuration startConfiguration = { 0.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
74 const Zivid::Motion::Configuration goalConfiguration = { 1.57f, 0.f, 0.f, 0.f, 1.57f, 0.f };
75
76 // Must match a name in the attachments definition in the planner config file
77 const std::string attachmentName = "my_attachment";
78
79 // Set an obstacle that obstructs the direct path for the attachment
80 planner.setObstacles(
81 { Zivid::Motion::Obstacle::fromPointCloud(
82 "sphere_obstacle", generateSpherePoints({ 1.1f, 1.0f, 1.2f }, 0.2f, 1000)) });
83
84 const Zivid::Motion::InitialState initialState{ startConfiguration };
85
86 const auto resultWithoutAttachment = planner.path(
87 initialState,
88 Zivid::Motion::PathRequest{ .goalConfigurations = { goalConfiguration },
89 .description = "Path without attachment" });
90 if(!resultWithoutAttachment)
91 {
92 throw std::runtime_error(
93 "Planning without active attachment failed with result: " + resultWithoutAttachment.toString());
94 }
95 assert(resultWithoutAttachment.path().size() == 1);
96 std::cout << "Path without attachment: " << resultWithoutAttachment.path().size() << " waypoints\n";
97
98 std::cout << "Setting attachment " << attachmentName << "\n";
99 planner.setAttachments({ attachmentName });
100
101 const auto resultWithAttachment = planner.path(
102 initialState,
103 Zivid::Motion::PathRequest{ .goalConfigurations = { goalConfiguration },
104 .description = "Path with attachment" });
105 if(!resultWithAttachment)
106 {
107 std::cout << "Planning with active attachment failed with result: " << resultWithAttachment.toString()
108 << "\n";
109 }
110 assert(resultWithAttachment.path().size() > 1);
111 std::cout << "With attachment: " << resultWithAttachment.path().size() << " waypoints\n";
112
113 if(useVisualizer)
114 {
115 std::cout << "Press enter to quit:";
116 std::cin.get();
117 }
118 }
119 catch(const std::exception &exception)
120 {
121 printException(exception);
122 std::cout << "Press enter to exit." << std::endl;
123 std::cin.get();
124 return EXIT_FAILURE;
125 }
126 return EXIT_SUCCESS;
127}
1import numpy as np
2from zividmotion import Application, InitialState, Obstacle, PathRequest, Planner, PlannerSettings, Profile
3
4USE_VISUALIZER = True
5
6
7def _start_planner(app: Application) -> Planner:
8 planner_settings = PlannerSettings(cell_name="my_cell", profile=Profile.testing)
9 return app.create_planner(planner_settings, use_visualizer=USE_VISUALIZER)
10
11
12# Utility method for creating a dummy obstacle
13def _generate_sphere_points(center_point: list[float], radius: float, num_points: int) -> list[list[float]]:
14 np.random.seed(42) # Set a fixed random seed for determinism
15 points = []
16 for _ in range(num_points):
17 theta = np.random.uniform(0, 2 * np.pi)
18 phi = np.random.uniform(0, np.pi)
19 x = center_point[0] + radius * np.sin(phi) * np.cos(theta)
20 y = center_point[1] + radius * np.sin(phi) * np.sin(theta)
21 z = center_point[2] + radius * np.cos(phi)
22 points.append([x, y, z])
23 return points
24
25
26def _main() -> None:
27 app = Application()
28
29 print("Starting planner")
30 planner = _start_planner(app)
31
32 start_configuration = [0.0, 0.0, 0.0, 0.0, 1.57, 0.0]
33 goal_configuration = [1.57, 0.0, 0.0, 0.0, 1.57, 0.0]
34 attachment_name = "my_attachment" # Must match a name in the attachments definition in the planner config file
35
36 # Set an obstacle that obstructs the direct path for the attachment
37 planner.set_obstacles(
38 obstacles=[
39 Obstacle.from_point_cloud(
40 name="sphere_obstacle",
41 points=_generate_sphere_points(center_point=[1.1, 1.0, 1.2], radius=0.2, num_points=1000),
42 )
43 ]
44 )
45
46 initial_state = InitialState(start_configuration=start_configuration)
47 result_without_attachment = planner.path(
48 initial_state=initial_state,
49 request=PathRequest(
50 goal_configurations=[goal_configuration],
51 description="Path without attachment",
52 ),
53 )
54 if not result_without_attachment:
55 raise RuntimeError(f"Planning without active attachment failed with error: {result_without_attachment.error}")
56 assert len(result_without_attachment.path) == 1
57 print(f"Path without attachment: {len(result_without_attachment.path)} waypoints")
58
59 print("Setting attachment", attachment_name)
60 planner.set_attachments(attachments=[attachment_name])
61
62 result_with_attachment = planner.path(
63 initial_state=initial_state,
64 request=PathRequest(
65 goal_configurations=[goal_configuration],
66 description="Path with attachment",
67 ),
68 )
69 if not result_with_attachment:
70 print(f"Planning with active attachment failed with error: {result_with_attachment.error}")
71 assert len(result_with_attachment.path) > 1
72 print(f"Path with attachment: {len(result_with_attachment.path)} waypoints")
73
74 if USE_VISUALIZER:
75 input("Press enter to quit:")
76
77
78if __name__ == "__main__":
79 _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 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 "my_cell",
32 Zivid::Motion::Profile::testing,
33 },
34 useVisualizer);
35 }
36} // namespace
37
38int main()
39{
40 const Zivid::Motion::Application app;
41
42 std::cout << "Starting planner\n";
43 const auto planner = startPlanner(app);
44
45 try
46 {
47 const Zivid::Motion::Configuration startConfiguration = { 0.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
48 const Zivid::Motion::Configuration invalidGoalConfiguration = { 100.f, 0.f, 0.f, 0.f, 1.57f, 0.f };
49
50 const auto unsuccessfulResult = planner.path(
51 Zivid::Motion::InitialState{ startConfiguration },
52 Zivid::Motion::PathRequest{ .goalConfigurations = { invalidGoalConfiguration },
53 .description = "Path with unreachable goal" });
54 std::cout << "Path result has status: " << unsuccessfulResult.toString() << "\n";
55
56 // Some operation that fails, like creating an InitialState from an unsuccessful path result
57 const Zivid::Motion::InitialState initialState{ unsuccessfulResult };
58 const auto result = planner.path(
59 Zivid::Motion::InitialState{ startConfiguration },
60 Zivid::Motion::PathRequest{ .goalConfigurations = { startConfiguration },
61 .description = "Path with invalid initial state" });
62 std::cout << result << "\n";
63 }
64 catch(const std::exception &exception)
65 {
66 const std::filesystem::path outputFolder = "my/desired/output/folder";
67
68 const auto logPath = planner.exportApiLog(outputFolder);
69 const auto packagePath = Zivid::Motion::packageApiLog(app, logPath);
70 std::cerr << "Planner failed, debug package available at: " + std::filesystem::canonical(packagePath).string()
71 << std::endl;
72
73 printException(exception);
74 std::cout << "Press enter to exit." << std::endl;
75 std::cin.get();
76 return EXIT_FAILURE;
77 }
78 return EXIT_SUCCESS;
79}
1from pathlib import Path
2
3from zividmotion import Application, InitialState, PathRequest, Planner, PlannerSettings, Profile, package_api_log
4
5
6def _start_planner(app: Application) -> Planner:
7 planner_settings = PlannerSettings(cell_name="my_cell", profile=Profile.testing)
8 return app.create_planner(planner_settings)
9
10
11def _main() -> None:
12 app = Application()
13
14 print("Starting planner")
15 planner = _start_planner(app)
16
17 start_configuration = [0, 0, 0, 0, 1.57, 0]
18 invalid_goal_configuration = [100, 0, 0, 0, 1.57, 0]
19
20 unsuccessful_result = planner.path(
21 initial_state=InitialState(start_configuration=start_configuration),
22 request=PathRequest(
23 goal_configurations=[invalid_goal_configuration],
24 description="Path with unreachable goal",
25 ),
26 )
27 assert unsuccessful_result.error is not None
28 print("Path result has error: ", unsuccessful_result.error)
29
30 try:
31 # Some operation that fails, like creating an InitialState from an unsuccessful path result
32 initial_state = InitialState(previous_result=unsuccessful_result)
33 result = planner.path(
34 initial_state=initial_state,
35 request=PathRequest(
36 goal_configurations=[start_configuration],
37 description="Path with invalid initial state",
38 ),
39 )
40 print(result)
41
42 except Exception as e:
43 output_folder = Path("my/desired/output/folder")
44
45 log_path = planner.export_api_log(output_directory=output_folder)
46 package_path = package_api_log(application=app, api_log_path=log_path)
47
48 raise RuntimeError(f"Planner failed, debug package available at: {package_path.resolve()}") from e
49
50
51if __name__ == "__main__":
52 _main()