22
22
MoveItPy ,
23
23
PlanRequestParameters ,
24
24
)
25
- from crane_plus_examples_py .utils import plan_and_execute
25
+ from crane_x7_examples_py .utils import plan_and_execute
26
26
27
27
28
28
def main (args = None ):
@@ -33,13 +33,13 @@ def main(args=None):
33
33
logger = get_logger ("pose_groupstate" )
34
34
35
35
# MoveItPy初期化
36
- crane_plus = MoveItPy (node_name = "moveit_py" )
37
- crane_plus_arm = crane_plus .get_planning_component ("arm" )
36
+ crane_x7 = MoveItPy (node_name = "moveit_py" )
37
+ crane_x7_arm = crane_x7 .get_planning_component ("arm" )
38
38
logger .info ("MoveItPy instance created" )
39
39
40
40
# planningのパラメータ設定
41
41
plan_request_params = PlanRequestParameters (
42
- crane_plus ,
42
+ crane_x7 ,
43
43
"ompl_rrtc" ,
44
44
)
45
45
@@ -48,37 +48,37 @@ def main(args=None):
48
48
plan_request_params .max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0
49
49
50
50
# 現在の位置から"home"ポジションに動かす
51
- crane_plus_arm .set_start_state_to_current_state ()
52
- crane_plus_arm .set_goal_state (configuration_name = "home" )
51
+ crane_x7_arm .set_start_state_to_current_state ()
52
+ crane_x7_arm .set_goal_state (configuration_name = "home" )
53
53
plan_and_execute (
54
- crane_plus ,
55
- crane_plus_arm ,
54
+ crane_x7 ,
55
+ crane_x7_arm ,
56
56
logger ,
57
57
single_plan_parameters = plan_request_params ,
58
58
)
59
59
60
60
# 現在の位置から"vertical"ポジションに動かす
61
- crane_plus_arm .set_start_state_to_current_state ()
62
- crane_plus_arm .set_goal_state (configuration_name = "vertical" )
61
+ crane_x7_arm .set_start_state_to_current_state ()
62
+ crane_x7_arm .set_goal_state (configuration_name = "vertical" )
63
63
plan_and_execute (
64
- crane_plus ,
65
- crane_plus_arm ,
64
+ crane_x7 ,
65
+ crane_x7_arm ,
66
66
logger ,
67
67
single_plan_parameters = plan_request_params ,
68
68
)
69
69
70
70
# 現在の位置から"home"ポジションに動かす
71
- crane_plus_arm .set_start_state_to_current_state ()
72
- crane_plus_arm .set_goal_state (configuration_name = "home" )
71
+ crane_x7_arm .set_start_state_to_current_state ()
72
+ crane_x7_arm .set_goal_state (configuration_name = "home" )
73
73
plan_and_execute (
74
- crane_plus ,
75
- crane_plus_arm ,
74
+ crane_x7 ,
75
+ crane_x7_arm ,
76
76
logger ,
77
77
single_plan_parameters = plan_request_params ,
78
78
)
79
79
80
80
# MoveItPyの終了
81
- crane_plus .shutdown ()
81
+ crane_x7 .shutdown ()
82
82
83
83
# rclpyの終了
84
84
rclpy .shutdown ()
0 commit comments