From 6c0daf26d837658735df74d7418cc30d61c09f10 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Mon, 21 Jul 2025 17:06:50 -0500 Subject: [PATCH 01/27] initial commit of the ros1 to ros2 port for jparse --- .gitignore | 3 +- .gitmodules | 7 +- Docker/Dockerfile | 174 +++++----- Docker/{entrypoint.sh => ros_entrypoint.sh} | 0 Docker/udev_rules/60-usb-serial.rules | 8 + README.md | 191 +---------- dockerfiles | 1 + jparse_collab_example.ipynb | 317 +++++++++--------- manipulator_control/CMakeLists.txt | 71 ++-- manipulator_control/README.md | 3 + .../{ => manipulator_control}/__init__.py | 0 manipulator_control/package.xml | 60 +--- manipulator_control/scripts/README.md | 7 + manipulator_control_ros1/CMakeLists.txt | 25 ++ manipulator_control_ros1/__init__.py | 0 .../config/kinova_arm_controllers_gazebo.yaml | 0 .../launch/franka_launch.launch | 0 .../launch/full_pose_trajectory.launch | 0 .../launch/jparse_april_tag_ros.launch | 0 .../launch/jparse_cpp.launch | 0 .../launch/kinova_gen3.launch | 0 .../launch/kinova_gen3_real.launch | 0 .../kinova_gen3_real_visual_servoing.launch | 0 .../launch/kinova_vel_control.launch | 0 .../launch/line_extended_singular_traj.launch | 0 .../launch/manip_velocity_control.launch | 0 .../launch/panda_main_torque.launch | 0 .../launch/position_trajectory.launch | 0 .../launch/se3_type_2_singular_traj.launch | 0 .../launch/xarm_launch.launch | 0 .../launch/xarm_main_vel.launch | 0 .../launch/xarm_python_using_cpp.launch | 0 .../launch/xarm_real_launch.launch | 0 .../launch/xarm_real_spacemouse_base.launch | 0 .../launch/xarm_spacemouse_teleop.launch | 0 .../msg/JparseTerms.msg | 0 .../msg/Matrow.msg | 0 manipulator_control_ros1/package.xml | 58 ++++ .../rviz/jparse_final.rviz | 0 .../rviz/kinova_gen3.rviz | 0 .../rviz/panda_rviz.rviz | 0 .../rviz/xarm_rviz.rviz | 0 .../scripts/device.py | 0 .../scripts/extract_lfd_images_from_bag.py | 0 .../scripts/input2action.py | 0 .../scripts/main_experiment_kinova.py | 0 .../scripts/panda_torque_main_experiment.py | 0 .../scripts/position_trajectory_generator.py | 0 .../scripts/se3_trajectory_generator.py | 0 .../scripts/spacemouse.py | 0 .../scripts/xarm_spacemouse.py | 0 .../scripts/xarm_vel_main_experiment.py | 0 .../scripts/xarm_vel_python_using_cpp.py | 0 .../setup.py | 0 .../src/jparse.cpp | 0 .../src/manipulator_control/jparse_cls.py | 0 .../srv/JParseSrv.srv | 0 xarm_ros1_base | 1 - 58 files changed, 427 insertions(+), 499 deletions(-) rename Docker/{entrypoint.sh => ros_entrypoint.sh} (100%) create mode 100644 Docker/udev_rules/60-usb-serial.rules create mode 160000 dockerfiles create mode 100644 manipulator_control/README.md rename manipulator_control/{ => manipulator_control}/__init__.py (100%) mode change 100644 => 100755 create mode 100644 manipulator_control/scripts/README.md create mode 100644 manipulator_control_ros1/CMakeLists.txt create mode 100644 manipulator_control_ros1/__init__.py rename {manipulator_control => manipulator_control_ros1}/config/kinova_arm_controllers_gazebo.yaml (100%) rename {manipulator_control => manipulator_control_ros1}/launch/franka_launch.launch (100%) rename {manipulator_control => manipulator_control_ros1}/launch/full_pose_trajectory.launch (100%) rename {manipulator_control => manipulator_control_ros1}/launch/jparse_april_tag_ros.launch (100%) rename {manipulator_control => manipulator_control_ros1}/launch/jparse_cpp.launch (100%) rename {manipulator_control => manipulator_control_ros1}/launch/kinova_gen3.launch (100%) rename {manipulator_control => manipulator_control_ros1}/launch/kinova_gen3_real.launch (100%) rename {manipulator_control => manipulator_control_ros1}/launch/kinova_gen3_real_visual_servoing.launch (100%) rename {manipulator_control => manipulator_control_ros1}/launch/kinova_vel_control.launch (100%) rename {manipulator_control => manipulator_control_ros1}/launch/line_extended_singular_traj.launch (100%) rename {manipulator_control => manipulator_control_ros1}/launch/manip_velocity_control.launch (100%) rename {manipulator_control => manipulator_control_ros1}/launch/panda_main_torque.launch (100%) rename {manipulator_control => manipulator_control_ros1}/launch/position_trajectory.launch (100%) rename {manipulator_control => manipulator_control_ros1}/launch/se3_type_2_singular_traj.launch (100%) rename {manipulator_control => manipulator_control_ros1}/launch/xarm_launch.launch (100%) rename {manipulator_control => manipulator_control_ros1}/launch/xarm_main_vel.launch (100%) rename {manipulator_control => manipulator_control_ros1}/launch/xarm_python_using_cpp.launch (100%) rename {manipulator_control => manipulator_control_ros1}/launch/xarm_real_launch.launch (100%) rename {manipulator_control => manipulator_control_ros1}/launch/xarm_real_spacemouse_base.launch (100%) rename {manipulator_control => manipulator_control_ros1}/launch/xarm_spacemouse_teleop.launch (100%) rename {manipulator_control => manipulator_control_ros1}/msg/JparseTerms.msg (100%) rename {manipulator_control => manipulator_control_ros1}/msg/Matrow.msg (100%) create mode 100644 manipulator_control_ros1/package.xml rename {manipulator_control => manipulator_control_ros1}/rviz/jparse_final.rviz (100%) rename {manipulator_control => manipulator_control_ros1}/rviz/kinova_gen3.rviz (100%) rename {manipulator_control => manipulator_control_ros1}/rviz/panda_rviz.rviz (100%) rename {manipulator_control => manipulator_control_ros1}/rviz/xarm_rviz.rviz (100%) rename {manipulator_control => manipulator_control_ros1}/scripts/device.py (100%) rename {manipulator_control => manipulator_control_ros1}/scripts/extract_lfd_images_from_bag.py (100%) rename {manipulator_control => manipulator_control_ros1}/scripts/input2action.py (100%) rename {manipulator_control => manipulator_control_ros1}/scripts/main_experiment_kinova.py (100%) rename {manipulator_control => manipulator_control_ros1}/scripts/panda_torque_main_experiment.py (100%) rename {manipulator_control => manipulator_control_ros1}/scripts/position_trajectory_generator.py (100%) rename {manipulator_control => manipulator_control_ros1}/scripts/se3_trajectory_generator.py (100%) rename {manipulator_control => manipulator_control_ros1}/scripts/spacemouse.py (100%) rename {manipulator_control => manipulator_control_ros1}/scripts/xarm_spacemouse.py (100%) rename {manipulator_control => manipulator_control_ros1}/scripts/xarm_vel_main_experiment.py (100%) rename {manipulator_control => manipulator_control_ros1}/scripts/xarm_vel_python_using_cpp.py (100%) rename {manipulator_control => manipulator_control_ros1}/setup.py (100%) rename {manipulator_control => manipulator_control_ros1}/src/jparse.cpp (100%) rename {manipulator_control => manipulator_control_ros1}/src/manipulator_control/jparse_cls.py (100%) rename {manipulator_control => manipulator_control_ros1}/srv/JParseSrv.srv (100%) delete mode 160000 xarm_ros1_base diff --git a/.gitignore b/.gitignore index 1827073..c7cc593 100644 --- a/.gitignore +++ b/.gitignore @@ -5,4 +5,5 @@ *.html *.pdf *.mp4 -*.avi \ No newline at end of file +*.avi +xArm-Python-SDK \ No newline at end of file diff --git a/.gitmodules b/.gitmodules index a5c1744..a0dedfa 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,4 @@ -[submodule "xarm_ros1_base"] - path = xarm_ros1_base - url = https://github.com/argallab/xarm_ros1_base.git +# [submodule "xarm_base"] +# path = xarm_base +# url = https://github.com/argallab/xarm_base.git +# branch = dev/jparse_ros2 diff --git a/Docker/Dockerfile b/Docker/Dockerfile index c928b61..74f4c46 100644 --- a/Docker/Dockerfile +++ b/Docker/Dockerfile @@ -1,79 +1,99 @@ -# Official Dockerfile for J-PARSE - -FROM tiryoh/ros-desktop-vnc:noetic - -ENV ROS_DISTRO noetic - -RUN set -x \ - && apt-get update \ - && apt-get install -y ros-noetic-moveit \ - && apt-get install -y ros-noetic-moveit-servo \ - && apt-get install -y ros-noetic-moveit-visual-tools \ - && apt-get install -y ros-noetic-moveit-ros-visualization \ - && apt-get install -y ros-noetic-graph-msgs \ - && apt-get install -y ros-noetic-rosparam-shortcuts \ - && apt-get install -y ros-noetic-control-toolbox \ - && apt-get install -y gstreamer1.0-tools gstreamer1.0-libav libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev libgstreamer-plugins-good1.0-dev gstreamer1.0-plugins-good gstreamer1.0-plugins-base \ - && rm -rf /var/lib/apt/lists/* - - -RUN set -x \ - && pip3 install conan==1.59 \ - && conan config set general.revisions_enabled=1 \ - && conan profile new default --detect > /dev/null \ - && conan profile update settings.compiler.libcxx=libstdc++11 default - - -RUN rosdep update \ - && echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc - -RUN apt-get update && apt-get install -y ros-noetic-ros-control ros-noetic-ros-controllers - - -# RUN /bin/bash -c 'conan config set general.revisions_enabled=1' -# RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; mkdir -p /ros_kortex_ws/src; cd ros_kortex_ws/src; git clone https://github.com/Kinovarobotics/ros_kortex; git clone https://github.com/Kinovarobotics/ros_kortex_vision; git clone https://github.com/ros-simulation/gazebo_ros_pkgs; cd ..; catkin_make' -# RUN echo "source /ros_kortex_ws/devel/setup.bash" >> ~/.bashrc - - -# Install ros real sense -RUN apt-get update && apt-get install -y ros-noetic-realsense2-camera && apt-get install -y ros-noetic-cv-bridge && apt install -y ros-noetic-sensor-msgs - -RUN apt-get install -y ros-noetic-gazebo-ros-pkgs ros-noetic-gazebo-ros-control - -# install xarm code -RUN cd / && mkdir -p dev_ws/src && cd dev_ws/src - -RUN source /opt/ros/noetic/setup.bash && git clone https://github.com/xArm-Developer/xarm_ros.git --recursive \ - && git clone -b noetic-devel https://github.com/armlabstanford/hrl-kdl.git \ - && git clone https://github.com/catkin/catkin_simple.git \ - && cd xarm_ros && git pull && git submodule sync && git submodule update --init --remote - - -RUN source /opt/ros/noetic/setup.bash -# install franka panda gazebo -# deps -RUN apt install -y ros-$ROS_DISTRO-gazebo-ros-control ros-${ROS_DISTRO}-rospy-message-converter ros-${ROS_DISTRO}-effort-controllers ros-${ROS_DISTRO}-joint-state-controller ros-${ROS_DISTRO}-moveit ros-${ROS_DISTRO}-moveit-commander ros-${ROS_DISTRO}-moveit-visual-tools -# RUN apt install -y ros-${ROS_DISTRO}-libfranka - -# RUN cd / && mkdir -p panda_ws/src && cd panda_ws/src -# RUN source /opt/ros/noetic/setup.bash && cd /panda_ws/src && git clone -b noetic-devel https://github.com/justagist/panda_simulator - -# RUN cd /panda_ws/src/panda_simulator && ./build_ws.sh -RUN pip install "numpy<1.24" && pip install numpy-quaternion==2020.5.11.13.33.35 && pip install numba - -RUN git clone https://github.com/xArm-Developer/xArm-Python-SDK.git && cd xArm-Python-SDK && python3 setup.py install - -RUN pip install hidapi==0.14.0.post4 -RUN apt install -y nano -RUN pip install mpld3 packaging -RUN apt install -y texlive texlive-latex-extra texlive-fonts-recommended dvipng - -RUN apt install ros-noetic-apriltag-ros -y - +FROM ros:jazzy +ENV ROS_DISTRO jazzy + +# install ros packages +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-${ROS_DISTRO}-ros2-control \ + ros-${ROS_DISTRO}-ros2-controllers \ + ros-${ROS_DISTRO}-kinematics-interface-kdl \ + ros-${ROS_DISTRO}-ament-cmake-clang-format \ + ros-${ROS_DISTRO}-rviz2 \ + ros-${ROS_DISTRO}-moveit \ + ros-${ROS_DISTRO}-moveit-ros-perception \ + ros-${ROS_DISTRO}-ackermann-msgs \ + ros-${ROS_DISTRO}-control-toolbox \ + ros-${ROS_DISTRO}-generate-parameter-library \ + ros-${ROS_DISTRO}-generate-parameter-library-py \ + ros-${ROS_DISTRO}-moveit-ros-control-interface \ + ros-${ROS_DISTRO}-pick-ik \ + ros-${ROS_DISTRO}-robot-state-publisher \ + ros-${ROS_DISTRO}-joint-state-publisher-gui \ + ros-${ROS_DISTRO}-pinocchio \ + && apt-get clean && rm -rf /var/lib/apt/lists/* + +# install packages +RUN apt-get update && apt-get install -q -y --no-install-recommends \ + dirmngr \ + gnupg2 \ + && apt-get clean && rm -rf /var/lib/apt/lists/* + +# install ubuntu packages +RUN apt-get update -yq && apt-get install -yq \ + python3 \ + apt-utils \ + locales \ + curl \ + gnupg \ + lsb-release \ + libx11-dev \ + libxau-dev \ + libxcb1-dev \ + libxdmcp-dev \ + libxext-dev \ + lsb-release \ + pkg-config \ + byobu \ + nano \ + python3-pip \ + ssh \ + vim \ + gdb \ + udev \ + python3-pyudev \ + ipython3 \ + mesa-utils \ + autoconf \ + automake \ + libtool \ + make \ + g++ \ + unzip \ + git \ + python3-scipy \ + python3-numpy \ + python3-cvxopt \ + python3-pandas \ + python3-colcon-common-extensions \ + python-is-python3 \ + python3-tk \ + python3-pytest-rerunfailures \ + python3-matplotlib \ + && apt-get clean && rm -rf /var/lib/apt/lists/* + +# RUN git clone https://github.com/xArm-Developer/xArm-Python-SDK.git && cd xArm-Python-SDK && python3 setup.py install + +# # install pinocchio +# RUN pip3 install pin + +# setup environment +ENV LANG C.UTF-8 +ENV LC_ALL C.UTF-8 + +ENV QT_X11_NO_MITSHM=1 + +# add udev rules +RUN mkdir -p /etc/udev/rules.d +COPY ./udev_rules/* /etc/udev/rules.d/ + +# create a generic workplace directory +RUN mkdir -p /home/workspace/src +WORKDIR /home/workspace + +# source ROS2 RUN echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc -COPY ./entrypoint.sh / -ENTRYPOINT [ "/entrypoint.sh" ] -CMD ["bash"] -ENV USER ubuntu -ENV PASSWD ubuntu \ No newline at end of file +# RUN chmod a+x ros_entrypoint.sh +COPY ./ros_entrypoint.sh / + +ENTRYPOINT ["/ros_entrypoint.sh"] +CMD ["bash"] \ No newline at end of file diff --git a/Docker/entrypoint.sh b/Docker/ros_entrypoint.sh similarity index 100% rename from Docker/entrypoint.sh rename to Docker/ros_entrypoint.sh diff --git a/Docker/udev_rules/60-usb-serial.rules b/Docker/udev_rules/60-usb-serial.rules new file mode 100644 index 0000000..de86816 --- /dev/null +++ b/Docker/udev_rules/60-usb-serial.rules @@ -0,0 +1,8 @@ +# PS3 controller +ACTION=="add", KERNEL=="js[0-9]*", SUBSYSTEM=="input", SUBSYSTEMS=="input", ATTRS{name}=="Sony PLAYSTATION(R)3 Controller", ATTRS{properties}=="0", SYMLINK+="ps3", MODE="0666" + +# Sip and puff interface +ACTION=="add", KERNEL=="js[0-9]*", SUBSYSTEM=="input", SUBSYSTEMS=="usb", ATTRS{idVendor}=="0a95", ATTRS{idProduct}=="0015", SYMLINK+="sip_and_puff", MODE="0666" + +# 3-axis Joystick (IPD CH-Products) +ACTION=="add", KERNEL=="js[0-9]*", SUBSYSTEM=="input", SUBSYSTEMS=="usb", ATTRS{idVendor}=="068e", ATTRS{idProduct}=="00ca", SYMLINK+="3_axis_joystick", MODE="0666" \ No newline at end of file diff --git a/README.md b/README.md index 42bb9a3..86c6f23 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,5 @@ # šŸ¦¾ J-PARSE: Jacobian-based Projection Algorithm for Resolving Singularities Effectively in Inverse Kinematic Control of Serial Manipulators - ### [Shivani Guptasarma](https://www.linkedin.com/in/shivani-guptasarma/), [Matthew Strong](https://peasant98.github.io/), [Honghao Zhen](https://www.linkedin.com/in/honghao-zhen/), and [Monroe Kennedy III](https://monroekennedy3.com/) @@ -18,6 +17,9 @@ _In Submission_ [![Project](https://img.shields.io/badge/Project_Page-J_PARSE-blue)](https://jparse-manip.github.io) [![ArXiv](https://img.shields.io/badge/Arxiv-J_PARSE-red)](https://arxiv.org/abs/2505.00306) +## ROS 2 DEV!!! +The `main` branch is supporting *ROS Noetic*. +The `ros2_dev` branch is developing the support for *ROS 2 Jazzy* (coming soon). ## FOR ARGALLAB Instructions to teleop robot with jparse and keyboard: @@ -39,29 +41,6 @@ Edits need to be still commpleted to test the robot after writing script for joy - [ ] work-around for the joint limits so that the robot does *not* immediately need to be power cycled - [ ] port to ros2 (need pinnochio) -## Quick Start with Docker - -To build the Docker image for the our environment, we use VNC docker, which allows for a graphical user interface displayable in the browser. - -### Use the Public Docker Image (Recommended) - -We have created a public Docker image that you can pull! -Steps: - -```sh -docker pull peasant98/jparse -docker run --privileged -p 6080:80 --shm-size=512m -v :/home/ubuntu/Desktop/jparse_ws/src peasant98/jparse -``` - -### Build the Image Yourself - -You can build the docker image yourself! To do so, follow the below steps: -```sh -cd Docker -docker build -t jparse . -docker run --privileged -p 6080:80 --shm-size=512m -v :/home/ubuntu/Desktop/jparse_ws/src jparse - -``` ### Instructions for Argallab We will need to build the docker image ourself (but I guess sincewe now have it done once, need to remind myself to upload to argallab docker hub). @@ -89,167 +68,3 @@ If display is having issues, then in the local machine (outside docker) enter th ``` xhost +local:docker ``` - -### Note - -We are working on a Python package that you can easily import into your project. We envision the below: - -```py -import jparse -``` - -Stay tuned! - -### Dependencies -*Note: these are handled in the Docker image directly, and are already installed!* - -1. [Catkin Simple](https://github.com/catkin/catkin_simple): https://github.com/catkin/catkin_simple -2. [HRL KDL](https://github.com/armlabstanford/hrl-kdl): https://github.com/armlabstanford/hrl-kdl - - -## Running Velocity Control (XArm) Example - -### Simulation -To run the XArm in simulation, first run -```bash -roslaunch manipulator_control xarm_launch.launch -``` - -#### Run Desired Trajectory -Next, run one of the trajectory generation scripts. This can either be the ellipse that has poses within and on the boundary of the reachable space of the arm (to test stability): -```bash -roslaunch manipulator_control full_pose_trajectory.launch robot:=xarm -``` -or for passing through the type-2 singularity (passing directly above the base link): -```bash -roslaunch manipulator_control se3_type_2_singular_traj.launch robot:=xarm -``` -To have more control over keypoints (stop at major and minor axis of ellipse), run -```bash -roslaunch manipulator_control se3_type_2_singular_traj.launch robot:=xarm key_points_only_bool:=true frequency:=0.1 use_rotation:=false -``` -or -```bash -roslaunch manipulator_control full_pose_trajectory.launch robot:=xarm key_points_only_bool:=true frequency:=0.1 use_rotation:=false -``` -(here frequency specifies how much time is spent at each keypoint). -or -```bash -roslaunch manipulator_control line_extended_singular_traj.launch robot:=xarm key_points_only_bool:=true frequency:=0.2 use_rotation:=false -``` -(line trajectory that goes from over the robot, to out of reach in front of the robot.) - -#### Run Control Method -```bash -roslaunch manipulator_control xarm_main_vel.launch is_sim:=true show_jparse_ellipses:=true phi_gain_position:=2.0 phi_gain_angular:=2.0 jparse_gamma:=0.2 method:=JParse -``` - -The arguments are -| Parameter | Attribute Description | -|------------|----------------------| -| `is_sim` | Boolean for sim or real | -| `show_jparse_ellipses` | Boolean for showing position JParse ellipsoids (for that method only) in rviz | -| `phi_gain_position` | Kp gain for JParse singular direction position | -| `phi_gain_angular` | Kp gain for JParse singular direction orientation | -| `jparse_gamma` | JParse threshold value gamma | -| `method` | "JParse", "JacobianPseudoInverse" (basic); "JacobianDampedLeastSquares"; "JacobianProjection"; "JacobianDynamicallyConsistentInverse" | - - -## Real Robot Velocity Control -### XArm Velocity Control Example -To run on the physical Xarm, the update is to use -```bash -roslaunch manipulator_control xarm_main_vel.launch is_sim:=false method:=JParse -``` -Recommended methods for physical system (to avoid unsafe motion) is: "JParse", "JacobianDampedLeastSquares" - - - - -## Running JParse with the SpaceMouse controller - -You can also run JParse with a human teleoperator using a SpaceMouse controller. This will allow for a fun sandbox to verify JParse. - -We plan to extend this to a simple learning policy as well. The code for that (collecting data, training a policy, and running inference) will be published soon! - -To run, you can run - -```sh -# run the real robot -roslaunch manipulator_control xarm_real_launch.launch using_spacemouse:=true - -# run the jparse method with or without jparse control -roslaunch xarm_main_vel.launch use_space_mouse:=true use_space_mouse_jparse:={true|false} - -# run the spacemouse example!! Make sure the use_native_xarm_spacemouse argument is OPPOSITE of use_space_mouse_jparse. -roslaunch xarm_spacemouse_teleop.launch use_native_xarm_spacemouse:={true|false} -``` - -## Run C++ JParse publisher and service -This allows for publishing JParse components and visualizing using a C++ script -```bash -roslaunch manipulator_control jparse_cpp.launch jparse_gamma:=0.2 singular_direction_gain_position:=2.0 singular_direction_gain_angular:=2.0 -``` - -The arguments are -| Parameter | Attribute Description | -|------------|----------------------| -| `namespace` | namespace of the robot (e.g. xarm) | -| `base_link_name` | Baselink frame | -| `end_link_name` | end-effector frame | -| `jparse_gamma` | JParse gamma value (0,1) | -| `singular_direction_gain_position` | gains in singular direction for position | -| `singular_direction_gain_angular` | gains in singular direction for orientation | -| `run_as_service` | (boolean) true/false | - -For running as a service: -```bash -roslaunch manipulator_control jparse_cpp.launch run_as_service:=true -``` -Then to run service from a terminal (Xarm example): - -```bash -rosservice call /jparse_srv "gamma: 0.2 -singular_direction_gain_position: 2.0 -singular_direction_gain_angular: 2.0 -base_link_name: 'link_base' -end_link_name: 'link_eef'" -``` -To see versatility, simply change the kinematic chain for the JParse solution for that segment. To view options for your kinematic tree: -```bash -rosrun rqt_tf_tree rqt_tf_tree -``` - -To test with the robot (using a python node to control the arm, with JParse coming from C++), first run script above, then: -```bash -roslaunch manipulator_control xarm_python_using_cpp.launch is_sim:=true phi_gain_position:=2.0 phi_gain_angular:=2.0 jparse_gamma:=0.2 use_service_bool:=true -``` -This has same parameters as the python version, but with the service versus message option. Message is faster/cleaner, but service is very versatile: -| Parameter | Attribute Description | -|------------|----------------------| -| `use_service_bool` | True: use service, False: use message| -| `jparse_gamma` | JParse gain (0,1)| -| `phi_gain_position` | gain on position component| -| `phi_gain_angular` | gain on angular component| -| `is_sim` | use of sim versus real (boolean)| diff --git a/dockerfiles b/dockerfiles new file mode 160000 index 0000000..9984629 --- /dev/null +++ b/dockerfiles @@ -0,0 +1 @@ +Subproject commit 99846292ad3c98109a6ccae606a098e026b7f6d8 diff --git a/jparse_collab_example.ipynb b/jparse_collab_example.ipynb index d52b497..3a29ab2 100644 --- a/jparse_collab_example.ipynb +++ b/jparse_collab_example.ipynb @@ -3,8 +3,8 @@ { "cell_type": "markdown", "metadata": { - "id": "view-in-github", - "colab_type": "text" + "colab_type": "text", + "id": "view-in-github" }, "source": [ "\"Open" @@ -82,27 +82,27 @@ }, { "cell_type": "markdown", - "source": [ - "### Set Radius to visualize JParse performance" - ], "metadata": { "id": "v-ZQHQW0aII9" - } + }, + "source": [ + "### Set Radius to visualize JParse performance" + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "v0l46tM9aN12" + }, + "outputs": [], "source": [ "singularity_bool = True\n", "if singularity_bool == True:\n", " radius = 2.0 #show ability to handle singular poses\n", "else:\n", " radius = 0.5 #show ability to handle non-singular poses\n" - ], - "metadata": { - "id": "v0l46tM9aN12" - }, - "execution_count": null, - "outputs": [] + ] }, { "cell_type": "markdown", @@ -116,6 +116,11 @@ }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "m8Wt3qa8hGJP" + }, + "outputs": [], "source": [ "class JParseClass(object):\n", "\n", @@ -230,12 +235,7 @@ " return J_parse, J_safety_nullspace\n", "\n", " return J_parse" - ], - "metadata": { - "id": "m8Wt3qa8hGJP" - }, - "execution_count": null, - "outputs": [] + ] }, { "cell_type": "code", @@ -452,15 +452,20 @@ }, { "cell_type": "markdown", - "source": [ - "# 3 Link Manipulator" - ], "metadata": { "id": "srvWBfhrimKE" - } + }, + "source": [ + "# 3 Link Manipulator" + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "6Ii4CemDiqLM" + }, + "outputs": [], "source": [ "# --- Import Libraries ---\n", "import numpy as np\n", @@ -479,15 +484,15 @@ "l1 = 1.0 # Link 1 length [m]\n", "l2 = 1.0 # Link 2 length [m]\n", "l3 = 1.0 # Link 3 length [m]" - ], - "metadata": { - "id": "6Ii4CemDiqLM" - }, - "execution_count": null, - "outputs": [] + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "Fw0Yg2QeisqU" + }, + "outputs": [], "source": [ "\n", "def forward_kinematics(q1, q2, q3):\n", @@ -562,24 +567,24 @@ " return dq[0], dq[1], dq[2]\n", "\n", "\n" - ], - "metadata": { - "id": "Fw0Yg2QeisqU" - }, - "execution_count": null, - "outputs": [] + ] }, { "cell_type": "markdown", - "source": [ - "Simulation Kinematics" - ], "metadata": { "id": "VrLvr-jQjS2G" - } + }, + "source": [ + "Simulation Kinematics" + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "4uVrTVD_jUW-" + }, + "outputs": [], "source": [ "# ======================\n", "# Problem 1: Trajectory Tracking\n", @@ -686,24 +691,24 @@ " q1_hist_B[i] = q1_B\n", " q2_hist_B[i] = q2_B\n", " q3_hist_B[i] = q3_B" - ], - "metadata": { - "id": "4uVrTVD_jUW-" - }, - "execution_count": null, - "outputs": [] + ] }, { "cell_type": "markdown", - "source": [ - "## 3-link animation path A" - ], "metadata": { "id": "CsQpNBDpjW5L" - } + }, + "source": [ + "## 3-link animation path A" + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "v_ymw0_gjZHX" + }, + "outputs": [], "source": [ "\n", "# ----------------------\n", @@ -766,24 +771,24 @@ "ax2_A.axis('equal')\n", "ax2_A.grid(True)\n", "plt.show()" - ], - "metadata": { - "id": "v_ymw0_gjZHX" - }, - "execution_count": null, - "outputs": [] + ] }, { "cell_type": "markdown", - "source": [ - "## Animate tracking path B\n" - ], "metadata": { "id": "TGzFMTBFjh1e" - } + }, + "source": [ + "## Animate tracking path B\n" + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "PXQvhzgKjkvH" + }, + "outputs": [], "source": [ "# ----------------------\n", "# Animate the 3-Link Manipulator (Problem 1, Part B)\n", @@ -850,24 +855,24 @@ "ax2_B.axis('equal')\n", "ax2_B.grid(True)\n", "plt.show()" - ], - "metadata": { - "id": "PXQvhzgKjkvH" - }, - "execution_count": null, - "outputs": [] + ] }, { "cell_type": "markdown", - "source": [ - "Download videos" - ], "metadata": { "id": "PfzTykTBjqy4" - } + }, + "source": [ + "Download videos" + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "-yGYRWb6jsE1" + }, + "outputs": [], "source": [ "# ----------------------\n", "# Save Plots and Animations\n", @@ -892,33 +897,34 @@ "files.download('end_effector_trajectory_partB.png')\n", "files.download('manipulator_animation_partA.mp4')\n", "files.download('manipulator_animation_partB.mp4')\n" - ], - "metadata": { - "id": "-yGYRWb6jsE1" - }, - "execution_count": null, - "outputs": [] + ] }, { "cell_type": "markdown", - "source": [ - "# 7-Link 3D Manipulator Example" - ], "metadata": { "id": "TS3u9uzoqbsN" - } + }, + "source": [ + "# 7-Link 3D Manipulator Example" + ] }, { "cell_type": "markdown", - "source": [ - "As a first step, we need to download a few packages, then restart the colab computer for the pinocchio library to load - this means you will need to manually run the blocks below this first one." - ], "metadata": { "id": "c5UiadlLhXT8" - } + }, + "source": [ + "As a first step, we need to download a few packages, then restart the colab computer for the pinocchio library to load - this means you will need to manually run the blocks below this first one." + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "collapsed": true, + "id": "QLm5UZhol4F-" + }, + "outputs": [], "source": [ "# 1) Download micromamba if you havenā€™t already\n", "!curl -Ls https://micro.mamba.pm/api/micromamba/linux-64/latest | tar -xvj bin/micromamba\n", @@ -931,27 +937,26 @@ "\n", "# 3) Kill the process to force a clean restart\n", "import os; os.kill(os.getpid(), 9)" - ], - "metadata": { - "collapsed": true, - "id": "QLm5UZhol4F-" - }, - "execution_count": null, - "outputs": [] + ] }, { "cell_type": "markdown", + "metadata": { + "id": "FFtLyKbi6D3F" + }, "source": [ "A restart was needed to ensure pinocchio is accessible - so continue from the following code block\n", "\n", "## Setup the 7-DOF 3D kinematic manipulator" - ], - "metadata": { - "id": "FFtLyKbi6D3F" - } + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "b7sFpeTl6RNi" + }, + "outputs": [], "source": [ "import numpy as np\n", "import pinocchio as pin\n", @@ -993,22 +998,17 @@ "data = model.createData()\n", "\n", "print(f\"Built model with {model.nq} DOFs and {model.njoints} joints\")" - ], - "metadata": { - "id": "b7sFpeTl6RNi" - }, - "execution_count": null, - "outputs": [] + ] }, { "cell_type": "markdown", + "metadata": { + "id": "Bu2AcrEsh47i" + }, "source": [ "## Rerun JPARSE class\n", "We need to rerun JPARSE class again since colab was restarted when loading pinnochio. This is the same implementation as above, just copied here for convenience." - ], - "metadata": { - "id": "Bu2AcrEsh47i" - } + ] }, { "cell_type": "code", @@ -1135,16 +1135,21 @@ }, { "cell_type": "markdown", + "metadata": { + "id": "pen88RfHAVnE" + }, "source": [ "## Trajectory & Control Loop (Position Control ONLY)\n", "Now that we have a 7dof, 3D manipulator, lets establish a helical trajectory, and track just the position of the end-effector" - ], - "metadata": { - "id": "pen88RfHAVnE" - } + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "vxzfW3jGAV0k" + }, + "outputs": [], "source": [ "import matplotlib.pyplot as plt\n", "\n", @@ -1200,15 +1205,13 @@ "\n", " q_hist[i] = q_wrapped\n", " x_hist[i] = ee_pos" - ], - "metadata": { - "id": "vxzfW3jGAV0k" - }, - "execution_count": null, - "outputs": [] + ] }, { "cell_type": "markdown", + "metadata": { + "id": "Yg7Djilod4F6" + }, "source": [ "### Animate (position only) in Plotly for interactivity\n", "\n", @@ -1216,13 +1219,16 @@ "\n", "\n", "* To interact with the plot, pause running then adjust orientation/position of the view then resume with the play button" - ], - "metadata": { - "id": "Yg7Djilod4F6" - } + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "collapsed": true, + "id": "GpyEXqvh5f5-" + }, + "outputs": [], "source": [ "# Install Plotly if needed\n", "!pip install plotly -q\n", @@ -1341,26 +1347,25 @@ ")\n", "# Display the interactive animation\n", "fig.show()" - ], - "metadata": { - "id": "GpyEXqvh5f5-", - "collapsed": true - }, - "execution_count": null, - "outputs": [] + ] }, { "cell_type": "markdown", + "metadata": { + "id": "xEdz6hdzxrSm" + }, "source": [ "# For tracking full 3D pose (position and orientation)\n", "The previous example only tracked position, this example tracks both position and orientation (a static orientation which must be controlled)." - ], - "metadata": { - "id": "xEdz6hdzxrSm" - } + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "SX_VmGjHxuR4" + }, + "outputs": [], "source": [ "# Full SE(3) Poseā€“Level Control Using JParse\n", "\n", @@ -1459,24 +1464,24 @@ " q_hist[i] = q_wrapped\n", " # q_hist[i] = q\n", " x_hist[i] = p_cur\n" - ], - "metadata": { - "id": "SX_VmGjHxuR4" - }, - "execution_count": null, - "outputs": [] + ] }, { "cell_type": "markdown", - "source": [ - "### Plot the error versus time" - ], "metadata": { "id": "uG2hwvoBKrRD" - } + }, + "source": [ + "### Plot the error versus time" + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "lY9syZ4xKtst" + }, + "outputs": [], "source": [ "# === Block 3: Plot Position, Orientation Error, AND Joint Angles vs. Time ===\n", "\n", @@ -1527,25 +1532,25 @@ "\n", "plt.tight_layout()\n", "plt.show()" - ], - "metadata": { - "id": "lY9syZ4xKtst" - }, - "execution_count": null, - "outputs": [] + ] }, { "cell_type": "markdown", + "metadata": { + "id": "kM-QgLrT0o5k" + }, "source": [ "## Use plotly to animate 3D full pose tracking\n", "To interact with the plot, pause running then adjust orientation/position of the view then resume with the play button" - ], - "metadata": { - "id": "kM-QgLrT0o5k" - } + ] }, { "cell_type": "code", + "execution_count": null, + "metadata": { + "id": "2RFHc19SOEOE" + }, + "outputs": [], "source": [ "# === Updated Plotly Animation Using Pinocchio FK (Script 1) ===\n", "\n", @@ -1696,28 +1701,24 @@ ")\n", "\n", "fig.show()" - ], - "metadata": { - "id": "2RFHc19SOEOE" - }, - "execution_count": null, - "outputs": [] + ] } ], "metadata": { "colab": { - "provenance": [], "authorship_tag": "ABX9TyPzNiLviQLosZVnFaEbvu19", - "include_colab_link": true + "include_colab_link": true, + "provenance": [] }, "kernelspec": { "display_name": "Python 3", "name": "python3" }, "language_info": { - "name": "python" + "name": "python", + "version": "3.10.12" } }, "nbformat": 4, "nbformat_minor": 0 -} \ No newline at end of file +} diff --git a/manipulator_control/CMakeLists.txt b/manipulator_control/CMakeLists.txt index 36b4168..ed39444 100644 --- a/manipulator_control/CMakeLists.txt +++ b/manipulator_control/CMakeLists.txt @@ -1,25 +1,50 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(manipulator_control) -find_package(catkin_simple REQUIRED) - -catkin_python_setup() - -catkin_simple() - - -## Add support for C++11, supported in ROS Kinetic and newer -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") - -find_package(Eigen3 REQUIRED) - -include_directories(include ${EIGEN3_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${PCL_LIBRARIES}) - - -cs_add_executable(jparse src/jparse.cpp) - - -cs_install() - -cs_export() -#https://github.com/catkin/catkin_simple \ No newline at end of file +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) + +include_directories(include) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +# make cpp executable +add_executable(cpp_executable src/cpp_node.cpp) # need to edit with the appropriate names later +ament_target_dependencies(cpp_executable rclcpp) + +# install cpp executables +install(TARGETS + cpp_executables + DESTINATION lib/${PROJECT_NAME} +) + +# install python modules +ament_python_install_package(${PROJECT_NAME}) + +# install python executables +install(PROGRAMS + scripts/python_node.py + DESTINATION lib/${PROJECT_NAME} +) # need to edit with the appropriate names later + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/manipulator_control/README.md b/manipulator_control/README.md new file mode 100644 index 0000000..366127d --- /dev/null +++ b/manipulator_control/README.md @@ -0,0 +1,3 @@ +# manipulator control + +Great video on creating ROS 2 package with both c++ and python nodes: https://www.youtube.com/watch?v=kAJXCoXrr5c \ No newline at end of file diff --git a/manipulator_control/__init__.py b/manipulator_control/manipulator_control/__init__.py old mode 100644 new mode 100755 similarity index 100% rename from manipulator_control/__init__.py rename to manipulator_control/manipulator_control/__init__.py diff --git a/manipulator_control/package.xml b/manipulator_control/package.xml index cbba38b..4238d86 100644 --- a/manipulator_control/package.xml +++ b/manipulator_control/package.xml @@ -1,58 +1,22 @@ - + + manipulator_control 0.0.0 - The manipulator_control package - Monroe Kennedy - Monroe Kennedy + TODO: Package description + root + TODO: License declaration + ament_cmake + ament_cmake_python - MIT - + rclcpp + rclpy + ament_lint_auto + ament_lint_common - catkin - catkin_simple - message_generation - roscpp - rospy - roscpp - rospy - - std_msgs - tf - tf2 - tf2_ros - sensor_msgs - tf_conversions - eigen_conversions - visualization_msgs - geometry_msgs - robot_controllers_msgs - kdl_parser - orocos_kdl - urdf - robot_controllers_interface - gazebo_msgs - moveit_core - kortex_driver - kortex_gazebo - kortex_vision - - - message_generation - - moveit_commander - actionlib_msgs - - - message_runtime - roscpp - rospy - - - - + ament_cmake diff --git a/manipulator_control/scripts/README.md b/manipulator_control/scripts/README.md new file mode 100644 index 0000000..0959120 --- /dev/null +++ b/manipulator_control/scripts/README.md @@ -0,0 +1,7 @@ +# scripts + +This is where all the python nodes will go. + +Need to add the shebang at the top: `#! /usr/bin/env python3 + +Then do `chmod +x ` to make it an executable! \ No newline at end of file diff --git a/manipulator_control_ros1/CMakeLists.txt b/manipulator_control_ros1/CMakeLists.txt new file mode 100644 index 0000000..36b4168 --- /dev/null +++ b/manipulator_control_ros1/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.0.2) +project(manipulator_control) + +find_package(catkin_simple REQUIRED) + +catkin_python_setup() + +catkin_simple() + + +## Add support for C++11, supported in ROS Kinetic and newer +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + +find_package(Eigen3 REQUIRED) + +include_directories(include ${EIGEN3_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${PCL_LIBRARIES}) + + +cs_add_executable(jparse src/jparse.cpp) + + +cs_install() + +cs_export() +#https://github.com/catkin/catkin_simple \ No newline at end of file diff --git a/manipulator_control_ros1/__init__.py b/manipulator_control_ros1/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/manipulator_control/config/kinova_arm_controllers_gazebo.yaml b/manipulator_control_ros1/config/kinova_arm_controllers_gazebo.yaml similarity index 100% rename from manipulator_control/config/kinova_arm_controllers_gazebo.yaml rename to manipulator_control_ros1/config/kinova_arm_controllers_gazebo.yaml diff --git a/manipulator_control/launch/franka_launch.launch b/manipulator_control_ros1/launch/franka_launch.launch similarity index 100% rename from manipulator_control/launch/franka_launch.launch rename to manipulator_control_ros1/launch/franka_launch.launch diff --git a/manipulator_control/launch/full_pose_trajectory.launch b/manipulator_control_ros1/launch/full_pose_trajectory.launch similarity index 100% rename from manipulator_control/launch/full_pose_trajectory.launch rename to manipulator_control_ros1/launch/full_pose_trajectory.launch diff --git a/manipulator_control/launch/jparse_april_tag_ros.launch b/manipulator_control_ros1/launch/jparse_april_tag_ros.launch similarity index 100% rename from manipulator_control/launch/jparse_april_tag_ros.launch rename to manipulator_control_ros1/launch/jparse_april_tag_ros.launch diff --git a/manipulator_control/launch/jparse_cpp.launch b/manipulator_control_ros1/launch/jparse_cpp.launch similarity index 100% rename from manipulator_control/launch/jparse_cpp.launch rename to manipulator_control_ros1/launch/jparse_cpp.launch diff --git a/manipulator_control/launch/kinova_gen3.launch b/manipulator_control_ros1/launch/kinova_gen3.launch similarity index 100% rename from manipulator_control/launch/kinova_gen3.launch rename to manipulator_control_ros1/launch/kinova_gen3.launch diff --git a/manipulator_control/launch/kinova_gen3_real.launch b/manipulator_control_ros1/launch/kinova_gen3_real.launch similarity index 100% rename from manipulator_control/launch/kinova_gen3_real.launch rename to manipulator_control_ros1/launch/kinova_gen3_real.launch diff --git a/manipulator_control/launch/kinova_gen3_real_visual_servoing.launch b/manipulator_control_ros1/launch/kinova_gen3_real_visual_servoing.launch similarity index 100% rename from manipulator_control/launch/kinova_gen3_real_visual_servoing.launch rename to manipulator_control_ros1/launch/kinova_gen3_real_visual_servoing.launch diff --git a/manipulator_control/launch/kinova_vel_control.launch b/manipulator_control_ros1/launch/kinova_vel_control.launch similarity index 100% rename from manipulator_control/launch/kinova_vel_control.launch rename to manipulator_control_ros1/launch/kinova_vel_control.launch diff --git a/manipulator_control/launch/line_extended_singular_traj.launch b/manipulator_control_ros1/launch/line_extended_singular_traj.launch similarity index 100% rename from manipulator_control/launch/line_extended_singular_traj.launch rename to manipulator_control_ros1/launch/line_extended_singular_traj.launch diff --git a/manipulator_control/launch/manip_velocity_control.launch b/manipulator_control_ros1/launch/manip_velocity_control.launch similarity index 100% rename from manipulator_control/launch/manip_velocity_control.launch rename to manipulator_control_ros1/launch/manip_velocity_control.launch diff --git a/manipulator_control/launch/panda_main_torque.launch b/manipulator_control_ros1/launch/panda_main_torque.launch similarity index 100% rename from manipulator_control/launch/panda_main_torque.launch rename to manipulator_control_ros1/launch/panda_main_torque.launch diff --git a/manipulator_control/launch/position_trajectory.launch b/manipulator_control_ros1/launch/position_trajectory.launch similarity index 100% rename from manipulator_control/launch/position_trajectory.launch rename to manipulator_control_ros1/launch/position_trajectory.launch diff --git a/manipulator_control/launch/se3_type_2_singular_traj.launch b/manipulator_control_ros1/launch/se3_type_2_singular_traj.launch similarity index 100% rename from manipulator_control/launch/se3_type_2_singular_traj.launch rename to manipulator_control_ros1/launch/se3_type_2_singular_traj.launch diff --git a/manipulator_control/launch/xarm_launch.launch b/manipulator_control_ros1/launch/xarm_launch.launch similarity index 100% rename from manipulator_control/launch/xarm_launch.launch rename to manipulator_control_ros1/launch/xarm_launch.launch diff --git a/manipulator_control/launch/xarm_main_vel.launch b/manipulator_control_ros1/launch/xarm_main_vel.launch similarity index 100% rename from manipulator_control/launch/xarm_main_vel.launch rename to manipulator_control_ros1/launch/xarm_main_vel.launch diff --git a/manipulator_control/launch/xarm_python_using_cpp.launch b/manipulator_control_ros1/launch/xarm_python_using_cpp.launch similarity index 100% rename from manipulator_control/launch/xarm_python_using_cpp.launch rename to manipulator_control_ros1/launch/xarm_python_using_cpp.launch diff --git a/manipulator_control/launch/xarm_real_launch.launch b/manipulator_control_ros1/launch/xarm_real_launch.launch similarity index 100% rename from manipulator_control/launch/xarm_real_launch.launch rename to manipulator_control_ros1/launch/xarm_real_launch.launch diff --git a/manipulator_control/launch/xarm_real_spacemouse_base.launch b/manipulator_control_ros1/launch/xarm_real_spacemouse_base.launch similarity index 100% rename from manipulator_control/launch/xarm_real_spacemouse_base.launch rename to manipulator_control_ros1/launch/xarm_real_spacemouse_base.launch diff --git a/manipulator_control/launch/xarm_spacemouse_teleop.launch b/manipulator_control_ros1/launch/xarm_spacemouse_teleop.launch similarity index 100% rename from manipulator_control/launch/xarm_spacemouse_teleop.launch rename to manipulator_control_ros1/launch/xarm_spacemouse_teleop.launch diff --git a/manipulator_control/msg/JparseTerms.msg b/manipulator_control_ros1/msg/JparseTerms.msg similarity index 100% rename from manipulator_control/msg/JparseTerms.msg rename to manipulator_control_ros1/msg/JparseTerms.msg diff --git a/manipulator_control/msg/Matrow.msg b/manipulator_control_ros1/msg/Matrow.msg similarity index 100% rename from manipulator_control/msg/Matrow.msg rename to manipulator_control_ros1/msg/Matrow.msg diff --git a/manipulator_control_ros1/package.xml b/manipulator_control_ros1/package.xml new file mode 100644 index 0000000..cbba38b --- /dev/null +++ b/manipulator_control_ros1/package.xml @@ -0,0 +1,58 @@ + + + manipulator_control + 0.0.0 + The manipulator_control package + Monroe Kennedy + Monroe Kennedy + + + MIT + + + + catkin + catkin_simple + message_generation + roscpp + rospy + roscpp + rospy + + std_msgs + tf + tf2 + tf2_ros + sensor_msgs + tf_conversions + eigen_conversions + visualization_msgs + geometry_msgs + robot_controllers_msgs + kdl_parser + orocos_kdl + urdf + robot_controllers_interface + gazebo_msgs + moveit_core + kortex_driver + kortex_gazebo + kortex_vision + + + message_generation + + moveit_commander + actionlib_msgs + + + message_runtime + roscpp + rospy + + + + + + + diff --git a/manipulator_control/rviz/jparse_final.rviz b/manipulator_control_ros1/rviz/jparse_final.rviz similarity index 100% rename from manipulator_control/rviz/jparse_final.rviz rename to manipulator_control_ros1/rviz/jparse_final.rviz diff --git a/manipulator_control/rviz/kinova_gen3.rviz b/manipulator_control_ros1/rviz/kinova_gen3.rviz similarity index 100% rename from manipulator_control/rviz/kinova_gen3.rviz rename to manipulator_control_ros1/rviz/kinova_gen3.rviz diff --git a/manipulator_control/rviz/panda_rviz.rviz b/manipulator_control_ros1/rviz/panda_rviz.rviz similarity index 100% rename from manipulator_control/rviz/panda_rviz.rviz rename to manipulator_control_ros1/rviz/panda_rviz.rviz diff --git a/manipulator_control/rviz/xarm_rviz.rviz b/manipulator_control_ros1/rviz/xarm_rviz.rviz similarity index 100% rename from manipulator_control/rviz/xarm_rviz.rviz rename to manipulator_control_ros1/rviz/xarm_rviz.rviz diff --git a/manipulator_control/scripts/device.py b/manipulator_control_ros1/scripts/device.py similarity index 100% rename from manipulator_control/scripts/device.py rename to manipulator_control_ros1/scripts/device.py diff --git a/manipulator_control/scripts/extract_lfd_images_from_bag.py b/manipulator_control_ros1/scripts/extract_lfd_images_from_bag.py similarity index 100% rename from manipulator_control/scripts/extract_lfd_images_from_bag.py rename to manipulator_control_ros1/scripts/extract_lfd_images_from_bag.py diff --git a/manipulator_control/scripts/input2action.py b/manipulator_control_ros1/scripts/input2action.py similarity index 100% rename from manipulator_control/scripts/input2action.py rename to manipulator_control_ros1/scripts/input2action.py diff --git a/manipulator_control/scripts/main_experiment_kinova.py b/manipulator_control_ros1/scripts/main_experiment_kinova.py similarity index 100% rename from manipulator_control/scripts/main_experiment_kinova.py rename to manipulator_control_ros1/scripts/main_experiment_kinova.py diff --git a/manipulator_control/scripts/panda_torque_main_experiment.py b/manipulator_control_ros1/scripts/panda_torque_main_experiment.py similarity index 100% rename from manipulator_control/scripts/panda_torque_main_experiment.py rename to manipulator_control_ros1/scripts/panda_torque_main_experiment.py diff --git a/manipulator_control/scripts/position_trajectory_generator.py b/manipulator_control_ros1/scripts/position_trajectory_generator.py similarity index 100% rename from manipulator_control/scripts/position_trajectory_generator.py rename to manipulator_control_ros1/scripts/position_trajectory_generator.py diff --git a/manipulator_control/scripts/se3_trajectory_generator.py b/manipulator_control_ros1/scripts/se3_trajectory_generator.py similarity index 100% rename from manipulator_control/scripts/se3_trajectory_generator.py rename to manipulator_control_ros1/scripts/se3_trajectory_generator.py diff --git a/manipulator_control/scripts/spacemouse.py b/manipulator_control_ros1/scripts/spacemouse.py similarity index 100% rename from manipulator_control/scripts/spacemouse.py rename to manipulator_control_ros1/scripts/spacemouse.py diff --git a/manipulator_control/scripts/xarm_spacemouse.py b/manipulator_control_ros1/scripts/xarm_spacemouse.py similarity index 100% rename from manipulator_control/scripts/xarm_spacemouse.py rename to manipulator_control_ros1/scripts/xarm_spacemouse.py diff --git a/manipulator_control/scripts/xarm_vel_main_experiment.py b/manipulator_control_ros1/scripts/xarm_vel_main_experiment.py similarity index 100% rename from manipulator_control/scripts/xarm_vel_main_experiment.py rename to manipulator_control_ros1/scripts/xarm_vel_main_experiment.py diff --git a/manipulator_control/scripts/xarm_vel_python_using_cpp.py b/manipulator_control_ros1/scripts/xarm_vel_python_using_cpp.py similarity index 100% rename from manipulator_control/scripts/xarm_vel_python_using_cpp.py rename to manipulator_control_ros1/scripts/xarm_vel_python_using_cpp.py diff --git a/manipulator_control/setup.py b/manipulator_control_ros1/setup.py similarity index 100% rename from manipulator_control/setup.py rename to manipulator_control_ros1/setup.py diff --git a/manipulator_control/src/jparse.cpp b/manipulator_control_ros1/src/jparse.cpp similarity index 100% rename from manipulator_control/src/jparse.cpp rename to manipulator_control_ros1/src/jparse.cpp diff --git a/manipulator_control/src/manipulator_control/jparse_cls.py b/manipulator_control_ros1/src/manipulator_control/jparse_cls.py similarity index 100% rename from manipulator_control/src/manipulator_control/jparse_cls.py rename to manipulator_control_ros1/src/manipulator_control/jparse_cls.py diff --git a/manipulator_control/srv/JParseSrv.srv b/manipulator_control_ros1/srv/JParseSrv.srv similarity index 100% rename from manipulator_control/srv/JParseSrv.srv rename to manipulator_control_ros1/srv/JParseSrv.srv diff --git a/xarm_ros1_base b/xarm_ros1_base deleted file mode 160000 index 789aa68..0000000 --- a/xarm_ros1_base +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 789aa686859a79cc677739ff9fd3c7d40ea44f79 From ce455cddc30a491e3d7637ff9284d8d668ca35c4 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Mon, 21 Jul 2025 17:08:55 -0500 Subject: [PATCH 02/27] More for the initial commit for porting --- .gitignore | 3 ++- manipulator_control/CMakeLists.txt | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index c7cc593..0b8b515 100644 --- a/.gitignore +++ b/.gitignore @@ -6,4 +6,5 @@ *.pdf *.mp4 *.avi -xArm-Python-SDK \ No newline at end of file +xArm-Python-SDK +dockerfiles \ No newline at end of file diff --git a/manipulator_control/CMakeLists.txt b/manipulator_control/CMakeLists.txt index ed39444..f2b56fd 100644 --- a/manipulator_control/CMakeLists.txt +++ b/manipulator_control/CMakeLists.txt @@ -29,7 +29,7 @@ install(TARGETS # install python modules ament_python_install_package(${PROJECT_NAME}) -# install python executables +# install python executables install(PROGRAMS scripts/python_node.py DESTINATION lib/${PROJECT_NAME} From eaab8df7a70d2f84f93090504cf9801e28bcb5f8 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Mon, 21 Jul 2025 17:11:34 -0500 Subject: [PATCH 03/27] Removing dockerfiles repo; not needed bc included under docker folder --- dockerfiles | 1 - 1 file changed, 1 deletion(-) delete mode 160000 dockerfiles diff --git a/dockerfiles b/dockerfiles deleted file mode 160000 index 9984629..0000000 --- a/dockerfiles +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 99846292ad3c98109a6ccae606a098e026b7f6d8 From 4c027ebbfd7929189cb8b4d2bf78ca9e75459d13 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Mon, 21 Jul 2025 22:35:25 -0500 Subject: [PATCH 04/27] Ported some things to ros2; need to still implement pinocchio in jparse_cls file; updated instructions --- README.md | 9 +- .../launch/xarm_main_vel.launch.xml | 41 ++ .../launch/xarm_real_launch.launch | 20 + manipulator_control/scripts/jparse_cls.py | 58 ++ .../scripts/xarm_vel_main_experiment.py | 585 ++++++++++++++++++ 5 files changed, 709 insertions(+), 4 deletions(-) create mode 100644 manipulator_control/launch/xarm_main_vel.launch.xml create mode 100644 manipulator_control/launch/xarm_real_launch.launch create mode 100644 manipulator_control/scripts/jparse_cls.py create mode 100644 manipulator_control/scripts/xarm_vel_main_experiment.py diff --git a/README.md b/README.md index 86c6f23..990f11d 100644 --- a/README.md +++ b/README.md @@ -23,14 +23,15 @@ The `ros2_dev` branch is developing the support for *ROS 2 Jazzy* (coming soon). ## FOR ARGALLAB Instructions to teleop robot with jparse and keyboard: +TODO: NEEDS TO BE TESTED and jparse_cls.py needs to be written! -Launch the following (velocity_control param for the xarm7_gripper_moveit_config launchfile is left as false in this case): +Launch the following: ``` -roslaunch xarm7_gripper_moveit_config realMove_exec.launch robot_ip:= add_gripper:=true +ros2 launch xarm_moveit_config xarm7_moveit_realmove.launch.py robot_ip:= add_gripper:=true -roslaunch manipulator_control xarm_main_vel.launch use_space_mouse:=true use_space_mouse_jparse:=true +ros2 launch manipulator_control xarm_main_vel.launch use_teleop_control:=true use_teleop_control_jparse:=true -rosrun teleop_twist_keyboard teleop_twist_keyboard.py _stamped:=True _frame_id:=link_eef cmd_vel:=robot_action +ros2 run teleop_twist_keyboard teleop_twist_keyboard stamped:=True frame_id:=link_eef --ros-args --remap cmd_vel:=robot_action ``` Edits need to be still commpleted to test the robot after writing script for joystick and sip/puff teleop. diff --git a/manipulator_control/launch/xarm_main_vel.launch.xml b/manipulator_control/launch/xarm_main_vel.launch.xml new file mode 100644 index 0000000..76d68f7 --- /dev/null +++ b/manipulator_control/launch/xarm_main_vel.launch.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/manipulator_control/launch/xarm_real_launch.launch b/manipulator_control/launch/xarm_real_launch.launch new file mode 100644 index 0000000..f94c84d --- /dev/null +++ b/manipulator_control/launch/xarm_real_launch.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/manipulator_control/scripts/jparse_cls.py b/manipulator_control/scripts/jparse_cls.py new file mode 100644 index 0000000..434716b --- /dev/null +++ b/manipulator_control/scripts/jparse_cls.py @@ -0,0 +1,58 @@ +import rclpy +from rclpy.Node import Node + +# pinocchio +import numpy as np +import pinocchio as pin +from urdf_parser_py.urdf import URDF + +# ros2 stuff +from visualization_msgs.msg import Marker, MarkerArray +from geometry_msgs.msg import Quaternion, Pose, Point, PoseStamped +from sensor_msgs.msg import JointState + +# this is the huge node that does all the jacobian calculations!! + + +class JParseClass(Node): + def __init__(self): + # Initialize any necessary variables or parameters here + """ + Base link: The base link of the robot. + End link: The end link of the robot. + """ + super().__init__('jparse_class') + + # parameters + self.declare_parameter('base_link', 'base_link', + description='Base link of the robot') + self.declare_parameter('end_link', 'end_effector_link', + description='End effector link of the robot') + + # Get parameters + self.base_link = self.get_parameter('base_link').get_parameter_value().string_value + self.end_effector_link = self.get_parameter('end_link').get_parameter_value().string_value + + # Load URDF from parameter server + urdf = URDF.from_parameter_server(self) + model = pin.buildModelFromUrdf(urdf.to_xml_string()) + self.data = model.createData() + self.model = model + + # publishers + self.marker_pub = self.create_publisher(MarkerArray, '/jparse_ellipsoid_marker', 10) + + # subscribers + self.joint_state_sub = self.create_subscription( + JointState, '/joint_states', self.joint_state_callback, 10) + + self.J_prev = None + self.J_prev_time = None + + # Timer: publish every 0.1s + self.create_timer(0.1, self.timer_callback) + + def timer_callback(self): + + + \ No newline at end of file diff --git a/manipulator_control/scripts/xarm_vel_main_experiment.py b/manipulator_control/scripts/xarm_vel_main_experiment.py new file mode 100644 index 0000000..33202c2 --- /dev/null +++ b/manipulator_control/scripts/xarm_vel_main_experiment.py @@ -0,0 +1,585 @@ +import sys +import rclpy +from rclpy.node import Node +import numpy as np +import math +import time + +# ros2 stuff +import tf2_ros +from tf.transformations import quaternion_from_euler, quaternion_matrix +from geometry_msgs.msg import Pose, PoseStamped, Vector3, TwistStamped +from std_msgs.msg import Float64MultiArray, Float64 +from sensor_msgs.msg import JointState +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + +try: + from xarm.wrapper import XArmAPI +except ImportError: + print("xarm package not installed, skipping xarm import") + +import jparse_cls + +# Huge TODO: might need to implement the control loop as a timer callback instead of a while loop to avoid blocking the node (7/21/25) +class ArmController(Node): + def __init__(self): + super().__init__('arm_controller') + + # Initialize parameters + self.base_link = self.declare_parameter('/base_link', 'base_link').get_parameter_value().string_value # defaults are for Kinova gen3 + self.end_link = self.declare_parameter('/end_link', 'lnk_eef').get_parameter_value().string_value + + self.is_sim = self.declare_parameter('~is_sim', False).get_parameter_value().bool_value #boolean to control if the robot is in simulation or not + + self.phi_gain = self.declare_parameter('~phi_gain', 10).get_parameter_value().double_value #gain for the phi term in the JParse method + self.phi_gain_position = self.declare_parameter('~phi_gain_position', 15).get_parameter_value().double_value #gain for the phi term in the JParse method for position control + self.phi_gain_angular = self.declare_parameter('~phi_gain_angular', 15).get_parameter_value().double_value #gain for the phi term in the JParse method for angular control + + self.jparse_gamma = self.declare_parameter('~jparse_gamma', 0.1).get_parameter_value().double_value #gamma for the JParse method + + # used to be self.use_space_mouse and self.use_space_mouse_jparse + self.use_teleop_control = self.declare_parameter('~use_teleop_control', False).get_parameter_value().bool_value #boolean to control if the robot is in teleoperation mode or not + self.use_teleop_control_jparse = self.declare_parameter('~use_teleop_control_jparse', False).get_parameter_value().bool_value #boolean to control if the robot is in teleoperation mode with JParse + + # used to be self.space_mouse_command + self.teleop_control_command = np.array([0,0,0,0,0,0]).T + + if self.is_sim == False: + # from IPython import embed; embed(banner1="hello in not sim") + self.robot_ip = self.declare_parameter('~robot_ip', '192.168.1.199').get_parameter_value().string_value + self.arm = XArmAPI(self.robot_ip) + self.arm.motion_enable(enable=True) + self.arm.set_mode(0) + self.arm.set_state(state=0) + time.sleep(1) + + # set joint velocity control mode + self.arm.set_mode(4) + self.arm.set_state(0) + time.sleep(1) + + # set parameters + self.position_only = self.declare_parameter('~position_only', False).get_parameter_value().bool_value #boolean to control if the robot is in position control mode or not + + # choose the method to use + self.method = self.declare_parameter('~method', 'JParse').get_parameter_value().string_value #options are JParse, JacobianPseudoInverse, JacobianDampedLeastSquares, JacobianProjection, JacobianDynamicallyConsistentInverse + + self.show_jparse_ellipses = self.declare_parameter('~show_jparse_ellipses', False).get_parameter_value().bool_value #boolean to control if the JParse ellipses are shown or not + + self.joint_states = None + self.target_pose = None + self.teleop_control_command = None + self.jacobian_calculator = jparse_cls.JParseClass(base_link=self.base_link, end_link=self.end_link) + + #get tf listener + self.tf2_buffer = tf2_ros.Buffer() + self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer, self) + self.get_logger().info("ArmController initialized with base link: {}, end link: {}".format(self.base_link, self.end_link)) + self.tf2_timeout = rclpy.duration.Duration(seconds=1.0) + + if self.is_sim == True: + #if we are in simulation, use the joint_states and target_pose topics + self.joint_states_topic = '/xarm/joint_states' + else: + self.joint_states_topic = '/joint_states' + + + # Initialize publishers and subscribers + self.initialize_subscribers() + self.initialize_publishers() + + # Define the joint names + self.joint_names = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7"] + + # Initialize the current positions + self.current_positions = [0.0] * len(self.joint_names) + + self.rate = self.create_rate(10) # 10 Hz + #home the robot + try: + self.velocity_home_robot() + except Exception as e: + self.get_logger().error(f"Failed to home the robot: {e}") + pass + finally: + # Clean up resources if needed + self.get_logger().info("Shutting down the control loop") + joint_zero_velocities = [0.0] * len(self.joint_names) + self.command_joint_velocities(joint_zero_velocities) + #now run the control loop + self.control_loop() + + ######################################################################################################### + ############################# SUBSCRIBERS AND PUBLISHERS INITIALIZATION ################################# + ######################################################################################################### + def initialize_subscribers(self): + # subscribers + self.joint_state_sub = self.create_subscription( + JointState, self.joint_states_topic, self.joint_states_callback, 10) + self.target_pose_sub = self.create_subscription( + PoseStamped, '/target_pose', self.target_pose_callback, 10) + self.teleop_control_sub = self.create_subscription( + TwistStamped, 'robot_action', self.teleop_control_callback, 10) + + def initialize_publishers(self): + # publishers + self.manip_measure_pub = self.create_publisher(Float64, '/manip_measure', 10) + self.inverse_cond_number = self.create_publisher(Float64, '/inverse_cond_number', 10) + #have certain messages to store raw error + self.pose_error_pub = self.create_publisher(PoseStamped, '/pose_error', 10) + self.position_error_pub = self.create_publisher(Vector3, '/position_error', 10) + self.orientation_error_pub = self.create_publisher(Vector3, '/orientation_error', 10) + #have certain messages to store control error + self.pose_error_control_pub = self.create_publisher(PoseStamped, '/pose_error_control', 10) + self.position_error_control_pub = self.create_publisher(Vector3, '/position_error_control', 10) + self.orientation_error_control_pub = self.create_publisher(Vector3, '/orientation_error_control', 10) + #publish the current end effector pose and target pose + self.current_end_effector_pose_pub = self.create_publisher(PoseStamped, '/current_end_effector_pose', 10) + self.current_target_pose_pub = self.create_publisher(PoseStamped, '/current_target_pose', 10) + + #TODO: need to check that the default topic is correct for the xarm in ros2!!!! (7/21/25) + joint_command_topic = self.declare_parameter('~joint_command_topic', '/xarm/xarm7_velo_traj_controller/command').get_parameter_value().string_value + self.joint_vel_pub = self.create_publisher(JointTrajectory, joint_command_topic, 10) + + + ########################################################################################################## + ############################# CALLBACKS FOR SUBSCRIBERS ################################################## + ########################################################################################################## + def joint_states_callback(self, msg): + """ + Callback function for the joint_states topic. This function will be called whenever a new message is received + on the joint_states topic. The message is a sensor_msgs/JointState message, which contains the current joint + and the corresponding joint velocities and efforts. This function will extract the joint positions, velocities, + and efforts and return them as lists. + """ + self.joint_states = msg + + def teleop_control_callback(self, msg): + """ + Callback function for the teleop_control topic (if using an interface). This function will be called whenever a new message is received + on the teleop_control topic. The message is a geometry_msgs/TwistStamped message, which contains the current + velocity of the end effector. This function will extract the linear and angular velocities and return them as + lists. + """ + teleop_control_command = np.array([msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z, msg.twist.angular.x, msg.twist.angular.y, msg.twist.angular.z]) + position_velocity = teleop_control_command[:3] + angular_velocity = teleop_control_command[3:] + position_velocity_norm = np.linalg.norm(position_velocity) + angular_velocity_norm = np.linalg.norm(angular_velocity) + if position_velocity_norm > 0.05: + position_velocity = position_velocity / position_velocity_norm * 0.05 + self.teleop_control_command = np.array([position_velocity[0], position_velocity[1],position_velocity[2],angular_velocity[0],angular_velocity[1],angular_velocity[2]]) #check if norm of the space mouse command is greater than 0.05, if so normalize it to this value + + #ensure we can get into the while loop + if self.use_teleop_control == True: + self.target_pose = PoseStamped() #set this to get in a loop + + def target_pose_callback(self, msg): + """ + Callback function for the target_pose topic. This function will be called whenever a new message is received + on the target_pose topic. The message is a geometry_msgs/PoseStamped message, which contains the target pose + """ + self.target_pose = msg + self.current_target_pose_pub.publish(self.target_pose) #if target pose is paused manually, this allows us to track the current target pose seen by the script + + def EndEffectorPose(self, q): + """ + This function computes the end-effector pose given the joint configuration q. + """ + current_pose = PoseStamped() + current_pose.header.frame_id = self.base_link + current_pose.header.stamp = self.get_clock().now().to_msg() + try: + trans = self.tf2_buffer.lookup_transform(self.base_link, self.end_link, rclpy.time.Time(), timeout=self.tf2_timeout) + current_pose.pose.position.x = trans.transform.translation.x + current_pose.pose.position.y = trans.transform.translation.y + current_pose.pose.position.z = trans.transform.translation.z + current_pose.pose.orientation.x = trans.transform.rotation.x + current_pose.pose.orientation.y = trans.transform.rotation.y + current_pose.pose.orientation.z = trans.transform.rotation.z + current_pose.pose.orientation.w = trans.transform.rotation.w + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): + self.get_logger().error("TF lookup failed") + self.get_logger().error("failed to lookup transform from %s to %s", self.base_link, self.end_link) + self.current_end_effector_pose_pub.publish(current_pose) + return current_pose + + ########################################################################################################## + ####################################### HELPER FUNCTIONS ################################################# + ########################################################################################################## + def rad2deg(self, q): + return q/math.pi*180.0 + + def EndEffectorVelocity(self, q, dq): + """ + This function computes the end-effector velocity given the joint configuration q and joint velocities dq. + """ + J = self.jacobian_calculator.jacobian(q) + J = np.array(J) + dq = np.array(dq) + dx = np.dot(J, dq) + return dx + + def rotation_matrix_to_axis_angle(self,R): + """ + Converts a rotation matrix to an axis-angle vector. + + Parameters: + R (numpy.ndarray): A 3x3 rotation matrix. + + Returns: + numpy.ndarray: Axis-angle vector (3 elements). + """ + if not (R.shape == (3, 3) and np.allclose(np.dot(R.T, R), np.eye(3)) and np.isclose(np.linalg.det(R), 1)): + raise ValueError("Input matrix must be a valid rotation matrix.") + + # Calculate the angle of rotation + angle = np.arccos((np.trace(R) - 1) / 2) + + if np.isclose(angle, 0): # No rotation + return np.zeros(3) + + if np.isclose(angle, np.pi): # 180 degrees rotation + # Special case for 180-degree rotation + # Extract the axis from the diagonal of R + axis = np.sqrt((np.diag(R) + 1) / 2) + # Adjust signs based on the matrix off-diagonals + axis[0] *= np.sign(R[2, 1] - R[1, 2]) + axis[1] *= np.sign(R[0, 2] - R[2, 0]) + axis[2] *= np.sign(R[1, 0] - R[0, 1]) + return axis * angle + + # General case + axis = np.array([ + R[2, 1] - R[1, 2], + R[0, 2] - R[2, 0], + R[1, 0] - R[0, 1] + ]) / (2 * np.sin(angle)) + + return axis * angle + + def pose_error(self, current_pose, target_pose, error_norm_max = 0.05, control_pose_error = True): + """ + This function computes the error between the current pose and the target pose. + """ + #Compute the position error + position_error = np.array([target_pose.pose.position.x - current_pose.pose.position.x, + target_pose.pose.position.y - current_pose.pose.position.y, + target_pose.pose.position.z - current_pose.pose.position.z]) + #if the norm of the position error is greater than the maximum allowed, scale it down + if control_pose_error == True: + if np.linalg.norm(position_error) > error_norm_max: + position_error = position_error / np.linalg.norm(position_error) * error_norm_max + + #convert the quaternion in posestamped to list of quaternions then pass this into quaternion_matrix to get the rotation matrix. Access only the rotation part of the matrix + goal_rotation_matrix = quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])[:3,:3] + current_rotation_matrix = quaternion_matrix([current_pose.pose.orientation.x, current_pose.pose.orientation.y, current_pose.pose.orientation.z, current_pose.pose.orientation.w])[:3,:3] + #Compute the orientation error + R_error= np.dot(goal_rotation_matrix, np.linalg.inv(current_rotation_matrix)) + # Extract the axis-angle lie algebra vector from the rotation matrix + angle_error = self.rotation_matrix_to_axis_angle(R_error) + # Return the position and orientation error + return position_error, angle_error + + def axis_angle_to_rotation_matrix(self, axis_angle): + """ + Converts an axis-angle vector to a rotation matrix. + + Parameters: + axis_angle (numpy.ndarray): Axis-angle vector (3 elements). + + Returns: + numpy.ndarray: A 3x3 rotation matrix. + """ + # Extract the axis and angle + axis = axis_angle / np.linalg.norm(axis_angle) + angle = np.linalg.norm(axis_angle) + + # Compute the skew-symmetric matrix + K = np.array([ + [0, -axis[2], axis[1]], + [axis[2], 0, -axis[0]], + [-axis[1], axis[0], 0] + ]) + + # Compute the rotation matrix using the Rodrigues' formula + R = np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * np.dot(K, K) + + return R + + def rotation_matrix_to_quaternion(self, R): + """ + Converts a rotation matrix to a quaternion. + + Parameters: + R (numpy.ndarray): A 3x3 rotation matrix. + + Returns: + numpy.ndarray: A 4-element quaternion. + """ + if not (R.shape == (3, 3) and np.allclose(np.dot(R.T, R), np.eye(3)) and np.isclose(np.linalg.det(R), 1)): + raise ValueError("Input matrix must be a valid rotation matrix.") + + # Compute the quaternion using the method from the book + q = np.zeros(4) + q[0] = np.sqrt(1 + R[0, 0] + R[1, 1] + R[2, 2]) / 2 + q[1] = (R[2, 1] - R[1, 2]) / (4 * q[0]) + q[2] = (R[0, 2] - R[2, 0]) / (4 * q[0]) + q[3] = (R[1, 0] - R[0, 1]) / (4 * q[0]) + + return q + + def velocity_home_robot(self): + """ + This function commands the robot to the home position using joint velocities. + """ + # do the following in a loop for 5 seconds: + t_start = self.get_clock().now() + duration = 0.0 + while not rclpy.is_shutdown(): + if duration >= 5.0: + break + self.get_logger().info("Homing the robot") + if self.joint_states: + duration = (self.get_clock().now() - t_start).seconds + q = [] + dq = [] + for joint_name in self.joint_names: + idx = self.joint_states.name.index(joint_name) + q.append(self.joint_states.position[idx]) + dq.append(self.joint_states.velocity[idx]) + self.current_positions = q + # Command the joint velocities + if self.is_sim == True: + kp_gain = 10.0 + else: + kp_gain = 0.5 + q_home = [-0.03142359480261803, -0.5166178941726685, 0.12042707949876785, 0.9197863936424255, -0.03142168000340462, 1.4172008037567139, 0.03490765020251274] + #now find the error between the current position and the home position and use joint velocities to move towards the home position + joint_velocities_list = kp_gain * (np.array(q_home) - np.array(q)) + + self.command_joint_velocities(joint_velocities_list) + + + ############################################################################################################## + ######################################### COMMANDS TO THE ROBOT ############################################## + ############################################################################################################## + def publish_pose_errors(self, position_error, angular_error, control_pose_error = True): + """ + Publishes the position and orientation errors as ROS messages. + """ + pose_error_msg = PoseStamped() + pose_error_msg.header.frame_id = self.base_link + pose_error_msg.header.stamp = self.get_clock().now().to_msg() + pose_error_msg.pose.position.x = position_error[0] + pose_error_msg.pose.position.y = position_error[1] + pose_error_msg.pose.position.z = position_error[2] + #for completeness, convert the axis angle to quaternion + Rmat = self.axis_angle_to_rotation_matrix(angular_error) + quat_from_Rmat = self.rotation_matrix_to_quaternion(Rmat) + # quat_from_Rmat = tf.transformations.quaternion_from_matrix(Rmat) + pose_error_msg.pose.orientation.x = quat_from_Rmat[0] + pose_error_msg.pose.orientation.y = quat_from_Rmat[1] + pose_error_msg.pose.orientation.z = quat_from_Rmat[2] + pose_error_msg.pose.orientation.w = quat_from_Rmat[3] + #publish the axis-angle orientation error + orientation_error_msg = Vector3() + orientation_error_msg.x = angular_error[0] + orientation_error_msg.y = angular_error[1] + orientation_error_msg.z = angular_error[2] + #publish the position error + position_error_msg = Vector3() + position_error_msg.x = position_error[0] + position_error_msg.y = position_error[1] + position_error_msg.z = position_error[2] + + #Now publish + if control_pose_error == True: + self.position_error_control_pub.publish(position_error_msg) + self.pose_error_control_pub.publish(pose_error_msg) + self.orientation_error_control_pub.publish(orientation_error_msg) + else: + self.pose_error_pub.publish(pose_error_msg) + self.position_error_pub.publish(position_error_msg) + self.orientation_error_pub.publish(orientation_error_msg) + + def command_joint_velocities(self, joint_vel_list): + """ + This function commands the joint velocities to the robot using the appropriate ROS message type. + """ + if self.is_sim: + # Create the JointTrajectory message + trajectory_msg = JointTrajectory() + trajectory_msg.joint_names = self.joint_names + + # Create a trajectory point + point = JointTrajectoryPoint() + + # Use current positions + point.positions = self.current_positions + + # Set velocities + #make velocity negative because Xarm has cw positive direction for joint velocities + joint_vel_list = [-v for v in joint_vel_list] + point.velocities = joint_vel_list #this is negative because Xarm has cw positive direction for joint velocities + + # Set accelerations to zero + point.accelerations = [0.0] * len(self.joint_names) + + # Effort (optional; set to None or skip if not needed) + point.effort = [0.0] * len(self.joint_names) + + # Set the time from start + point.time_from_start = rclpy.duration.Duration(seconds=0.1).to_msg() # 100 ms + + # Add the point to the trajectory + trajectory_msg.points.append(point) + + # Publish the trajectory + trajectory_msg.header.stamp = self.get_clock().now().to_msg() # Update timestamp + self.joint_vel_pub.publish(trajectory_msg) + else: + # this is on the real robot, directly send joint velociteies + # Send joint velocities to the arm + # log the velocities + self.get_logger().info("Joint velocities: %s", joint_vel_list) + if self.use_teleop_control_jparse: + self.arm.vc_set_joint_velocity(joint_vel_list, is_radian=True) + + ############################################################################################################### + ######################################### CONTROL LOOP ######################################################## + ############################################################################################################### + def control_loop(self): + """ + This function implements the control loop for the arm controller. + """ + + while not rclpy.is_shutdown(): + + if self.joint_states is not None and self.teleop_control_command is not None: # edited this 7/11/25 + t = self.get_clock().now() + # obtain the current joints + q = [] + dq = [] + effort = [] + for joint_name in self.joint_names: + idx = self.joint_states.name.index(joint_name) + q.append(self.joint_states.position[idx]) + dq.append(self.joint_states.velocity[idx]) + effort.append(self.joint_states.effort[idx]) + self.current_positions = q + # Calculate the JParsed Jacobian + + method = self.method #set by parameter, can be set from launch file + self.get_logger().info("Method being used: %s", method) + if method == "JacobianPseudoInverse": + #this is the traditional pseudo-inverse method for the jacobian + J_method, J_nullspace = self.jacobian_calculator.JacobianPseudoInverse(q=q, position_only=self.position_only, jac_nullspace_bool=True) + elif method == "JParse": + # The JParse method takes in the joint angles, gamma, position_only, and singular_direction_gain + if self.show_jparse_ellipses == True: + J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position, singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True, publish_jparse_ellipses=True, end_effector_pose=self.EndEffectorPose(q), verbose=False) + else: + J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position,singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True) + elif method == "JacobianDampedLeastSquares": + J_method, J_nullspace = self.jacobian_calculator.jacobian_damped_least_squares(q=q, damping=0.1, jac_nullspace_bool=True) #dampening of 0.1 works very well, 0.8 shows clear error + elif method == "JacobianProjection": + J_proj, J_nullspace = self.jacobian_calculator.jacobian_projection(q=q, gamma=0.1, jac_nullspace_bool=True) + J_method = np.linalg.pinv(J_proj) + elif method == "JacobianSafety": + # The JParse method takes in the joint angles, gamma, position_only, and singular_direction_gain + if self.show_jparse_ellipses == True: + J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position, singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True, publish_jparse_ellipses=True, end_effector_pose=self.EndEffectorPose(q), verbose=False, safety_only=True) + else: + J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position,singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True, safety_only=True) + elif method == "JacobianSafetyProjection": + # The JParse method takes in the joint angles, gamma, position_only, and singular_direction_gain + if self.show_jparse_ellipses == True: + J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position, singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True, publish_jparse_ellipses=True, end_effector_pose=self.EndEffectorPose(q), verbose=False, safety_projection_only=True) + else: + J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position,singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True, safety_projection_only=True) + + + manip_measure = self.jacobian_calculator.manipulability_measure(q) + self.manip_measure_pub.publish(manip_measure) + inverse_cond_number = self.jacobian_calculator.inverse_condition_number(q) + self.inverse_cond_number.publish(inverse_cond_number) + self.get_logger().info("Manipulability measure: %f", manip_measure) + self.get_logger().info("Inverse condition number: %f", inverse_cond_number) + #Calculate the delta_x (task space error) + target_pose = self.target_pose + current_pose = self.EndEffectorPose(q) + position_error, angular_error = self.pose_error(current_pose, target_pose, control_pose_error=False) + # log the pose error + self.publish_pose_errors(position_error, angular_error, control_pose_error=False) + #now find error for control (max error norm) + if self.is_sim == False: + #real world limits + error_norm_max = 0.10 + else: + #simulation limits + error_norm_max = 1.0 + position_error, angular_error = self.pose_error(current_pose, target_pose, error_norm_max = error_norm_max, control_pose_error=True) + #move in nullspace towards nominal pose (for xarm its 0 joint angle for Xarm7) + if self.is_sim == True: + kp_gain = 2.0 + else: + # kp_gain = 1.0 + kp_gain = 3.0 + + if self.is_sim == True: + nominal_motion_nullspace = np.matrix([-v*kp_gain for v in q]).T #send to home which is 0 joint position for all joints + else: + # nominal motion nullspace which checks if q magnitude is below threshold and chooses the minimum + null_space_angle_rate = 0.6 + + nominal_motion_nullspace = np.matrix([np.sign(-v*kp_gain)*np.min([np.abs(-v*kp_gain),null_space_angle_rate]) for v in q]).T #send to home which is 0 joint position for all joints + # log the pose error + self.publish_pose_errors(position_error, angular_error, control_pose_error=True) + + # Calculate and command the joint velocities + if self.position_only == True: + self.get_logger().info("Position only control") + position_error = np.matrix(position_error).T + if self.is_sim == True: + #use these gains only in simulation! + kp_gain = 10.0 + else: + kp_gain = 3.0 + Kp = np.diag([kp_gain, kp_gain, kp_gain]) # Proportional gain matrix + + task_vector = Kp @ position_error + joint_velocities = J_method @ task_vector + J_nullspace @ nominal_motion_nullspace + else: + # realworld gains (tested) + kp_gain_position = 1.0 + kp_gain_orientation = 1.0 + if self.is_sim == True: + #use these gains only in simulation! + kp_gain_position = 10.0 + kp_gain_orientation = 10.0 + Kp_position = np.diag([kp_gain_position, kp_gain_position, kp_gain_position]) # Proportional gain matrix + Kp_orientation = np.diag([kp_gain_orientation, kp_gain_orientation, kp_gain_orientation]) + task_position = Kp_position @ np.matrix(position_error).T + task_orientation = Kp_orientation @ np.matrix(angular_error).T + full_pose_error = np.matrix(np.concatenate((task_position.T, task_orientation.T),axis=1)).T + if self.use_teleop_control == False: + joint_velocities = J_method @ full_pose_error + J_nullspace @ nominal_motion_nullspace + if self.use_teleop_control == True: + #use the teleop control command to control the joint velocities + teleop_control_command = np.matrix(self.teleop_control_command).T + #now add this to the joint velocities + joint_velocities = J_method @ teleop_control_command + J_nullspace @ nominal_motion_nullspace + #check this + joint_velocities_list = np.array(joint_velocities).flatten().tolist() + # command the joint velocities + self.command_joint_velocities(joint_velocities_list) #this commands the joint velocities + self.rate.sleep() + self.get_logger().info("Control loop running") + +if __name__ == '__main__': + try: + ArmController() + except Exception as e: + print(f"An error occurred: {e}") + pass \ No newline at end of file From 1cd1d7455826b886c6c9839ad34f2383cc52c449 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Fri, 8 Aug 2025 18:43:50 -0500 Subject: [PATCH 05/27] Port to ROS 2 with pinocchio updates: Taken pinocchio code written in collab by stanford group and integrating it into jparse_cls.py; need to correctly load the urdf -- left notes in xarm_vel_main_experiment.py for things to try Opened an issue for example_robot_data repo that is linked to pinocchio for easier loading of the urdf (asked if they can add the xarm robots) Cleaned the code up and reorganized it -- removed unnecessary functions not need in ROS 2 port --- README.md | 10 +- manipulator_control/CMakeLists.txt | 18 +- manipulator_control/package.xml | 1 + manipulator_control/scripts/jparse_cls.py | 163 +++++++++++++++++- .../scripts/xarm_vel_main_experiment.py | 129 +++++++++----- 5 files changed, 260 insertions(+), 61 deletions(-) diff --git a/README.md b/README.md index 990f11d..f362a0e 100644 --- a/README.md +++ b/README.md @@ -21,7 +21,7 @@ _In Submission_ The `main` branch is supporting *ROS Noetic*. The `ros2_dev` branch is developing the support for *ROS 2 Jazzy* (coming soon). -## FOR ARGALLAB +## FOR ARGALLAB (ROS 2) Instructions to teleop robot with jparse and keyboard: TODO: NEEDS TO BE TESTED and jparse_cls.py needs to be written! @@ -37,10 +37,10 @@ ros2 run teleop_twist_keyboard teleop_twist_keyboard stamped:=True frame_id:=lin Edits need to be still commpleted to test the robot after writing script for joystick and sip/puff teleop. ## FOR ARGALLAB TO DOS (not in order of to do): -- [ ] Need to implement gripper in the `xarm_vel_experimenter.py` -- this is a function call to the api!!! the topic is `/gripper_action` -- [ ] self-avoidance collision -- [ ] work-around for the joint limits so that the robot does *not* immediately need to be power cycled -- [ ] port to ros2 (need pinnochio) +- [x] Need to implement gripper in the `xarm_vel_experimenter.py` -- this is a function call to the api!!! the topic is `/gripper_action` +- [ ] self-avoidance collision -- waiting for ros 2 port +- [ ] work-around for the joint limits so that the robot does *not* immediately need to be power cycled -- waiting for ros 2 port +- [ ] port to ros2 (need pinnochio) -- WORKING ON!! ### Instructions for Argallab diff --git a/manipulator_control/CMakeLists.txt b/manipulator_control/CMakeLists.txt index f2b56fd..f005e7b 100644 --- a/manipulator_control/CMakeLists.txt +++ b/manipulator_control/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(rclcpp REQUIRED) find_package(rclpy REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) include_directories(include) # uncomment the following section in order to fill in @@ -17,23 +18,24 @@ include_directories(include) # find_package( REQUIRED) # make cpp executable -add_executable(cpp_executable src/cpp_node.cpp) # need to edit with the appropriate names later -ament_target_dependencies(cpp_executable rclcpp) +# add_executable(cpp_executable src/cpp_node.cpp) # need to edit with the appropriate names later +# ament_target_dependencies(cpp_executable rclcpp) # install cpp executables -install(TARGETS - cpp_executables - DESTINATION lib/${PROJECT_NAME} -) +# install(TARGETS +# cpp_executables +# DESTINATION lib/${PROJECT_NAME} +# ) # install python modules ament_python_install_package(${PROJECT_NAME}) # install python executables install(PROGRAMS - scripts/python_node.py + scripts/jparse_cls.py + scripts/xarm_vel_main_experiment.py DESTINATION lib/${PROJECT_NAME} -) # need to edit with the appropriate names later +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/manipulator_control/package.xml b/manipulator_control/package.xml index 4238d86..aa96363 100644 --- a/manipulator_control/package.xml +++ b/manipulator_control/package.xml @@ -12,6 +12,7 @@ rclcpp rclpy + eigen ament_lint_auto ament_lint_common diff --git a/manipulator_control/scripts/jparse_cls.py b/manipulator_control/scripts/jparse_cls.py index 434716b..14f21ed 100644 --- a/manipulator_control/scripts/jparse_cls.py +++ b/manipulator_control/scripts/jparse_cls.py @@ -1,28 +1,175 @@ +#!/usr/bin/env python3 import rclpy -from rclpy.Node import Node +from rclpy.node import Node # pinocchio import numpy as np import pinocchio as pin -from urdf_parser_py.urdf import URDF +# from urdf_parser_py.urdf import Robot # ros2 stuff -from visualization_msgs.msg import Marker, MarkerArray -from geometry_msgs.msg import Quaternion, Pose, Point, PoseStamped -from sensor_msgs.msg import JointState +# from visualization_msgs.msg import Marker, MarkerArray +# from geometry_msgs.msg import Quaternion, Pose, Point, PoseStamped +# from sensor_msgs.msg import JointState # this is the huge node that does all the jacobian calculations!! class JParseClass(Node): - def __init__(self): + def __init__(self, base_link="link_base", end_link="link_eef"): # defaults are the xarm ones # Initialize any necessary variables or parameters here """ Base link: The base link of the robot. End link: The end link of the robot. """ super().__init__('jparse_class') + + self.base_link = base_link + self.end_link = end_link + from IPython import embed; embed() + # Load URDF from parameter server + # urdf = Robot.from_parameter_server() + # model = pin.buildModelFromUrdf(urdf.to_xml_string()) + # self.data = model.createData() + # self.model = model + # self.num_joints = 7 + + # publishers + # self.marker_pub = self.create_publisher(MarkerArray, '/jparse_ellipsoid_marker', 10) + + # subscribers + # self.joint_state_sub = self.create_subscription( + # JointState, '/joint_states', self.joint_state_callback, 10) + + self.J_prev = None + self.J_prev_time = None + + def svd_compose(self,U,S,Vt): + """ + This function takes SVD: U,S,V and recomposes them for J + """ + Zero_concat = np.zeros((U.shape[0],Vt.shape[0]-len(S))) + Sfull = np.zeros((U.shape[1],Vt.shape[0])) + for row in range(Sfull.shape[0]): + for col in range(Sfull.shape[1]): + if row == col: + if row < len(S): + Sfull[row,col] = S[row] + J_new =np.matrix(U)*Sfull*np.matrix(Vt) + return J_new + + + def JParse(self, J=None, jac_nullspace_bool=False, gamma=0.1, singular_direction_gain_position=1,singular_direction_gain_orientation=1, position_dimensions=None, angular_dimensions=None): + """ + input: Jacobian J (m x n) numpy matrix + Args: + - jac_nullspace_bool (default=False): Set this to true of the nullspace of the jacobian is desired + - gamma (default=0.1): threshold gain for singular directions + - singular_direction_gain_position (default=1): gain for singular directions in position + - singular_direction_gain_orientation (default=1): gain for singular directions in orientation + - position_dimensions (default=None): the number of dimensions for the position *NOTE: if this is not set, then all gains are be set to the singular_directin_gain_position value + - angular_dimensions (default=None): the number of dimensions for the orientation *NOTE: if this is not set, then all gains are be set to the singular_directin_gain_position value + + Note: if no information is provided for position or angular dimensions, then the rows of the jacobian are used to determine the gain matrix dimension, and the default (or user set) value of singular_direction_gain_position + is used by default. + + output: + - J_parse (n x m) numpy matrix + - (optional) J_safety_nullspace (n x n) numpy matrix : this can be used with a secondary objective like attracting joints to a nominal position (which will not interfere with primary task objective). + This is just the matrix, the joint-space objective must be calculated and multiplied by the user. + """ + + #Perform the SVD decomposition of the jacobian + U, S, Vt = np.linalg.svd(J) + #Find the adjusted condition number + sigma_max = np.max(S) + adjusted_condition_numbers = [sig / sigma_max for sig in S] + + #Find the projection Jacobian + U_new_proj = [] + S_new_proj = [] + for col in range(len(S)): + if S[col] > gamma*sigma_max: + #Singular row + U_new_proj.append(np.matrix(U[:,col]).T) + S_new_proj.append(S[col]) + U_new_proj = np.concatenate(U_new_proj,axis=1) #Careful which numpy version is being used for this!!!!! + J_proj = self.svd_compose(U_new_proj, S_new_proj, Vt) + + #Find the safety jacboian + S_new_safety = [s if (s/sigma_max) > gamma else gamma*sigma_max for s in S] + J_safety = self.svd_compose(U,S_new_safety,Vt) + + #Find the singular direction projection components + U_new_sing = [] + Phi = [] #these will be the ratio of s_i/s_max + set_empty_bool = True + for col in range(len(S)): + if adjusted_condition_numbers[col] <= gamma: + set_empty_bool = False + U_new_sing.append(np.matrix(U[:,col]).T) + Phi.append(adjusted_condition_numbers[col]/gamma) #division by gamma for s/(s_max * gamma), gives smooth transition for Kp =1.0; + + #set an empty Phi_singular matrix, populate if there were any adjusted + #condition numbers below the threshold + Phi_singular = np.zeros(U.shape) #initialize the singular projection matrix + + if set_empty_bool == False: + #construct the new U, as there were singular directions + U_new_sing = np.matrix(np.concatenate(U_new_sing,axis=1)) #Careful which numpy version is being used for this!!!!! + Phi_mat = np.matrix(np.diag(Phi)) + + # Now handle the gain conditions + if position_dimensions == None and angular_dimensions == None: + #neither dimensions have been set, this is the default case + gain_dimension = J.shape[0] + gains = np.array([singular_direction_gain_position]*gain_dimension, dtype=float) + elif angular_dimensions == None and position_dimensions != None: + #only position dimensions have been set + gain_dimension = position_dimensions + gains = np.array([singular_direction_gain_position]*gain_dimension, dtype=float) + elif position_dimensions == None and angular_dimensions != None: + #only angular dimensions have been set + gain_dimension = angular_dimensions + gains = np.array([singular_direction_gain_orientation]*gain_dimension, dtype=float) + else: + #both position and angular dimensions are filled + gains = np.array([singular_direction_gain_position]*position_dimensions + [singular_direction_gain_orientation]*angular_dimensions, dtype=float) + #now put them into a matrix: + Kp_singular = np.diag(gains) + + # Now put it all together: + Phi_singular = U_new_sing @ Phi_mat @ U_new_sing.T @ Kp_singular + + #Obtain psuedo-inverse of the safety jacobian and the projection jacobian + J_safety_pinv = np.linalg.pinv(J_safety) + J_proj_pinv = np.linalg.pinv(J_proj) + + if set_empty_bool == False: + J_parse = J_safety_pinv @ J_proj @ J_proj_pinv + J_safety_pinv @ Phi_singular + else: + J_parse = J_safety_pinv @ J_proj @ J_proj_pinv + + if jac_nullspace_bool == True: + #Find the nullspace of the jacobian + J_safety_nullspace = np.eye(J_safety.shape[1]) - J_safety_pinv @ J_safety + return J_parse, J_safety_nullspace + + return J_parse + +def main(args=None): + rclpy.init(args=args) + node = JParseClass() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == '__main__': + main() + + + +""" # parameters self.declare_parameter('base_link', 'base_link', description='Base link of the robot') @@ -53,6 +200,6 @@ def __init__(self): self.create_timer(0.1, self.timer_callback) def timer_callback(self): - - + pass +""" \ No newline at end of file diff --git a/manipulator_control/scripts/xarm_vel_main_experiment.py b/manipulator_control/scripts/xarm_vel_main_experiment.py index 33202c2..6b611db 100644 --- a/manipulator_control/scripts/xarm_vel_main_experiment.py +++ b/manipulator_control/scripts/xarm_vel_main_experiment.py @@ -2,6 +2,7 @@ import rclpy from rclpy.node import Node import numpy as np +import pinocchio as pin import math import time @@ -9,7 +10,7 @@ import tf2_ros from tf.transformations import quaternion_from_euler, quaternion_matrix from geometry_msgs.msg import Pose, PoseStamped, Vector3, TwistStamped -from std_msgs.msg import Float64MultiArray, Float64 +from std_msgs.msg import Float64MultiArray, Float64, Float32 from sensor_msgs.msg import JointState from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint @@ -26,8 +27,8 @@ def __init__(self): super().__init__('arm_controller') # Initialize parameters - self.base_link = self.declare_parameter('/base_link', 'base_link').get_parameter_value().string_value # defaults are for Kinova gen3 - self.end_link = self.declare_parameter('/end_link', 'lnk_eef').get_parameter_value().string_value + self.base_link = self.declare_parameter('/base_link', 'link_base').get_parameter_value().string_value # defaults are for xarm + self.end_link = self.declare_parameter('/end_link', 'link_eef').get_parameter_value().string_value self.is_sim = self.declare_parameter('~is_sim', False).get_parameter_value().bool_value #boolean to control if the robot is in simulation or not @@ -77,6 +78,16 @@ def __init__(self): self.get_logger().info("ArmController initialized with base link: {}, end link: {}".format(self.base_link, self.end_link)) self.tf2_timeout = rclpy.duration.Duration(seconds=1.0) + # gripper initialization + self.gripper_pose = 900 # means the gripper is open + self.gripper_speed = 300 # max + self.gripper_update_rate = 10 # Hz + self.gripper_min = 0 # closed + self.gripper_max = 900 # fully opened + # this enables the gripper and then sets the gripper to open position + self.arm.set_gripper_enable(True) + self.arm.set_gripper_position(self.gripper_pose) + if self.is_sim == True: #if we are in simulation, use the joint_states and target_pose topics self.joint_states_topic = '/xarm/joint_states' @@ -94,18 +105,38 @@ def __init__(self): # Initialize the current positions self.current_positions = [0.0] * len(self.joint_names) + ## NEW for pinocchio (8/8/2025) ## + # hard-coded for now + model_path = "/home/workspace/src/xarm_ros2/xarm_description/xarm7" + urdf_filename = "xarm7.urdf.xacro" ## note to self: need to change this from xacro to just urdf usign the command "xacro xarm7.urdf.xacro > xarm7.urdf" -- however, the robot needs to be launched but this is not working currently when I tested it + urdf_path = model_path + "/" + urdf_filename + + # to test as well + # urdf_string = self.get_parameter('robot_description').value + # self.model = pin.RobotWrapper.BuildFromURDF(urdf_string) # need to figure out what this does differently from the one below + pin.buildModelsFromUrdf() + # Create model and data + self.model = pin.buildModelsFromUrdf(urdf_filename) #pin.Model() + self.data = self.model.createData() + + # there is also this method: + + + + + self.rate = self.create_rate(10) # 10 Hz #home the robot - try: - self.velocity_home_robot() - except Exception as e: - self.get_logger().error(f"Failed to home the robot: {e}") - pass - finally: - # Clean up resources if needed - self.get_logger().info("Shutting down the control loop") - joint_zero_velocities = [0.0] * len(self.joint_names) - self.command_joint_velocities(joint_zero_velocities) + # try: + # self.velocity_home_robot() + # except Exception as e: + # self.get_logger().error(f"Failed to home the robot: {e}") + # pass + # finally: + # # Clean up resources if needed + # self.get_logger().info("Shutting down the control loop") + joint_zero_velocities = [0.0] * len(self.joint_names) + self.command_joint_velocities(joint_zero_velocities) #now run the control loop self.control_loop() @@ -120,6 +151,8 @@ def initialize_subscribers(self): PoseStamped, '/target_pose', self.target_pose_callback, 10) self.teleop_control_sub = self.create_subscription( TwistStamped, 'robot_action', self.teleop_control_callback, 10) + self.gripper_action_sub = self.create_subscription( + Float32, '/gripper_action', self.gripper_position_callback) def initialize_publishers(self): # publishers @@ -145,6 +178,15 @@ def initialize_publishers(self): ########################################################################################################## ############################# CALLBACKS FOR SUBSCRIBERS ################################################## ########################################################################################################## + def gripper_position_callback(self, msg): + """Callback for the gripper. It includes the gripper position value. Float32 value.""" + # self.get_logger().info(msg) + joystick_gripper_pose = msg.data + # convert the joystick value to gripper poses [-1, 1] is to [0, 900], where 0 is close and 900 is open + delta = joystick_gripper_pose * (self.gripper_speed / self.gripper_update_rate) + self.gripper_pose += delta + self.gripper_pose = max(min(self.gripper_pose, self.gripper_max), self.gripper_min) + def joint_states_callback(self, msg): """ Callback function for the joint_states topic. This function will be called whenever a new message is received @@ -181,7 +223,13 @@ def target_pose_callback(self, msg): """ self.target_pose = msg self.current_target_pose_pub.publish(self.target_pose) #if target pose is paused manually, this allows us to track the current target pose seen by the script - + + ########################################################################################################## + ####################################### HELPER FUNCTIONS ################################################# + ########################################################################################################## + def rad2deg(self, q): + return q/math.pi*180.0 + def EndEffectorPose(self, q): """ This function computes the end-effector pose given the joint configuration q. @@ -204,21 +252,16 @@ def EndEffectorPose(self, q): self.current_end_effector_pose_pub.publish(current_pose) return current_pose - ########################################################################################################## - ####################################### HELPER FUNCTIONS ################################################# - ########################################################################################################## - def rad2deg(self, q): - return q/math.pi*180.0 - - def EndEffectorVelocity(self, q, dq): - """ - This function computes the end-effector velocity given the joint configuration q and joint velocities dq. - """ - J = self.jacobian_calculator.jacobian(q) - J = np.array(J) - dq = np.array(dq) - dx = np.dot(J, dq) - return dx + # not needed for the xarm control so commenting out + # def EndEffectorVelocity(self, q, dq): + # """ + # This function computes the end-effector velocity given the joint configuration q and joint velocities dq. + # """ + # J = self.jacobian_calculator.jacobian(q) + # J = np.array(J) + # dq = np.array(dq) + # dx = np.dot(J, dq) + # return dx def rotation_matrix_to_axis_angle(self,R): """ @@ -452,6 +495,8 @@ def command_joint_velocities(self, joint_vel_list): def control_loop(self): """ This function implements the control loop for the arm controller. + + TODO: Need to compute the jacobian and then pass that to JParse and dont need all the comparisons!! (8/8/25) """ while not rclpy.is_shutdown(): @@ -469,12 +514,21 @@ def control_loop(self): effort.append(self.joint_states.effort[idx]) self.current_positions = q # Calculate the JParsed Jacobian + + # NEW # 8/8/2025 + # 1) Forward kinematics + pin.forwardKinematics(self.model, self.data, q) + pin.updateFramePlacements(self.model, self.data) + + # 2) Compute joint Jacobians + pin.computeJointJacobians(self.model, self.data, q) method = self.method #set by parameter, can be set from launch file self.get_logger().info("Method being used: %s", method) if method == "JacobianPseudoInverse": #this is the traditional pseudo-inverse method for the jacobian - J_method, J_nullspace = self.jacobian_calculator.JacobianPseudoInverse(q=q, position_only=self.position_only, jac_nullspace_bool=True) + # J_method, J_nullspace = self.jacobian_calculator.JacobianPseudoInverse(q=q, position_only=self.position_only, jac_nullspace_bool=True) + raise NotImplementedError elif method == "JParse": # The JParse method takes in the joint angles, gamma, position_only, and singular_direction_gain if self.show_jparse_ellipses == True: @@ -482,10 +536,12 @@ def control_loop(self): else: J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position,singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True) elif method == "JacobianDampedLeastSquares": - J_method, J_nullspace = self.jacobian_calculator.jacobian_damped_least_squares(q=q, damping=0.1, jac_nullspace_bool=True) #dampening of 0.1 works very well, 0.8 shows clear error + # J_method, J_nullspace = self.jacobian_calculator.jacobian_damped_least_squares(q=q, damping=0.1, jac_nullspace_bool=True) #dampening of 0.1 works very well, 0.8 shows clear error + raise NotImplementedError elif method == "JacobianProjection": - J_proj, J_nullspace = self.jacobian_calculator.jacobian_projection(q=q, gamma=0.1, jac_nullspace_bool=True) - J_method = np.linalg.pinv(J_proj) + # J_proj, J_nullspace = self.jacobian_calculator.jacobian_projection(q=q, gamma=0.1, jac_nullspace_bool=True) + # J_method = np.linalg.pinv(J_proj) + raise NotImplementedError elif method == "JacobianSafety": # The JParse method takes in the joint angles, gamma, position_only, and singular_direction_gain if self.show_jparse_ellipses == True: @@ -499,13 +555,6 @@ def control_loop(self): else: J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position,singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True, safety_projection_only=True) - - manip_measure = self.jacobian_calculator.manipulability_measure(q) - self.manip_measure_pub.publish(manip_measure) - inverse_cond_number = self.jacobian_calculator.inverse_condition_number(q) - self.inverse_cond_number.publish(inverse_cond_number) - self.get_logger().info("Manipulability measure: %f", manip_measure) - self.get_logger().info("Inverse condition number: %f", inverse_cond_number) #Calculate the delta_x (task space error) target_pose = self.target_pose current_pose = self.EndEffectorPose(q) From 00eeccbc0c011488d0861e10826a7a896d5433f7 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Mon, 11 Aug 2025 19:20:33 -0500 Subject: [PATCH 06/27] Big update: - robot is able to receive commands to home the robot - still need to test the jparse provided by Stanford group - needed to lower controller manager parameter in xarm_ros2/xarm_controller/config/xarm7_controller.yaml from 150 to 50 because computer could not keep up - lots of edits made to work for ros2 as well --- manipulator_control/CMakeLists.txt | 6 + .../launch/xarm_main_vel.launch.xml | 28 ++-- manipulator_control/scripts/jparse_cls.py | 2 +- .../scripts/xarm_vel_main_experiment.py | 152 ++++++++++++------ 4 files changed, 122 insertions(+), 66 deletions(-) diff --git a/manipulator_control/CMakeLists.txt b/manipulator_control/CMakeLists.txt index f005e7b..446736d 100644 --- a/manipulator_control/CMakeLists.txt +++ b/manipulator_control/CMakeLists.txt @@ -37,6 +37,12 @@ install(PROGRAMS DESTINATION lib/${PROJECT_NAME} ) +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/manipulator_control/launch/xarm_main_vel.launch.xml b/manipulator_control/launch/xarm_main_vel.launch.xml index 76d68f7..bc6cda3 100644 --- a/manipulator_control/launch/xarm_main_vel.launch.xml +++ b/manipulator_control/launch/xarm_main_vel.launch.xml @@ -18,24 +18,24 @@ - - + + - + - - - + + + - - - - - - - - + + + + + + + + \ No newline at end of file diff --git a/manipulator_control/scripts/jparse_cls.py b/manipulator_control/scripts/jparse_cls.py index 14f21ed..e722297 100644 --- a/manipulator_control/scripts/jparse_cls.py +++ b/manipulator_control/scripts/jparse_cls.py @@ -26,7 +26,7 @@ def __init__(self, base_link="link_base", end_link="link_eef"): # defaults are t self.base_link = base_link self.end_link = end_link - from IPython import embed; embed() + # from IPython import embed; embed() # Load URDF from parameter server # urdf = Robot.from_parameter_server() # model = pin.buildModelFromUrdf(urdf.to_xml_string()) diff --git a/manipulator_control/scripts/xarm_vel_main_experiment.py b/manipulator_control/scripts/xarm_vel_main_experiment.py index 6b611db..64519fe 100644 --- a/manipulator_control/scripts/xarm_vel_main_experiment.py +++ b/manipulator_control/scripts/xarm_vel_main_experiment.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python3 import sys import rclpy from rclpy.node import Node @@ -8,11 +9,19 @@ # ros2 stuff import tf2_ros -from tf.transformations import quaternion_from_euler, quaternion_matrix +### +# Depricated ROS1 package; using transforms3d or scipy.spatial.transform for ROS2 +# from tf2.transformations import quaternion_from_euler, quaternion_matrix +# from transforms3d import quaternion_matrix, quaternion_from_euler +from scipy.spatial.transform import Rotation as R +### + from geometry_msgs.msg import Pose, PoseStamped, Vector3, TwistStamped from std_msgs.msg import Float64MultiArray, Float64, Float32 from sensor_msgs.msg import JointState from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from rclpy.qos import QoSProfile, QoSDurabilityPolicy, ReliabilityPolicy, DurabilityPolicy +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup try: from xarm.wrapper import XArmAPI @@ -26,6 +35,8 @@ class ArmController(Node): def __init__(self): super().__init__('arm_controller') + self.get_logger().info("node started") + # Initialize parameters self.base_link = self.declare_parameter('/base_link', 'link_base').get_parameter_value().string_value # defaults are for xarm self.end_link = self.declare_parameter('/end_link', 'link_eef').get_parameter_value().string_value @@ -39,9 +50,9 @@ def __init__(self): self.jparse_gamma = self.declare_parameter('~jparse_gamma', 0.1).get_parameter_value().double_value #gamma for the JParse method # used to be self.use_space_mouse and self.use_space_mouse_jparse - self.use_teleop_control = self.declare_parameter('~use_teleop_control', False).get_parameter_value().bool_value #boolean to control if the robot is in teleoperation mode or not - self.use_teleop_control_jparse = self.declare_parameter('~use_teleop_control_jparse', False).get_parameter_value().bool_value #boolean to control if the robot is in teleoperation mode with JParse - + self.use_teleop_control = self.declare_parameter('~use_teleop_control', True).get_parameter_value().bool_value #boolean to control if the robot is in teleoperation mode or not + self.use_teleop_control_jparse = self.declare_parameter('~use_teleop_control_jparse', True).get_parameter_value().bool_value #boolean to control if the robot is in teleoperation mode with JParse + self.get_logger().info(f"VALUES HERE {self.use_teleop_control} and {self.use_teleop_control_jparse}") # used to be self.space_mouse_command self.teleop_control_command = np.array([0,0,0,0,0,0]).T @@ -88,12 +99,6 @@ def __init__(self): self.arm.set_gripper_enable(True) self.arm.set_gripper_position(self.gripper_pose) - if self.is_sim == True: - #if we are in simulation, use the joint_states and target_pose topics - self.joint_states_topic = '/xarm/joint_states' - else: - self.joint_states_topic = '/joint_states' - # Initialize publishers and subscribers self.initialize_subscribers() @@ -107,26 +112,28 @@ def __init__(self): ## NEW for pinocchio (8/8/2025) ## # hard-coded for now - model_path = "/home/workspace/src/xarm_ros2/xarm_description/xarm7" - urdf_filename = "xarm7.urdf.xacro" ## note to self: need to change this from xacro to just urdf usign the command "xacro xarm7.urdf.xacro > xarm7.urdf" -- however, the robot needs to be launched but this is not working currently when I tested it - urdf_path = model_path + "/" + urdf_filename + urdf_filename = "/workspace/xarm7.urdf" + # urdf_filename = "xarm7.urdf.xacro" ## note to self: need to change this from xacro to just urdf usign the command "xacro xarm7.urdf.xacro > xarm7.urdf" -- however, the robot needs to be launched but this is not working currently when I tested it + # urdf_path = model_path + "/" + urdf_filename - # to test as well + + # to test as wellquaternion_matrix # urdf_string = self.get_parameter('robot_description').value - # self.model = pin.RobotWrapper.BuildFromURDF(urdf_string) # need to figure out what this does differently from the one below - pin.buildModelsFromUrdf() + # robot = pin.RobotWrapper.BuildFromURDF(urdf_string) # need to figure out what this does differently from the one below + # self.model = robot.model + # self.data = robot.data + + # pin.buildModelsFromUrdf() # Create model and data - self.model = pin.buildModelsFromUrdf(urdf_filename) #pin.Model() + # from IPython import embed; embed(banner1="looking at pin stuff") + self.model = pin.buildModelFromUrdf(urdf_filename) #pin.Model() self.data = self.model.createData() - # there is also this method: + # another way: + # from robot data - - - - - self.rate = self.create_rate(10) # 10 Hz - #home the robot + # self.rate = self.create_rate(10) # 10 Hz + # #home the robot # try: # self.velocity_home_robot() # except Exception as e: @@ -135,24 +142,43 @@ def __init__(self): # finally: # # Clean up resources if needed # self.get_logger().info("Shutting down the control loop") - joint_zero_velocities = [0.0] * len(self.joint_names) - self.command_joint_velocities(joint_zero_velocities) + # joint_zero_velocities = [0.0] * len(self.joint_names) + # # self.timer = self.create_timer(10, self.command_joint_velocities(joint_zero_velocities)) + # self.command_joint_velocities(joint_zero_velocities) #now run the control loop - self.control_loop() + # self.control_loop() + + # self.timer = self.create_timer(10, self.timer_callback) + + + # def timer_callback(self): + # if self.joint_states is not None: + # self.velocity_home_robot() ######################################################################################################### ############################# SUBSCRIBERS AND PUBLISHERS INITIALIZATION ################################# ######################################################################################################### def initialize_subscribers(self): + self.get_logger().info("Init subs") + if self.is_sim == True: + #if we are in simulation, use the joint_states and target_pose topics + joint_states_topic = '/xarm/joint_states' + else: + joint_states_topic = '/joint_states' + + self.get_logger().info(f"joint states topic name {joint_states_topic}") # subscribers + # js_qos = QoSProfile(depth=10, durability=DurabilityPolicy.VOLATILE) + js_cb_g = ReentrantCallbackGroup() self.joint_state_sub = self.create_subscription( - JointState, self.joint_states_topic, self.joint_states_callback, 10) + JointState, joint_states_topic, self.joint_states_callback, 10) + # , callback_group=js_cb_g) self.target_pose_sub = self.create_subscription( - PoseStamped, '/target_pose', self.target_pose_callback, 10) + PoseStamped, '/target_pose', self.target_pose_callback, 1) self.teleop_control_sub = self.create_subscription( - TwistStamped, 'robot_action', self.teleop_control_callback, 10) + TwistStamped, 'robot_action', self.teleop_control_callback, 1) self.gripper_action_sub = self.create_subscription( - Float32, '/gripper_action', self.gripper_position_callback) + Float32, '/gripper_action', self.gripper_position_callback, 1) def initialize_publishers(self): # publishers @@ -170,7 +196,6 @@ def initialize_publishers(self): self.current_end_effector_pose_pub = self.create_publisher(PoseStamped, '/current_end_effector_pose', 10) self.current_target_pose_pub = self.create_publisher(PoseStamped, '/current_target_pose', 10) - #TODO: need to check that the default topic is correct for the xarm in ros2!!!! (7/21/25) joint_command_topic = self.declare_parameter('~joint_command_topic', '/xarm/xarm7_velo_traj_controller/command').get_parameter_value().string_value self.joint_vel_pub = self.create_publisher(JointTrajectory, joint_command_topic, 10) @@ -194,7 +219,9 @@ def joint_states_callback(self, msg): and the corresponding joint velocities and efforts. This function will extract the joint positions, velocities, and efforts and return them as lists. """ + # self.get_logger().info(f"HELLLLLOOOO joint states callback {msg}") self.joint_states = msg + self.velocity_home_robot() def teleop_control_callback(self, msg): """ @@ -214,7 +241,7 @@ def teleop_control_callback(self, msg): #ensure we can get into the while loop if self.use_teleop_control == True: - self.target_pose = PoseStamped() #set this to get in a loop + self.target_pose = PoseStamped() #set this tfrom scipy.spatial.transform import Rotation as R o get in a loop def target_pose_callback(self, msg): """ @@ -248,7 +275,8 @@ def EndEffectorPose(self, q): current_pose.pose.orientation.w = trans.transform.rotation.w except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): self.get_logger().error("TF lookup failed") - self.get_logger().error("failed to lookup transform from %s to %s", self.base_link, self.end_link) + self.get_logger().error(f"base link {self.base_link}") + self.get_logger().error(f"end link {self.end_link}") self.current_end_effector_pose_pub.publish(current_pose) return current_pose @@ -300,7 +328,7 @@ def rotation_matrix_to_axis_angle(self,R): ]) / (2 * np.sin(angle)) return axis * angle - + def pose_error(self, current_pose, target_pose, error_norm_max = 0.05, control_pose_error = True): """ This function computes the error between the current pose and the target pose. @@ -315,8 +343,18 @@ def pose_error(self, current_pose, target_pose, error_norm_max = 0.05, control_p position_error = position_error / np.linalg.norm(position_error) * error_norm_max #convert the quaternion in posestamped to list of quaternions then pass this into quaternion_matrix to get the rotation matrix. Access only the rotation part of the matrix - goal_rotation_matrix = quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])[:3,:3] - current_rotation_matrix = quaternion_matrix([current_pose.pose.orientation.x, current_pose.pose.orientation.y, current_pose.pose.orientation.z, current_pose.pose.orientation.w])[:3,:3] + # goal_rotation_matrix = quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])[:3,:3] + # current_rotation_matrix = quaternion_matrix([current_pose.pose.orientation.x, current_pose.pose.orientation.y, current_pose.pose.orientation.z, current_pose.pose.orientation.w])[:3,:3] + + # for ros 2 + goal_quat_scripy = np.array([target_pose.pose.orientation.w, target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z]) + current_rotation_quat_scipy = np.array([current_pose.pose.orientation.w, current_pose.pose.orientation.x, current_pose.pose.orientation.y, current_pose.pose.orientation.z]) + goal_rotation = R.from_quat(goal_quat_scripy) + current_rotation = R.from_quat(current_rotation_quat_scipy) + + goal_rotation_matrix = goal_rotation.as_matrix() + current_rotation_matrix = current_rotation.as_matrix() + #Compute the orientation error R_error= np.dot(goal_rotation_matrix, np.linalg.inv(current_rotation_matrix)) # Extract the axis-angle lie algebra vector from the rotation matrix @@ -379,12 +417,14 @@ def velocity_home_robot(self): # do the following in a loop for 5 seconds: t_start = self.get_clock().now() duration = 0.0 - while not rclpy.is_shutdown(): + while rclpy.ok(): if duration >= 5.0: break - self.get_logger().info("Homing the robot") - if self.joint_states: - duration = (self.get_clock().now() - t_start).seconds + # self.get_logger().info("Homing the robot") + # self.get_logger().info(f"DEBUGGING: {self.joint_states}") + if self.joint_states is not None: + # self.get_logger().info("IN THE IF STATMENT") + # duration = (self.get_clock().now() - t_start).seconds q = [] dq = [] for joint_name in self.joint_names: @@ -400,7 +440,7 @@ def velocity_home_robot(self): q_home = [-0.03142359480261803, -0.5166178941726685, 0.12042707949876785, 0.9197863936424255, -0.03142168000340462, 1.4172008037567139, 0.03490765020251274] #now find the error between the current position and the home position and use joint velocities to move towards the home position joint_velocities_list = kp_gain * (np.array(q_home) - np.array(q)) - + self.get_logger().info(f"joints for homing {joint_velocities_list}") self.command_joint_velocities(joint_velocities_list) @@ -485,8 +525,9 @@ def command_joint_velocities(self, joint_vel_list): # this is on the real robot, directly send joint velociteies # Send joint velocities to the arm # log the velocities - self.get_logger().info("Joint velocities: %s", joint_vel_list) + if self.use_teleop_control_jparse: + self.get_logger().info(f"Joint velocities: {joint_vel_list}") self.arm.vc_set_joint_velocity(joint_vel_list, is_radian=True) ############################################################################################################### @@ -499,7 +540,7 @@ def control_loop(self): TODO: Need to compute the jacobian and then pass that to JParse and dont need all the comparisons!! (8/8/25) """ - while not rclpy.is_shutdown(): + while rclpy.ok(): if self.joint_states is not None and self.teleop_control_command is not None: # edited this 7/11/25 t = self.get_clock().now() @@ -514,7 +555,7 @@ def control_loop(self): effort.append(self.joint_states.effort[idx]) self.current_positions = q # Calculate the JParsed Jacobian - + # from IPython import embed; embed(banner1="hellloooooo") # NEW # 8/8/2025 # 1) Forward kinematics pin.forwardKinematics(self.model, self.data, q) @@ -524,7 +565,7 @@ def control_loop(self): pin.computeJointJacobians(self.model, self.data, q) method = self.method #set by parameter, can be set from launch file - self.get_logger().info("Method being used: %s", method) + self.get_logger().info(f"Method being used: {method}") if method == "JacobianPseudoInverse": #this is the traditional pseudo-inverse method for the jacobian # J_method, J_nullspace = self.jacobian_calculator.JacobianPseudoInverse(q=q, position_only=self.position_only, jac_nullspace_bool=True) @@ -626,9 +667,18 @@ def control_loop(self): self.rate.sleep() self.get_logger().info("Control loop running") +# if __name__ == '__main__': +# try: +# ArmController() +# except Exception as e: +# print(f"An error occurred: {e}") +# pass + +def main(args=None): + rclpy.init(args=args) + node = ArmController() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() if __name__ == '__main__': - try: - ArmController() - except Exception as e: - print(f"An error occurred: {e}") - pass \ No newline at end of file + main() \ No newline at end of file From 1d35521c3f60078cd5c4ff165b69dfeddb6b76a9 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Mon, 11 Aug 2025 19:57:50 -0500 Subject: [PATCH 07/27] Need to edit where homing and control loop should be executed --- .../scripts/xarm_vel_main_experiment.py | 26 +++++++++++++++---- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/manipulator_control/scripts/xarm_vel_main_experiment.py b/manipulator_control/scripts/xarm_vel_main_experiment.py index 64519fe..76f0931 100644 --- a/manipulator_control/scripts/xarm_vel_main_experiment.py +++ b/manipulator_control/scripts/xarm_vel_main_experiment.py @@ -31,6 +31,8 @@ import jparse_cls # Huge TODO: might need to implement the control loop as a timer callback instead of a while loop to avoid blocking the node (7/21/25) +# Update as of 8/1125 -- need to make sure that velocity_home can be called somewhere not in the callback and then also need to edit the control loop with the new +# arguments to Jparse from jparse_cls.py; then need to figure out also where to call it to run class ArmController(Node): def __init__(self): super().__init__('arm_controller') @@ -148,12 +150,26 @@ def __init__(self): #now run the control loop # self.control_loop() - # self.timer = self.create_timer(10, self.timer_callback) + # self.timer = self.create_timer(10, self.timer_callback) # def timer_callback(self): - # if self.joint_states is not None: - # self.velocity_home_robot() + # # if self.joint_states is not None: + # # self.velocity_home_robot() + # #home the robot + # try: + # self.velocity_home_robot() + # except Exception as e: + # self.get_logger().error(f"Failed to home the robot: {e}") + # pass + # finally: + # # Clean up resources if needed + # self.get_logger().info("Shutting down the control loop") + # joint_zero_velocities = [0.0] * len(self.joint_names) + # # self.timer = self.create_timer(10, self.command_joint_velocities(joint_zero_velocities)) + # self.command_joint_velocities(joint_zero_velocities) + # # now run the control loop + # # self.control_loop() ######################################################################################################### ############################# SUBSCRIBERS AND PUBLISHERS INITIALIZATION ################################# @@ -415,7 +431,7 @@ def velocity_home_robot(self): This function commands the robot to the home position using joint velocities. """ # do the following in a loop for 5 seconds: - t_start = self.get_clock().now() + t_start = self.get_clock().now().nanoseconds duration = 0.0 while rclpy.ok(): if duration >= 5.0: @@ -424,7 +440,7 @@ def velocity_home_robot(self): # self.get_logger().info(f"DEBUGGING: {self.joint_states}") if self.joint_states is not None: # self.get_logger().info("IN THE IF STATMENT") - # duration = (self.get_clock().now() - t_start).seconds + duration = (self.get_clock().now().nanoseconds - t_start) / 1e9 q = [] dq = [] for joint_name in self.joint_names: From e696ec21e7e5a226db116fd43d3acb6afaad8774 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Wed, 13 Aug 2025 16:14:31 -0500 Subject: [PATCH 08/27] IT IS WORKING IN ROS 2git statusgit status but need to fix bug of not stopping when not issuing a command --- manipulator_control/scripts/jparse_cls.py | 14 - .../scripts/xarm_vel_main_experiment.py | 376 +++++++++--------- 2 files changed, 190 insertions(+), 200 deletions(-) diff --git a/manipulator_control/scripts/jparse_cls.py b/manipulator_control/scripts/jparse_cls.py index e722297..d103f6d 100644 --- a/manipulator_control/scripts/jparse_cls.py +++ b/manipulator_control/scripts/jparse_cls.py @@ -26,20 +26,6 @@ def __init__(self, base_link="link_base", end_link="link_eef"): # defaults are t self.base_link = base_link self.end_link = end_link - # from IPython import embed; embed() - # Load URDF from parameter server - # urdf = Robot.from_parameter_server() - # model = pin.buildModelFromUrdf(urdf.to_xml_string()) - # self.data = model.createData() - # self.model = model - # self.num_joints = 7 - - # publishers - # self.marker_pub = self.create_publisher(MarkerArray, '/jparse_ellipsoid_marker', 10) - - # subscribers - # self.joint_state_sub = self.create_subscription( - # JointState, '/joint_states', self.joint_state_callback, 10) self.J_prev = None self.J_prev_time = None diff --git a/manipulator_control/scripts/xarm_vel_main_experiment.py b/manipulator_control/scripts/xarm_vel_main_experiment.py index 76f0931..c931cb6 100644 --- a/manipulator_control/scripts/xarm_vel_main_experiment.py +++ b/manipulator_control/scripts/xarm_vel_main_experiment.py @@ -81,7 +81,7 @@ def __init__(self): self.show_jparse_ellipses = self.declare_parameter('~show_jparse_ellipses', False).get_parameter_value().bool_value #boolean to control if the JParse ellipses are shown or not self.joint_states = None - self.target_pose = None + # self.target_pose = None self.teleop_control_command = None self.jacobian_calculator = jparse_cls.JParseClass(base_link=self.base_link, end_link=self.end_link) @@ -114,7 +114,7 @@ def __init__(self): ## NEW for pinocchio (8/8/2025) ## # hard-coded for now - urdf_filename = "/workspace/xarm7.urdf" + # urdf_filename = "/workspace/xarm7.urdf" # urdf_filename = "xarm7.urdf.xacro" ## note to self: need to change this from xacro to just urdf usign the command "xacro xarm7.urdf.xacro > xarm7.urdf" -- however, the robot needs to be launched but this is not working currently when I tested it # urdf_path = model_path + "/" + urdf_filename @@ -128,13 +128,14 @@ def __init__(self): # pin.buildModelsFromUrdf() # Create model and data # from IPython import embed; embed(banner1="looking at pin stuff") - self.model = pin.buildModelFromUrdf(urdf_filename) #pin.Model() - self.data = self.model.createData() + # self.model, _, _ = pin.buildModelsFromUrdf(urdf_filename) #pin.Model() + # self.model = pin.buildModelFromUrdf(urdf_filename) #pin.Model() + # self.data = self.model.createData() # another way: # from robot data - # self.rate = self.create_rate(10) # 10 Hz + self.rate = self.create_rate(10) # 10 Hz # #home the robot # try: # self.velocity_home_robot() @@ -149,27 +150,32 @@ def __init__(self): # self.command_joint_velocities(joint_zero_velocities) #now run the control loop # self.control_loop() + self.counter = 0 + self.timer = self.create_timer(0.01, self.timer_callback) - # self.timer = self.create_timer(10, self.timer_callback) - - # def timer_callback(self): + def timer_callback(self): # # if self.joint_states is not None: # # self.velocity_home_robot() # #home the robot - # try: - # self.velocity_home_robot() - # except Exception as e: - # self.get_logger().error(f"Failed to home the robot: {e}") - # pass - # finally: - # # Clean up resources if needed - # self.get_logger().info("Shutting down the control loop") - # joint_zero_velocities = [0.0] * len(self.joint_names) - # # self.timer = self.create_timer(10, self.command_joint_velocities(joint_zero_velocities)) - # self.command_joint_velocities(joint_zero_velocities) - # # now run the control loop - # # self.control_loop() + # try: + # self.counter += 1 + # self.get_logger().info(f'Timer triggered! Count: {self.counter}') + # self.velocity_home_robot() + # joint_zero_velocities = [0.0] * len(self.joint_names) + # self.timer = self.create_timer(10, self.command_joint_velocities(joint_zero_velocities)) + # self.command_joint_velocities(joint_zero_velocities) + self.control_loop() + # except Exception as e: + # self.get_logger().error(f"Failed to home the robot: {e}") + # pass + # finally: + # # Clean up resources if needed + # self.get_logger().info("Shutting down the control loop") + # joint_zero_velocities = [0.0] * len(self.joint_names) + # self.command_joint_velocities(joint_zero_velocities) + # now run the control loop + # self.control_loop() ######################################################################################################### ############################# SUBSCRIBERS AND PUBLISHERS INITIALIZATION ################################# @@ -187,14 +193,13 @@ def initialize_subscribers(self): # js_qos = QoSProfile(depth=10, durability=DurabilityPolicy.VOLATILE) js_cb_g = ReentrantCallbackGroup() self.joint_state_sub = self.create_subscription( - JointState, joint_states_topic, self.joint_states_callback, 10) - # , callback_group=js_cb_g) - self.target_pose_sub = self.create_subscription( - PoseStamped, '/target_pose', self.target_pose_callback, 1) + JointState, joint_states_topic, self.joint_states_callback, 10, callback_group=js_cb_g) + # self.target_pose_sub = self.create_subscription( + # PoseStamped, '/target_pose', self.target_pose_callback, 1, callback_group=js_cb_g) self.teleop_control_sub = self.create_subscription( - TwistStamped, 'robot_action', self.teleop_control_callback, 1) + TwistStamped, 'robot_action', self.teleop_control_callback, 1, callback_group=js_cb_g) self.gripper_action_sub = self.create_subscription( - Float32, '/gripper_action', self.gripper_position_callback, 1) + Float32, '/gripper_action', self.gripper_position_callback, 1, callback_group=js_cb_g) def initialize_publishers(self): # publishers @@ -237,7 +242,7 @@ def joint_states_callback(self, msg): """ # self.get_logger().info(f"HELLLLLOOOO joint states callback {msg}") self.joint_states = msg - self.velocity_home_robot() + # self.velocity_home_robot() def teleop_control_callback(self, msg): """ @@ -255,17 +260,18 @@ def teleop_control_callback(self, msg): position_velocity = position_velocity / position_velocity_norm * 0.05 self.teleop_control_command = np.array([position_velocity[0], position_velocity[1],position_velocity[2],angular_velocity[0],angular_velocity[1],angular_velocity[2]]) #check if norm of the space mouse command is greater than 0.05, if so normalize it to this value - #ensure we can get into the while loop - if self.use_teleop_control == True: - self.target_pose = PoseStamped() #set this tfrom scipy.spatial.transform import Rotation as R o get in a loop + # #ensure we can get into the while loop + # if self.use_teleop_control == True: + # self.get_logger().info("in the teleop control callback") + # self.target_pose = PoseStamped() #set this to get in a loop - def target_pose_callback(self, msg): - """ - Callback function for the target_pose topic. This function will be called whenever a new message is received - on the target_pose topic. The message is a geometry_msgs/PoseStamped message, which contains the target pose - """ - self.target_pose = msg - self.current_target_pose_pub.publish(self.target_pose) #if target pose is paused manually, this allows us to track the current target pose seen by the script + # def target_pose_callback(self, msg): + # """ + # Callback function for the target_pose topic. This function will be called whenever a new message is received + # on the target_pose topic. The message is a geometry_msgs/PoseStamped message, which contains the target pose + # """ + # self.target_pose = msg + # self.current_target_pose_pub.publish(self.target_pose) #if target pose is paused manually, this allows us to track the current target pose seen by the script ########################################################################################################## ####################################### HELPER FUNCTIONS ################################################# @@ -302,7 +308,7 @@ def EndEffectorPose(self, q): # This function computes the end-effector velocity given the joint configuration q and joint velocities dq. # """ # J = self.jacobian_calculator.jacobian(q) - # J = np.array(J) + # J = np.array(J)duration # dq = np.array(dq) # dx = np.dot(J, dq) # return dx @@ -348,7 +354,9 @@ def rotation_matrix_to_axis_angle(self,R): def pose_error(self, current_pose, target_pose, error_norm_max = 0.05, control_pose_error = True): """ This function computes the error between the current pose and the target pose. + NOT NEEDED """ + self.get_logger().info(f'TARGET POSE: {target_pose}') #Compute the position error position_error = np.array([target_pose.pose.position.x - current_pose.pose.position.x, target_pose.pose.position.y - current_pose.pose.position.y, @@ -432,32 +440,35 @@ def velocity_home_robot(self): """ # do the following in a loop for 5 seconds: t_start = self.get_clock().now().nanoseconds - duration = 0.0 - while rclpy.ok(): - if duration >= 5.0: - break - # self.get_logger().info("Homing the robot") - # self.get_logger().info(f"DEBUGGING: {self.joint_states}") - if self.joint_states is not None: - # self.get_logger().info("IN THE IF STATMENT") - duration = (self.get_clock().now().nanoseconds - t_start) / 1e9 - q = [] - dq = [] - for joint_name in self.joint_names: - idx = self.joint_states.name.index(joint_name) - q.append(self.joint_states.position[idx]) - dq.append(self.joint_states.velocity[idx]) - self.current_positions = q - # Command the joint velocities - if self.is_sim == True: - kp_gain = 10.0 - else: - kp_gain = 0.5 - q_home = [-0.03142359480261803, -0.5166178941726685, 0.12042707949876785, 0.9197863936424255, -0.03142168000340462, 1.4172008037567139, 0.03490765020251274] - #now find the error between the current position and the home position and use joint velocities to move towards the home position - joint_velocities_list = kp_gain * (np.array(q_home) - np.array(q)) - self.get_logger().info(f"joints for homing {joint_velocities_list}") - self.command_joint_velocities(joint_velocities_list) + # duration = 0.0 + # while rclpy.ok(): + # while self.counter <= 10: + # if self.counter == 10: + # joint_zero_velocities = [0.0] * len(self.joint_names) + # self.command_joint_velocities(joint_zero_velocities) + # break + # # self.get_logger().info("Homing the robot") + # # self.get_logger().info(f"DEBUGGING: {self.joint_states}") + if self.joint_states is not None: + # self.get_logger().info("IN THE IF STATMENT") + # duration = (self.get_clock().now().nanoseconds - t_start) / 1e9 + q = [] + dq = [] + for joint_name in self.joint_names: + idx = self.joint_states.name.index(joint_name) + q.append(self.joint_states.position[idx]) + dq.append(self.joint_states.velocity[idx]) + self.current_positions = q + # Command the joint velocities + if self.is_sim == True: + kp_gain = 10.0 + else: + kp_gain = 0.5 + q_home = [-0.03142359480261803, -0.5166178941726685, 0.12042707949876785, 0.9197863936424255, -0.03142168000340462, 1.4172008037567139, 0.03490765020251274] + #now find the error between the current position and the home position and use joint velocities to move towards the home position + joint_velocities_list = kp_gain * (np.array(q_home) - np.array(q)) + self.get_logger().info(f"joints for homing {joint_velocities_list}") + self.command_joint_velocities(joint_velocities_list) ############################################################################################################## @@ -556,131 +567,124 @@ def control_loop(self): TODO: Need to compute the jacobian and then pass that to JParse and dont need all the comparisons!! (8/8/25) """ - while rclpy.ok(): - - if self.joint_states is not None and self.teleop_control_command is not None: # edited this 7/11/25 - t = self.get_clock().now() - # obtain the current joints - q = [] - dq = [] - effort = [] - for joint_name in self.joint_names: - idx = self.joint_states.name.index(joint_name) - q.append(self.joint_states.position[idx]) - dq.append(self.joint_states.velocity[idx]) - effort.append(self.joint_states.effort[idx]) - self.current_positions = q - # Calculate the JParsed Jacobian - # from IPython import embed; embed(banner1="hellloooooo") - # NEW # 8/8/2025 - # 1) Forward kinematics - pin.forwardKinematics(self.model, self.data, q) - pin.updateFramePlacements(self.model, self.data) - - # 2) Compute joint Jacobians - pin.computeJointJacobians(self.model, self.data, q) - - method = self.method #set by parameter, can be set from launch file - self.get_logger().info(f"Method being used: {method}") - if method == "JacobianPseudoInverse": - #this is the traditional pseudo-inverse method for the jacobian - # J_method, J_nullspace = self.jacobian_calculator.JacobianPseudoInverse(q=q, position_only=self.position_only, jac_nullspace_bool=True) - raise NotImplementedError - elif method == "JParse": - # The JParse method takes in the joint angles, gamma, position_only, and singular_direction_gain - if self.show_jparse_ellipses == True: - J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position, singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True, publish_jparse_ellipses=True, end_effector_pose=self.EndEffectorPose(q), verbose=False) - else: - J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position,singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True) - elif method == "JacobianDampedLeastSquares": - # J_method, J_nullspace = self.jacobian_calculator.jacobian_damped_least_squares(q=q, damping=0.1, jac_nullspace_bool=True) #dampening of 0.1 works very well, 0.8 shows clear error - raise NotImplementedError - elif method == "JacobianProjection": - # J_proj, J_nullspace = self.jacobian_calculator.jacobian_projection(q=q, gamma=0.1, jac_nullspace_bool=True) - # J_method = np.linalg.pinv(J_proj) - raise NotImplementedError - elif method == "JacobianSafety": - # The JParse method takes in the joint angles, gamma, position_only, and singular_direction_gain - if self.show_jparse_ellipses == True: - J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position, singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True, publish_jparse_ellipses=True, end_effector_pose=self.EndEffectorPose(q), verbose=False, safety_only=True) - else: - J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position,singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True, safety_only=True) - elif method == "JacobianSafetyProjection": - # The JParse method takes in the joint angles, gamma, position_only, and singular_direction_gain - if self.show_jparse_ellipses == True: - J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position, singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True, publish_jparse_ellipses=True, end_effector_pose=self.EndEffectorPose(q), verbose=False, safety_projection_only=True) - else: - J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position,singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True, safety_projection_only=True) - - #Calculate the delta_x (task space error) - target_pose = self.target_pose - current_pose = self.EndEffectorPose(q) - position_error, angular_error = self.pose_error(current_pose, target_pose, control_pose_error=False) - # log the pose error - self.publish_pose_errors(position_error, angular_error, control_pose_error=False) - #now find error for control (max error norm) - if self.is_sim == False: - #real world limits - error_norm_max = 0.10 - else: - #simulation limits - error_norm_max = 1.0 - position_error, angular_error = self.pose_error(current_pose, target_pose, error_norm_max = error_norm_max, control_pose_error=True) - #move in nullspace towards nominal pose (for xarm its 0 joint angle for Xarm7) + # while rclpy.ok(): + + if self.joint_states is not None and self.teleop_control_command is not None: # edited this 7/11/25 + t = self.get_clock().now() + # obtain the current joints + q = [] + dq = [] + effort = [] + for joint_name in self.joint_names: + idx = self.joint_states.name.index(joint_name) + q.append(self.joint_states.position[idx]) + dq.append(self.joint_states.velocity[idx]) + effort.append(self.joint_states.effort[idx]) + self.current_positions = q + # Calculate the JParsed Jacobian + # from IPython import embed; embed(banner1="hellloooooo") + # NEW # 8/8/2025 + urdf_filename = "/workspace/xarm7.urdf" + model = pin.buildModelFromUrdf(urdf_filename) # the model structure of the robot + data = model.createData() # the data structure of the robot + # 1) Forward kinematics + # from IPython import embed; embed(banner1="computing jacobians") + np_q = np.array(q) + if isinstance(np_q, np.ndarray): + print("The object is a NumPy array.") + self.get_logger().info(f'q: {np_q} {np_q}') + else: + print("The object is not a NumPy array.") + + pin.forwardKinematics(model, data, np_q) # needs to take in a numpy array + pin.updateFramePlacements(model, data) + + # 2) Compute joint Jacobians + pin.computeJointJacobians(model, data, np_q) + + # 3) Extract Jacobian + J6 = data.J #J3 = data.J[:3, :] -- will give us only position so removing it + print(f'JACOBIAN: {J6}') + + method = self.method #set by parameter, can be set from launch file + self.get_logger().info(f"Method being used: {method}") + if method == "JacobianPseudoInverse": + raise NotImplementedError + elif method == "JParse": + J_method, J_nullspace = self.jacobian_calculator.JParse(J=J6, jac_nullspace_bool=True, gamma=0.1, singular_direction_gain_position=1, singular_direction_gain_orientation=2, position_dimensions=3, angular_dimensions=3) + elif method == "JacobianDampedLeastSquares": + raise NotImplementedError + elif method == "JacobianProjection": + raise NotImplementedError + elif method == "JacobianSafety": + raise NotImplementedError + elif method == "JacobianSafetyProjection": + raise NotImplementedError + + #Calculate the delta_x (task space error) + current_pose = self.EndEffectorPose(q) + if self.is_sim == False: + #real world limits + error_norm_max = 0.10 + else: + #simulation limits + error_norm_max = 1.0 + #move in nullspace towards nominal pose (for xarm its 0 joint angle for Xarm7) + if self.is_sim == True: + kp_gain = 2.0 + else: + # kp_gain = 1.0 + kp_gain = 3.0 + + if self.is_sim == True: + nominal_motion_nullspace = np.matrix([-v*kp_gain for v in q]).T #send to home which is 0 joint position for all joints + else: + # nominal motion nullspace which checks if q magnitude is below threshold and chooses the minimum + null_space_angle_rate = 0.6 + + nominal_motion_nullspace = np.matrix([np.sign(-v*kp_gain)*np.min([np.abs(-v*kp_gain),null_space_angle_rate]) for v in q]).T #send to home which is 0 joint position for all joints + + # Calculate and command the joint velocities + if self.position_only == True: + self.get_logger().info("Position only control") + # position_error = np.matrix(position_error).T + raise NotImplementedError if self.is_sim == True: - kp_gain = 2.0 + #use these gains only in simulation! + kp_gain = 10.0 else: - # kp_gain = 1.0 kp_gain = 3.0 - + Kp = np.diag([kp_gain, kp_gain, kp_gain]) # Proportional gain matrix + + task_vector = Kp @ position_error + joint_velocities = J_method @ task_vector + J_nullspace @ nominal_motion_nullspace + else: + # realworld gains (tested) + kp_gain_position = 1.0 + kp_gain_orientation = 1.0 if self.is_sim == True: - nominal_motion_nullspace = np.matrix([-v*kp_gain for v in q]).T #send to home which is 0 joint position for all joints - else: - # nominal motion nullspace which checks if q magnitude is below threshold and chooses the minimum - null_space_angle_rate = 0.6 - - nominal_motion_nullspace = np.matrix([np.sign(-v*kp_gain)*np.min([np.abs(-v*kp_gain),null_space_angle_rate]) for v in q]).T #send to home which is 0 joint position for all joints - # log the pose error - self.publish_pose_errors(position_error, angular_error, control_pose_error=True) - - # Calculate and command the joint velocities - if self.position_only == True: - self.get_logger().info("Position only control") - position_error = np.matrix(position_error).T - if self.is_sim == True: - #use these gains only in simulation! - kp_gain = 10.0 - else: - kp_gain = 3.0 - Kp = np.diag([kp_gain, kp_gain, kp_gain]) # Proportional gain matrix - - task_vector = Kp @ position_error - joint_velocities = J_method @ task_vector + J_nullspace @ nominal_motion_nullspace - else: - # realworld gains (tested) - kp_gain_position = 1.0 - kp_gain_orientation = 1.0 - if self.is_sim == True: - #use these gains only in simulation! - kp_gain_position = 10.0 - kp_gain_orientation = 10.0 - Kp_position = np.diag([kp_gain_position, kp_gain_position, kp_gain_position]) # Proportional gain matrix - Kp_orientation = np.diag([kp_gain_orientation, kp_gain_orientation, kp_gain_orientation]) - task_position = Kp_position @ np.matrix(position_error).T - task_orientation = Kp_orientation @ np.matrix(angular_error).T - full_pose_error = np.matrix(np.concatenate((task_position.T, task_orientation.T),axis=1)).T - if self.use_teleop_control == False: - joint_velocities = J_method @ full_pose_error + J_nullspace @ nominal_motion_nullspace - if self.use_teleop_control == True: - #use the teleop control command to control the joint velocities - teleop_control_command = np.matrix(self.teleop_control_command).T - #now add this to the joint velocities - joint_velocities = J_method @ teleop_control_command + J_nullspace @ nominal_motion_nullspace - #check this - joint_velocities_list = np.array(joint_velocities).flatten().tolist() - # command the joint velocities - self.command_joint_velocities(joint_velocities_list) #this commands the joint velocities - self.rate.sleep() + #use these gains only in simulation! + kp_gain_position = 10.0 + kp_gain_orientation = 10.0 + Kp_position = np.diag([kp_gain_position, kp_gain_position, kp_gain_position]) # Proportional gain matrix + Kp_orientation = np.diag([kp_gain_orientation, kp_gain_orientation, kp_gain_orientation]) + if self.use_teleop_control == False: + # joint_velocities = J_method @ full_pose_error + J_nullspace @ nominal_motion_nullspace + raise NotImplementedError + if self.use_teleop_control == True: + #use the teleop control command to control the joint velocities + teleop_control_command = np.matrix(self.teleop_control_command).T + #now add this to the joint velocities + # print(f"J_method: {J_method} {np.array(J_method).shape}") + # print(f'teleop command: {teleop_control_command} {np.array(teleop_control_command).shape}') + # print(f'J_nullspace: {J_nullspace} {np.array(J_nullspace).shape}') + # print(f'nominal_motion_nullspace: {nominal_motion_nullspace} {np.array(nominal_motion_nullspace).shape}') + joint_velocities = J_method @ teleop_control_command + J_nullspace @ nominal_motion_nullspace + #check this + joint_velocities_list = np.array(joint_velocities).flatten().tolist() + # command the joint velocities + self.command_joint_velocities(joint_velocities_list) #this commands the joint velocities + # self.rate.sleep() self.get_logger().info("Control loop running") # if __name__ == '__main__': From baca398d962fd65b57ae53abaf7a4e9367810b41 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Wed, 13 Aug 2025 17:08:01 -0500 Subject: [PATCH 09/27] Edited the keyboard twist to include key timeout so robot does not move when no button is pressed --- .gitmodules | 3 +++ ros2_teleop_twist_keyboard | 1 + 2 files changed, 4 insertions(+) create mode 160000 ros2_teleop_twist_keyboard diff --git a/.gitmodules b/.gitmodules index a0dedfa..a150481 100644 --- a/.gitmodules +++ b/.gitmodules @@ -2,3 +2,6 @@ # path = xarm_base # url = https://github.com/argallab/xarm_base.git # branch = dev/jparse_ros2 +[submodule "ros2_teleop_twist_keyboard"] + path = ros2_teleop_twist_keyboard + url = https://github.com/argallab/ros2_teleop_twist_keyboard.git diff --git a/ros2_teleop_twist_keyboard b/ros2_teleop_twist_keyboard new file mode 160000 index 0000000..2bc281a --- /dev/null +++ b/ros2_teleop_twist_keyboard @@ -0,0 +1 @@ +Subproject commit 2bc281a6904bc92f572977c7048312c24bcd85bf From 8356ca64d5f9606f369fe6981e6d0e9c4d4f1de4 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Tue, 19 Aug 2025 15:43:15 -0500 Subject: [PATCH 10/27] Ros2 is tested and working --- .gitmodules | 3 + .../scripts/xarm_vel_main_experiment.py | 4 +- manipulator_control_ros1/CMakeLists.txt | 25 - manipulator_control_ros1/__init__.py | 0 .../config/kinova_arm_controllers_gazebo.yaml | 339 ------- .../launch/franka_launch.launch | 25 - .../launch/full_pose_trajectory.launch | 51 -- .../launch/jparse_april_tag_ros.launch | 9 - .../launch/jparse_cpp.launch | 19 - .../launch/kinova_gen3.launch | 11 - .../launch/kinova_gen3_real.launch | 11 - .../kinova_gen3_real_visual_servoing.launch | 13 - .../launch/kinova_vel_control.launch | 48 - .../launch/line_extended_singular_traj.launch | 50 - .../launch/manip_velocity_control.launch | 18 - .../launch/panda_main_torque.launch | 30 - .../launch/position_trajectory.launch | 27 - .../launch/se3_type_2_singular_traj.launch | 43 - .../launch/xarm_launch.launch | 9 - .../launch/xarm_main_vel.launch | 41 - .../launch/xarm_python_using_cpp.launch | 40 - .../launch/xarm_real_launch.launch | 19 - .../launch/xarm_real_spacemouse_base.launch | 86 -- .../launch/xarm_spacemouse_teleop.launch | 9 - manipulator_control_ros1/msg/JparseTerms.msg | 3 - manipulator_control_ros1/msg/Matrow.msg | 1 - manipulator_control_ros1/package.xml | 58 -- .../rviz/jparse_final.rviz | 428 --------- .../rviz/kinova_gen3.rviz | 328 ------- manipulator_control_ros1/rviz/panda_rviz.rviz | 366 -------- manipulator_control_ros1/rviz/xarm_rviz.rviz | 267 ------ manipulator_control_ros1/scripts/device.py | 21 - .../scripts/extract_lfd_images_from_bag.py | 105 --- .../scripts/input2action.py | 85 -- .../scripts/main_experiment_kinova.py | 852 ------------------ .../scripts/panda_torque_main_experiment.py | 534 ----------- .../scripts/position_trajectory_generator.py | 125 --- .../scripts/se3_trajectory_generator.py | 281 ------ .../scripts/spacemouse.py | 432 --------- .../scripts/xarm_spacemouse.py | 172 ---- .../scripts/xarm_vel_main_experiment.py | 571 ------------ .../scripts/xarm_vel_python_using_cpp.py | 603 ------------- manipulator_control_ros1/setup.py | 9 - manipulator_control_ros1/src/jparse.cpp | 718 --------------- .../src/manipulator_control/jparse_cls.py | 549 ----------- manipulator_control_ros1/srv/JParseSrv.srv | 8 - xarm_ros2_base | 1 + 47 files changed, 6 insertions(+), 7441 deletions(-) delete mode 100644 manipulator_control_ros1/CMakeLists.txt delete mode 100644 manipulator_control_ros1/__init__.py delete mode 100644 manipulator_control_ros1/config/kinova_arm_controllers_gazebo.yaml delete mode 100644 manipulator_control_ros1/launch/franka_launch.launch delete mode 100644 manipulator_control_ros1/launch/full_pose_trajectory.launch delete mode 100644 manipulator_control_ros1/launch/jparse_april_tag_ros.launch delete mode 100644 manipulator_control_ros1/launch/jparse_cpp.launch delete mode 100644 manipulator_control_ros1/launch/kinova_gen3.launch delete mode 100644 manipulator_control_ros1/launch/kinova_gen3_real.launch delete mode 100644 manipulator_control_ros1/launch/kinova_gen3_real_visual_servoing.launch delete mode 100644 manipulator_control_ros1/launch/kinova_vel_control.launch delete mode 100644 manipulator_control_ros1/launch/line_extended_singular_traj.launch delete mode 100644 manipulator_control_ros1/launch/manip_velocity_control.launch delete mode 100644 manipulator_control_ros1/launch/panda_main_torque.launch delete mode 100644 manipulator_control_ros1/launch/position_trajectory.launch delete mode 100644 manipulator_control_ros1/launch/se3_type_2_singular_traj.launch delete mode 100644 manipulator_control_ros1/launch/xarm_launch.launch delete mode 100644 manipulator_control_ros1/launch/xarm_main_vel.launch delete mode 100644 manipulator_control_ros1/launch/xarm_python_using_cpp.launch delete mode 100644 manipulator_control_ros1/launch/xarm_real_launch.launch delete mode 100644 manipulator_control_ros1/launch/xarm_real_spacemouse_base.launch delete mode 100644 manipulator_control_ros1/launch/xarm_spacemouse_teleop.launch delete mode 100644 manipulator_control_ros1/msg/JparseTerms.msg delete mode 100644 manipulator_control_ros1/msg/Matrow.msg delete mode 100644 manipulator_control_ros1/package.xml delete mode 100644 manipulator_control_ros1/rviz/jparse_final.rviz delete mode 100644 manipulator_control_ros1/rviz/kinova_gen3.rviz delete mode 100644 manipulator_control_ros1/rviz/panda_rviz.rviz delete mode 100644 manipulator_control_ros1/rviz/xarm_rviz.rviz delete mode 100644 manipulator_control_ros1/scripts/device.py delete mode 100755 manipulator_control_ros1/scripts/extract_lfd_images_from_bag.py delete mode 100644 manipulator_control_ros1/scripts/input2action.py delete mode 100755 manipulator_control_ros1/scripts/main_experiment_kinova.py delete mode 100755 manipulator_control_ros1/scripts/panda_torque_main_experiment.py delete mode 100755 manipulator_control_ros1/scripts/position_trajectory_generator.py delete mode 100755 manipulator_control_ros1/scripts/se3_trajectory_generator.py delete mode 100644 manipulator_control_ros1/scripts/spacemouse.py delete mode 100644 manipulator_control_ros1/scripts/xarm_spacemouse.py delete mode 100755 manipulator_control_ros1/scripts/xarm_vel_main_experiment.py delete mode 100755 manipulator_control_ros1/scripts/xarm_vel_python_using_cpp.py delete mode 100644 manipulator_control_ros1/setup.py delete mode 100644 manipulator_control_ros1/src/jparse.cpp delete mode 100755 manipulator_control_ros1/src/manipulator_control/jparse_cls.py delete mode 100644 manipulator_control_ros1/srv/JParseSrv.srv create mode 160000 xarm_ros2_base diff --git a/.gitmodules b/.gitmodules index a150481..dc7998d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -5,3 +5,6 @@ [submodule "ros2_teleop_twist_keyboard"] path = ros2_teleop_twist_keyboard url = https://github.com/argallab/ros2_teleop_twist_keyboard.git +[submodule "xarm_ros2_base"] + path = xarm_ros2_base + url = https://github.com/argallab/xarm_ros2_base.git diff --git a/manipulator_control/scripts/xarm_vel_main_experiment.py b/manipulator_control/scripts/xarm_vel_main_experiment.py index c931cb6..7a650db 100644 --- a/manipulator_control/scripts/xarm_vel_main_experiment.py +++ b/manipulator_control/scripts/xarm_vel_main_experiment.py @@ -552,10 +552,10 @@ def command_joint_velocities(self, joint_vel_list): # this is on the real robot, directly send joint velociteies # Send joint velocities to the arm # log the velocities - if self.use_teleop_control_jparse: - self.get_logger().info(f"Joint velocities: {joint_vel_list}") + # self.get_logger().info(f"Joint velocities: {joint_vel_list}") self.arm.vc_set_joint_velocity(joint_vel_list, is_radian=True) + self.arm.set_gripper_position(self.gripper_pose) ############################################################################################################### ######################################### CONTROL LOOP ######################################################## diff --git a/manipulator_control_ros1/CMakeLists.txt b/manipulator_control_ros1/CMakeLists.txt deleted file mode 100644 index 36b4168..0000000 --- a/manipulator_control_ros1/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(manipulator_control) - -find_package(catkin_simple REQUIRED) - -catkin_python_setup() - -catkin_simple() - - -## Add support for C++11, supported in ROS Kinetic and newer -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") - -find_package(Eigen3 REQUIRED) - -include_directories(include ${EIGEN3_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${PCL_LIBRARIES}) - - -cs_add_executable(jparse src/jparse.cpp) - - -cs_install() - -cs_export() -#https://github.com/catkin/catkin_simple \ No newline at end of file diff --git a/manipulator_control_ros1/__init__.py b/manipulator_control_ros1/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/manipulator_control_ros1/config/kinova_arm_controllers_gazebo.yaml b/manipulator_control_ros1/config/kinova_arm_controllers_gazebo.yaml deleted file mode 100644 index 6be6c8a..0000000 --- a/manipulator_control_ros1/config/kinova_arm_controllers_gazebo.yaml +++ /dev/null @@ -1,339 +0,0 @@ -gen3_arm_controller: - follow_joint_trajectory: - type: "robot_controllers/FollowJointTrajectoryController" - joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 - gravity_compensation: - type: "robot_controllers/GravityCompensation" - root: "base_link" - tip: "end_effector_link" - autostart: true - cartesian_twist: - type: "robot_controllers/CartesianTwistController" - root_name: "base_link" - tip_name: "end_effector_link" - velocity: - type: "robot_controllers/VelocityController" - joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - - joint_7 - weightless_torque: - type: "robot_controllers/WeightlessTorqueController" - root: "base_link" - tip: "end_effector_link" - autostart: false - limits: - # The joint limits are 10 degrees less than full motion. The gains ramp up to full motor torque at the limit - - name: shoulder_pan_joint - lo: -1.431 - hi: +1.431 - gain: 193.26 - - name: shoulder_lift_joint - lo: -1.047 - hi: +1.343 - gain: 754.929 - - name: elbow_flex_joint - lo: -2.076 - hi: +2.076 - gain: 379.18 - - name: wrist_flex_joint - lo: -2.007 - hi: +2.007 - gain: 147.25 - torque_control_arm: - type: "robot_controllers/TorqueControllerArm" - root: "base_link" - tip: "gripper_link" - autostart: false - limits: - # The effort change cannot be more than the gain in one timestep - - name: shoulder_pan_joint - effort: 33.82 - - name: shoulder_lift_joint - effort: 131.76 - - name: upperarm_roll_joint - effort: 76.94 - - name: elbow_flex_joint - effort: 66.18 - - name: forearm_roll_joint - effort: 29.35 - - name: wrist_flex_joint - effort: 25.7 - - name: wrist_roll_joint - effort: 7.36 - -arm_with_torso_controller: - follow_joint_trajectory: - type: "robot_controllers/FollowJointTrajectoryController" - joints: - - torso_lift_joint - - shoulder_pan_joint - - shoulder_lift_joint - - upperarm_roll_joint - - elbow_flex_joint - - forearm_roll_joint - - wrist_flex_joint - - wrist_roll_joint - -torso_controller: - follow_joint_trajectory: - type: "robot_controllers/FollowJointTrajectoryController" - joints: - - torso_lift_joint - -head_controller: - point_head: - type: "robot_controllers/PointHeadController" - follow_joint_trajectory: - type: "robot_controllers/FollowJointTrajectoryController" - joints: - - head_pan_joint - - head_tilt_joint - -base_controller: - type: "robot_controllers/DiffDriveBaseController" - max_velocity_x: 1.0 - max_acceleration_x: 0.75 - # hold position - moving_threshold: -0.01 - rotating_threshold: -0.01 - # autostart to get odom - autostart: true - # use laser to only slowly collide with things - laser_safety_dist: 1.0 - -base_torque_controller: - type: "robot_controllers/TorqueControllerBase" - autostart: false - -arm_base_controller: - type: "robot_controllers/TorqueControllerArmBase" - autostart: false - -gripper_controller: - gripper_action: - type: "robot_controllers/ParallelGripperController" - centering: - p: 1000.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - -bellows_controller: - type: "robot_controllers/ScaledMimicController" - mimic_joint: "torso_lift_joint" - controlled_joint: "bellows_joint" - mimic_scale: 0.5 - autostart: true - -robot_driver: - default_controllers: - - "arm_controller/follow_joint_trajectory" - - "arm_controller/gravity_compensation" - - "arm_controller/cartesian_twist" - - "arm_controller/velocity" - - "arm_controller/weightless_torque" - - "arm_controller/torque_control_arm" - - "arm_with_torso_controller/follow_joint_trajectory" - - "base_controller" - - "head_controller/follow_joint_trajectory" - - "head_controller/point_head" - - "torso_controller/follow_joint_trajectory" - - "base_torque_controller" - - "arm_base_controller" - - -gazebo: - default_controllers: - - "arm_controller/follow_joint_trajectory" - - "arm_controller/gravity_compensation" - - "arm_controller/cartesian_twist" - - "arm_controller/velocity" - - "arm_controller/weightless_torque" - - "arm_controller/torque_control_arm" - - "arm_with_torso_controller/follow_joint_trajectory" - - "base_controller" - - "head_controller/follow_joint_trajectory" - - "head_controller/point_head" - - "torso_controller/follow_joint_trajectory" - - "base_torque_controller" - - "arm_base_controller" - - "gripper_controller/gripper_action" - - "bellows_controller" - l_wheel_joint: - position: - p: 0.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - velocity: - p: 8.85 - d: 0.0 - i: 0.5 - i_clamp: 6.0 - r_wheel_joint: - position: - p: 0.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - velocity: - p: 8.85 - d: 0.0 - i: 0.5 - i_clamp: 6.0 - torso_lift_joint: - position: - p: 1000.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - velocity: - p: 100000.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - bellows_joint: - position: - p: 10.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - velocity: - p: 25.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - head_pan_joint: - position: - p: 2.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - velocity: - p: 2.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - head_tilt_joint: - position: - p: 10.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - velocity: - p: 3.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - shoulder_pan_joint: - position: - p: 100.0 - d: 0.1 - i: 0.0 - i_clamp: 0.0 - velocity: - p: 200.0 - d: 0.0 - i: 2.0 - i_clamp: 1.0 - shoulder_lift_joint: - position: - p: 100.0 - d: 0.1 - i: 0.0 - i_clamp: 0.0 - velocity: - p: 200.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - upperarm_roll_joint: - position: - p: 100.0 - d: 0.1 - i: 0.0 - i_clamp: 0.0 - velocity: - p: 10.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - elbow_flex_joint: - position: - p: 100.0 - d: 0.1 - i: 0.0 - i_clamp: 0.0 - velocity: - p: 150.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - forearm_roll_joint: - position: - p: 100.0 - d: 0.1 - i: 0.0 - i_clamp: 0.0 - velocity: - p: 150.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - wrist_flex_joint: - position: - p: 100.0 - d: 0.1 - i: 0.0 - i_clamp: 0.0 - velocity: - p: 100.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - wrist_roll_joint: - position: - p: 100.0 - d: 0.1 - i: 0.0 - i_clamp: 0.0 - velocity: - p: 100.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - l_gripper_finger_joint: - position: - p: 5000.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - velocity: - p: 0.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - r_gripper_finger_joint: - position: - p: 5000.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - velocity: - p: 0.0 - d: 0.0 - i: 0.0 - i_clamp: 0.0 - diff --git a/manipulator_control_ros1/launch/franka_launch.launch b/manipulator_control_ros1/launch/franka_launch.launch deleted file mode 100644 index 1566e5d..0000000 --- a/manipulator_control_ros1/launch/franka_launch.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/launch/full_pose_trajectory.launch b/manipulator_control_ros1/launch/full_pose_trajectory.launch deleted file mode 100644 index 308662f..0000000 --- a/manipulator_control_ros1/launch/full_pose_trajectory.launch +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/launch/jparse_april_tag_ros.launch b/manipulator_control_ros1/launch/jparse_april_tag_ros.launch deleted file mode 100644 index 787f3b6..0000000 --- a/manipulator_control_ros1/launch/jparse_april_tag_ros.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/launch/jparse_cpp.launch b/manipulator_control_ros1/launch/jparse_cpp.launch deleted file mode 100644 index c82d1e1..0000000 --- a/manipulator_control_ros1/launch/jparse_cpp.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/launch/kinova_gen3.launch b/manipulator_control_ros1/launch/kinova_gen3.launch deleted file mode 100644 index e559443..0000000 --- a/manipulator_control_ros1/launch/kinova_gen3.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/launch/kinova_gen3_real.launch b/manipulator_control_ros1/launch/kinova_gen3_real.launch deleted file mode 100644 index 947a011..0000000 --- a/manipulator_control_ros1/launch/kinova_gen3_real.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/launch/kinova_gen3_real_visual_servoing.launch b/manipulator_control_ros1/launch/kinova_gen3_real_visual_servoing.launch deleted file mode 100644 index edca51a..0000000 --- a/manipulator_control_ros1/launch/kinova_gen3_real_visual_servoing.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/launch/kinova_vel_control.launch b/manipulator_control_ros1/launch/kinova_vel_control.launch deleted file mode 100644 index c43fd24..0000000 --- a/manipulator_control_ros1/launch/kinova_vel_control.launch +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/launch/line_extended_singular_traj.launch b/manipulator_control_ros1/launch/line_extended_singular_traj.launch deleted file mode 100644 index 26675f0..0000000 --- a/manipulator_control_ros1/launch/line_extended_singular_traj.launch +++ /dev/null @@ -1,50 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/launch/manip_velocity_control.launch b/manipulator_control_ros1/launch/manip_velocity_control.launch deleted file mode 100644 index 05a926a..0000000 --- a/manipulator_control_ros1/launch/manip_velocity_control.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/launch/panda_main_torque.launch b/manipulator_control_ros1/launch/panda_main_torque.launch deleted file mode 100644 index ccb31d6..0000000 --- a/manipulator_control_ros1/launch/panda_main_torque.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/launch/position_trajectory.launch b/manipulator_control_ros1/launch/position_trajectory.launch deleted file mode 100644 index 989de6e..0000000 --- a/manipulator_control_ros1/launch/position_trajectory.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/launch/se3_type_2_singular_traj.launch b/manipulator_control_ros1/launch/se3_type_2_singular_traj.launch deleted file mode 100644 index da6be72..0000000 --- a/manipulator_control_ros1/launch/se3_type_2_singular_traj.launch +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/launch/xarm_launch.launch b/manipulator_control_ros1/launch/xarm_launch.launch deleted file mode 100644 index f59e699..0000000 --- a/manipulator_control_ros1/launch/xarm_launch.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/launch/xarm_main_vel.launch b/manipulator_control_ros1/launch/xarm_main_vel.launch deleted file mode 100644 index 84c876a..0000000 --- a/manipulator_control_ros1/launch/xarm_main_vel.launch +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/launch/xarm_python_using_cpp.launch b/manipulator_control_ros1/launch/xarm_python_using_cpp.launch deleted file mode 100644 index 92b333c..0000000 --- a/manipulator_control_ros1/launch/xarm_python_using_cpp.launch +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/launch/xarm_real_launch.launch b/manipulator_control_ros1/launch/xarm_real_launch.launch deleted file mode 100644 index 93d045f..0000000 --- a/manipulator_control_ros1/launch/xarm_real_launch.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/launch/xarm_real_spacemouse_base.launch b/manipulator_control_ros1/launch/xarm_real_spacemouse_base.launch deleted file mode 100644 index b548f07..0000000 --- a/manipulator_control_ros1/launch/xarm_real_spacemouse_base.launch +++ /dev/null @@ -1,86 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/launch/xarm_spacemouse_teleop.launch b/manipulator_control_ros1/launch/xarm_spacemouse_teleop.launch deleted file mode 100644 index d846b26..0000000 --- a/manipulator_control_ros1/launch/xarm_spacemouse_teleop.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control_ros1/msg/JparseTerms.msg b/manipulator_control_ros1/msg/JparseTerms.msg deleted file mode 100644 index b5487a5..0000000 --- a/manipulator_control_ros1/msg/JparseTerms.msg +++ /dev/null @@ -1,3 +0,0 @@ -Header header -manipulator_control/Matrow[] jparse -manipulator_control/Matrow[] jsafety_nullspace diff --git a/manipulator_control_ros1/msg/Matrow.msg b/manipulator_control_ros1/msg/Matrow.msg deleted file mode 100644 index f7ca10e..0000000 --- a/manipulator_control_ros1/msg/Matrow.msg +++ /dev/null @@ -1 +0,0 @@ -float64[] row diff --git a/manipulator_control_ros1/package.xml b/manipulator_control_ros1/package.xml deleted file mode 100644 index cbba38b..0000000 --- a/manipulator_control_ros1/package.xml +++ /dev/null @@ -1,58 +0,0 @@ - - - manipulator_control - 0.0.0 - The manipulator_control package - Monroe Kennedy - Monroe Kennedy - - - MIT - - - - catkin - catkin_simple - message_generation - roscpp - rospy - roscpp - rospy - - std_msgs - tf - tf2 - tf2_ros - sensor_msgs - tf_conversions - eigen_conversions - visualization_msgs - geometry_msgs - robot_controllers_msgs - kdl_parser - orocos_kdl - urdf - robot_controllers_interface - gazebo_msgs - moveit_core - kortex_driver - kortex_gazebo - kortex_vision - - - message_generation - - moveit_commander - actionlib_msgs - - - message_runtime - roscpp - rospy - - - - - - - diff --git a/manipulator_control_ros1/rviz/jparse_final.rviz b/manipulator_control_ros1/rviz/jparse_final.rviz deleted file mode 100644 index 849f15b..0000000 --- a/manipulator_control_ros1/rviz/jparse_final.rviz +++ /dev/null @@ -1,428 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 84 - Name: Displays - Property Tree Widget: - Expanded: - - /Status1 - - /MotionPlanning1 - - /MotionPlanning1/Scene Robot1 - - /MotionPlanning1/Planning Request1 - - /MotionPlanning1/Planned Path1 - - /Pose1 - - /MarkerArray1 - - /TF1 - - /TF1/Frames1 - - /TF1/Tree1 - Splitter Ratio: 0.7425600290298462 - Tree Height: 650 - - Class: rviz/Help - Name: Help - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Acceleration_Scaling_Factor: 1 - Class: moveit_rviz_plugin/MotionPlanning - Enabled: false - JointsTab_Use_Radians: true - Move Group Namespace: "" - MoveIt_Allow_Approximate_IK: false - MoveIt_Allow_External_Program: false - MoveIt_Allow_Replanning: false - MoveIt_Allow_Sensor_Positioning: false - MoveIt_Planning_Attempts: 10 - MoveIt_Planning_Time: 5 - MoveIt_Use_Cartesian_Path: false - MoveIt_Use_Constraint_Aware_IK: true - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 - Name: MotionPlanning - Planned Path: - Color Enabled: false - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - Loop Animation: false - Robot Alpha: 0.5 - Robot Color: 150; 50; 150 - Show Robot Collision: false - Show Robot Visual: true - Show Trail: false - State Display Time: 0.05 s - Trail Step Size: 1 - Trajectory Topic: move_group/display_planned_path - Use Sim Time: false - Planning Metrics: - Payload: 1 - Show Joint Torques: false - Show Manipulability: false - Show Manipulability Index: false - Show Weight Limit: false - TextHeight: 0.07999999821186066 - Planning Request: - Colliding Link Color: 255; 0; 0 - Goal State Alpha: 1 - Goal State Color: 250; 128; 0 - Interactive Marker Size: 0 - Joint Violation Color: 255; 0; 255 - Planning Group: xarm7 - Query Goal State: true - Query Start State: false - Show Workspace: false - Start State Alpha: 1 - Start State Color: 0; 255; 0 - Planning Scene Topic: move_group/monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 1 - Scene Color: 50; 230; 50 - Scene Display Time: 0.20000000298023224 - Show Scene Geometry: true - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels - Scene Robot: - Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - Robot Alpha: 0.5 - Show Robot Collision: false - Show Robot Visual: true - Value: false - Velocity_Scaling_Factor: 1 - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_eef: - Alpha: 1 - Show Axes: false - Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Alpha: 1 - Axes Length: 0.20000000298023224 - Axes Radius: 0.029999999329447746 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: Pose - Queue Size: 10 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Axes - Topic: /target_pose - Unreliable: false - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /jparse_ellipsoid_marker - Name: MarkerArray - Namespaces: - J_proj: true - J_safety: true - J_singular: true - Queue Size: 1 - Value: true - - Attached Body Color: 150; 50; 150 - Class: moveit_rviz_plugin/RobotState - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_eef: - Alpha: 1 - Show Axes: false - Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Name: RobotState - Robot Alpha: 1 - Robot Description: robot_description - Robot State Topic: display_robot_state - Show All Links: true - Show Highlights: true - Value: true - Visual Enabled: true - - Alpha: 1 - Autocompute Intensity Bounds: false - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: temperature - Class: rviz/Temperature - Color: 255; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: true - Invert Rainbow: true - Max Color: 255; 255; 255 - Max Intensity: 100 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Temperature - Position Transformer: "" - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: "" - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/TF - Enabled: true - Filter (blacklist): "" - Filter (whitelist): "" - Frame Timeout: 15 - Frames: - All Enabled: false - link1: - Value: false - link2: - Value: false - link3: - Value: false - link4: - Value: false - link5: - Value: false - link6: - Value: false - link7: - Value: false - link_base: - Value: false - link_eef: - Value: true - world: - Value: false - Marker Alpha: 1 - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - world: - link_base: - link1: - link2: - link3: - link4: - link5: - link6: - link7: - link_eef: - {} - Update Interval: 0 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /robot_action_vector - Name: Marker - Namespaces: - robot_action_vector: true - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - Value: true - Views: - Current: - Class: rviz/XYOrbit - Distance: 1.7171660661697388 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: 0.11356700211763382 - Y: 0.10592000186443329 - Z: 2.2351800055275817e-07 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: -0.04020166024565697 - Target Frame: world - Yaw: 1.9467729330062866 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 885 - Help: - collapsed: false - Hide Left Dock: false - Hide Right Dock: false - MotionPlanning: - collapsed: false - MotionPlanning - Trajectory Slider: - collapsed: false - QMainWindow State: 000000ff00000000fd00000001000000000000020c0000031bfc0200000007fb000000100044006900730070006c006100790073010000003d0000031b000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001410000017d0000017d00ffffff0000056e0000031b00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Views: - collapsed: false - Width: 1920 - X: 0 - Y: 28 diff --git a/manipulator_control_ros1/rviz/kinova_gen3.rviz b/manipulator_control_ros1/rviz/kinova_gen3.rviz deleted file mode 100644 index 483021e..0000000 --- a/manipulator_control_ros1/rviz/kinova_gen3.rviz +++ /dev/null @@ -1,328 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /TF1 - - /MarkerArray1 - - /MarkerArray1/Namespaces1 - - /Pose1 - Splitter Ratio: 0.5 - Tree Height: 346 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Name: Time - SyncMode: 0 - SyncSource: Image -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: "" - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - bracelet_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera_color_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_depth_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - end_effector_link: - Alpha: 1 - Show Axes: false - Show Trail: false - forearm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - half_arm_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - half_arm_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - shoulder_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - spherical_wrist_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - spherical_wrist_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tool_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: true - Filter (blacklist): "" - Filter (whitelist): "" - Frame Timeout: 2 - Frames: - All Enabled: false - base_link: - Value: true - bracelet_link: - Value: false - camera_color_frame: - Value: false - camera_depth_frame: - Value: false - camera_link: - Value: false - end_effector_link: - Value: true - forearm_link: - Value: false - half_arm_1_link: - Value: false - half_arm_2_link: - Value: false - shoulder_link: - Value: false - spherical_wrist_1_link: - Value: false - spherical_wrist_2_link: - Value: false - tag_1: - Value: true - tool_frame: - Value: false - Marker Alpha: 1 - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - base_link: - shoulder_link: - half_arm_1_link: - half_arm_2_link: - forearm_link: - spherical_wrist_1_link: - spherical_wrist_2_link: - bracelet_link: - end_effector_link: - camera_link: - camera_color_frame: - tag_1: - {} - camera_depth_frame: - {} - tool_frame: - {} - Update Interval: 0 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /visualization_marker - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /jparse_ellipsoid_marker - Name: MarkerArray - Namespaces: - J_proj: true - J_safety: true - Queue Size: 100 - Value: true - - Class: rviz/Camera - Enabled: false - Image Rendering: background and overlay - Image Topic: "" - Name: Camera - Overlay Alpha: 0.5 - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: false - Visibility: - "": true - Grid: true - Marker: true - MarkerArray: true - RobotModel: true - TF: true - Value: true - Zoom Factor: 1 - - Class: rviz/Image - Enabled: true - Image Topic: /camera/color/image_raw - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Alpha: 1 - Axes Length: 0.20000000298023224 - Axes Radius: 0.009999999776482582 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: Pose - Queue Size: 10 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Axes - Topic: /target_pose_servoing - Unreliable: false - Value: true - Enabled: true - Global Options: - Background Color: 243; 243; 243 - Default Light: true - Fixed Frame: base_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 2.0648767948150635 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: 0.3303897976875305 - Y: -0.10178785771131516 - Z: 0.046957336366176605 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.3747974634170532 - Target Frame: - Yaw: 1.3253982067108154 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 836 - Hide Left Dock: false - Hide Right Dock: true - Image: - collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000016b000002a6fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001e5000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610000000228000000bb0000001600fffffffb0000000a0049006d0061006700650100000228000000bb0000001600ffffff000000010000010f000002a6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002a6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000033f000002a600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1200 - X: 648 - Y: 28 diff --git a/manipulator_control_ros1/rviz/panda_rviz.rviz b/manipulator_control_ros1/rviz/panda_rviz.rviz deleted file mode 100644 index dad2ac3..0000000 --- a/manipulator_control_ros1/rviz/panda_rviz.rviz +++ /dev/null @@ -1,366 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /TF1/Frames1 - - /MarkerArray1 - - /MarkerArray1/Namespaces1 - Splitter Ratio: 0.5 - Tree Height: 539 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: "" - panda_1_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_1_hand_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_1_hand_tcp: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_1_leftfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_1_link0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_1_link0_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_1_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_1_link1_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_1_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_1_link2_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_1_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_1_link3_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_1_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_1_link4_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_1_link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_1_link5_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_1_link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_1_link6_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_1_link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_1_link7_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_1_link8: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_1_rightfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: true - Filter (blacklist): "" - Filter (whitelist): "" - Frame Timeout: 15 - Frames: - All Enabled: false - panda_1_EE: - Value: false - panda_1_K: - Value: false - panda_1_NE: - Value: false - panda_1_hand: - Value: true - panda_1_hand_sc: - Value: false - panda_1_hand_tcp: - Value: false - panda_1_leftfinger: - Value: false - panda_1_link0: - Value: true - panda_1_link0_sc: - Value: false - panda_1_link1: - Value: false - panda_1_link1_sc: - Value: false - panda_1_link2: - Value: false - panda_1_link2_sc: - Value: false - panda_1_link3: - Value: false - panda_1_link3_sc: - Value: false - panda_1_link4: - Value: false - panda_1_link4_sc: - Value: false - panda_1_link5: - Value: false - panda_1_link5_sc: - Value: false - panda_1_link6: - Value: false - panda_1_link6_sc: - Value: false - panda_1_link7: - Value: false - panda_1_link7_sc: - Value: false - panda_1_link8: - Value: false - panda_1_rightfinger: - Value: false - world: - Value: false - Marker Alpha: 1 - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - world: - panda_1_link0: - panda_1_link0_sc: - {} - panda_1_link1: - panda_1_link1_sc: - {} - panda_1_link2: - panda_1_link2_sc: - {} - panda_1_link3: - panda_1_link3_sc: - {} - panda_1_link4: - panda_1_link4_sc: - {} - panda_1_link5: - panda_1_link5_sc: - {} - panda_1_link6: - panda_1_link6_sc: - {} - panda_1_link7: - panda_1_link7_sc: - {} - panda_1_link8: - panda_1_NE: - panda_1_EE: - panda_1_K: - {} - panda_1_hand: - panda_1_hand_sc: - {} - panda_1_hand_tcp: - {} - panda_1_leftfinger: - {} - panda_1_rightfinger: - {} - Update Interval: 0 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /visualization_marker - Name: Marker - Namespaces: - se3_trajectory: true - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /jparse_ellipsoid_marker - Name: MarkerArray - Namespaces: - J_proj: true - J_safety: true - J_singular: true - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 243; 243; 243 - Default Light: true - Fixed Frame: panda_1_link0 - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 2.705379009246826 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.36979612708091736 - Target Frame: - Yaw: 0.8153983354568481 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 836 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000016b000002a6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002a6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002a6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000033f000002a600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1200 - X: 659 - Y: 28 diff --git a/manipulator_control_ros1/rviz/xarm_rviz.rviz b/manipulator_control_ros1/rviz/xarm_rviz.rviz deleted file mode 100644 index 66bd99f..0000000 --- a/manipulator_control_ros1/rviz/xarm_rviz.rviz +++ /dev/null @@ -1,267 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /TF1 - - /TF1/Frames1 - - /MarkerArray1 - - /MarkerArray1/Namespaces1 - Splitter Ratio: 0.5 - Tree Height: 536 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_eef: - Alpha: 1 - Show Axes: false - Show Trail: false - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /visualization_marker - Name: Marker - Namespaces: - se3_trajectory: true - Queue Size: 100 - Value: true - - Class: rviz/TF - Enabled: true - Filter (blacklist): "" - Filter (whitelist): "" - Frame Timeout: 15 - Frames: - All Enabled: false - link1: - Value: false - link2: - Value: false - link3: - Value: false - link4: - Value: false - link5: - Value: false - link6: - Value: false - link7: - Value: false - link_base: - Value: true - link_eef: - Value: true - world: - Value: false - Marker Alpha: 1 - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - world: - link_base: - link1: - link2: - link3: - link4: - link5: - link6: - link7: - link_eef: - {} - Update Interval: 0 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /jparse_ellipsoid_marker - Name: MarkerArray - Namespaces: - {} - Queue Size: 2 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /robot_action_vector - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /jparse_ellipsoid_marker_cpp - Name: MarkerArray - Namespaces: - J_proj: true - J_safety: true - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: link_base - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 1.7798486948013306 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: 0.22760114073753357 - Y: -0.09859215468168259 - Z: 0.294298380613327 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.18479715287685394 - Target Frame: - Yaw: 1.5104079246520996 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 833 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000156000002a3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002a3000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002a6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002a300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1200 - X: 716 - Y: 28 diff --git a/manipulator_control_ros1/scripts/device.py b/manipulator_control_ros1/scripts/device.py deleted file mode 100644 index 655a754..0000000 --- a/manipulator_control_ros1/scripts/device.py +++ /dev/null @@ -1,21 +0,0 @@ -import abc # for abstract base class definitions - - -class Device(metaclass=abc.ABCMeta): - """ - Base class for all robot controllers. - Defines basic interface for all controllers to adhere to. - """ - - @abc.abstractmethod - def start_control(self): - """ - Method that should be called externally before controller can - start receiving commands. - """ - raise NotImplementedError - - @abc.abstractmethod - def get_controller_state(self): - """Returns the current state of the device, a dictionary of pos, orn, grasp, and reset.""" - raise NotImplementedError \ No newline at end of file diff --git a/manipulator_control_ros1/scripts/extract_lfd_images_from_bag.py b/manipulator_control_ros1/scripts/extract_lfd_images_from_bag.py deleted file mode 100755 index 278524c..0000000 --- a/manipulator_control_ros1/scripts/extract_lfd_images_from_bag.py +++ /dev/null @@ -1,105 +0,0 @@ -#!/usr/bin/env python3 - -import rospy -import rosbag -import os -import cv2 -import numpy as np -from cv_bridge import CvBridge -from sensor_msgs.msg import Image -import rospkg -import sys - -def get_bagfile_path(bagname): - rospack = rospkg.RosPack() - package_path = rospack.get_path('manipulator_control') - bagfile_path = os.path.join(package_path, 'bag', 'xarm_real_bags', 'LfD', bagname) - - if not os.path.exists(bagfile_path): - raise FileNotFoundError(f"Bag file '{bagfile_path}' not found.") - - return bagfile_path - -def save_image(image_msg, output_dir, index): - bridge = CvBridge() - try: - # Convert ROS Image message to OpenCV BGR format - cv_image = bridge.imgmsg_to_cv2(image_msg, desired_encoding="bgr8") - filename = os.path.join(output_dir, f"image_{index:05d}.png") - cv2.imwrite(filename, cv_image) - rospy.loginfo(f"Saved image to {filename}") - return cv_image - except Exception as e: - rospy.logerr(f"Failed to convert and save image: {e}") - return None - -def process_bag(input_bag_path): - if not os.path.exists(input_bag_path): - rospy.logerr(f"Bag file not found: {input_bag_path}") - return - - # Create an output folder at the same level as the bag file - output_dir = os.path.join(os.path.dirname(input_bag_path), 'images') - os.makedirs(output_dir, exist_ok=True) - - rospy.loginfo(f"Saving images to: {output_dir}") - - index = 0 - frames = [] - timestamps = [] - - try: - with rosbag.Bag(input_bag_path, 'r') as bag: - for topic, msg, t in bag.read_messages(): - if topic == "/camera/rgb/image_raw":# and isinstance(msg, Image): - cv_image = save_image(msg, output_dir, index) - if cv_image is not None: - frames.append(cv_image) - timestamps.append(msg.header.stamp.to_sec()) - index += 1 - - rospy.loginfo(f"Extracted and saved {index} images") - - # If there are frames, create a video - if len(frames) > 1: - # Calculate average time difference to define FPS - time_diffs = np.diff(timestamps) - avg_time_diff = np.mean(time_diffs) - fps = round(1.0 / avg_time_diff) if avg_time_diff > 0 else 30 - - rospy.loginfo(f"Average time difference: {avg_time_diff:.4f} seconds => FPS: {fps}") - - # Get video dimensions from the first frame - height, width, _ = frames[0].shape - video_path = os.path.join(output_dir, 'output_video.avi') - - # Use the MJPG codec and .avi container, which tends to avoid color issues - fourcc = cv2.VideoWriter_fourcc(*'MJPG') - video_writer = cv2.VideoWriter(video_path, fourcc, fps, (width, height)) - - # Write frames in BGR format directly to the AVI file - for frame in frames: - video_writer.write(frame) - - video_writer.release() - rospy.loginfo(f"Saved MJPEG video to {video_path}") - - except Exception as e: - rospy.logerr(f"Error reading bag file: {e}") - -if __name__ == '__main__': - rospy.init_node('image_extraction_node') - - if len(sys.argv) < 2: - rospy.logerr("Usage: rosrun image_extraction.py ") - sys.exit(1) - - bagname = sys.argv[1] - - try: - input_bag_path = get_bagfile_path(bagname) - rospy.loginfo(f"Reading bag file: {input_bag_path}") - process_bag(input_bag_path) - except Exception as e: - rospy.logerr(f"Error: {e}") - sys.exit(1) \ No newline at end of file diff --git a/manipulator_control_ros1/scripts/input2action.py b/manipulator_control_ros1/scripts/input2action.py deleted file mode 100644 index 620400d..0000000 --- a/manipulator_control_ros1/scripts/input2action.py +++ /dev/null @@ -1,85 +0,0 @@ -""" -Utility functions for grabbing user inputs -""" - -import numpy as np - -# import robosuite as suite -# import robosuite.utils.transform_utils as T -# from robosuite.devices import * -# from robosuite.models.robots import * -# from robosuite.robots import * - -from spacemouse import SpaceMouse - - -def input2action(device, robot, active_arm="right", env_configuration=None): - """ - Converts an input from an active device into a valid action sequence that can be fed into an env.step() call - - If a reset is triggered from the device, immediately returns None. Else, returns the appropriate action - - Args: - device (Device): A device from which user inputs can be converted into actions. Can be either a Spacemouse or - Keyboard device class - - robot (Robot): Which robot we're controlling - - active_arm (str): Only applicable for multi-armed setups (e.g.: multi-arm environments or bimanual robots). - Allows inputs to be converted correctly if the control type (e.g.: IK) is dependent on arm choice. - Choices are {right, left} - - env_configuration (str or None): Only applicable for multi-armed environments. Allows inputs to be converted - correctly if the control type (e.g.: IK) is dependent on the environment setup. Options are: - {bimanual, single-arm-parallel, single-arm-opposed} - - Returns: - 2-tuple: - - - (None or np.array): Action interpreted from @device including any gripper action(s). None if we get a - reset signal from the device - - (None or int): 1 if desired close, -1 if desired open gripper state. None if get a reset signal from the - device - - """ - - - state = device.get_controller_state() - # Note: Devices output rotation with x and z flipped to account for robots starting with gripper facing down - # Also note that the outputted rotation is an absolute rotation, while outputted dpos is delta pos - # Raw delta rotations from neutral user input is captured in raw_drotation (roll, pitch, yaw) - dpos, rotation, raw_drotation, grasp, reset = ( - state["dpos"], - state["rotation"], - state["raw_drotation"], - state["grasp"], - state["reset"], - ) - - # If we're resetting, immediately return None - if reset: - return None, None - - # Get controller reference - #controller = robot.controller if not isinstance(robot, Bimanual) else robot.controller[active_arm] - - #gripper_dof = robot.gripper.dof - - # First process the raw drotation - drotation = raw_drotation[[1, 0, 2]] - - #elif controller.name == "OSC_POSE": - # Flip z - drotation[2] = -drotation[2] - # Scale rotation for teleoperation (tuned for OSC) -- gains tuned for each device - drotation = drotation * 50 - dpos = dpos * 125 - - # map 0 to -1 (open) and map 1 to 1 (closed) - grasp = 1 if grasp else -1 - - #action = np.concatenate([dpos, drotation, [grasp] * gripper_dof]) - action = np.concatenate([dpos, drotation]) - - # Return the action and grasp - return action, grasp \ No newline at end of file diff --git a/manipulator_control_ros1/scripts/main_experiment_kinova.py b/manipulator_control_ros1/scripts/main_experiment_kinova.py deleted file mode 100755 index 7ffd2b3..0000000 --- a/manipulator_control_ros1/scripts/main_experiment_kinova.py +++ /dev/null @@ -1,852 +0,0 @@ -#!/usr/bin/env python3 -import sys -import rospy -import numpy as np -import math -import scipy -import time -import cv2 - -import PyKDL as kdl -from urdf_parser_py.urdf import Robot -from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model -from pykdl_utils.kdl_kinematics import KDLKinematics -from pykdl_utils.kdl_kinematics import kdl_to_mat -from pykdl_utils.kdl_kinematics import joint_kdl_to_list -from pykdl_utils.kdl_kinematics import joint_list_to_kdl - -import geometry_msgs.msg -from geometry_msgs.msg import Pose, PoseStamped, Vector3, TwistStamped, TransformStamped, PoseWithCovarianceStamped -from std_msgs.msg import Float64MultiArray, Float64 -from sensor_msgs.msg import JointState -import tf -from tf import TransformerROS -import tf2_ros -from tf.transformations import quaternion_from_matrix, quaternion_matrix - -# import april tag detection array -from apriltag_ros.msg import AprilTagDetectionArray - -import moveit_commander -from moveit_commander import MoveGroupCommander, RobotCommander, PlanningSceneInterface -import moveit_msgs.msg - -from sensor_msgs.msg import Image -from cv_bridge import CvBridge - -import roslib - -from std_srvs.srv import Empty -import kortex_driver.msg -import kortex_driver.srv -from kortex_driver.msg import Base_JointSpeeds, JointSpeed -from kortex_driver.msg import ActionNotification, ActionEvent -from kortex_driver.srv import ReadAction, ReadActionRequest, ExecuteAction, ExecuteActionRequest - -import actionlib -from kortex_driver.srv import * -from kortex_driver.msg import * - -from manipulator_control import jparse_cls - - - -class ExampleInitializeGazeboRobot(object): - """Unpause Gazebo and home robot""" - def __init__(self): - # Initialize the node - self.HOME_ACTION_IDENTIFIER = 2 - self.last_action_notif_type = None - - try: - self.robot_name = rospy.get_param('~robot_name',"my_gen3") - self.action_topic_sub = rospy.Subscriber("/" + self.robot_name + "/action_topic", ActionNotification, self.cb_action_topic) - - # Wait for the driver to be initialised - while not rospy.has_param("/" + self.robot_name + "/is_initialized"): - time.sleep(0.1) - - # Init the services - read_action_full_name = '/' + self.robot_name + '/base/read_action' - rospy.wait_for_service(read_action_full_name) - self.read_action = rospy.ServiceProxy(read_action_full_name, ReadAction) - - execute_action_full_name = '/' + self.robot_name + '/base/execute_action' - rospy.wait_for_service(execute_action_full_name) - self.execute_action = rospy.ServiceProxy(execute_action_full_name, ExecuteAction) - except rospy.ROSException as e: - self.is_init_success = False - else: - self.is_init_success = True - - def cb_action_topic(self, notif): - self.last_action_notif_type = notif.action_event - - def wait_for_action_end_or_abort(self): - while not rospy.is_shutdown(): - if (self.last_action_notif_type == ActionEvent.ACTION_END): - rospy.loginfo("Received ACTION_END notification") - return True - elif (self.last_action_notif_type == ActionEvent.ACTION_ABORT): - rospy.loginfo("Received ACTION_ABORT notification") - return False - else: - time.sleep(0.01) - - def home_the_robot(self): - # The Home Action is used to home the robot. It cannot be deleted and is always ID #2: - self.last_action_notif_type = None - req = ReadActionRequest() - req.input.identifier = self.HOME_ACTION_IDENTIFIER - - try: - res = self.read_action(req) - except rospy.ServiceException: - rospy.logerr("Failed to call ReadAction") - return False - # Execute the HOME action if we could read it - else: - # What we just read is the input of the ExecuteAction service - req = ExecuteActionRequest() - req.input = res.output - rospy.loginfo("Sending the robot home...") - try: - self.execute_action(req) - except rospy.ServiceException: - rospy.logerr("Failed to call ExecuteAction") - return False - else: - return self.wait_for_action_end_or_abort() - - - -class ArmController: - def __init__(self): - rospy.init_node('arm_controller', anonymous=True) - - self.base_link = rospy.get_param('/base_link_name', 'base_link') #defaults are for Kinova gen3 - self.end_link = rospy.get_param('/end_link_name', 'end_effector_link') - - # Set parameters - self.position_only = rospy.get_param('/position_only', False) #boolean to control only position versus full pose - - self.is_sim = rospy.get_param('~is_sim', True) #boolean to control if the robot is in simulation or not - - self.phi_gain_position = rospy.get_param('~phi_gain_position', 2) #gain for the phi term in the JParse method - self.phi_gain_angular = rospy.get_param('~phi_gain_angular', 2) #gain for the phi term in the JParse method - - self.jparse_gamma = rospy.get_param('~jparse_gamma', 0.2) #gamma for the JParse method - #choose the method to use - self.method = rospy.get_param('~method', 'JParse') #options are JParse, JacobianPseudoInverse, JacobianDampedLeastSquares, JacobianProjection, JacobianDynamicallyConsistentInverse - self.show_jparse_ellipses = rospy.get_param('~show_jparse_ellipses', False) #boolean to control if the jparse ellipses are shown or not - self.use_space_mouse = rospy.get_param('~use_space_mouse', False) #boolean to control if the space mouse is used or not - self.use_space_mouse_jparse = rospy.get_param('~use_space_mouse_jparse', False) #boolean to control if the space mouse is used or not - - #decide if moveit is used or not - self.use_moveit_bool = rospy.get_param('~use_moveit', False) #boolean to control if moveit is used or not - - # Initialize variables - self.joint_states = None - self.target_pose = None - self.jacobian_calculator = jparse_cls.JParseClass(base_link=self.base_link, end_link=self.end_link) - - #For visual servoing - self.visual_servoing_bool = rospy.get_param('~visual_servoing', False) #boolean to control if visual servoing is used or not - self.camera_frame_name = rospy.get_param('~camera_link_name', 'camera_link') #name of the camera frame - self.servo_tag = rospy.get_param('~tag_name','tag_1') #name of the tag to use for visual servoing - self.apriltag_topic = rospy.get_param('~apriltag_topic','/tag_detections') - - #get tf listener - self.tfBuffer = tf2_ros.Buffer() - self.listener = tf2_ros.TransformListener(self.tfBuffer) - self.tfros = tf.TransformerROS() - self.tf_timeout = rospy.Duration(1.0) - - - if self.is_sim == True: - #if we are in simulation, use the joint_states and target_pose topics - joint_states_topic = '/my_gen3/joint_states' - else: - joint_states_topic = '/my_gen3/joint_states' - - rospy.Subscriber(joint_states_topic, JointState, self.joint_states_callback) - - #determine where target pose is coming from: - if self.visual_servoing_bool == True: - #visual servoing - rospy.Subscriber(self.apriltag_topic, AprilTagDetectionArray, self.apriltag_pose_visual_servoing_callback) - else: - #message publishing desired pose - rospy.Subscriber('/target_pose', PoseStamped, self.target_pose_callback) - - #if using spacemouse - rospy.Subscriber('/robot_action', TwistStamped, self.space_mouse_callback) - - self.joint_vel_command_real = rospy.Publisher('/my_gen3/in/joint_velocity', Base_JointSpeeds, queue_size=10) - - #For MoveIt! control - moveit_commander.roscpp_initialize(sys.argv) - self.moveit_commander = moveit_commander.RobotCommander() - try: - self.is_gripper_present = rospy.get_param(rospy.get_namespace() + "is_gripper_present", False) - if self.is_gripper_present: - gripper_joint_names = rospy.get_param(rospy.get_namespace() + "gripper_joint_names", []) - self.gripper_joint_name = gripper_joint_names[0] - else: - self.gripper_joint_name = "" - self.degrees_of_freedom = rospy.get_param(rospy.get_namespace() + "degrees_of_freedom", 7) - - # Create the MoveItInterface necessary objects - arm_group_name = "arm" - self.robot = moveit_commander.RobotCommander("robot_description") - self.scene = moveit_commander.PlanningSceneInterface(ns=rospy.get_namespace()) - self.arm_group = moveit_commander.MoveGroupCommander(arm_group_name, ns=rospy.get_namespace()) - self.display_trajectory_publisher = rospy.Publisher(rospy.get_namespace() + 'move_group/display_planned_path', - moveit_msgs.msg.DisplayTrajectory, - queue_size=20) - - if self.is_gripper_present: - gripper_group_name = "gripper" - self.gripper_group = moveit_commander.MoveGroupCommander(gripper_group_name, ns=rospy.get_namespace()) - - rospy.loginfo("Initializing node in namespace " + rospy.get_namespace()) - except Exception as e: - print (e) - self.is_init_success = False - else: - self.is_init_success = True - - - - #For Kinova velocity control - self.joint_vel_command = rospy.Publisher('/my_gen3/in/joint_velocity', Base_JointSpeeds, queue_size=1) - - #publish relevant topics - self.manip_measure_pub = rospy.Publisher('/manip_measure', Float64, queue_size=10) - self.inverse_cond_number = rospy.Publisher('/inverse_cond_number', Float64, queue_size=10) - #have certain messages to store raw error - self.pose_error_pub = rospy.Publisher('/pose_error', PoseStamped, queue_size=10) - self.position_error_pub = rospy.Publisher('/position_error', Vector3, queue_size=10) - self.orientation_error_pub = rospy.Publisher('/orientation_error', Vector3, queue_size=10) - #have certain messages to store control error - self.pose_error_control_pub = rospy.Publisher('/pose_error_control', PoseStamped, queue_size=10) - self.position_error_control_pub = rospy.Publisher('/position_error', Vector3, queue_size=10) - self.orientation_control_error_pub = rospy.Publisher('/orientation_error', Vector3, queue_size=10) - #publish the current end effector pose and target pose - self.current_end_effector_pose_pub = rospy.Publisher('/current_end_effector_pose', PoseStamped, queue_size=10) - self.current_target_pose_pub = rospy.Publisher('/current_target_pose', PoseStamped, queue_size=10) - - #For Kinova velocity control - self.HOME_ACTION_IDENTIFIER = 2 - self.last_action_notif_type = None - self.robot_name = rospy.get_param('~robot_name', "my_gen3") - # Wait for the driver to initialize - while not rospy.has_param("/" + self.robot_name + "/is_initialized"): - time.sleep(0.1) - read_action_full_name = '/' + self.robot_name + '/base/read_action' - rospy.wait_for_service(read_action_full_name) - self.read_action = rospy.ServiceProxy(read_action_full_name, ReadAction) - - execute_action_full_name = '/' + self.robot_name + '/base/execute_action' - rospy.wait_for_service(execute_action_full_name) - self.execute_action = rospy.ServiceProxy(execute_action_full_name, ExecuteAction) - - if self.is_sim == True: - rospy.wait_for_service('/my_gen3/base/send_joint_speeds_command') - try: - self.srv_joint_speeds = rospy.ServiceProxy('/my_gen3/base/send_joint_speeds_command', kortex_driver.srv.SendJointSpeedsCommand) - except: - rospy.logerr("Failed to connect to joint speeds service") - pass - self.joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6', 'joint_7'] - self.speed_req = kortex_driver.srv.SendJointSpeedsCommandRequest() - - joint_idx = [] - for jidx,jname in enumerate(self.joint_names): - joint_speed= kortex_driver.msg.JointSpeed() - joint_speed.joint_identifier= jidx - joint_speed.value= 0.0 - self.speed_req.input.joint_speeds.append(joint_speed) - joint_idx.append(jidx) - #now zip the joint names and indices - self.joint_idx_dict = dict(zip(self.joint_names, joint_idx)) - #Now start the control loop - self.rate = rospy.Rate(20) # 10 Hz - - # publisher of self.target_pose which is PoseStamped - self.target_pose_pub = rospy.Publisher('/target_pose_servoing', PoseStamped, queue_size=10) - - # Start the control loop - # hang out here for a bit to let the robot settle - self.control_loop() - - - def joint_states_callback(self, msg): - """ - Callback function for the joint_states topic. This function will be called whenever a new message is received - on the joint_states topic. The message is a sensor_msgs/JointState message, which contains the current joint - and the corresponding joint velocities and efforts. This function will extract the joint positions, velocities, - and efforts and return them as lists. - """ - self.joint_states = msg - - - def space_mouse_callback(self, msg): - """ - Callback function for the space_mouse topic. This function will be called whenever a new message is received - on the space_mouse topic. The message is a geometry_msgs/TwistStamped message, which contains the current - velocity of the end effector. This function will extract the linear and angular velocities and return them as - lists. - """ - space_mouse_command = np.array([msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z, msg.twist.angular.x, msg.twist.angular.y, msg.twist.angular.z]) - position_velocity = space_mouse_command[:3] - angular_velocity = space_mouse_command[3:] - position_velocity_norm = np.linalg.norm(position_velocity) - angular_velocity_norm = np.linalg.norm(angular_velocity) - if position_velocity_norm > 0.05: - position_velocity = position_velocity / position_velocity_norm * 0.05 - self.space_mouse_command = np.array([position_velocity[0], position_velocity[1],position_velocity[2],angular_velocity[0],angular_velocity[1],angular_velocity[2]]) #check if norm of the space mouse command is greater than 0.05, if so normalize it to this value - - #ensure we can get into the while loop - if self.use_space_mouse == True: - self.target_pose = PoseStamped() #set this to get in a loop - - - - def apriltag_pose_visual_servoing_callback(self,msg): - """ - 1. Looks up TF from the AprilTag (self.servo_tag) to the robot's base frame (self.base_link). - 2. Extracts the tag's Z-axis in base coordinates. - 3. Performs a dot-product with the base frame's X-axis to check if the tag is facing the robot. - 4. If yes, offsets the end-effector target pose 0.1m along the tag's Z-axis (in base frame). - 5. Stores the result in self.target_pose (PoseStamped). - We use tf directly instead of the msg, but the msg serves as a good trigger - """ - rospy.loginfo("Tag is detected") - if self.target_pose is None: - self.target_pose = PoseStamped() - - self.target_pose_pub.publish(self.target_pose) - - rospy.loginfo("Looking up transform from tag to base") - try: - # Lookup transform: from tag -> base - tag_to_base: TransformStamped = self.tfBuffer.lookup_transform( - source_frame= self.servo_tag, # "base_link" - target_frame=self.base_link, # "tag_1" - time=rospy.Time(0), # most recent transform - timeout=rospy.Duration(1.0) - ) - rospy.loginfo(f"Transform from {self.servo_tag} to {self.base_link} found") - except tf2_ros.LookupException as e: - rospy.logerr(f"Transform not found: {e}") - return - except tf2_ros.ExtrapolationException as e: - rospy.logerr(f"Transform error: {e}") - return - - # Extract translation - tx = tag_to_base.transform.translation.x - ty = tag_to_base.transform.translation.y - tz = tag_to_base.transform.translation.z - - # Extract quaternion and build a 4x4 transform matrix - rx = tag_to_base.transform.rotation.x - ry = tag_to_base.transform.rotation.y - rz = tag_to_base.transform.rotation.z - rw = tag_to_base.transform.rotation.w - tag_quat = [rx, ry, rz, rw] - tag_matrix = tf.transformations.quaternion_matrix(tag_quat) - - # The tag's Z-axis in its local frame is (0, 0, 1). - # We transform that into the base frame using the 4x4 matrix. - tag_z_axis_in_base = [ - tag_matrix[0, 2], - tag_matrix[1, 2], - tag_matrix[2, 2] - ] - - # The base frame's X-axis is simply (1, 0, 0). - # Dot product to check if the tag is facing the robot - base_x_axis = [1.0, 0.0, 0.0] - dot_product = (tag_z_axis_in_base[0] * base_x_axis[0] + - tag_z_axis_in_base[1] * base_x_axis[1] + - tag_z_axis_in_base[2] * base_x_axis[2]) - - if dot_product < 0: - rospy.loginfo("Tag is facing the robot. Computing target pose...") - - # Offset 0.1 meters along the tag's Z-axis - offset_distance = 0.4 - x_target = tx + offset_distance * tag_z_axis_in_base[0] - y_target = ty + offset_distance * tag_z_axis_in_base[1] - z_target = tz + offset_distance * tag_z_axis_in_base[2] - - # Fill self.target_pose - self.target_pose.header.stamp = rospy.Time.now() - self.target_pose.header.frame_id = self.base_link - self.target_pose.pose.position.x = x_target - self.target_pose.pose.position.y = y_target - self.target_pose.pose.position.z = z_target - - # rotate the tag's orientation - # first make a rotation matrix from the quaternion - angle = 2 * np.arccos(tag_quat[3]) - axis = np.array([tag_quat[0], tag_quat[1], tag_quat[2]])/np.sin(angle/2) - tag_rotation_matrix = self.axis_angle_to_rotation_matrix(axis*angle) - - R_ee_to_tag = np.array([[-1, 0, 0], - [0, 1, 0], - [0, 0, -1]]) - - R_ee = np.dot(tag_rotation_matrix, R_ee_to_tag) - - # convert the rotation matrix to a quaternion - q_ee = self.rotation_matrix_to_quaternion(R_ee) - rw, rx, ry, rz = q_ee - - # Orientation: you might just copy the tag's orientation, - # or set a specific orientation for your end effector. - self.target_pose.pose.orientation.x = rx - self.target_pose.pose.orientation.y = ry - self.target_pose.pose.orientation.z = rz - self.target_pose.pose.orientation.w = rw - - rospy.loginfo(f"Target pose set:\n{self.target_pose}") - else: - rospy.loginfo("Tag is NOT facing the robot. No action taken.") - - - - - def target_pose_callback(self, msg): - """ - Callback function for the target_pose topic. This function will be called whenever a new message is received - on the target_pose topic. The message is a geometry_msgs/PoseStamped message, which contains the target pose - """ - self.target_pose = msg - - def EndEffectorPose(self, q): - """ - This function computes the end-effector pose given the joint configuration q. - """ - current_pose = PoseStamped() - current_pose.header.frame_id = self.base_link - current_pose.header.stamp = rospy.Time.now() - try: - trans = self.tfBuffer.lookup_transform(self.base_link, self.end_link, rospy.Time(), timeout=self.tf_timeout) - current_pose.pose.position.x = trans.transform.translation.x - current_pose.pose.position.y = trans.transform.translation.y - current_pose.pose.position.z = trans.transform.translation.z - current_pose.pose.orientation.x = trans.transform.rotation.x - current_pose.pose.orientation.y = trans.transform.rotation.y - current_pose.pose.orientation.z = trans.transform.rotation.z - current_pose.pose.orientation.w = trans.transform.rotation.w - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): - rospy.logerr("TF lookup failed") - rospy.logerr("failed to lookup transform from %s to %s", self.base_link, self.end_link) - return current_pose - - def EndEffectorVelocity(self, q, dq): - """ - This function computes the end-effector velocity given the joint configuration q and joint velocities dq. - """ - J = self.jacobian_calculator.jacobian(q) - J = np.array(J) - dq = np.array(dq) - dx = np.dot(J, dq) - return dx - - def axis_angle_to_rotation_matrix(self, axis_angle): - """ - Converts an axis-angle vector to a rotation matrix. - - Parameters: - axis_angle (numpy.ndarray): Axis-angle vector (3 elements). - - Returns: - numpy.ndarray: A 3x3 rotation matrix. - """ - # Extract the axis and angle - axis = axis_angle / np.linalg.norm(axis_angle) - angle = np.linalg.norm(axis_angle) - - if angle == 0: - return np.eye(3) - - # Compute the skew-symmetric matrix - K = np.array([ - [0, -axis[2], axis[1]], - [axis[2], 0, -axis[0]], - [-axis[1], axis[0], 0] - ]) - - # Compute the rotation matrix using the Rodrigues' formula - R = np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * np.dot(K, K) - - return R - - def rotation_matrix_to_quaternion(self, R): - """ - Converts a rotation matrix to a quaternion. - - Parameters: - R (numpy.ndarray): A 3x3 rotation matrix. - - Returns: - numpy.ndarray: A 4-element quaternion. - - """ - rospy.loginfo(f"Rotation matrix: {R}") - if not (R.shape == (3, 3) and np.allclose(np.dot(R.T, R), np.eye(3)) and np.isclose(np.linalg.det(R), 1)): - raise ValueError("Input matrix must be a valid rotation matrix.") - # Compute the quaternion using the method from the book - q = np.zeros(4) - q[0] = np.sqrt(1 + R[0, 0] + R[1, 1] + R[2, 2]) / 2 - q[1] = (R[2, 1] - R[1, 2]) / (4 * q[0]) - q[2] = (R[0, 2] - R[2, 0]) / (4 * q[0]) - q[3] = (R[1, 0] - R[0, 1]) / (4 * q[0]) - - return q - - def rotation_matrix_to_axis_angle(self,R): - """ - Converts a rotation matrix to an axis-angle vector. - - Parameters: - R (numpy.ndarray): A 3x3 rotation matrix. - - Returns: - numpy.ndarray: Axis-angle vector (3 elements). - """ - if not (R.shape == (3, 3) and np.allclose(np.dot(R.T, R), np.eye(3)) and np.isclose(np.linalg.det(R), 1)): - raise ValueError("Input matrix must be a valid rotation matrix.") - - # Calculate the angle of rotation - val = (np.trace(R) - 1) / 2 - angle = np.arccos(np.around(val, 4)) - - if np.isclose(angle, 0): # No rotation - return np.zeros(3) - - if np.isclose(angle, np.pi): # 180 degrees rotation - # Special case for 180-degree rotation - # Extract the axis from the diagonal of R - axis = np.sqrt((np.diag(R) + 1) / 2) - # Adjust signs based on the matrix off-diagonals - axis[0] *= np.sign(R[2, 1] - R[1, 2]) - axis[1] *= np.sign(R[0, 2] - R[2, 0]) - axis[2] *= np.sign(R[1, 0] - R[0, 1]) - return axis * angle - - # General case - axis = np.array([ - R[2, 1] - R[1, 2], - R[0, 2] - R[2, 0], - R[1, 0] - R[0, 1] - ]) / (2 * np.sin(angle)) - - return axis * angle - - def pose_error(self, current_pose, target_pose, error_norm_max = 0.05, control_pose_error = True): - """ - This function computes the error between the current pose and the target pose. - """ - #Compute the position error - position_error = np.array([target_pose.pose.position.x - current_pose.pose.position.x, - target_pose.pose.position.y - current_pose.pose.position.y, - target_pose.pose.position.z - current_pose.pose.position.z]) - #if the norm of the position error is greater than the maximum allowed, scale it down - if control_pose_error == True: - if np.linalg.norm(position_error) > error_norm_max: - position_error = position_error / np.linalg.norm(position_error) * error_norm_max - - #convert the quaternion in posestamped to list of quaternions then pass this into quaternion_matrix to get the rotation matrix. Access only the rotation part of the matrix - goal_rotation_matrix = quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])[:3,:3] - current_rotation_matrix = quaternion_matrix([current_pose.pose.orientation.x, current_pose.pose.orientation.y, current_pose.pose.orientation.z, current_pose.pose.orientation.w])[:3,:3] - #Compute the orientation error - R_error= np.dot(goal_rotation_matrix, np.linalg.inv(current_rotation_matrix)) - rospy.loginfo(f"R_error: {R_error}") - # Extract the axis-angle lie algebra vector from the rotation matrix - angle_error = self.rotation_matrix_to_axis_angle(R_error) - # Return the position and orientation error - return position_error, angle_error - - - def publish_pose_errors(self, position_error, angular_error, control_pose_error = True): - """ - Publishes the position and orientation errors as ROS messages. - """ - pose_error_msg = PoseStamped() - pose_error_msg.header.frame_id = self.base_link - pose_error_msg.header.stamp = rospy.Time.now() - pose_error_msg.pose.position.x = position_error[0] - pose_error_msg.pose.position.y = position_error[1] - pose_error_msg.pose.position.z = position_error[2] - #for completeness, convert the axis angle to quaternion - Rmat = self.axis_angle_to_rotation_matrix(angular_error) - quat_from_Rmat = self.rotation_matrix_to_quaternion(Rmat) - pose_error_msg.pose.orientation.x = quat_from_Rmat[0] - pose_error_msg.pose.orientation.y = quat_from_Rmat[1] - pose_error_msg.pose.orientation.z = quat_from_Rmat[2] - pose_error_msg.pose.orientation.w = quat_from_Rmat[3] - #publish the axis-angle orientation error - orientation_error_msg = Vector3() - orientation_error_msg.x = angular_error[0] - orientation_error_msg.y = angular_error[1] - orientation_error_msg.z = angular_error[2] - #publish the position error - position_error_msg = Vector3() - position_error_msg.x = position_error[0] - position_error_msg.y = position_error[1] - position_error_msg.z = position_error[2] - - #Now publish - if control_pose_error == True: - self.position_error_control_pub.publish(position_error_msg) - self.pose_error_control_pub.publish(pose_error_msg) - self.orientation_control_error_pub.publish(orientation_error_msg) - else: - self.pose_error_pub.publish(pose_error_msg) - self.position_error_pub.publish(position_error_msg) - self.orientation_error_pub.publish(orientation_error_msg) - - - def nominal_joint_pose_nullspace(self, q, Vmax = 5, Kp_value_set=1.0): - """ - This function computes the nominal joint pose for the nullspace control of the Panda robot. - - Vmax is the maximum joint error norm. - """ - # Nominal joint pose for the Kinova robot - if self.is_sim == True: - q_nominal = np.array([0.00, -0.794, 0.00, -2.364, 0.00, 1.583, 0.785]) #joints panda_joint1 to panda_joint7 - else: - q_nominal = np.array([0.00, 0.26, 3.14, -2.27, 0.00, 0.96, 1.57]) - # Compute the joint pose error - q_error = q_nominal -q - q_error_norm = np.linalg.norm(q_error) - nominal_joint_motion = np.matrix(np.zeros(len(q))).T #if q_error_norm is less than 0.1, then return a zero vector - if q_error_norm > 0.1: - g = np.min([q_error_norm, Vmax]) - Kp = np.diag([Kp_value_set] * len(q)) - # Compute the nominal joint motion - nominal_joint_motion = (g/q_error_norm) * Kp@q_error - nominal_joint_motion = np.matrix(nominal_joint_motion).T - - return nominal_joint_motion - - - - def control_loop(self): - while not rospy.is_shutdown(): - if self.joint_states and self.target_pose: - # obtain the current joints - q = [] - dq = [] - effort = [] - for joint_name in self.joint_names: - idx = self.joint_states.name.index(joint_name) - q.append(self.joint_states.position[idx]) - dq.append(self.joint_states.velocity[idx]) - effort.append(self.joint_states.effort[idx]) - # Calculate the Method inv Jacobian - method = self.method #set by parameter, can be set from launch file - rospy.loginfo("Method being used: %s", method) - if method == "JacobianPseudoInverse": - #this is the traditional pseudo-inverse method for the jacobian - J_method, J_nullspace = self.jacobian_calculator.JacobianPseudoInverse(q=q, position_only=self.position_only, jac_nullspace_bool=True) - elif method == "JParse": - # The JParse method takes in the joint angles, gamma, position_only, and singular_direction_gain - if self.show_jparse_ellipses == True: - J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position, singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True, publish_jparse_ellipses=True, end_effector_pose=self.EndEffectorPose(q), verbose=False) - else: - J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position,singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True) - elif method == "JacobianDampedLeastSquares": - J_method, J_nullspace = self.jacobian_calculator.jacobian_damped_least_squares(q=q, damping=0.1, jac_nullspace_bool=True) #dampening of 0.1 works very well, 0.8 shows clear error - elif method == "JacobianProjection": - J_proj, J_nullspace = self.jacobian_calculator.jacobian_projection(q=q, gamma=0.1, jac_nullspace_bool=True) - J_method = np.linalg.pinv(J_proj) - - - manip_measure = self.jacobian_calculator.manipulability_measure(q) - self.manip_measure_pub.publish(manip_measure) - inverse_cond_number = self.jacobian_calculator.inverse_condition_number(q) - self.inverse_cond_number.publish(inverse_cond_number) - rospy.loginfo("Manipulability measure: %f", manip_measure) - rospy.loginfo("Inverse condition number: %f", inverse_cond_number) - - - #Calculate the delta_x (task space error) - target_pose = self.target_pose - current_pose = self.EndEffectorPose(q) - - current_end_effector_velocity = self.EndEffectorVelocity(q, dq) #full twist of the end effector - position_error, angular_error = self.pose_error(current_pose, target_pose, control_pose_error=False) - # log the pose error - self.publish_pose_errors(position_error, angular_error, control_pose_error=False) - if self.is_sim == False: - #real world limits - error_norm_max = 0.10 - else: - #simulation limits - error_norm_max = 0.30 - position_error, angular_error = self.pose_error(current_pose, target_pose, error_norm_max = error_norm_max, control_pose_error=True) - self.publish_pose_errors(position_error, angular_error, control_pose_error=True) - - #concatenate the position and angular error - full_pose_error = np.matrix(np.concatenate((position_error, angular_error))).T - #obtain nullspace control - if self.is_sim == True: - nominal_motion_nullspace = self.nominal_joint_pose_nullspace(q=q, Vmax=5, Kp_value_set=2.0) - else: - nominal_motion_nullspace = self.nominal_joint_pose_nullspace(q=q, Vmax=5, Kp_value_set=0.0) - # Calculate and command the joint velocities - if self.position_only == True: - rospy.loginfo("Position only control") - position_error = np.matrix(position_error).T - if self.is_sim == True: - #use these gains only in simulation! - kp_gain = 1.0 - else: - kp_gain = 1.0 - Kp = np.diag([kp_gain, kp_gain, kp_gain]) # Proportional gain matrix - - task_vector = Kp @ position_error - joint_velocities = J_method @ task_vector + J_nullspace @ nominal_motion_nullspace - else: - # realworld gains (tested) - kp_gain_position = 1.0 - kp_gain_orientation = 1.0 - if self.is_sim == True: - #use these gains only in simulation! - kp_gain_position = 1.0 - kp_gain_orientation = 1.0 - Kp_position = np.diag([kp_gain_position, kp_gain_position, kp_gain_position]) # Proportional gain matrix - Kp_orientation = np.diag([kp_gain_orientation, kp_gain_orientation, kp_gain_orientation]) - task_position = Kp_position @ np.matrix(position_error).T - task_orientation = Kp_orientation @ np.matrix(angular_error).T - full_pose_error = np.matrix(np.concatenate((task_position.T, task_orientation.T),axis=1)).T - if self.use_space_mouse == False: - joint_velocities = J_method @ full_pose_error + J_nullspace @ nominal_motion_nullspace - if self.use_space_mouse == True: - #use the space mouse command to control the joint velocities - space_mouse_command = np.matrix(self.space_mouse_command).T - #now add this to the joint velocities - joint_velocities = J_method @ space_mouse_command + J_nullspace @ nominal_motion_nullspace - - joint_velocities_list = np.array(joint_velocities).flatten().tolist() - - joint_vel_dict = dict(zip(self.joint_names, joint_velocities_list)) #this dictionary combines the joint names and velocities - # command the joint velocities - if self.use_moveit_bool: - success = self.reach_joint_angles(joint_velocities=joint_velocities_list, tolerance=0.001, control_frequency=5) - print("Joint velocities:", joint_velocities_list) - print("Success:", success) - - else: - self.command_joint_velocities(joint_vel_dict) #this commands the joint velocities - - rospy.loginfo("Control loop running") - - - - def reach_joint_angles(self, tolerance=0.01, control_frequency=20, - joint_velocities=[0.0]*7, - velocity_scaling_factor=1, acceleration_scaling_factor=1): - arm_group = self.arm_group - success = True - - # Increase the effective time_step if the increments are too small. - # For example, use a scaling factor or accumulate multiple increments. - time_step = 1.0 / control_frequency # base step - - # Get the current joint positions - joint_positions = arm_group.get_current_joint_values() - rospy.loginfo("Current joint positions: %s", joint_positions) - - # Adjust the target position based on commanded velocities. - try: - for i in range(len(joint_positions)): - # Here, you might apply a velocity scaling factor or accumulate changes. - joint_positions[i] += joint_velocities[i] * time_step * velocity_scaling_factor - - # Set the new goal for MoveIt! - arm_group.set_joint_value_target(joint_positions) - self.arm_group.set_goal_joint_tolerance(tolerance) - - # Plan the trajectory to this new goal - (success_flag, trajectory_message, planning_time, error_code) = arm_group.plan() - if not success_flag: - rospy.logerr("Planning failed with error code: %s", error_code) - return False - - # Execute the planned trajectory - success &= arm_group.go(wait=True) - except moveit_commander.MoveItCommanderException as e: - rospy.logerr("Planning failed with exception: %s", str(e)) - success = False - - new_joint_positions = arm_group.get_current_joint_values() - rospy.loginfo("New joint positions: %s", new_joint_positions) - return success - - def reach_named_position(self, target, velocity_scaling_factor=1, acceleration_scaling_factor=1): - arm_group = self.arm_group - - # Going to one of those targets - rospy.loginfo("Going to named target " + target) - # Set the target - arm_group.set_named_target(target) - - # Plan the trajectory - (success_flag, trajectory_message, planning_time, error_code) = arm_group.plan() - # Execute the trajectory and block while it's not finished - return arm_group.execute(trajectory_message, wait=True) #could use retimed_plan instead of trajectory_message - - def publish_joint_velocities(self, joint_velocities): - """ - Publishes the joint velocities to the /joint_velocities topic. - """ - joint_vel_msg = Float64MultiArray() - joint_velocities = np.array(joint_velocities).flatten().tolist() - joint_vel_msg.data = joint_velocities - self.joint_vel_pub.publish(joint_vel_msg) - - - def command_joint_velocities(self, joint_vel_dict): - """ - This function commands the joint velocities to the robot using the appropriate ROS message type. - """ - if self.is_sim == False: - vel_msg = Base_JointSpeeds() - # vel_msg.duration =0.01 #shouldn't need to set this - for idx in range(len(self.joint_names)): - - joint_speed = JointSpeed() - joint_speed.joint_identifier = idx - joint_speed.value = joint_vel_dict[self.joint_names[idx]] - vel_msg.joint_speeds.append(joint_speed) - - rospy.loginfo("SENT Joint velocities: %s", vel_msg.joint_speeds) - - self.joint_vel_command_real.publish(vel_msg) - else: - for joint_speed in self.speed_req.input.joint_speeds: - for name in self.joint_idx_dict: - if joint_speed.joint_identifier == self.joint_idx_dict[name]: - joint_speed.value= joint_vel_dict[name] #this is the joint velocity - - #the joint_speed.joint_identifier is the joint index, which is used to find the joint velocity name from self.joint_idx_dict, which is then used to find the joint velocity value from joint_vel_dict - self.srv_joint_speeds.call(self.speed_req) - -if __name__ == '__main__': - try: - controller = ArmController() - except rospy.ROSInterruptException: - pass - finally: - # Create a dictionary with zero velocities for all joints - zero_vel_dict = {joint_name: 0.0 for joint_name in controller.joint_names} - # Send zero velocities to stop the robot safely - controller.command_joint_velocities(zero_vel_dict) - rospy.loginfo("Program interrupted: Sent zero joint velocities for safe shutdown") \ No newline at end of file diff --git a/manipulator_control_ros1/scripts/panda_torque_main_experiment.py b/manipulator_control_ros1/scripts/panda_torque_main_experiment.py deleted file mode 100755 index a8b27f7..0000000 --- a/manipulator_control_ros1/scripts/panda_torque_main_experiment.py +++ /dev/null @@ -1,534 +0,0 @@ -#!/usr/bin/env python3 -import sys -import rospy -import numpy as np -import math -import scipy - -import PyKDL as kdl -from urdf_parser_py.urdf import Robot -from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model -from pykdl_utils.kdl_kinematics import KDLKinematics -from pykdl_utils.kdl_kinematics import kdl_to_mat -from pykdl_utils.kdl_kinematics import joint_kdl_to_list -from pykdl_utils.kdl_kinematics import joint_list_to_kdl - -import geometry_msgs.msg -from geometry_msgs.msg import Pose, PoseStamped, Vector3 -from std_msgs.msg import Float64MultiArray, Float64 -from sensor_msgs.msg import JointState -import tf -from tf import TransformerROS -import tf2_ros -from tf.transformations import quaternion_from_euler, quaternion_matrix - -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from sensor_msgs.msg import JointState - -#For panda torque control -from gazebo_msgs.srv import ApplyJointEffort, ApplyJointEffortRequest -from std_msgs.msg import Duration - -from manipulator_control import jparse_cls - - -class ArmController: - def __init__(self): - rospy.init_node('arm_controller', anonymous=True) - - self.base_link = rospy.get_param('/base_link_name', 'panda_1_link0') #defaults are for panda - self.end_link = rospy.get_param('/end_link_name', 'panda_1_hand') - - #Set parameters - self.position_only = rospy.get_param('~position_only', False) #boolean to control only position versus full pose - self.is_sim = rospy.get_param('~is_sim', False) #boolean to control if the robot is in simulation or not - - #choose the method to use - self.method = rospy.get_param('~method', 'JParse') #options are JParse, JacobianPseudoInverse, JacobianDampedLeastSquares, JacobianProjection, JacobianDynamicallyConsistentInverse - self.show_jparse_ellipses = rospy.get_param('~show_jparse_ellipses', False) #boolean to control if the jparse ellipses are shown or not - self.phi_gain_position = rospy.get_param('~phi_gain_position', 15) #gain for the phi term in the JParse method - self.phi_gain_angular = rospy.get_param('~phi_gain_angular', 15) #gain for the phi term in the JParse method - self.jparse_gamma = rospy.get_param('~jparse_gamma', 0.1) #gamma for the JParse method - - self.joint_states = None - self.target_pose = None - self.jacobian_calculator = jparse_cls.JParseClass(base_link=self.base_link, end_link=self.end_link) - - #get tf listener - self.tfBuffer = tf2_ros.Buffer() - self.listener = tf2_ros.TransformListener(self.tfBuffer) - self.tfros = tf.TransformerROS() - self.tf_timeout = rospy.Duration(1.0) - - rospy.Subscriber('/joint_states', JointState, self.joint_states_callback) - rospy.Subscriber('/target_pose', PoseStamped, self.target_pose_callback) - - self.manip_measure_pub = rospy.Publisher('/manip_measure', Float64, queue_size=10) - self.inverse_cond_number = rospy.Publisher('/inverse_cond_number', Float64, queue_size=10) - self.pose_error_pub = rospy.Publisher('/pose_error', PoseStamped, queue_size=10) - self.position_error_pub = rospy.Publisher('/position_error', Vector3, queue_size=10) - self.current_end_effector_pose_pub = rospy.Publisher('/current_end_effector_pose', PoseStamped, queue_size=10) - self.orientation_error_pub = rospy.Publisher('/orientation_error', Vector3, queue_size=10) - self.current_target_pose_pub = rospy.Publisher('/current_target_pose', PoseStamped, queue_size=10) - self.pub_joint_states = rospy.Publisher('/control_script_joint_states', JointState, queue_size=10) - - # Service client for /gazebo/apply_joint_effort - rospy.wait_for_service('/gazebo/apply_joint_effort') - self.apply_joint_effort = rospy.ServiceProxy('/gazebo/apply_joint_effort', ApplyJointEffort) - # Joint names - self.joint_names = [f'panda_1_joint{i}' for i in range(1, 8)] - - # Control parameters - self.frequency = 50.0 # Hz - # Loop duration - self.duration = rospy.Duration(1.0 / self.frequency) - - self.rate = rospy.Rate(self.frequency) # 10 Hz - #home the robot - self.torque_home_robot() - # Start the control loop - self.control_loop() - - def rad2deg(self, q): - return q/math.pi*180.0 - - def joint_states_callback(self, msg): - """ - Callback function for the joint_states topic. This function will be called whenever a new message is received - on the joint_states topic. The message is a sensor_msgs/JointState message, which contains the current joint - and the corresponding joint velocities and efforts. This function will extract the joint positions, velocities, - and efforts and return them as lists. - """ - self.joint_states = msg - self.pub_joint_states.publish(msg) - - def target_pose_callback(self, msg): - """ - Callback function for the target_pose topic. This function will be called whenever a new message is received - on the target_pose topic. The message is a geometry_msgs/PoseStamped message, which contains the target pose - """ - self.target_pose = msg - self.current_target_pose_pub.publish(self.target_pose) #if target pose is paused manually, this allows us to track the current target pose seen by the script - - def EndEffectorPose(self, q): - """ - This function computes the end-effector pose given the joint configuration q. - """ - current_pose = PoseStamped() - current_pose.header.frame_id = self.base_link - current_pose.header.stamp = rospy.Time.now() - # listener = tf.TransformListener() - try: - trans = self.tfBuffer.lookup_transform(self.base_link, self.end_link, rospy.Time(), timeout=self.tf_timeout) - current_pose.pose.position.x = trans.transform.translation.x - current_pose.pose.position.y = trans.transform.translation.y - current_pose.pose.position.z = trans.transform.translation.z - current_pose.pose.orientation.x = trans.transform.rotation.x - current_pose.pose.orientation.y = trans.transform.rotation.y - current_pose.pose.orientation.z = trans.transform.rotation.z - current_pose.pose.orientation.w = trans.transform.rotation.w - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): - rospy.logerr("TF lookup failed") - rospy.logerr("failed to lookup transform from %s to %s", self.base_link, self.end_link) - self.current_end_effector_pose_pub.publish(current_pose) - return current_pose - - def EndEffectorVelocity(self, q, dq): - """ - This function computes the end-effector velocity given the joint configuration q and joint velocities dq. - """ - J = self.jacobian_calculator.jacobian(q) - J = np.array(J) - dq = np.array(dq) - dx = np.dot(J, dq) - return dx - - def rotation_matrix_to_axis_angle(self,R): - """ - Converts a rotation matrix to an axis-angle vector. - - Parameters: - R (numpy.ndarray): A 3x3 rotation matrix. - - Returns: - numpy.ndarray: Axis-angle vector (3 elements). - """ - if not (R.shape == (3, 3) and np.allclose(np.dot(R.T, R), np.eye(3)) and np.isclose(np.linalg.det(R), 1)): - raise ValueError("Input matrix must be a valid rotation matrix.") - - # Calculate the angle of rotation - angle = np.arccos((np.trace(R) - 1) / 2) - - if np.isclose(angle, 0): # No rotation - return np.zeros(3) - - if np.isclose(angle, np.pi): # 180 degrees rotation - # Special case for 180-degree rotation - # Extract the axis from the diagonal of R - axis = np.sqrt((np.diag(R) + 1) / 2) - # Adjust signs based on the matrix off-diagonals - axis[0] *= np.sign(R[2, 1] - R[1, 2]) - axis[1] *= np.sign(R[0, 2] - R[2, 0]) - axis[2] *= np.sign(R[1, 0] - R[0, 1]) - return axis * angle - - # General case - axis = np.array([ - R[2, 1] - R[1, 2], - R[0, 2] - R[2, 0], - R[1, 0] - R[0, 1] - ]) / (2 * np.sin(angle)) - - return axis * angle - - - def pose_error(self, current_pose, target_pose, error_norm_max = 0.05): - """ - This function computes the error between the current pose and the target pose. - """ - #Compute the position error - position_error = np.array([target_pose.pose.position.x - current_pose.pose.position.x, - target_pose.pose.position.y - current_pose.pose.position.y, - target_pose.pose.position.z - current_pose.pose.position.z]) - #if the norm of the position error is greater than the maximum allowed, scale it down - if np.linalg.norm(position_error) > error_norm_max: - position_error = position_error / np.linalg.norm(position_error) * error_norm_max - - #convert the quaternion in posestamped to list of quaternions then pass this into quaternion_matrix to get the rotation matrix. Access only the rotation part of the matrix - goal_rotation_matrix = quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])[:3,:3] - current_rotation_matrix = quaternion_matrix([current_pose.pose.orientation.x, current_pose.pose.orientation.y, current_pose.pose.orientation.z, current_pose.pose.orientation.w])[:3,:3] - #Compute the orientation error - R_error= np.dot(goal_rotation_matrix, np.linalg.inv(current_rotation_matrix)) - # Extract the axis-angle lie algebra vector from the rotation matrix - angle_error = self.rotation_matrix_to_axis_angle(R_error) - # Return the position and orientation error - return position_error, angle_error - - def nominal_joint_pose_nullspace_panda(self, q, dq, Vmax = 5, Kp_value_set=5.0, Kd_value_set=3.0, homing=False): - """ - This function computes the nominal joint pose for the nullspace control of the Panda robot. - - Vmax is the maximum joint error norm. - """ - # Nominal joint pose for the Panda robot - if homing: - q_nominal = np.array([0.00, -0.794, 0.00, -2.364, 0.00, 1.583, 0.785]) #joints panda_joint1 to panda_joint7 - else: - q_nominal = np.array([0.00, -0.794, 0.00, -2.364, 0.00, 1.583, 0.785]) #joints panda_joint1 to panda_joint7 - # Compute the joint pose error - q_error = q_nominal -q - q_error_norm = np.linalg.norm(q_error) - nominal_joint_motion = np.matrix(np.zeros(len(q))).T #if q_error_norm is less than 0.1, then return a zero vector - if q_error_norm > 0.1: - g = np.min([q_error_norm, Vmax]) - if homing: - Kp_value = Kp_value_set #5.0 - else: - Kp_value = Kp_value_set #5 - Kp = np.diag([Kp_value] * len(q)) - if homing: - Kd_value = Kd_value_set #3.0 - else: - Kd_value = Kd_value_set #3 - Kd = np.diag([Kd_value] * len(q)) - - # Compute the nominal joint motion - nominal_joint_motion = (g/q_error_norm) * Kp@q_error - Kd@dq - nominal_joint_motion_norm = np.linalg.norm(nominal_joint_motion) - nominal_joint_motion = np.matrix(nominal_joint_motion).T - - return nominal_joint_motion - - - def axis_angle_to_rotation_matrix(self, axis_angle): - """ - Converts an axis-angle vector to a rotation matrix. - - Parameters: - axis_angle (numpy.ndarray): Axis-angle vector (3 elements). - - Returns: - numpy.ndarray: A 3x3 rotation matrix. - """ - # Extract the axis and angle - axis = axis_angle / np.linalg.norm(axis_angle) - angle = np.linalg.norm(axis_angle) - - # Compute the skew-symmetric matrix - K = np.array([ - [0, -axis[2], axis[1]], - [axis[2], 0, -axis[0]], - [-axis[1], axis[0], 0] - ]) - - # Compute the rotation matrix using the Rodrigues' formula - R = np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * np.dot(K, K) - - return R - - def rotation_matrix_to_quaternion(self, R): - """ - Converts a rotation matrix to a quaternion. - - Parameters: - R (numpy.ndarray): A 3x3 rotation matrix. - - Returns: - numpy.ndarray: A 4-element quaternion. - """ - if not (R.shape == (3, 3) and np.allclose(np.dot(R.T, R), np.eye(3)) and np.isclose(np.linalg.det(R), 1)): - raise ValueError("Input matrix must be a valid rotation matrix.") - - # Compute the quaternion using the method from the book - q = np.zeros(4) - q[0] = np.sqrt(1 + R[0, 0] + R[1, 1] + R[2, 2]) / 2 - q[1] = (R[2, 1] - R[1, 2]) / (4 * q[0]) - q[2] = (R[0, 2] - R[2, 0]) / (4 * q[0]) - q[3] = (R[1, 0] - R[0, 1]) / (4 * q[0]) - - return q - - def publish_pose_errors(self, position_error, angular_error): - """ - Publishes the position and orientation errors as ROS messages. - """ - pose_error_msg = PoseStamped() - pose_error_msg.header.frame_id = self.base_link - pose_error_msg.header.stamp = rospy.Time.now() - pose_error_msg.pose.position.x = position_error[0] - pose_error_msg.pose.position.y = position_error[1] - pose_error_msg.pose.position.z = position_error[2] - #for completeness, convert the axis angle to quaternion - Rmat = self.axis_angle_to_rotation_matrix(angular_error) - quat_from_Rmat = self.rotation_matrix_to_quaternion(Rmat) - pose_error_msg.pose.orientation.x = quat_from_Rmat[0] - pose_error_msg.pose.orientation.y = quat_from_Rmat[1] - pose_error_msg.pose.orientation.z = quat_from_Rmat[2] - pose_error_msg.pose.orientation.w = quat_from_Rmat[3] - #now publish - self.pose_error_pub.publish(pose_error_msg) - #publish the axis-angle orientation error - orientation_error_msg = Vector3() - orientation_error_msg.x = angular_error[0] - orientation_error_msg.y = angular_error[1] - orientation_error_msg.z = angular_error[2] - self.orientation_error_pub.publish(orientation_error_msg) - #publish the position error - position_error_msg = Vector3() - position_error_msg.x = position_error[0] - position_error_msg.y = position_error[1] - position_error_msg.z = position_error[2] - self.position_error_pub.publish(position_error_msg) - - - - def control_loop(self): - """ - This function implements the control loop for the arm controller. - """ - while not rospy.is_shutdown(): - if self.joint_states and self.target_pose: - t = rospy.Time.now() - # obtain the current joints - q = [] - dq = [] - effort = [] - for joint_name in self.joint_names: - idx = self.joint_states.name.index(joint_name) - q.append(self.joint_states.position[idx]) - dq.append(self.joint_states.velocity[idx]) - effort.append(self.joint_states.effort[idx]) - self.current_positions = q - #from stability analysis, the kp_gain for the jparse_singular direction and for position error must be equal - stable_kp_gain_position = self.phi_gain_position #15.0; match position gain - stable_kp_gain_angular = self.phi_gain_angular #15.0; match orientation gain - stable_kd_gain_position = self.phi_gain_position/1.5 #7.0 - stable_kd_gain_angular = self.phi_gain_angular/2.5 #5.0 - - method = self.method #set by parameter, can be set from launch file - if method == "JacobianPseudoInverse": - #this is the traditional pseudo-inverse method for the jacobian - J_method, J_nullspace = self.jacobian_calculator.JacobianPseudoInverse(q=q, position_only=self.position_only, jac_nullspace_bool=True) - J_method_twist = J_method - elif method == "JParse": - # The JParse method takes in the joint angles, gamma, position_only, and singular_direction_gain - if self.show_jparse_ellipses == True: - J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=stable_kp_gain_position, singular_direction_gain_angular=stable_kp_gain_angular, jac_nullspace_bool=True, publish_jparse_ellipses=True, end_effector_pose=self.EndEffectorPose(q)) - J_method_twist, J_nullspace_twist = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=stable_kd_gain_position, singular_direction_gain_angular=stable_kd_gain_angular, jac_nullspace_bool=True, publish_jparse_ellipses=False, end_effector_pose=self.EndEffectorPose(q)) - - else: - J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=stable_kp_gain_position, singular_direction_gain_angular=stable_kp_gain_angular, jac_nullspace_bool=True) - J_method_twist, J_nullspace_twist = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=stable_kd_gain_position, singular_direction_gain_angular=stable_kd_gain_angular, jac_nullspace_bool=True, publish_jparse_ellipses=False, end_effector_pose=self.EndEffectorPose(q)) - - # J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=0.1, position_only=self.position_only, singular_direction_gain=stable_kp_gain, jac_nullspace_bool=True) - elif method == "JacobianDampedLeastSquares": - J_method, J_nullspace = self.jacobian_calculator.jacobian_damped_least_squares(q=q, damping=0.1, jac_nullspace_bool=True) #dampening of 0.1 works very well, 0.8 shows clear error - J_method_twist = J_method - elif method == "JacobianTranspose": - J_method, J_nullspace = self.jacobian_calculator.jacobian_transpose(q=q) - J_method_twist = J_method - elif method == "JacobianNullspaceDecoupled": - J_method, J_nullspace = self.jacobian_calculator.jacobian_nullspace_decoupled(q=q, dq=dq, identity_weight_matrix=False) - J_method_twist = J_method - #Find jacobian dot - J_dot = self.jacobian_calculator.jacobian_dot(q=q, position_only=self.position_only, approx=True) #approximate method seems to work better than numerical differentiation - dq_vect = np.matrix(dq).T - manip_measure = self.jacobian_calculator.manipulability_measure(q) - self.manip_measure_pub.publish(manip_measure) - inverse_cond_number = self.jacobian_calculator.inverse_condition_number(q) - self.inverse_cond_number.publish(inverse_cond_number) - rospy.loginfo("Manipulability measure: %f", manip_measure) - rospy.loginfo("Inverse condition number: %f", inverse_cond_number) - #Calculate the delta_x (task space error) - target_pose = self.target_pose - current_pose = self.EndEffectorPose(q) - current_end_effector_velocity = self.EndEffectorVelocity(q, dq) #full twist of the end effector - if self.is_sim == False: - #real world limits - error_norm_max = 0.05 - else: - #simulation limits - error_norm_max = 0.05 - position_error, angular_error = self.pose_error(current_pose, target_pose, error_norm_max = error_norm_max) - #log the pose error - self.publish_pose_errors(position_error, angular_error) - - #get the mass matrix, coriolis, and gravity terms - Mass = self.jacobian_calculator.inertia(q) - Coriolis = self.jacobian_calculator.coriolis(q, dq) - Gravity = self.jacobian_calculator.gravity(q) - - #Nullspace control objectives - nominal_joint_motion = self.nominal_joint_pose_nullspace_panda(q, dq, Vmax = 0.5, Kp_value_set=5.0, Kd_value_set= 5.0) #VMax 50, Kp=5.0 - - error_calc_time = rospy.Time.now() - t - # Calculate and command the joint velocities - if self.position_only == True: - rospy.loginfo("Position only control") - position_error = np.matrix(position_error).T - # print("position error is:", position_error) - print("position error norm is:", np.linalg.norm(position_error)) - end_effector_velocity = np.matrix(current_end_effector_velocity[:3]).T - kp_gain = stable_kp_gain_position - Kp = np.diag([kp_gain, kp_gain, kp_gain]) # Proportional gain matrix - kd_gain = 4.0 - Kd = np.diag([kd_gain, kd_gain, kd_gain]) # Derivative gain matrix - task_vector = Kp @ position_error - Kd @ end_effector_velocity - # now calculate the joint velocities and joint torques - joint_velocities = J_method @ task_vector - # qddot = J_method @ task_vector - J_method @ J_dot[:3,:] @ dq_vect + J_nullspace @ nominal_joint_motion - qddot = J_method @ task_vector + J_nullspace @ nominal_joint_motion - joint_torque = Mass@qddot #+ Coriolis #+ Gravity - else: - # Task position and orientation - kp_gain_position = stable_kp_gain_position - kp_gain_orientation = stable_kp_gain_angular - kd_gain_position = stable_kd_gain_position - kd_gain_orientation = stable_kd_gain_angular - - if method == "JacobianNullspaceDecoupled": - kp_gain_position = 40.0 - kp_gain_orientation = 10.0 - kd_gain_position = 10.0 - kd_gain_orientation = 5.0 - - Kp_position = np.diag([kp_gain_position, kp_gain_position, kp_gain_position]) # Proportional gain matrix - Kp_orientation = np.diag([kp_gain_orientation, kp_gain_orientation, kp_gain_orientation]) - task_position = Kp_position @ np.matrix(position_error).T - task_orientation = Kp_orientation @ np.matrix(angular_error).T - task_pose = np.matrix(np.concatenate((task_position.T, task_orientation.T),axis=1)).T - #For velocities: twist of position and orientation - Kd_position = np.diag([kd_gain_position, kd_gain_position, kd_gain_position]) # Derivative gain matrix - Kd_orientation = np.diag([kd_gain_orientation, kd_gain_orientation, kd_gain_orientation]) - task_position_vel = Kd_position @ np.matrix(current_end_effector_velocity[:3]).T - task_orientation_vel = Kd_orientation @ np.matrix(current_end_effector_velocity[3:]).T - task_twist = np.matrix(np.concatenate((task_position_vel.T, task_orientation_vel.T),axis=1)).T - # now add task pose and task twist - task_vector = task_pose - task_twist #this is Kp*(xd-x) - Kd*(dx) - task_vector_pose = task_pose - task_vector_twist = -task_twist - # Now calculate the joint velocities and joint torques - joint_velocities = J_method @ task_vector - qddot = J_method @ task_vector_pose + J_method_twist @ task_vector_twist + J_nullspace @ nominal_joint_motion - joint_torque = Mass@qddot #+ Coriolis #+ Gravity - if q[3] > -0.1: - #if the robot is in a configuration where the middle joint is over-extended, add this term to bring it back - joint_torque[3] += -20.0 * (-0.1 - q[3]) - 0.5 * dq[3] - rospy.loginfo("WE ARE IN LOCKOUT Joint 3 torque: %f", joint_torque[3]) - - - - - joint_velocities_list = np.array(joint_velocities).flatten().tolist() - # command the joint velocities - # Create a dictionary for joint names and their corresponding torques and make the joint torques a list - joint_torque_flattent_list = np.array(joint_torque).flatten().tolist() - joint_torque_dict = dict(zip(self.joint_names, joint_torque_flattent_list)) - # Command the joint torques - self.command_joint_torques(joint_torque_dict) - self.rate.sleep() - rospy.loginfo("Control loop running") - - def torque_home_robot(self): - """ - This function commands the robot to the home position using joint torques. - """ - # do the following in a loop for 5 seconds: - t_start = rospy.Time.now() - duration = 0.0 - while not rospy.is_shutdown() and duration < 5.0: - rospy.loginfo("Homing the robot") - if self.joint_states: - duration = (rospy.Time.now() - t_start).to_sec() - q = [] - dq = [] - effort = [] - for joint_name in self.joint_names: - idx = self.joint_states.name.index(joint_name) - q.append(self.joint_states.position[idx]) - dq.append(self.joint_states.velocity[idx]) - effort.append(self.joint_states.effort[idx]) - self.current_positions = q - # Home position for the Panda robot - nominal_joint_motion = self.nominal_joint_pose_nullspace_panda(q, dq, Vmax = 50, homing=True) - qddot = nominal_joint_motion - Mass = self.jacobian_calculator.inertia(q) - joint_torque = Mass@qddot #+ Coriolis #+ Gravity - joint_torque_flattent_list = np.array(joint_torque).flatten().tolist() - joint_torque_dict = dict(zip(self.joint_names, joint_torque_flattent_list)) - # Command the joint torques - self.command_joint_torques(joint_torque_dict) - - - - def command_joint_torques(self, joint_torque_dict): - """ - This function iterates through the joint names and applies the corresponding torque to each joint. - """ - # print("in command joint torques") - try: - for i, joint_name in enumerate(self.joint_names): - # print("joint_name:", joint_name) - # print("joint_torque_dict[joint_name]:", joint_torque_dict[joint_name]) - self.apply_torque(joint_name, joint_torque_dict[joint_name]) - except rospy.ServiceException as e: - rospy.logerr(f"Failed to call /gazebo/apply_joint_effort: {e}") - - def apply_torque(self, joint_name, effort): - """Apply torque to a joint.""" - req = ApplyJointEffortRequest() - req.joint_name = joint_name - req.effort = effort - req.start_time = rospy.Time.now() - req.duration = self.duration - - try: - self.apply_joint_effort(req) - except rospy.ServiceException as e: - rospy.logerr(f"Failed to call /gazebo/apply_joint_effort: {e}") - - -if __name__ == '__main__': - try: - arm_controller = ArmController() - except rospy.ROSInterruptException: - pass \ No newline at end of file diff --git a/manipulator_control_ros1/scripts/position_trajectory_generator.py b/manipulator_control_ros1/scripts/position_trajectory_generator.py deleted file mode 100755 index 1cde749..0000000 --- a/manipulator_control_ros1/scripts/position_trajectory_generator.py +++ /dev/null @@ -1,125 +0,0 @@ -#!/usr/bin/env python3 - -import rospy -import math -from visualization_msgs.msg import Marker -from geometry_msgs.msg import Pose, PoseStamped - -class OvalTrajectory: - def __init__(self): - # Initialize ROS node - rospy.init_node('oval_trajectory_generator', anonymous=True) - - # RViz Marker publisher - self.marker_pub = rospy.Publisher('/visualization_marker', Marker, queue_size=10) - - # Pose publisher - self.pose_pub = rospy.Publisher('/target_pose', PoseStamped, queue_size=10) - - # Parameters for the oval trajectory - self.center = rospy.get_param('~center', [0.0, 0.0, 0.0]) # Center of the oval [x, y, z] - if type(self.center) is str: - self.center = [float(x) for x in self.center.strip('[]').split(',')] # Convert string to list of floats - - self.major_axis = float(rospy.get_param('~major_axis', "1.0")) # Length of the major axis - self.minor_axis = float(rospy.get_param('~minor_axis', "0.5")) # Length of the minor axis - self.pitch_axis = float(rospy.get_param('~pitch_axis', "0.0")) # Length of the pitch axis - self.plane = rospy.get_param('~plane', "xy") # Plane of the oval ('xy', 'yz', or 'xz') - self.frequency = float(rospy.get_param('~frequency', "0.2")) # Frequency of the trajectory (Hz) - self.base_frame = rospy.get_param('~base_frame', "base_link") # Base frame for the trajectory - - # Time tracking - self.start_time = rospy.Time.now() - - # Start the trajectory generation loop - self.trajectory_loop() - - def generate_position(self, time_elapsed): - """Generate the 3D position of the point on the oval.""" - angle = 2 * math.pi * self.frequency * time_elapsed - - if self.plane == 'xy': - x = self.center[0] + self.major_axis * math.cos(angle) - y = self.center[1] + self.minor_axis * math.sin(angle) - z = self.center[2] + self.pitch_axis * math.cos(angle) - elif self.plane == 'yz': - x = self.center[0] - y = self.center[1] + self.major_axis * math.cos(angle) - z = self.center[2] + self.minor_axis * math.sin(angle) - elif self.plane == 'xz': - x = self.center[0] + self.major_axis * math.cos(angle) - y = self.center[1] - z = self.center[2] + self.minor_axis * math.sin(angle) - else: - rospy.logwarn("Invalid plane specified. Defaulting to 'xy'.") - x = self.center[0] + self.major_axis * math.cos(angle) - y = self.center[1] + self.minor_axis * math.sin(angle) - z = self.center[2] - - return x, y, z - - def publish_marker(self, position): - """Publish the current goal position as an RViz marker.""" - marker = Marker() - marker.header.frame_id = self.base_frame - marker.header.stamp = rospy.Time.now() - marker.ns = "oval_trajectory" - marker.id = 0 - marker.type = Marker.SPHERE - marker.action = Marker.ADD - marker.pose.position.x = position[0] - marker.pose.position.y = position[1] - marker.pose.position.z = position[2] - marker.pose.orientation.x = 0.0 - marker.pose.orientation.y = 0.0 - marker.pose.orientation.z = 0.0 - marker.pose.orientation.w = 1.0 - marker.scale.x = 0.1 - marker.scale.y = 0.1 - marker.scale.z = 0.1 - marker.color.r = 1.0 - marker.color.g = 0.0 - marker.color.b = 0.0 - marker.color.a = 1.0 - - self.marker_pub.publish(marker) - - def publish_pose(self, position): - """Publish the current goal position as a geometry_msgs/Pose message.""" - pose = PoseStamped() - pose.header.frame_id = self.base_frame - pose.header.stamp = rospy.Time.now() - pose.pose.position.x = position[0] - pose.pose.position.y = position[1] - pose.pose.position.z = position[2] - pose.pose.orientation.x = 0.0 - pose.pose.orientation.y = 0.0 - pose.pose.orientation.z = 0.0 - pose.pose.orientation.w = 1.0 - - self.pose_pub.publish(pose) - - def trajectory_loop(self): - """Main loop for trajectory generation and marker/pose publishing.""" - rate = rospy.Rate(50) # 50 Hz control loop - - while not rospy.is_shutdown(): - # Calculate elapsed time - elapsed_time = (rospy.Time.now() - self.start_time).to_sec() - - # Generate current position on the oval - position = self.generate_position(elapsed_time) - - # Publish the marker and pose - self.publish_marker(position) - self.publish_pose(position) - - # Sleep to maintain loop rate - rate.sleep() - - -if __name__ == '__main__': - try: - OvalTrajectory() - except rospy.ROSInterruptException: - rospy.loginfo("Oval trajectory generator node terminated.") \ No newline at end of file diff --git a/manipulator_control_ros1/scripts/se3_trajectory_generator.py b/manipulator_control_ros1/scripts/se3_trajectory_generator.py deleted file mode 100755 index b06a12a..0000000 --- a/manipulator_control_ros1/scripts/se3_trajectory_generator.py +++ /dev/null @@ -1,281 +0,0 @@ -#!/usr/bin/env python3 - -import rospy -import math -import numpy as np -from visualization_msgs.msg import Marker -from geometry_msgs.msg import PoseStamped, Pose -from tf.transformations import quaternion_from_euler, quaternion_matrix - -class SE3Trajectory: - def __init__(self): - # Initialize ROS node - rospy.init_node('se3_trajectory_generator', anonymous=True) - - # RViz Marker publisher - self.marker_pub = rospy.Publisher('/visualization_marker', Marker, queue_size=10) - - # Pose publisher - self.pose_pub = rospy.Publisher('/target_pose', PoseStamped, queue_size=10) - - # Parameters for position trajectory - self.center = rospy.get_param('~center', [0.0, 0.0, 0.0]) # Center of the oval [x, y, z] - if type(self.center) is str: - self.center = [float(x) for x in self.center.strip('[]').split(',')] # Convert string to list of floats - - self.robot = rospy.get_param('~robot', "panda") # Robot type ('panda' or 'xarm' or 'kinova') - - self.key_points_only_bool = rospy.get_param('~key_points_only_bool', "False") # Boolean to determine if only key points should be published - self.use_rotation = rospy.get_param('~use_rotation', "True") # Boolean to determine if rotation should be used - - self.major_axis = float(rospy.get_param('~major_axis', "1.0")) # Length of the major axis - self.minor_axis = float(rospy.get_param('~minor_axis', "0.5")) # Length of the minor axis - self.pitch_axis = float(rospy.get_param('~pitch_axis', "0.0")) # Length of the pitch axis - - - self.plane = rospy.get_param('~plane', "xy") # Plane of the oval ('xy', 'yz', or 'xz') - self.frequency = float(rospy.get_param('~frequency', "0.2")) # Frequency of the trajectory (Hz) - - # Parameters for orientation trajectory - self.orientation_major_axis = float(rospy.get_param('~orientation_major_axis', "0.3")) - self.orientation_minor_axis = float(rospy.get_param('~orientation_minor_axis', "0.1")) - self.orientation_frequency = float(rospy.get_param('~orientation_frequency', "0.1")) # Frequency of orientation change - - #Get the base frame for the trajectory - self.base_frame = rospy.get_param('~base_frame', "base_link") # Base frame for the trajectory - - #set scale for marker arrow length - self.arrow_scale = 0.25 - - # Time tracking - self.start_time = rospy.Time.now() - - # Start the trajectory generation loop - self.trajectory_loop() - - def discrete_positions_around_ellipse(self, time_elapsed): - # Period (time spent at each critical point) - period = 1 / self.frequency - # Determine which critical point to select based on time_elapsed - critical_point_index = int((time_elapsed % (4 * period)) // period) - - if self.plane == 'xy': - if critical_point_index == 0: # Positive major axis - x = self.center[0] + self.major_axis - y = self.center[1] - z = self.center[2] - elif critical_point_index == 1: # Positive minor axis - x = self.center[0] - y = self.center[1] + self.minor_axis - z = self.center[2] - elif critical_point_index == 2: # Negative major axis - x = self.center[0] - self.major_axis - y = self.center[1] - z = self.center[2] - elif critical_point_index == 3: # Negative minor axis - x = self.center[0] - y = self.center[1] - self.minor_axis - z = self.center[2] - - elif self.plane == 'yz': - if critical_point_index == 0: # Positive major axis - x = self.center[0] - y = self.center[1] + self.major_axis - z = self.center[2] - elif critical_point_index == 1: # Positive minor axis - x = self.center[0] - y = self.center[1] - z = self.center[2] + self.minor_axis - elif critical_point_index == 2: # Negative major axis - x = self.center[0] - y = self.center[1] - self.major_axis - z = self.center[2] - elif critical_point_index == 3: # Negative minor axis - x = self.center[0] - y = self.center[1] - z = self.center[2] - self.minor_axis - - elif self.plane == 'xz': - if critical_point_index == 0: # Positive major axis - x = self.center[0] + self.major_axis - y = self.center[1] - z = self.center[2] - elif critical_point_index == 1: # Positive minor axis - x = self.center[0] - y = self.center[1] - z = self.center[2] + self.minor_axis - elif critical_point_index == 2: # Negative major axis - x = self.center[0] - self.major_axis - y = self.center[1] - z = self.center[2] - elif critical_point_index == 3: # Negative minor axis - x = self.center[0] - y = self.center[1] - z = self.center[2] - self.minor_axis - - else: - rospy.logwarn("Invalid plane specified. Defaulting to 'xy'.") - if critical_point_index == 0: # Positive major axis - x = self.center[0] + self.major_axis - y = self.center[1] - z = self.center[2] - elif critical_point_index == 1: # Positive minor axis - x = self.center[0] - y = self.center[1] + self.minor_axis - z = self.center[2] - elif critical_point_index == 2: # Negative major axis - x = self.center[0] - self.major_axis - y = self.center[1] - z = self.center[2] - elif critical_point_index == 3: # Negative minor axis - x = self.center[0] - y = self.center[1] - z = self.center[2] - self.minor_axis - - return x, y, z - - def generate_position(self, time_elapsed): - """Generate the 3D position of the point on the oval.""" - if self.key_points_only_bool: - x, y, z = self.discrete_positions_around_ellipse(time_elapsed) - - else: - angle = 2 * math.pi * self.frequency * time_elapsed - - if self.plane == 'xy': - x = self.center[0] + self.major_axis * math.cos(angle) - y = self.center[1] + self.minor_axis * math.sin(angle) - z = self.center[2] + self.pitch_axis * math.cos(angle) - elif self.plane == 'yz': - x = self.center[0] - y = self.center[1] + self.major_axis * math.cos(angle) - z = self.center[2] + self.minor_axis * math.sin(angle) - elif self.plane == 'xz': - x = self.center[0] + self.major_axis * math.cos(angle) - y = self.center[1] - z = self.center[2] + self.minor_axis * math.sin(angle) - else: - rospy.logwarn("Invalid plane specified. Defaulting to 'xy'.") - x = self.center[0] + self.major_axis * math.cos(angle) - y = self.center[1] + self.minor_axis * math.sin(angle) - z = self.center[2] - - - - return x, y, z - - def generate_orientation(self, time_elapsed): - """ - Generate the orientation as a quaternion tracing an oval. - this function returns a quaternion representing the orientation of the goal pose. - the type of the returned value is a list of 4 floats representing the quaternion in the order [x, y, z, w]. - """ - angle = 2 * math.pi * self.orientation_frequency * time_elapsed - - if self.use_rotation: - roll = np.pi #keep roll constant - pitch = self.orientation_major_axis * math.cos(angle) # Oval in pitch - yaw = self.orientation_minor_axis * math.sin(angle) # Oval in yaw - else: - if self.robot == "panda" or self.robot == "xarm": - roll = np.pi - pitch = 0 #self.orientation_major_axis - yaw = 0 #self.orientation_minor_axis - elif self.robot == "kinova": - roll = np.pi/2 - pitch = 0 - yaw = np.pi/2 - - # Generate quaternion from roll, pitch, yaw - quaternion = quaternion_from_euler(roll, pitch, yaw) - return quaternion - - def publish_marker(self, position, orientation): - """Publish the current goal position and orientation as RViz markers representing axes.""" - # Convert quaternion to rotation matrix - rotation_matrix = quaternion_matrix(orientation) - - # Define axis vectors (1 unit length) - x_axis = rotation_matrix[:3, 0] # Red - y_axis = rotation_matrix[:3, 1] # Green - z_axis = rotation_matrix[:3, 2] # Blue - - # Publish arrows for each axis - self.publish_arrow(position, x_axis, [1.0, 0.0, 0.0, 1.0], 0) # Red - self.publish_arrow(position, y_axis, [0.0, 1.0, 0.0, 1.0], 1) # Green - self.publish_arrow(position, z_axis, [0.0, 0.0, 1.0, 1.0], 2) # Blue - - def publish_arrow(self, position, axis_vector, color, marker_id): - """Publish an arrow marker for a given axis.""" - marker = Marker() - marker.header.frame_id = self.base_frame - marker.header.stamp = rospy.Time.now() - marker.ns = "se3_trajectory" - marker.id = marker_id - marker.type = Marker.ARROW - marker.action = Marker.ADD - - # Arrow start and end points - marker.points = [] - start_point = Pose().position - start_point.x = position[0] - start_point.y = position[1] - start_point.z = position[2] - end_point = Pose().position - end_point.x = position[0] + self.arrow_scale*axis_vector[0] - end_point.y = position[1] + self.arrow_scale*axis_vector[1] - end_point.z = position[2] + self.arrow_scale*axis_vector[2] - marker.points.append(start_point) - marker.points.append(end_point) - - # Arrow properties - marker.scale.x = 0.02 # Shaft diameter - marker.scale.y = 0.05 # Head diameter - marker.scale.z = 0.05 # Head length - marker.color.r = color[0] - marker.color.g = color[1] - marker.color.b = color[2] - marker.color.a = color[3] - - self.marker_pub.publish(marker) - - def publish_pose(self, position, orientation): - """Publish the current goal position and orientation as a geometry_msgs/Pose message.""" - pose = PoseStamped() - pose.header.frame_id = self.base_frame - pose.header.stamp = rospy.Time.now() - pose.pose.position.x = position[0] - pose.pose.position.y = position[1] - pose.pose.position.z = position[2] - pose.pose.orientation.x = orientation[0] - pose.pose.orientation.y = orientation[1] - pose.pose.orientation.z = orientation[2] - pose.pose.orientation.w = orientation[3] - - self.pose_pub.publish(pose) - - def trajectory_loop(self): - """Main loop for trajectory generation and marker/pose publishing.""" - rate = rospy.Rate(50) # 50 Hz control loop - - while not rospy.is_shutdown(): - # Calculate elapsed time - elapsed_time = (rospy.Time.now() - self.start_time).to_sec() - - # Generate position and orientation - position = self.generate_position(elapsed_time) - orientation = self.generate_orientation(elapsed_time) - - # Publish the marker and pose - self.publish_marker(position, orientation) - self.publish_pose(position, orientation) - - # Sleep to maintain loop rate - rate.sleep() - - -if __name__ == '__main__': - try: - SE3Trajectory() - except rospy.ROSInterruptException: - rospy.loginfo("SE(3) trajectory generator node terminated.") \ No newline at end of file diff --git a/manipulator_control_ros1/scripts/spacemouse.py b/manipulator_control_ros1/scripts/spacemouse.py deleted file mode 100644 index 2adb36c..0000000 --- a/manipulator_control_ros1/scripts/spacemouse.py +++ /dev/null @@ -1,432 +0,0 @@ -"""Driver class for SpaceMouse controller. - -This class provides a driver support to SpaceMouse on macOS. -In particular, we assume you are using a SpaceMouse Wireless by default. - -To set up a new SpaceMouse controller: - 1. Download and install driver from https://www.3dconnexion.com/service/drivers.html - 2. Install hidapi library through pip - (make sure you run uninstall hid first if it is installed). - 3. Make sure SpaceMouse is connected before running the script - 4. (Optional) Based on the model of SpaceMouse, you might need to change the - vendor id and product id that correspond to the device. - -For Linux support, you can find open-source Linux drivers and SDKs online. - See http://spacenav.sourceforge.net/ - -""" - -import threading -import time -from collections import namedtuple - -import numpy as np -import math - -try: - import hid -except ModuleNotFoundError as exc: - raise ImportError( - "Unable to load module hid, required to interface with SpaceMouse. " - "Only macOS is officially supported. Install the additional " - "requirements with `pip install -r requirements-extra.txt`" - ) from exc - -from device import Device -#from robosuite.utils.transform_utils import rotation_matrix - -AxisSpec = namedtuple("AxisSpec", ["channel", "byte1", "byte2", "scale"]) - -SPACE_MOUSE_SPEC = { - "x": AxisSpec(channel=1, byte1=1, byte2=2, scale=1), - "y": AxisSpec(channel=1, byte1=3, byte2=4, scale=-1), - "z": AxisSpec(channel=1, byte1=5, byte2=6, scale=-1), - "roll": AxisSpec(channel=1, byte1=7, byte2=8, scale=-1), - "pitch": AxisSpec(channel=1, byte1=9, byte2=10, scale=-1), - "yaw": AxisSpec(channel=1, byte1=11, byte2=12, scale=1), -} - -def unit_vector(data, axis=None, out=None): - """ - Returns ndarray normalized by length, i.e. eucledian norm, along axis. - - E.g.: - >>> v0 = numpy.random.random(3) - >>> v1 = unit_vector(v0) - >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) - True - - >>> v0 = numpy.random.rand(5, 4, 3) - >>> v1 = unit_vector(v0, axis=-1) - >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2) - >>> numpy.allclose(v1, v2) - True - - >>> v1 = unit_vector(v0, axis=1) - >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1) - >>> numpy.allclose(v1, v2) - True - - >>> v1 = numpy.empty((5, 4, 3), dtype=numpy.float32) - >>> unit_vector(v0, axis=1, out=v1) - >>> numpy.allclose(v1, v2) - True - - >>> list(unit_vector([])) - [] - - >>> list(unit_vector([1.0])) - [1.0] - - Args: - data (np.array): data to normalize - axis (None or int): If specified, determines specific axis along data to normalize - out (None or np.array): If specified, will store computation in this variable - - Returns: - None or np.array: If @out is not specified, will return normalized vector. Otherwise, stores the output in @out - """ - if out is None: - data = np.array(data, dtype=np.float32, copy=True) - if data.ndim == 1: - data /= math.sqrt(np.dot(data, data)) - return data - else: - if out is not data: - out[:] = np.array(data, copy=False) - data = out - length = np.atleast_1d(np.sum(data * data, axis)) - np.sqrt(length, length) - if axis is not None: - length = np.expand_dims(length, axis) - data /= length - if out is None: - return data - -def rotation_matrix(angle, direction, point=None): - """ - Returns matrix to rotate about axis defined by point and direction. - - E.g.: - >>> angle = (random.random() - 0.5) * (2*math.pi) - >>> direc = numpy.random.random(3) - 0.5 - >>> point = numpy.random.random(3) - 0.5 - >>> R0 = rotation_matrix(angle, direc, point) - >>> R1 = rotation_matrix(angle-2*math.pi, direc, point) - >>> is_same_transform(R0, R1) - True - - >>> R0 = rotation_matrix(angle, direc, point) - >>> R1 = rotation_matrix(-angle, -direc, point) - >>> is_same_transform(R0, R1) - True - - >>> I = numpy.identity(4, numpy.float32) - >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc)) - True - - >>> numpy.allclose(2., numpy.trace(rotation_matrix(math.pi/2, - ... direc, point))) - True - - Args: - angle (float): Magnitude of rotation - direction (np.array): (ax,ay,az) axis about which to rotate - point (None or np.array): If specified, is the (x,y,z) point about which the rotation will occur - - Returns: - np.array: 4x4 homogeneous matrix that includes the desired rotation - """ - sina = math.sin(angle) - cosa = math.cos(angle) - direction = unit_vector(direction[:3]) - # rotation matrix around unit vector - R = np.array(((cosa, 0.0, 0.0), (0.0, cosa, 0.0), (0.0, 0.0, cosa)), dtype=np.float32) - R += np.outer(direction, direction) * (1.0 - cosa) - direction *= sina - R += np.array( - ( - (0.0, -direction[2], direction[1]), - (direction[2], 0.0, -direction[0]), - (-direction[1], direction[0], 0.0), - ), - dtype=np.float32, - ) - M = np.identity(4) - M[:3, :3] = R - if point is not None: - # rotation not around origin - point = np.array(point[:3], dtype=np.float32, copy=False) - M[:3, 3] = point - np.dot(R, point) - return M - -def to_int16(y1, y2): - """ - Convert two 8 bit bytes to a signed 16 bit integer. - - Args: - y1 (int): 8-bit byte - y2 (int): 8-bit byte - - Returns: - int: 16-bit integer - """ - x = (y1) | (y2 << 8) - if x >= 32768: - x = -(65536 - x) - return x - - -def scale_to_control(x, axis_scale=350.0, min_v=-1.0, max_v=1.0): - """ - Normalize raw HID readings to target range. - - Args: - x (int): Raw reading from HID - axis_scale (float): (Inverted) scaling factor for mapping raw input value - min_v (float): Minimum limit after scaling - max_v (float): Maximum limit after scaling - - Returns: - float: Clipped, scaled input from HID - """ - x = x / axis_scale - x = min(max(x, min_v), max_v) - return x - - -def convert(b1, b2): - """ - Converts SpaceMouse message to commands. - - Args: - b1 (int): 8-bit byte - b2 (int): 8-bit byte - - Returns: - float: Scaled value from Spacemouse message - """ - return scale_to_control(to_int16(b1, b2)) - - -class SpaceMouse(Device): - """ - A minimalistic driver class for SpaceMouse with HID library. - - Note: Use hid.enumerate() to view all USB human interface devices (HID). - Make sure SpaceMouse is detected before running the script. - You can look up its vendor/product id from this method. - - Args: - vendor_id (int): HID device vendor id - product_id (int): HID device product id - pos_sensitivity (float): Magnitude of input position command scaling - rot_sensitivity (float): Magnitude of scale input rotation commands scaling - """ - - def __init__( - self, - vendor_id=9583, - product_id= 50741, - pos_sensitivity=1.0, - rot_sensitivity=1.0, - ): - - print("Opening SpaceMouse device") - self.vendor_id = vendor_id - self.product_id = product_id - self.device = hid.device() - self.device.open(self.vendor_id, self.product_id) # SpaceMouse - - self.pos_sensitivity = pos_sensitivity - self.rot_sensitivity = rot_sensitivity - - print("Manufacturer: %s" % self.device.get_manufacturer_string()) - print("Product: %s" % self.device.get_product_string()) - - # 6-DOF variables - self.x, self.y, self.z = 0, 0, 0 - self.roll, self.pitch, self.yaw = 0, 0, 0 - - self._display_controls() - - self.single_click_and_hold = False - - self._control = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - self._reset_state = 0 - self.rotation = np.array([[-1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, -1.0]]) - self._enabled = False - - # launch a new listener thread to listen to SpaceMouse - self.thread = threading.Thread(target=self.run) - self.thread.daemon = True - self.thread.start() - - @staticmethod - def _display_controls(): - """ - Method to pretty print controls. - """ - - def print_command(char, info): - char += " " * (30 - len(char)) - print("{}\t{}".format(char, info)) - - print("") - print_command("Control", "Command") - print_command("Right button", "reset simulation") - print_command("Left button (hold)", "close gripper") - print_command("Move mouse laterally", "move arm horizontally in x-y plane") - print_command("Move mouse vertically", "move arm vertically") - print_command("Twist mouse about an axis", "rotate arm about a corresponding axis") - print("") - - def _reset_internal_state(self): - """ - Resets internal state of controller, except for the reset signal. - """ - self.rotation = np.array([[-1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, -1.0]]) - # Reset 6-DOF variables - self.x, self.y, self.z = 0, 0, 0 - self.roll, self.pitch, self.yaw = 0, 0, 0 - # Reset control - self._control = np.zeros(6) - # Reset grasp - self.single_click_and_hold = False - - def start_control(self): - """ - Method that should be called externally before controller can - start receiving commands. - """ - self._reset_internal_state() - self._reset_state = 0 - self._enabled = True - - def get_controller_state(self): - """ - Grabs the current state of the 3D mouse. - - Returns: - dict: A dictionary containing dpos, orn, unmodified orn, grasp, and reset - """ - dpos = self.control[:3] * 0.005 * self.pos_sensitivity - roll, pitch, yaw = self.control[3:] * 0.005 * self.rot_sensitivity - - # convert RPY to an absolute orientation - drot1 = rotation_matrix(angle=-pitch, direction=[1.0, 0, 0], point=None)[:3, :3] - drot2 = rotation_matrix(angle=roll, direction=[0, 1.0, 0], point=None)[:3, :3] - drot3 = rotation_matrix(angle=yaw, direction=[0, 0, 1.0], point=None)[:3, :3] - - self.rotation = self.rotation.dot(drot1.dot(drot2.dot(drot3))) - - return dict( - dpos=dpos, - rotation=self.rotation, - raw_drotation=np.array([roll, pitch, yaw]), - grasp=self.control_gripper, - reset=self._reset_state, - ) - - def run(self): - """Listener method that keeps pulling new messages.""" - - t_last_click = -1 - - while True: - d = self.device.read(13) - if d is not None and self._enabled: - - if self.product_id == 50741: - ## logic for older spacemouse model - - if d[0] == 1: ## readings from 6-DoF sensor - self.y = convert(d[1], d[2]) - self.x = convert(d[3], d[4]) - self.z = convert(d[5], d[6]) * -1.0 - - elif d[0] == 2: - - self.roll = convert(d[1], d[2]) - self.pitch = convert(d[3], d[4]) - self.yaw = convert(d[5], d[6]) - - self._control = [ - self.x, - self.y, - self.z, - self.roll, - self.pitch, - self.yaw, - ] - else: - ## default logic for all other spacemouse models - - if d[0] == 1: ## readings from 6-DoF sensor - self.y = convert(d[1], d[2]) - self.x = convert(d[3], d[4]) - self.z = convert(d[5], d[6]) * -1.0 - - self.roll = convert(d[7], d[8]) - self.pitch = convert(d[9], d[10]) - self.yaw = convert(d[11], d[12]) - - self._control = [ - self.x, - self.y, - self.z, - self.roll, - self.pitch, - self.yaw, - ] - - if d[0] == 3: ## readings from the side buttons - - # press left button - if d[1] == 1: - t_click = time.time() - elapsed_time = t_click - t_last_click - t_last_click = t_click - self.single_click_and_hold = True - - # release left button - if d[1] == 0: - self.single_click_and_hold = False - - # right button is for reset - if d[1] == 2: - self._reset_state = 1 - self._enabled = False - self._reset_internal_state() - - @property - def control(self): - """ - Grabs current pose of Spacemouse - - Returns: - np.array: 6-DoF control value - """ - return np.array(self._control) - - @property - def control_gripper(self): - """ - Maps internal states into gripper commands. - - Returns: - float: Whether we're using single click and hold or not - """ - if self.single_click_and_hold: - return 1.0 - return 0 - - -if __name__ == "__main__": - - space_mouse = SpaceMouse() - print(space_mouse.get_controller_state()) - - while True: - time.sleep(1) - print(space_mouse.get_controller_state()) \ No newline at end of file diff --git a/manipulator_control_ros1/scripts/xarm_spacemouse.py b/manipulator_control_ros1/scripts/xarm_spacemouse.py deleted file mode 100644 index 18ef922..0000000 --- a/manipulator_control_ros1/scripts/xarm_spacemouse.py +++ /dev/null @@ -1,172 +0,0 @@ -#!/usr/bin/env python -import rospy -from spacemouse import SpaceMouse -from input2action import input2action -import numpy as np - -from sensor_msgs.msg import JointState - -from xarm.wrapper import XArmAPI -from std_msgs.msg import Bool -from geometry_msgs.msg import TwistStamped, PoseStamped -from scipy.spatial.transform import Rotation - -def euler_to_quaternion(roll, pitch, yaw): - rot = Rotation.from_euler('xyz', [roll, pitch, yaw]) - return rot.as_quat() - -class Spacemouse2Xarm: - def __init__(self): - rospy.init_node('spacemouse2xarm') - ip = rospy.get_param('~robot_ip', '192.168.1.199') - - self.arm = XArmAPI(ip) - self.arm.motion_enable(enable=True) - self.arm.set_mode(1) # mode 1: servoj mode (can do cartesian or joint control), mode 0: position control mode, mode 2: joint teaching mode, mode 3: does notexist lol, mode 4: joint velocity control mode, mode 5: cartesian velocity control mode, mode 6: joint online trajectory planning mode, mode 7: cartesian online trajectory planning mode - self.arm.set_state(0) # start 0: start motion, state 1: in motion, state 2: standby, state 3: paused state, state 4: stop state, state 5: system reset, state 6: stop - self.device = SpaceMouse(pos_sensitivity=1.0, rot_sensitivity=1.0) - self.device.start_control() - self.locked = False - self.last_grasp_press_time = rospy.Time.now().to_nsec() - - # publisher for pose and twist action to send to JParse controller - self.action_pub = rospy.Publisher('robot_action', TwistStamped, queue_size=10) - self.position_action_pub = rospy.Publisher('robot_position_action', PoseStamped, queue_size=10) - self.reset_sub = rospy.Subscriber('reset_xarm', Bool, self.reset_callback) - self.is_resetting = False - self.lock_hand = rospy.Publisher('lock_hand', Bool, queue_size=10) - - self.timer = rospy.Timer(rospy.Duration(1/30.0), self.timer_callback) - - # reset - self.is_resetting = True - self.arm.motion_enable(enable=True) - self.arm.set_mode(0) # mode 0: position control mode - self.arm.set_state(0) # start 0: start motion - self.arm.set_position(x=331, y=20.3, z=308, roll=173, pitch=0, yaw=0, - speed=100, is_radian=False, wait=True) - self.arm.motion_enable(enable=True) - - self.arm.set_mode(5) # mode 5: cartesian velocity control mode - self.arm.set_state(0) # start 0: start motion - self.is_resetting = False - - - self.joint_states_pub = rospy.Publisher('/joint_states', JointState, queue_size=10) - - self.use_native_xarm_spacemouse = rospy.get_param('~use_native_xarm_spacemouse', False) - - - - def reset_callback(self, msg): - """ - a reset callback function to reset the robot to a predefined position - """ - if msg.data: - self.is_resetting = True - self.arm.motion_enable(enable=True) - self.arm.set_mode(0) - self.arm.set_state(0) - self.arm.set_position(x=331, y=20.3, z=308, roll=173, pitch=0, yaw=0, - speed=100, is_radian=False, wait=True) - self.arm.motion_enable(enable=True) - self.arm.set_mode(1) - self.arm.set_state(0) - self.is_resetting = False - - def action_to_cartesian_velocity(self, actions): - """ - converts space mouse actions to cartesian velocities - """ - if actions is None: - return 0, 0, 0, 0, 0, 0 - - dt = 1000 / 30.0 - scale = 100 - ang_scale = 1 - vx = -scale * actions[0] / dt - vy = -scale * actions[1] / dt - vz = scale * actions[2] / dt - wx = -ang_scale * actions[3] - wy = -ang_scale * actions[4] - wz = ang_scale * actions[5] - return vx, vy, vz, wx, wy, wz - - def timer_callback(self, event): - cur_pose = self.arm.get_position()[1] - cur_pose = np.array(cur_pose) - actions, grasp = input2action(device=self.device, robot="xArm") - - if grasp is None: - grasp = 0 - - grasp = 850 - 860 * grasp - - # get joint states - status, joint_states = self.arm.get_joint_states(is_radian=True) - joint_pos = joint_states[0] - joint_vel = joint_states[1] - joint_torque = joint_states[2] - - - vx, vy, vz, wx, wy, wz = self.action_to_cartesian_velocity(actions) - cur_pose = cur_pose + np.array([vx, vy, vz, wx, wy, wz]) - action_pose_msg = PoseStamped() - action_pose_msg.pose.position.x = cur_pose[0] - action_pose_msg.pose.position.y = cur_pose[1] - action_pose_msg.pose.position.z = cur_pose[2] - roll, pitch, yaw = cur_pose[3], cur_pose[4], cur_pose[5] - q = euler_to_quaternion(roll, pitch, yaw) - action_pose_msg.pose.orientation.x = q[0] - action_pose_msg.pose.orientation.y = q[1] - action_pose_msg.pose.orientation.z = q[2] - action_pose_msg.pose.orientation.w = q[3] - action_pose_msg.header.stamp = rospy.Time.now() - - self.position_action_pub.publish(action_pose_msg) - twist_msg = TwistStamped() - twist_msg.twist.linear.x = vx - twist_msg.twist.linear.y = vy - twist_msg.twist.linear.z = vz - twist_msg.twist.angular.x = wx - twist_msg.twist.angular.y = wy - twist_msg.twist.angular.z = wz - twist_msg.header.stamp = rospy.Time.now() - - - position_velocity = np.array([vx, vy, vz]) - position_velocity_norm = np.linalg.norm(position_velocity) - if position_velocity_norm > 0.05: - position_velocity = position_velocity / (position_velocity_norm * 0.05) - - vx, vy, vz = position_velocity - - self.action_pub.publish(twist_msg) - current_time_ns = rospy.Time.now().to_nsec() - - - # grasp control - if grasp == 1: - if (current_time_ns - self.last_grasp_press_time) > 5e8: - self.last_grasp_press_time = current_time_ns - bool_msg = Bool() - bool_msg.data = not self.locked - self.locked = bool_msg.data - self.lock_hand.publish(bool_msg) - if not self.is_resetting: - v_scaling = 1000 - vx = vx * v_scaling - vy = vy * v_scaling - vz = vz * v_scaling - if self.use_native_xarm_spacemouse: - # use the native xarm cartesian velocity controller. - self.arm.vc_set_cartesian_velocity([vx, vy, vz, wx, wy, wz], is_radian=True) - ret_grip = self.arm.set_gripper_position(grasp) - - def spin(self): - rospy.spin() - self.arm.disconnect() - -if __name__ == '__main__': - node = Spacemouse2Xarm() - node.spin() \ No newline at end of file diff --git a/manipulator_control_ros1/scripts/xarm_vel_main_experiment.py b/manipulator_control_ros1/scripts/xarm_vel_main_experiment.py deleted file mode 100755 index 596171d..0000000 --- a/manipulator_control_ros1/scripts/xarm_vel_main_experiment.py +++ /dev/null @@ -1,571 +0,0 @@ -#!/usr/bin/env python3 -import sys -import rospy -import numpy as np -import math -import scipy -import time - -import PyKDL as kdl -from urdf_parser_py.urdf import Robot -from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model -from pykdl_utils.kdl_kinematics import KDLKinematics -from pykdl_utils.kdl_kinematics import kdl_to_mat -from pykdl_utils.kdl_kinematics import joint_kdl_to_list -from pykdl_utils.kdl_kinematics import joint_list_to_kdl - -import geometry_msgs.msg -from geometry_msgs.msg import Pose, PoseStamped, Vector3, TwistStamped -from std_msgs.msg import Float64MultiArray, Float64 -from sensor_msgs.msg import JointState -import tf -from tf import TransformerROS -import tf2_ros -from tf.transformations import quaternion_from_euler, quaternion_matrix - -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from sensor_msgs.msg import JointState - -try: - from xarm.wrapper import XArmAPI -except ImportError: - print("xarm package not installed, skipping xarm import") - -from manipulator_control import jparse_cls - -class ArmController: - def __init__(self): - rospy.init_node('arm_controller', anonymous=True) - - self.base_link = rospy.get_param('/base_link_name', 'link_base') #defaults are for Kinova gen3 - self.end_link = rospy.get_param('/end_link_name', 'link_eef') - - self.is_sim = rospy.get_param('~is_sim', False) #boolean to control if the robot is in simulation or not - - self.phi_gain = rospy.get_param('~phi_gain', 10) #gain for the phi term in the JParse method - self.phi_gain_position = rospy.get_param('~phi_gain_position', 15) #gain for the phi term in the JParse method - self.phi_gain_angular = rospy.get_param('~phi_gain_angular', 15) #gain for the phi term in the JParse method - - self.jparse_gamma = rospy.get_param('~jparse_gamma', 0.1) #gamma for the JParse method - - self.use_space_mouse = rospy.get_param('~use_space_mouse', False) #boolean to control if the space mouse is used or not - self.use_space_mouse_jparse = rospy.get_param('~use_space_mouse_jparse', False) #boolean to control if the space mouse is used or not - - self.space_mouse_command = np.array([0,0,0,0,0,0]).T - - if self.is_sim == False: - # from IPython import embed; embed(banner1="hello in not sim") - self.robot_ip = rospy.get_param('~robot_ip', '192.168.1.199 ') - self.arm = XArmAPI(self.robot_ip) - self.arm.motion_enable(enable=True) - self.arm.set_mode(0) - self.arm.set_state(state=0) - time.sleep(1) - - # set joint velocity control mode - self.arm.set_mode(4) - self.arm.set_state(0) - time.sleep(1) - - #Set parameters - self.position_only = rospy.get_param('~position_only', False) #boolean to control only position versus full pose - - #choose the method to use - self.method = rospy.get_param('~method', 'JParse') #options are JParse, JacobianPseudoInverse, JacobianDampedLeastSquares, JacobianProjection, JacobianDynamicallyConsistentInverse - - self.show_jparse_ellipses = rospy.get_param('~show_jparse_ellipses', False) #boolean to control if the jparse ellipses are shown or not - - self.joint_states = None - self.target_pose = None - self.space_mouse_command = None # added this 7/11/25 - self.jacobian_calculator = jparse_cls.JParseClass(base_link=self.base_link, end_link=self.end_link) - - #get tf listener - self.tfBuffer = tf2_ros.Buffer() - self.listener = tf2_ros.TransformListener(self.tfBuffer) - self.tfros = tf.TransformerROS() - self.tf_timeout = rospy.Duration(1.0) - - if self.is_sim == True: - #if we are in simulation, use the joint_states and target_pose topics - joint_states_topic = '/xarm/joint_states' - else: - joint_states_topic = '/joint_states' - - rospy.Subscriber(joint_states_topic, JointState, self.joint_states_callback) - rospy.Subscriber('/target_pose', PoseStamped, self.target_pose_callback) - rospy.Subscriber('/robot_action', TwistStamped, self.space_mouse_callback) - - - self.manip_measure_pub = rospy.Publisher('/manip_measure', Float64, queue_size=10) - self.inverse_cond_number = rospy.Publisher('/inverse_cond_number', Float64, queue_size=10) - #have certain messages to store raw error - self.pose_error_pub = rospy.Publisher('/pose_error', PoseStamped, queue_size=10) - self.position_error_pub = rospy.Publisher('/position_error', Vector3, queue_size=10) - self.orientation_error_pub = rospy.Publisher('/orientation_error', Vector3, queue_size=10) - #have certain messages to store control error - self.pose_error_control_pub = rospy.Publisher('/pose_error_control', PoseStamped, queue_size=10) - self.position_error_control_pub = rospy.Publisher('/position_error_control', Vector3, queue_size=10) - self.orientation_control_error_pub = rospy.Publisher('/orientation_error_control', Vector3, queue_size=10) - #publish the current end effector pose and target pose - self.current_end_effector_pose_pub = rospy.Publisher('/current_end_effector_pose', PoseStamped, queue_size=10) - self.current_target_pose_pub = rospy.Publisher('/current_target_pose', PoseStamped, queue_size=10) - - joint_command_topic = rospy.get_param('~joint_command_topic', '/xarm/xarm7_velo_traj_controller/command') - self.joint_vel_pub = rospy.Publisher(joint_command_topic, JointTrajectory, queue_size=10) - # Define the joint names - self.joint_names = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7"] - # Initialize the current positions - self.current_positions = [0.0] * len(self.joint_names) - - self.rate = rospy.Rate(50) # 10 Hz - #home the robot - try: - self.velocity_home_robot() - except rospy.ROSInterruptException: - rospy.loginfo("ROS Interrupt Exception") - pass - finally: - # Clean up resources if needed - rospy.loginfo("Shutting down the control loop") - joint_zero_velocities = [0.0] * len(self.joint_names) - self.command_joint_velocities(joint_zero_velocities) - #now run the control loop - self.control_loop() - - def rad2deg(self, q): - return q/math.pi*180.0 - - def joint_states_callback(self, msg): - """ - Callback function for the joint_states topic. This function will be called whenever a new message is received - on the joint_states topic. The message is a sensor_msgs/JointState message, which contains the current joint - and the corresponding joint velocities and efforts. This function will extract the joint positions, velocities, - and efforts and return them as lists. - """ - self.joint_states = msg - - def space_mouse_callback(self, msg): - """ - Callback function for the space_mouse topic. This function will be called whenever a new message is received - on the space_mouse topic. The message is a geometry_msgs/TwistStamped message, which contains the current - velocity of the end effector. This function will extract the linear and angular velocities and return them as - lists. - """ - space_mouse_command = np.array([msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z, msg.twist.angular.x, msg.twist.angular.y, msg.twist.angular.z]) - position_velocity = space_mouse_command[:3] - angular_velocity = space_mouse_command[3:] - position_velocity_norm = np.linalg.norm(position_velocity) - angular_velocity_norm = np.linalg.norm(angular_velocity) - if position_velocity_norm > 0.05: - position_velocity = position_velocity / position_velocity_norm * 0.05 - self.space_mouse_command = np.array([position_velocity[0], position_velocity[1],position_velocity[2],angular_velocity[0],angular_velocity[1],angular_velocity[2]]) #check if norm of the space mouse command is greater than 0.05, if so normalize it to this value - - #ensure we can get into the while loop - if self.use_space_mouse == True: - self.target_pose = PoseStamped() #set this to get in a loop - - - def target_pose_callback(self, msg): - """ - Callback function for the target_pose topic. This function will be called whenever a new message is received - on the target_pose topic. The message is a geometry_msgs/PoseStamped message, which contains the target pose - """ - self.target_pose = msg - self.current_target_pose_pub.publish(self.target_pose) #if target pose is paused manually, this allows us to track the current target pose seen by the script - - def EndEffectorPose(self, q): - """ - This function computes the end-effector pose given the joint configuration q. - """ - current_pose = PoseStamped() - current_pose.header.frame_id = self.base_link - current_pose.header.stamp = rospy.Time.now() - try: - trans = self.tfBuffer.lookup_transform(self.base_link, self.end_link, rospy.Time(), timeout=self.tf_timeout) - current_pose.pose.position.x = trans.transform.translation.x - current_pose.pose.position.y = trans.transform.translation.y - current_pose.pose.position.z = trans.transform.translation.z - current_pose.pose.orientation.x = trans.transform.rotation.x - current_pose.pose.orientation.y = trans.transform.rotation.y - current_pose.pose.orientation.z = trans.transform.rotation.z - current_pose.pose.orientation.w = trans.transform.rotation.w - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): - rospy.logerr("TF lookup failed") - rospy.logerr("failed to lookup transform from %s to %s", self.base_link, self.end_link) - self.current_end_effector_pose_pub.publish(current_pose) - return current_pose - - def EndEffectorVelocity(self, q, dq): - """ - This function computes the end-effector velocity given the joint configuration q and joint velocities dq. - """ - J = self.jacobian_calculator.jacobian(q) - J = np.array(J) - dq = np.array(dq) - dx = np.dot(J, dq) - return dx - - def rotation_matrix_to_axis_angle(self,R): - """ - Converts a rotation matrix to an axis-angle vector. - - Parameters: - R (numpy.ndarray): A 3x3 rotation matrix. - - Returns: - numpy.ndarray: Axis-angle vector (3 elements). - """ - if not (R.shape == (3, 3) and np.allclose(np.dot(R.T, R), np.eye(3)) and np.isclose(np.linalg.det(R), 1)): - raise ValueError("Input matrix must be a valid rotation matrix.") - - # Calculate the angle of rotation - angle = np.arccos((np.trace(R) - 1) / 2) - - if np.isclose(angle, 0): # No rotation - return np.zeros(3) - - if np.isclose(angle, np.pi): # 180 degrees rotation - # Special case for 180-degree rotation - # Extract the axis from the diagonal of R - axis = np.sqrt((np.diag(R) + 1) / 2) - # Adjust signs based on the matrix off-diagonals - axis[0] *= np.sign(R[2, 1] - R[1, 2]) - axis[1] *= np.sign(R[0, 2] - R[2, 0]) - axis[2] *= np.sign(R[1, 0] - R[0, 1]) - return axis * angle - - # General case - axis = np.array([ - R[2, 1] - R[1, 2], - R[0, 2] - R[2, 0], - R[1, 0] - R[0, 1] - ]) / (2 * np.sin(angle)) - - return axis * angle - - def pose_error(self, current_pose, target_pose, error_norm_max = 0.05, control_pose_error = True): - """ - This function computes the error between the current pose and the target pose. - """ - #Compute the position error - position_error = np.array([target_pose.pose.position.x - current_pose.pose.position.x, - target_pose.pose.position.y - current_pose.pose.position.y, - target_pose.pose.position.z - current_pose.pose.position.z]) - #if the norm of the position error is greater than the maximum allowed, scale it down - if control_pose_error == True: - if np.linalg.norm(position_error) > error_norm_max: - position_error = position_error / np.linalg.norm(position_error) * error_norm_max - - #convert the quaternion in posestamped to list of quaternions then pass this into quaternion_matrix to get the rotation matrix. Access only the rotation part of the matrix - goal_rotation_matrix = quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])[:3,:3] - current_rotation_matrix = quaternion_matrix([current_pose.pose.orientation.x, current_pose.pose.orientation.y, current_pose.pose.orientation.z, current_pose.pose.orientation.w])[:3,:3] - #Compute the orientation error - R_error= np.dot(goal_rotation_matrix, np.linalg.inv(current_rotation_matrix)) - # Extract the axis-angle lie algebra vector from the rotation matrix - angle_error = self.rotation_matrix_to_axis_angle(R_error) - # Return the position and orientation error - return position_error, angle_error - - def axis_angle_to_rotation_matrix(self, axis_angle): - """ - Converts an axis-angle vector to a rotation matrix. - - Parameters: - axis_angle (numpy.ndarray): Axis-angle vector (3 elements). - - Returns: - numpy.ndarray: A 3x3 rotation matrix. - """ - # Extract the axis and angle - axis = axis_angle / np.linalg.norm(axis_angle) - angle = np.linalg.norm(axis_angle) - - # Compute the skew-symmetric matrix - K = np.array([ - [0, -axis[2], axis[1]], - [axis[2], 0, -axis[0]], - [-axis[1], axis[0], 0] - ]) - - # Compute the rotation matrix using the Rodrigues' formula - R = np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * np.dot(K, K) - - return R - - def rotation_matrix_to_quaternion(self, R): - """ - Converts a rotation matrix to a quaternion. - - Parameters: - R (numpy.ndarray): A 3x3 rotation matrix. - - Returns: - numpy.ndarray: A 4-element quaternion. - """ - if not (R.shape == (3, 3) and np.allclose(np.dot(R.T, R), np.eye(3)) and np.isclose(np.linalg.det(R), 1)): - raise ValueError("Input matrix must be a valid rotation matrix.") - - # Compute the quaternion using the method from the book - q = np.zeros(4) - q[0] = np.sqrt(1 + R[0, 0] + R[1, 1] + R[2, 2]) / 2 - q[1] = (R[2, 1] - R[1, 2]) / (4 * q[0]) - q[2] = (R[0, 2] - R[2, 0]) / (4 * q[0]) - q[3] = (R[1, 0] - R[0, 1]) / (4 * q[0]) - - return q - - def publish_pose_errors(self, position_error, angular_error, control_pose_error = True): - """ - Publishes the position and orientation errors as ROS messages. - """ - pose_error_msg = PoseStamped() - pose_error_msg.header.frame_id = self.base_link - pose_error_msg.header.stamp = rospy.Time.now() - pose_error_msg.pose.position.x = position_error[0] - pose_error_msg.pose.position.y = position_error[1] - pose_error_msg.pose.position.z = position_error[2] - #for completeness, convert the axis angle to quaternion - Rmat = self.axis_angle_to_rotation_matrix(angular_error) - quat_from_Rmat = self.rotation_matrix_to_quaternion(Rmat) - # quat_from_Rmat = tf.transformations.quaternion_from_matrix(Rmat) - pose_error_msg.pose.orientation.x = quat_from_Rmat[0] - pose_error_msg.pose.orientation.y = quat_from_Rmat[1] - pose_error_msg.pose.orientation.z = quat_from_Rmat[2] - pose_error_msg.pose.orientation.w = quat_from_Rmat[3] - #publish the axis-angle orientation error - orientation_error_msg = Vector3() - orientation_error_msg.x = angular_error[0] - orientation_error_msg.y = angular_error[1] - orientation_error_msg.z = angular_error[2] - #publish the position error - position_error_msg = Vector3() - position_error_msg.x = position_error[0] - position_error_msg.y = position_error[1] - position_error_msg.z = position_error[2] - - #Now publish - if control_pose_error == True: - self.position_error_control_pub.publish(position_error_msg) - self.pose_error_control_pub.publish(pose_error_msg) - self.orientation_control_error_pub.publish(orientation_error_msg) - else: - self.pose_error_pub.publish(pose_error_msg) - self.position_error_pub.publish(position_error_msg) - self.orientation_error_pub.publish(orientation_error_msg) - - - def control_loop(self): - """ - This function implements the control loop for the arm controller. - """ - - while not rospy.is_shutdown(): - - if self.joint_states is not None and self.space_mouse_command is not None: # edited this 7/11/25 - t = rospy.Time.now() - # obtain the current joints - q = [] - dq = [] - effort = [] - for joint_name in self.joint_names: - idx = self.joint_states.name.index(joint_name) - q.append(self.joint_states.position[idx]) - dq.append(self.joint_states.velocity[idx]) - effort.append(self.joint_states.effort[idx]) - self.current_positions = q - # Calculate the JParsed Jacobian - - method = self.method #set by parameter, can be set from launch file - rospy.loginfo("Method being used: %s", method) - if method == "JacobianPseudoInverse": - #this is the traditional pseudo-inverse method for the jacobian - J_method, J_nullspace = self.jacobian_calculator.JacobianPseudoInverse(q=q, position_only=self.position_only, jac_nullspace_bool=True) - elif method == "JParse": - # The JParse method takes in the joint angles, gamma, position_only, and singular_direction_gain - if self.show_jparse_ellipses == True: - J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position, singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True, publish_jparse_ellipses=True, end_effector_pose=self.EndEffectorPose(q), verbose=False) - else: - J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position,singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True) - elif method == "JacobianDampedLeastSquares": - J_method, J_nullspace = self.jacobian_calculator.jacobian_damped_least_squares(q=q, damping=0.1, jac_nullspace_bool=True) #dampening of 0.1 works very well, 0.8 shows clear error - elif method == "JacobianProjection": - J_proj, J_nullspace = self.jacobian_calculator.jacobian_projection(q=q, gamma=0.1, jac_nullspace_bool=True) - J_method = np.linalg.pinv(J_proj) - elif method == "JacobianSafety": - # The JParse method takes in the joint angles, gamma, position_only, and singular_direction_gain - if self.show_jparse_ellipses == True: - J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position, singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True, publish_jparse_ellipses=True, end_effector_pose=self.EndEffectorPose(q), verbose=False, safety_only=True) - else: - J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position,singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True, safety_only=True) - elif method == "JacobianSafetyProjection": - # The JParse method takes in the joint angles, gamma, position_only, and singular_direction_gain - if self.show_jparse_ellipses == True: - J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position, singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True, publish_jparse_ellipses=True, end_effector_pose=self.EndEffectorPose(q), verbose=False, safety_projection_only=True) - else: - J_method, J_nullspace = self.jacobian_calculator.JParse(q=q, gamma=self.jparse_gamma, position_only=self.position_only, singular_direction_gain_position=self.phi_gain_position,singular_direction_gain_angular=self.phi_gain_angular, jac_nullspace_bool=True, safety_projection_only=True) - - - manip_measure = self.jacobian_calculator.manipulability_measure(q) - self.manip_measure_pub.publish(manip_measure) - inverse_cond_number = self.jacobian_calculator.inverse_condition_number(q) - self.inverse_cond_number.publish(inverse_cond_number) - rospy.loginfo("Manipulability measure: %f", manip_measure) - rospy.loginfo("Inverse condition number: %f", inverse_cond_number) - #Calculate the delta_x (task space error) - target_pose = self.target_pose - current_pose = self.EndEffectorPose(q) - position_error, angular_error = self.pose_error(current_pose, target_pose, control_pose_error=False) - # log the pose error - self.publish_pose_errors(position_error, angular_error, control_pose_error=False) - #now find error for control (max error norm) - if self.is_sim == False: - #real world limits - error_norm_max = 0.10 - else: - #simulation limits - error_norm_max = 1.0 - position_error, angular_error = self.pose_error(current_pose, target_pose, error_norm_max = error_norm_max, control_pose_error=True) - #move in nullspace towards nominal pose (for xarm its 0 joint angle for Xarm7) - if self.is_sim == True: - kp_gain = 2.0 - else: - # kp_gain = 1.0 - kp_gain = 3.0 - - if self.is_sim == True: - nominal_motion_nullspace = np.matrix([-v*kp_gain for v in q]).T #send to home which is 0 joint position for all joints - else: - # nominal motion nullspace which checks if q magnitude is below threshold and chooses the minimum - null_space_angle_rate = 0.6 - - nominal_motion_nullspace = np.matrix([np.sign(-v*kp_gain)*np.min([np.abs(-v*kp_gain),null_space_angle_rate]) for v in q]).T #send to home which is 0 joint position for all joints - # log the pose error - self.publish_pose_errors(position_error, angular_error, control_pose_error=True) - - # Calculate and command the joint velocities - if self.position_only == True: - rospy.loginfo("Position only control") - position_error = np.matrix(position_error).T - if self.is_sim == True: - #use these gains only in simulation! - kp_gain = 10.0 - else: - kp_gain = 3.0 - Kp = np.diag([kp_gain, kp_gain, kp_gain]) # Proportional gain matrix - - task_vector = Kp @ position_error - joint_velocities = J_method @ task_vector + J_nullspace @ nominal_motion_nullspace - else: - # realworld gains (tested) - kp_gain_position = 1.0 - kp_gain_orientation = 1.0 - if self.is_sim == True: - #use these gains only in simulation! - kp_gain_position = 10.0 - kp_gain_orientation = 10.0 - Kp_position = np.diag([kp_gain_position, kp_gain_position, kp_gain_position]) # Proportional gain matrix - Kp_orientation = np.diag([kp_gain_orientation, kp_gain_orientation, kp_gain_orientation]) - task_position = Kp_position @ np.matrix(position_error).T - task_orientation = Kp_orientation @ np.matrix(angular_error).T - full_pose_error = np.matrix(np.concatenate((task_position.T, task_orientation.T),axis=1)).T - if self.use_space_mouse == False: - joint_velocities = J_method @ full_pose_error + J_nullspace @ nominal_motion_nullspace - if self.use_space_mouse == True: - #use the space mouse command to control the joint velocities - space_mouse_command = np.matrix(self.space_mouse_command).T - #now add this to the joint velocities - joint_velocities = J_method @ space_mouse_command + J_nullspace @ nominal_motion_nullspace - #check this - joint_velocities_list = np.array(joint_velocities).flatten().tolist() - # command the joint velocities - self.command_joint_velocities(joint_velocities_list) #this commands the joint velocities - self.rate.sleep() - rospy.loginfo("Control loop running") - - - def velocity_home_robot(self): - """ - This function commands the robot to the home position using joint velocities. - """ - # do the following in a loop for 5 seconds: - t_start = rospy.Time.now() - duration = 0.0 - while not rospy.is_shutdown(): - if duration >= 5.0: - break - rospy.loginfo("Homing the robot") - if self.joint_states: - duration = (rospy.Time.now() - t_start).to_sec() - q = [] - dq = [] - for joint_name in self.joint_names: - idx = self.joint_states.name.index(joint_name) - q.append(self.joint_states.position[idx]) - dq.append(self.joint_states.velocity[idx]) - self.current_positions = q - # Command the joint velocities - if self.is_sim == True: - kp_gain = 10.0 - else: - kp_gain = 0.5 - q_home = [-0.03142359480261803, -0.5166178941726685, 0.12042707949876785, 0.9197863936424255, -0.03142168000340462, 1.4172008037567139, 0.03490765020251274] - #now find the error between the current position and the home position and use joint velocities to move towards the home position - joint_velocities_list = kp_gain * (np.array(q_home) - np.array(q)) - - self.command_joint_velocities(joint_velocities_list) - - - def command_joint_velocities(self, joint_vel_list): - """ - This function commands the joint velocities to the robot using the appropriate ROS message type. - """ - if self.is_sim: - # Create the JointTrajectory message - trajectory_msg = JointTrajectory() - trajectory_msg.joint_names = self.joint_names - - # Create a trajectory point - point = JointTrajectoryPoint() - - # Use current positions - point.positions = self.current_positions - - # Set velocities - #make velocity negative because Xarm has cw positive direction for joint velocities - joint_vel_list = [-v for v in joint_vel_list] - point.velocities = joint_vel_list #this is negative because Xarm has cw positive direction for joint velocities - - # Set accelerations to zero - point.accelerations = [0.0] * len(self.joint_names) - - # Effort (optional; set to None or skip if not needed) - point.effort = [0.0] * len(self.joint_names) - - # Set the time from start - point.time_from_start = rospy.Duration(0.1) # Duration in seconds - - # Add the point to the trajectory - trajectory_msg.points.append(point) - - # Publish the trajectory - trajectory_msg.header.stamp = rospy.Time.now() # Update timestamp - self.joint_vel_pub.publish(trajectory_msg) - else: - # this is on the real robot, directly send joint velociteies - # Send joint velocities to the arm - # log the velocities - rospy.loginfo("Joint velocities: %s", joint_vel_list) - if self.use_space_mouse_jparse: - self.arm.vc_set_joint_velocity(joint_vel_list, is_radian=True) - # elif self.use_keyboard_jparse: - # rospy.loginfo("hello using the keyboard jparse!") - # # self.arm.vc_set_joint_velocity(joint_vel_list, is_radian=True) # commenting out for now to test - - -if __name__ == '__main__': - try: - ArmController() - except rospy.ROSInterruptException: - pass - diff --git a/manipulator_control_ros1/scripts/xarm_vel_python_using_cpp.py b/manipulator_control_ros1/scripts/xarm_vel_python_using_cpp.py deleted file mode 100755 index dc3cc1a..0000000 --- a/manipulator_control_ros1/scripts/xarm_vel_python_using_cpp.py +++ /dev/null @@ -1,603 +0,0 @@ -#!/usr/bin/env python3 -import sys -import rospy -import numpy as np -import math -import scipy -import time - -import PyKDL as kdl -from urdf_parser_py.urdf import Robot -from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model -from pykdl_utils.kdl_kinematics import KDLKinematics -from pykdl_utils.kdl_kinematics import kdl_to_mat -from pykdl_utils.kdl_kinematics import joint_kdl_to_list -from pykdl_utils.kdl_kinematics import joint_list_to_kdl - -import geometry_msgs.msg -from geometry_msgs.msg import Pose, PoseStamped, Vector3, TwistStamped -from std_msgs.msg import Float64MultiArray, Float64 -from sensor_msgs.msg import JointState -import tf -from tf import TransformerROS -import tf2_ros -from tf.transformations import quaternion_from_euler, quaternion_matrix - -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from sensor_msgs.msg import JointState - -try: - from xarm.wrapper import XArmAPI -except ImportError: - print("xarm package not installed, skipping xarm import") - -from manipulator_control import jparse_cls - -from manipulator_control.msg import JparseTerms, Matrow -from manipulator_control.srv import JParseSrvResponse, JParseSrvRequest, JParseSrv - -class ArmController: - def __init__(self): - rospy.init_node('arm_controller', anonymous=True) - - self.base_link = rospy.get_param('/base_link_name', 'link_base') #defaults are for Kinova gen3 - self.end_link = rospy.get_param('/end_link_name', 'link_eef') - - self.is_sim = rospy.get_param('~is_sim', False) #boolean to control if the robot is in simulation or not - - self.phi_gain = rospy.get_param('~phi_gain', 10) #gain for the phi term in the JParse method - self.phi_gain_position = rospy.get_param('~phi_gain_position', 15) #gain for the phi term in the JParse method - self.phi_gain_angular = rospy.get_param('~phi_gain_angular', 15) #gain for the phi term in the JParse method - - self.jparse_gamma = rospy.get_param('~jparse_gamma', 0.1) #gamma for the JParse method - - self.use_space_mouse = rospy.get_param('~use_space_mouse', False) #boolean to control if the space mouse is used or not - self.use_space_mouse_jparse = rospy.get_param('~use_space_mouse_jparse', False) #boolean to control if the space mouse is used or not - - self.use_service_bool = rospy.get_param('~use_service_bool', False) #boolean to control if the service is used or not - - self.space_mouse_command = np.array([0,0,0,0,0,0]).T - - if self.is_sim == False: - self.robot_ip = rospy.get_param('~robot_ip', '192.168.1.199 ') - self.arm = XArmAPI(self.robot_ip) - self.arm.motion_enable(enable=True) - self.arm.set_mode(0) - self.arm.set_state(state=0) - time.sleep(1) - - # set joint velocity control mode - self.arm.set_mode(4) - self.arm.set_state(0) - time.sleep(1) - - #Set parameters - self.position_only = rospy.get_param('~position_only', False) #boolean to control only position versus full pose - - self.joint_states = None - self.target_pose = None - - #get tf listener - self.tfBuffer = tf2_ros.Buffer() - self.listener = tf2_ros.TransformListener(self.tfBuffer) - self.tfros = tf.TransformerROS() - self.tf_timeout = rospy.Duration(1.0) - - if self.is_sim == True: - #if we are in simulation, use the joint_states and target_pose topics - joint_states_topic = '/xarm/joint_states' - else: - joint_states_topic = '/joint_states' - - rospy.Subscriber(joint_states_topic, JointState, self.joint_states_callback) - rospy.Subscriber('/target_pose', PoseStamped, self.target_pose_callback) - rospy.Subscriber('/robot_action', TwistStamped, self.space_mouse_callback) - if self.use_service_bool == False: - #in this case use the published message - rospy.Subscriber("jparse_output", JparseTerms, self.jparse_terms_callback, queue_size=1) - #initialize the jparse and jsafety matrices - self.jparse_mat = None - self.jsafety_mat = None - - - self.manip_measure_pub = rospy.Publisher('/manip_measure', Float64, queue_size=10) - self.inverse_cond_number = rospy.Publisher('/inverse_cond_number', Float64, queue_size=10) - #have certain messages to store raw error - self.pose_error_pub = rospy.Publisher('/pose_error', PoseStamped, queue_size=10) - self.position_error_pub = rospy.Publisher('/position_error', Vector3, queue_size=10) - self.orientation_error_pub = rospy.Publisher('/orientation_error', Vector3, queue_size=10) - #have certain messages to store control error - self.pose_error_control_pub = rospy.Publisher('/pose_error_control', PoseStamped, queue_size=10) - self.position_error_control_pub = rospy.Publisher('/position_error_control', Vector3, queue_size=10) - self.orientation_control_error_pub = rospy.Publisher('/orientation_error_control', Vector3, queue_size=10) - #publish the current end effector pose and target pose - self.current_end_effector_pose_pub = rospy.Publisher('/current_end_effector_pose', PoseStamped, queue_size=10) - self.current_target_pose_pub = rospy.Publisher('/current_target_pose', PoseStamped, queue_size=10) - - joint_command_topic = rospy.get_param('~joint_command_topic', '/xarm/xarm7_velo_traj_controller/command') - self.joint_vel_pub = rospy.Publisher(joint_command_topic, JointTrajectory, queue_size=10) - # Define the joint names - self.joint_names = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7"] - # Initialize the current positions - self.current_positions = [0.0] * len(self.joint_names) - - self.rate = rospy.Rate(50) # 10 Hz - #home the robot - try: - self.velocity_home_robot() - except rospy.ROSInterruptException: - rospy.loginfo("ROS Interrupt Exception") - pass - finally: - # Clean up resources if needed - rospy.loginfo("Shutting down the control loop") - joint_zero_velocities = [0.0] * len(self.joint_names) - self.command_joint_velocities(joint_zero_velocities) - #now run the control loop - self.control_loop() - - def rad2deg(self, q): - return q/math.pi*180.0 - - def joint_states_callback(self, msg): - """ - Callback function for the joint_states topic. This function will be called whenever a new message is received - on the joint_states topic. The message is a sensor_msgs/JointState message, which contains the current joint - and the corresponding joint velocities and efforts. This function will extract the joint positions, velocities, - and efforts and return them as lists. - """ - self.joint_states = msg - - def space_mouse_callback(self, msg): - """ - Callback function for the space_mouse topic. This function will be called whenever a new message is received - on the space_mouse topic. The message is a geometry_msgs/TwistStamped message, which contains the current - velocity of the end effector. This function will extract the linear and angular velocities and return them as - lists. - """ - space_mouse_command = np.array([msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z, msg.twist.angular.x, msg.twist.angular.y, msg.twist.angular.z]) - position_velocity = space_mouse_command[:3] - angular_velocity = space_mouse_command[3:] - position_velocity_norm = np.linalg.norm(position_velocity) - angular_velocity_norm = np.linalg.norm(angular_velocity) - if position_velocity_norm > 0.05: - position_velocity = position_velocity / position_velocity_norm * 0.05 - self.space_mouse_command = np.array([position_velocity[0], position_velocity[1],position_velocity[2],angular_velocity[0],angular_velocity[1],angular_velocity[2]]) #check if norm of the space mouse command is greater than 0.05, if so normalize it to this value - - #ensure we can get into the while loop - if self.use_space_mouse == True: - self.target_pose = PoseStamped() #set this to get in a loop - - - def target_pose_callback(self, msg): - """ - Callback function for the target_pose topic. This function will be called whenever a new message is received - on the target_pose topic. The message is a geometry_msgs/PoseStamped message, which contains the target pose - """ - self.target_pose = msg - self.current_target_pose_pub.publish(self.target_pose) #if target pose is paused manually, this allows us to track the current target pose seen by the script - - def EndEffectorPose(self, q): - """ - This function computes the end-effector pose given the joint configuration q. - """ - current_pose = PoseStamped() - current_pose.header.frame_id = self.base_link - current_pose.header.stamp = rospy.Time.now() - # listener = tf.TransformListener() - try: - trans = self.tfBuffer.lookup_transform(self.base_link, self.end_link, rospy.Time(), timeout=self.tf_timeout) - current_pose.pose.position.x = trans.transform.translation.x - current_pose.pose.position.y = trans.transform.translation.y - current_pose.pose.position.z = trans.transform.translation.z - current_pose.pose.orientation.x = trans.transform.rotation.x - current_pose.pose.orientation.y = trans.transform.rotation.y - current_pose.pose.orientation.z = trans.transform.rotation.z - current_pose.pose.orientation.w = trans.transform.rotation.w - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): - rospy.logerr("TF lookup failed") - rospy.logerr("failed to lookup transform from %s to %s", self.base_link, self.end_link) - self.current_end_effector_pose_pub.publish(current_pose) - return current_pose - - def EndEffectorVelocity(self, q, dq): - """ - This function computes the end-effector velocity given the joint configuration q and joint velocities dq. - """ - J = self.jacobian_calculator.jacobian(q) - J = np.array(J) - dq = np.array(dq) - dx = np.dot(J, dq) - return dx - - def rotation_matrix_to_axis_angle(self,R): - """ - Converts a rotation matrix to an axis-angle vector. - - Parameters: - R (numpy.ndarray): A 3x3 rotation matrix. - - Returns: - numpy.ndarray: Axis-angle vector (3 elements). - """ - if not (R.shape == (3, 3) and np.allclose(np.dot(R.T, R), np.eye(3)) and np.isclose(np.linalg.det(R), 1)): - raise ValueError("Input matrix must be a valid rotation matrix.") - - # Calculate the angle of rotation - angle = np.arccos((np.trace(R) - 1) / 2) - - if np.isclose(angle, 0): # No rotation - return np.zeros(3) - - if np.isclose(angle, np.pi): # 180 degrees rotation - # Special case for 180-degree rotation - # Extract the axis from the diagonal of R - axis = np.sqrt((np.diag(R) + 1) / 2) - # Adjust signs based on the matrix off-diagonals - axis[0] *= np.sign(R[2, 1] - R[1, 2]) - axis[1] *= np.sign(R[0, 2] - R[2, 0]) - axis[2] *= np.sign(R[1, 0] - R[0, 1]) - return axis * angle - - # General case - axis = np.array([ - R[2, 1] - R[1, 2], - R[0, 2] - R[2, 0], - R[1, 0] - R[0, 1] - ]) / (2 * np.sin(angle)) - - return axis * angle - - def pose_error(self, current_pose, target_pose, error_norm_max = 0.05, control_pose_error = True): - """ - This function computes the error between the current pose and the target pose. - """ - #Compute the position error - position_error = np.array([target_pose.pose.position.x - current_pose.pose.position.x, - target_pose.pose.position.y - current_pose.pose.position.y, - target_pose.pose.position.z - current_pose.pose.position.z]) - #if the norm of the position error is greater than the maximum allowed, scale it down - if control_pose_error == True: - if np.linalg.norm(position_error) > error_norm_max: - position_error = position_error / np.linalg.norm(position_error) * error_norm_max - - #convert the quaternion in posestamped to list of quaternions then pass this into quaternion_matrix to get the rotation matrix. Access only the rotation part of the matrix - goal_rotation_matrix = quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])[:3,:3] - current_rotation_matrix = quaternion_matrix([current_pose.pose.orientation.x, current_pose.pose.orientation.y, current_pose.pose.orientation.z, current_pose.pose.orientation.w])[:3,:3] - #Compute the orientation error - R_error= np.dot(goal_rotation_matrix, np.linalg.inv(current_rotation_matrix)) - # Extract the axis-angle lie algebra vector from the rotation matrix - angle_error = self.rotation_matrix_to_axis_angle(R_error) - # Return the position and orientation error - return position_error, angle_error - - def axis_angle_to_rotation_matrix(self, axis_angle): - """ - Converts an axis-angle vector to a rotation matrix. - - Parameters: - axis_angle (numpy.ndarray): Axis-angle vector (3 elements). - - Returns: - numpy.ndarray: A 3x3 rotation matrix. - """ - # Extract the axis and angle - axis = axis_angle / np.linalg.norm(axis_angle) - angle = np.linalg.norm(axis_angle) - - # Compute the skew-symmetric matrix - K = np.array([ - [0, -axis[2], axis[1]], - [axis[2], 0, -axis[0]], - [-axis[1], axis[0], 0] - ]) - - # Compute the rotation matrix using the Rodrigues' formula - R = np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * np.dot(K, K) - - return R - - def rotation_matrix_to_quaternion(self, R): - """ - Converts a rotation matrix to a quaternion. - - Parameters: - R (numpy.ndarray): A 3x3 rotation matrix. - - Returns: - numpy.ndarray: A 4-element quaternion. - """ - if not (R.shape == (3, 3) and np.allclose(np.dot(R.T, R), np.eye(3)) and np.isclose(np.linalg.det(R), 1)): - raise ValueError("Input matrix must be a valid rotation matrix.") - - # Compute the quaternion using the method from the book - q = np.zeros(4) - q[0] = np.sqrt(1 + R[0, 0] + R[1, 1] + R[2, 2]) / 2 - q[1] = (R[2, 1] - R[1, 2]) / (4 * q[0]) - q[2] = (R[0, 2] - R[2, 0]) / (4 * q[0]) - q[3] = (R[1, 0] - R[0, 1]) / (4 * q[0]) - - return q - - def publish_pose_errors(self, position_error, angular_error, control_pose_error = True): - """ - Publishes the position and orientation errors as ROS messages. - """ - pose_error_msg = PoseStamped() - pose_error_msg.header.frame_id = self.base_link - pose_error_msg.header.stamp = rospy.Time.now() - pose_error_msg.pose.position.x = position_error[0] - pose_error_msg.pose.position.y = position_error[1] - pose_error_msg.pose.position.z = position_error[2] - #for completeness, convert the axis angle to quaternion - Rmat = self.axis_angle_to_rotation_matrix(angular_error) - quat_from_Rmat = self.rotation_matrix_to_quaternion(Rmat) - # quat_from_Rmat = tf.transformations.quaternion_from_matrix(Rmat) - pose_error_msg.pose.orientation.x = quat_from_Rmat[0] - pose_error_msg.pose.orientation.y = quat_from_Rmat[1] - pose_error_msg.pose.orientation.z = quat_from_Rmat[2] - pose_error_msg.pose.orientation.w = quat_from_Rmat[3] - #publish the axis-angle orientation error - orientation_error_msg = Vector3() - orientation_error_msg.x = angular_error[0] - orientation_error_msg.y = angular_error[1] - orientation_error_msg.z = angular_error[2] - #publish the position error - position_error_msg = Vector3() - position_error_msg.x = position_error[0] - position_error_msg.y = position_error[1] - position_error_msg.z = position_error[2] - - #Now publish - if control_pose_error == True: - self.position_error_control_pub.publish(position_error_msg) - self.pose_error_control_pub.publish(pose_error_msg) - self.orientation_control_error_pub.publish(orientation_error_msg) - else: - self.pose_error_pub.publish(pose_error_msg) - self.position_error_pub.publish(position_error_msg) - self.orientation_error_pub.publish(orientation_error_msg) - - - - def service_caller(self, service_name, request): - """ - Calls the JParseSrv, then builds and returns two numpy matrices: - - jparse_matrix: shape (R, C) - - jsafety_nullspace_matrix: shape (R, C) - where R is the number of rows (i.e. len(response.jparse)) - and C is the number of columns (i.e. len(response.jparse[0].row)). - """ - # wait for provider - rospy.wait_for_service(service_name) - try: - call = rospy.ServiceProxy(service_name, JParseSrv) - resp = call(request) - - # turn list into a 2D array - # each Matrow.row is a list of floats - if resp.jparse: - jparse_mat = np.stack([ np.array(mrow.row) for mrow in resp.jparse ]) - else: - jparse_mat = np.empty((0, 0)) - - if resp.jsafety_nullspace: - jsafety_mat = np.stack([ np.array(mrow.row) for mrow in resp.jsafety_nullspace ]) - else: - jsafety_mat = np.empty((0, 0)) - - return jparse_mat, jsafety_mat - - except rospy.ServiceException as e: - rospy.logerr("JParseSrv call failed: %s", e) - return None, None - - def jparse_terms_callback(self, msg): - """ - msg.jparse and msg.jsafety_nullspace are lists of Matrow, - each Matrow.row is a list of floats. - We stack them into RƗC numpy arrays. - """ - # build jparse matrix - if msg.jparse: - rows = [ np.array(mrow.row, dtype=float) for mrow in msg.jparse ] - self.jparse_mat = np.stack(rows, axis=0) - else: - self.jparse_mat = np.empty((0,0)) - - # build safetyā€nullspace matrix - if msg.jsafety_nullspace: - rows = [ np.array(mrow.row, dtype=float) for mrow in msg.jsafety_nullspace ] - self.jsafety_mat = np.stack(rows, axis=0) - else: - self.jsafety_mat = np.empty((0,0)) - - - def control_loop(self): - """ - This function implements the control loop for the arm controller. - """ - - while not rospy.is_shutdown(): - - if self.joint_states and self.target_pose: - # obtain the current joints - q = [] - dq = [] - effort = [] - for joint_name in self.joint_names: - idx = self.joint_states.name.index(joint_name) - q.append(self.joint_states.position[idx]) - dq.append(self.joint_states.velocity[idx]) - effort.append(self.joint_states.effort[idx]) - self.current_positions = q - # Calculate the JParsed Jacobian - - if self.use_service_bool == True: - #call the service to get the JParse and jsafety matrices - request = JParseSrvRequest() - request.gamma = self.jparse_gamma - request.base_link_name = self.base_link - request.end_link_name = self.end_link - request.singular_direction_gain_position = self.phi_gain_position - request.singular_direction_gain_angular = self.phi_gain_angular - jparse_mat, jsafety_mat = self.service_caller("/jparse_srv", request) - if jparse_mat is not None: - J_method, J_nullspace = jparse_mat, jsafety_mat - else: - #use the jparse_mat and jsafety_mat from the callback - J_method, J_nullspace = self.jparse_mat, self.jsafety_mat - - #Calculate the delta_x (task space error) - target_pose = self.target_pose - current_pose = self.EndEffectorPose(q) - position_error, angular_error = self.pose_error(current_pose, target_pose, control_pose_error=False) - # log the pose error - self.publish_pose_errors(position_error, angular_error, control_pose_error=False) - #now find error for control (max error norm) - if self.is_sim == False: - #real world limits - # error_norm_max = 0.05 - error_norm_max = 0.10 - else: - #simulation limits - error_norm_max = 1.0 - position_error, angular_error = self.pose_error(current_pose, target_pose, error_norm_max = error_norm_max, control_pose_error=True) - #move in nullspace towards nominal pose (for xarm its 0 joint angle for Xarm7) - if self.is_sim == True: - kp_gain = 2.0 - else: - # kp_gain = 1.0 - kp_gain = 3.0 - - if self.is_sim == True: - nominal_motion_nullspace = np.matrix([-v*kp_gain for v in q]).T #send to home which is 0 joint position for all joints - else: - # nominal motion nullspace which checks if q magnitude is below threshold and chooses the minimum - null_space_angle_rate = 0.6 - - nominal_motion_nullspace = np.matrix([np.sign(-v*kp_gain)*np.min([np.abs(-v*kp_gain),null_space_angle_rate]) for v in q]).T #send to home which is 0 joint position for all joints - # log the pose error - self.publish_pose_errors(position_error, angular_error, control_pose_error=True) - - # Calculate and command the joint velocities - if self.position_only == True: - rospy.loginfo("Position only control") - position_error = np.matrix(position_error).T - if self.is_sim == True: - #use these gains only in simulation! - kp_gain = 10.0 - else: - kp_gain = 3.0 - Kp = np.diag([kp_gain, kp_gain, kp_gain]) # Proportional gain matrix - - task_vector = Kp @ position_error - joint_velocities = J_method @ task_vector + J_nullspace @ nominal_motion_nullspace - else: - # realworld gains (tested) - kp_gain_position = 1.0 - kp_gain_orientation = 1.0 - if self.is_sim == True: - #use these gains only in simulation! - kp_gain_position = 10.0 - kp_gain_orientation = 10.0 - Kp_position = np.diag([kp_gain_position, kp_gain_position, kp_gain_position]) # Proportional gain matrix - Kp_orientation = np.diag([kp_gain_orientation, kp_gain_orientation, kp_gain_orientation]) - task_position = Kp_position @ np.matrix(position_error).T - task_orientation = Kp_orientation @ np.matrix(angular_error).T - full_pose_error = np.matrix(np.concatenate((task_position.T, task_orientation.T),axis=1)).T - if self.use_space_mouse == False: - joint_velocities = J_method @ full_pose_error + J_nullspace @ nominal_motion_nullspace - if self.use_space_mouse == True: - #use the space mouse command to control the joint velocities - space_mouse_command = np.matrix(self.space_mouse_command).T - #now add this to the joint velocities - joint_velocities = J_method @ space_mouse_command + J_nullspace @ nominal_motion_nullspace - joint_velocities_list = np.array(joint_velocities).flatten().tolist() - # command the joint velocities - self.command_joint_velocities(joint_velocities_list) #this commands the joint velocities - self.rate.sleep() - rospy.loginfo("Control loop running") - - - def velocity_home_robot(self): - """ - This function commands the robot to the home position using joint velocities. - """ - # do the following in a loop for 5 seconds: - t_start = rospy.Time.now() - duration = 0.0 - while not rospy.is_shutdown(): - if duration >= 5.0: - break - rospy.loginfo("Homing the robot") - if self.joint_states: - duration = (rospy.Time.now() - t_start).to_sec() - q = [] - dq = [] - for joint_name in self.joint_names: - idx = self.joint_states.name.index(joint_name) - q.append(self.joint_states.position[idx]) - dq.append(self.joint_states.velocity[idx]) - self.current_positions = q - # Command the joint velocities - if self.is_sim == True: - kp_gain = 10.0 - else: - kp_gain = 0.5 - q_home = [-0.03142359480261803, -0.5166178941726685, 0.12042707949876785, 0.9197863936424255, -0.03142168000340462, 1.4172008037567139, 0.03490765020251274] - #now find the error between the current position and the home position and use joint velocities to move towards the home position - joint_velocities_list = kp_gain * (np.array(q_home) - np.array(q)) - - self.command_joint_velocities(joint_velocities_list) - - - def command_joint_velocities(self, joint_vel_list): - """ - This function commands the joint velocities to the robot using the appropriate ROS message type. - """ - if self.is_sim: - # Create the JointTrajectory message - trajectory_msg = JointTrajectory() - trajectory_msg.joint_names = self.joint_names - - # Create a trajectory point - point = JointTrajectoryPoint() - - # Use current positions - point.positions = self.current_positions - - # Set velocities - #make velocity negative because Xarm has cw positive direction for joint velocities - joint_vel_list = [-v for v in joint_vel_list] - point.velocities = joint_vel_list #this is negative because Xarm has cw positive direction for joint velocities - - # Set accelerations to zero - point.accelerations = [0.0] * len(self.joint_names) - - # Effort (optional; set to None or skip if not needed) - point.effort = [0.0] * len(self.joint_names) - - # Set the time from start - point.time_from_start = rospy.Duration(0.1) # Duration in seconds - - # Add the point to the trajectory - trajectory_msg.points.append(point) - - # Publish the trajectory - trajectory_msg.header.stamp = rospy.Time.now() # Update timestamp - self.joint_vel_pub.publish(trajectory_msg) - else: - # this is on the real robot, directly send joint velociteies - # Send joint velocities to the arm - # log the velocities - rospy.loginfo("Joint velocities: %s", joint_vel_list) - if self.use_space_mouse_jparse: - self.arm.vc_set_joint_velocity(joint_vel_list, is_radian=True) - - -if __name__ == '__main__': - try: - ArmController() - except rospy.ROSInterruptException: - pass diff --git a/manipulator_control_ros1/setup.py b/manipulator_control_ros1/setup.py deleted file mode 100644 index 08c053b..0000000 --- a/manipulator_control_ros1/setup.py +++ /dev/null @@ -1,9 +0,0 @@ -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['manipulator_control'], - package_dir={'': 'src'} -) - -setup(**d) \ No newline at end of file diff --git a/manipulator_control_ros1/src/jparse.cpp b/manipulator_control_ros1/src/jparse.cpp deleted file mode 100644 index 8577637..0000000 --- a/manipulator_control_ros1/src/jparse.cpp +++ /dev/null @@ -1,718 +0,0 @@ -#include -#include -#include // for std::stringstream - - -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - - -class JPARSE -{ -public: - JPARSE(ros::NodeHandle& nh); - void Jnt_state_callback(const sensor_msgs::JointStateConstPtr& msg); - void Jparse_calculation(const Eigen::MatrixXd& J, Eigen::MatrixXd& J_parse, Eigen::MatrixXd& J_safety_nullspace, std::vector& jparse_singular_index, Eigen::MatrixXd& U_safety, Eigen::VectorXd& S_new_safety, Eigen::MatrixXd& U_new_proj, Eigen::VectorXd& S_new_proj, Eigen::MatrixXd& U_new_sing, Eigen::VectorXd& Phi, double& gamma, double& singular_direction_gain_position, double& singular_direction_gain_angular); - void Publish_JPARSE(const std_msgs::Header& header, const Eigen::MatrixXd& J_parse, const Eigen::MatrixXd& J_safety_nullspace); - void JPARSE_visualization(const std_msgs::Header& header, const Eigen::MatrixXd& J_parse, const Eigen::MatrixXd& J_safety_nullspace, const std::vector& jparse_singular_index, const Eigen::MatrixXd& U_safety, const Eigen::VectorXd& S_new_safety, const Eigen::MatrixXd& U_new_proj, const Eigen::VectorXd& S_new_proj, const Eigen::MatrixXd& U_new_sing, const Eigen::VectorXd& Phi); - void matrix_to_msg(const Eigen::MatrixXd& mat, std::vector& msg_rows); - - Eigen::MatrixXd pseudoInverse(const Eigen::MatrixXd& J, double tol = 1e-6); - - bool handleJparse( - manipulator_control::JParseSrv::Request& req, - manipulator_control::JParseSrv::Response& resp - ); - -private: - ros::NodeHandle nh_; - ros::Subscriber joint_state_sub_; - ros::Publisher jparse_pub_; - ros::Publisher jparse_markers_pub_; - std::string root_, tip_; - - urdf::Model model_; - KDL::Tree kdl_tree_; - KDL::Chain kdl_chain_; - KDL::Chain kdl_chain_service_; //for service option - boost::shared_ptr kdl_chain_dynamics_; - boost::shared_ptr jac_solver_; - boost::shared_ptr jac_solver_service_; //for service option - - KDL::JntArrayVel positions_; - std::vector joint_names_; - - tf2_ros::Buffer tfBuffer; - std::unique_ptr tfListener; - - double gamma_; //Jparse threshold gamma - double singular_direction_gain_position_; - double singular_direction_gain_angular_; - - //for service option - bool have_last_msg_ = false; - std::mutex last_msg_mutex_; - sensor_msgs::JointStateConstPtr joint_state_msg_service_; - ros::ServiceServer jparse_service_; - -}; - -JPARSE::JPARSE(ros::NodeHandle& nh) : nh_(nh) -{ - - ros::NodeHandle pnh("~"); - - bool run_as_service = false; - pnh.param("run_as_service", run_as_service, false); - - // Always subscribe, so we cache last_J_ - joint_state_sub_ = nh_.subscribe("joint_states", 1, - &JPARSE::Jnt_state_callback, this); - - if (run_as_service) - { - // advertise the service - jparse_service_ = nh_.advertiseService("jparse_srv", - &JPARSE::handleJparse, this); - ROS_INFO("JPARSE: running as service 'jparse_srv'"); - } - - pnh.param("base_link_name", root_, "base_link"); - pnh.param("end_link_name", tip_, "end_effector_link"); - - nh_.param("jparse_gamma", gamma_, 0.2); - nh_.param("singular_direction_gain_position", singular_direction_gain_position_, 1.0); - nh_.param("singular_direction_gain_angular", singular_direction_gain_angular_, 1.0); - - if (!model_.initParam("/robot_description") || - !kdl_parser::treeFromUrdfModel(model_, kdl_tree_)) - { - ROS_ERROR("Failed to load /robot_description or build KDL tree"); - return; - } - - //for getting the end-effector pose - tfBuffer.setUsingDedicatedThread(true); - tfListener.reset(new tf2_ros::TransformListener(tfBuffer)); - - if (!kdl_tree_.getChain(root_, tip_, kdl_chain_)) - { - ROS_ERROR("Failed to extract KDL chain from %s to %s", root_.c_str(), tip_.c_str()); - return; - } - - kdl_chain_dynamics_.reset(new KDL::ChainDynParam(kdl_chain_, KDL::Vector(0, 0, -9.81))); - jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_)); - positions_ = KDL::JntArrayVel(kdl_chain_.getNrOfJoints()); - - KDL::SetToZero(positions_.q); - KDL::SetToZero(positions_.qdot); - - for (size_t i = 0; i < kdl_chain_.getNrOfSegments(); ++i) - { - KDL::Joint joint = kdl_chain_.getSegment(i).getJoint(); - if (joint.getType() != KDL::Joint::None) - joint_names_.push_back(joint.getName()); - } - - joint_state_sub_ = nh_.subscribe("joint_states", 1, &JPARSE::Jnt_state_callback, this); - jparse_pub_ = nh_.advertise("jparse_output", 1); - jparse_markers_pub_ = nh_.advertise("/jparse_ellipsoid_marker_cpp", 1); -} - -void JPARSE::Jnt_state_callback(const sensor_msgs::JointStateConstPtr& msg) -{ - std::vector q, dq; - for (const auto& joint_name : joint_names_) - { - auto it = std::find(msg->name.begin(), msg->name.end(), joint_name); - if (it != msg->name.end()) - { - size_t idx = std::distance(msg->name.begin(), it); - q.push_back(msg->position[idx]); - dq.push_back(msg->velocity[idx]); - } - } - - if (q.size() != joint_names_.size()) return; - - for (size_t i = 0; i < joint_names_.size(); ++i) - { - positions_.q(i) = q[i]; - positions_.qdot(i) = dq[i]; - } - - KDL::Jacobian J_kdl(joint_names_.size()); - jac_solver_->JntToJac(positions_.q, J_kdl); - Eigen::MatrixXd J = J_kdl.data; - - //store a copy for the service - { - std::lock_guard guard(last_msg_mutex_); - joint_state_msg_service_ = msg; - have_last_msg_ = true; - } - - Eigen::MatrixXd J_parse, J_safety_nullspace; - //handle variable size kinematic chain - Eigen::JacobiSVD svd(J, Eigen::ComputeFullU | Eigen::ComputeFullV); - int n = svd.singularValues().size(); - std::vector jparse_singular_index(n, 0); // Elements in this list are 0 if non-singular, 1 if singular - Eigen::MatrixXd U_safety, U_new_proj, U_new_sing; - Eigen::VectorXd S_new_safety, S_new_proj, Phi; - - Jparse_calculation(J, J_parse, J_safety_nullspace, jparse_singular_index, U_safety, S_new_safety, U_new_proj, S_new_proj, U_new_sing, Phi, gamma_, singular_direction_gain_position_, singular_direction_gain_angular_); - Publish_JPARSE(msg->header, J_parse, J_safety_nullspace); - - Eigen::MatrixXd J_position = J.block(0, 0, 3, J.cols()); //extract J_v - Eigen::MatrixXd J_parse_position, J_safety_nullspace_position; - - //handle variable size kinematic chain - Eigen::JacobiSVD svd_position(J_position, Eigen::ComputeFullU | Eigen::ComputeFullV); - int n_position = svd_position.singularValues().size(); - std::vector jparse_singular_index_position(n_position, 0); // Elements in this list are 0 if non-singular, 1 if singular - Eigen::MatrixXd U_safety_position, U_new_proj_position, U_new_sing_position; - Eigen::VectorXd S_new_safety_position, S_new_proj_position, Phi_position; - - Jparse_calculation(J_position, J_parse_position, J_safety_nullspace_position, jparse_singular_index_position, U_safety_position, S_new_safety_position, U_new_proj_position, S_new_proj_position, U_new_sing_position, Phi_position, gamma_, singular_direction_gain_position_, singular_direction_gain_angular_); - JPARSE_visualization(msg->header, J_parse_position, J_safety_nullspace_position, jparse_singular_index_position, U_safety_position, S_new_safety_position, U_new_proj_position, S_new_proj_position, U_new_sing_position, Phi_position); -} - -Eigen::MatrixXd JPARSE::pseudoInverse(const Eigen::MatrixXd& J, double tol) -{ - Eigen::JacobiSVD svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV); - const Eigen::VectorXd& singularValues = svd.singularValues(); - Eigen::MatrixXd S_pinv = Eigen::MatrixXd::Zero(svd.matrixV().cols(), svd.matrixU().cols()); - - for (int i = 0; i < singularValues.size(); ++i) - { - if (singularValues(i) > tol) - { - S_pinv(i, i) = 1.0 / singularValues(i); - } - } - - return svd.matrixV() * S_pinv * svd.matrixU().transpose(); -} - -void JPARSE::Jparse_calculation(const Eigen::MatrixXd& J, Eigen::MatrixXd& J_parse, Eigen::MatrixXd& J_safety_nullspace, std::vector& jparse_singular_index, Eigen::MatrixXd& U_safety, Eigen::VectorXd& S_new_safety, Eigen::MatrixXd& U_new_proj, Eigen::VectorXd& S_new_proj, Eigen::MatrixXd& U_new_sing, Eigen::VectorXd& Phi, double& gamma, double& singular_direction_gain_position, double& singular_direction_gain_angular) -{ - /* - Steps are as follows: - 1. Find the SVD of J - 2. Find the adjusted condition number and Jparse singular index - 3. Find the Projection Jacobian - 4. Find the Safety Jacobian - 5. Find the singular direction projection components - 6. Find the pseudo inverse of J_safety and J_proj - 7. Find Jparse - 8. Find the null space of J_safety - */ - - //1. Find the SVD of J - Eigen::MatrixXd U, V; - Eigen::VectorXd S; - Eigen::JacobiSVD svd(J, Eigen::ComputeFullU | Eigen::ComputeFullV); - U = svd.matrixU(); - S = svd.singularValues(); - V = svd.matrixV(); - - if (U.rows() == U.cols() && U.determinant() < 0.0) { - int k = U.cols() - 1; // pick the last column - U.col(k) *= -1.0; - V.col(k) *= -1.0; - } - - U_safety = U; //Safety Jacobian shares the same U as the SVD of J - - //2. find the adjusted condition number - double max_singular_value = S.maxCoeff(); - std::vector adjusted_condition_numbers(S.size()); - for (int i = 0; i < S.size(); ++i) - { - adjusted_condition_numbers[i] = S(i) / max_singular_value; - } - - //3. Find the Projection Jacobian - std::vector valid_indices; - - for (int i = 0; i < S.size(); ++i) - { - // keep only the elements whose singular value is greater than the threshold - if (S(i) > gamma * max_singular_value) - { - valid_indices.push_back(i); - }else{ - jparse_singular_index[i] = 1; // set the index to 1 if the singular value is less than the threshold - } - } - - U_new_proj = Eigen::MatrixXd(U.rows(), valid_indices.size()); - S_new_proj = Eigen::VectorXd(valid_indices.size()); - - for (size_t i = 0; i < valid_indices.size(); ++i) - { - U_new_proj.col(i) = U.col(valid_indices[i]); - S_new_proj(i) = S(valid_indices[i]); - } - Eigen::MatrixXd S_new_proj_matrix = Eigen::MatrixXd::Zero(U_new_proj.cols(), V.rows()); - for (int i = 0; i < S_new_proj.size(); ++i) - { - S_new_proj_matrix(i, i) = S_new_proj(i); - } - Eigen::MatrixXd J_proj = U_new_proj * S_new_proj_matrix * V.transpose(); - - //4. Find the Safety Jacobian - S_new_safety = Eigen::VectorXd(S.size()); - for (int i = 0; i < S.size(); ++i) - { - //if the singular value is greater than the threshold, keep it otherwise set it to the threshold - if ((S(i) / max_singular_value) > gamma) - { - S_new_safety(i) = S(i); - } - else - { - S_new_safety(i) = gamma * max_singular_value; - } - } - - - Eigen::MatrixXd S_new_safety_matrix = Eigen::MatrixXd::Zero(U.rows(), V.cols()); - for (int i = 0; i < S_new_safety.size(); ++i) - { - S_new_safety_matrix(i, i) = S_new_safety(i); - } - Eigen::MatrixXd J_safety = U * S_new_safety_matrix * V.transpose(); - - //5. Find the singular direction projection components - Eigen::MatrixXd Phi_matrix, Kp_singular, Phi_singular; - std::vector valid_indices_sing; - bool set_empty_bool = true; // set to true if the valid indices are empty - for (int i = 0; i < S.size(); ++i) - { - if (adjusted_condition_numbers[i] <= gamma) - { - set_empty_bool = false; - valid_indices_sing.push_back(i); - } - } - - U_new_sing = Eigen::MatrixXd(U.rows(), valid_indices_sing.size()); - Phi = Eigen::VectorXd(valid_indices_sing.size()); - - if (set_empty_bool==false) - { - for (size_t i = 0; i < valid_indices_sing.size(); ++i) - { - U_new_sing.col(i) = U.col(valid_indices_sing[i]); - Phi(i) = adjusted_condition_numbers[valid_indices_sing[i]] / gamma; - } - - Phi_matrix = Eigen::MatrixXd::Zero(Phi.size(), Phi.size()); - for (int i = 0; i < Phi.size(); ++i) - { - Phi_matrix(i, i) = Phi(i); - } - - Kp_singular = Eigen::MatrixXd::Zero(U.rows(), U.cols()); - for (int i = 0; i < 3; ++i) - { - Kp_singular(i, i) = singular_direction_gain_position; - } - if (Kp_singular.cols() > 3) //checks if position only (J_v) or full jacobian - { - for (int i = 3; i < 6; ++i) - { - Kp_singular(i, i) = singular_direction_gain_angular; - } - } - Phi_singular = U_new_sing * Phi_matrix * U_new_sing.transpose() * Kp_singular; // put it all together - } - - //6. Find pseudo inverse of J_safety and J_proj - Eigen::MatrixXd J_safety_pinv = pseudoInverse(J_safety); - Eigen::MatrixXd J_proj_pinv = pseudoInverse(J_proj); - - //7. Find the Jparse - if (set_empty_bool==false) - { - J_parse = J_safety_pinv * J_proj * J_proj_pinv + J_safety_pinv * Phi_singular; - } - else - { - J_parse = J_safety_pinv * J_proj * J_proj_pinv; - } - - //8. Find the null space of J_safety - J_safety_nullspace = Eigen::MatrixXd::Identity(J_safety.cols(), J_safety.cols()) - J_safety_pinv * J_safety; - -} - -void JPARSE::JPARSE_visualization(const std_msgs::Header& header, const Eigen::MatrixXd& J_parse, const Eigen::MatrixXd& J_safety_nullspace, const std::vector& jparse_singular_index, const Eigen::MatrixXd& U_safety, const Eigen::VectorXd& S_new_safety, const Eigen::MatrixXd& U_new_proj, const Eigen::VectorXd& S_new_proj, const Eigen::MatrixXd& U_new_sing, const Eigen::VectorXd& Phi) -{ - //This script takes in the J_Parse matricies and visualizes them in RVIZ; this is done for position. - - //Get the end-effector position and orientation - geometry_msgs::TransformStamped transformStamped; - try - { - transformStamped = tfBuffer.lookupTransform(root_, tip_, ros::Time(0)); - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); - return; - } - - - //1. create a marker array - visualization_msgs::MarkerArray marker_array; - visualization_msgs::Marker J_safety_marker; - visualization_msgs::Marker J_proj_marker; - visualization_msgs::Marker J_singular_marker; - - //2. Set up the J_safety_marker - J_safety_marker.header = header; - J_safety_marker.header.frame_id = root_; - J_safety_marker.ns = "J_safety"; - J_safety_marker.id = 0; - J_safety_marker.type = visualization_msgs::Marker::SPHERE; - J_safety_marker.action = visualization_msgs::Marker::ADD; - //set the marker pose position to the end effector position - J_safety_marker.pose.position.x = transformStamped.transform.translation.x; - J_safety_marker.pose.position.y = transformStamped.transform.translation.y; - J_safety_marker.pose.position.z = transformStamped.transform.translation.z; - double ellipsoid_scale = 0.25; - - double safety_value_1 = std::max(0.001, S_new_safety(0)); - J_safety_marker.scale.x = ellipsoid_scale * safety_value_1; - - double safety_value_2 = std::max(0.001, S_new_safety(1)); - J_safety_marker.scale.y = ellipsoid_scale * safety_value_2; - - double safety_value_3 = std::max(0.001, S_new_safety(2)); - J_safety_marker.scale.z = ellipsoid_scale * safety_value_3; - - Eigen::Matrix3d R = U_safety.block<3,3>(0, 0); - - - Eigen::Quaterniond q_safety(R); - //normalize the quaternion - q_safety.normalize(); // optional if R is already perfectly orthonormal - J_safety_marker.pose.orientation.x = q_safety.x(); - J_safety_marker.pose.orientation.y = q_safety.y(); - J_safety_marker.pose.orientation.z = q_safety.z(); - J_safety_marker.pose.orientation.w = q_safety.w(); - - J_safety_marker.color.a = 0.7; - J_safety_marker.color.r = 1.0; - J_safety_marker.color.g = 0.0; - J_safety_marker.color.b = 0.0; - - // Add the J_safety_marker to the marker array - marker_array.markers.push_back(J_safety_marker); - - - // Determine if J_proj and J_singular exist - bool J_proj_exists = false; - bool J_singular_exists = false; - int number_of_singular_directions = 0; - for (int i = 0; i < 3; ++i) { - if (jparse_singular_index[i] == 0) { - // some directions are non-singular - J_proj_exists = true; - } else if (jparse_singular_index[i] == 1) { - // some directions are singular - J_singular_exists = true; - number_of_singular_directions++; - } - } - - - - - //2. setup the J_proj_marker if it exists - if(J_proj_exists==true){ - // Set up the J_proj_marker - J_proj_marker.header = header; - J_proj_marker.header.frame_id = root_; - J_proj_marker.ns = "J_proj"; - J_proj_marker.id = 1; - J_proj_marker.type = visualization_msgs::Marker::SPHERE; - J_proj_marker.action = visualization_msgs::Marker::ADD; - - - if(jparse_singular_index[0]==0){ - double proj_value_1 = std::max(0.001, S_new_proj(0)); - J_proj_marker.scale.x = ellipsoid_scale * proj_value_1; - }else{ - J_proj_marker.scale.x = 0.001; - } - - if(jparse_singular_index[1]==0){ - double proj_value_2 = std::max(0.001, S_new_proj(1)); - J_proj_marker.scale.y = ellipsoid_scale * proj_value_2; - }else{ - J_proj_marker.scale.y = 0.001; - } - - if(jparse_singular_index[2]==0){ - double proj_value_3 = std::max(0.001, S_new_proj(2)); - J_proj_marker.scale.z = ellipsoid_scale * proj_value_3; - }else{ - J_proj_marker.scale.z = 0.001; - } - - J_proj_marker.pose.position.x = transformStamped.transform.translation.x; - J_proj_marker.pose.position.y = transformStamped.transform.translation.y; - J_proj_marker.pose.position.z = transformStamped.transform.translation.z; - - Eigen::Matrix3d R_proj = U_safety.block<3,3>(0, 0); - Eigen::Quaterniond q_proj(R_proj); - - //normalize the quaternion - q_proj.normalize(); // optional if R is already perfectly orthonormal - J_proj_marker.pose.orientation.x = q_proj.x(); - J_proj_marker.pose.orientation.y = q_proj.y(); - J_proj_marker.pose.orientation.z = q_proj.z(); - J_proj_marker.pose.orientation.w = q_proj.w(); - - J_proj_marker.color.a = 0.7; - J_proj_marker.color.r = 0.0; - J_proj_marker.color.g = 0.0; - J_proj_marker.color.b = 1.0; - } - - // Add the J_proj_marker to the marker array - if(J_proj_exists==true){ - marker_array.markers.push_back(J_proj_marker); - } - - - //3. setup the J_singular_marker if it exists - - if (J_singular_exists) - { - // Extract end-effector position once - geometry_msgs::Point ee_pos; - ee_pos.x = transformStamped.transform.translation.x; - ee_pos.y = transformStamped.transform.translation.y; - ee_pos.z = transformStamped.transform.translation.z; - - double arrow_scale = 1.0; - - for (int idx = 0; idx < number_of_singular_directions; ++idx) - { - visualization_msgs::Marker marker; - // --- Header & identity --- - marker.header = header; // copy stamp & frame_id - marker.header.frame_id = root_; - marker.ns = "J_singular"; - marker.id = idx + 2; - marker.type = visualization_msgs::Marker::ARROW; - marker.action = visualization_msgs::Marker::ADD; - marker.lifetime = ros::Duration(0.1); - - // --- Start point --- - geometry_msgs::Point start_point = ee_pos; - - // --- Decide arrow direction --- - Eigen::Vector3d u_col = U_new_sing.block<3,1>(0, idx); // first 3 rows of column - double dot = ee_pos.x * u_col.x() - + ee_pos.y * u_col.y() - + ee_pos.z * u_col.z(); - - Eigen::Vector3d arrow_dir = (dot < 0.0 ? u_col : -u_col); - - // --- End point --- - double mag = std::abs(Phi(idx)); - geometry_msgs::Point end_point; - end_point.x = ee_pos.x + arrow_scale * arrow_dir.x() * mag; - end_point.y = ee_pos.y + arrow_scale * arrow_dir.y() * mag; - end_point.z = ee_pos.z + arrow_scale * arrow_dir.z() * mag; - - // --- Push points (clear just in case) --- - marker.points.clear(); - marker.points.push_back(start_point); - marker.points.push_back(end_point); - - // --- Fixed arrow sizing --- - marker.scale.x = 0.01; // shaft diameter - marker.scale.y = 0.05; // head diameter - marker.scale.z = 0.05; // head length - - // --- Color = solid red --- - marker.color.r = 1.0; - marker.color.g = 0.0; - marker.color.b = 0.0; - marker.color.a = 1.0; - - marker_array.markers.push_back(marker); - } - - } - - //publish the marker array - jparse_markers_pub_.publish(marker_array); -} - -bool JPARSE::handleJparse( - manipulator_control::JParseSrv::Request& req, - manipulator_control::JParseSrv::Response& res) - { - // find the jacobian for the joints specified in the request - - std::string root_service = req.base_link_name; - std::string tip_service = req.end_link_name; - std::vector joint_names; - double gamma_service = req.gamma; - double singular_direction_gain_position_service = req.singular_direction_gain_position; - double singular_direction_gain_angular_service = req.singular_direction_gain_angular; - sensor_msgs::JointStateConstPtr msg; - - ROS_INFO("JPARSE service: Received request for base_link: %s, end_link: %s", root_service.c_str(), tip_service.c_str()); - //setup the KDL chain - if (!kdl_tree_.getChain(root_service, tip_service, kdl_chain_service_)) - { - ROS_ERROR("Failed to extract KDL chain from %s to %s", root_service.c_str(), tip_service.c_str()); - return false; - } - jac_solver_service_.reset(new KDL::ChainJntToJacSolver(kdl_chain_service_)); - - KDL::JntArrayVel positions = KDL::JntArrayVel(kdl_chain_service_.getNrOfJoints()); - - KDL::SetToZero(positions.q); - KDL::SetToZero(positions.qdot); - - for (size_t i = 0; i < kdl_chain_service_.getNrOfSegments(); ++i) - { - KDL::Joint joint = kdl_chain_service_.getSegment(i).getJoint(); - if (joint.getType() != KDL::Joint::None) - joint_names.push_back(joint.getName()); - } - - - //send over the copy of the joint state message - { - std::lock_guard guard(last_msg_mutex_); - if (!have_last_msg_) { - ROS_WARN("No joint state yet, refusing service"); - return false; - } - msg = joint_state_msg_service_; - have_last_msg_ = false; - joint_state_msg_service_.reset(); - } - - std::vector q, dq; - for (const auto& joint_name : joint_names) - { - auto it = std::find(msg->name.begin(), msg->name.end(), joint_name); - if (it != msg->name.end()) - { - size_t idx = std::distance(msg->name.begin(), it); - q.push_back(msg->position[idx]); - dq.push_back(msg->velocity[idx]); - } - } - - if (q.size() != joint_names.size()) - { - ROS_ERROR("Joint state message does not contain all joint names"); - return false; - } - - for (size_t i = 0; i < joint_names.size(); ++i) - { - positions.q(i) = q[i]; - positions.qdot(i) = dq[i]; - } - - KDL::Jacobian J_kdl(joint_names.size()); - jac_solver_service_->JntToJac(positions.q, J_kdl); - Eigen::MatrixXd J = J_kdl.data; - - // Now run usual pipeline on J - Eigen::MatrixXd J_parse, J_safety_nullspace; - - //handle any kinematic chain size - Eigen::JacobiSVD svd(J, Eigen::ComputeFullU | Eigen::ComputeFullV); - int n = svd.singularValues().size(); - std::vector jparse_singular_index(n, 0);// Elements in this list are 0 if non-singular, 1 if singular - - Eigen::MatrixXd U_safety, U_new_proj, U_new_sing; - Eigen::VectorXd S_new_safety, S_new_proj, Phi; - - Jparse_calculation(J, J_parse, J_safety_nullspace, jparse_singular_index, U_safety, S_new_safety, U_new_proj, S_new_proj, U_new_sing, Phi, gamma_service, singular_direction_gain_position_service, singular_direction_gain_angular_service); - - // Fill response - matrix_to_msg(J_parse, res.jparse); - matrix_to_msg(J_safety_nullspace, res.jsafety_nullspace); - return true; - } - -void JPARSE::matrix_to_msg(const Eigen::MatrixXd& mat, std::vector& msg_rows) -{ - msg_rows.clear(); - for (int i = 0; i < mat.rows(); ++i) - { - manipulator_control::Matrow row; - for (int j = 0; j < mat.cols(); ++j) - { - row.row.push_back(mat(i, j)); - } - msg_rows.push_back(row); - } -} - -void JPARSE::Publish_JPARSE(const std_msgs::Header& header, const Eigen::MatrixXd& J_parse, const Eigen::MatrixXd& J_safety_nullspace) -{ - manipulator_control::JparseTerms msg; - msg.header = header; - - matrix_to_msg(J_parse, msg.jparse); - matrix_to_msg(J_safety_nullspace, msg.jsafety_nullspace); - - jparse_pub_.publish(msg); -} - - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "jparse_cpp_node"); - ros::NodeHandle nh; - JPARSE parser(nh); - - ros::AsyncSpinner spinner(2); // AsyncSpinner with 2 threads lets your subscriber and service, callbacks run concurrently. - spinner.start(); - ros::waitForShutdown(); - return 0; -} - diff --git a/manipulator_control_ros1/src/manipulator_control/jparse_cls.py b/manipulator_control_ros1/src/manipulator_control/jparse_cls.py deleted file mode 100755 index c4fc2e3..0000000 --- a/manipulator_control_ros1/src/manipulator_control/jparse_cls.py +++ /dev/null @@ -1,549 +0,0 @@ -#!/usr/bin/env python3 -import rospy -import numpy as np -#access pykdl for jacobian and other matricies -import PyKDL as kdl -from urdf_parser_py.urdf import Robot -from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model -from pykdl_utils.kdl_kinematics import KDLKinematics -from pykdl_utils.kdl_kinematics import kdl_to_mat -from pykdl_utils.kdl_kinematics import joint_list_to_kdl -#for plotting ellipsoids -from visualization_msgs.msg import Marker, MarkerArray -from geometry_msgs.msg import Quaternion, Pose, Point, PoseStamped - - -class JParseClass(object): - def __init__(self, base_link="base_link", end_link="end_effector_link"): - # Initialize any necessary variables or parameters here - """ - Base link: The base link of the robot. - End link: The end link of the robot. - """ - self.base_link = base_link - self.end_link = end_link - self.urdf = Robot.from_parameter_server() - self.kdl_tree = kdl_tree_from_urdf_model(self.urdf) - self.chain = self.kdl_tree.getChain(base_link, end_link) - self._fk_kdl = kdl.ChainFkSolverPos_recursive(self.chain) - self._ik_v_kdl = kdl.ChainIkSolverVel_pinv(self.chain) - self._ik_p_kdl = kdl.ChainIkSolverPos_NR(self.chain, self._fk_kdl, self._ik_v_kdl) - self._jac_kdl = kdl.ChainJntToJacSolver(self.chain) - self._dyn_kdl = kdl.ChainDynParam(self.chain, kdl.Vector(0,0,-9.81)) - self.kdl_kin = KDLKinematics(self.urdf, base_link, end_link) - self.num_joints = self.kdl_kin.num_joints - self.joint_names = self.kdl_kin.get_joint_names() - self.marker_pub = rospy.Publisher('/jparse_ellipsoid_marker', MarkerArray, queue_size=10) - - self.J_prev = None - self.J_prev_time = None - - def jacobian(self, q=[]): - """ - Computes the Jacobian matrix for the given joint configuration. - - Parameters: - q (list): A list of joint positions. - - Returns: - numpy.ndarray: The Jacobian matrix as a numpy array. - - This function converts the joint positions to a KDL joint array, computes the Jacobian using KDL, - and then converts the resulting KDL Jacobian matrix to a numpy array. - """ - j_kdl = kdl.Jacobian(self.num_joints) - q_kdl = joint_list_to_kdl(q) - self._jac_kdl.JntToJac(q_kdl, j_kdl) - J_mat = kdl_to_mat(j_kdl) #converts kdl to numpy matrix - return J_mat - - def jacobian_dot(self, q=[], position_only=False , approx=True): - """ - Computes the time derivative of the Jacobian matrix for the given joint configuration and joint velocities. - - Parameters: - q (list): A list of joint positions. - - Returns: - numpy.ndarray: The time derivative of the Jacobian matrix as a numpy array. - """ - J = self.jacobian(q) - if position_only == True: - #if we are only interested in the position, return the first 3 rows of the jacobian - J= J[:3, :] - - if approx == True: - #This is the approximate method for calculating the time derivative of the jacobian - J_dot = np.zeros(J.shape) - q_plus = q.copy() - q_minus = q.copy() - for i in range(self.num_joints): - q_plus[i] += 0.0001 - q_minus[i] -= 0.0001 - J_plus = self.jacobian(q_plus) - J_minus = self.jacobian(q_minus) - J_dot = (J_plus - J_minus) / 0.0002 - return J_dot - else: - #This is the exact method for calculating the time derivative of the jacobian - if self.J_prev is None: - self.J_prev = J - self.J_prev_time = rospy.Time.now() - J_dot = np.zeros(J.shape) - else: - dt = (rospy.Time.now() - self.J_prev_time).to_sec() - J_dot = (J - self.J_prev) / dt - self.J_prev = J - return J_dot - - def svd_compose(self,U,S,Vt): - """ - This function takes SVD: U,S,V and recomposes them for J - """ - Zero_concat = np.zeros((U.shape[0],Vt.shape[0]-len(S))) - Sfull = np.zeros((U.shape[1],Vt.shape[0])) - for row in range(Sfull.shape[0]): - for col in range(Sfull.shape[1]): - if row == col: - if row < len(S): - Sfull[row,col] = S[row] - J_new =np.matrix(U)*Sfull*np.matrix(Vt) - return J_new - - def manipulability_measure(self, q=[], use_inv_cond_number=False): - """ - This function computes the manipulability measure for the given joint configuration. - """ - #if we are using the manipulability measure, return that - J = self.jacobian(q) - return np.sqrt(np.linalg.det(J @ J.T)) - - def inverse_condition_number(self, q=[]): - """ - This function computes the inverse of the condition number for the given joint configuration. - """ - J = self.jacobian(q) - U, S, Vt = np.linalg.svd(J) - #find the min and max singular values - sigma_min = np.min(S) - sigma_max = np.max(S) - inv_cond_number = sigma_min/sigma_max - return inv_cond_number - - def JacobianPseudoInverse(self, q=[], position_only=False, jac_nullspace_bool = False): - """ - This function computes the pseudo-inverse of the Jacobian matrix for the given joint configuration. - """ - J = self.jacobian(q) - if position_only == True: - #if we are only interested in the position, return the first 3 rows of the jacobian - J= J[:3, :] - J_pinv = np.linalg.pinv(J) - if jac_nullspace_bool == True: - #Find the nullspace of the jacobian - J_nullspace = np.eye(J.shape[1]) - np.linalg.pinv(J) @ J - return J_pinv, J_nullspace - return J_pinv - - def JParse(self, q=[], gamma=0.1, position_only=False, jac_nullspace_bool = False , singular_direction_gain_position=1, singular_direction_gain_angular=1, verbose=False, publish_jparse_ellipses=False, internal_marker_flag=False, end_effector_pose=None, safety_only = False, safety_projection_only = False): - """ - This function computes the JParse matrix for the given joint configuration and gamma value. - This function should return the JParse matrix as a numpy array. - - gamma is the threshold value for the adjusted condition number. - - position_only is a boolean flag to indicate whether to use only the position part of the Jacobian. - - singular_direction_gain is the gain for the singular direction projection matrix. Nominal values range from 1 to 10. - """ - #obtain the original jacobian - J = self.jacobian(q) - - if position_only == True: - #if we are only interested in the position, return the first 3 rows of the jacobian - J= J[:3, :] - - #Perform the SVD decomposition of the jacobian - U, S, Vt = np.linalg.svd(J) - #Find the adjusted condition number - sigma_max = np.max(S) - adjusted_condition_numbers = [sig / sigma_max for sig in S] - - if verbose == True: - print("adjusted_condition_numbers:", adjusted_condition_numbers) - #Find the projection Jacobian - U_new_proj = [] - S_new_proj = [] - for col in range(len(S)): - if S[col] > gamma*sigma_max: - #Singular row - U_new_proj.append(np.matrix(U[:,col]).T) - S_new_proj.append(S[col]) - U_new_proj = np.concatenate(U_new_proj,axis=0).T - J_proj = self.svd_compose(U_new_proj, S_new_proj, Vt) - - #Find the safety jacboian - S_new_safety = [s if (s/sigma_max) > gamma else gamma*sigma_max for s in S] - J_safety = self.svd_compose(U,S_new_safety,Vt) - - #Find the singular direction projection components - U_new_sing = [] - Phi = [] #these will be the ratio of s_i/s_max - set_empty_bool = True - for col in range(len(S)): - if adjusted_condition_numbers[col] <= gamma: - set_empty_bool = False - U_new_sing.append(np.matrix(U[:,col]).T) - Phi.append(adjusted_condition_numbers[col]/gamma) #division by gamma for s/(s_max * gamma), gives smooth transition for Kp =1.0; - - #set an empty Phi_singular matrix, populate if there were any adjusted - #condition numbers below the threshold - Phi_singular = np.zeros(U.shape) #initialize the singular projection matrix - - if verbose == True: - print("set_empty_bool:", set_empty_bool) - if set_empty_bool == False: - #construct the new U, as there were singular directions - U_new_sing = np.matrix(np.concatenate(U_new_sing,axis=0)).T - Phi_mat = np.matrix(np.diag(Phi)) - if position_only == True: - gains = np.array([singular_direction_gain_position]*3, dtype=float) - else: - gains = np.array([singular_direction_gain_position]*3 + [singular_direction_gain_angular]*3, dtype=float) - Kp_singular = np.diag(gains) - # Now put it all together: - Phi_singular = U_new_sing @ Phi_mat @ U_new_sing.T @ Kp_singular - if verbose == True: - print("Kp_singular:", Kp_singular) - print("Phi_mat shape:", Phi_mat.shape, "Phi_mat:", Phi_mat) - print("U_new_sing shape:", U_new_sing.shape, "U_new_sing:", U_new_sing) - - #Obtain psuedo-inverse of the safety jacobian and the projection jacobian - J_safety_pinv = np.linalg.pinv(J_safety) - J_proj_pinv = np.linalg.pinv(J_proj) - - if set_empty_bool == False: - J_parse = J_safety_pinv @ J_proj @ J_proj_pinv + J_safety_pinv @ Phi_singular - else: - J_parse = J_safety_pinv @ J_proj @ J_proj_pinv - - #Publish the JParse ellipses - ellipse_dict = {"J_safety_u": U, "J_safety_s": S_new_safety, "J_proj_u": U_new_proj, "J_proj_s": S_new_proj, "J_singular_u": U_new_sing, "J_singular_s": Phi} - if internal_marker_flag == True: - #this is internal for jparse marker display - return ellipse_dict - - if publish_jparse_ellipses == True: - #only shows position ellipses - self.publish_position_Jparse_ellipses(q=q, gamma=gamma, jac_nullspace_bool=False, singular_direction_gain_position=singular_direction_gain_position, singular_direction_gain_angular=singular_direction_gain_angular, verbose=verbose, publish_jparse_ellipses=publish_jparse_ellipses, end_effector_pose=end_effector_pose) - - if jac_nullspace_bool == True and not safety_only and not safety_projection_only: - #Find the nullspace of the jacobian - J_safety_nullspace = np.eye(J_safety.shape[1]) - J_safety_pinv @ J_safety - - return J_parse, J_safety_nullspace - - if safety_only == True: - return J_safety_pinv - - if safety_projection_only == True: - return J_safety_pinv @ J_proj @ J_proj_pinv - - return J_parse - - ''' - The following are only useful for the dynamics terms (e.g. torque control) - They are not required to obtain the JParse matrix - ''' - def publish_position_Jparse_ellipses(self, q=[], gamma=0.1, jac_nullspace_bool = False , singular_direction_gain_position=1, singular_direction_gain_angular=1, verbose=False, publish_jparse_ellipses=False, end_effector_pose=None): - """ - this function displays the key ellipses for the JParse - """ - #obtain the key matricies - ellipse_marker_array = MarkerArray() - - ellipse_dict = self.JParse(q=q, gamma=gamma, position_only=True, jac_nullspace_bool=jac_nullspace_bool, singular_direction_gain_position=singular_direction_gain_position, singular_direction_gain_angular=singular_direction_gain_angular, verbose=verbose, publish_jparse_ellipses=False, internal_marker_flag=True, end_effector_pose=end_effector_pose) - frame_id = self.base_link - #add safety jacobian - J_safety_marker = self.generate_jparse_ellipses(mat_type="J_safety", U_mat=ellipse_dict["J_safety_u"], S_vect=ellipse_dict["J_safety_s"], marker_ns="J_safety", end_effector_pose=end_effector_pose, frame_id=frame_id) - ellipse_marker_array.markers.append(J_safety_marker[0]) - #if there are feasible directions, add J_proj - if len(ellipse_dict["J_proj_s"]) > 0: - #Pass in the full U to ensure the ellipse shows up at all, we will handle with approximate scaling for singular directions (make them 0.001) - J_proj_marker = self.generate_jparse_ellipses(mat_type="J_proj", U_mat=ellipse_dict["J_safety_u"], S_vect=ellipse_dict["J_proj_s"], marker_ns="J_proj", end_effector_pose=end_effector_pose, frame_id=frame_id) - ellipse_marker_array.markers.append(J_proj_marker[0]) - #if there are singular directions, add them - J_singular_marker = self.generate_jparse_ellipses(mat_type="J_singular", U_mat=ellipse_dict["J_singular_u"], S_vect=ellipse_dict["J_singular_s"], end_effector_pose=end_effector_pose, frame_id=frame_id) - if len(J_singular_marker) > 0: - for idx in range(len(J_singular_marker)): - ellipse_marker_array.markers.append(J_singular_marker[idx]) - self.marker_pub.publish(ellipse_marker_array) - - def generate_jparse_ellipses(self, mat_type=None, U_mat=None, S_vect=None, marker_ns="ellipse" , frame_id="base_link", end_effector_pose=None): - #This function takes in the singular value directions and plots ellipses or vectors - Marker_list = [] - pose = PoseStamped() - if mat_type in ["J_safety", "J_proj"]: - marker = Marker() - #Create the marker - marker.header.frame_id = frame_id - marker.header.stamp = rospy.Time.now() - marker.ns = marker_ns - if mat_type == "J_safety": - marker.id = 0 - marker_ns = "J_safety" - elif mat_type == "J_proj": - marker.id = 1 - marker_ns = "J_proj" - marker.type = Marker.SPHERE - marker.action = Marker.ADD - - #set the position of the marker - marker.pose.position.x = end_effector_pose.pose.position.x - marker.pose.position.y = end_effector_pose.pose.position.y - marker.pose.position.z = end_effector_pose.pose.position.z - - #set the scale based on singular values (adjust factor if needed); in practice, the ellipsoid should flatten, but for easy visualization, we make these dimensions very very small - ellipsoid_scale = 0.25 - marker.scale.x = 0.001 - marker.scale.y = 0.001 - marker.scale.z = 0.001 - if len(S_vect) == 1: - marker.scale.x = ellipsoid_scale*np.max([S_vect[0], 0.001]) - elif len(S_vect) == 2: - marker.scale.x = ellipsoid_scale*np.max([S_vect[0], 0.001]) - marker.scale.y = ellipsoid_scale*np.max([S_vect[1], 0.001]) - elif len(S_vect) >= 3: - marker.scale.x = ellipsoid_scale*np.max([S_vect[0], 0.001]) - marker.scale.y = ellipsoid_scale*np.max([S_vect[1], 0.001]) - marker.scale.z = ellipsoid_scale*np.max([S_vect[2], 0.001]) - - # Convert U (rotation matrix) to quaternion - q = self.rotation_matrix_to_quaternion(U_mat) - marker.pose.orientation = q - - # Set color (RGBA) - if mat_type == "J_safety": - marker.color.r = 1.0 - marker.color.g = 0.0 - marker.color.b = 0.0 - marker.color.a = 0.7 #transparency - elif mat_type == "J_proj": - marker.color.r = 0.0 - marker.color.g = 0.0 - marker.color.b = 1.0 - marker.color.a = 0.7 #transparency - Marker_list.append(marker) - - elif mat_type == "J_singular": - - for idx in range(len(S_vect)): - #Create the marker - marker = Marker() - marker_ns = "J_singular" - marker.header.frame_id = frame_id - marker.header.stamp = rospy.Time.now() - marker.ns = marker_ns - marker.id = idx+2 - marker.type = Marker.ARROW - marker.action = Marker.ADD - marker.lifetime = rospy.Duration(0.1) # or rclpy.duration.Duration(seconds=0.1) in ROS 2 - - #Arrow start point - start_point = Point() - start_point.x = end_effector_pose.pose.position.x - start_point.y = end_effector_pose.pose.position.y - start_point.z = end_effector_pose.pose.position.z - - #arrow end point - arrow_scale = 1.0 - end_point = Point() - # if points away from origin - if (end_effector_pose.pose.position.x*U_mat[0,idx] + end_effector_pose.pose.position.y*U_mat[1,idx] + end_effector_pose.pose.position.z*U_mat[2,idx]) < 0: - arrow_x = U_mat[0,idx] - arrow_y = U_mat[1,idx] - arrow_z = U_mat[2,idx] - else: - arrow_x = -U_mat[0,idx] - arrow_y = -U_mat[1,idx] - arrow_z = -U_mat[2,idx] - end_point.x = end_effector_pose.pose.position.x + arrow_scale*arrow_x*np.abs(S_vect[idx]) - end_point.y = end_effector_pose.pose.position.y + arrow_scale*arrow_y*np.abs(S_vect[idx]) - end_point.z = end_effector_pose.pose.position.z + arrow_scale*arrow_z*np.abs(S_vect[idx]) - - marker.points.append(start_point) - marker.points.append(end_point) - - # Scale (arrow thickness and head size) - marker.scale.x = 0.01 # Shaft diameter - marker.scale.y = 0.05 # Head diameter - marker.scale.z = 0.05 # Head length - - # Set color based on principal axis - marker.color.r = 1.0 - marker.color.g = 0.0 - marker.color.b = 0.0 - marker.color.a = 1.0 # Opaque - - Marker_list.append(marker) - - return Marker_list - - def rotation_matrix_to_quaternion(self, R): - """ - Convert a 3x3 rotation matrix to a quaternion. - :param R: 3x3 rotation matrix - :return: geometry_msgs/Quaternion - """ - q = Quaternion() - trace = np.trace(R) - - if trace > 0: - S = np.sqrt(trace + 1.0) * 2 - qw = 0.25 * S - qx = (R[2, 1] - R[1, 2]) / S - qy = (R[0, 2] - R[2, 0]) / S - qz = (R[1, 0] - R[0, 1]) / S - elif (R[0, 0] > R[1, 1]) and (R[0, 0] > R[2, 2]): - S = np.sqrt(1.0 + R[0, 0] - R[1, 1] - R[2, 2]) * 2 - qw = (R[2, 1] - R[1, 2]) / S - qx = 0.25 * S - qy = (R[0, 1] + R[1, 0]) / S - qz = (R[0, 2] + R[2, 0]) / S - elif R[1, 1] > R[2, 2]: - S = np.sqrt(1.0 + R[1, 1] - R[0, 0] - R[2, 2]) * 2 - qw = (R[0, 2] - R[2, 0]) / S - qx = (R[0, 1] + R[1, 0]) / S - qy = 0.25 * S - qz = (R[1, 2] + R[2, 1]) / S - else: - S = np.sqrt(1.0 + R[2, 2] - R[0, 0] - R[1, 1]) * 2 - qw = (R[1, 0] - R[0, 1]) / S - qx = (R[0, 2] + R[2, 0]) / S - qy = (R[1, 2] + R[2, 1]) / S - qz = 0.25 * S - - # Normalize quaternion - norm = np.sqrt(qw**2 + qx**2 + qy**2 + qz**2) - q.x = qx / norm - q.y = qy / norm - q.z = qz / norm - q.w = qw / norm - return q - - def cart_inertia(self, q=[]): - """ - This is not needed for the main JParse class, but is included here for reference. - """ - H = self.inertia(q) - J = self.jacobian(q) - J_pinv = np.linalg.pinv(J) - J_pinv_transpose = J_pinv.T - return J_pinv_transpose @ H @ J_pinv - - def inertia(self, q=[]): - """ - This is not needed for the main JParse class, but is included here for reference. - """ - h_kdl = kdl.JntSpaceInertiaMatrix(self.num_joints) - self._dyn_kdl.JntToMass(joint_list_to_kdl(q), h_kdl) - return kdl_to_mat(h_kdl) - - def coriolis(self,q=[], qdot=[]): - """ - This is not needed for the main JParse class, but is included here for reference. - """ - q = q #list - qdot = qdot #list - q_cori = [0.0 for idx in range(len(q))] - q_kdl = joint_list_to_kdl(q) - qdot_kdl = joint_list_to_kdl(qdot) - q_cori_kdl = joint_list_to_kdl(q_cori) - self._dyn_kdl.JntToCoriolis(q_kdl, qdot_kdl, q_cori_kdl) - q_cori_kdl = np.array([q_cori_kdl[i] for i in range(q_cori_kdl.rows())]) - q_cori_kdl = np.matrix(q_cori_kdl).T - return q_cori_kdl - - def gravity(self,q=[]): - """ - This is not needed for the main JParse class, but is included here for reference. - """ - q_grav = [0.0 for idx in range(len(q))] - q_kdl = joint_list_to_kdl(q) - q_grav_kdl = joint_list_to_kdl(q_grav) - self._dyn_kdl.JntToGravity(q_kdl,q_grav_kdl) - q_grav_kdl = np.array([q_grav_kdl[i] for i in range(q_grav_kdl.rows())]) - q_grav_kdl = np.matrix(q_grav_kdl).T - return q_grav_kdl - - ''' - Baseline implementations for comparison - ''' - def jacobian_damped_least_squares(self, q=[], damping=0.01, jac_nullspace_bool=False): - """ - COMPARISON METHOD (not used in JPARSE) - This function computes the damped least squares pseudo-inverse of the Jacobian matrix for the given joint configuration. - """ - J = self.jacobian(q) - J_dls = np.linalg.inv(J.T @ J + damping**2 * np.eye(J.shape[1])) @ J.T - if jac_nullspace_bool == False: - return J_dls - J_dls_nullspace = np.eye(J.shape[1]) - J_dls @ J - return J_dls, J_dls_nullspace - - def jacobian_transpose(self, q=[]): - """ - COMPARISON METHOD (not used in JPARSE) - This function computes the transpose of the Jacobian matrix for the given joint configuration. - """ - J = self.jacobian(q) - J_transpose = J.T - J_transpose_nullspace = np.eye(J_transpose.shape[0]) - J_transpose @ np.linalg.pinv(J_transpose) - return J_transpose, J_transpose_nullspace - - def jacobian_projection(self, q=[], gamma=0.1, jac_nullspace_bool=False): - """ - This function computes the projection of the Jacobian matrix onto feasible (non-singular) directions for the given joint configuration. - """ - J = self.jacobian(q) - #Perform the SVD decomposition of the jacobian - U, S, Vt = np.linalg.svd(J) - #Find the adjusted condition number - sigma_max = np.max(S) - #Find the projection Jacobian - U_new_proj = [] - S_new_proj = [] - for col in range(len(S)): - if S[col] > gamma*sigma_max: - U_new_proj.append(np.matrix(U[:,col]).T) - S_new_proj.append(S[col]) - else: - rospy.loginfo("Singular row found during projection") - print("the ratio of s_i/s_max is:", S[col]/sigma_max) - - U_new_proj = np.concatenate(U_new_proj,axis=0).T - J_proj = self.svd_compose(U_new_proj, S_new_proj, Vt) - #if there is no nullspace, return the projection jacobian - if jac_nullspace_bool == False: - return J_proj - J_proj_nullspace = np.eye(J_proj.shape[1]) - np.linalg.pinv(J_proj)@J_proj - return J_proj, J_proj_nullspace - - -def main(): - rospy.init_node('jparse_test', anonymous=True) - jparse = JParseClass() - q = [0, 0, 0, 0, 0, 0] - print("JParse:", jparse.JParse(q=q)) - print("JParse:", jparse.JParse(q=q, position_only=True)) - print("Jacobian:", jparse.jacobian(q=q)) - print("Jacobian:", jparse.jacobian(q=q, position_only=True)) - print("Jacobian Pseudo Inverse:", jparse.JacobianPseudoInverse(q=q)) - print("Jacobian Pseudo Inverse:", jparse.JacobianPseudoInverse(q=q, position_only=True)) - print("Jacobian Damped Least Squares:", jparse.jacobian_damped_least_squares(q=q)) - print("Jacobian Damped Least Squares:", jparse.jacobian_damped_least_squares(q=q, jac_nullspace_bool=True)) - print("Jacobian Transpose:", jparse.jacobian_transpose(q=q)) - -if __name__ == '__main__': - try: - main() - except rospy.ROSInterruptException: - pass - diff --git a/manipulator_control_ros1/srv/JParseSrv.srv b/manipulator_control_ros1/srv/JParseSrv.srv deleted file mode 100644 index 03716b9..0000000 --- a/manipulator_control_ros1/srv/JParseSrv.srv +++ /dev/null @@ -1,8 +0,0 @@ -float64 gamma -float64 singular_direction_gain_position -float64 singular_direction_gain_angular -string base_link_name -string end_link_name ---- -manipulator_control/Matrow[] jparse -manipulator_control/Matrow[] jsafety_nullspace \ No newline at end of file diff --git a/xarm_ros2_base b/xarm_ros2_base new file mode 160000 index 0000000..3a08d61 --- /dev/null +++ b/xarm_ros2_base @@ -0,0 +1 @@ +Subproject commit 3a08d61a32b7459c1911d0be97247d4eda94df13 From 07c18a661704c468f73351f25d5329ec19c0fa05 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Tue, 19 Aug 2025 15:46:54 -0500 Subject: [PATCH 11/27] Edited forked teleop twist repo --- ros2_teleop_twist_keyboard | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2_teleop_twist_keyboard b/ros2_teleop_twist_keyboard index 2bc281a..73bf35c 160000 --- a/ros2_teleop_twist_keyboard +++ b/ros2_teleop_twist_keyboard @@ -1 +1 @@ -Subproject commit 2bc281a6904bc92f572977c7048312c24bcd85bf +Subproject commit 73bf35c5f41ede41c661d7f53fd6239da91beab8 From 1a03596fb14e7a11309e898672dae00a7a4ab1b6 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Tue, 19 Aug 2025 15:47:46 -0500 Subject: [PATCH 12/27] Adding submodule for xarm control interfaces --- xarm_ros2_base | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/xarm_ros2_base b/xarm_ros2_base index 3a08d61..9d054b5 160000 --- a/xarm_ros2_base +++ b/xarm_ros2_base @@ -1 +1 @@ -Subproject commit 3a08d61a32b7459c1911d0be97247d4eda94df13 +Subproject commit 9d054b577ee65f6aea4680da6ed80ea81756073e From 5b29b798b3218a2413ba3d8bf2ba2bec5b6ff501 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Tue, 19 Aug 2025 16:54:39 -0500 Subject: [PATCH 13/27] Snp tested on physical robot and working --- xarm_ros2_base | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/xarm_ros2_base b/xarm_ros2_base index 9d054b5..e5c4af8 160000 --- a/xarm_ros2_base +++ b/xarm_ros2_base @@ -1 +1 @@ -Subproject commit 9d054b577ee65f6aea4680da6ed80ea81756073e +Subproject commit e5c4af865d83f310878afebaf88eb817e7f7b7b5 From 21775e7dd1ab20089d95803b7975c8f4b5a249de Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Tue, 19 Aug 2025 17:20:24 -0500 Subject: [PATCH 14/27] Joystick tested on physical robot and working --- xarm_ros2_base | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/xarm_ros2_base b/xarm_ros2_base index e5c4af8..09f2c8f 160000 --- a/xarm_ros2_base +++ b/xarm_ros2_base @@ -1 +1 @@ -Subproject commit e5c4af865d83f310878afebaf88eb817e7f7b7b5 +Subproject commit 09f2c8f332bfc87c4f352d0ae88e2f8bc4e55bfa From 6ead803ab1c91f0d6668be357a1232b7ccff601d Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Wed, 20 Aug 2025 11:43:28 -0500 Subject: [PATCH 15/27] Cleaned up the code and added more information in README --- README.md | 28 +++- manipulator_control/README.md | 5 +- .../scripts/xarm_vel_main_experiment.py | 137 +----------------- 3 files changed, 32 insertions(+), 138 deletions(-) diff --git a/README.md b/README.md index f362a0e..5222eb9 100644 --- a/README.md +++ b/README.md @@ -19,19 +19,38 @@ _In Submission_ ## ROS 2 DEV!!! The `main` branch is supporting *ROS Noetic*. -The `ros2_dev` branch is developing the support for *ROS 2 Jazzy* (coming soon). +The `ros2_dev` branch is supporting *ROS 2 Jazzy*. ## FOR ARGALLAB (ROS 2) + +### Joystick or SNP Teleop +``` +ros2 launch xarm_moveit_config xarm7_moveit_realmove.launch.py robot_ip:= add_gripper:=true + +ros2 launch manipulator_control xarm_main_vel.launch use_teleop_control:=true use_teleop_control_jparse:=true + +ros2 launch xarm_teleop xarm_teleop.launch.xml JOY:=true paradigm:=3 +``` + +``` +ros2 launch xarm_moveit_config xarm7_moveit_realmove.launch.py robot_ip:= add_gripper:=true + +ros2 launch manipulator_control xarm_main_vel.launch use_teleop_control:=true use_teleop_control_jparse:=true + +ros2 launch xarm_teleop xarm_teleop.launch.xml SNP:=true +``` + +### Keyboard Teleop Instructions to teleop robot with jparse and keyboard: -TODO: NEEDS TO BE TESTED and jparse_cls.py needs to be written! -Launch the following: +Launch the following for keyboard teleop: ``` ros2 launch xarm_moveit_config xarm7_moveit_realmove.launch.py robot_ip:= add_gripper:=true ros2 launch manipulator_control xarm_main_vel.launch use_teleop_control:=true use_teleop_control_jparse:=true ros2 run teleop_twist_keyboard teleop_twist_keyboard stamped:=True frame_id:=link_eef --ros-args --remap cmd_vel:=robot_action + ``` Edits need to be still commpleted to test the robot after writing script for joystick and sip/puff teleop. @@ -40,11 +59,10 @@ Edits need to be still commpleted to test the robot after writing script for joy - [x] Need to implement gripper in the `xarm_vel_experimenter.py` -- this is a function call to the api!!! the topic is `/gripper_action` - [ ] self-avoidance collision -- waiting for ros 2 port - [ ] work-around for the joint limits so that the robot does *not* immediately need to be power cycled -- waiting for ros 2 port -- [ ] port to ros2 (need pinnochio) -- WORKING ON!! +- [x] port to ros2 (need pinnochio) ### Instructions for Argallab -We will need to build the docker image ourself (but I guess sincewe now have it done once, need to remind myself to upload to argallab docker hub). Below are the instructions for building the image ourself and the docker container: ```sh cd /Docker diff --git a/manipulator_control/README.md b/manipulator_control/README.md index 366127d..cc5d12b 100644 --- a/manipulator_control/README.md +++ b/manipulator_control/README.md @@ -1,3 +1,6 @@ # manipulator control -Great video on creating ROS 2 package with both c++ and python nodes: https://www.youtube.com/watch?v=kAJXCoXrr5c \ No newline at end of file +Great video on creating ROS 2 package with both c++ and python nodes: https://www.youtube.com/watch?v=kAJXCoXrr5c + +## Important +Code developed by Assistive Robotics and Manipulation laboratory, Stanford University and adapted by Demiana Barsoum, Argallab, Northwestern University for ROS 2 in August 2025. Copyright (c) 2025. \ No newline at end of file diff --git a/manipulator_control/scripts/xarm_vel_main_experiment.py b/manipulator_control/scripts/xarm_vel_main_experiment.py index 7a650db..5c8b522 100644 --- a/manipulator_control/scripts/xarm_vel_main_experiment.py +++ b/manipulator_control/scripts/xarm_vel_main_experiment.py @@ -30,9 +30,7 @@ import jparse_cls -# Huge TODO: might need to implement the control loop as a timer callback instead of a while loop to avoid blocking the node (7/21/25) -# Update as of 8/1125 -- need to make sure that velocity_home can be called somewhere not in the callback and then also need to edit the control loop with the new -# arguments to Jparse from jparse_cls.py; then need to figure out also where to call it to run + class ArmController(Node): def __init__(self): super().__init__('arm_controller') @@ -111,71 +109,15 @@ def __init__(self): # Initialize the current positions self.current_positions = [0.0] * len(self.joint_names) - - ## NEW for pinocchio (8/8/2025) ## - # hard-coded for now - # urdf_filename = "/workspace/xarm7.urdf" - # urdf_filename = "xarm7.urdf.xacro" ## note to self: need to change this from xacro to just urdf usign the command "xacro xarm7.urdf.xacro > xarm7.urdf" -- however, the robot needs to be launched but this is not working currently when I tested it - # urdf_path = model_path + "/" + urdf_filename - - - # to test as wellquaternion_matrix - # urdf_string = self.get_parameter('robot_description').value - # robot = pin.RobotWrapper.BuildFromURDF(urdf_string) # need to figure out what this does differently from the one below - # self.model = robot.model - # self.data = robot.data - - # pin.buildModelsFromUrdf() - # Create model and data - # from IPython import embed; embed(banner1="looking at pin stuff") - # self.model, _, _ = pin.buildModelsFromUrdf(urdf_filename) #pin.Model() - # self.model = pin.buildModelFromUrdf(urdf_filename) #pin.Model() - # self.data = self.model.createData() - - # another way: - # from robot data self.rate = self.create_rate(10) # 10 Hz - # #home the robot - # try: - # self.velocity_home_robot() - # except Exception as e: - # self.get_logger().error(f"Failed to home the robot: {e}") - # pass - # finally: - # # Clean up resources if needed - # self.get_logger().info("Shutting down the control loop") - # joint_zero_velocities = [0.0] * len(self.joint_names) - # # self.timer = self.create_timer(10, self.command_joint_velocities(joint_zero_velocities)) - # self.command_joint_velocities(joint_zero_velocities) - #now run the control loop - # self.control_loop() - self.counter = 0 + self.timer = self.create_timer(0.01, self.timer_callback) def timer_callback(self): - # # if self.joint_states is not None: - # # self.velocity_home_robot() - # #home the robot - # try: - # self.counter += 1 - # self.get_logger().info(f'Timer triggered! Count: {self.counter}') - # self.velocity_home_robot() - # joint_zero_velocities = [0.0] * len(self.joint_names) - # self.timer = self.create_timer(10, self.command_joint_velocities(joint_zero_velocities)) - # self.command_joint_velocities(joint_zero_velocities) self.control_loop() - # except Exception as e: - # self.get_logger().error(f"Failed to home the robot: {e}") - # pass - # finally: - # # Clean up resources if needed - # self.get_logger().info("Shutting down the control loop") - # joint_zero_velocities = [0.0] * len(self.joint_names) - # self.command_joint_velocities(joint_zero_velocities) - # now run the control loop - # self.control_loop() + ######################################################################################################### ############################# SUBSCRIBERS AND PUBLISHERS INITIALIZATION ################################# @@ -188,14 +130,11 @@ def initialize_subscribers(self): else: joint_states_topic = '/joint_states' - self.get_logger().info(f"joint states topic name {joint_states_topic}") # subscribers # js_qos = QoSProfile(depth=10, durability=DurabilityPolicy.VOLATILE) js_cb_g = ReentrantCallbackGroup() self.joint_state_sub = self.create_subscription( JointState, joint_states_topic, self.joint_states_callback, 10, callback_group=js_cb_g) - # self.target_pose_sub = self.create_subscription( - # PoseStamped, '/target_pose', self.target_pose_callback, 1, callback_group=js_cb_g) self.teleop_control_sub = self.create_subscription( TwistStamped, 'robot_action', self.teleop_control_callback, 1, callback_group=js_cb_g) self.gripper_action_sub = self.create_subscription( @@ -226,7 +165,6 @@ def initialize_publishers(self): ########################################################################################################## def gripper_position_callback(self, msg): """Callback for the gripper. It includes the gripper position value. Float32 value.""" - # self.get_logger().info(msg) joystick_gripper_pose = msg.data # convert the joystick value to gripper poses [-1, 1] is to [0, 900], where 0 is close and 900 is open delta = joystick_gripper_pose * (self.gripper_speed / self.gripper_update_rate) @@ -240,9 +178,7 @@ def joint_states_callback(self, msg): and the corresponding joint velocities and efforts. This function will extract the joint positions, velocities, and efforts and return them as lists. """ - # self.get_logger().info(f"HELLLLLOOOO joint states callback {msg}") self.joint_states = msg - # self.velocity_home_robot() def teleop_control_callback(self, msg): """ @@ -260,19 +196,6 @@ def teleop_control_callback(self, msg): position_velocity = position_velocity / position_velocity_norm * 0.05 self.teleop_control_command = np.array([position_velocity[0], position_velocity[1],position_velocity[2],angular_velocity[0],angular_velocity[1],angular_velocity[2]]) #check if norm of the space mouse command is greater than 0.05, if so normalize it to this value - # #ensure we can get into the while loop - # if self.use_teleop_control == True: - # self.get_logger().info("in the teleop control callback") - # self.target_pose = PoseStamped() #set this to get in a loop - - # def target_pose_callback(self, msg): - # """ - # Callback function for the target_pose topic. This function will be called whenever a new message is received - # on the target_pose topic. The message is a geometry_msgs/PoseStamped message, which contains the target pose - # """ - # self.target_pose = msg - # self.current_target_pose_pub.publish(self.target_pose) #if target pose is paused manually, this allows us to track the current target pose seen by the script - ########################################################################################################## ####################################### HELPER FUNCTIONS ################################################# ########################################################################################################## @@ -302,17 +225,6 @@ def EndEffectorPose(self, q): self.current_end_effector_pose_pub.publish(current_pose) return current_pose - # not needed for the xarm control so commenting out - # def EndEffectorVelocity(self, q, dq): - # """ - # This function computes the end-effector velocity given the joint configuration q and joint velocities dq. - # """ - # J = self.jacobian_calculator.jacobian(q) - # J = np.array(J)duration - # dq = np.array(dq) - # dx = np.dot(J, dq) - # return dx - def rotation_matrix_to_axis_angle(self,R): """ Converts a rotation matrix to an axis-angle vector. @@ -366,10 +278,6 @@ def pose_error(self, current_pose, target_pose, error_norm_max = 0.05, control_p if np.linalg.norm(position_error) > error_norm_max: position_error = position_error / np.linalg.norm(position_error) * error_norm_max - #convert the quaternion in posestamped to list of quaternions then pass this into quaternion_matrix to get the rotation matrix. Access only the rotation part of the matrix - # goal_rotation_matrix = quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])[:3,:3] - # current_rotation_matrix = quaternion_matrix([current_pose.pose.orientation.x, current_pose.pose.orientation.y, current_pose.pose.orientation.z, current_pose.pose.orientation.w])[:3,:3] - # for ros 2 goal_quat_scripy = np.array([target_pose.pose.orientation.w, target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z]) current_rotation_quat_scipy = np.array([current_pose.pose.orientation.w, current_pose.pose.orientation.x, current_pose.pose.orientation.y, current_pose.pose.orientation.z]) @@ -439,19 +347,7 @@ def velocity_home_robot(self): This function commands the robot to the home position using joint velocities. """ # do the following in a loop for 5 seconds: - t_start = self.get_clock().now().nanoseconds - # duration = 0.0 - # while rclpy.ok(): - # while self.counter <= 10: - # if self.counter == 10: - # joint_zero_velocities = [0.0] * len(self.joint_names) - # self.command_joint_velocities(joint_zero_velocities) - # break - # # self.get_logger().info("Homing the robot") - # # self.get_logger().info(f"DEBUGGING: {self.joint_states}") if self.joint_states is not None: - # self.get_logger().info("IN THE IF STATMENT") - # duration = (self.get_clock().now().nanoseconds - t_start) / 1e9 q = [] dq = [] for joint_name in self.joint_names: @@ -554,8 +450,8 @@ def command_joint_velocities(self, joint_vel_list): # log the velocities if self.use_teleop_control_jparse: # self.get_logger().info(f"Joint velocities: {joint_vel_list}") - self.arm.vc_set_joint_velocity(joint_vel_list, is_radian=True) - self.arm.set_gripper_position(self.gripper_pose) + self.arm.vc_set_joint_velocity(joint_vel_list, is_radian=True) # arm command + self.arm.set_gripper_position(self.gripper_pose) # gripper command ############################################################################################################### ######################################### CONTROL LOOP ######################################################## @@ -582,7 +478,6 @@ def control_loop(self): effort.append(self.joint_states.effort[idx]) self.current_positions = q # Calculate the JParsed Jacobian - # from IPython import embed; embed(banner1="hellloooooo") # NEW # 8/8/2025 urdf_filename = "/workspace/xarm7.urdf" model = pin.buildModelFromUrdf(urdf_filename) # the model structure of the robot @@ -649,15 +544,6 @@ def control_loop(self): self.get_logger().info("Position only control") # position_error = np.matrix(position_error).T raise NotImplementedError - if self.is_sim == True: - #use these gains only in simulation! - kp_gain = 10.0 - else: - kp_gain = 3.0 - Kp = np.diag([kp_gain, kp_gain, kp_gain]) # Proportional gain matrix - - task_vector = Kp @ position_error - joint_velocities = J_method @ task_vector + J_nullspace @ nominal_motion_nullspace else: # realworld gains (tested) kp_gain_position = 1.0 @@ -675,25 +561,12 @@ def control_loop(self): #use the teleop control command to control the joint velocities teleop_control_command = np.matrix(self.teleop_control_command).T #now add this to the joint velocities - # print(f"J_method: {J_method} {np.array(J_method).shape}") - # print(f'teleop command: {teleop_control_command} {np.array(teleop_control_command).shape}') - # print(f'J_nullspace: {J_nullspace} {np.array(J_nullspace).shape}') - # print(f'nominal_motion_nullspace: {nominal_motion_nullspace} {np.array(nominal_motion_nullspace).shape}') joint_velocities = J_method @ teleop_control_command + J_nullspace @ nominal_motion_nullspace - #check this joint_velocities_list = np.array(joint_velocities).flatten().tolist() # command the joint velocities self.command_joint_velocities(joint_velocities_list) #this commands the joint velocities - # self.rate.sleep() self.get_logger().info("Control loop running") -# if __name__ == '__main__': -# try: -# ArmController() -# except Exception as e: -# print(f"An error occurred: {e}") -# pass - def main(args=None): rclpy.init(args=args) node = ArmController() From 510df082c86cea076ba3b1523db23b1ceb38ba47 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Wed, 20 Aug 2025 11:45:45 -0500 Subject: [PATCH 16/27] Updated readme --- xarm_ros2_base | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/xarm_ros2_base b/xarm_ros2_base index 09f2c8f..159160f 160000 --- a/xarm_ros2_base +++ b/xarm_ros2_base @@ -1 +1 @@ -Subproject commit 09f2c8f332bfc87c4f352d0ae88e2f8bc4e55bfa +Subproject commit 159160f4e2189d40f36b2fd9a732423439853dc9 From 9895f24f0f417d43382e05abf7383504f9e0908e Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Wed, 20 Aug 2025 11:48:41 -0500 Subject: [PATCH 17/27] Added teleop twist keyboard submodule in xarm_ros2_base with other control interfaces --- .gitmodules | 7 ------- ros2_teleop_twist_keyboard | 1 - xarm_ros2_base | 2 +- 3 files changed, 1 insertion(+), 9 deletions(-) delete mode 160000 ros2_teleop_twist_keyboard diff --git a/.gitmodules b/.gitmodules index dc7998d..1eb3ca1 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,10 +1,3 @@ -# [submodule "xarm_base"] -# path = xarm_base -# url = https://github.com/argallab/xarm_base.git -# branch = dev/jparse_ros2 -[submodule "ros2_teleop_twist_keyboard"] - path = ros2_teleop_twist_keyboard - url = https://github.com/argallab/ros2_teleop_twist_keyboard.git [submodule "xarm_ros2_base"] path = xarm_ros2_base url = https://github.com/argallab/xarm_ros2_base.git diff --git a/ros2_teleop_twist_keyboard b/ros2_teleop_twist_keyboard deleted file mode 160000 index 73bf35c..0000000 --- a/ros2_teleop_twist_keyboard +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 73bf35c5f41ede41c661d7f53fd6239da91beab8 diff --git a/xarm_ros2_base b/xarm_ros2_base index 159160f..abccc01 160000 --- a/xarm_ros2_base +++ b/xarm_ros2_base @@ -1 +1 @@ -Subproject commit 159160f4e2189d40f36b2fd9a732423439853dc9 +Subproject commit abccc01b226a8f003e1f8e23a35e64605319acbb From 98255700e75776e8efd7e92c03f13827e72e85a2 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Wed, 20 Aug 2025 14:58:31 -0500 Subject: [PATCH 18/27] Working on porting key_support including gui envs and key inputs from ROS to ROS 2 --- xarm_ros2_base | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/xarm_ros2_base b/xarm_ros2_base index abccc01..686cdb0 160000 --- a/xarm_ros2_base +++ b/xarm_ros2_base @@ -1 +1 @@ -Subproject commit abccc01b226a8f003e1f8e23a35e64605319acbb +Subproject commit 686cdb01bc71559777e39f7e2a4e016ef769af65 From 66a27a77259b113d2baff20a85293b6af1e38a17 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Wed, 20 Aug 2025 16:21:42 -0500 Subject: [PATCH 19/27] Testing ported code for gui and key_input --- xarm_ros2_base | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/xarm_ros2_base b/xarm_ros2_base index 686cdb0..b03d3ed 160000 --- a/xarm_ros2_base +++ b/xarm_ros2_base @@ -1 +1 @@ -Subproject commit 686cdb01bc71559777e39f7e2a4e016ef769af65 +Subproject commit b03d3eded8e691844e718b9c89d1680442c262d1 From 4a13461eb61b77cb9b9863c230980df6a8f146bb Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Wed, 20 Aug 2025 17:10:51 -0500 Subject: [PATCH 20/27] Testing the key input --- xarm_ros2_base | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/xarm_ros2_base b/xarm_ros2_base index b03d3ed..369a792 160000 --- a/xarm_ros2_base +++ b/xarm_ros2_base @@ -1 +1 @@ -Subproject commit b03d3eded8e691844e718b9c89d1680442c262d1 +Subproject commit 369a7925404c8abe9184997e8d2338e4a1864a53 From 22c57af897324618192a587f012a3c354dff5111 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Wed, 20 Aug 2025 18:03:53 -0500 Subject: [PATCH 21/27] Key inputs for studies via keyboard tested and working --- xarm_ros2_base | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/xarm_ros2_base b/xarm_ros2_base index 369a792..816f41e 160000 --- a/xarm_ros2_base +++ b/xarm_ros2_base @@ -1 +1 @@ -Subproject commit 369a7925404c8abe9184997e8d2338e4a1864a53 +Subproject commit 816f41e9ad39b7b10345bad3db72fc7a3da61c4c From 2d11b80326eb9a2d04b48d1ff97ef5e6b22e4d83 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Thu, 21 Aug 2025 12:45:49 -0500 Subject: [PATCH 22/27] Editing the the state and mode of robot during recording --- xarm_ros2_base | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/xarm_ros2_base b/xarm_ros2_base index 816f41e..20c5d60 160000 --- a/xarm_ros2_base +++ b/xarm_ros2_base @@ -1 +1 @@ -Subproject commit 816f41e9ad39b7b10345bad3db72fc7a3da61c4c +Subproject commit 20c5d602d5f06b544f95125580dac3639dc5184a From a66e136bfaf1501bb33a4bb35a535d54409dedcd Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Thu, 21 Aug 2025 12:55:52 -0500 Subject: [PATCH 23/27] Updating Dockerfile with new installations required for creating image --- Docker/Dockerfile | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/Docker/Dockerfile b/Docker/Dockerfile index 74f4c46..5f79d0a 100644 --- a/Docker/Dockerfile +++ b/Docker/Dockerfile @@ -5,6 +5,9 @@ ENV ROS_DISTRO jazzy RUN apt-get update && apt-get install -y --no-install-recommends \ ros-${ROS_DISTRO}-ros2-control \ ros-${ROS_DISTRO}-ros2-controllers \ + ros-${ROS_DISTRO}-joy \ + ros-${ROS_DISTRO}-colcon-clean \ + ros-${ROS_DISTRO}-common-plugins \ ros-${ROS_DISTRO}-kinematics-interface-kdl \ ros-${ROS_DISTRO}-ament-cmake-clang-format \ ros-${ROS_DISTRO}-rviz2 \ @@ -59,6 +62,7 @@ RUN apt-get update -yq && apt-get install -yq \ g++ \ unzip \ git \ + transform3d \ python3-scipy \ python3-numpy \ python3-cvxopt \ @@ -68,12 +72,13 @@ RUN apt-get update -yq && apt-get install -yq \ python3-tk \ python3-pytest-rerunfailures \ python3-matplotlib \ + python3-box2d \ && apt-get clean && rm -rf /var/lib/apt/lists/* # RUN git clone https://github.com/xArm-Developer/xArm-Python-SDK.git && cd xArm-Python-SDK && python3 setup.py install -# # install pinocchio -# RUN pip3 install pin +# install box 2d +RUN pip3 install Box2D --break-system-packages # setup environment ENV LANG C.UTF-8 From e7a3305baec76bf3a718db2d771348c246af73c7 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Fri, 22 Aug 2025 17:08:49 -0500 Subject: [PATCH 24/27] Added key input for new trial --- xarm_ros2_base | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/xarm_ros2_base b/xarm_ros2_base index 20c5d60..5e7c969 160000 --- a/xarm_ros2_base +++ b/xarm_ros2_base @@ -1 +1 @@ -Subproject commit 20c5d602d5f06b544f95125580dac3639dc5184a +Subproject commit 5e7c969dd323ff5333e348d57d6c5636af3f0653 From 7209fdbd73437d79aaf3d50fd28490d10042e0b0 Mon Sep 17 00:00:00 2001 From: dbarsoum Date: Fri, 22 Aug 2025 17:12:48 -0500 Subject: [PATCH 25/27] Updating xarm_ros2_base head to main --- xarm_ros2_base | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/xarm_ros2_base b/xarm_ros2_base index 5e7c969..55524f3 160000 --- a/xarm_ros2_base +++ b/xarm_ros2_base @@ -1 +1 @@ -Subproject commit 5e7c969dd323ff5333e348d57d6c5636af3f0653 +Subproject commit 55524f31a082b5e3e08d3a9fd8fd57f166b5c8e6 From 4448834bfb4ae6ea8920ebcc8143776443f25d79 Mon Sep 17 00:00:00 2001 From: Sharwin Patil Date: Mon, 10 Nov 2025 15:07:16 -0600 Subject: [PATCH 26/27] Adjusted path for robot urdf file from example-robot-data repo --- manipulator_control/scripts/xarm_vel_main_experiment.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/manipulator_control/scripts/xarm_vel_main_experiment.py b/manipulator_control/scripts/xarm_vel_main_experiment.py index 5c8b522..bbf427d 100644 --- a/manipulator_control/scripts/xarm_vel_main_experiment.py +++ b/manipulator_control/scripts/xarm_vel_main_experiment.py @@ -479,7 +479,8 @@ def control_loop(self): self.current_positions = q # Calculate the JParsed Jacobian # NEW # 8/8/2025 - urdf_filename = "/workspace/xarm7.urdf" + # TODO(Sharwin or Demiana): Use pinocchio function to access example instead of public repo + urdf_filename = "/home/workspace/src/example-robot-data/robots/xarm_description/urdf/xarm7.urdf" model = pin.buildModelFromUrdf(urdf_filename) # the model structure of the robot data = model.createData() # the data structure of the robot # 1) Forward kinematics From 6c7262f6ef84741d5aa8bcdb10a7788cf918803a Mon Sep 17 00:00:00 2001 From: Sharwin Patil Date: Wed, 10 Dec 2025 15:31:22 -0600 Subject: [PATCH 27/27] Small edits for pfields_demo --- .gitmodules | 3 --- .../scripts/xarm_vel_main_experiment.py | 14 +++++++++++--- xarm_ros2_base | 2 +- 3 files changed, 12 insertions(+), 7 deletions(-) delete mode 100644 .gitmodules diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index 1eb3ca1..0000000 --- a/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "xarm_ros2_base"] - path = xarm_ros2_base - url = https://github.com/argallab/xarm_ros2_base.git diff --git a/manipulator_control/scripts/xarm_vel_main_experiment.py b/manipulator_control/scripts/xarm_vel_main_experiment.py index bbf427d..bbe76fe 100644 --- a/manipulator_control/scripts/xarm_vel_main_experiment.py +++ b/manipulator_control/scripts/xarm_vel_main_experiment.py @@ -137,6 +137,8 @@ def initialize_subscribers(self): JointState, joint_states_topic, self.joint_states_callback, 10, callback_group=js_cb_g) self.teleop_control_sub = self.create_subscription( TwistStamped, 'robot_action', self.teleop_control_callback, 1, callback_group=js_cb_g) + self.joint_vel_control_sub = self.create_subscription( + JointTrajectoryPoint, 'joint_vel_control', self.joint_vel_control_callback, 1, callback_group=js_cb_g) self.gripper_action_sub = self.create_subscription( Float32, '/gripper_action', self.gripper_position_callback, 1, callback_group=js_cb_g) @@ -180,7 +182,7 @@ def joint_states_callback(self, msg): """ self.joint_states = msg - def teleop_control_callback(self, msg): + def teleop_control_callback(self, msg: TwistStamped): """ Callback function for the teleop_control topic (if using an interface). This function will be called whenever a new message is received on the teleop_control topic. The message is a geometry_msgs/TwistStamped message, which contains the current @@ -196,6 +198,12 @@ def teleop_control_callback(self, msg): position_velocity = position_velocity / position_velocity_norm * 0.05 self.teleop_control_command = np.array([position_velocity[0], position_velocity[1],position_velocity[2],angular_velocity[0],angular_velocity[1],angular_velocity[2]]) #check if norm of the space mouse command is greater than 0.05, if so normalize it to this value + def joint_vel_control_callback(self, msg: JointTrajectoryPoint): + # Convert JointTrajectory into a list of list of joint velocities + joint_vels: list[float] = msg.velocities + self.command_joint_velocities(joint_vels) + + ########################################################################################################## ####################################### HELPER FUNCTIONS ################################################# ########################################################################################################## @@ -409,7 +417,7 @@ def publish_pose_errors(self, position_error, angular_error, control_pose_error self.position_error_pub.publish(position_error_msg) self.orientation_error_pub.publish(orientation_error_msg) - def command_joint_velocities(self, joint_vel_list): + def command_joint_velocities(self, joint_vel_list: list[float]): """ This function commands the joint velocities to the robot using the appropriate ROS message type. """ @@ -445,7 +453,7 @@ def command_joint_velocities(self, joint_vel_list): trajectory_msg.header.stamp = self.get_clock().now().to_msg() # Update timestamp self.joint_vel_pub.publish(trajectory_msg) else: - # this is on the real robot, directly send joint velociteies + # this is on the real robot, directly send joint velocities # Send joint velocities to the arm # log the velocities if self.use_teleop_control_jparse: diff --git a/xarm_ros2_base b/xarm_ros2_base index 55524f3..5c6a449 160000 --- a/xarm_ros2_base +++ b/xarm_ros2_base @@ -1 +1 @@ -Subproject commit 55524f31a082b5e3e08d3a9fd8fd57f166b5c8e6 +Subproject commit 5c6a449c29b0f71d4178c678aaa0c786d990459e