joint_values.pyについて動作確認できた

crane_plus_examples_py/crane_plus_examples_py/joint_values.py

@@ -1,11 +1,11 @@
 # Copyright 2020 RT Corporation
-# 
+#
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
 # You may obtain a copy of the License at
-# 
+#
 #     http://www.apache.org/licenses/LICENSE-2.0
-# 
+#
 # Unless required by applicable law or agreed to in writing, software
 # distributed under the License is distributed on an "AS IS" BASIS,
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
@@ -20,16 +20,14 @@
 from rclpy.logging import get_logger
 
 # moveit python library
+from moveit.core.robot_state import RobotState
 from moveit.planning import (
     MoveItPy,
     PlanRequestParameters,
 )
+from moveit.core.kinematic_constraints import construct_joint_constraint
 
-from utils import plan_and_execute
-
-
-def to_radians(deg_angle):
-    return deg_angle * math.pi / 180.0
+from crane_plus_examples_py.utils import plan_and_execute
 
 
 def main(args=None):
@@ -43,41 +41,35 @@ def main(args=None):
     crane_plus = MoveItPy(node_name="moveit_py")
     crane_plus_arm = crane_plus.get_planning_component("arm")
     logger.info("MoveItPy instance created")
+
+    # instantiate a a RobotState instance using the current robot model
+    robot_model = crane_plus.get_robot_model()
+    robot_state = RobotState(robot_model)
 
-    # 速度＆加速度のスケーリングファクタを設定
     plan_request_params = PlanRequestParameters(
         crane_plus,
         "ompl_rrtc",
     )
-    plan_request_params.max_acceleration_scaling_factor(1.0)  # Set 0.0 ~ 1.0
-    plan_request_params.max_velocity_scaling_factor(1.0)  # Set 0.0 ~ 1.0
-
-    # 現在の位置から"vertical"ポジションに動かす
-    crane_plus_arm.set_start_state_to_current_state()
-    crane_plus_arm.set_goal_state(configuration_name="vertical")
-    plan_and_execute(
-        crane_plus,
-        crane_plus_arm,
-        logger,
-        single_plan_parameters=plan_request_params,
-    )
-
-    # 現在の位置からarmの関節を全て順番に45度動かす
-    joint_values = crane_plus_arm.get_named_target_state_values("arm")
-    target_joint_value = math.radians(45.0)
 
-    # joint_valuesはDict型なので、keyの数（=jointの数）だけ関節を動かす
-    for key in enumerate(joint_values.keys()):
-        joint_values[key] = target_joint_value
+    # armの関節のjoint1〜4を順番に45[deg]ずつ動かす
+    joint_names = ["crane_plus_joint1", "crane_plus_joint2", "crane_plus_joint3", "crane_plus_joint4"]
+    target_joint_value = math.radians(45)
+    for joint_name in joint_names:
+        joint_values = {joint_name: target_joint_value}
+        robot_state.joint_positions = joint_values
+        joint_constraint = construct_joint_constraint(
+            robot_state=robot_state,
+            joint_model_group=crane_plus.get_robot_model().get_joint_model_group("arm"),
+        )
         crane_plus_arm.set_start_state_to_current_state()
-        crane_plus_arm.set_goal_state(configuration_name="vertical")
+        crane_plus_arm.set_goal_state(motion_plan_constraints=[joint_constraint])
         plan_and_execute(
             crane_plus,
             crane_plus_arm,
             logger,
             single_plan_parameters=plan_request_params,
         )
-    
+
     # 現在の位置から"vertical"ポジションに動かす
     crane_plus_arm.set_start_state_to_current_state()
     crane_plus_arm.set_goal_state(configuration_name="vertical")
@@ -91,6 +83,9 @@ def main(args=None):
     # MoveItPyの終了
     crane_plus.shutdown()
 
+    # rclpyの終了
+    rclpy.shutdown()
+
 
 if __name__ == "__main__":
     main()