Import example code and basic prototype

examples/cpp/ex_follow_target.cpp

@@ -0,0 +1,71 @@
+/// Example that uses MoveIt 2 to follow a target inside Ignition Gazebo
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <rclcpp/qos.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+const std::string MOVE_GROUP = "arm";
+
+class MoveItFollowTarget : public rclcpp::Node
+{
+public:
+  /// Constructor
+  MoveItFollowTarget();
+
+  /// Move group interface for the robot
+  moveit::planning_interface::MoveGroupInterface move_group_;
+  /// Subscriber for target pose
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr target_pose_sub_;
+  /// Target pose that is used to detect changes
+  geometry_msgs::msg::Pose previous_target_pose_;
+
+private:
+  /// Callback that plans and executes trajectory each time the target pose is changed
+  void target_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg);
+};
+
+MoveItFollowTarget::MoveItFollowTarget() : Node("ex_follow_target"),
+                                           move_group_(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
+{
+  // Use upper joint velocity and acceleration limits
+  this->move_group_.setMaxAccelerationScalingFactor(1.0);
+  this->move_group_.setMaxVelocityScalingFactor(1.0);
+
+  // Subscribe to target pose
+  target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
+
+  RCLCPP_INFO(this->get_logger(), "Initialization successful.");
+}
+
+void MoveItFollowTarget::target_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg)
+{
+  // Return if target pose is unchanged
+  if (msg->pose == previous_target_pose_)
+  {
+    return;
+  }
+
+  RCLCPP_INFO(this->get_logger(), "Target pose has changed. Planning and executing...");
+
+  // Plan and execute motion
+  this->move_group_.setPoseTarget(msg->pose);
+  this->move_group_.move();
+
+  // Update for next callback
+  previous_target_pose_ = msg->pose;
+}
+
+int main(int argc, char *argv[])
+{
+  rclcpp::init(argc, argv);
+
+  auto target_follower = std::make_shared<MoveItFollowTarget>();
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(target_follower);
+  executor.spin();
+
+  rclcpp::shutdown();
+  return EXIT_SUCCESS;
+}

examples/py/ex_follow_target.py

@@ -0,0 +1,83 @@
+#!/usr/bin/env python3
+"""Example that uses MoveIt 2 to follow a target inside Ignition Gazebo"""
+
+import rclpy
+from geometry_msgs.msg import Pose, PoseStamped
+from pymoveit2 import MoveIt2
+from pymoveit2.robots import panda
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.node import Node
+from rclpy.qos import QoSProfile
+
+
+class MoveItFollowTarget(Node):
+    def __init__(self):
+
+        super().__init__("ex_follow_target_py")
+
+        # 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,
+            execute_via_moveit=True,
+            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 a subscriber for target pose
+        self.__previous_target_pose = Pose()
+        self.create_subscription(
+            msg_type=PoseStamped,
+            topic="/target_pose",
+            callback=self.target_pose_callback,
+            qos_profile=QoSProfile(depth=1),
+            callback_group=self._callback_group,
+        )
+
+        self.get_logger().info("Initialization successful.")
+
+    def target_pose_callback(self, msg: PoseStamped):
+        """
+        Plan and execute trajectory each time the target pose is changed
+        """
+
+        # Return if target pose is unchanged
+        if msg.pose == self.__previous_target_pose:
+            return
+
+        self.get_logger().info("Target pose has changed. Planning and executing...")
+
+        # Plan and execute motion
+        self._moveit2.move_to_pose(
+            position=msg.pose.position,
+            quat_xyzw=msg.pose.orientation,
+        )
+
+        # Update for next callback
+        self.__previous_target_pose = msg.pose
+
+
+def main(args=None):
+
+    rclpy.init(args=args)
+
+    target_follower = MoveItFollowTarget()
+
+    executor = rclpy.executors.MultiThreadedExecutor(2)
+    executor.add_node(target_follower)
+    executor.spin()
+
+    rclpy.shutdown()
+    exit(0)
+
+
+if __name__ == "__main__":
+    main()

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()