Add fix_KXR_demo (amabe bthesis demo)

jsk_2021_fix_kxr/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(jsk_2021_fix_kxr)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  sensor_msgs
+  message_generation
+ )
+
+add_message_files(
+  FILES
+  Repair.msg
+  )
+
+add_service_files(
+  FILES
+  RepairInfo.srv
+  )
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  sensor_msgs
+  )
+
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES ros_lecture
+CATKIN_DEPENDS roscpp rospy std_msgs sensor_msgs message_runtime
+#  DEPENDS system_lib
+)
+
+install(DIRECTORY euslisp
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  )

jsk_2021_fix_kxr/README.md

@@ -0,0 +1,48 @@
+# jsk_2021_fix_kxr
+Fix KXR whose connector was removal by dual_panda2.
+
+## Installation
+### Connecting KXR with your PC and starting communication
+According to [here](https://www.google.com/url?sa=j&url=https%3A%2F%2Fgitlab.jsk.imi.i.u-tokyo.ac.jp%2Frcb4eus%2Frcb4eus%2F-%2Fwikis%2Fhome&uct=1649641403&usg=XDviwd1cl8Mh3gkoKqJNqcVi3k8.&source=chat), set up KXR and connect KXR to 133.x network through wifi. 
+
+
+After setting up KXR, connet to your pc
+```
+cd ~/prog/rcb4eus/Arduino/M5Stack/M5Stack_rcb4_rosserial
+roslaunch rosserial_wifi.launch
+```
+
+In the other terminal, start communication with dual_panda2
+```
+cd ~/prog/rcb4eus
+source ~/(ws)/devel/setup.bash
+roseus ~/.../jsk_2021_fix_kxr/euslisp/main/kxr_main.l
+```
+### Launching dual_panda2
+In leus@dual_panda2,
+```
+roslaunch jsk_panda_startup dual_panda2.launch
+```
+
+In your pc, launch detection nodes to recognize apriltag and terminal of connector 
+```
+rossetdualpanda2
+source ~/(ws)/devel/setup.bash
+roslaunch jsk_2021_fix_kxr fix_kxr_detections.launch
+```
+
+## Execution
+After setting KXR on workbench as shown in the figure below、start demo
+
+<img src="https://user-images.githubusercontent.com/48650394/180359214-30ca4f64-6571-4abb-a6cf-ea169517f0cb.png" width="550px">
+
+```
+rossetdualpanda2
+source ~/(ws)/devel/setup.bash
+roseus jsk_2021_fix_kxr/euslisp/main/panda_main.l
+```
+
+## Video
+See [here](https://drive.google.com/file/d/14vz8pw_OfTtmtCxj7IQx12pf1VkxYJls/view?usp=sharing)
+
+##

jsk_2021_fix_kxr/config/apriltag/settings.yaml

@@ -0,0 +1,12 @@
+# AprilTag 3 code parameters
+# Find descriptions in apriltag/include/apriltag.h:struct apriltag_detector
+#                      apriltag/include/apriltag.h:struct apriltag_family
+tag_family:        'tag36h11' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12
+tag_threads:       2          # default: 2
+tag_decimate:      1.0        # default: 1.0
+tag_blur:          0.0        # default: 0.0
+tag_refine_edges:  1          # default: 1
+tag_debug:         1          # default: 0
+max_hamming_dist:  2
+# Other parameters
+publish_tf:        true       # default: false

jsk_2021_fix_kxr/config/apriltag/tags_for_left.yaml

@@ -0,0 +1,57 @@
+# # Definitions of tags to detect
+#
+# ## General remarks
+#
+# - All length in meters
+# - Ellipsis (...) signifies that the previous element can be repeated multiple times.
+#
+# ## Standalone tag definitions
+# ### Remarks
+#
+# - name is optional
+#
+# ### Syntax
+#
+# standalone_tags:
+#   [
+#     {id: ID, size: SIZE, name: NAME},
+#     ...
+#   ]
+standalone_tags:
+  [
+  {id: 1, size: 0.0295, name: 'lpov-body_back'},
+  {id: 2, size: 0.0152, name: 'lpov-rarm_elbow_p_servo'},
+  {id: 3, size: 0.0152, name: 'lpov-rarm_shoulder_r_servo'},
+  {id: 4, size: 0.0152, name: 'lpov-rleg_ankle_r_servo'},
+  {id: 5, size: 0.0152, name: 'lpov-larm_elbow_p_servo'},
+  {id: 6, size: 0.0152, name: 'lpov-larm_shoulder_r_servo'},
+  {id: 7, size: 0.0152, name: 'lpov-lleg_ankle_r_servo'},
+  {id: 8, size: 0.0052, name: 'lpov-rarm_elbow_p_cable_r_f'},
+  {id: 9, size: 0.0052, name: 'lpov-rarm_elbow_p_cable_r_b'},
+  {id: 10, size: 0.0052, name: 'lpov-rarm_elbow_p_cable_l_f'},
+  {id: 11, size: 0.0052, name: 'lpov-rarm_elbow_p_cable_l_b'},
+  ]
+# ## Tag bundle definitions
+# ### Remarks
+#
+# - name is optional
+# - x, y, z have default values of 0 thus they are optional
+# - qw has default value of 1 and qx, qy, qz have default values of 0 thus they are optional
+#
+# ### Syntax
+#
+# tag_bundles:
+#   [
+#     {
+#       name: 'CUSTOM_BUNDLE_NAME',
+#       layout:
+#         [
+#           {id: ID, size: SIZE, x: X_POS, y: Y_POS, z: Z_POS, qw: QUAT_W_VAL, qx: QUAT_X_VAL, qy: QUAT_Y_VAL, qz: QUAT_Z_VAL},
+#           ...
+#         ]
+#     },
+#     ...
+#   ]
+tag_bundles:
+  [
+  ]

jsk_2021_fix_kxr/config/apriltag/tags_for_right.yaml

@@ -0,0 +1,57 @@
+# # Definitions of tags to detect
+#
+# ## General remarks
+#
+# - All length in meters
+# - Ellipsis (...) signifies that the previous element can be repeated multiple times.
+#
+# ## Standalone tag definitions
+# ### Remarks
+#
+# - name is optional
+#
+# ### Syntax
+#
+# standalone_tags:
+#   [
+#     {id: ID, size: SIZE, name: NAME},
+#     ...
+#   ]
+standalone_tags:
+  [
+  {id: 1, size: 0.0295, name: 'rpov-body_back'},
+  {id: 2, size: 0.0152, name: 'rpov-rarm_elbow_p_servo'},
+  {id: 3, size: 0.0152, name: 'rpov-rarm_shoulder_r_servo'},
+  {id: 4, size: 0.0152, name: 'rpov-rleg_ankle_r_servo'},
+  {id: 5, size: 0.0152, name: 'rpov-larm_elbow_p_servo'},
+  {id: 6, size: 0.0152, name: 'rpov-larm_shoulder_r_servo'},
+  {id: 7, size: 0.0152, name: 'rpov-lleg_ankle_r_servo'},
+  {id: 8, size: 0.0052, name: 'rpov-rarm_elbow_p_cable_r_f'},
+  {id: 9, size: 0.0052, name: 'rpov-rarm_elbow_p_cable_r_b'},
+  {id: 10, size: 0.0052, name: 'rpov-rarm_elbow_p_cable_l_f'},
+  {id: 11, size: 0.0052, name: 'rpov-rarm_elbow_p_cable_l_b'},
+  ]
+# ## Tag bundle definitions
+# ### Remarks
+#
+# - name is optional
+# - x, y, z have default values of 0 thus they are optional
+# - qw has default value of 1 and qx, qy, qz have default values of 0 thus they are optional
+#
+# ### Syntax
+#
+# tag_bundles:
+#   [
+#     {
+#       name: 'CUSTOM_BUNDLE_NAME',
+#       layout:
+#         [
+#           {id: ID, size: SIZE, x: X_POS, y: Y_POS, z: Z_POS, qw: QUAT_W_VAL, qx: QUAT_X_VAL, qy: QUAT_Y_VAL, qz: QUAT_Z_VAL},
+#           ...
+#         ]
+#     },
+#     ...
+#   ]
+tag_bundles:
+  [
+  ]

jsk_2021_fix_kxr/euslisp/arrange_to_jig/arrange_to_jig.l

@@ -0,0 +1,37 @@
+#!/usr/bin/env roseus
+
+(require "package://panda_eus/euslisp/dual_panda-interface.l")
+(require "package://jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l")
+(require "package://jsk_2021_fix_kxr/euslisp/panda_commons/utils.l")
+
+(ros::roseus-add-msgs "franka_msgs")
+(ros::roseus-add-msgs "franka_gripper")
+(ros::roseus-add-msgs "std_msgs")
+(ros::roseus-add-msgs "sensor_msgs")
+(ros::roseus-add-msgs "geometry_msgs")
+
+(ros::load-ros-manifest "franka_msgs")
+(ros::load-ros-manifest "apriltag_ros")
+
+(ros::roseus "grasp_and_insert_cable")
+
+(dual_panda-init)
+
+(objects (list *robot*))
+
+(send *ri* :set-joint-pd-gain "larm_joint1" 1000 30)
+(send *ri* :set-joint-pd-gain "larm_joint2" 1000 30)
+(send *ri* :set-joint-pd-gain "larm_joint3" 1000 30)
+(send *ri* :set-joint-pd-gain "larm_joint4" 1000 30)
+(send *ri* :set-joint-pd-gain "larm_joint5" 500 15)
+(send *ri* :set-joint-pd-gain "larm_joint6" 300 15)
+(send *ri* :set-joint-pd-gain "larm_joint7" 100 10)
+(send *ri* :set-joint-pd-gain "rarm_joint1" 1000 30)
+(send *ri* :set-joint-pd-gain "rarm_joint2" 1000 30)
+(send *ri* :set-joint-pd-gain "rarm_joint3" 1000 30)
+(send *ri* :set-joint-pd-gain "rarm_joint4" 1000 30)
+(send *ri* :set-joint-pd-gain "rarm_joint5" 500 15)
+(send *ri* :set-joint-pd-gain "rarm_joint6" 300 15)
+(send *ri* :set-joint-pd-gain "rarm_joint7" 100 10)
+
+(setq search-body-result (search-body))

jsk_2021_fix_kxr/euslisp/cable-insertion/cable-insertion_heuristic_init.l

@@ -0,0 +1,33 @@
+#!/usr/bin/env roseus
+
+(require "package://panda_eus/euslisp/dual_panda-interface.l")
+(ros::roseus-add-msgs "franka_msgs")
+(ros::roseus-add-msgs "franka_gripper")
+(dual_panda-init)
+
+(objects (list *robot*))
+
+(send *ri* :stop-grasp :rarm :width 0.006)
+
+(send *ri* :set-joint-pd-gain "rarm_joint1" 1000 30)
+(send *ri* :set-joint-pd-gain "rarm_joint2" 1000 30)
+(send *ri* :set-joint-pd-gain "rarm_joint3" 1000 30)
+(send *ri* :set-joint-pd-gain "rarm_joint4" 1000 30)
+(send *ri* :set-joint-pd-gain "rarm_joint5" 500 15)
+(send *ri* :set-joint-pd-gain "rarm_joint6" 300 15)
+(send *ri* :set-joint-pd-gain "rarm_joint7" 100 10)
+
+(send *robot* :reset-pose)
+(send *robot* :larm :move-end-pos #f(-500 0 0))
+(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(547 -18 825) :rpy (float-vector -3.14 1.57 1.57))) :translation-axis t :rotation-axis t)
+(send *ri* :angle-vector (send *robot* :angle-vector) 3000)
+(send *ri* :wait-interpolation)
+
+(do-until-key
+ (print "waiting for Enter...")
+ (unix::sleep 1))
+
+(send *ri* :start-grasp :rarm :effort 100)
+(unix::sleep 1)
+
+(exit)

jsk_2021_fix_kxr/euslisp/cable-insertion/cable-insertion_heuristic_insert.l

@@ -0,0 +1,71 @@
+#!/usr/bin/env roseus
+
+(require "package://panda_eus/euslisp/dual_panda-interface.l")
+(ros::roseus-add-msgs "franka_msgs")
+(ros::roseus-add-msgs "franka_gripper")
+(ros::roseus-add-msgs "std_msgs")                           
+(ros::roseus-add-msgs "sensor_msgs")                        
+(ros::roseus-add-msgs "geometry_msgs") 
+(ros::load-ros-manifest "franka_msgs")
+
+(dual_panda-init)
+
+(objects (list *robot*))
+
+(ros::roseus "cable-insertion_heuristic_insert")
+
+(send *ri* :set-joint-pd-gain "rarm_joint1" 1000 30)
+(send *ri* :set-joint-pd-gain "rarm_joint2" 1000 30)
+(send *ri* :set-joint-pd-gain "rarm_joint3" 1000 30)
+(send *ri* :set-joint-pd-gain "rarm_joint4" 1000 30)
+(send *ri* :set-joint-pd-gain "rarm_joint5" 500 15)
+(send *ri* :set-joint-pd-gain "rarm_joint6" 300 15)
+(send *ri* :set-joint-pd-gain "rarm_joint7" 100 10)
+
+(setq ee-pos #f(547 -18 825))
+(setq ee-rpy #f(-3.14 1.57 1.57))
+(setq wrench_init (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k))
+
+(send *robot* :reset-pose)
+(send *robot* :larm :move-end-pos #f(-500 0 0))
+(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos ee-pos :rpy ee-rpy)) :translation-axis t :rotation-axis t)
+(send *ri* :angle-vector (send *robot* :angle-vector) 3000)
+(send *ri* :wait-interpolation)
+
+(setq i 0)
+(while (< i 20)
+  (setq base_stroke #f(0.6 0 0))
+
+  (setq wrench (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k))
+  (setq wrench_diff (v- wrench wrench_init))
+  (setq d_x (elt wrench_diff 0))
+  (setq d_y (elt wrench_diff 1))
+
+  (if (> d_x 1.2)
+      (setq base_stroke #f(0.2 0.0 0.3)))
+  (if (< d_x -0.7)
+      (setq base_stroke #f(0.2 0.0 -0.3)))
+
+  (if (> d_y 1.2)
+      (setq base_stroke #f(0.2 -0.3 0)))
+  (if (< d_y -1.2)
+      (setq base_stroke #f(-0.05 0.5 0)))
+  (send *robot* :angle-vector (send *ri* :state :potentio-vector))
+  (send *robot* :rarm :move-end-pos base_stroke)
+  (send *ri* :angle-vector (send *robot* :angle-vector) 1000)
+  (send *ri* :wait-interpolation)
+  (setq i (+ i 1))
+  (ros::ros-info (format nil "i = ~A" i))
+  (ros::ros-info (format nil "stroke is ~A" base_stroke)))
+
+(send *ri* :stop-grasp :rarm :width 0.003)
+
+(send *robot* :rarm :move-end-pos #f(10 0 -3))
+(send *ri* :angle-vector (send *robot* :angle-vector) 3000)
+(send *ri* :wait-interpolation)
+
+(send *robot* :rarm :move-end-pos #f(-15 0 0))
+(send *ri* :angle-vector (send *robot* :angle-vector) 2000)
+(send *ri* :wait-interpolation)
+
+(exit)

jsk_2021_fix_kxr/euslisp/cable-insertion/cable-insertion_naive_grasp.l

@@ -0,0 +1,12 @@
+#!/usr/bin/env roseus
+
+(require "package://panda_eus/euslisp/dual_panda-interface.l")
+(ros::roseus-add-msgs "franka_msgs")
+(ros::roseus-add-msgs "franka_gripper")
+
+(dual_panda-init)
+
+(send *ri* :start-grasp :rarm)
+(unix::sleep 1)
+
+(exit)

jsk_2021_fix_kxr/euslisp/cable-insertion/cable-insertion_naive_init.l

@@ -0,0 +1,26 @@
+#!/usr/bin/env roseus
+
+(require "package://panda_eus/euslisp/dual_panda-interface.l")
+(ros::roseus-add-msgs "franka_msgs")
+(ros::roseus-add-msgs "franka_gripper")
+
+(dual_panda-init)
+
+(objects (list *robot*))
+
+(send *ri* :stop-grasp :rarm :width 0.006)
+
+(send *robot* :reset-pose)
+(send *robot* :larm :move-end-pos #f(-500 0 0))
+(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(547 -18 825) :rpy (float-vector -3.14 1.57 1.57))) :translation-axis t :rotation-axis t)
+(send *ri* :angle-vector (send *robot* :angle-vector) 3000)
+(send *ri* :wait-interpolation)
+
+(do-until-key
+ (print "waiting for Enter...")
+ (unix::sleep 1))
+
+(send *ri* :start-grasp :rarm :effort 100)
+(unix::sleep 1)
+
+(exit)