Merge pull request #1544 from pazeshun/add_dual_panda2

Add dual_panda2

jsk_panda_robot/README.md

@@ -0,0 +1,126 @@
+# JSK Panda Robot
+## Installation
+- Following pages provide informative resources.
+- Official manual page: https://frankaemika.github.io/docs/installation_linux.html
+- Franka Community: https://www.franka-community.de/
+
+
+### Installation for User PC
+1. Install OpenHaptics and Touch Device Driver from here: https://support.3dsystems.com/s/article/OpenHaptics-for-Linux-Developer-Edition-v34?language=en_US
+   * If your Ubuntu version is not supported, check JSK backup: https://drive.google.com/drive/folders/1FiQ4m3XtoDlwdRIq3H7LJv8T882SBVBl
+
+2. Install ROS packages:
+   ```bash
+   mkdir -p ~/franka_ws/src
+   cd ~/franka_ws/src
+   wstool init
+   wstool merge https://raw.githubusercontent.com/jsk-ros-pkg/jsk_robot/master/jsk_panda_robot/jsk_panda_user.rosinstall
+   wstool update
+   cd ../
+   source /opt/ros/melodic/setup.bash
+   rosdep install -y -r --from-paths src --ignore-src --skip-keys=librealsense2,realsense2_camera  # See comments in jsk_panda.rosinstall
+   catkin build jsk_panda_startup jsk_panda_teleop
+   source devel/setup.bash
+   ```
+3. If you want to use TouchUSBs (haptic devices), see [this README](https://github.com/pazeshun/Geomagic_Touch_ROS_Drivers/tree/dual-phantom-readme#use-multiple-devices) to setup devices.
+   * Note that this README was written for old Ubuntu.
+     - On Ubuntu 18.04, run `Touch_Setup` instead of `/opt/geomagic_touch_device_driver/Geomagic_Touch_Setup`. If you want to check device status, run `Touch_Diagnostic` instead of `/opt/geomagic_touch_device_driver/Geomagic_Touch_Diagnostic`.
+
+### Installation for Panda Controller PC
+1. Please see and follow installation written in: https://frankaemika.github.io/docs/installation_linux.html
+   * Note that you need to install real-time kernel (`PREEMPT-PR` kernel) for real-time control.
+   * Ref: Current controller PC uses following kernel:  `dual-panda1`: `5.4.19-rt11`, `dual-panda2`: `5.4.93-rt51`
+2. Install librealsense2:
+   https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md#installing-the-packages
+3. Install OpenHaptics and Touch Device Driver from here: https://support.3dsystems.com/s/article/OpenHaptics-for-Linux-Developer-Edition-v34?language=en_US
+   * If your Ubuntu version is not supported, check JSK backup: https://drive.google.com/drive/folders/1FiQ4m3XtoDlwdRIq3H7LJv8T882SBVBl
+4. Install ROS packages:
+   ```bash
+   mkdir -p ~/franka_ws/src
+   cd ~/franka_ws/src
+   wstool init
+   wstool merge https://raw.githubusercontent.com/jsk-ros-pkg/jsk_robot/master/jsk_panda_robot/jsk_panda.rosinstall
+   wstool update
+   cd ../
+   source /opt/ros/melodic/setup.bash
+   rosdep install -y -r --from-paths src --ignore-src --skip-keys=librealsense2,realsense2_camera  # See comments in jsk_panda.rosinstall
+   catkin build jsk_panda_startup jsk_panda_teleop
+   source devel/setup.bash
+   ```
+
+
+## Running Dual-Panda
+### Boot robot
+1. Please turn on the controller box and unlock joints by accessing desk.
+### Via roseus
+1. Start controller on controller PC:
+   ```bash
+   # dual_panda1: robot having Xtion camera on head, web cameras on both hands, microphone on right hand
+   # dual_panda2: robot having RealSense D435 on both hands
+   ssh leus@dual-panda1.jsk.imi.i.u-tokyo.ac.jp  # Or ssh leus@dual-panda2.jsk.imi.i.u-tokyo.ac.jp
+   roslaunch jsk_panda_startup dual_panda1.launch  # Or roslaunch jsk_panda_startup dual_panda2.launch
+   ```
+
+2. Controlling Dual-Panda via roseus:
+   1. Setting up network:
+      ```bash
+      rossetmaster dual-panda1.jsk.imi.i.u-tokyo.ac.jp  # Or rossetmaster dual-panda2.jsk.imi.i.u-tokyo.ac.jp
+      rossetip
+      ```
+   2. Execute following script in roseus:
+      ```lisp
+      (load "package://panda_eus/euslisp/dual_panda-interface.l")
+      (dual_panda-init)
+      (send *robot* :angle-vector (send *robot* :reset-pose))
+      (when (send *ri* :check-error)
+        (send *ri* :recover-error))
+      (send *ri* :angle-vector (send *robot* :angle-vector) 3000)
+      ```
+      `(send *ri* :recover-error)` is required every time when you press and release the black switch (`activated` -> `monitored stop` -> `activated`).
+#### Record/play rosbag
+```bash
+roslaunch jsk_panda_startup dual_panda1_record.launch  # Or roslaunch jsk_panda_startup dual_panda2_record.launch
+# Generated bag file: /tmp/panda_rosbag/data_*.bag
+roslaunch jsk_panda_startup dual_panda1_play.launch bagfile_name:=/path/to/bagfile  # Or roslaunch jsk_panda_startup dual_panda2_play.launch bagfile_name:=/path/to/bagfile
+```
+### Teleop
+#### Start program
+1. Start controller on controller PC:
+   ```bash
+   ssh leus@dual-panda1.jsk.imi.i.u-tokyo.ac.jp
+   roslaunch jsk_panda_teleop start_panda_teleop_follower_side.launch start_bilateral:=true
+   ```
+   `start_bilateral:=true` connects haptic device and dual_panda from the beginning, i.e. it moves the robot immediately after running leader (user) side.
+   If you start with `start_bilateral:=false`, you would need to call following service call:
+   ```bash
+   rosservice call /dual_panda/control_bilateral "pose_connecting: true
+     force_connecting: true
+     reset_phantom: true
+     wait: 3.0"
+   ```
+
+2. Start user PC:
+   ```bash
+   rossetmaster dual-panda1.jsk.imi.i.u-tokyo.ac.jp
+   rossetip
+   rosrun jsk_panda_teleop start_master_side.sh
+   ```
+   Then you should see rviz popup like this:
+   ![image](https://user-images.githubusercontent.com/14994939/181154686-cb8e95c8-96a7-47ac-b074-bf3c72bc5ee8.png)
+
+#### Move robot
+1. Press connection mode switch (white button) to change connect or not-connect status between the haptic devices and the robot.
+2. Press gripper switch to close / open gripper.
+
+![image](https://user-images.githubusercontent.com/43567489/159150507-75122802-121e-4a22-abd1-b9540890950b.png)
+
+#### Record/play rosbag
+```bash
+roslaunch jsk_panda_teleop panda_teleop_record.launch
+# Generated bag file: /tmp/panda_rosbag/data_*.bag
+roslaunch jsk_panda_teleop panda_teleop_play.launch bagfile_name:=/path/to/bagfile
+```
+
+#### Trouble Shooting
+1. `Failed to initialize haptic device`  -> Please give access to haptic devices, i.e `sudo chmod 777 /dev/ttyACM[0-1]`
+

jsk_panda_robot/jsk_panda.rosinstall

@@ -0,0 +1,32 @@
+# This file is for workspace for panda controller PC
+
+- git:
+    local-name: jsk-ros-pkg/jsk_robot
+    uri: https://github.com/jsk-ros-pkg/jsk_robot.git
+    version: master
+
+# Wait https://github.com/bharatm11/Geomagic_Touch_ROS_Drivers/pull/8 to be merged.
+# NOTE: even when that is merged, you have to use dual-phantom-beta branch, not hydro-devel, until dual-phantom-beta is merged into hydro-devel
+- git:
+    local-name: phantom_drivers
+    uri: https://github.com/pazeshun/Geomagic_Touch_ROS_Drivers.git
+    version: dual-phantom-readme
+
+# Wait https://github.com/frankaemika/franka_ros/pull/298 to be released.
+- git:
+    local-name: franka_ros
+    uri: https://github.com/pazeshun/franka_ros.git
+    version: install-FrankaCombinableHW
+
+# Wait sound_classification to be released.
+- git:
+    local-name: jsk-ros-pkg/jsk_recognition
+    uri: https://github.com/jsk-ros-pkg/jsk_recognition.git
+    version: f455d96f431d094177be5f7569c79a3e25e8e360
+
+# ros-$ROS_DISTRO-librealsense2 is unstable, so we install librealsense2 from intel apt repo and build realsense2_camera with it.
+# See Notice of https://github.com/IntelRealSense/realsense-ros/tree/2.3.2#method-1-the-ros-distribution.
+- git:
+    local-name: realsense-ros
+    uri: https://github.com/IntelRealSense/realsense-ros.git
+    version: 2.3.2

jsk_panda_robot/jsk_panda_accessories/README.md

@@ -0,0 +1,13 @@
+# Tool mounter
+Mounting any tools onto panda-hand.  Plate mounter has M3 screw wholes placed by 10mm x 10mm.
+
+![](tool_mounter/hand_adapter.png)
+
+Designed by Yoichiro Kawamura.
+
+# tweezers
+Expansion parts to grasp small things.
+
+<img width="252" alt="tweezer_v2_right" src="https://user-images.githubusercontent.com/48650394/122322295-3781de00-cf60-11eb-926e-7aec0473b5ab.png">
+
+Designed by Koki Amabe.

jsk_panda_robot/jsk_panda_startup/CMakeLists.txt

@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 2.8.0)
+find_package(catkin)
+project(jsk_panda_startup)
+
+catkin_package()
+
+#############
+## Install ##
+#############
+
+install(DIRECTORY launch config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS)

jsk_panda_robot/jsk_panda_startup/config/dual_panda_control_node.yaml

@@ -0,0 +1,83 @@
+robot_hardware:
+  - rarm
+  - larm
+
+rarm:
+  type: franka_hw/FrankaCombinableHW
+  arm_id: rarm
+  joint_names:
+    - rarm_joint1
+    - rarm_joint2
+    - rarm_joint3
+    - rarm_joint4
+    - rarm_joint5
+    - rarm_joint6
+    - rarm_joint7
+  # Configure the threshold angle for printing joint limit warnings.
+  joint_limit_warning_threshold: 0.1 # [rad]
+  # Activate rate limiter? [true|false]
+  rate_limiting: true
+  # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
+  cutoff_frequency: 1000
+  # Internal controller for motion generators [joint_impedance|cartesian_impedance]
+  internal_controller: joint_impedance
+  # Used to decide whether to enforce realtime mode [enforce|ignore]
+  realtime_config: enforce
+  # Configure the initial defaults for the collision behavior reflexes.
+  collision_config:
+    # lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+    # upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+    # lower_torque_thresholds_nominal:      [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+    # upper_torque_thresholds_nominal:      [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+    # lower_force_thresholds_acceleration:  [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+    # upper_force_thresholds_acceleration:  [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+    # lower_force_thresholds_nominal:       [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+    # upper_force_thresholds_nominal:       [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+    lower_torque_thresholds_acceleration: [40.0, 40.0, 36.0, 36.0, 32.0, 28.0, 24.0]  # [Nm] (* 2.0)
+    upper_torque_thresholds_acceleration: [40.0, 40.0, 36.0, 36.0, 32.0, 28.0, 24.0]  # [Nm] (* 2.0)
+    lower_torque_thresholds_nominal:      [40.0, 40.0, 36.0, 36.0, 32.0, 28.0, 24.0]  # [Nm] (* 2.0)
+    upper_torque_thresholds_nominal:      [40.0, 40.0, 36.0, 36.0, 32.0, 28.0, 24.0]  # [Nm] (* 2.0)
+    lower_force_thresholds_acceleration:  [40.0, 40.0, 40.0, 50.0, 50.0, 50.0]  # [N, N, N, Nm, Nm, Nm] (* 2.0)
+    upper_force_thresholds_acceleration:  [40.0, 40.0, 40.0, 50.0, 50.0, 50.0]  # [N, N, N, Nm, Nm, Nm] (* 2.0)
+    lower_force_thresholds_nominal:       [40.0, 40.0, 40.0, 50.0, 50.0, 50.0]  # [N, N, N, Nm, Nm, Nm] (* 2.0)
+    upper_force_thresholds_nominal:       [40.0, 40.0, 40.0, 50.0, 50.0, 50.0]  # [N, N, N, Nm, Nm, Nm] (* 2.0)
+
+larm:
+  type: franka_hw/FrankaCombinableHW
+  arm_id: larm
+  joint_names:
+    - larm_joint1
+    - larm_joint2
+    - larm_joint3
+    - larm_joint4
+    - larm_joint5
+    - larm_joint6
+    - larm_joint7
+  # Configure the threshold angle for printing joint limit warnings.
+  joint_limit_warning_threshold: 0.1 # [rad]
+  # Activate rate limiter? [true|false]
+  rate_limiting: true
+  # Cutoff frequency of the low-pass filter. Set to >= 1000 to deactivate.
+  cutoff_frequency: 1000
+  # Internal controller for motion generators [joint_impedance|cartesian_impedance]
+  internal_controller: joint_impedance
+  # Used to decide whether to enforce realtime mode [enforce|ignore]
+  realtime_config: enforce
+  # Configure the initial defaults for the collision behavior reflexes.
+  collision_config:
+    # lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+    # upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+    # lower_torque_thresholds_nominal:      [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+    # upper_torque_thresholds_nominal:      [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+    # lower_force_thresholds_acceleration:  [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+    # upper_force_thresholds_acceleration:  [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+    # lower_force_thresholds_nominal:       [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+    # upper_force_thresholds_nominal:       [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+    lower_torque_thresholds_acceleration: [40.0, 40.0, 36.0, 36.0, 32.0, 28.0, 24.0]  # [Nm] (* 2.0)
+    upper_torque_thresholds_acceleration: [40.0, 40.0, 36.0, 36.0, 32.0, 28.0, 24.0]  # [Nm] (* 2.0)
+    lower_torque_thresholds_nominal:      [40.0, 40.0, 36.0, 36.0, 32.0, 28.0, 24.0]  # [Nm] (* 2.0)
+    upper_torque_thresholds_nominal:      [40.0, 40.0, 36.0, 36.0, 32.0, 28.0, 24.0]  # [Nm] (* 2.0)
+    lower_force_thresholds_acceleration:  [40.0, 40.0, 40.0, 50.0, 50.0, 50.0]  # [N, N, N, Nm, Nm, Nm] (* 2.0)
+    upper_force_thresholds_acceleration:  [40.0, 40.0, 40.0, 50.0, 50.0, 50.0]  # [N, N, N, Nm, Nm, Nm] (* 2.0)
+    lower_force_thresholds_nominal:       [40.0, 40.0, 40.0, 50.0, 50.0, 50.0]  # [N, N, N, Nm, Nm, Nm] (* 2.0)
+    upper_force_thresholds_nominal:       [40.0, 40.0, 40.0, 50.0, 50.0, 50.0]  # [N, N, N, Nm, Nm, Nm] (* 2.0)

jsk_panda_robot/jsk_panda_startup/config/dual_panda_controllers.yaml

@@ -0,0 +1,120 @@
+dual_panda_effort_joint_trajectory_controller:
+  type: effort_controllers/JointTrajectoryController
+  joints:
+    - rarm_joint1
+    - rarm_joint2
+    - rarm_joint3
+    - rarm_joint4
+    - rarm_joint5
+    - rarm_joint6
+    - rarm_joint7
+    - larm_joint1
+    - larm_joint2
+    - larm_joint3
+    - larm_joint4
+    - larm_joint5
+    - larm_joint6
+    - larm_joint7
+  constraints:
+    goal_time: 0.5
+    larm_joint1:
+      goal: 0.05
+    larm_joint2:
+      goal: 0.05
+    larm_joint3:
+      goal: 0.05
+    larm_joint4:
+      goal: 0.05
+    larm_joint5:
+      goal: 0.05
+    larm_joint6:
+      goal: 0.05
+    larm_joint7:
+      goal: 0.05
+    rarm_joint1:
+      goal: 0.05
+    rarm_joint2:
+      goal: 0.05
+    rarm_joint3:
+      goal: 0.05
+    rarm_joint4:
+      goal: 0.05
+    rarm_joint5:
+      goal: 0.05
+    rarm_joint6:
+      goal: 0.05
+    rarm_joint7:
+      goal: 0.05
+  gains:
+    rarm_joint1: {p: 600, d: 30, i: 0, i_clamp: 1}
+    rarm_joint2: {p: 600, d: 30, i: 0, i_clamp: 1}
+    rarm_joint3: {p: 600, d: 30, i: 0, i_clamp: 1}
+    rarm_joint4: {p: 600, d: 30, i: 0, i_clamp: 1}
+    rarm_joint5: {p: 250, d: 10, i: 0, i_clamp: 1}
+    rarm_joint6: {p: 150, d: 10, i: 0, i_clamp: 1}
+    rarm_joint7: {p: 50, d: 5, i: 0, i_clamp: 1}
+    larm_joint1: {p: 600, d: 30, i: 0, i_clamp: 1}
+    larm_joint2: {p: 600, d: 30, i: 0, i_clamp: 1}
+    larm_joint3: {p: 600, d: 30, i: 0, i_clamp: 1}
+    larm_joint4: {p: 600, d: 30, i: 0, i_clamp: 1}
+    larm_joint5: {p: 250, d: 10, i: 0, i_clamp: 1}
+    larm_joint6: {p: 150, d: 10, i: 0, i_clamp: 1}
+    larm_joint7: {p: 50, d: 5, i: 0, i_clamp: 1}
+
+rarm_state_controller:
+  type: franka_control/FrankaStateController
+  arm_id: rarm
+  joint_names:
+    - rarm_joint1
+    - rarm_joint2
+    - rarm_joint3
+    - rarm_joint4
+    - rarm_joint5
+    - rarm_joint6
+    - rarm_joint7
+  publish_rate: 30  # [Hz]
+
+larm_state_controller:
+  type: franka_control/FrankaStateController
+  arm_id: larm
+  joint_names:
+    - larm_joint1
+    - larm_joint2
+    - larm_joint3
+    - larm_joint4
+    - larm_joint5
+    - larm_joint6
+    - larm_joint7
+  publish_rate: 30  # [Hz]
+
+rarm_cartesian_impedance_controller:
+  type: franka_example_controllers/CartesianImpedanceExampleController
+  arm_id: rarm
+  joint_names:
+    - rarm_joint1
+    - rarm_joint2
+    - rarm_joint3
+    - rarm_joint4
+    - rarm_joint5
+    - rarm_joint6
+    - rarm_joint7
+# # Keep params the same as DualArmCartesianPoseController
+# # (https://github.com/frankaemika/franka_ros/pull/270/files#diff-6edeaeced01c9386cab21e9b756259aa82a94cebca584324d241454e97d76863R20)
+# rarm_cartesian_impedance_controllerdynamic_reconfigure_compliance_param_node:
+#   nullspace_stiffness: 0.0
+
+larm_cartesian_impedance_controller:
+  type: franka_example_controllers/CartesianImpedanceExampleController
+  arm_id: larm
+  joint_names:
+    - larm_joint1
+    - larm_joint2
+    - larm_joint3
+    - larm_joint4
+    - larm_joint5
+    - larm_joint6
+    - larm_joint7
+# # Keep params the same as DualArmCartesianPoseController
+# # (https://github.com/frankaemika/franka_ros/pull/270/files#diff-6edeaeced01c9386cab21e9b756259aa82a94cebca584324d241454e97d76863R20)
+# larm_cartesian_impedance_controllerdynamic_reconfigure_compliance_param_node:
+#   nullspace_stiffness: 0.0