From e2158ad688ef958e17df0b6951642c7bce417400 Mon Sep 17 00:00:00 2001 From: softyanija Date: Thu, 3 Jun 2021 16:06:51 +0900 Subject: [PATCH 001/127] made ws and base of amabe_screw.l --- jsk_2021_fix_kxr/euslisp/#amabe_screw.l# | 14 ++++++++++++++ jsk_2021_fix_kxr/euslisp/amabe_screw.l | 14 ++++++++++++++ jsk_2021_fix_kxr/euslisp/amabe_screw_memo.txt | 3 +++ 3 files changed, 31 insertions(+) create mode 100644 jsk_2021_fix_kxr/euslisp/#amabe_screw.l# create mode 100644 jsk_2021_fix_kxr/euslisp/amabe_screw.l create mode 100644 jsk_2021_fix_kxr/euslisp/amabe_screw_memo.txt diff --git a/jsk_2021_fix_kxr/euslisp/#amabe_screw.l# b/jsk_2021_fix_kxr/euslisp/#amabe_screw.l# new file mode 100644 index 0000000000..b1d86a04bf --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/#amabe_screw.l# @@ -0,0 +1,14 @@ +#!/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 *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1246.738) :rpy (float-vector 0.456 1.57 -1.0))) :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(425.117 53.404 1136.875) :rpy (float-vector -1.57 0.492 -3.124))) :rotation-axis t) +|# diff --git a/jsk_2021_fix_kxr/euslisp/amabe_screw.l b/jsk_2021_fix_kxr/euslisp/amabe_screw.l new file mode 100644 index 0000000000..58e823d242 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/amabe_screw.l @@ -0,0 +1,14 @@ +#!/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 *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1246.738) :rpy (float-vector 0.456 1.57 -2.67))) :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(425.117 53.404 1136.875) :rpy (float-vector -1.57 0.492 0))) :rotation-axis t) +|# diff --git a/jsk_2021_fix_kxr/euslisp/amabe_screw_memo.txt b/jsk_2021_fix_kxr/euslisp/amabe_screw_memo.txt new file mode 100644 index 0000000000..352ec431de --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/amabe_screw_memo.txt @@ -0,0 +1,3 @@ +#締める前 +r (422.782 -14.541 1246.738 / 0.456 1.485 -2.67) +l (425.117 53.404 1136.875 / -1.63 0.492 -3.124) From 49e531fdd3d09a6a6057a4386b9dde86555b35d2 Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 4 Jun 2021 15:19:04 +0900 Subject: [PATCH 002/127] made amabe_screw.l and _pre --- jsk_2021_fix_kxr/euslisp/#amabe_screw.l# | 14 ----------- jsk_2021_fix_kxr/euslisp/#amabe_screw_pre.l# | 26 ++++++++++++++++++++ jsk_2021_fix_kxr/euslisp/.#amabe_screw_pre.l | 1 + jsk_2021_fix_kxr/euslisp/amabe_screw.l | 18 ++++++++------ jsk_2021_fix_kxr/euslisp/amabe_screw_pre.l | 26 ++++++++++++++++++++ 5 files changed, 64 insertions(+), 21 deletions(-) delete mode 100644 jsk_2021_fix_kxr/euslisp/#amabe_screw.l# create mode 100644 jsk_2021_fix_kxr/euslisp/#amabe_screw_pre.l# create mode 120000 jsk_2021_fix_kxr/euslisp/.#amabe_screw_pre.l create mode 100644 jsk_2021_fix_kxr/euslisp/amabe_screw_pre.l diff --git a/jsk_2021_fix_kxr/euslisp/#amabe_screw.l# b/jsk_2021_fix_kxr/euslisp/#amabe_screw.l# deleted file mode 100644 index b1d86a04bf..0000000000 --- a/jsk_2021_fix_kxr/euslisp/#amabe_screw.l# +++ /dev/null @@ -1,14 +0,0 @@ -#!/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 *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1246.738) :rpy (float-vector 0.456 1.57 -1.0))) :rotation-axis t) -(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(425.117 53.404 1136.875) :rpy (float-vector -1.57 0.492 -3.124))) :rotation-axis t) -|# diff --git a/jsk_2021_fix_kxr/euslisp/#amabe_screw_pre.l# b/jsk_2021_fix_kxr/euslisp/#amabe_screw_pre.l# new file mode 100644 index 0000000000..5a4c0e4572 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/#amabe_screw_pre.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 *robot* :reset-pose) +(send *robot* :rarm :move-end-pos #f(-500 0 0)) +(send *robot* :larm :move-end-pos #f(-500 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1246.738) :rpy (float-vector 0.456 1.57 1))) :rotation-\ +axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(425.117 53.404 1136.875) :rpy (float-vector -1.57 0.492 pi))) :rota\ +tion-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +;; (send *ri* :start-grasp :rarm) +;; (send *ri* :start-grasp :larm) diff --git a/jsk_2021_fix_kxr/euslisp/.#amabe_screw_pre.l b/jsk_2021_fix_kxr/euslisp/.#amabe_screw_pre.l new file mode 120000 index 0000000000..2bce4a06e8 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/.#amabe_screw_pre.l @@ -0,0 +1 @@ +amabe@ubuntu.9399:1622701014 \ No newline at end of file diff --git a/jsk_2021_fix_kxr/euslisp/amabe_screw.l b/jsk_2021_fix_kxr/euslisp/amabe_screw.l index 58e823d242..eb336cb78e 100644 --- a/jsk_2021_fix_kxr/euslisp/amabe_screw.l +++ b/jsk_2021_fix_kxr/euslisp/amabe_screw.l @@ -5,10 +5,14 @@ (ros::roseus-add-msgs "franka_msgs") (ros::roseus-add-msgs "franka_gripper") -(dual_panda-init) - -(objects (list *robot*)) - #| -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1246.738) :rpy (float-vector 0.456 1.57 -2.67))) :rotation-axis t) -(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(425.117 53.404 1136.875) :rpy (float-vector -1.57 0.492 0))) :rotation-axis t) -|# +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1231.738) :rpy (float-vector 0.456 1.57 1))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1231.738) :rpy (float-vector 0.456 1.57 3))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1246.738) :rpy (float-vector 0.456 1.57 3))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) diff --git a/jsk_2021_fix_kxr/euslisp/amabe_screw_pre.l b/jsk_2021_fix_kxr/euslisp/amabe_screw_pre.l new file mode 100644 index 0000000000..5510d96862 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/amabe_screw_pre.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 *robot* :reset-pose) +(send *robot* :rarm :move-end-pos #f(-500 0 0)) +(send *robot* :larm :move-end-pos #f(-500 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1246.738) :rpy (float-vector 0.456 1.57 1))) :rotation-\ +axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(425.117 53.404 1136.875) :rpy (float-vector -1.57 0.492 pi))) :rota\ +tion-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +;; (send *ri* :start-grasp :rarm) +;; (send *ri* :start-grasp :larm) From 58c23ad9a662c39fc38253fc3c1aae4480a20ffa Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 7 Jun 2021 14:44:27 +0900 Subject: [PATCH 003/127] tighten a screw twice --- jsk_2021_fix_kxr/euslisp/#amabe_screw_pre.l# | 26 -------------------- jsk_2021_fix_kxr/euslisp/.#amabe_screw_pre.l | 1 - jsk_2021_fix_kxr/euslisp/amabe_screw.l | 26 +++++++++++++++++--- jsk_2021_fix_kxr/euslisp/amabe_screw_pre.l | 6 ++--- 4 files changed, 25 insertions(+), 34 deletions(-) delete mode 100644 jsk_2021_fix_kxr/euslisp/#amabe_screw_pre.l# delete mode 120000 jsk_2021_fix_kxr/euslisp/.#amabe_screw_pre.l diff --git a/jsk_2021_fix_kxr/euslisp/#amabe_screw_pre.l# b/jsk_2021_fix_kxr/euslisp/#amabe_screw_pre.l# deleted file mode 100644 index 5a4c0e4572..0000000000 --- a/jsk_2021_fix_kxr/euslisp/#amabe_screw_pre.l# +++ /dev/null @@ -1,26 +0,0 @@ -#!/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 *robot* :reset-pose) -(send *robot* :rarm :move-end-pos #f(-500 0 0)) -(send *robot* :larm :move-end-pos #f(-500 0 0)) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1246.738) :rpy (float-vector 0.456 1.57 1))) :rotation-\ -axis t) -(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(425.117 53.404 1136.875) :rpy (float-vector -1.57 0.492 pi))) :rota\ -tion-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - -;; (send *ri* :start-grasp :rarm) -;; (send *ri* :start-grasp :larm) diff --git a/jsk_2021_fix_kxr/euslisp/.#amabe_screw_pre.l b/jsk_2021_fix_kxr/euslisp/.#amabe_screw_pre.l deleted file mode 120000 index 2bce4a06e8..0000000000 --- a/jsk_2021_fix_kxr/euslisp/.#amabe_screw_pre.l +++ /dev/null @@ -1 +0,0 @@ -amabe@ubuntu.9399:1622701014 \ No newline at end of file diff --git a/jsk_2021_fix_kxr/euslisp/amabe_screw.l b/jsk_2021_fix_kxr/euslisp/amabe_screw.l index eb336cb78e..0798f6c747 100644 --- a/jsk_2021_fix_kxr/euslisp/amabe_screw.l +++ b/jsk_2021_fix_kxr/euslisp/amabe_screw.l @@ -5,14 +5,34 @@ (ros::roseus-add-msgs "franka_msgs") (ros::roseus-add-msgs "franka_gripper") -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1231.738) :rpy (float-vector 0.456 1.57 1))) :rotation-axis t) +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 1))) :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1231.738) :rpy (float-vector 0.456 1.57 3))) :rotation-axis t) +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.456 1.57 1))) :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1246.738) :rpy (float-vector 0.456 1.57 3))) :rotation-axis t) +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.456 1.57 3))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 3))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 1))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.456 1.57 1))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.456 1.57 3))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 3))) :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) diff --git a/jsk_2021_fix_kxr/euslisp/amabe_screw_pre.l b/jsk_2021_fix_kxr/euslisp/amabe_screw_pre.l index 5510d96862..4300e2a40d 100644 --- a/jsk_2021_fix_kxr/euslisp/amabe_screw_pre.l +++ b/jsk_2021_fix_kxr/euslisp/amabe_screw_pre.l @@ -15,10 +15,8 @@ (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1246.738) :rpy (float-vector 0.456 1.57 1))) :rotation-\ -axis t) -(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(425.117 53.404 1136.875) :rpy (float-vector -1.57 0.492 pi))) :rota\ -tion-axis t) +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 1))) :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(425.117 53.404 1069.875) :rpy (float-vector -1.57 0.492 pi))) :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) From b013b99c983ee3cfb61f981c42ea424e15de52a1 Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 7 Jun 2021 18:20:30 +0900 Subject: [PATCH 004/127] made amabe_loosen_screw --- jsk_2021_fix_kxr/euslisp/#amabe_screw.l# | 38 +++++++++++++ jsk_2021_fix_kxr/euslisp/.#amabe_screw.l | 1 + jsk_2021_fix_kxr/euslisp/amabe_loosen_screw.l | 54 +++++++++++++++++++ .../euslisp/amabe_loosen_screw_pre.l | 32 +++++++++++ jsk_2021_fix_kxr/euslisp/amabe_screw_pre.l | 13 ++++- 5 files changed, 136 insertions(+), 2 deletions(-) create mode 100644 jsk_2021_fix_kxr/euslisp/#amabe_screw.l# create mode 120000 jsk_2021_fix_kxr/euslisp/.#amabe_screw.l create mode 100644 jsk_2021_fix_kxr/euslisp/amabe_loosen_screw.l create mode 100644 jsk_2021_fix_kxr/euslisp/amabe_loosen_screw_pre.l diff --git a/jsk_2021_fix_kxr/euslisp/#amabe_screw.l# b/jsk_2021_fix_kxr/euslisp/#amabe_screw.l# new file mode 100644 index 0000000000..0798f6c747 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/#amabe_screw.l# @@ -0,0 +1,38 @@ +#!/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") + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 1))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.456 1.57 1))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.456 1.57 3))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 3))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 1))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.456 1.57 1))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.456 1.57 3))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 3))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) diff --git a/jsk_2021_fix_kxr/euslisp/.#amabe_screw.l b/jsk_2021_fix_kxr/euslisp/.#amabe_screw.l new file mode 120000 index 0000000000..6c7cac996a --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/.#amabe_screw.l @@ -0,0 +1 @@ +amabe@ubuntu.9127:1623043840 \ No newline at end of file diff --git a/jsk_2021_fix_kxr/euslisp/amabe_loosen_screw.l b/jsk_2021_fix_kxr/euslisp/amabe_loosen_screw.l new file mode 100644 index 0000000000..84d1b4a460 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/amabe_loosen_screw.l @@ -0,0 +1,54 @@ +#!/usr/bin/env roseus + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(setq tem_d (- (send *robot* :rarm_joint7 :joint-angle) 180)) +(send *robot* :rarm_joint7 :joint-angle tem_d)) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 0))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 1500) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(setq tem_d (- (send *robot* :rarm_joint7 :joint-angle) 180)) +(send *robot* :rarm_joint7 :joint-angle tem_d)) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 0))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 1500) +(send *ri* :wait-interpolation) +#| +(send *ri* :start-grasp :rarm) +(unix::sleep 2) +(send *ri* :stop-grasp :rarm) +(send *ri* :start-grasp :rarm) +(unix::sleep 1) +(send *ri* :start-grasp :rarm) +(unix::sleep 1) +(send *ri* :start-grasp :rarm) +(unix::sleep 1) +|# diff --git a/jsk_2021_fix_kxr/euslisp/amabe_loosen_screw_pre.l b/jsk_2021_fix_kxr/euslisp/amabe_loosen_screw_pre.l new file mode 100644 index 0000000000..5343af1d2d --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/amabe_loosen_screw_pre.l @@ -0,0 +1,32 @@ +#!/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 *robot* :reset-pose) +(send *robot* :rarm :move-end-pos #f(-500 0 0)) +(send *robot* :larm :move-end-pos #f(-500 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +#| +(send *ri* :start-grasp :rarm) +(unix::sleep 2) +(send *ri* :stop-grasp :rarm) +(send *ri* :start-grasp :rarm) +(unix::sleep 1) +(send *ri* :start-grasp :rarm) +(unix::sleep 1) +(send *ri* :start-grasp :rarm) +(unix::sleep 1) +|# diff --git a/jsk_2021_fix_kxr/euslisp/amabe_screw_pre.l b/jsk_2021_fix_kxr/euslisp/amabe_screw_pre.l index 4300e2a40d..d9ef534b4f 100644 --- a/jsk_2021_fix_kxr/euslisp/amabe_screw_pre.l +++ b/jsk_2021_fix_kxr/euslisp/amabe_screw_pre.l @@ -20,5 +20,14 @@ (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) -;; (send *ri* :start-grasp :rarm) -;; (send *ri* :start-grasp :larm) +#| +(send *ri* :start-grasp :rarm) +(unix::sleep 2) +(send *ri* :stop-grasp :rarm) +(send *ri* :start-grasp :rarm) +(unix::sleep 1) +(send *ri* :start-grasp :rarm) +(unix::sleep 1) +(send *ri* :start-grasp :rarm) +(unix::sleep 1) +|# From 6af496b8952151ed61c967b6163efb33ea6a1cf1 Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 7 Jun 2021 19:17:58 +0900 Subject: [PATCH 005/127] made amabe_pick_screw.l --- jsk_2021_fix_kxr/euslisp/#amabe_pick_screw.l# | 1172 +++++++++++++++++ jsk_2021_fix_kxr/euslisp/amabe_pick_screw.l | 54 + 2 files changed, 1226 insertions(+) create mode 100644 jsk_2021_fix_kxr/euslisp/#amabe_pick_screw.l# create mode 100644 jsk_2021_fix_kxr/euslisp/amabe_pick_screw.l diff --git a/jsk_2021_fix_kxr/euslisp/#amabe_pick_screw.l# b/jsk_2021_fix_kxr/euslisp/#amabe_pick_screw.l# new file mode 100644 index 0000000000..91b4af2a30 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/#amabe_pick_screw.l# @@ -0,0 +1,1172 @@ +amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ source ~/franka_ws/devel/setup.bash +amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ rossetmaster 133.11.216.227 +set ROS_MASTER_URI to http://133.11.216.227:11311 +[http://133.11.216.227:11311][] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ rossetip +set ROS_IP and ROS_HOSTNAME to 133.11.216.79 +[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus +configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" +;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin +connected to Xserver DISPLAY=:0 +X events are being asynchronously monitored. +;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb +;; extending gcstack 0x564ea20ad690[16374] --> 0x564ea2533600[32748] top=3c74 +irtgl irtglc irtviewer +EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) +roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) +eustf roseus_c_util 1.irteusgl$ roseus +configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" +;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin +connected to Xserver DISPLAY=:0 +X events are being asynchronously monitored. +;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb +;; extending gcstack 0x555e813e1690[16374] --> 0x555e81867600[32748] top=3c74 +irtgl irtglc irtviewer +EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020)roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) +eustf roseus_c_util 1.irteusgl$(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*)) +:|PACKAGE://PANDA_EUS/EUSLISP/DUAL_PANDA-INTERFACE.L| +2.irteusgl$ nil +2.irteusgl$ nil +3.irteusgl$ nil +4.irteusgl$ nil +4.irteusgl$ [ERROR] [1623044935.914571433]: [registerPublisher] Failed to contact master at [133.11.216.227:11311]. Retrying... + C-c C-c[ INFO] [1623044976.404423882]: Connected to master at [133.11.216.227:11311] +interrupt5.B1-irteusgl$ reset +6.irteusgl$ (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*)) +nil +7.irteusgl$ nil +7.irteusgl$ nil +8.irteusgl$ nil +9.irteusgl$ nil +9.irteusgl$ [ WARN] [1623044992.517489268]: Waiting for actionlib interface forever because controller-timeout is nil +[ERROR] [1623044992.792812777]: unknown ros::param::get, key=/gazebo/time_step +[ WARN] [1623044992.792957261]: real franka environemt interface is not impelemented +# +10.irteusgl$ nil +10.irteusgl$ ;; (make-irtviewer) executed +(#) +11.irteusgl$ (send *ri* :start-grasp :rarm) +nil +12.irteusgl$ (send *ri* :start-grasp :rarm) +nil +13.irteusgl$ (send *ri* :start-grasp :rarm) +nil +14.irteusgl$ (send *ri* :stop-grasp :rarm) +nil +15.irteusgl$ (send *ri* :start-grasp :rarm) +nil +16.irteusgl$ (send *ri* :stop-grasp :rarm) +nil +17.irteusgl$ (send *ri* :stop-grasp :rarm) +nil +18.irteusgl$ (send *ri* :start-grasp :rarm) +(send *ri* :stop-grasp :rarm) +nil +19.irteusgl$ nil +20.irteusgl$ (send *ri* :start-grasp :rarm) +(send *ri* :wait-interpolation) +(send *ri* :stop-grasp :rarm) +nil +21.irteusgl$ [ERROR] [1623045257.212841572]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] :wait-for-result (return nil when no goal exists) +(nil) +22.irteusgl$ nil +23.irteusgl$ (send *ri* :start-grasp :rarm) +(send *ri* :wait-interpolation) +(send *ri* :stop-grasp :rarm) +nil +24.irteusgl$ [ERROR] [1623045276.931426625]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] :wait-for-result (return nil when no goal exists) +(nil) +25.irteusgl$ nil +26.irteusgl$ (send *ri* :start-grasp :rarm) +(unix::sleep 2) +(send *ri* :stop-grasp :rarm) +nil +27.irteusgl$ t +28.irteusgl$ nil +29.irteusgl$ (send *ri* :start-grasp :rarm) +(unix::sleep 2) +(send *ri* :stop-grasp :rarm) +nil +30.irteusgl$ t +31.irteusgl$ nil +32.irteusgl$ (send *ri* :start-grasp :rarm) +(unix::sleep 2) +(send *ri* :stop-grasp :rarm) +nil +33.irteusgl$ t +34.irteusgl$ nil +35.irteusgl$ (send *ri* :start-grasp :rarm :effort 100) +(unix::sleep 2) +(send *ri* :stop-grasp :rarm) +nil +36.irteusgl$ t +37.irteusgl$ nil +38.irteusgl$ (send *ri* :start-grasp :rarm :effort 100) +(unix::sleep 2) +(send *ri* :stop-grasp :rarm) +nil +39.irteusgl$ t +40.irteusgl$ nil +41.irteusgl$ (send *ri* :start-grasp :rarm) +(unix::sleep 1) +(send *ri* :start-grasp :rarm) +(unix::sleep 1) +(send *ri* :start-grasp :rarm) +(unix::sleep 1) +nil +42.irteusgl$ t +43.irteusgl$ nil +44.irteusgl$ t +45.irteusgl$ nil +46.irteusgl$ t +47.irteusgl$ (send *ri* :stop-grasp :rarm) +nil +48.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 1))) :rotation-axis t) +#f(10.1933 13.065 23.5346 -96.2821 -5.44666 108.256 -68.9385 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) +49.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 1.57))) :rotation-axis t) +#f(12.7212 12.8301 20.7004 -96.3012 -4.74834 108.314 -36.7336 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) +50.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 0))) :rotation-axis t) +#f(2.83096 13.9768 31.8934 -96.2379 -7.71685 108.065 -124.769 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) +51.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0 1.57 0))) :rotation-axis t) +#f(5.04567 13.6681 29.3727 -96.2492 -7.03154 108.124 -99.0868 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) +52.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0 1.57 1.57))) :rotation-axis t) +#f(12.1963 12.88 21.296 -96.2915 -4.91815 108.294 -10.4985 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) +53.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0 1.57 0))) :rotation-axis t) +#f(3.99637 13.813 30.569 -96.2402 -7.36409 108.091 -98.8717 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) +54.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 1.57 1.57 0))) :rotation-axis t) +#f(-31.8789 45.9122 87.7798 -93.3033 -45.9812 93.9305 -166.003 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) +55.irteusgl$ (send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(-31.8789 45.9122 87.7798 -93.3033 -45.9812 93.9305 -166.003 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) +56.irteusgl$ (nil) +57.irteusgl$ (send *robot* :reset-pose) +(send *robot* :rarm :move-end-pos #f(-500 0 0)) +(send *robot* :larm :move-end-pos #f(-500 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +#f(0.0 15.0 0.0 -135.0 0.0 150.0 45.0 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) +58.irteusgl$ nil +58.irteusgl$ #f(-0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) +59.irteusgl$ nil +59.irteusgl$ #f(-0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +60.irteusgl$ nil +60.irteusgl$ #f(-0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +61.irteusgl$ quit +nil +61.irteusgl$ [ INFO] [1623046201.935916968]: cell* ROSEUS_EXIT(context*, int, cell**) +[ INFO] [1623046201.936018253]: exiting roseus 0 +2 +2.irteusgl$ quit +[ INFO] [1623046246.936411950]: cell* ROSEUS_EXIT(context*, int, cell**) +[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus +configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" +;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin +connected to Xserver DISPLAY=:0 +X events are being asynchronously monitored. +;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb +;; extending gcstack 0x56522c44e690[16374] --> 0x56522c8d4600[32748] top=3c74 +irtgl irtglc irtviewer +EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) +roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) +eustf roseus_c_util 1.irteusgl$ (load "amabe_pick_screw.l") +[ WARN] [1623046306.391917094]: Waiting for actionlib interface forever because controller-timeout is nil +[ERROR] [1623046306.934896202]: unknown ros::param::get, key=/gazebo/time_step +[ WARN] [1623046306.935059904]: real franka environemt interface is not impelemented +;; (make-irtviewer) executed +t +2.irteusgl$(load "amabe_pick_screw.l") +[ WARN] [1623046351.177453286]: topic /robot_interface_marker_array already advertised +[ WARN] [1623046351.391899966]: topic /dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory/goal already advertised +[ WARN] [1623046351.392077732]: topic /dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory/cancel already advertised +[ WARN] [1623046351.923439229]: Waiting for actionlib interface forever because controller-timeout is nil +[ERROR] [1623046352.155330580]: unknown ros::param::get, key=/gazebo/time_step +[ WARN] [1623046352.155570732]: real franka environemt interface is not impelemented +[ WARN] [1623046352.353335420]: topic /dual_panda/error_recovery/goal already advertised +[ WARN] [1623046352.353492482]: topic /dual_panda/error_recovery/cancel already advertised +[ WARN] [1623046352.703279913]: topic /dual_panda/rarm/franka_gripper/grasp/goal already advertised +[ WARN] [1623046352.703393498]: topic /dual_panda/rarm/franka_gripper/grasp/cancel already advertised +[ WARN] [1623046353.049249524]: topic /dual_panda/rarm/franka_gripper/homing/goal already advertised +[ WARN] [1623046353.049391117]: topic /dual_panda/rarm/franka_gripper/homing/cancel already advertised +[ WARN] [1623046353.357959211]: topic /dual_panda/rarm/franka_gripper/move/goal already advertised +[ WARN] [1623046353.358077884]: topic /dual_panda/rarm/franka_gripper/move/cancel already advertised +[ WARN] [1623046353.732909860]: topic /dual_panda/rarm/franka_gripper/stop/goal already advertised +[ WARN] [1623046353.733026452]: topic /dual_panda/rarm/franka_gripper/stop/cancel already advertised +[ WARN] [1623046354.242373960]: topic /dual_panda/larm/franka_gripper/grasp/goal already advertised +[ WARN] [1623046354.242412283]: topic /dual_panda/larm/franka_gripper/grasp/cancel already advertised +[ WARN] [1623046354.668871498]: topic /dual_panda/larm/franka_gripper/homing/goal already advertised +[ WARN] [1623046354.668909799]: topic /dual_panda/larm/franka_gripper/homing/cancel already advertised +[ WARN] [1623046355.011662932]: topic /dual_panda/larm/franka_gripper/move/goal already advertised +[ WARN] [1623046355.011808590]: topic /dual_panda/larm/franka_gripper/move/cancel already advertised +[ WARN] [1623046355.332269291]: topic /dual_panda/larm/franka_gripper/stop/goal already advertised +[ WARN] [1623046355.332393882]: topic /dual_panda/larm/franka_gripper/stop/cancel already advertised +[ WARN] [1623046359.953247266]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +t +3.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) +#f(19.7508 12.3665 12.8659 -96.3179 -2.90629 108.4 -11.8014 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +4.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 1.57 1.57 0))) :rotation-axis t) +#f(-31.9482 45.8785 87.7429 -93.3317 -45.9629 93.967 -166.003 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +5.irteusgl$ (send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(-31.9482 45.8785 87.7429 -93.3317 -45.9629 93.967 -166.003 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +6.irteusgl$ (nil) +7.irteusgl$ (send *ri* :start-grasp :rarm) +nil +8.irteusgl$ (send *robot* :angle-vector) +#f(-31.9482 45.8785 87.7429 -93.3317 -45.9629 93.967 -166.003 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +9.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) +#f(-19.7126 21.2167 59.6165 -95.7379 -19.0069 106.006 -1.50889 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +10.irteusgl$ (send *ri* :stop-grasp :rarm) +nil +11.irteusgl$ (send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(-19.7126 21.2167 59.6165 -95.7379 -19.0069 106.006 -1.50889 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +12.irteusgl$ (nil) +13.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 2.35))) :rotation-axis t) +#f(-13.939 18.2742 51.943 -95.9608 -14.9859 106.944 40.6532 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +14.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) +#f(-7.96828 16.2458 44.5598 -96.0966 -11.9054 107.503 83.9547 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +15.irteusgl$ (send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(-7.96828 16.2458 44.5598 -96.0966 -11.9054 107.503 83.9547 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +16.irteusgl$ (nil) +17.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) +#f(-7.81748 16.6588 44.2003 -97.7981 -12.2703 109.523 84.2145 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +18.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 0))) :rotation-axis t) +#f(-26.1478 27.5223 69.1118 -96.9075 -26.6605 105.74 -86.6893 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +19.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) +#f(-3.65123 15.6057 39.2723 -97.8583 -10.4619 109.786 83.0507 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +20.irteusgl$ (send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(-3.65123 15.6057 39.2723 -97.8583 -10.4619 109.786 83.0507 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +21.irteusgl$ (nil) +22.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 0))) :rotation-axis t) +#f(-22.155 23.4973 62.7751 -97.2663 -21.8138 107.333 -89.6592 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +23.irteusgl$ (send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(-22.155 23.4973 62.7751 -97.2663 -21.8138 107.333 -89.6592 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +24.irteusgl$ (nil) +25.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) +#f(-0.214011 14.906 35.2808 -97.9034 -9.12829 109.964 82.1914 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +26.irteusgl$ (send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(-0.214011 14.906 35.2808 -97.9034 -9.12829 109.964 82.1914 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +27.irteusgl$ (nil) +28.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) +#f(-0.11189 14.8888 35.1624 -97.9101 -9.08588 109.978 82.1654 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +29.irteusgl$ (send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(-0.11189 14.8888 35.1624 -97.9101 -9.08588 109.978 82.1654 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +30.irteusgl$ (nil) +31.irteusgl$ (send *ri* :state :potentio-vector) +#f(-0.161458 15.1127 35.2137 -97.9581 -9.35694 109.766 81.7806 0.037818 -1.79924 -0.021435 -77.9177 -0.19856 76.2007 45.8857) +32.irteusgl$ (setq tem_d (send *ri* :state :potentio-vector)) +#f(-0.157438 15.1093 35.2109 -97.9589 -9.35549 109.769 81.7809 0.037696 -1.79938 -0.021738 -77.9175 -0.198656 76.2005 45.8858) +33.irteusgl$ setq +nil +34.irteusgl$ tem_d +#f(-0.157438 15.1093 35.2109 -97.9589 -9.35549 109.769 81.7809 0.037696 -1.79938 -0.021738 -77.9175 -0.198656 76.2005 45.8858) +35.irteusgl$ (send *robot* :rarm_joint7 :joint-angle) +82.1654 +36.irteusgl$ (send *robot* :rarm_joint7 :joint-angle (- (send *robot* :rarm_joint7 :joint-angle) 90)) +-7.83462 +37.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) +#f(8.65018 13.2283 25.2861 -96.2735 -5.93437 108.22 80.1129 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +38.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) +nil +38.irteusgl$ #f(8.71093 13.6077 25.1301 -97.9845 -6.14099 110.277 80.26 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +39.irteusgl$ (setq tem_d (- (send *ri :rarm-joint7 :joint-angle) 180)) +(send *robot* :rarm-joint7 :joint-angle tem_d)) +Call Stack (max depth: 20): + 0: at (send *ri :rarm-joint7 :joint-angle) + 1: at (- (send *ri :rarm-joint7 :joint-angle) 180) + 2: at (setq tem_d (- (send *ri :rarm-joint7 :joint-angle) 180)) + 3: at # +/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: unbound variable *ri in (send *ri :rarm-joint7 :joint-angle) +40.E1-irteusgl$ Call Stack (max depth: 20): + 0: at (send *robot* :rarm-joint7 :joint-angle tem_d) + 1: at euserror + 2: at euserror + 3: at (send *ri :rarm-joint7 :joint-angle) + 4: at (- (send *ri :rarm-joint7 :joint-angle) 180) + 5: at (setq tem_d (- (send *ri :rarm-joint7 :joint-angle) 180)) + 6: at # +/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :rarm-joint7 in (send *robot* :rarm-joint7 :joint-angle tem_d) +41.E2-irteusgl$ reset +nil +42.E2-irteusgl$ 43.irteusgl$ (setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) +(send *robot* :rarm-joint7 :joint-angle tem_d)) +Call Stack (max depth: 20): + 0: at (send *ri :rarm_joint7 :joint-angle) + 1: at (- (send *ri :rarm_joint7 :joint-angle) 180) + 2: at (setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) + 3: at # +/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: unbound variable *ri in (send *ri :rarm_joint7 :joint-angle) +44.E1-irteusgl$ Call Stack (max depth: 20): + 0: at (send *robot* :rarm-joint7 :joint-angle tem_d) + 1: at euserror + 2: at euserror + 3: at (send *ri :rarm_joint7 :joint-angle) + 4: at (- (send *ri :rarm_joint7 :joint-angle) 180) + 5: at (setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) + 6: at # +/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :rarm-joint7 in (send *robot* :rarm-joint7 :joint-angle tem_d) +45.E2-irteusgl$ reset +nil +46.E2-irteusgl$ 47.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) +#f(8.77822 13.6002 25.0544 -97.9849 -6.12046 110.278 80.2467 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +48.irteusgl$ o(setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) +(send *robot* :rarm_joint7 :joint-angle tem_d)) +Call Stack (max depth: 20): + 0: at (send *ri :rarm_joint7 :joint-angle) + 1: at (- (send *ri :rarm_joint7 :joint-angle) 180) + 2: at (setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) + 3: at # +/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: unbound variable *ri in (send *ri :rarm_joint7 :joint-angle) +49.E1-irteusgl$ Call Stack (max depth: 20): + 0: at (forward-message-to rarm_joint7_jt args) + 1: at (forward-message-to rarm_joint7_jt args) + 2: at (send *robot* :rarm_joint7 :joint-angle tem_d) + 3: at euserror + 4: at euserror + 5: at (send *ri :rarm_joint7 :joint-angle) + 6: at (- (send *ri :rarm_joint7 :joint-angle) 180) + 7: at (setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) + 8: at # +/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: number expected in (forward-message-to rarm_joint7_jt args) +50.E2-irteusgl$ reset +nil +51.E2-irteusgl$ 52.irteusgl$ (setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) +Call Stack (max depth: 20): + 0: at (send *ri :rarm_joint7 :joint-angle) + 1: at (- (send *ri :rarm_joint7 :joint-angle) 180) + 2: at (setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) + 3: at # +/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: unbound variable *ri in (send *ri :rarm_joint7 :joint-angle) +53.E1-irteusgl$ (setq tem_d (- (send *ri* :rarm_joint7 :joint-angle) 180)) +Call Stack (max depth: 20): + 0: at (send *ri* :rarm_joint7 :joint-angle) + 1: at (- (send *ri* :rarm_joint7 :joint-angle) 180) + 2: at (setq tem_d (- (send *ri* :rarm_joint7 :joint-angle) 180)) + 3: at euserror + 4: at euserror + 5: at (send *ri :rarm_joint7 :joint-angle) + 6: at (- (send *ri :rarm_joint7 :joint-angle) 180) + 7: at (setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) + 8: at # +/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :rarm_joint7 in (send *ri* :rarm_joint7 :joint-angle) +54.E2-irteusgl$(send *ri* :rarm_joint7 :joint-angle) +Call Stack (max depth: 20): + 0: at (send *ri* :rarm_joint7 :joint-angle) + 1: at euserror + 2: at euserror + 3: at (send *ri* :rarm_joint7 :joint-angle) + 4: at (- (send *ri* :rarm_joint7 :joint-angle) 180) + 5: at (setq tem_d (- (send *ri* :rarm_joint7 :joint-angle) 180)) + 6: at euserror + 7: at euserror + 8: at (send *ri :rarm_joint7 :joint-angle) + 9: at (- (send *ri :rarm_joint7 :joint-angle) 180) + 10: at (setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) + 11: at # +/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :rarm_joint7 in (send *ri* :rarm_joint7 :joint-angle) +55.E3-irteusgl$ reset +56.irteusgl$ (send *robot* :rarm_joint7 :joint-angle) +80.2467 +57.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) +#f(8.84526 13.5927 24.979 -97.9853 -6.10002 110.28 80.2334 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +58.irteusgl$ (setq tem_d (- (send *robot* :rarm_joint7 :joint-angle) 180)) +(send *robot* :rarm_joint7 :joint-angle tem_d)) +-99.7666 +59.irteusgl$ -99.7666 +60.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 0))) :rotation-axis t) +nil +61.irteusgl$ #f(8.82039 13.2107 25.094 -96.274 -5.88317 108.224 -99.8286 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +62.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 1500) +(send *ri* :wait-interpolation) + +(setq tem_d (- (send *robot* :rarm_joint7 :joint-angle) 180)) +(send *robot* :rarm_joint7 :joint-angle tem_d)) + +(send *ri* :angle-vector (send *robot* :angle-vector) 1500) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 0))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 1500) +(send *ri* :wait-interpolation) +#f(22.5265 12.2457 9.78533 -96.3236 -2.20435 108.423 77.6978 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +63.irteusgl$ nil +63.irteusgl$ #f(22.5265 12.2457 9.78533 -96.3236 -2.20435 108.423 77.6978 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +64.irteusgl$ (nil) +65.irteusgl$ nil +65.irteusgl$ #f(22.5443 12.6063 9.7331 -98.0387 -2.28319 110.493 77.7543 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +66.irteusgl$ nil +66.irteusgl$ #f(22.5443 12.6063 9.7331 -98.0387 -2.28319 110.493 77.7543 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +67.irteusgl$ [ WARN] [1623049013.641453933]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +(nil) +68.irteusgl$ nil +68.irteusgl$ -102.246 +69.irteusgl$ -102.246 +70.irteusgl$ nil +71.irteusgl$ nil +71.irteusgl$ #f(22.5443 12.6063 9.7331 -98.0387 -2.28319 110.493 -102.246 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +72.irteusgl$ [ WARN] [1623049014.566786646]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +(nil) +73.irteusgl$ nil +73.irteusgl$ #f(22.4809 12.2485 9.83568 -96.3238 -2.21481 108.423 -102.204 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +74.irteusgl$ nil +74.irteusgl$ #f(22.4809 12.2485 9.83568 -96.3238 -2.21481 108.423 -102.204 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +75.irteusgl$ [ WARN] [1623049017.979824563]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal + +;; extending gcstack 0x56522c8d4600[32738] --> 0x56524cf02e80[65476] top=7f55 +(nil) +76.irteusgl$ quit +[ INFO] [1623049254.470344764]: cell* ROSEUS_EXIT(context*, int, cell**) +[ INFO] [1623049254.470419628]: exiting roseus 0 +[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus +configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" +;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin +connected to Xserver DISPLAY=:0 +X events are being asynchronously monitored. +;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb +;; extending gcstack 0x55ce469ea690[16374] --> 0x55ce46e70600[32748] top=3c74 +irtgl irtglc irtviewer +EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) +roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) +eustf roseus_c_util 1.irteusgl$ (load "amabe_pick_screw_pre.l") +[ WARN] [1623049314.383712251]: Waiting for actionlib interface forever because controller-timeout is nil +[ERROR] [1623049315.346264777]: unknown ros::param::get, key=/gazebo/time_step +[ WARN] [1623049315.346419993]: real franka environemt interface is not impelemented + C-c C-cinterrupt2.B1-irteusgl$ quit +[ INFO] [1623049327.579663917]: cell* ROSEUS_EXIT(context*, int, cell**) +[ INFO] [1623049327.579803291]: exiting roseus 0 +[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus +configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" +;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin +connected to Xserver DISPLAY=:0 +X events are being asynchronously monitored. +;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb +;; extending gcstack 0x559728ad6690[16374] --> 0x559728f5c600[32748] top=3c74 +irtgl irtglc irtviewer +EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) +roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) +eustf roseus_c_util 1.irteusgl$ (load "amabe_pick_screw_pre.l") +[ WARN] [1623049362.754813921]: Waiting for actionlib interface forever because controller-timeout is nil +[ERROR] [1623049363.340785788]: unknown ros::param::get, key=/gazebo/time_step +[ WARN] [1623049363.340953570]: real franka environemt interface is not impelemented + C-c C-c[ERROR] [1623049376.375959353]: [registerPublisher] Failed to contact master at [133.11.216.227:11311]. Retrying... +[ INFO] [1623049376.789943490]: Connected to master at [133.11.216.227:11311] +interrupt2.B1-irteusgl$ reset +3.irteusgl$ quit +[ INFO] [1623049382.168834368]: cell* ROSEUS_EXIT(context*, int, cell**) +[ INFO] [1623049382.168950431]: exiting roseus 0 +[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus +configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" +;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin +connected to Xserver DISPLAY=:0 +X events are being asynchronously monitored. +;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb +;; extending gcstack 0x55712a25d690[16374] --> 0x55712a6e3600[32748] top=3c74 +irtgl irtglc irtviewer +EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) +roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) +eustf roseus_c_util 1.irteusgl$ (load "amabe_pick_screw_pre.l") +[ WARN] [1623049457.357976285]: Waiting for actionlib interface forever because controller-timeout is nil +[ERROR] [1623049458.045549258]: unknown ros::param::get, key=/gazebo/time_step +[ WARN] [1623049458.045961293]: real franka environemt interface is not impelemented +;; (make-irtviewer) executed +[ WARN] [1623049466.001354460]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623049466.001865036]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623049466.002381821]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +t +2.irteusgl$ (load "amabe_pick_screw.l") +t +3.irteusgl$ (load "amabe_pick_screw.l") +t +4.irteusgl$ (load "amabe_pick_screw.l") +[ WARN] [1623049727.346257474]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623049729.127287714]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623049731.749538494]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +t +5.irteusgl$ (load "amabe_pick_screw.l") +[ WARN] [1623049741.904454795]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623049741.906164142]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623049744.127459156]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +t +6.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(65.085 14.8997 -37.8951 -96.1777 9.52507 107.845 70.1069 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +7.irteusgl$ nil +7.irteusgl$ #f(65.085 14.8997 -37.8951 -96.1777 9.52507 107.845 70.1069 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +8.irteusgl$ (nil) +9.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 1500) +(send *ri* :wait-interpolation) +#f(64.9311 15.2943 -37.574 -97.8869 9.82562 109.884 69.8952 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +10.irteusgl$ nil +10.irteusgl$ #f(64.9311 15.2943 -37.574 -97.8869 9.82562 109.884 69.8952 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +11.irteusgl$ (nil) +12.irteusgl$ (load "amabe_pick_screw.l") +[ WARN] [1623049805.591549586]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623049807.565687182]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623049807.567631132]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +t +13.irteusgl$ (send *ri* :start-grasp :rarm) +nil +14.irteusgl$ (load "amabe_pick_screw.l") +[ WARN] [1623050330.792249109]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623050332.903996845]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623050335.128778219]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +t +15.irteusgl$ (load "amabe_pick_screw.l") +[ WARN] [1623050397.588762836]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623050397.607837882]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623050397.609749579]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623050399.698250098]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +t +16.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(94.2665 53.8612 -93.5533 -92.4783 53.7296 88.6379 45.0703 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +17.irteusgl$ nil +17.irteusgl$ #f(94.2665 53.8612 -93.5533 -92.4783 53.7296 88.6379 45.0703 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +18.irteusgl$ (nil) +19.irteusgl$ (load "amabe_pick_screw.l") +[ WARN] [1623050495.713558001]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623050497.738797790]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +t +20.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(92.247 65.3101 -101.682 -91.4105 64.6229 80.0454 40.9219 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +21.irteusgl$ nil +21.irteusgl$ #f(92.247 65.3101 -101.682 -91.4105 64.6229 80.0454 40.9219 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +22.irteusgl$ (nil) +23.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(92.3814 64.7619 -101.287 -91.4594 64.1021 80.4835 41.093 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +24.irteusgl$ nil +24.irteusgl$ #f(92.3814 64.7619 -101.287 -91.4594 64.1021 80.4835 41.093 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +25.irteusgl$ (nil) +26.irteusgl$ (load "amabe_pick_screw.l") +[ WARN] [1623050592.314653794]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623050594.626695734]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +t +27.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(90.5206 71.3833 -106.17 -90.9361 70.4383 75.0542 39.2271 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +28.irteusgl$ nil +28.irteusgl$ #f(90.5206 71.3833 -106.17 -90.9361 70.4383 75.0542 39.2271 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +29.irteusgl$ (nil) +30.irteusgl$ (load "amabe_pick_screw.l") +t +31.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(89.175 75.2568 -109.166 -90.6793 74.2206 71.7171 38.3414 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +32.irteusgl$ nil +32.irteusgl$ #f(89.175 75.2568 -109.166 -90.6793 74.2206 71.7171 38.3414 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +33.irteusgl$ (nil) +34.irteusgl$ (load "amabe_pick_screw.l") +[ WARN] [1623050854.432390139]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +t +35.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(88.2492 77.6323 -111.071 -90.5427 76.5851 69.6107 37.8788 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +36.irteusgl$ nil +36.irteusgl$ #f(88.2492 77.6323 -111.071 -90.5427 76.5851 69.6107 37.8788 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +37.irteusgl$ +;; extending gcstack 0x55712a6e3600[32738] --> 0x55714b9123c0[65476] top=7f53 +(nil) +38.irteusgl$ (load "amabe_pick_screw.l") +[ WARN] [1623050894.185163357]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623050896.348864105]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +t +39.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(87.6189 79.141 -112.312 -90.4653 78.1096 68.2487 37.6187 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +40.irteusgl$ nil +40.irteusgl$ #f(87.6189 79.141 -112.312 -90.4653 78.1096 68.2487 37.6187 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +41.irteusgl$ (nil) +42.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(87.9218 78.4264 -111.721 -90.5009 77.3849 68.8963 37.7384 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +43.irteusgl$ nil +43.irteusgl$ #f(87.9218 78.4264 -111.721 -90.5009 77.3849 68.8963 37.7384 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +44.irteusgl$ (nil) +45.irteusgl$ (load "amabe_pick_screw.l") +[ WARN] [1623051014.052718012]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623051014.061815984]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623051016.245064549]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +t +46.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(87.3963 79.6553 -112.741 -90.4406 78.6339 67.7801 37.5363 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +47.irteusgl$ nil +47.irteusgl$ #f(87.3963 79.6553 -112.741 -90.4406 78.6339 67.7801 37.5363 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +48.irteusgl$ (nil) +49.irteusgl$ (load "amabe_pick_screw.l") +[ WARN] [1623051088.168469343]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623051090.224919357]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623051092.259653155]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +t +50.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(87.0388 80.4622 -113.421 -90.4038 79.4616 67.0404 37.4135 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +51.irteusgl$ nil +51.irteusgl$ #f(87.0388 80.4622 -113.421 -90.4038 79.4616 67.0404 37.4135 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +52.irteusgl$ (nil) +53.irteusgl$ (load "amabe_pick_screw.l") +[ WARN] [1623051205.313071648]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623051207.315177003]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623051209.324084743]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623051211.364285142]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +t +54.irteusgl$ (load "amabe_pick_screw.l") +[ WARN] [1623051225.303973908]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623051228.894120579]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623051230.907995238]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +[ WARN] [1623051233.002747055]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +t +55.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(85.9036 82.8831 -115.511 -90.3077 81.9867 64.7874 37.095 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +56.irteusgl$ nil +56.irteusgl$ #f(85.9036 82.8831 -115.511 -90.3077 81.9867 64.7874 37.095 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +57.irteusgl$ (nil) +58.irteusgl$ (load "amabe_pick_screw.l") +Call Stack (max depth: 20): + 0: at (apply #'ros::load-org-for-ros ros::fullname args) + 1: at (apply #'ros::load-org-for-ros ros::fullname args) + 2: at (let ((ros::fullname fname)) (when (substringp "package://" fname) (setq ros::fullname (ros::resolve-ros-path fname)) (when ros::*compile-message* (let* ((ros::urlname (url-pathname fname)) (package-name (send ros::urlname :host)) (ros::path-name (format nil "~A_~A" package-name (substitute 95 47 (concatenate string (subseq (send ros::urlname :directory-string) 1) (send ros::urlname :name))))) (ros::old-module (find ros::path-name *loaded-modules* :key #'lisp::load-module-file-name :test #'equal))) (unless ros::old-module (let* ((ros::ppath (unix:getenv "CMAKE_PREFIX_PATH")) (dir (format nil "~A/share/roseus/ros/~A" (subseq ros::ppath 0 (position 58 ros::ppath)) package-name))) (unless (probe-file dir) (unix:mkdir dir)) (compiler:compile-file-if-src-newer ros::fullname (format nil "~A/~A" dir ros::path-name)) (return-from load (load (format nil "~A/~A.so" dir ros::path-name) :entry (format nil "___~A" ros::path-name)))))))) (if (null ros::fullname) (error "file ~s not found" fname)) (apply #'ros::load-org-for-ros ros::fullname args)) + 3: at (load "amabe_pick_screw.l") + 4: at # +/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: file #P"amabe_pick_screw.l" not found in (apply #'ros::load-org-for-ros ros::fullname args) +59.E1-irteusgl$ reset +60.irteusgl$ (load "amabe_loosen_screw.l") +[ WARN] [1623052329.569961631]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal +t +61.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(85.8388 83.0154 -115.627 -90.3031 82.1267 64.6628 37.0799 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +62.irteusgl$ nil +62.irteusgl$ #f(85.8388 83.0154 -115.627 -90.3031 82.1267 64.6628 37.0799 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +63.irteusgl$ (nil) +64.irteusgl$ (load "amabe_loosen_screw.l") +t +65.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(85.8068 83.0805 -115.685 -90.3009 82.1956 64.6015 37.0725 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +66.irteusgl$ nil +66.irteusgl$ #f(85.8068 83.0805 -115.685 -90.3009 82.1956 64.6015 37.0725 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +67.irteusgl$ (nil) +68.irteusgl$ (load "amabe_loosen_screw.l") +t +69.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(85.791 83.1126 -115.713 -90.2998 82.2296 64.5712 37.0689 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +70.irteusgl$ nil +70.irteusgl$ #f(85.791 83.1126 -115.713 -90.2998 82.2296 64.5712 37.0689 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +71.irteusgl$ (nil) +72.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(86.8597 83.7929 -114.315 -92.0083 82.3125 66.1357 36.7928 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +73.irteusgl$ nil +73.irteusgl$ #f(86.8597 83.7929 -114.315 -92.0083 82.3125 66.1357 36.7928 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +74.irteusgl$ (nil) +75.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(86.4176 81.8127 -114.577 -90.3473 80.8618 65.7899 37.2262 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +76.irteusgl$ nil +76.irteusgl$ #f(86.4176 81.8127 -114.577 -90.3473 80.8618 65.7899 37.2262 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +77.irteusgl$ (nil) +78.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(86.7625 81.0711 -113.939 -90.3774 80.0903 66.4786 37.326 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +79.irteusgl$ nil +79.irteusgl$ #f(86.7625 81.0711 -113.939 -90.3774 80.0903 66.4786 37.326 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +80.irteusgl$ (nil) +81.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(87.786 81.7133 -112.574 -92.0845 80.2056 68.0594 37.0473 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +82.irteusgl$ nil +82.irteusgl$ #f(87.786 81.7133 -112.574 -92.0845 80.2056 68.0594 37.0473 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +83.irteusgl$ (nil) +84.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(87.336 79.7934 -112.857 -90.434 78.7748 67.654 37.5145 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +85.irteusgl$ nil +85.irteusgl$ #f(87.336 79.7934 -112.857 -90.434 78.7748 67.654 37.5145 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +86.irteusgl$ (nil) +87.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(88.3311 80.4137 -111.513 -92.14 78.9099 69.2442 37.2337 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +88.irteusgl$ nil +88.irteusgl$ #f(88.3311 80.4137 -111.513 -92.14 78.9099 69.2442 37.2337 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +89.irteusgl$ (nil) +90.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(87.8776 78.5318 -111.808 -90.4955 77.4914 68.8011 37.7204 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +91.irteusgl$ nil +91.irteusgl$ #f(87.8776 78.5318 -111.808 -90.4955 77.4914 68.8011 37.7204 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +92.irteusgl$ (nil) +93.irteusgl$ (load "amabe_loosen_screw.l") +t +94.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(86.7981 80.9928 -113.872 -90.3808 80.0095 66.5509 37.3372 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +95.irteusgl$ nil +95.irteusgl$ #f(86.7981 80.9928 -113.872 -90.3808 80.0095 66.5509 37.3372 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +96.irteusgl$ (nil) +97.irteusgl$ (load "amabe_loosen_screw.l") +t +98.irteusgl$ (send *ri* :stop-grasp) +Call Stack (max depth: 20): + 0: at (send *ri* :stop-grasp) + 1: at # +/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: mismatch argument in (send *ri* :stop-grasp) +99.E1-irteusgl$ (send *ri* :stop-grasp :rarm) +nil +100.E1-irteusgl$ (send *ri* :start-grasp :rarm) +nil +101.E1-irteusgl$ (send *ri* :start-grasp :rarm :effort 100) +nil +102.E1-irteusgl$ quit +[ INFO] [1623055368.674837708]: cell* ROSEUS_EXIT(context*, int, cell**) +[ INFO] [1623055368.675009454]: exiting roseus 0 +[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ quit + +Command 'quit' not found, did you mean: + + command 'qgit' from deb qgit + command 'quiz' from deb bsdgames + command 'luit' from deb x11-utils + command 'quilt' from deb quilt + command 'quot' from deb quota + +Try: sudo apt install + +[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus +configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" +;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin +connected to Xserver DISPLAY=:0 +X events are being asynchronously monitored. +;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb +;; extending gcstack 0x5561c3244690[16374] --> 0x5561c36ca600[32748] top=3c74 +irtgl irtglc irtviewer +EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) +roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) +eustf roseus_c_util 1.irteusgl$ (load "amabe_loosen_screw_pre.l") +[ WARN] [1623056168.225945147]: Waiting for actionlib interface forever because controller-timeout is nil +[ERROR] [1623056168.552274525]: unknown ros::param::get, key=/gazebo/time_step +[ WARN] [1623056168.552430451]: real franka environemt interface is not impelemented +;; (make-irtviewer) executed +t +2.irteusgl$ (send *ri* :stop-grasp :larm) +nil +3.irteusgl$ quit +[ INFO] [1623056216.300169409]: cell* ROSEUS_EXIT(context*, int, cell**) +[ INFO] [1623056216.300295286]: exiting roseus 0 +[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus +configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" +;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin +connected to Xserver DISPLAY=:0 +X events are being asynchronously monitored. +;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb +;; extending gcstack 0x56482e518690[16374] --> 0x56482e99e600[32748] top=3c74 +irtgl irtglc irtviewer +EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) +roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) +eustf roseus_c_util 1.irteusgl$ (load "amabe_loosen_screw_pre.l") +[ WARN] [1623057704.496292996]: Waiting for actionlib interface forever because controller-timeout is nil +[ERROR] [1623057704.906291755]: unknown ros::param::get, key=/gazebo/time_step +[ WARN] [1623057704.906440868]: real franka environemt interface is not impelemented + +;; extending gcstack 0x56482e99e600[32738] --> 0x56484cae8490[65476] top=7fe1 +;; (make-irtviewer) executed +t +2.irteusgl$ (send *ri* :set-all-joint-pd-gain 30.0 0.5) +nil +3.irteusgl$ (send *ri* :set-all-joint-pd-gain 300.0 5.0) +nil +4.irteusgl$ (send *ri* :set-all-joint-pd-gain 1000.0 5.0) +nil +5.irteusgl$ (send *ri* :set-all-joint-pd-gain 2000.0 5.0) +nil +6.irteusgl$ (send *ri* :set-all-joint-pd-gain 5000.0 5.0) +nil +7.irteusgl$ (send *ri* :set-all-joint-pd-gain 1000.0 5.0) +nil +8.irteusgl$ (send *ri* :set-all-joint-pd-gain 30.0 0.5) +nil +9.irteusgl$ quit +[ INFO] [1623058260.223158329]: cell* ROSEUS_EXIT(context*, int, cell**) +[ INFO] [1623058260.223300134]: exiting roseus 0 +[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus +configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" +;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin +connected to Xserver DISPLAY=:0 +X events are being asynchronously monitored. +;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb +;; extending gcstack 0x55d3729ae690[16374] --> 0x55d372e34600[32748] top=3c74 +irtgl irtglc irtviewer +EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) +roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) +eustf roseus_c_util 1.irteusgl$ (load "amabe_loosen_screw_pre.l") +[ WARN] [1623058443.040893370]: Waiting for actionlib interface forever because controller-timeout is nil +[ERROR] [1623058444.921455549]: unknown ros::param::get, key=/gazebo/time_step +[ WARN] [1623058444.921590804]: real franka environemt interface is not impelemented +;; (make-irtviewer) executed +t +2.irteusgl$ (send *ri* :set-all-joint-pd-gain 30.0 0.5) +nil +3.irteusgl$ (send *ri* :set-all-joint-pd-gain 1000.0 5.0) +nil +4.irteusgl$ (send *ri* :set-all-joint-pd-gain 30.0 0.5) +nil +5.irteusgl$ (send *ri* :set-all-joint-pd-gain 1000.0 5.0) +nil +6.irteusgl$ (send *ri* :set-all-joint-pd-gain 30.0 0.5) +nil +7.irteusgl$ (send *ri* :set-all-joint-pd-gain 1000.0 5.0) +nil +8.irteusgl$ (send *ri* :set-all-joint-pd-gain 3000.0 5.0) +nil +9.irteusgl$ (send *ri* :set-all-joint-pd-gain 1000.0 5.0) +nil +10.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 0))) :rotation-axis t) +#f(0.579297 14.348 34.4915 -96.2079 -8.50591 107.968 -98.1348 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +11.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) +#f(8.22998 13.2755 25.7608 -96.2706 -6.05809 108.21 -9.76135 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +12.irteusgl$ (send *robot* :angle-vector) +#f(8.22998 13.2755 25.7608 -96.2706 -6.05809 108.21 -9.76135 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +13.irteusgl$ (send *robot* :rarm-joint7 :angle-vector) +Call Stack (max depth: 20): + 0: at (send *robot* :rarm-joint7 :angle-vector) + 1: at # +/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :rarm-joint7 in (send *robot* :rarm-joint7 :angle-vector) +14.E1-irteusgl$ (send *robot* :rarm-joint7 :joint-angle) +Call Stack (max depth: 20): + 0: at (send *robot* :rarm-joint7 :joint-angle) + 1: at euserror + 2: at euserror + 3: at (send *robot* :rarm-joint7 :angle-vector) + 4: at # +/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :rarm-joint7 in (send *robot* :rarm-joint7 :joint-angle) +15.E2-irteusgl$ (send *robot* :rarm_joint7 :joint-angle) +-9.76135 +16.E2-irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(8.25454 13.273 25.7329 -96.2707 -6.0499 108.211 -9.76642 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +17.E2-irteusgl$ nil +17.E2-irteusgl$ #f(8.25454 13.273 25.7329 -96.2707 -6.0499 108.211 -9.76642 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +18.E2-irteusgl$ (nil) +19.E2-irteusgl$ reset +20.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) +#f(8.2792 13.2702 25.705 -96.2709 -6.04259 108.211 -9.77115 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +21.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(8.3038 13.2675 25.6772 -96.271 -6.03529 108.212 -9.77587 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +22.irteusgl$ nil +22.irteusgl$ #f(8.3038 13.2675 25.6772 -96.271 -6.03529 108.212 -9.77587 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +23.irteusgl$ (nil) +24.irteusgl$ quit +[ INFO] [1623059138.268176327]: cell* ROSEUS_EXIT(context*, int, cell**) +[ INFO] [1623059138.268330318]: exiting roseus 0 +[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus +configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" +;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin +connected to Xserver DISPLAY=:0 +X events are being asynchronously monitored. +;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb +;; extending gcstack 0x5622e523b690[16374] --> 0x5622e56c1600[32748] top=3c74 +irtgl irtglc irtviewer +EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) +roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) +eustf roseus_c_util 1.irteusgl$ (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*)) +:|PACKAGE://PANDA_EUS/EUSLISP/DUAL_PANDA-INTERFACE.L| +2.irteusgl$ nil +2.irteusgl$ nil +3.irteusgl$ nil +4.irteusgl$ nil +4.irteusgl$ [ WARN] [1623059192.830940422]: Waiting for actionlib interface forever because controller-timeout is nil +[ERROR] [1623059193.062943866]: unknown ros::param::get, key=/gazebo/time_step +[ WARN] [1623059193.063075076]: real franka environemt interface is not impelemented + +;; extending gcstack 0x5622e56c1600[32738] --> 0x5623036fe1a0[65476] top=7fe1 +# +5.irteusgl$ nil +5.irteusgl$ ;; (make-irtviewer) executed +(#) +6.irteusgl$ quit +[ INFO] [1623059204.645088661]: cell* ROSEUS_EXIT(context*, int, cell**) +[ INFO] [1623059204.645228720]: exiting roseus 0 +[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus +configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" +;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin +connected to Xserver DISPLAY=:0 +X events are being asynchronously monitored. +;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb +;; extending gcstack 0x555bccffd690[16374] --> 0x555bcd483600[32748] top=3c74 +irtgl irtglc irtviewer +EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) +roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) +eustf roseus_c_util 1.irteusgl$ (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*)) +:|PACKAGE://PANDA_EUS/EUSLISP/DUAL_PANDA-INTERFACE.L| +2.irteusgl$ nil +2.irteusgl$ nil +3.irteusgl$ nil +4.irteusgl$ nil +4.irteusgl$ [ WARN] [1623059297.149020601]: Waiting for actionlib interface forever because controller-timeout is nil +[ERROR] [1623059297.474965333]: unknown ros::param::get, key=/gazebo/time_step +[ WARN] [1623059297.475072470]: real franka environemt interface is not impelemented + +;; extending gcstack 0x555bcd483600[32738] --> 0x555beb5cfdb0[65476] top=7fe1 +# +5.irteusgl$ nil +5.irteusgl$ ;; (make-irtviewer) executed +(#) +6.irteusgl$ (send *robot* :reset-pose) +(send *robot* :rarm :move-end-pos #f(-500 0 0)) +(send *robot* :larm :move-end-pos #f(-500 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) +#f(0.0 15.0 0.0 -135.0 0.0 150.0 45.0 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) +7.irteusgl$ nil +7.irteusgl$ #f(-0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) +8.irteusgl$ nil +8.irteusgl$ #f(-0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +9.irteusgl$ nil +9.irteusgl$ #f(-0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +10.irteusgl$ nil +10.irteusgl$ (nil) +11.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) +#f(18.0066 12.4413 14.7926 -96.366 -3.36376 108.418 -11.4914 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +12.irteusgl$ nil +12.irteusgl$ #f(18.0066 12.4413 14.7926 -96.366 -3.36376 108.418 -11.4914 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +13.irteusgl$ (nil) +14.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1096) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(18.1553 15.8123 14.4388 -104.518 -4.51919 119.817 -10.7282 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +15.irteusgl$ nil +15.irteusgl$ #f(18.1553 15.8123 14.4388 -104.518 -4.51919 119.817 -10.7282 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +16.irteusgl$ (nil) +17.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1098) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(18.1445 15.7011 14.4544 -104.369 -4.48154 119.56 -10.7528 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +18.irteusgl$ nil +18.irteusgl$ #f(18.1445 15.7011 14.4544 -104.369 -4.48154 119.56 -10.7528 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +19.irteusgl$ (nil) +20.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1102) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(18.1264 15.4829 14.4816 -104.064 -4.40685 119.045 -10.8019 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +21.irteusgl$ nil +21.irteusgl$ #f(18.1264 15.4829 14.4816 -104.064 -4.40685 119.045 -10.8019 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +22.irteusgl$ (nil) +23.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1101) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(18.1273 15.5371 14.4788 -104.141 -4.42656 119.173 -10.7889 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +24.irteusgl$ nil +24.irteusgl$ #f(18.1273 15.5371 14.4788 -104.141 -4.42656 119.173 -10.7889 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +25.irteusgl$ (nil) +26.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1100) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +#f(18.1282 15.5917 14.4759 -104.218 -4.44641 119.302 -10.7759 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +27.irteusgl$ nil +27.irteusgl$ #f(18.1282 15.5917 14.4759 -104.218 -4.44641 119.302 -10.7759 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +28.irteusgl$ (nil) +29.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) +#f(17.9424 12.4636 14.8771 -96.313 -3.37302 108.38 -11.4989 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +30.irteusgl$ nil +30.irteusgl$ #f(17.9424 12.4636 14.8771 -96.313 -3.37302 108.38 -11.4989 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +31.irteusgl$ (nil) +32.irteusgl$ (send *ri* :start-grasp :rarm) +(unix::sleep 2) +nil +33.irteusgl$ t +34.irteusgl$ (send *ri* :stop-grasp :rarm) +(unix::sleep 2) +nil +35.irteusgl$ t +36.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -164.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) +#f(8.77648 0.789839 9.4671 -111.046 -0.155226 111.868 -26.745 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) +37.irteusgl$ (load "amabe_pick_screw.l") +[ WARN] [1623060060.016109360]: topic /robot_interface_marker_array already advertised +[ WARN] [1623060060.034890038]: topic /dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory/goal already advertised +[ WARN] [1623060060.034926556]: topic /dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory/cancel already advertised +[ WARN] [1623060060.061263362]: Waiting for actionlib interface forever because controller-timeout is nil +[ERROR] [1623060060.080068688]: unknown ros::param::get, key=/gazebo/time_step +[ WARN] [1623060060.080126959]: real franka environemt interface is not impelemented +[ WARN] [1623060060.097829471]: topic /dual_panda/error_recovery/goal already advertised +[ WARN] [1623060060.097869279]: topic /dual_panda/error_recovery/cancel already advertised +[ WARN] [1623060060.124736796]: topic /dual_panda/rarm/franka_gripper/grasp/goal already advertised +[ WARN] [1623060060.124769582]: topic /dual_panda/rarm/franka_gripper/grasp/cancel already advertised +[ WARN] [1623060060.150759748]: topic /dual_panda/rarm/franka_gripper/homing/goal already advertised +[ WARN] [1623060060.150789586]: topic /dual_panda/rarm/franka_gripper/homing/cancel already advertised +[ WARN] [1623060060.178508717]: topic /dual_panda/rarm/franka_gripper/move/goal already advertised +[ WARN] [1623060060.178543869]: topic /dual_panda/rarm/franka_gripper/move/cancel already advertised +[ WARN] [1623060060.207030028]: topic /dual_panda/rarm/franka_gripper/stop/goal already advertised +[ WARN] [1623060060.207081049]: topic /dual_panda/rarm/franka_gripper/stop/cancel already advertised +[ WARN] [1623060060.237351499]: topic /dual_panda/larm/franka_gripper/grasp/goal already advertised +[ WARN] [1623060060.237460925]: topic /dual_panda/larm/franka_gripper/grasp/cancel already advertised +[ WARN] [1623060060.266930707]: topic /dual_panda/larm/franka_gripper/homing/goal already advertised +[ WARN] [1623060060.267055717]: topic /dual_panda/larm/franka_gripper/homing/cancel already advertised +[ WARN] [1623060060.294989929]: topic /dual_panda/larm/franka_gripper/move/goal already advertised +[ WARN] [1623060060.295035699]: topic /dual_panda/larm/franka_gripper/move/cancel already advertised +[ WARN] [1623060060.321165821]: topic /dual_panda/larm/franka_gripper/stop/goal already advertised +[ WARN] [1623060060.321225822]: topic /dual_panda/larm/franka_gripper/stop/cancel already advertised + C-c C-cinterrupt38.B1-irteusgl$quit +[ INFO] [1623060095.028685921]: cell* ROSEUS_EXIT(context*, int, cell**) +[ INFO] [1623060095.028809209]: exiting roseus 0 +[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus +configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" +;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin +connected to Xserver DISPLAY=:0 +X events are being asynchronously monitored. +;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb +;; extending gcstack 0x55d9b5699690[16374] --> 0x55d9b5b1f600[32748] top=3c74 +irtgl irtglc irtviewer +EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) +roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) +eustf roseus_c_util 1.irteusgl$ (load "amabe_pick_screw.l") +[ WARN] [1623060261.807193078]: Waiting for actionlib interface forever because controller-timeout is nil +[ERROR] [1623060262.136650065]: unknown ros::param::get, key=/gazebo/time_step +[ WARN] [1623060262.136778304]: real franka environemt interface is not impelemented + +;; extending gcstack 0x55d9b5b1f600[32738] --> 0x55d9d3c6bb60[65476] top=7fe1 +;; (make-irtviewer) executed +Call Stack (max depth: 20): + 0: at (apply #'ros::load-org-for-ros ros::fullname args) + 1: at (apply #'ros::load-org-for-ros ros::fullname args) + 2: at (let ((ros::fullname fname)) (when (substringp "package://" fname) (setq ros::fullname (ros::resolve-ros-path fname)) (when ros::*compile-message* (let* ((ros::urlname (url-pathname fname)) (package-name (send ros::urlname :host)) (ros::path-name (format nil "~A_~A" package-name (substitute 95 47 (concatenate string (subseq (send ros::urlname :directory-string) 1) (send ros::urlname :name))))) (ros::old-module (find ros::path-name *loaded-modules* :key #'lisp::load-module-file-name :test #'equal))) (unless ros::old-module (let* ((ros::ppath (unix:getenv "CMAKE_PREFIX_PATH")) (dir (format nil "~A/share/roseus/ros/~A" (subseq ros::ppath 0 (position 58 ros::ppath)) package-name))) (unless (probe-file dir) (unix:mkdir dir)) (compiler:compile-file-if-src-newer ros::fullname (format nil "~A/~A" dir ros::path-name)) (return-from load (load (format nil "~A/~A.so" dir ros::path-name) :entry (format nil "___~A" ros::path-name)))))))) (if (null ros::fullname) (error "file ~s not found" fname)) (apply #'ros::load-org-for-ros ros::fullname args)) + 3: at (load "amabe_pick_screw.l") + 4: at # +/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: unbound variable send in (apply #'ros::load-org-for-ros ros::fullname args) +2.E1-irteusgl$ reset +3.irteusgl$ (load "amabe_pick_screw.l") +[ WARN] [1623060316.650644526]: topic /robot_interface_marker_array already advertised +[ WARN] [1623060316.669911352]: topic /dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory/goal already advertised +[ WARN] [1623060316.669948573]: topic /dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory/cancel already advertised +[ WARN] [1623060316.698484340]: Waiting for actionlib interface forever because controller-timeout is nil +[ERROR] [1623060316.714652806]: unknown ros::param::get, key=/gazebo/time_step +[ WARN] [1623060316.714736783]: real franka environemt interface is not impelemented +[ WARN] [1623060316.734699282]: topic /dual_panda/error_recovery/goal already advertised +[ WARN] [1623060316.734811112]: topic /dual_panda/error_recovery/cancel already advertised +[ WARN] [1623060316.763558255]: topic /dual_panda/rarm/franka_gripper/grasp/goal already advertised +[ WARN] [1623060316.763656993]: topic /dual_panda/rarm/franka_gripper/grasp/cancel already advertised +[ WARN] [1623060316.793553681]: topic /dual_panda/rarm/franka_gripper/homing/goal already advertised +[ WARN] [1623060316.793678039]: topic /dual_panda/rarm/franka_gripper/homing/cancel already advertised +[ WARN] [1623060316.824876680]: topic /dual_panda/rarm/franka_gripper/move/goal already advertised +[ WARN] [1623060316.825006436]: topic /dual_panda/rarm/franka_gripper/move/cancel already advertised +[ WARN] [1623060316.853737484]: topic /dual_panda/rarm/franka_gripper/stop/goal already advertised +[ WARN] [1623060316.853877576]: topic /dual_panda/rarm/franka_gripper/stop/cancel already advertised +[ WARN] [1623060316.884739828]: topic /dual_panda/larm/franka_gripper/grasp/goal already advertised +[ WARN] [1623060316.884870854]: topic /dual_panda/larm/franka_gripper/grasp/cancel already advertised +[ WARN] [1623060316.917870233]: topic /dual_panda/larm/franka_gripper/homing/goal already advertised +[ WARN] [1623060316.918000352]: topic /dual_panda/larm/franka_gripper/homing/cancel already advertised +[ WARN] [1623060316.949212265]: topic /dual_panda/larm/franka_gripper/move/goal already advertised +[ WARN] [1623060316.949351283]: topic /dual_panda/larm/franka_gripper/move/cancel already advertised +[ WARN] [1623060316.980023480]: topic /dual_panda/larm/franka_gripper/stop/goal already advertised +[ WARN] [1623060316.980153361]: topic /dual_panda/larm/franka_gripper/stop/cancel already advertised +t +4.irteusgl$ quit +[ INFO] [1623060519.040638339]: cell* ROSEUS_EXIT(context*, int, cell**) +[ INFO] [1623060519.040685918]: exiting roseus 0 +[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ +[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ \ No newline at end of file diff --git a/jsk_2021_fix_kxr/euslisp/amabe_pick_screw.l b/jsk_2021_fix_kxr/euslisp/amabe_pick_screw.l new file mode 100644 index 0000000000..21ecdf747a --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/amabe_pick_screw.l @@ -0,0 +1,54 @@ +# +!/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 *robot* :reset-pose) +(send *robot* :rarm :move-end-pos #f(-500 0 0)) +(send *robot* :larm :move-end-pos #f(-500 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1096) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :rarm) +(unix::sleep 2) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -164.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *ri* :stop-grasp :rarm) + + +#| +(send *ri* :start-grasp :rarm) +(unix::sleep 2) +(send *ri* :stop-grasp :rarm) +(send *ri* :start-grasp :rarm) +(unix::sleep 1) +(send *ri* :start-grasp :rarm) +(unix::sleep 1) +(send *ri* :start-grasp :rarm) +(unix::sleep 1) +|# From ef382724a0ce92d54b0417866e8559b2f398f238 Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 8 Jun 2021 18:22:20 +0900 Subject: [PATCH 006/127] made amabe_plug.l --- jsk_2021_fix_kxr/euslisp/.#amabe_screw.l | 1 - jsk_2021_fix_kxr/euslisp/amabe_plug.l | 40 ++++++++++++++++++++++++ 2 files changed, 40 insertions(+), 1 deletion(-) delete mode 120000 jsk_2021_fix_kxr/euslisp/.#amabe_screw.l create mode 100644 jsk_2021_fix_kxr/euslisp/amabe_plug.l diff --git a/jsk_2021_fix_kxr/euslisp/.#amabe_screw.l b/jsk_2021_fix_kxr/euslisp/.#amabe_screw.l deleted file mode 120000 index 6c7cac996a..0000000000 --- a/jsk_2021_fix_kxr/euslisp/.#amabe_screw.l +++ /dev/null @@ -1 +0,0 @@ -amabe@ubuntu.9127:1623043840 \ No newline at end of file diff --git a/jsk_2021_fix_kxr/euslisp/amabe_plug.l b/jsk_2021_fix_kxr/euslisp/amabe_plug.l new file mode 100644 index 0000000000..5a070917c3 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/amabe_plug.l @@ -0,0 +1,40 @@ +#!/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 *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(431 -219.735 1100) :rpy (float-vector 0.0 1.57 0.0))) :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(413 340 1421) :rpy (float-vector -1.57 1.57 1.57))) :rotation-axis t) + +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :rarm) +(unix::sleep 2) + +(send *robot* :rarm :move-end-pos #f(21 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *ri* :stop-grasp :rarm) +(send *robot* :rarm :move-end-pos #f(-40 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +#| +(send *ri* :start-grasp :rarm) +(unix::sleep 2) +(send *ri* :stop-grasp :rarm) +(send *ri* :start-grasp :rarm) +(unix::sleep 1) +(send *ri* :start-grasp :rarm) +(unix::sleep 1) +(send *ri* :start-grasp :rarm) +(unix::sleep 1) +|# From eedc2884c849b83f437b7c149646c6e4a134b922 Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 8 Jun 2021 20:02:50 +0900 Subject: [PATCH 007/127] made half of amabe_set_fix.l --- jsk_2021_fix_kxr/euslisp/amabe_set_kxr.l | 42 ++++++++++++++++++++++++ 1 file changed, 42 insertions(+) create mode 100644 jsk_2021_fix_kxr/euslisp/amabe_set_kxr.l diff --git a/jsk_2021_fix_kxr/euslisp/amabe_set_kxr.l b/jsk_2021_fix_kxr/euslisp/amabe_set_kxr.l new file mode 100644 index 0000000000..2da8362d31 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/amabe_set_kxr.l @@ -0,0 +1,42 @@ +#!/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 *robot* :reset-pose) +(send *robot* :rarm :move-end-pos #f(-350 0 0)) +(send *robot* :larm :move-end-pos #f(-500 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(510 -310 1170.364) :rpy (float-vector 1.57 0.0 -3.14))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(510 -310 1070.364) :rpy (float-vector 1.57 0.0 -3.14))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(510 -260 1070.364) :rpy (float-vector 1.57 0.0 -3.14))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :rarm) +(unix::sleep 2) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(510 -260 1070.364) :rpy (float-vector 1.57 0.0 -1.57))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *ri* :stop-grasp :rarm) +(unix::sleep 2) + +(send *robot* :rarm :move-end-pos #f(-50 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) From ca019022b9c979d2dcfcc97a5293f8f990596b59 Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 11 Jun 2021 13:02:07 +0900 Subject: [PATCH 008/127] made amabe_set_kxr.l --- jsk_2021_fix_kxr/euslisp/amabe_set_kxr.l | 34 +++++++++++++++++++++++- 1 file changed, 33 insertions(+), 1 deletion(-) diff --git a/jsk_2021_fix_kxr/euslisp/amabe_set_kxr.l b/jsk_2021_fix_kxr/euslisp/amabe_set_kxr.l index 2da8362d31..bbee2645ba 100644 --- a/jsk_2021_fix_kxr/euslisp/amabe_set_kxr.l +++ b/jsk_2021_fix_kxr/euslisp/amabe_set_kxr.l @@ -10,11 +10,43 @@ (objects (list *robot*)) (send *robot* :reset-pose) -(send *robot* :rarm :move-end-pos #f(-350 0 0)) +(send *robot* :rarm :move-end-pos #f(-500 0 0)) (send *robot* :larm :move-end-pos #f(-500 0 0)) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 -88 1200) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 177 1200) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 -88 1093.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 177 1093.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :rarm) +(send *ri* :start-grasp :larm) +(unix::sleep 2) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 -138 1093.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 127 1093.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :stop-grasp :rarm) +(send *ri* :stop-grasp :larm) +(unix::sleep 2) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 -138 1300.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 127 1300.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 -310 1300.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(510 -310 1170.364) :rpy (float-vector 1.57 0.0 -3.14))) :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) From 227aa3a2b227cfd7a64a48920ce5b76fe744b405 Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 14 Jun 2021 19:35:41 +0900 Subject: [PATCH 009/127] add grasp_demo.l --- jsk_2021_fix_kxr/euslisp/grasp_demo.l | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 jsk_2021_fix_kxr/euslisp/grasp_demo.l diff --git a/jsk_2021_fix_kxr/euslisp/grasp_demo.l b/jsk_2021_fix_kxr/euslisp/grasp_demo.l new file mode 100644 index 0000000000..8ffef60924 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/grasp_demo.l @@ -0,0 +1,14 @@ +(send *ri* :start-grasp :rarm) +(unix::sleep 1) + +(send *ri* :stop-grasp :rarm) +(unix::sleep 1) + +(send *ri* :start-grasp :rarm) +(unix::sleep 1) + +(send *ri* :stop-grasp :rarm :width 0.04) +(unix::sleep 1) + +(send *ri* :stop-grasp :rarm) +(unix::sleep 1) From 6560926b2069a33c04dd7eb92404a0cf6a55c49f Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 18 Jun 2021 13:52:56 +0900 Subject: [PATCH 010/127] making amabe_pub_posestamped.l --- jsk_2021_fix_kxr/euslisp/a.out | 0 jsk_2021_fix_kxr/euslisp/amabe_pick_screw.l | 3 +- .../euslisp/amabe_pub_pose_stamped.l | 50 +++++++++++++++++++ 3 files changed, 51 insertions(+), 2 deletions(-) create mode 100644 jsk_2021_fix_kxr/euslisp/a.out create mode 100644 jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l diff --git a/jsk_2021_fix_kxr/euslisp/a.out b/jsk_2021_fix_kxr/euslisp/a.out new file mode 100644 index 0000000000..e69de29bb2 diff --git a/jsk_2021_fix_kxr/euslisp/amabe_pick_screw.l b/jsk_2021_fix_kxr/euslisp/amabe_pick_screw.l index 21ecdf747a..d18807fd89 100644 --- a/jsk_2021_fix_kxr/euslisp/amabe_pick_screw.l +++ b/jsk_2021_fix_kxr/euslisp/amabe_pick_screw.l @@ -1,5 +1,4 @@ -# -!/usr/bin/env roseus +#!/usr/bin/env roseus (require "package://panda_eus/euslisp/dual_panda-interface.l") diff --git a/jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l b/jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l new file mode 100644 index 0000000000..62123d6035 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l @@ -0,0 +1,50 @@ +#!/usr/bin/env roseus + +(require "package://panda_eus/euslisp/dual_panda-interface.l") + +(ros::roseus-add-msgs "std_msgs") +(ros::roseus-add-msgs "sensor_msgs") +(ros::roseus-add-msgs "geometry_msgs") + +(ros::roseus "pose-stamped-publisher") + +(dual_panda-init) + +(defvar *joints* nil) +#| +(setq buf #f(0.0 15.0 0.0 -135.0 0.0 150.0 45.0 7.0 8.0 0.0 15.0 0.0 -135.0 0.0 150.0 45.0 14.0 15.0)) +(setq buf (coerce buf cons)) +(setq l (- (length buf) 1)) +(setq i l) +(setq ans nil) +(while (<= 0 i) + (if (and (not (= i 7)) (not (= i 8)) (not (= i 16)) (not (= i 17))) + (if (not ans) + (setq ans (cons (elt buf i) nil)) + (setq ans (cons (elt buf i) ans)))) + (setq i (- i 1))) +|# + +(defun joints-cb (msg) + (setq *joints* (send msg :position)) + (coerce *joints* cons) + ) + +(ros::rate 10) +(ros::subscribe "/dual_panda/joint_states" sensor_msgs::JointState #'joints-cb 1) + +(while (ros::ok) + (let ((pose-stamped-rarm (instance geometry_msgs::PoseStamped :init))) + ;;(send joint-states (coerce (mapcar #'(lambda (x) (rad2deg x)) (coerce *joints* cons)) float-vector)) + (setq joint-states (coerce (mapcar #'(lambda (x) (rad2deg x)) (coerce *joints* cons)) float-vector)) + (send *robot* :angle-vector joint-states) + (setq pose-stamped-rarm (ros::coords->tf-pose-stamped (send *robot* :rarm :end-coords) base)) + (ros::publish "/pose_stamped_rarm" pose-stamped-rarm) + (ros::spin-once) + (ros::sleep))) +#| +(ros::load-ros-manifest "sensor_msgs") +(setq tem (send (one-shot-subscribe "/dual_panda/joint_states" sensor_msgs::JointState) :position)) +(mapcar #'(lambda (x) (rad2deg x)) (list pi pi/2 pi)) +(coerce #f(1 2 3) cons) +|# From 3fd9acec96f0c3dec2a137038af32096ce9435f0 Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 18 Jun 2021 14:34:25 +0900 Subject: [PATCH 011/127] success to publish /pose_stamped_rarm --- .../euslisp/amabe_pub_pose_stamped.l | 53 ++++++++++++------- 1 file changed, 33 insertions(+), 20 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l b/jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l index 62123d6035..5dc46496f3 100644 --- a/jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l +++ b/jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l @@ -8,37 +8,35 @@ (ros::roseus "pose-stamped-publisher") -(dual_panda-init) +(setq *robot* (dual_panda)) + +(ros::advertise "/pose_stamped_rarm" geometry_msgs::PoseStamped 1) (defvar *joints* nil) -#| -(setq buf #f(0.0 15.0 0.0 -135.0 0.0 150.0 45.0 7.0 8.0 0.0 15.0 0.0 -135.0 0.0 150.0 45.0 14.0 15.0)) -(setq buf (coerce buf cons)) -(setq l (- (length buf) 1)) -(setq i l) -(setq ans nil) -(while (<= 0 i) - (if (and (not (= i 7)) (not (= i 8)) (not (= i 16)) (not (= i 17))) - (if (not ans) - (setq ans (cons (elt buf i) nil)) - (setq ans (cons (elt buf i) ans)))) - (setq i (- i 1))) -|# (defun joints-cb (msg) - (setq *joints* (send msg :position)) - (coerce *joints* cons) - ) + (setq buf (send msg :position)) + (setq buf (coerce buf cons)) + (setq l (- (length buf) 1)) + (setq i l) + (setq ans nil) + (while (<= 0 i) + (if (and (not (= i 7)) (not (= i 8)) (not (= i 16)) (not (= i 17))) + (if (not ans) + (setq ans (cons (elt buf i) nil)) + (setq ans (cons (elt buf i) ans)))) + (setq i (- i 1))) + (setq *joints* (coerce ans float-vector))) (ros::rate 10) (ros::subscribe "/dual_panda/joint_states" sensor_msgs::JointState #'joints-cb 1) (while (ros::ok) (let ((pose-stamped-rarm (instance geometry_msgs::PoseStamped :init))) - ;;(send joint-states (coerce (mapcar #'(lambda (x) (rad2deg x)) (coerce *joints* cons)) float-vector)) (setq joint-states (coerce (mapcar #'(lambda (x) (rad2deg x)) (coerce *joints* cons)) float-vector)) - (send *robot* :angle-vector joint-states) - (setq pose-stamped-rarm (ros::coords->tf-pose-stamped (send *robot* :rarm :end-coords) base)) + (if (= (length joint-states) 14) + (send *robot* :angle-vector joint-states)) + (setq pose-stamped-rarm (ros::coords->tf-pose-stamped (send *robot* :rarm :end-coords) "dual_arm_base")) (ros::publish "/pose_stamped_rarm" pose-stamped-rarm) (ros::spin-once) (ros::sleep))) @@ -48,3 +46,18 @@ (mapcar #'(lambda (x) (rad2deg x)) (list pi pi/2 pi)) (coerce #f(1 2 3) cons) |# + +#| +(setq buf #f(0.0 15.0 0.0 -135.0 0.0 150.0 45.0 7.0 8.0 0.0 15.0 0.0 -135.0 0.0 150.0 45.0 14.0 15.0)) +(setq buf (coerce buf cons)) +(setq l (- (length buf) 1)) +(setq i l) +(setq ans nil) +(while (<= 0 i) + (if (and (not (= i 7)) (not (= i 8)) (not (= i 16)) (not (= i 17))) + (if (not ans) + (setq ans (cons (elt buf i) nil)) + (setq ans (cons (elt buf i) ans)))) + (setq i (- i 1))) +(coerce ans float-vector) +|# From 56471a4727d498f4c809a50f124f7e36f92158df Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 18 Jun 2021 14:45:43 +0900 Subject: [PATCH 012/127] amabe_pub_pose_stamped.l can send posestamped of both hands --- jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l b/jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l index 5dc46496f3..a3fc35c64c 100644 --- a/jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l +++ b/jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l @@ -10,7 +10,8 @@ (setq *robot* (dual_panda)) -(ros::advertise "/pose_stamped_rarm" geometry_msgs::PoseStamped 1) +(ros::advertise "/pose_stamped/rarm" geometry_msgs::PoseStamped 1) +(ros::advertise "/pose_stamped/larm" geometry_msgs::PoseStamped 1) (defvar *joints* nil) @@ -32,12 +33,14 @@ (ros::subscribe "/dual_panda/joint_states" sensor_msgs::JointState #'joints-cb 1) (while (ros::ok) - (let ((pose-stamped-rarm (instance geometry_msgs::PoseStamped :init))) + (let ((pose-stamped-rarm (instance geometry_msgs::PoseStamped :init) (pose-stamped-larm (instance geometry_msgs::PoseStamped :init)))) (setq joint-states (coerce (mapcar #'(lambda (x) (rad2deg x)) (coerce *joints* cons)) float-vector)) (if (= (length joint-states) 14) (send *robot* :angle-vector joint-states)) (setq pose-stamped-rarm (ros::coords->tf-pose-stamped (send *robot* :rarm :end-coords) "dual_arm_base")) - (ros::publish "/pose_stamped_rarm" pose-stamped-rarm) + (setq pose-stamped-larm (ros::coords->tf-pose-stamped (send *robot* :larm :end-coords) "dual_arm_base")) + (ros::publish "/pose_stamped/rarm" pose-stamped-rarm) + (ros::publish "/pose_stamped/larm" pose-stamped-larm) (ros::spin-once) (ros::sleep))) #| From 126ef90c5c96a7d424868c52cde2853f0b15287a Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 18 Jun 2021 15:36:28 +0900 Subject: [PATCH 013/127] changed topic name which amabe_pub_pose_stamped.l publishes --- .../euslisp/amabe_pub_pose_stamped.l | 30 ++++--------------- 1 file changed, 5 insertions(+), 25 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l b/jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l index a3fc35c64c..6776dd7123 100644 --- a/jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l +++ b/jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l @@ -10,8 +10,8 @@ (setq *robot* (dual_panda)) -(ros::advertise "/pose_stamped/rarm" geometry_msgs::PoseStamped 1) -(ros::advertise "/pose_stamped/larm" geometry_msgs::PoseStamped 1) +(ros::advertise "/dual_panda/end_coords/rarm" geometry_msgs::PoseStamped 1) +(ros::advertise "/dual_panda/end_coords/larm" geometry_msgs::PoseStamped 1) (defvar *joints* nil) @@ -21,6 +21,7 @@ (setq l (- (length buf) 1)) (setq i l) (setq ans nil) + (print "publishing end-coords...") (while (<= 0 i) (if (and (not (= i 7)) (not (= i 8)) (not (= i 16)) (not (= i 17))) (if (not ans) @@ -39,28 +40,7 @@ (send *robot* :angle-vector joint-states)) (setq pose-stamped-rarm (ros::coords->tf-pose-stamped (send *robot* :rarm :end-coords) "dual_arm_base")) (setq pose-stamped-larm (ros::coords->tf-pose-stamped (send *robot* :larm :end-coords) "dual_arm_base")) - (ros::publish "/pose_stamped/rarm" pose-stamped-rarm) - (ros::publish "/pose_stamped/larm" pose-stamped-larm) + (ros::publish "/dual_panda/end_coords/rarm" pose-stamped-rarm) + (ros::publish "/dual_panda/end_coords/larm" pose-stamped-larm) (ros::spin-once) (ros::sleep))) -#| -(ros::load-ros-manifest "sensor_msgs") -(setq tem (send (one-shot-subscribe "/dual_panda/joint_states" sensor_msgs::JointState) :position)) -(mapcar #'(lambda (x) (rad2deg x)) (list pi pi/2 pi)) -(coerce #f(1 2 3) cons) -|# - -#| -(setq buf #f(0.0 15.0 0.0 -135.0 0.0 150.0 45.0 7.0 8.0 0.0 15.0 0.0 -135.0 0.0 150.0 45.0 14.0 15.0)) -(setq buf (coerce buf cons)) -(setq l (- (length buf) 1)) -(setq i l) -(setq ans nil) -(while (<= 0 i) - (if (and (not (= i 7)) (not (= i 8)) (not (= i 16)) (not (= i 17))) - (if (not ans) - (setq ans (cons (elt buf i) nil)) - (setq ans (cons (elt buf i) ans)))) - (setq i (- i 1))) -(coerce ans float-vector) -|# From 7e2252f55baea9cbc5e2430923bb78b0bd61799a Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 25 Jun 2021 15:25:30 +0900 Subject: [PATCH 014/127] add cable-insertion_naive_init.l --- .../euslisp/cable-insertion_naive_init.l | 15 +++++++++++++++ 1 file changed, 15 insertions(+) create mode 100644 jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l new file mode 100644 index 0000000000..733e4ce665 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l @@ -0,0 +1,15 @@ +#!/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 *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 824) :rpy (float-vector -3.14 1.57 1.57))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) From 770849e8ed4b81a52172f573038449eed58ce2f8 Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 25 Jun 2021 15:28:15 +0900 Subject: [PATCH 015/127] delete amabe_pub_pose_stamped.l because I made same role program in jsk_panda_robot --- .../euslisp/amabe_pub_pose_stamped.l | 46 ------------------- 1 file changed, 46 deletions(-) delete mode 100644 jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l diff --git a/jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l b/jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l deleted file mode 100644 index 6776dd7123..0000000000 --- a/jsk_2021_fix_kxr/euslisp/amabe_pub_pose_stamped.l +++ /dev/null @@ -1,46 +0,0 @@ -#!/usr/bin/env roseus - -(require "package://panda_eus/euslisp/dual_panda-interface.l") - -(ros::roseus-add-msgs "std_msgs") -(ros::roseus-add-msgs "sensor_msgs") -(ros::roseus-add-msgs "geometry_msgs") - -(ros::roseus "pose-stamped-publisher") - -(setq *robot* (dual_panda)) - -(ros::advertise "/dual_panda/end_coords/rarm" geometry_msgs::PoseStamped 1) -(ros::advertise "/dual_panda/end_coords/larm" geometry_msgs::PoseStamped 1) - -(defvar *joints* nil) - -(defun joints-cb (msg) - (setq buf (send msg :position)) - (setq buf (coerce buf cons)) - (setq l (- (length buf) 1)) - (setq i l) - (setq ans nil) - (print "publishing end-coords...") - (while (<= 0 i) - (if (and (not (= i 7)) (not (= i 8)) (not (= i 16)) (not (= i 17))) - (if (not ans) - (setq ans (cons (elt buf i) nil)) - (setq ans (cons (elt buf i) ans)))) - (setq i (- i 1))) - (setq *joints* (coerce ans float-vector))) - -(ros::rate 10) -(ros::subscribe "/dual_panda/joint_states" sensor_msgs::JointState #'joints-cb 1) - -(while (ros::ok) - (let ((pose-stamped-rarm (instance geometry_msgs::PoseStamped :init) (pose-stamped-larm (instance geometry_msgs::PoseStamped :init)))) - (setq joint-states (coerce (mapcar #'(lambda (x) (rad2deg x)) (coerce *joints* cons)) float-vector)) - (if (= (length joint-states) 14) - (send *robot* :angle-vector joint-states)) - (setq pose-stamped-rarm (ros::coords->tf-pose-stamped (send *robot* :rarm :end-coords) "dual_arm_base")) - (setq pose-stamped-larm (ros::coords->tf-pose-stamped (send *robot* :larm :end-coords) "dual_arm_base")) - (ros::publish "/dual_panda/end_coords/rarm" pose-stamped-rarm) - (ros::publish "/dual_panda/end_coords/larm" pose-stamped-larm) - (ros::spin-once) - (ros::sleep))) From af18cf9c7dcc50a94ce7f6cbfec3dab6e28520ff Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 25 Jun 2021 15:32:06 +0900 Subject: [PATCH 016/127] add cable-insertion_naive_grasp.l --- jsk_2021_fix_kxr/euslisp/cable_insertion_naive_grasp.l | 1 + 1 file changed, 1 insertion(+) create mode 100644 jsk_2021_fix_kxr/euslisp/cable_insertion_naive_grasp.l diff --git a/jsk_2021_fix_kxr/euslisp/cable_insertion_naive_grasp.l b/jsk_2021_fix_kxr/euslisp/cable_insertion_naive_grasp.l new file mode 100644 index 0000000000..146e8072bf --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/cable_insertion_naive_grasp.l @@ -0,0 +1 @@ +(send *ri* :rarm :start-grasp) From 50d83c9a697d51ef4928e2b3e130922518067a4a Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 25 Jun 2021 17:18:10 +0900 Subject: [PATCH 017/127] modified name of *_grasp.l and some of content --- jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l new file mode 100644 index 0000000000..7b69374790 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l @@ -0,0 +1,3 @@ +#!/usr/bin/env roseus + +(send *ri* :rarm :start-grasp) From e0cbcfd7292472a2f800add583dd607605196621 Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 25 Jun 2021 17:27:34 +0900 Subject: [PATCH 018/127] changed stop-grasp width from 0.04 to 0.02 --- jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l | 2 ++ 1 file changed, 2 insertions(+) diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l index 733e4ce665..dc306eb275 100644 --- a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l +++ b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l @@ -8,6 +8,8 @@ (objects (list *robot*)) +(send *ri* :stop-grasp :rarm :width 0.02) + (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 824) :rpy (float-vector -3.14 1.57 1.57))) :rotation-axis t) From 2768603e1ce3667abb396edea4597672a09c99af Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 25 Jun 2021 17:48:04 +0900 Subject: [PATCH 019/127] modified detail of *_grasp.l --- jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l index 7b69374790..83e60fed05 100644 --- a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l +++ b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l @@ -1,3 +1,3 @@ #!/usr/bin/env roseus -(send *ri* :rarm :start-grasp) +(send *ri* :start-grasp :rarm) From d2b12c25a5ac63848b221d65ffca4ac1b87f6150 Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 25 Jun 2021 17:49:13 +0900 Subject: [PATCH 020/127] add cable-insertion_naive_insert.l --- .../euslisp/cable-insertion_naive_insert.l | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l new file mode 100644 index 0000000000..b9a28604b3 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l @@ -0,0 +1,14 @@ +#!/usr/bin/env roseus + +(send *robot* :rarm :move-end-pos #f(6 0 0)) + +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :stop-grasp :rarm :width 0.004) + +(send *robot* :rarm :move-end-pos #f(-6 0 0)) + +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + From d288e5e9b6caf0cf285f9369f440996bbf92177d Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 28 Jun 2021 16:10:35 +0900 Subject: [PATCH 021/127] add cable-inserition_naive_demonstration_collector.py --- .../euslisp/cable-insertion_naive_init.l | 0 .../euslisp/cable-insertion_naive_insert.l | 0 ...insertion_naive_demonstration_collector.py | 99 +++++++++++++++++++ 3 files changed, 99 insertions(+) mode change 100644 => 100755 jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l mode change 100644 => 100755 jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l create mode 100755 jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l old mode 100644 new mode 100755 diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l old mode 100644 new mode 100755 diff --git a/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py b/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py new file mode 100755 index 0000000000..ce2b6876fb --- /dev/null +++ b/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py @@ -0,0 +1,99 @@ +"""Running eus scripts as demonstration. +""" +#!/usr/bin/env python +import rospy +import os +from os.path import join +import shlex +import subprocess + + +class ScriptDemonstrationCollector(object): + + def __init__(self, initializer_script_path, main_script_path, save_dir_base='/tmp'): + """Euslisp script based demonstration collector. + + Args: + initializer_script_path (str): Path to initializer script. + main_script_path (str): Path to main script. + save_dir_base (str): Path to save rosbag file + """ + self.initializer_script_path = initializer_script_path + self.main_script_path = main_script_path + + def get_demonstration(self): + """Getting demonstration + """ + self.run_eus_scrpt(self.initializer_script_path, wait=True) + self.start_rosbag_record() + self.run_eus_scrpt(self.main_script_path, wait=True) + self.stop_rosbag_record() + self.check_if_save_demo() + + def run_eus_scrpt(self, file_path, wait=True): + """Run euslistp script + + Args: + file_path (str): executable file path + wait (bool, optional): Wait until the script to be finished Defaults to True. + + Raises: + RuntimeError: File not found. + + Returns: + int: PID of subproces. + """ + #print(os.path.exists(file_path)) + if not os.path.exists(file_path): + raise FileNotFoundError( + "{}, file path dosen't exist".format(file_path)) + command = shlex.split("{}".format(file_path)) + rospy.loginfo('Running {}...', file_path) + proc = subprocess.Popen(command) + if wait: + proc.communicate() + return proc + + def start_rosbag_record(self): + """Start rosbag record subprocess. + """ + self.target_dir = join(self.save_dir_base, 'rosbag') + self.num_bagfies = len([name for name in os.listdir( + self.target_dir) if os.path.isfile((self.target_dir, name))]) + options = "save_dir:={} bagfile_prefix:={:03}".format( + self.target_dir, self.num_bagfies) + rospy.loginfo( + "Start recording, saving file in {}...".format(self.target_dir)) + command = "roslaunch jsk_panda_startup panda_record.launch {}".format( + options) + command = shlex.split(command) + self.rosbag_proc = subprocess.Popen(command) + + def stop_rosbag_record(self): + """Kill rosbag subprocess to end record. + """ + rospy.loginfo("Stop recording rosbag...") + if self.rosbag_proc is not None: + self.rosbag_proc.send_signal(subprocess.signal.SIGINT) + + def check_if_save_demo(self): + """Ask user if you want to save this demonstration or not. + """ + save_demo = raw_input("Save this trajectory? [yes/no]") + if save_demo == 'no': + for f in os.listdir(self.target_dir): + if f.startswith('{:03}'.format(self.num_bagfies)): + bagfile_name = join(self.target_dir, f) + cmd = "rm -rf {}".format(bagfile_name) + cmd = shlex.split(cmd) + subprocess.call(cmd) + +# initializer_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l" +# main_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insertion.l" +# node = ScriptDemonstrationCollector(initializer_script_path=initializer_script_path,main_script_path=main_script_path) +# node.get_demonstration() + +initializer_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l" +main_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l" +node = ScriptDemonstrationCollector(initializer_script_path=initializer_script_path,main_script_path=main_script_path) +node.get_demonstration() From afdfdd0931e96b26f4e08699ee397672380ffdd1 Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 28 Jun 2021 19:05:56 +0900 Subject: [PATCH 022/127] deleted unnecessary commentouts --- .../cable-insertion_naive_demonstration_collector.py | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py b/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py index ce2b6876fb..e150d30639 100755 --- a/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py +++ b/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py @@ -43,7 +43,6 @@ def run_eus_scrpt(self, file_path, wait=True): Returns: int: PID of subproces. """ - #print(os.path.exists(file_path)) if not os.path.exists(file_path): raise FileNotFoundError( "{}, file path dosen't exist".format(file_path)) @@ -57,6 +56,7 @@ def run_eus_scrpt(self, file_path, wait=True): def start_rosbag_record(self): """Start rosbag record subprocess. """ + print("start rosbag") self.target_dir = join(self.save_dir_base, 'rosbag') self.num_bagfies = len([name for name in os.listdir( self.target_dir) if os.path.isfile((self.target_dir, name))]) @@ -88,11 +88,6 @@ def check_if_save_demo(self): cmd = shlex.split(cmd) subprocess.call(cmd) -# initializer_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l" -# main_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insertion.l" -# node = ScriptDemonstrationCollector(initializer_script_path=initializer_script_path,main_script_path=main_script_path) -# node.get_demonstration() - initializer_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l" main_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l" node = ScriptDemonstrationCollector(initializer_script_path=initializer_script_path,main_script_path=main_script_path) From 7a7277e1c0d371797851067e9007ef98f77bb378 Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 28 Jun 2021 19:12:52 +0900 Subject: [PATCH 023/127] add (exit) to end of init.l --- jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l | 2 ++ 1 file changed, 2 insertions(+) diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l index dc306eb275..9800d1ec39 100755 --- a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l +++ b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l @@ -15,3 +15,5 @@ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(547 -18 824) :rpy (float-vector -3.14 1.57 1.57))) :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) + +(exit) From 2d4d0926fd6b6ca0cd73ada407f45dd242a0f27e Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 28 Jun 2021 19:34:32 +0900 Subject: [PATCH 024/127] add self.save_dir_base --- .../scripts/cable-insertion_naive_demonstration_collector.py | 1 + 1 file changed, 1 insertion(+) diff --git a/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py b/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py index e150d30639..8c485e0396 100755 --- a/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py +++ b/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py @@ -20,6 +20,7 @@ def __init__(self, initializer_script_path, main_script_path, save_dir_base='/tm """ self.initializer_script_path = initializer_script_path self.main_script_path = main_script_path + self.save_dir_base = save_dir_base def get_demonstration(self): """Getting demonstration From 715636d6b38c9433aff54a2772267d9efa8a1672 Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 28 Jun 2021 19:44:12 +0900 Subject: [PATCH 025/127] set save_dir_base /home/amabe/rosbag/panda --- .../scripts/cable-insertion_naive_demonstration_collector.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py b/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py index 8c485e0396..19fe2ea9f6 100755 --- a/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py +++ b/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py @@ -10,7 +10,7 @@ class ScriptDemonstrationCollector(object): - def __init__(self, initializer_script_path, main_script_path, save_dir_base='/tmp'): + def __init__(self, initializer_script_path, main_script_path, save_dir_base='/home/amabe/rosbag/panda'): """Euslisp script based demonstration collector. Args: @@ -58,7 +58,7 @@ def start_rosbag_record(self): """Start rosbag record subprocess. """ print("start rosbag") - self.target_dir = join(self.save_dir_base, 'rosbag') + self.target_dir = join(self.save_dir_base, 'cable-insertion_naive') self.num_bagfies = len([name for name in os.listdir( self.target_dir) if os.path.isfile((self.target_dir, name))]) options = "save_dir:={} bagfile_prefix:={:03}".format( From 7dc9cef7d2bcad5c88aba06a7c4c4c87c347cfec Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 29 Jun 2021 11:36:22 +0900 Subject: [PATCH 026/127] modified grasp.l and insert.l to made them conforming to demonstration --- .../euslisp/cable-insertion_naive_grasp.l | 9 +++++++++ .../euslisp/cable-insertion_naive_insert.l | 14 ++++++++++++-- 2 files changed, 21 insertions(+), 2 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l index 83e60fed05..40d608c952 100644 --- a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l +++ b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l @@ -1,3 +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) diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l index b9a28604b3..90fcfaaaf7 100755 --- a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l +++ b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l @@ -1,14 +1,24 @@ #!/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 *robot* :angle-vector (send *ri* :state :potentio-vector)) + (send *robot* :rarm :move-end-pos #f(6 0 0)) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) (send *ri* :stop-grasp :rarm :width 0.004) - +(unix::sleep 1) (send *robot* :rarm :move-end-pos #f(-6 0 0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) +(exit) From 38bb9617317b9d9e0bf30f5543dc7767079d256d Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 29 Jun 2021 12:18:40 +0900 Subject: [PATCH 027/127] modeified L63, start_rosbag_record --- .../scripts/cable-insertion_naive_demonstration_collector.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py b/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py index 19fe2ea9f6..8e2b9f8ea6 100755 --- a/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py +++ b/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py @@ -60,7 +60,7 @@ def start_rosbag_record(self): print("start rosbag") self.target_dir = join(self.save_dir_base, 'cable-insertion_naive') self.num_bagfies = len([name for name in os.listdir( - self.target_dir) if os.path.isfile((self.target_dir, name))]) + self.target_dir) if os.path.isfile(os.path.join(self.target_dir, name))]) options = "save_dir:={} bagfile_prefix:={:03}".format( self.target_dir, self.num_bagfies) rospy.loginfo( From 212a657c1d387781a5f2647e3749fd44f3123f52 Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 29 Jun 2021 12:34:38 +0900 Subject: [PATCH 028/127] add setting_script_path in demonstration.py --- .../euslisp/cable-insertion_naive_insert.l | 1 + .../cable-insertion_naive_demonstration_collector.py | 12 +++++++----- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l index 90fcfaaaf7..b3ca4c35ff 100755 --- a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l +++ b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l @@ -17,6 +17,7 @@ (send *ri* :stop-grasp :rarm :width 0.004) (unix::sleep 1) + (send *robot* :rarm :move-end-pos #f(-6 0 0)) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) diff --git a/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py b/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py index 8e2b9f8ea6..5d2e4c46e6 100755 --- a/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py +++ b/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py @@ -10,7 +10,7 @@ class ScriptDemonstrationCollector(object): - def __init__(self, initializer_script_path, main_script_path, save_dir_base='/home/amabe/rosbag/panda'): + def __init__(self, initializer_script_path, setting_script_path, main_script_path, save_dir_base='/home/amabe/rosbag/panda'): """Euslisp script based demonstration collector. Args: @@ -19,19 +19,20 @@ def __init__(self, initializer_script_path, main_script_path, save_dir_base='/ho save_dir_base (str): Path to save rosbag file """ self.initializer_script_path = initializer_script_path + self.setting_script_path = setting_script_path self.main_script_path = main_script_path self.save_dir_base = save_dir_base def get_demonstration(self): """Getting demonstration """ - self.run_eus_scrpt(self.initializer_script_path, wait=True) + self.run_eus_script(self.initializer_script_path, wait=True) self.start_rosbag_record() - self.run_eus_scrpt(self.main_script_path, wait=True) + self.run_eus_script(self.main_script_path, wait=True) self.stop_rosbag_record() self.check_if_save_demo() - def run_eus_scrpt(self, file_path, wait=True): + def run_eus_script(self, file_path, wait=True): """Run euslistp script Args: @@ -90,6 +91,7 @@ def check_if_save_demo(self): subprocess.call(cmd) initializer_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l" +setting_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l" main_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l" -node = ScriptDemonstrationCollector(initializer_script_path=initializer_script_path,main_script_path=main_script_path) +node = ScriptDemonstrationCollector(initializer_script_path=initializer_script_path,setting_script_path=setting_script_path,main_script_path=main_script_path) node.get_demonstration() From 9b8cf2fde59c375dd0874f04377ec17233f64bc8 Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 29 Jun 2021 13:25:16 +0900 Subject: [PATCH 029/127] add setting fase in demonstration --- jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l | 0 jsk_2021_fix_kxr/euslisp/cable_insertion_naive_grasp.l | 1 - .../cable-insertion_naive_demonstration_collector.py | 10 ++++++++++ 3 files changed, 10 insertions(+), 1 deletion(-) mode change 100644 => 100755 jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l delete mode 100644 jsk_2021_fix_kxr/euslisp/cable_insertion_naive_grasp.l diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l old mode 100644 new mode 100755 diff --git a/jsk_2021_fix_kxr/euslisp/cable_insertion_naive_grasp.l b/jsk_2021_fix_kxr/euslisp/cable_insertion_naive_grasp.l deleted file mode 100644 index 146e8072bf..0000000000 --- a/jsk_2021_fix_kxr/euslisp/cable_insertion_naive_grasp.l +++ /dev/null @@ -1 +0,0 @@ -(send *ri* :rarm :start-grasp) diff --git a/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py b/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py index 5d2e4c46e6..6883f01c7d 100755 --- a/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py +++ b/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py @@ -27,6 +27,16 @@ def get_demonstration(self): """Getting demonstration """ self.run_eus_script(self.initializer_script_path, wait=True) + + while True: + wait_setting = raw_input("press Enter to finish setting") + if wait_setting == "": + print("you pressed Enter") + break + else: + print("you pressed other key") + + self.run_eus_script(self.setting_script_path, wait=True) self.start_rosbag_record() self.run_eus_script(self.main_script_path, wait=True) self.stop_rosbag_record() From 2eb8a1fa2d203b00ae962b92e538ff0a659f31d9 Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 29 Jun 2021 14:08:33 +0900 Subject: [PATCH 030/127] made loop which wait for Enter in init.l and adjast demonstration to it --- jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l | 7 +++++++ .../cable-insertion_naive_demonstration_collector.py | 10 ---------- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l index 9800d1ec39..f678331a03 100755 --- a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l +++ b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l @@ -16,4 +16,11 @@ (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) +(unix::sleep 1) + (exit) diff --git a/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py b/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py index 6883f01c7d..5d2e4c46e6 100755 --- a/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py +++ b/jsk_2021_fix_kxr/scripts/cable-insertion_naive_demonstration_collector.py @@ -27,16 +27,6 @@ def get_demonstration(self): """Getting demonstration """ self.run_eus_script(self.initializer_script_path, wait=True) - - while True: - wait_setting = raw_input("press Enter to finish setting") - if wait_setting == "": - print("you pressed Enter") - break - else: - print("you pressed other key") - - self.run_eus_script(self.setting_script_path, wait=True) self.start_rosbag_record() self.run_eus_script(self.main_script_path, wait=True) self.stop_rosbag_record() From 9ef6ed3f36f33ca9cef34a6a92a9f2d42660c25b Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 29 Jun 2021 18:59:53 +0900 Subject: [PATCH 031/127] made minor change to insert cable, but couldn't --- jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l | 6 +++--- jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l index f678331a03..5bbfc96e61 100755 --- a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l +++ b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l @@ -8,11 +8,11 @@ (objects (list *robot*)) -(send *ri* :stop-grasp :rarm :width 0.02) +(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 824) :rpy (float-vector -3.14 1.57 1.57))) :rotation-axis t) +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(547 -18 823) :rpy (float-vector -3.14 1.57 1.57))) :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) @@ -20,7 +20,7 @@ (print "waiting for Enter...") (unix::sleep 1)) -(send *ri* :start-grasp :rarm) +(send *ri* :start-grasp :rarm :effort 100) (unix::sleep 1) (exit) diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l index b3ca4c35ff..166c84abaf 100755 --- a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l +++ b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l @@ -10,15 +10,15 @@ (send *robot* :angle-vector (send *ri* :state :potentio-vector)) -(send *robot* :rarm :move-end-pos #f(6 0 0)) +(send *robot* :rarm :move-end-pos #f(12 0 0)) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :angle-vector (send *robot* :angle-vector) 5000) (send *ri* :wait-interpolation) (send *ri* :stop-grasp :rarm :width 0.004) (unix::sleep 1) -(send *robot* :rarm :move-end-pos #f(-6 0 0)) +(send *robot* :rarm :move-end-pos #f(-12 0 0)) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) From 01b943ee6693f16ce49ffc47bb2f4ad909720113 Mon Sep 17 00:00:00 2001 From: softyanija Date: Thu, 1 Jul 2021 12:59:57 +0900 Subject: [PATCH 032/127] add :translation-axis t in init.l --- jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l index 5bbfc96e61..9a46100bbc 100755 --- a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l +++ b/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l @@ -12,7 +12,7 @@ (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 823) :rpy (float-vector -3.14 1.57 1.57))) :rotation-axis t) +(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) From 3fb0735be1df01bbf5831f5da9e7c6ac3ef47eee Mon Sep 17 00:00:00 2001 From: softyanija Date: Thu, 1 Jul 2021 14:43:58 +0900 Subject: [PATCH 033/127] add files to heuristic_insertion demo --- jsk_2021_fix_kxr/euslisp/#amabe_set_kxr.l# | 73 ++++++++++++++ .../euslisp/cable-insertion_heuristic_init.l | 26 +++++ .../cable-insertion_heuristic_insert.l | 35 +++++++ ...rtion_heuristic_demonstration_collector.py | 97 +++++++++++++++++++ 4 files changed, 231 insertions(+) create mode 100644 jsk_2021_fix_kxr/euslisp/#amabe_set_kxr.l# create mode 100755 jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_init.l create mode 100644 jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_insert.l create mode 100644 jsk_2021_fix_kxr/scripts/cable-insertion_heuristic_demonstration_collector.py diff --git a/jsk_2021_fix_kxr/euslisp/#amabe_set_kxr.l# b/jsk_2021_fix_kxr/euslisp/#amabe_set_kxr.l# new file mode 100644 index 0000000000..b5b7e30477 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/#amabe_set_kxr.l# @@ -0,0 +1,73 @@ +#!/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 *robot* :reset-pose) +(send *robot* :rarm :move-end-pos #f(-500 0 0)) +(send *robot* :larm :move-end-pos #f(-500 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 -88 1200) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 177 1200) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 -88 1093.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 177 1093.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :rarm) +(send *ri* :start-grasp :larm) +(unix::sleep 2) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 -138 1093.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 127 1093.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :stop-grasp :rarm) +(send *ri* :stop-grasp :larm) +(unix::sleep 2) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 -138 1300.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 127 1300.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 -310 1300.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(510 -310 1170.364) :rpy (float-vector 1.57 0.0 -3.14))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(510 -310 1070.364) :rpy (float-vector 1.57 0.0 -3.14))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(510 -260 1070.364) :rpy (float-vector 1.57 0.0 -3.14))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :rarm) +(unix::sleep 2) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(510 -260 1070.364) :rpy (float-vector 1.57 0.0 -1.57))) :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *ri* :stop-grasp :rarm) +(unix::sleep 2) + +(send *robot* :rarm :move-end-pos #f(-50 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_init.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_init.l new file mode 100755 index 0000000000..9a46100bbc --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_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) diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_insert.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_insert.l new file mode 100644 index 0000000000..873fdd0e15 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_insert.l @@ -0,0 +1,35 @@ +#!/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) + +(setq ee-pos #f(547 -18 825)) +(setq ee-rpy #f(-3.14 1.57 1.57)) + +(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) +(setq stroke #f(0 0 -1)) + +(while (< i 10) + (setq ee-pos (v+ ee-pos stroke)) + (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) 2000) + (send *ri* :wait-interpolation) + (setq i (+ i 1))) + + + + + diff --git a/jsk_2021_fix_kxr/scripts/cable-insertion_heuristic_demonstration_collector.py b/jsk_2021_fix_kxr/scripts/cable-insertion_heuristic_demonstration_collector.py new file mode 100644 index 0000000000..5bf31d4882 --- /dev/null +++ b/jsk_2021_fix_kxr/scripts/cable-insertion_heuristic_demonstration_collector.py @@ -0,0 +1,97 @@ +"""Running eus scripts as demonstration. +""" +#!/usr/bin/env python +import rospy +import os +from os.path import join +import shlex +import subprocess + + +class ScriptDemonstrationCollector(object): + + def __init__(self, initializer_script_path, setting_script_path, main_script_path, save_dir_base='/home/amabe/rosbag/panda'): + """Euslisp script based demonstration collector. + + Args: + initializer_script_path (str): Path to initializer script. + main_script_path (str): Path to main script. + save_dir_base (str): Path to save rosbag file + """ + self.initializer_script_path = initializer_script_path + self.setting_script_path = setting_script_path + self.main_script_path = main_script_path + self.save_dir_base = save_dir_base + + def get_demonstration(self): + """Getting demonstration + """ + self.run_eus_script(self.initializer_script_path, wait=True) + self.start_rosbag_record() + self.run_eus_script(self.main_script_path, wait=True) + self.stop_rosbag_record() + self.check_if_save_demo() + + def run_eus_script(self, file_path, wait=True): + """Run euslistp script + + Args: + file_path (str): executable file path + wait (bool, optional): Wait until the script to be finished Defaults to True. + + Raises: + RuntimeError: File not found. + + Returns: + int: PID of subproces. + """ + if not os.path.exists(file_path): + raise FileNotFoundError( + "{}, file path dosen't exist".format(file_path)) + command = shlex.split("{}".format(file_path)) + rospy.loginfo('Running {}...', file_path) + proc = subprocess.Popen(command) + if wait: + proc.communicate() + return proc + + def start_rosbag_record(self): + """Start rosbag record subprocess. + """ + print("start rosbag") + self.target_dir = join(self.save_dir_base, 'cable-insertion_naive') + self.num_bagfies = len([name for name in os.listdir( + self.target_dir) if os.path.isfile(os.path.join(self.target_dir, name))]) + options = "save_dir:={} bagfile_prefix:={:03}".format( + self.target_dir, self.num_bagfies) + rospy.loginfo( + "Start recording, saving file in {}...".format(self.target_dir)) + command = "roslaunch jsk_panda_startup panda_record.launch {}".format( + options) + command = shlex.split(command) + self.rosbag_proc = subprocess.Popen(command) + + def stop_rosbag_record(self): + """Kill rosbag subprocess to end record. + """ + rospy.loginfo("Stop recording rosbag...") + if self.rosbag_proc is not None: + self.rosbag_proc.send_signal(subprocess.signal.SIGINT) + + def check_if_save_demo(self): + """Ask user if you want to save this demonstration or not. + """ + save_demo = raw_input("Save this trajectory? [yes/no]") + if save_demo == 'no': + for f in os.listdir(self.target_dir): + if f.startswith('{:03}'.format(self.num_bagfies)): + bagfile_name = join(self.target_dir, f) + cmd = "rm -rf {}".format(bagfile_name) + cmd = shlex.split(cmd) + subprocess.call(cmd) + +initializer_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_init.l" +setting_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_grasp.l" +main_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l" +node = ScriptDemonstrationCollector(initializer_script_path=initializer_script_path,setting_script_path=setting_script_path,main_script_path=main_script_path) +node.get_demonstration() From 7c8bf429aa5c3f73e41cf5c4d65a2075aa9d86d9 Mon Sep 17 00:00:00 2001 From: softyanija Date: Thu, 1 Jul 2021 17:22:25 +0900 Subject: [PATCH 034/127] now we can choice success or fault or don't save --- ...rtion_heuristic_demonstration_collector.py | 51 ++++++++++++++----- 1 file changed, 38 insertions(+), 13 deletions(-) diff --git a/jsk_2021_fix_kxr/scripts/cable-insertion_heuristic_demonstration_collector.py b/jsk_2021_fix_kxr/scripts/cable-insertion_heuristic_demonstration_collector.py index 5bf31d4882..0dffee6dc3 100644 --- a/jsk_2021_fix_kxr/scripts/cable-insertion_heuristic_demonstration_collector.py +++ b/jsk_2021_fix_kxr/scripts/cable-insertion_heuristic_demonstration_collector.py @@ -10,7 +10,7 @@ class ScriptDemonstrationCollector(object): - def __init__(self, initializer_script_path, setting_script_path, main_script_path, save_dir_base='/home/amabe/rosbag/panda'): + def __init__(self, initializer_script_path, setting_script_path, main_script_path, save_dir_base='/home/amabe/rosbag/panda/cable-insertion_heuristic'): """Euslisp script based demonstration collector. Args: @@ -59,13 +59,10 @@ def start_rosbag_record(self): """Start rosbag record subprocess. """ print("start rosbag") - self.target_dir = join(self.save_dir_base, 'cable-insertion_naive') - self.num_bagfies = len([name for name in os.listdir( - self.target_dir) if os.path.isfile(os.path.join(self.target_dir, name))]) - options = "save_dir:={} bagfile_prefix:={:03}".format( - self.target_dir, self.num_bagfies) + self.save_dir_base + options = "save_dir:={}".format(self.save_dir_base) rospy.loginfo( - "Start recording, saving file in {}...".format(self.target_dir)) + "Start recording, saving file in {}...".format(self.save_dir_base)) command = "roslaunch jsk_panda_startup panda_record.launch {}".format( options) command = shlex.split(command) @@ -81,16 +78,44 @@ def stop_rosbag_record(self): def check_if_save_demo(self): """Ask user if you want to save this demonstration or not. """ - save_demo = raw_input("Save this trajectory? [yes/no]") - if save_demo == 'no': - for f in os.listdir(self.target_dir): - if f.startswith('{:03}'.format(self.num_bagfies)): - bagfile_name = join(self.target_dir, f) + while True: + save_demo = raw_input("Success, fault, don't save [s/f/n]") + if save_demo == 's': + self.target_dir = join(self.save_dir_base, 'success') + self.num_bagfiles = len([name for name in os.listdir( self.target_dir) if os.path.isfile(os.path.join(self.target_dir, name))]) + for f in os.listdir(self.save_dir_base): + if os.path.isfile(os.path.join(self.save_dir_base,f)): + bagfile_name_origin = join(self.save_dir_base, f) + bagfile_name_con = join(self.target_dir, '{}_'.format(self.num_bagfiles) + f) + cmd = "mv {} {}".format(bagfile_name_origin,bagfile_name_con) + cmd = shlex.split(cmd) + subprocess.call(cmd) + break + + if save_demo == 'f': + self.target_dir = join(self.save_dir_base, 'fault') + self.num_bagfiles = len([name for name in os.listdir( self.target_dir) if os.path.isfile(os.path.join(self.target_dir, name))]) + for f in os.listdir(self.save_dir_base): + if os.path.isfile(os.path.join(self.save_dir_base,f)): + bagfile_name_origin = join(self.save_dir_base, f) + bagfile_name_con = join(self.target_dir, '{}_'.format(self.num_bagfiles) + f) + cmd = "mv {} {}".format(bagfile_name_origin,bagfile_name_con) + cmd = shlex.split(cmd) + subprocess.call(cmd) + break + + if save_demo == 'n': + print("trashing this data") + for f in os.listdir(self.save_dir_base): + if os.path.isfile(os.path.join(self.save_dir_base,f)): + bagfile_name = join(self.save_dir_base, f) cmd = "rm -rf {}".format(bagfile_name) + print(cmd) cmd = shlex.split(cmd) subprocess.call(cmd) + break -initializer_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_init.l" +initializer_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_init.l" setting_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_grasp.l" main_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l" node = ScriptDemonstrationCollector(initializer_script_path=initializer_script_path,setting_script_path=setting_script_path,main_script_path=main_script_path) From 661fd8744d99bd7d1abf8f28202abf07b2f1c9a3 Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 24 Sep 2021 15:59:37 +0900 Subject: [PATCH 035/127] commit the progress until midtermreport --- .../euslisp/cable-insertion_heuristic_init.l | 9 +- .../cable-insertion_heuristic_insert.l | 56 ++++++-- ...tion_heuristic_demonstration_collector.py# | 122 ++++++++++++++++++ ...rtion_heuristic_demonstration_collector.py | 14 +- 4 files changed, 183 insertions(+), 18 deletions(-) mode change 100644 => 100755 jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_insert.l create mode 100644 jsk_2021_fix_kxr/scripts/#cable-insertion_heuristic_demonstration_collector.py# diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_init.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_init.l index 9a46100bbc..0b220ecd15 100755 --- a/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_init.l +++ b/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_init.l @@ -3,13 +3,20 @@ (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) diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_insert.l b/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_insert.l old mode 100644 new mode 100755 index 873fdd0e15..ddea8867fa --- a/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_insert.l +++ b/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_insert.l @@ -3,15 +3,28 @@ (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*)) -(send *ri* :stop-grasp :rarm :width 0.006) +(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)) @@ -20,16 +33,39 @@ (send *ri* :wait-interpolation) (setq i 0) -(setq stroke #f(0 0 -1)) +(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)) -(while (< i 10) - (setq ee-pos (v+ ee-pos stroke)) - (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) 2000) + (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))) + (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) diff --git a/jsk_2021_fix_kxr/scripts/#cable-insertion_heuristic_demonstration_collector.py# b/jsk_2021_fix_kxr/scripts/#cable-insertion_heuristic_demonstration_collector.py# new file mode 100644 index 0000000000..08be3cc84b --- /dev/null +++ b/jsk_2021_fix_kxr/scripts/#cable-insertion_heuristic_demonstration_collector.py# @@ -0,0 +1,122 @@ +"""Running eus scripts as demonstration. +""" +#!/usr/bin/env python +import rospy +import os +from os.path import join +import shlex +import subprocess + + +class ScriptDemonstrationCollector(object): + + def __init__(self, initializer_script_path, setting_script_path, main_script_path, save_dir_base='/home/amabe/rosbag/panda/cable-insertion_heuristic'): + """Euslisp script based demonstration collector. + + Args: + initializer_script_path (str): Path to initializer script. + main_script_path (str): Path to main script. + save_dir_base (str): Path to save rosbag file + """ + self.initializer_script_path = initializer_script_path + self.setting_script_path = setting_script_path + self.main_script_path = main_script_path + self.save_dir_base = save_dir_base + + def get_demonstration(self): + """Getting demonstration + """ + self.run_eus_script(self.initializer_script_path, wait=True) + self.start_rosbag_record() + self.run_eus_script(self.main_script_path, wait=True) + self.stop_rosbag_record() + self.check_if_save_demo() + + def run_eus_script(self, file_path, wait=True): + """Run euslistp script + + Args: + file_path (str): executable file path + wait (bool, optional): Wait until the script to be finished Defaults to True. + + Raises: + RuntimeError: File not found. + + Returns: + int: PID of subproces. + """ + if not os.path.exists(file_path): + raise FileNotFoundError( + "{}, file path dosen't exist".format(file_path)) + command = shlex.split("{}".format(file_path)) + rospy.loginfo('Running {}...', file_path) + proc = subprocess.Popen(command) + if wait: + proc.communicate() + return proc + + def start_rosbag_record(self): + """Start rosbag record subprocess. + """ + print("start rosbag") + self.save_dir_base + options = "save_dir:={}".format(self.save_dir_base) + rospy.loginfo( + "Start recording, saving file in {}...".format(self.save_dir_base)) + command = "roslaunch jsk_panda_startup panda_record.launch {}".format( + options) + command = shlex.split(command) + self.rosbag_proc = subprocess.Popen(command) + + def stop_rosbag_record(self): + """Kill rosbag subprocess to end record. + """ + rospy.loginfo("Stop recording rosbag...") + if self.rosbag_proc is not None: + self.rosbag_proc.send_signal(subprocess.signal.SIGINT) + + def check_if_save_demo(self): + """Ask user if you want to save this demonstration or not. + """ + while True: + save_demo = raw_input("Success, fault, don't save [s/f/n]") + if save_demo == 's': + self.target_dir = join(self.save_dir_base, 'success') + self.num_bagfiles = len([name for name in os.listdir( self.target_dir) if os.path.isfile(os.path.join(self.target_dir, name))]) + for f in os.listdir(self.save_dir_base): + if os.path.isfile(os.path.join(self.save_dir_base,f)): + bagfile_name_origin = join(self.save_dir_base, f) + bagfile_name_con = join(self.target_dir, '{:03}_success_'.format(self.num_bagfiles) + f) + cmd = "mv {} {}".format(bagfile_name_origin,bagfile_name_con) + cmd = shlex.split(cmd) + subprocess.call(cmd) + break + + if save_demo == 'f': + self.target_dir = join(self.save_dir_base, 'fault') + self.num_bagfiles = len([name for name in os.listdir( self.target_dir) if os.path.isfile(os.path.join(self.target_dir, name))]) + for f in os.listdir(self.save_dir_base): + if os.path.isfile(os.path.join(self.save_dir_base,f)): + bagfile_name_origin = join(self.save_dir_base, f) + bagfile_name_con = join(self.target_dir, '{:03}_fault_'.format(self.num_bagfiles) + f) + cmd = "mv {} {}".format(bagfile_name_origin,bagfile_name_con) + cmd = shlex.split(cmd) + subprocess.call(cmd) + break + + if save_demo == 'n': + print("trashing this data") + for f in os.listdir(self.save_dir_base): + if os.path.isfile(os.path.join(self.save_dir_base,f)): + bagfile_name = join(self.save_dir_base, f) + cmd = "rm -rf {}".format(bagfile_name) + print(cmd) + cmd = shlex.split(cmd) + subprocess.call(cmd) + break + +initializer_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_init.l" +setting_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_grasp.l" +main_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_insert.l" +node = ScriptDemonstrationCollector(initializer_script_path=initializer_script_path,setting_script_path=setting_script_path,main_script_path=main_script_path) +node.get_demonstration() diff --git a/jsk_2021_fix_kxr/scripts/cable-insertion_heuristic_demonstration_collector.py b/jsk_2021_fix_kxr/scripts/cable-insertion_heuristic_demonstration_collector.py index 0dffee6dc3..ea42599b7d 100644 --- a/jsk_2021_fix_kxr/scripts/cable-insertion_heuristic_demonstration_collector.py +++ b/jsk_2021_fix_kxr/scripts/cable-insertion_heuristic_demonstration_collector.py @@ -86,7 +86,7 @@ def check_if_save_demo(self): for f in os.listdir(self.save_dir_base): if os.path.isfile(os.path.join(self.save_dir_base,f)): bagfile_name_origin = join(self.save_dir_base, f) - bagfile_name_con = join(self.target_dir, '{}_'.format(self.num_bagfiles) + f) + bagfile_name_con = join(self.target_dir, '{:03}_success_'.format(self.num_bagfiles) + f) cmd = "mv {} {}".format(bagfile_name_origin,bagfile_name_con) cmd = shlex.split(cmd) subprocess.call(cmd) @@ -98,7 +98,7 @@ def check_if_save_demo(self): for f in os.listdir(self.save_dir_base): if os.path.isfile(os.path.join(self.save_dir_base,f)): bagfile_name_origin = join(self.save_dir_base, f) - bagfile_name_con = join(self.target_dir, '{}_'.format(self.num_bagfiles) + f) + bagfile_name_con = join(self.target_dir, '{:03}_fault_'.format(self.num_bagfiles) + f) cmd = "mv {} {}".format(bagfile_name_origin,bagfile_name_con) cmd = shlex.split(cmd) subprocess.call(cmd) @@ -109,14 +109,14 @@ def check_if_save_demo(self): for f in os.listdir(self.save_dir_base): if os.path.isfile(os.path.join(self.save_dir_base,f)): bagfile_name = join(self.save_dir_base, f) - cmd = "rm -rf {}".format(bagfile_name) - print(cmd) - cmd = shlex.split(cmd) - subprocess.call(cmd) + cmd = "rm -rf {}".format(bagfile_name) + print(cmd) + cmd = shlex.split(cmd) + subprocess.call(cmd) break initializer_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_init.l" setting_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_grasp.l" -main_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l" +main_script_path = "/home/amabe/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_insert.l" node = ScriptDemonstrationCollector(initializer_script_path=initializer_script_path,setting_script_path=setting_script_path,main_script_path=main_script_path) node.get_demonstration() From b70089a007379fa21bd165fe1426e947b51613a1 Mon Sep 17 00:00:00 2001 From: softyanija Date: Thu, 7 Oct 2021 17:54:11 +0900 Subject: [PATCH 036/127] wrote repairdemo_init.l and place.l --- jsk_2021_fix_kxr/euslisp/repairdemo_init.l | 17 +++++++++ jsk_2021_fix_kxr/euslisp/repairdemo_place.l | 42 +++++++++++++++++++++ 2 files changed, 59 insertions(+) create mode 100644 jsk_2021_fix_kxr/euslisp/repairdemo_init.l create mode 100644 jsk_2021_fix_kxr/euslisp/repairdemo_place.l diff --git a/jsk_2021_fix_kxr/euslisp/repairdemo_init.l b/jsk_2021_fix_kxr/euslisp/repairdemo_init.l new file mode 100644 index 0000000000..647d5284aa --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/repairdemo_init.l @@ -0,0 +1,17 @@ +#!/usr/bin/env roseus + +(require "package://panda_eus/euslisp/dual_panda-interface.l") + +(dual_panda-init) + +(objects (list *robot*)) + +(send *robot* :reset-pose) +(send *robot* :larm :move-end-pos #f(-500 0 0)) +(send *robot* :rarm :move-end-pos #f(-500 0 0)) + +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :stop-grasp :rarm) +(send *ri* :stop-grasp :larm) diff --git a/jsk_2021_fix_kxr/euslisp/repairdemo_place.l b/jsk_2021_fix_kxr/euslisp/repairdemo_place.l new file mode 100644 index 0000000000..99952613ce --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/repairdemo_place.l @@ -0,0 +1,42 @@ +#!/usr/bin/env roseus + +;;before grasp position +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(345 -246.5 1330) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(345 -84.8 1330) :rpy (float-vector 1.57 1.57 -3.14))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :larm :move-end-pos #f(30 0 0)) +(send *robot* :rarm :move-end-pos #f(30 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :rarm) +(send *ri* :start-grasp :larm) +(unix::sleep 2) + +;lift KXR +(send *robot* :larm :move-end-pos #f(-15 0 0)) +(send *robot* :rarm :move-end-pos #f(-15 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +;place KXR to cable_insert +(send *robot* :larm :move-end-pos #f(0 0 -150)) +(send *robot* :rarm :move-end-pos #f(0 0 150)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :larm :move-end-pos #f(15 0 0)) +(send *robot* :rarm :move-end-pos #f(15 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :stop-grasp :rarm) +(send *ri* :stop-grasp :larm) +(unix::sleep 2) + +(send *robot* :larm :move-end-pos #f(-30 0 0)) +(send *robot* :rarm :move-end-pos #f(-30 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) From 0124b8ccea5b1913e89a128b5b782b944b70d71e Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 8 Oct 2021 17:03:57 +0900 Subject: [PATCH 037/127] completed repairdemo --- .../euslisp/repairdemo/#repairdemo_init.l# | 29 ++++++++ .../repairdemo/repairdemo_cableinsert.l | 73 +++++++++++++++++++ .../euslisp/repairdemo/repairdemo_init.l | 30 ++++++++ .../euslisp/repairdemo/repairdemo_main.l | 6 ++ .../{ => repairdemo}/repairdemo_place.l | 8 +- .../euslisp/repairdemo/repairdemo_return.l | 57 +++++++++++++++ jsk_2021_fix_kxr/euslisp/repairdemo_init.l | 17 ----- 7 files changed, 200 insertions(+), 20 deletions(-) create mode 100644 jsk_2021_fix_kxr/euslisp/repairdemo/#repairdemo_init.l# create mode 100644 jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_cableinsert.l create mode 100644 jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_init.l create mode 100644 jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_main.l rename jsk_2021_fix_kxr/euslisp/{ => repairdemo}/repairdemo_place.l (89%) create mode 100644 jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_return.l delete mode 100644 jsk_2021_fix_kxr/euslisp/repairdemo_init.l diff --git a/jsk_2021_fix_kxr/euslisp/repairdemo/#repairdemo_init.l# b/jsk_2021_fix_kxr/euslisp/repairdemo/#repairdemo_init.l# new file mode 100644 index 0000000000..d01897bd1a --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/repairdemo/#repairdemo_init.l# @@ -0,0 +1,29 @@ +#!/usr/bin/env roseus + +(require "package://panda_eus/euslisp/dual_panda-interface.l") + +(dual_panda-init) + +(objects (list *robot*)) + +(send *ri* :stop-grasp :rarm) +(send *ri* :stop-grasp :larm) +(unix::sleep 2) + +(send *robot* :reset-pose) +(send *robot* :larm :move-end-pos #f(-500 0 0)) +(send *robot* :rarm :move-end-pos #f(-500 0 0)) + +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +#| wordkbench init +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(412 100 1300) :rpy (float-vector -3.14 1.57 1.57))) :translation-axis t :rotation-axis t)|# + +#| check KXR position +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(345 -246.5 1330) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(345 -84.8 1330) :rpy (float-vector 1.57 1.57 -3.14))) :translation-axis t :rotation-axis t) +(send *robot* :larm :move-end-pos #f(-50 0 0)) +(send *robot* :rarm :move-end-pos #f(-50 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) |# diff --git a/jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_cableinsert.l b/jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_cableinsert.l new file mode 100644 index 0000000000..8f35a9bc49 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_cableinsert.l @@ -0,0 +1,73 @@ +#!/usr/bin/env roseus + +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(345 260 1290) :rpy (float-vector -1.57 0 0))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(345 210 1290) :rpy (float-vector -1.57 0 0))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) +(send *ri* :start-grasp :larm) +(unix::sleep 1) + +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(345 210 1290) :rpy (float-vector -1.57 0 -1.57))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :stop-grasp :rarm) +(unix::sleep 1) + +(send *robot* :rarm :move-end-pos #f(-120 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :move-end-pos #f(0 0 150)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :move-end-pos #f(50 0 50)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :rarm) +(unix::sleep 1) + +;start to insertion +(send *robot* :rarm :move-end-pos #f(20 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :stop-grasp :rarm) +(unix::sleep 1) + +(send *robot* :rarm :move-end-pos #f(-20 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) +;end insertion + +(send *robot* :rarm :move-end-pos #f(-50 0 -50)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :move-end-pos #f(0 0 -150)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :move-end-pos #f(120 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :rarm) +(unix::sleep 1) + +(send *ri* :stop-grasp :larm) +(unix::sleep 1) + +(send *robot* :larm :move-end-pos #f(-70 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +;;check if successed cable-insertion + +;;end checking + diff --git a/jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_init.l b/jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_init.l new file mode 100644 index 0000000000..65bb9fdbcf --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_init.l @@ -0,0 +1,30 @@ +#!/usr/bin/env roseus + +(require "package://panda_eus/euslisp/dual_panda-interface.l") + +(dual_panda-init) + +(objects (list *robot*)) + + +(send *ri* :stop-grasp :rarm) +(send *ri* :stop-grasp :larm) +(unix::sleep 2) + +(send *robot* :reset-pose) +(send *robot* :larm :move-end-pos #f(-500 0 0)) +(send *robot* :rarm :move-end-pos #f(-500 0 0)) + +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +#| wordkbench init +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(412 100 1300) :rpy (float-vector -3.14 1.57 1.57))) :translation-axis t :rotation-axis t)|# + +#| check KXR position +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(345 -246.5 1330) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(345 -84.8 1330) :rpy (float-vector 1.57 1.57 -3.14))) :translation-axis t :rotation-axis t) +(send *robot* :larm :move-end-pos #f(-50 0 0)) +(send *robot* :rarm :move-end-pos #f(-50 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) |# diff --git a/jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_main.l b/jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_main.l new file mode 100644 index 0000000000..b382c80a63 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_main.l @@ -0,0 +1,6 @@ +#!/usr/bin/env roseus + +(load "repairdemo_init.l") +(load "repairdemo_place.l") +(load "repairdemo_cableinsert.l") +(load "repairdemo_return.l") diff --git a/jsk_2021_fix_kxr/euslisp/repairdemo_place.l b/jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_place.l similarity index 89% rename from jsk_2021_fix_kxr/euslisp/repairdemo_place.l rename to jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_place.l index 99952613ce..22a6bfeb0f 100644 --- a/jsk_2021_fix_kxr/euslisp/repairdemo_place.l +++ b/jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_place.l @@ -32,11 +32,13 @@ (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) -(send *ri* :stop-grasp :rarm) (send *ri* :stop-grasp :larm) (unix::sleep 2) -(send *robot* :larm :move-end-pos #f(-30 0 0)) -(send *robot* :rarm :move-end-pos #f(-30 0 0)) +(send *robot* :larm :move-end-pos #f(-100 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :larm :move-end-pos #f(0 0 -200)) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) diff --git a/jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_return.l b/jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_return.l new file mode 100644 index 0000000000..c127dbbe6e --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/repairdemo/repairdemo_return.l @@ -0,0 +1,57 @@ +#!/usr/bin/env roseus + +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(345 280 1330) :rpy (float-vector -1.57 1.57 0))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :larm :move-end-pos #f(0 0 200)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :larm :move-end-pos #f(30 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :larm) +(unix::sleep 1) + +#| state of this position +#f(-45.7282 -24.4921 60.716 -105.043 21.0379 92.2515 147.288 -24.7494 -8.22244 -5.22154 -104.346 -0.698772 95.9557 -74.1599) +|# + +(send *robot* :larm :move-end-pos #f(-100 -40 0)) +(send *robot* :rarm :move-end-pos #f(-100 40 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :larm :move-end-pos #f(-40 30 0)) +(send *robot* :rarm :move-end-pos #f(-40 -30 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :larm :move-end-pos #f(-30 0 0)) +(send *robot* :rarm :move-end-pos #f(-30 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :larm :move-end-pos #f(0 50 0)) +(send *robot* :rarm :move-end-pos #f(0 -50 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :stop-grasp :rarm) +(send *ri* :stop-grasp :larm) +(unix::sleep 2) + +(send *robot* :larm :move-end-pos #f(-20 -50 -50)) +(send *robot* :rarm :move-end-pos #f(-20 50 -50)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(unix::sleep 10) + +(send *robot* :reset-pose) +(send *robot* :larm :move-end-pos #f(-500 0 0)) +(send *robot* :rarm :move-end-pos #f(-500 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) diff --git a/jsk_2021_fix_kxr/euslisp/repairdemo_init.l b/jsk_2021_fix_kxr/euslisp/repairdemo_init.l deleted file mode 100644 index 647d5284aa..0000000000 --- a/jsk_2021_fix_kxr/euslisp/repairdemo_init.l +++ /dev/null @@ -1,17 +0,0 @@ -#!/usr/bin/env roseus - -(require "package://panda_eus/euslisp/dual_panda-interface.l") - -(dual_panda-init) - -(objects (list *robot*)) - -(send *robot* :reset-pose) -(send *robot* :larm :move-end-pos #f(-500 0 0)) -(send *robot* :rarm :move-end-pos #f(-500 0 0)) - -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - -(send *ri* :stop-grasp :rarm) -(send *ri* :stop-grasp :larm) From fff14edaf893ccf98cd8a33e31997e3146b19c9e Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 11 Oct 2021 12:53:12 +0900 Subject: [PATCH 038/127] organized files --- ...t.l => #cable-insertion_heuristic_init.l#} | 0 jsk_2021_fix_kxr/euslisp/#repairdemo_place.l# | 14 ++++++++ jsk_2021_fix_kxr/euslisp/a.out | 0 .../cable-insertion_heuristic_init.l | 33 +++++++++++++++++++ .../cable-insertion_heuristic_insert.l | 0 .../cable-insertion_naive_grasp.l | 0 .../cable-insertion_naive_init.l | 0 .../cable-insertion_naive_insert.l | 0 .../{ => move_test}/amabe_loosen_screw.l | 0 .../{ => move_test}/amabe_loosen_screw_pre.l | 0 .../{ => move_test}/amabe_pick_screw.l | 0 .../euslisp/{ => move_test}/amabe_plug.l | 0 .../euslisp/{ => move_test}/amabe_screw.l | 0 .../{ => move_test}/amabe_screw_memo.txt | 0 .../euslisp/{ => move_test}/amabe_screw_pre.l | 0 .../euslisp/{ => move_test}/amabe_set_kxr.l | 0 .../euslisp/{ => move_test}/grasp_demo.l | 0 .../euslisp/repairdemo/#repairdemo_return.l# | 9 +++++ 18 files changed, 56 insertions(+) rename jsk_2021_fix_kxr/euslisp/{cable-insertion_heuristic_init.l => #cable-insertion_heuristic_init.l#} (100%) create mode 100644 jsk_2021_fix_kxr/euslisp/#repairdemo_place.l# delete mode 100644 jsk_2021_fix_kxr/euslisp/a.out create mode 100755 jsk_2021_fix_kxr/euslisp/cable-insertion/cable-insertion_heuristic_init.l rename jsk_2021_fix_kxr/euslisp/{ => cable-insertion}/cable-insertion_heuristic_insert.l (100%) rename jsk_2021_fix_kxr/euslisp/{ => cable-insertion}/cable-insertion_naive_grasp.l (100%) rename jsk_2021_fix_kxr/euslisp/{ => cable-insertion}/cable-insertion_naive_init.l (100%) rename jsk_2021_fix_kxr/euslisp/{ => cable-insertion}/cable-insertion_naive_insert.l (100%) rename jsk_2021_fix_kxr/euslisp/{ => move_test}/amabe_loosen_screw.l (100%) rename jsk_2021_fix_kxr/euslisp/{ => move_test}/amabe_loosen_screw_pre.l (100%) rename jsk_2021_fix_kxr/euslisp/{ => move_test}/amabe_pick_screw.l (100%) rename jsk_2021_fix_kxr/euslisp/{ => move_test}/amabe_plug.l (100%) rename jsk_2021_fix_kxr/euslisp/{ => move_test}/amabe_screw.l (100%) rename jsk_2021_fix_kxr/euslisp/{ => move_test}/amabe_screw_memo.txt (100%) rename jsk_2021_fix_kxr/euslisp/{ => move_test}/amabe_screw_pre.l (100%) rename jsk_2021_fix_kxr/euslisp/{ => move_test}/amabe_set_kxr.l (100%) rename jsk_2021_fix_kxr/euslisp/{ => move_test}/grasp_demo.l (100%) create mode 100644 jsk_2021_fix_kxr/euslisp/repairdemo/#repairdemo_return.l# diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_init.l b/jsk_2021_fix_kxr/euslisp/#cable-insertion_heuristic_init.l# similarity index 100% rename from jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_init.l rename to jsk_2021_fix_kxr/euslisp/#cable-insertion_heuristic_init.l# diff --git a/jsk_2021_fix_kxr/euslisp/#repairdemo_place.l# b/jsk_2021_fix_kxr/euslisp/#repairdemo_place.l# new file mode 100644 index 0000000000..193a42e5cd --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/#repairdemo_place.l# @@ -0,0 +1,14 @@ +#!/usr/bin/env roseus + +(send *ri* :larm :start-grasp) + +;;before grasp position +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(345 -246.5 1330) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(345 -84.8 1330) :rpy (float-vector 1.83 1.57 -2.86))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :larm :move-end-pos #f(30 0 0)) +(send *robot* :rarm :move-end-pos #f(30 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) diff --git a/jsk_2021_fix_kxr/euslisp/a.out b/jsk_2021_fix_kxr/euslisp/a.out deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion/cable-insertion_heuristic_init.l b/jsk_2021_fix_kxr/euslisp/cable-insertion/cable-insertion_heuristic_init.l new file mode 100755 index 0000000000..0b220ecd15 --- /dev/null +++ b/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) diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_insert.l b/jsk_2021_fix_kxr/euslisp/cable-insertion/cable-insertion_heuristic_insert.l similarity index 100% rename from jsk_2021_fix_kxr/euslisp/cable-insertion_heuristic_insert.l rename to jsk_2021_fix_kxr/euslisp/cable-insertion/cable-insertion_heuristic_insert.l diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l b/jsk_2021_fix_kxr/euslisp/cable-insertion/cable-insertion_naive_grasp.l similarity index 100% rename from jsk_2021_fix_kxr/euslisp/cable-insertion_naive_grasp.l rename to jsk_2021_fix_kxr/euslisp/cable-insertion/cable-insertion_naive_grasp.l diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l b/jsk_2021_fix_kxr/euslisp/cable-insertion/cable-insertion_naive_init.l similarity index 100% rename from jsk_2021_fix_kxr/euslisp/cable-insertion_naive_init.l rename to jsk_2021_fix_kxr/euslisp/cable-insertion/cable-insertion_naive_init.l diff --git a/jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l b/jsk_2021_fix_kxr/euslisp/cable-insertion/cable-insertion_naive_insert.l similarity index 100% rename from jsk_2021_fix_kxr/euslisp/cable-insertion_naive_insert.l rename to jsk_2021_fix_kxr/euslisp/cable-insertion/cable-insertion_naive_insert.l diff --git a/jsk_2021_fix_kxr/euslisp/amabe_loosen_screw.l b/jsk_2021_fix_kxr/euslisp/move_test/amabe_loosen_screw.l similarity index 100% rename from jsk_2021_fix_kxr/euslisp/amabe_loosen_screw.l rename to jsk_2021_fix_kxr/euslisp/move_test/amabe_loosen_screw.l diff --git a/jsk_2021_fix_kxr/euslisp/amabe_loosen_screw_pre.l b/jsk_2021_fix_kxr/euslisp/move_test/amabe_loosen_screw_pre.l similarity index 100% rename from jsk_2021_fix_kxr/euslisp/amabe_loosen_screw_pre.l rename to jsk_2021_fix_kxr/euslisp/move_test/amabe_loosen_screw_pre.l diff --git a/jsk_2021_fix_kxr/euslisp/amabe_pick_screw.l b/jsk_2021_fix_kxr/euslisp/move_test/amabe_pick_screw.l similarity index 100% rename from jsk_2021_fix_kxr/euslisp/amabe_pick_screw.l rename to jsk_2021_fix_kxr/euslisp/move_test/amabe_pick_screw.l diff --git a/jsk_2021_fix_kxr/euslisp/amabe_plug.l b/jsk_2021_fix_kxr/euslisp/move_test/amabe_plug.l similarity index 100% rename from jsk_2021_fix_kxr/euslisp/amabe_plug.l rename to jsk_2021_fix_kxr/euslisp/move_test/amabe_plug.l diff --git a/jsk_2021_fix_kxr/euslisp/amabe_screw.l b/jsk_2021_fix_kxr/euslisp/move_test/amabe_screw.l similarity index 100% rename from jsk_2021_fix_kxr/euslisp/amabe_screw.l rename to jsk_2021_fix_kxr/euslisp/move_test/amabe_screw.l diff --git a/jsk_2021_fix_kxr/euslisp/amabe_screw_memo.txt b/jsk_2021_fix_kxr/euslisp/move_test/amabe_screw_memo.txt similarity index 100% rename from jsk_2021_fix_kxr/euslisp/amabe_screw_memo.txt rename to jsk_2021_fix_kxr/euslisp/move_test/amabe_screw_memo.txt diff --git a/jsk_2021_fix_kxr/euslisp/amabe_screw_pre.l b/jsk_2021_fix_kxr/euslisp/move_test/amabe_screw_pre.l similarity index 100% rename from jsk_2021_fix_kxr/euslisp/amabe_screw_pre.l rename to jsk_2021_fix_kxr/euslisp/move_test/amabe_screw_pre.l diff --git a/jsk_2021_fix_kxr/euslisp/amabe_set_kxr.l b/jsk_2021_fix_kxr/euslisp/move_test/amabe_set_kxr.l similarity index 100% rename from jsk_2021_fix_kxr/euslisp/amabe_set_kxr.l rename to jsk_2021_fix_kxr/euslisp/move_test/amabe_set_kxr.l diff --git a/jsk_2021_fix_kxr/euslisp/grasp_demo.l b/jsk_2021_fix_kxr/euslisp/move_test/grasp_demo.l similarity index 100% rename from jsk_2021_fix_kxr/euslisp/grasp_demo.l rename to jsk_2021_fix_kxr/euslisp/move_test/grasp_demo.l diff --git a/jsk_2021_fix_kxr/euslisp/repairdemo/#repairdemo_return.l# b/jsk_2021_fix_kxr/euslisp/repairdemo/#repairdemo_return.l# new file mode 100644 index 0000000000..eed9b85651 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/repairdemo/#repairdemo_return.l# @@ -0,0 +1,9 @@ +#!/usr/bin/env roseus + +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(345 280 1330) :rpy (float-vector -1.57 1.57 0))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :larm :move-end-pos #f(0 0 200)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) \ No newline at end of file From 08dae46869e62049b3a9f566c90abfe407166cc4 Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 11 Oct 2021 18:31:34 +0900 Subject: [PATCH 039/127] made check-pose.l --- jsk_2021_fix_kxr/euslisp/kxr/check-pose.l | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100644 jsk_2021_fix_kxr/euslisp/kxr/check-pose.l diff --git a/jsk_2021_fix_kxr/euslisp/kxr/check-pose.l b/jsk_2021_fix_kxr/euslisp/kxr/check-pose.l new file mode 100644 index 0000000000..aa12bfa871 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/kxr/check-pose.l @@ -0,0 +1,12 @@ +(load "/home/amabe/prog/rcb4eus/rosrcb4.l") +(make-kxr-robot "kxrl2l5a3h2g") +;;(send *ri* :arm2-open) + +;;(send *ri* :hold :rarm) +;;send *ri* :angle-vector (send *robot* :angle-vector) 100 +;;(send *ri* :timer-on) +;;2番サーボ修理の構え +;;(send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 -90.0 -90.0 0.0 -27.0 -27.0 -90.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) + +;;3番サーボ修理の構え(8番もいけそう) +;;#f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0) From 3b80c9cc3ca8ce25718d537a8ca4b1ed1b6b9fa5 Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 12 Oct 2021 17:00:11 +0900 Subject: [PATCH 040/127] made rarmcoop_test demo --- jsk_2021_fix_kxr/euslisp/kxr/check-pose.l | 36 ++++++++++++++--- .../rarmcoop_test/rarmcoop_test_main.l | 40 +++++++++++++++++++ 2 files changed, 70 insertions(+), 6 deletions(-) create mode 100644 jsk_2021_fix_kxr/euslisp/rarmcoop_test/rarmcoop_test_main.l diff --git a/jsk_2021_fix_kxr/euslisp/kxr/check-pose.l b/jsk_2021_fix_kxr/euslisp/kxr/check-pose.l index aa12bfa871..66dceafdf7 100644 --- a/jsk_2021_fix_kxr/euslisp/kxr/check-pose.l +++ b/jsk_2021_fix_kxr/euslisp/kxr/check-pose.l @@ -1,12 +1,36 @@ (load "/home/amabe/prog/rcb4eus/rosrcb4.l") (make-kxr-robot "kxrl2l5a3h2g") -;;(send *ri* :arm2-open) - +(send *ri* :arm2-open) +(send *ri* :timer-on) +(send *ri* :hold-all) ;;(send *ri* :hold :rarm) -;;send *ri* :angle-vector (send *robot* :angle-vector) 100 -;;(send *ri* :timer-on) + +;;(send *ri* :angle-vector (send *robot* :angle-vector) 100) + ;;2番サーボ修理の構え -;;(send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 -90.0 -90.0 0.0 -27.0 -27.0 -90.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) +#|(send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 -90.0 -90.0 0.0 -27.0 -27.0 -90.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 100) +|# ;;3番サーボ修理の構え(8番もいけそう) -;;#f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0) +#|(send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 100) +|# + +#| 20211012 +ここでケーブルを抜く +(send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 100) + +;pandaが腕をいい位置にもってく + +(send *ri* :free :rarm) + +;pandaがケーブルを挿す + +(send *ri* :hold :rarm) + +;腕が動くことを示す +(send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 -90.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 100) +|# diff --git a/jsk_2021_fix_kxr/euslisp/rarmcoop_test/rarmcoop_test_main.l b/jsk_2021_fix_kxr/euslisp/rarmcoop_test/rarmcoop_test_main.l new file mode 100644 index 0000000000..3b32834818 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/rarmcoop_test/rarmcoop_test_main.l @@ -0,0 +1,40 @@ +#!/usr/bin/env roseus + +(require "package://panda_eus/euslisp/dual_panda-interface.l") + +(dual_panda-init) + +(objects (list *robot*)) + +;setting +(send *ri* :stop-grasp :rarm) +(send *ri* :stop-grasp :larm) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(444 -96.5 1301.6) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(432.17 224.157 1400) :rpy (float-vector -1.57 0.84 1.60))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :rarm) + +;grasp +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(433 170.8 1340.32) :rpy (float-vector -1.57 0.84 1.60))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :larm) +(unix::sleep 1) + +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(433 185.8 1310.22) :rpy (float-vector -1.533 0.56 1.63))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(433 185.8 1280.22) :rpy (float-vector -1.533 0.0 1.63))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *ri* :stop-grasp :larm) + +(send *robot* :larm :move-end-pos #f(-100 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) From b7678522b3dc9a994af06c5e73019a86f6edcc53 Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 20 Oct 2021 16:22:03 +0900 Subject: [PATCH 041/127] wrote some files to packaging --- jsk_2021_fix_kxr/CMakeLists.txt | 10 ++++++++++ jsk_2021_fix_kxr/README.md | 3 +++ jsk_2021_fix_kxr/package.xml | 19 +++++++++++++++++++ 3 files changed, 32 insertions(+) create mode 100644 jsk_2021_fix_kxr/CMakeLists.txt create mode 100644 jsk_2021_fix_kxr/README.md create mode 100644 jsk_2021_fix_kxr/package.xml diff --git a/jsk_2021_fix_kxr/CMakeLists.txt b/jsk_2021_fix_kxr/CMakeLists.txt new file mode 100644 index 0000000000..8b8b06ab23 --- /dev/null +++ b/jsk_2021_fix_kxr/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_2021_fix_kxr) + +find_package(catkin REQUIRED COMPONENTS) + +catkin_package() + +install(DIRECTORY euslisp + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) diff --git a/jsk_2021_fix_kxr/README.md b/jsk_2021_fix_kxr/README.md new file mode 100644 index 0000000000..744d7552e8 --- /dev/null +++ b/jsk_2021_fix_kxr/README.md @@ -0,0 +1,3 @@ +# jsk_2021_fix_kxr + +## 仮置き \ No newline at end of file diff --git a/jsk_2021_fix_kxr/package.xml b/jsk_2021_fix_kxr/package.xml new file mode 100644 index 0000000000..8800410caa --- /dev/null +++ b/jsk_2021_fix_kxr/package.xml @@ -0,0 +1,19 @@ + + + jsk_2021_fix_kxr + 0.0.4 + The jsk_2021_fix_kxr package + + Koki Amabe + + BSD + + catkin + + fetcheus + jsk_maps + pr2eus + + + + From de547061c5aecc528e8cd60fd5e989f60b84f651 Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 20 Oct 2021 18:45:43 +0900 Subject: [PATCH 042/127] made files for apriltag --- .../apriltag_setting/settings.yaml | 11 +++++ jsk_2021_fix_kxr/apriltag_setting/tags.yaml | 47 +++++++++++++++++++ .../apriltag_continuous_detection.launch | 20 ++++++++ 3 files changed, 78 insertions(+) create mode 100644 jsk_2021_fix_kxr/apriltag_setting/settings.yaml create mode 100644 jsk_2021_fix_kxr/apriltag_setting/tags.yaml create mode 100644 jsk_2021_fix_kxr/launch/apriltag_continuous_detection.launch diff --git a/jsk_2021_fix_kxr/apriltag_setting/settings.yaml b/jsk_2021_fix_kxr/apriltag_setting/settings.yaml new file mode 100644 index 0000000000..15859c22a1 --- /dev/null +++ b/jsk_2021_fix_kxr/apriltag_setting/settings.yaml @@ -0,0 +1,11 @@ +# AprilTag 3 code parameters +# Find descriptions in apriltag/include/apriltag.h:struct apriltag_detector +# apriltag/include/apriltag.h:struct apriltag_family +tag_family: 'tagCircle21h7' # 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: 0 # default: 0 +# Other parameters +publish_tf: true # default: false diff --git a/jsk_2021_fix_kxr/apriltag_setting/tags.yaml b/jsk_2021_fix_kxr/apriltag_setting/tags.yaml new file mode 100644 index 0000000000..ba47c25bcf --- /dev/null +++ b/jsk_2021_fix_kxr/apriltag_setting/tags.yaml @@ -0,0 +1,47 @@ +# # 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: 000, size: 0.014, name: test_000}, + ] +# ## 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: + [ + ] diff --git a/jsk_2021_fix_kxr/launch/apriltag_continuous_detection.launch b/jsk_2021_fix_kxr/launch/apriltag_continuous_detection.launch new file mode 100644 index 0000000000..6febd2045e --- /dev/null +++ b/jsk_2021_fix_kxr/launch/apriltag_continuous_detection.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + From cd36bb15d11e3b48588c0830a07be9cafdeca2a2 Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 22 Oct 2021 16:54:49 +0900 Subject: [PATCH 043/127] wrote timeout-joint-list func --- jsk_2021_fix_kxr/euslisp/kxr/#check-pose.l# | 37 +++++++++++++++++++ jsk_2021_fix_kxr/euslisp/kxr/.#check-pose.l | 1 + .../euslisp/kxr/servo-publish_test.l | 11 ++++++ .../panda-kxr_coop/panda-kxr_coop_main.l | 31 ++++++++++++++++ 4 files changed, 80 insertions(+) create mode 100644 jsk_2021_fix_kxr/euslisp/kxr/#check-pose.l# create mode 120000 jsk_2021_fix_kxr/euslisp/kxr/.#check-pose.l create mode 100644 jsk_2021_fix_kxr/euslisp/kxr/servo-publish_test.l create mode 100644 jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l diff --git a/jsk_2021_fix_kxr/euslisp/kxr/#check-pose.l# b/jsk_2021_fix_kxr/euslisp/kxr/#check-pose.l# new file mode 100644 index 0000000000..5916563322 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/kxr/#check-pose.l# @@ -0,0 +1,37 @@ +(load "/home/amabe/prog/rcb4eus/rosrcb4.l") +(make-kxr-robot "kxrl2l5a3h2g") +(send *ri* :arm2-open) +(send *ri* :timer-on) +(send *ri* :hold-all) +;;(send *ri* :hold :rarm) + +;;(send *ri* :angle-vector (send *robot* :angle-vector) 100) + +;;2番サーボ修理の構え +#|(send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 -90.0 -90.0 0.0 -27.0 -27.0 -90.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 100) + +|# + +;;3番サーボ修理の構え(8番もいけそう) +#|(send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 100) +|# + +#| 20211012 +ここでケーブルを抜く +(send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 100) + +;pandaが腕をいい位置にもってく + +(send *ri* :free :rarm) + +;pandaがケーブルを挿す + +(send *ri* :hold :rarm) + +;腕が動くことを示す +(send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 -90.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 100) +|# diff --git a/jsk_2021_fix_kxr/euslisp/kxr/.#check-pose.l b/jsk_2021_fix_kxr/euslisp/kxr/.#check-pose.l new file mode 120000 index 0000000000..d1b2b33e85 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/kxr/.#check-pose.l @@ -0,0 +1 @@ +amabe@ubuntu.1818:1634545184 \ No newline at end of file diff --git a/jsk_2021_fix_kxr/euslisp/kxr/servo-publish_test.l b/jsk_2021_fix_kxr/euslisp/kxr/servo-publish_test.l new file mode 100644 index 0000000000..ab72515e37 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/kxr/servo-publish_test.l @@ -0,0 +1,11 @@ +(load "/home/amabe/prog/rcb4eus/rosrcb4.l") + +(make-kxr-robot "kxrl2l5a3h2g") + +(send *ri* :ros-open) + +(send *ri* :timer-on) + +(send *ri* :hold-all) + +(init-publish) diff --git a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l new file mode 100644 index 0000000000..99f8b3ea55 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l @@ -0,0 +1,31 @@ +#!/usr/bin/env roseus + +(require "package://panda_eus/euslisp/dual_panda-interface.l") + +(ros::roseus-add-msgs "std_msgs") +(ros::roseus-add-msgs "sensor_msgs") +(ros::load-ros-manifest "sensor_msgs") + +(ros::roseus "repair-robot") + +(setq joint_state (one-shot-subscribe "/puppet_joint_states" sensor_msgs::jointstate)) + +;;make list of timeout-servo +(defun make-timeout-servo-list () + (let (timeout-joint-list) + (setq timeout-joint-list '()) + (setq count (- (length (send joint_state :name)) 1)) + (while (> count -1) + (if (< (elt (send joint_state :effort) count) 0.1) + (setq timeout-joint-list (cons (elt (send joint_state :name) count) timeout-joint-list))) + (setq count (- count 1))) + (print timeout-joint-list))) + + + + + + +;;(ros::rate 10) + +;(sensor_msgs::jointstate 1) From 6c497480b9dd1717ff70c49bda76c93f90fb7674 Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 25 Oct 2021 18:18:33 +0900 Subject: [PATCH 044/127] now KXR can move accoding to /panda_order --- jsk_2021_fix_kxr/euslisp/kxr/.#check-pose.l | 1 - .../panda-kxr_coop/panda-kxr_coop_kxr.l | 32 ++++++++++++++ .../panda-kxr_coop/panda-kxr_coop_main.l | 43 +++++++++++++++++-- .../rarmcoop_test/.#rarmcoop_test_main.l | 1 + 4 files changed, 72 insertions(+), 5 deletions(-) delete mode 120000 jsk_2021_fix_kxr/euslisp/kxr/.#check-pose.l create mode 100644 jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l create mode 120000 jsk_2021_fix_kxr/euslisp/rarmcoop_test/.#rarmcoop_test_main.l diff --git a/jsk_2021_fix_kxr/euslisp/kxr/.#check-pose.l b/jsk_2021_fix_kxr/euslisp/kxr/.#check-pose.l deleted file mode 120000 index d1b2b33e85..0000000000 --- a/jsk_2021_fix_kxr/euslisp/kxr/.#check-pose.l +++ /dev/null @@ -1 +0,0 @@ -amabe@ubuntu.1818:1634545184 \ No newline at end of file diff --git a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l new file mode 100644 index 0000000000..b557117e67 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l @@ -0,0 +1,32 @@ +#!/usr/bin/env roseus + +(load "/home/amabe/prog/rcb4eus/rosrcb4.l") +(make-kxr-robot "kxrl2l5a3h2g") +(send *ri* :ros-open) +(send *ri* :timer-on) +(send *ri* :hold-all) +(init-publish) + +(ros::roseus-add-msgs "std_msgs") +(ros::roseus-add-msgs "sensor_msgs") +(ros::load-ros-manifest "std_msgs") +(ros::load-ros-manifest "sensor_msgs") + + +(ros::rate 10) + +(while (ros::ok) + (let (order (instance std_msgs::String :init)) + (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) + (print msg) + (cond ((string-equal msg "RARM-SHOULDER-R") + (progn + (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 -90.0 -90.0 0.0 -27.0 -27.0 -90.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 100))) + ((string-equal msg "RARM-ELBOW-P") + (progn + (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 100)))) + (ros::spin-once) + (ros::sleep))) + diff --git a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l index 99f8b3ea55..55750c60ff 100644 --- a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l +++ b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l @@ -8,7 +8,9 @@ (ros::roseus "repair-robot") -(setq joint_state (one-shot-subscribe "/puppet_joint_states" sensor_msgs::jointstate)) +(ros::advertise "/panda_order" std_msgs::String 1) + +;(setq joint_state (one-shot-subscribe "/puppet_joint_states" sensor_msgs::jointstate)) ;;make list of timeout-servo (defun make-timeout-servo-list () @@ -21,11 +23,44 @@ (setq count (- count 1))) (print timeout-joint-list))) - +(dual_panda-init) + +(objects (list *robot*)) + +;setting +(send *ri* :stop-grasp :rarm) +(send *ri* :stop-grasp :larm) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(444 -96.5 1301.6) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(432.17 224.157 1400) :rpy (float-vector -1.57 0.84 1.60))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :rarm) + + +;rotate KXR's rarm by himself + +;grasp +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(433 170.8 1340.32) :rpy (float-vector -1.57 0.84 1.60))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :larm) +(unix::sleep 1) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(433 185.8 1310.22) :rpy (float-vector -1.533 0.56 1.63))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(433 185.8 1280.22) :rpy (float-vector -1.533 0.0 1.63))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) +(send *ri* :stop-grasp :larm) -;;(ros::rate 10) +(send *robot* :larm :move-end-pos #f(-100 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) -;(sensor_msgs::jointstate 1) +;;move KXR's rarm by himlelf diff --git a/jsk_2021_fix_kxr/euslisp/rarmcoop_test/.#rarmcoop_test_main.l b/jsk_2021_fix_kxr/euslisp/rarmcoop_test/.#rarmcoop_test_main.l new file mode 120000 index 0000000000..b2b5064374 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/rarmcoop_test/.#rarmcoop_test_main.l @@ -0,0 +1 @@ +amabe@ubuntu.20910:1635125893 \ No newline at end of file From f3576fc923acc529bc5de147150f860f237964ff Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 26 Oct 2021 16:11:43 +0900 Subject: [PATCH 045/127] made outline of branch according to timeout servo --- .../panda-kxr_coop/panda-kxr_coop_kxr.l | 50 +++++++++----- .../panda-kxr_coop/panda-kxr_coop_main.l | 66 +++++++++++++++++-- 2 files changed, 93 insertions(+), 23 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l index b557117e67..5479533cb0 100644 --- a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l +++ b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l @@ -12,21 +12,39 @@ (ros::load-ros-manifest "std_msgs") (ros::load-ros-manifest "sensor_msgs") - (ros::rate 10) -(while (ros::ok) - (let (order (instance std_msgs::String :init)) - (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) - (print msg) - (cond ((string-equal msg "RARM-SHOULDER-R") - (progn - (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 -90.0 -90.0 0.0 -27.0 -27.0 -90.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 100))) - ((string-equal msg "RARM-ELBOW-P") - (progn - (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 100)))) - (ros::spin-once) - (ros::sleep))) - +(defun servo-onoff() + (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) + (print msg) + (cond ((string-equal msg "rarm-free") + (progn + (send *ri* :rarm-free))) + ((string-equal msg "larm-free") + (progn + (send *ri* :larm-free))) + ((string-equal msg "rarm-hold") + (progn + (send *ri* :rarm-hold))) + ((string-equal msg "larm-hold") + (progn + (send *ri* :larm-hold))))) + +(setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) +(print msg) + +;指定された姿勢を取る +(cond ((string-equal msg "RARM-SHOULDER-R") + (progn + (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 -90.0 -90.0 0.0 -27.0 -27.0 -90.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 100)) + ((string-equal msg "RARM-ELBOW-P") + (progn + (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 100))))) + +;;指令を受けた後腕のサーボを切る +(servo-onoff) + +;;ケーブルがさせた後腕のサーボon +(servo-onoff) diff --git a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l index 55750c60ff..a7e7068fe9 100644 --- a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l +++ b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l @@ -10,18 +10,16 @@ (ros::advertise "/panda_order" std_msgs::String 1) -;(setq joint_state (one-shot-subscribe "/puppet_joint_states" sensor_msgs::jointstate)) +(defvar *timeout-servo-list*) ;;make list of timeout-servo (defun make-timeout-servo-list () - (let (timeout-joint-list) - (setq timeout-joint-list '()) - (setq count (- (length (send joint_state :name)) 1)) + (setq *timeout-servo-list* '()) + (setq count (- (length (send joint_states :name)) 1)) (while (> count -1) (if (< (elt (send joint_state :effort) count) 0.1) - (setq timeout-joint-list (cons (elt (send joint_state :name) count) timeout-joint-list))) - (setq count (- count 1))) - (print timeout-joint-list))) + (setq *timeout-servo-list* (cons (elt (send joint_states :name) count) *timeout-servo-list*))) + (setq count (- count 1))))) (dual_panda-init) @@ -30,6 +28,59 @@ ;setting (send *ri* :stop-grasp :rarm) (send *ri* :stop-grasp :larm) +(send *robot* :reset-pose) +(send *robot* :rarm :move-end-pos #f(-500 0 0)) +(send *robot* :larm :move-end-pos #f(-500 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(setq joint_states (one-shot-subscribe "/puppet_joint_states" sensor_msgs::jointstate)) + + +(cond ((string-equal (subseq (elt *timeout-servo-list* 0) 0 4) "rarm") + ()) + ((string-equal (subseq (elt *timeout-servo-list* 0) 0 4) "larm") + ())) + +#| +;rarm用に切り替えた直後 + +;rarmを崖に近づける +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(444 -300.5 1400) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(467 -90.157 1310) :rpy (float-vector 0.0 1.501 -3.061))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :larm :move-end-pos #f(10 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :larm) +(unix:sleep 1) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(444 -188.5 1400) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(444 -32.157 1300) :rpy (float-vector 2.341 1.552 -2.369))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :move-end-pos #f(100 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :rarm) +(unix:sleep 2) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(444 -96.5 1300) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(444 60.157 1300) :rpy (float-vector 2.341 1.552 -2.369))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* :stop-grasp :larm) +(unis:sleep 1) + +(send *robot* :larm :move-end-pos #f(-100 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(444 -96.5 1301.6) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(432.17 224.157 1400) :rpy (float-vector -1.57 0.84 1.60))) :translation-axis t :rotation-axis t) @@ -64,3 +115,4 @@ (send *ri* :wait-interpolation) ;;move KXR's rarm by himlelf +|# From 30041746a27c307c84e458196d0482d90ec4b1d7 Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 27 Oct 2021 18:41:22 +0900 Subject: [PATCH 046/127] made and incorporate func to coop --- .../panda-kxr_coop/panda-kxr_coop_kxr.l | 74 ++++++++++-- .../panda-kxr_coop/panda-kxr_coop_main.l | 110 ++++++++++++++++-- 2 files changed, 160 insertions(+), 24 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l index 5479533cb0..dd5d6409ee 100644 --- a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l +++ b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l @@ -12,7 +12,7 @@ (ros::load-ros-manifest "std_msgs") (ros::load-ros-manifest "sensor_msgs") -(ros::rate 10) +(ros::rate 1) (defun servo-onoff() (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) @@ -30,21 +30,71 @@ (progn (send *ri* :larm-hold))))) -(setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) -(print msg) +(defun take-posture() + (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) + (cond ((string-equal msg "RARM-SHOULDER-R") + (progn + (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 -90.0 -90.0 0.0 -27.0 -27.0 -90.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 200) + ((string-equal msg "RARM-ELBOW-P") + (progn + (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 200))))))) + +(defun check-move() + (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) + (cond ((string-equal msg "check-rarm-elbow-p") + (progn + (send *robot* :rarm-elbow-p :joint-angle 40) + (send *ri* :angle-vector (send *robot* :angle-vector) 150) + (unix:sleep 3) + + (send *robot* :rarm-elbow-p :joint-angle -40) + (send *ri* :angle-vector (send *robot* :angle-vector) 150) + (unix:sleep 3) + + (send *robot* :rarm-elbow-p :joint-angle 0) + (send *ri* :angle-vector (send *robot* :angle-vector) 150) + (unix:sleep 3))))) + -;指定された姿勢を取る -(cond ((string-equal msg "RARM-SHOULDER-R") - (progn - (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 -90.0 -90.0 0.0 -27.0 -27.0 -90.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 100)) - ((string-equal msg "RARM-ELBOW-P") - (progn - (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 100))))) +(take-posture) ;;指令を受けた後腕のサーボを切る (servo-onoff) +(setq f 0) +(while (= f 0) + (joint-publish) + (if () + (progn + (send *ri* :rarm-hold) + (send *robot* :rarm-elbow-p :joint-angle 40) + (send *ri* :angle-vector (send *robot* :angle-vector) 150) + (unix:sleep 3) + + (send *robot* :rarm-elbow-p :joint-angle -40) + (send *ri* :angle-vector (send *robot* :angle-vector) 150) + (unix:sleep 3) + + (send *robot* :rarm-elbow-p :joint-angle 0) + (send *ri* :angle-vector (send *robot* :angle-vector) 150) + (unix:sleep 3) + + (if () + (setq f 1)) + ))) + + + + (unix:sleep 1)) + +(while (ros::ok) + (print "hoge") + (ros::sleep)) + ;;ケーブルがさせた後腕のサーボon (servo-onoff) + +;;サーボが生き返ったかの確認 +(check-move) diff --git a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l index a7e7068fe9..e08c6d078b 100644 --- a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l +++ b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l @@ -11,13 +11,15 @@ (ros::advertise "/panda_order" std_msgs::String 1) (defvar *timeout-servo-list*) +(defvar *panda_order*) + +(setq *panda_order* (instance std_msgs::String :init)) -;;make list of timeout-servo (defun make-timeout-servo-list () (setq *timeout-servo-list* '()) (setq count (- (length (send joint_states :name)) 1)) (while (> count -1) - (if (< (elt (send joint_state :effort) count) 0.1) + (if (< (elt (send joint_states :effort) count) 0.1) (setq *timeout-servo-list* (cons (elt (send joint_states :name) count) *timeout-servo-list*))) (setq count (- count 1))))) @@ -25,22 +27,106 @@ (objects (list *robot*)) +(defun reset-pose-high() + (send *ri* :stop-grasp :rarm) + (send *ri* :stop-grasp :larm) + (send *robot* :reset-pose) + (send *robot* :rarm :move-end-pos #f(-500 0 0)) + (send *robot* :larm :move-end-pos #f(-500 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation)) + ;setting -(send *ri* :stop-grasp :rarm) -(send *ri* :stop-grasp :larm) -(send *robot* :reset-pose) -(send *robot* :rarm :move-end-pos #f(-500 0 0)) -(send *robot* :larm :move-end-pos #f(-500 0 0)) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) +(reset-pose-high) (setq joint_states (one-shot-subscribe "/puppet_joint_states" sensor_msgs::jointstate)) +(make-timeout-servo-list) (cond ((string-equal (subseq (elt *timeout-servo-list* 0) 0 4) "rarm") - ()) - ((string-equal (subseq (elt *timeout-servo-list* 0) 0 4) "larm") - ())) + (progn + (reset-pose-high) + (send *panda_order* :data "RARM-SHOULDER-R") + (ros::publish "/panda_order" *panda_order*) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(444 -300.5 1400) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(467 -90.157 1305) :rpy (float-vector 0.0 1.501 -3.061))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *robot* :larm :move-end-pos #f(10 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *ri* :start-grasp :larm) + (unix:sleep 1) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(444 -188.5 1400) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(444 -32.157 1300) :rpy (float-vector 2.341 1.552 -2.369))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 5000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm :move-end-pos #f(100 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *ri* :start-grasp :rarm) + (unix:sleep 2) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(444 -96.5 1300) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(444 60.157 1300) :rpy (float-vector 2.341 1.552 -2.369))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *panda_order* :data "rarm-free") + (ros::publish "/panda_order" *panda_order*) + + ;;wait for insert-cable + + (send *panda_order* :data "rarm-hold") + (ros::publish "/panda_order" *panda_order*) + + (send *ri* :stop-grasp :larm) + (unix:sleep 1) + + (send *robot* :larm :move-end-pos #f(-100 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(444 -96.5 1301.6) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(432.17 234.157 1400) :rpy (float-vector -1.57 0.84 1.60))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(433 180.8 1340.32) :rpy (float-vector -1.57 0.84 1.60))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *ri* :start-grasp :larm) + (unix::sleep 1) + + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(433 195.8 1310.22) :rpy (float-vector -1.533 0.56 1.63))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(433 210.8 1280.22) :rpy (float-vector -1.533 0.0 1.63))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + + + (send *ri* :stop-grasp :larm) + (unix:sleep 1) + + (send *robot* :larm :move-end-pos #f(-100 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation))) + + ((string-equal (subseq (elt *timeout-servo-list* 0) 0 4) "larm") + (progn + (reset-pose-high) + (print "hoge") + (unix:sleep 1)))) #| ;rarm用に切り替えた直後 From 801a371246470825e27c5bcecedb776d74f9d3b7 Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 29 Oct 2021 14:46:25 +0900 Subject: [PATCH 047/127] made panda-kxr-coop demo --- .../panda-kxr_coop/panda-kxr_coop_kxr.l | 74 ++++++++--------- .../panda-kxr_coop/panda-kxr_coop_main.l | 83 ++++++++++++++----- .../euslisp/panda-kxr_coop/subsc_test.l | 33 ++++++++ 3 files changed, 130 insertions(+), 60 deletions(-) create mode 100644 jsk_2021_fix_kxr/euslisp/panda-kxr_coop/subsc_test.l diff --git a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l index dd5d6409ee..4f54608ed8 100644 --- a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l +++ b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l @@ -7,13 +7,13 @@ (send *ri* :hold-all) (init-publish) +(ros::roseus "kxr_robot") + (ros::roseus-add-msgs "std_msgs") (ros::roseus-add-msgs "sensor_msgs") (ros::load-ros-manifest "std_msgs") (ros::load-ros-manifest "sensor_msgs") -(ros::rate 1) - (defun servo-onoff() (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) (print msg) @@ -35,11 +35,11 @@ (cond ((string-equal msg "RARM-SHOULDER-R") (progn (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 -90.0 -90.0 0.0 -27.0 -27.0 -90.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 200) - ((string-equal msg "RARM-ELBOW-P") - (progn - (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 200))))))) + (send *ri* :angle-vector (send *robot* :angle-vector) 200))) + ((string-equal msg "RARM-ELBOW-P") + (progn + (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 200))))) (defun check-move() (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) @@ -57,44 +57,38 @@ (send *ri* :angle-vector (send *robot* :angle-vector) 150) (unix:sleep 3))))) +(joint-publish) (take-posture) -;;指令を受けた後腕のサーボを切る (servo-onoff) (setq f 0) (while (= f 0) - (joint-publish) - (if () - (progn - (send *ri* :rarm-hold) - (send *robot* :rarm-elbow-p :joint-angle 40) - (send *ri* :angle-vector (send *robot* :angle-vector) 150) - (unix:sleep 3) - - (send *robot* :rarm-elbow-p :joint-angle -40) - (send *ri* :angle-vector (send *robot* :angle-vector) 150) - (unix:sleep 3) - - (send *robot* :rarm-elbow-p :joint-angle 0) - (send *ri* :angle-vector (send *robot* :angle-vector) 150) - (unix:sleep 3) - - (if () - (setq f 1)) - ))) - - - - (unix:sleep 1)) - -(while (ros::ok) - (print "hoge") - (ros::sleep)) - -;;ケーブルがさせた後腕のサーボon -(servo-onoff) + (print "waiting for panda_order") + (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) + (print "got msg") + (print msg) + (unix:sleep 3) + (cond ((string-equal msg "ope-finish") + (joint-publish)) + + ((string-equal msg "check-rarm") + (progn + (send *ri* :rarm-hold) + (send *robot* :rarm-elbow-p :joint-angle 40) + (send *ri* :angle-vector (send *robot* :angle-vector) 150) + (unix:sleep 2) + + (send *robot* :rarm-elbow-p :joint-angle -40) + (send *ri* :angle-vector (send *robot* :angle-vector) 150) + (unix:sleep 2) + + (send *robot* :rarm-elbow-p :joint-angle 0) + (send *ri* :angle-vector (send *robot* :angle-vector) 150) + (unix:sleep 2) + (print "rarmmove"))) + ((string-equal msg "ope-success") + (setq f 1))) + (unix:sleep 3)) -;;サーボが生き返ったかの確認 -(check-move) diff --git a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l index e08c6d078b..60d6259499 100644 --- a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l +++ b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l @@ -11,17 +11,21 @@ (ros::advertise "/panda_order" std_msgs::String 1) (defvar *timeout-servo-list*) +(defvar *reparing-servo-num*) (defvar *panda_order*) + (setq *panda_order* (instance std_msgs::String :init)) (defun make-timeout-servo-list () (setq *timeout-servo-list* '()) (setq count (- (length (send joint_states :name)) 1)) - (while (> count -1) - (if (< (elt (send joint_states :effort) count) 0.1) - (setq *timeout-servo-list* (cons (elt (send joint_states :name) count) *timeout-servo-list*))) - (setq count (- count 1))))) + (while (> count -1) + (if (< (elt (send joint_states :effort) count) 0.1) + (progn + (setq *timeout-servo-list* (cons (elt (send joint_states :name) count) *timeout-servo-list*)) + (setq *reparing-servo-num* count))) + (setq count (- count 1)))) (dual_panda-init) @@ -46,8 +50,11 @@ (cond ((string-equal (subseq (elt *timeout-servo-list* 0) 0 4) "rarm") (progn (reset-pose-high) - (send *panda_order* :data "RARM-SHOULDER-R") + (send *panda_order* :data "RARM-ELBOW-P") (ros::publish "/panda_order" *panda_order*) + (ros::spin-once) + + (unix:sleep 3) (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(444 -300.5 1400) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(467 -90.157 1305) :rpy (float-vector 0.0 1.501 -3.061))) :translation-axis t :rotation-axis t) @@ -78,14 +85,6 @@ (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) - (send *panda_order* :data "rarm-free") - (ros::publish "/panda_order" *panda_order*) - - ;;wait for insert-cable - - (send *panda_order* :data "rarm-hold") - (ros::publish "/panda_order" *panda_order*) - (send *ri* :stop-grasp :larm) (unix:sleep 1) @@ -113,23 +112,67 @@ (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) + (send *panda_order* :data "rarm-free") + (ros::publish "/panda_order" *panda_order*) + (unix:sleep 5) + ;;wait for insert-cable + (print "cable-inserting") + (unix:sleep 10) + (send *panda_order* :data "ope-finish") + (ros::publish "/panda_order" *panda_order*) + (print "ope-finish") + + (setq f 0) + (while (= f 0) + (print "waiting for joint_states") + (setq joint_states (one-shot-subscribe "/puppet_joint_states" sensor_msgs::jointstate)) + (if (> (elt (send joint_states :effort) *reparing-servo-num*) 0.1) + (progn + (setq f 1)))) + + ;;check effort (send *ri* :stop-grasp :larm) (unix:sleep 1) (send *robot* :larm :move-end-pos #f(-100 0 0)) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) - (send *ri* :wait-interpolation))) - - ((string-equal (subseq (elt *timeout-servo-list* 0) 0 4) "larm") - (progn - (reset-pose-high) - (print "hoge") - (unix:sleep 1)))) + (send *ri* :wait-interpolation) + + (send *panda_order* :data "rarm-hold") + (ros::publish "/panda_order" *panda_order*) + (unix:sleep 7) + + (send *panda_order* :data "check-rarm") + (ros::publish "/panda_order" *panda_order*) + (unix:sleep 13) + + (send *panda_order* :data "ope-success") + (ros::publish "/panda_order" *panda_order*) + )) + + ((string-equal (subseq (elt *timeout-servo-list* 0) 0 4) "larm") + (progn + (reset-pose-high) + (print "hoge") + (unix:sleep 1)))) #| ;rarm用に切り替えた直後 +(setq f 0) +(while (< f 100) + (send *panda_order* :data "tte") + (ros::publish "/panda_order" *panda_order*) + (ros::spin-once) + (setq f (+ 1 f)) + (print "huga")) + +(do-until-key + (send *panda_order* :data "tte") + (ros::publish "/panda_order" *panda_order*) + (ros::spin-once) + (print "huga")) ;rarmを崖に近づける (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(444 -300.5 1400) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) diff --git a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/subsc_test.l b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/subsc_test.l new file mode 100644 index 0000000000..1241ba15e1 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/subsc_test.l @@ -0,0 +1,33 @@ +#!/usr/bin/env roseus + +(ros::roseus "kxr_robot") + +(ros::roseus-add-msgs "std_msgs") +(ros::roseus-add-msgs "sensor_msgs") +(ros::load-ros-manifest "std_msgs") +(ros::load-ros-manifest "sensor_msgs") + +(defvar *panda_order*) + +(defun order-cb (msg) + (let ((order (instance std_msgs::String :init) hoge)) + (setq hoge (send msg :data)) + (setq *panda_order* (send msg :data)) + (ros::ros-info "~A" hoge))) + +(ros::subscribe "/panda_order" std_msgs::String #'order-cb) + +(ros::rate 1) +(do-until-key + (ros::subscribe "/panda_order" std_msgs::String #'order-cb) + (print "hoge") + (print *panda_order*) + (ros::spin-once) + (ros::sleep)) + +(while (ros::ok) + (ros::subscribe "/panda_order" std_msgs::String #'order-cb) + (print "hoge") + (print *panda_order*) + (ros::spin-once) + (ros::sleep)) From 723352c0e918df869ae5734d813372ba2f627238 Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 1 Nov 2021 12:07:15 +0900 Subject: [PATCH 048/127] add larm mode --- .../panda-kxr_coop/#panda-kxr_coop_kxr.l# | 98 +++++++++++++++++++ .../panda-kxr_coop/.#panda-kxr_coop_kxr.l | 1 + .../panda-kxr_coop/panda-kxr_coop_kxr.l | 4 + .../panda-kxr_coop/panda-kxr_coop_main.l | 20 +++- 4 files changed, 121 insertions(+), 2 deletions(-) create mode 100644 jsk_2021_fix_kxr/euslisp/panda-kxr_coop/#panda-kxr_coop_kxr.l# create mode 120000 jsk_2021_fix_kxr/euslisp/panda-kxr_coop/.#panda-kxr_coop_kxr.l diff --git a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/#panda-kxr_coop_kxr.l# b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/#panda-kxr_coop_kxr.l# new file mode 100644 index 0000000000..dc98b2c2f7 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/#panda-kxr_coop_kxr.l# @@ -0,0 +1,98 @@ +#!/usr/bin/env roseus + +(load "/home/amabe/prog/rcb4eus/rosrcb4.l") +(make-kxr-robot "kxrl2l5a3h2g") +(send *ri* :ros-open) +(send *ri* :timer-on) +(send *ri* :hold-all) +(init-publish) + +(ros::roseus "kxr_robot") + +(ros::roseus-add-msgs "std_msgs") +(ros::roseus-add-msgs "sensor_msgs") +(ros::load-ros-manifest "std_msgs") +(ros::load-ros-manifest "sensor_msgs") + +(defun servo-onoff() + (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) + (print msg) + (cond ((string-equal msg "rarm-free") + (progn + (send *ri* :rarm-free))) + ((string-equal msg "larm-free") + (progn + (send *ri* :larm-free))) + ((string-equal msg "rarm-hold") + (progn + (send *ri* :rarm-hold))) + ((string-equal msg "larm-hold") + (progn + (send *ri* :larm-hold))))) + +(defun take-posture() + (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) + (cond ((string-equal msg "RARM-SHOULDER-R") + (progn + (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 -90.0 -90.0 0.0 -27.0 -27.0 -90.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 200))) + ((string-equal msg "RARM-ELBOW-P") + (progn + (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 200))) + ((string-equal msg "LARM-ELBOW-P") + (progn + (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 200))))) + +(defun check-move() + (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) + (cond ((string-equal msg "check-rarm-elbow-p") + (progn + (send *robot* :rarm-elbow-p :joint-angle 40) + (send *ri* :angle-vector (send *robot* :angle-vector) 150) + (unix:sleep 3) + + (send *robot* :rarm-elbow-p :joint-angle -40) + (send *ri* :angle-vector (send *robot* :angle-vector) 150) + (unix:sleep 3) + + (send *robot* :rarm-elbow-p :joint-angle 0) + (send *ri* :angle-vector (send *robot* :angle-vector) 150) + (unix:sleep 3))))) + +(joint-publish) + +(take-posture) + +(servo-onoff) + +(setq f 0) +(while (= f 0) + (print "waiting for panda_order") + (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) + (print "got msg") + (print msg) + (unix:sleep 3) + (cond ((string-equal msg "ope-finish") + (joint-publish)) + + ((string-equal msg "check-rarm") + (progn + (send *ri* :rarm-hold) + (send *robot* :rarm-elbow-p :joint-angle 40) + (send *ri* :angle-vector (send *robot* :angle-vector) 150) + (unix:sleep 2) + + (send *robot* :rarm-elbow-p :joint-angle -40) + (send *ri* :angle-vector (send *robot* :angle-vector) 150) + (unix:sleep 2) + + (send *robot* :rarm-elbow-p :joint-angle 0) + (send *ri* :angle-vector (send *robot* :angle-vector) 150) + (unix:sleep 2) + (print "rarmmove"))) + ((string-equal msg "ope-success") + (setq f 1))) + (unix:sleep 3)) + diff --git a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/.#panda-kxr_coop_kxr.l b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/.#panda-kxr_coop_kxr.l new file mode 120000 index 0000000000..ef8ff7e4d8 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/.#panda-kxr_coop_kxr.l @@ -0,0 +1 @@ +amabe@ubuntu.31379:1635728622 \ No newline at end of file diff --git a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l index 4f54608ed8..dc98b2c2f7 100644 --- a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l +++ b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_kxr.l @@ -37,6 +37,10 @@ (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 -90.0 -90.0 0.0 -27.0 -27.0 -90.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) (send *ri* :angle-vector (send *robot* :angle-vector) 200))) ((string-equal msg "RARM-ELBOW-P") + (progn + (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 200))) + ((string-equal msg "LARM-ELBOW-P") (progn (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) (send *ri* :angle-vector (send *robot* :angle-vector) 200))))) diff --git a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l index 60d6259499..885d65d878 100644 --- a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l +++ b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/panda-kxr_coop_main.l @@ -155,8 +155,24 @@ ((string-equal (subseq (elt *timeout-servo-list* 0) 0 4) "larm") (progn (reset-pose-high) - (print "hoge") - (unix:sleep 1)))) + (send *panda_order* :data "LARM-ELBOW-P") + (ros::publish "/panda_order" *panda_order*) + (ros::spin-once) + + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(276 -125.5 1320) :rpy (float-vector -2.97 1.545 0.16))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *robot* :larm :move-end-pos #f(25 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *ri* :start-grasp :larm) + (unix:sleep 2) + + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(443 114.5 1300) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 5000) + (send *ri* :wait-interpolation)))) #| ;rarm用に切り替えた直後 From 18f5f35557c9b02b20ca03d584dc89cc2bed2b0f Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 1 Nov 2021 16:35:59 +0900 Subject: [PATCH 049/127] changed kxr.l --- .../panda-kxr_coop/#panda-kxr_coop_kxr.l# | 98 ------------------- .../panda-kxr_coop/.#panda-kxr_coop_kxr.l | 1 - 2 files changed, 99 deletions(-) delete mode 100644 jsk_2021_fix_kxr/euslisp/panda-kxr_coop/#panda-kxr_coop_kxr.l# delete mode 120000 jsk_2021_fix_kxr/euslisp/panda-kxr_coop/.#panda-kxr_coop_kxr.l diff --git a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/#panda-kxr_coop_kxr.l# b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/#panda-kxr_coop_kxr.l# deleted file mode 100644 index dc98b2c2f7..0000000000 --- a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/#panda-kxr_coop_kxr.l# +++ /dev/null @@ -1,98 +0,0 @@ -#!/usr/bin/env roseus - -(load "/home/amabe/prog/rcb4eus/rosrcb4.l") -(make-kxr-robot "kxrl2l5a3h2g") -(send *ri* :ros-open) -(send *ri* :timer-on) -(send *ri* :hold-all) -(init-publish) - -(ros::roseus "kxr_robot") - -(ros::roseus-add-msgs "std_msgs") -(ros::roseus-add-msgs "sensor_msgs") -(ros::load-ros-manifest "std_msgs") -(ros::load-ros-manifest "sensor_msgs") - -(defun servo-onoff() - (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) - (print msg) - (cond ((string-equal msg "rarm-free") - (progn - (send *ri* :rarm-free))) - ((string-equal msg "larm-free") - (progn - (send *ri* :larm-free))) - ((string-equal msg "rarm-hold") - (progn - (send *ri* :rarm-hold))) - ((string-equal msg "larm-hold") - (progn - (send *ri* :larm-hold))))) - -(defun take-posture() - (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) - (cond ((string-equal msg "RARM-SHOULDER-R") - (progn - (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 -90.0 -90.0 0.0 -27.0 -27.0 -90.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 200))) - ((string-equal msg "RARM-ELBOW-P") - (progn - (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 200))) - ((string-equal msg "LARM-ELBOW-P") - (progn - (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 200))))) - -(defun check-move() - (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) - (cond ((string-equal msg "check-rarm-elbow-p") - (progn - (send *robot* :rarm-elbow-p :joint-angle 40) - (send *ri* :angle-vector (send *robot* :angle-vector) 150) - (unix:sleep 3) - - (send *robot* :rarm-elbow-p :joint-angle -40) - (send *ri* :angle-vector (send *robot* :angle-vector) 150) - (unix:sleep 3) - - (send *robot* :rarm-elbow-p :joint-angle 0) - (send *ri* :angle-vector (send *robot* :angle-vector) 150) - (unix:sleep 3))))) - -(joint-publish) - -(take-posture) - -(servo-onoff) - -(setq f 0) -(while (= f 0) - (print "waiting for panda_order") - (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) - (print "got msg") - (print msg) - (unix:sleep 3) - (cond ((string-equal msg "ope-finish") - (joint-publish)) - - ((string-equal msg "check-rarm") - (progn - (send *ri* :rarm-hold) - (send *robot* :rarm-elbow-p :joint-angle 40) - (send *ri* :angle-vector (send *robot* :angle-vector) 150) - (unix:sleep 2) - - (send *robot* :rarm-elbow-p :joint-angle -40) - (send *ri* :angle-vector (send *robot* :angle-vector) 150) - (unix:sleep 2) - - (send *robot* :rarm-elbow-p :joint-angle 0) - (send *ri* :angle-vector (send *robot* :angle-vector) 150) - (unix:sleep 2) - (print "rarmmove"))) - ((string-equal msg "ope-success") - (setq f 1))) - (unix:sleep 3)) - diff --git a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/.#panda-kxr_coop_kxr.l b/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/.#panda-kxr_coop_kxr.l deleted file mode 120000 index ef8ff7e4d8..0000000000 --- a/jsk_2021_fix_kxr/euslisp/panda-kxr_coop/.#panda-kxr_coop_kxr.l +++ /dev/null @@ -1 +0,0 @@ -amabe@ubuntu.31379:1635728622 \ No newline at end of file From e38e9cdc0f94d4dd37ecb7afd8783e494812b222 Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 29 Nov 2021 15:59:00 +0900 Subject: [PATCH 050/127] made apriltag_dual_panda_second.launch --- jsk_2021_fix_kxr/CMakeLists.txt | 24 +- jsk_2021_fix_kxr/euslisp/#amabe_pick_screw.l# | 1172 ----------------- jsk_2021_fix_kxr/euslisp/#amabe_screw.l# | 38 - jsk_2021_fix_kxr/euslisp/#amabe_set_kxr.l# | 73 - jsk_2021_fix_kxr/euslisp/#repairdemo_place.l# | 14 - .../euslisp/maker_test/.#marker_point.l | 1 + .../euslisp/maker_test/marker_point.l | 13 + .../rarmcoop_test/.#rarmcoop_test_main.l | 1 - .../launch/apriltag_dual_panda_second.launch | 20 + jsk_2021_fix_kxr/msg/Repair.msg | 4 + jsk_2021_fix_kxr/package.xml | 9 + 11 files changed, 69 insertions(+), 1300 deletions(-) delete mode 100644 jsk_2021_fix_kxr/euslisp/#amabe_pick_screw.l# delete mode 100644 jsk_2021_fix_kxr/euslisp/#amabe_screw.l# delete mode 100644 jsk_2021_fix_kxr/euslisp/#amabe_set_kxr.l# delete mode 100644 jsk_2021_fix_kxr/euslisp/#repairdemo_place.l# create mode 120000 jsk_2021_fix_kxr/euslisp/maker_test/.#marker_point.l create mode 100644 jsk_2021_fix_kxr/euslisp/maker_test/marker_point.l delete mode 120000 jsk_2021_fix_kxr/euslisp/rarmcoop_test/.#rarmcoop_test_main.l create mode 100644 jsk_2021_fix_kxr/launch/apriltag_dual_panda_second.launch create mode 100644 jsk_2021_fix_kxr/msg/Repair.msg diff --git a/jsk_2021_fix_kxr/CMakeLists.txt b/jsk_2021_fix_kxr/CMakeLists.txt index 8b8b06ab23..866ac21724 100644 --- a/jsk_2021_fix_kxr/CMakeLists.txt +++ b/jsk_2021_fix_kxr/CMakeLists.txt @@ -1,9 +1,29 @@ cmake_minimum_required(VERSION 2.8.3) project(jsk_2021_fix_kxr) -find_package(catkin REQUIRED COMPONENTS) +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + message_generation + ) -catkin_package() +add_message_files( + FILES + Repair.msg + ) + +generate_messages( + DEPENDENCIES + std_msgs + ) + +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES ros_lecture +CATKIN_DEPENDS roscpp rospy std_msgs message_runtime +# DEPENDS system_lib +) install(DIRECTORY euslisp DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} diff --git a/jsk_2021_fix_kxr/euslisp/#amabe_pick_screw.l# b/jsk_2021_fix_kxr/euslisp/#amabe_pick_screw.l# deleted file mode 100644 index 91b4af2a30..0000000000 --- a/jsk_2021_fix_kxr/euslisp/#amabe_pick_screw.l# +++ /dev/null @@ -1,1172 +0,0 @@ -amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ source ~/franka_ws/devel/setup.bash -amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ rossetmaster 133.11.216.227 -set ROS_MASTER_URI to http://133.11.216.227:11311 -[http://133.11.216.227:11311][] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ rossetip -set ROS_IP and ROS_HOSTNAME to 133.11.216.79 -[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus -configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" -;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin -connected to Xserver DISPLAY=:0 -X events are being asynchronously monitored. -;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb -;; extending gcstack 0x564ea20ad690[16374] --> 0x564ea2533600[32748] top=3c74 -irtgl irtglc irtviewer -EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) -roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) -eustf roseus_c_util 1.irteusgl$ roseus -configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" -;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin -connected to Xserver DISPLAY=:0 -X events are being asynchronously monitored. -;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb -;; extending gcstack 0x555e813e1690[16374] --> 0x555e81867600[32748] top=3c74 -irtgl irtglc irtviewer -EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020)roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) -eustf roseus_c_util 1.irteusgl$(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*)) -:|PACKAGE://PANDA_EUS/EUSLISP/DUAL_PANDA-INTERFACE.L| -2.irteusgl$ nil -2.irteusgl$ nil -3.irteusgl$ nil -4.irteusgl$ nil -4.irteusgl$ [ERROR] [1623044935.914571433]: [registerPublisher] Failed to contact master at [133.11.216.227:11311]. Retrying... - C-c C-c[ INFO] [1623044976.404423882]: Connected to master at [133.11.216.227:11311] -interrupt5.B1-irteusgl$ reset -6.irteusgl$ (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*)) -nil -7.irteusgl$ nil -7.irteusgl$ nil -8.irteusgl$ nil -9.irteusgl$ nil -9.irteusgl$ [ WARN] [1623044992.517489268]: Waiting for actionlib interface forever because controller-timeout is nil -[ERROR] [1623044992.792812777]: unknown ros::param::get, key=/gazebo/time_step -[ WARN] [1623044992.792957261]: real franka environemt interface is not impelemented -# -10.irteusgl$ nil -10.irteusgl$ ;; (make-irtviewer) executed -(#) -11.irteusgl$ (send *ri* :start-grasp :rarm) -nil -12.irteusgl$ (send *ri* :start-grasp :rarm) -nil -13.irteusgl$ (send *ri* :start-grasp :rarm) -nil -14.irteusgl$ (send *ri* :stop-grasp :rarm) -nil -15.irteusgl$ (send *ri* :start-grasp :rarm) -nil -16.irteusgl$ (send *ri* :stop-grasp :rarm) -nil -17.irteusgl$ (send *ri* :stop-grasp :rarm) -nil -18.irteusgl$ (send *ri* :start-grasp :rarm) -(send *ri* :stop-grasp :rarm) -nil -19.irteusgl$ nil -20.irteusgl$ (send *ri* :start-grasp :rarm) -(send *ri* :wait-interpolation) -(send *ri* :stop-grasp :rarm) -nil -21.irteusgl$ [ERROR] [1623045257.212841572]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] :wait-for-result (return nil when no goal exists) -(nil) -22.irteusgl$ nil -23.irteusgl$ (send *ri* :start-grasp :rarm) -(send *ri* :wait-interpolation) -(send *ri* :stop-grasp :rarm) -nil -24.irteusgl$ [ERROR] [1623045276.931426625]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] :wait-for-result (return nil when no goal exists) -(nil) -25.irteusgl$ nil -26.irteusgl$ (send *ri* :start-grasp :rarm) -(unix::sleep 2) -(send *ri* :stop-grasp :rarm) -nil -27.irteusgl$ t -28.irteusgl$ nil -29.irteusgl$ (send *ri* :start-grasp :rarm) -(unix::sleep 2) -(send *ri* :stop-grasp :rarm) -nil -30.irteusgl$ t -31.irteusgl$ nil -32.irteusgl$ (send *ri* :start-grasp :rarm) -(unix::sleep 2) -(send *ri* :stop-grasp :rarm) -nil -33.irteusgl$ t -34.irteusgl$ nil -35.irteusgl$ (send *ri* :start-grasp :rarm :effort 100) -(unix::sleep 2) -(send *ri* :stop-grasp :rarm) -nil -36.irteusgl$ t -37.irteusgl$ nil -38.irteusgl$ (send *ri* :start-grasp :rarm :effort 100) -(unix::sleep 2) -(send *ri* :stop-grasp :rarm) -nil -39.irteusgl$ t -40.irteusgl$ nil -41.irteusgl$ (send *ri* :start-grasp :rarm) -(unix::sleep 1) -(send *ri* :start-grasp :rarm) -(unix::sleep 1) -(send *ri* :start-grasp :rarm) -(unix::sleep 1) -nil -42.irteusgl$ t -43.irteusgl$ nil -44.irteusgl$ t -45.irteusgl$ nil -46.irteusgl$ t -47.irteusgl$ (send *ri* :stop-grasp :rarm) -nil -48.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 1))) :rotation-axis t) -#f(10.1933 13.065 23.5346 -96.2821 -5.44666 108.256 -68.9385 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) -49.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 1.57))) :rotation-axis t) -#f(12.7212 12.8301 20.7004 -96.3012 -4.74834 108.314 -36.7336 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) -50.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 0))) :rotation-axis t) -#f(2.83096 13.9768 31.8934 -96.2379 -7.71685 108.065 -124.769 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) -51.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0 1.57 0))) :rotation-axis t) -#f(5.04567 13.6681 29.3727 -96.2492 -7.03154 108.124 -99.0868 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) -52.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0 1.57 1.57))) :rotation-axis t) -#f(12.1963 12.88 21.296 -96.2915 -4.91815 108.294 -10.4985 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) -53.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0 1.57 0))) :rotation-axis t) -#f(3.99637 13.813 30.569 -96.2402 -7.36409 108.091 -98.8717 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) -54.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 1.57 1.57 0))) :rotation-axis t) -#f(-31.8789 45.9122 87.7798 -93.3033 -45.9812 93.9305 -166.003 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) -55.irteusgl$ (send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(-31.8789 45.9122 87.7798 -93.3033 -45.9812 93.9305 -166.003 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) -56.irteusgl$ (nil) -57.irteusgl$ (send *robot* :reset-pose) -(send *robot* :rarm :move-end-pos #f(-500 0 0)) -(send *robot* :larm :move-end-pos #f(-500 0 0)) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -#f(0.0 15.0 0.0 -135.0 0.0 150.0 45.0 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) -58.irteusgl$ nil -58.irteusgl$ #f(-0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) -59.irteusgl$ nil -59.irteusgl$ #f(-0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -60.irteusgl$ nil -60.irteusgl$ #f(-0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -61.irteusgl$ quit -nil -61.irteusgl$ [ INFO] [1623046201.935916968]: cell* ROSEUS_EXIT(context*, int, cell**) -[ INFO] [1623046201.936018253]: exiting roseus 0 -2 -2.irteusgl$ quit -[ INFO] [1623046246.936411950]: cell* ROSEUS_EXIT(context*, int, cell**) -[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus -configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" -;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin -connected to Xserver DISPLAY=:0 -X events are being asynchronously monitored. -;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb -;; extending gcstack 0x56522c44e690[16374] --> 0x56522c8d4600[32748] top=3c74 -irtgl irtglc irtviewer -EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) -roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) -eustf roseus_c_util 1.irteusgl$ (load "amabe_pick_screw.l") -[ WARN] [1623046306.391917094]: Waiting for actionlib interface forever because controller-timeout is nil -[ERROR] [1623046306.934896202]: unknown ros::param::get, key=/gazebo/time_step -[ WARN] [1623046306.935059904]: real franka environemt interface is not impelemented -;; (make-irtviewer) executed -t -2.irteusgl$(load "amabe_pick_screw.l") -[ WARN] [1623046351.177453286]: topic /robot_interface_marker_array already advertised -[ WARN] [1623046351.391899966]: topic /dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory/goal already advertised -[ WARN] [1623046351.392077732]: topic /dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory/cancel already advertised -[ WARN] [1623046351.923439229]: Waiting for actionlib interface forever because controller-timeout is nil -[ERROR] [1623046352.155330580]: unknown ros::param::get, key=/gazebo/time_step -[ WARN] [1623046352.155570732]: real franka environemt interface is not impelemented -[ WARN] [1623046352.353335420]: topic /dual_panda/error_recovery/goal already advertised -[ WARN] [1623046352.353492482]: topic /dual_panda/error_recovery/cancel already advertised -[ WARN] [1623046352.703279913]: topic /dual_panda/rarm/franka_gripper/grasp/goal already advertised -[ WARN] [1623046352.703393498]: topic /dual_panda/rarm/franka_gripper/grasp/cancel already advertised -[ WARN] [1623046353.049249524]: topic /dual_panda/rarm/franka_gripper/homing/goal already advertised -[ WARN] [1623046353.049391117]: topic /dual_panda/rarm/franka_gripper/homing/cancel already advertised -[ WARN] [1623046353.357959211]: topic /dual_panda/rarm/franka_gripper/move/goal already advertised -[ WARN] [1623046353.358077884]: topic /dual_panda/rarm/franka_gripper/move/cancel already advertised -[ WARN] [1623046353.732909860]: topic /dual_panda/rarm/franka_gripper/stop/goal already advertised -[ WARN] [1623046353.733026452]: topic /dual_panda/rarm/franka_gripper/stop/cancel already advertised -[ WARN] [1623046354.242373960]: topic /dual_panda/larm/franka_gripper/grasp/goal already advertised -[ WARN] [1623046354.242412283]: topic /dual_panda/larm/franka_gripper/grasp/cancel already advertised -[ WARN] [1623046354.668871498]: topic /dual_panda/larm/franka_gripper/homing/goal already advertised -[ WARN] [1623046354.668909799]: topic /dual_panda/larm/franka_gripper/homing/cancel already advertised -[ WARN] [1623046355.011662932]: topic /dual_panda/larm/franka_gripper/move/goal already advertised -[ WARN] [1623046355.011808590]: topic /dual_panda/larm/franka_gripper/move/cancel already advertised -[ WARN] [1623046355.332269291]: topic /dual_panda/larm/franka_gripper/stop/goal already advertised -[ WARN] [1623046355.332393882]: topic /dual_panda/larm/franka_gripper/stop/cancel already advertised -[ WARN] [1623046359.953247266]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -t -3.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) -#f(19.7508 12.3665 12.8659 -96.3179 -2.90629 108.4 -11.8014 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -4.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 1.57 1.57 0))) :rotation-axis t) -#f(-31.9482 45.8785 87.7429 -93.3317 -45.9629 93.967 -166.003 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -5.irteusgl$ (send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(-31.9482 45.8785 87.7429 -93.3317 -45.9629 93.967 -166.003 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -6.irteusgl$ (nil) -7.irteusgl$ (send *ri* :start-grasp :rarm) -nil -8.irteusgl$ (send *robot* :angle-vector) -#f(-31.9482 45.8785 87.7429 -93.3317 -45.9629 93.967 -166.003 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -9.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) -#f(-19.7126 21.2167 59.6165 -95.7379 -19.0069 106.006 -1.50889 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -10.irteusgl$ (send *ri* :stop-grasp :rarm) -nil -11.irteusgl$ (send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(-19.7126 21.2167 59.6165 -95.7379 -19.0069 106.006 -1.50889 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -12.irteusgl$ (nil) -13.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 2.35))) :rotation-axis t) -#f(-13.939 18.2742 51.943 -95.9608 -14.9859 106.944 40.6532 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -14.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) -#f(-7.96828 16.2458 44.5598 -96.0966 -11.9054 107.503 83.9547 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -15.irteusgl$ (send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(-7.96828 16.2458 44.5598 -96.0966 -11.9054 107.503 83.9547 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -16.irteusgl$ (nil) -17.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) -#f(-7.81748 16.6588 44.2003 -97.7981 -12.2703 109.523 84.2145 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -18.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 0))) :rotation-axis t) -#f(-26.1478 27.5223 69.1118 -96.9075 -26.6605 105.74 -86.6893 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -19.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) -#f(-3.65123 15.6057 39.2723 -97.8583 -10.4619 109.786 83.0507 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -20.irteusgl$ (send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(-3.65123 15.6057 39.2723 -97.8583 -10.4619 109.786 83.0507 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -21.irteusgl$ (nil) -22.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 0))) :rotation-axis t) -#f(-22.155 23.4973 62.7751 -97.2663 -21.8138 107.333 -89.6592 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -23.irteusgl$ (send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(-22.155 23.4973 62.7751 -97.2663 -21.8138 107.333 -89.6592 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -24.irteusgl$ (nil) -25.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) -#f(-0.214011 14.906 35.2808 -97.9034 -9.12829 109.964 82.1914 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -26.irteusgl$ (send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(-0.214011 14.906 35.2808 -97.9034 -9.12829 109.964 82.1914 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -27.irteusgl$ (nil) -28.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) -#f(-0.11189 14.8888 35.1624 -97.9101 -9.08588 109.978 82.1654 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -29.irteusgl$ (send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(-0.11189 14.8888 35.1624 -97.9101 -9.08588 109.978 82.1654 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -30.irteusgl$ (nil) -31.irteusgl$ (send *ri* :state :potentio-vector) -#f(-0.161458 15.1127 35.2137 -97.9581 -9.35694 109.766 81.7806 0.037818 -1.79924 -0.021435 -77.9177 -0.19856 76.2007 45.8857) -32.irteusgl$ (setq tem_d (send *ri* :state :potentio-vector)) -#f(-0.157438 15.1093 35.2109 -97.9589 -9.35549 109.769 81.7809 0.037696 -1.79938 -0.021738 -77.9175 -0.198656 76.2005 45.8858) -33.irteusgl$ setq -nil -34.irteusgl$ tem_d -#f(-0.157438 15.1093 35.2109 -97.9589 -9.35549 109.769 81.7809 0.037696 -1.79938 -0.021738 -77.9175 -0.198656 76.2005 45.8858) -35.irteusgl$ (send *robot* :rarm_joint7 :joint-angle) -82.1654 -36.irteusgl$ (send *robot* :rarm_joint7 :joint-angle (- (send *robot* :rarm_joint7 :joint-angle) 90)) --7.83462 -37.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) -#f(8.65018 13.2283 25.2861 -96.2735 -5.93437 108.22 80.1129 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -38.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) -nil -38.irteusgl$ #f(8.71093 13.6077 25.1301 -97.9845 -6.14099 110.277 80.26 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -39.irteusgl$ (setq tem_d (- (send *ri :rarm-joint7 :joint-angle) 180)) -(send *robot* :rarm-joint7 :joint-angle tem_d)) -Call Stack (max depth: 20): - 0: at (send *ri :rarm-joint7 :joint-angle) - 1: at (- (send *ri :rarm-joint7 :joint-angle) 180) - 2: at (setq tem_d (- (send *ri :rarm-joint7 :joint-angle) 180)) - 3: at # -/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: unbound variable *ri in (send *ri :rarm-joint7 :joint-angle) -40.E1-irteusgl$ Call Stack (max depth: 20): - 0: at (send *robot* :rarm-joint7 :joint-angle tem_d) - 1: at euserror - 2: at euserror - 3: at (send *ri :rarm-joint7 :joint-angle) - 4: at (- (send *ri :rarm-joint7 :joint-angle) 180) - 5: at (setq tem_d (- (send *ri :rarm-joint7 :joint-angle) 180)) - 6: at # -/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :rarm-joint7 in (send *robot* :rarm-joint7 :joint-angle tem_d) -41.E2-irteusgl$ reset -nil -42.E2-irteusgl$ 43.irteusgl$ (setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) -(send *robot* :rarm-joint7 :joint-angle tem_d)) -Call Stack (max depth: 20): - 0: at (send *ri :rarm_joint7 :joint-angle) - 1: at (- (send *ri :rarm_joint7 :joint-angle) 180) - 2: at (setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) - 3: at # -/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: unbound variable *ri in (send *ri :rarm_joint7 :joint-angle) -44.E1-irteusgl$ Call Stack (max depth: 20): - 0: at (send *robot* :rarm-joint7 :joint-angle tem_d) - 1: at euserror - 2: at euserror - 3: at (send *ri :rarm_joint7 :joint-angle) - 4: at (- (send *ri :rarm_joint7 :joint-angle) 180) - 5: at (setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) - 6: at # -/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :rarm-joint7 in (send *robot* :rarm-joint7 :joint-angle tem_d) -45.E2-irteusgl$ reset -nil -46.E2-irteusgl$ 47.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) -#f(8.77822 13.6002 25.0544 -97.9849 -6.12046 110.278 80.2467 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -48.irteusgl$ o(setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) -(send *robot* :rarm_joint7 :joint-angle tem_d)) -Call Stack (max depth: 20): - 0: at (send *ri :rarm_joint7 :joint-angle) - 1: at (- (send *ri :rarm_joint7 :joint-angle) 180) - 2: at (setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) - 3: at # -/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: unbound variable *ri in (send *ri :rarm_joint7 :joint-angle) -49.E1-irteusgl$ Call Stack (max depth: 20): - 0: at (forward-message-to rarm_joint7_jt args) - 1: at (forward-message-to rarm_joint7_jt args) - 2: at (send *robot* :rarm_joint7 :joint-angle tem_d) - 3: at euserror - 4: at euserror - 5: at (send *ri :rarm_joint7 :joint-angle) - 6: at (- (send *ri :rarm_joint7 :joint-angle) 180) - 7: at (setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) - 8: at # -/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: number expected in (forward-message-to rarm_joint7_jt args) -50.E2-irteusgl$ reset -nil -51.E2-irteusgl$ 52.irteusgl$ (setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) -Call Stack (max depth: 20): - 0: at (send *ri :rarm_joint7 :joint-angle) - 1: at (- (send *ri :rarm_joint7 :joint-angle) 180) - 2: at (setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) - 3: at # -/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: unbound variable *ri in (send *ri :rarm_joint7 :joint-angle) -53.E1-irteusgl$ (setq tem_d (- (send *ri* :rarm_joint7 :joint-angle) 180)) -Call Stack (max depth: 20): - 0: at (send *ri* :rarm_joint7 :joint-angle) - 1: at (- (send *ri* :rarm_joint7 :joint-angle) 180) - 2: at (setq tem_d (- (send *ri* :rarm_joint7 :joint-angle) 180)) - 3: at euserror - 4: at euserror - 5: at (send *ri :rarm_joint7 :joint-angle) - 6: at (- (send *ri :rarm_joint7 :joint-angle) 180) - 7: at (setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) - 8: at # -/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :rarm_joint7 in (send *ri* :rarm_joint7 :joint-angle) -54.E2-irteusgl$(send *ri* :rarm_joint7 :joint-angle) -Call Stack (max depth: 20): - 0: at (send *ri* :rarm_joint7 :joint-angle) - 1: at euserror - 2: at euserror - 3: at (send *ri* :rarm_joint7 :joint-angle) - 4: at (- (send *ri* :rarm_joint7 :joint-angle) 180) - 5: at (setq tem_d (- (send *ri* :rarm_joint7 :joint-angle) 180)) - 6: at euserror - 7: at euserror - 8: at (send *ri :rarm_joint7 :joint-angle) - 9: at (- (send *ri :rarm_joint7 :joint-angle) 180) - 10: at (setq tem_d (- (send *ri :rarm_joint7 :joint-angle) 180)) - 11: at # -/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :rarm_joint7 in (send *ri* :rarm_joint7 :joint-angle) -55.E3-irteusgl$ reset -56.irteusgl$ (send *robot* :rarm_joint7 :joint-angle) -80.2467 -57.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) -#f(8.84526 13.5927 24.979 -97.9853 -6.10002 110.28 80.2334 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -58.irteusgl$ (setq tem_d (- (send *robot* :rarm_joint7 :joint-angle) 180)) -(send *robot* :rarm_joint7 :joint-angle tem_d)) --99.7666 -59.irteusgl$ -99.7666 -60.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 0))) :rotation-axis t) -nil -61.irteusgl$ #f(8.82039 13.2107 25.094 -96.274 -5.88317 108.224 -99.8286 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -62.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 1500) -(send *ri* :wait-interpolation) - -(setq tem_d (- (send *robot* :rarm_joint7 :joint-angle) 180)) -(send *robot* :rarm_joint7 :joint-angle tem_d)) - -(send *ri* :angle-vector (send *robot* :angle-vector) 1500) -(send *ri* :wait-interpolation) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 0))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 1500) -(send *ri* :wait-interpolation) -#f(22.5265 12.2457 9.78533 -96.3236 -2.20435 108.423 77.6978 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -63.irteusgl$ nil -63.irteusgl$ #f(22.5265 12.2457 9.78533 -96.3236 -2.20435 108.423 77.6978 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -64.irteusgl$ (nil) -65.irteusgl$ nil -65.irteusgl$ #f(22.5443 12.6063 9.7331 -98.0387 -2.28319 110.493 77.7543 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -66.irteusgl$ nil -66.irteusgl$ #f(22.5443 12.6063 9.7331 -98.0387 -2.28319 110.493 77.7543 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -67.irteusgl$ [ WARN] [1623049013.641453933]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -(nil) -68.irteusgl$ nil -68.irteusgl$ -102.246 -69.irteusgl$ -102.246 -70.irteusgl$ nil -71.irteusgl$ nil -71.irteusgl$ #f(22.5443 12.6063 9.7331 -98.0387 -2.28319 110.493 -102.246 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -72.irteusgl$ [ WARN] [1623049014.566786646]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -(nil) -73.irteusgl$ nil -73.irteusgl$ #f(22.4809 12.2485 9.83568 -96.3238 -2.21481 108.423 -102.204 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -74.irteusgl$ nil -74.irteusgl$ #f(22.4809 12.2485 9.83568 -96.3238 -2.21481 108.423 -102.204 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -75.irteusgl$ [ WARN] [1623049017.979824563]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal - -;; extending gcstack 0x56522c8d4600[32738] --> 0x56524cf02e80[65476] top=7f55 -(nil) -76.irteusgl$ quit -[ INFO] [1623049254.470344764]: cell* ROSEUS_EXIT(context*, int, cell**) -[ INFO] [1623049254.470419628]: exiting roseus 0 -[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus -configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" -;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin -connected to Xserver DISPLAY=:0 -X events are being asynchronously monitored. -;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb -;; extending gcstack 0x55ce469ea690[16374] --> 0x55ce46e70600[32748] top=3c74 -irtgl irtglc irtviewer -EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) -roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) -eustf roseus_c_util 1.irteusgl$ (load "amabe_pick_screw_pre.l") -[ WARN] [1623049314.383712251]: Waiting for actionlib interface forever because controller-timeout is nil -[ERROR] [1623049315.346264777]: unknown ros::param::get, key=/gazebo/time_step -[ WARN] [1623049315.346419993]: real franka environemt interface is not impelemented - C-c C-cinterrupt2.B1-irteusgl$ quit -[ INFO] [1623049327.579663917]: cell* ROSEUS_EXIT(context*, int, cell**) -[ INFO] [1623049327.579803291]: exiting roseus 0 -[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus -configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" -;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin -connected to Xserver DISPLAY=:0 -X events are being asynchronously monitored. -;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb -;; extending gcstack 0x559728ad6690[16374] --> 0x559728f5c600[32748] top=3c74 -irtgl irtglc irtviewer -EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) -roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) -eustf roseus_c_util 1.irteusgl$ (load "amabe_pick_screw_pre.l") -[ WARN] [1623049362.754813921]: Waiting for actionlib interface forever because controller-timeout is nil -[ERROR] [1623049363.340785788]: unknown ros::param::get, key=/gazebo/time_step -[ WARN] [1623049363.340953570]: real franka environemt interface is not impelemented - C-c C-c[ERROR] [1623049376.375959353]: [registerPublisher] Failed to contact master at [133.11.216.227:11311]. Retrying... -[ INFO] [1623049376.789943490]: Connected to master at [133.11.216.227:11311] -interrupt2.B1-irteusgl$ reset -3.irteusgl$ quit -[ INFO] [1623049382.168834368]: cell* ROSEUS_EXIT(context*, int, cell**) -[ INFO] [1623049382.168950431]: exiting roseus 0 -[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus -configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" -;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin -connected to Xserver DISPLAY=:0 -X events are being asynchronously monitored. -;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb -;; extending gcstack 0x55712a25d690[16374] --> 0x55712a6e3600[32748] top=3c74 -irtgl irtglc irtviewer -EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) -roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) -eustf roseus_c_util 1.irteusgl$ (load "amabe_pick_screw_pre.l") -[ WARN] [1623049457.357976285]: Waiting for actionlib interface forever because controller-timeout is nil -[ERROR] [1623049458.045549258]: unknown ros::param::get, key=/gazebo/time_step -[ WARN] [1623049458.045961293]: real franka environemt interface is not impelemented -;; (make-irtviewer) executed -[ WARN] [1623049466.001354460]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623049466.001865036]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623049466.002381821]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -t -2.irteusgl$ (load "amabe_pick_screw.l") -t -3.irteusgl$ (load "amabe_pick_screw.l") -t -4.irteusgl$ (load "amabe_pick_screw.l") -[ WARN] [1623049727.346257474]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623049729.127287714]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623049731.749538494]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -t -5.irteusgl$ (load "amabe_pick_screw.l") -[ WARN] [1623049741.904454795]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623049741.906164142]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623049744.127459156]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -t -6.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(65.085 14.8997 -37.8951 -96.1777 9.52507 107.845 70.1069 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -7.irteusgl$ nil -7.irteusgl$ #f(65.085 14.8997 -37.8951 -96.1777 9.52507 107.845 70.1069 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -8.irteusgl$ (nil) -9.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 1500) -(send *ri* :wait-interpolation) -#f(64.9311 15.2943 -37.574 -97.8869 9.82562 109.884 69.8952 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -10.irteusgl$ nil -10.irteusgl$ #f(64.9311 15.2943 -37.574 -97.8869 9.82562 109.884 69.8952 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -11.irteusgl$ (nil) -12.irteusgl$ (load "amabe_pick_screw.l") -[ WARN] [1623049805.591549586]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623049807.565687182]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623049807.567631132]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -t -13.irteusgl$ (send *ri* :start-grasp :rarm) -nil -14.irteusgl$ (load "amabe_pick_screw.l") -[ WARN] [1623050330.792249109]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623050332.903996845]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623050335.128778219]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -t -15.irteusgl$ (load "amabe_pick_screw.l") -[ WARN] [1623050397.588762836]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623050397.607837882]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623050397.609749579]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623050399.698250098]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -t -16.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(94.2665 53.8612 -93.5533 -92.4783 53.7296 88.6379 45.0703 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -17.irteusgl$ nil -17.irteusgl$ #f(94.2665 53.8612 -93.5533 -92.4783 53.7296 88.6379 45.0703 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -18.irteusgl$ (nil) -19.irteusgl$ (load "amabe_pick_screw.l") -[ WARN] [1623050495.713558001]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623050497.738797790]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -t -20.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(92.247 65.3101 -101.682 -91.4105 64.6229 80.0454 40.9219 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -21.irteusgl$ nil -21.irteusgl$ #f(92.247 65.3101 -101.682 -91.4105 64.6229 80.0454 40.9219 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -22.irteusgl$ (nil) -23.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(92.3814 64.7619 -101.287 -91.4594 64.1021 80.4835 41.093 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -24.irteusgl$ nil -24.irteusgl$ #f(92.3814 64.7619 -101.287 -91.4594 64.1021 80.4835 41.093 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -25.irteusgl$ (nil) -26.irteusgl$ (load "amabe_pick_screw.l") -[ WARN] [1623050592.314653794]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623050594.626695734]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -t -27.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(90.5206 71.3833 -106.17 -90.9361 70.4383 75.0542 39.2271 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -28.irteusgl$ nil -28.irteusgl$ #f(90.5206 71.3833 -106.17 -90.9361 70.4383 75.0542 39.2271 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -29.irteusgl$ (nil) -30.irteusgl$ (load "amabe_pick_screw.l") -t -31.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(89.175 75.2568 -109.166 -90.6793 74.2206 71.7171 38.3414 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -32.irteusgl$ nil -32.irteusgl$ #f(89.175 75.2568 -109.166 -90.6793 74.2206 71.7171 38.3414 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -33.irteusgl$ (nil) -34.irteusgl$ (load "amabe_pick_screw.l") -[ WARN] [1623050854.432390139]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -t -35.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(88.2492 77.6323 -111.071 -90.5427 76.5851 69.6107 37.8788 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -36.irteusgl$ nil -36.irteusgl$ #f(88.2492 77.6323 -111.071 -90.5427 76.5851 69.6107 37.8788 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -37.irteusgl$ -;; extending gcstack 0x55712a6e3600[32738] --> 0x55714b9123c0[65476] top=7f53 -(nil) -38.irteusgl$ (load "amabe_pick_screw.l") -[ WARN] [1623050894.185163357]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623050896.348864105]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -t -39.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(87.6189 79.141 -112.312 -90.4653 78.1096 68.2487 37.6187 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -40.irteusgl$ nil -40.irteusgl$ #f(87.6189 79.141 -112.312 -90.4653 78.1096 68.2487 37.6187 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -41.irteusgl$ (nil) -42.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(87.9218 78.4264 -111.721 -90.5009 77.3849 68.8963 37.7384 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -43.irteusgl$ nil -43.irteusgl$ #f(87.9218 78.4264 -111.721 -90.5009 77.3849 68.8963 37.7384 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -44.irteusgl$ (nil) -45.irteusgl$ (load "amabe_pick_screw.l") -[ WARN] [1623051014.052718012]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623051014.061815984]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623051016.245064549]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -t -46.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(87.3963 79.6553 -112.741 -90.4406 78.6339 67.7801 37.5363 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -47.irteusgl$ nil -47.irteusgl$ #f(87.3963 79.6553 -112.741 -90.4406 78.6339 67.7801 37.5363 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -48.irteusgl$ (nil) -49.irteusgl$ (load "amabe_pick_screw.l") -[ WARN] [1623051088.168469343]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623051090.224919357]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623051092.259653155]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -t -50.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(87.0388 80.4622 -113.421 -90.4038 79.4616 67.0404 37.4135 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -51.irteusgl$ nil -51.irteusgl$ #f(87.0388 80.4622 -113.421 -90.4038 79.4616 67.0404 37.4135 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -52.irteusgl$ (nil) -53.irteusgl$ (load "amabe_pick_screw.l") -[ WARN] [1623051205.313071648]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623051207.315177003]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623051209.324084743]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623051211.364285142]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -t -54.irteusgl$ (load "amabe_pick_screw.l") -[ WARN] [1623051225.303973908]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623051228.894120579]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623051230.907995238]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -[ WARN] [1623051233.002747055]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -t -55.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(85.9036 82.8831 -115.511 -90.3077 81.9867 64.7874 37.095 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -56.irteusgl$ nil -56.irteusgl$ #f(85.9036 82.8831 -115.511 -90.3077 81.9867 64.7874 37.095 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -57.irteusgl$ (nil) -58.irteusgl$ (load "amabe_pick_screw.l") -Call Stack (max depth: 20): - 0: at (apply #'ros::load-org-for-ros ros::fullname args) - 1: at (apply #'ros::load-org-for-ros ros::fullname args) - 2: at (let ((ros::fullname fname)) (when (substringp "package://" fname) (setq ros::fullname (ros::resolve-ros-path fname)) (when ros::*compile-message* (let* ((ros::urlname (url-pathname fname)) (package-name (send ros::urlname :host)) (ros::path-name (format nil "~A_~A" package-name (substitute 95 47 (concatenate string (subseq (send ros::urlname :directory-string) 1) (send ros::urlname :name))))) (ros::old-module (find ros::path-name *loaded-modules* :key #'lisp::load-module-file-name :test #'equal))) (unless ros::old-module (let* ((ros::ppath (unix:getenv "CMAKE_PREFIX_PATH")) (dir (format nil "~A/share/roseus/ros/~A" (subseq ros::ppath 0 (position 58 ros::ppath)) package-name))) (unless (probe-file dir) (unix:mkdir dir)) (compiler:compile-file-if-src-newer ros::fullname (format nil "~A/~A" dir ros::path-name)) (return-from load (load (format nil "~A/~A.so" dir ros::path-name) :entry (format nil "___~A" ros::path-name)))))))) (if (null ros::fullname) (error "file ~s not found" fname)) (apply #'ros::load-org-for-ros ros::fullname args)) - 3: at (load "amabe_pick_screw.l") - 4: at # -/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: file #P"amabe_pick_screw.l" not found in (apply #'ros::load-org-for-ros ros::fullname args) -59.E1-irteusgl$ reset -60.irteusgl$ (load "amabe_loosen_screw.l") -[ WARN] [1623052329.569961631]: [/dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory] feedback-cb # received wrong goal -t -61.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(85.8388 83.0154 -115.627 -90.3031 82.1267 64.6628 37.0799 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -62.irteusgl$ nil -62.irteusgl$ #f(85.8388 83.0154 -115.627 -90.3031 82.1267 64.6628 37.0799 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -63.irteusgl$ (nil) -64.irteusgl$ (load "amabe_loosen_screw.l") -t -65.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(85.8068 83.0805 -115.685 -90.3009 82.1956 64.6015 37.0725 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -66.irteusgl$ nil -66.irteusgl$ #f(85.8068 83.0805 -115.685 -90.3009 82.1956 64.6015 37.0725 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -67.irteusgl$ (nil) -68.irteusgl$ (load "amabe_loosen_screw.l") -t -69.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(85.791 83.1126 -115.713 -90.2998 82.2296 64.5712 37.0689 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -70.irteusgl$ nil -70.irteusgl$ #f(85.791 83.1126 -115.713 -90.2998 82.2296 64.5712 37.0689 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -71.irteusgl$ (nil) -72.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(86.8597 83.7929 -114.315 -92.0083 82.3125 66.1357 36.7928 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -73.irteusgl$ nil -73.irteusgl$ #f(86.8597 83.7929 -114.315 -92.0083 82.3125 66.1357 36.7928 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -74.irteusgl$ (nil) -75.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(86.4176 81.8127 -114.577 -90.3473 80.8618 65.7899 37.2262 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -76.irteusgl$ nil -76.irteusgl$ #f(86.4176 81.8127 -114.577 -90.3473 80.8618 65.7899 37.2262 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -77.irteusgl$ (nil) -78.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(86.7625 81.0711 -113.939 -90.3774 80.0903 66.4786 37.326 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -79.irteusgl$ nil -79.irteusgl$ #f(86.7625 81.0711 -113.939 -90.3774 80.0903 66.4786 37.326 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -80.irteusgl$ (nil) -81.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(87.786 81.7133 -112.574 -92.0845 80.2056 68.0594 37.0473 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -82.irteusgl$ nil -82.irteusgl$ #f(87.786 81.7133 -112.574 -92.0845 80.2056 68.0594 37.0473 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -83.irteusgl$ (nil) -84.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(87.336 79.7934 -112.857 -90.434 78.7748 67.654 37.5145 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -85.irteusgl$ nil -85.irteusgl$ #f(87.336 79.7934 -112.857 -90.434 78.7748 67.654 37.5145 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -86.irteusgl$ (nil) -87.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(88.3311 80.4137 -111.513 -92.14 78.9099 69.2442 37.2337 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -88.irteusgl$ nil -88.irteusgl$ #f(88.3311 80.4137 -111.513 -92.14 78.9099 69.2442 37.2337 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -89.irteusgl$ (nil) -90.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(87.8776 78.5318 -111.808 -90.4955 77.4914 68.8011 37.7204 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -91.irteusgl$ nil -91.irteusgl$ #f(87.8776 78.5318 -111.808 -90.4955 77.4914 68.8011 37.7204 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -92.irteusgl$ (nil) -93.irteusgl$ (load "amabe_loosen_screw.l") -t -94.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 3.14))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(86.7981 80.9928 -113.872 -90.3808 80.0095 66.5509 37.3372 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -95.irteusgl$ nil -95.irteusgl$ #f(86.7981 80.9928 -113.872 -90.3808 80.0095 66.5509 37.3372 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -96.irteusgl$ (nil) -97.irteusgl$ (load "amabe_loosen_screw.l") -t -98.irteusgl$ (send *ri* :stop-grasp) -Call Stack (max depth: 20): - 0: at (send *ri* :stop-grasp) - 1: at # -/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: mismatch argument in (send *ri* :stop-grasp) -99.E1-irteusgl$ (send *ri* :stop-grasp :rarm) -nil -100.E1-irteusgl$ (send *ri* :start-grasp :rarm) -nil -101.E1-irteusgl$ (send *ri* :start-grasp :rarm :effort 100) -nil -102.E1-irteusgl$ quit -[ INFO] [1623055368.674837708]: cell* ROSEUS_EXIT(context*, int, cell**) -[ INFO] [1623055368.675009454]: exiting roseus 0 -[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ quit - -Command 'quit' not found, did you mean: - - command 'qgit' from deb qgit - command 'quiz' from deb bsdgames - command 'luit' from deb x11-utils - command 'quilt' from deb quilt - command 'quot' from deb quota - -Try: sudo apt install - -[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus -configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" -;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin -connected to Xserver DISPLAY=:0 -X events are being asynchronously monitored. -;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb -;; extending gcstack 0x5561c3244690[16374] --> 0x5561c36ca600[32748] top=3c74 -irtgl irtglc irtviewer -EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) -roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) -eustf roseus_c_util 1.irteusgl$ (load "amabe_loosen_screw_pre.l") -[ WARN] [1623056168.225945147]: Waiting for actionlib interface forever because controller-timeout is nil -[ERROR] [1623056168.552274525]: unknown ros::param::get, key=/gazebo/time_step -[ WARN] [1623056168.552430451]: real franka environemt interface is not impelemented -;; (make-irtviewer) executed -t -2.irteusgl$ (send *ri* :stop-grasp :larm) -nil -3.irteusgl$ quit -[ INFO] [1623056216.300169409]: cell* ROSEUS_EXIT(context*, int, cell**) -[ INFO] [1623056216.300295286]: exiting roseus 0 -[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus -configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" -;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin -connected to Xserver DISPLAY=:0 -X events are being asynchronously monitored. -;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb -;; extending gcstack 0x56482e518690[16374] --> 0x56482e99e600[32748] top=3c74 -irtgl irtglc irtviewer -EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) -roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) -eustf roseus_c_util 1.irteusgl$ (load "amabe_loosen_screw_pre.l") -[ WARN] [1623057704.496292996]: Waiting for actionlib interface forever because controller-timeout is nil -[ERROR] [1623057704.906291755]: unknown ros::param::get, key=/gazebo/time_step -[ WARN] [1623057704.906440868]: real franka environemt interface is not impelemented - -;; extending gcstack 0x56482e99e600[32738] --> 0x56484cae8490[65476] top=7fe1 -;; (make-irtviewer) executed -t -2.irteusgl$ (send *ri* :set-all-joint-pd-gain 30.0 0.5) -nil -3.irteusgl$ (send *ri* :set-all-joint-pd-gain 300.0 5.0) -nil -4.irteusgl$ (send *ri* :set-all-joint-pd-gain 1000.0 5.0) -nil -5.irteusgl$ (send *ri* :set-all-joint-pd-gain 2000.0 5.0) -nil -6.irteusgl$ (send *ri* :set-all-joint-pd-gain 5000.0 5.0) -nil -7.irteusgl$ (send *ri* :set-all-joint-pd-gain 1000.0 5.0) -nil -8.irteusgl$ (send *ri* :set-all-joint-pd-gain 30.0 0.5) -nil -9.irteusgl$ quit -[ INFO] [1623058260.223158329]: cell* ROSEUS_EXIT(context*, int, cell**) -[ INFO] [1623058260.223300134]: exiting roseus 0 -[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus -configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" -;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin -connected to Xserver DISPLAY=:0 -X events are being asynchronously monitored. -;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb -;; extending gcstack 0x55d3729ae690[16374] --> 0x55d372e34600[32748] top=3c74 -irtgl irtglc irtviewer -EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) -roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) -eustf roseus_c_util 1.irteusgl$ (load "amabe_loosen_screw_pre.l") -[ WARN] [1623058443.040893370]: Waiting for actionlib interface forever because controller-timeout is nil -[ERROR] [1623058444.921455549]: unknown ros::param::get, key=/gazebo/time_step -[ WARN] [1623058444.921590804]: real franka environemt interface is not impelemented -;; (make-irtviewer) executed -t -2.irteusgl$ (send *ri* :set-all-joint-pd-gain 30.0 0.5) -nil -3.irteusgl$ (send *ri* :set-all-joint-pd-gain 1000.0 5.0) -nil -4.irteusgl$ (send *ri* :set-all-joint-pd-gain 30.0 0.5) -nil -5.irteusgl$ (send *ri* :set-all-joint-pd-gain 1000.0 5.0) -nil -6.irteusgl$ (send *ri* :set-all-joint-pd-gain 30.0 0.5) -nil -7.irteusgl$ (send *ri* :set-all-joint-pd-gain 1000.0 5.0) -nil -8.irteusgl$ (send *ri* :set-all-joint-pd-gain 3000.0 5.0) -nil -9.irteusgl$ (send *ri* :set-all-joint-pd-gain 1000.0 5.0) -nil -10.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 0))) :rotation-axis t) -#f(0.579297 14.348 34.4915 -96.2079 -8.50591 107.968 -98.1348 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -11.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) -#f(8.22998 13.2755 25.7608 -96.2706 -6.05809 108.21 -9.76135 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -12.irteusgl$ (send *robot* :angle-vector) -#f(8.22998 13.2755 25.7608 -96.2706 -6.05809 108.21 -9.76135 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -13.irteusgl$ (send *robot* :rarm-joint7 :angle-vector) -Call Stack (max depth: 20): - 0: at (send *robot* :rarm-joint7 :angle-vector) - 1: at # -/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :rarm-joint7 in (send *robot* :rarm-joint7 :angle-vector) -14.E1-irteusgl$ (send *robot* :rarm-joint7 :joint-angle) -Call Stack (max depth: 20): - 0: at (send *robot* :rarm-joint7 :joint-angle) - 1: at euserror - 2: at euserror - 3: at (send *robot* :rarm-joint7 :angle-vector) - 4: at # -/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: cannot find method :rarm-joint7 in (send *robot* :rarm-joint7 :joint-angle) -15.E2-irteusgl$ (send *robot* :rarm_joint7 :joint-angle) --9.76135 -16.E2-irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(8.25454 13.273 25.7329 -96.2707 -6.0499 108.211 -9.76642 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -17.E2-irteusgl$ nil -17.E2-irteusgl$ #f(8.25454 13.273 25.7329 -96.2707 -6.0499 108.211 -9.76642 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -18.E2-irteusgl$ (nil) -19.E2-irteusgl$ reset -20.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) -#f(8.2792 13.2702 25.705 -96.2709 -6.04259 108.211 -9.77115 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -21.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(8.3038 13.2675 25.6772 -96.271 -6.03529 108.212 -9.77587 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -22.irteusgl$ nil -22.irteusgl$ #f(8.3038 13.2675 25.6772 -96.271 -6.03529 108.212 -9.77587 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -23.irteusgl$ (nil) -24.irteusgl$ quit -[ INFO] [1623059138.268176327]: cell* ROSEUS_EXIT(context*, int, cell**) -[ INFO] [1623059138.268330318]: exiting roseus 0 -[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus -configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" -;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin -connected to Xserver DISPLAY=:0 -X events are being asynchronously monitored. -;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb -;; extending gcstack 0x5622e523b690[16374] --> 0x5622e56c1600[32748] top=3c74 -irtgl irtglc irtviewer -EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) -roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) -eustf roseus_c_util 1.irteusgl$ (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*)) -:|PACKAGE://PANDA_EUS/EUSLISP/DUAL_PANDA-INTERFACE.L| -2.irteusgl$ nil -2.irteusgl$ nil -3.irteusgl$ nil -4.irteusgl$ nil -4.irteusgl$ [ WARN] [1623059192.830940422]: Waiting for actionlib interface forever because controller-timeout is nil -[ERROR] [1623059193.062943866]: unknown ros::param::get, key=/gazebo/time_step -[ WARN] [1623059193.063075076]: real franka environemt interface is not impelemented - -;; extending gcstack 0x5622e56c1600[32738] --> 0x5623036fe1a0[65476] top=7fe1 -# -5.irteusgl$ nil -5.irteusgl$ ;; (make-irtviewer) executed -(#) -6.irteusgl$ quit -[ INFO] [1623059204.645088661]: cell* ROSEUS_EXIT(context*, int, cell**) -[ INFO] [1623059204.645228720]: exiting roseus 0 -[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus -configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" -;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin -connected to Xserver DISPLAY=:0 -X events are being asynchronously monitored. -;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb -;; extending gcstack 0x555bccffd690[16374] --> 0x555bcd483600[32748] top=3c74 -irtgl irtglc irtviewer -EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) -roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) -eustf roseus_c_util 1.irteusgl$ (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*)) -:|PACKAGE://PANDA_EUS/EUSLISP/DUAL_PANDA-INTERFACE.L| -2.irteusgl$ nil -2.irteusgl$ nil -3.irteusgl$ nil -4.irteusgl$ nil -4.irteusgl$ [ WARN] [1623059297.149020601]: Waiting for actionlib interface forever because controller-timeout is nil -[ERROR] [1623059297.474965333]: unknown ros::param::get, key=/gazebo/time_step -[ WARN] [1623059297.475072470]: real franka environemt interface is not impelemented - -;; extending gcstack 0x555bcd483600[32738] --> 0x555beb5cfdb0[65476] top=7fe1 -# -5.irteusgl$ nil -5.irteusgl$ ;; (make-irtviewer) executed -(#) -6.irteusgl$ (send *robot* :reset-pose) -(send *robot* :rarm :move-end-pos #f(-500 0 0)) -(send *robot* :larm :move-end-pos #f(-500 0 0)) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) -#f(0.0 15.0 0.0 -135.0 0.0 150.0 45.0 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) -7.irteusgl$ nil -7.irteusgl$ #f(-0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0 0.0 15.0 0.0 -135.0 0.0 150.0 45.0) -8.irteusgl$ nil -8.irteusgl$ #f(-0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -9.irteusgl$ nil -9.irteusgl$ #f(-0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -10.irteusgl$ nil -10.irteusgl$ (nil) -11.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) -#f(18.0066 12.4413 14.7926 -96.366 -3.36376 108.418 -11.4914 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -12.irteusgl$ nil -12.irteusgl$ #f(18.0066 12.4413 14.7926 -96.366 -3.36376 108.418 -11.4914 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -13.irteusgl$ (nil) -14.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1096) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(18.1553 15.8123 14.4388 -104.518 -4.51919 119.817 -10.7282 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -15.irteusgl$ nil -15.irteusgl$ #f(18.1553 15.8123 14.4388 -104.518 -4.51919 119.817 -10.7282 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -16.irteusgl$ (nil) -17.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1098) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(18.1445 15.7011 14.4544 -104.369 -4.48154 119.56 -10.7528 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -18.irteusgl$ nil -18.irteusgl$ #f(18.1445 15.7011 14.4544 -104.369 -4.48154 119.56 -10.7528 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -19.irteusgl$ (nil) -20.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1102) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(18.1264 15.4829 14.4816 -104.064 -4.40685 119.045 -10.8019 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -21.irteusgl$ nil -21.irteusgl$ #f(18.1264 15.4829 14.4816 -104.064 -4.40685 119.045 -10.8019 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -22.irteusgl$ (nil) -23.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1101) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(18.1273 15.5371 14.4788 -104.141 -4.42656 119.173 -10.7889 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -24.irteusgl$ nil -24.irteusgl$ #f(18.1273 15.5371 14.4788 -104.141 -4.42656 119.173 -10.7889 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -25.irteusgl$ (nil) -26.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1100) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) -#f(18.1282 15.5917 14.4759 -104.218 -4.44641 119.302 -10.7759 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -27.irteusgl$ nil -27.irteusgl$ #f(18.1282 15.5917 14.4759 -104.218 -4.44641 119.302 -10.7759 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -28.irteusgl$ (nil) -29.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) - -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) -#f(17.9424 12.4636 14.8771 -96.313 -3.37302 108.38 -11.4989 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -30.irteusgl$ nil -30.irteusgl$ #f(17.9424 12.4636 14.8771 -96.313 -3.37302 108.38 -11.4989 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -31.irteusgl$ (nil) -32.irteusgl$ (send *ri* :start-grasp :rarm) -(unix::sleep 2) -nil -33.irteusgl$ t -34.irteusgl$ (send *ri* :stop-grasp :rarm) -(unix::sleep 2) -nil -35.irteusgl$ t -36.irteusgl$ (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -164.541 1181.738) :rpy (float-vector 0.0 1.57 1.57))) :rotation-axis t) -#f(8.77648 0.789839 9.4671 -111.046 -0.155226 111.868 -26.745 -0.004461 -1.85659 0.004415 -77.7771 0.000242 75.9211 45.0) -37.irteusgl$ (load "amabe_pick_screw.l") -[ WARN] [1623060060.016109360]: topic /robot_interface_marker_array already advertised -[ WARN] [1623060060.034890038]: topic /dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory/goal already advertised -[ WARN] [1623060060.034926556]: topic /dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory/cancel already advertised -[ WARN] [1623060060.061263362]: Waiting for actionlib interface forever because controller-timeout is nil -[ERROR] [1623060060.080068688]: unknown ros::param::get, key=/gazebo/time_step -[ WARN] [1623060060.080126959]: real franka environemt interface is not impelemented -[ WARN] [1623060060.097829471]: topic /dual_panda/error_recovery/goal already advertised -[ WARN] [1623060060.097869279]: topic /dual_panda/error_recovery/cancel already advertised -[ WARN] [1623060060.124736796]: topic /dual_panda/rarm/franka_gripper/grasp/goal already advertised -[ WARN] [1623060060.124769582]: topic /dual_panda/rarm/franka_gripper/grasp/cancel already advertised -[ WARN] [1623060060.150759748]: topic /dual_panda/rarm/franka_gripper/homing/goal already advertised -[ WARN] [1623060060.150789586]: topic /dual_panda/rarm/franka_gripper/homing/cancel already advertised -[ WARN] [1623060060.178508717]: topic /dual_panda/rarm/franka_gripper/move/goal already advertised -[ WARN] [1623060060.178543869]: topic /dual_panda/rarm/franka_gripper/move/cancel already advertised -[ WARN] [1623060060.207030028]: topic /dual_panda/rarm/franka_gripper/stop/goal already advertised -[ WARN] [1623060060.207081049]: topic /dual_panda/rarm/franka_gripper/stop/cancel already advertised -[ WARN] [1623060060.237351499]: topic /dual_panda/larm/franka_gripper/grasp/goal already advertised -[ WARN] [1623060060.237460925]: topic /dual_panda/larm/franka_gripper/grasp/cancel already advertised -[ WARN] [1623060060.266930707]: topic /dual_panda/larm/franka_gripper/homing/goal already advertised -[ WARN] [1623060060.267055717]: topic /dual_panda/larm/franka_gripper/homing/cancel already advertised -[ WARN] [1623060060.294989929]: topic /dual_panda/larm/franka_gripper/move/goal already advertised -[ WARN] [1623060060.295035699]: topic /dual_panda/larm/franka_gripper/move/cancel already advertised -[ WARN] [1623060060.321165821]: topic /dual_panda/larm/franka_gripper/stop/goal already advertised -[ WARN] [1623060060.321225822]: topic /dual_panda/larm/franka_gripper/stop/cancel already advertised - C-c C-cinterrupt38.B1-irteusgl$quit -[ INFO] [1623060095.028685921]: cell* ROSEUS_EXIT(context*, int, cell**) -[ INFO] [1623060095.028809209]: exiting roseus 0 -[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ roseus -configuring by "/opt/ros/melodic/share/euslisp/jskeus/eus//lib/eusrt.l" -;; readmacro ;; object ;; packsym ;; common ;; constants ;; stream ;; string ;; loader ;; pprint ;; process ;; hashtab ;; array ;; mathtran ;; eusdebug ;; eusforeign ;; extnum ;; coordinates ;; tty ;; history ;; toplevel ;; trans ;; comp ;; builtins ;; par ;; intersection ;; geoclasses ;; geopack ;; geobody ;; primt ;; compose ;; polygon ;; viewing ;; viewport ;; viewsurface ;; hid ;; shadow ;; bodyrel ;; dda ;; helpsub ;; eushelp ;; xforeign ;; Xdecl ;; Xgraphics ;; Xcolor ;; Xeus ;; Xevent ;; Xpanel ;; Xitem ;; Xtext ;; Xmenu ;; Xscroll ;; Xcanvas ;; Xtop ;; Xapplwin -connected to Xserver DISPLAY=:0 -X events are being asynchronously monitored. -;; pixword ;; RGBHLS ;; convolve ;; piximage ;; pbmfile ;; image_correlation ;; oglforeign ;; gldecl ;; glconst ;; glforeign ;; gluconst ;; gluforeign ;; glxconst ;; glxforeign ;; eglforeign ;; eglfunc ;; glutil ;; gltexture ;; glprim ;; gleus ;; glview ;; toiv-undefined ;; fstringdouble irtmath irtutil irtc irtgeoc irtgraph ___time ___pgsql irtgeo euspqp pqp irtscene irtmodel irtdyna irtrobot irtsensor irtbvh irtcollada irtstl irtwrl irtpointcloud eusbullet bullet irtcollision irtx eusjpeg euspng png irtimage irtglrgb -;; extending gcstack 0x55d9b5699690[16374] --> 0x55d9b5b1f600[32748] top=3c74 -irtgl irtglc irtviewer -EusLisp 9.27( 1.2.2) for Linux64 created on ip-172-30-1-193(Wed Aug 12 20:03:47 UTC 2020) -roseus ;; loading roseus("1.7.4") on euslisp((9.27 ip-172-30-1-193 Wed Aug 12 20:03:47 UTC 2020 1.2.2)) -eustf roseus_c_util 1.irteusgl$ (load "amabe_pick_screw.l") -[ WARN] [1623060261.807193078]: Waiting for actionlib interface forever because controller-timeout is nil -[ERROR] [1623060262.136650065]: unknown ros::param::get, key=/gazebo/time_step -[ WARN] [1623060262.136778304]: real franka environemt interface is not impelemented - -;; extending gcstack 0x55d9b5b1f600[32738] --> 0x55d9d3c6bb60[65476] top=7fe1 -;; (make-irtviewer) executed -Call Stack (max depth: 20): - 0: at (apply #'ros::load-org-for-ros ros::fullname args) - 1: at (apply #'ros::load-org-for-ros ros::fullname args) - 2: at (let ((ros::fullname fname)) (when (substringp "package://" fname) (setq ros::fullname (ros::resolve-ros-path fname)) (when ros::*compile-message* (let* ((ros::urlname (url-pathname fname)) (package-name (send ros::urlname :host)) (ros::path-name (format nil "~A_~A" package-name (substitute 95 47 (concatenate string (subseq (send ros::urlname :directory-string) 1) (send ros::urlname :name))))) (ros::old-module (find ros::path-name *loaded-modules* :key #'lisp::load-module-file-name :test #'equal))) (unless ros::old-module (let* ((ros::ppath (unix:getenv "CMAKE_PREFIX_PATH")) (dir (format nil "~A/share/roseus/ros/~A" (subseq ros::ppath 0 (position 58 ros::ppath)) package-name))) (unless (probe-file dir) (unix:mkdir dir)) (compiler:compile-file-if-src-newer ros::fullname (format nil "~A/~A" dir ros::path-name)) (return-from load (load (format nil "~A/~A.so" dir ros::path-name) :entry (format nil "___~A" ros::path-name)))))))) (if (null ros::fullname) (error "file ~s not found" fname)) (apply #'ros::load-org-for-ros ros::fullname args)) - 3: at (load "amabe_pick_screw.l") - 4: at # -/opt/ros/melodic/share/euslisp/jskeus/eus/Linux64/bin/irteusgl 0 error: unbound variable send in (apply #'ros::load-org-for-ros ros::fullname args) -2.E1-irteusgl$ reset -3.irteusgl$ (load "amabe_pick_screw.l") -[ WARN] [1623060316.650644526]: topic /robot_interface_marker_array already advertised -[ WARN] [1623060316.669911352]: topic /dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory/goal already advertised -[ WARN] [1623060316.669948573]: topic /dual_panda/dual_panda_effort_joint_trajectory_controller/follow_joint_trajectory/cancel already advertised -[ WARN] [1623060316.698484340]: Waiting for actionlib interface forever because controller-timeout is nil -[ERROR] [1623060316.714652806]: unknown ros::param::get, key=/gazebo/time_step -[ WARN] [1623060316.714736783]: real franka environemt interface is not impelemented -[ WARN] [1623060316.734699282]: topic /dual_panda/error_recovery/goal already advertised -[ WARN] [1623060316.734811112]: topic /dual_panda/error_recovery/cancel already advertised -[ WARN] [1623060316.763558255]: topic /dual_panda/rarm/franka_gripper/grasp/goal already advertised -[ WARN] [1623060316.763656993]: topic /dual_panda/rarm/franka_gripper/grasp/cancel already advertised -[ WARN] [1623060316.793553681]: topic /dual_panda/rarm/franka_gripper/homing/goal already advertised -[ WARN] [1623060316.793678039]: topic /dual_panda/rarm/franka_gripper/homing/cancel already advertised -[ WARN] [1623060316.824876680]: topic /dual_panda/rarm/franka_gripper/move/goal already advertised -[ WARN] [1623060316.825006436]: topic /dual_panda/rarm/franka_gripper/move/cancel already advertised -[ WARN] [1623060316.853737484]: topic /dual_panda/rarm/franka_gripper/stop/goal already advertised -[ WARN] [1623060316.853877576]: topic /dual_panda/rarm/franka_gripper/stop/cancel already advertised -[ WARN] [1623060316.884739828]: topic /dual_panda/larm/franka_gripper/grasp/goal already advertised -[ WARN] [1623060316.884870854]: topic /dual_panda/larm/franka_gripper/grasp/cancel already advertised -[ WARN] [1623060316.917870233]: topic /dual_panda/larm/franka_gripper/homing/goal already advertised -[ WARN] [1623060316.918000352]: topic /dual_panda/larm/franka_gripper/homing/cancel already advertised -[ WARN] [1623060316.949212265]: topic /dual_panda/larm/franka_gripper/move/goal already advertised -[ WARN] [1623060316.949351283]: topic /dual_panda/larm/franka_gripper/move/cancel already advertised -[ WARN] [1623060316.980023480]: topic /dual_panda/larm/franka_gripper/stop/goal already advertised -[ WARN] [1623060316.980153361]: topic /dual_panda/larm/franka_gripper/stop/cancel already advertised -t -4.irteusgl$ quit -[ INFO] [1623060519.040638339]: cell* ROSEUS_EXIT(context*, int, cell**) -[ INFO] [1623060519.040685918]: exiting roseus 0 -[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ -[http://133.11.216.227:11311][133.11.216.79] amabe@ubuntu:~/franka_ws/src/jsk_demos/jsk_2021_fix_kxr/euslisp$ \ No newline at end of file diff --git a/jsk_2021_fix_kxr/euslisp/#amabe_screw.l# b/jsk_2021_fix_kxr/euslisp/#amabe_screw.l# deleted file mode 100644 index 0798f6c747..0000000000 --- a/jsk_2021_fix_kxr/euslisp/#amabe_screw.l# +++ /dev/null @@ -1,38 +0,0 @@ -#!/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") - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 1))) :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.456 1.57 1))) :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.456 1.57 3))) :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 3))) :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 1))) :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.456 1.57 1))) :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1166.738) :rpy (float-vector 0.456 1.57 3))) :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(434 -14.541 1181.738) :rpy (float-vector 0.456 1.57 3))) :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) diff --git a/jsk_2021_fix_kxr/euslisp/#amabe_set_kxr.l# b/jsk_2021_fix_kxr/euslisp/#amabe_set_kxr.l# deleted file mode 100644 index b5b7e30477..0000000000 --- a/jsk_2021_fix_kxr/euslisp/#amabe_set_kxr.l# +++ /dev/null @@ -1,73 +0,0 @@ -#!/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 *robot* :reset-pose) -(send *robot* :rarm :move-end-pos #f(-500 0 0)) -(send *robot* :larm :move-end-pos #f(-500 0 0)) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 -88 1200) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) -(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 177 1200) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 -88 1093.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) -(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 177 1093.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - -(send *ri* :start-grasp :rarm) -(send *ri* :start-grasp :larm) -(unix::sleep 2) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 -138 1093.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) -(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 127 1093.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - -(send *ri* :stop-grasp :rarm) -(send *ri* :stop-grasp :larm) -(unix::sleep 2) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 -138 1300.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) -(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 127 1300.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(513 -310 1300.682) :rpy (float-vector -1.57 1.57 0.0))) :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(510 -310 1170.364) :rpy (float-vector 1.57 0.0 -3.14))) :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(510 -310 1070.364) :rpy (float-vector 1.57 0.0 -3.14))) :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(510 -260 1070.364) :rpy (float-vector 1.57 0.0 -3.14))) :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) - -(send *ri* :start-grasp :rarm) -(unix::sleep 2) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(510 -260 1070.364) :rpy (float-vector 1.57 0.0 -1.57))) :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) - -(send *ri* :stop-grasp :rarm) -(unix::sleep 2) - -(send *robot* :rarm :move-end-pos #f(-50 0 0)) -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) diff --git a/jsk_2021_fix_kxr/euslisp/#repairdemo_place.l# b/jsk_2021_fix_kxr/euslisp/#repairdemo_place.l# deleted file mode 100644 index 193a42e5cd..0000000000 --- a/jsk_2021_fix_kxr/euslisp/#repairdemo_place.l# +++ /dev/null @@ -1,14 +0,0 @@ -#!/usr/bin/env roseus - -(send *ri* :larm :start-grasp) - -;;before grasp position -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(345 -246.5 1330) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) -(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(345 -84.8 1330) :rpy (float-vector 1.83 1.57 -2.86))) :translation-axis t :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - -(send *robot* :larm :move-end-pos #f(30 0 0)) -(send *robot* :rarm :move-end-pos #f(30 0 0)) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) diff --git a/jsk_2021_fix_kxr/euslisp/maker_test/.#marker_point.l b/jsk_2021_fix_kxr/euslisp/maker_test/.#marker_point.l new file mode 120000 index 0000000000..2a920f3522 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/maker_test/.#marker_point.l @@ -0,0 +1 @@ +amabe@ubuntu.17596:1638181128 \ No newline at end of file diff --git a/jsk_2021_fix_kxr/euslisp/maker_test/marker_point.l b/jsk_2021_fix_kxr/euslisp/maker_test/marker_point.l new file mode 100644 index 0000000000..9aebf7dba6 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/maker_test/marker_point.l @@ -0,0 +1,13 @@ +#!/usr/bin/env roseus + +(require "package://panda_eus/euslisp/dual_panda-interface.l") + +(ros::roseus-add-msgs "std_msgs") +(ros::roseus-add-msgs "sensor_msgs") +(ros::load-ros-manifest "sensor_msgs") + +(ros::roseus "marker_test") + +(dual_panda-init) + +(objects (list *robot*)) diff --git a/jsk_2021_fix_kxr/euslisp/rarmcoop_test/.#rarmcoop_test_main.l b/jsk_2021_fix_kxr/euslisp/rarmcoop_test/.#rarmcoop_test_main.l deleted file mode 120000 index b2b5064374..0000000000 --- a/jsk_2021_fix_kxr/euslisp/rarmcoop_test/.#rarmcoop_test_main.l +++ /dev/null @@ -1 +0,0 @@ -amabe@ubuntu.20910:1635125893 \ No newline at end of file diff --git a/jsk_2021_fix_kxr/launch/apriltag_dual_panda_second.launch b/jsk_2021_fix_kxr/launch/apriltag_dual_panda_second.launch new file mode 100644 index 0000000000..9079335e42 --- /dev/null +++ b/jsk_2021_fix_kxr/launch/apriltag_dual_panda_second.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/jsk_2021_fix_kxr/msg/Repair.msg b/jsk_2021_fix_kxr/msg/Repair.msg new file mode 100644 index 0000000000..516620a496 --- /dev/null +++ b/jsk_2021_fix_kxr/msg/Repair.msg @@ -0,0 +1,4 @@ +Header header +string pose +string servo +string process diff --git a/jsk_2021_fix_kxr/package.xml b/jsk_2021_fix_kxr/package.xml index 8800410caa..caf5952cfe 100644 --- a/jsk_2021_fix_kxr/package.xml +++ b/jsk_2021_fix_kxr/package.xml @@ -10,6 +10,15 @@ catkin + roscpp + rospy + std_msgs + message_generation + + roscpp + rospy + std_msgs + message_runtime fetcheus jsk_maps pr2eus From 856b5f2cb4c15c59660e9f774b325964ffd1594c Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 29 Nov 2021 18:00:06 +0900 Subject: [PATCH 051/127] adjast tags' size --- jsk_2021_fix_kxr/launch/apriltag_dual_panda_second.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsk_2021_fix_kxr/launch/apriltag_dual_panda_second.launch b/jsk_2021_fix_kxr/launch/apriltag_dual_panda_second.launch index 9079335e42..e391458b32 100644 --- a/jsk_2021_fix_kxr/launch/apriltag_dual_panda_second.launch +++ b/jsk_2021_fix_kxr/launch/apriltag_dual_panda_second.launch @@ -6,8 +6,8 @@ - - + + From 15bb56c82e8769d11238d1ac9997e21e15b15145 Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 1 Dec 2021 19:14:49 +0900 Subject: [PATCH 052/127] [fix_kxr] made marker_pickup.l --- .../euslisp/marker_test/marker_pickup.l | 60 +++++++++++++++++++ .../euslisp/marker_test/parameters.l | 5 ++ 2 files changed, 65 insertions(+) create mode 100644 jsk_2021_fix_kxr/euslisp/marker_test/marker_pickup.l create mode 100644 jsk_2021_fix_kxr/euslisp/marker_test/parameters.l diff --git a/jsk_2021_fix_kxr/euslisp/marker_test/marker_pickup.l b/jsk_2021_fix_kxr/euslisp/marker_test/marker_pickup.l new file mode 100644 index 0000000000..996dd7ecbc --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/marker_test/marker_pickup.l @@ -0,0 +1,60 @@ +#!/usr/bin/env roseus + +(require "package://panda_eus/euslisp/dual_panda-interface.l") +(require "package://jsk_2021_fix_kxr/euslisp/marker_test/parameters.l") + +(ros::roseus-add-msgs "std_msgs") + +(ros::load-ros-manifest "apriltag_ros") + +(ros::roseus "marker_test") + +(dual_panda-init) + +(objects (list *robot*)) +(defvar *dual_arm_base-to-marker-coords*) +(defvar *rarm_end_effector-to-marker-coords*) + +(defun grasp-marker (marker-name) + (let ((base-to-marker-origin nil) + (base-to-pre-grasp-pose nil) + (base-to-grasp-pose nil) + (stamp nil) + (ret nil)) + (ros::ros-info "grasp a marker") + (setq stamp (ros::time-now)) + (send *robot* :reset-pose) + (send *robot* :rarm :move-end-pos #f(-50 0 0)) + (send *robot* :larm :move-end-pos #f(-50 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 1500) + (send *ri* :wait-interpolation) + + (when (send *tfl* :wait-for-transform "dual_arm_base" marker-name stamp 5) + (setq base-to-marker-origin (send *tfl* :lookup-transform "dual_arm_base" marker-name stamp))) + (setq base-to-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *marker-to-grasp-pose*)) + + ;pre-grasp-pose + (send *robot* :rarm :inverse-kinematics base-to-grasp-pose) + (send *robot* :rarm :move-end-pos #f(-50 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + ;grasp-pose + (send *robot* :rarm :inverse-kinematics base-to-grasp-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 1500) + (send *ri* :wait-interpolation) + + (send *ri* :start-grasp :rarm) + (unix:sleep 1) + (send *ri* :stop-grasp :rarm) + (unix:sleep 1) + + (unix:sleep 1) + + ;return to start-pose + (send *robot* :reset-pose) + (send *robot* :rarm :move-end-pos #f(-50 0 0)) + (send *robot* :larm :move-end-pos #f(-50 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (ros::ros-info "finish") + )) diff --git a/jsk_2021_fix_kxr/euslisp/marker_test/parameters.l b/jsk_2021_fix_kxr/euslisp/marker_test/parameters.l new file mode 100644 index 0000000000..0643ab0236 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/marker_test/parameters.l @@ -0,0 +1,5 @@ +(setq *marker-to-grasp-pose* + (make-cascoords + :pos #f(12.503 -38.046 48.706) + :rpy #f(2.517 1.299 -2.2) + )) From f45bd5a34a10e85652753255c94791a29b692ee3 Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 1 Dec 2021 19:19:28 +0900 Subject: [PATCH 053/127] [fix_kxr_apriltag] changed 'size' from diameter to radius --- jsk_2021_fix_kxr/apriltag_setting/tags.yaml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/jsk_2021_fix_kxr/apriltag_setting/tags.yaml b/jsk_2021_fix_kxr/apriltag_setting/tags.yaml index ba47c25bcf..c767767ce7 100644 --- a/jsk_2021_fix_kxr/apriltag_setting/tags.yaml +++ b/jsk_2021_fix_kxr/apriltag_setting/tags.yaml @@ -19,7 +19,9 @@ # ] standalone_tags: [ - {id: 000, size: 0.014, name: test_000}, + {id: 000, size: 0.0075, name: test_000}, + {id: 001, size: 0.0075, name: test_001}, + {id: 002, size: 0.0075, name: test_002}, ] # ## Tag bundle definitions # ### Remarks From bddc32c75557f403590978b1241e4242aedd144c Mon Sep 17 00:00:00 2001 From: softyanija Date: Thu, 2 Dec 2021 10:56:07 +0900 Subject: [PATCH 054/127] deleted maker_test/ --- .../euslisp/maker_test/.#marker_point.l | 1 - jsk_2021_fix_kxr/euslisp/maker_test/marker_point.l | 13 ------------- 2 files changed, 14 deletions(-) delete mode 120000 jsk_2021_fix_kxr/euslisp/maker_test/.#marker_point.l delete mode 100644 jsk_2021_fix_kxr/euslisp/maker_test/marker_point.l diff --git a/jsk_2021_fix_kxr/euslisp/maker_test/.#marker_point.l b/jsk_2021_fix_kxr/euslisp/maker_test/.#marker_point.l deleted file mode 120000 index 2a920f3522..0000000000 --- a/jsk_2021_fix_kxr/euslisp/maker_test/.#marker_point.l +++ /dev/null @@ -1 +0,0 @@ -amabe@ubuntu.17596:1638181128 \ No newline at end of file diff --git a/jsk_2021_fix_kxr/euslisp/maker_test/marker_point.l b/jsk_2021_fix_kxr/euslisp/maker_test/marker_point.l deleted file mode 100644 index 9aebf7dba6..0000000000 --- a/jsk_2021_fix_kxr/euslisp/maker_test/marker_point.l +++ /dev/null @@ -1,13 +0,0 @@ -#!/usr/bin/env roseus - -(require "package://panda_eus/euslisp/dual_panda-interface.l") - -(ros::roseus-add-msgs "std_msgs") -(ros::roseus-add-msgs "sensor_msgs") -(ros::load-ros-manifest "sensor_msgs") - -(ros::roseus "marker_test") - -(dual_panda-init) - -(objects (list *robot*)) From 29dc0d76f38e719027aaf619c2748cd5d7863bd3 Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 3 Dec 2021 18:36:47 +0900 Subject: [PATCH 055/127] [fix_kxr] start to make grasp_cable demo --- .../euslisp/grasp_cable/parameters.l | 30 ++++++++++ jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l | 57 +++++++++++++++++++ .../euslisp/marker_test/marker_pickup.l | 1 + 3 files changed, 88 insertions(+) create mode 100644 jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l create mode 100644 jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l new file mode 100644 index 0000000000..f7990a4992 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l @@ -0,0 +1,30 @@ +#!/usr/bin/env roseus + +;;; parameters for cable +(setq *front-of-cable-pose* + (make-cascoords + :pos #f(1.07 -100.66 -39.27) + :rpy #f(1.563 0.025 3.122) + )) +(setq *cable-front-to-pre-grasp-pose* + (make-cascoords + :pos #f(-37.691 11.443 -7.628) + :rpy #f(-1.374 0.172 1.421) + )) + +(setq *cable-front-to-grasp-pose* + (make-cascoords + :pos #f(-21.262 11.534 -8.893) + :rpy #f(-1.401 0.177 1.409) + )) + +(setq *cable-frame* "cable-frame") + + + +;;; parameters for servo +(setq *servo-to-servo-waiting-pose* + (make-cascoords + :pos #f(12.503 -38.046 48.706) + :rpy #f(2.517 1.299 -2.2) + )) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l new file mode 100644 index 0000000000..17b93c5c0c --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l @@ -0,0 +1,57 @@ +#!/usr/bin/env roseus + +(require "package://panda_eus/euslisp/dual_panda-interface.l") +(require "package://jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l") +(ros::roseus-add-msgs "std_msgs") + +(ros::load-ros-manifest "apriltag_ros") + +(ros::roseus "grasp_cable") + +(dual_panda-init) + +(objects (list *robot*)) + +(send *robot* :reset-pose) +(send *robot* :rarm :move-end-pos #f(-500 0 0)) +(send *robot* :larm :move-end-pos #f(-500 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(350.616 -340 1100) :rpy (float-vector 0.0 1.0 2.96))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(defun rarm-grasp-cable (servo-name) + (let ((base-to-marker-origin nil) + (base-to-pre-prasp-pose nil) + (base-to-grasp-pose nil) + (stamp nil) + (ret nil)) + (ros::ros-info "grasp cable") + (setq stamp (ros::time-now)) + (setq marker-name "cable_front") + (when (send *tfl* :wait-for-transform "dual_arm_base" marker-name stamp 5) + (setq base-to-marker-origin (send *tfl* :lookup-transform "dual_arm_base" marker-name stamp))) + (setq base-to-pre-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *cable-front-to-pre-grasp-pose*)) + (setq base-to-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *cable-front-to-grasp-pose*)) + + ;pre-grasp-pose + (send *ri* :start-grasp :rarm) + (unix:sleep 1) + (send *ri* :stop-grasp :rarm :width 0.02) + (unix:sleep 1) + + (send *robot* :rarm :inverse-kinematics base-to-pre-grasp-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + ;grasp-pose + (send *robot* :rarm :inverse-kinematics base-to-grasp-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 1500) + (send *ri* :wait-interpolation) + + (send *ri* :start-grasp :rarm) + (unix:sleep 2) + (send *ri* :stop-grasp :rarm) + (unix:sleep 2) diff --git a/jsk_2021_fix_kxr/euslisp/marker_test/marker_pickup.l b/jsk_2021_fix_kxr/euslisp/marker_test/marker_pickup.l index 996dd7ecbc..df807aeb24 100644 --- a/jsk_2021_fix_kxr/euslisp/marker_test/marker_pickup.l +++ b/jsk_2021_fix_kxr/euslisp/marker_test/marker_pickup.l @@ -58,3 +58,4 @@ (send *ri* :wait-interpolation) (ros::ros-info "finish") )) + From 167a6c9df183a0d40b315a4be10772fd67362d78 Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 7 Dec 2021 14:27:35 +0900 Subject: [PATCH 056/127] [fix_kxr] add ckeck-camera function --- jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l | 23 +++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l index 17b93c5c0c..b1724ddb1d 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l @@ -12,6 +12,10 @@ (objects (list *robot*)) +(defvar *marker-000*) +(defvar *marker-001*) +(defvar *marker-002*) + (send *robot* :reset-pose) (send *robot* :rarm :move-end-pos #f(-500 0 0)) (send *robot* :larm :move-end-pos #f(-500 0 0)) @@ -54,4 +58,21 @@ (send *ri* :start-grasp :rarm) (unix:sleep 2) (send *ri* :stop-grasp :rarm) - (unix:sleep 2) + (unix:sleep 2))) + + +(defun check-camera () + (let ((base-to-marker-000 nil) + (base-to-marker-001 nil) + (base-to-marker-002 nil) + (stamp nil) + (ret nil)) + (ros::ros-info "grasp cable") + (setq stamp (ros::time-now)) + (setq marker-name "cable_front") + (when (send *tfl* :wait-for-transform "dual_arm_base" "test_000" stamp 5) + (setq *marker-000* (send *tfl* :lookup-transform "dual_arm_base" "test_000" stamp))) + (when (send *tfl* :wait-for-transform "dual_arm_base" "test_000" stamp 5) + (setq *marker-001* (send *tfl* :lookup-transform "dual_arm_base" "test_001" stamp))) + (when (send *tfl* :wait-for-transform "dual_arm_base" "test_001" stamp 5) + (setq *marker-002* (send *tfl* :lookup-transform "dual_arm_base" "test_002" stamp))))) From a47afc662026cf2e0cc64dee39293dee65ae316e Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 8 Dec 2021 18:20:27 +0900 Subject: [PATCH 057/127] [fix_kxr_apriltag] change tag from 21circle to 36h11 --- jsk_2021_fix_kxr/apriltag_setting/settings.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsk_2021_fix_kxr/apriltag_setting/settings.yaml b/jsk_2021_fix_kxr/apriltag_setting/settings.yaml index 15859c22a1..0fa73996e0 100644 --- a/jsk_2021_fix_kxr/apriltag_setting/settings.yaml +++ b/jsk_2021_fix_kxr/apriltag_setting/settings.yaml @@ -1,11 +1,11 @@ # AprilTag 3 code parameters # Find descriptions in apriltag/include/apriltag.h:struct apriltag_detector # apriltag/include/apriltag.h:struct apriltag_family -tag_family: 'tagCircle21h7' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12 +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: 0 # default: 0 +tag_debug: 1 # default: 0 # Other parameters publish_tf: true # default: false From da6731dcdfa872870a533dd66aad922b5f946045 Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 8 Dec 2021 18:46:11 +0900 Subject: [PATCH 058/127] [fix_kxr_apriltag] 002 of tag36h11 can be recognized --- jsk_2021_fix_kxr/apriltag_setting/tags.yaml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/jsk_2021_fix_kxr/apriltag_setting/tags.yaml b/jsk_2021_fix_kxr/apriltag_setting/tags.yaml index c767767ce7..8d9427a0ec 100644 --- a/jsk_2021_fix_kxr/apriltag_setting/tags.yaml +++ b/jsk_2021_fix_kxr/apriltag_setting/tags.yaml @@ -19,9 +19,7 @@ # ] standalone_tags: [ - {id: 000, size: 0.0075, name: test_000}, - {id: 001, size: 0.0075, name: test_001}, - {id: 002, size: 0.0075, name: test_002}, + {id: 002, size: 0.0153, name: rarm_elbow_p_servo}, ] # ## Tag bundle definitions # ### Remarks From 4d5a205c63b30934e90b05cf1152cdb306ee5474 Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 8 Dec 2021 19:19:45 +0900 Subject: [PATCH 059/127] [fix_kxr_apriltag] add max_hamming_dist --- jsk_2021_fix_kxr/apriltag_setting/settings.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/jsk_2021_fix_kxr/apriltag_setting/settings.yaml b/jsk_2021_fix_kxr/apriltag_setting/settings.yaml index 0fa73996e0..33a67af1d3 100644 --- a/jsk_2021_fix_kxr/apriltag_setting/settings.yaml +++ b/jsk_2021_fix_kxr/apriltag_setting/settings.yaml @@ -7,5 +7,6 @@ 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 From 9dfddb79774a1b6ad5752b2b2f28422c6a84f133 Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 8 Dec 2021 19:20:49 +0900 Subject: [PATCH 060/127] [fix_kxr_apriltag] add some tag info --- jsk_2021_fix_kxr/apriltag_setting/tags.yaml | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/jsk_2021_fix_kxr/apriltag_setting/tags.yaml b/jsk_2021_fix_kxr/apriltag_setting/tags.yaml index 8d9427a0ec..d500faf0ea 100644 --- a/jsk_2021_fix_kxr/apriltag_setting/tags.yaml +++ b/jsk_2021_fix_kxr/apriltag_setting/tags.yaml @@ -19,7 +19,13 @@ # ] standalone_tags: [ - {id: 002, size: 0.0153, name: rarm_elbow_p_servo}, + {id: 2, size: 0.0153, name: 'rarm_elbow_p_servo'}, + {id: 3, size: 0.0153, name: 'rarm_shoulder_r_servo'}, + {id: 4, size: 0.0153, name: 'rleg_ankle_r_servo'}, + {id: 8, size: 0.0052, name: 'rarm_elbow_p_cable_r1'}, + {id: 9, size: 0.0052, name: 'rarm_elbow_p_cable_r2'}, + {id: 10, size: 0.0052, name: 'rarm_elbow_p_cable_l1'}, + {id: 11, size: 0.0052, name: 'rarm_elbow_p_cable_l2'}, ] # ## Tag bundle definitions # ### Remarks From 909c9b61dce1e2840fccc1d97508dcc8fc685f14 Mon Sep 17 00:00:00 2001 From: softyanija Date: Thu, 9 Dec 2021 14:45:44 +0900 Subject: [PATCH 061/127] change name apriltag_setting/ to config/ --- .../config/apriltag/settings.yaml | 12 +++++ .../config/apriltag/tags_for_left.yaml | 53 +++++++++++++++++++ .../config/apriltag/tags_for_right.yaml | 53 +++++++++++++++++++ 3 files changed, 118 insertions(+) create mode 100644 jsk_2021_fix_kxr/config/apriltag/settings.yaml create mode 100644 jsk_2021_fix_kxr/config/apriltag/tags_for_left.yaml create mode 100644 jsk_2021_fix_kxr/config/apriltag/tags_for_right.yaml diff --git a/jsk_2021_fix_kxr/config/apriltag/settings.yaml b/jsk_2021_fix_kxr/config/apriltag/settings.yaml new file mode 100644 index 0000000000..33a67af1d3 --- /dev/null +++ b/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 diff --git a/jsk_2021_fix_kxr/config/apriltag/tags_for_left.yaml b/jsk_2021_fix_kxr/config/apriltag/tags_for_left.yaml new file mode 100644 index 0000000000..769fe02ecc --- /dev/null +++ b/jsk_2021_fix_kxr/config/apriltag/tags_for_left.yaml @@ -0,0 +1,53 @@ +# # 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: 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: 8, size: 0.0052, name: 'lpov-rarm_elbow_p_cable_r1'}, + {id: 9, size: 0.0052, name: 'lpov-rarm_elbow_p_cable_r2'}, + {id: 10, size: 0.0052, name: 'lpov-rarm_elbow_p_cable_l1'}, + {id: 11, size: 0.0052, name: 'lpov-rarm_elbow_p_cable_l2'}, + ] +# ## 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: + [ + ] diff --git a/jsk_2021_fix_kxr/config/apriltag/tags_for_right.yaml b/jsk_2021_fix_kxr/config/apriltag/tags_for_right.yaml new file mode 100644 index 0000000000..bee77f42bf --- /dev/null +++ b/jsk_2021_fix_kxr/config/apriltag/tags_for_right.yaml @@ -0,0 +1,53 @@ +# # 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: 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: 8, size: 0.0052, name: 'rpov-rarm_elbow_p_cable_r1'}, + {id: 9, size: 0.0052, name: 'rpov-rarm_elbow_p_cable_r2'}, + {id: 10, size: 0.0052, name: 'rpov-rarm_elbow_p_cable_l1'}, + {id: 11, size: 0.0052, name: 'rpov-rarm_elbow_p_cable_l2'}, + ] +# ## 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: + [ + ] From 417c132db9bd2849bf084ee52f4c144529d467ad Mon Sep 17 00:00:00 2001 From: softyanija Date: Thu, 9 Dec 2021 14:50:44 +0900 Subject: [PATCH 062/127] write launch of both right and left --- .../apriltag_setting/settings.yaml | 12 ----- jsk_2021_fix_kxr/apriltag_setting/tags.yaml | 53 ------------------- ...unch => apriltag_doctor_panda_left.launch} | 13 +++-- ...nch => apriltag_doctor_panda_right.launch} | 8 +-- 4 files changed, 13 insertions(+), 73 deletions(-) delete mode 100644 jsk_2021_fix_kxr/apriltag_setting/settings.yaml delete mode 100644 jsk_2021_fix_kxr/apriltag_setting/tags.yaml rename jsk_2021_fix_kxr/launch/{apriltag_continuous_detection.launch => apriltag_doctor_panda_left.launch} (53%) rename jsk_2021_fix_kxr/launch/{apriltag_dual_panda_second.launch => apriltag_doctor_panda_right.launch} (71%) diff --git a/jsk_2021_fix_kxr/apriltag_setting/settings.yaml b/jsk_2021_fix_kxr/apriltag_setting/settings.yaml deleted file mode 100644 index 33a67af1d3..0000000000 --- a/jsk_2021_fix_kxr/apriltag_setting/settings.yaml +++ /dev/null @@ -1,12 +0,0 @@ -# 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 diff --git a/jsk_2021_fix_kxr/apriltag_setting/tags.yaml b/jsk_2021_fix_kxr/apriltag_setting/tags.yaml deleted file mode 100644 index d500faf0ea..0000000000 --- a/jsk_2021_fix_kxr/apriltag_setting/tags.yaml +++ /dev/null @@ -1,53 +0,0 @@ -# # 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: 2, size: 0.0153, name: 'rarm_elbow_p_servo'}, - {id: 3, size: 0.0153, name: 'rarm_shoulder_r_servo'}, - {id: 4, size: 0.0153, name: 'rleg_ankle_r_servo'}, - {id: 8, size: 0.0052, name: 'rarm_elbow_p_cable_r1'}, - {id: 9, size: 0.0052, name: 'rarm_elbow_p_cable_r2'}, - {id: 10, size: 0.0052, name: 'rarm_elbow_p_cable_l1'}, - {id: 11, size: 0.0052, name: 'rarm_elbow_p_cable_l2'}, - ] -# ## 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: - [ - ] diff --git a/jsk_2021_fix_kxr/launch/apriltag_continuous_detection.launch b/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_left.launch similarity index 53% rename from jsk_2021_fix_kxr/launch/apriltag_continuous_detection.launch rename to jsk_2021_fix_kxr/launch/apriltag_doctor_panda_left.launch index 6febd2045e..02a158bca3 100644 --- a/jsk_2021_fix_kxr/launch/apriltag_continuous_detection.launch +++ b/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_left.launch @@ -1,20 +1,23 @@ - - - + + + - - + + + + + diff --git a/jsk_2021_fix_kxr/launch/apriltag_dual_panda_second.launch b/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_right.launch similarity index 71% rename from jsk_2021_fix_kxr/launch/apriltag_dual_panda_second.launch rename to jsk_2021_fix_kxr/launch/apriltag_doctor_panda_right.launch index e391458b32..568005421c 100644 --- a/jsk_2021_fix_kxr/launch/apriltag_dual_panda_second.launch +++ b/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_right.launch @@ -1,18 +1,20 @@ - + - - + + + + From 01e36c69dce975408f18b9813d528c5ea32e7fa9 Mon Sep 17 00:00:00 2001 From: softyanija Date: Thu, 9 Dec 2021 18:31:56 +0900 Subject: [PATCH 063/127] [euslisp/grasp_cable] start to make grasp_cable-demo --- .../euslisp/grasp_cable/grasp_cable.l | 39 +++++++++++++++++++ jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l | 17 +------- 2 files changed, 40 insertions(+), 16 deletions(-) create mode 100644 jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l new file mode 100644 index 0000000000..1d6c4c0c30 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l @@ -0,0 +1,39 @@ +#!/usr/bin/env roseus + +(require "package://panda_eus/euslisp/dual_panda-interface.l") +(require "package://jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l") +(require "package://jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l") + +(ros::roseus-add-msgs "std_msgs") + +(ros::load-ros-manifest "apriltag_ros") + +(ros::roseus "grasp_cable") + +(dual_panda-init) + +(objects (list *robot*)) + +(defvar *marker-000*) +(defvar *marker-001*) +(defvar *marker-002*) + +(send *robot* :reset-pose) +(send *robot* :rarm :move-end-pos #f(-500 0 100)) +(send *robot* :larm :move-end-pos #f(-500 0 100)) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +;;before rarm grasp kxr +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(277.724 510.724 1373.929) :rpy (float-vector -0.827 1.457 2.009))) :translation-axis t :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(341.18 -136.643 1312.764) :rpy (float-vector -0.031 1.556 -1.602))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +;;lower rarm +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(277.724 510.724 1443.929) :rpy (float-vector -0.827 1.457 2.009))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *ri* start-grasp :rarm) +(unix:sleep 1) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l index b1724ddb1d..bdb1394145 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l @@ -2,30 +2,15 @@ (require "package://panda_eus/euslisp/dual_panda-interface.l") (require "package://jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l") + (ros::roseus-add-msgs "std_msgs") (ros::load-ros-manifest "apriltag_ros") -(ros::roseus "grasp_cable") - -(dual_panda-init) - -(objects (list *robot*)) - (defvar *marker-000*) (defvar *marker-001*) (defvar *marker-002*) -(send *robot* :reset-pose) -(send *robot* :rarm :move-end-pos #f(-500 0 0)) -(send *robot* :larm :move-end-pos #f(-500 0 0)) -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) - -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(350.616 -340 1100) :rpy (float-vector 0.0 1.0 2.96))) :translation-axis t :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -(send *ri* :wait-interpolation) - (defun rarm-grasp-cable (servo-name) (let ((base-to-marker-origin nil) (base-to-pre-prasp-pose nil) From 0fe3860d00b5063decffae4c2b7e651567592175 Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 10 Dec 2021 16:57:27 +0900 Subject: [PATCH 064/127] [grasp_cable] get parameters of grasp rarm-elbow-p from below --- .../euslisp/grasp_cable/grasp_cable.l | 14 ++--- .../euslisp/grasp_cable/parameters.l | 24 +++++---- jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l | 51 +++++++++++++++++++ 3 files changed, 73 insertions(+), 16 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l index 1d6c4c0c30..6669c84726 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l @@ -14,9 +14,6 @@ (objects (list *robot*)) -(defvar *marker-000*) -(defvar *marker-001*) -(defvar *marker-002*) (send *robot* :reset-pose) (send *robot* :rarm :move-end-pos #f(-500 0 100)) @@ -25,15 +22,20 @@ (send *ri* :wait-interpolation) ;;before rarm grasp kxr -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(277.724 510.724 1373.929) :rpy (float-vector -0.827 1.457 2.009))) :translation-axis t :rotation-axis t) -(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(341.18 -136.643 1312.764) :rpy (float-vector -0.031 1.556 -1.602))) :translation-axis t :rotation-axis t) +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(341.012 -147.428 1370.354) :rpy (float-vector 1.222 1.551 -0.386))) :translation-axis t :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(227.932 139.233 1435.999) :rpy (float-vector -0.09 1.312 2.977))) :translation-axis t :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) + ;;lower rarm -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(277.724 510.724 1443.929) :rpy (float-vector -0.827 1.457 2.009))) :translation-axis t :rotation-axis t) +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(341.012 -147.428 1301.354) :rpy (float-vector 1.222 1.551 -0.386))) :translation-axis t :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) +;grasp kxr (send *ri* start-grasp :rarm) (unix:sleep 1) + +;;grasp elbow from below +(larm-grasp-rarm-elbow-p-servo-from-below) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l index f7990a4992..b53181716a 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l @@ -1,21 +1,25 @@ #!/usr/bin/env roseus ;;; parameters for cable -(setq *front-of-cable-pose* +(setq *rarm-elbow-p-to-inter1-pose* (make-cascoords - :pos #f(1.07 -100.66 -39.27) - :rpy #f(1.563 0.025 3.122) + :pos #f(-168.038 -3.379 89.959) + :rpy #f(0.042 -0.008 1.539) )) -(setq *cable-front-to-pre-grasp-pose* +(setq *rarm-elbow-p-to-inter2-pose* (make-cascoords - :pos #f(-37.691 11.443 -7.628) - :rpy #f(-1.374 0.172 1.421) + :pos #f(-141.254 -5.393 -96.748) + :rpy #f(0.024 -0.937 -0.027) )) - -(setq *cable-front-to-grasp-pose* +(setq *rarm-elbow-p-to-pre-grasp-pose* + (make-cascoords + :pos #f(-28.257 -2.82 -95.35) + :rpy #f(-2.973 -1.515 2.986) + )) +(setq *rarm-elbow-p-to-grasp-pose* (make-cascoords - :pos #f(-21.262 11.534 -8.893) - :rpy #f(-1.401 0.177 1.409) + :pos #f(-24.612 4.967 -27.886) + :rpy #f(-2.962 -1.494 3.004) )) (setq *cable-frame* "cable-frame") diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l index bdb1394145..706da2bc4e 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l @@ -11,6 +11,57 @@ (defvar *marker-001*) (defvar *marker-002*) +(defun larm-grasp-rarm-elbow-p-servo-from-below () + (let ((base-to-marker-origin nil) + (base-to-inter1-pose nil) + (base-to-inter2-pose nil) + (base-to-inter3-pose nil) + (base-to-pre-grasp-pose nil) + (base-to-grasp-pose nil) + (stamp nil) + (ret nil)) + (ros::ros-info "start to grasp rarm-elbow-p-servo from below") + (setq stamp (ros::time-now)) + (setq marker-name "lpov-rarm_elbow_p_servo") + (when (send *tfl* :wait-for-transform "dual_arm_base" marker-name stamp 5) + (setq base-to-marker-origin (send *tfl* :lookup-transform "dual_arm_base" marker-name stamp))) + (setq base-to-inter1-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *rarm-elbow-p-to-inter1-pose*)) + (setq base-to-inter2-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *rarm-elbow-p-to-inter2-pose*)) + (setq base-to-pre-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *rarm-elbow-p-to-pre-grasp-pose*)) + (setq base-to-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *rarm-elbow-p-to-grasp-pose*)) + + (send *ri* :stop-grasp :larm) + (unix:sleep 1) + + ;;move larm under rarm-elbow-p-servo + (send *robot* :larm :inverse-kinematics base-to-inter1-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *robot* :larm :inverse-kinematics base-to-inter2-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *robot* :larm :inverse-kinematics base-to-pre-grasp-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *robot* :larm :inverse-kinematics base-to-grasp-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *ri* :start-grasp :larm) + (unix:sleep 1))) + +#| +memo to get parameters +(send *robot* :angle-vector (send *ri* :state :potentio-vector)) +(setq btgp (send *robot* :larm :end-coords)) +(setq param (send (send (send base-to-marker-origin :inverse-transformation) :copy-worldcoords) :transform (send btgp :copy-worldcoords))) +(setq test (send (send base-to-marker-origin :copy-worldcoords) :transform param)) +(setq param (send (send marker-to-base :copy-worldcoords) :transform (send btgp :copy-worldcoords))) + +|# (defun rarm-grasp-cable (servo-name) (let ((base-to-marker-origin nil) (base-to-pre-prasp-pose nil) From b1f7849e9767154e58a886077a689e725055311a Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 10 Dec 2021 18:50:10 +0900 Subject: [PATCH 065/127] [euslisp/grasp_cable] make larm-grasp-rarm-elbow-p-servo-from-below function --- .../euslisp/grasp_cable/grasp_cable.l | 18 +++++++++++-- .../euslisp/grasp_cable/parameters.l | 27 ++++++++++++++++++- jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l | 6 ++++- 3 files changed, 47 insertions(+), 4 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l index 6669c84726..a58a24bd8d 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l @@ -23,6 +23,7 @@ ;;before rarm grasp kxr (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(341.012 -147.428 1370.354) :rpy (float-vector 1.222 1.551 -0.386))) :translation-axis t :rotation-axis t) +(send *robot* :larm_joint1 :joint-angle -30) (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(227.932 139.233 1435.999) :rpy (float-vector -0.09 1.312 2.977))) :translation-axis t :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) @@ -33,9 +34,22 @@ (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) -;grasp kxr -(send *ri* start-grasp :rarm) +;;grasp kxr +(send *ri* :start-grasp :rarm) (unix:sleep 1) ;;grasp elbow from below (larm-grasp-rarm-elbow-p-servo-from-below) + +(send *ri* :stop-grasp :rarm) + +;;prepare to grasp cable +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(341.012 -147.428 1430.354) :rpy (float-vector 1.222 1.551 -0.386))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(setq search-tag-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *rarm-elbow-p-to-search-cable-pose*)) +(send *robot* :rarm_joint1 :joint-angle 30) +(send *robot* :rarm :inverse-kinematics search-tag-pose) + +(search-servo-tag "rpov-rarm_elbow_p_servo") diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l index b53181716a..a376a0107e 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l @@ -1,6 +1,31 @@ #!/usr/bin/env roseus -;;; parameters for cable +(defvar *base-to-rarm-elbow-p-servo*) + + + +;;; parameters +(setq *rarm-elbow-p-to-search-cable-pose* + (make-cascoords + :pos #f(-3.797 -98.159 34.853) + :rpy #f(1.549 0.916 3.105) + )) +(setq *rarm-elbow-p-to-search-tag-pose-1* + (make-cascoords + :pos #f(1.133 -85.27 61.446) + :rpy #f(1.581 1.228 3.132) + )) +(setq *rarm-elbow-p-to-search-tag-pose-2* + (make-cascoords + :pos #f(1.898 -42.878 78.671) + :rpy #f(-1.931 1.548 -0.355) + )) +(setq *rarm-elbow-p-to-search-tag-pose-3* + (make-cascoords + :pos #f(2.51 -45.813 49.055) + :rpy #f(-1.675 1.106 -0.081) + )) + (setq *rarm-elbow-p-to-inter1-pose* (make-cascoords :pos #f(-168.038 -3.379 89.959) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l index 706da2bc4e..a8baeac6fc 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l @@ -11,6 +11,9 @@ (defvar *marker-001*) (defvar *marker-002*) +(defun srearch-servo-tag (marker-name) + + (defun larm-grasp-rarm-elbow-p-servo-from-below () (let ((base-to-marker-origin nil) (base-to-inter1-pose nil) @@ -24,7 +27,8 @@ (setq stamp (ros::time-now)) (setq marker-name "lpov-rarm_elbow_p_servo") (when (send *tfl* :wait-for-transform "dual_arm_base" marker-name stamp 5) - (setq base-to-marker-origin (send *tfl* :lookup-transform "dual_arm_base" marker-name stamp))) + (setq base-to-marker-origin (send *tfl* :lookup-transform "dual_arm_base" marker-name stamp)) + (setq *base-to-rarm-elbow-p-servo* base-to-marker-origin)) (setq base-to-inter1-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *rarm-elbow-p-to-inter1-pose*)) (setq base-to-inter2-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *rarm-elbow-p-to-inter2-pose*)) (setq base-to-pre-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *rarm-elbow-p-to-pre-grasp-pose*)) From 242465d0306b998a32b8aaf4a071e3fb5a6f5acf Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 10 Dec 2021 20:41:16 +0900 Subject: [PATCH 066/127] [euslisp/grasp_cable] make rarm-srearch-servo-tag and func which support this --- .../euslisp/grasp_cable/grasp_cable.l | 2 +- jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l | 55 +++++++++++++++++-- 2 files changed, 51 insertions(+), 6 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l index a58a24bd8d..dd5258f72d 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l @@ -52,4 +52,4 @@ (send *robot* :rarm_joint1 :joint-angle 30) (send *robot* :rarm :inverse-kinematics search-tag-pose) -(search-servo-tag "rpov-rarm_elbow_p_servo") +(rarm-search-servo-tag "rpov-rarm_elbow_p_servo") diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l index a8baeac6fc..9d5639d2c1 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l @@ -7,11 +7,56 @@ (ros::load-ros-manifest "apriltag_ros") -(defvar *marker-000*) -(defvar *marker-001*) -(defvar *marker-002*) - -(defun srearch-servo-tag (marker-name) +;;; check if an id exists in target-list +(defun id-in-list (id target-list) + (dolist (x target-list) + (if (eq id x) + (return-from id-in-list t) + nil)) + nil) + +(defun marker-name-to-id (marker-name) + (if (string= marker-name "rpov-rarm_elbow_p_servo") + (print 2)) + (if (string= marker-name "lpov-rarm_elbow_p_servo") + (print 2))) + +(defun rarm-srearch-servo-tag (marker-name) + (let ((detection-results-msg nil) + (target-id nil) + (id-list nil) + (base-to-search-pose nil) + (count 0)) + (ros::ros-info "searching tag...") + (setq target-id (marker-name-to-id marker-name)) + (while t + (setq detection-results-msg (one-shot-subscribe "/right_tag_detections" apriltag_ros::AprilTagDetectionArray)) + (when detection-results-msg + (ros::ros-info "Got a message") + (setq id-list (mapcar #'(lambda (x) (elt (send x :id) 0)) (send detection-results-msg :detections))) + (if (id-in-list tesrget-id) + (progn + (ros::ros-info "target-marker was found") + (return from rarm-srearch-servo-tag t)) + (progn + (setq count (+ count 1)) + (case count + (1 + (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *rarm-elbow-p-to-search-tag-pose-1*)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation)) + (2 + (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *rarm-elbow-p-to-search-tag-pose-2*)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation)) + (3 + (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *rarm-elbow-p-to-search-tag-pose-3*)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation)) + (4 + (ros::ros-info "Can't find tag") + (return from rarm-srearch-servo-tag t))))))))) + (defun larm-grasp-rarm-elbow-p-servo-from-below () From ad970d92899d77e4eae18a4dfcaed80de171dd27 Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 13 Dec 2021 15:20:38 +0900 Subject: [PATCH 067/127] [euslisp/grasp_cable] complete rarm-search-servo-tag --- .../euslisp/grasp_cable/grasp_cable.l | 2 ++ .../euslisp/grasp_cable/parameters.l | 2 +- jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l | 22 +++++++++++-------- 3 files changed, 16 insertions(+), 10 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l index dd5258f72d..a6598f99b5 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l @@ -51,5 +51,7 @@ (setq search-tag-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *rarm-elbow-p-to-search-cable-pose*)) (send *robot* :rarm_joint1 :joint-angle 30) (send *robot* :rarm :inverse-kinematics search-tag-pose) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) (rarm-search-servo-tag "rpov-rarm_elbow_p_servo") diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l index a376a0107e..75e80059c9 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l @@ -1,7 +1,7 @@ #!/usr/bin/env roseus (defvar *base-to-rarm-elbow-p-servo*) - +(defvar *target-id*) ;;; parameters diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l index 9d5639d2c1..21ece7d2b2 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l @@ -17,47 +17,51 @@ (defun marker-name-to-id (marker-name) (if (string= marker-name "rpov-rarm_elbow_p_servo") - (print 2)) + (setq *target-id* 2) (if (string= marker-name "lpov-rarm_elbow_p_servo") - (print 2))) + (setq *target-id* 2)))) -(defun rarm-srearch-servo-tag (marker-name) +(defun rarm-search-servo-tag (marker-name) (let ((detection-results-msg nil) (target-id nil) (id-list nil) (base-to-search-pose nil) (count 0)) (ros::ros-info "searching tag...") - (setq target-id (marker-name-to-id marker-name)) + (marker-name-to-id marker-name) + (setq target-id *target-id*) + (format t "target-id is ~d~%" target-id) (while t (setq detection-results-msg (one-shot-subscribe "/right_tag_detections" apriltag_ros::AprilTagDetectionArray)) (when detection-results-msg (ros::ros-info "Got a message") (setq id-list (mapcar #'(lambda (x) (elt (send x :id) 0)) (send detection-results-msg :detections))) - (if (id-in-list tesrget-id) + (print id-list) + (if (id-in-list target-id id-list) (progn (ros::ros-info "target-marker was found") - (return from rarm-srearch-servo-tag t)) + (return-from rarm-search-servo-tag t)) (progn (setq count (+ count 1)) (case count (1 (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *rarm-elbow-p-to-search-tag-pose-1*)) + (send *robot* :rarm :inverse-kinematics base-to-search-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation)) (2 (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *rarm-elbow-p-to-search-tag-pose-2*)) + (send *robot* :rarm :inverse-kinematics base-to-search-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation)) (3 (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *rarm-elbow-p-to-search-tag-pose-3*)) + (send *robot* :rarm :inverse-kinematics base-to-search-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation)) (4 (ros::ros-info "Can't find tag") - (return from rarm-srearch-servo-tag t))))))))) - - + (return-from rarm-search-servo-tag nil))))))))) (defun larm-grasp-rarm-elbow-p-servo-from-below () (let ((base-to-marker-origin nil) From 82344f9c2df6f1db04b0fdfb98d73f503692f09f Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 13 Dec 2021 18:39:57 +0900 Subject: [PATCH 068/127] [euslisp/grasp_cable] preparing to grasp cable --- .../euslisp/grasp_cable/grasp_cable.l | 15 ++- .../euslisp/grasp_cable/parameters.l | 106 ++++++++++++------ jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l | 74 ++++++++++-- 3 files changed, 150 insertions(+), 45 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l index a6598f99b5..46c301906d 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l @@ -14,7 +14,6 @@ (objects (list *robot*)) - (send *robot* :reset-pose) (send *robot* :rarm :move-end-pos #f(-500 0 100)) (send *robot* :larm :move-end-pos #f(-500 0 100)) @@ -48,10 +47,22 @@ (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) -(setq search-tag-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *rarm-elbow-p-to-search-cable-pose*)) +(setq search-tag-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *servo-to-search-cable-pose*)) (send *robot* :rarm_joint1 :joint-angle 30) (send *robot* :rarm :inverse-kinematics search-tag-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) (rarm-search-servo-tag "rpov-rarm_elbow_p_servo") +(setq stamp (ros::time-now)) +(when (send *tfl* :wait-for-transform "dual_arm_base" marker-name stamp 5) + (setq *base-to-rarm-elbow-p-servo* (send *tfl* :lookup-transform "dual_arm_base" "rpov-rarm_elbow_p_servo" stamp))) + +(setq search-tag-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *servo-to-search-cable-pose*)) +(send *robot* :rarm_joint1 :joint-angle 30) +(send *robot* :rarm :inverse-kinematics search-tag-pose) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +;; +(rarm-grasp-cable "rpov-rarm_elbow_p_servo" "right") diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l index 75e80059c9..523659fdaf 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l @@ -2,50 +2,92 @@ (defvar *base-to-rarm-elbow-p-servo*) (defvar *target-id*) +(defvar *target-id-list*) + +(defun marker-name-to-servo-id (marker-name) + (setq marker-name (subseq marker-name 5 (length marker-name))) + (if (string= marker-name "rarm_elbow_p_servo") + (return-from marker-name-to-servo-id 2))) + +(defun servo-name-to-cable-id-list (servo-name) + (setq servo-name (subseq servo-name 5 (length servo-name))) + (if (string= servo-name "rarm_elbow_p_servo") + (return-from servo-name-to-cable-id-list (list 8 9 10 11)))) ;;; parameters -(setq *rarm-elbow-p-to-search-cable-pose* - (make-cascoords - :pos #f(-3.797 -98.159 34.853) - :rpy #f(1.549 0.916 3.105) +(setq *servo-to-search-cable-pose* + (make-cascoords + :pos #f(-3.797 -98.159 34.853) + :rpy #f(1.549 0.916 3.105) + )) +(setq *servo-to-search-cable-pose-1* + (gmake-cascoords + :pos #f(-1.162 -91.94 18.84) + :rpy #f(0.205 1.494 1.799) )) -(setq *rarm-elbow-p-to-search-tag-pose-1* - (make-cascoords - :pos #f(1.133 -85.27 61.446) - :rpy #f(1.581 1.228 3.132) +(setq *servo-to-search-cable-pose-2* + (gmake-cascoords + :pos #f(-17.376 -144.684 -6.541) + :rpy #f(-1.561 0.409 3.087) + )) +(setq *servo-to-search-cable-pose-3* + (gmake-cascoords + :pos #f(44.827 -95.802 12.263) + :rpy #f(2.19 0.856 3.056) )) -(setq *rarm-elbow-p-to-search-tag-pose-2* - (make-cascoords - :pos #f(1.898 -42.878 78.671) - :rpy #f(-1.931 1.548 -0.355) +(setq *cable-front-to-pre-grasp-pose-1* + (gmake-cascoords + :pos #f(-57.691 22.526 17.675) + :rpy #f(-1.655 0.272 1.413) + )) +(setq *cable-front-to-pre-grasp-pose-2* + (gmake-cascoords + :pos #f(-39.479 16.61 12.343) + :rpy #f(-1.365 0.367 1.57) )) -(setq *rarm-elbow-p-to-search-tag-pose-3* - (make-cascoords - :pos #f(2.51 -45.813 49.055) - :rpy #f(-1.675 1.106 -0.081) +(setq *cable-front-to-grasp-pose* + (gmake-cascoords + :pos #f(-16.344 21.751 16.114) + :rpy #f(-1.45 0.449 1.552) )) + +(setq *servo-to-search-tag-pose-1* + (make-cascoords + :pos #f(1.133 -85.27 61.446) + :rpy #f(1.581 1.228 3.132) + )) +(setq *servo-to-search-tag-pose-2* + (make-cascoords + :pos #f(1.898 -42.878 78.671) + :rpy #f(-1.931 1.548 -0.355) + )) +(setq *servo-to-search-tag-pose-3* + (make-cascoords + :pos #f(2.51 -45.813 49.055) + :rpy #f(-1.675 1.106 -0.081) + )) (setq *rarm-elbow-p-to-inter1-pose* - (make-cascoords - :pos #f(-168.038 -3.379 89.959) - :rpy #f(0.042 -0.008 1.539) - )) + (make-cascoords + :pos #f(-168.038 -3.379 89.959) + :rpy #f(0.042 -0.008 1.539) + )) (setq *rarm-elbow-p-to-inter2-pose* - (make-cascoords - :pos #f(-141.254 -5.393 -96.748) - :rpy #f(0.024 -0.937 -0.027) - )) + (make-cascoords + :pos #f(-141.254 -5.393 -96.748) + :rpy #f(0.024 -0.937 -0.027) + )) (setq *rarm-elbow-p-to-pre-grasp-pose* - (make-cascoords - :pos #f(-28.257 -2.82 -95.35) - :rpy #f(-2.973 -1.515 2.986) - )) + (make-cascoords + :pos #f(-28.257 -2.82 -95.35) + :rpy #f(-2.973 -1.515 2.986) + )) (setq *rarm-elbow-p-to-grasp-pose* - (make-cascoords - :pos #f(-24.612 4.967 -27.886) - :rpy #f(-2.962 -1.494 3.004) - )) + (make-cascoords + :pos #f(-24.612 4.967 -27.886) + :rpy #f(-2.962 -1.494 3.004) + )) (setq *cable-frame* "cable-frame") diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l index 21ece7d2b2..b178796c7a 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l @@ -28,9 +28,7 @@ (base-to-search-pose nil) (count 0)) (ros::ros-info "searching tag...") - (marker-name-to-id marker-name) - (setq target-id *target-id*) - (format t "target-id is ~d~%" target-id) + (setq target-id (marker-name-to-servo-id marker-name)) (while t (setq detection-results-msg (one-shot-subscribe "/right_tag_detections" apriltag_ros::AprilTagDetectionArray)) (when detection-results-msg @@ -45,17 +43,17 @@ (setq count (+ count 1)) (case count (1 - (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *rarm-elbow-p-to-search-tag-pose-1*)) + (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *servo-to-search-tag-pose-1*)) (send *robot* :rarm :inverse-kinematics base-to-search-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation)) (2 - (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *rarm-elbow-p-to-search-tag-pose-2*)) + (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *servo-to-search-tag-pose-2*)) (send *robot* :rarm :inverse-kinematics base-to-search-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation)) (3 - (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *rarm-elbow-p-to-search-tag-pose-3*)) + (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *servo-to-search-tag-pose-3*)) (send *robot* :rarm :inverse-kinematics base-to-search-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation)) @@ -109,19 +107,73 @@ #| memo to get parameters (send *robot* :angle-vector (send *ri* :state :potentio-vector)) -(setq btgp (send *robot* :larm :end-coords)) +(setq btgp (send *robot* :rarm :end-coords)) (setq param (send (send (send base-to-marker-origin :inverse-transformation) :copy-worldcoords) :transform (send btgp :copy-worldcoords))) (setq test (send (send base-to-marker-origin :copy-worldcoords) :transform param)) -(setq param (send (send marker-to-base :copy-worldcoords) :transform (send btgp :copy-worldcoords))) - |# -(defun rarm-grasp-cable (servo-name) + +(defun rarm-grasp-cable (servo-name side) (let ((base-to-marker-origin nil) (base-to-pre-prasp-pose nil) (base-to-grasp-pose nil) + (cable-id-list nil) + (target-id-f nil) + (target-id-b nil + (grasp-cable-id nil) + (grasp-cable-side nil) (stamp nil) (ret nil)) - (ros::ros-info "grasp cable") + (ros::ros-info "searching cable...") + (setq cable-id-list (servo-name-to-cable-id-list servo-name)) + (if (string= side "right") + (progn + (setq target-id-f (elt cable-id-list 0)) + (setq taregt-id-b (elt cable-id-list 1))) + (progn + (setq target-id-f (elt cable-id-list 2)) + (setq taregt-id-b (elt cable-id-list 3)))) + + (while t + (setq detection-results-msg (one-shot-subscribe "/right_tag_detections" apriltag_ros::AprilTagDetectionArray)) + (when detection-results-msg + (ros::ros-info "Got a message") + (setq id-list (mapcar #'(lambda (x) (elt (send x :id) 0)) (send detection-results-msg :detections))) + (if (id-in-list target-id-f id-list) + (progn + (ros::ros-info "front-marker was found") + (setq grasp-cable-side "front") + (return))) + (if (id-in-list target-id-b id-list) + (progn + (ros::ros-info "back-marker was found") + (setq grasp-cable-side "back") + (return))) + (setq count (+ count 1)) + (case count ;;12/14 restart from here + (1 + (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *rarm-elbow-p-to-search-cable-pose-1*)) + (send *robot* :rarm :inverse-kinematics base-to-search-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation)) + (2 + (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *rarm-elbow-p-to-search-tag-pose-2*)) + (send *robot* :rarm :inverse-kinematics base-to-search-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation)) + (3 + (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *rarm-elbow-p-to-search-tag-pose-3*)) + (send *robot* :rarm :inverse-kinematics base-to-search-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation)) + (4 + (ros::ros-info "Can't find tag") + (return-from rarm-search-servo-tag nil))))))))) + + + + + + (ros::ros-info "start to grasp cable") (setq stamp (ros::time-now)) (setq marker-name "cable_front") (when (send *tfl* :wait-for-transform "dual_arm_base" marker-name stamp 5) From ad57108211afc3945676187c7715d300e3e3ef6b Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 14 Dec 2021 11:42:06 +0900 Subject: [PATCH 069/127] [euslisp/grasp_cable] fix typo in parameters.l --- jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l index 523659fdaf..6beadc110c 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l @@ -22,32 +22,32 @@ :rpy #f(1.549 0.916 3.105) )) (setq *servo-to-search-cable-pose-1* - (gmake-cascoords + (make-cascoords :pos #f(-1.162 -91.94 18.84) :rpy #f(0.205 1.494 1.799) )) (setq *servo-to-search-cable-pose-2* - (gmake-cascoords + (make-cascoords :pos #f(-17.376 -144.684 -6.541) :rpy #f(-1.561 0.409 3.087) )) (setq *servo-to-search-cable-pose-3* - (gmake-cascoords + (make-cascoords :pos #f(44.827 -95.802 12.263) :rpy #f(2.19 0.856 3.056) )) (setq *cable-front-to-pre-grasp-pose-1* - (gmake-cascoords + (make-cascoords :pos #f(-57.691 22.526 17.675) :rpy #f(-1.655 0.272 1.413) )) (setq *cable-front-to-pre-grasp-pose-2* - (gmake-cascoords + (make-cascoords :pos #f(-39.479 16.61 12.343) :rpy #f(-1.365 0.367 1.57) )) (setq *cable-front-to-grasp-pose* - (gmake-cascoords + (make-cascoords :pos #f(-16.344 21.751 16.114) :rpy #f(-1.45 0.449 1.552) )) From 4e64ac434a2f2defc6faf4be8a688504ad42a917 Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 14 Dec 2021 17:04:11 +0900 Subject: [PATCH 070/127] [euslisp/grasp_cable] made rarm_grasp_cable --- .../euslisp/grasp_cable/grasp_cable.l | 9 +-- .../euslisp/grasp_cable/parameters.l | 21 +++++- jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l | 74 ++++++++++++------- 3 files changed, 70 insertions(+), 34 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l index 46c301906d..a668af53e3 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l @@ -54,15 +54,10 @@ (send *ri* :wait-interpolation) (rarm-search-servo-tag "rpov-rarm_elbow_p_servo") + (setq stamp (ros::time-now)) -(when (send *tfl* :wait-for-transform "dual_arm_base" marker-name stamp 5) +(when (send *tfl* :wait-for-transform "dual_arm_base" "rpov-rarm_elbow_p_servo" stamp 5) (setq *base-to-rarm-elbow-p-servo* (send *tfl* :lookup-transform "dual_arm_base" "rpov-rarm_elbow_p_servo" stamp))) -(setq search-tag-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *servo-to-search-cable-pose*)) -(send *robot* :rarm_joint1 :joint-angle 30) -(send *robot* :rarm :inverse-kinematics search-tag-pose) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - ;; (rarm-grasp-cable "rpov-rarm_elbow_p_servo" "right") diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l index 6beadc110c..bb8583fd1f 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l @@ -51,7 +51,26 @@ :pos #f(-16.344 21.751 16.114) :rpy #f(-1.45 0.449 1.552) )) - +(setq *servo-to-slide-insert-cable-inter-pose* + (make-cascoords + :pos #f(-16.344 21.751 16.114) + :rpy #f(-1.45 0.449 1.552) + )) +(setq *servo-to-slide-insert-cable-pose* + (make-cascoords + :pos #f(-7.11 -32.813 17.233) + :rpy #f(0.099 0.936 2.891) + )) +(setq *servo-to-lower-insert-cable-pre-pose* + (make-cascoords + :pos #f(0.313 -29.084 14.451) + :rpy #f(0.076 0.952 2.952) + )) +(setq *servo-to-lower-insert-cable-pose* + (make-cascoords + :pos #f(3.789 -31.211 10.625) + :rpy #f(0.111 0.946 2.953) + )) (setq *servo-to-search-tag-pose-1* (make-cascoords :pos #f(1.133 -85.27 61.446) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l b/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l index b178796c7a..68e09350ba 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l @@ -118,21 +118,25 @@ memo to get parameters (base-to-grasp-pose nil) (cable-id-list nil) (target-id-f nil) - (target-id-b nil + (target-id-b nil) (grasp-cable-id nil) (grasp-cable-side nil) (stamp nil) (ret nil)) + (setq stamp (ros::time-now)) + (when (send *tfl* :wait-for-transform "dual_arm_base" servo-name stamp 5) + (setq base-to-marker-origin (send *tfl* :lookup-transform "dual_arm_base" servo-name stamp))) (ros::ros-info "searching cable...") (setq cable-id-list (servo-name-to-cable-id-list servo-name)) (if (string= side "right") (progn + (ros::ros-info "target is right cable") (setq target-id-f (elt cable-id-list 0)) (setq taregt-id-b (elt cable-id-list 1))) (progn - (setq target-id-f (elt cable-id-list 2)) - (setq taregt-id-b (elt cable-id-list 3)))) - + (ros::ros-info "target is right cable") + (setq target-id-f (elt cable-id-list 2)) + (setq taregt-id-b (elt cable-id-list 3)))) (while t (setq detection-results-msg (one-shot-subscribe "/right_tag_detections" apriltag_ros::AprilTagDetectionArray)) (when detection-results-msg @@ -149,58 +153,76 @@ memo to get parameters (setq grasp-cable-side "back") (return))) (setq count (+ count 1)) - (case count ;;12/14 restart from here + (case count (1 - (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *rarm-elbow-p-to-search-cable-pose-1*)) - (send *robot* :rarm :inverse-kinematics base-to-search-pose) + (setq base-to-search-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-search-cable-pose-1*)) + (setq ret (send *robot* :rarm :inverse-kinematics base-to-search-pose)) + (if (not ret) (ros::ros-error "IK-failed") nil) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation)) (2 - (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *rarm-elbow-p-to-search-tag-pose-2*)) - (send *robot* :rarm :inverse-kinematics base-to-search-pose) + (setq base-to-search-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-search-cable-pose-2*)) + (setq ret (send *robot* :rarm :inverse-kinematics base-to-search-pose)) + (if (not ret) (ros::ros-error "IK-failed") nil) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation)) (3 - (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *rarm-elbow-p-to-search-tag-pose-3*)) - (send *robot* :rarm :inverse-kinematics base-to-search-pose) + (setq base-to-search-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-search-cable-pose-3*)) + (setq ret (send *robot* :rarm :inverse-kinematics base-to-search-pose)) + (if (not ret) (ros::ros-error "IK-failed") nil) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation)) - (4 - (ros::ros-info "Can't find tag") - (return-from rarm-search-servo-tag nil))))))))) - - - - + (4 + (ros::ros-info "Can't find cable") + (return-from rarm-grasp-cable nil))))) (ros::ros-info "start to grasp cable") (setq stamp (ros::time-now)) - (setq marker-name "cable_front") + (setq buf (subseq servo-name 0 (- (length servo-name) 5))) + (setq s (subseq side 0 1)) + (setq p (subseq grasp-cable-side 0 1)) + (setq marker-name (format nil "~A~A~A~A~A" buf "cable_" s "_" p)) + (setq stamp (ros::time-now)) (when (send *tfl* :wait-for-transform "dual_arm_base" marker-name stamp 5) (setq base-to-marker-origin (send *tfl* :lookup-transform "dual_arm_base" marker-name stamp))) - (setq base-to-pre-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *cable-front-to-pre-grasp-pose*)) - (setq base-to-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *cable-front-to-grasp-pose*)) + (if (string= grasp-cable-side "front") + (progn + (setq base-to-pre-grasp-pose-1 (send (send base-to-marker-origin :copy-worldcoords) :transform *cable-front-to-pre-grasp-pose-1*)) + (setq base-to-pre-grasp-pose-2 (send (send base-to-marker-origin :copy-worldcoords) :transform *cable-front-to-pre-grasp-pose-2*)) + (setq base-to-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *cable-front-to-grasp-pose*))) + (progn + (setq base-to-pre-grasp-pose-1 (send (send base-to-marker-origin :copy-worldcoords) :transform *cable-back-to-pre-grasp-pose-1*)) + (setq base-to-pre-grasp-pose-2 (send (send base-to-marker-origin :copy-worldcoords) :transform *cable-back-to-pre-grasp-pose-2*)) + (setq base-to-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *cable-back-to-grasp-pose*)))) - ;pre-grasp-pose + (send *robot* :rarm :move-end-pos #f(-30 0 20)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + ;;pre-grasp-poses (send *ri* :start-grasp :rarm) (unix:sleep 1) (send *ri* :stop-grasp :rarm :width 0.02) (unix:sleep 1) - (send *robot* :rarm :inverse-kinematics base-to-pre-grasp-pose) + (send *robot* :rarm :inverse-kinematics base-to-pre-grasp-pose-1) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *robot* :rarm :inverse-kinematics base-to-pre-grasp-pose-2) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) - ;grasp-pose + ;;grasp-pose (send *robot* :rarm :inverse-kinematics base-to-grasp-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 1500) (send *ri* :wait-interpolation) + ;;grasp (send *ri* :start-grasp :rarm) - (unix:sleep 2) - (send *ri* :stop-grasp :rarm) (unix:sleep 2))) +(defun rarm-insert-cable (servo-name) + (let () (defun check-camera () (let ((base-to-marker-000 nil) From ac6c116951f9df557077458587cc26d7a1f7ca98 Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 14 Dec 2021 17:14:24 +0900 Subject: [PATCH 071/127] [euslisp] rename grasp_cable/ -> grasp_and_insert_cable/ --- .../euslisp/{grasp_cable => grasp_and_insert_cable}/grasp_cable.l | 0 .../euslisp/{grasp_cable => grasp_and_insert_cable}/parameters.l | 0 .../euslisp/{grasp_cable => grasp_and_insert_cable}/utils.l | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename jsk_2021_fix_kxr/euslisp/{grasp_cable => grasp_and_insert_cable}/grasp_cable.l (100%) rename jsk_2021_fix_kxr/euslisp/{grasp_cable => grasp_and_insert_cable}/parameters.l (100%) rename jsk_2021_fix_kxr/euslisp/{grasp_cable => grasp_and_insert_cable}/utils.l (100%) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_cable.l similarity index 100% rename from jsk_2021_fix_kxr/euslisp/grasp_cable/grasp_cable.l rename to jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_cable.l diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l similarity index 100% rename from jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l rename to jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l diff --git a/jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l similarity index 100% rename from jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l rename to jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l From 6f8ff1f3b0a85b025eec1740ab035ff61677b6d2 Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 14 Dec 2021 17:18:51 +0900 Subject: [PATCH 072/127] [euslisp/grasp_and_insert_cable] adjust descriptions in files --- .../{grasp_cable.l => grasp_and_insert_cable.l} | 6 +++--- jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) rename jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/{grasp_cable.l => grasp_and_insert_cable.l} (92%) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l similarity index 92% rename from jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_cable.l rename to jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l index a668af53e3..50aa75477e 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l @@ -1,14 +1,14 @@ #!/usr/bin/env roseus (require "package://panda_eus/euslisp/dual_panda-interface.l") -(require "package://jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l") -(require "package://jsk_2021_fix_kxr/euslisp/grasp_cable/utils.l") +(require "package://jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l") +(require "package://jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l") (ros::roseus-add-msgs "std_msgs") (ros::load-ros-manifest "apriltag_ros") -(ros::roseus "grasp_cable") +(ros::roseus "grasp_and_insert_cable") (dual_panda-init) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l index 68e09350ba..d84ae03756 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l @@ -1,7 +1,7 @@ #!/usr/bin/env roseus (require "package://panda_eus/euslisp/dual_panda-interface.l") -(require "package://jsk_2021_fix_kxr/euslisp/grasp_cable/parameters.l") +(require "package://jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l") (ros::roseus-add-msgs "std_msgs") From 6772120f17eaf585ff4595aec9d13a186519af10 Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 14 Dec 2021 17:54:50 +0900 Subject: [PATCH 073/127] [euslisp/grasp_and_insert_cable] changed name of grobal var of frame, '-' to '_' --- .../grasp_and_insert_cable.l | 2 ++ .../grasp_and_insert_cable/parameters.l | 10 ++++----- .../euslisp/grasp_and_insert_cable/utils.l | 21 +++++++++++++------ 3 files changed, 22 insertions(+), 11 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l index 50aa75477e..567a522d5c 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l @@ -61,3 +61,5 @@ ;; (rarm-grasp-cable "rpov-rarm_elbow_p_servo" "right") + +(rarm-insert-cable "rpov-rarm_elbow_p_servo" "right") diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l index bb8583fd1f..c30b1d335d 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l @@ -1,6 +1,6 @@ #!/usr/bin/env roseus -(defvar *base-to-rarm-elbow-p-servo*) +(defvar *base_to_rarm_elbow_p_servo*) (defvar *target-id*) (defvar *target-id-list*) @@ -51,22 +51,22 @@ :pos #f(-16.344 21.751 16.114) :rpy #f(-1.45 0.449 1.552) )) -(setq *servo-to-slide-insert-cable-inter-pose* +(setq *servo-to-slide-insert-right-cable-inter-pose* (make-cascoords :pos #f(-16.344 21.751 16.114) :rpy #f(-1.45 0.449 1.552) )) -(setq *servo-to-slide-insert-cable-pose* +(setq *servo-to-slide-insert-right-cable-pose* (make-cascoords :pos #f(-7.11 -32.813 17.233) :rpy #f(0.099 0.936 2.891) )) -(setq *servo-to-lower-insert-cable-pre-pose* +(setq *servo-to-lower-insert-right-cable-pre-pose* (make-cascoords :pos #f(0.313 -29.084 14.451) :rpy #f(0.076 0.952 2.952) )) -(setq *servo-to-lower-insert-cable-pose* +(setq *servo-to-lower-insert-right-cable-pose* (make-cascoords :pos #f(3.789 -31.211 10.625) :rpy #f(0.111 0.946 2.953) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l index d84ae03756..b557fe56d8 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l @@ -43,17 +43,17 @@ (setq count (+ count 1)) (case count (1 - (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *servo-to-search-tag-pose-1*)) + (setq base-to-search-pose (send (send *base_to_rarm_elbow_p_servo* :copy-worldcoords) :transform *servo-to-search-tag-pose-1*)) (send *robot* :rarm :inverse-kinematics base-to-search-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation)) (2 - (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *servo-to-search-tag-pose-2*)) + (setq base-to-search-pose (send (send *base_to_rarm_elbow_p_servo* :copy-worldcoords) :transform *servo-to-search-tag-pose-2*)) (send *robot* :rarm :inverse-kinematics base-to-search-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation)) (3 - (setq base-to-search-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *servo-to-search-tag-pose-3*)) + (setq base-to-search-pose (send (send *base_to_rarm_elbow_p_servo* :copy-worldcoords) :transform *servo-to-search-tag-pose-3*)) (send *robot* :rarm :inverse-kinematics base-to-search-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation)) @@ -75,7 +75,7 @@ (setq marker-name "lpov-rarm_elbow_p_servo") (when (send *tfl* :wait-for-transform "dual_arm_base" marker-name stamp 5) (setq base-to-marker-origin (send *tfl* :lookup-transform "dual_arm_base" marker-name stamp)) - (setq *base-to-rarm-elbow-p-servo* base-to-marker-origin)) + (setq *base_to_rarm_elbow_p_servo* base-to-marker-origin)) (setq base-to-inter1-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *rarm-elbow-p-to-inter1-pose*)) (setq base-to-inter2-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *rarm-elbow-p-to-inter2-pose*)) (setq base-to-pre-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *rarm-elbow-p-to-pre-grasp-pose*)) @@ -221,8 +221,17 @@ memo to get parameters (send *ri* :start-grasp :rarm) (unix:sleep 2))) -(defun rarm-insert-cable (servo-name) - (let () +(defun rarm-insert-cable (servo-name side) + (let ((base-to-marker-origin nil) + (base-to-pre-prasp-pose nil) + (base-to-grasp-pose nil) + (cable-id-list nil) + (stamp nil) + (ret nil)) + *base_to_rarm_elbow_p_servo* + (setq buf (subseq servo-name 5 (length servo-name))) + (setq base-to-marker-origin (format nil "~A~A~A" "*base_to_" buf "*")) + (defun check-camera () (let ((base-to-marker-000 nil) From 228f9ab1ae4b00d8ef0ea349188f696358fe2418 Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 15 Dec 2021 15:38:13 +0900 Subject: [PATCH 074/127] [euslisp/grasp_and_insert_cable] deleted check_camera func --- .../euslisp/grasp_and_insert_cable/utils.l | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l index b557fe56d8..280853a3cf 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l @@ -231,20 +231,3 @@ memo to get parameters *base_to_rarm_elbow_p_servo* (setq buf (subseq servo-name 5 (length servo-name))) (setq base-to-marker-origin (format nil "~A~A~A" "*base_to_" buf "*")) - - -(defun check-camera () - (let ((base-to-marker-000 nil) - (base-to-marker-001 nil) - (base-to-marker-002 nil) - (stamp nil) - (ret nil)) - (ros::ros-info "grasp cable") - (setq stamp (ros::time-now)) - (setq marker-name "cable_front") - (when (send *tfl* :wait-for-transform "dual_arm_base" "test_000" stamp 5) - (setq *marker-000* (send *tfl* :lookup-transform "dual_arm_base" "test_000" stamp))) - (when (send *tfl* :wait-for-transform "dual_arm_base" "test_000" stamp 5) - (setq *marker-001* (send *tfl* :lookup-transform "dual_arm_base" "test_001" stamp))) - (when (send *tfl* :wait-for-transform "dual_arm_base" "test_001" stamp 5) - (setq *marker-002* (send *tfl* :lookup-transform "dual_arm_base" "test_002" stamp))))) From fa60e28e37fed0e34f9ae5904d08371c15f5e6d6 Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 15 Dec 2021 17:49:09 +0900 Subject: [PATCH 075/127] deleted #cable-insertion_heuristic_init.l# --- .../#cable-insertion_heuristic_init.l# | 33 ------------------- 1 file changed, 33 deletions(-) delete mode 100755 jsk_2021_fix_kxr/euslisp/#cable-insertion_heuristic_init.l# diff --git a/jsk_2021_fix_kxr/euslisp/#cable-insertion_heuristic_init.l# b/jsk_2021_fix_kxr/euslisp/#cable-insertion_heuristic_init.l# deleted file mode 100755 index 0b220ecd15..0000000000 --- a/jsk_2021_fix_kxr/euslisp/#cable-insertion_heuristic_init.l# +++ /dev/null @@ -1,33 +0,0 @@ -#!/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) From 187e5c200b20a70f97225991ab1600b23f25f115 Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 15 Dec 2021 19:03:32 +0900 Subject: [PATCH 076/127] [euslisp/grasp_and_insert_cable] start to make arrangement motion --- .../grasp_and_insert_cable.l | 22 ++++++++++++++++++- .../grasp_and_insert_cable/parameters.l | 9 ++++---- .../euslisp/grasp_and_insert_cable/utils.l | 18 +++++++++++---- 3 files changed, 40 insertions(+), 9 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l index 567a522d5c..af36962bfe 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l @@ -4,8 +4,13 @@ (require "package://jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l") (require "package://jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/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") @@ -14,6 +19,21 @@ (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) + (send *robot* :reset-pose) (send *robot* :rarm :move-end-pos #f(-500 0 100)) (send *robot* :larm :move-end-pos #f(-500 0 100)) @@ -57,7 +77,7 @@ (setq stamp (ros::time-now)) (when (send *tfl* :wait-for-transform "dual_arm_base" "rpov-rarm_elbow_p_servo" stamp 5) - (setq *base-to-rarm-elbow-p-servo* (send *tfl* :lookup-transform "dual_arm_base" "rpov-rarm_elbow_p_servo" stamp))) + (setq *base_to_rarm_elbow_p_servo* (send *tfl* :lookup-transform "dual_arm_base" "rpov-rarm_elbow_p_servo" stamp))) ;; (rarm-grasp-cable "rpov-rarm_elbow_p_servo" "right") diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l index c30b1d335d..420c0b7018 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l @@ -53,14 +53,15 @@ )) (setq *servo-to-slide-insert-right-cable-inter-pose* (make-cascoords - :pos #f(-16.344 21.751 16.114) - :rpy #f(-1.45 0.449 1.552) + :pos #f(34.757 -12.996 42.748) + :rpy #f(-2.515 1.479 -2.446) )) (setq *servo-to-slide-insert-right-cable-pose* (make-cascoords - :pos #f(-7.11 -32.813 17.233) - :rpy #f(0.099 0.936 2.891) + :pos #f(33.3 -13.281 37.626) + :rpy #f(-1.774 1.482 -1.652) )) + (setq *servo-to-lower-insert-right-cable-pre-pose* (make-cascoords :pos #f(0.313 -29.084 14.451) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l index 280853a3cf..289a0efa4a 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l @@ -223,11 +223,21 @@ memo to get parameters (defun rarm-insert-cable (servo-name side) (let ((base-to-marker-origin nil) - (base-to-pre-prasp-pose nil) - (base-to-grasp-pose nil) + (base-to-pre-insert-pose nil) + (base-to-insert-pose nil) (cable-id-list nil) (stamp nil) (ret nil)) - *base_to_rarm_elbow_p_servo* (setq buf (subseq servo-name 5 (length servo-name))) - (setq base-to-marker-origin (format nil "~A~A~A" "*base_to_" buf "*")) + (setq base-to-marker-origin (eval (read-from-string (format nil "~A~A~A" "*base_to_" buf "*")))) + + (setq base-to-pre-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-right-cable-inter-pose*)) + (setq base-to-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-right-cable-pose*)) + + ;;set hand + (send *robot* :rarm :inverse-kinematics base-to-pre-insert-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *robot* :rarm :inverse-kinematics base-to-insert-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) From 32a0f1240163f42974218a8bc3315fbea70f23aa Mon Sep 17 00:00:00 2001 From: softyanija Date: Thu, 16 Dec 2021 14:39:19 +0900 Subject: [PATCH 077/127] [euslisp/grasp_and_insert_cable] completed arrangement motion --- .../grasp_and_insert_cable/parameters.l | 1 + .../euslisp/grasp_and_insert_cable/utils.l | 35 +++++++++++++++++++ 2 files changed, 36 insertions(+) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l index 420c0b7018..89980ef26d 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l @@ -14,6 +14,7 @@ (if (string= servo-name "rarm_elbow_p_servo") (return-from servo-name-to-cable-id-list (list 8 9 10 11)))) +(setq *null-wrench* #f(0 0 0 0 0 0)) ;;; parameters (setq *servo-to-search-cable-pose* diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l index 289a0efa4a..aa1fa498f6 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l @@ -241,3 +241,38 @@ memo to get parameters (send *robot* :rarm :inverse-kinematics base-to-insert-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) + + ;;get corret wrench and set as wrench-init + (setq wrench-init (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k)) + (while (equal wrench-init *null-wrench*) + (ros::ros-info "Couldn't get correct wrench") + (setq wrench-init (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k))) + + (setq i 0) + (setq z-lim 3) + (setq base-stroke #f(0.5 0 0)) + (while (< i 30) + (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 diff-z (elt wrench-diff 2)) + (if (> diff-z z-lim) + (progn + (ros::ros-info "detect contacting to serface of servo") + (return)) + (progn + (ros::ros-info "still hanging in the air") + (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)) + (setq stroke (* (elt base-stroke 0) i)) + (ros::ros-info (format nil "stroke is ~A" stroke))))))) + +#| +(setq wrench-init (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k))) +(send *robot* :rarm :move-end-pos base-stroke) +(send *ri* :angle-vector (send *robot* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(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)) +|# From 2ef5a41da0f1eefc65caa718e95d59341ee02665 Mon Sep 17 00:00:00 2001 From: softyanija Date: Thu, 16 Dec 2021 17:41:36 +0900 Subject: [PATCH 078/127] [euslisp/grasn_and_insert_cable] now rarm can move around cable socket corresponding to side --- .../euslisp/grasp_and_insert_cable/utils.l | 24 +++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l index aa1fa498f6..ec989bf321 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l @@ -230,9 +230,13 @@ memo to get parameters (ret nil)) (setq buf (subseq servo-name 5 (length servo-name))) (setq base-to-marker-origin (eval (read-from-string (format nil "~A~A~A" "*base_to_" buf "*")))) - - (setq base-to-pre-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-right-cable-inter-pose*)) - (setq base-to-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-right-cable-pose*)) + (if (string= side "right") + (progn + (setq base-to-pre-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-right-cable-inter-pose*)) + (setq base-to-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-right-cable-pose*))) + (progn + (setq base-to-pre-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-left-cable-inter-pose*)) + (setq base-to-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-left-cable-pose*)))) ;;set hand (send *robot* :rarm :inverse-kinematics base-to-pre-insert-pose) @@ -246,6 +250,7 @@ memo to get parameters (setq wrench-init (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k)) (while (equal wrench-init *null-wrench*) (ros::ros-info "Couldn't get correct wrench") + ;;TODO escape from singular pose (setq wrench-init (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k))) (setq i 0) @@ -266,7 +271,12 @@ memo to get parameters (send *ri* :wait-interpolation) (setq i (+ i 1)) (setq stroke (* (elt base-stroke 0) i)) - (ros::ros-info (format nil "stroke is ~A" stroke))))))) + (ros::ros-info (format nil "stroke is ~A" stroke))))) + + ;;start to insert cable + + + )) #| (setq wrench-init (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k))) @@ -275,4 +285,10 @@ memo to get parameters (send *ri* :wait-interpolation) (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)) + +(send *robot* :rarm :move-end-pos #f(0 0 -1)) +(send *ri* :angle-vector (send *robot* :angle-vector) 1000) +(send *ri* :wait-interpolation) +(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)) |# From dbb1e31c72c5e40441fa3d1e6a24e6ea480e99cd Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 17 Dec 2021 17:09:48 +0900 Subject: [PATCH 079/127] [euslisp] make panda_commons/ and move utils.l and parameters.l to this dir from grasp_and_insert_cable/ --- .../euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l | 4 ++-- .../{grasp_and_insert_cable => panda_commons}/parameters.l | 0 .../euslisp/{grasp_and_insert_cable => panda_commons}/utils.l | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) rename jsk_2021_fix_kxr/euslisp/{grasp_and_insert_cable => panda_commons}/parameters.l (100%) rename jsk_2021_fix_kxr/euslisp/{grasp_and_insert_cable => panda_commons}/utils.l (99%) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l index af36962bfe..ca92b3c7aa 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l @@ -1,8 +1,8 @@ #!/usr/bin/env roseus (require "package://panda_eus/euslisp/dual_panda-interface.l") -(require "package://jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l") -(require "package://jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.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") diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l similarity index 100% rename from jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l rename to jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l similarity index 99% rename from jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l rename to jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index ec989bf321..f0c6eaf697 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -1,7 +1,7 @@ #!/usr/bin/env roseus (require "package://panda_eus/euslisp/dual_panda-interface.l") -(require "package://jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/parameters.l") +(require "package://jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l") (ros::roseus-add-msgs "std_msgs") From 30169efdc311795cd334daf33c6a9663924908cb Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 17 Dec 2021 17:40:05 +0900 Subject: [PATCH 080/127] [config/apriltag] changed tag name of cables --- jsk_2021_fix_kxr/config/apriltag/tags_for_left.yaml | 8 ++++---- jsk_2021_fix_kxr/config/apriltag/tags_for_right.yaml | 8 ++++---- jsk_2021_fix_kxr/launch/apriltag_doctor_panda.launch | 4 ++++ 3 files changed, 12 insertions(+), 8 deletions(-) create mode 100644 jsk_2021_fix_kxr/launch/apriltag_doctor_panda.launch diff --git a/jsk_2021_fix_kxr/config/apriltag/tags_for_left.yaml b/jsk_2021_fix_kxr/config/apriltag/tags_for_left.yaml index 769fe02ecc..ae747b3bc3 100644 --- a/jsk_2021_fix_kxr/config/apriltag/tags_for_left.yaml +++ b/jsk_2021_fix_kxr/config/apriltag/tags_for_left.yaml @@ -22,10 +22,10 @@ standalone_tags: {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: 8, size: 0.0052, name: 'lpov-rarm_elbow_p_cable_r1'}, - {id: 9, size: 0.0052, name: 'lpov-rarm_elbow_p_cable_r2'}, - {id: 10, size: 0.0052, name: 'lpov-rarm_elbow_p_cable_l1'}, - {id: 11, size: 0.0052, name: 'lpov-rarm_elbow_p_cable_l2'}, + {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 diff --git a/jsk_2021_fix_kxr/config/apriltag/tags_for_right.yaml b/jsk_2021_fix_kxr/config/apriltag/tags_for_right.yaml index bee77f42bf..08963c868f 100644 --- a/jsk_2021_fix_kxr/config/apriltag/tags_for_right.yaml +++ b/jsk_2021_fix_kxr/config/apriltag/tags_for_right.yaml @@ -22,10 +22,10 @@ standalone_tags: {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: 8, size: 0.0052, name: 'rpov-rarm_elbow_p_cable_r1'}, - {id: 9, size: 0.0052, name: 'rpov-rarm_elbow_p_cable_r2'}, - {id: 10, size: 0.0052, name: 'rpov-rarm_elbow_p_cable_l1'}, - {id: 11, size: 0.0052, name: 'rpov-rarm_elbow_p_cable_l2'}, + {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 diff --git a/jsk_2021_fix_kxr/launch/apriltag_doctor_panda.launch b/jsk_2021_fix_kxr/launch/apriltag_doctor_panda.launch new file mode 100644 index 0000000000..a285fb9e9d --- /dev/null +++ b/jsk_2021_fix_kxr/launch/apriltag_doctor_panda.launch @@ -0,0 +1,4 @@ + + + + From 0ca08a94beb293d375727acd7c6b3f262e8462fe Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 17 Dec 2021 18:43:05 +0900 Subject: [PATCH 081/127] [euslisp/arrange_to_jig] start to write arrange_to_jig.l --- .../euslisp/arrange_to_jig/arrange_to_jig.l | 35 +++++++++++++++++++ .../euslisp/panda_commons/utils.l | 15 ++++++++ 2 files changed, 50 insertions(+) create mode 100644 jsk_2021_fix_kxr/euslisp/arrange_to_jig/arrange_to_jig.l diff --git a/jsk_2021_fix_kxr/euslisp/arrange_to_jig/arrange_to_jig.l b/jsk_2021_fix_kxr/euslisp/arrange_to_jig/arrange_to_jig.l new file mode 100644 index 0000000000..ff2fdcc1e7 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/arrange_to_jig/arrange_to_jig.l @@ -0,0 +1,35 @@ +#!/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) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index f0c6eaf697..a881d9cb76 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -21,6 +21,21 @@ (if (string= marker-name "lpov-rarm_elbow_p_servo") (setq *target-id* 2)))) +(defun search-body () + (let () + + (send *robot* :reset-coop-pose) + (send *robot* :larm :move-end-pos #f(0 -80 0)) + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(230 -140.079 1500.936) :rpy (float-vector 3.1 2.1 0))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + ;;recog marker here + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(230 -140.079 1500.936) :rpy (float-vector 3.1 1.7 0))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (defun rarm-search-servo-tag (marker-name) (let ((detection-results-msg nil) (target-id nil) From 72fd7601f1638a5109eda9894254163a602f9657 Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 17 Dec 2021 18:48:11 +0900 Subject: [PATCH 082/127] [config/apriltag] add new tags --- jsk_2021_fix_kxr/config/apriltag/tags_for_left.yaml | 4 ++++ jsk_2021_fix_kxr/config/apriltag/tags_for_right.yaml | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/jsk_2021_fix_kxr/config/apriltag/tags_for_left.yaml b/jsk_2021_fix_kxr/config/apriltag/tags_for_left.yaml index ae747b3bc3..6d060f27a5 100644 --- a/jsk_2021_fix_kxr/config/apriltag/tags_for_left.yaml +++ b/jsk_2021_fix_kxr/config/apriltag/tags_for_left.yaml @@ -19,9 +19,13 @@ # ] 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'}, diff --git a/jsk_2021_fix_kxr/config/apriltag/tags_for_right.yaml b/jsk_2021_fix_kxr/config/apriltag/tags_for_right.yaml index 08963c868f..3848ab72be 100644 --- a/jsk_2021_fix_kxr/config/apriltag/tags_for_right.yaml +++ b/jsk_2021_fix_kxr/config/apriltag/tags_for_right.yaml @@ -19,9 +19,13 @@ # ] 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'}, From da5977c9eb23372464747ca1d5063dfd87058e41 Mon Sep 17 00:00:00 2001 From: softyanija Date: Sat, 18 Dec 2021 15:01:27 +0900 Subject: [PATCH 083/127] [euslisp/arrange_to_jig] made search-body func --- .../euslisp/arrange_to_jig/arrange_to_jig.l | 2 + .../euslisp/panda_commons/parameters.l | 2 + .../euslisp/panda_commons/utils.l | 38 +++++++++++++++++-- 3 files changed, 39 insertions(+), 3 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/arrange_to_jig/arrange_to_jig.l b/jsk_2021_fix_kxr/euslisp/arrange_to_jig/arrange_to_jig.l index ff2fdcc1e7..0194af189d 100644 --- a/jsk_2021_fix_kxr/euslisp/arrange_to_jig/arrange_to_jig.l +++ b/jsk_2021_fix_kxr/euslisp/arrange_to_jig/arrange_to_jig.l @@ -33,3 +33,5 @@ (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)) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l index 89980ef26d..bcfea8d557 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l @@ -1,5 +1,7 @@ #!/usr/bin/env roseus +(defvar *base_to_body_front*) +(defvar *base_to_body_back*) (defvar *base_to_rarm_elbow_p_servo*) (defvar *target-id*) (defvar *target-id-list*) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index a881d9cb76..643fc5d156 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -22,20 +22,52 @@ (setq *target-id* 2)))) (defun search-body () - (let () + (let ((detection-results-msg nil) + (body-front-id 0) + (body-back-id 1) + (id-list nil)) + (ros::ros-info "searching body tag...") (send *robot* :reset-coop-pose) (send *robot* :larm :move-end-pos #f(0 -80 0)) (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(230 -140.079 1500.936) :rpy (float-vector 3.1 2.1 0))) :translation-axis t :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) - ;;recog marker here - + ;;recognize marker + (setq detection-results-msg (one-shot-subscribe "/right_tag_detections" apriltag_ros::AprilTagDetectionArray)) + (when detection-results-msg + (ros::ros-info "Got a tag info") + (setq id-list (mapcar #'(lambda (x) (elt (send x :id) 0)) (send detection-results-msg :detections))) + (if (id-in-list body-front-id id-list) + (progn + (ros::ros-info "body-front was found") + (return-from search-body "front"))) + (if (id-in-list body-back-id id-list) + (progn + (ros::ros-info "body-back was found") + (return-from search-body "front")))) + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(230 -140.079 1500.936) :rpy (float-vector 3.1 1.7 0))) :translation-axis t :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) + (setq detection-results-msg (one-shot-subscribe "/right_tag_detections" apriltag_ros::AprilTagDetectionArray)) + (when detection-results-msg + (ros::ros-info "Got a tag info") + (setq id-list (mapcar #'(lambda (x) (elt (send x :id) 0)) (send detection-results-msg :detections))) + (if (id-in-list body-front-id id-list) + (progn + (ros::ros-info "body-front was found") + (return-from search-body "front"))) + (if (id-in-list body-back-id id-list) + (progn + (ros::ros-info "body-back was found") + (return-from search-body "front")))) + + (ros::ros-info "couldn't find body") + (return-from search-body nil))) + (defun rarm-search-servo-tag (marker-name) (let ((detection-results-msg nil) (target-id nil) From cb54a01887f180e64024295b5e23811b273c87a0 Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 20 Dec 2021 21:23:37 +0900 Subject: [PATCH 084/127] [euslisp/grasp_and_insert_cable] write moving to above socket --- .../grasp_and_insert_cable.l | 2 + .../euslisp/panda_commons/parameters.l | 2 +- .../euslisp/panda_commons/utils.l | 58 +++++++++++++++++++ 3 files changed, 61 insertions(+), 1 deletion(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l index ca92b3c7aa..a51532e604 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l @@ -83,3 +83,5 @@ (rarm-grasp-cable "rpov-rarm_elbow_p_servo" "right") (rarm-insert-cable "rpov-rarm_elbow_p_servo" "right") +(rarm-insert-cable-test "rpov-rarm_elbow_p_servo" "right") + diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l index bcfea8d557..b874ca526f 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l @@ -62,7 +62,7 @@ (setq *servo-to-slide-insert-right-cable-pose* (make-cascoords :pos #f(33.3 -13.281 37.626) - :rpy #f(-1.774 1.482 -1.652) + :rpy #f(-1.57 1.57 -1.57) )) (setq *servo-to-lower-insert-right-cable-pre-pose* diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index 643fc5d156..071a7f939c 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -325,6 +325,64 @@ memo to get parameters )) +(defun rarm-insert-cable-test (servo-name side) + (let ((base-to-marker-origin nil) + (base-to-pre-insert-pose nil) + (base-to-insert-pose nil) + (cable-id-list nil) + (stamp nil) + (ret nil)) + (setq buf (subseq servo-name 5 (length servo-name))) + (setq base-to-marker-origin (eval (read-from-string (format nil "~A~A~A" "*base_to_" buf "*")))) + (if (string= side "right") + (progn + (setq base-to-pre-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-right-cable-inter-pose*)) + (setq base-to-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-right-cable-pose*))) + (progn + (setq base-to-pre-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-left-cable-inter-pose*)) + (setq base-to-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-left-cable-pose*)))) + + ;;set hand + (send *robot* :rarm :inverse-kinematics base-to-pre-insert-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *robot* :rarm :inverse-kinematics base-to-insert-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + ;;get corret wrench and set as wrench-init + (setq wrench-init (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k)) + (while (equal wrench-init *null-wrench*) + (ros::ros-info "Couldn't get correct wrench") + ;;TODO escape from singular pose + (setq wrench-init (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k))) + + (setq i 0) + (setq z-lim 3) + (setq base-stroke #f(0.5 0 0)) + (while (< i 30) + (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 diff-z (elt wrench-diff 2)) + (if (> diff-z z-lim) + (progn + (ros::ros-info "detect contacting to serface of servo") + (return)) + (progn + (ros::ros-info "still hanging in the air") + (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)) + (setq stroke (* (elt base-stroke 0) i)) + (ros::ros-info (format nil "stroke is ~A" stroke))))) + + (send *robot* :angle-vector (send *ri* :state :potentio-vector)) + (send *robot* :rarm :move-end-pos #f(0 0 -8)) + (send *ri* :angle-vector (send *robot* :angle-vector) 4000) + (send *ri* :wait-interpolation) + )) + #| (setq wrench-init (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k))) (send *robot* :rarm :move-end-pos base-stroke) From 285e371bce99f1e2ae14efd196c333b4225420f1 Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 21 Dec 2021 17:01:32 +0900 Subject: [PATCH 085/127] [euslisp/grasp_and_insert_cable] start to make cable insert algorithm --- .../grasp_and_insert_cable.l | 1 - .../euslisp/panda_commons/utils.l | 117 +++++++++++++++++- 2 files changed, 115 insertions(+), 3 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l index a51532e604..d5d16d5279 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l @@ -84,4 +84,3 @@ (rarm-insert-cable "rpov-rarm_elbow_p_servo" "right") (rarm-insert-cable-test "rpov-rarm_elbow_p_servo" "right") - diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index 071a7f939c..a0fb8d86e2 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -21,6 +21,24 @@ (if (string= marker-name "lpov-rarm_elbow_p_servo") (setq *target-id* 2)))) +(defun move-rarm-x (delta) + (send *robot* :angle-vector (send *ri* :state :potentio-vector)) + (send :rarm :move-end-pos (float-vector delta 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation)) + +(defun move-rarm-y (delta) + (send *robot* :angle-vector (send *ri* :state :potentio-vector)) + (send :rarm :move-end-pos (float-vector 0 delta 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation)) + +(defun move-rarm-z (delta) + (send *robot* :angle-vector (send *ri* :state :potentio-vector)) + (send :rarm :move-end-pos (float-vector 0 0 delta)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation)) + (defun search-body () (let ((detection-results-msg nil) (body-front-id 0) @@ -358,7 +376,7 @@ memo to get parameters (setq wrench-init (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k))) (setq i 0) - (setq z-lim 3) + (setq z-lim 4) (setq base-stroke #f(0.5 0 0)) (while (< i 30) (setq wrench (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k)) @@ -381,7 +399,102 @@ memo to get parameters (send *robot* :rarm :move-end-pos #f(0 0 -8)) (send *ri* :angle-vector (send *robot* :angle-vector) 4000) (send *ri* :wait-interpolation) - )) + + (setq h_delta 1.5) + (steq i_delta 1.0) + (setq z-lim 3) + (setq total-insert 0) + (setq mode 1) + (while t + (setq wrench-pre (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k)) + (case mode + (1 + (ros::ros-info "Move to +y") + (move-rarm-y h_delta) + (move-rarm-x i_delta) + (unix:sleep 1) + (setq wrench-tmp (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k)) + (setq wrench-diff (v- wrench-tmp wrench-pre)) + (setq diff-z (elt wrench-diff 2)) + (if (< diff-z z-lim) + (progn + (ros::ros-info "Good") + (setq total_insert (+ total_insert i_delta)) + (if (> h_delta 0.5) + (setq h_delta (- h_delta 0.1))) + (ros::ros-info (format nil "total_insert is ~A" total_insert)) + (ros::ros-info (format nil "Now h_delta is ~A" h_delta))) + (progn + (ros:ros-info "Bad") + (move-rarm-y (- 0 h_delta)) + (move-rarm-x (- 0 i_delta)) + (setq mode 2)))) + (2 + (ros::ros-info "Move to -y") + (move-rarm-y (- 0 h_delta)) + (move-rarm-x i_delta) + (unix:sleep 1) + (setq wrench-tmp (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k)) + (setq wrench-diff (v- wrench-tmp wrench-pre)) + (setq diff-z (elt wrench-diff 2)) + (if (< diff-z z-lim) + (progn + (ros::ros-info "Good") + (setq total_insert (+ total_insert i_delta)) + (if (> h_delta 0.5) + (setq h_delta (- h_delta 0.1))) + (ros::ros-info (format nil "total_insert is ~A" total_insert)) + (ros::ros-info (format nil "Now h_delta is ~A" h_delta))) + (progn + (ros:ros-info "Bad") + (move-rarm-y h_delta) + (move-rarm-x (- 0 i_delta)) + (setq mode 3)))) + (3 + (ros::ros-info "Move to +z") + (move-rarm-z h_delta) + (move-rarm-x i_delta) + (unix:sleep 1) + (setq wrench-tmp (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k)) + (setq wrench-diff (v- wrench-tmp wrench-pre)) + (setq diff-z (elt wrench-diff 2)) + (if (< diff-z z-lim) + (progn + (ros::ros-info "Good") + (setq total_insert (+ total_insert i_delta)) + (if (> h_delta 0.5) + (setq h_delta (- h_delta 0.1))) + (ros::ros-info (format nil "total_insert is ~A" total_insert)) + (ros::ros-info (format nil "Now h_delta is ~A" h_delta))) + (progn + (ros:ros-info "Bad") + (move-rarm-y h_delta) + (move-rarm-x (- 0 i_delta)) + (setq mode 4)))) + (4 + (ros::ros-info "Move to -z") + (move-rarm-z (- 0 h_delta)) + (move-rarm-x i_delta) + (unix:sleep 1) + (setq wrench-tmp (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k)) + (setq wrench-diff (v- wrench-tmp wrench-pre)) + (setq diff-z (elt wrench-diff 2)) + (if (< diff-z z-lim) + (progn + (ros::ros-info "Good") + (setq total_insert (+ total_insert i_delta)) + (if (> h_delta 0.5) + (setq h_delta (- h_delta 0.1))) + (ros::ros-info (format nil "total_insert is ~A" total_insert)) + (ros::ros-info (format nil "Now h_delta is ~A" h_delta))) + (progn + (ros:ros-info "Bad") + (move-rarm-y h_delta) + (move-rarm-x (- 0 i_delta)) + (setq mode 1))))) + + + #| (setq wrench-init (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k))) From 108d9c5d19c5ddad2a18c39589b4eb77fbc6f8cb Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 21 Dec 2021 18:50:36 +0900 Subject: [PATCH 086/127] [euslisp/grasp_and_insert_cable] made insert motion --- .../grasp_and_insert_cable.l | 6 ++ .../euslisp/panda_commons/parameters.l | 2 +- .../euslisp/panda_commons/utils.l | 67 +++++++++++-------- 3 files changed, 47 insertions(+), 28 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l index d5d16d5279..fe30d44508 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l @@ -33,6 +33,7 @@ (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* :angle-vector (send *ri* :state :potentio-vector)) (send *robot* :reset-pose) (send *robot* :rarm :move-end-pos #f(-500 0 100)) @@ -84,3 +85,8 @@ (rarm-insert-cable "rpov-rarm_elbow_p_servo" "right") (rarm-insert-cable-test "rpov-rarm_elbow_p_servo" "right") + +(setq *servo-to-slide-insert-right-cable-pose* + (make-cascoords + :pos #f(33.3 -13.281 37.626) + :rpy #f(-1.57 1.57 -1.57))) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l index b874ca526f..eb7f85399d 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l @@ -61,7 +61,7 @@ )) (setq *servo-to-slide-insert-right-cable-pose* (make-cascoords - :pos #f(33.3 -13.281 37.626) + :pos #f(28 -13.281 37.626) :rpy #f(-1.57 1.57 -1.57) )) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index a0fb8d86e2..07a8c18cdf 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -23,19 +23,19 @@ (defun move-rarm-x (delta) (send *robot* :angle-vector (send *ri* :state :potentio-vector)) - (send :rarm :move-end-pos (float-vector delta 0 0)) + (send *robot* :rarm :move-end-pos (float-vector delta 0 0)) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation)) (defun move-rarm-y (delta) (send *robot* :angle-vector (send *ri* :state :potentio-vector)) - (send :rarm :move-end-pos (float-vector 0 delta 0)) + (send *robot* :rarm :move-end-pos (float-vector 0 delta 0)) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation)) (defun move-rarm-z (delta) (send *robot* :angle-vector (send *ri* :state :potentio-vector)) - (send :rarm :move-end-pos (float-vector 0 0 delta)) + (send *robot* :rarm :move-end-pos (float-vector 0 0 delta)) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation)) @@ -369,6 +369,8 @@ memo to get parameters (send *ri* :wait-interpolation) ;;get corret wrench and set as wrench-init + + (setq wrench-init (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k)) (while (equal wrench-init *null-wrench*) (ros::ros-info "Couldn't get correct wrench") @@ -396,16 +398,17 @@ memo to get parameters (ros::ros-info (format nil "stroke is ~A" stroke))))) (send *robot* :angle-vector (send *ri* :state :potentio-vector)) - (send *robot* :rarm :move-end-pos #f(0 0 -8)) + (send *robot* :rarm :move-end-pos #f(0 0 -5)) (send *ri* :angle-vector (send *robot* :angle-vector) 4000) (send *ri* :wait-interpolation) - (setq h_delta 1.5) - (steq i_delta 1.0) + (setq h_delta 1.2) + (setq i_delta 1.0) (setq z-lim 3) (setq total-insert 0) + (setq total-insert-th 7.0) (setq mode 1) - (while t + (while t (setq wrench-pre (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k)) (case mode (1 @@ -419,16 +422,16 @@ memo to get parameters (if (< diff-z z-lim) (progn (ros::ros-info "Good") - (setq total_insert (+ total_insert i_delta)) + (setq total-insert (+ total-insert i_delta)) (if (> h_delta 0.5) (setq h_delta (- h_delta 0.1))) - (ros::ros-info (format nil "total_insert is ~A" total_insert)) + (ros::ros-info (format nil "total_insert is ~A" total-insert)) (ros::ros-info (format nil "Now h_delta is ~A" h_delta))) (progn - (ros:ros-info "Bad") + (ros::ros-info "Bad") (move-rarm-y (- 0 h_delta)) - (move-rarm-x (- 0 i_delta)) - (setq mode 2)))) + (move-rarm-x (- 0 i_delta)))) + (setq mode 2)) (2 (ros::ros-info "Move to -y") (move-rarm-y (- 0 h_delta)) @@ -440,16 +443,16 @@ memo to get parameters (if (< diff-z z-lim) (progn (ros::ros-info "Good") - (setq total_insert (+ total_insert i_delta)) + (setq total-insert (+ total-insert i_delta)) (if (> h_delta 0.5) (setq h_delta (- h_delta 0.1))) - (ros::ros-info (format nil "total_insert is ~A" total_insert)) + (ros::ros-info (format nil "total_insert is ~A" total-insert)) (ros::ros-info (format nil "Now h_delta is ~A" h_delta))) (progn - (ros:ros-info "Bad") + (ros::ros-info "Bad") (move-rarm-y h_delta) - (move-rarm-x (- 0 i_delta)) - (setq mode 3)))) + (move-rarm-x (- 0 i_delta)))) + (setq mode 3)) (3 (ros::ros-info "Move to +z") (move-rarm-z h_delta) @@ -461,16 +464,16 @@ memo to get parameters (if (< diff-z z-lim) (progn (ros::ros-info "Good") - (setq total_insert (+ total_insert i_delta)) + (setq total-insert (+ total-insert i_delta)) (if (> h_delta 0.5) (setq h_delta (- h_delta 0.1))) - (ros::ros-info (format nil "total_insert is ~A" total_insert)) + (ros::ros-info (format nil "total_insert is ~A" total-insert)) (ros::ros-info (format nil "Now h_delta is ~A" h_delta))) (progn - (ros:ros-info "Bad") + (ros::ros-info "Bad") (move-rarm-y h_delta) - (move-rarm-x (- 0 i_delta)) - (setq mode 4)))) + (move-rarm-x (- 0 i_delta)))) + (setq mode 4)) (4 (ros::ros-info "Move to -z") (move-rarm-z (- 0 h_delta)) @@ -482,16 +485,26 @@ memo to get parameters (if (< diff-z z-lim) (progn (ros::ros-info "Good") - (setq total_insert (+ total_insert i_delta)) + (setq total-insert (+ total-insert i_delta)) (if (> h_delta 0.5) (setq h_delta (- h_delta 0.1))) - (ros::ros-info (format nil "total_insert is ~A" total_insert)) + (ros::ros-info (format nil "total_insert is ~A" total-insert)) (ros::ros-info (format nil "Now h_delta is ~A" h_delta))) (progn - (ros:ros-info "Bad") + (ros::ros-info "Bad") (move-rarm-y h_delta) - (move-rarm-x (- 0 i_delta)) - (setq mode 1))))) + (move-rarm-x (- 0 i_delta)))) + (setq mode 1))) + (if (> total-insert total-insert-th) + (progn + (send *robot* :angle-vector (send *ri* :state :potentio-vector)) + (send *robot* :rarm :move-end-pos #f(10 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + (send *robot* :rarm :move-end-pos #f(-10 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation))) + ) From 0a3c45072e8a0f644d7abf77f06b7eb0313a4bc1 Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 22 Dec 2021 17:22:11 +0900 Subject: [PATCH 087/127] [euslisp/grasp_and_insert_cable] adjusting to complete demo --- .../grasp_and_insert_cable.l | 6 --- .../euslisp/panda_commons/parameters.l | 24 ++++++--- .../euslisp/panda_commons/utils.l | 52 ++++++++++++------- 3 files changed, 49 insertions(+), 33 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l index fe30d44508..a7e80ba9f1 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l @@ -83,10 +83,4 @@ ;; (rarm-grasp-cable "rpov-rarm_elbow_p_servo" "right") -(rarm-insert-cable "rpov-rarm_elbow_p_servo" "right") (rarm-insert-cable-test "rpov-rarm_elbow_p_servo" "right") - -(setq *servo-to-slide-insert-right-cable-pose* - (make-cascoords - :pos #f(33.3 -13.281 37.626) - :rpy #f(-1.57 1.57 -1.57))) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l index eb7f85399d..5cc82aeea6 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l @@ -26,8 +26,8 @@ )) (setq *servo-to-search-cable-pose-1* (make-cascoords - :pos #f(-1.162 -91.94 18.84) - :rpy #f(0.205 1.494 1.799) + :pos #f(-2.566 -82.76 38.397) + :rpy #f(0.188 1.494 1.785) )) (setq *servo-to-search-cable-pose-2* (make-cascoords @@ -36,24 +36,34 @@ )) (setq *servo-to-search-cable-pose-3* (make-cascoords - :pos #f(44.827 -95.802 12.263) - :rpy #f(2.19 0.856 3.056) + :pos #f(13.83 -55.579 91.701) + :rpy #f(-1.799 1.134 -0.262) + )) +(setq *servo-to-cable-front-grasp-ready* + (make-cascoords + :pos #f(34.974 -76.339 49.308) + :rpy #f(1.453 0.441 1.446) )) (setq *cable-front-to-pre-grasp-pose-1* (make-cascoords - :pos #f(-57.691 22.526 17.675) - :rpy #f(-1.655 0.272 1.413) + :pos #f(-45.691 22.526 27.675) + :rpy #f(-1.655 0.3 1.413) )) (setq *cable-front-to-pre-grasp-pose-2* (make-cascoords :pos #f(-39.479 16.61 12.343) - :rpy #f(-1.365 0.367 1.57) + :rpy #f(-1.365 0.3 1.57) )) (setq *cable-front-to-grasp-pose* (make-cascoords :pos #f(-16.344 21.751 16.114) :rpy #f(-1.45 0.449 1.552) )) +(setq *cable-front-change-pose-before-insert* + (make-cascoords + :pos #f(0 0 0) + :rpy #f(0 1.57 0) + )) (setq *servo-to-slide-insert-right-cable-inter-pose* (make-cascoords :pos #f(34.757 -12.996 42.748) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index 07a8c18cdf..9b959077f3 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -175,12 +175,15 @@ memo to get parameters (setq btgp (send *robot* :rarm :end-coords)) (setq param (send (send (send base-to-marker-origin :inverse-transformation) :copy-worldcoords) :transform (send btgp :copy-worldcoords))) (setq test (send (send base-to-marker-origin :copy-worldcoords) :transform param)) +(objects (list *robot* base-to-marker-origin test)) |# (defun rarm-grasp-cable (servo-name side) - (let ((base-to-marker-origin nil) + (let ((base-to-servo-marker-origin nil) + (base-to-cable-marker-origin nil) (base-to-pre-prasp-pose nil) (base-to-grasp-pose nil) + (self-coords nil) (cable-id-list nil) (target-id-f nil) (target-id-b nil) @@ -188,9 +191,10 @@ memo to get parameters (grasp-cable-side nil) (stamp nil) (ret nil)) + (setq count 0) (setq stamp (ros::time-now)) (when (send *tfl* :wait-for-transform "dual_arm_base" servo-name stamp 5) - (setq base-to-marker-origin (send *tfl* :lookup-transform "dual_arm_base" servo-name stamp))) + (setq base-to-servo-marker-origin (send *tfl* :lookup-transform "dual_arm_base" servo-name stamp))) (ros::ros-info "searching cable...") (setq cable-id-list (servo-name-to-cable-id-list servo-name)) (if (string= side "right") @@ -220,19 +224,19 @@ memo to get parameters (setq count (+ count 1)) (case count (1 - (setq base-to-search-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-search-cable-pose-1*)) + (setq base-to-search-pose (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *servo-to-search-cable-pose-1*)) (setq ret (send *robot* :rarm :inverse-kinematics base-to-search-pose)) (if (not ret) (ros::ros-error "IK-failed") nil) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation)) (2 - (setq base-to-search-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-search-cable-pose-2*)) + (setq base-to-search-pose (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *servo-to-search-cable-pose-2*)) (setq ret (send *robot* :rarm :inverse-kinematics base-to-search-pose)) (if (not ret) (ros::ros-error "IK-failed") nil) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation)) (3 - (setq base-to-search-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-search-cable-pose-3*)) + (setq base-to-search-pose (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *servo-to-search-cable-pose-3*)) (setq ret (send *robot* :rarm :inverse-kinematics base-to-search-pose)) (if (not ret) (ros::ros-error "IK-failed") nil) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) @@ -240,6 +244,7 @@ memo to get parameters (4 (ros::ros-info "Can't find cable") (return-from rarm-grasp-cable nil))))) + (setq base-to-search-pose (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *servo-to-cable-front-grasp-ready*)) (ros::ros-info "start to grasp cable") (setq stamp (ros::time-now)) @@ -249,20 +254,22 @@ memo to get parameters (setq marker-name (format nil "~A~A~A~A~A" buf "cable_" s "_" p)) (setq stamp (ros::time-now)) (when (send *tfl* :wait-for-transform "dual_arm_base" marker-name stamp 5) - (setq base-to-marker-origin (send *tfl* :lookup-transform "dual_arm_base" marker-name stamp))) + (setq base-to-cable-marker-origin (send *tfl* :lookup-transform "dual_arm_base" marker-name stamp))) + (send *robot* :rarm :move-end-pos #f(-20 0 30)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *robot* :rarm :inverse-kinematics base-to-search-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) (if (string= grasp-cable-side "front") (progn - (setq base-to-pre-grasp-pose-1 (send (send base-to-marker-origin :copy-worldcoords) :transform *cable-front-to-pre-grasp-pose-1*)) - (setq base-to-pre-grasp-pose-2 (send (send base-to-marker-origin :copy-worldcoords) :transform *cable-front-to-pre-grasp-pose-2*)) - (setq base-to-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *cable-front-to-grasp-pose*))) + (setq base-to-pre-grasp-pose-1 (send (send base-to-cable-marker-origin :copy-worldcoords) :transform *cable-front-to-pre-grasp-pose-1*)) + (setq base-to-pre-grasp-pose-2 (send (send base-to-cable-marker-origin :copy-worldcoords) :transform *cable-front-to-pre-grasp-pose-2*)) + (setq base-to-grasp-pose (send (send base-to-cable-marker-origin :copy-worldcoords) :transform *cable-front-to-grasp-pose*))) (progn - (setq base-to-pre-grasp-pose-1 (send (send base-to-marker-origin :copy-worldcoords) :transform *cable-back-to-pre-grasp-pose-1*)) - (setq base-to-pre-grasp-pose-2 (send (send base-to-marker-origin :copy-worldcoords) :transform *cable-back-to-pre-grasp-pose-2*)) - (setq base-to-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *cable-back-to-grasp-pose*)))) - - (send *robot* :rarm :move-end-pos #f(-30 0 20)) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) - (send *ri* :wait-interpolation) + (setq base-to-pre-grasp-pose-1 (send (send base-to-cable-marker-origin :copy-worldcoords) :transform *cable-back-to-pre-grasp-pose-1*)) + (setq base-to-pre-grasp-pose-2 (send (send base-to-cable-marker-origin :copy-worldcoords) :transform *cable-back-to-pre-grasp-pose-2*)) + (setq base-to-grasp-pose (send (send base-to-cable-marker-origin :copy-worldcoords) :transform *cable-back-to-grasp-pose*)))) ;;pre-grasp-poses (send *ri* :start-grasp :rarm) @@ -271,7 +278,7 @@ memo to get parameters (unix:sleep 1) (send *robot* :rarm :inverse-kinematics base-to-pre-grasp-pose-1) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) (send *robot* :rarm :inverse-kinematics base-to-pre-grasp-pose-2) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) @@ -279,12 +286,17 @@ memo to get parameters ;;grasp-pose (send *robot* :rarm :inverse-kinematics base-to-grasp-pose) - (send *ri* :angle-vector (send *robot* :angle-vector) 1500) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) ;;grasp (send *ri* :start-grasp :rarm) - (unix:sleep 2))) + (unix:sleep 2) + + (setq self-coords (send *robot* :rarm :end-coords)) + (setq target-pos (send (setq self-coords (send *robot* :rarm :end-coords)) :worldpos)) + (send (setq target-coords (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *cable-front-change-pose-before-insert*)) :replace-pos target-pos) + )) (defun rarm-insert-cable (servo-name side) (let ((base-to-marker-origin nil) @@ -302,7 +314,7 @@ memo to get parameters (progn (setq base-to-pre-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-left-cable-inter-pose*)) (setq base-to-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-left-cable-pose*)))) - + ;;set hand (send *robot* :rarm :inverse-kinematics base-to-pre-insert-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) From fcb6b10e517a43b3835295da292e800fc29c04b8 Mon Sep 17 00:00:00 2001 From: softyanija Date: Thu, 23 Dec 2021 16:23:29 +0900 Subject: [PATCH 088/127] [euslisp/grasp_and_insert_cable] adjusting to complete demo 2 --- .../grasp_and_insert_cable.l | 1 + .../euslisp/panda_commons/parameters.l | 29 +++++++-- .../euslisp/panda_commons/utils.l | 63 ++++++++++++++++--- 3 files changed, 79 insertions(+), 14 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l index a7e80ba9f1..c06d43a3e6 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l @@ -81,6 +81,7 @@ (setq *base_to_rarm_elbow_p_servo* (send *tfl* :lookup-transform "dual_arm_base" "rpov-rarm_elbow_p_servo" stamp))) ;; + (rarm-grasp-cable "rpov-rarm_elbow_p_servo" "right") (rarm-insert-cable-test "rpov-rarm_elbow_p_servo" "right") diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l index 5cc82aeea6..b7c5110a04 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l @@ -18,6 +18,8 @@ (setq *null-wrench* #f(0 0 0 0 0 0)) + + ;;; parameters (setq *servo-to-search-cable-pose* (make-cascoords @@ -47,31 +49,46 @@ (setq *cable-front-to-pre-grasp-pose-1* (make-cascoords :pos #f(-45.691 22.526 27.675) - :rpy #f(-1.655 0.3 1.413) + :rpy #f(-1.655 0.5 1.413) )) (setq *cable-front-to-pre-grasp-pose-2* (make-cascoords :pos #f(-39.479 16.61 12.343) - :rpy #f(-1.365 0.3 1.57) + :rpy #f(-1.365 0.5 1.57) )) (setq *cable-front-to-grasp-pose* (make-cascoords - :pos #f(-16.344 21.751 16.114) - :rpy #f(-1.45 0.449 1.552) + :pos #f(-16.344 21.751 10.114) + :rpy #f(-1.45 0.5 1.552) )) (setq *cable-front-change-pose-before-insert* (make-cascoords :pos #f(0 0 0) :rpy #f(0 1.57 0) )) +(setq *cable-front-change-pose-before-insert-1* + (make-cascoords + :pos #f(17.295 -52.435 39.996) + :rpy #f(1.279 0.372 1.172) + )) +(setq *cable-front-change-pose-before-insert-2* + (make-cascoords + :pos #f(17.353 -43.806 45.33) + :rpy #f(1.294 0.654 1.195) + )) +(setq *cable-front-change-pose-before-insert-3* + (make-cascoords + :pos #f(22.596 -32.874 50.651) + :rpy #f(1.347 0.974 1.204) + )) (setq *servo-to-slide-insert-right-cable-inter-pose* (make-cascoords - :pos #f(34.757 -12.996 42.748) + :pos #f(34.757 -12.996 40.748) :rpy #f(-2.515 1.479 -2.446) )) (setq *servo-to-slide-insert-right-cable-pose* (make-cascoords - :pos #f(28 -13.281 37.626) + :pos #f(28 -13.281 35.626) :rpy #f(-1.57 1.57 -1.57) )) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index 9b959077f3..3540a1437c 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -244,6 +244,7 @@ memo to get parameters (4 (ros::ros-info "Can't find cable") (return-from rarm-grasp-cable nil))))) + (setq base-to-search-pose (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *servo-to-cable-front-grasp-ready*)) (ros::ros-info "start to grasp cable") @@ -255,9 +256,11 @@ memo to get parameters (setq stamp (ros::time-now)) (when (send *tfl* :wait-for-transform "dual_arm_base" marker-name stamp 5) (setq base-to-cable-marker-origin (send *tfl* :lookup-transform "dual_arm_base" marker-name stamp))) + (send *robot* :rarm :move-end-pos #f(-20 0 30)) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) + (send *robot* :rarm_joint1 :joint-angle 60) (send *robot* :rarm :inverse-kinematics base-to-search-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) @@ -288,14 +291,36 @@ memo to get parameters (send *robot* :rarm :inverse-kinematics base-to-grasp-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) + (send *robot* :rarm :move-end-pos #f(2 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) ;;grasp (send *ri* :start-grasp :rarm) (unix:sleep 2) - (setq self-coords (send *robot* :rarm :end-coords)) - (setq target-pos (send (setq self-coords (send *robot* :rarm :end-coords)) :worldpos)) - (send (setq target-coords (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *cable-front-change-pose-before-insert*)) :replace-pos target-pos) + (setq base-to-change-pose-1 (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *cable-front-change-pose-before-insert-1*)) + (setq base-to-change-pose-2 (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *cable-front-change-pose-before-insert-2*)) + (setq base-to-change-pose-3 (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *cable-front-change-pose-before-insert-3*)) + + (send *robot* :rarm :inverse-kinematics base-to-change-pose-1) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + (send *robot* :rarm :inverse-kinematics base-to-change-pose-2) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (send *robot* :rarm :inverse-kinematics base-to-change-pose-3) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + ;; (setq self-coords (send *robot* :rarm :end-coords)) + ;; (setq target-pos (send (setq self-coords (send *robot* :rarm :end-coords)) :worldpos)) + ;; (send (setq target-coords (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *cable-front-change-pose-before-insert*)) :replace-pos target-pos) + + ;; (send *robot* :rarm :inverse-kinematics target-coords) + ;; (send *robot* :rarm :move-end-pos #f(5 10 10)) + ;; (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + ;; (send *ri* :wait-interpolation) )) (defun rarm-insert-cable (servo-name side) @@ -420,11 +445,13 @@ memo to get parameters (setq total-insert 0) (setq total-insert-th 7.0) (setq mode 1) - (while t + (setq count 0) + (while t (setq wrench-pre (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k)) (case mode (1 (ros::ros-info "Move to +y") + (setq count (+ count 1)) (move-rarm-y h_delta) (move-rarm-x i_delta) (unix:sleep 1) @@ -446,6 +473,7 @@ memo to get parameters (setq mode 2)) (2 (ros::ros-info "Move to -y") + (setq count (+ count 1)) (move-rarm-y (- 0 h_delta)) (move-rarm-x i_delta) (unix:sleep 1) @@ -467,6 +495,7 @@ memo to get parameters (setq mode 3)) (3 (ros::ros-info "Move to +z") + (setq count (+ count 1)) (move-rarm-z h_delta) (move-rarm-x i_delta) (unix:sleep 1) @@ -488,6 +517,7 @@ memo to get parameters (setq mode 4)) (4 (ros::ros-info "Move to -z") + (setq count (+ count 1)) (move-rarm-z (- 0 h_delta)) (move-rarm-x i_delta) (unix:sleep 1) @@ -507,17 +537,26 @@ memo to get parameters (move-rarm-y h_delta) (move-rarm-x (- 0 i_delta)))) (setq mode 1))) - (if (> total-insert total-insert-th) + (ros::ros-info (format nil "times of try is ~A" count)) + (if (or (> total-insert total-insert-th) (> count 20)) (progn + (ros::ros-info "rushing") (send *robot* :angle-vector (send *ri* :state :potentio-vector)) (send *robot* :rarm :move-end-pos #f(10 0 0)) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) (send *robot* :rarm :move-end-pos #f(-10 0 0)) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) - (send *ri* :wait-interpolation))) - ) - + (send *ri* :wait-interpolation) + (return-from rarm-insert-cable t))) + ;; (setq f hoge) + ;; (if f + ;; (progn + ;; (ros::ros-info "success") + ;; (return from rarm-insert-cable-test t)) + ;; (progn + ;; (return to start point))) + ))) @@ -534,4 +573,12 @@ memo to get parameters (send *ri* :wait-interpolation) (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 cable-copy (send base-to-cable-marker-origin :copy-worldcoords)) + (send cable-copy (send cable_origin :copy-worldcoords)) + (setq cable-rpy (elt (rpy-angle (send cable-copy :rot)) 0)) + (if (> (elt cable-rpy 0) 0.7) + (progn + (rplaca) + (send base-to-cable-marker-origin :rpy 0.7 (elt cable-rpy 1) (elt cable-rpy 2)) |# From 300db0b1ad24759ccc8b5e3b83466df0f49a3b31 Mon Sep 17 00:00:00 2001 From: softyanija Date: Thu, 23 Dec 2021 16:26:20 +0900 Subject: [PATCH 089/127] [euslisp/grasp_and_insert_cable] rename func --- .../grasp_and_insert_cable.l | 2 +- .../euslisp/panda_commons/utils.l | 57 ------------------- 2 files changed, 1 insertion(+), 58 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l index c06d43a3e6..5c8430eac5 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l @@ -84,4 +84,4 @@ (rarm-grasp-cable "rpov-rarm_elbow_p_servo" "right") -(rarm-insert-cable-test "rpov-rarm_elbow_p_servo" "right") +(rarm-insert-cable "rpov-rarm_elbow_p_servo" "right") diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index 3540a1437c..335b92ea9c 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -324,63 +324,6 @@ memo to get parameters )) (defun rarm-insert-cable (servo-name side) - (let ((base-to-marker-origin nil) - (base-to-pre-insert-pose nil) - (base-to-insert-pose nil) - (cable-id-list nil) - (stamp nil) - (ret nil)) - (setq buf (subseq servo-name 5 (length servo-name))) - (setq base-to-marker-origin (eval (read-from-string (format nil "~A~A~A" "*base_to_" buf "*")))) - (if (string= side "right") - (progn - (setq base-to-pre-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-right-cable-inter-pose*)) - (setq base-to-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-right-cable-pose*))) - (progn - (setq base-to-pre-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-left-cable-inter-pose*)) - (setq base-to-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-left-cable-pose*)))) - - ;;set hand - (send *robot* :rarm :inverse-kinematics base-to-pre-insert-pose) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) - (send *ri* :wait-interpolation) - (send *robot* :rarm :inverse-kinematics base-to-insert-pose) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) - (send *ri* :wait-interpolation) - - ;;get corret wrench and set as wrench-init - (setq wrench-init (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k)) - (while (equal wrench-init *null-wrench*) - (ros::ros-info "Couldn't get correct wrench") - ;;TODO escape from singular pose - (setq wrench-init (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k))) - - (setq i 0) - (setq z-lim 3) - (setq base-stroke #f(0.5 0 0)) - (while (< i 30) - (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 diff-z (elt wrench-diff 2)) - (if (> diff-z z-lim) - (progn - (ros::ros-info "detect contacting to serface of servo") - (return)) - (progn - (ros::ros-info "still hanging in the air") - (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)) - (setq stroke (* (elt base-stroke 0) i)) - (ros::ros-info (format nil "stroke is ~A" stroke))))) - - ;;start to insert cable - - - )) - -(defun rarm-insert-cable-test (servo-name side) (let ((base-to-marker-origin nil) (base-to-pre-insert-pose nil) (base-to-insert-pose nil) From 9d8baf93f6cab9285deddcc6bb9142b8813e1d75 Mon Sep 17 00:00:00 2001 From: softyanija Date: Thu, 23 Dec 2021 18:52:44 +0900 Subject: [PATCH 090/127] [euslisp/grasp_and_insert_cable] making move to set KXR to jig --- .../grasp_and_insert_cable.l | 41 ++++++++++++------- .../euslisp/panda_commons/parameters.l | 10 +++++ .../euslisp/panda_commons/utils.l | 9 ++-- 3 files changed, 41 insertions(+), 19 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l index 5c8430eac5..991e469c73 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l @@ -35,29 +35,42 @@ (send *ri* :set-joint-pd-gain "rarm_joint7" 100 10) (send *robot* :angle-vector (send *ri* :state :potentio-vector)) -(send *robot* :reset-pose) -(send *robot* :rarm :move-end-pos #f(-500 0 100)) -(send *robot* :larm :move-end-pos #f(-500 0 100)) -(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *robot* :reset-coop-pose) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) -;;before rarm grasp kxr -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(341.012 -147.428 1370.354) :rpy (float-vector 1.222 1.551 -0.386))) :translation-axis t :rotation-axis t) -(send *robot* :larm_joint1 :joint-angle -30) -(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(227.932 139.233 1435.999) :rpy (float-vector -0.09 1.312 2.977))) :translation-axis t :rotation-axis t) +(while t + (setq found-body (search-body)) + (if (= found-body "front") + (progn + (ros::ros-info "Try to reverse body"))) + (if (= found-body "back") + (progn + (ros::ros-info "Recordind positon of kxr") + (setq stamp (ros::time-now)) + (when (send *tfl* :wait-for-transform "dual_arm_base" "rpov-body_back" stamp 5) + (setq *base_to_body_back* (send *tfl* :lookup-transform "dual_arm_base" "rpov-body_back" stamp))) + (return)))) + +;;assume that KXR is facing down +;;assume that KXR + + + +;;set KXR to jig +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(308.472 -259.081 1212.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(305.157 -14.307 1212.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) - -;;lower rarm -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(341.012 -147.428 1301.354) :rpy (float-vector 1.222 1.551 -0.386))) :translation-axis t :rotation-axis t) +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1210.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1214.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) -;;grasp kxr -(send *ri* :start-grasp :rarm) -(unix:sleep 1) +;;release KXR +;; ;;grasp elbow from below (larm-grasp-rarm-elbow-p-servo-from-below) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l index b7c5110a04..9833ac5a3c 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l @@ -21,6 +21,16 @@ ;;; parameters +(setq *body-back-to-larm-grasp-pose* + (make-cascoords + :pos #f(-124.991 17 -55) + :rpy #f(2.171 1.536 2.122) + )) +(setq *body-back-to-rarm-grasp-pose* + (make-cascoords + :pos #f(124.456 17 -55) + :rpy #f(2.576 1.517 -0.646) + )) (setq *servo-to-search-cable-pose* (make-cascoords :pos #f(-3.797 -98.159 34.853) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index 335b92ea9c..95f8e18f52 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -3,10 +3,6 @@ (require "package://panda_eus/euslisp/dual_panda-interface.l") (require "package://jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l") -(ros::roseus-add-msgs "std_msgs") - -(ros::load-ros-manifest "apriltag_ros") - ;;; check if an id exists in target-list (defun id-in-list (id target-list) (dolist (x target-list) @@ -43,6 +39,7 @@ (let ((detection-results-msg nil) (body-front-id 0) (body-back-id 1) + (found-id nil) (id-list nil)) (ros::ros-info "searching body tag...") @@ -60,11 +57,13 @@ (if (id-in-list body-front-id id-list) (progn (ros::ros-info "body-front was found") + (setq found-id body-front-id) (return-from search-body "front"))) (if (id-in-list body-back-id id-list) (progn (ros::ros-info "body-back was found") - (return-from search-body "front")))) + (setq found-id body-front-id) + (return-from search-body "back")))) (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(230 -140.079 1500.936) :rpy (float-vector 3.1 1.7 0))) :translation-axis t :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) From e0e10bde6862770aca183f05d0e708f55c361123 Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 24 Dec 2021 17:34:51 +0900 Subject: [PATCH 091/127] [euslisp/grasp_and_insert_cable] made mve to set KXR to jig, and reomve after cable insertion --- .../grasp_and_insert_cable.l | 99 ++++++++++++++++--- .../euslisp/panda_commons/parameters.l | 2 +- .../euslisp/panda_commons/utils.l | 56 ++++++++++- 3 files changed, 141 insertions(+), 16 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l index 991e469c73..a259562505 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l @@ -41,25 +41,46 @@ (while t (setq found-body (search-body)) - (if (= found-body "front") + (if (string= found-body "front") (progn (ros::ros-info "Try to reverse body"))) - (if (= found-body "back") + (if (string= found-body "back") (progn - (ros::ros-info "Recordind positon of kxr") + (ros::ros-info "Recordind position of kxr") (setq stamp (ros::time-now)) (when (send *tfl* :wait-for-transform "dual_arm_base" "rpov-body_back" stamp 5) (setq *base_to_body_back* (send *tfl* :lookup-transform "dual_arm_base" "rpov-body_back" stamp))) (return)))) -;;assume that KXR is facing down -;;assume that KXR - +(setq base-to-larm-grasp-pose (send (send *base_to_body_back* :copy-worldcoords) :transform *body-back-to-larm-grasp-pose*)) +(setq base-to-rarm-grasp-pose (send (send *base_to_body_back* :copy-worldcoords) :transform *body-back-to-rarm-grasp-pose*)) +(send *robot* :rarm :inverse-kinematics base-to-larm-grasp-pose) +(send *robot* :rarm :move-end-pos #f(-100 0 0)) +(send *robot* :larm :inverse-kinematics base-to-rarm-grasp-pose) +(send *robot* :larm :move-end-pos #f(-100 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) -;;set KXR to jig -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(308.472 -259.081 1212.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) -(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(305.157 -14.307 1212.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) +(send *ri* :stop-grasp :rarm) +(send *ri* :stop-grasp :larm) +(unix:sleep 2) + +(send *robot* :rarm :inverse-kinematics base-to-larm-grasp-pose) +(send *robot* :larm :inverse-kinematics base-to-rarm-grasp-pose) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :rarm) +(send *ri* :start-grasp :larm) +(unix:sleep 2) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(308.472 -259.081 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(305.157 -14.307 1220.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1220.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) @@ -68,20 +89,30 @@ (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) -;;release KXR +(send *ri* :stop-grasp :larm) +(unix:sleep 1) + +(send *robot* :larm :move-end-pos #f(-100 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 2500) +(send *ri* :wait-interpolation) ;; ;;grasp elbow from below (larm-grasp-rarm-elbow-p-servo-from-below) (send *ri* :stop-grasp :rarm) +(unix:sleep 1) + +(send *robot* :rarm :move-end-pos #f(-100 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) ;;prepare to grasp cable -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(341.012 -147.428 1430.354) :rpy (float-vector 1.222 1.551 -0.386))) :translation-axis t :rotation-axis t) +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(270.152 15.464 1414.154) :rpy (float-vector 2.5 1.551 -0.386))) :translation-axis t :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) -(setq search-tag-pose (send (send *base-to-rarm-elbow-p-servo* :copy-worldcoords) :transform *servo-to-search-cable-pose*)) +(setq search-tag-pose (send (send *base_to_rarm_elbow_p_servo* :copy-worldcoords) :transform *servo-to-search-cable-pose*)) (send *robot* :rarm_joint1 :joint-angle 30) (send *robot* :rarm :inverse-kinematics search-tag-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) @@ -98,3 +129,47 @@ (rarm-grasp-cable "rpov-rarm_elbow_p_servo" "right") (rarm-insert-cable "rpov-rarm_elbow_p_servo" "right") + +(send *ri* :stop-grasp :rarm :width 0.007) +(unix:sleep 1) +(send *robot* :rarm :move-end-pos #f(-30 0 30)) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :move-end-pos #f(-70 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *ri* :stop-grasp :rarm) +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1210.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) +(send *robot* :rarm :move-end-pos #f(-100 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1210.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :rarm) +(unix:sleep 1) + +(larm-release-rarm-elbow-p-servo-from-below) + +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1214.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) +(send *robot* :larm :move-end-pos #f(-100 0 0)) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1214.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 2000) +(send *ri* :wait-interpolation) + +(send *ri* :start-grasp :larm) +(unix:sleep 1) + +(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(308.472 -259.081 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) +(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(305.157 -14.307 1220.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +;;KXR stand up diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l index 9833ac5a3c..c18ddd205c 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l @@ -135,7 +135,7 @@ )) (setq *rarm-elbow-p-to-inter2-pose* (make-cascoords - :pos #f(-141.254 -5.393 -96.748) + :pos #f(-160.254 -5.393 -96.748) :rpy #f(0.024 -0.937 -0.027) )) (setq *rarm-elbow-p-to-pre-grasp-pose* diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index 95f8e18f52..a9c4dd6bb7 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -1,7 +1,14 @@ #!/usr/bin/env roseus (require "package://panda_eus/euslisp/dual_panda-interface.l") -(require "package://jsk_2021_fix_kxr/euslisp/panda_commons/parameters.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") ;;; check if an id exists in target-list (defun id-in-list (id target-list) @@ -162,12 +169,53 @@ (send *ri* :wait-interpolation) (send *robot* :larm :inverse-kinematics base-to-grasp-pose) + (send *robot* :larm :move-end-pos #f(-3 0 0)) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) (send *ri* :start-grasp :larm) - (unix:sleep 1))) + (unix:sleep 1) + + (send *robot* :larm :move-end-pos #f(-3 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + )) +(defun larm-release-rarm-elbow-p-servo-from-below () + (let ((base-to-marker-origin nil) + (base-to-inter1-pose nil) + (base-to-inter2-pose nil) + (base-to-inter3-pose nil) + (base-to-pre-grasp-pose nil) + (base-to-grasp-pose nil) + (stamp nil) + (ret nil)) + (ros::ros-info "start to release rarm-elbow-p-servo from below") + (setq stamp (ros::time-now)) + (setq marker-name "lpov-rarm_elbow_p_servo") + (setq base-to-marker-origin *base_to_rarm_elbow_p_servo*) + (setq base-to-inter1-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *rarm-elbow-p-to-inter1-pose*)) + (setq base-to-inter2-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *rarm-elbow-p-to-inter2-pose*)) + (setq base-to-pre-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *rarm-elbow-p-to-pre-grasp-pose*)) + (setq base-to-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *rarm-elbow-p-to-grasp-pose*)) + + (send *ri* :stop-grasp :larm) + (unix:sleep 1) + + (send *robot* :larm :inverse-kinematics base-to-pre-grasp-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *robot* :larm :inverse-kinematics base-to-inter2-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *robot* :larm :inverse-kinematics base-to-inter1-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation))) + + + #| memo to get parameters (send *robot* :angle-vector (send *ri* :state :potentio-vector)) @@ -498,7 +546,9 @@ memo to get parameters ;; (return from rarm-insert-cable-test t)) ;; (progn ;; (return to start point))) - ))) + ) + + )) From ad7fc33c31c99c149816a80036d4db22bd836ab4 Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 27 Dec 2021 18:35:27 +0900 Subject: [PATCH 092/127] [euslisp/grasp_and_insert_cable] added larm-release-rarm-elbow-p-servo-from-below func --- .../grasp_and_insert_cable.l | 3 +- .../euslisp/panda_commons/parameters.l | 19 +++++++++ .../euslisp/panda_commons/utils.l | 42 +++++++++++++++++++ 3 files changed, 63 insertions(+), 1 deletion(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l index a259562505..f99d073cad 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l @@ -96,7 +96,8 @@ (send *ri* :angle-vector (send *robot* :angle-vector) 2500) (send *ri* :wait-interpolation) -;; +(larm-search-rarm-elbow-p-servo-tag) + ;;grasp elbow from below (larm-grasp-rarm-elbow-p-servo-from-below) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l index c18ddd205c..31be2a8064 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l @@ -31,6 +31,24 @@ :pos #f(124.456 17 -55) :rpy #f(2.576 1.517 -0.646) )) + +(setq *rarm_elbow_servo_p_search-pose-1* + (make-cascoords + :pos #f(0 0 -15) + :rpy #f(0 -0.3 0) + )) +(setq *rarm_elbow_servo_p_search-pose-2* + (make-cascoords + :pos #f(25 0 35) + :rpy #f(0 0.5 0) + )) +(setq *rarm_elbow_servo_p_search-pose-3* + (make-cascoords + :pos #f(30 30 0) + :rpy #f(-0.5 0 0) + )) + + (setq *servo-to-search-cable-pose* (make-cascoords :pos #f(-3.797 -98.159 34.853) @@ -51,6 +69,7 @@ :pos #f(13.83 -55.579 91.701) :rpy #f(-1.799 1.134 -0.262) )) + (setq *servo-to-cable-front-grasp-ready* (make-cascoords :pos #f(34.974 -76.339 49.308) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index a9c4dd6bb7..e751085a65 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -132,6 +132,48 @@ (ros::ros-info "Can't find tag") (return-from rarm-search-servo-tag nil))))))))) +(defun larm-search-rarm-elbow-p-servo-tag () + (let ((detection-results-msg nil) + (target-id nil) + (id-list nil) + (base-to-search-pose nil) + (count 0)) + (ros::ros-info "searching tag...") + (setq marker-name "lpov-rarm_elbow_p_servo") + (setq target-id (marker-name-to-servo-id marker-name)) + (setq start-pose (send (send *robot* :larm :end-coords) :copy-worldcoords)) + (while t + (setq detection-results-msg (one-shot-subscribe "/left_tag_detections" apriltag_ros::AprilTagDetectionArray)) + (when detection-results-msg + (ros::ros-info "Got a message") + (setq id-list (mapcar #'(lambda (x) (elt (send x :id) 0)) (send detection-results-msg :detections))) + (print id-list) + (if (id-in-list target-id id-list) + (progn + (ros::ros-info "target-marker was found") + (return-from larm-search-rarm-elbow-p-servo-tag t)) + (progn + (setq count (+ count 1)) + (case count + (1 + (setq base-to-search-pose (send (send start-pose :copy-worldcoords) :transform *rarm_elbow_servo_p_search-pose-1*)) + (send *robot* :larm :inverse-kinematics base-to-search-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation)) + (2 + (setq base-to-search-pose (send (send start-pose :copy-worldcoords) :transform *rarm_elbow_servo_p_search-pose-2*)) + (send *robot* :larm :inverse-kinematics base-to-search-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation)) + (3 + (setq base-to-search-pose (send (send start-pose :copy-worldcoords) :transform *rarm_elbow_servo_p_search-pose-3*)) + (send *robot* :larm :inverse-kinematics base-to-search-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation)) + (4 + (ros::ros-info "Can't find tag") + (return-from larm-search-rarm-elbow-p-servo-tag nil))))))))) + (defun larm-grasp-rarm-elbow-p-servo-from-below () (let ((base-to-marker-origin nil) (base-to-inter1-pose nil) From b51ca67b99fdc0961151da1f17a70633ea8c5a8e Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 28 Dec 2021 20:12:19 +0900 Subject: [PATCH 093/127] [euslisp/grasp_and_insert_cable] adjust detail to accurate insertion --- .../euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l | 2 +- jsk_2021_fix_kxr/euslisp/panda_commons/utils.l | 5 ++++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l index f99d073cad..e94129a608 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l @@ -131,7 +131,7 @@ (rarm-insert-cable "rpov-rarm_elbow_p_servo" "right") -(send *ri* :stop-grasp :rarm :width 0.007) +(send *ri* :stop-grasp :rarm :width 0.005) (unix:sleep 1) (send *robot* :rarm :move-end-pos #f(-30 0 30)) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index e751085a65..174ca8a5c6 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -1,4 +1,4 @@ -#!/usr/bin/env roseus +x#!/usr/bin/env roseus (require "package://panda_eus/euslisp/dual_panda-interface.l") (ros::roseus-add-msgs "franka_msgs") @@ -421,6 +421,9 @@ memo to get parameters (ret nil)) (setq buf (subseq servo-name 5 (length servo-name))) (setq base-to-marker-origin (eval (read-from-string (format nil "~A~A~A" "*base_to_" buf "*")))) + (setq stamp (ros::time-now)) + (when (send *tfl* :wait-for-transform "dual_arm_base" servo-name stamp 5) + (setq base-to-marker-origin (send *tfl* :lookup-transform "dual_arm_base" servo-name stamp))) (if (string= side "right") (progn (setq base-to-pre-insert-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *servo-to-slide-insert-right-cable-inter-pose*)) From 1c71960d0f7210c3550c82533419cebb7d416a53 Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 28 Dec 2021 21:41:48 +0900 Subject: [PATCH 094/127] [euslisp/grasp_and_insert_cable] start to use color detection to accurate grasp --- .../euslisp/panda_commons/utils.l | 22 +++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index 174ca8a5c6..5e47734f16 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -1,4 +1,4 @@ -x#!/usr/bin/env roseus +#!/usr/bin/env roseus (require "package://panda_eus/euslisp/dual_panda-interface.l") (ros::roseus-add-msgs "franka_msgs") @@ -9,7 +9,7 @@ x#!/usr/bin/env roseus (ros::load-ros-manifest "franka_msgs") (ros::load-ros-manifest "apriltag_ros") - +(ros::load-ros-manifest "opencv_apps") ;;; check if an id exists in target-list (defun id-in-list (id target-list) (dolist (x target-list) @@ -384,6 +384,24 @@ memo to get parameters (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) + (return-from rarm-grasp-cable nil) + + (setq rects (send (one-shot-subscribe "/rarm_cable_terminal_detection/general_contours/rectangles" opencv_apps::RotatedRectArrayStamped) :rects)) + (when rects + (progn + (setq l (length rects)) + (setq i 0) + (setq max_i 0) + (setq size_max 0) + (setq size_buf 0) + (while (< i l) + (setq size_buf (* (send (send (elt rects i) :size) :width) (send (send (elt rects i) :size) :height))) + (if (> size_buf size_max) + (progn + (setq size_max size_buf) + (setq max_i i))) + (setq i (+ i 1))))) + ;;grasp (send *ri* :start-grasp :rarm) (unix:sleep 2) From 63e800f3e869021f680670c544ffa3eb5028a816 Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 29 Dec 2021 16:39:22 +0900 Subject: [PATCH 095/127] [euslisp/grasp_and_insert_cable] complete color recognition part --- .../grasp_and_insert_cable.l | 1 + .../euslisp/panda_commons/parameters.l | 2 +- .../euslisp/panda_commons/utils.l | 109 ++++++++++++++---- 3 files changed, 91 insertions(+), 21 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l index e94129a608..8977b6d66a 100644 --- a/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l +++ b/jsk_2021_fix_kxr/euslisp/grasp_and_insert_cable/grasp_and_insert_cable.l @@ -12,6 +12,7 @@ (ros::load-ros-manifest "franka_msgs") (ros::load-ros-manifest "apriltag_ros") +(ros::load-ros-manifest "opencv_apps") (ros::roseus "grasp_and_insert_cable") diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l index 31be2a8064..c753a15b8c 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l @@ -117,7 +117,7 @@ )) (setq *servo-to-slide-insert-right-cable-pose* (make-cascoords - :pos #f(28 -13.281 35.626) + :pos #f(32 -13.281 38.626) :rpy #f(-1.57 1.57 -1.57) )) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index 5e47734f16..d3564e0c30 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -366,7 +366,7 @@ memo to get parameters ;;pre-grasp-poses (send *ri* :start-grasp :rarm) (unix:sleep 1) - (send *ri* :stop-grasp :rarm :width 0.02) + (send *ri* :stop-grasp :rarm :width 0.01) (unix:sleep 1) (send *robot* :rarm :inverse-kinematics base-to-pre-grasp-pose-1) @@ -384,27 +384,96 @@ memo to get parameters (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) - (return-from rarm-grasp-cable nil) - - (setq rects (send (one-shot-subscribe "/rarm_cable_terminal_detection/general_contours/rectangles" opencv_apps::RotatedRectArrayStamped) :rects)) - (when rects - (progn - (setq l (length rects)) - (setq i 0) - (setq max_i 0) - (setq size_max 0) - (setq size_buf 0) - (while (< i l) - (setq size_buf (* (send (send (elt rects i) :size) :width) (send (send (elt rects i) :size) :height))) - (if (> size_buf size_max) + (setq goal_x 670) + (setq goal_y 650) + (setq robust_x 60) + (setq min_x (- goal_x (/ robust_x 2))) + (setq max_x (+ goal_x (/ robust_x 2))) + (setq min_y 630) + (setq max_y 675) + (setq k_x 0.13) + (setq k_y 0.116) + (setq fx 0) + (setq fy 0) + (while (< fx 3) + (setq rects (send (one-shot-subscribe "/rarm_cable_terminal_detection/general_contours/rectangles" opencv_apps::RotatedRectArrayStamped) :rects)) + (when rects + (progn + (setq l (length rects)) + (setq i 0) + (setq max_i 0) + (setq size_max 0) + (setq size_buf 0) + + (while (< i l) + (setq size_buf (* (send (send (elt rects i) :size) :width) (send (send (elt rects i) :size) :height))) + (if (> size_buf size_max) + (progn + (setq size_max size_buf) + (setq max_i i))) + (ros::ros-info (format nil "fx:~A index:~A, size:~A" fx i size_buf)) + (setq i (+ i 1))) + (setq x (send (send (elt rects max_i) :center) :x)) + (setq y (send (send (elt rects max_i) :center) :y)) + (ros::ros-info (format nil "x:~A, y:~A" x y)) + (if (or (< x min_x) (> x max_x)) (progn - (setq size_max size_buf) - (setq max_i i))) - (setq i (+ i 1))))) + (setq delta_x (* k_x (- x goal_x))) + (setq fx 0)) + (progn + (setq delta_x (* 0.4 (* k_x (- x goal_x)))) + (setq fx (+ fx 1)))) + (send *robot* :rarm :move-end-pos (float-vector 0 delta_x 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 1500) + (send *ri* :wait-interpolation) + (unix:sleep 1)))) + + (while (< fy 3) + (setq rects (send (one-shot-subscribe "/rarm_cable_terminal_detection/general_contours/rectangles" opencv_apps::RotatedRectArrayStamped) :rects)) + (when rects + (progn + (setq l (length rects)) + (setq i 0) + (setq max_i 0) + (setq size_max 0) + (setq size_buf 0) + + (while (< i l) + (setq size_buf (* (send (send (elt rects i) :size) :width) (send (send (elt rects i) :size) :height))) + (if (> size_buf size_max) + (progn + (setq size_max size_buf) + (setq max_i i))) + (ros::ros-info (format nil "fy:~A index:~A, size:~A" fy i size_buf)) + (setq i (+ i 1))) + (setq x (send (send (elt rects max_i) :center) :x)) + (setq y (send (send (elt rects max_i) :center) :y)) + (ros::ros-info (format nil "x:~A, y:~A" x y)) + (cond + ((< y min_y) (progn + (setq delta_y (* 0.7 (* k_y (- y goal_y)))) + (setq fy 0))) + ((> y max_y) (progn + (setq delta_y (* 1.0 (* k_y (- y goal_y)))) + (setq fy 0))) + (t (progn + (setq delta_y 0) + (setq fy (+ fy 1))))) + (send *robot* :rarm :move-end-pos (float-vector 0 0 delta_y)) + (send *ri* :angle-vector (send *robot* :angle-vector) 1500) + (send *ri* :wait-interpolation) + (unix:sleep 1)))) - ;;grasp (send *ri* :start-grasp :rarm) - (unix:sleep 2) + (unix:sleep 1) + (send *ri* :stop-grasp :rarm :width 0.003) + (unix:sleep 1) + + (send *robot* :rarm :move-end-pos (float-vector 2 2 -4)) + (send *ri* :angle-vector (send *robot* :angle-vector) 1500) + (send *ri* :wait-interpolation) + (send *ri* :start-grasp :rarm) + (unix:sleep 1) (setq base-to-change-pose-1 (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *cable-front-change-pose-before-insert-1*)) (setq base-to-change-pose-2 (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *cable-front-change-pose-before-insert-2*)) @@ -488,7 +557,7 @@ memo to get parameters (ros::ros-info (format nil "stroke is ~A" stroke))))) (send *robot* :angle-vector (send *ri* :state :potentio-vector)) - (send *robot* :rarm :move-end-pos #f(0 0 -5)) + (send *robot* :rarm :move-end-pos #f(0 0 -8)) (send *ri* :angle-vector (send *robot* :angle-vector) 4000) (send *ri* :wait-interpolation) From 5c10378da913e4a6948bda6dd6f99ed736a51995 Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 3 Jan 2022 11:57:40 +0900 Subject: [PATCH 096/127] [srv,euslisp] prepare RepairInfo service and wrote test program --- jsk_2021_fix_kxr/CMakeLists.txt | 5 ++++ .../euslisp/service-test/client.l | 26 +++++++++++++++++++ .../euslisp/service-test/server.l | 23 ++++++++++++++++ jsk_2021_fix_kxr/srv/RepairInfo.srv | 3 +++ 4 files changed, 57 insertions(+) create mode 100755 jsk_2021_fix_kxr/euslisp/service-test/client.l create mode 100755 jsk_2021_fix_kxr/euslisp/service-test/server.l create mode 100644 jsk_2021_fix_kxr/srv/RepairInfo.srv diff --git a/jsk_2021_fix_kxr/CMakeLists.txt b/jsk_2021_fix_kxr/CMakeLists.txt index 866ac21724..70d85e999c 100644 --- a/jsk_2021_fix_kxr/CMakeLists.txt +++ b/jsk_2021_fix_kxr/CMakeLists.txt @@ -13,6 +13,11 @@ add_message_files( Repair.msg ) +add_service_files( + FILES + RepairInfo.srv + ) + generate_messages( DEPENDENCIES std_msgs diff --git a/jsk_2021_fix_kxr/euslisp/service-test/client.l b/jsk_2021_fix_kxr/euslisp/service-test/client.l new file mode 100755 index 0000000000..aae3b1e727 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/service-test/client.l @@ -0,0 +1,26 @@ +#!/usr/bin/env roseus +(ros::load-ros-manifest "roseus") +(ros::roseus "add_two_ints_client") +(when (setq *arguments* (member "client.l" lisp::*eustop-argument* :test #'substringp)) + (cond ((= (length *arguments*) 1) + (setq x (random 10) y (random 20))) + ((= (length *arguments*) 3) + (setq x (read-from-string (elt *arguments* 1)) + y (read-from-string (elt *arguments* 2)))) + (t + (ros::ros-error "Usage: ~A [x y]~%" (elt *arguments* 0)) + (exit 1)))) + +(ros::wait-for-service "add_two_ints") +(dotimes (i 100) + (setq req (instance roseus::AddTwoIntsRequest :init)) + (send req :a x) + (send req :b y) + (setq before (ros::time-now)) + (case (mod i 3) + (0 (setq res (ros::service-call "add_two_ints" req t))) + (1 (setq res (ros::service-call "add_two_ints" req nil))) + (2 (setq res (ros::service-call "add_two_ints" req t)))) + (setq after (ros::time-now)) + (format t "~d + ~d = ~d~ (~A sec)~%" (send req :a) (send req :b) (send res :sum) (send (ros::time- after before) :to-sec)) + (unix:sleep 1)) diff --git a/jsk_2021_fix_kxr/euslisp/service-test/server.l b/jsk_2021_fix_kxr/euslisp/service-test/server.l new file mode 100755 index 0000000000..fd7493f7ca --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/service-test/server.l @@ -0,0 +1,23 @@ +#!/usr/bin/env roseus +(ros::load-ros-manifest "roseus") +(defun add-two-ints (req) + (let ((m (send req :response))) + (format *error-output* "Returning [~d + ~d = ~d]" + (send req :a) (send req :b) + (+ (send req :a) (send req :b))) + (send m :sum (+ (send req :a) (send req :b))) + m)) + +(ros::roseus "add_two_ints_server") +(ros::advertise-service "add_two_ints" roseus::AddTwoInts #'add-two-ints) +(do-until-key + (ros::spin-once)) + + +;; (ros::roseus "server_test") +;; (ros::roseus-add-srvs "jsk_2021_fix_kxr") +;; (ros::roseus-add-srvs "roseus") + +;; (defun AddTwoInts +;; (ros::advertise-service "addtwoints" roseus::AddTwoInts #'add-two-int +;; ) diff --git a/jsk_2021_fix_kxr/srv/RepairInfo.srv b/jsk_2021_fix_kxr/srv/RepairInfo.srv new file mode 100644 index 0000000000..82b2269f76 --- /dev/null +++ b/jsk_2021_fix_kxr/srv/RepairInfo.srv @@ -0,0 +1,3 @@ +std_msgs/String input +--- +std_msgs/String output From a2fa2f248d3b916bff80a676fbf8e4cda1d87c72 Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 3 Jan 2022 15:34:10 +0900 Subject: [PATCH 097/127] [euslisp] add conditional branch --- .../euslisp/service-test/client.l | 58 +++++++++++-------- .../euslisp/service-test/server.l | 46 ++++++++++----- jsk_2021_fix_kxr/srv/RepairInfo.srv | 4 +- 3 files changed, 67 insertions(+), 41 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/service-test/client.l b/jsk_2021_fix_kxr/euslisp/service-test/client.l index aae3b1e727..37fe15aae1 100755 --- a/jsk_2021_fix_kxr/euslisp/service-test/client.l +++ b/jsk_2021_fix_kxr/euslisp/service-test/client.l @@ -1,26 +1,36 @@ #!/usr/bin/env roseus -(ros::load-ros-manifest "roseus") -(ros::roseus "add_two_ints_client") -(when (setq *arguments* (member "client.l" lisp::*eustop-argument* :test #'substringp)) - (cond ((= (length *arguments*) 1) - (setq x (random 10) y (random 20))) - ((= (length *arguments*) 3) - (setq x (read-from-string (elt *arguments* 1)) - y (read-from-string (elt *arguments* 2)))) - (t - (ros::ros-error "Usage: ~A [x y]~%" (elt *arguments* 0)) - (exit 1)))) -(ros::wait-for-service "add_two_ints") -(dotimes (i 100) - (setq req (instance roseus::AddTwoIntsRequest :init)) - (send req :a x) - (send req :b y) - (setq before (ros::time-now)) - (case (mod i 3) - (0 (setq res (ros::service-call "add_two_ints" req t))) - (1 (setq res (ros::service-call "add_two_ints" req nil))) - (2 (setq res (ros::service-call "add_two_ints" req t)))) - (setq after (ros::time-now)) - (format t "~d + ~d = ~d~ (~A sec)~%" (send req :a) (send req :b) (send res :sum) (send (ros::time- after before) :to-sec)) - (unix:sleep 1)) +(ros::load-ros-manifest "jsk_2021_fix_kxr") +(ros::roseus "client") + +(setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) +(send req :action "hoge") +(setq res (ros::service-call "repair_reaction" req t)) + + + +;; (ros::load-ros-manifest "roseus") +;; (ros::roseus "add_two_ints_client") +;; (when (setq *arguments* (member "client.l" lisp::*eustop-argument* :test #'substringp)) +;; (cond ((= (length *arguments*) 1) +;; (setq x (random 10) y (random 20))) +;; ((= (length *arguments*) 3) +;; (setq x (read-from-string (elt *arguments* 1)) +;; y (read-from-string (elt *arguments* 2)))) +;; (t +;; (ros::ros-error "Usage: ~A [x y]~%" (elt *arguments* 0)) +;; (exit 1)))) + +;; (ros::wait-for-service "add_two_ints") +;; (dotimes (i 100) +;; (setq req (instance roseus::AddTwoIntsRequest :init)) +;; (send req :a x) +;; (send req :b y) +;; (setq before (ros::time-now)) +;; (case (mod i 3) +;; (0 (setq res (ros::service-call "add_two_ints" req t))) +;; (1 (setq res (ros::service-call "add_two_ints" req nil))) +;; (2 (setq res (ros::service-call "add_two_ints" req t)))) +;; (setq after (ros::time-now)) +;; (format t "~d + ~d = ~d~ (~A sec)~%" (send req :a) (send req :b) (send res :sum) (send (ros::time- after before) :to-sec)) +;; (unix:sleep 1)) diff --git a/jsk_2021_fix_kxr/euslisp/service-test/server.l b/jsk_2021_fix_kxr/euslisp/service-test/server.l index fd7493f7ca..4938b1f82b 100755 --- a/jsk_2021_fix_kxr/euslisp/service-test/server.l +++ b/jsk_2021_fix_kxr/euslisp/service-test/server.l @@ -1,23 +1,39 @@ #!/usr/bin/env roseus -(ros::load-ros-manifest "roseus") -(defun add-two-ints (req) - (let ((m (send req :response))) - (format *error-output* "Returning [~d + ~d = ~d]" - (send req :a) (send req :b) - (+ (send req :a) (send req :b))) - (send m :sum (+ (send req :a) (send req :b))) + +(ros::load-ros-manifest "jsk_2021_fix_kxr") + +(defun hoge () + (ros::ros-info "hoge")) + +(setq time 0) +(ros::ros-info (format nil "time : ~d" time)) +(defun repair_reaction (req) + (let ((m (send req :response)) + (f "success")) + (ros::ros-info (format nil "request is ~A" (send req :action))) + (setq action (send req :action)) + (if (string= action "hoge") + (send m :result "hoge") + (send m :result "huga")) m)) -(ros::roseus "add_two_ints_server") -(ros::advertise-service "add_two_ints" roseus::AddTwoInts #'add-two-ints) +(ros::roseus "server") +(ros::advertise-service "repair_reaction" jsk_2021_fix_kxr::RepairInfo #'repair_reaction) (do-until-key (ros::spin-once)) -;; (ros::roseus "server_test") -;; (ros::roseus-add-srvs "jsk_2021_fix_kxr") -;; (ros::roseus-add-srvs "roseus") +;; (ros::load-ros-manifest "roseus") +;; (defun add-two-ints (req) +;; (let ((m (send req :response))) +;; (format *error-output* "Returning [~d + ~d = ~d]" +;; (send req :a) (send req :b) +;; (+ (send req :a) (send req :b))) +;; (send m :sum (+ (send req :a) (send req :b))) +;; m)) + +;; (ros::roseus "add_two_ints_server") +;; (ros::advertise-service "add_two_ints" roseus::AddTwoInts #'add-two-ints) +;; (do-until-key +;; (ros::spin-once)) -;; (defun AddTwoInts -;; (ros::advertise-service "addtwoints" roseus::AddTwoInts #'add-two-int -;; ) diff --git a/jsk_2021_fix_kxr/srv/RepairInfo.srv b/jsk_2021_fix_kxr/srv/RepairInfo.srv index 82b2269f76..6933f7d19d 100644 --- a/jsk_2021_fix_kxr/srv/RepairInfo.srv +++ b/jsk_2021_fix_kxr/srv/RepairInfo.srv @@ -1,3 +1,3 @@ -std_msgs/String input +string action --- -std_msgs/String output +string result From 0976cf6996114ae6c3439e9b0994f2365bfb8fc5 Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 4 Jan 2022 14:26:07 +0900 Subject: [PATCH 098/127] [euslisp/service-test] check communication with kxr --- .../euslisp/service-test/client.l | 2 +- .../euslisp/service-test/kxr_service_test.l | 72 +++++++++++++++++++ 2 files changed, 73 insertions(+), 1 deletion(-) create mode 100644 jsk_2021_fix_kxr/euslisp/service-test/kxr_service_test.l diff --git a/jsk_2021_fix_kxr/euslisp/service-test/client.l b/jsk_2021_fix_kxr/euslisp/service-test/client.l index 37fe15aae1..f3073ee5a5 100755 --- a/jsk_2021_fix_kxr/euslisp/service-test/client.l +++ b/jsk_2021_fix_kxr/euslisp/service-test/client.l @@ -4,7 +4,7 @@ (ros::roseus "client") (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) -(send req :action "hoge") +(send req :action "rarm-free") (setq res (ros::service-call "repair_reaction" req t)) diff --git a/jsk_2021_fix_kxr/euslisp/service-test/kxr_service_test.l b/jsk_2021_fix_kxr/euslisp/service-test/kxr_service_test.l new file mode 100644 index 0000000000..fa39fe54fe --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/service-test/kxr_service_test.l @@ -0,0 +1,72 @@ +#!/usr/bin/env roseus + +(load "/home/amabe/prog/rcb4eus/rosrcb4.l") +(make-kxr-robot "kxrl2l5a3h2g") +(send *ri* :ros-open) +(send *ri* :timer-on) +(send *ri* :hold-all) +(init-publish) + +(ros::roseus "kxr_robot") + +(ros::roseus-add-msgs "std_msgs") +(ros::roseus-add-msgs "sensor_msgs") +(ros::load-ros-manifest "std_msgs") +(ros::load-ros-manifest "sensor_msgs") +(ros::load-ros-manifest "jsk_2021_fix_kxr") + +(defun rarm-free () + (ros::ros-info "rarm free") + (send *ri* :rarm-free)) + +(defun larm-free () + (ros::ros-info "larm free") + (send *ri* :larm-free)) + +(defun rarm-hold () + (ros::ros-info "rarm hold") + (send *ri* :rarm-hold)) + +(defun repair_reaction (req) + (let ((m (send req :response)) + (f 0)) + (ros::ros-info (format nil "request is ~A" (send req :action))) + (setq action (send req :action)) + (if (string= action "rarm-free") + (progn + (rarm-free) + (send m :result "success") + (setq f 1))) + (if (string= action "larm-free") + (progn + (larm-free) + (send m :result "success") + (setq f 1))) + (if (string= action "rarm-hold") + (progn + (rarm-hold) + (send m :result "success") + (setq f 1))) + m)) + +(ros::roseus "kxr_server") +(ros::advertise-service "repair_reaction" jsk_2021_fix_kxr::RepairInfo #'repair_reaction) +(do-until-key + (ros::spin-once)) + + +;; (defun servo-onoff() +;; (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) +;; (print msg) +;; (cond ((string-equal msg "rarm-free") +;; (progn +;; (send *ri* :rarm-free))) +;; ((string-equal msg "larm-free") +;; (progn +;; (send *ri* :larm-free))) +;; ((string-equal msg "rarm-hold") +;; (progn +;; (send *ri* :rarm-hold))) +;; ((string-equal msg "larm-hold") +;; (progn +;; (send *ri* :larm-hold))))) From b4f7d447e7760093e7048553beea06cefdf4142d Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 4 Jan 2022 16:37:14 +0900 Subject: [PATCH 099/127] [euslisp/main] push before change RepairInfo and do catkin build --- jsk_2021_fix_kxr/euslisp/main/kxr_main.l | 142 +++++++++++++++++++++ jsk_2021_fix_kxr/euslisp/main/panda_main.l | 47 +++++++ 2 files changed, 189 insertions(+) create mode 100644 jsk_2021_fix_kxr/euslisp/main/kxr_main.l create mode 100644 jsk_2021_fix_kxr/euslisp/main/panda_main.l diff --git a/jsk_2021_fix_kxr/euslisp/main/kxr_main.l b/jsk_2021_fix_kxr/euslisp/main/kxr_main.l new file mode 100644 index 0000000000..a96e57a542 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/main/kxr_main.l @@ -0,0 +1,142 @@ +#!/usr/bin/env roseus + +(load "/home/amabe/prog/rcb4eus/rosrcb4.l") +(make-kxr-robot "kxrl2l5a3h2g") +(send *ri* :ros-open) +(send *ri* :timer-on) +(send *ri* :hold-all) +(init-publish) + +(ros::roseus "kxr_robot") + +(ros::roseus-add-msgs "std_msgs") +(ros::roseus-add-msgs "sensor_msgs") +(ros::load-ros-manifest "std_msgs") +(ros::load-ros-manifest "sensor_msgs") +(ros::load-ros-manifest "jsk_2021_fix_kxr") + +(defun rarm-free () + (ros::ros-info "rarm free") + (send *ri* :rarm-free)) + +(defun larm-free () + (ros::ros-info "larm free") + (send *ri* :larm-free)) + +(defun rarm-hold () + (ros::ros-info "rarm hold") + (send *ri* :rarm-hold)) + +(defun larm-hold () + (ros::ros-info "larm hold") + (send *ri* :larm-hold)) + +(defun rleg-free () + (ros::ros-info "rleg free") + (send *ri* :rleg-free)) + +(defun lleg-free () + (ros::ros-info "lleg free") + (send *ri* :lleg-free)) + +(defun rleg-hold () + (ros::ros-info "rleg hold") + (send *ri* :rleg-hold)) + +(defun lleg-hold () + (ros::ros-info "lleg hold") + (send *ri* :lleg-hold)) + +(defun show-rarm-shoulder-r () + (ros::ros-info "show-rarm-shoulder-r") + (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 -90.0 -90.0 0.0 -27.0 -27.0 -90.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 200)) + +(defun show-rarm-elbow-p () + (ros::ros-info "show-rarm-elbow-p") + (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 200)) + +(defun send-servo-info () + (ros::ros-info "send-servo-info") + (joint-publish)) + +(defun repair_reaction (req) + (let ((m (send req :response)) + (f 0)) + (ros::ros-info (format nil "request is ~A" (send req :action))) + (setq action (send req :action)) + (if (string= action "rarm-free") + (progn + (rarm-free) + (setq f 1))) + (if (string= action "larm-free") + (progn + (larm-free) + (setq f 1))) + (if (string= action "rarm-hold") + (progn + (rarm-hold) + (setq f 1))) + (if (string= action "larm-hold") + (progn + (larm-hold) + (setq f 1))) + (if (string= action "rleg-free") + (progn + (rleg-free) + (setq f 1))) + (if (string= action "lleg-free") + (progn + (lleg-free) + (setq f 1))) + (if (string= action "rleg-hold") + (progn + (rleg-hold) + (setq f 1))) + (if (string= action "lleg-hold") + (progn + (lleg-hold) + (setq f 1))) + (if (string= action "show-rarm-elbow-p") + (progn + (show-rarm-elbow-p) + (setq f 1))) + (if (string= action "show-rarm-sholder-r") + (progn + (show-rarm-sholder-r) + (setq f 1))) + (if (string= action "send-servo-info") + (progn + (send-servo-info) + (setq f 1))) + + + (if (= f 1) + (progn + (send m :result "success")) + (progn + (send m :result "false"))) + m)) + +(ros::roseus "kxr_server") +(ros::advertise-service "repair_reaction" jsk_2021_fix_kxr::RepairInfo #'repair_reaction) +(do-until-key + (ros::spin-once)) + + +;; (defun servo-onoff() +;; (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) +;; (print msg) +;; (cond ((string-equal msg "rarm-free") +;; (progn +;; (send *ri* :rarm-free))) +;; ((string-equal msg "larm-free") +;; (progn +;; (send *ri* :larm-free))) +;; ((string-equal msg "rarm-hold") +;; (progn +;; (send *ri* :rarm-hold))) +;; ((string-equal msg "larm-hold") +;; (progn +;; (send *ri* :larm-hold))))) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda_main.l b/jsk_2021_fix_kxr/euslisp/main/panda_main.l new file mode 100644 index 0000000000..4c695c59b8 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/main/panda_main.l @@ -0,0 +1,47 @@ +#!/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::load-ros-manifest "opencv_apps") +(ros::load-ros-manifest "jsk_2021_fix_kxr") + +(ros::roseus "panda_robot") + +(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) +(send *robot* :angle-vector (send *ri* :state :potentio-vector)) + +(send *robot* :reset-coop-pose) +(send *ri* :angle-vector (send *robot* :angle-vector) 3000) +(send *ri* :wait-interpolation) + +(setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) +(send req :action "send-servo-info") +(setq res (ros::service-call "repair_reaction" req t)) +(setq joint_states (one-shot-subscribe "/puppet_joint_states" sensor_msgs::jointstate)) From 46d7550f04d6c142f6eb3a6885e9dcccca2ecb7f Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 4 Jan 2022 18:21:52 +0900 Subject: [PATCH 100/127] [euslisp/main] changed RepairInfo.srv and now we can send joint_state as service --- jsk_2021_fix_kxr/CMakeLists.txt | 4 ++- jsk_2021_fix_kxr/euslisp/main/kxr_main.l | 37 ++++++++++------------ jsk_2021_fix_kxr/euslisp/main/panda_main.l | 2 +- jsk_2021_fix_kxr/package.xml | 2 ++ jsk_2021_fix_kxr/srv/RepairInfo.srv | 1 + 5 files changed, 23 insertions(+), 23 deletions(-) diff --git a/jsk_2021_fix_kxr/CMakeLists.txt b/jsk_2021_fix_kxr/CMakeLists.txt index 70d85e999c..2db16fbecc 100644 --- a/jsk_2021_fix_kxr/CMakeLists.txt +++ b/jsk_2021_fix_kxr/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs + sensor_msgs message_generation ) @@ -21,12 +22,13 @@ add_service_files( generate_messages( DEPENDENCIES std_msgs + sensor_msgs ) catkin_package( # INCLUDE_DIRS include # LIBRARIES ros_lecture -CATKIN_DEPENDS roscpp rospy std_msgs message_runtime +CATKIN_DEPENDS roscpp rospy std_msgs sensor_msgs message_runtime # DEPENDS system_lib ) diff --git a/jsk_2021_fix_kxr/euslisp/main/kxr_main.l b/jsk_2021_fix_kxr/euslisp/main/kxr_main.l index a96e57a542..4bdfa76dd0 100644 --- a/jsk_2021_fix_kxr/euslisp/main/kxr_main.l +++ b/jsk_2021_fix_kxr/euslisp/main/kxr_main.l @@ -7,8 +7,6 @@ (send *ri* :hold-all) (init-publish) -(ros::roseus "kxr_robot") - (ros::roseus-add-msgs "std_msgs") (ros::roseus-add-msgs "sensor_msgs") (ros::load-ros-manifest "std_msgs") @@ -59,7 +57,21 @@ (defun send-servo-info () (ros::ros-info "send-servo-info") - (joint-publish)) + (setq msg (instance sensor_msgs::jointstate :init)) + (send msg :name (send *robot* :joint-access-names)) + (send *robot* :angle-vector (send *robot* :ri :read-angle-vector)) + (send msg :effort (check-timeout-servo :ri t :name (send *robot* :name))) + (send msg :position (send *robot* :angle-vector)) + (send *robot* :ri :real-orientation) + (send *robot* :ri :robot :angle-vector (send *robot* :angle-vector)) + (send *robot* :ri :robot :draw-torque + ((send *robot* :ri :viewer) . x::viewer) + :torque-vector (send *robot* :ri :read-torque-vector) + :flush nil :size 10.0) + (send *robot* :ri :viewer :draw-objects :flush t) + (send *robot* :ri :viewer :flush) + (send *irtviewer* :draw-objects :flush t) + msg) (defun repair_reaction (req) (let ((m (send req :response)) @@ -108,7 +120,7 @@ (setq f 1))) (if (string= action "send-servo-info") (progn - (send-servo-info) + (send m :joint_state (send-servo-info)) (setq f 1))) @@ -123,20 +135,3 @@ (ros::advertise-service "repair_reaction" jsk_2021_fix_kxr::RepairInfo #'repair_reaction) (do-until-key (ros::spin-once)) - - -;; (defun servo-onoff() -;; (setq msg (send (one-shot-subscribe "/panda_order" std_msgs::String) :data)) -;; (print msg) -;; (cond ((string-equal msg "rarm-free") -;; (progn -;; (send *ri* :rarm-free))) -;; ((string-equal msg "larm-free") -;; (progn -;; (send *ri* :larm-free))) -;; ((string-equal msg "rarm-hold") -;; (progn -;; (send *ri* :rarm-hold))) -;; ((string-equal msg "larm-hold") -;; (progn -;; (send *ri* :larm-hold))))) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda_main.l b/jsk_2021_fix_kxr/euslisp/main/panda_main.l index 4c695c59b8..b5682b10f6 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda_main.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda_main.l @@ -44,4 +44,4 @@ (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) (send req :action "send-servo-info") (setq res (ros::service-call "repair_reaction" req t)) -(setq joint_states (one-shot-subscribe "/puppet_joint_states" sensor_msgs::jointstate)) +(send (send res :joint_state) :effort) diff --git a/jsk_2021_fix_kxr/package.xml b/jsk_2021_fix_kxr/package.xml index caf5952cfe..42212c3a7c 100644 --- a/jsk_2021_fix_kxr/package.xml +++ b/jsk_2021_fix_kxr/package.xml @@ -13,11 +13,13 @@ roscpp rospy std_msgs + sensor_msgs message_generation roscpp rospy std_msgs + sensor_msgs message_runtime fetcheus jsk_maps diff --git a/jsk_2021_fix_kxr/srv/RepairInfo.srv b/jsk_2021_fix_kxr/srv/RepairInfo.srv index 6933f7d19d..79da6ac403 100644 --- a/jsk_2021_fix_kxr/srv/RepairInfo.srv +++ b/jsk_2021_fix_kxr/srv/RepairInfo.srv @@ -1,3 +1,4 @@ string action --- string result +sensor_msgs/JointState joint_state From 9ee45a9d6fd6087b8259a014b6964d9ba91dc212 Mon Sep 17 00:00:00 2001 From: softyanija Date: Tue, 4 Jan 2022 20:07:00 +0900 Subject: [PATCH 101/127] [euslisp/main] start to write panda_main --- jsk_2021_fix_kxr/euslisp/main/panda_main.l | 18 ++++++++++++++- .../euslisp/panda_commons/parameters.l | 12 ++-------- .../euslisp/panda_commons/utils.l | 22 +++++++++++++++++++ 3 files changed, 41 insertions(+), 11 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda_main.l b/jsk_2021_fix_kxr/euslisp/main/panda_main.l index b5682b10f6..5787367d0b 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda_main.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda_main.l @@ -37,6 +37,8 @@ (send *ri* :set-joint-pd-gain "rarm_joint7" 100 10) (send *robot* :angle-vector (send *ri* :state :potentio-vector)) +(ros::ros-info "Start repairing") + (send *robot* :reset-coop-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) @@ -44,4 +46,18 @@ (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) (send req :action "send-servo-info") (setq res (ros::service-call "repair_reaction" req t)) -(send (send res :joint_state) :effort) +(setq (send res :joint_state)) +(setq timeout-servo-list (make-timeout-servo-list joint_states)) + +(while timeout-servo-list + (setq target ) + (if (string= target "rarm-elbow-p-servo") + (progn + (ros::ros-info "target is rarm-elbow-p-servo") + (panda-repair-rarm-elbow-p-servo))) + (if (string= target "larm-elbow-p-servo") + (progn + (ros::ros-info "target is larm-elbow-p-servo") + (panda-repair-larn-elbow-p-servo)))) + +(ros::ros-info "Finished repairing") diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l index c753a15b8c..028feb722c 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l @@ -5,16 +5,8 @@ (defvar *base_to_rarm_elbow_p_servo*) (defvar *target-id*) (defvar *target-id-list*) - -(defun marker-name-to-servo-id (marker-name) - (setq marker-name (subseq marker-name 5 (length marker-name))) - (if (string= marker-name "rarm_elbow_p_servo") - (return-from marker-name-to-servo-id 2))) - -(defun servo-name-to-cable-id-list (servo-name) - (setq servo-name (subseq servo-name 5 (length servo-name))) - (if (string= servo-name "rarm_elbow_p_servo") - (return-from servo-name-to-cable-id-list (list 8 9 10 11)))) +(defvar *reparing-servo-num*) +(defvar *panda_order*) (setq *null-wrench* #f(0 0 0 0 0 0)) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index d3564e0c30..d2882a37c7 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -11,6 +11,17 @@ (ros::load-ros-manifest "apriltag_ros") (ros::load-ros-manifest "opencv_apps") ;;; check if an id exists in target-list + +(defun marker-name-to-servo-id (marker-name) + (setq marker-name (subseq marker-name 5 (length marker-name))) + (if (string= marker-name "rarm_elbow_p_servo") + (return-from marker-name-to-servo-id 2))) + +(defun servo-name-to-cable-id-list (servo-name) + (setq servo-name (subseq servo-name 5 (length servo-name))) + (if (string= servo-name "rarm_elbow_p_servo") + (return-from servo-name-to-cable-id-list (list 8 9 10 11)))) + (defun id-in-list (id target-list) (dolist (x target-list) (if (eq id x) @@ -24,6 +35,17 @@ (if (string= marker-name "lpov-rarm_elbow_p_servo") (setq *target-id* 2)))) +(defun make-timeout-servo-list (joint_states) + (setq timeout-servo-list '()) + (setq count (- (length (send joint_states :name)) 1)) + (while (> count -1) + (if (< (elt (send joint_states :effort) count) 0.1) + (progn + (setq timeout-servo-list (cons (elt (send joint_states :name) count) *timeout-servo-list*)) + (setq *reparing-servo-num* count))) + (setq count (- count 1))) + timeout-servo-list) + (defun move-rarm-x (delta) (send *robot* :angle-vector (send *ri* :state :potentio-vector)) (send *robot* :rarm :move-end-pos (float-vector delta 0 0)) From dec1503419b60c6e6bfaff71dbebff8b15d62b35 Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 5 Jan 2022 15:38:28 +0900 Subject: [PATCH 102/127] [euslisp/main] made decide-target-servo func --- jsk_2021_fix_kxr/euslisp/main/panda_main.l | 32 ++++++++++--------- .../euslisp/panda_commons/utils.l | 10 ++++-- 2 files changed, 25 insertions(+), 17 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda_main.l b/jsk_2021_fix_kxr/euslisp/main/panda_main.l index 5787367d0b..6019bfa3f5 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda_main.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda_main.l @@ -43,21 +43,23 @@ (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) -(setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) -(send req :action "send-servo-info") -(setq res (ros::service-call "repair_reaction" req t)) -(setq (send res :joint_state)) -(setq timeout-servo-list (make-timeout-servo-list joint_states)) - -(while timeout-servo-list - (setq target ) - (if (string= target "rarm-elbow-p-servo") +(while t + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "send-servo-info") + (setq res (ros::service-call "repair_reaction" req t)) + (setq joint_states (send res :joint_state)) + (setq timeout-servo-list (make-timeout-servo-list joint_states)) + (if timeout-servo-list (progn - (ros::ros-info "target is rarm-elbow-p-servo") - (panda-repair-rarm-elbow-p-servo))) - (if (string= target "larm-elbow-p-servo") - (progn - (ros::ros-info "target is larm-elbow-p-servo") - (panda-repair-larn-elbow-p-servo)))) + (setq target (decide-target-servo timeout-servo-list)) + (if (string-equal target "rarm-elbow-p") + (progn + (ros::ros-info "target is rarm-elbow-p-servo") + (panda-repair-rarm-elbow-p-servo))) + (if (string-equal target "larm-elbow-p") + (progn + (ros::ros-info "target is larm-elbow-p-servo") + (panda-repair-larn-elbow-p-servo)))) + (return-from nil))) (ros::ros-info "Finished repairing") diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index d2882a37c7..cf019b18c1 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -41,11 +41,17 @@ (while (> count -1) (if (< (elt (send joint_states :effort) count) 0.1) (progn - (setq timeout-servo-list (cons (elt (send joint_states :name) count) *timeout-servo-list*)) - (setq *reparing-servo-num* count))) + (if (not (string= (elt (send joint_states :name) count) "HEAD-NECK-Y")) + (progn + (setq timeout-servo-list (cons (elt (send joint_states :name) count) timeout-servo-list)) + (setq *reparing-servo-num* count))))) (setq count (- count 1))) timeout-servo-list) +(defun decide-target-servo (timeout-servo-list) + (setq target-servo (elt timeout-servo-list 0)) + target-servo) + (defun move-rarm-x (delta) (send *robot* :angle-vector (send *ri* :state :potentio-vector)) (send *robot* :rarm :move-end-pos (float-vector delta 0 0)) From 6e61d1d4abe61be7b21bdd7a5559d677e7ccb64e Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 5 Jan 2022 16:12:45 +0900 Subject: [PATCH 103/127] [euslisp/main] made base --- .../euslisp/main/panda-repair-larm-elbow-p-servo.l | 8 ++++++++ .../euslisp/main/panda-repair-rarm-elbow-p-servo.l | 8 ++++++++ jsk_2021_fix_kxr/euslisp/main/panda_main.l | 4 +++- 3 files changed, 19 insertions(+), 1 deletion(-) create mode 100644 jsk_2021_fix_kxr/euslisp/main/panda-repair-larm-elbow-p-servo.l create mode 100644 jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-repair-larm-elbow-p-servo.l b/jsk_2021_fix_kxr/euslisp/main/panda-repair-larm-elbow-p-servo.l new file mode 100644 index 0000000000..b4a268a79f --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/main/panda-repair-larm-elbow-p-servo.l @@ -0,0 +1,8 @@ +#!/usr/bin/env roseus + +(defun panda-repair-larm-elbow-p-servo () + (ros::ros-info "Start repairing larm-elbow-p-servo") + + (unix:sleep 1) + + (ros::ros-info "End repairing larm-elbow-p-servo")) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l b/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l new file mode 100644 index 0000000000..61f360e7a8 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l @@ -0,0 +1,8 @@ +#!/usr/bin/env roseus + +(defun panda-repair-rarm-elbow-p-servo () + (ros::ros-info "Start repairing rarm-elbow-p-servo") + + (unix:sleep 1) + + (ros::ros-info "End repairing rarm-elbow-p-servo")) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda_main.l b/jsk_2021_fix_kxr/euslisp/main/panda_main.l index 6019bfa3f5..e685f63a2f 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda_main.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda_main.l @@ -3,6 +3,8 @@ (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") +(require "package://jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l") +(require "package://jsk_2021_fix_kxr/euslisp/main/panda-repair-larm-elbow-p-servo.l") (ros::roseus-add-msgs "franka_msgs") (ros::roseus-add-msgs "franka_gripper") @@ -59,7 +61,7 @@ (if (string-equal target "larm-elbow-p") (progn (ros::ros-info "target is larm-elbow-p-servo") - (panda-repair-larn-elbow-p-servo)))) + (panda-repair-larm-elbow-p-servo)))) (return-from nil))) (ros::ros-info "Finished repairing") From 234efb410b0ebd2af3f7ebea532583e70e68d1a2 Mon Sep 17 00:00:00 2001 From: softyanija Date: Wed, 5 Jan 2022 17:09:17 +0900 Subject: [PATCH 104/127] [euslisp/main] wrote program for repairing rarm-elbow-p-servo --- .../main/panda-repair-rarm-elbow-p-servo.l | 151 ++++++++++++++++++ .../euslisp/panda_commons/utils.l | 20 +-- 2 files changed, 161 insertions(+), 10 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l b/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l index 61f360e7a8..63bcd0695d 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l @@ -2,7 +2,158 @@ (defun panda-repair-rarm-elbow-p-servo () (ros::ros-info "Start repairing rarm-elbow-p-servo") + + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "show-rarm-elbow-p") + (setq res (ros::service-call "repair_reaction" req t)) + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "rarm-free") + (setq res (ros::service-call "repair_reaction" req t)) + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "larm-free") + (setq res (ros::service-call "repair_reaction" req t)) + (while t + (setq found-body (search-body)) + (if (string= found-body "front") + (progn + (ros::ros-info "Try to reverse body"))) + (if (string= found-body "back") + (progn + (ros::ros-info "Recordind position of kxr") + (setq stamp (ros::time-now)) + (when (send *tfl* :wait-for-transform "dual_arm_base" "rpov-body_back" stamp 5) + (setq *base_to_body_back* (send *tfl* :lookup-transform "dual_arm_base" "rpov-body_back" stamp))) + (return)))) + + (setq base-to-larm-grasp-pose (send (send *base_to_body_back* :copy-worldcoords) :transform *body-back-to-larm-grasp-pose*)) + (setq base-to-rarm-grasp-pose (send (send *base_to_body_back* :copy-worldcoords) :transform *body-back-to-rarm-grasp-pose*)) + + (send *robot* :rarm :inverse-kinematics base-to-larm-grasp-pose) + (send *robot* :rarm :move-end-pos #f(-100 0 0)) + (send *robot* :larm :inverse-kinematics base-to-rarm-grasp-pose) + (send *robot* :larm :move-end-pos #f(-100 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + + (send *ri* :stop-grasp :rarm) + (send *ri* :stop-grasp :larm) + (unix:sleep 2) + + (send *robot* :rarm :inverse-kinematics base-to-larm-grasp-pose) + (send *robot* :larm :inverse-kinematics base-to-rarm-grasp-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *ri* :start-grasp :rarm) + (send *ri* :start-grasp :larm) + (unix:sleep 2) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(308.472 -259.081 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(305.157 -14.307 1220.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1220.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1210.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1214.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *ri* :stop-grasp :larm) + (unix:sleep 1) + + (send *robot* :larm :move-end-pos #f(-100 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2500) + (send *ri* :wait-interpolation) + + (larm-search-rarm-elbow-p-servo-tag) + + ;;grasp elbow from below + (larm-grasp-rarm-elbow-p-servo-from-below) + + (send *ri* :stop-grasp :rarm) + (unix:sleep 1) + + (send *robot* :rarm :move-end-pos #f(-100 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + ;;prepare to grasp cable + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(270.152 15.464 1414.154) :rpy (float-vector 2.5 1.551 -0.386))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (setq search-tag-pose (send (send *base_to_rarm_elbow_p_servo* :copy-worldcoords) :transform *servo-to-search-cable-pose*)) + (send *robot* :rarm_joint1 :joint-angle 30) + (send *robot* :rarm :inverse-kinematics search-tag-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (rarm-search-servo-tag "rpov-rarm_elbow_p_servo") + + (setq stamp (ros::time-now)) + (when (send *tfl* :wait-for-transform "dual_arm_base" "rpov-rarm_elbow_p_servo" stamp 5) + (setq *base_to_rarm_elbow_p_servo* (send *tfl* :lookup-transform "dual_arm_base" "rpov-rarm_elbow_p_servo" stamp))) + + ;; + (while t + (rarm-grasp-cable "rpov-rarm_elbow_p_servo" "right") + + (rarm-insert-cable "rpov-rarm_elbow_p_servo" "right") + + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "send-servo-info") + (setq res (ros::service-call "repair_reaction" req t)) + (setq joint_states (send res :joint_state)) + (setq timeout-servo-list (make-timeout-servo-list joint_states)) + (if (not timeout-servo-list) + (progn + (ros::ros-info "Success to insert cable") + (return-from nil)) + (progn + (setq target (decide-target-servo timeout-servo-list)) + (if (string-equal target "rarm-elbow-p") + (progn + (ros::ros-info "Success to insert cable") + (return-from nil)) + (progn + (ros::ros-info "Failed to insert cable") + (ros::ros-info "Retry to insert cable")))))) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1210.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :rarm :move-end-pos #f(-100 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1210.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *ri* :start-grasp :rarm) (unix:sleep 1) + (larm-release-rarm-elbow-p-servo-from-below) + + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1214.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *robot* :larm :move-end-pos #f(-100 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1214.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *ri* :start-grasp :larm) + (unix:sleep 1) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(308.472 -259.081 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(305.157 -14.307 1220.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + ) + (ros::ros-info "End repairing rarm-elbow-p-servo")) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index cf019b18c1..ab084ecca5 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -698,16 +698,16 @@ memo to get parameters (send *robot* :rarm :move-end-pos #f(-10 0 0)) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) - (return-from rarm-insert-cable t))) - ;; (setq f hoge) - ;; (if f - ;; (progn - ;; (ros::ros-info "success") - ;; (return from rarm-insert-cable-test t)) - ;; (progn - ;; (return to start point))) - ) - + (send *ri* :stop-grasp :rarm :width 0.005) + (unix:sleep 1) + (send *robot* :rarm :move-end-pos #f(-30 0 30)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm :move-end-pos #f(-70 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (return-from rarm-insert-cable t)))) )) From b8da80ae1644eb99cdee1b4a84d6f401b1003477 Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 7 Jan 2022 11:36:23 +0900 Subject: [PATCH 105/127] [launch] add cable_terminal_detection.launch --- .../launch/cable_terminal_detection.launch | 39 +++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 jsk_2021_fix_kxr/launch/cable_terminal_detection.launch diff --git a/jsk_2021_fix_kxr/launch/cable_terminal_detection.launch b/jsk_2021_fix_kxr/launch/cable_terminal_detection.launch new file mode 100644 index 0000000000..c835ccc429 --- /dev/null +++ b/jsk_2021_fix_kxr/launch/cable_terminal_detection.launch @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From ee9b68a8eeb3c25bd57eff72873ab17e355b728a Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 7 Jan 2022 13:12:57 +0900 Subject: [PATCH 106/127] [euslisp/main] add stand-pose func --- jsk_2021_fix_kxr/euslisp/main/kxr_main.l | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/jsk_2021_fix_kxr/euslisp/main/kxr_main.l b/jsk_2021_fix_kxr/euslisp/main/kxr_main.l index 4bdfa76dd0..77b0c28417 100644 --- a/jsk_2021_fix_kxr/euslisp/main/kxr_main.l +++ b/jsk_2021_fix_kxr/euslisp/main/kxr_main.l @@ -55,6 +55,11 @@ (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) (send *ri* :angle-vector (send *robot* :angle-vector) 200)) +(defun stand-pose () + (ros::ros-info "stand-pose") + (send *robot* :angle-vector #f(0.0 -20.0 40.0 -20.0 0.0 0.0 -20.0 40.0 -20.0 0.0 0.0 -30.0 0.0 -27.0 -27.0 0.0 30.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 200)) + (defun send-servo-info () (ros::ros-info "send-servo-info") (setq msg (instance sensor_msgs::jointstate :init)) From 2f07e36679f12fb2150a5c0f5a890985f42adcfb Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 7 Jan 2022 20:36:08 +0900 Subject: [PATCH 107/127] [euslisp/main] wrote part of picking up KXR and release --- jsk_2021_fix_kxr/euslisp/main/kxr_main.l | 2 +- .../euslisp/main/panda-release-kxr.l | 49 ++++++++++ .../main/panda-repair-rarm-elbow-p-servo.l | 84 ----------------- .../euslisp/main/panda-return-rarm.l | 38 ++++++++ .../euslisp/main/panda-set-rarm.l | 89 +++++++++++++++++++ jsk_2021_fix_kxr/euslisp/main/panda_main.l | 35 ++++++-- 6 files changed, 206 insertions(+), 91 deletions(-) create mode 100644 jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l create mode 100644 jsk_2021_fix_kxr/euslisp/main/panda-return-rarm.l create mode 100644 jsk_2021_fix_kxr/euslisp/main/panda-set-rarm.l diff --git a/jsk_2021_fix_kxr/euslisp/main/kxr_main.l b/jsk_2021_fix_kxr/euslisp/main/kxr_main.l index 77b0c28417..ff4826b5b1 100644 --- a/jsk_2021_fix_kxr/euslisp/main/kxr_main.l +++ b/jsk_2021_fix_kxr/euslisp/main/kxr_main.l @@ -58,7 +58,7 @@ (defun stand-pose () (ros::ros-info "stand-pose") (send *robot* :angle-vector #f(0.0 -20.0 40.0 -20.0 0.0 0.0 -20.0 40.0 -20.0 0.0 0.0 -30.0 0.0 -27.0 -27.0 0.0 30.0 0.0 -27.0 -27.0 0.0 10.0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 200)) + (send *ri* :angle-vector (send *robot* :angle-vector) 300)) (defun send-servo-info () (ros::ros-info "send-servo-info") diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l b/jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l new file mode 100644 index 0000000000..59fa052924 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l @@ -0,0 +1,49 @@ +#!/usr/bin/env roseus + +(defun panda-release-kxr () + (ros::ros-info "Releasing KXR") + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(308.472 -259.081 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(305.157 -14.307 1220.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm :move-end-pos #f(-180 0 0)) + (send *robot* :larm :move-end-pos #f(-180 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm :move-end-pos #f(-50 -50 0)) + (send *robot* :larm :move-end-pos #f(-50 50 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm :move-end-pos #f(40 0 0)) + (send *robot* :larm :move-end-pos #f(40 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *ri* :stop-grasp :rarm) + (send *ri* :stop-grasp :larm) + (unix:sleep 2) + + (send *robot* :rarm :move-end-pos #f(-50 0 0)) + (send *robot* :larm :move-end-pos #f(-50 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm :move-end-pos #f(0 100 0)) + (send *robot* :larm :move-end-pos #f(0 -100 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "rarm-hold") + (setq res (ros::service-call "repair_reaction" req t)) + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "larm-hold") + (setq res (ros::service-call "repair_reaction" req t)) + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "stand-pose") + (setq res (ros::service-call "repair_reaction" req t)) + ) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l b/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l index 63bcd0695d..8f2cce2e31 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l @@ -2,90 +2,6 @@ (defun panda-repair-rarm-elbow-p-servo () (ros::ros-info "Start repairing rarm-elbow-p-servo") - - (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) - (send req :action "show-rarm-elbow-p") - (setq res (ros::service-call "repair_reaction" req t)) - (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) - (send req :action "rarm-free") - (setq res (ros::service-call "repair_reaction" req t)) - (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) - (send req :action "larm-free") - (setq res (ros::service-call "repair_reaction" req t)) - - (while t - (setq found-body (search-body)) - (if (string= found-body "front") - (progn - (ros::ros-info "Try to reverse body"))) - (if (string= found-body "back") - (progn - (ros::ros-info "Recordind position of kxr") - (setq stamp (ros::time-now)) - (when (send *tfl* :wait-for-transform "dual_arm_base" "rpov-body_back" stamp 5) - (setq *base_to_body_back* (send *tfl* :lookup-transform "dual_arm_base" "rpov-body_back" stamp))) - (return)))) - - (setq base-to-larm-grasp-pose (send (send *base_to_body_back* :copy-worldcoords) :transform *body-back-to-larm-grasp-pose*)) - (setq base-to-rarm-grasp-pose (send (send *base_to_body_back* :copy-worldcoords) :transform *body-back-to-rarm-grasp-pose*)) - - (send *robot* :rarm :inverse-kinematics base-to-larm-grasp-pose) - (send *robot* :rarm :move-end-pos #f(-100 0 0)) - (send *robot* :larm :inverse-kinematics base-to-rarm-grasp-pose) - (send *robot* :larm :move-end-pos #f(-100 0 0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) - - (send *ri* :stop-grasp :rarm) - (send *ri* :stop-grasp :larm) - (unix:sleep 2) - - (send *robot* :rarm :inverse-kinematics base-to-larm-grasp-pose) - (send *robot* :larm :inverse-kinematics base-to-rarm-grasp-pose) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) - (send *ri* :wait-interpolation) - - (send *ri* :start-grasp :rarm) - (send *ri* :start-grasp :larm) - (unix:sleep 2) - - (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(308.472 -259.081 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) - (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(305.157 -14.307 1220.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) - (send *ri* :angle-vector (send *robot* :angle-vector) 3000) - (send *ri* :wait-interpolation) - - (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) - (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1220.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) - (send *ri* :angle-vector (send *robot* :angle-vector) 3000) - (send *ri* :wait-interpolation) - - (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1210.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) - (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1214.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) - (send *ri* :angle-vector (send *robot* :angle-vector) 3000) - (send *ri* :wait-interpolation) - - (send *ri* :stop-grasp :larm) - (unix:sleep 1) - - (send *robot* :larm :move-end-pos #f(-100 0 0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 2500) - (send *ri* :wait-interpolation) - - (larm-search-rarm-elbow-p-servo-tag) - - ;;grasp elbow from below - (larm-grasp-rarm-elbow-p-servo-from-below) - - (send *ri* :stop-grasp :rarm) - (unix:sleep 1) - - (send *robot* :rarm :move-end-pos #f(-100 0 0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) - (send *ri* :wait-interpolation) - - ;;prepare to grasp cable - (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(270.152 15.464 1414.154) :rpy (float-vector 2.5 1.551 -0.386))) :translation-axis t :rotation-axis t) - (send *ri* :angle-vector (send *robot* :angle-vector) 3000) - (send *ri* :wait-interpolation) (setq search-tag-pose (send (send *base_to_rarm_elbow_p_servo* :copy-worldcoords) :transform *servo-to-search-cable-pose*)) (send *robot* :rarm_joint1 :joint-angle 30) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-return-rarm.l b/jsk_2021_fix_kxr/euslisp/main/panda-return-rarm.l new file mode 100644 index 0000000000..349cd78919 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/main/panda-return-rarm.l @@ -0,0 +1,38 @@ +#!/usr/bin/env roseus + +(defun panda-return-rarm () + (ros::ros-info "Returning to neutral position") + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1210.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :rarm :move-end-pos #f(-100 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1210.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *ri* :start-grasp :rarm) + (unix:sleep 1) + + (larm-release-rarm-elbow-p-servo-from-below) + + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1214.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *robot* :larm :move-end-pos #f(-100 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1214.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *ri* :start-grasp :larm) + (unix:sleep 1) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(308.472 -259.081 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(305.157 -14.307 1220.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (ros::ros-info "End returning") + ) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-set-rarm.l b/jsk_2021_fix_kxr/euslisp/main/panda-set-rarm.l new file mode 100644 index 0000000000..0d4208807b --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/main/panda-set-rarm.l @@ -0,0 +1,89 @@ +#!/usr/bin/env roseus + +(defun panda-set-rarm () + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "show-rarm-elbow-p") + (setq res (ros::service-call "repair_reaction" req t)) + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "rarm-free") + (setq res (ros::service-call "repair_reaction" req t)) + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "larm-free") + (setq res (ros::service-call "repair_reaction" req t)) + + (while t + (setq found-body (search-body)) + (if (string= found-body "front") + (progn + (ros::ros-info "Try to reverse body"))) + (if (string= found-body "back") + (progn + (ros::ros-info "Recordind position of kxr") + (setq stamp (ros::time-now)) + (when (send *tfl* :wait-for-transform "dual_arm_base" "rpov-body_back" stamp 5) + (setq *base_to_body_back* (send *tfl* :lookup-transform "dual_arm_base" "rpov-body_back" stamp))) + (return)))) + + (setq base-to-larm-grasp-pose (send (send *base_to_body_back* :copy-worldcoords) :transform *body-back-to-larm-grasp-pose*)) + (setq base-to-rarm-grasp-pose (send (send *base_to_body_back* :copy-worldcoords) :transform *body-back-to-rarm-grasp-pose*)) + + (send *robot* :rarm :inverse-kinematics base-to-larm-grasp-pose) + (send *robot* :rarm :move-end-pos #f(-100 0 0)) + (send *robot* :larm :inverse-kinematics base-to-rarm-grasp-pose) + (send *robot* :larm :move-end-pos #f(-100 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + + (send *ri* :stop-grasp :rarm) + (send *ri* :stop-grasp :larm) + (unix:sleep 2) + + (send *robot* :rarm :inverse-kinematics base-to-larm-grasp-pose) + (send *robot* :larm :inverse-kinematics base-to-rarm-grasp-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *ri* :start-grasp :rarm) + (send *ri* :start-grasp :larm) + (unix:sleep 2) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(308.472 -259.081 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(305.157 -14.307 1220.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1220.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1210.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1214.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *ri* :stop-grasp :larm) + (unix:sleep 1) + + (send *robot* :larm :move-end-pos #f(-100 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2500) + (send *ri* :wait-interpolation) + + (larm-search-rarm-elbow-p-servo-tag) + + ;;grasp elbow from below + (larm-grasp-rarm-elbow-p-servo-from-below) + + (send *ri* :stop-grasp :rarm) + (unix:sleep 1) + + (send *robot* :rarm :move-end-pos #f(-100 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + ;;prepare to grasp cable + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(270.152 15.464 1414.154) :rpy (float-vector 2.5 1.551 -0.386))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + + ) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda_main.l b/jsk_2021_fix_kxr/euslisp/main/panda_main.l index e685f63a2f..89f60d0b04 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda_main.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda_main.l @@ -5,6 +5,9 @@ (require "package://jsk_2021_fix_kxr/euslisp/panda_commons/utils.l") (require "package://jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l") (require "package://jsk_2021_fix_kxr/euslisp/main/panda-repair-larm-elbow-p-servo.l") +(require "package://jsk_2021_fix_kxr/euslisp/main/panda-set-rarm.l") +(require "package://jsk_2021_fix_kxr/euslisp/main/panda-return-rarm.l") +(require "package://jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l") (ros::roseus-add-msgs "franka_msgs") (ros::roseus-add-msgs "franka_gripper") @@ -54,14 +57,34 @@ (if timeout-servo-list (progn (setq target (decide-target-servo timeout-servo-list)) - (if (string-equal target "rarm-elbow-p") + (if (string-equal (subseq target 0 4) "rarm") (progn - (ros::ros-info "target is rarm-elbow-p-servo") - (panda-repair-rarm-elbow-p-servo))) - (if (string-equal target "larm-elbow-p") + (ros::ros-info "Set rarm") + (panda-set-rarm) + (if (string-equal target "rarm-elbow-p") + (progn + (ros::ros-info "target is rarm-elbow-p-servo") + (panda-repair-rarm-elbow-p-servo))) + (if (string-equal target "rarm-gripper-r") + (progn + (ros::ros-info "target is rarm-gripper-r-servo") + (panda-repair-rarm-gripper-r-servo)))) + (panda-return-rarm) + ) + (if (string-equal (subseq target 0 4) "larm") (progn - (ros::ros-info "target is larm-elbow-p-servo") - (panda-repair-larm-elbow-p-servo)))) + (ros::ros-info "Set larm") + (panda-set-rarm) + (if (string-equal target "larm-elbow-p") + (progn + (ros::ros-info "target is larm-elbow-p-servo") + (panda-repair-rarm-elbow-p-servo))) + (if (string-equal target "larm-gripper-r") + (progn + (ros::ros-info "target is larm-gripper-r-servo") + (panda-repair-larm-gripper-r-servo)))))) (return-from nil))) +(panda-release-kxr) + (ros::ros-info "Finished repairing") From 208c2e71acb7f864ec775b30256c65ff730a0591 Mon Sep 17 00:00:00 2001 From: softyanija Date: Sat, 8 Jan 2022 17:05:51 +0900 Subject: [PATCH 108/127] [euslisp/main] prepared for repair rleg-ankle-r-servo demo --- .../euslisp/main/panda-repair-rleg-ankle-r.l | 81 +++++++++++++++++++ .../euslisp/main/panda-set-rleg.l | 75 +++++++++++++++++ jsk_2021_fix_kxr/euslisp/main/panda_main.l | 16 +++- .../euslisp/panda_commons/parameters.l | 8 ++ 4 files changed, 176 insertions(+), 4 deletions(-) create mode 100644 jsk_2021_fix_kxr/euslisp/main/panda-repair-rleg-ankle-r.l create mode 100644 jsk_2021_fix_kxr/euslisp/main/panda-set-rleg.l diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-repair-rleg-ankle-r.l b/jsk_2021_fix_kxr/euslisp/main/panda-repair-rleg-ankle-r.l new file mode 100644 index 0000000000..eaa8bd1368 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/main/panda-repair-rleg-ankle-r.l @@ -0,0 +1,81 @@ +#!/usr/bin/env roseus + +(defun panda-repair-rleg-ankle-r () + (ros::ros-info "Start repairing rleg-ankle-r-servo") + + (setq res (ros::service-call "repair_reaction" req t)) + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "rarm-hold") + (setq res (ros::service-call "repair_reaction" req t)) + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "larm-hold") + (setq res (ros::service-call "repair_reaction" req t)) + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "show-rleg-ankle-r-1") + (setq res (ros::service-call "repair_reaction" req t)) + + ;;move-rarm + + (setq stamp (ros::time-now)) + (when (send *tfl* :wait-for-transform "dual_arm_base" "rpov-rleg-ankle-r-servo-b" stamp 5) + (setq *base_to_rleg_ankle_r_servo_b* (send *tfl* :lookup-transform "dual_arm_base" "rpov-rleg-ankle-r-servo-b" stamp))) + + ;;grasp ankle-r from back + (rarm-grasp-rleg-ankle-r-from-back) + + (send *ri* :stop-grasp :larm) + (unix:sleep 1) + + (send *robot* :larm :move-end-pos #f(-100 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *robot* :larm :move-end-pos #f(-70 100 -50)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "show-rleg-ankle-r-2") + (setq res (ros::service-call "repair_reaction" req t)) + + ;;convert back to front + (setq *base_to_rleg_ankle_r_servo* (send (send *base_to_rleg_ankle_r_servo_b* :copy-worldcoords) :transform *servo-back-to-front*)) + + (larm-search-servo-tag "lpov-rleg_ankle_r_servo") + + (setq stamp (ros::time-now)) + (when (send *tfl* :wait-for-transform "dual_arm_base" "lpov-rleg_ankle_r_servo" stamp 5) + (setq *base_to_rarm_elbow_p_servo* (send *tfl* :lookup-transform "dual_arm_base" "lpov-rleg_ankle_r_servo" stamp))) + + ;; + (while t + (larm-grasp-cable "lpov-rleg_ankle_r_servo" "left") + + (larm-insert-cable "lpov-rleg_ankle_r_servo" "left") + + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "send-servo-info") + (setq res (ros::service-call "repair_reaction" req t)) + (setq joint_states (send res :joint_state)) + (setq timeout-servo-list (make-timeout-servo-list joint_states)) + (if (not timeout-servo-list) + (progn + (ros::ros-info "Success to insert cable") + (return-from nil)) + (progn + (setq target (decide-target-servo timeout-servo-list)) + (if (string-equal target "rleg-ankle-r") + (progn + (ros::ros-info "Failed to insert cable") + (ros::ros-info "Retry to insert cable")) + (progn + (ros::ros-info "Success to insert cable") + (return-from nil)) + )))) + + (send *ri* :stop-grasp :rarm) + (unix:sleep 1) + + (send *robot* :rarm :move-end-pos #f(-100 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation)) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-set-rleg.l b/jsk_2021_fix_kxr/euslisp/main/panda-set-rleg.l new file mode 100644 index 0000000000..f402bb815e --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/main/panda-set-rleg.l @@ -0,0 +1,75 @@ +#!/usr/bin/env roseus + +(defun panda-set-rleg () + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "show-rarm-elbow-p") + (setq res (ros::service-call "repair_reaction" req t)) + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "rarm-free") + (setq res (ros::service-call "repair_reaction" req t)) + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "larm-free") + (setq res (ros::service-call "repair_reaction" req t)) + + (while t + (setq found-body (search-body)) + (if (string= found-body "front") + (progn + (ros::ros-info "Try to reverse body"))) + (if (string= found-body "back") + (progn + (ros::ros-info "Recordind position of kxr") + (setq stamp (ros::time-now)) + (when (send *tfl* :wait-for-transform "dual_arm_base" "rpov-body_back" stamp 5) + (setq *base_to_body_back* (send *tfl* :lookup-transform "dual_arm_base" "rpov-body_back" stamp))) + (return)))) + + (setq base-to-larm-grasp-pose (send (send *base_to_body_back* :copy-worldcoords) :transform *body-back-to-larm-grasp-pose*)) + (setq base-to-rarm-grasp-pose (send (send *base_to_body_back* :copy-worldcoords) :transform *body-back-to-rarm-grasp-pose*)) + + (send *robot* :rarm :inverse-kinematics base-to-larm-grasp-pose) + (send *robot* :rarm :move-end-pos #f(-100 0 0)) + (send *robot* :larm :inverse-kinematics base-to-rarm-grasp-pose) + (send *robot* :larm :move-end-pos #f(-100 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + + (send *ri* :stop-grasp :rarm) + (send *ri* :stop-grasp :larm) + (unix:sleep 2) + + (send *robot* :rarm :inverse-kinematics base-to-larm-grasp-pose) + (send *robot* :larm :inverse-kinematics base-to-rarm-grasp-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *ri* :start-grasp :rarm) + (send *ri* :start-grasp :larm) + (unix:sleep 2) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(308.472 -259.081 1205.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(305.157 -14.307 1205.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *ri* :stop-grasp :larm) + (unix:sleep 1) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1220.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1210.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1214.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *ri* :stop-grasp :rarm) + (unix:sleep 1) + + (send *robot* :rarm :move-end-pos #f(-100 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2500) + (send *ri* :wait-interpolation) + + ;;move-rarm + ) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda_main.l b/jsk_2021_fix_kxr/euslisp/main/panda_main.l index 89f60d0b04..524fa30e03 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda_main.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda_main.l @@ -68,9 +68,8 @@ (if (string-equal target "rarm-gripper-r") (progn (ros::ros-info "target is rarm-gripper-r-servo") - (panda-repair-rarm-gripper-r-servo)))) - (panda-return-rarm) - ) + (panda-repair-rarm-gripper-r-servo))) + (panda-return-rarm))) (if (string-equal (subseq target 0 4) "larm") (progn (ros::ros-info "Set larm") @@ -82,7 +81,16 @@ (if (string-equal target "larm-gripper-r") (progn (ros::ros-info "target is larm-gripper-r-servo") - (panda-repair-larm-gripper-r-servo)))))) + (panda-repair-larm-gripper-r-servo))))) + (if (string-equal (subseq target 0 4) "rleg") + (progn + (ros::ros-info "Set rleg") + (panda-set-rleg) + (if (string-equal target "rleg-ankle-r") + (progn + (ros::ros-info "target is rleg-ankle-r") + (panda-repair-rleg-ankle-r))) + (panda-return-rleg))) (return-from nil))) (panda-release-kxr) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l index 028feb722c..35b32c6ae7 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l @@ -3,6 +3,8 @@ (defvar *base_to_body_front*) (defvar *base_to_body_back*) (defvar *base_to_rarm_elbow_p_servo*) +(defvar *base_to_rleg_ankle_r_servo*) +(defvar *base_to_rleg_ankle_r_servo_b*) (defvar *target-id*) (defvar *target-id-list*) (defvar *reparing-servo-num*) @@ -13,6 +15,12 @@ ;;; parameters + +(setq *servo-back-to-front* + (make-cascoords + :pos #f(-4 0 -25) + :rpy #f(0 3.14 0) + )) (setq *body-back-to-larm-grasp-pose* (make-cascoords :pos #f(-124.991 17 -55) From 162442e4fccf52f889c8d8cb2870a1ef79d5f119 Mon Sep 17 00:00:00 2001 From: softyanija Date: Sat, 8 Jan 2022 17:38:00 +0900 Subject: [PATCH 109/127] [euslisp/main] make poses for repairing rleg-ankle-r-servo --- jsk_2021_fix_kxr/euslisp/main/kxr_main.l | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/jsk_2021_fix_kxr/euslisp/main/kxr_main.l b/jsk_2021_fix_kxr/euslisp/main/kxr_main.l index ff4826b5b1..0a91e3115e 100644 --- a/jsk_2021_fix_kxr/euslisp/main/kxr_main.l +++ b/jsk_2021_fix_kxr/euslisp/main/kxr_main.l @@ -55,6 +55,16 @@ (send *robot* :angle-vector #f(0.0 -10.0 40.0 -20.0 0.0 0.0 -10.0 40.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) (send *ri* :angle-vector (send *robot* :angle-vector) 200)) +(defun show-rleg-ankle-r-1 () + (ros::ros-info "show-rleg-ankle-r-1") + (send *robot* :angle-vector #f(0.0 20.0 50.0 -70.0 0.0 90.0 -10.0 70.0 -20.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 200)) + +(defun show-rleg-ankle-r-2 () + (ros::ros-info "show-rleg-ankle-r-2") + (send *robot* :angle-vector #f(0.0 20.0 50.0 -70.0 0.0 90.0 -10.0 70.0 -20.0 0.0 -90.0 -180.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 200)) + (defun stand-pose () (ros::ros-info "stand-pose") (send *robot* :angle-vector #f(0.0 -20.0 40.0 -20.0 0.0 0.0 -20.0 40.0 -20.0 0.0 0.0 -30.0 0.0 -27.0 -27.0 0.0 30.0 0.0 -27.0 -27.0 0.0 10.0)) @@ -123,12 +133,19 @@ (progn (show-rarm-sholder-r) (setq f 1))) + (if (string= action "show-rleg-ankle-r-1") + (progn + (show-rleg-ankle-r-1) + (setq f 1))) + (if (string= action "show-rleg-ankle-r-2") + (progn + (show-rleg-ankle-r-2) + (setq f 1))) (if (string= action "send-servo-info") (progn (send m :joint_state (send-servo-info)) (setq f 1))) - (if (= f 1) (progn (send m :result "success")) From 2d0af349d30d297886ecafcbc230ee72ac25ddcd Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 10 Jan 2022 13:39:13 +0900 Subject: [PATCH 110/127] fix_kxr_detections.launch --- .../launch/fix_kxr_detections.launch | 42 +++++++++++++++++++ 1 file changed, 42 insertions(+) create mode 100644 jsk_2021_fix_kxr/launch/fix_kxr_detections.launch diff --git a/jsk_2021_fix_kxr/launch/fix_kxr_detections.launch b/jsk_2021_fix_kxr/launch/fix_kxr_detections.launch new file mode 100644 index 0000000000..516b7313e0 --- /dev/null +++ b/jsk_2021_fix_kxr/launch/fix_kxr_detections.launch @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 7c526b0af6aee31944db90a968ef367e623a038d Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 10 Jan 2022 19:50:30 +0900 Subject: [PATCH 111/127] [euslisp/main] fix typo --- jsk_2021_fix_kxr/euslisp/main/kxr_main.l | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/main/kxr_main.l b/jsk_2021_fix_kxr/euslisp/main/kxr_main.l index 0a91e3115e..eb16963e34 100644 --- a/jsk_2021_fix_kxr/euslisp/main/kxr_main.l +++ b/jsk_2021_fix_kxr/euslisp/main/kxr_main.l @@ -67,7 +67,7 @@ (defun stand-pose () (ros::ros-info "stand-pose") - (send *robot* :angle-vector #f(0.0 -20.0 40.0 -20.0 0.0 0.0 -20.0 40.0 -20.0 0.0 0.0 -30.0 0.0 -27.0 -27.0 0.0 30.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *robot* :angle-vector #f(0.0 -20.0 40.0 -20.0 0.0 0.0 -20.0 40.0 -20.0 0.0 0.0 -40.0 0.0 -27.0 -27.0 0.0 40.0 0.0 -27.0 -27.0 0.0 10.0)) (send *ri* :angle-vector (send *robot* :angle-vector) 300)) (defun send-servo-info () @@ -91,8 +91,8 @@ (defun repair_reaction (req) (let ((m (send req :response)) (f 0)) - (ros::ros-info (format nil "request is ~A" (send req :action))) (setq action (send req :action)) + (ros::ros-info (format nil "request is ~A" action)) (if (string= action "rarm-free") (progn (rarm-free) @@ -129,9 +129,9 @@ (progn (show-rarm-elbow-p) (setq f 1))) - (if (string= action "show-rarm-sholder-r") + (if (string= action "show-rarm-shoulder-r") (progn - (show-rarm-sholder-r) + (show-rarm-shoulder-r) (setq f 1))) (if (string= action "show-rleg-ankle-r-1") (progn @@ -145,12 +145,17 @@ (progn (send m :joint_state (send-servo-info)) (setq f 1))) + (if (string= action "stand-pose") + (progn + (stand-pose) + (setq f 1))) (if (= f 1) (progn (send m :result "success")) (progn (send m :result "false"))) + (setq f 0) m)) (ros::roseus "kxr_server") From 718a830f478f9f85310d3f2d8c542de52434ce20 Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 10 Jan 2022 19:51:43 +0900 Subject: [PATCH 112/127] [euslisp/main] fixed details for demo --- .../euslisp/main/panda-release-kxr.l | 2 ++ .../main/panda-repair-rarm-elbow-p-servo.l | 24 ++++++++++++------- .../euslisp/main/panda-set-rarm.l | 4 +++- jsk_2021_fix_kxr/euslisp/main/panda_main.l | 11 +++++---- .../euslisp/panda_commons/utils.l | 14 +++++------ 5 files changed, 33 insertions(+), 22 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l b/jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l index 59fa052924..40406fa776 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l @@ -40,9 +40,11 @@ (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) (send req :action "rarm-hold") (setq res (ros::service-call "repair_reaction" req t)) + (unix:sleep 2) (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) (send req :action "larm-hold") (setq res (ros::service-call "repair_reaction" req t)) + (unix:sleep 2) (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) (send req :action "stand-pose") (setq res (ros::service-call "repair_reaction" req t)) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l b/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l index 8f2cce2e31..320eef937a 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l @@ -34,18 +34,25 @@ (setq target (decide-target-servo timeout-servo-list)) (if (string-equal target "rarm-elbow-p") (progn - (ros::ros-info "Success to insert cable") - (return-from nil)) + (ros::ros-info "Failed to insert cable") + (ros::ros-info "Retry to insert cable")) (progn - (ros::ros-info "Failed to insert cable") - (ros::ros-info "Retry to insert cable")))))) - - (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1210.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (ros::ros-info "Success to insert cable") + (return-from nil)) + ) + ) + ) + ) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1205.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) (send *robot* :rarm :move-end-pos #f(-100 0 0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) - (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1210.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *ri* :stop-grasp :rarm) + (unix:sleep 1) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1205.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) @@ -70,6 +77,5 @@ (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(305.157 -14.307 1220.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) - ) (ros::ros-info "End repairing rarm-elbow-p-servo")) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-set-rarm.l b/jsk_2021_fix_kxr/euslisp/main/panda-set-rarm.l index 0d4208807b..9ebe70bf59 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda-set-rarm.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda-set-rarm.l @@ -85,5 +85,7 @@ (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) - + (send *robot* :rarm :move-end-pos #f(0 -100 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) ) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda_main.l b/jsk_2021_fix_kxr/euslisp/main/panda_main.l index 524fa30e03..f622c63ee9 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda_main.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda_main.l @@ -3,11 +3,6 @@ (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") -(require "package://jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l") -(require "package://jsk_2021_fix_kxr/euslisp/main/panda-repair-larm-elbow-p-servo.l") -(require "package://jsk_2021_fix_kxr/euslisp/main/panda-set-rarm.l") -(require "package://jsk_2021_fix_kxr/euslisp/main/panda-return-rarm.l") -(require "package://jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l") (ros::roseus-add-msgs "franka_msgs") (ros::roseus-add-msgs "franka_gripper") @@ -20,6 +15,12 @@ (ros::load-ros-manifest "opencv_apps") (ros::load-ros-manifest "jsk_2021_fix_kxr") +(require "package://jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l") +(require "package://jsk_2021_fix_kxr/euslisp/main/panda-repair-larm-elbow-p-servo.l") +(require "package://jsk_2021_fix_kxr/euslisp/main/panda-set-rarm.l") +(require "package://jsk_2021_fix_kxr/euslisp/main/panda-return-rarm.l") +(require "package://jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l") + (ros::roseus "panda_robot") (dual_panda-init) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index ab084ecca5..0128c41846 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -405,12 +405,12 @@ memo to get parameters (send *ri* :wait-interpolation) ;;grasp-pose - (send *robot* :rarm :inverse-kinematics base-to-grasp-pose) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) - (send *ri* :wait-interpolation) - (send *robot* :rarm :move-end-pos #f(2 0 0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) - (send *ri* :wait-interpolation) + ;; (send *robot* :rarm :inverse-kinematics base-to-grasp-pose) + ;; (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + ;; (send *ri* :wait-interpolation) + ;; (send *robot* :rarm :move-end-pos #f(2 0 0)) + ;; (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + ;; (send *ri* :wait-interpolation) (setq goal_x 670) (setq goal_y 650) @@ -567,7 +567,7 @@ memo to get parameters (setq i 0) (setq z-lim 4) (setq base-stroke #f(0.5 0 0)) - (while (< i 30) + (while (< i 15) (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 diff-z (elt wrench-diff 2)) From d996424461af1a8e327edd1040c2cb1973efc891 Mon Sep 17 00:00:00 2001 From: softyanija Date: Sun, 16 Jan 2022 17:47:39 +0900 Subject: [PATCH 113/127] add show_grasp_param to visualize --- .../show_grasp_param/show_grasp_param_left.l | 34 +++++++++++++++++++ .../show_grasp_param/show_grasp_param_right.l | 34 +++++++++++++++++++ .../launch/show_grasp_param_left.launch | 8 +++++ .../launch/show_grasp_param_right.launch | 8 +++++ 4 files changed, 84 insertions(+) create mode 100644 jsk_2021_fix_kxr/euslisp/show_grasp_param/show_grasp_param_left.l create mode 100755 jsk_2021_fix_kxr/euslisp/show_grasp_param/show_grasp_param_right.l create mode 100644 jsk_2021_fix_kxr/launch/show_grasp_param_left.launch create mode 100644 jsk_2021_fix_kxr/launch/show_grasp_param_right.launch diff --git a/jsk_2021_fix_kxr/euslisp/show_grasp_param/show_grasp_param_left.l b/jsk_2021_fix_kxr/euslisp/show_grasp_param/show_grasp_param_left.l new file mode 100644 index 0000000000..5026d122d9 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/show_grasp_param/show_grasp_param_left.l @@ -0,0 +1,34 @@ +#!/usr/bin/env roseus + +(ros::roseus-add-msgs "geometry_msgs") +(ros::roseus-add-msgs "image_view2") + +(ros::roseus "show-grasp-param-left") + +;; (ros::subscribe "/rarm_vision/camera/color/image_raw" image_view2::ImageMarker2 #'cb) + + +;; (defun cb (msg) +;; (let (mrk (instance image_view2::ImageMarker2 :init)) +;; (send mrk :type image_view2::ImageMarker2::*TEXT*) +;; (send mrk :position (instance geometry_msgs::Point :init :x 320 :y 40)) +;; (send mrk :scale 2.5) ;; this is proocol +;; (send mrk :fill_color (instance std_msgs::ColorRGBA :init :r 0.0 :g 1.0 :b 0.0 :a 0.5)) +;; (send mrk :text "Which shelf?") +;; (ros::publish "/image_marker" mrk) +;; )) + +(ros::advertise "image_marker" image_view2::ImageMarker2 1) + +(ros::rate 10) +(while (ros::ok) + (let ((mrk (instance image_view2::ImageMarker2 :init))) + (send mrk :type image_view2::ImageMarker2::*POLYGON*) + (send mrk :points (list (instance geometry_msgs::Point :init :x 630 :y 630) + (instance geometry_msgs::Point :init :x 710 :y 630) + (instance geometry_msgs::Point :init :x 710 :y 690) + (instance geometry_msgs::Point :init :x 630 :y 690))) + (ros::publish "image_marker" mrk) + (ros::spin-once) + (ros::sleep) + )) diff --git a/jsk_2021_fix_kxr/euslisp/show_grasp_param/show_grasp_param_right.l b/jsk_2021_fix_kxr/euslisp/show_grasp_param/show_grasp_param_right.l new file mode 100755 index 0000000000..f7db7fd12f --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/show_grasp_param/show_grasp_param_right.l @@ -0,0 +1,34 @@ +#!/usr/bin/env roseus + +(ros::roseus-add-msgs "geometry_msgs") +(ros::roseus-add-msgs "image_view2") + +(ros::roseus "show-grasp-param-right") + +;; (ros::subscribe "/rarm_vision/camera/color/image_raw" image_view2::ImageMarker2 #'cb) + + +;; (defun cb (msg) +;; (let (mrk (instance image_view2::ImageMarker2 :init)) +;; (send mrk :type image_view2::ImageMarker2::*TEXT*) +;; (send mrk :position (instance geometry_msgs::Point :init :x 320 :y 40)) +;; (send mrk :scale 2.5) ;; this is proocol +;; (send mrk :fill_color (instance std_msgs::ColorRGBA :init :r 0.0 :g 1.0 :b 0.0 :a 0.5)) +;; (send mrk :text "Which shelf?") +;; (ros::publish "/image_marker" mrk) +;; )) + +(ros::advertise "image_marker" image_view2::ImageMarker2 1) + +(ros::rate 10) +(while (ros::ok) + (let ((mrk (instance image_view2::ImageMarker2 :init))) + (send mrk :type image_view2::ImageMarker2::*POLYGON*) + (send mrk :points (list (instance geometry_msgs::Point :init :x 630 :y 630) + (instance geometry_msgs::Point :init :x 710 :y 630) + (instance geometry_msgs::Point :init :x 710 :y 690) + (instance geometry_msgs::Point :init :x 630 :y 690))) + (ros::publish "image_marker" mrk) + (ros::spin-once) + (ros::sleep) + )) diff --git a/jsk_2021_fix_kxr/launch/show_grasp_param_left.launch b/jsk_2021_fix_kxr/launch/show_grasp_param_left.launch new file mode 100644 index 0000000000..bb0a7f43f5 --- /dev/null +++ b/jsk_2021_fix_kxr/launch/show_grasp_param_left.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/jsk_2021_fix_kxr/launch/show_grasp_param_right.launch b/jsk_2021_fix_kxr/launch/show_grasp_param_right.launch new file mode 100644 index 0000000000..117df23a12 --- /dev/null +++ b/jsk_2021_fix_kxr/launch/show_grasp_param_right.launch @@ -0,0 +1,8 @@ + + + + + + + + From a62c0e5c9e8853b0d33804145356353e152e69b9 Mon Sep 17 00:00:00 2001 From: softyanija Date: Sun, 16 Jan 2022 19:53:53 +0900 Subject: [PATCH 114/127] [euslisp/main] adjusted grasp-cable and add arange-cable --- .../main/panda-repair-rarm-elbow-p-servo.l | 2 + .../euslisp/main/panda-set-rarm.l | 19 ++- jsk_2021_fix_kxr/euslisp/main/panda_main.l | 2 + .../euslisp/panda_commons/parameters.l | 40 +++++-- .../euslisp/panda_commons/utils.l | 111 ++++++++++-------- 5 files changed, 108 insertions(+), 66 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l b/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l index 320eef937a..815c8f4eb3 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l @@ -17,6 +17,8 @@ ;; (while t + (rarm-arange-cable "rpov-rarm_elbow_p_servo" "right") + (rarm-grasp-cable "rpov-rarm_elbow_p_servo" "right") (rarm-insert-cable "rpov-rarm_elbow_p_servo" "right") diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-set-rarm.l b/jsk_2021_fix_kxr/euslisp/main/panda-set-rarm.l index 9ebe70bf59..f42d158dac 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda-set-rarm.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda-set-rarm.l @@ -4,12 +4,16 @@ (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) (send req :action "show-rarm-elbow-p") (setq res (ros::service-call "repair_reaction" req t)) + (unix:sleep 2) (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) (send req :action "rarm-free") (setq res (ros::service-call "repair_reaction" req t)) + (unix:sleep 1) (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) (send req :action "larm-free") (setq res (ros::service-call "repair_reaction" req t)) + (unix:sleep 1) + (while t (setq found-body (search-body)) @@ -51,14 +55,19 @@ (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) - (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) - (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1220.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -85.155 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 159.619 1220.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) - (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1210.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) - (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1214.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) - (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -85.155 1213.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 159.619 1213.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 1000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -81.155 1213.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 163.619 1213.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 1000) (send *ri* :wait-interpolation) (send *ri* :stop-grasp :larm) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda_main.l b/jsk_2021_fix_kxr/euslisp/main/panda_main.l index f622c63ee9..f8e0df7463 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda_main.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda_main.l @@ -45,6 +45,8 @@ (ros::ros-info "Start repairing") +(send *ri* :stop-grasp :rarm) +(send *ri* :stop-grasp :larm) (send *robot* :reset-coop-pose) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l index 35b32c6ae7..c582dd9f0e 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l @@ -48,6 +48,26 @@ :rpy #f(-0.5 0 0) )) +(setq *servo-to-arange-cable-pose-1* + (make-cascoords + :pos #f(30.646 -43.753 41.312) + :rpy #f(2.783 1.34 2.785) + )) +(setq *servo-to-arange-cable-pose-2* + (make-cascoords + :pos #f(30.066 -4.353 51.323) + :rpy #f(2.67 1.353 2.69) + )) +(setq *servo-to-arange-cable-pose-3* + (make-cascoords + :pos #f(29.544 -15.79 41.237) + :rpy #f(2.618 1.341 2.806) + )) +(setq *servo-to-arange-cable-pose-4* + (make-cascoords + :pos #f(31.268 -80.999 89.198) + :rpy #f(2.782 1.393 2.887) + )) (setq *servo-to-search-cable-pose* (make-cascoords @@ -56,39 +76,39 @@ )) (setq *servo-to-search-cable-pose-1* (make-cascoords - :pos #f(-2.566 -82.76 38.397) - :rpy #f(0.188 1.494 1.785) + :pos #f(46.676 -79.639 45.276) + :rpy #f(1.727 0.974 1.716) )) (setq *servo-to-search-cable-pose-2* (make-cascoords - :pos #f(-17.376 -144.684 -6.541) - :rpy #f(-1.561 0.409 3.087) + :pos #f(61.917 -9.069 60.789) + :rpy #f(2.458 1.464 2.421) )) (setq *servo-to-search-cable-pose-3* (make-cascoords - :pos #f(13.83 -55.579 91.701) - :rpy #f(-1.799 1.134 -0.262) + :pos #f(61.24 18.619 63.366) + :rpy #f(-1.812 1.174 -1.821) )) (setq *servo-to-cable-front-grasp-ready* (make-cascoords :pos #f(34.974 -76.339 49.308) - :rpy #f(1.453 0.441 1.446) + :rpy #f(1.453 0.7 1.446) )) (setq *cable-front-to-pre-grasp-pose-1* (make-cascoords :pos #f(-45.691 22.526 27.675) - :rpy #f(-1.655 0.5 1.413) + :rpy #f(-1.655 0.7 1.413) )) (setq *cable-front-to-pre-grasp-pose-2* (make-cascoords :pos #f(-39.479 16.61 12.343) - :rpy #f(-1.365 0.5 1.57) + :rpy #f(-1.365 0.7 1.57) )) (setq *cable-front-to-grasp-pose* (make-cascoords :pos #f(-16.344 21.751 10.114) - :rpy #f(-1.45 0.5 1.552) + :rpy #f(-1.45 0.7 1.552) )) (setq *cable-front-change-pose-before-insert* (make-cascoords diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index 0128c41846..5c15603a9f 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -55,19 +55,19 @@ (defun move-rarm-x (delta) (send *robot* :angle-vector (send *ri* :state :potentio-vector)) (send *robot* :rarm :move-end-pos (float-vector delta 0 0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :angle-vector (send *robot* :angle-vector) 1000) (send *ri* :wait-interpolation)) (defun move-rarm-y (delta) (send *robot* :angle-vector (send *ri* :state :potentio-vector)) (send *robot* :rarm :move-end-pos (float-vector 0 delta 0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :angle-vector (send *robot* :angle-vector) 1000) (send *ri* :wait-interpolation)) (defun move-rarm-z (delta) (send *robot* :angle-vector (send *ri* :state :potentio-vector)) (send *robot* :rarm :move-end-pos (float-vector 0 0 delta)) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :angle-vector (send *robot* :angle-vector) 1000) (send *ri* :wait-interpolation)) (defun search-body () @@ -294,6 +294,34 @@ memo to get parameters (setq test (send (send base-to-marker-origin :copy-worldcoords) :transform param)) (objects (list *robot* base-to-marker-origin test)) |# +(defun rarm-arange-cable (servo-name side) + (let ((base-to-servo-marker-origin nil)) + (setq base-to-servo-marker-origin *base_to_rarm_elbow_p_servo*) + (setq base-to-arange-cable-pose-1 (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *servo-to-arange-cable-pose-1*)) + (setq base-to-arange-cable-pose-2 (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *servo-to-arange-cable-pose-2*)) + (setq base-to-arange-cable-pose-3 (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *servo-to-arange-cable-pose-3*)) + (setq base-to-arange-cable-pose-4 (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *servo-to-arange-cable-pose-4*)) + + (send *ri* :start-grasp :rarm) + (setq ret (send *robot* :rarm :inverse-kinematics base-to-arange-cable-pose-1)) + (if (not ret) (ros::ros-error "IK-failed") nil) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (setq ret (send *robot* :rarm :inverse-kinematics base-to-arange-cable-pose-2)) + (if (not ret) (ros::ros-error "IK-failed") nil) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (setq ret (send *robot* :rarm :inverse-kinematics base-to-arange-cable-pose-3)) + (if (not ret) (ros::ros-error "IK-failed") nil) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (setq ret (send *robot* :rarm :inverse-kinematics base-to-arange-cable-pose-4)) + (if (not ret) (ros::ros-error "IK-failed") nil) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation))) (defun rarm-grasp-cable (servo-name side) (let ((base-to-servo-marker-origin nil) @@ -309,10 +337,8 @@ memo to get parameters (stamp nil) (ret nil)) (setq count 0) - (setq stamp (ros::time-now)) - (when (send *tfl* :wait-for-transform "dual_arm_base" servo-name stamp 5) - (setq base-to-servo-marker-origin (send *tfl* :lookup-transform "dual_arm_base" servo-name stamp))) (ros::ros-info "searching cable...") + (setq base-to-servo-marker-origin *base_to_rarm_elbow_p_servo*) (setq cable-id-list (servo-name-to-cable-id-list servo-name)) (if (string= side "right") (progn @@ -374,13 +400,8 @@ memo to get parameters (when (send *tfl* :wait-for-transform "dual_arm_base" marker-name stamp 5) (setq base-to-cable-marker-origin (send *tfl* :lookup-transform "dual_arm_base" marker-name stamp))) - (send *robot* :rarm :move-end-pos #f(-20 0 30)) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) - (send *ri* :wait-interpolation) (send *robot* :rarm_joint1 :joint-angle 60) - (send *robot* :rarm :inverse-kinematics base-to-search-pose) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) - (send *ri* :wait-interpolation) + (if (string= grasp-cable-side "front") (progn (setq base-to-pre-grasp-pose-1 (send (send base-to-cable-marker-origin :copy-worldcoords) :transform *cable-front-to-pre-grasp-pose-1*)) @@ -394,31 +415,29 @@ memo to get parameters ;;pre-grasp-poses (send *ri* :start-grasp :rarm) (unix:sleep 1) - (send *ri* :stop-grasp :rarm :width 0.01) + (send *ri* :stop-grasp :rarm :width 0.015) (unix:sleep 1) - (send *robot* :rarm :inverse-kinematics base-to-pre-grasp-pose-1) - (send *ri* :angle-vector (send *robot* :angle-vector) 3000) - (send *ri* :wait-interpolation) - (send *robot* :rarm :inverse-kinematics base-to-pre-grasp-pose-2) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) - (send *ri* :wait-interpolation) - - ;;grasp-pose - ;; (send *robot* :rarm :inverse-kinematics base-to-grasp-pose) - ;; (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + ;; (send *robot* :rarm :inverse-kinematics base-to-pre-grasp-pose-1) + ;; (send *ri* :angle-vector (send *robot* :angle-vector) 3000) ;; (send *ri* :wait-interpolation) - ;; (send *robot* :rarm :move-end-pos #f(2 0 0)) + ;; (send *robot* :rarm :inverse-kinematics base-to-pre-grasp-pose-2) ;; (send *ri* :angle-vector (send *robot* :angle-vector) 2000) ;; (send *ri* :wait-interpolation) + ;;grasp-pose + (send base-to-grasp-pose :rpy -0.1 0.3 1.7) + (send *robot* :rarm :inverse-kinematics base-to-grasp-pose) + (send *robot* :rarm :move-end-pos #f(-2 12 25)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (setq goal_x 670) - (setq goal_y 650) - (setq robust_x 60) - (setq min_x (- goal_x (/ robust_x 2))) - (setq max_x (+ goal_x (/ robust_x 2))) + (setq goal_y 660) + (setq min_x 630) + (setq max_x 710) (setq min_y 630) - (setq max_y 675) + (setq max_y 690) (setq k_x 0.13) (setq k_y 0.116) (setq fx 0) @@ -494,37 +513,22 @@ memo to get parameters (send *ri* :start-grasp :rarm) (unix:sleep 1) - (send *ri* :stop-grasp :rarm :width 0.003) + (send *ri* :stop-grasp :rarm :width 0.0008) (unix:sleep 1) - - (send *robot* :rarm :move-end-pos (float-vector 2 2 -4)) - (send *ri* :angle-vector (send *robot* :angle-vector) 1500) + (send *robot* :rarm :move-end-pos #f(5 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) - (send *ri* :start-grasp :rarm) - (unix:sleep 1) (setq base-to-change-pose-1 (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *cable-front-change-pose-before-insert-1*)) (setq base-to-change-pose-2 (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *cable-front-change-pose-before-insert-2*)) (setq base-to-change-pose-3 (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *cable-front-change-pose-before-insert-3*)) - (send *robot* :rarm :inverse-kinematics base-to-change-pose-1) - (send *ri* :angle-vector (send *robot* :angle-vector) 3000) - (send *ri* :wait-interpolation) (send *robot* :rarm :inverse-kinematics base-to-change-pose-2) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) (send *robot* :rarm :inverse-kinematics base-to-change-pose-3) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) - - ;; (setq self-coords (send *robot* :rarm :end-coords)) - ;; (setq target-pos (send (setq self-coords (send *robot* :rarm :end-coords)) :worldpos)) - ;; (send (setq target-coords (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *cable-front-change-pose-before-insert*)) :replace-pos target-pos) - - ;; (send *robot* :rarm :inverse-kinematics target-coords) - ;; (send *robot* :rarm :move-end-pos #f(5 10 10)) - ;; (send *ri* :angle-vector (send *robot* :angle-vector) 2000) - ;; (send *ri* :wait-interpolation) )) (defun rarm-insert-cable (servo-name side) @@ -584,6 +588,7 @@ memo to get parameters (setq stroke (* (elt base-stroke 0) i)) (ros::ros-info (format nil "stroke is ~A" stroke))))) + (send *ri* :start-grasp :rarm) (send *robot* :angle-vector (send *ri* :state :potentio-vector)) (send *robot* :rarm :move-end-pos #f(0 0 -8)) (send *ri* :angle-vector (send *robot* :angle-vector) 4000) @@ -619,7 +624,8 @@ memo to get parameters (progn (ros::ros-info "Bad") (move-rarm-y (- 0 h_delta)) - (move-rarm-x (- 0 i_delta)))) + (move-rarm-x (- 0.1 i_delta)) + (ros::ros-info (format nil "total_insert is ~A" total-insert)))) (setq mode 2)) (2 (ros::ros-info "Move to -y") @@ -641,7 +647,8 @@ memo to get parameters (progn (ros::ros-info "Bad") (move-rarm-y h_delta) - (move-rarm-x (- 0 i_delta)))) + (move-rarm-x (- 0.1 i_delta)) + (ros::ros-info (format nil "total_insert is ~A" total-insert)))) (setq mode 3)) (3 (ros::ros-info "Move to +z") @@ -663,7 +670,8 @@ memo to get parameters (progn (ros::ros-info "Bad") (move-rarm-y h_delta) - (move-rarm-x (- 0 i_delta)))) + (move-rarm-x (- 0.1 i_delta)) + (ros::ros-info (format nil "total_insert is ~A" total-insert)))) (setq mode 4)) (4 (ros::ros-info "Move to -z") @@ -685,7 +693,8 @@ memo to get parameters (progn (ros::ros-info "Bad") (move-rarm-y h_delta) - (move-rarm-x (- 0 i_delta)))) + (move-rarm-x (- 0.1 i_delta)) + (ros::ros-info (format nil "total_insert is ~A" total-insert)))) (setq mode 1))) (ros::ros-info (format nil "times of try is ~A" count)) (if (or (> total-insert total-insert-th) (> count 20)) From 40ed93a2f178ba4185dc40a7fab69d2b65582848 Mon Sep 17 00:00:00 2001 From: softyanija Date: Thu, 20 Jan 2022 11:08:11 +0900 Subject: [PATCH 115/127] [euslisp] succesed insertion demo --- jsk_2021_fix_kxr/euslisp/main/kxr_main.l | 23 +++++++++++++++++++ .../euslisp/main/panda-release-kxr.l | 4 ++++ .../main/panda-repair-rarm-elbow-p-servo.l | 4 +++- .../euslisp/panda_commons/parameters.l | 7 +++++- .../euslisp/panda_commons/utils.l | 16 ++++++++----- 5 files changed, 46 insertions(+), 8 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/main/kxr_main.l b/jsk_2021_fix_kxr/euslisp/main/kxr_main.l index eb16963e34..27ce4937fc 100644 --- a/jsk_2021_fix_kxr/euslisp/main/kxr_main.l +++ b/jsk_2021_fix_kxr/euslisp/main/kxr_main.l @@ -70,6 +70,25 @@ (send *robot* :angle-vector #f(0.0 -20.0 40.0 -20.0 0.0 0.0 -20.0 40.0 -20.0 0.0 0.0 -40.0 0.0 -27.0 -27.0 0.0 40.0 0.0 -27.0 -27.0 0.0 10.0)) (send *ri* :angle-vector (send *robot* :angle-vector) 300)) +(defun check-rarm-elbow-p () + (ros::ros-info "check-rarm-elbow-p") + (send *robot* :angle-vector #f(0.0 -20.0 40.0 -20.0 0.0 0.0 -20.0 40.0 -20.0 0.0 90.0 -90.0 0.0 -27.0 -27.0 0.0 40.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 300) + (unix:sleep 1) + (send *robot* :rarm-elbow-p :joint-angle 60) + (send *ri* :angle-vector (send *robot* :angle-vector) 300) + (unix:sleep 1) + (send *robot* :rarm-elbow-p :joint-angle 0) + (send *ri* :angle-vector (send *robot* :angle-vector) 300) + (unix:sleep 1) + (send *robot* :rarm-elbow-p :joint-angle 60) + (send *ri* :angle-vector (send *robot* :angle-vector) 300) + (unix:sleep 1) + (send *robot* :rarm-elbow-p :joint-angle 0) + (send *ri* :angle-vector (send *robot* :angle-vector) 300) + (unix:sleep 1)) + + (defun send-servo-info () (ros::ros-info "send-servo-info") (setq msg (instance sensor_msgs::jointstate :init)) @@ -149,6 +168,10 @@ (progn (stand-pose) (setq f 1))) + (if (string= action "check-rarm-elbow-p") + (progn + (check-rarm-elbow-p) + (setq f 1))) (if (= f 1) (progn diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l b/jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l index 40406fa776..bdcb1e3305 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l @@ -48,4 +48,8 @@ (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) (send req :action "stand-pose") (setq res (ros::service-call "repair_reaction" req t)) + (unix:sleep 2) + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "check-rarm-elbow-p") + (setq res (ros::service-call "repair_reaction" req t)) ) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l b/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l index 815c8f4eb3..a2c4f97cc6 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l @@ -11,11 +11,13 @@ (rarm-search-servo-tag "rpov-rarm_elbow_p_servo") + (unix:sleep 2) (setq stamp (ros::time-now)) (when (send *tfl* :wait-for-transform "dual_arm_base" "rpov-rarm_elbow_p_servo" stamp 5) (setq *base_to_rarm_elbow_p_servo* (send *tfl* :lookup-transform "dual_arm_base" "rpov-rarm_elbow_p_servo" stamp))) - ;; + (send *ri* :start-grasp :larm) + (while t (rarm-arange-cable "rpov-rarm_elbow_p_servo" "right") diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l index c582dd9f0e..80b63f9a72 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l @@ -130,6 +130,11 @@ :pos #f(22.596 -32.874 50.651) :rpy #f(1.347 0.974 1.204) )) +(setq *cable-front-change-pose-before-insert-4* + (make-cascoords + :pos #f(26.596 -22.874 50.651) + :rpy #f(1.32 1.374 1.404) + )) (setq *servo-to-slide-insert-right-cable-inter-pose* (make-cascoords :pos #f(34.757 -12.996 40.748) @@ -184,7 +189,7 @@ )) (setq *rarm-elbow-p-to-grasp-pose* (make-cascoords - :pos #f(-24.612 4.967 -27.886) + :pos #f(-24.612 4.967 -30.886) :rpy #f(-2.962 -1.494 3.004) )) diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index 5c15603a9f..8442133cd4 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -313,10 +313,10 @@ memo to get parameters (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) - (setq ret (send *robot* :rarm :inverse-kinematics base-to-arange-cable-pose-3)) - (if (not ret) (ros::ros-error "IK-failed") nil) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) - (send *ri* :wait-interpolation) + ;; (setq ret (send *robot* :rarm :inverse-kinematics base-to-arange-cable-pose-3)) + ;; (if (not ret) (ros::ros-error "IK-failed") nil) + ;; (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + ;; (send *ri* :wait-interpolation) (setq ret (send *robot* :rarm :inverse-kinematics base-to-arange-cable-pose-4)) (if (not ret) (ros::ros-error "IK-failed") nil) @@ -426,9 +426,9 @@ memo to get parameters ;; (send *ri* :wait-interpolation) ;;grasp-pose - (send base-to-grasp-pose :rpy -0.1 0.3 1.7) + (send base-to-grasp-pose :rpy -0.1 0.5 1.6) (send *robot* :rarm :inverse-kinematics base-to-grasp-pose) - (send *robot* :rarm :move-end-pos #f(-2 12 25)) + (send *robot* :rarm :move-end-pos #f(-3 12 28)) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) @@ -522,6 +522,7 @@ memo to get parameters (setq base-to-change-pose-1 (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *cable-front-change-pose-before-insert-1*)) (setq base-to-change-pose-2 (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *cable-front-change-pose-before-insert-2*)) (setq base-to-change-pose-3 (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *cable-front-change-pose-before-insert-3*)) + (setq base-to-change-pose-4 (send (send base-to-servo-marker-origin :copy-worldcoords) :transform *cable-front-change-pose-before-insert-4*)) (send *robot* :rarm :inverse-kinematics base-to-change-pose-2) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) @@ -529,6 +530,9 @@ memo to get parameters (send *robot* :rarm :inverse-kinematics base-to-change-pose-3) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) + (send *robot* :rarm :inverse-kinematics base-to-change-pose-4) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) )) (defun rarm-insert-cable (servo-name side) From 7ad2a46ab82d7b1a2915fd0d46964913c842656c Mon Sep 17 00:00:00 2001 From: softyanija Date: Sun, 23 Jan 2022 19:04:44 +0900 Subject: [PATCH 116/127] [euslisp] made progress --- jsk_2021_fix_kxr/euslisp/main/kxr_main.l | 17 +++++-- .../euslisp/main/panda-release-kxr.l | 50 +++++++++++++++---- .../main/panda-repair-rarm-elbow-p-servo.l | 4 +- 3 files changed, 55 insertions(+), 16 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/main/kxr_main.l b/jsk_2021_fix_kxr/euslisp/main/kxr_main.l index 27ce4937fc..b5695dde2a 100644 --- a/jsk_2021_fix_kxr/euslisp/main/kxr_main.l +++ b/jsk_2021_fix_kxr/euslisp/main/kxr_main.l @@ -65,14 +65,19 @@ (send *robot* :angle-vector #f(0.0 20.0 50.0 -70.0 0.0 90.0 -10.0 70.0 -20.0 0.0 -90.0 -180.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) (send *ri* :angle-vector (send *robot* :angle-vector) 200)) +(defun before-stand-pose () + (ros::ros-info "before-stand-pose") + (send *robot* :angle-vector #f(0.0 -45.0 95.0 -50.0 0.0 0.0 -45.0 95.0 -50.0 0.0 0.0 -90.0 0.0 -27.0 -27.0 0.0 90.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 200)) + (defun stand-pose () (ros::ros-info "stand-pose") - (send *robot* :angle-vector #f(0.0 -20.0 40.0 -20.0 0.0 0.0 -20.0 40.0 -20.0 0.0 0.0 -40.0 0.0 -27.0 -27.0 0.0 40.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *robot* :angle-vector #f(0.0 -20.0 35.0 -20.0 0.0 0.0 -20.0 35.0 -20.0 0.0 0.0 -40.0 0.0 -27.0 -27.0 0.0 40.0 0.0 -27.0 -27.0 0.0 10.0)) (send *ri* :angle-vector (send *robot* :angle-vector) 300)) (defun check-rarm-elbow-p () (ros::ros-info "check-rarm-elbow-p") - (send *robot* :angle-vector #f(0.0 -20.0 40.0 -20.0 0.0 0.0 -20.0 40.0 -20.0 0.0 90.0 -90.0 0.0 -27.0 -27.0 0.0 40.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *robot* :angle-vector #f(0.0 -20.0 35.0 -20.0 0.0 0.0 -20.0 35.0 -20.0 0.0 90.0 -90.0 0.0 -27.0 -27.0 0.0 40.0 0.0 -27.0 -27.0 0.0 10.0)) (send *ri* :angle-vector (send *robot* :angle-vector) 300) (unix:sleep 1) (send *robot* :rarm-elbow-p :joint-angle 60) @@ -86,7 +91,9 @@ (unix:sleep 1) (send *robot* :rarm-elbow-p :joint-angle 0) (send *ri* :angle-vector (send *robot* :angle-vector) 300) - (unix:sleep 1)) + (unix:sleep 1) + (send *robot* :angle-vector #f(0.0 -20.0 35.0 -20.0 0.0 0.0 -20.0 35.0 -20.0 0.0 0.0 -40.0 0.0 -27.0 -27.0 0.0 40.0 0.0 -27.0 -27.0 0.0 10.0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 300)) (defun send-servo-info () @@ -164,6 +171,10 @@ (progn (send m :joint_state (send-servo-info)) (setq f 1))) + (if (string= action "before-stand-pose") + (progn + (before-stand-pose) + (setq f 1))) (if (string= action "stand-pose") (progn (stand-pose) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l b/jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l index bdcb1e3305..495f0917c6 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l @@ -3,23 +3,32 @@ (defun panda-release-kxr () (ros::ros-info "Releasing KXR") - (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(308.472 -259.081 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) - (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(305.157 -14.307 1220.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(358.472 -259.081 1230.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(355.157 -14.307 1230.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(358.472 -259.081 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(355.157 -14.307 1220.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) - (send *robot* :rarm :move-end-pos #f(-180 0 0)) - (send *robot* :larm :move-end-pos #f(-180 0 0)) + (send *robot* :rarm :move-end-pos #f(-130 0 0)) + (send *robot* :larm :move-end-pos #f(-130 0 0)) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "before-stand-pose") + (setq res (ros::service-call "repair_reaction" req t)) + (send *robot* :rarm :move-end-pos #f(-50 -50 0)) (send *robot* :larm :move-end-pos #f(-50 50 0)) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) - (send *robot* :rarm :move-end-pos #f(40 0 0)) - (send *robot* :larm :move-end-pos #f(40 0 0)) + (send *robot* :rarm :move-end-pos #f(20 0 0)) + (send *robot* :larm :move-end-pos #f(20 0 0)) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) @@ -27,20 +36,19 @@ (send *ri* :stop-grasp :larm) (unix:sleep 2) - (send *robot* :rarm :move-end-pos #f(-50 0 0)) - (send *robot* :larm :move-end-pos #f(-50 0 0)) + (send *robot* :rarm :move-end-pos #f(-50 15 0)) + (send *robot* :larm :move-end-pos #f(-50 -15 0)) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) - (send *robot* :rarm :move-end-pos #f(0 100 0)) - (send *robot* :larm :move-end-pos #f(0 -100 0)) + (send *robot* :rarm :move-end-pos #f(-50 130 0)) + (send *robot* :larm :move-end-pos #f(-50 -130 0)) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) (send req :action "rarm-hold") (setq res (ros::service-call "repair_reaction" req t)) - (unix:sleep 2) (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) (send req :action "larm-hold") (setq res (ros::service-call "repair_reaction" req t)) @@ -53,3 +61,23 @@ (send req :action "check-rarm-elbow-p") (setq res (ros::service-call "repair_reaction" req t)) ) + +;; (send *ri* :stop-grasp :rarm) +;; (send *ri* :stop-grasp :larm) +;; (unix:sleep 1) + +;; (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(358.472 -259.081 1217.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) +;; (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(358.157 -14.307 1217.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) +;; (send *robot* :rarm :move-end-pos #f(-100 0 0)) +;; (send *robot* :larm :move-end-pos #f(-100 0 0)) +;; (send *ri* :angle-vector (send *robot* :angle-vector) 2000) +;; (send *ri* :wait-interpolation) + +;; (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(358.472 -259.081 1205.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) +;; (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(355.157 -14.307 1205.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) +;; (send *ri* :angle-vector (send *robot* :angle-vector) 3000) +;; (send *ri* :wait-interpolation) + +;; (send *ri* :start-grasp :rarm) +;; (send *ri* :start-grasp :larm) +;; (unix:sleep 1) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l b/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l index a2c4f97cc6..f532eec1b2 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l @@ -77,8 +77,8 @@ (send *ri* :start-grasp :larm) (unix:sleep 1) - (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(308.472 -259.081 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) - (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(305.157 -14.307 1220.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(308.472 -259.081 1217.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(305.157 -14.307 1217.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) From c3702247f45534d04a706a7573137071b425b27e Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 8 Jul 2022 14:18:34 +0900 Subject: [PATCH 117/127] [euslisp] make rleg-ankle demo --- .../euslisp/main/panda-repair-rleg-ankle-r.l | 106 +++++++++++------- .../euslisp/main/panda-set-rleg.l | 37 +++--- jsk_2021_fix_kxr/euslisp/main/panda_main.l | 2 + .../euslisp/panda_commons/parameters.l | 21 ++++ .../euslisp/panda_commons/utils.l | 61 +++++++++- 5 files changed, 165 insertions(+), 62 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-repair-rleg-ankle-r.l b/jsk_2021_fix_kxr/euslisp/main/panda-repair-rleg-ankle-r.l index eaa8bd1368..7cae54be27 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda-repair-rleg-ankle-r.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda-repair-rleg-ankle-r.l @@ -15,10 +15,20 @@ (setq res (ros::service-call "repair_reaction" req t)) ;;move-rarm + (send *robot* :rarm_joint1 :joint-angle 30) + (send *robot* :rarm_joint3 :joint-angle -20) + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(390.444 -87.992 1363.0) :rpy (float-vector 1.57 1.526 0))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + (send *robot* :rarm_joint7 :joint-angle 0) + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(456.263 -105.465 1325.954) :rpy (float-vector 1.641 0.512 -3.029))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + (setq stamp (ros::time-now)) - (when (send *tfl* :wait-for-transform "dual_arm_base" "rpov-rleg-ankle-r-servo-b" stamp 5) - (setq *base_to_rleg_ankle_r_servo_b* (send *tfl* :lookup-transform "dual_arm_base" "rpov-rleg-ankle-r-servo-b" stamp))) + (when (send *tfl* :wait-for-transform "dual_arm_base" "rpov-rleg_ankle_r_servo_b" stamp 5) + (setq *base_to_rleg_ankle_r_servo_b* (send *tfl* :lookup-transform "dual_arm_base" "rpov-rleg_ankle_r_servo_b" stamp))) ;;grasp ankle-r from back (rarm-grasp-rleg-ankle-r-from-back) @@ -30,7 +40,7 @@ (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) - (send *robot* :larm :move-end-pos #f(-70 100 -50)) + (send *robot* :larm :move-end-pos #f(-70 100 -70)) (send *ri* :angle-vector (send *robot* :angle-vector) 2000) (send *ri* :wait-interpolation) @@ -38,44 +48,54 @@ (send req :action "show-rleg-ankle-r-2") (setq res (ros::service-call "repair_reaction" req t)) - ;;convert back to front - (setq *base_to_rleg_ankle_r_servo* (send (send *base_to_rleg_ankle_r_servo_b* :copy-worldcoords) :transform *servo-back-to-front*)) - - (larm-search-servo-tag "lpov-rleg_ankle_r_servo") - - (setq stamp (ros::time-now)) - (when (send *tfl* :wait-for-transform "dual_arm_base" "lpov-rleg_ankle_r_servo" stamp 5) - (setq *base_to_rarm_elbow_p_servo* (send *tfl* :lookup-transform "dual_arm_base" "lpov-rleg_ankle_r_servo" stamp))) - - ;; - (while t - (larm-grasp-cable "lpov-rleg_ankle_r_servo" "left") - - (larm-insert-cable "lpov-rleg_ankle_r_servo" "left") - - (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) - (send req :action "send-servo-info") - (setq res (ros::service-call "repair_reaction" req t)) - (setq joint_states (send res :joint_state)) - (setq timeout-servo-list (make-timeout-servo-list joint_states)) - (if (not timeout-servo-list) - (progn - (ros::ros-info "Success to insert cable") - (return-from nil)) - (progn - (setq target (decide-target-servo timeout-servo-list)) - (if (string-equal target "rleg-ankle-r") - (progn - (ros::ros-info "Failed to insert cable") - (ros::ros-info "Retry to insert cable")) - (progn - (ros::ros-info "Success to insert cable") - (return-from nil)) - )))) - - (send *ri* :stop-grasp :rarm) - (unix:sleep 1) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(412.83 235.711 1381.438) :rpy (float-vector 0 1.511 -1.665))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) - (send *robot* :rarm :move-end-pos #f(-100 0 0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) - (send *ri* :wait-interpolation)) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(391.546 202.961 1263.654) :rpy (float-vector -1.606 -0.012 1.544))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (exit) + ;;convert back to front + ;; (setq *base_to_rleg_ankle_r_servo* (send (send *base_to_rleg_ankle_r_servo_b* :copy-worldcoords) :transform *servo-back-to-front*)) + + ;; (larm-search-servo-tag "lpov-rleg_ankle_r_servo") + + ;; (setq stamp (ros::time-now)) + ;; (when (send *tfl* :wait-for-transform "dual_arm_base" "lpov-rleg_ankle_r_servo" stamp 5) + ;; (setq *base_to_rarm_elbow_p_servo* (send *tfl* :lookup-transform "dual_arm_base" "lpov-rleg_ankle_r_servo" stamp))) + + ;; ;; + ;; (while t + ;; (larm-grasp-cable "lpov-rleg_ankle_r_servo" "left") + + ;; (larm-insert-cable "lpov-rleg_ankle_r_servo" "left") + + ;; (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + ;; (send req :action "send-servo-info") + ;; (setq res (ros::service-call "repair_reaction" req t)) + ;; (setq joint_states (send res :joint_state)) + ;; (setq timeout-servo-list (make-timeout-servo-list joint_states)) + ;; (if (not timeout-servo-list) + ;; (progn + ;; (ros::ros-info "Success to insert cable") + ;; (return-from nil)) + ;; (progn + ;; (setq target (decide-target-servo timeout-servo-list)) + ;; (if (string-equal target "rleg-ankle-r") + ;; (progn + ;; (ros::ros-info "Failed to insert cable") + ;; (ros::ros-info "Retry to insert cable")) + ;; (progn + ;; (ros::ros-info "Success to insert cable") + ;; (return-from nil)) + ;; )))) + + ;; (send *ri* :stop-grasp :rarm) + ;; (unix:sleep 1) + + ;; (send *robot* :rarm :move-end-pos #f(-100 0 0)) + ;; (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + ;; (send *ri* :wait-interpolation) + ) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda-set-rleg.l b/jsk_2021_fix_kxr/euslisp/main/panda-set-rleg.l index f402bb815e..978a1f626c 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda-set-rleg.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda-set-rleg.l @@ -4,12 +4,6 @@ (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) (send req :action "show-rarm-elbow-p") (setq res (ros::service-call "repair_reaction" req t)) - (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) - (send req :action "rarm-free") - (setq res (ros::service-call "repair_reaction" req t)) - (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) - (send req :action "larm-free") - (setq res (ros::service-call "repair_reaction" req t)) (while t (setq found-body (search-body)) @@ -46,28 +40,37 @@ (send *ri* :start-grasp :larm) (unix:sleep 2) - (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(308.472 -259.081 1205.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) - (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(305.157 -14.307 1205.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "rarm-free") + (setq res (ros::service-call "repair_reaction" req t)) + (setq req (instance jsk_2021_fix_kxr::RepairInfoRequest :init)) + (send req :action "larm-free") + (setq res (ros::service-call "repair_reaction" req t)) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(308.472 -259.081 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(305.157 -14.307 1220.274) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) - (send *ri* :stop-grasp :larm) - (unix:sleep 1) - - (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) - (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1220.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -85.155 1220.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 159.619 1220.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) (send *ri* :angle-vector (send *robot* :angle-vector) 3000) (send *ri* :wait-interpolation) - (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -75.155 1210.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) - (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 169.619 1214.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) - (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -85.155 1213.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 159.619 1213.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 1000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(300.472 -81.155 1213.164) :rpy (float-vector 1.57 1.526 0.0))) :translation-axis t :rotation-axis t) + (send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(297.605 163.619 1213.223) :rpy (float-vector -1.224 1.5 0.362))) :translation-axis t :rotation-axis t) + (send *ri* :angle-vector (send *robot* :angle-vector) 1000) (send *ri* :wait-interpolation) (send *ri* :stop-grasp :rarm) (unix:sleep 1) - (send *robot* :rarm :move-end-pos #f(-100 0 0)) + (send *robot* :rarm :move-end-pos #f(-150 0 0)) (send *ri* :angle-vector (send *robot* :angle-vector) 2500) (send *ri* :wait-interpolation) diff --git a/jsk_2021_fix_kxr/euslisp/main/panda_main.l b/jsk_2021_fix_kxr/euslisp/main/panda_main.l index f8e0df7463..f28841ed93 100644 --- a/jsk_2021_fix_kxr/euslisp/main/panda_main.l +++ b/jsk_2021_fix_kxr/euslisp/main/panda_main.l @@ -17,7 +17,9 @@ (require "package://jsk_2021_fix_kxr/euslisp/main/panda-repair-rarm-elbow-p-servo.l") (require "package://jsk_2021_fix_kxr/euslisp/main/panda-repair-larm-elbow-p-servo.l") +(require "package://jsk_2021_fix_kxr/euslisp/main/panda-repair-rleg-ankle-r.l") (require "package://jsk_2021_fix_kxr/euslisp/main/panda-set-rarm.l") +(require "package://jsk_2021_fix_kxr/euslisp/main/panda-set-rleg.l") (require "package://jsk_2021_fix_kxr/euslisp/main/panda-return-rarm.l") (require "package://jsk_2021_fix_kxr/euslisp/main/panda-release-kxr.l") diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l index 80b63f9a72..6f424480a5 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/parameters.l @@ -193,6 +193,27 @@ :rpy #f(-2.962 -1.494 3.004) )) +(setq *rleg-ankle-r-b-to-pre-grasp-pose* + (make-cascoords + :pos #f(-3.695 -53.343 -1.488) + :rpy #f(1.496 -0.004 0.024) + )) +(setq *rleg-ankle-r-b-to-inter-pose1* + (make-cascoords + :pos #f(-4.525 -77.122 23.349) + :rpy #f(1.46 0.212 0.035) + )) +(setq *rleg-ankle-r-b-to-pre-grasp-pose* + (make-cascoords + :pos #f(-3.695 -53.343 -1.488) + :rpy #f(1.496 -0.004 0.024) + )) +(setq *rleg-ankle-r-b-to-grasp-pose* + (make-cascoords + :pos #f(-3.615 -40.964 0.019) + :rpy #f(1.479 -0.018 0.025) + )) + (setq *cable-frame* "cable-frame") diff --git a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l index 8442133cd4..08f827f06c 100644 --- a/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l +++ b/jsk_2021_fix_kxr/euslisp/panda_commons/utils.l @@ -722,8 +722,65 @@ memo to get parameters (send *ri* :wait-interpolation) (return-from rarm-insert-cable t)))) )) - - + +(defun rarm-grasp-rleg-ankle-r-from-back () + (let ((base-to-marker-origin nil) + (base-to-pre-grasp-pose nil) + (base-to-grasp-pose nil) + (stamp nil) + (ret nil)) + (ros::ros-info "start to grasp rleg-ankle-r from back") + (setq stamp (ros::time-now)) + (setq marker-name "rpov-rleg_ankle_r_servo_b") + (when (send *tfl* :wait-for-transform "dual_arm_base" marker-name stamp 5) + (setq base-to-marker-origin (send *tfl* :lookup-transform "dual_arm_base" marker-name stamp)) + (setq *base_to_rleg_ankle_r_servo_b* base-to-marker-origin)) + (setq base-to-inter-pose1 (send (send base-to-marker-origin :copy-worldcoords) :transform *rleg-ankle-r-b-to-inter-pose1*)) + (setq base-to-pre-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *rleg-ankle-r-b-to-pre-grasp-pose*)) + (setq base-to-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *rleg-ankle-r-b-to-grasp-pose*)) + + ;;move larm under rarm-elbow-p-servo + (send *ri* :stop-grasp :rarm :width 0.042) + (unix:sleep 1) + + ;; (send *robot* :rarm :inverse-kinematics base-to-inter-pose1) + ;; (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + ;; (send *ri* :wait-interpolation) + + (send *robot* :rarm_joint1 :joint-angle 39.59) + (send *robot* :rarm_joint2 :joint-angle 18.15) + (send *robot* :rarm_joint3 :joint-angle -19.73) + (send *robot* :rarm_joint4 :joint-angle -66.7541) + (send *robot* :rarm_joint5 :joint-angle 29.039) + (send *robot* :rarm_joint6 :joint-angle 88.774) + (send *robot* :rarm_joint7 :joint-angle -23.2779) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm_joint1 :joint-angle 40.5064) + (send *robot* :rarm_joint2 :joint-angle 27.7574) + (send *robot* :rarm_joint3 :joint-angle -14.85) + (send *robot* :rarm_joint4 :joint-angle -52.52) + (send *robot* :rarm_joint5 :joint-angle 15.84) + (send *robot* :rarm_joint6 :joint-angle 82.712) + (send *robot* :rarm_joint7 :joint-angle -11.47) + (send *robot* :rarm :move-end-pos #f(-25 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + (send *robot* :rarm_joint1 :joint-angle 40.5064) + (send *robot* :rarm_joint2 :joint-angle 27.7574) + (send *robot* :rarm_joint3 :joint-angle -14.85) + (send *robot* :rarm_joint4 :joint-angle -52.52) + (send *robot* :rarm_joint5 :joint-angle 15.84) + (send *robot* :rarm_joint6 :joint-angle 82.712) + (send *robot* :rarm_joint7 :joint-angle -11.47) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + + (send *ri* :start-grasp :rarm) + (unix:sleep 1) + )) #| (setq wrench-init (send (one-shot-subscribe "/dual_panda/rarm_state_controller/franka_states" franka_msgs::FrankaState) :k_f_ext_hat_k))) From b554da4eb70af4af19907c34b49d656e8d31f1cd Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 8 Jul 2022 14:44:45 +0900 Subject: [PATCH 118/127] [launch] add comment which show apriltag_continuous_detection.launch is default code of launching apriltag recognition node --- .../apriltag_continuous_detection.launch | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 jsk_2021_fix_kxr/launch/apriltag_continuous_detection.launch diff --git a/jsk_2021_fix_kxr/launch/apriltag_continuous_detection.launch b/jsk_2021_fix_kxr/launch/apriltag_continuous_detection.launch new file mode 100644 index 0000000000..7c1efc322a --- /dev/null +++ b/jsk_2021_fix_kxr/launch/apriltag_continuous_detection.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + From 945ac6ebb120a8d0c68fce23da475e92dad29111 Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 8 Jul 2022 15:06:45 +0900 Subject: [PATCH 119/127] [euslisp/repairdemo] detele tmp files --- .../euslisp/repairdemo/#repairdemo_init.l# | 29 ------------------- .../euslisp/repairdemo/#repairdemo_return.l# | 9 ------ 2 files changed, 38 deletions(-) delete mode 100644 jsk_2021_fix_kxr/euslisp/repairdemo/#repairdemo_init.l# delete mode 100644 jsk_2021_fix_kxr/euslisp/repairdemo/#repairdemo_return.l# diff --git a/jsk_2021_fix_kxr/euslisp/repairdemo/#repairdemo_init.l# b/jsk_2021_fix_kxr/euslisp/repairdemo/#repairdemo_init.l# deleted file mode 100644 index d01897bd1a..0000000000 --- a/jsk_2021_fix_kxr/euslisp/repairdemo/#repairdemo_init.l# +++ /dev/null @@ -1,29 +0,0 @@ -#!/usr/bin/env roseus - -(require "package://panda_eus/euslisp/dual_panda-interface.l") - -(dual_panda-init) - -(objects (list *robot*)) - -(send *ri* :stop-grasp :rarm) -(send *ri* :stop-grasp :larm) -(unix::sleep 2) - -(send *robot* :reset-pose) -(send *robot* :larm :move-end-pos #f(-500 0 0)) -(send *robot* :rarm :move-end-pos #f(-500 0 0)) - -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - -#| wordkbench init -(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(412 100 1300) :rpy (float-vector -3.14 1.57 1.57))) :translation-axis t :rotation-axis t)|# - -#| check KXR position -(send *robot* :rarm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(345 -246.5 1330) :rpy (float-vector 0.0 1.57 -1.57))) :translation-axis t :rotation-axis t) -(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(345 -84.8 1330) :rpy (float-vector 1.57 1.57 -3.14))) :translation-axis t :rotation-axis t) -(send *robot* :larm :move-end-pos #f(-50 0 0)) -(send *robot* :rarm :move-end-pos #f(-50 0 0)) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) |# diff --git a/jsk_2021_fix_kxr/euslisp/repairdemo/#repairdemo_return.l# b/jsk_2021_fix_kxr/euslisp/repairdemo/#repairdemo_return.l# deleted file mode 100644 index eed9b85651..0000000000 --- a/jsk_2021_fix_kxr/euslisp/repairdemo/#repairdemo_return.l# +++ /dev/null @@ -1,9 +0,0 @@ -#!/usr/bin/env roseus - -(send *robot* :larm :inverse-kinematics (send (send *robot* :copy-worldcoords) :transform (make-coords :pos #f(345 280 1330) :rpy (float-vector -1.57 1.57 0))) :translation-axis t :rotation-axis t) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) - -(send *robot* :larm :move-end-pos #f(0 0 200)) -(send *ri* :angle-vector (send *robot* :angle-vector) 3000) -(send *ri* :wait-interpolation) \ No newline at end of file From f4b371cb73db64be94b437cf9dc6d6bbbc2d0294 Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 8 Jul 2022 16:44:45 +0900 Subject: [PATCH 120/127] [euslisp] make experiment dir and move market_test to this --- .../experiment/marker_test/marker_pickup.l | 61 +++++++++++++++++++ .../experiment/marker_test/parameters.l | 5 ++ 2 files changed, 66 insertions(+) create mode 100644 jsk_2021_fix_kxr/euslisp/experiment/marker_test/marker_pickup.l create mode 100644 jsk_2021_fix_kxr/euslisp/experiment/marker_test/parameters.l diff --git a/jsk_2021_fix_kxr/euslisp/experiment/marker_test/marker_pickup.l b/jsk_2021_fix_kxr/euslisp/experiment/marker_test/marker_pickup.l new file mode 100644 index 0000000000..74aa9d61e0 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/experiment/marker_test/marker_pickup.l @@ -0,0 +1,61 @@ +#!/usr/bin/env roseus + +(require "package://panda_eus/euslisp/dual_panda-interface.l") +(require "package://jsk_2021_fix_kxr/euslisp/experiment/marker_test/parameters.l") + +(ros::roseus-add-msgs "std_msgs") + +(ros::load-ros-manifest "apriltag_ros") + +(ros::roseus "marker_test") + +(dual_panda-init) + +(objects (list *robot*)) +(defvar *dual_arm_base-to-marker-coords*) +(defvar *rarm_end_effector-to-marker-coords*) + +(defun grasp-marker (marker-name) + (let ((base-to-marker-origin nil) + (base-to-pre-grasp-pose nil) + (base-to-grasp-pose nil) + (stamp nil) + (ret nil)) + (ros::ros-info "grasp a marker") + (setq stamp (ros::time-now)) + (send *robot* :reset-pose) + (send *robot* :rarm :move-end-pos #f(-50 0 0)) + (send *robot* :larm :move-end-pos #f(-50 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 1500) + (send *ri* :wait-interpolation) + + (when (send *tfl* :wait-for-transform "dual_arm_base" marker-name stamp 5) + (setq base-to-marker-origin (send *tfl* :lookup-transform "dual_arm_base" marker-name stamp))) + (setq base-to-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *marker-to-grasp-pose*)) + + ;pre-grasp-pose + (send *robot* :rarm :inverse-kinematics base-to-grasp-pose) + (send *robot* :rarm :move-end-pos #f(-50 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 3000) + (send *ri* :wait-interpolation) + ;grasp-pose + (send *robot* :rarm :inverse-kinematics base-to-grasp-pose) + (send *ri* :angle-vector (send *robot* :angle-vector) 1500) + (send *ri* :wait-interpolation) + + (send *ri* :start-grasp :rarm) + (unix:sleep 1) + (send *ri* :stop-grasp :rarm) + (unix:sleep 1) + + (unix:sleep 1) + + ;return to start-pose + (send *robot* :reset-pose) + (send *robot* :rarm :move-end-pos #f(-50 0 0)) + (send *robot* :larm :move-end-pos #f(-50 0 0)) + (send *ri* :angle-vector (send *robot* :angle-vector) 2000) + (send *ri* :wait-interpolation) + (ros::ros-info "finish") + )) + diff --git a/jsk_2021_fix_kxr/euslisp/experiment/marker_test/parameters.l b/jsk_2021_fix_kxr/euslisp/experiment/marker_test/parameters.l new file mode 100644 index 0000000000..0643ab0236 --- /dev/null +++ b/jsk_2021_fix_kxr/euslisp/experiment/marker_test/parameters.l @@ -0,0 +1,5 @@ +(setq *marker-to-grasp-pose* + (make-cascoords + :pos #f(12.503 -38.046 48.706) + :rpy #f(2.517 1.299 -2.2) + )) From dff4fe71bf682ce53d6cb747db0e1340eb6447ab Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 8 Jul 2022 16:48:04 +0900 Subject: [PATCH 121/127] [euslisp/move_test] detele tmp files --- .../euslisp/marker_test/marker_pickup.l | 61 ------------------- .../euslisp/marker_test/parameters.l | 5 -- 2 files changed, 66 deletions(-) delete mode 100644 jsk_2021_fix_kxr/euslisp/marker_test/marker_pickup.l delete mode 100644 jsk_2021_fix_kxr/euslisp/marker_test/parameters.l diff --git a/jsk_2021_fix_kxr/euslisp/marker_test/marker_pickup.l b/jsk_2021_fix_kxr/euslisp/marker_test/marker_pickup.l deleted file mode 100644 index df807aeb24..0000000000 --- a/jsk_2021_fix_kxr/euslisp/marker_test/marker_pickup.l +++ /dev/null @@ -1,61 +0,0 @@ -#!/usr/bin/env roseus - -(require "package://panda_eus/euslisp/dual_panda-interface.l") -(require "package://jsk_2021_fix_kxr/euslisp/marker_test/parameters.l") - -(ros::roseus-add-msgs "std_msgs") - -(ros::load-ros-manifest "apriltag_ros") - -(ros::roseus "marker_test") - -(dual_panda-init) - -(objects (list *robot*)) -(defvar *dual_arm_base-to-marker-coords*) -(defvar *rarm_end_effector-to-marker-coords*) - -(defun grasp-marker (marker-name) - (let ((base-to-marker-origin nil) - (base-to-pre-grasp-pose nil) - (base-to-grasp-pose nil) - (stamp nil) - (ret nil)) - (ros::ros-info "grasp a marker") - (setq stamp (ros::time-now)) - (send *robot* :reset-pose) - (send *robot* :rarm :move-end-pos #f(-50 0 0)) - (send *robot* :larm :move-end-pos #f(-50 0 0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 1500) - (send *ri* :wait-interpolation) - - (when (send *tfl* :wait-for-transform "dual_arm_base" marker-name stamp 5) - (setq base-to-marker-origin (send *tfl* :lookup-transform "dual_arm_base" marker-name stamp))) - (setq base-to-grasp-pose (send (send base-to-marker-origin :copy-worldcoords) :transform *marker-to-grasp-pose*)) - - ;pre-grasp-pose - (send *robot* :rarm :inverse-kinematics base-to-grasp-pose) - (send *robot* :rarm :move-end-pos #f(-50 0 0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 3000) - (send *ri* :wait-interpolation) - ;grasp-pose - (send *robot* :rarm :inverse-kinematics base-to-grasp-pose) - (send *ri* :angle-vector (send *robot* :angle-vector) 1500) - (send *ri* :wait-interpolation) - - (send *ri* :start-grasp :rarm) - (unix:sleep 1) - (send *ri* :stop-grasp :rarm) - (unix:sleep 1) - - (unix:sleep 1) - - ;return to start-pose - (send *robot* :reset-pose) - (send *robot* :rarm :move-end-pos #f(-50 0 0)) - (send *robot* :larm :move-end-pos #f(-50 0 0)) - (send *ri* :angle-vector (send *robot* :angle-vector) 2000) - (send *ri* :wait-interpolation) - (ros::ros-info "finish") - )) - diff --git a/jsk_2021_fix_kxr/euslisp/marker_test/parameters.l b/jsk_2021_fix_kxr/euslisp/marker_test/parameters.l deleted file mode 100644 index 0643ab0236..0000000000 --- a/jsk_2021_fix_kxr/euslisp/marker_test/parameters.l +++ /dev/null @@ -1,5 +0,0 @@ -(setq *marker-to-grasp-pose* - (make-cascoords - :pos #f(12.503 -38.046 48.706) - :rpy #f(2.517 1.299 -2.2) - )) From 211cef024d547f8634beb40a87223ba021e9bf91 Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 8 Jul 2022 17:05:06 +0900 Subject: [PATCH 122/127] [euslisp/move_test] delete amabe_screw_memo.xtx --- jsk_2021_fix_kxr/euslisp/move_test/amabe_screw_memo.txt | 3 --- 1 file changed, 3 deletions(-) delete mode 100644 jsk_2021_fix_kxr/euslisp/move_test/amabe_screw_memo.txt diff --git a/jsk_2021_fix_kxr/euslisp/move_test/amabe_screw_memo.txt b/jsk_2021_fix_kxr/euslisp/move_test/amabe_screw_memo.txt deleted file mode 100644 index 352ec431de..0000000000 --- a/jsk_2021_fix_kxr/euslisp/move_test/amabe_screw_memo.txt +++ /dev/null @@ -1,3 +0,0 @@ -#締める前 -r (422.782 -14.541 1246.738 / 0.456 1.485 -2.67) -l (425.117 53.404 1136.875 / -1.63 0.492 -3.124) From e0dba2ff2d5d29a5cede7a047a20414b7885d856 Mon Sep 17 00:00:00 2001 From: softyanija Date: Mon, 11 Jul 2022 16:05:07 +0900 Subject: [PATCH 123/127] [launch/apriltag_doctor_panda_left&right] fix path to config files --- jsk_2021_fix_kxr/launch/apriltag_doctor_panda_left.launch | 4 ++-- jsk_2021_fix_kxr/launch/apriltag_doctor_panda_right.launch | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_left.launch b/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_left.launch index 02a158bca3..49fa49cb3b 100644 --- a/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_left.launch +++ b/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_left.launch @@ -6,8 +6,8 @@ - - + + diff --git a/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_right.launch b/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_right.launch index 568005421c..21b97cb6af 100644 --- a/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_right.launch +++ b/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_right.launch @@ -6,8 +6,8 @@ - - + + From 2f2a17fa8368d7e8e9250edeb231ad92bc7a15c3 Mon Sep 17 00:00:00 2001 From: softyanija Date: Fri, 22 Jul 2022 11:53:14 +0900 Subject: [PATCH 124/127] Writing README --- jsk_2021_fix_kxr/README.md | 33 ++++++++++++++++++++++++++++++++- 1 file changed, 32 insertions(+), 1 deletion(-) diff --git a/jsk_2021_fix_kxr/README.md b/jsk_2021_fix_kxr/README.md index 744d7552e8..0e6247e669 100644 --- a/jsk_2021_fix_kxr/README.md +++ b/jsk_2021_fix_kxr/README.md @@ -1,3 +1,34 @@ # jsk_2021_fix_kxr +Fix kxr whose connector was removal by dual_panda2. -## 仮置き \ No newline at end of file +## Installation +### Launching dual_panda2 +In leus@dual_panda2 +'''bash +roslaunch jsk_panda_startup dual_panda2.launch +''' +In your pc, launch detection nodes to recognize apriltag and terminal of connector +'''bash +rossetdualpanda2 +source ~/(ws)/devel/setup.bash +roslaunch jsk_2021_fix_kxr fix_kxr_detections.launch +''' +### Connecting of kxr +According to [here](), connect kxr to 133.x network through wifi. + +In the other turminal, kxr +'''bash + +''' + +## Execution +'''bash +rossetdualpanda2 +source ~/(ws)/devel/setup.bash +roseus jsk_2021_fix_kxr/euslisp/main/pand_main.l +''' + +## Video +See [here](https://drive.google.com/file/d/14vz8pw_OfTtmtCxj7IQx12pf1VkxYJls/view?usp=sharing) + +## \ No newline at end of file From 16e90923c0fe8ed1befb392f10731d51bc5b5143 Mon Sep 17 00:00:00 2001 From: Koki Amabe <48650394+softyanija@users.noreply.github.com> Date: Fri, 22 Jul 2022 13:09:30 +0900 Subject: [PATCH 125/127] update README --- jsk_2021_fix_kxr/README.md | 46 +++++++++++++++++++++++++------------- 1 file changed, 30 insertions(+), 16 deletions(-) diff --git a/jsk_2021_fix_kxr/README.md b/jsk_2021_fix_kxr/README.md index 0e6247e669..1da721c2c6 100644 --- a/jsk_2021_fix_kxr/README.md +++ b/jsk_2021_fix_kxr/README.md @@ -1,34 +1,48 @@ # jsk_2021_fix_kxr -Fix kxr whose connector was removal by dual_panda2. +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 through wifi +``` +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 -'''bash +In leus@dual_panda2, +``` roslaunch jsk_panda_startup dual_panda2.launch -''' +``` + In your pc, launch detection nodes to recognize apriltag and terminal of connector -'''bash +``` rossetdualpanda2 source ~/(ws)/devel/setup.bash roslaunch jsk_2021_fix_kxr fix_kxr_detections.launch -''' -### Connecting of kxr -According to [here](), connect kxr to 133.x network through wifi. +``` -In the other turminal, kxr -'''bash +## Execution +After setting KXR on workbench as shown in the figure below、start demo -''' + -## Execution -'''bash +``` rossetdualpanda2 source ~/(ws)/devel/setup.bash -roseus jsk_2021_fix_kxr/euslisp/main/pand_main.l -''' +roseus jsk_2021_fix_kxr/euslisp/main/panda_main.l +``` ## Video See [here](https://drive.google.com/file/d/14vz8pw_OfTtmtCxj7IQx12pf1VkxYJls/view?usp=sharing) -## \ No newline at end of file +## From 6b89714c6adc8bb7710b8a9ee21c3255fbe3985d Mon Sep 17 00:00:00 2001 From: Koki Amabe <48650394+softyanija@users.noreply.github.com> Date: Fri, 22 Jul 2022 13:12:53 +0900 Subject: [PATCH 126/127] fix expression --- jsk_2021_fix_kxr/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/jsk_2021_fix_kxr/README.md b/jsk_2021_fix_kxr/README.md index 1da721c2c6..b40cbbad50 100644 --- a/jsk_2021_fix_kxr/README.md +++ b/jsk_2021_fix_kxr/README.md @@ -6,7 +6,7 @@ Fix KXR whose connector was removal by dual_panda2. 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 through wifi +After setting up KXR, connet to your pc ``` cd ~/prog/rcb4eus/Arduino/M5Stack/M5Stack_rcb4_rosserial roslaunch rosserial_wifi.launch From 5677ef3b8d2b8efdb3860422bd73eecb4583e28f Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Thu, 24 Nov 2022 14:29:08 +0900 Subject: [PATCH 127/127] Change camera topic name (/*arm_vision/camera -> /*arm_camera) --- .../euslisp/show_grasp_param/show_grasp_param_left.l | 2 +- .../euslisp/show_grasp_param/show_grasp_param_right.l | 2 +- jsk_2021_fix_kxr/launch/apriltag_doctor_panda_left.launch | 4 ++-- jsk_2021_fix_kxr/launch/apriltag_doctor_panda_right.launch | 4 ++-- jsk_2021_fix_kxr/launch/cable_terminal_detection.launch | 4 ++-- jsk_2021_fix_kxr/launch/fix_kxr_detections.launch | 4 ++-- jsk_2021_fix_kxr/launch/show_grasp_param_left.launch | 6 +++--- jsk_2021_fix_kxr/launch/show_grasp_param_right.launch | 6 +++--- 8 files changed, 16 insertions(+), 16 deletions(-) diff --git a/jsk_2021_fix_kxr/euslisp/show_grasp_param/show_grasp_param_left.l b/jsk_2021_fix_kxr/euslisp/show_grasp_param/show_grasp_param_left.l index 5026d122d9..a486f0b023 100644 --- a/jsk_2021_fix_kxr/euslisp/show_grasp_param/show_grasp_param_left.l +++ b/jsk_2021_fix_kxr/euslisp/show_grasp_param/show_grasp_param_left.l @@ -5,7 +5,7 @@ (ros::roseus "show-grasp-param-left") -;; (ros::subscribe "/rarm_vision/camera/color/image_raw" image_view2::ImageMarker2 #'cb) +;; (ros::subscribe "/rarm_camera/color/image_raw" image_view2::ImageMarker2 #'cb) ;; (defun cb (msg) diff --git a/jsk_2021_fix_kxr/euslisp/show_grasp_param/show_grasp_param_right.l b/jsk_2021_fix_kxr/euslisp/show_grasp_param/show_grasp_param_right.l index f7db7fd12f..db9d682564 100755 --- a/jsk_2021_fix_kxr/euslisp/show_grasp_param/show_grasp_param_right.l +++ b/jsk_2021_fix_kxr/euslisp/show_grasp_param/show_grasp_param_right.l @@ -5,7 +5,7 @@ (ros::roseus "show-grasp-param-right") -;; (ros::subscribe "/rarm_vision/camera/color/image_raw" image_view2::ImageMarker2 #'cb) +;; (ros::subscribe "/rarm_camera/color/image_raw" image_view2::ImageMarker2 #'cb) ;; (defun cb (msg) diff --git a/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_left.launch b/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_left.launch index 49fa49cb3b..c168d7ad13 100644 --- a/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_left.launch +++ b/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_left.launch @@ -1,7 +1,7 @@ - - + + diff --git a/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_right.launch b/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_right.launch index 21b97cb6af..91e7fffde1 100644 --- a/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_right.launch +++ b/jsk_2021_fix_kxr/launch/apriltag_doctor_panda_right.launch @@ -1,7 +1,7 @@ - - + + diff --git a/jsk_2021_fix_kxr/launch/cable_terminal_detection.launch b/jsk_2021_fix_kxr/launch/cable_terminal_detection.launch index c835ccc429..7358722074 100644 --- a/jsk_2021_fix_kxr/launch/cable_terminal_detection.launch +++ b/jsk_2021_fix_kxr/launch/cable_terminal_detection.launch @@ -4,7 +4,7 @@ - + @@ -21,7 +21,7 @@ - + diff --git a/jsk_2021_fix_kxr/launch/fix_kxr_detections.launch b/jsk_2021_fix_kxr/launch/fix_kxr_detections.launch index 516b7313e0..eb457e2ede 100644 --- a/jsk_2021_fix_kxr/launch/fix_kxr_detections.launch +++ b/jsk_2021_fix_kxr/launch/fix_kxr_detections.launch @@ -7,7 +7,7 @@ - + @@ -24,7 +24,7 @@ - + diff --git a/jsk_2021_fix_kxr/launch/show_grasp_param_left.launch b/jsk_2021_fix_kxr/launch/show_grasp_param_left.launch index bb0a7f43f5..4ae7c7534f 100644 --- a/jsk_2021_fix_kxr/launch/show_grasp_param_left.launch +++ b/jsk_2021_fix_kxr/launch/show_grasp_param_left.launch @@ -1,8 +1,8 @@ - - + + - + diff --git a/jsk_2021_fix_kxr/launch/show_grasp_param_right.launch b/jsk_2021_fix_kxr/launch/show_grasp_param_right.launch index 117df23a12..83ce0fb55e 100644 --- a/jsk_2021_fix_kxr/launch/show_grasp_param_right.launch +++ b/jsk_2021_fix_kxr/launch/show_grasp_param_right.launch @@ -1,8 +1,8 @@ - - + + - +