Import example code and basic prototype

examples/py/ex_throw_object.py

@@ -0,0 +1,151 @@
+#!/usr/bin/env python3
+
+from copy import deepcopy
+from threading import Thread
+
+import rclpy
+from geometry_msgs.msg import Point, Quaternion
+from pymoveit2 import MoveIt2, MoveIt2Gripper
+from pymoveit2.robots import panda
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.node import Node
+
+
+class MoveItThrowObject(Node):
+    def __init__(self):
+
+        super().__init__("ex_throw_object")
+
+        # Create callback group that allows execution of callbacks in parallel without restrictions
+        self._callback_group = ReentrantCallbackGroup()
+
+        # Create MoveIt 2 interface
+        self._moveit2 = MoveIt2(
+            node=self,
+            joint_names=panda.joint_names(),
+            base_link_name=panda.base_link_name(),
+            end_effector_name=panda.end_effector_name(),
+            group_name=panda.MOVE_GROUP_ARM,
+            callback_group=self._callback_group,
+        )
+        # Use upper joint velocity and acceleration limits
+        self._moveit2.max_velocity = 1.0
+        self._moveit2.max_acceleration = 1.0
+
+        # Create MoveIt 2 interface for gripper
+        self._moveit2_gripper = MoveIt2Gripper(
+            node=self,
+            gripper_joint_names=panda.gripper_joint_names(),
+            open_gripper_joint_positions=panda.OPEN_GRIPPER_JOINT_POSITIONS,
+            closed_gripper_joint_positions=panda.CLOSED_GRIPPER_JOINT_POSITIONS,
+            gripper_group_name=panda.MOVE_GROUP_GRIPPER,
+            callback_group=self._callback_group,
+        )
+
+        self.get_logger().info("Initialization successful.")
+
+    def throw(self):
+        """
+        Plan and execute hard-coded trajectory with intention to throw an object
+        """
+
+        self.get_logger().info("Throwing... Wish me luck!")
+
+        throwing_object_pos = Point(x=0.5, y=0.0, z=0.015)
+        default_quat = Quaternion(x=1.0, y=0.0, z=0.0, w=0.0)
+
+        # Open gripper
+        self._moveit2_gripper.open()
+        self._moveit2_gripper.wait_until_executed()
+
+        # Move above object
+        position_above_object = deepcopy(throwing_object_pos)
+        position_above_object.z += 0.15
+        self._moveit2.move_to_pose(
+            position=position_above_object,
+            quat_xyzw=default_quat,
+        )
+        self._moveit2.wait_until_executed()
+
+        # Move to grasp position
+        self._moveit2.move_to_pose(
+            position=throwing_object_pos,
+            quat_xyzw=default_quat,
+        )
+        self._moveit2.wait_until_executed()
+
+        # Close gripper
+        self._moveit2_gripper.close()
+        self._moveit2_gripper.wait_until_executed()
+
+        # Decrease speed
+        self._moveit2.max_velocity = 0.25
+        self._moveit2.max_acceleration = 0.1
+
+        # Move above object (again)
+        self._moveit2.move_to_pose(
+            position=position_above_object,
+            quat_xyzw=default_quat,
+        )
+        self._moveit2.wait_until_executed()
+
+        # Move to pre-throw configuration
+        joint_configuration_pre_throw = [0.0, -1.75, 0.0, -0.1, 0.0, 3.6, 0.8]
+        self._moveit2.move_to_configuration(joint_configuration_pre_throw)
+        self._moveit2.wait_until_executed()
+
+        # Increase speed
+        self._moveit2.max_velocity = 1.0
+        self._moveit2.max_acceleration = 1.0
+
+        # Throw itself
+        joint_configuration_throw = [0.0, 1.0, 0.0, -1.1, 0.0, 1.9, 0.8]
+        self._moveit2.move_to_configuration(joint_configuration_throw)
+
+        # Release object while executing motion
+        sleep_duration_s = 1.2
+        if rclpy.ok():
+            self.create_rate(1 / sleep_duration_s).sleep()
+        self._moveit2_gripper.open()
+        self._moveit2_gripper.wait_until_executed()
+        self._moveit2.wait_until_executed()
+
+        # Return to default configuration
+        joint_configuration_default = [
+            0.0,
+            -0.7853981633974483,
+            0.0,
+            -2.356194490192345,
+            0.0,
+            1.5707963267948966,
+            0.7853981633974483,
+        ]
+        self._moveit2.move_to_configuration(joint_configuration_default)
+        self._moveit2.wait_until_executed()
+
+
+def main(args=None):
+
+    rclpy.init(args=args)
+
+    object_thrower = MoveItThrowObject()
+
+    # Spin the node in background thread(s)
+    executor = rclpy.executors.MultiThreadedExecutor(3)
+    executor.add_node(object_thrower)
+    executor_thread = Thread(target=executor.spin, daemon=True, args=())
+    executor_thread.start()
+
+    # Wait for everything to setup
+    sleep_duration_s = 2.0
+    if rclpy.ok():
+        object_thrower.create_rate(1 / sleep_duration_s).sleep()
+
+    object_thrower.throw()
+
+    rclpy.shutdown()
+    exit(0)
+
+
+if __name__ == "__main__":
+    main()