diff --git a/.gitignore b/.gitignore index 1827073..0b8b515 100644 --- a/.gitignore +++ b/.gitignore @@ -5,4 +5,6 @@ *.html *.pdf *.mp4 -*.avi \ No newline at end of file +*.avi +xArm-Python-SDK +dockerfiles \ No newline at end of file diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index a5c1744..0000000 --- a/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "xarm_ros1_base"] - path = xarm_ros1_base - url = https://github.com/argallab/xarm_ros1_base.git diff --git a/Docker/Dockerfile b/Docker/Dockerfile index c928b61..5f79d0a 100644 --- a/Docker/Dockerfile +++ b/Docker/Dockerfile @@ -1,79 +1,104 @@ -# 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}-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 \ + 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 \ + transform3d \ + python3-scipy \ + python3-numpy \ + python3-cvxopt \ + python3-pandas \ + python3-colcon-common-extensions \ + python-is-python3 \ + 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 box 2d +RUN pip3 install Box2D --break-system-packages + +# 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..5222eb9 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,53 +17,52 @@ _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 supporting *ROS 2 Jazzy*. -## FOR ARGALLAB -Instructions to teleop robot with jparse and keyboard: +## FOR ARGALLAB (ROS 2) -Launch the following (velocity_control param for the xarm7_gripper_moveit_config launchfile is left as false in this case): +### Joystick or SNP Teleop ``` -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 launch xarm_teleop xarm_teleop.launch.xml JOY:=true paradigm:=3 ``` -Edits need to be still commpleted to test the robot after writing script for joystick and sip/puff teleop. +``` +ros2 launch xarm_moveit_config xarm7_moveit_realmove.launch.py robot_ip:= add_gripper:=true -## 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) +ros2 launch manipulator_control xarm_main_vel.launch use_teleop_control:=true use_teleop_control_jparse:=true -## Quick Start with Docker +ros2 launch xarm_teleop xarm_teleop.launch.xml SNP:=true +``` -To build the Docker image for the our environment, we use VNC docker, which allows for a graphical user interface displayable in the browser. +### Keyboard Teleop +Instructions to teleop robot with jparse and keyboard: -### Use the Public Docker Image (Recommended) +Launch the following for keyboard teleop: +``` +ros2 launch xarm_moveit_config xarm7_moveit_realmove.launch.py robot_ip:= add_gripper:=true -We have created a public Docker image that you can pull! -Steps: +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 -```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 +Edits need to be still commpleted to test the robot after writing script for joystick and sip/puff teleop. -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 +## FOR ARGALLAB TO DOS (not in order of to do): +- [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 +- [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 @@ -89,167 +87,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/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..446736d 100644 --- a/manipulator_control/CMakeLists.txt +++ b/manipulator_control/CMakeLists.txt @@ -1,25 +1,58 @@ -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) +find_package(Eigen3 REQUIRED NO_MODULE) + +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/jparse_cls.py + scripts/xarm_vel_main_experiment.py + 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 + # 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..cc5d12b --- /dev/null +++ b/manipulator_control/README.md @@ -0,0 +1,6 @@ +# manipulator control + +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/config/kinova_arm_controllers_gazebo.yaml b/manipulator_control/config/kinova_arm_controllers_gazebo.yaml deleted file mode 100644 index 6be6c8a..0000000 --- a/manipulator_control/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/launch/franka_launch.launch b/manipulator_control/launch/franka_launch.launch deleted file mode 100644 index 1566e5d..0000000 --- a/manipulator_control/launch/franka_launch.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control/launch/full_pose_trajectory.launch b/manipulator_control/launch/full_pose_trajectory.launch deleted file mode 100644 index 308662f..0000000 --- a/manipulator_control/launch/full_pose_trajectory.launch +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control/launch/jparse_april_tag_ros.launch b/manipulator_control/launch/jparse_april_tag_ros.launch deleted file mode 100644 index 787f3b6..0000000 --- a/manipulator_control/launch/jparse_april_tag_ros.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control/launch/jparse_cpp.launch b/manipulator_control/launch/jparse_cpp.launch deleted file mode 100644 index c82d1e1..0000000 --- a/manipulator_control/launch/jparse_cpp.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control/launch/kinova_gen3.launch b/manipulator_control/launch/kinova_gen3.launch deleted file mode 100644 index e559443..0000000 --- a/manipulator_control/launch/kinova_gen3.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control/launch/kinova_gen3_real.launch b/manipulator_control/launch/kinova_gen3_real.launch deleted file mode 100644 index 947a011..0000000 --- a/manipulator_control/launch/kinova_gen3_real.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control/launch/kinova_gen3_real_visual_servoing.launch b/manipulator_control/launch/kinova_gen3_real_visual_servoing.launch deleted file mode 100644 index edca51a..0000000 --- a/manipulator_control/launch/kinova_gen3_real_visual_servoing.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control/launch/kinova_vel_control.launch b/manipulator_control/launch/kinova_vel_control.launch deleted file mode 100644 index c43fd24..0000000 --- a/manipulator_control/launch/kinova_vel_control.launch +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control/launch/line_extended_singular_traj.launch b/manipulator_control/launch/line_extended_singular_traj.launch deleted file mode 100644 index 26675f0..0000000 --- a/manipulator_control/launch/line_extended_singular_traj.launch +++ /dev/null @@ -1,50 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control/launch/manip_velocity_control.launch b/manipulator_control/launch/manip_velocity_control.launch deleted file mode 100644 index 05a926a..0000000 --- a/manipulator_control/launch/manip_velocity_control.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control/launch/panda_main_torque.launch b/manipulator_control/launch/panda_main_torque.launch deleted file mode 100644 index ccb31d6..0000000 --- a/manipulator_control/launch/panda_main_torque.launch +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control/launch/position_trajectory.launch b/manipulator_control/launch/position_trajectory.launch deleted file mode 100644 index 989de6e..0000000 --- a/manipulator_control/launch/position_trajectory.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control/launch/se3_type_2_singular_traj.launch b/manipulator_control/launch/se3_type_2_singular_traj.launch deleted file mode 100644 index da6be72..0000000 --- a/manipulator_control/launch/se3_type_2_singular_traj.launch +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control/launch/xarm_launch.launch b/manipulator_control/launch/xarm_launch.launch deleted file mode 100644 index f59e699..0000000 --- a/manipulator_control/launch/xarm_launch.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control/launch/xarm_main_vel.launch b/manipulator_control/launch/xarm_main_vel.launch.xml similarity index 58% rename from manipulator_control/launch/xarm_main_vel.launch rename to manipulator_control/launch/xarm_main_vel.launch.xml index 84c876a..bc6cda3 100644 --- a/manipulator_control/launch/xarm_main_vel.launch +++ b/manipulator_control/launch/xarm_main_vel.launch.xml @@ -11,31 +11,31 @@ - - - - + + + - + - - - + + + - - - - - - - - + + + + + + + + + \ No newline at end of file diff --git a/manipulator_control/launch/xarm_python_using_cpp.launch b/manipulator_control/launch/xarm_python_using_cpp.launch deleted file mode 100644 index 92b333c..0000000 --- a/manipulator_control/launch/xarm_python_using_cpp.launch +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control/launch/xarm_real_launch.launch b/manipulator_control/launch/xarm_real_launch.launch index 93d045f..f94c84d 100644 --- a/manipulator_control/launch/xarm_real_launch.launch +++ b/manipulator_control/launch/xarm_real_launch.launch @@ -1,7 +1,7 @@ - + @@ -9,9 +9,10 @@ + - + diff --git a/manipulator_control/launch/xarm_real_spacemouse_base.launch b/manipulator_control/launch/xarm_real_spacemouse_base.launch deleted file mode 100644 index b548f07..0000000 --- a/manipulator_control/launch/xarm_real_spacemouse_base.launch +++ /dev/null @@ -1,86 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/manipulator_control/launch/xarm_spacemouse_teleop.launch b/manipulator_control/launch/xarm_spacemouse_teleop.launch deleted file mode 100644 index d846b26..0000000 --- a/manipulator_control/launch/xarm_spacemouse_teleop.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - \ 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/msg/JparseTerms.msg b/manipulator_control/msg/JparseTerms.msg deleted file mode 100644 index b5487a5..0000000 --- a/manipulator_control/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/msg/Matrow.msg b/manipulator_control/msg/Matrow.msg deleted file mode 100644 index f7ca10e..0000000 --- a/manipulator_control/msg/Matrow.msg +++ /dev/null @@ -1 +0,0 @@ -float64[] row diff --git a/manipulator_control/package.xml b/manipulator_control/package.xml index cbba38b..aa96363 100644 --- a/manipulator_control/package.xml +++ b/manipulator_control/package.xml @@ -1,58 +1,23 @@ - + + 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 + eigen + 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/rviz/jparse_final.rviz b/manipulator_control/rviz/jparse_final.rviz deleted file mode 100644 index 849f15b..0000000 --- a/manipulator_control/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/rviz/kinova_gen3.rviz b/manipulator_control/rviz/kinova_gen3.rviz deleted file mode 100644 index 483021e..0000000 --- a/manipulator_control/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/rviz/panda_rviz.rviz b/manipulator_control/rviz/panda_rviz.rviz deleted file mode 100644 index dad2ac3..0000000 --- a/manipulator_control/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/rviz/xarm_rviz.rviz b/manipulator_control/rviz/xarm_rviz.rviz deleted file mode 100644 index 66bd99f..0000000 --- a/manipulator_control/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/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/scripts/device.py b/manipulator_control/scripts/device.py deleted file mode 100644 index 655a754..0000000 --- a/manipulator_control/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/scripts/extract_lfd_images_from_bag.py b/manipulator_control/scripts/extract_lfd_images_from_bag.py deleted file mode 100755 index 278524c..0000000 --- a/manipulator_control/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/scripts/input2action.py b/manipulator_control/scripts/input2action.py deleted file mode 100644 index 620400d..0000000 --- a/manipulator_control/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/scripts/jparse_cls.py b/manipulator_control/scripts/jparse_cls.py new file mode 100644 index 0000000..d103f6d --- /dev/null +++ b/manipulator_control/scripts/jparse_cls.py @@ -0,0 +1,191 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node + +# pinocchio +import numpy as np +import pinocchio as pin +# 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 + +# this is the huge node that does all the jacobian calculations!! + + +class JParseClass(Node): + 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 + + 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') + 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): + pass +""" + \ No newline at end of file diff --git a/manipulator_control/scripts/main_experiment_kinova.py b/manipulator_control/scripts/main_experiment_kinova.py deleted file mode 100755 index 7ffd2b3..0000000 --- a/manipulator_control/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/scripts/panda_torque_main_experiment.py b/manipulator_control/scripts/panda_torque_main_experiment.py deleted file mode 100755 index a8b27f7..0000000 --- a/manipulator_control/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/scripts/position_trajectory_generator.py b/manipulator_control/scripts/position_trajectory_generator.py deleted file mode 100755 index 1cde749..0000000 --- a/manipulator_control/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/scripts/se3_trajectory_generator.py b/manipulator_control/scripts/se3_trajectory_generator.py deleted file mode 100755 index b06a12a..0000000 --- a/manipulator_control/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/scripts/spacemouse.py b/manipulator_control/scripts/spacemouse.py deleted file mode 100644 index 2adb36c..0000000 --- a/manipulator_control/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/scripts/xarm_spacemouse.py b/manipulator_control/scripts/xarm_spacemouse.py deleted file mode 100644 index 18ef922..0000000 --- a/manipulator_control/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/scripts/xarm_vel_main_experiment.py b/manipulator_control/scripts/xarm_vel_main_experiment.py old mode 100755 new mode 100644 index 596171d..bbe76fe --- a/manipulator_control/scripts/xarm_vel_main_experiment.py +++ b/manipulator_control/scripts/xarm_vel_main_experiment.py @@ -1,61 +1,64 @@ #!/usr/bin/env python3 import sys -import rospy +import rclpy +from rclpy.node import Node import numpy as np +import pinocchio as pin 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 +# ros2 stuff +import tf2_ros +### +# 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 +### -import geometry_msgs.msg 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 -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 +from rclpy.qos import QoSProfile, QoSDurabilityPolicy, ReliabilityPolicy, DurabilityPolicy +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup try: from xarm.wrapper import XArmAPI except ImportError: print("xarm package not installed, skipping xarm import") -from manipulator_control import jparse_cls +import jparse_cls + -class ArmController: +class ArmController(Node): 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') + super().__init__('arm_controller') - self.is_sim = rospy.get_param('~is_sim', False) #boolean to control if the robot is in simulation or not + self.get_logger().info("node started") - 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 + # 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 + + 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.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.jparse_gamma = self.declare_parameter('~jparse_gamma', 0.1).get_parameter_value().double_value #gamma for the JParse method - self.space_mouse_command = np.array([0,0,0,0,0,0]).T + # used to be self.use_space_mouse and self.use_space_mouse_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 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.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) @@ -67,74 +70,108 @@ def __init__(self): 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 + # 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 = 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 + # 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.space_mouse_command = None # added this 7/11/25 + # 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.tfBuffer = tf2_ros.Buffer() - self.listener = tf2_ros.TransformListener(self.tfBuffer) - self.tfros = tf.TransformerROS() - self.tf_timeout = rospy.Duration(1.0) + 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) + + # 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) + + + # 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 + + self.timer = self.create_timer(0.01, self.timer_callback) + + def timer_callback(self): + self.control_loop() + + + ######################################################################################################### + ############################# 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' - 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) - + # 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.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) - 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) + 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 = 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) + 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 = 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) + 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 = rospy.Publisher('/current_end_effector_pose', PoseStamped, queue_size=10) - self.current_target_pose_pub = rospy.Publisher('/current_target_pose', PoseStamped, queue_size=10) + 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) - 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) + 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) - 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 + ########################################################################################################## + ############################# CALLBACKS FOR SUBSCRIBERS ################################################## + ########################################################################################################## + def gripper_position_callback(self, msg): + """Callback for the gripper. It includes the gripper position value. Float32 value.""" + 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): """ @@ -145,44 +182,43 @@ def joint_states_callback(self, msg): """ self.joint_states = msg - def space_mouse_callback(self, msg): + def teleop_control_callback(self, msg: TwistStamped): """ - 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 + 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. """ - 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:] + 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.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 + 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_space_mouse == True: - self.target_pose = PoseStamped() #set this to get in a loop + 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) - 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 ################################################# + ########################################################################################################## + 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. """ current_pose = PoseStamped() current_pose.header.frame_id = self.base_link - current_pose.header.stamp = rospy.Time.now() + current_pose.header.stamp = self.get_clock().now().to_msg() try: - trans = self.tfBuffer.lookup_transform(self.base_link, self.end_link, rospy.Time(), timeout=self.tf_timeout) + 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 @@ -191,21 +227,12 @@ def EndEffectorPose(self, q): 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.get_logger().error("TF lookup failed") + 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 - - 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. @@ -247,7 +274,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, @@ -257,9 +286,15 @@ 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]) + 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 @@ -314,14 +349,42 @@ def rotation_matrix_to_quaternion(self, R): 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: + if self.joint_states is not None: + 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) + + ############################################################################################################## + ######################################### 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 = rospy.Time.now() + 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] @@ -348,175 +411,13 @@ def publish_pose_errors(self, position_error, angular_error, control_pose_error 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) + 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 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): + 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. """ @@ -543,29 +444,143 @@ def command_joint_velocities(self, joint_vel_list): point.effort = [0.0] * len(self.joint_names) # Set the time from start - point.time_from_start = rospy.Duration(0.1) # Duration in seconds + 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 = rospy.Time.now() # Update timestamp + 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 - 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 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) # arm command + self.arm.set_gripper_position(self.gripper_pose) # gripper command + + ############################################################################################################### + ######################################### CONTROL LOOP ######################################################## + ############################################################################################################### + 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 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 + # NEW # 8/8/2025 + # 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 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 + 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]) + 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 + joint_velocities = J_method @ teleop_control_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.get_logger().info("Control loop running") + +def main(args=None): + rclpy.init(args=args) + node = ArmController() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() if __name__ == '__main__': - try: - ArmController() - except rospy.ROSInterruptException: - pass - + main() \ No newline at end of file diff --git a/manipulator_control/scripts/xarm_vel_python_using_cpp.py b/manipulator_control/scripts/xarm_vel_python_using_cpp.py deleted file mode 100755 index dc3cc1a..0000000 --- a/manipulator_control/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/setup.py b/manipulator_control/setup.py deleted file mode 100644 index 08c053b..0000000 --- a/manipulator_control/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/src/jparse.cpp b/manipulator_control/src/jparse.cpp deleted file mode 100644 index 8577637..0000000 --- a/manipulator_control/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/src/manipulator_control/jparse_cls.py b/manipulator_control/src/manipulator_control/jparse_cls.py deleted file mode 100755 index c4fc2e3..0000000 --- a/manipulator_control/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/srv/JParseSrv.srv b/manipulator_control/srv/JParseSrv.srv deleted file mode 100644 index 03716b9..0000000 --- a/manipulator_control/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_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 diff --git a/xarm_ros2_base b/xarm_ros2_base new file mode 160000 index 0000000..5c6a449 --- /dev/null +++ b/xarm_ros2_base @@ -0,0 +1 @@ +Subproject commit 5c6a449c29b0f71d4178c678aaa0c786d990459e