[WIP] Add panda robot

jsk_panda_robot/README.md

@@ -0,0 +1,74 @@
+# 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 from here; https://support.3dsystems.com/s/article/OpenHaptics-for-Linux-Developer-Edition-v34?language=en_US
+
+2. Install ROS packages.
+```
+mkdir -p ~/franka_ws/src
+cd ~/franka_ws/src
+wstool init
+wstool merge https://raw.githubusercontent.com/ykawamura96/jsk_robot/add_panda_robot/jsk_panda_robot/jsk_panda.rosinstall
+wstool update
+cd ../
+source /opt/ros/melodic/setup.bash
+rosdep install -y -r --from-paths src --ignore-src
+catkin build
+source devel/setup.bash
+```
+### 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:
+2. Please do the same ROS environment setup as `Installation for User PC` section above.
+
+
+
+## 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;
+   1.  `ssh leus@dual_panda.jsk.imi.i.u-tokyo.ac.jp`
+   2.  `roslaunch jsk_panda_startup dual_panda.launch`
+
+2. Controlling Dual-Panda via roseus
+   1. `$ rossetpanda`
+   2. execute following script in roseus;
+      ```
+      (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)
+      ```
+### Teleop
+#### Start program
+1. Start controller on controller PC;
+   1.  `$ ssh leus@dual_panda.jsk.imi.i.u-tokyo.ac.jp`
+   2.  `$ roslaunch jsk_panda_teleop start_panda_rt_controller.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:
+        ```
+        $ rosservice call /dual_panda/control_bilateral "pose_connecting: true
+            force_connecting: true
+            reset_phantom: true
+            wait: 3.0" 
+        ```
+2.  Start user PC: `$ rosrun  jsk_panda_teleop start_master_side.sh`, then you should see rviz popup like this: 
+![image](https://user-images.githubusercontent.com/43567489/159150327-5e4d246b-2311-4eb4-814a-7f6fd11b6f29.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)
+
+#### 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,22 @@
+# This file is for work space for panda PC.
+# Currently .rosintall is shared with user PC's one.
+
+
+# Wait until https://github.com/jsk-ros-pkg/jsk_robot/pull/1343 to be merged
+- git:
+    local-name: jsk-ros-pkg/jsk_robot
+    uri: https://github.com/ykawamura96/jsk_robot.git
+    version: add_panda_robot
+
+# Need for using multiple haptic devices
+# wait https://github.com/bharatm11/Geomagic_Touch_ROS_Drivers/pull/5 to be merged.
+- git:
+    local-name: phantom_drivers
+    uri: https://github.com/ykawamura96/Geomagic_Touch_ROS_Drivers.git
+    version: dual_phantom
+
+# Need for using dual_arm_impedance controller
+- git:
+    local-name: franka_ros
+    uri: https://github.com/ykawamura96/franka_ros.git
+    version: add_dual_arm_impedance_controller

jsk_panda_robot/jsk_panda_accessories/README.md

@@ -0,0 +1,7 @@
+
+# Tool mounter
+Mounting any tools onto panda-hand.  Plate mounter has M3 screw wholes placed by 10mm x 10mm.
+![](https://raw.githubusercontent.com/ykawamura96/franka_demos/mech/add_adapter_parts/franka_accessories/panda_wrist/hand_adapter.png)
+# 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">

jsk_panda_robot/jsk_panda_startup/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 2.8.0)
+find_package(catkin)
+project(jsk_panda_startup)
+
+catkin_package()
+
+#############
+## Install ##
+#############
+
+install(DIRECTORY launch config scripts
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS)
+
+
+catkin_add_env_hooks(99.jsk_panda_startup SHELLS bash zsh sh
+  DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks)

jsk_panda_robot/jsk_panda_startup/config/dual_panda_control_node.yaml

@@ -0,0 +1,67 @@
+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]
+
+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]

jsk_panda_robot/jsk_panda_startup/config/dual_panda_controllers.yaml

@@ -0,0 +1,177 @@
+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}
+
+dual_panda_pose_cartesian_controller:
+  type: franka_example_controllers/CartesianPoseExampleController
+  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}
+
+
+
+dual_arm_cartesian_impedance_example_controller:
+    type: franka_example_controllers/DualArmCartesianImpedanceExampleController
+    right:
+        arm_id: rarm
+        joint_names:
+            - rarm_joint1
+            - rarm_joint2
+            - rarm_joint3
+            - rarm_joint4
+            - rarm_joint5
+            - rarm_joint6
+            - rarm_joint7
+    left:
+        arm_id: larm
+        joint_names:
+            - larm_joint1
+            - larm_joint2
+            - larm_joint3
+            - larm_joint4
+            - larm_joint5
+            - larm_joint6
+            - larm_joint7
+
+
+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]