Samples
Setup
Generate Planning Data
이 예시는 Motion 플래너를 시작하기 전에 필요한 로봇 셀의 계획 데이터를 생성하는 방법을 보여줍니다.
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
이 예시는 Motion 플래너 실행에 필요한 모든 파일을 압축 파일(zip)로 묶어 배포하고 다른 컴퓨터에서 압축을 해제하는 방법을 보여줍니다.
플래너 설정을 완료한 워크스테이션에서 다음 코드 조각을 실행하십시오.
1#include <Zivid/Motion/Application.h>
2#include <Zivid/Motion/Packaging.h>
3
4#include <filesystem>
5#include <iostream>
6
7namespace
8{
9 void printException(const std::exception &e, const int level = 0)
10 {
11 std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
12 try
13 {
14 std::rethrow_if_nested(e);
15 }
16 catch(const std::exception &nestedException)
17 {
18 printException(nestedException, level + 1);
19 }
20 catch(...)
21 {}
22 }
23} // namespace
24
25int main()
26{
27 try
28 {
29 const Zivid::Motion::Application app;
30 const std::filesystem::path outputPath = "demo_cell.zip";
31
32 // In this example, we package not only the configuration files for the cell, but also the generated data
33 // for the testing setup.
34 std::cout << "Packaging data...\n";
35 Zivid::Motion::packageCell(app, "demo_cell", outputPath, { Zivid::Motion::Profile::testing });
36
37 std::cout << "Data package stored at: " << std::filesystem::canonical(outputPath) << "\n";
38 }
39 catch(const std::exception &exception)
40 {
41 printException(exception);
42 return EXIT_FAILURE;
43 }
44 return EXIT_SUCCESS;
45}
1from pathlib import Path
2
3from zividmotion import Application, Profile, package_cell
4
5
6def _main() -> None:
7 app = Application()
8
9 output_path = Path("demo_cell.zip")
10
11 # In this example, we package not only the configuration files for the cell, but also the generated data
12 # for the testing setup.
13 print("Packaging data...")
14 package_cell(app, cell_name="demo_cell", output_path=output_path, include_generated_data=[Profile.testing])
15 print(f"Data package stored at: {output_path.resolve()}")
16
17
18if __name__ == "__main__":
19 _main()
압축 파일을 받은 사람은 다음 코드 조각을 실행하여 설치 프로그램을 압축 해제할 수 있습니다.
1#include <Zivid/Motion/Application.h>
2#include <Zivid/Motion/Packaging.h>
3
4#include <filesystem>
5#include <iostream>
6
7namespace
8{
9 void printException(const std::exception &e, const int level = 0)
10 {
11 std::cerr << std::string(level * 4, ' ') << (level ? "+ " : "") << "Exception: " << e.what() << '\n';
12 try
13 {
14 std::rethrow_if_nested(e);
15 }
16 catch(const std::exception &nestedException)
17 {
18 printException(nestedException, level + 1);
19 }
20 catch(...)
21 {}
22 }
23} // namespace
24
25int main()
26{
27 try
28 {
29 const Zivid::Motion::Application app;
30 const std::filesystem::path packagePath = "demo_cell.zip";
31
32 std::cout << "Installing package from " << std::filesystem::canonical(packagePath) << "...\n";
33 Zivid::Motion::installPackage(app, packagePath);
34
35 std::cout << "Package installed.\n";
36 }
37 catch(const std::exception &exception)
38 {
39 printException(exception);
40 return EXIT_FAILURE;
41 }
42 return EXIT_SUCCESS;
43}
1from pathlib import Path
2
3from zividmotion import Application, install_package
4
5
6def _main() -> None:
7 app = Application()
8
9 package_path = Path("demo_cell.zip")
10
11 print(f"Installing package from {package_path.resolve()}...")
12 install_package(app, package_path)
13
14 print("Package installed.")
15
16
17if __name__ == "__main__":
18 _main()
Visualize Cell
이 예제는 구성된 로봇 셀에 대한 3D 시각화 창을 여는 방법을 보여줍니다. 이는 Motion 플래너를 생성하거나 실행하기 전에 셀 설정을 검사하는 데 유용합니다.
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
이 예시는 Motion 플래너의 동적 환경 모델에 상자와 같은 간단한 장애물을 추가하는 방법을 보여줍니다.
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
이 예제는 Motion 플래너의 동적 환경 모델에 사용자 지정 CAD 파일을 추가하는 방법을 보여줍니다. 정적 장애물을 나타내는 CAD 파일은 런타임에 동적 장애물로 추가하는 것보다 URDF 파일에 정적 장애물로 추가하는 것이 더 효율적으로 처리됩니다.
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
이 예제는 Zivid 카메라의 포인트 클라우드를 Motion 플래너의 동적 환경 모델에 추가하는 방법을 보여줍니다. 이를 위해서는 Zivid SDK가 설치되어 있어야 하고 Zivid 카메라에 연결되어 있어야 합니다.
1#include <Zivid/Motion/Application.h>
2#include <Zivid/Motion/Planner.h>
3#include <Zivid/Motion/Visualizer.h>
4#include <Zivid/Zivid.h>
5
6#include <iostream>
7
8namespace
9{
10 constexpr auto useVisualizer = 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 ¢er, 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}
추가 종속성을 설치하려면 다음 명령을 사용하십시오.
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
이 예시는 교체 가능한 공구와 운반 물체를 사용하여 접근 및 후퇴 경로를 포함한 간소화된 물체 선택 방법을 보여줍니다.
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 ¢er, 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 ¢er, 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}
추가 종속성을 설치하려면 다음 명령을 사용하십시오.
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 ¢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::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
이 예제에서는 플래너 설정을 재현하고 동일한 플래너 상태에서 실패한 API 호출을 다시 실행하는 데 필요한 모든 내용을 포함하는 ZIP 아카이브를 생성하는 방법을 보여줍니다. 이 예제는 오류가 발생하는 것이 정상입니다.
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()