├── .docker
├── camera_calibration_app
│ ├── Dockerfile
│ └── deploy_calibration_app.yaml
├── cameras
│ ├── Dockerfile.zed
│ ├── docker-compose-zed-2i-1.yaml
│ ├── docker-compose-zed-2i-2.yaml
│ ├── docker-compose-zed-mini.yaml
│ └── ros_entrypoint.sh
├── control
│ ├── Dockerfile.control
│ ├── docker-compose-control-server-real-dev.yaml
│ └── docker-compose-control-server-real.yaml
├── llms
│ └── docker-compose-ollama.yaml
├── motion_planning
│ ├── Dockerfile.motion_planning
│ ├── docker-compose-motion-planning-dev.yaml
│ └── docker-compose-motion-planning.yaml
├── object_detection
│ ├── Dockerfile
│ └── deploy_object_detection.yaml
├── policy_deployment
│ ├── Dockerfile.policy_deployment
│ └── docker-compose-policy-deployment-dev.yaml
├── ros_entrypoint.sh
├── transporter_data_collection_app
│ ├── Dockerfile
│ └── deploy_transporter_data_collection_app.yaml
└── transporter_deployment
│ ├── Dockerfile
│ └── deploy_transporter.yaml
├── .foxglove
└── mujoco_panda_position.json
├── .gitattributes
├── .github
└── workflows
│ ├── calibration_app.yaml
│ ├── control.yaml
│ ├── motion_planning.yaml
│ ├── object_detection.yaml
│ ├── pages.yaml
│ ├── policy_deployment.yaml
│ ├── transporter_data_collection_app.yaml
│ ├── transporter_deployment.yaml
│ └── zed.yaml
├── .gitignore
├── .gitmodules
├── LICENSE
├── README.md
├── assets
├── Lite-6.gif
├── control_debug.png
├── franka-emika.gif
├── robotics_toolkit.jpeg
├── ufactory.png
└── workspace.jpg
├── docs
├── .DS_Store
├── Gemfile
├── _config.yml
├── _includes
│ └── head_custom.html
├── _sass
│ └── color_schemes
│ │ └── ros2_robotics_research_toolkit.scss
├── assets
│ ├── .DS_Store
│ └── index
│ │ ├── .DS_Store
│ │ ├── link_robotics.jpeg
│ │ └── robotics_toolkit.jpeg
├── contribution-guidelines.md
├── favicon.ico
├── hardware_setup
│ ├── franka-emika-panda.md
│ ├── hardware-setup.md
│ └── ufactory-lite6.md
├── index.md
├── software_setup
│ ├── docker.md
│ ├── host_installation.md
│ └── software-setup.md
└── tutorials
│ ├── launch-mujoco-sim.md
│ ├── moveit-motion-planning.md
│ ├── onnx-policy-deployment.md
│ ├── ros2-control.md
│ └── tutorials.md
├── pyproject.toml
├── scripts
├── client_machine_setup.sh
├── nuc_setup.sh
└── ros_workspace_setup.sh
└── src
├── applications
├── camera_calibration
│ ├── README.md
│ ├── config
│ │ ├── camera_calibration.yaml
│ │ └── zed2i.yaml
│ ├── deploy_client.yaml
│ ├── deploy_franka.yaml
│ ├── deploy_robotiq.yaml
│ ├── notebooks
│ │ └── move_to_start.ipynb
│ └── results
│ │ └── .gitkeep
├── llm_inference
│ ├── CMakeLists.txt
│ ├── README.md
│ ├── launch
│ │ └── llm_client.launch.py
│ ├── package.xml
│ └── src
│ │ └── llm_client.py
├── motion_planning
│ ├── CMakeLists.txt
│ ├── README.md
│ ├── config
│ │ ├── moveit_cpp.yaml
│ │ ├── moveit_cpp_mujoco.yaml
│ │ └── planning_scene.rviz
│ ├── launch
│ │ ├── motion_planning.launch.py
│ │ └── motion_planning_prerequisites.launch.py
│ ├── notebooks
│ │ └── moveit_notebook_tutorial.ipynb
│ ├── package.xml
│ └── src
│ │ ├── motion_planning.py
│ │ ├── motion_planning_planning_scene.py
│ │ └── scripted_pick_place.py
├── mujoco_object_detection_tests
│ ├── README.md
│ ├── mj_franka.py
│ ├── models
│ │ └── .gitkeep
│ └── start_grounded_dino_service.yaml
├── mujoco_ros2_controller_tests
│ ├── README.md
│ ├── mj_franka.py
│ ├── models
│ │ └── .gitkeep
│ └── start_controller.yaml
├── policy_deployment
│ ├── README.md
│ ├── config
│ │ ├── policy.yaml
│ │ └── transporter_deployment.yaml
│ ├── deploy_client.yaml
│ ├── deploy_nuc.yaml
│ ├── launch
│ │ ├── language_conditioned_policy.launch.py
│ │ └── policy.launch.py
│ ├── package.xml
│ ├── panda_policy_deployment_demos
│ │ ├── __init__.py
│ │ ├── language_conditioned_policy.py
│ │ ├── single_image_pose_policy.py
│ │ ├── transporter.py
│ │ └── transporter_deployment_app.py
│ ├── resource
│ │ └── panda_policy_deployment_demos
│ ├── setup.cfg
│ └── setup.py
├── transporter_data_collection
│ ├── README.md
│ ├── config
│ │ ├── application_params.yaml
│ │ └── zed2i.yaml
│ ├── data
│ │ ├── dataset_info.json
│ │ ├── features.json
│ │ ├── transport_cubes-train.tfrecord-00000
│ │ └── transport_cubes-train.tfrecord-00001
│ ├── deploy_client.yaml
│ ├── deploy_nuc.yaml
│ ├── hf_data_upload.py
│ └── load_data_example.py
└── transporter_deployment
│ ├── MUJOCO_LOG.TXT
│ ├── README.md
│ ├── config
│ ├── franka_table.yaml
│ ├── franka_table_real.yaml
│ └── zed2i.yaml
│ ├── deploy_client.yaml
│ ├── deploy_franka.yaml
│ ├── deploy_mujoco_sim_prerequisites.yaml
│ ├── deploy_robotiq.yaml
│ ├── launch_mujoco_sim.py
│ ├── mujoco_models
│ └── rearrangement.mjb
│ ├── robot_workspaces
│ ├── __init__.py
│ └── franka_table.py
│ └── sort_fruit.py
├── control
└── control_launch_configs
│ ├── CMakeLists.txt
│ ├── README.md
│ ├── config
│ ├── servo_pose_jtc.yaml
│ └── servo_pose_position.yaml
│ ├── launch
│ ├── deprecated_mujoco.launch.py
│ ├── franka.launch.py
│ └── robotiq.launch.py
│ ├── notebooks
│ └── moveit_notebook_tutorial.ipynb
│ └── package.xml
└── data_collection
└── README.md
/.docker/camera_calibration_app/Dockerfile:
--------------------------------------------------------------------------------
1 | ARG ROS_DISTRO=humble
2 | FROM ros:${ROS_DISTRO}-ros-base
3 | MAINTAINER Peter David Fagan "peterdavidfagan@gmail.com"
4 |
5 | # set default DDS to Cyclone
6 | ENV ROS_DISTRO=humble
7 | ENV RMW_IMPLEMENTATION rmw_cyclonedds_cpp
8 |
9 | # install system dependencies
10 | RUN apt-get update
11 | RUN DEBIAN_FRONTEND=noninteractive apt-get install -y \
12 | make \
13 | build-essential \
14 | libssl-dev \
15 | zlib1g-dev \
16 | libbz2-dev \
17 | libreadline-dev \
18 | libsqlite3-dev \
19 | wget \
20 | curl \
21 | llvm \
22 | libncurses5-dev \
23 | libncursesw5-dev \
24 | xz-utils \
25 | tk-dev \
26 | libffi-dev \
27 | liblzma-dev \
28 | git \
29 | neovim \
30 | libxcb-cursor0 \
31 | libgmp-dev && \
32 | rm -rf /var/lib/apt/lists/*
33 |
34 | # set up ROS workspace
35 | ENV ROS_UNDERLAY /root/calibration_ws/install
36 | WORKDIR $ROS_UNDERLAY/..
37 | COPY src/cameras/moveit2_camera_calibration ./src/cameras/moveit2_camera_calibration
38 | COPY src/motion_planning ./src/motion_planning
39 | COPY src/applications ./src/applications
40 | COPY src/control/franka_ros2/franka_description ./src/control/franka_ros2/franka_description
41 | COPY src/control/ros2_robotiq_gripper/robotiq_description ./src/control/ros2_robotiq_gripper/robotiq_description
42 | COPY pyproject.toml .
43 |
44 | # install python virtual environment
45 | RUN git clone https://github.com/pyenv/pyenv.git /pyenv
46 | ENV PYENV_ROOT /pyenv
47 | RUN /pyenv/bin/pyenv install 3.10.6
48 | RUN eval "$(/pyenv/bin/pyenv init -)" && /pyenv/bin/pyenv local 3.10.6 && pip install numpy poetry setuptools wheel six auditwheel
49 | RUN mkdir -p .venv
50 | RUN eval "$(/pyenv/bin/pyenv init -)" && /pyenv/bin/pyenv local 3.10.6 && poetry config virtualenvs.in-project true --local && poetry install --only main,dm_env,camera
51 |
52 |
53 | RUN cd ./src/motion_planning && \
54 | for repo in moveit2/moveit2.repos $(f="moveit2/moveit2_rolling.repos"; test -r $f && echo $f); do vcs import < "$repo"; done
55 |
56 | # set up workspace
57 | RUN . .venv/bin/activate && \
58 | . /opt/ros/${ROS_DISTRO}/setup.sh && \
59 | apt-get update && \
60 | apt-get install -y ros-${ROS_DISTRO}-rmw-cyclonedds-cpp && \
61 | rosdep update && \
62 | DEBIAN_FRONTEND=noninteractive \
63 | rosdep install --from-paths src --ignore-src -r -y --rosdistro ${ROS_DISTRO} && \
64 | colcon build && \
65 | rm -rf /var/lib/apt/lists/*
66 |
67 | # create entrypoint
68 | COPY .docker/ros_entrypoint.sh /sbin/ros_entrypoint.sh
69 | RUN sudo chmod 755 /sbin/ros_entrypoint.sh
70 | ENTRYPOINT ["/sbin/ros_entrypoint.sh"]
--------------------------------------------------------------------------------
/.docker/camera_calibration_app/deploy_calibration_app.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | calibration_app:
5 | image: ghcr.io/peterdavidfagan/moveit2_camera_calibration:humble
6 | container_name: moveit2_camera_calibration
7 | build:
8 | context: ../../
9 | dockerfile: .docker/camera_calibration_app/Dockerfile
10 | environment:
11 | - NVIDIA_VISIBLE_DEVICES=all
12 | devices:
13 | - "/dev:/dev"
14 | privileged: true
15 | runtime: nvidia
16 | network_mode: "host"
17 | command: python3 ./src/moveit2_camera_calibration/moveit2_camera_calibration/camera_calibration_app.py
18 |
--------------------------------------------------------------------------------
/.docker/cameras/Dockerfile.zed:
--------------------------------------------------------------------------------
1 | FROM nvidia/cuda:12.1.0-devel-ubuntu22.04
2 | MAINTAINER Peter David Fagan "peterdavidfagan@gmail.com"
3 |
4 | # set default DDS to Cyclone
5 | ENV ROS_DISTRO=humble
6 | ENV RMW_IMPLEMENTATION rmw_cyclonedds_cpp
7 | ENV NVIDIA_DRIVER_CAPABILITIES \
8 | ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}compute,video,utility
9 | ARG DEBIAN_FRONTEND=noninteractive
10 |
11 |
12 | # set up ROS workspace
13 | ENV ROS_UNDERLAY /root/zed_ws/install
14 | WORKDIR $ROS_UNDERLAY/..
15 | COPY src/cameras/zed* ./src/cameras
16 |
17 | # base system installations
18 | RUN apt-get update && \
19 | apt-get install -y neovim software-properties-common build-essential sudo git curl wget python3-pip libspdlog-dev \
20 | libeigen3-dev lsb-release ffmpeg libsm6 libxext6 zstd && \
21 | apt-get upgrade -y
22 |
23 | # install ROS
24 | RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \
25 | echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null
26 |
27 | RUN apt-get update && apt-get upgrade -y && \
28 | apt-get install -y ros-${ROS_DISTRO}-desktop && \
29 | apt-get install -y python3-rosdep && \
30 | apt-get install -y python3-vcstool && \
31 | apt-get install -y python3-colcon-common-extensions
32 |
33 | RUN pip3 install colcon-mixin && \
34 | colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml && \
35 | colcon mixin update default
36 |
37 | RUN rosdep init && \
38 | rosdep fix-permissions && \
39 | rosdep update
40 |
41 | # install the zed sdk
42 | ARG UBUNTU_RELEASE_YEAR=22
43 | ARG ZED_SDK_MAJOR=4
44 | ARG ZED_SDK_MINOR=0
45 | ARG CUDA_MAJOR=12
46 | ARG CUDA_MINOR=1
47 |
48 | RUN echo "Europe/Paris" > /etc/localtime ; echo "CUDA Version ${CUDA_MAJOR}.${CUDA_MINOR}.0" > /usr/local/cuda/version.txt
49 |
50 | # setup the ZED SDK
51 | RUN apt-get update -y || true ; apt-get install --no-install-recommends lsb-release wget less udev sudo zstd build-essential cmake python3 python3-pip libpng-dev libgomp1 -y && \
52 | python3 -m pip install numpy opencv-python && \
53 | wget -q -O ZED_SDK_Linux_Ubuntu${UBUNTU_RELEASE_YEAR}.run https://download.stereolabs.com/zedsdk/${ZED_SDK_MAJOR}.${ZED_SDK_MINOR}/cu${CUDA_MAJOR}${CUDA_MINOR%.*}/ubuntu${UBUNTU_RELEASE_YEAR} && \
54 | chmod +x ZED_SDK_Linux_Ubuntu${UBUNTU_RELEASE_YEAR}.run && \
55 | ./ZED_SDK_Linux_Ubuntu${UBUNTU_RELEASE_YEAR}.run -- silent skip_tools skip_cuda && \
56 | ln -sf /lib/x86_64-linux-gnu/libusb-1.0.so.0 /usr/lib/x86_64-linux-gnu/libusb-1.0.so && \
57 | rm ZED_SDK_Linux_Ubuntu${UBUNTU_RELEASE_YEAR}.run && \
58 | rm -rf /var/lib/apt/lists/*
59 | RUN pip3 install requests
60 | RUN python3 /usr/local/zed/get_python_api.py && python3 -m pip install --ignore-installed /root/zed_ws/pyzed-4.0-cp310-cp310-linux_x86_64.whl
61 |
62 | # install ros dependencies and build workspace
63 | RUN . /opt/ros/${ROS_DISTRO}/setup.sh && \
64 | apt-get update && \
65 | apt-get install -y ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \
66 | wget && \
67 | apt-get update && \
68 | rosdep update && \
69 | DEBIAN_FRONTEND=noninteractive \
70 | rosdep install --from-paths src --ignore-src -r -y --rosdistro ${ROS_DISTRO} && \
71 | colcon build --parallel-workers $(nproc) --symlink-install \
72 | --event-handlers console_direct+ --base-paths src \
73 | --cmake-args ' -DCMAKE_BUILD_TYPE=Release' \
74 | ' -DCMAKE_LIBRARY_PATH=/usr/local/cuda-11/lib64' \
75 | ' -DCMAKE_CXX_FLAGS="-Wl,--allow-shlib-undefined"' \
76 | ' --no-warn-unused-cli' && \
77 | rm -rf /var/lib/apt/lists/*
78 |
79 | # create entrypoint
80 | COPY .docker/cameras/ros_entrypoint.sh /sbin/ros_entrypoint.sh
81 | RUN sudo chmod 755 /sbin/ros_entrypoint.sh
82 | ENTRYPOINT ["/sbin/ros_entrypoint.sh"]
83 |
--------------------------------------------------------------------------------
/.docker/cameras/docker-compose-zed-2i-1.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | zed2i_1:
5 | image: ghcr.io/peterdavidfagan/zed2:humble
6 | container_name: zed2i_1
7 | build:
8 | context: ../../
9 | dockerfile: .docker/cameras/Dockerfile.zed
10 | environment:
11 | - NVIDIA_VISIBLE_DEVICES=all
12 | devices:
13 | - "/dev:/dev"
14 | privileged: true
15 | runtime: nvidia
16 | network_mode: "host"
17 | command: ros2 launch zed_wrapper zed_camera.launch.py camera_name:=zed2i_1 camera_model:=zed2i node_name:=zed2i_1 serial_number:=35215462
18 | deploy:
19 | placement:
20 | constraints:
21 | - node.hostname == thor
22 |
--------------------------------------------------------------------------------
/.docker/cameras/docker-compose-zed-2i-2.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | zed2i_2:
5 | image: ghcr.io/peterdavidfagan/zed2:humble
6 | container_name: zed2i_2
7 | build:
8 | context: ../../
9 | dockerfile: .docker/cameras/Dockerfile.zed
10 | environment:
11 | - NVIDIA_VISIBLE_DEVICES=all
12 | devices:
13 | - "/dev:/dev"
14 | privileged: true
15 | runtime: nvidia
16 | network_mode: "host"
17 | command: ros2 launch zed_wrapper zed_camera.launch.py camera_name:=zed2i_2 camera_model:=zed2i node_name:=zed2i_2 serial_number:=32907025
18 | deploy:
19 | placement:
20 | constraints:
21 | - node.hostname == thor
22 |
--------------------------------------------------------------------------------
/.docker/cameras/docker-compose-zed-mini.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | zed_mini:
5 | image: ghcr.io/peterdavidfagan/zed2:humble
6 | container_name: zed_mini
7 | build:
8 | context: ../../
9 | dockerfile: .docker/cameras/Dockerfile.zed
10 | environment:
11 | - NVIDIA_VISIBLE_DEVICES=all
12 | devices:
13 | - "/dev:/dev"
14 | privileged: true
15 | runtime: nvidia
16 | network_mode: "host"
17 | command: ros2 launch zed_wrapper zed_camera.launch.py camera_name:=zed_mini camera_model:=zedm node_name:=zed_mini serial_number:=15102076
18 | deploy:
19 | placement:
20 | constraints:
21 | - node.hostname == thor
22 |
--------------------------------------------------------------------------------
/.docker/cameras/ros_entrypoint.sh:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | # setup ros2 environment
4 | source "/opt/ros/$ROS_DISTRO/setup.bash" --
5 | source "$ROS_UNDERLAY/setup.bash" --
6 |
7 | # rebuild zed wrapper as we may have updated configs
8 | colcon build --packages-select zed_wrapper
9 | source "$ROS_UNDERLAY/setup.bash" --
10 |
11 | # excute docker command
12 | exec "$@"
13 |
--------------------------------------------------------------------------------
/.docker/control/Dockerfile.control:
--------------------------------------------------------------------------------
1 | ARG ROS_DISTRO=humble
2 | FROM ros:${ROS_DISTRO}-ros-base
3 | MAINTAINER Peter David Fagan "peterdavidfagan@gmail.com"
4 |
5 | # set default DDS to Cyclone
6 | ENV ROS_DISTRO=humble
7 | ENV RMW_IMPLEMENTATION rmw_cyclonedds_cpp
8 |
9 | # install system dependencies
10 | RUN apt-get update
11 | RUN DEBIAN_FRONTEND=noninteractive apt-get install -y \
12 | make \
13 | build-essential \
14 | libssl-dev \
15 | zlib1g-dev \
16 | libbz2-dev \
17 | libreadline-dev \
18 | libsqlite3-dev \
19 | wget \
20 | curl \
21 | llvm \
22 | libncurses5-dev \
23 | libncursesw5-dev \
24 | xz-utils \
25 | tk-dev \
26 | libffi-dev \
27 | liblzma-dev \
28 | git \
29 | neovim \
30 | libxcb-cursor0 \
31 | libgmp-dev && \
32 | rm -rf /var/lib/apt/lists/*
33 |
34 | # set up ROS workspace
35 | ENV ROS_UNDERLAY /root/panda_ws/install
36 | WORKDIR $ROS_UNDERLAY/..
37 | COPY src/control ./src/control
38 | COPY src/motion_planning ./src/motion_planning
39 | COPY src/applications ./src/applications
40 | COPY libfranka ./libfranka
41 | COPY scripts/ros_workspace_setup.sh ./scripts/ros_workspace_setup.sh
42 | COPY pyproject.toml .
43 |
44 | # install python virtual environment
45 | RUN git clone https://github.com/pyenv/pyenv.git /pyenv
46 | ENV PYENV_ROOT /pyenv
47 | RUN /pyenv/bin/pyenv install 3.10.6
48 | RUN eval "$(/pyenv/bin/pyenv init -)" && /pyenv/bin/pyenv local 3.10.6 && pip install numpy poetry setuptools wheel six auditwheel
49 | RUN mkdir -p .venv
50 | RUN eval "$(/pyenv/bin/pyenv init -)" && /pyenv/bin/pyenv local 3.10.6 && poetry config virtualenvs.in-project true --local && poetry install --only main && poetry cache clear --all .
51 |
52 | # set up workspace
53 | RUN cd scripts && ./ros_workspace_setup.sh --poetry_build false && \
54 | rm -rf /var/lib/apt/lists/*
55 |
56 | # create entrypoint
57 | COPY .docker/ros_entrypoint.sh /sbin/ros_entrypoint.sh
58 | RUN sudo chmod 755 /sbin/ros_entrypoint.sh
59 | ENTRYPOINT ["/sbin/ros_entrypoint.sh"]
60 |
--------------------------------------------------------------------------------
/.docker/control/docker-compose-control-server-real-dev.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | control_server:
5 | image: ghcr.io/peterdavidfagan/panda_control:humble
6 | container_name: franka_robotiq_control_server
7 | build:
8 | context: ../../
9 | dockerfile: .docker/control/Dockerfile.control
10 | volumes:
11 | - ../../src:/root/panda_ws/src
12 | devices:
13 | - "/dev:/dev"
14 | privileged: true
15 | network_mode: "host"
16 | cap_add:
17 | - SYS_NICE
18 | ulimits:
19 | rtprio: 70
20 | rttime: -1 # corresponds to 'unlimited'
21 | memlock: 8428281856
22 | command: ros2 launch panda_control_demos franka_robotiq.launch.py use_fake_hardware:=false
23 | deploy:
24 | restart_policy:
25 | condition: any
26 | placement:
27 | constraints:
28 | - node.hostname == robot
29 |
--------------------------------------------------------------------------------
/.docker/control/docker-compose-control-server-real.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | control_server:
5 | image: ghcr.io/peterdavidfagan/panda_control:humble
6 | container_name: franka_robotiq_control_server
7 | build:
8 | context: ../../
9 | dockerfile: .docker/control/Dockerfile.control
10 | devices:
11 | - "/dev:/dev"
12 | privileged: true
13 | network_mode: "host"
14 | cap_add:
15 | - SYS_NICE
16 | ulimits:
17 | rtprio: 70
18 | rttime: -1 # corresponds to 'unlimited'
19 | memlock: 8428281856
20 | command: ros2 launch panda_control_demos franka_robotiq.launch.py use_fake_hardware:=false
21 | deploy:
22 | restart_policy:
23 | condition: any
24 | placement:
25 | constraints:
26 | - node.hostname == robot
27 |
--------------------------------------------------------------------------------
/.docker/llms/docker-compose-ollama.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | ollama:
5 | image: ollama/ollama:latest
6 | environment:
7 | - NVIDIA_VISIBLE_DEVICES=all
8 | volumes:
9 | - /ollama:/root/.ollama
10 | runtime: nvidia
11 |
--------------------------------------------------------------------------------
/.docker/motion_planning/Dockerfile.motion_planning:
--------------------------------------------------------------------------------
1 | ARG ROS_DISTRO=humble
2 | FROM ros:${ROS_DISTRO}-ros-base
3 | MAINTAINER Peter David Fagan "peterdavidfagan@gmail.com"
4 |
5 | # set default DDS to Cyclone
6 | ENV ROS_DISTRO=humble
7 | ENV RMW_IMPLEMENTATION rmw_cyclonedds_cpp
8 |
9 | # install system dependencies
10 | RUN apt-get update
11 | RUN DEBIAN_FRONTEND=noninteractive apt-get install -y \
12 | make \
13 | build-essential \
14 | libssl-dev \
15 | zlib1g-dev \
16 | libbz2-dev \
17 | libreadline-dev \
18 | libsqlite3-dev \
19 | wget \
20 | curl \
21 | llvm \
22 | libncurses5-dev \
23 | libncursesw5-dev \
24 | xz-utils \
25 | tk-dev \
26 | libffi-dev \
27 | liblzma-dev \
28 | git \
29 | neovim \
30 | libxcb-cursor0 \
31 | libgmp-dev && \
32 | rm -rf /var/lib/apt/lists/*
33 |
34 | # set up ROS workspace
35 | ENV ROS_UNDERLAY /root/panda_ws/install
36 | WORKDIR $ROS_UNDERLAY/..
37 | COPY src/control ./src/control
38 | COPY src/motion_planning ./src/motion_planning
39 | COPY src/applications ./src/applications
40 | COPY libfranka ./libfranka
41 | COPY scripts/ros_workspace_setup.sh ./scripts/ros_workspace_setup.sh
42 | COPY pyproject.toml .
43 |
44 | # install python virtual environment
45 | RUN git clone https://github.com/pyenv/pyenv.git /pyenv
46 | ENV PYENV_ROOT /pyenv
47 | RUN /pyenv/bin/pyenv install 3.10.6
48 | RUN eval "$(/pyenv/bin/pyenv init -)" && /pyenv/bin/pyenv local 3.10.6 && pip install numpy poetry setuptools wheel six auditwheel
49 | RUN mkdir -p .venv
50 | RUN eval "$(/pyenv/bin/pyenv init -)" && /pyenv/bin/pyenv local 3.10.6 && poetry config virtualenvs.in-project true --local && poetry install --only main && poetry cache clear --all .
51 |
52 | # set up workspace
53 | RUN cd scripts && ./ros_workspace_setup.sh --poetry_build false && \
54 | rm -rf /var/lib/apt/lists/*
55 |
56 | # create entrypoint
57 | COPY .docker/ros_entrypoint.sh /sbin/ros_entrypoint.sh
58 | RUN sudo chmod 755 /sbin/ros_entrypoint.sh
59 | ENTRYPOINT ["/sbin/ros_entrypoint.sh"]
60 |
--------------------------------------------------------------------------------
/.docker/motion_planning/docker-compose-motion-planning-dev.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | moveit:
5 | image: ghcr.io/peterdavidfagan/panda_motion_planning:humble
6 | build:
7 | context: ../../
8 | dockerfile: .docker/motion_planning/Dockerfile.motion_planning
9 | environment:
10 | - DISPLAY=${DISPLAY}
11 | - XAUTHORITY=${DOCKER_XAUTH}
12 | volumes:
13 | - /tmp/.X11-unix:/tml/.X11-unix:rw
14 | - ${DOCKER_XAUTH}:${DOCKER_XAUTH}
15 | - ../../src:/root/panda_ws/src
16 | devices:
17 | - "/dev:/dev"
18 | privileged: true
19 | network_mode: "host"
20 | command: ros2 launch panda_motion_planning_demos motion_planning_prerequisites.launch.py
21 |
--------------------------------------------------------------------------------
/.docker/motion_planning/docker-compose-motion-planning.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | moveit:
5 | image: ghcr.io/peterdavidfagan/panda_motion_planning:humble
6 | build:
7 | context: ../../
8 | dockerfile: .docker/motion_planning/Dockerfile.motion_planning
9 | environment:
10 | - DISPLAY=${DISPLAY}
11 | - XAUTHORITY=${DOCKER_XAUTH}
12 | volumes:
13 | - /tmp/.X11-unix:/tml/.X11-unix:rw
14 | - ${DOCKER_XAUTH}:${DOCKER_XAUTH}
15 | devices:
16 | - "/dev:/dev"
17 | privileged: true
18 | network_mode: "host"
19 | command: ros2 launch panda_motion_planning_demos motion_planning_prerequisites.launch.py
20 |
--------------------------------------------------------------------------------
/.docker/object_detection/Dockerfile:
--------------------------------------------------------------------------------
1 | ARG ROS_DISTRO=humble
2 | FROM ros:${ROS_DISTRO}-ros-base
3 | MAINTAINER Peter David Fagan "peterdavidfagan@gmail.com"
4 |
5 | # set default DDS to Cyclone
6 | ENV ROS_DISTRO=humble
7 | ENV RMW_IMPLEMENTATION rmw_cyclonedds_cpp
8 |
9 | # install system dependencies
10 | RUN apt-get update
11 | RUN DEBIAN_FRONTEND=noninteractive apt-get install -y \
12 | make \
13 | build-essential \
14 | libssl-dev \
15 | zlib1g-dev \
16 | libbz2-dev \
17 | libreadline-dev \
18 | libsqlite3-dev \
19 | wget \
20 | curl \
21 | llvm \
22 | libncurses5-dev \
23 | libncursesw5-dev \
24 | xz-utils \
25 | tk-dev \
26 | libffi-dev \
27 | liblzma-dev \
28 | git \
29 | neovim \
30 | libxcb-cursor0 \
31 | libgmp-dev && \
32 | rm -rf /var/lib/apt/lists/*
33 |
34 | # set up ROS workspace
35 | ENV ROS_UNDERLAY /root/object_detection_ws/install
36 | WORKDIR $ROS_UNDERLAY/..
37 | COPY src/object_detection ./src/object_detection
38 | COPY src/applications/transporter_deployment ./src/applications/transporter_deployment
39 | COPY pyproject.toml .
40 |
41 | # install python virtual environment
42 | RUN git clone https://github.com/pyenv/pyenv.git /pyenv
43 | ENV PYENV_ROOT /pyenv
44 | RUN /pyenv/bin/pyenv install 3.10.6
45 | RUN eval "$(/pyenv/bin/pyenv init -)" && /pyenv/bin/pyenv local 3.10.6 && pip install numpy poetry setuptools wheel six auditwheel
46 | RUN mkdir -p .venv
47 | RUN eval "$(/pyenv/bin/pyenv init -)" && /pyenv/bin/pyenv local 3.10.6 && poetry config virtualenvs.in-project true --local && poetry install --only main,camera,huggingface
48 |
49 | # set up workspace
50 | RUN . .venv/bin/activate && \
51 | . /opt/ros/${ROS_DISTRO}/setup.sh && \
52 | apt-get update && \
53 | apt-get install -y ros-${ROS_DISTRO}-rmw-cyclonedds-cpp && \
54 | rosdep update && \
55 | DEBIAN_FRONTEND=noninteractive \
56 | rosdep install --from-paths src --ignore-src -r -y --rosdistro ${ROS_DISTRO} && \
57 | colcon build && \
58 | rm -rf /var/lib/apt/lists/*
59 |
60 | # create entrypoint
61 | COPY .docker/ros_entrypoint.sh /sbin/ros_entrypoint.sh
62 | RUN sudo chmod 755 /sbin/ros_entrypoint.sh
63 | ENTRYPOINT ["/sbin/ros_entrypoint.sh"]
64 |
--------------------------------------------------------------------------------
/.docker/object_detection/deploy_object_detection.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | object_detection:
5 | image: ghcr.io/peterdavidfagan/object_detection:humble
6 | container_name: object_detection
7 | build:
8 | context: ../../
9 | dockerfile: .docker/object_detection/Dockerfile
10 | environment:
11 | - NVIDIA_VISIBLE_DEVICES=all
12 | devices:
13 | - "/dev:/dev"
14 | privileged: true
15 | runtime: nvidia
16 | network_mode: "host"
17 | command: python3 ./src/object_detection/ros2_object_detection/ros2_object_detection_components/ros2_object_detection_components/grounded_dino.py
18 |
--------------------------------------------------------------------------------
/.docker/policy_deployment/Dockerfile.policy_deployment:
--------------------------------------------------------------------------------
1 | ARG ROS_DISTRO=humble
2 | FROM ros:${ROS_DISTRO}-ros-base
3 | MAINTAINER Peter David Fagan "peterdavidfagan@gmail.com"
4 |
5 | # set default DDS to Cyclone
6 | ENV ROS_DISTRO=humble
7 | ENV RMW_IMPLEMENTATION rmw_cyclonedds_cpp
8 |
9 | # install system dependencies
10 | RUN apt-get update
11 | RUN DEBIAN_FRONTEND=noninteractive apt-get install -y \
12 | make \
13 | build-essential \
14 | libssl-dev \
15 | zlib1g-dev \
16 | libbz2-dev \
17 | libreadline-dev \
18 | libsqlite3-dev \
19 | wget \
20 | curl \
21 | llvm \
22 | libncurses5-dev \
23 | libncursesw5-dev \
24 | xz-utils \
25 | tk-dev \
26 | libffi-dev \
27 | liblzma-dev \
28 | git \
29 | neovim \
30 | libxcb-cursor0 \
31 | libgmp-dev && \
32 | rm -rf /var/lib/apt/lists/*
33 |
34 | # set up ROS workspace
35 | ENV ROS_UNDERLAY /root/panda_ws/install
36 | WORKDIR $ROS_UNDERLAY/..
37 | COPY src/applications ./src/applications
38 | COPY src/control ./src/control
39 | COPY src/data_collection ./src/data_collection
40 | COPY src/motion_planning ./src/motion_planning
41 | COPY libfranka ./libfranka
42 | COPY scripts/ros_workspace_setup.sh ./scripts/ros_workspace_setup.sh
43 | COPY pyproject.toml .
44 |
45 | # install python virtual environment
46 | RUN git clone https://github.com/pyenv/pyenv.git /pyenv
47 | ENV PYENV_ROOT /pyenv
48 | RUN /pyenv/bin/pyenv install 3.10.6
49 | RUN eval "$(/pyenv/bin/pyenv init -)" && /pyenv/bin/pyenv local 3.10.6 && pip install numpy poetry setuptools wheel six auditwheel
50 | RUN mkdir -p .venv
51 | RUN eval "$(/pyenv/bin/pyenv init -)" && /pyenv/bin/pyenv local 3.10.6 && poetry config virtualenvs.in-project true --local && poetry install
52 |
53 | # set up workspace
54 | RUN cd scripts && ./ros_workspace_setup.sh --poetry_build false && \
55 | rm -rf /var/lib/apt/lists/*
56 |
57 | # create entrypoint
58 | COPY .docker/ros_entrypoint.sh /sbin/ros_entrypoint.sh
59 | RUN sudo chmod 755 /sbin/ros_entrypoint.sh
60 | ENTRYPOINT ["/sbin/ros_entrypoint.sh"]
61 |
--------------------------------------------------------------------------------
/.docker/policy_deployment/docker-compose-policy-deployment-dev.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | moveit:
5 | image: ghcr.io/peterdavidfagan/panda_policy_deployment:humble
6 | build:
7 | context: ../../
8 | dockerfile: .docker/policy_deployment/Dockerfile.policy_deployment
9 | environment:
10 | - DISPLAY=${DISPLAY}
11 | - XAUTHORITY=${DOCKER_XAUTH}
12 | volumes:
13 | - /tmp/.X11-unix:/tml/.X11-unix:rw
14 | - ${DOCKER_XAUTH}:${DOCKER_XAUTH}
15 | - ../../src:/root/panda_ws/src
16 | devices:
17 | - "/dev:/dev"
18 | privileged: true
19 | network_mode: "host"
20 | command: echo test
21 |
--------------------------------------------------------------------------------
/.docker/ros_entrypoint.sh:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | # setup python environment
4 | source .venv/bin/activate
5 |
6 | # setup ros2 environment
7 | source "/opt/ros/$ROS_DISTRO/setup.bash" --
8 | source "$ROS_UNDERLAY/setup.bash" --
9 |
10 | # excute docker command
11 | exec "$@"
12 |
--------------------------------------------------------------------------------
/.docker/transporter_data_collection_app/Dockerfile:
--------------------------------------------------------------------------------
1 | ARG ROS_DISTRO=humble
2 | FROM ros:${ROS_DISTRO}-ros-base
3 | MAINTAINER Peter David Fagan "peterdavidfagan@gmail.com"
4 |
5 | # set default DDS to Cyclone
6 | ENV ROS_DISTRO=humble
7 | ENV RMW_IMPLEMENTATION rmw_cyclonedds_cpp
8 |
9 | # install system dependencies
10 | RUN apt-get update
11 | RUN DEBIAN_FRONTEND=noninteractive apt-get install -y \
12 | make \
13 | build-essential \
14 | libssl-dev \
15 | zlib1g-dev \
16 | libbz2-dev \
17 | libreadline-dev \
18 | libsqlite3-dev \
19 | wget \
20 | curl \
21 | llvm \
22 | libncurses5-dev \
23 | libncursesw5-dev \
24 | xz-utils \
25 | tk-dev \
26 | libffi-dev \
27 | liblzma-dev \
28 | git \
29 | neovim \
30 | libxcb-cursor0 \
31 | libgmp-dev && \
32 | rm -rf /var/lib/apt/lists/*
33 |
34 | # set up ROS workspace
35 | ENV ROS_UNDERLAY /root/data_collection_ws/install
36 | WORKDIR $ROS_UNDERLAY/..
37 | COPY src/data_collection ./src/data_collection
38 | COPY src/motion_planning ./src/motion_planning
39 | COPY src/applications ./src/applications
40 | COPY src/control/franka_ros2/franka_description ./src/control/franka_ros2/franka_description
41 | COPY src/control/ros2_robotiq_gripper/robotiq_description ./src/control/ros2_robotiq_gripper/robotiq_description
42 | COPY pyproject.toml .
43 |
44 | # install python virtual environment
45 | RUN git clone https://github.com/pyenv/pyenv.git /pyenv
46 | ENV PYENV_ROOT /pyenv
47 | RUN /pyenv/bin/pyenv install 3.10.6
48 | RUN eval "$(/pyenv/bin/pyenv init -)" && /pyenv/bin/pyenv local 3.10.6 && pip install numpy poetry setuptools wheel six auditwheel
49 | RUN mkdir -p .venv
50 | RUN eval "$(/pyenv/bin/pyenv init -)" && /pyenv/bin/pyenv local 3.10.6 && poetry config virtualenvs.in-project true --local && poetry install --only main,dm_env,camera,transporter_data,huggingface
51 |
52 | # install moveit repo deps
53 | RUN cd ./src/motion_planning && \
54 | for repo in moveit2/moveit2.repos $(f="moveit2/moveit2_rolling.repos"; test -r $f && echo $f); do vcs import < "$repo"; done
55 |
56 | # set up workspace
57 | RUN . .venv/bin/activate && \
58 | . /opt/ros/${ROS_DISTRO}/setup.sh && \
59 | apt-get update && \
60 | apt-get install -y ros-${ROS_DISTRO}-rmw-cyclonedds-cpp && \
61 | rosdep update && \
62 | DEBIAN_FRONTEND=noninteractive \
63 | rosdep install --from-paths src --ignore-src -r -y --rosdistro ${ROS_DISTRO} && \
64 | colcon build && \
65 | rm -rf /var/lib/apt/lists/*
66 |
67 | # create entrypoint
68 | COPY .docker/ros_entrypoint.sh /sbin/ros_entrypoint.sh
69 | RUN sudo chmod 755 /sbin/ros_entrypoint.sh
70 | ENTRYPOINT ["/sbin/ros_entrypoint.sh"]
--------------------------------------------------------------------------------
/.docker/transporter_data_collection_app/deploy_transporter_data_collection_app.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | calibration_app:
5 | image: ghcr.io/peterdavidfagan/moveit2_data_collection:humble
6 | container_name: moveit2_data_collection
7 | build:
8 | context: ../../
9 | dockerfile: .docker/transporter_data_collection_app/Dockerfile
10 | environment:
11 | - NVIDIA_VISIBLE_DEVICES=all
12 | devices:
13 | - "/dev:/dev"
14 | privileged: true
15 | runtime: nvidia
16 | network_mode: "host"
17 | command: python3 ./src/data_collection/moveit2_data_collection/moveit2_data_collection/transporter_data_collection_app.py
18 |
--------------------------------------------------------------------------------
/.docker/transporter_deployment/Dockerfile:
--------------------------------------------------------------------------------
1 | ARG ROS_DISTRO=humble
2 | FROM ros:${ROS_DISTRO}-ros-base
3 | MAINTAINER Peter David Fagan "peterdavidfagan@gmail.com"
4 |
5 | # set default DDS to Cyclone
6 | ENV ROS_DISTRO=humble
7 | ENV RMW_IMPLEMENTATION rmw_cyclonedds_cpp
8 |
9 | # install system dependencies
10 | RUN apt-get update
11 | RUN DEBIAN_FRONTEND=noninteractive apt-get install -y \
12 | make \
13 | build-essential \
14 | libssl-dev \
15 | zlib1g-dev \
16 | libbz2-dev \
17 | libreadline-dev \
18 | libsqlite3-dev \
19 | wget \
20 | curl \
21 | llvm \
22 | libncurses5-dev \
23 | libncursesw5-dev \
24 | xz-utils \
25 | tk-dev \
26 | libffi-dev \
27 | liblzma-dev \
28 | git \
29 | neovim \
30 | libxcb-cursor0 \
31 | libgmp-dev && \
32 | rm -rf /var/lib/apt/lists/*
33 |
34 | # set up ROS workspace
35 | ENV ROS_UNDERLAY /root/transporter_deployment_ws/install
36 | WORKDIR $ROS_UNDERLAY/..
37 | COPY src/cameras/moveit2_camera_calibration ./src/cameras/moveit2_camera_calibration
38 | COPY src/object_detection ./src/object_detection
39 | COPY src/motion_planning ./src/motion_planning
40 | COPY src/applications/transporter_deployment ./src/applications/transporter_deployment
41 | COPY src/control/franka_ros2/franka_description ./src/control/franka_ros2/franka_description
42 | COPY src/control/ros2_robotiq_gripper/robotiq_description ./src/control/ros2_robotiq_gripper/robotiq_description
43 | COPY pyproject.toml .
44 |
45 | # install python virtual environment
46 | RUN git clone https://github.com/pyenv/pyenv.git /pyenv
47 | ENV PYENV_ROOT /pyenv
48 | RUN /pyenv/bin/pyenv install 3.10.6
49 | RUN eval "$(/pyenv/bin/pyenv init -)" && /pyenv/bin/pyenv local 3.10.6 && pip install numpy poetry setuptools wheel six auditwheel
50 | RUN mkdir -p .venv
51 | RUN eval "$(/pyenv/bin/pyenv init -)" && /pyenv/bin/pyenv local 3.10.6 && poetry config virtualenvs.in-project true --local && poetry install --only main,dm_env,camera
52 |
53 | # install moveit repo deps
54 | RUN cd ./src/motion_planning && \
55 | for repo in moveit2/moveit2.repos $(f="moveit2/moveit2_rolling.repos"; test -r $f && echo $f); do vcs import < "$repo"; done
56 |
57 | # set up workspace
58 | RUN . .venv/bin/activate && \
59 | . /opt/ros/${ROS_DISTRO}/setup.sh && \
60 | apt-get update && \
61 | apt-get install -y ros-${ROS_DISTRO}-rmw-cyclonedds-cpp && \
62 | rosdep update && \
63 | DEBIAN_FRONTEND=noninteractive \
64 | rosdep install --from-paths src --ignore-src -r -y --rosdistro ${ROS_DISTRO} && \
65 | colcon build && \
66 | rm -rf /var/lib/apt/lists/*
67 |
68 | # create entrypoint
69 | COPY .docker/ros_entrypoint.sh /sbin/ros_entrypoint.sh
70 | RUN sudo chmod 755 /sbin/ros_entrypoint.sh
71 | ENTRYPOINT ["/sbin/ros_entrypoint.sh"]
72 |
--------------------------------------------------------------------------------
/.docker/transporter_deployment/deploy_transporter.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | calibration_app:
5 | image: ghcr.io/peterdavidfagan/transporter_deployment:humble
6 | container_name: transporter_deployment
7 | build:
8 | context: ../../
9 | dockerfile: .docker/transporter_deployment/Dockerfile
10 | environment:
11 | - NVIDIA_VISIBLE_DEVICES=all
12 | devices:
13 | - "/dev:/dev"
14 | privileged: true
15 | runtime: nvidia
16 | network_mode: "host"
17 | command: python3 ./src/applications/transporter_deployment/sort_fruit.py
18 |
--------------------------------------------------------------------------------
/.gitattributes:
--------------------------------------------------------------------------------
1 | *.mjb filter=lfs diff=lfs merge=lfs -text
2 |
--------------------------------------------------------------------------------
/.github/workflows/calibration_app.yaml:
--------------------------------------------------------------------------------
1 | name: moveit2_camera_calibration
2 |
3 | on:
4 | push:
5 | branches: ['franka_emika_panda']
6 | paths:
7 | - src/cameras/moveit2_camera_calibration/**
8 | - .docker/camera_calibration_app/**
9 | - .github/workflows/calibration_app.yaml
10 | workflow_dispatch:
11 |
12 | env:
13 | REGISTRY: ghcr.io
14 | ORG_NAME: peterdavidfagan
15 | IMAGE_NAME: moveit2_camera_calibration
16 |
17 | jobs:
18 | build-and-push-image:
19 | runs-on: ubuntu-latest
20 | permissions:
21 | contents: read
22 | packages: write
23 |
24 | steps:
25 | - name: Checkout repository
26 | uses: actions/checkout@v3
27 | with:
28 | submodules: recursive
29 | token: ${{ secrets.ORG_PAT }}
30 |
31 | - name: Set up QEMU
32 | uses: docker/setup-qemu-action@v2
33 |
34 | - name: Set up Docker Buildx
35 | uses: docker/setup-buildx-action@v2
36 |
37 | - name: Log in to the Container registry
38 | uses: docker/login-action@f054a8b539a109f9f41c372932f1ae047eff08c9
39 | with:
40 | registry: ${{ env.REGISTRY }}
41 | username: ${{ github.actor }}
42 | password: ${{ secrets.GITHUB_TOKEN }}
43 |
44 | - name: Build and push Docker image
45 | uses: docker/build-push-action@v3
46 | with:
47 | context: .
48 | platforms: linux/amd64
49 | file: .docker/camera_calibration_app/Dockerfile
50 | push: true
51 | no-cache: true
52 | tags: ${{ env.REGISTRY }}/${{ env.ORG_NAME }}/${{ env.IMAGE_NAME }}:humble
53 |
--------------------------------------------------------------------------------
/.github/workflows/control.yaml:
--------------------------------------------------------------------------------
1 | name: control
2 |
3 | on:
4 | push:
5 | branches: ['franka_emika_panda']
6 | paths:
7 | - .gitmodules
8 | - src/**
9 | - .docker/control/Dockerfile.control
10 | - .github/workflows/control.yaml
11 | workflow_dispatch:
12 |
13 | env:
14 | REGISTRY: ghcr.io
15 | ORG_NAME: peterdavidfagan
16 | IMAGE_NAME: panda_control
17 |
18 | jobs:
19 | build-and-push-image:
20 | runs-on: ubuntu-latest
21 | permissions:
22 | contents: read
23 | packages: write
24 |
25 | steps:
26 | - name: Checkout repository
27 | uses: actions/checkout@v4
28 | with:
29 | submodules: recursive
30 | lfs: false
31 | token: ${{ secrets.ORG_PAT }}
32 |
33 | - name: Free Space
34 | run: |
35 | sudo rm -rf /opt/ghc
36 | sudo rm -rf /opt/hostedtoolcache
37 | sudo rm -rf /usr/share/dotnet
38 | sudo rm -rf "$AGENT_TOOLSDIRECTORY"
39 | sudo rm -rf ${GITHUB_WORKSPACE}/.git
40 |
41 | - name: Set up QEMU
42 | uses: docker/setup-qemu-action@v2
43 |
44 | - name: Set up Docker Buildx
45 | uses: docker/setup-buildx-action@v2
46 |
47 | - name: Log in to the Container registry
48 | uses: docker/login-action@f054a8b539a109f9f41c372932f1ae047eff08c9
49 | with:
50 | registry: ${{ env.REGISTRY }}
51 | username: ${{ github.actor }}
52 | password: ${{ secrets.GITHUB_TOKEN }}
53 |
54 | - name: Build and push Docker image
55 | uses: docker/build-push-action@v3
56 | with:
57 | context: .
58 | platforms: linux/amd64
59 | file: .docker/control/Dockerfile.control
60 | push: true
61 | no-cache: true
62 | tags: ${{ env.REGISTRY }}/${{ env.ORG_NAME }}/${{ env.IMAGE_NAME }}:humble
63 |
--------------------------------------------------------------------------------
/.github/workflows/motion_planning.yaml:
--------------------------------------------------------------------------------
1 | name: motion_planning
2 |
3 | on:
4 | push:
5 | branches: ['franka_emika_panda']
6 | paths:
7 | - .gitmodules
8 | - src/**
9 | - .docker/motion_planning/Dockerfile.motion_planning
10 | - .github/workflows/motion_planning.yaml
11 | workflow_dispatch:
12 |
13 | env:
14 | REGISTRY: ghcr.io
15 | ORG_NAME: peterdavidfagan
16 | IMAGE_NAME: panda_motion_planning
17 |
18 | jobs:
19 | build-and-push-image:
20 | runs-on: ubuntu-latest
21 | permissions:
22 | contents: read
23 | packages: write
24 |
25 | steps:
26 | - name: Checkout repository
27 | uses: actions/checkout@v4
28 | with:
29 | submodules: recursive
30 | lfs: false
31 | token: ${{ secrets.ORG_PAT }}
32 |
33 | - name: Free Space
34 | run: |
35 | sudo rm -rf /opt/ghc
36 | sudo rm -rf /opt/hostedtoolcache
37 | sudo rm -rf /usr/share/dotnet
38 | sudo rm -rf "$AGENT_TOOLSDIRECTORY"
39 | sudo rm -rf ${GITHUB_WORKSPACE}/.git
40 |
41 | - name: Set up QEMU
42 | uses: docker/setup-qemu-action@v2
43 |
44 | - name: Set up Docker Buildx
45 | uses: docker/setup-buildx-action@v2
46 |
47 | - name: Log in to the Container registry
48 | uses: docker/login-action@f054a8b539a109f9f41c372932f1ae047eff08c9
49 | with:
50 | registry: ${{ env.REGISTRY }}
51 | username: ${{ github.actor }}
52 | password: ${{ secrets.GITHUB_TOKEN }}
53 |
54 | - name: Build and push Docker image
55 | uses: docker/build-push-action@v3
56 | with:
57 | context: .
58 | platforms: linux/amd64
59 | file: .docker/motion_planning/Dockerfile.motion_planning
60 | push: true
61 | no-cache: true
62 | tags: ${{ env.REGISTRY }}/${{ env.ORG_NAME }}/${{ env.IMAGE_NAME }}:humble
63 |
--------------------------------------------------------------------------------
/.github/workflows/object_detection.yaml:
--------------------------------------------------------------------------------
1 | name: object_detection
2 |
3 | on:
4 | push:
5 | branches: ['franka_emika_panda']
6 | paths:
7 | - src/object_detection/**
8 | - .docker/object_detection/**
9 | - .github/workflows/object_detection.yaml
10 | workflow_dispatch:
11 |
12 | env:
13 | REGISTRY: ghcr.io
14 | ORG_NAME: peterdavidfagan
15 | IMAGE_NAME: object_detection
16 |
17 | jobs:
18 | build-and-push-image:
19 | runs-on: ubuntu-latest
20 | permissions:
21 | contents: read
22 | packages: write
23 |
24 | steps:
25 | - name: Checkout repository
26 | uses: actions/checkout@v3
27 | with:
28 | submodules: recursive
29 | token: ${{ secrets.ORG_PAT }}
30 |
31 | - name: Free Space
32 | run: |
33 | sudo rm -rf /opt/ghc
34 | sudo rm -rf /opt/hostedtoolcache
35 | sudo rm -rf /usr/share/dotnet
36 | sudo rm -rf "$AGENT_TOOLSDIRECTORY"
37 | sudo rm -rf ${GITHUB_WORKSPACE}/.git
38 |
39 | - name: Set up QEMU
40 | uses: docker/setup-qemu-action@v2
41 |
42 | - name: Set up Docker Buildx
43 | uses: docker/setup-buildx-action@v2
44 |
45 | - name: Log in to the Container registry
46 | uses: docker/login-action@f054a8b539a109f9f41c372932f1ae047eff08c9
47 | with:
48 | registry: ${{ env.REGISTRY }}
49 | username: ${{ github.actor }}
50 | password: ${{ secrets.GITHUB_TOKEN }}
51 |
52 | - name: Build and push Docker image
53 | uses: docker/build-push-action@v3
54 | with:
55 | context: .
56 | platforms: linux/amd64, linux/arm64
57 | file: .docker/object_detection/Dockerfile
58 | push: true
59 | no-cache: true
60 | tags: ${{ env.REGISTRY }}/${{ env.ORG_NAME }}/${{ env.IMAGE_NAME }}:humble
61 |
--------------------------------------------------------------------------------
/.github/workflows/pages.yaml:
--------------------------------------------------------------------------------
1 | name: documentation
2 |
3 | on:
4 | push:
5 | branches:
6 | - "franka_emika_panda"
7 | paths:
8 | - "docs/**"
9 | workflow_dispatch:
10 |
11 | permissions:
12 | contents: read
13 | pages: write
14 | id-token: write
15 |
16 | # Allow one concurrent deployment
17 | concurrency:
18 | group: "pages"
19 | cancel-in-progress: true
20 |
21 | jobs:
22 | # Build job
23 | build:
24 | runs-on: ubuntu-latest
25 | defaults:
26 | run:
27 | working-directory: docs
28 | steps:
29 | - name: Checkout
30 | uses: actions/checkout@v3
31 | - name: Setup Ruby
32 | uses: ruby/setup-ruby@v1
33 | with:
34 | ruby-version: '3.1' # Not needed with a .ruby-version file
35 | bundler-cache: true # runs 'bundle install' and caches installed gems automatically
36 | cache-version: 0 # Increment this number if you need to re-download cached gems
37 | working-directory: '${{ github.workspace }}/docs'
38 | - name: Setup Pages
39 | id: pages
40 | uses: actions/configure-pages@v3
41 | - name: Build with Jekyll
42 | run: bundle exec jekyll build --baseurl "${{ steps.pages.outputs.base_path }}"
43 | env:
44 | JEKYLL_ENV: production
45 | - name: Upload artifact
46 | uses: actions/upload-pages-artifact@v1
47 | with:
48 | path: "docs/_site/"
49 |
50 | deploy:
51 | environment:
52 | name: github-pages
53 | url: ${{ steps.deployment.outputs.page_url }}
54 | runs-on: ubuntu-latest
55 | needs: build
56 | steps:
57 | - name: Deploy to GitHub Pages
58 | id: deployment
59 | uses: actions/deploy-pages@v2
60 |
--------------------------------------------------------------------------------
/.github/workflows/policy_deployment.yaml:
--------------------------------------------------------------------------------
1 | name: policy_deployment
2 |
3 | on:
4 | push:
5 | branches: ['franka_emika_panda']
6 | paths:
7 | - src/applications/policy_deployment/**
8 | - .docker/policy_deployment/**
9 | - .github/workflows/policy_deployment.yaml
10 | workflow_dispatch:
11 |
12 | env:
13 | REGISTRY: ghcr.io
14 | ORG_NAME: peterdavidfagan
15 | IMAGE_NAME: policy_deployment
16 |
17 | jobs:
18 | build-and-push-image:
19 | runs-on: ubuntu-latest
20 | permissions:
21 | contents: read
22 | packages: write
23 |
24 | steps:
25 | - name: Checkout repository
26 | uses: actions/checkout@v3
27 | with:
28 | submodules: recursive
29 | token: ${{ secrets.ORG_PAT }}
30 |
31 | - name: Set up QEMU
32 | uses: docker/setup-qemu-action@v2
33 |
34 | - name: Set up Docker Buildx
35 | uses: docker/setup-buildx-action@v2
36 |
37 | - name: Log in to the Container registry
38 | uses: docker/login-action@f054a8b539a109f9f41c372932f1ae047eff08c9
39 | with:
40 | registry: ${{ env.REGISTRY }}
41 | username: ${{ github.actor }}
42 | password: ${{ secrets.GITHUB_TOKEN }}
43 |
44 | - name: Build and push Docker image
45 | uses: docker/build-push-action@v3
46 | with:
47 | context: .
48 | platforms: linux/amd64
49 | file: .docker/policy_deployment/Dockerfile.policy_deployment
50 | push: true
51 | no-cache: true
52 | tags: ${{ env.REGISTRY }}/${{ env.ORG_NAME }}/${{ env.IMAGE_NAME }}:humble
53 |
--------------------------------------------------------------------------------
/.github/workflows/transporter_data_collection_app.yaml:
--------------------------------------------------------------------------------
1 | name: moveit2_data_collection
2 |
3 | on:
4 | push:
5 | branches: ['franka_emika_panda']
6 | paths:
7 | - src/data_collection/moveit2_data_collection/**
8 | - .docker/transporter_data_collection_app/**
9 | - .github/workflows/transporter_data_collection_app.yaml
10 | workflow_dispatch:
11 |
12 | env:
13 | REGISTRY: ghcr.io
14 | ORG_NAME: peterdavidfagan
15 | IMAGE_NAME: moveit2_data_collection
16 |
17 | jobs:
18 | build-and-push-image:
19 | runs-on: ubuntu-latest
20 | permissions:
21 | contents: read
22 | packages: write
23 |
24 | steps:
25 | - name: Checkout repository
26 | uses: actions/checkout@v3
27 | with:
28 | submodules: recursive
29 | token: ${{ secrets.ORG_PAT }}
30 |
31 | - name: Set up QEMU
32 | uses: docker/setup-qemu-action@v2
33 |
34 | - name: Set up Docker Buildx
35 | uses: docker/setup-buildx-action@v2
36 |
37 | - name: Log in to the Container registry
38 | uses: docker/login-action@f054a8b539a109f9f41c372932f1ae047eff08c9
39 | with:
40 | registry: ${{ env.REGISTRY }}
41 | username: ${{ github.actor }}
42 | password: ${{ secrets.GITHUB_TOKEN }}
43 |
44 | - name: Build and push Docker image
45 | uses: docker/build-push-action@v3
46 | with:
47 | context: .
48 | platforms: linux/amd64
49 | file: .docker/transporter_data_collection_app/Dockerfile
50 | push: true
51 | no-cache: true
52 | tags: ${{ env.REGISTRY }}/${{ env.ORG_NAME }}/${{ env.IMAGE_NAME }}:humble
53 |
--------------------------------------------------------------------------------
/.github/workflows/transporter_deployment.yaml:
--------------------------------------------------------------------------------
1 | name: transporter_deployment
2 |
3 | on:
4 | push:
5 | branches: ['franka_emika_panda']
6 | paths:
7 | - src/applications/transporter_deployment/**
8 | - .docker/transporter_deployment/**
9 | - .github/workflows/transporter_deployment.yaml
10 | workflow_dispatch:
11 |
12 | env:
13 | REGISTRY: ghcr.io
14 | ORG_NAME: peterdavidfagan
15 | IMAGE_NAME: transporter_deployment
16 |
17 | jobs:
18 | build-and-push-image:
19 | runs-on: ubuntu-latest
20 | permissions:
21 | contents: read
22 | packages: write
23 |
24 | steps:
25 | - name: Checkout repository
26 | uses: actions/checkout@v3
27 | with:
28 | submodules: recursive
29 | token: ${{ secrets.ORG_PAT }}
30 |
31 | - name: Set up QEMU
32 | uses: docker/setup-qemu-action@v2
33 |
34 | - name: Set up Docker Buildx
35 | uses: docker/setup-buildx-action@v2
36 |
37 | - name: Log in to the Container registry
38 | uses: docker/login-action@f054a8b539a109f9f41c372932f1ae047eff08c9
39 | with:
40 | registry: ${{ env.REGISTRY }}
41 | username: ${{ github.actor }}
42 | password: ${{ secrets.GITHUB_TOKEN }}
43 |
44 | - name: Build and push Docker image
45 | uses: docker/build-push-action@v3
46 | with:
47 | context: .
48 | platforms: linux/amd64
49 | file: .docker/transporter_deployment/Dockerfile
50 | push: true
51 | no-cache: true
52 | tags: ${{ env.REGISTRY }}/${{ env.ORG_NAME }}/${{ env.IMAGE_NAME }}:humble
53 |
--------------------------------------------------------------------------------
/.github/workflows/zed.yaml:
--------------------------------------------------------------------------------
1 | name: zed2
2 |
3 | on:
4 | push:
5 | branches: ['franka_emika_panda']
6 | paths:
7 | - .gitmodules
8 | - src/cameras/**
9 | - .docker/cameras/Dockerfile.zed
10 | - .github/workflows/zed.yaml
11 | workflow_dispatch:
12 |
13 | env:
14 | REGISTRY: ghcr.io
15 | ORG_NAME: peterdavidfagan
16 | IMAGE_NAME: zed2
17 |
18 | jobs:
19 | build-and-push-image:
20 | runs-on: ubuntu-latest
21 | permissions:
22 | contents: read
23 | packages: write
24 |
25 | steps:
26 | - name: Checkout repository
27 | uses: actions/checkout@v3
28 | with:
29 | submodules: recursive
30 | token: ${{ secrets.ORG_PAT }}
31 |
32 | - name: Free Space
33 | run: |
34 | sudo rm -rf /opt/ghc
35 | sudo rm -rf /opt/hostedtoolcache
36 | sudo rm -rf /usr/share/dotnet
37 | sudo rm -rf "$AGENT_TOOLSDIRECTORY"
38 | sudo rm -rf ${GITHUB_WORKSPACE}/.git
39 |
40 | - name: Set up QEMU
41 | uses: docker/setup-qemu-action@v2
42 |
43 | - name: Set up Docker Buildx
44 | uses: docker/setup-buildx-action@v2
45 |
46 | - name: Log in to the Container registry
47 | uses: docker/login-action@f054a8b539a109f9f41c372932f1ae047eff08c9
48 | with:
49 | registry: ${{ env.REGISTRY }}
50 | username: ${{ github.actor }}
51 | password: ${{ secrets.GITHUB_TOKEN }}
52 |
53 | - name: Build and push Docker image
54 | uses: docker/build-push-action@v3
55 | with:
56 | context: .
57 | file: .docker/cameras/Dockerfile.zed
58 | push: true
59 | no-cache: true
60 | tags: ${{ env.REGISTRY }}/${{ env.ORG_NAME }}/${{ env.IMAGE_NAME }}:humble
61 |
--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 | # mujoco
2 | MUJOCO_LOG.txt
3 |
4 | # Byte-compiled / optimized / DLL files
5 | __pycache__/
6 | *.py[cod]
7 | *$py.class
8 |
9 | # C extensions
10 | *.so
11 |
12 | # Distribution / packaging
13 | .Python
14 | build/
15 | develop-eggs/
16 | dist/
17 | downloads/
18 | eggs/
19 | .eggs/
20 | lib/
21 | lib64/
22 | parts/
23 | sdist/
24 | var/
25 | wheels/
26 | share/python-wheels/
27 | *.egg-info/
28 | .installed.cfg
29 | *.egg
30 | MANIFEST
31 |
32 | # PyInstaller
33 | # Usually these files are written by a python script from a template
34 | # before PyInstaller builds the exe, so as to inject date/other infos into it.
35 | *.manifest
36 | *.spec
37 |
38 | # Installer logs
39 | pip-log.txt
40 | pip-delete-this-directory.txt
41 |
42 | # Unit test / coverage reports
43 | htmlcov/
44 | .tox/
45 | .nox/
46 | .coverage
47 | .coverage.*
48 | .cache
49 | nosetests.xml
50 | coverage.xml
51 | *.cover
52 | *.py,cover
53 | .hypothesis/
54 | .pytest_cache/
55 | cover/
56 |
57 | # Translations
58 | *.mo
59 | *.pot
60 |
61 | # Django stuff:
62 | *.log
63 | local_settings.py
64 | db.sqlite3
65 | db.sqlite3-journal
66 |
67 | # Flask stuff:
68 | instance/
69 | .webassets-cache
70 |
71 | # Scrapy stuff:
72 | .scrapy
73 |
74 | # Sphinx documentation
75 | docs/_build/
76 |
77 | # PyBuilder
78 | .pybuilder/
79 | target/
80 |
81 | # Jupyter Notebook
82 | .ipynb_checkpoints
83 |
84 | # IPython
85 | profile_default/
86 | ipython_config.py
87 |
88 | # pyenv
89 | # For a library or package, you might want to ignore these files since the code is
90 | # intended to run in multiple environments; otherwise, check them in:
91 | # .python-version
92 |
93 | # pipenv
94 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
95 | # However, in case of collaboration, if having platform-specific dependencies or dependencies
96 | # having no cross-platform support, pipenv may install dependencies that don't work, or not
97 | # install all needed dependencies.
98 | #Pipfile.lock
99 |
100 | # poetry
101 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
102 | # This is especially recommended for binary packages to ensure reproducibility, and is more
103 | # commonly ignored for libraries.
104 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
105 | #poetry.lock
106 |
107 | # pdm
108 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control.
109 | #pdm.lock
110 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it
111 | # in version control.
112 | # https://pdm.fming.dev/#use-with-ide
113 | .pdm.toml
114 |
115 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm
116 | __pypackages__/
117 |
118 | # Celery stuff
119 | celerybeat-schedule
120 | celerybeat.pid
121 |
122 | # SageMath parsed files
123 | *.sage.py
124 |
125 | # Environments
126 | .env
127 | .venv
128 | env/
129 | venv/
130 | ENV/
131 | env.bak/
132 | venv.bak/
133 |
134 | # Spyder project settings
135 | .spyderproject
136 | .spyproject
137 |
138 | # Rope project settings
139 | .ropeproject
140 |
141 | # mkdocs documentation
142 | /site
143 |
144 | # mypy
145 | .mypy_cache/
146 | .dmypy.json
147 | dmypy.json
148 |
149 | # Pyre type checker
150 | .pyre/
151 |
152 | # pytype static type analyzer
153 | .pytype/
154 |
155 | # Cython debug symbols
156 | cython_debug/
157 |
158 | # PyCharm
159 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can
160 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
161 | # and can be added to the global gitignore or merged into this file. For a more nuclear
162 | # option (not recommended) you can uncomment the following to ignore the entire idea folder.
163 | #.idea/
164 |
--------------------------------------------------------------------------------
/.gitmodules:
--------------------------------------------------------------------------------
1 | [submodule "src/audio/ros2_whisper"]
2 | path = src/audio/ros2_whisper
3 | url = git@github.com:peterdavidfagan/ros2_whisper.git
4 | [submodule "src/control/serial"]
5 | path = src/control/serial
6 | url = git@github.com:tylerjw/serial.git
7 | branch = ros2
8 | [submodule "src/motion_planning/franka_robotiq"]
9 | path = src/motion_planning/franka_robotiq
10 | url = git@github.com:peterdavidfagan/franka_robotiq.git
11 | [submodule "src/control/franka_ros2"]
12 | path = src/control/franka_ros2
13 | url = git@github.com:peterdavidfagan/franka_ros2.git
14 | branch = main
15 | [submodule "src/control/topic_based_ros2_control"]
16 | path = src/control/topic_based_ros2_control
17 | url = git@github.com:PickNikRobotics/topic_based_ros2_control.git
18 | [submodule "src/grasping/ros2_deep_grasps"]
19 | path = src/grasping/ros2_deep_grasps
20 | url = git@github.com:peterdavidfagan/ros2_deep_grasps.git
21 | [submodule "src/llm/ros2_dspy"]
22 | path = src/llm/ros2_dspy
23 | url = git@github.com:peterdavidfagan/ros2_dspy.git
24 | [submodule "src/mujoco/mujoco_ros"]
25 | path = src/mujoco/mujoco_ros
26 | url = git@github.com:peterdavidfagan/mujoco_ros.git
27 | [submodule "src/data_collection/moveit2_data_collector"]
28 | path = src/data_collection/moveit2_data_collector
29 | url = git@github.com:peterdavidfagan/moveit2_data_collector.git
30 | [submodule "src/cameras/moveit2_camera_calibration"]
31 | path = src/cameras/moveit2_camera_calibration
32 | url = git@github.com:peterdavidfagan/moveit2_camera_calibration.git
33 | [submodule "src/cameras/zed-ros2-wrapper"]
34 | path = src/cameras/zed-ros2-wrapper
35 | url = git@github.com:stereolabs/zed-ros2-wrapper.git
36 | branch = master
37 | [submodule "src/motion_planning/moveit2"]
38 | path = src/motion_planning/moveit2
39 | url = git@github.com:ros-planning/moveit2.git
40 | branch = main
41 | [submodule "src/control/ros2_control"]
42 | path = src/control/ros2_control
43 | url = git@github.com:ros-controls/ros2_control.git
44 | branch = humble
45 | [submodule "src/control/ros2_controllers"]
46 | path = src/control/ros2_controllers
47 | branch = humble
48 | url = git@github.com:ros-controls/ros2_controllers.git
49 | [submodule "src/motion_planning/generate_parameter_library"]
50 | path = src/motion_planning/generate_parameter_library
51 | url = git@github.com:PickNikRobotics/generate_parameter_library.git
52 | [submodule "libfranka"]
53 | path = libfranka
54 | url = git@github.com:tingelst/libfranka.git
55 | [submodule "src/motion_planning/moveit2_policy_msgs"]
56 | path = src/motion_planning/moveit2_policy_msgs
57 | url = git@github.com:peterdavidfagan/moveit2_policy_msgs.git
58 | [submodule "src/control/ros2_robotiq_gripper"]
59 | path = src/control/ros2_robotiq_gripper
60 | url = git@github.com:peterdavidfagan/ros2_robotiq_gripper.git
61 | [submodule "src/object_detection/ros2_object_detection"]
62 | path = src/object_detection/ros2_object_detection
63 | url = git@github.com:peterdavidfagan/ros2_object_detection.git
64 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | [](https://github.com/peterdavidfagan/ros2_robotics_research_toolkit/blob/franka_emika_panda/.github/workflows/pages.yaml)
2 | [](https://github.com/peterdavidfagan/ros2_robotics_research_toolkit/blob/franka_emika_panda/.github/workflows/control.yaml)
3 | [](https://github.com/peterdavidfagan/ros2_robotics_research_toolkit/blob/franka_emika_panda/.github/workflows/motion_planning.yaml)
4 | [](https://github.com/peterdavidfagan/ros2_robotics_research_toolkit/blob/franka_emika_panda/.github/workflows/zed.yaml)
5 | [](https://github.com/peterdavidfagan/ros2_robotics_research_toolkit/blob/franka_emika_panda/.github/workflows/calibration_app.yaml)
6 |
7 |
8 | # ROS 2 Robot Learning Workspace
9 | This toolkit provides a set of ROS packages with examples of using these packages in robot learning research. An official release of this workspace will be published for the Franka Robotics Panda robot in the coming weeks at which point all demos will be tested and verified for fixed versions of Franka Robotics firmware.
10 |
11 | 
12 |
13 | [**[Documentation]**](https://peterdavidfagan.com/ros2_robotics_research_toolkit/)
14 |
15 |
16 | # Supported Franka Robotics Versions
17 |
18 | | OS | ROS 2 Version | Franka Robot | Franka System Version | Libfranka Version |
19 | | --- | --- | --- | --- | --- |
20 | | Ubuntu 22.04 | Humble | Panda | 4.2.2 | [custom backport](https://github.com/tingelst/libfranka/tree/1e4388ba2b39f7bace3d2cd3364996e0d8bdf9ac) |
21 |
22 |
23 | # Docker Support
24 | In order to avoid users having to manage the installation and building of this ROS workspace I am releasing Docker containers which should function across various Ubuntu operating systems. Where the host system requires specific dependencies or settings (e.g. realtime kernel patch) I provide a setup shell script to configure the host system, further details to be added to the official documentation.
25 |
--------------------------------------------------------------------------------
/assets/Lite-6.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/assets/Lite-6.gif
--------------------------------------------------------------------------------
/assets/control_debug.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/assets/control_debug.png
--------------------------------------------------------------------------------
/assets/franka-emika.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/assets/franka-emika.gif
--------------------------------------------------------------------------------
/assets/robotics_toolkit.jpeg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/assets/robotics_toolkit.jpeg
--------------------------------------------------------------------------------
/assets/ufactory.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/assets/ufactory.png
--------------------------------------------------------------------------------
/assets/workspace.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/assets/workspace.jpg
--------------------------------------------------------------------------------
/docs/.DS_Store:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/docs/.DS_Store
--------------------------------------------------------------------------------
/docs/Gemfile:
--------------------------------------------------------------------------------
1 | source 'https://rubygems.org'
2 |
3 | gem "jekyll", "~> 4.3.3" # installed by `gem jekyll`
4 | # gem "webrick" # required when using Ruby >= 3 and Jekyll <= 4.2.2
5 |
6 | gem "just-the-docs", "0.7.0" # pinned to the current release
7 | # gem "just-the-docs" # always download the latest release
8 |
--------------------------------------------------------------------------------
/docs/_config.yml:
--------------------------------------------------------------------------------
1 | title: Toolkit Documentation
2 | description: Guides for performing robot learning research with ROS 2 ecosystem.
3 | theme: just-the-docs
4 |
5 | url: https://peterdavidfagan.github.io/ros2_robotics_research_toolkit # this url not being used right now
6 | color_scheme: ros2_robotics_research_toolkit # TODO: update name to DROID
7 |
8 | aux_links:
9 | Official GitHub Repository: https://github.com/peterdavidfagan/ros2_robotics_research_toolkit
10 |
--------------------------------------------------------------------------------
/docs/_includes/head_custom.html:
--------------------------------------------------------------------------------
1 |
2 |
--------------------------------------------------------------------------------
/docs/_sass/color_schemes/ros2_robotics_research_toolkit.scss:
--------------------------------------------------------------------------------
1 | @import "./color_schemes/light";
2 |
3 | // add custom colours here
4 | $nord-blue: #5E81AC; // can be updated assumed primary author's university color scheme
5 |
6 | $link-color: $nord-blue;
7 | $btn-primary-color: $nord-blue;
8 |
--------------------------------------------------------------------------------
/docs/assets/.DS_Store:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/docs/assets/.DS_Store
--------------------------------------------------------------------------------
/docs/assets/index/.DS_Store:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/docs/assets/index/.DS_Store
--------------------------------------------------------------------------------
/docs/assets/index/link_robotics.jpeg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/docs/assets/index/link_robotics.jpeg
--------------------------------------------------------------------------------
/docs/assets/index/robotics_toolkit.jpeg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/docs/assets/index/robotics_toolkit.jpeg
--------------------------------------------------------------------------------
/docs/contribution-guidelines.md:
--------------------------------------------------------------------------------
1 | ---
2 | layout: default
3 | title: Contribution Guidelines
4 | nav_order: 6
5 | ---
6 |
7 | # Contribution Guidelines
8 |
9 | It's always great to build software with a broader community of developers. This repository welcomes contributions. For now feel free to open issues/prs with suggested changes to the existing codebase. In future more detailed contribution guidelines will be provided.
10 |
--------------------------------------------------------------------------------
/docs/favicon.ico:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/docs/favicon.ico
--------------------------------------------------------------------------------
/docs/hardware_setup/franka-emika-panda.md:
--------------------------------------------------------------------------------
1 | ---
2 | layout: default
3 | title: Franka Emika Panda
4 | parent: Hardware Setup
5 | nav_order: 1
6 | ---
7 |
8 |
9 |
10 | Table of contents
11 |
12 | {: .text-delta }
13 | 1. TOC
14 | {:toc}
15 |
16 |
--------------------------------------------------------------------------------
/docs/hardware_setup/hardware-setup.md:
--------------------------------------------------------------------------------
1 | ---
2 | layout: default
3 | title: Hardware Setup
4 | nav_order: 2
5 | has_children: true
6 | permalink: /docs/hardware-setup
7 | ---
8 |
9 |
--------------------------------------------------------------------------------
/docs/hardware_setup/ufactory-lite6.md:
--------------------------------------------------------------------------------
1 | ---
2 | layout: default
3 | title: UFactory Lite6
4 | parent: Hardware Setup
5 | nav_order: 2
6 | ---
7 |
8 |
9 |
10 | Table of contents
11 |
12 | {: .text-delta }
13 | 1. TOC
14 | {:toc}
15 |
16 |
--------------------------------------------------------------------------------
/docs/index.md:
--------------------------------------------------------------------------------
1 | ---
2 | layout: default
3 | title: Introduction
4 | nav_order: 1
5 | ---
6 |
7 | # 🤖 ROS 2 Robotics Research Toolkit
8 |
9 | 
10 |
11 | This toolkit organises a ROS 2 workspace containing packages for supporting robot learning research applications. Included in this toolkit is documentation for various use cases. Feel free to leave feedback on the official GitHub repository. ❤️ Open-source software.
12 |
13 | ## Supported Hardware Platforms
14 |
15 | As this toolkit is built on top of the ROS 2 ecosystem, most packages are robot agnostic and the integration of new robots shouldn't take too much additional work. The toolkit officially supports the following hardware:
16 |
17 | **Robots**
18 |
19 | * [Franka Emika Panda](https://franka.de/)
20 | * [UFactory Lite 6](https://www.ufactory.cc/cost-effective-cobot-robots/?utm_source=google+search&utm_medium=cpc&utm_campaign=branding&utm_id=lite6&gad_source=1&gclid=Cj0KCQjwqpSwBhClARIsADlZ_TlpTdc8vQFga-vzzFeHuxixMaVwYPAD9ClVVzEIYqWx5MAz1rWz54YaAll-EALw_wcB)
21 |
22 | **Cameras**
23 |
24 | * [Zed 2i](https://www.stereolabs.com/products/zed-2)
25 | * [Zed mini](https://store.stereolabs.com/en-gb/products/zed-mini)
26 |
27 | ## Supported Use Cases
28 |
29 | * Speech-to-Text
30 | * Camera Calibration
31 | * Motion Planning
32 | * Deploying Neural Network Policies
33 |
--------------------------------------------------------------------------------
/docs/software_setup/docker.md:
--------------------------------------------------------------------------------
1 | ---
2 | layout: default
3 | title: Running Dockerized Applications
4 | parent: Software Setup
5 | nav_order: 2
6 | ---
7 |
8 |
9 |
10 | Table of contents
11 |
12 | {: .text-delta }
13 | 1. TOC
14 | {:toc}
15 |
16 |
--------------------------------------------------------------------------------
/docs/software_setup/host_installation.md:
--------------------------------------------------------------------------------
1 | ---
2 | layout: default
3 | title: Building ROS 2 Workspace on Host
4 | parent: Software Setup
5 | nav_order: 1
6 | ---
7 |
8 |
9 |
10 | Table of contents
11 |
12 | {: .text-delta }
13 | 1. TOC
14 | {:toc}
15 |
16 |
--------------------------------------------------------------------------------
/docs/software_setup/software-setup.md:
--------------------------------------------------------------------------------
1 | ---
2 | layout: default
3 | title: Software Setup
4 | nav_order: 3
5 | has_children: true
6 | permalink: /docs/software-setup
7 | ---
8 |
9 |
--------------------------------------------------------------------------------
/docs/tutorials/launch-mujoco-sim.md:
--------------------------------------------------------------------------------
1 | ---
2 | layout: default
3 | title: Launch MuJoCo ROS Simulation
4 | parent: Tutorials
5 | nav_order: 4
6 | ---
7 |
8 |
9 |
10 | Table of contents
11 |
12 | {: .text-delta }
13 | 1. TOC
14 | {:toc}
15 |
16 |
--------------------------------------------------------------------------------
/docs/tutorials/moveit-motion-planning.md:
--------------------------------------------------------------------------------
1 | ---
2 | layout: default
3 | title: Motion Planning with MoveIt
4 | parent: Tutorials
5 | nav_order: 2
6 | ---
7 |
8 |
9 |
10 | Table of contents
11 |
12 | {: .text-delta }
13 | 1. TOC
14 | {:toc}
15 |
16 |
--------------------------------------------------------------------------------
/docs/tutorials/onnx-policy-deployment.md:
--------------------------------------------------------------------------------
1 | ---
2 | layout: default
3 | title: Deploying Policies with ONNX
4 | parent: Tutorials
5 | nav_order: 3
6 | ---
7 |
8 |
9 |
10 | Table of contents
11 |
12 | {: .text-delta }
13 | 1. TOC
14 | {:toc}
15 |
16 |
--------------------------------------------------------------------------------
/docs/tutorials/ros2-control.md:
--------------------------------------------------------------------------------
1 | ---
2 | layout: default
3 | title: Starting ROS 2 Controllers
4 | parent: Tutorials
5 | nav_order: 1
6 | ---
7 |
8 |
9 |
10 | Table of contents
11 |
12 | {: .text-delta }
13 | 1. TOC
14 | {:toc}
15 |
16 |
17 | # Background
18 |
19 | Lets review ROS 2 control to understand the core concepts behind how the library manages controllers.
20 |
21 | Please consult the official [ROS 2 control documentation]() for further information.
22 |
23 | # Configuring Controllers
24 |
25 | There are two files we care to configure to control which controllers we launch as part of our application:
26 |
27 | 1. The launch file which launches the controller manager and controllers.
28 | 2. ROS 2 controller configuration files which control the configuration of controllers (e.g. the controller update frequency).
29 |
30 |
31 | # Running Controllers via Docker (Recommended)
32 |
33 | A built docker container is shipped as part of this repository, feel free to pull the latest version of the container with:
34 |
35 | ```bash
36 | docker pull ghcr.io/peterdavidfagan/panda_control:rolling
37 | ```
38 |
39 | Once you have pulled the built container you can run the container via the docker compose script present under `.docker/control` from the root directory of the repository.
40 |
41 | ```bash
42 | docker compose -f docker-compose-control-server.launch.py up
43 | ```
44 |
45 | You can expect to see output similar to the following:
46 |
47 | ```
48 | ```
49 |
50 | # Running Controllers on the Host
51 |
52 |
--------------------------------------------------------------------------------
/docs/tutorials/tutorials.md:
--------------------------------------------------------------------------------
1 | ---
2 | layout: default
3 | title: Tutorials
4 | has_children: true
5 | nav_order: 3
6 | permalink: /docs/tutorials
7 | ---
8 |
--------------------------------------------------------------------------------
/pyproject.toml:
--------------------------------------------------------------------------------
1 | [project]
2 | name = "ros2_robotics_research_toolkit"
3 | version = "0.0.0"
4 | description = "A ROS 2 workspace to support robot learning research within the ROS 2 ecosystem."
5 | authors = [
6 | { name = "Peter David Fagan", email = "peterdavidfagan@gmail.com" }
7 | ]
8 | readme = "README.md"
9 |
10 | [tool.poetry]
11 | package-mode=false
12 | name = "ros2_robotics_research_toolkit"
13 | version = "0.0.0"
14 | description = "A ROS 2 workspace to support robot learning research within the ROS 2 ecosystem."
15 | license = "Apache-2.0"
16 | authors = [
17 | "Peter David Fagan "
18 | ]
19 | readme = "README.md"
20 | classifiers = [
21 | "Operating System :: POSIX :: Linux"
22 | ]
23 |
24 | [tool.poetry.dependencies]
25 | python = "3.10.6"
26 | numpy = "^1.16.0"
27 | notebook = "^7.0.0"
28 | scipy = "^1.13.0"
29 | catkin-pkg = "^1.0.0"
30 | empy = "3.3.4"
31 | lark = "^1.1.9"
32 | dspy-ai = "^2.4.9"
33 |
34 | [tool.poetry.group.dm_env]
35 | optional = true
36 |
37 | [tool.poetry.group.dm_env.dependencies]
38 | dm_env = "1.6"
39 | onnx = "^1.16.0"
40 | onnxruntime = "^1.17.3"
41 | einops = "^0.8.0"
42 | jaxlib = "^0.4.26"
43 | jax = "^0.4.26"
44 | pyqt6 = "^6.7.0" # used for GUI applications only, moved to ensure arm builds
45 |
46 | [tool.poetry.group.camera]
47 | optional = true
48 |
49 | [tool.poetry.group.camera.dependencies]
50 | opencv-python = "4.6.0.66"
51 | opencv-contrib-python = "4.6.0.66"
52 | cvbridge3 = "^1.1"
53 |
54 | [tool.poetry.group.transporter_data]
55 | optional = true
56 |
57 | [tool.poetry.group.transporter_data.dependencies]
58 | tensorflow-cpu = {version="2.15.0", markers = "sys_platform == 'linux'"}
59 | envlogger = {version="^1.1", extras=["tfds"], markers = "sys_platform == 'linux'"}
60 | tensorflow-datasets = "4.9.3"
61 | rlds = {version="^0.1.7", markers = "sys_platform == 'linux'"}
62 |
63 | [tool.poetry.group.huggingface]
64 | optional = true
65 |
66 | [tool.poetry.group.huggingface.dependencies]
67 | huggingface-hub = "^0.23.0"
68 | transformers = "^4.41.1"
69 | torch = "^2.3.0"
70 | pillow = "^10.3.0"
71 |
72 | [tool.black]
73 | line-length = 120
74 |
75 | [tool.flake8]
76 | max-line-length = 120
77 |
--------------------------------------------------------------------------------
/scripts/client_machine_setup.sh:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | ROOT_DIR="$(git rev-parse --show-toplevel)"
4 | DOCKER_COMPOSE_DIR="$ROOT_DIR/.docker/motion_planning"
5 | DOCKER_COMPOSE_FILE="$DOCKER_COMPOSE_DIR/docker-compose-motion_planning.yaml"
6 | LAPTOP_IP="192.168.1.11"
7 |
8 | echo "Welcome to the lite6_ws setup process."
9 |
10 | # install docker
11 | echo -e "\nInstall docker \n"
12 |
13 | apt-get update
14 | apt-get install ca-certificates curl gnupg
15 | install -m 0755 -d /etc/apt/keyrings
16 | curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo gpg --dearmor -o /etc/apt/keyrings/docker.gpg
17 | chmod a+r /etc/apt/keyrings/docker.gpg
18 | echo \
19 | "deb [arch="$(dpkg --print-architecture)" signed-by=/etc/apt/keyrings/docker.gpg] https://download.docker.com/linux/ubuntu \
20 | "$(. /etc/os-release && echo "$VERSION_CODENAME")" stable" | \
21 | sudo tee /etc/apt/sources.list.d/docker.list > /dev/null
22 | apt-get update
23 | apt-get install docker-ce docker-ce-cli containerd.io docker-buildx-plugin docker-compose-plugin
24 |
25 | # ensure GUI window is accessible from container
26 | echo -e "Set Docker Xauth for x11 forwarding \n"
27 |
28 | export DOCKER_XAUTH=/tmp/.docker.xauth
29 | echo "export DOCKER_XAUTH=/tmp/.docker.xauth" >> ~/.bashrc
30 | rm $DOCKER_XAUTH
31 | touch $DOCKER_XAUTH
32 | xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $DOCKER_XAUTH nmerge -
33 |
34 | # find ethernet interface on device
35 | echo -e "\n set static ip \n"
36 |
37 | echo "Select an Ethernet interface to set a static IP for:"
38 |
39 | interfaces=$(ip -o link show | grep -Eo '^[0-9]+: (en|eth|ens|eno|enp)[a-z0-9]*' | awk -F' ' '{print $2}')
40 |
41 | # Display available interfaces for the user to choose from
42 | select interface_name in $interfaces; do
43 | if [ -n "$interface_name" ]; then
44 | break
45 | else
46 | echo "Invalid selection. Please choose a valid interface."
47 | fi
48 | done
49 |
50 | echo "You've selected: $interface_name"
51 |
52 | # Add and configure the static IP connection
53 | nmcli connection delete "laptop_static"
54 | nmcli connection add con-name "laptop_static" ifname "$interface_name" type ethernet
55 | nmcli connection modify "laptop_static" ipv4.method manual ipv4.address $LAPTOP_IP/24
56 | nmcli connection up "laptop_static"
57 |
58 | echo "Static IP configuration complete for interface $interface_name."
59 |
60 |
--------------------------------------------------------------------------------
/scripts/nuc_setup.sh:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | export ROOT_DIR=$(git rev-parse --show-toplevel)
4 | export NUC_IP="192.168.1.10"
5 |
6 | echo "Welcome to the lite6_ws setup process."
7 |
8 | read -p "Is this your first time setting up the machine? (yes/no): " first_time
9 |
10 | if [ "$first_time" = "yes" ]; then
11 | echo "Great! Let's proceed with the setup."
12 |
13 | # ensure submodules are cloned
14 | echo "Repulling all submodules."
15 | read -p "Enter the user whose ssh credentials will be used: " USERNAME
16 | eval "$(ssh-agent -s)"
17 | ssh-add /home/$USERNAME/.ssh/id_ed25519
18 | ROOT_DIR="$(git rev-parse --show-toplevel)"
19 | cd $ROOT_DIR && git submodule update --recursive --init
20 |
21 | # install docker
22 | echo -e "\nInstall docker \n"
23 |
24 | apt-get update
25 | apt-get install ca-certificates curl gnupg
26 | install -m 0755 -d /etc/apt/keyrings
27 | curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo gpg --dearmor -o /etc/apt/keyrings/docker.gpg
28 | chmod a+r /etc/apt/keyrings/docker.gpg
29 | echo \
30 | "deb [arch="$(dpkg --print-architecture)" signed-by=/etc/apt/keyrings/docker.gpg] https://download.docker.com/linux/ubuntu \
31 | "$(. /etc/os-release && echo "$VERSION_CODENAME")" stable" | \
32 | sudo tee /etc/apt/sources.list.d/docker.list > /dev/null
33 | apt-get update
34 | apt-get install docker-ce docker-ce-cli containerd.io docker-buildx-plugin docker-compose-plugin
35 | systemctl enable docker
36 |
37 | # perform rt-patch
38 | echo -e "\nPerform realtime patch of kernel \n"
39 |
40 | apt update && apt install ubuntu-advantage-tools
41 | pro attach $UBUNTU_PRO_TOKEN
42 | pro enable realtime-kernel
43 |
44 | # cpu frequency scaling
45 | echo -e "\nSet cpu frequency scaling settings \n"
46 |
47 | apt install cpufrequtils -y
48 | systemctl disable ondemand
49 | systemctl enable cpufrequtils
50 | sh -c 'echo "GOVERNOR=performance" > /etc/default/cpufrequtils'
51 | systemctl daemon-reload && sudo systemctl restart cpufrequtils
52 |
53 | else
54 | echo -e "\nWelcome back!\n"
55 | fi
56 |
57 | # find ethernet interface on device
58 | echo -e "\n set static ip \n"
59 |
60 | echo "Select an Ethernet interface to set a static IP for:"
61 |
62 | interfaces=$(ip -o link show | grep -Eo '^[0-9]+: (en|eth|ens|eno|enp)[a-z0-9]*' | awk -F' ' '{print $2}')
63 |
64 | # Display available interfaces for the user to choose from
65 | select interface_name in $interfaces; do
66 | if [ -n "$interface_name" ]; then
67 | break
68 | else
69 | echo "Invalid selection. Please choose a valid interface."
70 | fi
71 | done
72 |
73 | echo "You've selected: $interface_name"
74 |
75 | # Add and configure the static IP connection
76 | nmcli connection delete "nuc_static"
77 | nmcli connection add con-name "nuc_static" ifname "$interface_name" type ethernet
78 | nmcli connection modify "nuc_static" ipv4.method manual ipv4.address $NUC_IP/24
79 | nmcli connection up "nuc_static"
80 |
81 | echo "Static IP configuration complete for interface $interface_name."
82 |
83 | # turn on ssh
84 | echo -e "\n turn on ssh \n"
85 | systemctl enable ssh
86 |
87 | # turn off display manager
88 | systemctl disable display-manager.service
89 |
90 |
--------------------------------------------------------------------------------
/scripts/ros_workspace_setup.sh:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 |
3 | # parse input args
4 | # Initialize variables with default values
5 | poetry_build=true
6 |
7 | # Function to display usage information
8 | usage() {
9 | echo "Usage: $0 [--poetry_build ]"
10 | echo "Options:"
11 | echo " --poetry_build Specify whether to build poetry env or not (optional, default: false)"
12 | exit 1
13 | }
14 |
15 | # Parse command line options
16 | while [[ $# -gt 0 ]]; do
17 | key="$1"
18 | case $key in
19 | --poetry_build)
20 | case $2 in
21 | true)
22 | poetry_build=true
23 | ;;
24 | false)
25 | poetry_build=false
26 | ;;
27 | *)
28 | echo "Invalid value for --poetry_build. Please use 'true' or 'false'."
29 | usage
30 | ;;
31 | esac
32 | shift # past argument
33 | shift # past value
34 | ;;
35 | *) # unknown option
36 | echo "Unknown option: $key"
37 | usage
38 | ;;
39 | esac
40 | done
41 |
42 | set -e
43 |
44 | # enter root directory
45 | cd ..
46 |
47 | # install libfranka
48 | sudo apt update && apt upgrade -y || true
49 | sudo apt remove "*libfranka*" -y || true
50 | sudo apt install -y build-essential cmake git libpoco-dev libeigen3-dev
51 | cd libfranka
52 | if [ -d "./build" ]; then
53 | rm -rf build
54 | fi
55 | mkdir build
56 | cd build
57 | cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF ..
58 | cmake --build .
59 | cpack -G DEB
60 | sudo dpkg -i libfranka*.deb
61 | cd ../..
62 |
63 | # install and activate python environment
64 | if $poetry_build; then
65 | [ -e poetry.lock ] && rm poetry.lock
66 | poetry install
67 | source $(poetry env info --path)/bin/activate
68 | else
69 | source .venv/bin/activate
70 | fi
71 |
72 | # source humble installation and install middleware
73 | source /opt/ros/humble/setup.bash
74 | sudo apt install ros-$ROS_DISTRO-rmw-cyclonedds-cpp -y
75 |
76 | # install moveit dependencies
77 | if [ -d "./src/motion_planning" ]; then
78 | cd ./src/motion_planning
79 | for repo in moveit2/moveit2.repos $(f="moveit2/moveit2_rolling.repos"; test -r $f && echo $f); do vcs import < "$repo"; done
80 | else
81 | echo "Ignoring motion planning dependencies"
82 | fi
83 |
84 | # install rosdep dependencies
85 | cd ..
86 | rosdep update
87 | rosdep install --from-paths . --ignore-src --rosdistro $ROS_DISTRO -r -y
88 | cd ..
89 |
90 | # build the workspace
91 | colcon build --packages-ignore libfranka libfranka-common
92 | source ./install/setup.bash
93 |
--------------------------------------------------------------------------------
/src/applications/camera_calibration/README.md:
--------------------------------------------------------------------------------
1 | # Camera Calibration
2 |
3 | https://github.com/peterdavidfagan/moveit2_camera_calibration/assets/42982057/9c4e3532-9f56-4a14-89c3-fb88c366c038
4 |
5 | This application is deployed via Docker containers that are run on the Intel NUC and client machine.
6 |
7 | To run the control server run:
8 |
9 | ```
10 | docker compose -f deploy_nuc.yaml up
11 | ```
12 |
13 | To run the camera calibration app, cameras and motion planning software run:
14 |
15 | ```
16 | docker compose -f deploy_client.yaml up
17 | ```
18 |
--------------------------------------------------------------------------------
/src/applications/camera_calibration/config/camera_calibration.yaml:
--------------------------------------------------------------------------------
1 | camera_image_topic: /zed/zed_camera/rgb_raw/image_raw_color
2 | camera_info_topic: /zed/zed_camera/rgb_raw/camera_info
3 |
4 | charuco:
5 | squares_x: 14
6 | squares_y: 9
7 | square_length: 0.02
8 | marker_length: 0.015
9 |
10 | workspace:
11 | x_min: -0.1
12 | x_max: 0.1
13 | y_min: -0.1
14 | y_max: 0.1
15 | z_min: -0.1
16 | z_max: 0.1
17 | rot_x_min: -20.0
18 | rot_x_max: 20.0
19 | rot_y_min: -20.0
20 | rot_y_max: 20.0
21 | rot_z_min: -20.0
22 | rot_z_max: 20.0
23 |
24 | eye_to_hand_calibration:
25 | num_samples: 50 # for debug purposes
26 | sample_delay: 0.2
27 |
28 | eye_in_hand_calibration:
29 | num_samples: 50 # for debug purposes
30 | sample_delay: 0.2
31 |
--------------------------------------------------------------------------------
/src/applications/camera_calibration/config/zed2i.yaml:
--------------------------------------------------------------------------------
1 | # config/zed2i_yaml
2 | # Parameters for Stereoabs zed2i camera
3 | ---
4 | /**:
5 | ros__parameters:
6 | general:
7 | camera_model: 'zed2i'
8 | camera_name: 'zed2i' # usually overwritten by launch file
9 | serial_number: 35215462
10 | grab_resolution: 'HD2K' # Maximize resolution for calibration
11 | grab_frame_rate: 30 # ZED SDK internal grabbing rate
12 |
13 | depth:
14 | min_depth: 0.2 # Min: 0.2, Max: 3.0
15 | max_depth: 10.0 # Max: 40.0l
16 |
--------------------------------------------------------------------------------
/src/applications/camera_calibration/deploy_client.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | zed_camera:
5 | image: ghcr.io/peterdavidfagan/zed2:humble
6 | container_name: zed_camera
7 | build:
8 | context: ../../../../
9 | dockerfile: .docker/cameras/Dockerfile.zed
10 | environment:
11 | - NVIDIA_VISIBLE_DEVICES=all
12 | volumes:
13 | - ./config/zed2i.yaml:/root/zed_ws/src/cameras/zed_wrapper/config/zed2i.yaml
14 | devices:
15 | - "/dev:/dev"
16 | network_mode: "host"
17 | privileged: true
18 | runtime: nvidia
19 | command: ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zed2i node_name:=zed_camera
20 |
21 | motion_planning_prerequisites:
22 | image: ghcr.io/peterdavidfagan/panda_motion_planning:humble
23 | build:
24 | context: ../../../../
25 | dockerfile: .docker/motion_planning/Dockerfile.motion_planning
26 | environment:
27 | - DISPLAY=${DISPLAY}
28 | - XAUTHORITY=${DOCKER_XAUTH}
29 | volumes:
30 | - /tmp/.X11-unix:/tml/.X11-unix:rw
31 | - ~/.Xauthority:/root/.Xauthority
32 | #- ${DOCKER_XAUTH}:${DOCKER_XAUTH}
33 | network_mode: "host"
34 | command: ros2 launch panda_motion_planning_demos motion_planning_prerequisites.launch.py
35 |
36 | motion_planning_notebook:
37 | image: ghcr.io/peterdavidfagan/panda_motion_planning:humble
38 | build:
39 | context: ../../../../
40 | dockerfile: .docker/motion_planning/Dockerfile.motion_planning
41 | environment:
42 | - DISPLAY=${DISPLAY}
43 | - XAUTHORITY=${DOCKER_XAUTH}
44 | volumes:
45 | - /tmp/.X11-unix:/tml/.X11-unix:rw
46 | - ~/.Xauthority:/root/.Xauthority
47 | #- ${DOCKER_XAUTH}:${DOCKER_XAUTH}
48 | network_mode: "host"
49 | command: python3 -m notebook --allow-root
50 |
51 | camera_calibration_app:
52 | image: ghcr.io/peterdavidfagan/moveit2_camera_calibration:humble
53 | build:
54 | context: ../../../../
55 | dockerfile: .docker/camera_calibration_app/Dockerfile
56 | environment:
57 | - NVIDIA_VISIBLE_DEVICES=all
58 | - DISPLAY=${DISPLAY}
59 | - XAUTHORITY=${DOCKER_XAUTH}
60 | volumes:
61 | - /tmp/.X11-unix:/tml/.X11-unix:rw
62 | - ~/.Xauthority:/root/.Xauthority
63 | #- ${DOCKER_XAUTH}:${DOCKER_XAUTH}
64 | - ./results:/root/calibration_ws/results
65 | - ./config:/root/calibration_ws/src/applications/camera_calibration/config
66 | - ./../../cameras:/root/calibration_ws/src/cameras
67 | network_mode: "host"
68 | runtime: nvidia
69 | command: python3 ./src/cameras/moveit2_camera_calibration/moveit2_camera_calibration/camera_calibration_app.py
70 |
71 |
--------------------------------------------------------------------------------
/src/applications/camera_calibration/deploy_franka.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | franka_control_server:
5 | image: ghcr.io/peterdavidfagan/panda_control:humble
6 | container_name: franka_control_server
7 | build:
8 | context: ../../../../
9 | dockerfile: .docker/control/Dockerfile.control
10 | devices:
11 | - "/dev:/dev"
12 | privileged: true
13 | cap_add:
14 | - SYS_NICE
15 | ulimits:
16 | rtprio: 70
17 | rttime: -1 # corresponds to 'unlimited'
18 | memlock: 8428281856
19 | network_mode: "host"
20 | command: ros2 launch panda_control_demos franka.launch.py use_fake_hardware:=false
21 |
22 |
--------------------------------------------------------------------------------
/src/applications/camera_calibration/deploy_robotiq.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | robotiq_control_server:
5 | image: ghcr.io/peterdavidfagan/panda_control:humble
6 | container_name: robotiq_control_server
7 | build:
8 | context: ../../../../
9 | dockerfile: .docker/control/Dockerfile.control
10 | devices:
11 | - "/dev:/dev"
12 | privileged: true
13 | cap_add:
14 | - SYS_NICE
15 | ulimits:
16 | rtprio: 70
17 | rttime: -1 # corresponds to 'unlimited'
18 | memlock: 8428281856
19 | network_mode: "host"
20 | command: ros2 launch panda_control_demos robotiq.launch.py use_fake_hardware:=false
21 |
--------------------------------------------------------------------------------
/src/applications/camera_calibration/results/.gitkeep:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/src/applications/camera_calibration/results/.gitkeep
--------------------------------------------------------------------------------
/src/applications/llm_inference/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.22)
2 | project(panda_llm_inference_demos)
3 |
4 | find_package(ament_cmake REQUIRED)
5 |
6 | install(PROGRAMS
7 | src/llm_client.py
8 | DESTINATION lib/${PROJECT_NAME}
9 | )
10 |
11 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
12 |
13 | ament_package()
14 |
--------------------------------------------------------------------------------
/src/applications/llm_inference/README.md:
--------------------------------------------------------------------------------
1 |
2 |
--------------------------------------------------------------------------------
/src/applications/llm_inference/launch/llm_client.launch.py:
--------------------------------------------------------------------------------
1 | from launch import LaunchDescription
2 | from launch_param_builder import ParameterBuilder
3 | from launch_ros.actions import Node
4 | from launch.actions import ExecuteProcess, RegisterEventHandler, TimerAction
5 | from launch.event_handlers import OnProcessStart
6 |
7 | def generate_launch_description():
8 |
9 | # launch ollama inference server
10 | start_ollama_server = ExecuteProcess(
11 | cmd=["ros2", "launch", "ros2_dspy", "ollama_inference.launch.py"],
12 | output="screen"
13 | )
14 |
15 | llm_client_node = Node(
16 | package="panda_llm_inference_demos",
17 | executable="llm_client.py",
18 | output="both",
19 | )
20 |
21 | return LaunchDescription([
22 | start_ollama_server,
23 | RegisterEventHandler(
24 | OnProcessStart(
25 | target_action=start_ollama_server,
26 | on_start=[
27 | TimerAction(
28 | period=20.0,
29 | actions=[llm_client_node]
30 | )]
31 | )
32 | )
33 | ])
34 |
--------------------------------------------------------------------------------
/src/applications/llm_inference/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | panda_llm_inference_demos
5 | 0.0.0
6 | TODO: Package description
7 | Peter David Fagan
8 | TODO: License declaration
9 |
10 | ament_cmake
11 |
12 | backward_ros
13 |
14 | rviz2
15 | xacro
16 | urdf
17 | launch
18 | launch_ros
19 | tf2_ros
20 | joint_state_publisher
21 | robot_state_publisher
22 | controller_manager
23 | joint_trajectory_controller
24 | joint_state_broadcaster
25 | moveit_ros_move_group
26 | jupyter-notebook
27 |
28 | ament_lint_auto
29 | ament_lint_common
30 |
31 |
32 | ament_cmake
33 |
34 |
35 |
--------------------------------------------------------------------------------
/src/applications/llm_inference/src/llm_client.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python3
2 | """Demonstrating policy deployment for a langauge-conditioned policy."""
3 |
4 | import rclpy
5 | from rclpy.action import ActionClient
6 | from rclpy.node import Node
7 | import numpy as np
8 | from cv_bridge import CvBridge
9 |
10 | from geometry_msgs.msg import PoseStamped
11 | from moveit_msgs.srv import ServoCommandType
12 | from ros2_dspy_msgs.action import QA
13 | from std_srvs.srv import SetBool
14 | from ament_index_python.packages import get_package_share_directory
15 |
16 | import onnx
17 | import onnxruntime as ort
18 |
19 |
20 | from panda_policy_deployment_demos.panda_policy_deployment_demos_parameters import policy as params
21 |
22 | class LLMClient(Node):
23 | """Dummy policy for testing."""
24 |
25 | def __init__(self):
26 | super().__init__("llm_client")
27 | self._logger.info("Language-Conditioned policy initialized")
28 |
29 | # initialize llm action client
30 | self.llm_client = ActionClient(self, QA, "/ollama_predict")
31 |
32 | # store current response
33 | self.current_response = None
34 |
35 | def reason(self, language_command):
36 | """Reason about the language command."""
37 | # construct question
38 | QA_goal = QA.Goal()
39 | QA_goal.question = language_command
40 | self._logger.info(f"Asking: {language_command}")
41 |
42 | # send question to llm and reset current command based on response
43 | self.llm_client.wait_for_server()
44 | self.goal_future = self.llm_client.send_goal_async(QA_goal)
45 | self.goal_future.add_done_callback(self.reason_response_callback)
46 |
47 | def reason_response_callback(self, future):
48 | """Callback for the reason response."""
49 | goal_handle = future.result()
50 | if not goal_handle.accepted:
51 | self._logger.error("Goal rejected :(")
52 | return
53 |
54 | self._logger.info("Goal accepted")
55 | get_result_future = goal_handle.get_result_async()
56 | get_result_future.add_done_callback(self.reason_result_callback)
57 |
58 | def reason_result_callback(self, future):
59 | """Callback for the reason result."""
60 | result = future.result().result
61 | self.logger.info(f"Reasoned command: {result}")
62 | self.current_response = result
63 |
64 |
65 | def main():
66 | rclpy.init()
67 | logger = rclpy.logging.get_logger("policy_deployment")
68 |
69 | # try a basic language command
70 | client = LLMClient()
71 | client.reason("How should I grasp a cup?")
72 | response = client.current_response
73 |
74 | client.destroy_node()
75 | rclpy.shutdown()
76 |
77 |
78 | if __name__ == "__main__":
79 | main()
80 |
--------------------------------------------------------------------------------
/src/applications/motion_planning/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.22)
2 | project(panda_motion_planning_demos)
3 |
4 | find_package(ament_cmake REQUIRED)
5 |
6 | install(PROGRAMS
7 | src/motion_planning.py
8 | src/motion_planning_planning_scene.py
9 | src/scripted_pick_place.py
10 | DESTINATION lib/${PROJECT_NAME}
11 | )
12 |
13 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
14 | install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
15 | install(DIRECTORY notebooks DESTINATION share/${PROJECT_NAME})
16 |
17 | ament_package()
18 |
--------------------------------------------------------------------------------
/src/applications/motion_planning/README.md:
--------------------------------------------------------------------------------
1 | # Motion Planning Tutorials
2 |
--------------------------------------------------------------------------------
/src/applications/motion_planning/config/moveit_cpp.yaml:
--------------------------------------------------------------------------------
1 | planning_scene_monitor_options:
2 | name: "planning_scene_monitor"
3 | robot_description: "robot_description"
4 | joint_state_topic: "/joint_states"
5 | attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
6 | publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene"
7 | monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
8 | wait_for_initial_state_timeout: 10.0
9 |
10 | planning_pipelines:
11 | pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp"]
12 |
13 | plan_request_params:
14 | planning_attempts: 1
15 | planning_pipeline: pilz_industrial_motion_planner
16 | planner_id: "LIN"
17 | planner_time: 1.0
18 | max_velocity_scaling_factor: 0.1
19 | max_acceleration_scaling_factor: 0.1
20 |
21 | pilz_lin:
22 | plan_request_params:
23 | planning_attempts: 1
24 | planning_pipeline: pilz_industrial_motion_planner
25 | planner_id: "LIN"
26 | max_velocity_scaling_factor: 0.1
27 | max_acceleration_scaling_factor: 0.1
28 | planning_time: 0.8
29 |
30 | pilz_ptp:
31 | plan_request_params:
32 | planning_attempts: 1
33 | planning_pipeline: pilz_industrial_motion_planner
34 | planner_id: "PTP"
35 | max_velocity_scaling_factor: 0.1
36 | max_acceleration_scaling_factor: 0.1
37 | planning_time: 0.8
38 |
39 | ompl_rrtc:
40 | plan_request_params:
41 | planning_attempts: 1
42 | planning_pipeline: ompl
43 | planner_id: "RRTConnectkConfigDefault"
44 | max_velocity_scaling_factor: 0.1
45 | max_acceleration_scaling_factor: 0.1
46 | planning_time: 0.8
--------------------------------------------------------------------------------
/src/applications/motion_planning/config/moveit_cpp_mujoco.yaml:
--------------------------------------------------------------------------------
1 | planning_scene_monitor_options:
2 | name: "planning_scene_monitor"
3 | robot_description: "robot_description"
4 | joint_state_topic: "/mujoco_joint_states"
5 | attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
6 | publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene"
7 | monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
8 | wait_for_initial_state_timeout: 10.0
9 |
10 | planning_pipelines:
11 | pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp"]
12 |
13 | plan_request_params:
14 | planning_attempts: 1
15 | planning_pipeline: pilz_industrial_motion_planner
16 | planner_id: "LIN"
17 | planner_time: 1.0
18 | max_velocity_scaling_factor: 0.1
19 | max_acceleration_scaling_factor: 0.1
20 |
21 | pilz_lin:
22 | plan_request_params:
23 | planning_attempts: 1
24 | planning_pipeline: pilz_industrial_motion_planner
25 | planner_id: "LIN"
26 | max_velocity_scaling_factor: 0.1
27 | max_acceleration_scaling_factor: 0.1
28 | planning_time: 0.8
29 |
30 | pilz_ptp:
31 | plan_request_params:
32 | planning_attempts: 1
33 | planning_pipeline: pilz_industrial_motion_planner
34 | planner_id: "PTP"
35 | max_velocity_scaling_factor: 0.1
36 | max_acceleration_scaling_factor: 0.1
37 | planning_time: 0.8
38 |
39 | ompl_rrtc:
40 | plan_request_params:
41 | planning_attempts: 1
42 | planning_pipeline: ompl
43 | planner_id: "RRTConnectkConfigDefault"
44 | max_velocity_scaling_factor: 0.1
45 | max_acceleration_scaling_factor: 0.1
46 | planning_time: 0.8
--------------------------------------------------------------------------------
/src/applications/motion_planning/config/planning_scene.rviz:
--------------------------------------------------------------------------------
1 | Panels:
2 | - Class: rviz_common/Displays
3 | Help Height: 78
4 | Name: Displays
5 | Property Tree Widget:
6 | Expanded:
7 | - /Global Options1
8 | - /Status1
9 | Splitter Ratio: 0.5
10 | Tree Height: 215
11 | - Class: rviz_common/Selection
12 | Name: Selection
13 | - Class: rviz_common/Tool Properties
14 | Expanded:
15 | - /2D Goal Pose1
16 | - /Publish Point1
17 | Name: Tool Properties
18 | Splitter Ratio: 0.5886790156364441
19 | - Class: rviz_common/Views
20 | Expanded:
21 | - /Current View1
22 | Name: Views
23 | Splitter Ratio: 0.5
24 | - Class: rviz_common/Time
25 | Experimental: false
26 | Name: Time
27 | SyncMode: 0
28 | SyncSource: ""
29 | Visualization Manager:
30 | Class: ""
31 | Displays:
32 | - Alpha: 0.5
33 | Cell Size: 1
34 | Class: rviz_default_plugins/Grid
35 | Color: 160; 160; 164
36 | Enabled: true
37 | Line Style:
38 | Line Width: 0.029999999329447746
39 | Value: Lines
40 | Name: Grid
41 | Normal Cell Count: 0
42 | Offset:
43 | X: 0
44 | Y: 0
45 | Z: 0
46 | Plane: XY
47 | Plane Cell Count: 10
48 | Reference Frame:
49 | Value: true
50 | - Class: rviz_default_plugins/Image
51 | Enabled: true
52 | Max Value: 1
53 | Median window: 5
54 | Min Value: 0
55 | Name: Image
56 | Normalize Range: true
57 | Topic:
58 | Depth: 5
59 | Durability Policy: Volatile
60 | History Policy: Keep Last
61 | Reliability Policy: Reliable
62 | Value: /front_camera
63 | Value: true
64 | - Class: rviz_default_plugins/Image
65 | Enabled: true
66 | Max Value: 1
67 | Median window: 5
68 | Min Value: 0
69 | Name: Image
70 | Normalize Range: true
71 | Topic:
72 | Depth: 5
73 | Durability Policy: Volatile
74 | History Policy: Keep Last
75 | Reliability Policy: Reliable
76 | Value: /overhead_camera
77 | Value: true
78 | - Class: moveit_rviz_plugin/PlanningScene
79 | Enabled: true
80 | Move Group Namespace: ""
81 | Name: PlanningScene
82 | Planning Scene Topic: /monitored_planning_scene
83 | Robot Description: robot_description
84 | Scene Geometry:
85 | Scene Alpha: 0.8999999761581421
86 | Scene Color: 50; 230; 50
87 | Scene Display Time: 0.009999999776482582
88 | Show Scene Geometry: true
89 | Voxel Coloring: Z-Axis
90 | Voxel Rendering: Occupied Voxels
91 | Scene Robot:
92 | Attached Body Color: 150; 50; 150
93 | Links:
94 | All Links Enabled: true
95 | Expand Joint Details: false
96 | Expand Link Details: false
97 | Expand Tree: false
98 | Link Tree Style: Links in Alphabetic Order
99 | panda_link0:
100 | Alpha: 1
101 | Show Axes: false
102 | Show Trail: false
103 | Value: true
104 | panda_link0_sc:
105 | Alpha: 1
106 | Show Axes: false
107 | Show Trail: false
108 | Value: true
109 | panda_link1:
110 | Alpha: 1
111 | Show Axes: false
112 | Show Trail: false
113 | Value: true
114 | panda_link1_sc:
115 | Alpha: 1
116 | Show Axes: false
117 | Show Trail: false
118 | Value: true
119 | panda_link2:
120 | Alpha: 1
121 | Show Axes: false
122 | Show Trail: false
123 | Value: true
124 | panda_link2_sc:
125 | Alpha: 1
126 | Show Axes: false
127 | Show Trail: false
128 | Value: true
129 | panda_link3:
130 | Alpha: 1
131 | Show Axes: false
132 | Show Trail: false
133 | Value: true
134 | panda_link3_sc:
135 | Alpha: 1
136 | Show Axes: false
137 | Show Trail: false
138 | Value: true
139 | panda_link4:
140 | Alpha: 1
141 | Show Axes: false
142 | Show Trail: false
143 | Value: true
144 | panda_link4_sc:
145 | Alpha: 1
146 | Show Axes: false
147 | Show Trail: false
148 | Value: true
149 | panda_link5:
150 | Alpha: 1
151 | Show Axes: false
152 | Show Trail: false
153 | Value: true
154 | panda_link5_sc:
155 | Alpha: 1
156 | Show Axes: false
157 | Show Trail: false
158 | Value: true
159 | panda_link6:
160 | Alpha: 1
161 | Show Axes: false
162 | Show Trail: false
163 | Value: true
164 | panda_link6_sc:
165 | Alpha: 1
166 | Show Axes: false
167 | Show Trail: false
168 | Value: true
169 | panda_link7:
170 | Alpha: 1
171 | Show Axes: false
172 | Show Trail: false
173 | Value: true
174 | panda_link7_sc:
175 | Alpha: 1
176 | Show Axes: false
177 | Show Trail: false
178 | Value: true
179 | panda_link8:
180 | Alpha: 1
181 | Show Axes: false
182 | Show Trail: false
183 | Robot Alpha: 1
184 | Show Robot Collision: false
185 | Show Robot Visual: true
186 | Value: true
187 | Enabled: true
188 | Global Options:
189 | Background Color: 48; 48; 48
190 | Fixed Frame: world
191 | Frame Rate: 30
192 | Name: root
193 | Tools:
194 | - Class: rviz_default_plugins/Interact
195 | Hide Inactive Objects: true
196 | - Class: rviz_default_plugins/MoveCamera
197 | - Class: rviz_default_plugins/Select
198 | - Class: rviz_default_plugins/FocusCamera
199 | - Class: rviz_default_plugins/Measure
200 | Line color: 128; 128; 0
201 | - Class: rviz_default_plugins/SetInitialPose
202 | Covariance x: 0.25
203 | Covariance y: 0.25
204 | Covariance yaw: 0.06853891909122467
205 | Topic:
206 | Depth: 5
207 | Durability Policy: Volatile
208 | History Policy: Keep Last
209 | Reliability Policy: Reliable
210 | Value: /initialpose
211 | - Class: rviz_default_plugins/SetGoal
212 | Topic:
213 | Depth: 5
214 | Durability Policy: Volatile
215 | History Policy: Keep Last
216 | Reliability Policy: Reliable
217 | Value: /goal_pose
218 | - Class: rviz_default_plugins/PublishPoint
219 | Single click: true
220 | Topic:
221 | Depth: 5
222 | Durability Policy: Volatile
223 | History Policy: Keep Last
224 | Reliability Policy: Reliable
225 | Value: /clicked_point
226 | Transformation:
227 | Current:
228 | Class: rviz_default_plugins/TF
229 | Value: true
230 | Views:
231 | Current:
232 | Class: rviz_default_plugins/Orbit
233 | Distance: 4.837461948394775
234 | Enable Stereo Rendering:
235 | Stereo Eye Separation: 0.05999999865889549
236 | Stereo Focal Distance: 1
237 | Swap Stereo Eyes: false
238 | Value: false
239 | Focal Point:
240 | X: 0
241 | Y: 0
242 | Z: 0
243 | Focal Shape Fixed Size: true
244 | Focal Shape Size: 0.05000000074505806
245 | Invert Z Axis: false
246 | Name: Current View
247 | Near Clip Distance: 0.009999999776482582
248 | Pitch: 0.32039839029312134
249 | Target Frame:
250 | Value: Orbit (rviz)
251 | Yaw: 1.0353977680206299
252 | Saved: ~
253 | Window Geometry:
254 | Displays:
255 | collapsed: false
256 | Height: 846
257 | Hide Left Dock: false
258 | Hide Right Dock: false
259 | Image:
260 | collapsed: false
261 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000162000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001a5000000860000002800fffffffb0000000a0049006d0061006700650100000231000000bc0000002800ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000003d000002b0000000a400fffffffa000000010100000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000a0056006900650077007301000003a10000010f0000010000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
262 | Selection:
263 | collapsed: false
264 | Time:
265 | collapsed: false
266 | Tool Properties:
267 | collapsed: false
268 | Views:
269 | collapsed: false
270 | Width: 1200
271 | X: 919
272 | Y: 832
273 |
--------------------------------------------------------------------------------
/src/applications/motion_planning/launch/motion_planning.launch.py:
--------------------------------------------------------------------------------
1 | """
2 | A launch file for running the motion planning python api tutorial
3 | """
4 | import os
5 | import pathlib
6 | from ament_index_python.packages import get_package_share_directory
7 | from launch import LaunchDescription
8 | from launch_ros.actions import Node
9 | from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler, TimerAction
10 | from launch.event_handlers import OnProcessStart
11 | from launch.launch_description_sources import PythonLaunchDescriptionSource
12 | from launch.substitutions import LaunchConfiguration
13 | from moveit_configs_utils import MoveItConfigsBuilder
14 |
15 | FILE_PATH = pathlib.Path(__file__).parent.absolute()
16 | DOCKER_COMPOSE_FILE_PATH = str(FILE_PATH) + "/../../../../../.docker/foxglove/docker-compose-motion-planning.yaml"
17 |
18 | def generate_launch_description():
19 |
20 | # declare parameter for using robot ip
21 | robot_ip = DeclareLaunchArgument(
22 | "robot_ip",
23 | default_value="192.168.106.99",
24 | description="Robot IP",
25 | )
26 |
27 | # declare parameter for using gripper
28 | use_gripper = DeclareLaunchArgument(
29 | "use_gripper",
30 | default_value="true",
31 | description="Use gripper",
32 | )
33 |
34 | # declare parameter for using fake controller
35 | use_fake_hardware = DeclareLaunchArgument(
36 | "use_fake_hardware",
37 | default_value="false",
38 | description="Use fake hardware",
39 | )
40 |
41 |
42 | start_motion_planning_prerequisites = ExecuteProcess(
43 | cmd=["ros2", "launch", "panda_motion_planning_demos", "motion_planning_prerequisites.launch.py", "use_fake_hardware:=false"],
44 | output="screen",
45 | )
46 |
47 | moveit_config = (
48 | MoveItConfigsBuilder(robot_name="panda", package_name="franka_robotiq_moveit_config")
49 | .robot_description(file_path=get_package_share_directory("franka_robotiq_description") + "/urdf/robot.urdf.xacro",
50 | mappings={
51 | "robot_ip": LaunchConfiguration("robot_ip"),
52 | "robotiq_gripper": LaunchConfiguration("use_gripper"),
53 | "use_fake_hardware": LaunchConfiguration("use_fake_hardware"),
54 | })
55 | .robot_description_semantic("config/panda.srdf.xacro")
56 | .trajectory_execution("config/moveit_controllers.yaml")
57 | .moveit_cpp(
58 | file_path=get_package_share_directory("panda_motion_planning_demos")
59 | + "/config/moveit_cpp.yaml"
60 | )
61 | .to_moveit_configs()
62 | )
63 |
64 | moveit_config_dict = moveit_config.to_dict()
65 | moveit_py_node = Node(
66 | name="moveit_py",
67 | package="panda_motion_planning_demos",
68 | executable="motion_planning.py",
69 | output="both",
70 | arguments=[
71 | "--ros-args",
72 | "--log-level",
73 | "info"],
74 | parameters=[
75 | moveit_config_dict,
76 | ],
77 | )
78 |
79 |
80 | return LaunchDescription(
81 | [
82 | # parameters
83 | robot_ip,
84 | use_gripper,
85 | use_fake_hardware,
86 |
87 | # launching processes
88 | start_motion_planning_prerequisites,
89 | RegisterEventHandler(
90 | OnProcessStart(
91 | target_action=start_motion_planning_prerequisites,
92 | on_start=[
93 | TimerAction(
94 | period=5.0,
95 | actions=[
96 | moveit_py_node,
97 | ]
98 | )
99 | ]
100 | )
101 | )
102 | ]
103 | )
104 |
105 |
106 |
--------------------------------------------------------------------------------
/src/applications/motion_planning/launch/motion_planning_prerequisites.launch.py:
--------------------------------------------------------------------------------
1 | """
2 | Launch file for motion planning prerequisites but not motion planning application software.
3 |
4 | This file is used to interactively script motion planning with a jupyter notebook or python scripts.
5 | """
6 |
7 | import os
8 | from launch import LaunchDescription
9 | from launch.conditions import IfCondition
10 | from launch.launch_description_sources import load_python_launch_file_as_module
11 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
12 | from launch_ros.actions import Node, SetParameter
13 | from launch.actions import DeclareLaunchArgument, ExecuteProcess
14 | from ament_index_python.packages import get_package_share_directory
15 | from moveit_configs_utils import MoveItConfigsBuilder
16 |
17 |
18 | def generate_launch_description():
19 |
20 | # declare parameter for using robot ip
21 | robot_ip = DeclareLaunchArgument(
22 | "robot_ip",
23 | default_value="192.168.106.99",
24 | description="Robot IP",
25 | )
26 |
27 | # declare parameter for using gripper
28 | use_gripper = DeclareLaunchArgument(
29 | "use_gripper",
30 | default_value="true",
31 | description="Use gripper",
32 | )
33 |
34 | # declare parameter for using fake controller
35 | use_fake_hardware = DeclareLaunchArgument(
36 | "use_fake_hardware",
37 | default_value="false",
38 | description="Use fake hardware",
39 | )
40 |
41 | moveit_config = (
42 | MoveItConfigsBuilder(robot_name="panda", package_name="franka_robotiq_moveit_config")
43 | .robot_description(file_path=get_package_share_directory("franka_robotiq_description") + "/urdf/robot.urdf.xacro",
44 | mappings={
45 | "robot_ip": LaunchConfiguration("robot_ip"),
46 | "robotiq_gripper": LaunchConfiguration("use_gripper"),
47 | "use_fake_hardware": LaunchConfiguration("use_fake_hardware"),
48 | })
49 | .robot_description_semantic("config/panda.srdf.xacro")
50 | .trajectory_execution("config/moveit_controllers.yaml")
51 | .moveit_cpp(
52 | file_path=get_package_share_directory("panda_motion_planning_demos")
53 | + "/config/moveit_cpp.yaml"
54 | )
55 | .to_moveit_configs()
56 | )
57 |
58 | rviz_config_file = os.path.join(
59 | get_package_share_directory("panda_motion_planning_demos"),
60 | "config",
61 | "planning_scene.rviz",
62 | )
63 |
64 | rviz_node = Node(
65 | package="rviz2",
66 | executable="rviz2",
67 | name="rviz2",
68 | output="log",
69 | arguments=["-d", rviz_config_file],
70 | parameters=[
71 | moveit_config.robot_description,
72 | moveit_config.robot_description_semantic,
73 | ],
74 | )
75 |
76 | static_tf = Node(
77 | package="tf2_ros",
78 | executable="static_transform_publisher",
79 | name="static_transform_publisher",
80 | output="log",
81 | arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
82 | )
83 |
84 | joint_state_publisher = Node(
85 | package='joint_state_publisher',
86 | executable='joint_state_publisher',
87 | name='joint_state_publisher',
88 | output='screen',
89 | parameters=[
90 | {'source_list': ['/panda/joint_states', '/robotiq/joint_states'],
91 | 'rate': 30}],
92 | )
93 |
94 | robot_state_publisher = Node(
95 | package="robot_state_publisher",
96 | executable="robot_state_publisher",
97 | name="robot_state_publisher",
98 | output="both",
99 | parameters=[
100 | moveit_config.robot_description,
101 | ],
102 | )
103 |
104 |
105 | return LaunchDescription(
106 | [
107 | robot_ip,
108 | use_gripper,
109 | use_fake_hardware,
110 | rviz_node,
111 | static_tf,
112 | joint_state_publisher,
113 | robot_state_publisher,
114 | ]
115 | )
116 |
117 |
--------------------------------------------------------------------------------
/src/applications/motion_planning/notebooks/moveit_notebook_tutorial.ipynb:
--------------------------------------------------------------------------------
1 | {
2 | "cells": [
3 | {
4 | "cell_type": "markdown",
5 | "id": "4df8633e",
6 | "metadata": {},
7 | "source": [
8 | "# Introduction\n",
9 | "\n",
10 | "Welcome to this tutorial on using jupyter notebooks with Moveit 2. A great benefit of being able to interact with MoveIt via a Python notebook is the ability to rapidly prototype code. We hope you find this interface intuitive and that you gain value from using MoveIt via Python notebooks.\n",
11 | "\n",
12 | "In this tutorial we will cover the following: \n",
13 | "\n",
14 | "* The required imports to run the notebook\n",
15 | "* A motion planning example\n",
16 | "* A teleoperation example\n",
17 | "\n",
18 | "If you have suggestions or feedback for this tutorial please post an issue on GitHub (https://github.com/ros-planning/moveit2_tutorials) and tag @peterdavidfagan."
19 | ]
20 | },
21 | {
22 | "cell_type": "markdown",
23 | "id": "91901c66",
24 | "metadata": {},
25 | "source": [
26 | "## Imports\n",
27 | "\n",
28 | "Note: to launch this notebook and the nodes it depends on you must first specify a launch file. Details are provided earlier in this tutorial ()."
29 | ]
30 | },
31 | {
32 | "cell_type": "code",
33 | "execution_count": null,
34 | "id": "7bd810f5",
35 | "metadata": {},
36 | "outputs": [],
37 | "source": [
38 | "import time\n",
39 | "\n",
40 | "# generic ros libraries\n",
41 | "import rclpy\n",
42 | "from rclpy.logging import get_logger\n",
43 | "\n",
44 | "# moveit python library\n",
45 | "from moveit.core.robot_state import RobotState\n",
46 | "from moveit.planning import (\n",
47 | " MoveItPy,\n",
48 | " MultiPipelinePlanRequestParameters,\n",
49 | ")\n",
50 | "\n",
51 | "from ament_index_python.packages import get_package_share_directory\n",
52 | "from moveit_configs_utils import MoveItConfigsBuilder\n",
53 | "\n",
54 | "def plan_and_execute(\n",
55 | " robot,\n",
56 | " planning_component,\n",
57 | " logger,\n",
58 | " single_plan_parameters=None,\n",
59 | " multi_plan_parameters=None,\n",
60 | " sleep_time=0.0,\n",
61 | "):\n",
62 | " \"\"\"Helper function to plan and execute a motion.\"\"\"\n",
63 | " # plan to goal\n",
64 | " logger.info(\"Planning trajectory\")\n",
65 | " if multi_plan_parameters is not None:\n",
66 | " plan_result = planning_component.plan(\n",
67 | " multi_plan_parameters=multi_plan_parameters\n",
68 | " )\n",
69 | " elif single_plan_parameters is not None:\n",
70 | " plan_result = planning_component.plan(\n",
71 | " single_plan_parameters=single_plan_parameters\n",
72 | " )\n",
73 | " else:\n",
74 | " plan_result = planning_component.plan()\n",
75 | "\n",
76 | " # execute the plan\n",
77 | " if plan_result:\n",
78 | " logger.info(\"Executing plan\")\n",
79 | " robot_trajectory = plan_result.trajectory\n",
80 | " robot.execute(robot_trajectory, controllers=[])\n",
81 | " else:\n",
82 | " logger.error(\"Planning failed\")\n",
83 | "\n",
84 | " time.sleep(sleep_time)\n",
85 | "\n",
86 | "\n",
87 | "# set params\n",
88 | "robot_ip = \"192.168.106.99\"\n",
89 | "use_gripper = \"true\" \n",
90 | "use_fake_hardware = \"true\" \n",
91 | "\n",
92 | "moveit_config = (\n",
93 | " MoveItConfigsBuilder(robot_name=\"panda\", package_name=\"franka_robotiq_moveit_config\")\n",
94 | " .robot_description(file_path=get_package_share_directory(\"franka_robotiq_description\") + \"/urdf/robot.urdf.xacro\",\n",
95 | " mappings={\n",
96 | " \"robot_ip\": robot_ip,\n",
97 | " \"robotiq_gripper\": use_gripper,\n",
98 | " \"use_fake_hardware\": use_fake_hardware,\n",
99 | " })\n",
100 | " .robot_description_semantic(\"config/panda.srdf.xacro\", \n",
101 | " mappings={\n",
102 | " \"robotiq_gripper\": use_gripper,\n",
103 | " })\n",
104 | " .trajectory_execution(\"config/moveit_controllers.yaml\")\n",
105 | " .moveit_cpp(\n",
106 | " file_path=get_package_share_directory(\"panda_motion_planning_demos\")\n",
107 | " + \"/config/moveit_cpp.yaml\"\n",
108 | " )\n",
109 | " .to_moveit_configs()\n",
110 | " ).to_dict()\n",
111 | "\n"
112 | ]
113 | },
114 | {
115 | "cell_type": "markdown",
116 | "id": "d44015e2",
117 | "metadata": {},
118 | "source": [
119 | "## Setup"
120 | ]
121 | },
122 | {
123 | "cell_type": "code",
124 | "execution_count": null,
125 | "id": "f9d767d3",
126 | "metadata": {},
127 | "outputs": [],
128 | "source": [
129 | "rclpy.init()\n",
130 | "logger = get_logger(\"moveit_py.pose_goal\")\n",
131 | " \n",
132 | "# instantiate MoveItPy instance and get planning component\n",
133 | "panda = MoveItPy(node_name=\"moveit_py\", config_dict=moveit_config)\n",
134 | "panda_arm = panda.get_planning_component(\"panda_arm\")\n",
135 | "logger.info(\"MoveItPy instance created\")"
136 | ]
137 | },
138 | {
139 | "cell_type": "markdown",
140 | "id": "2f72a61e",
141 | "metadata": {},
142 | "source": [
143 | "## Motion Planning Example"
144 | ]
145 | },
146 | {
147 | "cell_type": "code",
148 | "execution_count": null,
149 | "id": "2f590234",
150 | "metadata": {},
151 | "outputs": [],
152 | "source": [
153 | " ###########################################################################\n",
154 | "# Plan 1 - set goal state with RobotState object\n",
155 | "###########################################################################\n",
156 | "\n",
157 | "# instantiate a RobotState instance using the current robot model\n",
158 | "robot_model = panda.get_robot_model()\n",
159 | "robot_state = RobotState(robot_model)\n",
160 | "\n",
161 | "# randomize the robot state\n",
162 | "robot_state.set_to_random_positions()\n",
163 | "\n",
164 | "# set plan start state to current state\n",
165 | "panda_arm.set_start_state_to_current_state()\n",
166 | "\n",
167 | "# set goal state to the initialized robot state\n",
168 | "logger.info(\"Set goal state to the initialized robot state\")\n",
169 | "panda_arm.set_goal_state(robot_state=robot_state)\n",
170 | "\n",
171 | "# plan to goal\n",
172 | "plan_and_execute(panda, panda_arm, logger, sleep_time=3.0)"
173 | ]
174 | },
175 | {
176 | "cell_type": "code",
177 | "execution_count": null,
178 | "id": "08ca9268",
179 | "metadata": {},
180 | "outputs": [],
181 | "source": [
182 | "###########################################################################\n",
183 | "# Plan 2 - set goal state with PoseStamped message\n",
184 | "###########################################################################\n",
185 | "\n",
186 | "# set plan start state to current state\n",
187 | "panda_arm.set_start_state_to_current_state()\n",
188 | "\n",
189 | "# set pose goal with PoseStamped message\n",
190 | "from geometry_msgs.msg import PoseStamped\n",
191 | "\n",
192 | "pose_goal = PoseStamped()\n",
193 | "pose_goal.header.frame_id = \"panda_link0\"\n",
194 | "pose_goal.pose.orientation.w = 1.0\n",
195 | "pose_goal.pose.position.x = 0.28\n",
196 | "pose_goal.pose.position.y = -0.2\n",
197 | "pose_goal.pose.position.z = 0.5\n",
198 | "panda_arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link=\"panda_link8\")\n",
199 | "\n",
200 | "# plan to goal\n",
201 | "plan_and_execute(panda, panda_arm, logger, sleep_time=3.0)"
202 | ]
203 | },
204 | {
205 | "cell_type": "code",
206 | "execution_count": null,
207 | "id": "1b2cba9e",
208 | "metadata": {},
209 | "outputs": [],
210 | "source": []
211 | }
212 | ],
213 | "metadata": {
214 | "kernelspec": {
215 | "display_name": "Python 3 (ipykernel)",
216 | "language": "python",
217 | "name": "python3"
218 | },
219 | "language_info": {
220 | "codemirror_mode": {
221 | "name": "ipython",
222 | "version": 3
223 | },
224 | "file_extension": ".py",
225 | "mimetype": "text/x-python",
226 | "name": "python",
227 | "nbconvert_exporter": "python",
228 | "pygments_lexer": "ipython3",
229 | "version": "3.10.12"
230 | }
231 | },
232 | "nbformat": 4,
233 | "nbformat_minor": 5
234 | }
235 |
--------------------------------------------------------------------------------
/src/applications/motion_planning/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | panda_motion_planning_demos
5 | 0.0.0
6 | TODO: Package description
7 | Peter David Fagan
8 | TODO: License declaration
9 |
10 | ament_cmake
11 |
12 | backward_ros
13 | mujoco_ros
14 | moveit_py
15 | rviz2
16 | xacro
17 | urdf
18 | launch
19 | launch_ros
20 | tf2_ros
21 | joint_state_publisher
22 | robot_state_publisher
23 | controller_manager
24 | joint_trajectory_controller
25 | joint_state_broadcaster
26 | jupyter-notebook
27 |
28 | ament_lint_auto
29 | ament_lint_common
30 |
31 |
32 | ament_cmake
33 |
34 |
35 |
--------------------------------------------------------------------------------
/src/applications/motion_planning/src/motion_planning.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python3
2 | """
3 | A script to outline the fundamentals of the moveit_py motion planning API.
4 | """
5 |
6 | import time
7 |
8 | # generic ros libraries
9 | import rclpy
10 | from rclpy.logging import get_logger
11 |
12 | # moveit python library
13 | from moveit.core.robot_state import RobotState
14 | from moveit.planning import (
15 | MoveItPy,
16 | MultiPipelinePlanRequestParameters,
17 | )
18 |
19 | from ament_index_python.packages import get_package_share_directory
20 | from moveit_configs_utils import MoveItConfigsBuilder
21 |
22 |
23 | def plan_and_execute(
24 | robot,
25 | planning_component,
26 | logger,
27 | single_plan_parameters=None,
28 | multi_plan_parameters=None,
29 | sleep_time=0.0,
30 | ):
31 | """Helper function to plan and execute a motion."""
32 | # plan to goal
33 | logger.info("Planning trajectory")
34 | if multi_plan_parameters is not None:
35 | plan_result = planning_component.plan(
36 | multi_plan_parameters=multi_plan_parameters
37 | )
38 | elif single_plan_parameters is not None:
39 | plan_result = planning_component.plan(
40 | single_plan_parameters=single_plan_parameters
41 | )
42 | else:
43 | plan_result = planning_component.plan()
44 |
45 | # execute the plan
46 | if plan_result:
47 | logger.info("Executing plan")
48 | robot_trajectory = plan_result.trajectory
49 | logger.info("Trajectory: {}".format(robot_trajectory))
50 | robot.execute(robot_trajectory, controllers=[])
51 | else:
52 | logger.error("Planning failed")
53 |
54 | time.sleep(sleep_time)
55 |
56 |
57 | def main():
58 |
59 | ###################################################################
60 | # MoveItPy Setup
61 | ###################################################################
62 | rclpy.init()
63 | logger = get_logger("moveit_py.pose_goal")
64 |
65 | # instantiate MoveItPy instance and get planning component
66 | panda = MoveItPy(node_name="moveit_py")
67 | panda_arm = panda.get_planning_component("panda_arm")
68 | logger.info("MoveItPy instance created")
69 |
70 | ###########################################################################
71 | # Plan 1 - set goal state with RobotState object
72 | ###########################################################################
73 |
74 | # instantiate a RobotState instance using the current robot model
75 | robot_model = panda.get_robot_model()
76 | robot_state = RobotState(robot_model)
77 |
78 | # randomize the robot state
79 | robot_state.set_to_random_positions()
80 |
81 | # set plan start state to current state
82 | panda_arm.set_start_state_to_current_state()
83 |
84 | # set goal state to the initialized robot state
85 | logger.info("Set goal state to the initialized robot state")
86 | panda_arm.set_goal_state(robot_state=robot_state)
87 |
88 | # plan to goal
89 | plan_and_execute(panda, panda_arm, logger, sleep_time=3.0)
90 |
91 | ###########################################################################
92 | # Plan 2 - set goal state with PoseStamped message
93 | ###########################################################################
94 |
95 | # set plan start state to current state
96 | panda_arm.set_start_state_to_current_state()
97 |
98 | # set pose goal with PoseStamped message
99 | from geometry_msgs.msg import PoseStamped
100 |
101 | pose_goal = PoseStamped()
102 | pose_goal.header.frame_id = "panda_link0"
103 | pose_goal.pose.orientation.w = 1.0
104 | pose_goal.pose.position.x = 0.28
105 | pose_goal.pose.position.y = -0.2
106 | pose_goal.pose.position.z = 0.5
107 | panda_arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link="panda_link8")
108 |
109 | # plan to goal
110 | plan_and_execute(panda, panda_arm, logger, sleep_time=3.0)
111 |
112 | ###########################################################################
113 | # Plan 3 - set goal state with constraints
114 | ###########################################################################
115 |
116 | # set plan start state to current state
117 | panda_arm.set_start_state_to_current_state()
118 |
119 | # set constraints message
120 | from moveit.core.kinematic_constraints import construct_joint_constraint
121 |
122 | joint_values = {
123 | "panda_joint1": -1.0,
124 | "panda_joint2": 0.7,
125 | "panda_joint3": 0.7,
126 | "panda_joint4": -1.5,
127 | "panda_joint5": -0.7,
128 | "panda_joint6": 2.0,
129 | "panda_joint7": 0.0,
130 | }
131 | robot_state.joint_positions = joint_values
132 | joint_constraint = construct_joint_constraint(
133 | robot_state=robot_state,
134 | joint_model_group=panda.get_robot_model().get_joint_model_group("panda_arm"),
135 | )
136 | panda_arm.set_goal_state(motion_plan_constraints=[joint_constraint])
137 |
138 | # plan to goal
139 | plan_and_execute(panda, panda_arm, logger, sleep_time=3.0)
140 |
141 | if __name__ == "__main__":
142 | main()
143 |
--------------------------------------------------------------------------------
/src/applications/motion_planning/src/motion_planning_planning_scene.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python3
2 | """
3 | Shows how to use a planning scene in MoveItPy to add collision objects and perform collision checking.
4 | """
5 |
6 | import time
7 | import rclpy
8 | from rclpy.logging import get_logger
9 |
10 | from moveit.planning import MoveItPy
11 |
12 | from geometry_msgs.msg import Pose
13 | from moveit_msgs.msg import CollisionObject
14 | from shape_msgs.msg import SolidPrimitive
15 |
16 | from ament_index_python.packages import get_package_share_directory
17 | from moveit_configs_utils import MoveItConfigsBuilder
18 |
19 | def plan_and_execute(
20 | robot,
21 | planning_component,
22 | logger,
23 | sleep_time=0.0,
24 | ):
25 | """Helper function to plan and execute a motion."""
26 | # plan to goal
27 | logger.info("Planning trajectory")
28 | plan_result = planning_component.plan()
29 |
30 | # execute the plan
31 | if plan_result:
32 | logger.info("Executing plan")
33 | robot_trajectory = plan_result.trajectory
34 | robot.execute(robot_trajectory, controllers=[])
35 | else:
36 | logger.error("Planning failed")
37 |
38 | time.sleep(sleep_time)
39 |
40 |
41 | def add_collision_objects(planning_scene_monitor):
42 | """Helper function that adds collision objects to the planning scene."""
43 | object_positions = [
44 | (0.15, 0.1, 0.5),
45 | (0.25, 0.0, 1.0),
46 | (-0.25, -0.3, 0.8),
47 | (0.25, 0.3, 0.75),
48 | ]
49 | object_dimensions = [
50 | (0.1, 0.4, 0.1),
51 | (0.1, 0.4, 0.1),
52 | (0.2, 0.2, 0.2),
53 | (0.15, 0.15, 0.15),
54 | ]
55 |
56 | with planning_scene_monitor.read_write() as scene:
57 | collision_object = CollisionObject()
58 | collision_object.header.frame_id = "panda_link0"
59 | collision_object.id = "boxes"
60 |
61 | for position, dimensions in zip(object_positions, object_dimensions):
62 | box_pose = Pose()
63 | box_pose.position.x = position[0]
64 | box_pose.position.y = position[1]
65 | box_pose.position.z = position[2]
66 |
67 | box = SolidPrimitive()
68 | box.type = SolidPrimitive.BOX
69 | box.dimensions = dimensions
70 |
71 | collision_object.primitives.append(box)
72 | collision_object.primitive_poses.append(box_pose)
73 | collision_object.operation = CollisionObject.ADD
74 |
75 | scene.apply_collision_object(collision_object)
76 | scene.current_state.update() # Important to ensure the scene is updated
77 |
78 |
79 | def main():
80 | ###################################################################
81 | # MoveItPy Setup
82 | ###################################################################
83 | rclpy.init()
84 | logger = get_logger("moveit_py_planning_scene")
85 |
86 | # instantiate MoveItPy instance and get planning component
87 | panda = MoveItPy(node_name="moveit_py_planning_scene")
88 | panda_arm = panda.get_planning_component("panda_arm")
89 | planning_scene_monitor = panda.get_planning_scene_monitor()
90 | logger.info("MoveItPy instance created")
91 |
92 | ###################################################################
93 | # Plan with collision objects
94 | ###################################################################
95 |
96 | add_collision_objects(planning_scene_monitor)
97 | panda_arm.set_start_state_to_current_state()
98 | panda_arm.set_goal_state(configuration_name="home")
99 | plan_and_execute(panda, panda_arm, logger, sleep_time=3.0)
100 |
101 | ###################################################################
102 | # Check collisions
103 | ###################################################################
104 | with planning_scene_monitor.read_only() as scene:
105 | robot_state = scene.current_state
106 | original_joint_positions = robot_state.get_joint_group_positions("panda_arm")
107 |
108 | # Set the pose goal
109 | pose_goal = Pose()
110 | pose_goal.position.x = 0.25
111 | pose_goal.position.y = 0.25
112 | pose_goal.position.z = 0.5
113 | pose_goal.orientation.w = 1.0
114 |
115 | # Set the robot state and check collisions
116 | robot_state.set_from_ik("panda_arm", pose_goal, "panda_link8")
117 | robot_state.update() # required to update transforms
118 | robot_collision_status = scene.is_state_colliding(
119 | robot_state=robot_state, joint_model_group_name="panda_arm", verbose=True
120 | )
121 | logger.info(f"\nRobot is in collision: {robot_collision_status}\n")
122 |
123 | # Restore th original state
124 | robot_state.set_joint_group_positions(
125 | "panda_arm",
126 | original_joint_positions,
127 | )
128 | robot_state.update() # required to update transforms
129 |
130 | time.sleep(3.0)
131 |
132 | ###################################################################
133 | # Remove collision objects and return to the ready pose
134 | ###################################################################
135 |
136 | with planning_scene_monitor.read_write() as scene:
137 | scene.remove_all_collision_objects()
138 | scene.current_state.update()
139 |
140 | panda_arm.set_start_state_to_current_state()
141 | panda_arm.set_goal_state(configuration_name="ready")
142 | plan_and_execute(panda, panda_arm, logger, sleep_time=3.0)
143 |
144 |
145 | if __name__ == "__main__":
146 | main()
147 |
--------------------------------------------------------------------------------
/src/applications/motion_planning/src/scripted_pick_place.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python3
2 | """
3 | A script to outline the fundamentals of the moveit_py motion planning API.
4 | """
5 |
6 | import time
7 | from copy import deepcopy
8 |
9 | # generic ros libraries
10 | import rclpy
11 | from rclpy.action import ActionClient
12 | from rclpy.node import Node
13 | from rclpy.logging import get_logger
14 |
15 | # moveit python library
16 | from moveit.core.robot_state import RobotState
17 | from moveit.planning import (
18 | MoveItPy,
19 | MultiPipelinePlanRequestParameters,
20 | )
21 |
22 | from ament_index_python.packages import get_package_share_directory
23 | from moveit_configs_utils import MoveItConfigsBuilder
24 | from geometry_msgs.msg import PoseStamped
25 | from control_msgs.action import GripperCommand
26 |
27 |
28 | # define gripper action client
29 | class GripperClient(Node):
30 |
31 | def __init__(self):
32 | super().__init__("gripper_client")
33 | self.gripper_action_client = ActionClient(
34 | self,
35 | GripperCommand,
36 | "/robotiq_position_controller/gripper_cmd"
37 | )
38 |
39 | def close_gripper(self):
40 | goal = GripperCommand.Goal()
41 | goal.command.position = 0.8
42 | goal.command.max_effort = 3.0
43 | self.gripper_action_client.wait_for_server()
44 | return self.gripper_action_client.send_goal_async(goal)
45 |
46 | def open_gripper(self):
47 | goal = GripperCommand.Goal()
48 | goal.command.position = 0.0
49 | goal.command.max_effort = 3.0
50 | self.gripper_action_client.wait_for_server()
51 | return self.gripper_action_client.send_goal_async(goal)
52 |
53 |
54 | def plan_and_execute(
55 | robot,
56 | planning_component,
57 | logger,
58 | single_plan_parameters=None,
59 | multi_plan_parameters=None,
60 | sleep_time=0.0,
61 | ):
62 | """Helper function to plan and execute a motion."""
63 | # plan to goal
64 | logger.info("Planning trajectory")
65 | if multi_plan_parameters is not None:
66 | plan_result = planning_component.plan(
67 | multi_plan_parameters=multi_plan_parameters
68 | )
69 | elif single_plan_parameters is not None:
70 | plan_result = planning_component.plan(
71 | single_plan_parameters=single_plan_parameters
72 | )
73 | else:
74 | plan_result = planning_component.plan()
75 |
76 | # execute the plan
77 | if plan_result:
78 | logger.info("Executing plan")
79 | robot_trajectory = plan_result.trajectory
80 | logger.info("Trajectory: {}".format(robot_trajectory))
81 | robot.execute(robot_trajectory, controllers=[])
82 | else:
83 | logger.error("Planning failed")
84 |
85 | time.sleep(sleep_time)
86 |
87 | def pick_and_place_block(
88 | panda,
89 | panda_arm,
90 | logger,
91 | gripper_client,
92 | pick_pose,
93 | place_pose,
94 | ):
95 | """Helper function to pick and place a block."""
96 | # convert poses to PoseStamped messages
97 | pick_pose_msg = PoseStamped()
98 | pick_pose_msg.header.frame_id = "panda_link0"
99 | pick_pose_msg.pose.orientation.x = 0.9238795
100 | pick_pose_msg.pose.orientation.y = -0.3826834
101 | pick_pose_msg.pose.orientation.z = 0.0
102 | pick_pose_msg.pose.orientation.w = 0.0
103 | pick_pose_msg.pose.position.x = pick_pose[0]
104 | pick_pose_msg.pose.position.y = pick_pose[1]
105 | pick_pose_msg.pose.position.z = pick_pose[2]
106 |
107 | place_pose_msg = PoseStamped()
108 | place_pose_msg.header.frame_id = "panda_link0"
109 | place_pose_msg.pose.orientation.x = 0.9238795
110 | place_pose_msg.pose.orientation.y = -0.3826834
111 | place_pose_msg.pose.orientation.z = 0.0
112 | place_pose_msg.pose.orientation.w = 0.0
113 | place_pose_msg.pose.position.x = place_pose[0]
114 | place_pose_msg.pose.position.y = place_pose[1]
115 | place_pose_msg.pose.position.z = place_pose[2]
116 |
117 |
118 | # prepick pose
119 | logger.info("Going to prepick pose")
120 | panda_arm.set_start_state_to_current_state()
121 | pre_pick_pose_msg = deepcopy(pick_pose_msg)
122 | pre_pick_pose_msg.pose.position.z += 0.1
123 | panda_arm.set_goal_state(pose_stamped_msg=pre_pick_pose_msg, pose_link="panda_link8")
124 | plan_and_execute(panda, panda_arm, logger, sleep_time=3.0)
125 |
126 | # pick pose
127 | logger.info("Going to pick pose")
128 | panda_arm.set_start_state_to_current_state()
129 | panda_arm.set_goal_state(pose_stamped_msg=pick_pose_msg, pose_link="panda_link8")
130 | plan_and_execute(panda, panda_arm, logger, sleep_time=3.0)
131 |
132 | # close gripper
133 | logger.info("Closing gripper")
134 | gripper_client.close_gripper()
135 | time.sleep(2.0)
136 |
137 | # raise arm
138 | logger.info("Raising arm")
139 | panda_arm.set_start_state_to_current_state()
140 | pre_pick_pose_msg.pose.position.z += 0.2
141 | panda_arm.set_goal_state(pose_stamped_msg=pre_pick_pose_msg, pose_link="panda_link8")
142 | plan_and_execute(panda, panda_arm, logger, sleep_time=3.0)
143 |
144 | # preplace pose
145 | logger.info("Going to preplace pose")
146 | panda_arm.set_start_state_to_current_state()
147 | pre_place_pose_msg = deepcopy(place_pose_msg)
148 | pre_place_pose_msg.pose.position.z += 0.1
149 | panda_arm.set_goal_state(pose_stamped_msg=pre_place_pose_msg, pose_link="panda_link8")
150 | plan_and_execute(panda, panda_arm, logger, sleep_time=3.0)
151 |
152 | # place pose
153 | logger.info("Going to place pose")
154 | panda_arm.set_start_state_to_current_state()
155 | panda_arm.set_goal_state(pose_stamped_msg=place_pose_msg, pose_link="panda_link8")
156 | plan_and_execute(panda, panda_arm, logger, sleep_time=3.0)
157 |
158 | # open gripper
159 | logger.info("Opening gripper")
160 | gripper_client.open_gripper()
161 | time.sleep(2.0)
162 |
163 | # raise arm
164 | logger.info("Raising arm")
165 | panda_arm.set_start_state_to_current_state()
166 | pre_place_pose_msg.pose.position.z += 0.2
167 | panda_arm.set_goal_state(pose_stamped_msg=pre_place_pose_msg, pose_link="panda_link8")
168 | plan_and_execute(panda, panda_arm, logger, sleep_time=3.0)
169 |
170 | def main():
171 |
172 | # for this demo example we will hard code the poses
173 | height_adjustment = 0.175
174 | BLOCK1_POSE = [0.4251066, 0.0881298, height_adjustment]
175 | BLOCK2_POSE = [0.59047847, -0.07463033, height_adjustment]
176 | BLOCK3_POSE = [0.33900857, -0.19225322, height_adjustment]
177 | PLACE1_POSE = [0.5, 0.0, height_adjustment]
178 | PLACE2_POSE = PLACE1_POSE.copy()
179 | PLACE2_POSE[-1] += 0.08
180 | PLACE3_POSE = PLACE1_POSE.copy()
181 | PLACE3_POSE[-1] += 0.13
182 |
183 | rclpy.init()
184 | logger = get_logger("moveit_py.pose_goal")
185 |
186 | panda = MoveItPy(node_name="moveit_py")
187 | panda_arm = panda.get_planning_component("panda_arm")
188 | gripper_client = GripperClient()
189 | logger.info("MoveItPy instance created")
190 |
191 | pick_and_place_block(
192 | panda,
193 | panda_arm,
194 | logger,
195 | gripper_client,
196 | pick_pose=BLOCK1_POSE,
197 | place_pose=PLACE1_POSE,
198 | )
199 |
200 | pick_and_place_block(
201 | panda,
202 | panda_arm,
203 | logger,
204 | gripper_client,
205 | pick_pose=BLOCK2_POSE,
206 | place_pose=PLACE2_POSE,
207 | )
208 |
209 | pick_and_place_block(
210 | panda,
211 | panda_arm,
212 | logger,
213 | gripper_client,
214 | pick_pose=BLOCK3_POSE,
215 | place_pose=PLACE3_POSE,
216 | )
217 |
218 |
219 | if __name__ == "__main__":
220 | main()
221 |
--------------------------------------------------------------------------------
/src/applications/mujoco_object_detection_tests/README.md:
--------------------------------------------------------------------------------
1 | # Introduction
2 |
--------------------------------------------------------------------------------
/src/applications/mujoco_object_detection_tests/mj_franka.py:
--------------------------------------------------------------------------------
1 | import os
2 | import time
3 |
4 | import mujoco
5 | import mujoco.viewer
6 |
7 | import mujoco_ros
8 | from mujoco_ros.franka_env import FrankaEnv
9 |
10 | model_filepath = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'models', 'rearrangement.mjb')
11 |
12 | if __name__=="__main__":
13 | m = mujoco.MjModel.from_binary_path(model_filepath)
14 | d = mujoco.MjData(m)
15 |
16 | env = FrankaEnv(
17 | m,
18 | d,
19 | command_interface="effort",
20 | control_steps=5,
21 | control_timer_freq=1e-2,
22 | )
23 |
24 | # instaniate mujoco_ros environment
25 | with mujoco.viewer.launch_passive(
26 | model=m,
27 | data=d,
28 | show_left_ui=True,
29 | show_right_ui=True,
30 | ) as viewer:
31 |
32 | # run interactive viewer application
33 | while viewer.is_running():
34 | time.sleep(0.05)
35 | env.is_syncing = True
36 | viewer.sync()
37 | env.is_syncing = False
38 |
--------------------------------------------------------------------------------
/src/applications/mujoco_object_detection_tests/models/.gitkeep:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/src/applications/mujoco_object_detection_tests/models/.gitkeep
--------------------------------------------------------------------------------
/src/applications/mujoco_object_detection_tests/start_grounded_dino_service.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | control_server:
5 | image: ghcr.io/peterdavidfagan/panda_object_detection:humble
6 | container_name: object_detection
7 | build:
8 | context: ../../../../
9 | dockerfile: .docker/object_detection/Dockerfile
10 | devices:
11 | - "/dev:/dev"
12 | privileged: true
13 | network_mode: "host"
14 | command: python3 ./src/object_detection/ros2_object_detection_components/ros2_object_detection_components/grounded_dino.py
15 |
--------------------------------------------------------------------------------
/src/applications/mujoco_ros2_controller_tests/README.md:
--------------------------------------------------------------------------------
1 | # Introduction
2 | Testing ROS 2 controller implementations in mujoco simulation environments.
3 |
4 | [Screencast from 05-20-2024 10:49:10 AM.webm](https://github.com/peterdavidfagan/ros2_robotics_research_toolkit/assets/42982057/8ff660af-a192-4edb-b30e-4bf38a6e8825)
5 |
6 | # Getting Started
7 |
--------------------------------------------------------------------------------
/src/applications/mujoco_ros2_controller_tests/mj_franka.py:
--------------------------------------------------------------------------------
1 | import os
2 | import time
3 |
4 | import mujoco
5 | import mujoco.viewer
6 |
7 | import mujoco_ros
8 | from mujoco_ros.franka_env import FrankaEnv
9 |
10 | model_filepath = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'models', 'franka_controller_test.mjb')
11 |
12 | if __name__=="__main__":
13 | m = mujoco.MjModel.from_binary_path(model_filepath)
14 | d = mujoco.MjData(m)
15 | with mujoco.viewer.launch_passive(
16 | model=m,
17 | data=d,
18 | show_left_ui=False,
19 | show_right_ui=False,
20 | ) as viewer:
21 | # instaniate mujoco_ros environment
22 | env = FrankaEnv(
23 | m,
24 | d,
25 | command_interface="effort",
26 | control_steps=5,
27 | control_timer_freq=1e-2,
28 | )
29 |
30 | # run interactive viewer application
31 | while viewer.is_running():
32 | time.sleep(0.05)
33 | env.is_syncing = True
34 | viewer.sync()
35 | env.is_syncing = False
36 |
--------------------------------------------------------------------------------
/src/applications/mujoco_ros2_controller_tests/models/.gitkeep:
--------------------------------------------------------------------------------
1 |
2 |
--------------------------------------------------------------------------------
/src/applications/mujoco_ros2_controller_tests/start_controller.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | control_server:
5 | image: ghcr.io/peterdavidfagan/panda_control:humble
6 | container_name: control_server
7 | build:
8 | context: ../../../../
9 | dockerfile: .docker/control/Dockerfile.control
10 | devices:
11 | - "/dev:/dev"
12 | privileged: true
13 | cap_add:
14 | - SYS_NICE
15 | ulimits:
16 | rtprio: 70
17 | rttime: -1 # corresponds to 'unlimited'
18 | memlock: 8428281856
19 | network_mode: "host"
20 | command: ros2 launch panda_control_demos franka_robotiq.launch.py use_fake_hardware:=true
21 |
--------------------------------------------------------------------------------
/src/applications/policy_deployment/README.md:
--------------------------------------------------------------------------------
1 | # Policy Deployment Tutorials
2 |
--------------------------------------------------------------------------------
/src/applications/policy_deployment/config/policy.yaml:
--------------------------------------------------------------------------------
1 | policy:
2 | num_sensors: {
3 | type: int,
4 | default_value: 1,
5 | description: "Used for indexing through sensors in this config file.",
6 | }
7 |
8 | sensor_queue: {
9 | type: int,
10 | default_value: 5,
11 | description: "",
12 | }
13 |
14 | sensor_slop: {
15 | type: double,
16 | default_value: 0.2,
17 | description: "",
18 | }
19 |
20 | sensor1:
21 | type: {
22 | type: string,
23 | default_value: "sensor_msgs/Image",
24 | description: "",
25 | }
26 | topic: {
27 | type: string,
28 | default_value: "/front_camera",
29 | description: "",
30 | }
31 | qos: {
32 | type: int,
33 | default_value: 10,
34 | description: "",
35 | }
36 |
37 | command:
38 | type: {
39 | type: string,
40 | default_value: "geometry_msgs/PoseStamped",
41 | description: "",
42 | }
43 | topic: {
44 | type: string,
45 | default_value: "/servo_node/pose_target_cmds",
46 | description: "",
47 | }
48 | qos: {
49 | type: int,
50 | default_value: 10,
51 | description: "",
52 | }
53 |
54 |
--------------------------------------------------------------------------------
/src/applications/policy_deployment/config/transporter_deployment.yaml:
--------------------------------------------------------------------------------
1 | workspace:
2 | table_height_offset: 0.0
3 |
4 | camera:
5 | image_topic: /zed/zed_camera/rgb/image_rect_color
6 | depth_topic: /zed/zed_camera/depth/depth_registered
7 |
8 | intrinsics:
9 | fx: 523.4645385742188
10 | fy: 523.4645385742188
11 | cx: 561.8798828125
12 | cy: 309.3200378417969
13 |
14 | extrinsics:
15 | x: 1.04322964
16 | y: -0.11564952
17 | z: 0.84412144
18 | qx: 0.67835388
19 | qy: 0.63676566
20 | qz: -0.22833043
21 | qw: -0.28675899
22 |
23 | crop:
24 | top_left_u: 395
25 | top_left_v: 220
26 | bottom_right_u: 755
27 | bottom_right_v: 580
--------------------------------------------------------------------------------
/src/applications/policy_deployment/deploy_client.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | zed_camera:
5 | image: ghcr.io/peterdavidfagan/zed2:humble
6 | container_name: zed_camera
7 | build:
8 | context: ../../../../
9 | dockerfile: .docker/cameras/Dockerfile.zed
10 | environment:
11 | - NVIDIA_VISIBLE_DEVICES=all
12 | volumes:
13 | - ./config/zed2i.yaml:/root/zed_ws/src/cameras/zed-ros2-wrapper/zed_wrapper/config/zed2i.yaml
14 | devices:
15 | - "/dev:/dev"
16 | network_mode: "host"
17 | privileged: true
18 | runtime: nvidia
19 | command: ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zed2i node_name:=zed_camera
20 |
21 | motion_planning_prerequisites:
22 | image: ghcr.io/peterdavidfagan/panda_motion_planning:humble
23 | build:
24 | context: ../../../../
25 | dockerfile: .docker/motion_planning/Dockerfile.motion_planning
26 | environment:
27 | - DISPLAY=${DISPLAY}
28 | - XAUTHORITY=${DOCKER_XAUTH}
29 | volumes:
30 | - /tmp/.X11-unix:/tml/.X11-unix:rw
31 | - ${DOCKER_XAUTH}:${DOCKER_XAUTH}
32 | network_mode: "host"
33 | command: ros2 launch panda_motion_planning_demos motion_planning_prerequisites.launch.py
34 |
35 | motion_planning_notebook:
36 | image: ghcr.io/peterdavidfagan/panda_motion_planning:humble
37 | build:
38 | context: ../../../../
39 | dockerfile: .docker/motion_planning/Dockerfile.motion_planning
40 | environment:
41 | - DISPLAY=${DISPLAY}
42 | - XAUTHORITY=${DOCKER_XAUTH}
43 | volumes:
44 | - /tmp/.X11-unix:/tml/.X11-unix:rw
45 | - ${DOCKER_XAUTH}:${DOCKER_XAUTH}
46 | network_mode: "host"
47 | command: python3 -m notebook --allow-root
48 |
49 | camera_calibration_app:
50 | image: ghcr.io/peterdavidfagan/moveit2_camera_calibration:humble
51 | build:
52 | context: ../../../../
53 | dockerfile: .docker/camera_calibration_app/Dockerfile
54 | environment:
55 | - NVIDIA_VISIBLE_DEVICES=all
56 | - DISPLAY=${DISPLAY}
57 | - XAUTHORITY=${DOCKER_XAUTH}
58 | volumes:
59 | - /tmp/.X11-unix:/tml/.X11-unix:rw
60 | - ${DOCKER_XAUTH}:${DOCKER_XAUTH}
61 | - ./results:/root/calibration_ws/src/applications/camera_calibration/results
62 | - ./config:/root/calibration_ws/src/applications/camera_calibration/config
63 | - ../../cameras:/root/calibration_ws/src/cameras
64 | network_mode: "host"
65 | runtime: nvidia
66 | command: python3 ./src/cameras/moveit2_camera_calibration/moveit2_camera_calibration/camera_calibration_app.py
67 |
68 |
--------------------------------------------------------------------------------
/src/applications/policy_deployment/deploy_nuc.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | control_server:
5 | image: ghcr.io/peterdavidfagan/panda_control:humble
6 | container_name: control_server
7 | networks:
8 | - ros_application
9 | build:
10 | context: ../../../../
11 | dockerfile: .docker/control/Dockerfile.control
12 | devices:
13 | - "/dev:/dev"
14 | privileged: true
15 | cap_add:
16 | - SYS_NICE
17 | ulimits:
18 | rtprio: 70
19 | rttime: -1 # corresponds to 'unlimited'
20 | memlock: 8428281856
21 | network_mode: "host"
22 | command: ros2 launch panda_control_demos franka_robotiq.launch.py use_fake_hardware:=false
23 |
24 |
--------------------------------------------------------------------------------
/src/applications/policy_deployment/launch/language_conditioned_policy.launch.py:
--------------------------------------------------------------------------------
1 | from launch import LaunchDescription
2 | from launch_param_builder import ParameterBuilder
3 | from launch_ros.actions import Node
4 |
5 |
6 | def generate_launch_description():
7 |
8 | policy_params = {
9 | "policy_node": ParameterBuilder("panda_policy_deployment_demos")
10 | .yaml("config/policy.yaml")
11 | .to_dict()
12 | }
13 |
14 | policy_node = Node(
15 | package="panda_policy_deployment_demos",
16 | executable="language_conditioned_policy",
17 | parameters=[policy_params],
18 | output="both",
19 | )
20 |
21 | return LaunchDescription([policy_node])
22 |
--------------------------------------------------------------------------------
/src/applications/policy_deployment/launch/policy.launch.py:
--------------------------------------------------------------------------------
1 | from launch import LaunchDescription
2 | from launch_param_builder import ParameterBuilder
3 | from launch_ros.actions import Node
4 |
5 |
6 | def generate_launch_description():
7 |
8 | policy_params = {
9 | "policy_node": ParameterBuilder("panda_policy_deployment_demos")
10 | .yaml("config/policy.yaml")
11 | .to_dict()
12 | }
13 |
14 | policy_node = Node(
15 | package="panda_policy_deployment_demos",
16 | executable="policy_deployment",
17 | parameters=[policy_params],
18 | output="both",
19 | )
20 |
21 | return LaunchDescription([policy_node])
22 |
--------------------------------------------------------------------------------
/src/applications/policy_deployment/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | panda_policy_deployment_demos
5 | 0.0.0
6 | TODO: Package description
7 | Peter David Fagan
8 | TODO: License declaration
9 |
10 | ament_cmake
11 |
12 | generate_parameter_library
13 | ros2_dspy
14 | ros2_dspy_msgs
15 |
16 | rviz2
17 | xacro
18 | urdf
19 | launch
20 | launch_ros
21 | tf2_ros
22 | joint_state_publisher
23 | robot_state_publisher
24 | controller_manager
25 | joint_trajectory_controller
26 | joint_state_broadcaster
27 | moveit_ros_move_group
28 | moveit_py
29 | jupyter-notebook
30 | onnx
31 | jaxonnxruntime
32 | einops
33 |
34 | ament_copyright
35 | ament_flake8
36 | ament_pep257
37 | python3-pytest
38 |
39 |
40 | ament_python
41 |
42 |
43 |
--------------------------------------------------------------------------------
/src/applications/policy_deployment/panda_policy_deployment_demos/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/src/applications/policy_deployment/panda_policy_deployment_demos/__init__.py
--------------------------------------------------------------------------------
/src/applications/policy_deployment/panda_policy_deployment_demos/language_conditioned_policy.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python3
2 | """Demonstrating policy deployment for a langauge-conditioned policy."""
3 |
4 | import rclpy
5 | from rclpy.action import ActionClient
6 | import numpy as np
7 | from cv_bridge import CvBridge
8 |
9 | from moveit.policies import Policy
10 | from geometry_msgs.msg import PoseStamped
11 | from moveit_msgs.srv import ServoCommandType
12 | from ros2_dspy_msgs.action import QA
13 | from std_srvs.srv import SetBool
14 | from ament_index_python.packages import get_package_share_directory
15 |
16 | import onnx
17 | import onnxruntime as ort
18 |
19 |
20 | from panda_policy_deployment_demos.panda_policy_deployment_demos_parameters import policy as params
21 |
22 | class LanguageConditionedPolicy(Policy):
23 | """Dummy policy for testing."""
24 |
25 | def __init__(self):
26 | super().__init__(params)
27 | self._logger.info("Language-Conditioned policy initialized")
28 |
29 | # use CvBridge to convert sensor_msgs/Image to numpy array
30 | self.cv_bridge = CvBridge()
31 |
32 | # keep track of current language command
33 | self.current_command = None
34 |
35 | # start onnx inference session
36 | #model_path = get_package_share_directory("panda_policy_deployment_demos") + "/models/your_model.onnx"
37 | #self.cnn = ort.InferenceSession(model_path)
38 |
39 | # initialize servo service client
40 | self.servo_command_client = self.create_client(ServoCommandType, "/servo_node/switch_command_type")
41 | while not self.servo_command_client.wait_for_service(timeout_sec=1.0):
42 | self.get_logger().info("service not available, waiting again...")
43 | self.servo_pause_client = self.create_client(SetBool, "/servo_node/pause_servo")
44 | while not self.servo_pause_client.wait_for_service(timeout_sec=1.0):
45 | self.get_logger().info("service not available, waiting again...")
46 |
47 | # initialize llm action client
48 | self.llm_client = ActionClient(self, QA, "/ollama_predict")
49 |
50 | def reason(self, language_command):
51 | """Reason about the language command."""
52 | # construct question
53 | QA_goal = QA.Goal()
54 | QA_goal.question = language_command
55 | self._logger.info(f"Asking: {language_command}")
56 |
57 | # send question to llm and reset current command based on response
58 | self.llm_client.wait_for_server()
59 | self.goal_future = self.llm_client.send_goal_async(QA_goal)
60 | self.goal_future.add_done_callback(self.reason_response_callback)
61 |
62 | def reason_response_callback(self, future):
63 | """Callback for the reason response."""
64 | goal_handle = future.result()
65 | if not goal_handle.accepted:
66 | self._logger.error("Goal rejected :(")
67 | return
68 |
69 | self._logger.info("Goal accepted")
70 | get_result_future = goal_handle.get_result_async()
71 | get_result_future.add_done_callback(self.reason_result_callback)
72 |
73 | def reason_result_callback(self, future):
74 | """Callback for the reason result."""
75 | result = future.result().result
76 | self.logger.info(f"Reasoned command: {result}")
77 | self.current_command = result
78 |
79 | def set_servo_command_type(self, command_type):
80 | """Sets the servo command type."""
81 | request = ServoCommandType.Request()
82 | request.command_type = command_type
83 | self.future = self.servo_command_client.call_async(request)
84 | rclpy.spin_until_future_complete(self, self.future)
85 | if self.future.result():
86 | self.get_logger().info("Servo command type set to: %s" % command_type)
87 | else:
88 | self.get_logger().error("Failed to set servo command type: %s" % command_type)
89 |
90 | def forward(self, sensor_msg):
91 | """Forward pass of the policy."""
92 | if (self._is_active) and (self.current_command is not None):
93 | # convert image data to jax array
94 | cv_img = self.cv_bridge.imgmsg_to_cv2(sensor_msg, desired_encoding="rgb8")
95 |
96 | # combine image and langauge command
97 | #input_data = np.concatenate((cv_img, self.current_command), axis=0)
98 |
99 | # perform inference with onnx model
100 | #prediction = self.cnn.run(None, {"input": input_data})
101 |
102 | # For this demo, we will just publish random target poses
103 | target = PoseStamped()
104 | target.header.stamp = self.get_clock().now().to_msg()
105 | target.header.frame_id = "panda_link0"
106 | target.pose.position.x = np.random.uniform(0.3, 0.7)
107 | target.pose.position.y = np.random.uniform(-0.2, 0.2)
108 | target.pose.position.z = np.random.uniform(0.25, 0.55)
109 | target.pose.orientation.w = 0.0
110 | target.pose.orientation.x = 0.924
111 | target.pose.orientation.y = -0.382
112 | target.pose.orientation.z = 0.0
113 | self.command_pub.publish(target)
114 | else:
115 | self._logger.debug("Policy is not active")
116 |
117 |
118 | def main():
119 | rclpy.init()
120 | logger = rclpy.logging.get_logger("policy_deployment")
121 |
122 | policy = LanguageConditionedPolicy()
123 | policy.active = True
124 | policy.set_servo_command_type(ServoCommandType.Request.POSE)
125 |
126 | # try a basic language command
127 | policy.reason("How should I grasp a cup?")
128 |
129 | rclpy.spin(policy)
130 | policy.destroy_node()
131 | rclpy.shutdown()
132 |
133 |
134 | if __name__ == "__main__":
135 | main()
136 |
--------------------------------------------------------------------------------
/src/applications/policy_deployment/panda_policy_deployment_demos/single_image_pose_policy.py:
--------------------------------------------------------------------------------
1 | #!/usr/bin/env python3
2 | """Demonstrating policy deployment for a policy that accepts a single image as input."""
3 |
4 | import rclpy
5 | import numpy as np
6 | from cv_bridge import CvBridge
7 |
8 | from moveit.policies import Policy
9 | from geometry_msgs.msg import PoseStamped
10 | from moveit_msgs.srv import ServoCommandType
11 | from std_srvs.srv import SetBool
12 | from ament_index_python.packages import get_package_share_directory
13 |
14 | import onnx
15 | import onnxruntime as ort
16 |
17 | from panda_policy_deployment_demos.panda_policy_deployment_demos_parameters import policy as params
18 |
19 | class SingleImagePolicy(Policy):
20 | """Dummy policy for testing."""
21 |
22 | def __init__(self):
23 | super().__init__(params)
24 | self._logger.info("Dummy policy initialized")
25 |
26 | # use CvBridge to convert sensor_msgs/Image to numpy array
27 | self.cv_bridge = CvBridge()
28 |
29 | # start onnx inference session
30 | #model_path = get_package_share_directory("panda_policy_deployment_demos") + "/models/your_model.onnx"
31 | #self.cnn = ort.InferenceSession(model_path)
32 |
33 | # initialize servo service client
34 | self.servo_command_client = self.create_client(ServoCommandType, "/servo_node/switch_command_type")
35 | while not self.servo_command_client.wait_for_service(timeout_sec=1.0):
36 | self.get_logger().info("service not available, waiting again...")
37 | self.servo_pause_client = self.create_client(SetBool, "/servo_node/pause_servo")
38 | while not self.servo_pause_client.wait_for_service(timeout_sec=1.0):
39 | self.get_logger().info("service not available, waiting again...")
40 |
41 |
42 | def set_servo_command_type(self, command_type):
43 | """Sets the servo command type."""
44 | request = ServoCommandType.Request()
45 | request.command_type = command_type
46 | self.future = self.servo_command_client.call_async(request)
47 | rclpy.spin_until_future_complete(self, self.future)
48 | if self.future.result():
49 | self.get_logger().info("Servo command type set to: %s" % command_type)
50 | else:
51 | self.get_logger().error("Failed to set servo command type: %s" % command_type)
52 |
53 | def forward(self, sensor_msg):
54 | """Forward pass of the policy."""
55 | if self._is_active:
56 | # convert image data to jax array
57 | cv_img = self.cv_bridge.imgmsg_to_cv2(sensor_msg, desired_encoding="rgb8")
58 | # perform inference with onnx model
59 | #prediction = self.cnn.run(None, {"input": cv_img})
60 |
61 | # For this demo, we will just publish random target poses
62 | target = PoseStamped()
63 | target.header.stamp = self.get_clock().now().to_msg()
64 | target.header.frame_id = "panda_link0"
65 | target.pose.position.x = np.random.uniform(0.3, 0.7)
66 | target.pose.position.y = np.random.uniform(-0.2, 0.2)
67 | target.pose.position.z = np.random.uniform(0.25, 0.55)
68 | target.pose.orientation.w = 0.0
69 | target.pose.orientation.x = 0.924
70 | target.pose.orientation.y = -0.382
71 | target.pose.orientation.z = 0.0
72 | self.command_pub.publish(target)
73 | else:
74 | self._logger.debug("Policy is not active")
75 |
76 |
77 | def main():
78 | rclpy.init()
79 | logger = rclpy.logging.get_logger("policy_deployment")
80 |
81 | policy = SingleImagePolicy()
82 | policy.active = True
83 | policy.set_servo_command_type(ServoCommandType.Request.POSE)
84 |
85 | rclpy.spin(policy)
86 | policy.destroy_node()
87 | rclpy.shutdown()
88 |
89 |
90 | if __name__ == "__main__":
91 | main()
92 |
--------------------------------------------------------------------------------
/src/applications/policy_deployment/panda_policy_deployment_demos/transporter_deployment_app.py:
--------------------------------------------------------------------------------
1 | import os
2 | import sys
3 | import argparse
4 | import yaml
5 | import math
6 | import numpy as np
7 | from ament_index_python.packages import get_package_share_directory
8 |
9 | from scipy.spatial.transform import Rotation as R
10 | from scipy.interpolate import griddata
11 | import scipy.spatial.transform as st
12 | from PyQt6.QtWidgets import *
13 | from PyQt6.QtGui import *
14 | from PyQt6.QtCore import *
15 |
16 | import rclpy
17 | from rclpy.action import ActionClient
18 | from rclpy.node import Node
19 | from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup
20 | from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
21 |
22 | from cv_bridge import CvBridge
23 | from moveit2_policy_msgs.action import Transporter
24 | from sensor_msgs.msg import Image, CameraInfo
25 | import message_filters
26 |
27 | import cv2
28 |
29 | from moveit2_data_collector.robot_workspaces.franka_table import FrankaTable
30 | import envlogger
31 | from envlogger.backends import tfds_backend_writer
32 | import tensorflow as tf
33 | import tensorflow_datasets as tfds
34 |
35 |
36 | class TransporterActionClient(Node):
37 |
38 | def __init__(self):
39 | super().__init__('transporter_action_client')
40 |
41 | deployment_param_path = os.path.join(get_package_share_directory("panda_policy_deployment_demos"), "config", "transporter_deployment.yaml")
42 | with open(deployment_param_path, 'r') as f:
43 | self.config = yaml.load(f, Loader=yaml.SafeLoader)
44 |
45 | self.cv_bridge = CvBridge()
46 | self.action_client = ActionClient(self, Transporter, 'transporter')
47 |
48 | self.rgb=None
49 | self.depth=None
50 | self.goal_success=False
51 |
52 | def send_goal(self):
53 | # compose goal
54 | goal = Transporter.Goal()
55 |
56 | # request goal from action server
57 | self.action_client.wait_for_server()
58 | self.goal_future = self.action_client.send_goal_async(goal, feedback_callback=self.feedback_callback)
59 | self.goal_future.add_done_callback(self.goal_response_callback)
60 |
61 | def goal_response_callback(self, future):
62 | goal_handle = future.result()
63 | if not goal_handle.accepted:
64 | self.get_logger().info('Goal rejected :(')
65 | return
66 |
67 | self.get_logger().info('Goal accepted :)')
68 |
69 | self._get_result_future = goal_handle.get_result_async()
70 | self._get_result_future.add_done_callback(self.get_result_callback)
71 |
72 | def get_result_callback(self, future):
73 | result = future.result().result
74 | self.get_logger().info('Result: {0}'.format(result.success))
75 | self.goal_success = True
76 |
77 | def feedback_callback(self, feedback_msg):
78 | feedback = feedback_msg.feedback
79 | self.get_logger().info('Received feedback: {0}'.format(feedback.status))
80 |
81 |
82 | def main(args=None):
83 | rclpy.init(args=args)
84 | action_client = TransporterActionClient()
85 | while True:
86 | action_client.send_goal()
87 | rclpy.spin_once(action_client)
88 |
89 | if __name__ == '__main__':
90 | main()
91 |
--------------------------------------------------------------------------------
/src/applications/policy_deployment/resource/panda_policy_deployment_demos:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/src/applications/policy_deployment/resource/panda_policy_deployment_demos
--------------------------------------------------------------------------------
/src/applications/policy_deployment/setup.cfg:
--------------------------------------------------------------------------------
1 | [develop]
2 | script-dir=$base/lib/panda_policy_deployment_demos
3 | [install]
4 | install-scripts=$base/lib/panda_policy_deployment_demos
5 |
--------------------------------------------------------------------------------
/src/applications/policy_deployment/setup.py:
--------------------------------------------------------------------------------
1 | from setuptools import setup
2 |
3 | package_name = 'panda_policy_deployment_demos'
4 |
5 | setup(
6 | name=package_name,
7 | version='0.0.0',
8 | packages=[package_name],
9 | data_files=[
10 | ('share/ament_index/resource_index/packages',
11 | ['resource/' + package_name]),
12 | ('share/' + package_name, ['package.xml']),
13 | ('share/' + package_name + '/launch', ['launch/policy.launch.py', 'launch/language_conditioned_policy.launch.py']),
14 | ('share/' + package_name + '/config', ['config/policy.yaml', 'config/transporter_deployment.yaml']),
15 | ],
16 | install_requires=['setuptools'],
17 | zip_safe=True,
18 | maintainer='Peter David Fagan',
19 | maintainer_email='peterdavidfagan@gmail.com',
20 | description='TODO: Package description',
21 | license='TODO: License declaration',
22 | tests_require=['pytest'],
23 | entry_points={
24 | 'console_scripts': [
25 | 'policy_deployment = panda_policy_deployment_demos.single_image_pose_policy:main',
26 | 'language_conditioned_policy = panda_policy_deployment_demos.language_conditioned_policy:main'
27 | ],
28 | },
29 | )
30 |
31 | from generate_parameter_library_py.setup_helper import generate_parameter_module
32 |
33 | generate_parameter_module(
34 | "panda_policy_deployment_demos_parameters", # python module name for parameter library
35 | "config/policy.yaml", # path to input yaml file
36 | )
37 |
--------------------------------------------------------------------------------
/src/applications/transporter_data_collection/README.md:
--------------------------------------------------------------------------------
1 | [](https://colab.research.google.com/drive/1Zu_bj0xQGbxMKLTK0X1SR-WrK71Iv5-B?usp=sharing)
2 |
3 | # Transporter Data Collection
4 |
5 | https://github.com/peterdavidfagan/moveit2_data_collector/assets/42982057/f2a3e4ac-b8eb-466c-b17f-bdd2d8fa688b
6 |
7 | This application is deployed via Docker containers that are run on the Intel NUC and client machine.
8 |
9 | To run the control server run:
10 |
11 | ```
12 | docker compose -f deploy_nuc.yaml up
13 | ```
14 |
15 | To run the camera calibration app, cameras and motion planning software run:
16 |
17 | ```
18 | docker compose -f deploy_client.yaml up
19 | ```
20 |
21 | # Published Datasets
22 |
23 | An example dataset which was collected using this software can be found [here](https://huggingface.co/datasets/peterdavidfagan/transporter_networks).
24 |
--------------------------------------------------------------------------------
/src/applications/transporter_data_collection/config/application_params.yaml:
--------------------------------------------------------------------------------
1 | workspace:
2 | table_height_offset: 0.0
3 |
4 | camera:
5 | image_topic: /zed/zed_camera/rgb/image_rect_color
6 | depth_topic: /zed/zed_camera/depth/depth_registered
7 |
8 | intrinsics:
9 | fx: 523.4645385742188
10 | fy: 523.4645385742188
11 | cx: 561.8798828125
12 | cy: 309.3200378417969
13 |
14 | extrinsics:
15 | x: 1.04322964
16 | y: -0.11564952
17 | z: 0.84412144
18 | qx: 0.67835388
19 | qy: 0.63676566
20 | qz: -0.22833043
21 | qw: -0.28675899
22 |
--------------------------------------------------------------------------------
/src/applications/transporter_data_collection/config/zed2i.yaml:
--------------------------------------------------------------------------------
1 | # config/zed2i_yaml
2 | # Parameters for Stereoabs zed2i camera
3 | ---
4 | /**:
5 | ros__parameters:
6 | general:
7 | camera_model: 'zed2i'
8 | camera_name: 'zed2i' # usually overwritten by launch file
9 | serial_number: 35215462
10 | grab_resolution: 'HD2K' # The native camera grab resolution. 'HD2K', 'HD1080', 'HD720', 'VGA', 'AUTO'
11 | grab_frame_rate: 30 # ZED SDK internal grabbing rate
12 | pub_frame_rate: 5.0
13 |
14 | depth:
15 | depth_mode: 'ULTRA'
16 | depth_confidence: 50
17 | min_depth: 0.2 # Min: 0.2, Max: 3.0
18 | max_depth: 10.0 # Max: 40.0
19 |
20 | pos_tracking:
21 | set_as_static: true
22 | initial_base_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Initial position of the `camera_link` frame in the map -> [X, Y, Z, R, P, Y]
23 |
24 |
--------------------------------------------------------------------------------
/src/applications/transporter_data_collection/data/dataset_info.json:
--------------------------------------------------------------------------------
1 | {
2 | "fileFormat": "tfrecord",
3 | "name": "transport_cubes",
4 | "splits": [
5 | {
6 | "filepathTemplate": "{DATASET}-{SPLIT}.{FILEFORMAT}-{SHARD_INDEX}",
7 | "name": "train",
8 | "numBytes": "75030455",
9 | "shardLengths": [
10 | "1",
11 | "1"
12 | ]
13 | }
14 | ],
15 | "version": "0.0.1"
16 | }
--------------------------------------------------------------------------------
/src/applications/transporter_data_collection/data/transport_cubes-train.tfrecord-00000:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/src/applications/transporter_data_collection/data/transport_cubes-train.tfrecord-00000
--------------------------------------------------------------------------------
/src/applications/transporter_data_collection/data/transport_cubes-train.tfrecord-00001:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/src/applications/transporter_data_collection/data/transport_cubes-train.tfrecord-00001
--------------------------------------------------------------------------------
/src/applications/transporter_data_collection/deploy_client.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | zed_camera:
5 | image: ghcr.io/peterdavidfagan/zed2:humble
6 | build:
7 | context: ../../../../
8 | dockerfile: .docker/cameras/Dockerfile.zed
9 | environment:
10 | - NVIDIA_VISIBLE_DEVICES=all
11 | volumes:
12 | - ./config/zed2i.yaml:/root/zed_ws/src/cameras/zed_wrapper/config/zed2i.yaml
13 | devices:
14 | - "/dev:/dev"
15 | network_mode: "host"
16 | privileged: true
17 | runtime: nvidia
18 | command: ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zed2i node_name:=zed_camera
19 |
20 | motion_planning_prerequisites:
21 | image: ghcr.io/peterdavidfagan/panda_motion_planning:humble
22 | build:
23 | context: ../../../../
24 | dockerfile: .docker/motion_planning/Dockerfile.motion_planning
25 | environment:
26 | - DISPLAY=${DISPLAY}
27 | - XAUTHORITY=${DOCKER_XAUTH}
28 | volumes:
29 | - /tmp/.X11-unix:/tml/.X11-unix:rw
30 | #- ${DOCKER_XAUTH}:${DOCKER_XAUTH}
31 | - ../../applications:/root/panda_ws/src/applications
32 | network_mode: "host"
33 | command: ros2 launch panda_motion_planning_demos motion_planning_prerequisites.launch.py
34 |
35 | transporter_data_collection_app:
36 | image: ghcr.io/peterdavidfagan/moveit2_data_collection:humble
37 | build:
38 | context: ../../../../
39 | dockerfile: .docker/transporter_data_collection_app/Dockerfile
40 | environment:
41 | - NVIDIA_VISIBLE_DEVICES=all
42 | - DISPLAY=${DISPLAY}
43 | #- XAUTHORITY=${DOCKER_XAUTH}
44 | volumes:
45 | - /tmp/.X11-unix:/tml/.X11-unix:rw
46 | #- ${DOCKER_XAUTH}:${DOCKER_XAUTH}
47 | - ./config:/root/data_collection_ws/src/applications/transporter_data_collection/config
48 | - ./../../data_collection:/root/data_collection_ws/src/data_collection
49 | - ./../../motion_planning:/root/data_collection_ws/src/motion_planning
50 | - ./../../applications:/root/data_collection_ws/src/applications
51 | - ./data:/root/data_collection_ws/src/data_collection/moveit2_data_collector/moveit2_data_collector/data
52 | network_mode: "host"
53 | runtime: nvidia
54 | command: python3 src/data_collection/moveit2_data_collector/moveit2_data_collector/transporter_data_collection_app.py
55 |
56 |
--------------------------------------------------------------------------------
/src/applications/transporter_data_collection/deploy_nuc.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | control_server:
5 | image: ghcr.io/peterdavidfagan/panda_control:humble
6 | container_name: control_server
7 | build:
8 | context: ../../../../
9 | dockerfile: .docker/control/Dockerfile.control
10 | devices:
11 | - "/dev:/dev"
12 | privileged: true
13 | cap_add:
14 | - SYS_NICE
15 | ulimits:
16 | rtprio: 70
17 | rttime: -1 # corresponds to 'unlimited'
18 | memlock: 8428281856
19 | network_mode: "host"
20 | command: ros2 launch panda_control_demos franka_robotiq.launch.py use_fake_hardware:=false
21 |
22 |
--------------------------------------------------------------------------------
/src/applications/transporter_data_collection/hf_data_upload.py:
--------------------------------------------------------------------------------
1 | import os
2 | import tarfile
3 |
4 | import tensorflow_datasets as tfds
5 | from huggingface_hub import HfApi
6 |
7 | LOCAL_FILEPATH="/home/robot/Code/ros2_robotics_research_toolkit/src/applications/transporter_data_collection/data"
8 | OUTPUT_FILENAME="data.tar.xz"
9 | COMPRESSED_LOCAL_FILEPATH="/home/robot/Code/ros2_robotics_research_toolkit/src/applications/transporter_data_collection/data.tar.xz"
10 | REPO_FILEPATH="/data.tar.xz"
11 |
12 | if __name__=="__main__":
13 | # compress the file
14 | with tarfile.open(OUTPUT_FILENAME, "w:xz") as tar:
15 | tar.add(LOCAL_FILEPATH, arcname=".")
16 |
17 | # upload to huggingface
18 | api = HfApi()
19 | api.upload_file(
20 | repo_id="peterdavidfagan/transporter_networks",
21 | repo_type="dataset",
22 | path_or_fileobj=COMPRESSED_LOCAL_FILEPATH,
23 | path_in_repo="/data.tar.xz",
24 | )
--------------------------------------------------------------------------------
/src/applications/transporter_data_collection/load_data_example.py:
--------------------------------------------------------------------------------
1 | import os
2 | import tarfile
3 |
4 | import tensorflow_datasets as tfds
5 | from huggingface_hub import hf_hub_download
6 |
7 | DATA_DIR="/home/robot"
8 | FILENAME="data.tar.xz"
9 | EXTRACTED_FILENAME="data"
10 | FILEPATH=os.path.join(DATA_DIR, FILENAME)
11 | EXTRACTED_FILEPATH=os.path.join(DATA_DIR, EXTRACTED_FILENAME)
12 |
13 | # download data from huggingface
14 | hf_hub_download(
15 | repo_id="peterdavidfagan/transporter_networks",
16 | repo_type="dataset",
17 | filename=FILENAME,
18 | local_dir=DATA_DIR,
19 | )
20 |
21 | # uncompress file
22 | with tarfile.open(FILEPATH, 'r:xz') as tar:
23 | tar.extractall(path=DATA_DIR)
24 | os.remove(FILEPATH)
25 |
26 | # load with tfds
27 | ds = tfds.builder_from_directory(EXTRACTED_FILEPATH).as_dataset()['train']
28 |
29 | # basic inspection of data
30 | print(ds.element_spec)
31 | for eps in ds:
32 | print(eps["extrinsics"])
33 | for step in eps["steps"]:
34 | print(step["is_first"])
35 | print(step["is_last"])
36 | print(step["is_terminal"])
37 | print(step["action"])
38 |
--------------------------------------------------------------------------------
/src/applications/transporter_deployment/MUJOCO_LOG.TXT:
--------------------------------------------------------------------------------
1 | Thu May 30 09:39:14 2024
2 | ERROR: mj_setPtrData: mjData buffer size mismatch, expected size: 2099296, actual size: 355543010346496
3 |
4 |
--------------------------------------------------------------------------------
/src/applications/transporter_deployment/README.md:
--------------------------------------------------------------------------------
1 | # Transporter Model Deployment
2 |
--------------------------------------------------------------------------------
/src/applications/transporter_deployment/config/franka_table.yaml:
--------------------------------------------------------------------------------
1 | workspace:
2 | table_height_offset: 0.0
3 | target_locations:
4 | a_red_apple:
5 | location: [0.55, 0.3, 0.4]
6 | size: [0.075, 0.15, 0.01]
7 | a_green_apple:
8 | location: [0.55, -0.4, 0.4]
9 | size: [0.075, 0.15, 0.01]
10 |
11 | robot:
12 | ip: ''
13 | use_fake_hardware: 'true'
14 | use_gripper: 'true'
15 | gripper_controller: '/robotiq/robotiq_gripper_controller/gripper_cmd'
16 | gripper_tcp_offset: 0.17
17 |
18 | camera:
19 | rgb_topic: '/overhead_camera_rgb'
20 | depth_topic: '/overhead_camera_depth'
21 |
22 | intrinsics:
23 | fx: -407.43914864
24 | fy: 407.43914864
25 | cx: 319.5
26 | cy: 319.5
27 |
28 | extrinsics:
29 | x: 0.7
30 | y: 0.0
31 | z: 1.3
32 | qx: 0.0
33 | qy: 0.0
34 | qz: -0.707
35 | qw: 0.707
36 |
37 | grounded_dino:
38 | confidence_threshold: 0.1
39 | prompts:
40 | - a red apple.
--------------------------------------------------------------------------------
/src/applications/transporter_deployment/config/franka_table_real.yaml:
--------------------------------------------------------------------------------
1 | workspace:
2 | table_height_offset: 0.0
3 | target_locations:
4 | a_red_apple:
5 | location: [0.35, 0.0, 0.3]
6 | size: [0.025, 0.025, 0.01]
7 |
8 | robot:
9 | ip: '192.168.106.99'
10 | use_fake_hardware: 'false'
11 | use_gripper: 'true'
12 | gripper_controller: '/robotiq/robotiq_gripper_controller/gripper_cmd'
13 | gripper_tcp_offset: 0.17
14 |
15 | camera:
16 | rgb_topic: '/zed/zed_camera/rgb/image_rect_color'
17 | depth_topic: '/zed/zed_camera/depth/depth_registered'
18 |
19 | intrinsics:
20 | fx: 523.4645385742188
21 | fy: 523.4645385742188
22 | cx: 561.8798828125
23 | cy: 309.3200378417969
24 |
25 | extrinsics:
26 | x: 1.1357451
27 | y: -0.05703823
28 | z: 0.83214218
29 | qx: 0.64914152
30 | qy: 0.62559443
31 | qz: -0.26746816
32 | qw: -0.34015832
33 |
34 | grounded_dino:
35 | confidence_threshold: 0.1
36 | prompts:
37 | - a red apple.
38 |
--------------------------------------------------------------------------------
/src/applications/transporter_deployment/config/zed2i.yaml:
--------------------------------------------------------------------------------
1 | # config/zed2i_yaml
2 | # Parameters for Stereoabs zed2i camera
3 | ---
4 | /**:
5 | ros__parameters:
6 | general:
7 | camera_model: 'zed2i'
8 | camera_name: 'zed2i' # usually overwritten by launch file
9 | serial_number: 35215462
10 | grab_resolution: 'HD2K' # The native camera grab resolution. 'HD2K', 'HD1080', 'HD720', 'VGA', 'AUTO'
11 | grab_frame_rate: 30 # ZED SDK internal grabbing rate
12 | pub_frame_rate: 5.0
13 |
14 | depth:
15 | depth_mode: 'ULTRA'
16 | depth_confidence: 50
17 | min_depth: 0.2 # Min: 0.2, Max: 3.0
18 | max_depth: 10.0 # Max: 40.0
19 |
20 | pos_tracking:
21 | set_as_static: true
22 | initial_base_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Initial position of the `camera_link` frame in the map -> [X, Y, Z, R, P, Y]
23 |
24 |
--------------------------------------------------------------------------------
/src/applications/transporter_deployment/deploy_client.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | zed_camera:
5 | image: ghcr.io/peterdavidfagan/zed2:humble
6 | build:
7 | context: ../../../../
8 | dockerfile: .docker/cameras/Dockerfile.zed
9 | environment:
10 | - NVIDIA_VISIBLE_DEVICES=all
11 | volumes:
12 | - ./config/zed2i.yaml:/root/zed_ws/src/cameras/zed_wrapper/config/zed2i.yaml
13 | devices:
14 | - "/dev:/dev"
15 | network_mode: "host"
16 | privileged: true
17 | runtime: nvidia
18 | command: ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zed2i node_name:=zed_camera
19 |
20 | motion_planning_prerequisites:
21 | image: ghcr.io/peterdavidfagan/panda_motion_planning:humble
22 | build:
23 | context: ../../../../
24 | dockerfile: .docker/motion_planning/Dockerfile.motion_planning
25 | environment:
26 | - DISPLAY=${DISPLAY}
27 | volumes:
28 | - /tmp/.X11-unix:/tml/.X11-unix:rw
29 | - ../../applications:/root/panda_ws/src/applications
30 | network_mode: "host"
31 | command: ros2 launch panda_motion_planning_demos motion_planning_prerequisites.launch.py
32 |
33 | object_detection:
34 | image: ghcr.io/peterdavidfagan/object_detection:humble
35 | container_name: object_detection
36 | build:
37 | context: ../../
38 | dockerfile: .docker/object_detection/Dockerfile
39 | environment:
40 | - NVIDIA_VISIBLE_DEVICES=all
41 | devices:
42 | - "/dev:/dev"
43 | privileged: true
44 | runtime: nvidia
45 | network_mode: "host"
46 | command: python3 ./src/object_detection/ros2_object_detection/ros2_object_detection_components/ros2_object_detection_components/grounded_dino.py --topic=/zed/zed_camera/rgb/image_rect_color
47 |
48 | transporter_deployment:
49 | image: ghcr.io/peterdavidfagan/transporter_deployment:humble
50 | build:
51 | context: ../../../../
52 | dockerfile: .docker/transporter_deployment/Dockerfile
53 | environment:
54 | - NVIDIA_VISIBLE_DEVICES=all
55 | - DISPLAY=${DISPLAY}
56 | volumes:
57 | - /tmp/.X11-unix:/tml/.X11-unix:rw
58 | - ./../../data_collection:/root/data_collection_ws/src/data_collection
59 | - ./../../motion_planning:/root/data_collection_ws/src/motion_planning
60 | - ./../../applications:/root/data_collection_ws/src/applications
61 | network_mode: "host"
62 | runtime: nvidia
63 | command: python3 src/applications/transporter_deployment/sort_fruit.py
64 |
--------------------------------------------------------------------------------
/src/applications/transporter_deployment/deploy_franka.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | franka_control_server:
5 | image: ghcr.io/peterdavidfagan/panda_control:humble
6 | container_name: franka_control_server
7 | build:
8 | context: ../../../../
9 | dockerfile: .docker/control/Dockerfile.control
10 | devices:
11 | - "/dev:/dev"
12 | privileged: true
13 | cap_add:
14 | - SYS_NICE
15 | ulimits:
16 | rtprio: 70
17 | rttime: -1 # corresponds to 'unlimited'
18 | memlock: 8428281856
19 | network_mode: "host"
20 | command: ros2 launch panda_control_demos franka.launch.py use_fake_hardware:=false
21 |
22 |
--------------------------------------------------------------------------------
/src/applications/transporter_deployment/deploy_mujoco_sim_prerequisites.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | franka_control_server:
5 | image: ghcr.io/peterdavidfagan/panda_control:humble
6 | container_name: franka_control_server
7 | build:
8 | context: ../../../../
9 | dockerfile: .docker/control/Dockerfile.control
10 | devices:
11 | - "/dev:/dev"
12 | privileged: true
13 | cap_add:
14 | - SYS_NICE
15 | ulimits:
16 | rtprio: 70
17 | rttime: -1 # corresponds to 'unlimited'
18 | memlock: 8428281856
19 | network_mode: "host"
20 | command: ros2 launch panda_control_demos franka.launch.py use_fake_hardware:=false
21 |
22 | robotiq_control_server:
23 | image: ghcr.io/peterdavidfagan/panda_control:humble
24 | container_name: robotiq_control_server
25 | build:
26 | context: ../../../../
27 | dockerfile: .docker/control/Dockerfile.control
28 | devices:
29 | - "/dev:/dev"
30 | privileged: true
31 | cap_add:
32 | - SYS_NICE
33 | ulimits:
34 | rtprio: 70
35 | rttime: -1 # corresponds to 'unlimited'
36 | memlock: 8428281856
37 | network_mode: "host"
38 | command: ros2 launch panda_control_demos robotiq.launch.py use_fake_hardware:=false
39 |
40 | motion_planning_prerequisites:
41 | image: ghcr.io/peterdavidfagan/panda_motion_planning:humble
42 | build:
43 | context: ../../../../
44 | dockerfile: .docker/motion_planning/Dockerfile.motion_planning
45 | environment:
46 | - DISPLAY=${DISPLAY}
47 | volumes:
48 | - /tmp/.X11-unix:/tml/.X11-unix:rw
49 | - ../../applications:/root/panda_ws/src/applications
50 | network_mode: "host"
51 | command: ros2 launch panda_motion_planning_demos motion_planning_prerequisites.launch.py
52 |
53 | object_detection:
54 | image: ghcr.io/peterdavidfagan/object_detection:humble
55 | container_name: object_detection
56 | build:
57 | context: ../../
58 | dockerfile: .docker/object_detection/Dockerfile
59 | environment:
60 | - NVIDIA_VISIBLE_DEVICES=all
61 | devices:
62 | - "/dev:/dev"
63 | privileged: true
64 | runtime: nvidia
65 | network_mode: "host"
66 | command: python3 ./src/object_detection/ros2_object_detection/ros2_object_detection_components/ros2_object_detection_components/grounded_dino.py
67 |
68 |
69 |
--------------------------------------------------------------------------------
/src/applications/transporter_deployment/deploy_robotiq.yaml:
--------------------------------------------------------------------------------
1 | version: "3"
2 |
3 | services:
4 | robotiq_control_server:
5 | image: ghcr.io/peterdavidfagan/panda_control:humble
6 | container_name: robotiq_control_server
7 | build:
8 | context: ../../../../
9 | dockerfile: .docker/control/Dockerfile.control
10 | devices:
11 | - "/dev:/dev"
12 | privileged: true
13 | cap_add:
14 | - SYS_NICE
15 | ulimits:
16 | rtprio: 70
17 | rttime: -1 # corresponds to 'unlimited'
18 | memlock: 8428281856
19 | network_mode: "host"
20 | command: ros2 launch panda_control_demos robotiq.launch.py use_fake_hardware:=false
21 |
--------------------------------------------------------------------------------
/src/applications/transporter_deployment/launch_mujoco_sim.py:
--------------------------------------------------------------------------------
1 | import os
2 | import time
3 |
4 | import mujoco
5 | import mujoco.viewer
6 |
7 | import mujoco_ros
8 | from mujoco_ros.franka_env import FrankaApples
9 |
10 | model_filepath = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'mujoco_models', 'rearrangement.mjb')
11 |
12 | if __name__=="__main__":
13 | m = mujoco.MjModel.from_binary_path(model_filepath)
14 | d = mujoco.MjData(m)
15 |
16 | env = FrankaApples(
17 | m,
18 | d,
19 | command_interface="effort",
20 | control_steps=5,
21 | control_timer_freq=1e-2,
22 | )
23 | env.reset()
24 |
25 | with mujoco.viewer.launch_passive(
26 | model=m,
27 | data=d,
28 | show_left_ui=True,
29 | show_right_ui=False,
30 | ) as viewer:
31 |
32 | # run interactive viewer application
33 | while viewer.is_running():
34 | time.sleep(0.05)
35 | env.is_syncing = True
36 | viewer.sync()
37 | env.is_syncing = False
38 |
--------------------------------------------------------------------------------
/src/applications/transporter_deployment/mujoco_models/rearrangement.mjb:
--------------------------------------------------------------------------------
1 | version https://git-lfs.github.com/spec/v1
2 | oid sha256:fb3d87876df9d7dc94a4820d15273201b06a84d77c89d93b6a7acb0d5cb2c0ff
3 | size 234441188
4 |
--------------------------------------------------------------------------------
/src/applications/transporter_deployment/robot_workspaces/__init__.py:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/peterdavidfagan/ros2_robotics_research_toolkit/ea5ace248e10ab6d1632054356fdc8d10c0f68f9/src/applications/transporter_deployment/robot_workspaces/__init__.py
--------------------------------------------------------------------------------
/src/applications/transporter_deployment/sort_fruit.py:
--------------------------------------------------------------------------------
1 | """A basic debug/poc of identifying fruits with dino and sorting them."""
2 | import os
3 | import yaml
4 | import numpy as np
5 | import rclpy
6 | from robot_workspaces.franka_table import FrankaTable
7 | from scipy.spatial.transform import Rotation as R
8 |
9 | script_dir = os.path.dirname(os.path.abspath(__file__))
10 | yaml_file_path = os.path.join(script_dir, 'config', 'franka_table_real.yaml')
11 |
12 | def sort_apples(config):
13 | """
14 | Demonstrate apple sorting with basic object detection.
15 | """
16 | env = FrankaTable(config)
17 | env.reset()
18 | domain_model = env.props_info
19 |
20 | for key, prop_info in domain_model.items():
21 | bbox = prop_info['bbox']
22 | pixel_coords = [(bbox[0] + bbox[2])//2, (bbox[1] + bbox[3])//2]
23 | pick_pose = env.pixel_2_world(pixel_coords)
24 | quat = R.from_euler('xyz', [0, 180, 180], degrees=True).as_quat()
25 | pick_pose = np.concatenate([pick_pose, quat])
26 |
27 | pick_action = {
28 | 'pose': pick_pose,
29 | 'pixel_coords': pixel_coords,
30 | 'gripper_rot': 0.0,
31 | }
32 |
33 | def sample_pose_in_rect(location, size):
34 | x, y, z = location
35 | length, width, height = size
36 |
37 | x_pos = np.random.uniform(x, x + length)
38 | y_pos = np.random.uniform(y, y + width)
39 | z_pos = np.random.uniform(z, z + height)
40 |
41 | return [x_pos, y_pos, z_pos]
42 |
43 | # TODO: detect target location instead of getting from config or use transporter to infer from demos
44 | if '_'.join(prop_info['prop_name'].split('_')[:-1]) == 'a_red_apple':
45 | place_pose = sample_pose_in_rect(
46 | **env.config['workspace']['target_locations']['a_red_apple']
47 | )
48 | quat = R.from_euler('xyz', [0, 180, 180], degrees=True).as_quat()
49 | place_pose = np.concatenate([place_pose, quat])
50 |
51 | place_action = {
52 | 'pose': place_pose, # randomly sample place target
53 | 'pixel_coords': pixel_coords,
54 | 'gripper_rot': 0.0,
55 | }
56 |
57 | elif '_'.join(prop_info['prop_name'].split('_')[:-1]) == 'a_green_apple':
58 | place_pose = sample_pose_in_rect(
59 | **env.config['workspace']['target_locations']['a_green_apple']
60 | )
61 | quat = R.from_euler('xyz', [0, 180, 180], degrees=True).as_quat()
62 | place_pose = np.concatenate([place_pose, quat])
63 | place_action = {
64 | 'pose': place_pose, # randomly sample place target
65 | 'pixel_coords': pixel_coords,
66 | 'gripper_rot': 0.0,
67 | }
68 |
69 | else:
70 | raise NotImplementedError
71 |
72 | env.step(pick_action)
73 | env.step(place_action)
74 | env.reset()
75 |
76 | env.close()
77 |
78 | def main(args=None):
79 | with open(yaml_file_path, 'r') as file:
80 | config = yaml.safe_load(file)
81 |
82 | rclpy.init(args=args)
83 | while True: # we always want to be sorting apples :)
84 | sort_apples(config)
85 |
86 | rclpy.shutdown()
87 |
88 | if __name__=="__main__":
89 | main()
90 |
--------------------------------------------------------------------------------
/src/control/control_launch_configs/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.22)
2 | project(panda_control_demos)
3 |
4 | find_package(ament_cmake REQUIRED)
5 |
6 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
7 | install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
8 | install(DIRECTORY notebooks DESTINATION share/${PROJECT_NAME})
9 |
10 | ament_package()
11 |
--------------------------------------------------------------------------------
/src/control/control_launch_configs/README.md:
--------------------------------------------------------------------------------
1 | # Control Tutorials
2 |
3 |
--------------------------------------------------------------------------------
/src/control/control_launch_configs/config/servo_pose_jtc.yaml:
--------------------------------------------------------------------------------
1 | ###############################################
2 | # Modify all parameters related to servoing here
3 | ###############################################
4 |
5 | ## Properties of outgoing commands
6 | publish_period: 0.1 # publish at a low frequency as using JTC to interpolate
7 | max_expected_latency: 0.5
8 |
9 | incoming_command_timeout: 0.5
10 | command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
11 | scale:
12 | # Scale parameters are only used if command_in_type=="unitless"
13 | linear: 1.0 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
14 | rotational: 1.0 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
15 | # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
16 | joint: 1.0
17 |
18 | # What type of topic does your robot driver expect?
19 | # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
20 | command_out_type: trajectory_msgs/JointTrajectory
21 |
22 | # What to publish? Can save some bandwidth as most robots only require positions or velocities
23 | publish_joint_positions: true
24 | publish_joint_velocities: true
25 | publish_joint_accelerations: true
26 |
27 | ## Plugins for smoothing outgoing commands
28 | use_smoothing: false
29 | smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
30 |
31 | # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
32 | # which other nodes can use as a source for information about the planning environment.
33 | # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node),
34 | # then is_primary_planning_scene_monitor needs to be set to false.
35 | is_primary_planning_scene_monitor: true
36 |
37 | ## MoveIt properties
38 | move_group_name: panda_arm # Often 'manipulator' or 'arm'
39 | #planning_frame: panda_link0 # The MoveIt planning frame. Often 'base_link' or 'world'
40 |
41 | ## Other frames
42 | ee_frame: panda_link8 # The name of the IK tip link
43 |
44 | ## Configure handling of singularities and joint limits
45 | lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity)
46 | hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
47 | joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
48 | leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620)
49 |
50 | ## Topic names
51 | cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands
52 | joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
53 | joint_topic: /mujoco_joint_states
54 | status_topic: ~/status # Publish status to this topic
55 | command_out_topic: /panda_jtc_controller/joint_trajectory # Publish outgoing commands here
56 |
57 | ## Collision checking for the entire robot body
58 | check_collisions: false # Check collisions?
59 | collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
60 | self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
61 | scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]
62 |
--------------------------------------------------------------------------------
/src/control/control_launch_configs/config/servo_pose_position.yaml:
--------------------------------------------------------------------------------
1 | ###############################################
2 | # Modify all parameters related to servoing here
3 | ###############################################
4 |
5 | ## Properties of outgoing commands
6 | publish_period: 0.1 # publish at a low frequency as using JTC to interpolate
7 | max_expected_latency: 0.5
8 |
9 | incoming_command_timeout: 0.5
10 | command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
11 | scale:
12 | # Scale parameters are only used if command_in_type=="unitless"
13 | linear: 1.0 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
14 | rotational: 1.0 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
15 | # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
16 | joint: 1.0
17 |
18 | # What type of topic does your robot driver expect?
19 | # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
20 | command_out_type: std_msgs/Float64MultiArray
21 |
22 | # What to publish? Can save some bandwidth as most robots only require positions or velocities
23 | publish_joint_positions: true
24 | publish_joint_velocities: true
25 | publish_joint_accelerations: true
26 |
27 | ## Plugins for smoothing outgoing commands
28 | use_smoothing: false
29 | smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
30 |
31 | # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
32 | # which other nodes can use as a source for information about the planning environment.
33 | # NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node),
34 | # then is_primary_planning_scene_monitor needs to be set to false.
35 | is_primary_planning_scene_monitor: true
36 |
37 | ## MoveIt properties
38 | move_group_name: panda_arm # Often 'manipulator' or 'arm'
39 | #planning_frame: panda_link0 # The MoveIt planning frame. Often 'base_link' or 'world'
40 |
41 | ## Other frames
42 | ee_frame: panda_link8 # The name of the IK tip link
43 |
44 | ## Configure handling of singularities and joint limits
45 | lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity)
46 | hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
47 | joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
48 | leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620)
49 |
50 | ## Topic names
51 | cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands
52 | joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
53 | joint_topic: /mujoco_joint_states
54 | status_topic: ~/status # Publish status to this topic
55 | command_out_topic: /panda_position_controller/commands # Publish outgoing commands here
56 |
57 | ## Collision checking for the entire robot body
58 | check_collisions: false # Check collisions?
59 | collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
60 | self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
61 | scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]
62 |
--------------------------------------------------------------------------------
/src/control/control_launch_configs/launch/deprecated_mujoco.launch.py:
--------------------------------------------------------------------------------
1 | """
2 | A launch file for running the motion planning python api tutorial
3 | """
4 | import os
5 | import pathlib
6 | from ament_index_python.packages import get_package_share_directory
7 | from launch import LaunchDescription
8 | from launch_ros.actions import Node
9 | from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler, TimerAction
10 | from launch.event_handlers import OnProcessStart
11 | from launch.launch_description_sources import PythonLaunchDescriptionSource
12 | from launch.substitutions import LaunchConfiguration
13 | from moveit_configs_utils import MoveItConfigsBuilder
14 |
15 | FILE_PATH = pathlib.Path(__file__).parent.absolute()
16 | # TODO: use get package share directory
17 | NOTEBOOK_DIR = get_package_share_directory("panda_control_demos") + "/notebooks"
18 |
19 | def generate_launch_description():
20 |
21 | # declare parameter for using robot ip
22 | robot_ip = DeclareLaunchArgument(
23 | "robot_ip",
24 | default_value="192.168.106.99",
25 | description="Robot IP",
26 | )
27 |
28 | # declare parameter for using gripper
29 | use_gripper = DeclareLaunchArgument(
30 | "use_gripper",
31 | default_value="true",
32 | description="Use gripper",
33 | )
34 |
35 | # declare parameter for using fake controller
36 | use_fake_hardware = DeclareLaunchArgument(
37 | "use_fake_hardware",
38 | default_value="true",
39 | description="Use fake hardware",
40 | )
41 |
42 |
43 | start_foxglove_bridge = ExecuteProcess(
44 | cmd=["ros2", "launch", "foxglove_bridge", "foxglove_bridge_launch.xml"],
45 | output="screen",
46 | )
47 |
48 | # for some reason with docker its quite slow to stream data
49 | #start_foxglove_studio = ExecuteProcess(
50 | # cmd=["docker", "compose", "-f", DOCKER_COMPOSE_FILE_PATH, "up"],
51 | # output="screen",
52 | # )
53 |
54 | #open_foxglove_studio = ExecuteProcess(
55 | # cmd=["xdg-open", "http://localhost:8080"],
56 | # output="screen",
57 | # )
58 |
59 | start_mujoco_sim = ExecuteProcess(
60 | cmd=["ros2", "launch", "mujoco_ros", "mjsim.launch.py"],
61 | output="screen",
62 | )
63 |
64 | start_ros_2_controllers = ExecuteProcess(
65 | cmd=["ros2", "launch", "panda_control_demos", "control_server.launch.py"],
66 | output="screen",
67 | )
68 |
69 | start_moveit_prerequisites = ExecuteProcess(
70 | cmd=["ros2", "launch", "panda_motion_planning_demos", "motion_planning_prerequisites.launch.py"],
71 | output="screen",
72 | )
73 |
74 | start_moveit_servo = ExecuteProcess(
75 | cmd=["ros2", "launch", "panda_control_demos", "servo_pose_jtc.launch.py"],
76 | output="screen",
77 | )
78 |
79 | start_control_demo = ExecuteProcess(
80 | cmd=["ros2", "run", "panda_control_demos", "pose_tracking_example.py"],
81 | output="screen",
82 | )
83 |
84 | return LaunchDescription(
85 | [
86 | # parameters
87 | robot_ip,
88 | use_gripper,
89 | use_fake_hardware,
90 |
91 | # launching processes
92 | start_mujoco_sim,
93 | RegisterEventHandler(
94 | OnProcessStart(
95 | target_action=start_mujoco_sim,
96 | on_start=[
97 | TimerAction(
98 | period=5.0,
99 | actions=[
100 | start_foxglove_bridge,
101 | ]
102 | )
103 | ]
104 | )
105 | ),
106 |
107 | RegisterEventHandler(
108 | OnProcessStart(
109 | target_action=start_foxglove_bridge,
110 | on_start=[
111 | TimerAction(
112 | period=10.0,
113 | actions=[
114 | start_ros_2_controllers,
115 | start_moveit_prerequisites,
116 | start_moveit_servo,
117 | ]
118 | ),
119 | ]
120 | )
121 | ),
122 |
123 | RegisterEventHandler(
124 | OnProcessStart(
125 | target_action=start_moveit_servo,
126 | on_start=[
127 | TimerAction(
128 | period=1.0,
129 | actions=[
130 | start_control_demo,
131 | ]
132 | )
133 | ]
134 | )
135 | )
136 | ]
137 | )
138 |
--------------------------------------------------------------------------------
/src/control/control_launch_configs/launch/franka.launch.py:
--------------------------------------------------------------------------------
1 | import os
2 | from ament_index_python.packages import get_package_share_directory
3 | from moveit_configs_utils import MoveItConfigsBuilder
4 |
5 | import launch_ros
6 | from launch import LaunchDescription
7 | from launch.actions import (
8 | OpaqueFunction,
9 | IncludeLaunchDescription,
10 | DeclareLaunchArgument,
11 | RegisterEventHandler,
12 | TimerAction,
13 | ExecuteProcess)
14 | from launch_ros.actions import Node, SetParameter
15 | from launch.conditions import IfCondition, UnlessCondition
16 | from launch.event_handlers import OnProcessStart
17 | from launch.launch_description_sources import (
18 | PythonLaunchDescriptionSource,
19 | load_python_launch_file_as_module
20 | )
21 | from launch.substitutions import (
22 | Command,
23 | FindExecutable,
24 | LaunchConfiguration,
25 | PathJoinSubstitution,
26 | )
27 | from launch_ros.substitutions import FindPackageShare
28 |
29 |
30 | def generate_launch_description():
31 |
32 | robot_ip = DeclareLaunchArgument(
33 | "robot_ip",
34 | default_value="192.168.106.99",
35 | description="Robot IP",
36 | )
37 |
38 | use_gripper = DeclareLaunchArgument(
39 | "use_gripper",
40 | default_value="true",
41 | description="Use gripper",
42 | )
43 |
44 | use_fake_hardware = DeclareLaunchArgument(
45 | "use_fake_hardware",
46 | default_value="true",
47 | description="Use fake hardware",
48 | )
49 |
50 | # set up controller manager for Franka Robotics Panda arm under namespace panda
51 | moveit_config = (
52 | MoveItConfigsBuilder(robot_name="panda", package_name="franka_robotiq_moveit_config")
53 | .robot_description(file_path=get_package_share_directory("franka_robotiq_description") + "/urdf/robot.urdf.xacro",
54 | mappings={
55 | "robot_ip": LaunchConfiguration("robot_ip"),
56 | "robotiq_gripper": LaunchConfiguration("use_gripper"),
57 | "use_fake_hardware": LaunchConfiguration("use_fake_hardware"),
58 | })
59 | .robot_description_semantic("config/panda.srdf.xacro")
60 | .trajectory_execution("config/moveit_controllers.yaml")
61 | .to_moveit_configs()
62 | )
63 |
64 | panda_control_node = Node(
65 | package="controller_manager",
66 | executable="ros2_control_node",
67 | parameters=[
68 | moveit_config.robot_description,
69 | os.path.join(get_package_share_directory("franka_robotiq_moveit_config"), "config", "panda_controllers.yaml"),
70 | ],
71 | output="both",
72 | namespace="panda",
73 | condition=UnlessCondition(LaunchConfiguration('use_fake_hardware'))
74 | )
75 |
76 | panda_control_node_mj = Node(
77 | package="controller_manager",
78 | executable="ros2_control_node",
79 | parameters=[
80 | moveit_config.robot_description,
81 | os.path.join(get_package_share_directory("franka_robotiq_moveit_config"), "config", "panda_controllers_mujoco.yaml"),
82 | ],
83 | output="both",
84 | namespace="panda",
85 | condition=IfCondition(LaunchConfiguration('use_fake_hardware'))
86 | )
87 |
88 | load_panda_controllers = []
89 | for controller in [
90 | 'joint_state_broadcaster',
91 | 'joint_trajectory_controller'
92 | #'joint_impedance_example_controller',
93 | #'gravity_compensation_example_controller',
94 | ]:
95 | load_panda_controllers += [
96 | ExecuteProcess(
97 | cmd=["ros2 run controller_manager spawner {} -c /panda/controller_manager".format(controller)],
98 | shell=True,
99 | output="screen",
100 | )
101 | ]
102 |
103 | return LaunchDescription(
104 | [
105 | robot_ip,
106 | use_gripper,
107 | use_fake_hardware,
108 | panda_control_node,
109 | panda_control_node_mj,
110 | ] + load_panda_controllers
111 | )
112 |
--------------------------------------------------------------------------------
/src/control/control_launch_configs/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | panda_control_demos
5 | 0.0.0
6 | TODO: Package description
7 | Peter David Fagan
8 | TODO: License declaration
9 |
10 | ament_cmake
11 |
12 | backward_ros
13 | mujoco_ros
14 | moveit_py
15 | rviz2
16 | xacro
17 | urdf
18 | launch
19 | launch_ros
20 | tf2_ros
21 | joint_state_publisher
22 | robot_state_publisher
23 | controller_manager
24 | joint_trajectory_controller
25 | joint_state_broadcaster
26 | jupyter-notebook
27 |
28 | ament_lint_auto
29 | ament_lint_common
30 |
31 |
32 | ament_cmake
33 |
34 |
35 |
--------------------------------------------------------------------------------
/src/data_collection/README.md:
--------------------------------------------------------------------------------
1 | # Overview
2 |
3 | For collecting teleoperated robot trajectories for robot learning the [DROID](https://github.com/droid-dataset/droid) data collection platform is recommended.
4 |
5 | For collecting more rudimentary datasets for training transporter networks and other more primitive models, consider using the `moveit2_data_collector` package.
6 |
--------------------------------------------------------------------------------