andrejorsula/pymoveit2 repository overview

sudo apt install ros-$ROS_DISTRO-pymoveit2

pip install pymoveit2

cd $WS
git clone https://github.com/AndrejOrsula/pymoveit2.git
pymoveit2/.docker/run.bash $ROS_DISTRO --network-host --ipc-host --gui

cd $COLCON_WS
git clone https://github.com/AndrejOrsula/pymoveit2.git src/pymoveit2
rosdep install -y -r -i --rosdistro $ROS_DISTRO --from-paths src/pymoveit2
colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release"
source install/local_setup.bash

# Forward (joint positions -> end effector pose)
ros2 run pymoveit2 ex_fk.py
# Inverse (end effector pose -> joint positions)
ros2 run pymoveit2 ex_ik.py --ros-args -p position:="[0.3, 0.0, 0.3]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]"

# Move to a joint configuration (default to SRDF group state)
ros2 run pymoveit2 ex_joint_goal.py
# Move to a pose (motion in joint space or Cartesian space)
ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.3, 0.0, 0.3]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False
# Move while maintaining a fixed orientation of the end effector
ros2 run pymoveit2 ex_orientation_path_constraint.py --ros-args -p use_orientation_constraint:=True
# Move via real-time servoing (MoveIt 2 Servo)
ros2 run pymoveit2 ex_servo.py
# Actuate the gripper (action: {toggle, open, close})
ros2 run pymoveit2 ex_gripper.py --ros-args -p action:="toggle"

# Move the robot through `RobotSession` (initialized via `connect()`)
ros2 run pymoveit2 ex_session.py --ros-args -p position:="[0.3, 0.0, 0.3]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]"

# Add a primitive shape to the planning scene (shape: {box, sphere, cone, cylinder})
ros2 run pymoveit2 ex_collision_primitive.py --ros-args -p shape:="sphere" -p position:="[0.5, 0.0, 0.5]" -p dimensions:="[0.04]"
# Add a triangular mesh to the planning scene (action: {add, remove}) [Note: Requires `trimesh` Python package]
ros2 run pymoveit2 ex_collision_mesh.py --ros-args -p action:="add" -p position:="[0.5, 0.0, 0.5]" -p quat_xyzw:="[0.0, 0.0, -0.707, 0.707]"
# Allow or forbid collisions with a planning scene object (allow: {true, false})
ros2 run pymoveit2 ex_allow_collisions.py --ros-args -p id:="sphere" -p allow:=true
# Remove all objects from the planning scene
ros2 run pymoveit2 ex_clear_planning_scene.py

from pymoveit2 import connect

with connect() as robot:
    # Move to a pose while checking for failures
    if not robot.move_to_pose([0.4, 0.0, 0.4], [1.0, 0.0, 0.0, 0.0]):
        print(robot.last_failure())
        exit(1)

    # Actuate the gripper
    robot.gripper.close()

    # Move to SRDF group state
    robot.move_to_configuration()

from pymoveit2 import RobotDescription, MoveIt2, GripperInterface, MoveIt2Servo

node = ...  # your rclpy node

description = RobotDescription.from_node(node) # note: in the unlikely case of failure, `pymoveit2.robots` provides static robot presets
moveit2 = MoveIt2(node=node, **description.moveit2_kwargs())
gripper = GripperInterface(node=node, **description.moveit2_gripper_kwargs()) if description.gripper_group_name else None
servo = MoveIt2Servo(node=node, frame_id=str(description.moveit2_kwargs()["base_link_name"]))