From 86d133a7d0076e55ba7bb4033b30b13c323ee59b Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Tue, 29 Aug 2023 13:04:03 -0500 Subject: [PATCH] Implement behavior tree (#22) * Started development of a cleaned up dockerfile * Continued docker setup * Finished up updated dockerfile * here we go again * Removed armv7 * Added requirement for vtk * Replaced kinpy installation with fork * Added updated docker compose script * Moved tf to blue * Started integration of a bezier curve planner * Split kinematics into separate package * Started arming behaviors * Removed drake stuff and finished arming behaviors * Created base controller * Started implementing trajectory controller * wip: continued joint trajectory controller implementation * Finished initial version of trajectory interface * Rebuild image to include blue changes * Finished joint trajectory controller * wip: finished initial version of behavior tree * removed branch from pipeline * Started debugging tree * Fixed data gathering behaviors * Finished debugging behavior tree * Finished initial version of behavior tree * Added gripper and cleanup behaviors * started trajectory replanner * Cleaned up params * cleaned up behavior tree * Resolved pr comments as much as i feel like resolving them * Fixed bug in frame names * updated issue templates --- .docker/Dockerfile | 330 +++++++++--------- .dockerignore | 7 +- .github/ISSUE_TEMPLATE/bug-report.yaml | 2 +- .github/ISSUE_TEMPLATE/documentation.yaml | 2 +- .github/ISSUE_TEMPLATE/feature-request.yaml | 2 +- .github/workflows/ci.yaml | 2 +- .github/workflows/docker.yaml | 54 ++- angler.repos | 17 +- angler_behaviors/LICENSE | 17 + .../behavior_tree}/__init__.py | 0 .../behavior_tree/behaviors}/__init__.py | 0 .../behavior_tree/behaviors/cleanup.py | 31 ++ .../behavior_tree/behaviors/configure.py | 64 ++++ .../behavior_tree/behaviors/mission.py | 95 +++++ .../behavior_tree/primitives}/__init__.py | 0 .../behavior_tree/primitives/arming.py | 212 +++++++++++ .../behavior_tree/primitives/blackboard.py | 181 ++++++++++ .../behavior_tree/primitives/control.py | 116 ++++++ .../behavior_tree/primitives/gripper.py | 62 ++++ .../behavior_tree/primitives/planning.py | 90 +++++ .../primitives/service_clients.py | 279 +++++++++++++++ angler_behaviors/behavior_tree/tree.py | 119 +++++++ .../launch/behavior_tree.launch.py | 53 +++ angler_behaviors/package.xml | 25 ++ .../resource/angler_behaviors | 0 angler_behaviors/setup.cfg | 4 + angler_behaviors/setup.py | 47 +++ .../test/test_copyright.py | 12 +- .../launch/bluerov2_heavy_alpha.launch.py | 46 +-- angler_bringup/package.xml | 6 +- .../controllers}/__init__.py | 0 angler_control/controllers/controller.py | 153 ++++++++ .../robot_trajectory_controller/__init__.py | 0 ...se_multidof_joint_trajectory_controller.py | 307 ++++++++++++++++ .../robot_trajectory_controller/trajectory.py | 244 +++++++++++++ .../robot_trajectory_controller/utils.py | 82 +++++ .../__init__.py | 0 .../constraints.py} | 0 .../hierarchy.py | 4 +- .../tasks.py | 238 ++++--------- .../tpik_controller.py} | 211 +++++------ .../tpik/utils.py | 118 ------- angler_control/launch/control.launch.py | 10 +- angler_control/package.xml | 5 + angler_control/setup.py | 2 +- angler_control/test/test_flake8.py | 25 -- .../config/bluerov2_heavy_alpha/ardusub.parm | 29 +- .../bluerov2_heavy_alpha/controllers.yaml | 5 +- .../config/bluerov2_heavy_alpha/planning.yaml | 2 +- .../config/bluerov2_heavy_alpha/tasks.yaml | 3 + angler_description/package.xml | 2 - angler_kinematics/LICENSE | 17 + .../angler_kinematics/__init__.py | 0 .../angler_kinematics/jacobian.py | 229 ++++++++++++ angler_kinematics/package.xml | 27 ++ angler_kinematics/resource/angler_kinematics | 0 angler_kinematics/setup.cfg | 4 + angler_kinematics/setup.py | 42 +++ .../test/test_copyright.py | 12 +- angler_mux/demux/single_manipulator_demux.py | 9 +- angler_mux/test/test_flake8.py | 25 -- angler_planning/planners/__init__.py | 0 .../base_planner.py | 8 +- .../planners/waypoint_planners/__init__.py | 0 .../__init__.py | 0 .../planner.py | 31 +- .../trajectories/__init__.py | 0 .../trajectories/library/diagonal_left.json | 29 ++ .../trajectories/library/diagonal_right.json | 29 ++ .../trajectories/library/forward.json | 29 ++ .../trajectories/library/lawn_mower.json | 95 +++++ .../trajectories/library/left.json | 29 ++ .../trajectories/library/right.json | 29 ++ .../trajectories/trajectory_library.py | 67 ++-- angler_planning/setup.py | 4 +- angler_planning/test/test_flake8.py | 25 -- .../trajectories/library/straight.json | 59 ---- angler_utils/angler_utils/initial_position.py | 28 +- angler_utils/scripts/arm.sh | 2 +- angler_utils/scripts/disarm.sh | 2 +- angler_utils/test/test_flake8.py | 25 -- requirements-build.txt | 1 - sim.repos | 18 + 83 files changed, 3311 insertions(+), 879 deletions(-) create mode 100644 angler_behaviors/LICENSE rename {angler_control/inverse_kinematic_controllers => angler_behaviors/behavior_tree}/__init__.py (100%) rename {angler_control/inverse_kinematic_controllers/tpik => angler_behaviors/behavior_tree/behaviors}/__init__.py (100%) create mode 100644 angler_behaviors/behavior_tree/behaviors/cleanup.py create mode 100644 angler_behaviors/behavior_tree/behaviors/configure.py create mode 100644 angler_behaviors/behavior_tree/behaviors/mission.py rename {angler_planning/waypoint_planners => angler_behaviors/behavior_tree/primitives}/__init__.py (100%) create mode 100644 angler_behaviors/behavior_tree/primitives/arming.py create mode 100644 angler_behaviors/behavior_tree/primitives/blackboard.py create mode 100644 angler_behaviors/behavior_tree/primitives/control.py create mode 100644 angler_behaviors/behavior_tree/primitives/gripper.py create mode 100644 angler_behaviors/behavior_tree/primitives/planning.py create mode 100644 angler_behaviors/behavior_tree/primitives/service_clients.py create mode 100644 angler_behaviors/behavior_tree/tree.py create mode 100644 angler_behaviors/launch/behavior_tree.launch.py create mode 100644 angler_behaviors/package.xml rename angler_planning/waypoint_planners/preplanned_end_effector_planner/__init__.py => angler_behaviors/resource/angler_behaviors (100%) create mode 100644 angler_behaviors/setup.cfg create mode 100644 angler_behaviors/setup.py rename angler_mux/test/test_pep257.py => angler_behaviors/test/test_copyright.py (69%) rename {angler_planning/waypoint_planners/preplanned_end_effector_planner/trajectories => angler_control/controllers}/__init__.py (100%) create mode 100644 angler_control/controllers/controller.py create mode 100644 angler_control/controllers/robot_trajectory_controller/__init__.py create mode 100644 angler_control/controllers/robot_trajectory_controller/base_multidof_joint_trajectory_controller.py create mode 100644 angler_control/controllers/robot_trajectory_controller/trajectory.py create mode 100644 angler_control/controllers/robot_trajectory_controller/utils.py create mode 100644 angler_control/controllers/tpik_joint_trajectory_controller/__init__.py rename angler_control/{inverse_kinematic_controllers/tpik/constraint.py => controllers/tpik_joint_trajectory_controller/constraints.py} (100%) rename angler_control/{inverse_kinematic_controllers/tpik => controllers/tpik_joint_trajectory_controller}/hierarchy.py (97%) rename angler_control/{inverse_kinematic_controllers/tpik => controllers/tpik_joint_trajectory_controller}/tasks.py (76%) rename angler_control/{inverse_kinematic_controllers/tpik/controller.py => controllers/tpik_joint_trajectory_controller/tpik_controller.py} (74%) delete mode 100644 angler_control/inverse_kinematic_controllers/tpik/utils.py delete mode 100644 angler_control/test/test_flake8.py create mode 100644 angler_kinematics/LICENSE create mode 100644 angler_kinematics/angler_kinematics/__init__.py create mode 100644 angler_kinematics/angler_kinematics/jacobian.py create mode 100644 angler_kinematics/package.xml create mode 100644 angler_kinematics/resource/angler_kinematics create mode 100644 angler_kinematics/setup.cfg create mode 100644 angler_kinematics/setup.py rename angler_utils/test/test_pep257.py => angler_kinematics/test/test_copyright.py (69%) delete mode 100644 angler_mux/test/test_flake8.py create mode 100644 angler_planning/planners/__init__.py rename angler_planning/{waypoint_planners => planners}/base_planner.py (91%) create mode 100644 angler_planning/planners/waypoint_planners/__init__.py create mode 100644 angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/__init__.py rename angler_planning/{ => planners}/waypoint_planners/preplanned_end_effector_planner/planner.py (81%) create mode 100644 angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/__init__.py create mode 100644 angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/diagonal_left.json create mode 100644 angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/diagonal_right.json create mode 100644 angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/forward.json create mode 100644 angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/lawn_mower.json create mode 100644 angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/left.json create mode 100644 angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/right.json rename angler_planning/{ => planners}/waypoint_planners/preplanned_end_effector_planner/trajectories/trajectory_library.py (81%) delete mode 100644 angler_planning/test/test_flake8.py delete mode 100644 angler_planning/waypoint_planners/preplanned_end_effector_planner/trajectories/library/straight.json delete mode 100644 angler_utils/test/test_flake8.py create mode 100644 sim.repos diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 8e199a6..5e24afa 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -8,9 +8,6 @@ LABEL maintainer="Evan Palmer" LABEL maintainer-email="evanp922@gmail.com" ENV DEBIAN_FRONTEND=noninteractive -WORKDIR /root/ws_angler - -COPY . src/angler # Install apt packages RUN apt-get -q update \ @@ -31,35 +28,6 @@ RUN apt-get -q update \ && apt-get clean -y \ && rm -rf /var/lib/apt/lists/* -# Install the Python requirements that aren't available as rosdeps -RUN python3 -m pip install -r $(pwd)/src/angler/requirements-build.txt - -# Install gstreamer -RUN apt-get -q update \ - && apt-get -q -y upgrade \ - && apt-get -q install --no-install-recommends -y \ - python3-gi \ - gstreamer1.0-tools \ - gir1.2-gstreamer-1.0 \ - gir1.2-gst-plugins-base-1.0 \ - gstreamer1.0-plugins-good \ - gstreamer1.0-plugins-ugly \ - gstreamer1.0-plugins-bad \ - gstreamer1.0-libav \ - && apt-get autoremove -y \ - && apt-get clean -y \ - && rm -rf /var/lib/apt/lists/* - -# Install all ROS dependencies -RUN apt-get -q update \ - && apt-get -q -y upgrade \ - && rosdep update \ - && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false \ - && rm -rf src \ - && apt-get autoremove -y \ - && apt-get clean -y \ - && rm -rf /var/lib/apt/lists/* - # Configure a new non-root user ARG USERNAME=angler ARG USER_UID=1000 @@ -71,6 +39,20 @@ RUN groupadd --gid $USER_GID $USERNAME \ && chmod 0440 /etc/sudoers.d/$USERNAME \ && echo "source /usr/share/bash-completion/completions/git" >> /home/$USERNAME/.bashrc +# Install all ROS dependencies for testing +# Remove the source code from this stage +WORKDIR /root/ws_angler +COPY . src/angler + +RUN apt-get -q update \ + && apt-get -q -y upgrade \ + && rosdep update \ + && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false \ + && rm -rf src \ + && apt-get autoremove -y \ + && apt-get clean -y \ + && rm -rf /var/lib/apt/lists/* + # Switch to the non-root user to install MAVROS dependencies USER $USERNAME ENV USER=$USERNAME @@ -85,36 +67,137 @@ RUN wget https://raw.githubusercontent.com/mavlink/mavros/ros2/mavros/scripts/in USER root ENV USER=root -################################################################# -# sim: install Gazebo and the tools needed for Gazebo simulation -################################################################# -FROM ci as sim +################################################## +# deps: Install all external project dependencies +################################################## +FROM ci as deps -# Install Gazebo Garden: https://gazebosim.org/docs/garden/install_ubuntu -RUN wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg -RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null -RUN apt-get update \ - && apt-get -y --quiet --no-install-recommends install \ - gz-garden \ - && apt-get autoremove -y \ - && apt-get clean -y \ - && rm -rf /var/lib/apt/lists/* +ENV DEBIAN_FRONTEND=noninteractive +ENV GZ_VERSION=garden -# Install ArduPilot and ardupilot_gazebo dependencies +# Install gstreamer RUN apt-get -q update \ && apt-get -q -y upgrade \ && apt-get -q install --no-install-recommends -y \ - python3-wxgtk4.0 \ - rapidjson-dev \ - xterm \ + python3-gi \ + gstreamer1.0-tools \ + gir1.2-gstreamer-1.0 \ + gir1.2-gst-plugins-base-1.0 \ + gstreamer1.0-plugins-good \ + gstreamer1.0-plugins-ugly \ + gstreamer1.0-plugins-bad \ + gstreamer1.0-libav \ && apt-get autoremove -y \ && apt-get clean -y \ && rm -rf /var/lib/apt/lists/* -# Switch to the non-root user to install ArduSub +# Install the Python requirements that aren't available as rosdeps +COPY requirements-build.txt . +RUN python3 -m pip install -r requirements-build.txt \ + && rm -rf requirements-build.txt + +# Switch to the non-root user +# We are going to use this for the rest of the installation USER $USERNAME ENV USER=$USERNAME +# Create a user-level ROS workspace for us to use +ENV USER_WORKSPACE=/home/$USERNAME/ws_angler +WORKDIR $USER_WORKSPACE + +# Install the external project requirements +COPY --chown=$USER_UID:$USER_GID angler.repos src/ +RUN sudo apt-get -q update \ + && sudo apt-get -q -y upgrade \ + && vcs import src < src/angler.repos \ + && rosdep update \ + && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} \ + # Install custom version of kinpy + && cd src/kinpy \ + && python3 -m pip install . \ + && cd ../.. \ + && rm -rf angler.repos \ + && sudo apt-get autoremove -y \ + && sudo apt-get clean -y \ + && sudo rm -rf /var/lib/apt/lists/* + +RUN . "/opt/ros/${ROS_DISTRO}/setup.sh" \ + && colcon build \ + # Update .bashrc to source the workspace + && echo "source ${USER_WORKSPACE}/install/setup.sh" >> /home/$USERNAME/.bashrc + +# Now install any remaining Angler rosdeps +COPY --chown=$USER_UID:$USER_GID . src/angler +RUN sudo apt-get -q update \ + && sudo apt-get -q -y upgrade \ + && rosdep update \ + && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} \ + && rm -rf src/angler \ + && sudo apt-get autoremove -y \ + && sudo apt-get clean -y \ + && sudo rm -rf /var/lib/apt/lists/* + +# Install development tools for testing from the command line +# We install this here so that we can use it in the source and develop stages +RUN sudo apt-get -q update \ + && sudo apt-get -q -y upgrade \ + && sudo apt-get -q install --no-install-recommends -y \ + iputils-ping \ + net-tools \ + gdb \ + nano \ + htop \ + && sudo apt-get autoremove -y \ + && sudo apt-get clean -y \ + && sudo rm -rf /var/lib/apt/lists/* + + +############################################################## +# robot: Minimal installation needed for operation on a robot +############################################################## +FROM deps as robot + +ENV DEBIAN_FRONTEND=noninteractive + +ENV USER_WORKSPACE=/home/$USERNAME/ws_angler +WORKDIR $USER_WORKSPACE + +# Get the source code and build +# We don't need to update the .bashrc file this time, that was +# done in the previous stage +COPY --chown=$USER_UID:$USER_GID . src/angler +RUN . "/opt/ros/${ROS_DISTRO}/setup.sh" \ + && colcon build \ + && . "install/setup.sh" + + +################################################################# +# sim: install Gazebo and the tools needed for Gazebo simulation +################################################################# +FROM deps as sim +# Inspiration for this stage comes from the following source +# https://github.com/clydemcqueen/orca4/blob/77152829e1d65781717ca55379c229145d6006e9/docker/Dockerfile#L1 + +# Install Gazebo Garden: https://gazebosim.org/docs/garden/install_ubuntu +RUN sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg \ + && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null \ + && sudo apt-get update \ + && sudo apt-get -y --quiet --no-install-recommends install gz-garden \ + && sudo apt-get autoremove -y \ + && sudo apt-get clean -y \ + && sudo rm -rf /var/lib/apt/lists/* + +# Install ArduPilot and ardupilot_gazebo dependencies +RUN sudo apt-get -q update \ + && sudo apt-get -q -y upgrade \ + && sudo apt-get -q install --no-install-recommends -y \ + python3-wxgtk4.0 \ + rapidjson-dev \ + xterm \ + && sudo apt-get autoremove -y \ + && sudo apt-get clean -y \ + && sudo rm -rf /var/lib/apt/lists/* + # Clone ArduSub # ArduSub is installed for simulation purposes ONLY # When deployed onto hardware, the native installation of ArduSub @@ -144,151 +227,82 @@ RUN [ "/bin/bash" , "-c" , " \ && cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo \ && make -j4" ] -# Switch back to the root user -USER root -ENV USER=root - -# Extend the ros_entrypoint to source the simulation environment -COPY .docker/entrypoints/sim.sh / -RUN sed -i '/source "\/opt\/ros\/$ROS_DISTRO\/setup\.bash" --/a source /sim.sh' /ros_entrypoint.sh - -################################################## -# deps: Install all external project dependencies -################################################## -FROM sim as deps - -ENV GZ_VERSION=garden -ENV USER_WORKSPACE=/home/$USERNAME/ws_angler/install -ENV DEBIAN_FRONTEND=noninteractive -WORKDIR $USER_WORKSPACE/.. - -COPY angler.repos src/ - -RUN apt-get -q update \ - && apt-get -q -y upgrade \ - && vcs import src < src/angler.repos \ +# Install the external simulation packages +# Most of these dependencies should go away in the future, but they exist +# now because they are either buggy or need to be built from source to support +# Gazebo Garden +ENV USER_WORKSPACE=/home/$USERNAME/ws_angler/ +WORKDIR $USER_WORKSPACE +COPY sim.repos src/ +RUN sudo apt-get -q update \ + && sudo apt-get -q -y upgrade \ + && vcs import src < src/sim.repos \ && rosdep update \ - && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false --skip-keys="gz-transport12 gz-sim7 gz-math7 gz-msgs9 gz-plugin2" \ - && apt-get autoremove -y \ - && apt-get clean -y \ - && rm -rf /var/lib/apt/lists/* + && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --skip-keys="gz-transport12 gz-sim7 gz-math7 gz-msgs9 gz-plugin2" \ + && sudo apt-get autoremove -y \ + && sudo apt-get clean -y \ + && sudo rm -rf /var/lib/apt/lists/* +# Build the external simulation packages RUN . "/opt/ros/${ROS_DISTRO}/setup.sh" \ && colcon build \ - # Update /ros_entrypoint.sh to source the workspace - && sed -i "s#/opt/ros/\$ROS_DISTRO/setup.bash#$USER_WORKSPACE/setup.sh#g" /ros_entrypoint.sh \ - && echo "source ${USER_WORKSPACE}/setup.sh" >> /home/$USERNAME/.bashrc + && . "install/setup.sh" + +# Setup the environment variables needed for simulation +COPY .docker/entrypoints/sim.sh / +RUN echo "if [ -f /sim.sh ]; then source /sim.sh; fi" >> /home/$USERNAME/.bashrc + #################################################### # develop: Setup the image for development purposes #################################################### -FROM deps as develop +FROM sim as develop -ENV GZ_VERSION=garden -ENV ROS_UNDERLAY /root/ws_angler/install ENV DEBIAN_FRONTEND=noninteractive -WORKDIR $ROS_UNDERLAY/.. - -COPY . src/angler # Install debugging/linting Python packages -RUN python3 -m pip install -r $(pwd)/src/angler/requirements-dev.txt - -RUN apt-get -q update \ - && apt-get -q -y upgrade \ - && vcs import src < src/angler/angler.repos \ - && rosdep update \ - && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false --skip-keys="gz-transport12 gz-sim7 gz-math7 gz-msgs9 gz-plugin2" \ - && rm -rf src \ - && apt-get autoremove -y \ - && apt-get clean -y \ - && rm -rf /var/lib/apt/lists/* - -# Install development tools -RUN apt-get -q update \ - && apt-get -q -y upgrade \ - && apt-get -q install --no-install-recommends -y \ - iputils-ping \ - net-tools \ - gdb \ - nano \ - && apt-get autoremove -y \ - && apt-get clean -y \ - && rm -rf /var/lib/apt/lists/* +WORKDIR /home/$USERNAME +COPY requirements-dev.txt . +RUN python3 -m pip install -r requirements-dev.txt \ + && rm -rf requirements-dev.txt +# Set up the development environment COPY .docker/entrypoints/dev.sh / +RUN echo "if [ -f /dev.sh ]; then source /dev.sh; fi" >> /home/$USERNAME/.bashrc # WARNING: This is a temporary solution for disabling the setuputils installation warning ENV PYTHONWARNINGS="ignore" ARG WORKSPACE RUN echo "if [ -f ${WORKSPACE}/install/setup.bash ]; then source ${WORKSPACE}/install/setup.bash; fi" >> /home/$USERNAME/.bashrc \ - && echo "if [ -f /opt/ros/${ROS_DISTRO}/setup.bash ]; then source /opt/ros/${ROS_DISTRO}/setup.bash; fi" >> /home/$USERNAME/.bashrc \ - # Expose the environment variables to the non-root user - && echo "if [ -f /sim.sh ]; then source /sim.sh; fi" >> /home/$USERNAME/.bashrc \ - && echo "if [ -f /dev.sh ]; then source /dev.sh; fi" >> /home/$USERNAME/.bashrc + && echo "if [ -f /opt/ros/${ROS_DISTRO}/setup.bash ]; then source /opt/ros/${ROS_DISTRO}/setup.bash; fi" >> /home/$USERNAME/.bashrc ###################################################################################### # develop-nvidia: Setup the image for development purposes with NVIDIA driver support ###################################################################################### -FROM deps as develop-nvidia - -# Install NVIDIA software -RUN apt-get update \ - && apt-get install -y -qq --no-install-recommends \ - libglvnd0 \ - libgl1 \ - libglx0 \ - libegl1 \ - libxext6 \ - libx11-6 +FROM sim as develop-nvidia + +ENV DEBIAN_FRONTEND=noninteractive # Env vars for the nvidia-container-runtime. ENV NVIDIA_VISIBLE_DEVICES all ENV NVIDIA_DRIVER_CAPABILITIES graphics,utility,compute ENV QT_X11_NO_MITSHM 1 -ENV GZ_VERSION=garden -ENV ROS_UNDERLAY /root/ws_angler/install -ENV DEBIAN_FRONTEND=noninteractive -WORKDIR $ROS_UNDERLAY/.. - -COPY . src/angler - # Install debugging/linting Python packages -RUN python3 -m pip install -r $(pwd)/src/angler/requirements-dev.txt - -RUN apt-get -q update \ - && apt-get -q -y upgrade \ - && vcs import src < src/angler/angler.repos \ - && rosdep update \ - && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --as-root=apt:false --skip-keys="gz-transport12 gz-sim7 gz-math7 gz-msgs9 gz-plugin2" \ - && rm -rf src \ - && apt-get autoremove -y \ - && apt-get clean -y \ - && rm -rf /var/lib/apt/lists/* - -# Install development tools -RUN apt-get -q update \ - && apt-get -q -y upgrade \ - && apt-get -q install --no-install-recommends -y \ - iputils-ping \ - net-tools \ - gdb \ - nano \ - && apt-get autoremove -y \ - && apt-get clean -y \ - && rm -rf /var/lib/apt/lists/* +WORKDIR /home/$USERNAME +COPY requirements-dev.txt . +RUN python3 -m pip install -r requirements-dev.txt \ + && rm -rf requirements-dev.txt +# Set up the development environment COPY .docker/entrypoints/dev.sh / +RUN echo "if [ -f /dev.sh ]; then source /dev.sh; fi" >> /home/$USERNAME/.bashrc # WARNING: This is a temporary solution for disabling the setuputils installation warning ENV PYTHONWARNINGS="ignore" ARG WORKSPACE RUN echo "if [ -f ${WORKSPACE}/install/setup.bash ]; then source ${WORKSPACE}/install/setup.bash; fi" >> /home/$USERNAME/.bashrc \ - && echo "if [ -f /opt/ros/${ROS_DISTRO}/setup.bash ]; then source /opt/ros/${ROS_DISTRO}/setup.bash; fi" >> /home/$USERNAME/.bashrc \ - # Expose the environment variables to the non-root user - && echo "if [ -f /sim.sh ]; then source /sim.sh; fi" >> /home/$USERNAME/.bashrc \ - && echo "if [ -f /dev.sh ]; then source /dev.sh; fi" >> /home/$USERNAME/.bashrc + && echo "if [ -f /opt/ros/${ROS_DISTRO}/setup.bash ]; then source /opt/ros/${ROS_DISTRO}/setup.bash; fi" >> /home/$USERNAME/.bashrc diff --git a/.dockerignore b/.dockerignore index 56c1d11..888c12c 100644 --- a/.dockerignore +++ b/.dockerignore @@ -3,10 +3,15 @@ # Except the following !angler_bringup +!angler_control +!angler_description !angler_planning -!angler_msgs !angler_mux +!angler_utils +!angler_kinematics +!angler_behaviors !angler.repos +!sim.repos !.docker/entrypoints !requirements-build.txt !requirements-dev.txt diff --git a/.github/ISSUE_TEMPLATE/bug-report.yaml b/.github/ISSUE_TEMPLATE/bug-report.yaml index 32dbb39..8ba16ae 100644 --- a/.github/ISSUE_TEMPLATE/bug-report.yaml +++ b/.github/ISSUE_TEMPLATE/bug-report.yaml @@ -1,7 +1,7 @@ name: Bug Report description: Report a bug. title: "[BUG]: " -labels: [bug] +labels: [bug, needs triage] body: - type: markdown diff --git a/.github/ISSUE_TEMPLATE/documentation.yaml b/.github/ISSUE_TEMPLATE/documentation.yaml index 6203056..2cb41a6 100644 --- a/.github/ISSUE_TEMPLATE/documentation.yaml +++ b/.github/ISSUE_TEMPLATE/documentation.yaml @@ -1,7 +1,7 @@ name: Documentation Improvement description: Report an issue related to the Angler documentation. title: "[DOC]: " -labels: [documentation] +labels: [documentation, needs triage] body: - type: markdown diff --git a/.github/ISSUE_TEMPLATE/feature-request.yaml b/.github/ISSUE_TEMPLATE/feature-request.yaml index 8f20bf1..c134ff0 100644 --- a/.github/ISSUE_TEMPLATE/feature-request.yaml +++ b/.github/ISSUE_TEMPLATE/feature-request.yaml @@ -1,7 +1,7 @@ name: Feature Request description: Suggest a new idea for the project. title: "[FEATURE]: " -labels: [enhancement] +labels: [enhancement, needs triage] body: - type: markdown diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 6cfc7d3..13a6e77 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -31,7 +31,7 @@ jobs: submodules: recursive - name: Log into registry - uses: docker/login-action@v2.0.0 + uses: docker/login-action@v2.2.0 with: registry: ghcr.io username: ${{ github.actor }} diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml index ed3eeef..4064f04 100644 --- a/.github/workflows/docker.yaml +++ b/.github/workflows/docker.yaml @@ -31,7 +31,7 @@ jobs: - name: Log into registry if: env.PUSH == 'true' - uses: docker/login-action@v2.0.0 + uses: docker/login-action@v2.2.0 with: registry: ghcr.io username: ${{ github.actor }} @@ -40,14 +40,14 @@ jobs: - name: Extract Docker metadata if: env.PUSH == 'true' id: meta - uses: docker/metadata-action@v4.0.1 + uses: docker/metadata-action@v4.6.0 with: images: ghcr.io/${{ github.repository }} tags: | type=raw,value=${{ matrix.ROS_DISTRO }}-${{ github.job }} - name: Build and push Docker image - uses: docker/build-push-action@v3.0.0 + uses: docker/build-push-action@v4.1.1 with: context: . file: .docker/Dockerfile @@ -56,3 +56,51 @@ jobs: tags: ${{ steps.meta.outputs.tags }} labels: ${{ steps.meta.outputs.labels }} push: ${{ env.PUSH }} + + robot: + strategy: + fail-fast: false + matrix: + ROS_DISTRO: [humble, rolling, iron] + runs-on: ubuntu-latest + permissions: + packages: write + contents: read + steps: + - name: Checkout repository + uses: actions/checkout@v3.5.3 + + - name: Set up QEMU + uses: docker/setup-qemu-action@v2.2.0 + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v2 + + - name: Log into registry + if: env.PUSH == 'true' + uses: docker/login-action@v2.2.0 + with: + registry: ghcr.io + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + + - name: Extract Docker metadata + if: env.PUSH == 'true' + id: meta + uses: docker/metadata-action@v4.6.0 + with: + images: ghcr.io/${{ github.repository }} + tags: | + type=raw,value=${{ matrix.ROS_DISTRO }}-${{ github.job }} + + - name: Build and push Docker image + uses: docker/build-push-action@v4.1.1 + with: + context: . + file: .docker/Dockerfile + build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} + target: ${{ github.job }} + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} + push: ${{ env.PUSH }} + platforms: linux/amd64,linux/arm64 diff --git a/angler.repos b/angler.repos index c5b297e..6162a32 100644 --- a/angler.repos +++ b/angler.repos @@ -10,17 +10,12 @@ repositories: url: https://github.com/evan-palmer/blue.git version: main - ros_gz: + kinpy: type: git - url: https://github.com/gazebosim/ros_gz - version: ros2 + url: https://github.com/evan-palmer/kinpy.git + version: master - gz-plugin: + py_trees_ros_viewer: type: git - url: https://github.com/gazebosim/gz-plugin.git - version: gz-plugin2 - - gz_ros2_control: - type: git - url: https://github.com/evan-palmer/gz_ros2_control.git - version: patch-rolling-garden + url: https://github.com/splintered-reality/py_trees_ros_viewer.git + version: devel diff --git a/angler_behaviors/LICENSE b/angler_behaviors/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/angler_behaviors/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/angler_control/inverse_kinematic_controllers/__init__.py b/angler_behaviors/behavior_tree/__init__.py similarity index 100% rename from angler_control/inverse_kinematic_controllers/__init__.py rename to angler_behaviors/behavior_tree/__init__.py diff --git a/angler_control/inverse_kinematic_controllers/tpik/__init__.py b/angler_behaviors/behavior_tree/behaviors/__init__.py similarity index 100% rename from angler_control/inverse_kinematic_controllers/tpik/__init__.py rename to angler_behaviors/behavior_tree/behaviors/__init__.py diff --git a/angler_behaviors/behavior_tree/behaviors/cleanup.py b/angler_behaviors/behavior_tree/behaviors/cleanup.py new file mode 100644 index 0000000..9df9762 --- /dev/null +++ b/angler_behaviors/behavior_tree/behaviors/cleanup.py @@ -0,0 +1,31 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import py_trees + + +def make_on_mission_complete_behavior() -> py_trees.behaviour.Behaviour: + """Make a behavior that initiates a post-mission completion sequence. + + Returns: + A post-mission behavior. + """ + # For now, we just idle on mission completion + return py_trees.behaviours.Running("Idle") diff --git a/angler_behaviors/behavior_tree/behaviors/configure.py b/angler_behaviors/behavior_tree/behaviors/configure.py new file mode 100644 index 0000000..b5b290e --- /dev/null +++ b/angler_behaviors/behavior_tree/behaviors/configure.py @@ -0,0 +1,64 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import operator + +import py_trees +from behavior_tree.primitives.arming import make_system_arming_behavior + + +def make_setup_behavior( + setup_finished_flag_key: str, armed_key: str +) -> py_trees.behaviour.Behaviour: + """Make a behavior that sets up the system prior to beginning a mission. + + Args: + setup_finished_flag_key: The key at which the setup flag is stored. + + Returns: + A system setup behavior. + """ + # Don't repeat setup if we have already done it + check_setup_finished = py_trees.behaviours.CheckBlackboardVariableValue( + name="Setup complete?", + check=py_trees.common.ComparisonExpression( + variable=setup_finished_flag_key, value=True, operator=operator.eq + ), + ) + + # Once we have a plan, arm so that we can start trajectory tracking + arming = make_system_arming_behavior( + arm=True, armed_key=armed_key, use_passthrough_mode=True + ) + + # Finish up by indicating that the setup has finished + finished_setup = py_trees.behaviours.SetBlackboardVariable( + "Setup finished!", setup_finished_flag_key, True, overwrite=True + ) + + setup = py_trees.composites.Sequence( + name="Setup the system for a mission", + memory=True, + children=[arming, finished_setup], + ) + + return py_trees.composites.Selector( + name="Run system setup", memory=False, children=[check_setup_finished, setup] + ) diff --git a/angler_behaviors/behavior_tree/behaviors/mission.py b/angler_behaviors/behavior_tree/behaviors/mission.py new file mode 100644 index 0000000..6f3eb8f --- /dev/null +++ b/angler_behaviors/behavior_tree/behaviors/mission.py @@ -0,0 +1,95 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import operator + +import py_trees +import py_trees_ros +from behavior_tree.primitives.control import make_execute_multidof_trajectory_behavior +from behavior_tree.primitives.planning import make_high_level_planning_behavior + + +def make_save_start_mission_behavior( + start_mission_key: str, +) -> py_trees.behaviour.Behaviour: + """Make a behavior that saves the "start mission" flag to the blackboard. + + Args: + start_mission_key: The key at which the flag should be saved. + + Returns: + A behavior that saves the key to trigger arming. + """ + return py_trees_ros.subscribers.EventToBlackboard( + name="ROS2BB: Start mission", + topic_name="/angler/cmd/start_mission", + qos_profile=py_trees_ros.utilities.qos_profile_unlatched(), + variable_name=start_mission_key, + ) + + +def make_execute_mission_behavior( + start_mission_key: str, + robot_state_key: str, + planner_id: str, + controller_id: str, +) -> py_trees.behaviour.Behaviour: + """Make a behavior that executes a mission. + + Args: + start_mission_key: The key at which the signal indicating that a mission should + start is stored. + robot_state_key: The key at which the robot state is stored. + planner_id: The key at which the high-level planner ID is stored. + controller_id: The key at which the joint trajectory controller ID is + stored. + + Returns: + A system setup behavior. + """ + # Start by checking whether or not to start the mission + check_start_mission = py_trees.behaviours.CheckBlackboardVariableValue( + name="Start the mission?", + check=py_trees.common.ComparisonExpression( + variable=start_mission_key, value=True, operator=operator.eq + ), + ) + + planning_result_key = "planning_result" + + get_mission_plan = make_high_level_planning_behavior( + robot_state_key=robot_state_key, + planner_id=planner_id, + planning_result_key=planning_result_key, + ) + + execute_mission = make_execute_multidof_trajectory_behavior( + trajectory_key=planning_result_key, controller_id=controller_id + ) + + return py_trees.composites.Sequence( + name="Execute mission", + memory=True, + children=[ + check_start_mission, + get_mission_plan, + execute_mission, + ], + ) diff --git a/angler_planning/waypoint_planners/__init__.py b/angler_behaviors/behavior_tree/primitives/__init__.py similarity index 100% rename from angler_planning/waypoint_planners/__init__.py rename to angler_behaviors/behavior_tree/primitives/__init__.py diff --git a/angler_behaviors/behavior_tree/primitives/arming.py b/angler_behaviors/behavior_tree/primitives/arming.py new file mode 100644 index 0000000..8262e28 --- /dev/null +++ b/angler_behaviors/behavior_tree/primitives/arming.py @@ -0,0 +1,212 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import operator + +import py_trees +import py_trees_ros +from behavior_tree.primitives.blackboard import ( + FunctionOfBlackboardVariables, + ToBlackboardNonBlocking, +) +from behavior_tree.primitives.service_clients import FromConstant +from std_msgs.msg import Bool +from std_srvs.srv import SetBool + + +def make_save_armed_behavior(arm_system_key: str) -> py_trees.behaviour.Behaviour: + """Save a command to arm/disarm the system to the blackboard. + + Returns: + A ToBlackboard behavior which saves commands to arm/disarm the system to the + blackboard. + """ + return ToBlackboardNonBlocking( + name="ROS2BB: Arm system", + topic_name="/angler/cmd/arm_autonomy", + topic_type=Bool, + qos_profile=py_trees_ros.utilities.qos_profile_unlatched(), + blackboard_variables={arm_system_key: "data"}, + initialise_variables={arm_system_key: False}, + clearing_policy=py_trees.common.ClearingPolicy.ON_SUCCESS, + ) + + +def make_subsystem_arming_behavior( + arm: bool, subsystem_name: str, arming_topic: str +) -> py_trees.behaviour.Behaviour: + """Make a behavior that arms/disarms a subsystem (e.g., a whole-body controller). + + Args: + arm: True if arming; False if disarming. + subsystem_name: The name of the sub-system to arm. + arming_topic: The topic over which to send the arming request. + + Returns: + A behavior that arms/disarms a subsystem. + """ + + def check_arming_success(response: SetBool.Response) -> bool: + return response.success + + arm_response_key = "arm_request_response" + arm_result_key = "arm_request_result" + + arm_request = FromConstant( + name=f"{subsystem_name}: send arming request", + service_type=SetBool, + service_name=arming_topic, + service_request=SetBool.Request(data=arm), + key_response=arm_response_key, + ) + transform_response = FunctionOfBlackboardVariables( + name=f"{subsystem_name}: transform arming response to bool", + input_keys=[arm_response_key], + output_key=arm_result_key, + function=check_arming_success, + ) + check_response = py_trees.behaviours.CheckBlackboardVariableValue( + name=f"{subsystem_name}: verify response", + check=py_trees.common.ComparisonExpression( + variable=arm_result_key, + value=True, + operator=operator.eq, + ), + ) + + return py_trees.composites.Sequence( + name=f"{subsystem_name}: Arming" if arm else ": Disarming", + memory=True, + children=[arm_request, transform_response, check_response], + ) + + +def make_system_arming_behavior(arm: bool, armed_key: str, use_passthrough_mode: bool): + """Create a behavior that arms/disarms the system. + + The full system arming sequence includes: + 1. Enabling/disabling PWM passthrough mode (if configured) + 2. Arming/disarming the Blue controller + 3. Arming/disarming the Angler controller + + Args: + arm: Set to `True` to arm the system; set to `False` to disarm. + armed_key: The key at which to store the resulting arming status. + use_passthrough_mode: Use the Blue PWM passthrough mode. Take care when using + this mode. All safety checks onboard the system will be disabled. + Furthermore, make sure to leave PWM passthrough mode before shutdown, or + the system parameters will not be restored. + + Returns: + A behavior that runs the full system arming sequence. + """ + set_armed_state = py_trees.behaviours.SetBlackboardVariable( + name="Set the current arming status", + variable_name=armed_key, + variable_value=arm, + overwrite=True, + ) + + # Put everything together + behaviors = [ + make_subsystem_arming_behavior(arm, "Blue Controller", "/blue/cmd/arm"), + make_subsystem_arming_behavior(arm, "Angler Controller", "/angler/cmd/arm"), + set_armed_state, + ] + + if use_passthrough_mode: + behaviors.insert( + 0, + make_subsystem_arming_behavior( + arm, "Passthrough Mode", "/blue/cmd/enable_passthrough" + ), + ) + + check_already_disarmed = py_trees.behaviours.CheckBlackboardVariableValue( + name="Already armed?" if arm else "Already disarmed?", + check=py_trees.common.ComparisonExpression( + variable=armed_key, value=arm, operator=operator.eq + ), + ) + do_arming = py_trees.composites.Sequence( + name="Enable system autonomy" if arm else "Disable system autonomy", + memory=True, + children=behaviors, # type: ignore + ) + + return py_trees.composites.Selector( + "Don't do what has already been done!", + memory=False, + children=[check_already_disarmed, do_arming], + ) + + +def make_block_on_disarm_behavior( + arm_system_key: str, + armed_key: str, + tasks: py_trees.behaviour.Behaviour, + on_disarm_behavior: py_trees.behaviour.Behaviour | None = None, +) -> py_trees.behaviour.Behaviour: + """Make a behavior that blocks when the system is disarmed. + + Args: + arm_system_key: The key at which the arm system flag is stored. + armed_key: The key at which to store the resulting arming status. + tasks: A behavior with the tasks to run. + on_disarm_behavior: An optional behavior to run when a disarm is triggered. This + will be executed as a Sequence behavior following the disarm behavior. + + Returns: + A Selector behavior with the disarm EternalGuard as the highest priority + behavior and the provided tasks as second priority. + """ + + def check_disarm_on_blackboard( + blackboard: py_trees.blackboard.Blackboard, + ) -> bool: + # We want to stop when a user issues a disarm command + return not blackboard.get(arm_system_key) + + # Default to disarming passthrough mode just in case + disarming = make_system_arming_behavior( + False, armed_key=armed_key, use_passthrough_mode=True + ) + + behaviors = [disarming] + + if on_disarm_behavior is not None: + behaviors.append(on_disarm_behavior) # type: ignore + + on_disarm = py_trees.composites.Sequence( + name="Execute disarm sequence", + memory=True, + children=behaviors, # type: ignore + ) + + disarm = py_trees.decorators.EternalGuard( + name="Disarm?", + condition=check_disarm_on_blackboard, + blackboard_keys=[arm_system_key], + child=on_disarm, + ) + + return py_trees.composites.Selector( + name="Block tasks on disarm", memory=False, children=[disarm, tasks] + ) diff --git a/angler_behaviors/behavior_tree/primitives/blackboard.py b/angler_behaviors/behavior_tree/primitives/blackboard.py new file mode 100644 index 0000000..0e4b70c --- /dev/null +++ b/angler_behaviors/behavior_tree/primitives/blackboard.py @@ -0,0 +1,181 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: + +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. + +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import copy +from typing import Any, Callable + +import py_trees +from py_trees_ros.subscribers import Handler +from rclpy.qos import QoSProfile + + +class FunctionOfBlackboardVariables(py_trees.behaviour.Behaviour): + """Apply a function to a set of blackboard variables and save the result. + + This implementation is inspired by some work done by Charles Dawson on a project + that we worked on together. + """ + + def __init__( + self, + name: str, + input_keys: list[str], + output_key: str, + function: Callable[..., Any], + ): + """Create a new FunctionOfBlackboardVariables behavior. + + Args: + name: The name of the behavior. + input_keys: The blackboard keys whose values will be passed as arguments to + the provided function. + output_key: The blackboard key where the result of the function should + be saved. + function: A function to call using the values stored at the input keys as + (positional) arguments. + """ + super().__init__(name) + + self.input_keys = input_keys + self.output_key = output_key + self.function = function + + self.blackboard = self.attach_blackboard_client() + + for key in self.input_keys: + self.blackboard.register_key(key=key, access=py_trees.common.Access.READ) + + self.blackboard.register_key( + key=self.output_key, access=py_trees.common.Access.WRITE + ) + + def initialise(self) -> None: + """Initialize the behavior.""" + self.blackboard.unset(self.output_key) + + def update(self) -> py_trees.common.Status: + """Apply the function to the saved blackboard variables. + + Returns: + `SUCCESS` if the function is able to access all required keys and execute + the provided function; `FAILURE`, otherwise. + """ + for key in self.input_keys: + if not self.blackboard.exists(key): + self.logger.error( + f"Tried to read {key} from the blackboard, but it doesn't exist!" + ) + + return py_trees.common.Status.FAILURE + + args = [self.blackboard.get(key) for key in self.input_keys] + result = None + + try: + result = self.function(*args) + except Exception as e: + self.logger.error( + f"Tried to apply function to {self.input_keys}, but failed: {e}" + ) + return py_trees.common.Status.FAILURE + + self.blackboard.set(self.output_key, result) + + return py_trees.common.Status.SUCCESS + + +class ToBlackboardNonBlocking(Handler): + """Write a ROS message to the blackboard. + + This class is a port of the `py_trees_ros.subscribers.ToBlackboard`, but modifies + it to make the behavior non-blocking. + """ + + def __init__( + self, + name: str, + topic_name: str, + topic_type: Any, + qos_profile: QoSProfile, + blackboard_variables: dict[str, Any], + initialise_variables: dict[str, Any] | None = None, + clearing_policy=py_trees.common.ClearingPolicy.ON_INITIALISE, + ): + """Create a new non-blocking `ToBlackboard` behavior. + + Args: + name: The name of the behavior. + topic_name: The name of the topic to connect to. + topic_type: The class of the message type (e.g., :obj:`std_msgs.msg.String`) + qos_profile: The QoSProfile for the subscriber. + blackboard_variables: The blackboard variable to write to. This should be + formatted as: {names (keys): message subfields (values)}. Use a value of + `None` to indicate that the entire message should be saved. + initialise_variables: Initialize the blackboard variables to some defaults. + clearing_policy: When to clear the data. Defaults to + `py_trees.common.ClearingPolicy.ON_INITIALISE`. + """ + super().__init__( + name=name, + topic_name=topic_name, + topic_type=topic_type, + qos_profile=qos_profile, + clearing_policy=clearing_policy, + ) + self.blackboard = self.attach_blackboard_client(name=self.name) + self.logger = py_trees.logging.Logger(f"{self.name}") + self.blackboard_variable_mapping = blackboard_variables + + # Register the keys + for name in self.blackboard_variable_mapping: + self.blackboard.register_key(key=name, access=py_trees.common.Access.WRITE) + + # Set the keys to some initial values + if initialise_variables is not None: + for k, v in initialise_variables.items(): + self.blackboard.set(k, v) + + def update(self): + """Write the data (if available) to the blackboard. + + Returns: + This behavior always returns `SUCCESS`. + """ + with self.data_guard: + if self.msg is None: + self.feedback_message = "no message received yet" + return py_trees.common.Status.SUCCESS + else: + for k, v in self.blackboard_variable_mapping.items(): + if v is None: + self.blackboard.set(k, self.msg, overwrite=True) + else: + fields = v.split(".") + value = copy.copy(self.msg) + for field in fields: + value = getattr(value, field) + self.blackboard.set(k, value, overwrite=True) + + self.feedback_message = "saved incoming message" + + if self.clearing_policy == py_trees.common.ClearingPolicy.ON_SUCCESS: + self.msg = None + + return py_trees.common.Status.SUCCESS diff --git a/angler_behaviors/behavior_tree/primitives/control.py b/angler_behaviors/behavior_tree/primitives/control.py new file mode 100644 index 0000000..79f2804 --- /dev/null +++ b/angler_behaviors/behavior_tree/primitives/control.py @@ -0,0 +1,116 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import py_trees +from behavior_tree.primitives.blackboard import FunctionOfBlackboardVariables +from control_msgs.action import FollowJointTrajectory +from geometry_msgs.msg import Transform +from moveit_msgs.srv import GetMotionPlan +from py_trees_ros.action_clients import FromBlackboard +from trajectory_msgs.msg import MultiDOFJointTrajectoryPoint + + +def make_move_to_end_effector_pose_behavior( + desired_pose_key: str, controller_id: str +) -> py_trees.behaviour.Behaviour: + """Make a behavior that moves the system to a given end-effector pose. + + Args: + desired_pose_key: The blackboard key which holds the desired pose (saved as a + Transform). + controller_id: The ID of the whole-body controller to load. + + Returns: + A behavior that moves the system to a desired end-effector pose. + """ + + def make_move_to_pose_goal(desired_pose: Transform) -> FollowJointTrajectory.Goal: + point = MultiDOFJointTrajectoryPoint() + point.transforms.append(desired_pose) # type: ignore + goal = FollowJointTrajectory.Goal() + goal.multi_dof_trajectory.points.append(point) # type: ignore + return goal + + desired_ee_pose_key = "desired_end_effector_pose" + + get_desired_pose = FunctionOfBlackboardVariables( + name="Get the desired end-effector pose", + input_keys=[desired_pose_key], + output_key=desired_ee_pose_key, + function=make_move_to_pose_goal, + ) + + move_to_pose = FromBlackboard( + name="Move to the desired end-effector pose", + action_type=FollowJointTrajectory, + action_name=f"/angler/{controller_id}/execute_trajectory", + key=desired_ee_pose_key, + ) + + return py_trees.composites.Sequence( + name="Move to end-effector pose", + memory=True, + children=[get_desired_pose, move_to_pose], + ) + + +def make_execute_multidof_trajectory_behavior( + trajectory_key: str, controller_id: str +) -> py_trees.behaviour.Behaviour: + """Make a behavior that executes a multi-DOF joint trajectory. + + Args: + trajectory_key: The blackboard key at which the trajectory is being stored. + controller_id: The ID of the controller to run. + + Returns: + A behavior that executes a multi-DOF joint trajectory. + """ + + def make_follow_trajectory_goal( + response: GetMotionPlan.Response, + ) -> FollowJointTrajectory.Goal: + goal = FollowJointTrajectory.Goal() + goal.multi_dof_trajectory = ( + response.motion_plan_response.trajectory.multi_dof_joint_trajectory + ) + return goal + + desired_trajectory_key = "desired_trajectory" + + get_desired_trajectory = FunctionOfBlackboardVariables( + name="Get the desired trajectory to track", + input_keys=[trajectory_key], + output_key=desired_trajectory_key, + function=make_follow_trajectory_goal, + ) + + follow_trajectory = FromBlackboard( + name="Follow the joint trajectory", + action_type=FollowJointTrajectory, + action_name=f"/angler/{controller_id}/execute_trajectory", + key=desired_trajectory_key, + ) + + return py_trees.composites.Sequence( + name="Load and execute a multi-DOF joint trajectory", + memory=True, + children=[get_desired_trajectory, follow_trajectory], + ) diff --git a/angler_behaviors/behavior_tree/primitives/gripper.py b/angler_behaviors/behavior_tree/primitives/gripper.py new file mode 100644 index 0000000..fb891f5 --- /dev/null +++ b/angler_behaviors/behavior_tree/primitives/gripper.py @@ -0,0 +1,62 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import py_trees +import py_trees_ros +from std_msgs.msg import Float64MultiArray + + +def make_alpha_jaws_velocity_control_behavior( + open_gripper: bool, rate: float +) -> py_trees.behaviour.Behaviour: + """Make a behavior that opens/closes the Reach Alpha jaws. + + Args: + open_gripper: Whether or not to open the gripper. + rate: The velocity that the gripper should open at (mm/s). + + Returns: + A behavior that controls the Reach Alpha jaws using the + forward_velocity_controller. + """ + gripper_command_key = "gripper_command" + + if open_gripper: + rate = abs(rate) + else: + rate = -abs(rate) + + set_command = py_trees.behaviours.SetBlackboardVariable( + name="Set gripper command", + variable_name=gripper_command_key, + variable_value=Float64MultiArray(data=[rate, 0.0, 0.0, 0.0, 0.0]), + overwrite=True, + ) + publish_command = py_trees_ros.publishers.FromBlackboard( + name="Publish gripper command", + topic_name="/forward_velocity_controller/commands", + topic_type=Float64MultiArray, + qos_profile=py_trees_ros.utilities.qos_profile_unlatched(), + blackboard_variable=gripper_command_key, + ) + + return py_trees.composites.Sequence( + name="Command the gripper", memory=True, children=[set_command, publish_command] + ) diff --git a/angler_behaviors/behavior_tree/primitives/planning.py b/angler_behaviors/behavior_tree/primitives/planning.py new file mode 100644 index 0000000..31ac8e0 --- /dev/null +++ b/angler_behaviors/behavior_tree/primitives/planning.py @@ -0,0 +1,90 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import py_trees +import py_trees_ros +from behavior_tree.primitives.blackboard import FunctionOfBlackboardVariables +from behavior_tree.primitives.service_clients import FromBlackboard +from moveit_msgs.msg import RobotState +from moveit_msgs.srv import GetMotionPlan +from rclpy.qos import qos_profile_sensor_data + + +def make_save_robot_state_behavior( + robot_state_key: str, +) -> py_trees.behaviour.Behaviour: + """Save the current robot state. + + Returns: + A ToBlackboard behavior which saves the robot state. + """ + return py_trees_ros.subscribers.ToBlackboard( + name="ROS2BB: Robot state", + topic_name="/angler/state", + topic_type=RobotState, + qos_profile=qos_profile_sensor_data, + blackboard_variables={robot_state_key: None}, + clearing_policy=py_trees.common.ClearingPolicy.NEVER, + ) + + +def make_high_level_planning_behavior( + robot_state_key: str, planning_result_key: str, planner_id: str +) -> py_trees.behaviour.Behaviour: + """Create a high-level planning behavior. + + Args: + robot_state_key: The key at which the robot state is being stored. + planning_result_key: The key at which the planning result is saved. + planner_id: The ID of the planner to perform high-level planning with. + + Returns: + Behavior that creates a high-level mission plan and saves the result to the + blackboard. + """ + + def make_planning_request(state: RobotState) -> GetMotionPlan.Request: + request = GetMotionPlan.Request() + request.motion_plan_request.start_state = state + request.motion_plan_request.planner_id = planner_id + return request + + planner_request_key = "high_level_planner_request" + + plan_request_behavior = FunctionOfBlackboardVariables( + name="Make high-level planner request", + input_keys=[robot_state_key], + output_key=planner_request_key, + function=make_planning_request, + ) + + plan_behavior = FromBlackboard( + name="Plan a high-level mission", + service_type=GetMotionPlan, + service_name=f"/angler/{planner_id}/plan", + key_request=planner_request_key, + key_response=planning_result_key, + ) + + return py_trees.composites.Sequence( + name="Get high-level plan", + memory=True, + children=[plan_request_behavior, plan_behavior], + ) diff --git a/angler_behaviors/behavior_tree/primitives/service_clients.py b/angler_behaviors/behavior_tree/primitives/service_clients.py new file mode 100644 index 0000000..d787792 --- /dev/null +++ b/angler_behaviors/behavior_tree/primitives/service_clients.py @@ -0,0 +1,279 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# +# License: BSD +# https://raw.github.com/splintered-reality/py_trees_ros/license/LICENSE +# +############################################################################## +# Documentation +############################################################################## + +""" +Behaviours for ROS services. + +Note from Evan: This is a direct port from this PR: +https://github.com/splintered-reality/py_trees_ros/pull/215 +I want to use this, but don't feel like building yet another package +from source. This will be removed when the PR is merged. +""" + +############################################################################## +# Imports +############################################################################## + +import typing +import uuid +from asyncio.tasks import wait_for + +import py_trees +from py_trees_ros import exceptions + +############################################################################## +# Behaviours +############################################################################## + + +class FromBlackboard(py_trees.behaviour.Behaviour): + """ + An service client interface that draws requests from the blackboard. The + lifecycle of this behaviour works as follows: + + * :meth:`initialise`: check blackboard for a request and send + * :meth:`update`: if a request was sent, monitor progress + * :meth:`terminate`: if interrupted while running, send a cancel request + + As a consequence, the status of this behaviour can be interpreted as follows: + + * :data:`~py_trees.common.Status.FAILURE`: no request was found to send, + the server was not ready, or it failed while executing + * :data:`~py_trees.common.Status.RUNNING`: a request was sent and is still + executing + * :data:`~py_trees.common.Status.SUCCESS`: sent request has completed with success + + To block on the arrival of a request on the blackboard, use with the + :class:`py_trees.behaviours.WaitForBlackboardVariable` behaviour. e.g. + + Args: + name: name of the behaviour + service_type: spec type for the service + service_name: where you can find the service + key_request: name of the key for the request on the blackboard + key_response: optional name of the key for the response on the blackboard (default: None) + wait_for_server_timeout_sec: use negative values for a blocking but periodic check (default: -3.0) + + .. note:: + The default setting for timeouts (a negative value) will suit + most use cases. With this setting the behaviour will periodically check and + issue a warning if the server can't be found. Actually aborting the setup can + usually be left up to the behaviour tree manager. + """ + + def __init__( + self, + name: str, + service_type: typing.Any, + service_name: str, + key_request: str, + key_response: typing.Optional[str] = None, + wait_for_server_timeout_sec: float = -3.0, + ): + super().__init__(name) + self.service_type = service_type + self.service_name = service_name + self.wait_for_server_timeout_sec = wait_for_server_timeout_sec + self.blackboard = self.attach_blackboard_client(name=self.name) + self.blackboard.register_key( + key="request", + access=py_trees.common.Access.READ, + # make sure to namespace it if not already + remap_to=py_trees.blackboard.Blackboard.absolute_name("/", key_request), + ) + self.write_response_to_blackboard = key_response is not None + if self.write_response_to_blackboard: + self.blackboard.register_key( + key="response", + access=py_trees.common.Access.WRITE, + # make sure to namespace it if not already + remap_to=py_trees.blackboard.Blackboard.absolute_name( + "/", key_response + ), + ) + + self.node = None + self.service_client = None + + def setup(self, **kwargs): + """ + Setup the service client and ensure it is available. + + Args: + **kwargs (:obj:`dict`): distribute arguments to this + behaviour and in turn, all of it's children + + Raises: + :class:`KeyError`: if a ros2 node isn't passed under the key 'node' in kwargs + :class:`~py_trees_ros.exceptions.TimedOutError`: if the service server could not be found + """ + self.logger.debug("{}.setup()".format(self.qualified_name)) + try: + self.node = kwargs["node"] + except KeyError as e: + error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format( + self.qualified_name + ) + raise KeyError(error_message) from e # 'direct cause' traceability + + self.service_client = self.node.create_client( + srv_type=self.service_type, srv_name=self.service_name + ) + + result = None + if self.wait_for_server_timeout_sec > 0.0: + result = self.service_client.wait_for_service( + timeout_sec=self.wait_for_server_timeout_sec + ) + else: + iterations = 0 + period_sec = -1.0 * self.wait_for_server_timeout_sec + while not result: + iterations += 1 + result = self.service_client.wait_for_service(timeout_sec=period_sec) + if not result: + self.node.get_logger().warning( + "waiting for service server ... [{}s][{}][{}]".format( + iterations * period_sec, + self.service_name, + self.qualified_name, + ) + ) + if not result: + self.feedback_message = "timed out waiting for the server [{}]".format( + self.service_name + ) + self.node.get_logger().error( + "{}[{}]".format(self.feedback_message, self.qualified_name) + ) + raise exceptions.TimedOutError(self.feedback_message) + else: + self.feedback_message = "... connected to service server [{}]".format( + self.service_name + ) + self.node.get_logger().info( + "{}[{}]".format(self.feedback_message, self.qualified_name) + ) + + def initialise(self): + """ + Reset the internal variables and kick off a new request request. + """ + self.logger.debug("{}.initialise()".format(self.qualified_name)) + + # initialise some temporary variables + self.service_future = None + + try: + if self.service_client.service_is_ready(): + self.service_future = self.service_client.call_async( + self.blackboard.request + ) + except (KeyError, TypeError): + pass # self.service_future will be None, check on that + + def update(self): + """ + Check only to see whether the underlying service server has + succeeded, is running, or has cancelled/aborted for some reason and + map these to the usual behaviour return states. + + Returns: + :class:`py_trees.common.Status` + """ + self.logger.debug("{}.update()".format(self.qualified_name)) + + if self.service_future is None: + # either there was no request on the blackboard, the request's type + # was wrong, or the service server wasn't ready + return py_trees.common.Status.FAILURE + elif not self.service_future.done(): + # service has been called but hasn't yet returned a result + return py_trees.common.Status.RUNNING + else: + # service has succeeded; get the result + self.response = self.service_future.result() + if self.write_response_to_blackboard: + self.blackboard.response = self.response + return py_trees.common.Status.SUCCESS + + def terminate(self, new_status: py_trees.common.Status): + """ + If running and the current request has not already succeeded, cancel it. + + Args: + new_status: the behaviour is transitioning to this new status + """ + self.logger.debug( + "{}.terminate({})".format( + self.qualified_name, + "{}->{}".format(self.status, new_status) + if self.status != new_status + else "{}".format(new_status), + ) + ) + if (self.service_future is not None) and (not self.service_future.done()): + self.service_client.remove_pending_request(self.service_future) + + def shutdown(self): + """ + Clean up the service client when shutting down. + """ + self.service_client.destroy() + + +class FromConstant(FromBlackboard): + """ + Convenience version of the service client that only ever sends the + same goal. + + .. see-also: :class:`py_trees_ros.service_clients.FromBlackboard` + + Args: + name: name of the behaviour + name: name of the behaviour + service_type: spec type for the service + service_name: where you can find the service + service_request: the request to send + key_response: optional name of the key for the response on the blackboard (default: None) + wait_for_server_timeout_sec: use negative values for a blocking but periodic check (default: -3.0) + + .. note:: + The default setting for timeouts (a negative value) will suit + most use cases. With this setting the behaviour will periodically check and + issue a warning if the server can't be found. Actually aborting the setup can + usually be left up to the behaviour tree manager. + """ + + def __init__( + self, + name: str, + service_type: typing.Any, + service_name: str, + service_request: typing.Any, + key_response: typing.Optional[str] = None, + wait_for_server_timeout_sec: float = -3.0, + ): + unique_id = uuid.uuid4() + key_request = "/goal_" + str(unique_id) + super().__init__( + service_type=service_type, + service_name=service_name, + key_request=key_request, + key_response=key_response, + name=name, + wait_for_server_timeout_sec=wait_for_server_timeout_sec, + ) + # parent already instantiated a blackboard client + self.blackboard.register_key( + key=key_request, + access=py_trees.common.Access.WRITE, + ) + self.blackboard.set(name=key_request, value=service_request) diff --git a/angler_behaviors/behavior_tree/tree.py b/angler_behaviors/behavior_tree/tree.py new file mode 100644 index 0000000..6c745f1 --- /dev/null +++ b/angler_behaviors/behavior_tree/tree.py @@ -0,0 +1,119 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import py_trees +import py_trees_ros +import rclpy +from behavior_tree.behaviors.cleanup import make_on_mission_complete_behavior +from behavior_tree.behaviors.configure import make_setup_behavior +from behavior_tree.behaviors.mission import ( + make_execute_mission_behavior, + make_save_start_mission_behavior, +) +from behavior_tree.primitives.arming import ( + make_block_on_disarm_behavior, + make_save_armed_behavior, +) +from behavior_tree.primitives.planning import make_save_robot_state_behavior + + +def make_angler_tree() -> py_trees.behaviour.Behaviour: + """Make a behavior tree that runs the full Angler system. + + Returns: + A behavior tree that provides full-system autonomy. + """ + root = py_trees.composites.Parallel( + name="Angler Autonomy", + policy=py_trees.common.ParallelPolicy.SuccessOnAll(synchronise=False), + ) + + # Add the data gathering behaviors + start_mission_key = "start_mission" + arm_system_key = "arm_system" + armed_key = "armed" + robot_state_key = "robot_state" + + data_gathering = py_trees.composites.Sequence( + name="Data gathering", + memory=True, + children=[ + make_save_start_mission_behavior(start_mission_key), + make_save_armed_behavior(arm_system_key), + make_save_robot_state_behavior(robot_state_key), + ], + ) + + root.add_child(data_gathering) + + setup_finished_flag_key = "setup_finished" + + setup_and_execute_mission = py_trees.composites.Sequence( + name="Setup and execute mission", + memory=True, + children=[ + make_setup_behavior( + setup_finished_flag_key=setup_finished_flag_key, armed_key=armed_key + ), + make_execute_mission_behavior( + start_mission_key=start_mission_key, + robot_state_key=robot_state_key, + planner_id="preplanned_end_effector_waypoint_planner", + controller_id="tpik_joint_trajectory_controller", + ), + make_on_mission_complete_behavior(), + ], + ) + + tasks = make_block_on_disarm_behavior( + arm_system_key=arm_system_key, + armed_key=armed_key, + tasks=setup_and_execute_mission, + on_disarm_behavior=py_trees.behaviours.SetBlackboardVariable( + name="Setup needs to be redone", + variable_name=setup_finished_flag_key, + variable_value=False, + overwrite=True, + ), + ) + + root.add_child(tasks) + + return root + + +def main(args: list[str] | None = None): + """Run the pre-planned waypoint planner.""" + rclpy.init(args=args) + + # Create the behavior tree + root = make_angler_tree() + tree = py_trees_ros.trees.BehaviourTree(root) + + # Setup the tree; this will throw if there is a timeout + tree.setup(timeout=15.0) + + # Run the tree at a rate of 20hz + tree.tick_tock(50) + + rclpy.spin(tree.node) # type: ignore + + tree.shutdown() + rclpy.shutdown() diff --git a/angler_behaviors/launch/behavior_tree.launch.py b/angler_behaviors/launch/behavior_tree.launch.py new file mode 100644 index 0000000..c9dbaa8 --- /dev/null +++ b/angler_behaviors/launch/behavior_tree.launch.py @@ -0,0 +1,53 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + """Generate a launch description for the Angler behavior tree. + + Returns: + LaunchDescription: The Angler behavior tree launch description. + """ + args = [ + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description=("Use the simulated Gazebo clock."), + ), + ] + + nodes = [ + Node( + package="angler_behaviors", + executable="behavior_tree", + name="behavior_tree", + output="screen", + parameters=[ + {"use_sim_time": LaunchConfiguration("use_sim_time")}, + ], + ), + ] + + return LaunchDescription(args + nodes) diff --git a/angler_behaviors/package.xml b/angler_behaviors/package.xml new file mode 100644 index 0000000..c01b7aa --- /dev/null +++ b/angler_behaviors/package.xml @@ -0,0 +1,25 @@ + + + + + angler_behaviors + 0.0.1 + Behavior tree responsible for system autonomy. + + Evan Palmer + MIT + + https://github.com/evan-palmer/blue.git + https://github.com/evan-palmer/blue/issues + + py_trees + py_trees_ros + + ament_copyright + ament_flake8 + python3-pytest + + + ament_python + + diff --git a/angler_planning/waypoint_planners/preplanned_end_effector_planner/__init__.py b/angler_behaviors/resource/angler_behaviors similarity index 100% rename from angler_planning/waypoint_planners/preplanned_end_effector_planner/__init__.py rename to angler_behaviors/resource/angler_behaviors diff --git a/angler_behaviors/setup.cfg b/angler_behaviors/setup.cfg new file mode 100644 index 0000000..c9f9c83 --- /dev/null +++ b/angler_behaviors/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/angler_behaviors +[install] +install_scripts=$base/lib/angler_behaviors diff --git a/angler_behaviors/setup.py b/angler_behaviors/setup.py new file mode 100644 index 0000000..b7b1c9b --- /dev/null +++ b/angler_behaviors/setup.py @@ -0,0 +1,47 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import os +from glob import glob + +from setuptools import find_packages, setup + +package_name = "angler_behaviors" + +setup( + name=package_name, + version="0.0.1", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + (os.path.join("share", package_name), glob("launch/*.launch.py")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Evan Palmer", + maintainer_email="evanp922gmail.com", + description="Behavior tree responsible for system autonomy.", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": ["behavior_tree = behavior_tree.tree:main"], + }, +) diff --git a/angler_mux/test/test_pep257.py b/angler_behaviors/test/test_copyright.py similarity index 69% rename from angler_mux/test/test_pep257.py rename to angler_behaviors/test/test_copyright.py index 4eddb46..8f18fa4 100644 --- a/angler_mux/test/test_pep257.py +++ b/angler_behaviors/test/test_copyright.py @@ -13,11 +13,15 @@ # limitations under the License. import pytest -from ament_pep257.main import main +from ament_copyright.main import main +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) +@pytest.mark.copyright @pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): +def test_copyright(): rc = main(argv=[".", "test"]) - assert rc == 0, "Found code style errors / warnings" + assert rc == 0, "Found errors" diff --git a/angler_bringup/launch/bluerov2_heavy_alpha.launch.py b/angler_bringup/launch/bluerov2_heavy_alpha.launch.py index d589511..8c365fd 100644 --- a/angler_bringup/launch/bluerov2_heavy_alpha.launch.py +++ b/angler_bringup/launch/bluerov2_heavy_alpha.launch.py @@ -211,9 +211,9 @@ def generate_launch_description() -> LaunchDescription: ), DeclareLaunchArgument( "whole_body_controller", - default_value="tpik", + default_value="tpik_joint_trajectory_controller", description="The whole-body controller to load.", - choices=["tpik"], + choices=["tpik_joint_trajectory_controller"], ), ] @@ -252,7 +252,7 @@ def generate_launch_description() -> LaunchDescription: namespace=namespace, parameters=[ controllers_file, - {"use_sim_time": use_sim, "robot_description": robot_description}, + {"use_sim_time": use_sim}, ], condition=UnlessCondition(use_sim), ) @@ -311,7 +311,11 @@ def generate_launch_description() -> LaunchDescription: ), # We need to wait for the Gazebo spawner to finish, but # don't have access to that because it is called from blue - TimerAction(period=2.0, actions=[joint_state_broadcaster_spawner]), + TimerAction( + period=2.0, + actions=[joint_state_broadcaster_spawner], + condition=IfCondition(use_sim), + ), RegisterEventHandler( event_handler=OnProcessExit( target_action=joint_state_broadcaster_spawner, on_exit=[rviz] @@ -324,14 +328,6 @@ def generate_launch_description() -> LaunchDescription: on_exit=[alpha_controller_spawner], ) ), - Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[ - {"use_sim_time": use_sim, "robot_description": robot_description} - ], - ), Node( package="angler_utils", executable="initial_position_setter", @@ -355,24 +351,6 @@ def generate_launch_description() -> LaunchDescription: ) ), ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="base_link_to_base_footprint", - arguments=[ - "--x", - "-0.0", - "--y", - "0.0", - "--z", - "0.0", - "--frame-id", - ["base_link"], - "--child-frame-id", - ["base_footprint"], - ], - output="screen", - ), ] includes = [ @@ -445,6 +423,14 @@ def generate_launch_description() -> LaunchDescription: }.items(), condition=IfCondition(LaunchConfiguration("use_whole_body_control")), ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("angler_behaviors"), "behavior_tree.launch.py"] + ) + ), + launch_arguments={"use_sim_time": use_sim}.items(), + ), ] return LaunchDescription(args + nodes + includes) diff --git a/angler_bringup/package.xml b/angler_bringup/package.xml index 59a020d..48e57f8 100644 --- a/angler_bringup/package.xml +++ b/angler_bringup/package.xml @@ -14,17 +14,17 @@ Evan Palmer - mavros - mavros_extras - ament_copyright ament_flake8 ament_pep257 python3-pytest + mavros + mavros_extras hardware_interface velocity_controllers xacro + foxglove_bridge ament_cmake diff --git a/angler_planning/waypoint_planners/preplanned_end_effector_planner/trajectories/__init__.py b/angler_control/controllers/__init__.py similarity index 100% rename from angler_planning/waypoint_planners/preplanned_end_effector_planner/trajectories/__init__.py rename to angler_control/controllers/__init__.py diff --git a/angler_control/controllers/controller.py b/angler_control/controllers/controller.py new file mode 100644 index 0000000..872b746 --- /dev/null +++ b/angler_control/controllers/controller.py @@ -0,0 +1,153 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +from abc import ABC, abstractmethod + +from moveit_msgs.msg import RobotState +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from std_srvs.srv import SetBool + + +class BaseController(ABC, Node): + """Base class for a whole-body controller.""" + + def __init__(self, controller_name: str) -> None: + """Create a new controller interface. + + Args: + controller_name: The name of the controller. + """ + ABC.__init__(self) + Node.__init__(self, controller_name) + + self.declare_parameter("control_rate", 30.0) + + self.armed = False + self.robot_state = RobotState() + self.dt = 1 / ( + self.get_parameter("control_rate").get_parameter_value().double_value + ) + + # Services + self.arm_controller_srv = self.create_service( + SetBool, "/angler/cmd/arm", self.arm_controller_cb + ) + + # Subscribers + self.robot_state_sub = self.create_subscription( + RobotState, + "/angler/state", + self._robot_state_cb, + qos_profile=qos_profile_sensor_data, + ) + + # Create a new callback group for the control loop timer + # Don't start it automatically though. + self.timer_cb_group = ReentrantCallbackGroup() + self.control_loop_timer = self.create_timer(self.dt, self._update) + + def on_robot_state_update(self, state: RobotState) -> None: + """Execute this function on robot state update. + + Args: + state: The most recent robot state update. + """ + # Don't do anything with the state by default + ... + + def _robot_state_cb(self, state: RobotState) -> None: + """Update the current robot state and run the `on_robot_state` callback. + + Args: + state: The current robot state. + """ + self.on_robot_state_update(state) + self.state = state + + @abstractmethod + def on_update(self) -> None: + """Execute this function on control loop iteration.""" + raise NotImplementedError("This method has not yet been implemented!") + + def _update(self) -> None: + """Run the control loop.""" + if not self.armed: + # Don't do anything if we aren't armed + ... + else: + self.on_update() + + def on_arm(self) -> bool: + """Execute this function on system arming. + + Returns: + Whether or not the arming procedure succeeded. + """ + # Default to indicating success + return True + + def on_disarm(self) -> bool: + """Execute this function on system disarming. + + Returns: + Whether or not the disarming procedure succeeded. + """ + # Default to indicating success + return True + + def arm_controller_cb( + self, request: SetBool.Request, response: SetBool.Response + ) -> SetBool.Response: + """Arm/disarm the controller. + + This can be overridden to support more complex pre-arming checks. + + Args: + request: The arm/disarm request. + response: The request result. + + Returns: + The request result. + """ + if request.data: + result = self.on_arm() + self.armed = self.armed if not result else result + else: + result = self.on_disarm() + self.armed = self.armed if not result else not result + + if not result: + response.success = False + response.message = ( + "Failed to " + + ("arm" if request.data else "disarm") + + " the Angler controller!" + ) + else: + response.success = True + response.message = ( + "Successfully " + + ("armed" if self.armed else "disarmed") + + " the Angler controller!" + ) + + return response diff --git a/angler_control/controllers/robot_trajectory_controller/__init__.py b/angler_control/controllers/robot_trajectory_controller/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/angler_control/controllers/robot_trajectory_controller/base_multidof_joint_trajectory_controller.py b/angler_control/controllers/robot_trajectory_controller/base_multidof_joint_trajectory_controller.py new file mode 100644 index 0000000..cac7960 --- /dev/null +++ b/angler_control/controllers/robot_trajectory_controller/base_multidof_joint_trajectory_controller.py @@ -0,0 +1,307 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import threading +from abc import abstractmethod + +import controllers.robot_trajectory_controller.utils as controller_utils +import numpy as np +from control_msgs.action import FollowJointTrajectory +from controllers.controller import BaseController +from controllers.robot_trajectory_controller.trajectory import MultiDOFTrajectory +from geometry_msgs.msg import Quaternion +from rclpy.action import ActionServer, GoalResponse +from rclpy.action.server import ServerGoalHandle +from rclpy.time import Duration +from trajectory_msgs.msg import MultiDOFJointTrajectoryPoint + + +class BaseMultiDOFJointTrajectoryController(BaseController): + """Base interface for a MultiDOFJointTrajectory controller.""" + + def __init__(self, controller_name: str) -> None: + """Create a new multi-DOF joint trajectory controller. + + Args: + controller_name: The unique controller name. + """ + super().__init__(controller_name) + + self.declare_parameters( + namespace="", + parameters=[ # type: ignore + ("goal_position_tolerance", 0.2), + ("goal_orientation_tolerance", 0.1), + ("goal_linear_velocity_tolerance", np.inf), + ("goal_angular_velocity_tolerance", np.inf), + ("goal_time_tolerance", 10.0), + ], + ) + + # Keep track of whether or not a goal is running/preempted to enable transition + # between preempted goals + self.current_uid = 0 + self.execute_trajectory_lock = threading.Lock() + self._running = False + + # Store a command for the child to execute + self.command: MultiDOFJointTrajectoryPoint | None = None + + # Get the tolerances for the controller + self.goal_position_tolerance = ( + self.get_parameter("goal_position_tolerance") + .get_parameter_value() + .double_value + ) + self.goal_orientation_tolerance = ( + self.get_parameter("goal_orientation_tolerance") + .get_parameter_value() + .double_value + ) + self.goal_linear_velocity_tolerance = ( + self.get_parameter("goal_linear_velocity_tolerance") + .get_parameter_value() + .double_value + ) + self.goal_angular_velocity_tolerance = ( + self.get_parameter("goal_angular_velocity_tolerance") + .get_parameter_value() + .double_value + ) + self.goal_time_tolerance = Duration( + seconds=self.get_parameter("goal_time_tolerance") # type: ignore + .get_parameter_value() + .double_value + ) + + self.execute_trajectory_client = ActionServer( + self, + FollowJointTrajectory, + f"/angler/{controller_name}/execute_trajectory", + goal_callback=self.handle_action_request_cb, + execute_callback=self.execute_trajectory_cb, + ) + + def destroy_node(self) -> None: + """Shutdown the node and all clients.""" + self.execute_trajectory_client.destroy() + return super().destroy_node() + + @property + @abstractmethod + def joint_state(self) -> MultiDOFJointTrajectoryPoint: + """Get the current joint state to control. + + Returns: + The joint state. + """ + raise NotImplementedError("This method has not yet been implemented!") + + def handle_action_request_cb(self, goal_handle: ServerGoalHandle) -> GoalResponse: + """Accept or reject the action request. + + Args: + goal_handle: The action goal. + + Returns: + Whether or not the goal has been accepted. + """ + if not self.armed: + return GoalResponse.REJECT + + return GoalResponse.ACCEPT + + def check_joint_at_goal( + self, + current_state: MultiDOFJointTrajectoryPoint, + desired_state: MultiDOFJointTrajectoryPoint, + index: int, + ) -> bool: + """Check whether or not the joint is at its goal state. + + Args: + current_state: The current joint state. + desired_state: The desired joint state. + index: The joint index. + + Returns: + Whether or not the joint is at its goal. + """ + current_pos = controller_utils.convert_tf_to_array( + current_state.transforms[index] # type: ignore + ) + desired_pos = controller_utils.convert_tf_to_array( + desired_state.transforms[index] # type: ignore + ) + + position_at_goal = ( + np.linalg.norm(desired_pos[:3] - current_pos[:3]) + < self.goal_position_tolerance + ) + + def quat_to_array(quat: Quaternion): + return np.array([quat.x, quat.y, quat.z, quat.w]) + + current_rot = quat_to_array( + current_state.transforms[index].rotation # type: ignore + ) + desired_rot = quat_to_array( + desired_state.transforms[index].rotation # type: ignore + ) + + error_eps = ( + current_rot[3] * desired_rot[:3] + - desired_rot[3] * current_rot[:3] + + np.cross(current_rot[:3], desired_rot[:3]) + ) + + rotation_at_goal = ( + np.linalg.norm(np.hstack((error_eps))) < self.goal_orientation_tolerance + ) + + if ( + self.trajectory.trajectory.points[0].velocities # type: ignore + and self.trajectory.trajectory.points[-1].velocities # type: ignore + ): + current_vel = controller_utils.convert_twist_to_array( + current_state.velocities[index] # type: ignore + ) + desired_vel = controller_utils.convert_twist_to_array( + desired_state.velocities[index] # type: ignore + ) + + vel_error = np.linalg.norm(desired_vel - current_vel) # type: ignore + + vel_at_goal = ( + vel_error[:3] < self.goal_linear_velocity_tolerance + and vel_error[3:] < self.goal_angular_velocity_tolerance + ) + + return position_at_goal and rotation_at_goal and vel_at_goal + + return position_at_goal and rotation_at_goal + + def on_update(self) -> None: + """Return if no goal has been received.""" + if not self._running: + return + + sample = self.trajectory.sample(self.get_clock().now()) + + if sample is not None: + self.command = sample + else: + self.get_logger().info( + f"Failed to sample trajectory at time {self.get_clock().now()}" + ) + + async def execute_trajectory_cb( + self, goal_handle: ServerGoalHandle + ) -> FollowJointTrajectory.Result: + """Execute a joint trajectory. + + Args: + goal_handle: The desired trajectory to track. + + Returns: + The result of the trajectory execution. + """ + # Keep track of the current goal UID to handle preemption + uid = self.current_uid + 1 + self.current_uid += 1 + + result = FollowJointTrajectory.Result() + + rate = self.create_rate(1 / self.dt) + + with self.execute_trajectory_lock: + # Update the new trajectory + self.trajectory = MultiDOFTrajectory( + goal_handle.request.multi_dof_trajectory, + self.joint_state, + self.get_clock().now(), + ) + self._running = True + + while not all( + [ + self.check_joint_at_goal( + self.joint_state, + self.trajectory.trajectory.points[-1], # type: ignore + i, + ) + for i in range(len(self.joint_state.transforms)) + ] + ): + # The goal has been flagged as inactive + if not goal_handle.is_active: + self._running = False + result.error_code = FollowJointTrajectory.Result.INVALID_GOAL + result.error_string = "Goal aborted!" + goal_handle.abort() + return result + + end_goal_time = controller_utils.add_ros_time_duration_msg( + self.trajectory.starting_time, + self.trajectory.trajectory.points[-1].time_from_start, # type: ignore # noqa + ) + + # Failed to get to the goal waypoint in the tolerance time + if self.get_clock().now() - end_goal_time > self.goal_time_tolerance: + self._running = False + result.error_code = ( + FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED + ) + result.error_string = ( + "Failed to complete the goal within the provided time" + f" tolerance {self.goal_time_tolerance}" + ) + goal_handle.abort() + return result + + # Goal has been cancelled + if goal_handle.is_cancel_requested: + self._running = False + result.error_code = FollowJointTrajectory.Result.INVALID_GOAL + result.error_string = ( + "The current action was cancelled by a client!" + ) + goal_handle.canceled() + return result + + # Goal has been preempted by another goal + if uid != self.current_uid: + self._running = False + result.error_code = FollowJointTrajectory.Result.INVALID_GOAL + result.error_string = ( + "The current action was cancelled by a new incoming action" + ) + goal_handle.abort() + return result + + rate.sleep() + + # The goal was reached within all tolerances + self._running = False + result.error_code = FollowJointTrajectory.Result.SUCCESSFUL + result.error_string = "Successfully executed the provided trajectory" + goal_handle.succeed() + + return result diff --git a/angler_control/controllers/robot_trajectory_controller/trajectory.py b/angler_control/controllers/robot_trajectory_controller/trajectory.py new file mode 100644 index 0000000..1adf8be --- /dev/null +++ b/angler_control/controllers/robot_trajectory_controller/trajectory.py @@ -0,0 +1,244 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import controllers.robot_trajectory_controller.utils as controller_utils +import numpy as np +from geometry_msgs.msg import Transform, Twist +from rclpy.time import Duration, Time +from scipy.interpolate import CubicHermiteSpline, interp1d +from scipy.spatial.transform import Rotation as R +from trajectory_msgs.msg import MultiDOFJointTrajectory, MultiDOFJointTrajectoryPoint + + +class MultiDOFTrajectory: + """Wraps a MultiDOFJointTrajectory for sampling purposes. + + The MultiDOFTrajectory wrapper provides and interface for sampling points between + a discrete set of MultiDOFJointTrajectoryPoint messages. This helps provide + continuity between points. + """ + + def __init__( + self, + trajectory: MultiDOFJointTrajectory, + current_state: MultiDOFJointTrajectoryPoint, + current_time: Time, + ) -> None: + """Create a new MultiDOFJointTrajectory interface. + + Args: + trajectory: The trajectory to create an interface for. + current_state: The current state of the robot prior to beginning the + trajectory. + current_time: The current ROS timestamp. + """ + self.trajectory = trajectory + self.starting_state = current_state + self.starting_time = current_time + + def sample(self, t: Time) -> MultiDOFJointTrajectoryPoint | None: + """Sample the trajectory at a given timestamp. + + Args: + t: The timestamp to sample the trajectory at. + + Returns: + The point at the given timestamp if it exists; None, otherwise. + """ + if len(self.trajectory.points) <= 0: + return None + + # Attempting to sample before the initial state + if t < self.starting_time: + return None + + first_point = self.trajectory.points[0] # type: ignore + first_point_timestamp = controller_utils.add_ros_time_duration_msg( + self.starting_time, first_point.time_from_start + ) + + if t < first_point_timestamp: + return self.interpolate( + self.starting_state, + first_point, + self.starting_time, + first_point_timestamp, + t, + ) + + for i in range(len(self.trajectory.points) - 1): + point = self.trajectory.points[i] # type: ignore + next_point = self.trajectory.points[i + 1] # type: ignore + + t0 = controller_utils.add_ros_time_duration_msg( + self.starting_time, point.time_from_start + ) + t1 = controller_utils.add_ros_time_duration_msg( + self.starting_time, next_point.time_from_start + ) + + if t0 <= t < t1: + return self.interpolate(point, next_point, t0, t1, t) + + # We are sampling past the trajectory now; return the last point in the + # trajectory + return self.trajectory.points[-1] # type: ignore + + def interpolate( + self, + start_point: MultiDOFJointTrajectoryPoint, + end_point: MultiDOFJointTrajectoryPoint, + start_time: Time, + end_time: Time, + sample_time: Time, + ) -> MultiDOFJointTrajectoryPoint: + """Interpolate between two points and sample the resulting function. + + This method currently only supports interpolation between positions and + velocities. Interpolation between points with accelerations is not yet + supported. + + For position-only interfaces, linear interpolation is used. For position + + velocity interfaces, cubic Hermite spline interpolation is used. + + Args: + start_point: The segment starting point. + end_point: The segment end point. + start_time: The timestamp of the first point in the segment. + end_time: The timestamp of the second point in the segment. + sample_time: The timestamp at which to sample. + + Returns: + The identified point at the provided sample time. + """ + duration_from_start: Duration = sample_time - start_time + duration_between_points: Duration = end_time - start_time + + has_velocity = start_point.velocities and end_point.velocities + + # Check if the sample time is before the start point or if the sample time is + # after the end point + if ( + duration_from_start.nanoseconds < 0 + or duration_from_start.nanoseconds > duration_between_points.nanoseconds + ): + has_velocity = False + + result = MultiDOFJointTrajectoryPoint() + result.time_from_start.nanosec = sample_time + + if not has_velocity: + # Perform linear interpolation between points + for i in range(len(start_point.transforms)): + # Get the desired positions + start_tf: Transform = start_point.transforms[i] # type: ignore + end_tf: Transform = end_point.transforms[i] # type: ignore + + tfs = np.array( + [ + controller_utils.convert_tf_to_array(start_tf), + controller_utils.convert_tf_to_array(end_tf), + ] + ).T + t = np.array([start_time.nanoseconds, end_time.nanoseconds]) + + # Perform linear interpolation + interpolation = interp1d(t, tfs, kind="linear") + sample = interpolation(sample_time.nanoseconds) + + # Reconstruct the sample as a ROS message + sample_msg = Transform() + + ( + sample_msg.translation.x, + sample_msg.translation.y, + sample_msg.translation.z, + ) = sample[:3] + + ( + sample_msg.rotation.x, + sample_msg.rotation.y, + sample_msg.rotation.z, + sample_msg.rotation.w, + ) = R.from_euler("xyz", sample[3:]).as_quat() + + result.transforms.append(sample_msg) # type: ignore + elif has_velocity: + for i in range(len(start_point.transforms)): + # Get the desired positions + start_tf: Transform = start_point.transforms[i] # type: ignore + end_tf: Transform = end_point.transforms[i] # type: ignore + + # Get the desired velocities + start_vel = start_point.velocities[i] # type: ignore + end_vel = end_point.velocities[i] # type: ignore + + tfs = np.array( + [ + controller_utils.convert_tf_to_array(start_tf), + controller_utils.convert_tf_to_array(end_tf), + ] + ) + vels = np.array( + [ + controller_utils.convert_twist_to_array(start_vel), + controller_utils.convert_twist_to_array(end_vel), + ] + ) + t = np.array([start_time.nanoseconds, end_time.nanoseconds]) + + # Interpolate using a cubic hermite spline + spline = CubicHermiteSpline(t, tfs, vels) + sample_tf = spline(sample_time.nanoseconds) + sample_vel = spline(sample_time.nanoseconds, 1) + + # Now reconstruct the TF message + sample_tf_msg = Transform() + + ( + sample_tf_msg.translation.x, + sample_tf_msg.translation.y, + sample_tf_msg.translation.z, + ) = sample_tf[:3] + + ( + sample_tf_msg.rotation.x, + sample_tf_msg.rotation.y, + sample_tf_msg.rotation.z, + sample_tf_msg.rotation.w, + ) = R.from_euler("xyz", sample_tf[3:]).as_quat() + + # Reconstruct the velocity message + sample_twist_msg = Twist() + + ( + sample_twist_msg.linear.x, + sample_twist_msg.linear.y, + sample_twist_msg.linear.z, + sample_twist_msg.angular.x, + sample_twist_msg.angular.y, + sample_twist_msg.angular.z, + ) = sample_vel + + # Finish up by saving the samples + result.transforms.append(sample_tf_msg) # type: ignore + result.velocities.append(sample_twist_msg) # type: ignore + + return result diff --git a/angler_control/controllers/robot_trajectory_controller/utils.py b/angler_control/controllers/robot_trajectory_controller/utils.py new file mode 100644 index 0000000..1dc4518 --- /dev/null +++ b/angler_control/controllers/robot_trajectory_controller/utils.py @@ -0,0 +1,82 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import numpy as np +from builtin_interfaces.msg import Duration as DurationMsg +from geometry_msgs.msg import Transform, Twist +from rclpy.time import Time +from scipy.spatial.transform import Rotation as R + + +def add_ros_time_duration_msg(time: Time, duration: DurationMsg) -> Time: + """Add a ROS Time and a Duration message together to create a new Time. + + Args: + time: The Time addend. + duration: The Duration message addend. + + Returns: + The resulting sum as a new Time object. + """ + duration_nanoseconds = duration.sec * 10**9 + duration.nanosec + return Time( + nanoseconds=time.nanoseconds + duration_nanoseconds, clock_type=time.clock_type + ) + + +def convert_tf_to_array(tf: Transform) -> np.ndarray: + """Convert a Transform message to a numpy array. + + Args: + tf: The transform to convert. + + Returns: + The resulting numpy array. + """ + return np.array( + [ + tf.translation.x, + tf.translation.y, + tf.translation.z, + *R.from_quat( + [tf.rotation.x, tf.rotation.y, tf.rotation.z, tf.rotation.w] + ).as_euler("xyz"), + ] + ) + + +def convert_twist_to_array(twist: Twist) -> np.ndarray: + """Convert a Twist message to a numpy array. + + Args: + twist: The twist to convert. + + Returns: + The resulting numpy array. + """ + return np.array( + [ + twist.linear.x, + twist.linear.y, + twist.linear.z, + twist.angular.x, + twist.angular.z, + ] + ) diff --git a/angler_control/controllers/tpik_joint_trajectory_controller/__init__.py b/angler_control/controllers/tpik_joint_trajectory_controller/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/angler_control/inverse_kinematic_controllers/tpik/constraint.py b/angler_control/controllers/tpik_joint_trajectory_controller/constraints.py similarity index 100% rename from angler_control/inverse_kinematic_controllers/tpik/constraint.py rename to angler_control/controllers/tpik_joint_trajectory_controller/constraints.py diff --git a/angler_control/inverse_kinematic_controllers/tpik/hierarchy.py b/angler_control/controllers/tpik_joint_trajectory_controller/hierarchy.py similarity index 97% rename from angler_control/inverse_kinematic_controllers/tpik/hierarchy.py rename to angler_control/controllers/tpik_joint_trajectory_controller/hierarchy.py index 163ed25..c3f12e8 100644 --- a/angler_control/inverse_kinematic_controllers/tpik/hierarchy.py +++ b/angler_control/controllers/tpik_joint_trajectory_controller/hierarchy.py @@ -24,12 +24,12 @@ import numpy as np import yaml # type: ignore -from inverse_kinematic_controllers.tpik.constraint import ( +from controllers.tpik_joint_trajectory_controller.constraints import ( Constraint, EqualityConstraint, SetConstraint, ) -from inverse_kinematic_controllers.tpik.tasks import task_library +from controllers.tpik_joint_trajectory_controller.tasks import task_library class TaskHierarchy: diff --git a/angler_control/inverse_kinematic_controllers/tpik/tasks.py b/angler_control/controllers/tpik_joint_trajectory_controller/tasks.py similarity index 76% rename from angler_control/inverse_kinematic_controllers/tpik/tasks.py rename to angler_control/controllers/tpik_joint_trajectory_controller/tasks.py index b7e78fe..6ccfcc4 100644 --- a/angler_control/inverse_kinematic_controllers/tpik/tasks.py +++ b/angler_control/controllers/tpik_joint_trajectory_controller/tasks.py @@ -20,15 +20,43 @@ from typing import Any -import inverse_kinematic_controllers.tpik.utils as tpik_utils import numpy as np -from geometry_msgs.msg import Quaternion, Transform -from inverse_kinematic_controllers.tpik.constraint import ( +from controllers.tpik_joint_trajectory_controller.constraints import ( EqualityConstraint, SetConstraint, ) +from geometry_msgs.msg import Quaternion, Transform from scipy.spatial.transform import Rotation as R +import angler_kinematics.jacobian as jacobian # type: ignore + + +def calculate_quaternion_error(desired: Quaternion, actual: Quaternion) -> np.ndarray: + """Calculate the error between two quaternions. + + Args: + desired: The desired orientation. + actual: The actual orientation. + + Returns: + The angle between the two quaternions as a quaternion. + """ + + def quat_to_array(quat: Quaternion): + return np.array([quat.x, quat.y, quat.z, quat.w]) + + quat_d = quat_to_array(desired) + quat_a = quat_to_array(actual) + + error_eps = ( + quat_a[3] * quat_d[:3] + - quat_d[3] * quat_a[:3] + + np.cross(quat_a[:3], quat_d[:3]) + ) + error_lambda = quat_d[3] * quat_a[3] + quat_d[:3].T @ quat_a[:3] + + return np.hstack((error_eps, error_lambda)) + class VehicleRollPitchTask(EqualityConstraint): """Control the vehicle roll and pitch angles.""" @@ -129,12 +157,8 @@ def jacobian(self) -> np.ndarray: Returns: The Jacobian for a task which controls the vehicle roll and pitch. """ - J_ko = tpik_utils.calculate_vehicle_angular_velocity_jacobian( - self.rot_map_to_base - ) - J = np.zeros((2, 6 + self.n_manipulator_joints)) - J[:, 3:6] = np.array([[1, 0, 0], [0, 1, 0]]) @ np.linalg.pinv(J_ko) + J[:, :6] = jacobian.calculate_vehicle_roll_pitch_jacobian(self.rot_map_to_base) return J @@ -239,12 +263,8 @@ def jacobian(self) -> np.ndarray: Returns: The Jacobian for a task which controls the vehicle yaw. """ - J_ko = tpik_utils.calculate_vehicle_angular_velocity_jacobian( - self.rot_map_to_base - ) - J = np.zeros((1, 6 + self.n_manipulator_joints)) - J[:, 3:6] = np.array([0, 0, 1]) @ np.linalg.inv(J_ko) + J[:, :6] = jacobian.calculate_vehicle_yaw_jacobian(self.rot_map_to_base) return J @@ -258,113 +278,13 @@ def error(self) -> np.ndarray: return self.desired_value - self.current_value # type: ignore -class VehicleOrientationTask(EqualityConstraint): - """Control the vehicle orientation.""" - - name = "vehicle_orientation_eq" - - def __init__(self, gain: float, priority: float) -> None: - """Create a new vehicle orientation task. - - Args: - gain: The task gain. - priority: The task priority. - """ - super().__init__(gain, priority) - - self.current_rot = Quaternion() - self.desired_rot = Quaternion() - self.rot_base_to_map = Quaternion() - self.n_manipulator_joints = 0 - - @staticmethod - def create_task_from_params( - gain: float, - priority: float, - roll: float | None = None, - pitch: float | None = None, - yaw: float | None = None, - ) -> Any: - """Create a new vehicle orientation task. - - Args: - gain: The task gain. - priority: The task priority. - roll: The desired vehicle roll, if known beforehand. Defaults to - None. - pitch: The desired vehicle pitch, if known beforehand. Defaults to None. - yaw: The desired vehicle yaw, if known beforehand. Defaults to None. - """ - task = VehicleOrientationTask(gain, priority) - - if None not in [roll, pitch, yaw]: - dr = R.from_euler("xyz", [roll, pitch, yaw]) # type: ignore - desired_quat = Quaternion() - ( - desired_quat.x, - desired_quat.y, - desired_quat.z, - desired_quat.w, - ) = dr.as_quat(False) - - task.desired_rot = desired_quat - - return task - - def update( - self, - current_rot: Quaternion, - desired_rot: Quaternion | None = None, - n_manipulator_joints: int | None = None, - ) -> None: - """Update the vehicle orientation context. - - Args: - current_rot: The current vehicle orientation. - desired_rot: The desired vehicle orientation. Defaults to None. - n_manipulator_joints: The total number of manipulator joints. Defaults to - None. - """ - self.rot_base_to_map = current_rot - self.current_rot = current_rot - - if desired_rot is not None: - self.desired_rot = desired_rot - - if n_manipulator_joints is not None: - n_manipulator_joints = n_manipulator_joints - - @property - def jacobian(self) -> np.ndarray: - """Calculate the vehicle orientation Jacobian. - - Returns: - The vehicle orientation Jacobian. - """ - J = np.zeros((3, 6 + self.n_manipulator_joints)) - J[:, 3:6] = tpik_utils.quaternion_to_rotation(self.rot_base_to_map).as_matrix() - - return J - - @property - def error(self) -> np.ndarray: - """Calculate the reference signal for the vehicle orientation. - - Returns: - The reference signal needed to move the vehicle to the desired orientation. - """ - return tpik_utils.calculate_quaternion_error( - self.desired_rot, self.current_rot - )[:3].reshape((3, 1)) - - class EndEffectorPoseTask(EqualityConstraint): """Control the end-effector pose.""" name = "end_effector_pose_eq" def __init__(self, gain: float, priority: float) -> None: - """Create a new end effector pose task. + """Create a new end-effector pose task. Args: gain: The task gain. @@ -391,26 +311,26 @@ def create_task_from_params( pitch: float | None = None, yaw: float | None = None, ) -> Any: - """Create a new end effector pose task from a set of parameters. + """Create a new end-effector pose task from a set of parameters. Args: gain: The task gain. priority: The task priority. - x: The desired end effector x position in the inertial (map) frame. Defaults + x: The desired end-effector x position in the inertial (map) frame. Defaults to None. - y: The desired end effector y position in the inertial (map) frame. Defaults + y: The desired end-effector y position in the inertial (map) frame. Defaults to None. - z: The desired end effector z position in the inertial (map) frame. Defaults + z: The desired end-effector z position in the inertial (map) frame. Defaults to None. - roll: The desired end effector roll in the inertial (map) frame. Defaults + roll: The desired end-effector roll in the inertial (map) frame. Defaults to None. - pitch: The desired end effector pitch in the inertial (map) frame. Defaults + pitch: The desired end-effector pitch in the inertial (map) frame. Defaults to None. - yaw: The desired end effector yaw in the inertial (map) frame. Defaults + yaw: The desired end-effector yaw in the inertial (map) frame. Defaults to None. Returns: - A new end effector pose task. + A new end-effector pose task. """ task = EndEffectorPoseTask(gain, priority) @@ -426,9 +346,7 @@ def create_task_from_params( task.desired_value.rotation.w, # type: ignore ) = R.from_euler( "xyz", [roll, pitch, yaw] # type: ignore - ).as_quat( - False - ) + ).as_quat() return task @@ -442,21 +360,21 @@ def update( serial_chain: Any | None = None, desired_pose: Transform | None = None, ) -> None: - """Update the current context of the end effector pose task. + """Update the current context of the end-effector pose task. Args: joint_angles: The current manipulator joint angles. current_pose: The current vehicle pose in the inertial (map) frame. tf_map_to_ee: The transformation from the inertial (map) frame to the - end effector frame. + end-effector frame. tf_map_to_base: The transformation from the inertial (map) frame to the vehicle base frame. tf_base_to_manipulator_base: The transformation from the vehicle base frame to the manipulator base frame. tf_manipulator_base_to_ee: The transformation from the manipulator base - frame to the end effector frame. + frame to the end-effector frame. serial_chain: The manipulator kinpy serial chain. Defaults to None. - desired_pose: The desired end effector pose. Defaults to None. + desired_pose: The desired end-effector pose. Defaults to None. """ self.joint_angles = joint_angles self.current_value = tf_map_to_ee @@ -477,60 +395,21 @@ def jacobian(self) -> np.ndarray: Returns: The UVMS Jacobian. """ - # Get the transformation translations - # denoted as r_{from frame}{to frame}_{with respect to x frame} - r_B0_B = tpik_utils.point_to_array(self.tf_base_to_manipulator_base.translation) - eta_0ee_0 = tpik_utils.point_to_array( - self.tf_manipulator_base_to_ee.translation - ) - - # Get the transformation rotations - # denoted as R_{from frame}_{to frame} - R_0_B = np.linalg.inv( - tpik_utils.quaternion_to_rotation( - self.tf_base_to_manipulator_base.rotation - ).as_matrix() - ) - R_B_I = np.linalg.inv( - tpik_utils.quaternion_to_rotation(self.tf_map_to_base.rotation).as_matrix() + return jacobian.calculate_uvms_jacobian( + self.tf_base_to_manipulator_base, + self.tf_manipulator_base_to_ee, + self.tf_map_to_base, + len(self.joint_angles), + self.serial_chain, + self.joint_angles, ) - R_0_I = R_B_I @ R_0_B - - r_B0_I = R_B_I @ r_B0_B - eta_0ee_I = R_0_I @ eta_0ee_0 - - def get_skew_matrix(x: np.ndarray) -> np.ndarray: - # Expect a 3x1 vector - return np.array( - [ - [0, -x[2][0], x[1][0]], # type: ignore - [x[2][0], 0, -x[0][0]], - [-x[1][0], x[0][0], 0], - ], - ) - - J = np.zeros((6, 6 + len(self.joint_angles))) - J_man = tpik_utils.calculate_manipulator_jacobian( - self.serial_chain, self.joint_angles - ) - - # Position Jacobian - J[:3, :3] = R_B_I - J[:3, 3:6] = -(get_skew_matrix(r_B0_I) + get_skew_matrix(eta_0ee_I)) @ R_B_I # type: ignore # noqa - J[:3, 6:] = R_0_I @ J_man[:3] - - # Orientation Jacobian - J[3:6, 3:6] = R_B_I - J[3:6, 6:] = R_0_I @ J_man[3:] - - return J @property def error(self) -> np.ndarray: """Calculate the reference signal for the controller. Returns: - The reference signal to use to drive the system to the desired end effector + The reference signal to use to drive the system to the desired end-effector pose. """ pos_error = np.array( @@ -540,7 +419,7 @@ def error(self) -> np.ndarray: self.desired_value.translation.z - self.current_value.translation.z, # type: ignore # noqa ] ).reshape((3, 1)) - ori_error = tpik_utils.calculate_quaternion_error( + ori_error = calculate_quaternion_error( self.desired_value.rotation, self.current_value.rotation # type: ignore )[:3].reshape((3, 1)) @@ -741,7 +620,9 @@ def jacobian(self) -> np.ndarray: The joint configuration Jacobian. """ J = np.zeros((self.n_manipulator_joints, 6 + self.n_manipulator_joints)) - J[0:, 6:] = np.eye(self.n_manipulator_joints) + J[0:, 6:] = jacobian.calculate_joint_configuration_jacobian( + self.n_manipulator_joints + ) return J @@ -762,7 +643,6 @@ def error(self) -> np.ndarray: EndEffectorPoseTask, ManipulatorJointLimitTask, ManipulatorJointConfigurationTask, - VehicleOrientationTask, VehicleRollPitchTask, VehicleYawTask, ] diff --git a/angler_control/inverse_kinematic_controllers/tpik/controller.py b/angler_control/controllers/tpik_joint_trajectory_controller/tpik_controller.py similarity index 74% rename from angler_control/inverse_kinematic_controllers/tpik/controller.py rename to angler_control/controllers/tpik_joint_trajectory_controller/tpik_controller.py index 7ef38e2..0277453 100644 --- a/angler_control/inverse_kinematic_controllers/tpik/controller.py +++ b/angler_control/controllers/tpik_joint_trajectory_controller/tpik_controller.py @@ -21,28 +21,27 @@ import kinpy import numpy as np import rclpy -from geometry_msgs.msg import Transform, Twist -from inverse_kinematic_controllers.tpik.constraint import ( +from controllers.robot_trajectory_controller.base_multidof_joint_trajectory_controller import ( # noqa + BaseMultiDOFJointTrajectoryController, +) +from controllers.tpik_joint_trajectory_controller.constraints import ( EqualityConstraint, SetConstraint, ) -from inverse_kinematic_controllers.tpik.hierarchy import TaskHierarchy -from inverse_kinematic_controllers.tpik.tasks import ( +from controllers.tpik_joint_trajectory_controller.hierarchy import TaskHierarchy +from controllers.tpik_joint_trajectory_controller.tasks import ( EndEffectorPoseTask, ManipulatorJointConfigurationTask, ManipulatorJointLimitTask, - VehicleOrientationTask, VehicleRollPitchTask, VehicleYawTask, ) +from geometry_msgs.msg import Transform, Twist from moveit_msgs.msg import RobotState, RobotTrajectory -from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.duration import Duration from rclpy.executors import MultiThreadedExecutor -from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy, QoSProfile, qos_profile_sensor_data +from rclpy.qos import QoSDurabilityPolicy, QoSProfile from std_msgs.msg import String -from std_srvs.srv import SetBool from tf2_ros import TransformException # type: ignore from tf2_ros import Time from tf2_ros.buffer import Buffer @@ -83,48 +82,50 @@ def construct_augmented_jacobian(jacobians: list[np.ndarray]) -> np.ndarray: return np.vstack(tuple(jacobians)) -class TPIK(Node): - """Task-priority inverse kinematic (TPIK) controller. +class TpikController(BaseMultiDOFJointTrajectoryController): + """Set-based task-priority inverse kinematic (TPIK) controller. The TPIK controller is responsible for calculating kinematically feasible system velocities subject to equality and set-based constraints. + + Only high-priority set-based tasks have been implemented at the moment. """ def __init__(self) -> None: """Create a new TPIK node.""" - super().__init__("tpik") + super().__init__("tpik_joint_trajectory_controller") self.declare_parameter("hierarchy_file", "") - self.declare_parameter("inertial_frame", "map") - self.declare_parameter("base_frame", "base_link") - self.declare_parameter("manipulator_base_link", "alpha_base_link") - self.declare_parameter("manipulator_end_link", "alpha_standard_jaws_tool") - self.declare_parameter("control_rate", 50.0) - - # Keep track of the robot state for the tasks - self.state = RobotState() + self.declare_parameters( + "frames", + parameters=[ # type: ignore + ("inertial_frame", "map"), + ("base_frame", "base_link"), + ("manipulator_base_link", "alpha_base_link"), + ("manipulator_end_link", "alpha_standard_jaws_tool"), + ], + ) # Don't run the controller until everything has been properly initialized self._description_received = False self._init_state_received = False - # Require a service call to arm the controller - self._armed = False - # Keep track of the frames self.inertial_frame = ( - self.get_parameter("inertial_frame").get_parameter_value().string_value + self.get_parameter("frames.inertial_frame") + .get_parameter_value() + .string_value ) self.base_frame = ( - self.get_parameter("base_frame").get_parameter_value().string_value + self.get_parameter("frames.base_frame").get_parameter_value().string_value ) self.manipulator_base_frame = ( - self.get_parameter("manipulator_base_link") + self.get_parameter("frames.manipulator_base_link") .get_parameter_value() .string_value ) self.manipulator_ee_frame = ( - self.get_parameter("manipulator_end_link") + self.get_parameter("frames.manipulator_end_link") .get_parameter_value() .string_value ) @@ -138,17 +139,6 @@ def __init__(self) -> None: self.tf_buffer = Buffer(cache_time=Duration(seconds=1)) self.tf_listener = TransformListener(self.tf_buffer, self) - self.manipulator_base_link_frame = ( - self.get_parameter("manipulator_end_link") - .get_parameter_value() - .string_value - ) - self.manipulator_ee_frame = ( - self.get_parameter("manipulator_end_link") - .get_parameter_value() - .string_value - ) - # Subscribers self.robot_description_sub = self.create_subscription( String, @@ -156,72 +146,52 @@ def __init__(self) -> None: self.read_robot_description_cb, QoSProfile(depth=5, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL), ) - self.robot_state_sub = self.create_subscription( - RobotState, - "/angler/state", - self.robot_state_cb, - qos_profile=qos_profile_sensor_data, - ) # Publishers self.robot_trajectory_pub = self.create_publisher( RobotTrajectory, "/angler/robot_trajectory", 1 ) - # Services - self.arm_controller_srv = self.create_service( - SetBool, "/angler/tpik/arm", self.arm_controller_cb - ) - - # Create a new callback group for the control loop timer - self.timer_cb_group = MutuallyExclusiveCallbackGroup() - - # Create a timer to run the TPIK controller - self.control_timer = self.create_timer( - 1 / self.get_parameter("control_rate").get_parameter_value().double_value, - self.update, - self.timer_cb_group, - ) - @property - def initialized(self) -> bool: - """Check whether or not the controller has been fully initialized. + def joint_state(self) -> MultiDOFJointTrajectoryPoint: + """Get the end-effector pose in the inertial frame. + + We control the end-effector state with this particular variation of the + controller. Returns: - Whether or not the controller has been initialized. + The end-effector pose in the inertial frame. """ - return self._description_received and self._init_state_received - - def arm_controller_cb( - self, request: SetBool.Request, response: SetBool.Response - ) -> SetBool.Response: - """Arm/disarm the TPIK controller. - - Args: - request: The arm/disarm request. - response: The request result. + point = MultiDOFJointTrajectoryPoint() + + # Control the end-effector pose + try: + tf_map_to_ee = self.tf_buffer.lookup_transform( + self.inertial_frame, + self.manipulator_ee_frame, + Time(), + Duration(nanoseconds=10000000), # 10 ms + ) + except TransformException as e: + self.get_logger().warning( + "Failed to get the current transformation from the map to end-effector" + f"frame: {e}" + ) + return point + + point.transforms.append(tf_map_to_ee.transform) # type: ignore + return point + + def on_arm(self) -> bool: + """Make sure that the system has been initialized before enabling arming. + + The controller is considered initialized when the robot description and initial + state have been received. Returns: - The request result. + Whether or not the system has been fully initialized. """ - if request.data: - if self.initialized: - self._armed = True - response.success = self._armed - response.message = "Successfully armed the TPIK controller" - else: - self._armed = False - response.success = self._armed - response.message = ( - "Failed to arm the TPIK controller: controller has not" - " yet been initialized" - ) - else: - self._armed = False - response.success = True - response.message = "Successfully disarmed the TPIK controller" - - return response + return self._description_received and self._init_state_received def read_robot_description_cb(self, robot_description: String) -> None: """Create a kinpy serial chain from the robot description. @@ -230,12 +200,12 @@ def read_robot_description_cb(self, robot_description: String) -> None: robot_description: The robot description message. """ end_link = ( - self.get_parameter("manipulator_end_link") + self.get_parameter("frames.manipulator_end_link") .get_parameter_value() .string_value ) start_link = ( - self.get_parameter("manipulator_base_link") + self.get_parameter("frames.manipulator_base_link") .get_parameter_value() .string_value ) @@ -258,7 +228,6 @@ def read_robot_description_cb(self, robot_description: String) -> None: task.serial_chain = self.serial_chain # type: ignore self._description_received = True - self.get_logger().debug("Robot description received!") def calculate_system_velocity( @@ -318,15 +287,19 @@ def calculate_system_velocity_rec( hierarchy, system_velocities, jacobians, nullspace ) - def update(self) -> None: - """Calculate and send the desired system velocities.""" - if not self._armed: - return - - self.update_context() + def on_update(self) -> None: + """Calculate the desired system velocities using set-based TPIK.""" + # Update the joint command first + super().on_update() hierarchies = self.hierarchy.hierarchies + # Set the desired trajectory value + for hierarchy in hierarchies: + for task in hierarchy: + if isinstance(task, EndEffectorPoseTask) and self.command is not None: + task.desired_value = self.command.transforms[0] # type: ignore + # Check whether or not the hierarchies have any set tasks has_set_tasks = any( [any([isinstance(y, SetConstraint) for y in x]) for x in hierarchies] @@ -425,6 +398,7 @@ def get_robot_trajectory_from_velocities( # Create the full system command trajectory.multi_dof_joint_trajectory.joint_names = ["vehicle"] + # TODO(evan): don't hard-code this trajectory.joint_trajectory.joint_names = [ "alpha_axis_a" ] + self.serial_chain.get_joint_parameter_names()[::-1] @@ -433,23 +407,21 @@ def get_robot_trajectory_from_velocities( return trajectory - def robot_state_cb(self, state: RobotState) -> None: - """Update the current robot state. + def on_robot_state_update(self, state: RobotState) -> None: + """Update the current task contexts. Args: - state: The current robot state. + state: The current state of the robot. """ - self.state = state - self._init_state_received = True - self.update_context() + # Indicate that the initial robot state has been received + if not self._init_state_received: + self._init_state_received = True - def update_context(self) -> None: - """Update the current state variables for each task.""" # Get some of the more commonly used state variables # The joint state includes the linear jaws joint angle. We exclude this, # because we aren't controlling it within this specific framework. - joint_angles = np.array(self.state.joint_state.position[1:][::-1]) - vehicle_pose: Transform = self.state.multi_dof_joint_state.transforms[0] # type: ignore # noqa + joint_angles = np.array(state.joint_state.position[1:][::-1]) + vehicle_pose: Transform = state.multi_dof_joint_state.transforms[0] # type: ignore # noqa for task in self.hierarchy.tasks: if isinstance(task, ManipulatorJointLimitTask): @@ -462,19 +434,26 @@ def update_context(self) -> None: task.update(joint_angles.reshape((len(joint_angles), 1))) elif isinstance(task, VehicleYawTask): task.update(vehicle_pose.rotation) - elif isinstance(task, VehicleOrientationTask): - task.update(vehicle_pose.rotation) elif isinstance(task, EndEffectorPoseTask): # Get the necessary transforms try: tf_map_to_ee = self.tf_buffer.lookup_transform( - self.inertial_frame, self.manipulator_ee_frame, Time() + self.inertial_frame, + self.manipulator_ee_frame, + Time(), + Duration(nanoseconds=10000000), # 10 ms ) tf_base_to_manipulator_base = self.tf_buffer.lookup_transform( - self.base_frame, self.manipulator_base_frame, Time() + self.base_frame, + self.manipulator_base_frame, + Time(), + Duration(nanoseconds=10000000), # 10 ms ) tf_manipulator_base_to_ee = self.tf_buffer.lookup_transform( - self.manipulator_base_frame, self.manipulator_ee_frame, Time() + self.manipulator_base_frame, + self.manipulator_ee_frame, + Time(), + Duration(nanoseconds=10000000), # 10 ms ) except TransformException as e: self.get_logger().error( @@ -495,7 +474,7 @@ def main(args: list[str] | None = None): """Run the TPIK controller.""" rclpy.init(args=args) - node = TPIK() + node = TpikController() executor = MultiThreadedExecutor() rclpy.spin(node, executor) diff --git a/angler_control/inverse_kinematic_controllers/tpik/utils.py b/angler_control/inverse_kinematic_controllers/tpik/utils.py deleted file mode 100644 index 2831e44..0000000 --- a/angler_control/inverse_kinematic_controllers/tpik/utils.py +++ /dev/null @@ -1,118 +0,0 @@ -# Copyright 2023, Evan Palmer -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. - -from typing import Any - -import numpy as np -from geometry_msgs.msg import Point, Quaternion, Vector3 -from scipy.spatial.transform import Rotation as R - - -def quaternion_to_rotation(quat: Quaternion) -> R: - """Convert a Quaternion message into a SciPy Rotation. - - Args: - quat: The Quaternion message to convert. - - Returns: - The resulting SciPy Rotation. - """ - return R.from_quat([quat.x, quat.y, quat.z, quat.w]) - - -def point_to_array(point: Point | Vector3) -> np.ndarray: - """Convert a Point message into a numpy array. - - Args: - point: The Point message to convert. - - Returns: - The resulting point as a 3x1 numpy array. - """ - return np.array([point.x, point.y, point.z]).reshape((3, 1)) - - -def calculate_quaternion_error(desired: Quaternion, actual: Quaternion) -> np.ndarray: - """Calculate the error between two quaternions. - - Args: - desired: The desired orientation. - actual: The actual orientation. - - Returns: - The angle between the two quaternions as a quaternion. - """ - - def quat_to_array(quat: Quaternion): - return np.array([quat.x, quat.y, quat.z, quat.w]) - - quat_d = quat_to_array(desired) - quat_a = quat_to_array(actual) - - error_eps = ( - quat_a[3] * quat_d[:3] - - quat_d[3] * quat_a[:3] - + np.cross(quat_a[:3], quat_d[:3]) - ) - error_lambda = quat_d[3] * quat_a[3] + quat_d[:3].T @ quat_a[:3] - - return np.hstack((error_eps, error_lambda)) - - -def calculate_vehicle_angular_velocity_jacobian( - rot_map_to_base: Quaternion, -) -> np.ndarray: - """Calculate the angular velocity Jacobian for the vehicle. - - This is defined by Gianluca Antonelli in his textbook "Underwater Robotics" in - Equation 2.3, and is denoted there as "J_k,o". - - Args: - rot_map_to_base: The quaternion describing the rotation from the inertial frame - to the vehicle-fixed frame (map -> base_link). - - Returns: - The vehicle's angular velocity Jacobian. - """ - rot = quaternion_to_rotation(rot_map_to_base) - roll, pitch, _ = rot.as_euler("xyz") - - return np.array( - [ - [1, 0, -np.sin(pitch)], - [0, np.cos(roll), np.cos(pitch) * np.sin(roll)], - [0, -np.sin(roll), np.cos(pitch) * np.cos(roll)], - ] # type: ignore - ) - - -def calculate_manipulator_jacobian( - serial_chain: Any, joint_angles: np.ndarray -) -> np.ndarray: - """Generate the manipulator Jacobian using a kinpy serial chain. - - Args: - serial_chain: The kinpy serial chain. - joint_angles: The current joint angles. - - Returns: - The manipulator Jacobian matrix. - """ - return np.array(serial_chain.jacobian(joint_angles)) diff --git a/angler_control/launch/control.launch.py b/angler_control/launch/control.launch.py index 26d5e61..24fd30d 100644 --- a/angler_control/launch/control.launch.py +++ b/angler_control/launch/control.launch.py @@ -49,9 +49,9 @@ def generate_launch_description() -> LaunchDescription: ), DeclareLaunchArgument( "controller", - default_value="tpik", + default_value="tpik_joint_trajectory_controller", description="The whole-body controller to load.", - choices=["tpik"], + choices=["tpik_joint_trajectory_controller"], ), ] @@ -71,7 +71,11 @@ def generate_launch_description() -> LaunchDescription: "use_sim_time": LaunchConfiguration("use_sim_time"), } ], - condition=IfCondition(PythonExpression(["'", controller, "' == 'tpik'"])), + condition=IfCondition( + PythonExpression( + ["'", controller, "' == 'tpik_joint_trajectory_controller'"] + ) + ), ), ] diff --git a/angler_control/package.xml b/angler_control/package.xml index e9ea4ca..965562f 100644 --- a/angler_control/package.xml +++ b/angler_control/package.xml @@ -1,12 +1,17 @@ + angler_control 0.0.1 A collection of whole-body controllers. + Evan Palmer MIT + https://github.com/evan-palmer/blue.git + https://github.com/evan-palmer/blue/issues + python3-scipy python3-numpy diff --git a/angler_control/setup.py b/angler_control/setup.py index ac919ed..e7ba184 100644 --- a/angler_control/setup.py +++ b/angler_control/setup.py @@ -43,7 +43,7 @@ tests_require=["pytest"], entry_points={ "console_scripts": [ - "tpik = inverse_kinematic_controllers.tpik.controller:main", + "tpik_joint_trajectory_controller = controllers.tpik_joint_trajectory_controller.tpik_controller:main", # noqa ], }, ) diff --git a/angler_control/test/test_flake8.py b/angler_control/test/test_flake8.py deleted file mode 100644 index f494570..0000000 --- a/angler_control/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import pytest -from ament_flake8.main import main_with_errors - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len( - errors - ) + "\n".join(errors) diff --git a/angler_description/config/bluerov2_heavy_alpha/ardusub.parm b/angler_description/config/bluerov2_heavy_alpha/ardusub.parm index 4cbf404..59cc543 100644 --- a/angler_description/config/bluerov2_heavy_alpha/ardusub.parm +++ b/angler_description/config/bluerov2_heavy_alpha/ardusub.parm @@ -62,23 +62,18 @@ INS_ACC3SCAL_Y 1.000 INS_ACC3SCAL_Z 1.000 FRAME_CONFIG 2.000 -# Use EK3 -EK2_ENABLE 0 -EK3_ENABLE 1 -AHRS_EKF_TYPE 3 +# Enable external visual navigation +EK2_ENABLE 0.000 +EK3_ENABLE 1.000 +AHRS_EKF_TYPE 3.000 -# Enable external nav -# From https://ardupilot.org/copter/docs/common-vio-tracking-camera.html -GPS_TYPE 0 # Disable GPS -VISO_TYPE 1 # External vision -EK3_SRC1_POSXY 6 # External nav -EK3_SRC1_VELXY 6 # External nav -EK3_SRC1_POSZ 1 # Baro is the primary z source -EK3_SRC1_VELZ 6 # External nav z velocity influences EKF -COMPASS_USE 0 # Disable compass 1 -COMPASS_USE2 0 # Disable compass 2 -COMPASS_USE3 0 # Disable compass 3 -EK3_SRC1_YAW 6 # External nav +VISO_TYPE 1.000 +EK3_GPS_TYPE 3.000 +EK3_SRC1_POSXY 6.000 +EK3_SRC1_VELXY 6.000 +EK3_SRC1_POSZ 1.000 +EK3_SRC1_YAW 6.000 +EK3_SRC1_VELZ 6.000 -# Disable noisy barometer SITL setting +# See https://github.com/clydemcqueen/bluerov2_ignition/issues/7 SIM_BARO_RND 0.01 diff --git a/angler_description/config/bluerov2_heavy_alpha/controllers.yaml b/angler_description/config/bluerov2_heavy_alpha/controllers.yaml index ae0a379..3ab2305 100644 --- a/angler_description/config/bluerov2_heavy_alpha/controllers.yaml +++ b/angler_description/config/bluerov2_heavy_alpha/controllers.yaml @@ -96,5 +96,6 @@ feedback_joint_velocity_trajectory_controller: tpik: ros__parameters: - manipulator_base_link: "alpha_base_link" - manipulator_end_link: "alpha_standard_jaws_tool" + frames: + manipulator_base_link: "alpha_base_link" + manipulator_end_link: "alpha_standard_jaws_tool" diff --git a/angler_description/config/bluerov2_heavy_alpha/planning.yaml b/angler_description/config/bluerov2_heavy_alpha/planning.yaml index fd64504..9e479b8 100644 --- a/angler_description/config/bluerov2_heavy_alpha/planning.yaml +++ b/angler_description/config/bluerov2_heavy_alpha/planning.yaml @@ -1,3 +1,3 @@ preplanned_end_effector_waypoint_planner: ros__parameters: - trajectory_name: "straight" + trajectory_name: "forward" diff --git a/angler_description/config/bluerov2_heavy_alpha/tasks.yaml b/angler_description/config/bluerov2_heavy_alpha/tasks.yaml index 53bacea..e5fb1cb 100644 --- a/angler_description/config/bluerov2_heavy_alpha/tasks.yaml +++ b/angler_description/config/bluerov2_heavy_alpha/tasks.yaml @@ -18,9 +18,12 @@ gain: 0.3 joint: 6 # axis e +# This task is required for trajectory tracking - task: "end_effector_pose_eq" priority: 2 gain: 0.3 + # Set the initial position to move to + # when the controller is armed x: 0.0 y: 0.0 z: -1.0 diff --git a/angler_description/package.xml b/angler_description/package.xml index 77266a7..c353366 100644 --- a/angler_description/package.xml +++ b/angler_description/package.xml @@ -16,8 +16,6 @@ ament_cmake - gz_ros2_control - ament_lint_auto ament_cmake_copyright diff --git a/angler_kinematics/LICENSE b/angler_kinematics/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/angler_kinematics/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/angler_kinematics/angler_kinematics/__init__.py b/angler_kinematics/angler_kinematics/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/angler_kinematics/angler_kinematics/jacobian.py b/angler_kinematics/angler_kinematics/jacobian.py new file mode 100644 index 0000000..af20253 --- /dev/null +++ b/angler_kinematics/angler_kinematics/jacobian.py @@ -0,0 +1,229 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +from typing import Any + +import numpy as np +from geometry_msgs.msg import Point, Quaternion, Transform, Vector3 +from scipy.spatial.transform import Rotation as R + + +def quaternion_to_rotation(quat: Quaternion) -> R: + """Convert a Quaternion message into a SciPy Rotation. + + Args: + quat: The Quaternion message to convert. + + Returns: + The resulting SciPy Rotation. + """ + return R.from_quat([quat.x, quat.y, quat.z, quat.w]) + + +def point_to_array(point: Point | Vector3) -> np.ndarray: + """Convert a Point message into a numpy array. + + Args: + point: The Point message to convert. + + Returns: + The resulting point as a 3x1 numpy array. + """ + return np.array([point.x, point.y, point.z]).reshape((3, 1)) + + +def get_skew_matrix(x: np.ndarray) -> np.ndarray: + """Convert a 3x1 matrix of coefficients into a skew-symmetric matrix. + + Args: + x: The matrix to convert. + + Returns: + A skew-symmetric matrix. + """ + return np.array( + [ + [0, -x[2][0], x[1][0]], # type: ignore + [x[2][0], 0, -x[0][0]], + [-x[1][0], x[0][0], 0], + ], + ) + + +def calculate_vehicle_angular_velocity_jacobian( + rot_map_to_base: Quaternion, +) -> np.ndarray: + """Calculate the angular velocity Jacobian for the vehicle. + + This is defined by Gianluca Antonelli in his textbook "Underwater Robotics" in + Equation 2.3, and is denoted there as "J_k,o". + + Args: + rot_map_to_base: The quaternion describing the rotation from the inertial frame + to the vehicle-fixed frame (map -> base_link). + + Returns: + The vehicle's angular velocity Jacobian. + """ + rot = quaternion_to_rotation(rot_map_to_base) + roll, pitch, _ = rot.as_euler("xyz") + + return np.array( + [ + [1, 0, -np.sin(pitch)], + [0, np.cos(roll), np.cos(pitch) * np.sin(roll)], + [0, -np.sin(roll), np.cos(pitch) * np.cos(roll)], + ] # type: ignore + ) + + +def calculate_manipulator_jacobian( + serial_chain: Any, joint_angles: np.ndarray +) -> np.ndarray: + """Generate the manipulator Jacobian using a kinpy serial chain. + + Args: + serial_chain: The kinpy serial chain. + joint_angles: The current joint angles. + + Returns: + The 6xn manipulator Jacobian matrix. + """ + return np.array(serial_chain.jacobian(joint_angles)) + + +def calculate_vehicle_roll_pitch_jacobian(rot_map_to_base: Quaternion) -> np.ndarray: + """Calculate the vehicle roll-pitch Jacobian. + + Args: + rot_map_to_base: A quaternion which defines the rotation from the map frame to + the base frame. + + Returns: + The 2x6 vehicle roll-pitch Jacobian. + """ + J_ko = calculate_vehicle_angular_velocity_jacobian(rot_map_to_base) + + J = np.zeros((2, 6)) + J[:, 3:6] = np.array([[1, 0, 0], [0, 1, 0]]) @ np.linalg.pinv(J_ko) + + return J + + +def calculate_vehicle_yaw_jacobian(rot_map_to_base: Quaternion) -> np.ndarray: + """Calculate the vehicle yaw Jacobian. + + Args: + rot_map_to_base: A quaternion which defines the rotation from the map frame to + the base frame. + + Returns: + The 1x6 vehicle yaw Jacobian. + """ + J_ko = calculate_vehicle_angular_velocity_jacobian(rot_map_to_base) + + J = np.zeros((1, 6)) + J[:, 3:6] = np.array([0, 0, 1]) @ np.linalg.inv(J_ko) + + return J + + +def calculate_vehicle_orientation_jacobian(rot_base_to_map: Quaternion) -> np.ndarray: + """Calculate the vehicle orientation Jacobian. + + Args: + rot_base_to_map: A quaternion which defines the rotation from the base frame to + the map frame. + + Returns: + The 3x6 vehicle orientation Jacobian + """ + J = np.zeros((3, 6)) + J[:, 3:6] = quaternion_to_rotation(rot_base_to_map).as_matrix() + + return J + + +def calculate_uvms_jacobian( + tf_base_to_manipulator_base: Transform, + tf_manipulator_base_to_ee: Transform, + tf_map_to_base: Transform, + n_manipulator_joints: int, + serial_chain: Any, + joint_angles: np.ndarray, +) -> np.ndarray: + """Calculate the full UVMS Jacobian. + + Args: + tf_base_to_manipulator_base: A Transform describing the transformation from the + base frame to the manipulator base frame with respect to the base frame. + tf_manipulator_base_to_ee: A Transform describing the transformation from the + manipulator base frame to the end-effector frame with respect to the + manipulator base frame. + tf_map_to_base: A Transform describing the transformation from the + map frame to the vehicle base frame with respect to the map frame. + n_manipulator_joints: The number of joints that the manipulator has. + serial_chain: The kinpy serial chain. + joint_angles: The current manipulator joint angles. + + Returns: + The 3x6+n UVMS Jacobian. + """ + # Get the transformation translations + # denoted as r_{from frame}{to frame}_{with respect to x frame} + r_B0_B = point_to_array(tf_base_to_manipulator_base.translation) + eta_0ee_0 = point_to_array(tf_manipulator_base_to_ee.translation) + + # Get the transformation rotations + # denoted as R_{from frame}_{to frame} + R_0_B = np.linalg.inv( + quaternion_to_rotation(tf_base_to_manipulator_base.rotation).as_matrix() + ) + R_B_I = np.linalg.inv(quaternion_to_rotation(tf_map_to_base.rotation).as_matrix()) + R_0_I = R_B_I @ R_0_B + + r_B0_I = R_B_I @ r_B0_B + eta_0ee_I = R_0_I @ eta_0ee_0 + + J = np.zeros((6, 6 + n_manipulator_joints)) + J_man = calculate_manipulator_jacobian(serial_chain, joint_angles) + + # Position Jacobian + J[:3, :3] = R_B_I + J[:3, 3:6] = -(get_skew_matrix(r_B0_I) + get_skew_matrix(eta_0ee_I)) @ R_B_I # type: ignore # noqa + J[:3, 6:] = R_0_I @ J_man[:3] + + # Orientation Jacobian + J[3:6, 3:6] = R_B_I + J[3:6, 6:] = R_0_I @ J_man[3:] + + return J + + +def calculate_joint_configuration_jacobian(n_manipulator_joints: int) -> np.ndarray: + """Calculate the joint configuration Jacobian. + + Args: + n_manipulator_joints: The total number of joints that the manipulator has. + + Returns: + The nxn manipulator configuration Jacobian. + """ + return np.eye(n_manipulator_joints) diff --git a/angler_kinematics/package.xml b/angler_kinematics/package.xml new file mode 100644 index 0000000..d90abbb --- /dev/null +++ b/angler_kinematics/package.xml @@ -0,0 +1,27 @@ + + + + + angler_kinematics + 0.0.1 + Interfaces for calculating the system kinematics. + + Evan Palmer + MIT + + https://github.com/evan-palmer/blue.git + https://github.com/evan-palmer/blue/issues + + python3-scipy + python3-numpy + geometry_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/angler_kinematics/resource/angler_kinematics b/angler_kinematics/resource/angler_kinematics new file mode 100644 index 0000000..e69de29 diff --git a/angler_kinematics/setup.cfg b/angler_kinematics/setup.cfg new file mode 100644 index 0000000..83567c4 --- /dev/null +++ b/angler_kinematics/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/angler_kinematics +[install] +install_scripts=$base/lib/angler_kinematics diff --git a/angler_kinematics/setup.py b/angler_kinematics/setup.py new file mode 100644 index 0000000..81d33a0 --- /dev/null +++ b/angler_kinematics/setup.py @@ -0,0 +1,42 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +from setuptools import find_packages, setup + +package_name = "angler_kinematics" + +setup( + name=package_name, + version="0.0.1", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Evan Palmer", + maintainer_email="evanp922gmail.com", + description="Interfaces for calculating the system kinematics.", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [], + }, +) diff --git a/angler_utils/test/test_pep257.py b/angler_kinematics/test/test_copyright.py similarity index 69% rename from angler_utils/test/test_pep257.py rename to angler_kinematics/test/test_copyright.py index 4eddb46..8f18fa4 100644 --- a/angler_utils/test/test_pep257.py +++ b/angler_kinematics/test/test_copyright.py @@ -13,11 +13,15 @@ # limitations under the License. import pytest -from ament_pep257.main import main +from ament_copyright.main import main +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) +@pytest.mark.copyright @pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): +def test_copyright(): rc = main(argv=[".", "test"]) - assert rc == 0, "Found code style errors / warnings" + assert rc == 0, "Found errors" diff --git a/angler_mux/demux/single_manipulator_demux.py b/angler_mux/demux/single_manipulator_demux.py index fcbf11e..7b7bc49 100644 --- a/angler_mux/demux/single_manipulator_demux.py +++ b/angler_mux/demux/single_manipulator_demux.py @@ -37,9 +37,12 @@ def __init__(self) -> None: """Create a new demuxer for the vehicle and a manipulator.""" super().__init__("single_manipulator_velocity_demux") - self.declare_parameter("vehicle_control_topic", "/blue/ismc/cmd_vel") - self.declare_parameter( - "manipulator_control_topic", "/forward_velocity_controller/commands" + self.declare_parameters( + namespace="", + parameters=[ # type: ignore + ("vehicle_control_topic", "/blue/ismc/cmd_vel"), + ("manipulator_control_topic", "/forward_velocity_controller/commands"), + ], ) vehicle_control_topic = ( diff --git a/angler_mux/test/test_flake8.py b/angler_mux/test/test_flake8.py deleted file mode 100644 index f494570..0000000 --- a/angler_mux/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import pytest -from ament_flake8.main import main_with_errors - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len( - errors - ) + "\n".join(errors) diff --git a/angler_planning/planners/__init__.py b/angler_planning/planners/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/angler_planning/waypoint_planners/base_planner.py b/angler_planning/planners/base_planner.py similarity index 91% rename from angler_planning/waypoint_planners/base_planner.py rename to angler_planning/planners/base_planner.py index e95e1d3..0fa207a 100644 --- a/angler_planning/waypoint_planners/base_planner.py +++ b/angler_planning/planners/base_planner.py @@ -24,11 +24,11 @@ from rclpy.node import Node -class WaypointPlanner(Node, ABC): - """Base class for a waypoint planner.""" +class Planner(Node, ABC): + """Base class for a planner.""" def __init__(self, node_name: str) -> None: - """Create a new waypoint planner. + """Create a new planner. Args: node_name: The name of the ROS node. @@ -38,7 +38,7 @@ def __init__(self, node_name: str) -> None: ABC.__init__(self) self.planning_srv = self.create_service( - GetMotionPlan, f"~/angler/{node_name}/get_motion_plan", self.plan + GetMotionPlan, f"/angler/{node_name}/plan", self.plan ) @abstractmethod diff --git a/angler_planning/planners/waypoint_planners/__init__.py b/angler_planning/planners/waypoint_planners/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/__init__.py b/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/angler_planning/waypoint_planners/preplanned_end_effector_planner/planner.py b/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/planner.py similarity index 81% rename from angler_planning/waypoint_planners/preplanned_end_effector_planner/planner.py rename to angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/planner.py index d797c49..4807f14 100644 --- a/angler_planning/waypoint_planners/preplanned_end_effector_planner/planner.py +++ b/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/planner.py @@ -24,27 +24,32 @@ from ament_index_python.packages import get_package_share_directory from moveit_msgs.msg import MoveItErrorCodes from moveit_msgs.srv import GetMotionPlan -from waypoint_planners.base_planner import WaypointPlanner -from waypoint_planners.preplanned_end_effector_planner.trajectories.trajectory_library import ( # noqa +from planners.base_planner import Planner +from planners.waypoint_planners.preplanned_end_effector_planner.trajectories.trajectory_library import ( # noqa TrajectoryLibrary as tl, ) -class PrePlannedEndEffectorPlanner(WaypointPlanner): +class PrePlannedEndEffectorPlanner(Planner): """Planning interface for loading pre-planned end-effector trajectories.""" def __init__(self) -> None: """Create a new planning interface.""" super().__init__("preplanned_end_effector_waypoint_planner") - self.declare_parameter("trajectory_name", "") - self.declare_parameter( - "library_path", - os.path.join( - get_package_share_directory("angler_planning"), - "trajectories", - "library", - ), + self.declare_parameters( + namespace="", + parameters=[ # type: ignore + ("trajectory_name", ""), + ( + "library_path", + os.path.join( + get_package_share_directory("angler_planning"), + "trajectories", + "library", + ), + ), + ], ) trajectory_name = ( @@ -79,7 +84,9 @@ def plan( # Get the motion plan response response.motion_plan_response.trajectory = self.trajectory response.motion_plan_response.planning_time = 0.0 - response.motion_plan_response.error_code = MoveItErrorCodes.SUCCESS + response.motion_plan_response.error_code = MoveItErrorCodes( + val=MoveItErrorCodes.SUCCESS + ) response.motion_plan_response.group_name = ( request.motion_plan_request.group_name ) diff --git a/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/__init__.py b/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/diagonal_left.json b/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/diagonal_left.json new file mode 100644 index 0000000..6ef0c45 --- /dev/null +++ b/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/diagonal_left.json @@ -0,0 +1,29 @@ +{ + "name": "diagonal_left", + "parent": "map", + "child": "alpha_standard_jaws_tool", + "waypoints": [ + { + "time_from_start": 5.0, + "transform": { + "x": 0.0, + "y": 0.0, + "z": -1.0, + "rx": 0.0, + "ry": 0.0, + "rz": 0.0 + } + }, + { + "time_from_start": 10.0, + "transform": { + "x": 1.5, + "y": 1.5, + "z": -1.0, + "rx": 0.0, + "ry": 0.0, + "rz": 0.0 + } + } + ] +} diff --git a/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/diagonal_right.json b/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/diagonal_right.json new file mode 100644 index 0000000..5484bef --- /dev/null +++ b/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/diagonal_right.json @@ -0,0 +1,29 @@ +{ + "name": "diagonal_right", + "parent": "map", + "child": "alpha_standard_jaws_tool", + "waypoints": [ + { + "time_from_start": 5.0, + "transform": { + "x": 0.0, + "y": 0.0, + "z": -1.0, + "rx": 0.0, + "ry": 0.0, + "rz": 0.0 + } + }, + { + "time_from_start": 10.0, + "transform": { + "x": 1.5, + "y": -1.5, + "z": -1.0, + "rx": 0.0, + "ry": 0.0, + "rz": 0.0 + } + } + ] +} diff --git a/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/forward.json b/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/forward.json new file mode 100644 index 0000000..fb94633 --- /dev/null +++ b/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/forward.json @@ -0,0 +1,29 @@ +{ + "name": "forward", + "parent": "map", + "child": "alpha_standard_jaws_tool", + "waypoints": [ + { + "time_from_start": 5.0, + "transform": { + "x": 0.0, + "y": 0.0, + "z": -1.0, + "rx": 0.0, + "ry": 0.0, + "rz": 0.0 + } + }, + { + "time_from_start": 10.0, + "transform": { + "x": 2.0, + "y": 0.0, + "z": -1.0, + "rx": 0.0, + "ry": 0.0, + "rz": 0.0 + } + } + ] +} diff --git a/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/lawn_mower.json b/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/lawn_mower.json new file mode 100644 index 0000000..66934a3 --- /dev/null +++ b/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/lawn_mower.json @@ -0,0 +1,95 @@ +{ + "name": "lawn_mower", + "parent": "map", + "child": "alpha_standard_jaws_tool", + "waypoints": [ + { + "time_from_start": 5.0, + "transform": { + "x": 0.0, + "y": 0.0, + "z": -1.0, + "rx": 0.0, + "ry": 0.0, + "rz": 0.0 + } + }, + { + "time_from_start": 10.0, + "transform": { + "x": 0.5, + "y": 0.0, + "z": -1.0, + "rx": 0.0, + "ry": 0.0, + "rz": 0.0 + } + }, + { + "time_from_start": 10.0, + "transform": { + "x": 0.5, + "y": 0.5, + "z": -1.0, + "rx": 0.0, + "ry": 0.0, + "rz": 0.0 + } + }, + { + "time_from_start": 10.0, + "transform": { + "x": 1.0, + "y": 0.5, + "z": -1.0, + "rx": 0.0, + "ry": 0.0, + "rz": 0.0 + } + }, + { + "time_from_start": 10.0, + "transform": { + "x": 1.0, + "y": -0.5, + "z": -1.0, + "rx": 0.0, + "ry": 0.0, + "rz": 0.0 + } + }, + { + "time_from_start": 10.0, + "transform": { + "x": 1.5, + "y": -0.5, + "z": -1.0, + "rx": 0.0, + "ry": 0.0, + "rz": 0.0 + } + }, + { + "time_from_start": 10.0, + "transform": { + "x": 1.5, + "y": 0.0, + "z": -1.0, + "rx": 0.0, + "ry": 0.0, + "rz": 0.0 + } + }, + { + "time_from_start": 10.0, + "transform": { + "x": 2.0, + "y": 0.0, + "z": -1.0, + "rx": 0.0, + "ry": 0.0, + "rz": 0.0 + } + } + ] +} diff --git a/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/left.json b/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/left.json new file mode 100644 index 0000000..84c59f1 --- /dev/null +++ b/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/left.json @@ -0,0 +1,29 @@ +{ + "name": "left", + "parent": "map", + "child": "alpha_standard_jaws_tool", + "waypoints": [ + { + "time_from_start": 5.0, + "transform": { + "x": 0.0, + "y": 0.0, + "z": -1.0, + "rx": 0.0, + "ry": 0.0, + "rz": 0.0 + } + }, + { + "time_from_start": 10.0, + "transform": { + "x": 0.0, + "y": 2.0, + "z": -1.0, + "rx": 0.0, + "ry": 0.0, + "rz": 0.0 + } + } + ] +} diff --git a/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/right.json b/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/right.json new file mode 100644 index 0000000..edd25da --- /dev/null +++ b/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/right.json @@ -0,0 +1,29 @@ +{ + "name": "right", + "parent": "map", + "child": "alpha_standard_jaws_tool", + "waypoints": [ + { + "time_from_start": 5.0, + "transform": { + "x": 0.0, + "y": 0.0, + "z": -1.0, + "rx": 0.0, + "ry": 0.0, + "rz": 0.0 + } + }, + { + "time_from_start": 10.0, + "transform": { + "x": 0.0, + "y": -2.0, + "z": -1.0, + "rx": 0.0, + "ry": 0.0, + "rz": 0.0 + } + } + ] +} diff --git a/angler_planning/waypoint_planners/preplanned_end_effector_planner/trajectories/trajectory_library.py b/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/trajectory_library.py similarity index 81% rename from angler_planning/waypoint_planners/preplanned_end_effector_planner/trajectories/trajectory_library.py rename to angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/trajectory_library.py index eeaf85f..c5b9fa6 100644 --- a/angler_planning/waypoint_planners/preplanned_end_effector_planner/trajectories/trajectory_library.py +++ b/angler_planning/planners/waypoint_planners/preplanned_end_effector_planner/trajectories/trajectory_library.py @@ -28,7 +28,7 @@ def create_waypoint_msg( - waypoint: dict[str, dict[str, float]] + waypoint: dict[str, dict[str, float] | float] ) -> MultiDOFJointTrajectoryPoint: """Create a new Waypoint message from the JSON data. @@ -40,50 +40,69 @@ def create_waypoint_msg( """ point = MultiDOFJointTrajectoryPoint() + # Get the time-since-start for the point + point.time_from_start.nanosec = int( + waypoint["time_from_start"] * 1e9 # type: ignore + ) + # Get the desired pose tf = Transform() tf.translation.x, tf.translation.y, tf.translation.z, rx, ry, rz = waypoint[ "transform" - ].values() + ].values() # type: ignore tf.rotation.x, tf.rotation.y, tf.rotation.z, tf.rotation.w = R.from_euler( "xyz", [rx, ry, rz] - ).as_quat(False) + ).as_quat() + + point.transforms = [tf] # Get the desired velocity vel = Twist() - ( - vel.linear.x, - vel.linear.y, - vel.linear.z, - vel.angular.x, - vel.angular.y, - vel.angular.z, - ) = waypoint["velocity"].values() + try: + ( + vel.linear.x, + vel.linear.y, + vel.linear.z, + vel.angular.x, + vel.angular.y, + vel.angular.z, + ) = waypoint[ + "velocity" + ].values() # type: ignore + + point.velocities = [vel] + except KeyError: + # If the velocity isn't provided, don't use it + ... # Finally, get the acceleration accel = Twist() - ( - accel.linear.x, - accel.linear.y, - accel.linear.z, - accel.angular.x, - accel.angular.y, - accel.angular.z, - ) = waypoint["acceleration"].values() - - point.transforms = [point] - point.velocities = [vel] - point.accelerations = [accel] + try: + ( + accel.linear.x, + accel.linear.y, + accel.linear.z, + accel.angular.x, + accel.angular.y, + accel.angular.z, + ) = waypoint[ + "acceleration" + ].values() # type: ignore + + point.accelerations = [accel] + except KeyError: + # Similar to velocity, we don't require acceleration + ... return point def create_robot_trajectory_msg( - parent: str, child: str, waypoints: list[dict[str, dict[str, float]]] + parent: str, child: str, waypoints: list[dict[str, dict[str, float] | float]] ) -> RobotTrajectory: """Creata robot trajectory from JSON parameters. diff --git a/angler_planning/setup.py b/angler_planning/setup.py index 1d64e50..7706180 100644 --- a/angler_planning/setup.py +++ b/angler_planning/setup.py @@ -36,7 +36,7 @@ ( os.path.join("share", package_name, "trajectories", "library"), glob( - "waypoint_planners/preplanned_end_effector_planner/trajectories/library/*.json" # noqa + "planners/waypoint_planners/preplanned_end_effector_planner/trajectories/library/*.json" # noqa ), ), ( @@ -55,7 +55,7 @@ tests_require=["pytest"], entry_points={ "console_scripts": [ - "preplanned_end_effector_waypoint_planner = waypoint_planners.preplanned_end_effector_planner.planner:main", # noqa + "preplanned_end_effector_waypoint_planner = planners.waypoint_planners.preplanned_end_effector_planner.planner:main", # noqa" ], }, ) diff --git a/angler_planning/test/test_flake8.py b/angler_planning/test/test_flake8.py deleted file mode 100644 index f494570..0000000 --- a/angler_planning/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import pytest -from ament_flake8.main import main_with_errors - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len( - errors - ) + "\n".join(errors) diff --git a/angler_planning/waypoint_planners/preplanned_end_effector_planner/trajectories/library/straight.json b/angler_planning/waypoint_planners/preplanned_end_effector_planner/trajectories/library/straight.json deleted file mode 100644 index 05e4870..0000000 --- a/angler_planning/waypoint_planners/preplanned_end_effector_planner/trajectories/library/straight.json +++ /dev/null @@ -1,59 +0,0 @@ -{ - "name": "straight", - "parent": "map", - "child": "alpha_standard_jaws_tool", - "waypoints": [ - { - "transform": { - "x": 0.0, - "y": 0.0, - "z": 0.0, - "rx": 0.0, - "ry": 0.0, - "rz": 0.0 - }, - "velocity": { - "x": 0.5, - "y": 0.0, - "z": 0.0, - "rx": 0.0, - "ry": 0.0, - "rz": 0.0 - }, - "acceleration": { - "x": 0.0, - "y": 0.0, - "z": 0.0, - "rx": 0.0, - "ry": 0.0, - "rz": 0.0 - } - }, - { - "transform": { - "x": 1.0, - "y": 0.0, - "z": 0.0, - "rx": 0.0, - "ry": 0.0, - "rz": 0.0 - }, - "velocity": { - "x": 0.0, - "y": 0.0, - "z": 0.0, - "rx": 0.0, - "ry": 0.0, - "rz": 0.0 - }, - "acceleration": { - "x": 0.0, - "y": 0.0, - "z": 0.0, - "rx": 0.0, - "ry": 0.0, - "rz": 0.0 - } - } - ] -} diff --git a/angler_utils/angler_utils/initial_position.py b/angler_utils/angler_utils/initial_position.py index 9b58101..1a16210 100644 --- a/angler_utils/angler_utils/initial_position.py +++ b/angler_utils/angler_utils/initial_position.py @@ -42,20 +42,22 @@ def __init__(self) -> None: """Create a new initial position setter.""" super().__init__("initial_position_setter") - # Parameters - self.declare_parameter( - "initial_positions_file", - os.path.join( - get_package_share_directory("angler_description"), - "config", - "initial_positions.yaml", - ), + self.declare_parameters( + namespace="", + parameters=[ # type: ignore + ( + "initial_positions_file", + os.path.join( + get_package_share_directory("angler_description"), + "config", + "initial_positions.yaml", + ), + ), + ("controller_cmd_topic", "/forward_velocity_controller/commands"), + ("position_tol", 0.1), + ("joint_velocity", 0.5), + ], ) - self.declare_parameter( - "controller_cmd_topic", "/forward_velocity_controller/commands" - ) - self.declare_parameter("position_tol", 0.1) - self.declare_parameter("joint_velocity", 0.5) # fast af boi # Get the desired initial positions with open( diff --git a/angler_utils/scripts/arm.sh b/angler_utils/scripts/arm.sh index 2981afd..8f146c2 100755 --- a/angler_utils/scripts/arm.sh +++ b/angler_utils/scripts/arm.sh @@ -6,4 +6,4 @@ ros2 service call /blue/cmd/enable_passthrough std_srvs/srv/SetBool data:\ true\ ros2 service call /blue/cmd/arm std_srvs/srv/SetBool data:\ true\ -ros2 service call /angler/tpik/arm std_srvs/srv/SetBool data:\ true\ +ros2 service call /angler/cmd/arm std_srvs/srv/SetBool data:\ true\ diff --git a/angler_utils/scripts/disarm.sh b/angler_utils/scripts/disarm.sh index 16a4e0f..c9bba3e 100755 --- a/angler_utils/scripts/disarm.sh +++ b/angler_utils/scripts/disarm.sh @@ -6,4 +6,4 @@ ros2 service call /blue/cmd/enable_passthrough std_srvs/srv/SetBool data:\ false ros2 service call /blue/cmd/arm std_srvs/srv/SetBool data:\ false\ -ros2 service call /angler/tpik/arm std_srvs/srv/SetBool data:\ false\ +ros2 service call /angler/cmd/arm std_srvs/srv/SetBool data:\ false\ diff --git a/angler_utils/test/test_flake8.py b/angler_utils/test/test_flake8.py deleted file mode 100644 index f494570..0000000 --- a/angler_utils/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import pytest -from ament_flake8.main import main_with_errors - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, "Found %d code style errors / warnings:\n" % len( - errors - ) + "\n".join(errors) diff --git a/requirements-build.txt b/requirements-build.txt index 61992ca..8b2288a 100644 --- a/requirements-build.txt +++ b/requirements-build.txt @@ -1,4 +1,3 @@ # Requirements unavailable as rosdeps: # https://github.com/ros/rosdistro/blob/master/rosdep/python.yaml qtm -kinpy diff --git a/sim.repos b/sim.repos new file mode 100644 index 0000000..1480429 --- /dev/null +++ b/sim.repos @@ -0,0 +1,18 @@ +# Many of these dependencies should go away as they have bugs +# resolved and release Gazebo Garden dependencies. +repositories: + + ros_gz: + type: git + url: https://github.com/gazebosim/ros_gz + version: ros2 + + gz-plugin: + type: git + url: https://github.com/gazebosim/gz-plugin.git + version: gz-plugin2 + + gz_ros2_control: + type: git + url: https://github.com/evan-palmer/gz_ros2_control.git + version: patch-rolling-garden