Plan Robot Arm Movements with MoveIt! & ROS

1. 1
   Install ROS 2 and MoveIt!

   Motion Planning with MoveIt! requires a working ROS 2 environment. Follow the official installation guides for your OS. Once ROS 2 is installed, install the MoveIt! tutorials package which contains the demo environment.
2. 2
   Launch the Simulation

   Source your ROS 2 environment and launch the MoveIt! demo. This starts a simulated Panda robot arm in the RViz visualization tool. You'll see the robot model and interactive markers you can drag to set goals manually.
3. 3
   Create a Python Planning Script

   Create a Python file (e.g., `plan_arm.py`) and add the starter code. This script uses the `moveit_py` library to connect to the running MoveIt! instance, define a target pose for the arm, and prepare the planning component.
4. 4
   Plan and Execute the Motion

   With the simulation still running in one terminal, open a new terminal, source your ROS environment, and run your Python script. Observe the RViz window: you'll see the planned trajectory visualized, and then the robot arm will execute the movement.

import rclpy
from rclpy.node import Node
from moveit.core.robot_state import RobotState
from moveit.planning import MoveItPy
from geometry_msgs.msg import PoseStamped

def main():
    rclpy.init()
    logger = rclpy.logging.get_logger("moveit_py_demo")

    # Instantiate MoveItPy
    moveit = MoveItPy(node_name="moveit_py")
    logger.info("MoveItPy instance created")

    # Get the planning component for the panda arm
    panda_arm = moveit.get_planning_component("panda_arm")
    logger.info("Planning component for 'panda_arm' obtained")

    # Set a target pose
    target_pose = PoseStamped()
    target_pose.header.frame_id = "panda_link0"
    target_pose.pose.position.x = 0.3
    target_pose.pose.position.y = 0.1
    target_pose.pose.position.z = 0.6
    target_pose.pose.orientation.w = 1.0
    target_pose.pose.orientation.x = 0.0
    target_pose.pose.orientation.y = 0.0
    target_pose.pose.orientation.z = 0.0

    # Set the goal and plan
    panda_arm.set_goal_state(pose_stamped_msg=target_pose, pose_link="panda_link8")
    plan_result = panda_arm.plan()

    # Execute the plan
    if plan_result:
        logger.info("Executing plan")
        robot_trajectory = plan_result.trajectory
        moveit.execute(robot_trajectory, controllers=[])
    else:
        logger.error("Planning failed")

    rclpy.shutdown()

if __name__ == "__main__":
    main()