diff --git a/jsk_2024_10_semi/pr2_tubes/.catkin_workspace b/jsk_2024_10_semi/pr2_tubes/.catkin_workspace new file mode 100644 index 000000000..52fd97e7e --- /dev/null +++ b/jsk_2024_10_semi/pr2_tubes/.catkin_workspace @@ -0,0 +1 @@ +# This file currently only serves to mark the location of a catkin workspace for tool integration diff --git a/jsk_2024_10_semi/pr2_tubes/CMakeLists.txt b/jsk_2024_10_semi/pr2_tubes/CMakeLists.txt new file mode 100644 index 000000000..fd690c7b8 --- /dev/null +++ b/jsk_2024_10_semi/pr2_tubes/CMakeLists.txt @@ -0,0 +1,208 @@ +cmake_minimum_required(VERSION 3.0.2) +project(pr2_surgery) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + jsk_recognition_msgs + roscpp + roseus + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# jsk_recognition_msgs# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES pr2_surgery +# CATKIN_DEPENDS jsk_recognition_msgs roscpp roseus rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/pr2_surgery.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/pr2_surgery_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_pr2_surgery.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/jsk_2024_10_semi/pr2_tubes/config/table_marker.yaml b/jsk_2024_10_semi/pr2_tubes/config/table_marker.yaml new file mode 100644 index 000000000..0db13fdb8 --- /dev/null +++ b/jsk_2024_10_semi/pr2_tubes/config/table_marker.yaml @@ -0,0 +1,16 @@ +boxes: +- dimensions: + - 0.5 + - 0.3 + - 0.2 + frame_id: base_footprint + name: table + orientation: + - 3.9531047036326084e-10 + - 0.008311730048626616 + - 4.293287988035861e-12 + - 0.9999654569751891 + position: + - 0.7592881917953491 + - 0.015209555625915527 + - 0.849845826625824 diff --git a/jsk_2024_10_semi/pr2_tubes/launch/recognize_wound.launch b/jsk_2024_10_semi/pr2_tubes/launch/recognize_wound.launch new file mode 100755 index 000000000..a8459f8e7 --- /dev/null +++ b/jsk_2024_10_semi/pr2_tubes/launch/recognize_wound.launch @@ -0,0 +1,157 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + queue_size: 100 + + + + + + display_interactive_manipulator: true + display_interactive_manipulator_only_selected: true + display_description_only_selected: true + + + + + + + config_file: $(find pr2_surgery)/config/table_marker.yaml + config_auto_save: true + + + + + + + index: 0 + + + + + + + + + use_multiple_attention: false + + + + + + + keep_organized: true + approximate_sync: true + max_queue_size: 100 + + + + + + + use_indices: false + keep_organized: true + h_limit_max: 10 + h_limit_min: -10 + s_limit_max: 255 + s_limit_min: 35 + i_limit_max: 255 + i_limit_min: 0 + + + + + + + tolerance: 0.02 + min_size: 10 + downsample_enable: true + cluster_filter: 1 + + + + + + + + align_boxes: true + align_boxes_with_plane: false + force_to_flip_z_axis: false + use_pca: false + target_frame_id: base_footprint + + + + + diff --git a/jsk_2024_10_semi/pr2_tubes/package.xml b/jsk_2024_10_semi/pr2_tubes/package.xml new file mode 100644 index 000000000..c7b57c504 --- /dev/null +++ b/jsk_2024_10_semi/pr2_tubes/package.xml @@ -0,0 +1,74 @@ + + + pr2_surgery + 0.0.0 + The pr2_surgery package + + + + + mech-user + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + jsk_recognition_msgs + roscpp + roseus + rospy + std_msgs + jsk_recognition_msgs + roscpp + roseus + rospy + std_msgs + jsk_recognition_msgs + roscpp + roseus + rospy + std_msgs + + + + + + + + diff --git a/jsk_2024_10_semi/pr2_tubes/src/CMakeLists.txt b/jsk_2024_10_semi/pr2_tubes/src/CMakeLists.txt new file mode 120000 index 000000000..201681686 --- /dev/null +++ b/jsk_2024_10_semi/pr2_tubes/src/CMakeLists.txt @@ -0,0 +1 @@ +/opt/ros/noetic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/jsk_2024_10_semi/pr2_tubes/src/main.l b/jsk_2024_10_semi/pr2_tubes/src/main.l new file mode 100755 index 000000000..ebe16d50a --- /dev/null +++ b/jsk_2024_10_semi/pr2_tubes/src/main.l @@ -0,0 +1,472 @@ +#!/usr/bin/env roseus + +#| DESCRIPTION +PR2で血管モデルを縫合する． +|# + +;;Load pkgs around PR2 Interface +(require "package://pr2eus/pr2.l") +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") ;;*ri* +(require "package://pr2eus/speak.l") ;;pkg for speaking + +;;ros msgs +(ros::load-ros-package "jsk_recognition_msgs") +(ros::roseus-add-msgs "speech_recognition_msgs") +(ros::roseus-add-msgs "geometry_msgs") + + +;;ここをかえるだけで調整できる！（外界の変化に合わせて） +;; Drawing Pram +(setq *draw-param-left* 0.2) +(setq *draw-param-right* 0.6) +;; Env Param +(setq *centerx* 700) +(setq *centery* 0) +(setq *centerz* 755) +(setq *deskw* 500) +;; Hampen Param +(setq *target-ypos* 0) +(setq *target-xpos* 0) +(setq *target-zpos* 30) +;; Traj Param +(setq *needle_len* 70) +(setq *interval* 10) +(setq *traj_num* 10) +(setq *traj_len* (* *interval* *traj_num*)) +;; Set remain +(setq *remain* 450) ;;450 ;;how long remaining thread? +(setq *diff_remain* 55) ;;55 +;; Offset +(setq *rarm_offset* 2) + + +;; Frag (DO NOT CHANGE) +(setq *start-flag* 0) +(setq *is_first* 1) + +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; Environment ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;; Define set-environment method +(defun set-env() + ;;Set center + (setq *deskh* *centerz*) + + (setq *center* (make-cube 10 10 10)) + (send *center* :translate (float-vector *centerx* *centery* *centerz*)) + (send *center* :set-color :black) + (setq *o* (send *center* :copy-worldcoords)) + + ;;Set desk + (setq *desk* (make-cube *deskw* *deskw* *deskh*)) + (send *desk* :translate (float-vector *centerx* *centery* (/ *centerz* 2))) + (send *desk* :set-color :brown) + + ;;Set needle + (setq *needle* (make-cylinder 0.5 *needle_len*)) + (send *needle* :translate (v+ (float-vector -100 20 300) (send *center* :pos))) + (send *needle* :set-color :yellow) + (send *needle* :rotate #d90 :y) + + ;;Set hampen + (setq *hampen* (make-cube 50 50 50)) + (send *hampen* :translate (v+ (float-vector 0 0 10) (send *center* :pos))) + (send *hampen* :set-color :white) + (send *hampen* :rotate #d45 :x) + + (send *desk* :put :left-coords + (make-cascoords + :coords (send (send *desk* :copy-worldcoords) :translate (float-vector 0 0 500)) + :parent *pr2*)) + (send *needle* :put :left-coords + (make-cascoords + :coords (send (send *needle* :copy-worldcoords) :translate (float-vector 0 0 -5)) + :parent *needle*)) +) + +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; Sensing ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;; Voice Detection +(defun speech-cb(msg) + (setq *ans* (elt (send msg :transcript) 0)) + (print *ans*) + (if (equal *ans* "どうぞ") + (setq *start-flag* 1) + ;; 1回フラグが立てばそれでいい + )) + +(defun detect-voice () + (ros::ros-info "start waiting for call ~%") + (ros::subscribe "speech_to_text" speech_recognition_msgs::SpeechRecognitionCandidates #'speech-cb) + (ros::rate 10) + (while (not (eq *start-flag* 1)) + (ros::spin-once) + (ros::sleep) + (ros::ros-info "wait for voice command ...") + ) + ) + +;; Perception +(defun cb-recog(msg) + (ros::ros-info "boxes ~A" (length (send msg :boxes))) + (if (and (> (length (send msg :boxes)) 0) (> (send (send (elt (send msg :boxes) 0) :dimensions) :x) 0)) + (setq *target-ypos-tmp* (send (send (send (elt (send msg :boxes) 0) :pose) :position) :y)) + ) + ) + +(defun detect-wound() + (ros::subscribe "/recognize_wound/cluster_decomposer/boxes" jsk_recognition_msgs::BoundingBoxArray #'cb-recog) + (ros::rate 10) + (while (null *target-ypos-tmp*) + (ros::ros-info "wait for target...") + (ros::sleep) + (ros::spin-once)) + (setq *target-ypos* *target-ypos-tmp*) + ) + +(defun cb-rforce(msg) + (ros::ros-info "checking right-virtual-force") + (setq *rforce* (send (send (send msg :wrench) :force) :z)) + (format t "rforce: ~A ~%" *rforce*) + (if (> (send (send (send msg :wrench) :force) :z) *draw-param-right*) + (progn + (setq *right-drawing* t) + (format t "drawing~%") + ) + ) + ) + +(defun cb-lforce(msg) + (ros::ros-info "checking left-virtual-force") + (setq *lforce* (send (send (send msg :wrench) :force) :z)) + (if (> (send (send (send msg :wrench) :force) :z) *draw-param-left*) + (progn + (setq *left-drawing* t) + (format t "drawing~%") + ) + ) + ) +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; Stitching ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; Path ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;; Define Trajectory Class +(defclass traj + :super cascaded-coords + :slots (points)) + +(defmethod traj + (:init (&rest args) + (send-super* :init args) + (dotimes (i *traj_num*) + (push + (make-cube 10 10 10 :pos (float-vector (* (- i 5) *interval*) 0 0)) + points)) + (dotimes (i (- (length points) 1)) + (send (elt points i) :assoc (elt points (+ 1 i)))) + (send self :assoc (car points)) + self) + (:points () points) + ) + + +;; Define set-environment method +(defun set-env() + ;;Set center + (setq *deskh* *centerz*) + + (setq *center* (make-cube 10 10 10)) + (send *center* :translate (float-vector *centerx* *centery* *centerz*)) + (send *center* :set-color :black) + (setq *o* (send *center* :copy-worldcoords)) + + ;;Set desk + (setq *desk* (make-cube *deskw* *deskw* *deskh*)) + (send *desk* :translate (float-vector *centerx* *centery* (/ *centerz* 2))) + (send *desk* :set-color :brown) + + ;;Set needle + (setq *needle* (make-cylinder 0.5 *needle_len*)) + (send *needle* :translate (v+ (float-vector -100 20 300) (send *center* :pos))) + (send *needle* :set-color :yellow) + (send *needle* :rotate #d90 :y) + + ;;Set hampen + (setq *hampen* (make-cube 50 50 50)) + (send *hampen* :translate (v+ (float-vector 0 0 10) (send *center* :pos))) + (send *hampen* :set-color :white) + (send *hampen* :rotate #d45 :x) + + (send *desk* :put :left-coords + (make-cascoords + :coords (send (send *desk* :copy-worldcoords) :translate (float-vector 0 0 500)) + :parent *pr2*)) + (send *needle* :put :left-coords + (make-cascoords + :coords (send (send *needle* :copy-worldcoords) :translate (float-vector 0 0 -5)) + :parent *needle*)) +) + +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; Motion ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;;Define preparation task. +(defun preparation() + (send *ri* :speak-jp "縫合を始めます．注意してください．" :wait t) + (send *pr2* :reset-pose) ;;Set Initial Pose + (send *pr2* :torso :waist-z :joint-angle 240) + (send *ri* :start-grasp :arms) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + (send *pr2* :head :look-at (send *center* :worldpos)) + + (send *pr2* :larm :inverse-kinematics + (send (send (send *needle* :get :left-coords) + :copy-worldcoords) + :rotate (deg2rad 90) :z) + :rotation-axis :z) ;;IK to needle pose + (send *pr2* :larm :end-coords :assoc *needle*) ;; + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ;;Grasp needle (real) + (send *ri* :speak-jp "左手に，針を持たせてください．" :wait t) + (send *ri* :stop-grasp :larm :wait t) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + ) + + +;;Define initial task. +(defun init-task() + (send *ri* :speak-jp "手を閉じます．挟まれないように気をつけてください" :wait t) + (send *ri* :start-grasp :larm :wait t) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + (send *ri* :speak-jp "これから針を持って作業します．離れてください．" :wait t) + (unix:sleep 2) + + ;;Grasp needle (dummy) + (send *pr2* :larm :inverse-kinematics (send (send (send *needle* :get :left-coords) :copy-worldcoords) :rotate (deg2rad 90) :z) :rotation-axis :z) + (send *pr2* :larm :end-coords :assoc *needle*) ;;針を固定 + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) +) + + +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; others ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +(defun stop() + (send *ri* :stop-grasp) + ) + +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;;;;main.l;;;;; +;;Set PR2 +(if (not (boundp '*pr2*)) (pr2-init)) + +;; Set the experimental environments. +(set-env) + +;; Show all objects in irtviewer. +(objects (list *pr2* *center* *desk* *needle* *hampen*)) + +;; Human help Robot to grab needle. +(preparation) + +;; The Nurse pass the needle saying "douzo!" +(if (send *ri* :simulation-modep) + (unix:sleep 2) + (detect-voice) + ) + +;; Do initial task. +(init-task) + +;; Dummy pos to be able to calculate IK +(setq current-coords (send (send *desk* :get :left-coords) :copy-worldcoords)) +(setq new-coords (send current-coords :translate (float-vector 0 100 50))) +(send *pr2* :larm :inverse-kinematics new-coords :rotation-axis :x) +(send *pr2* :larm :end-coords :assoc *needle*) +(send *ri* :angle-vector (send *pr2* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +;; Perception +;; Perception (dummy) +(ros::subscribe "/recognize_wound/cluster_decomposer/boxes" jsk_recognition_msgs::BoundingBoxArray #'cb-recog) +(if (send *ri* :simulation-modep) + (setq *target-ypos-tmp* 0) + (setq *target-ypos-tmp* nil) + ) +(detect-wound) + +;; Stitch +(dotimes (i 4) ;;5は難しい。。。 + ;; Make trajectory points. + (setq r (instance traj :init)) + (send r :rotate pi/2 :z) + (setq *target* (v+ (send *center* :pos) (float-vector (+ *target-xpos* (* i 3)) (+ *target-ypos* (* *traj_len* 0.12)) *target-zpos*))) ;; 0.2は実験則 ;; Trajectoryの中心座標をHampenにあわせている + (send r :locate *target* :world) + (objects (append (list *pr2* *center* *desk* *needle* *hampen*) (send r :points))) + + ;; Set start position. + ;; Rarm + (setq *r-pos* (send (elt (send r :points) 9) :copy-worldcoords)) + (send *r-pos* :rotate pi/2 :y :local) + (send *r-pos* :rotate -pi/2 :z :local) ;; if -pi/2, rarm state is parallel to desk + (send *r-pos* :translate #f(-10 0 0) :local) + (send *pr2* :rarm :inverse-kinematics *r-pos* :rotation-axis t) + (send *ri* :angle-vector (send *pr2* :angle-vector) 500) + (send *ri* :stop-grasp :rarm) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + ;; Follow the trajectory points. Stitch. + (setq tm 500) + (setq path (list)) + (setq time-vec (list 4000)) + (dolist (e (send r :points)) + (let (ee) + (setq ee (send e :copy-worldcoords)) + (send ee :translate (float-vector (- *needle_len* *rarm_offset*) 0 0) :local) + (send ee :rotate pi :z :local) + (send ee :rotate pi/2 :y :local) + (send ee :draw-on :flush t :size 100) + (push (send *pr2* :larm :inverse-kinematics ee :rotation-axis t) path) + (push tm time-vec) + ) + ) + (pop time-vec) + (nreverse time-vec) + (nreverse path) + (send *ri* :angle-vector-sequence path time-vec + :default-controller 0.001 + :min-time 0.0001 + :minjerk-interpolation t + ) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ;; Pass on the needle. (左手から右手に針を戻す) + (send *ri* :start-grasp :rarm :wait t) + (send *ri* :wait-interpolation) + (unix:sleep 1) + (send *ri* :stop-grasp :larm :wait t) + (send *ri* :wait-interpolation) + + ;; Fix the right arm and neadle (針の固定を左から右に移す) + (send *pr2* :larm :end-coords :dissoc *needle*) + (send *pr2* :rarm :end-coords :assoc *needle*) + (format t "is_first: ~A ~%" *is_first*) + ;; Draw the needle from the object. (対象物から針を抜き出) + (if (= *is_first* 1) + (progn (send *pr2* :rarm :move-end-pos (float-vector 0 0 (- 0 *remain*)) :local) + (send *ri* :angle-vector-sequence (list (send *pr2* :angle-vector)) (list 10000) + :default-controller 0.001 + :min-time 0.0001 + :minjerk-interpolation t + ) (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + (setq *remain* (- *remain* *diff_remain*)) + ) + (progn + (setq remain_tmp (- *remain* *diff_remain*)) + (send *pr2* :rarm :move-end-pos (float-vector 0 0 -60) :local) + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + ;;(setq *remain* (- *remain* 60));; オリジナリティポイント: 1回縫うと針を引ける量が減るのを再現 + (format t "remaining ~A ~%" *remain*) ;; for debug! (*remain*が0になったらbreakするようにしたい) + (if (> *remain* remain_tmp) + (progn + (setq *right-drawing* nil) + (setq *position1* (send *ri* :state :angle-vector)) + (ros::subscribe "/right_endeffector/wrench" geometry_msgs::WrenchStamped #'cb-rforce 1) + (send *pr2* :rarm :move-end-pos (float-vector 0 0 (- remain_tmp)) :local) + (setq *position2* (send *pr2* :angle-vector)) + ;;(send *ri* :angle-vector (print (send *pr2* :angle-vector)) 10000) ;;bug + (send *ri* :angle-vector-sequence (list (send *pr2* :angle-vector)) (list 10000) + :default-controller 0.001 + :min-time 0.0001 + :minjerk-interpolation t + ) + (send *irtviewer* :draw-objects) + (ros::duration-sleep 2.0) + (while (null *right-drawing*) + (ros::duration-sleep 0.05) + (ros::spin-once) + (if (not (send *ri* :interpolatingp)) + (setq *right-drawing* t)) + ) + (send *ri* :cancel-angle-vector :wait t) + ;;(send *ri* :stop-motion) + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + ) + ) + ) + ) + + + ;; Move left hand to the delivery point. + (setq *pass-larm* (send (send (send (send (send *center* :copy-worldcoords) + :translate (float-vector 45 (* *needle_len* 0.5) 150)) + :rotate pi/2 :z) + :rotate pi/2 :y) + :rotate pi :x)) + (send *pass-larm* :draw-on :flush t :size 100) ;; for debug! + (send *pr2* :larm :inverse-kinematics *pass-larm* :rotation-axis t :use-torso t) ;; Use torso! (腰を使わないとIKが解けないので注意) + (send *ri* :angle-vector (send *pr2* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ;; Move right hand to the delivery point. + (setq *pass-rarm* (send (send (send (send *center* :copy-worldcoords) + :translate (float-vector 45 (- 0 (* *needle_len* 0.5)) 150)) + :translate #f(-10 0 0)) + :rotate -pi/2 :x)) + (send *pass-rarm* :draw-on :flush t :size 100) ;; for debug! + (send *pr2* :rarm :inverse-kinematics *pass-rarm* :rotation-axis t) ;; Do not use torso! (左手との相対座標が狂う) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (send *irtviewer* :draw-objects) + + ;; Pass on the needle. (右手から左手に針を戻す) + (send *ri* :start-grasp :larm) + (send *ri* :wait-interpolation) + (unix:sleep 1) + (send *ri* :stop-grasp :rarm) + (send *ri* :wait-interpolation) + + ;; Fix the left arm and needle again (針の固定を右から左に移す) + (send *pr2* :rarm :end-coords :dissoc *needle*) + (send *pr2* :larm :end-coords :assoc *needle*) + (send *ri* :speak-jp "繰り返し，つぎの経路で縫います．注意してください．" :wait t) + (unix:sleep 1) + (send *pr2* :rarm :move-end-pos (float-vector 0 0 -50) :local) + (send *ri* :wait-interpolation) + (ros::rate 10) + (setq *left-drawing* nil) + ;;余っている糸を左に引く作業を知的化した + (ros::subscribe "/left_endeffector/wrench" geometry_msgs::WrenchStamped #'cb-lforce 1) + (send *pr2* :larm :move-end-pos (float-vector 0 0 -240) :local) + ;;(send *ri* :angle-vector (send *pr2* :angle-vector) 15000) + (send *ri* :angle-vector-sequence (list (send *pr2* :angle-vector)) (list 15000) + :default-controller 0.001 + :min-time 0.0001 + :end-coords-interpolation t + ) + (while (null *left-drawing*) + (ros::duration-sleep 0.02) + (ros::spin-once) + (if (not (send *ri* :interpolatingp)) + (setq *left-drawing* t)) + ) + (send *ri* :cancel-angle-vector :wait t) + (setq *is_first* 0) + ) + +(send *pr2* :reset-pose) ;;Set Initial Pose +(send *ri* :stop-grasp :arms) +(send *ri* :angle-vector (send *pr2* :angle-vector) 10000) +(send *ri* :wait-interpolation) +(send *irtviewer* :draw-objects) + +(send *ri* :speak-jp "縫合がおわりました．お疲れ様でした．" :wait t) diff --git a/jsk_2024_10_semi/pr2_tubes/src/needle.l b/jsk_2024_10_semi/pr2_tubes/src/needle.l new file mode 100644 index 000000000..eec03ccc7 --- /dev/null +++ b/jsk_2024_10_semi/pr2_tubes/src/needle.l @@ -0,0 +1,82 @@ +#| +Desctiption : +Make a bend needle model here! + +Author : +@Michi-Tsubaki +|# + +;; Load irteus visualization +(require "package://pr2eus/pr2.l") +(require "package://pr2eus/pr2-utils.l") +(require "package://pr2eus/pr2-interface.l") +(require "package://pr2eus/speak.l") + +; Define needle parameters +(setq *needle-radius* 35) ; radius of curvature +(setq *needle-thickness* 0.5) ; thickness of the needle +(setq *needle-angle* (/ pi 2)) ; angle of the arc + +;; Create points for the needle path +(setq segments 40) +(setq theta-step (/ *needle-angle* segments)) +(setq points nil) +(dotimes (i (+ segments 1)) + (let ((theta (* i theta-step))) + (push (float-vector + (* *needle-radius* (- 1 (cos theta))) ; x + 0 ; y + (* *needle-radius* (sin theta))) ; z + points))) +(setq points (reverse points)) + +;; Function to calculate rotation matrix from two points +(defun make-rotation-matrix (p1 p2) + (let* ((diff (v- p2 p1)) + (z-axis (normalize-vector diff)) + (y-axis (float-vector 0 1 0)) + (x-axis (v* y-axis z-axis))) + (setq y-axis (v* z-axis x-axis)) ; ensure orthogonality + (transpose (matrix (normalize-vector x-axis) + (normalize-vector y-axis) + (normalize-vector z-axis))))) + +;; Create cylinders along the arc +(setq needle-parts nil) +(let ((prev-point (car points))) + (dolist (point (cdr points)) + (let* ((diff (v- point prev-point)) + (height (norm diff)) + (center (midpoint 0.5 prev-point point)) + (cylinder (make-cylinder *needle-thickness* height :segments 12)) + (rot-mat (make-rotation-matrix prev-point point))) + ;; Create coordinates at center with proper orientation + (setq coords (make-coords :pos center :rot rot-mat)) + ;; Orient and position cylinder + (send cylinder :transform coords) + (push cylinder needle-parts) + (setq prev-point point)))) + +;; Create the bodyset +(setq needle-bodies (reverse needle-parts)) + +;; Associate all parts with the first one +(dolist (body (cdr needle-bodies)) + (send (car needle-bodies) :assoc body)) + +;; 針の色（銀色）にする +(dolist (body needle-bodies) + (send body :set-color :silver)) + +;; needleをbodyset-linkとして定義する +(setq *needle* + (instance bodyset-link :init (make-cascoords) + :bodies needle-bodies)) + +;; 表示する +(make-irtviewer) +(objects (list *needle*)) + +;; Optional: rotate the view to better see the needle +(send *irtviewer* :viewing :look #f(0 -1 0.1)) +(send *irtviewer* :draw-objects)