diff --git a/demos/baxter_paper_filing/euslisp/baxterlgv8-sensor-eval.l b/demos/baxter_paper_filing/euslisp/baxterlgv8-sensor-eval.l index b0efe3543..c10440193 100644 --- a/demos/baxter_paper_filing/euslisp/baxterlgv8-sensor-eval.l +++ b/demos/baxter_paper_filing/euslisp/baxterlgv8-sensor-eval.l @@ -73,9 +73,9 @@ (send *ri* :wait-interpolation) (unix::sleep 1) (send *ri* :move-hand :larm - (send *baxter* :hand :larm :angle-vector (float-vector 0 72 72)) 1500) + (send *baxter* :hand :larm :angle-vector (float-vector 0 72 72)) 1000) (send *ri* :move-hand :larm - (send *baxter* :hand :larm :angle-vector (float-vector 0 90 90)) 1500 :wait nil) + (send *baxter* :hand :larm :angle-vector (float-vector 0 90 90)) 1000 :wait nil) (let (cancel-func stopped-p l-prox r-prox l-thre r-thre l-refl r-refl) (if intensity (progn diff --git a/demos/sphand_ros/sphand_driver/euslisp/baxterlgv8-gv8-movement.l b/demos/sphand_ros/sphand_driver/euslisp/baxterlgv8-gv8-movement.l index 9a13e115d..1d30f5200 100644 --- a/demos/sphand_ros/sphand_driver/euslisp/baxterlgv8-gv8-movement.l +++ b/demos/sphand_ros/sphand_driver/euslisp/baxterlgv8-gv8-movement.l @@ -11,9 +11,9 @@ (&key (type :lgripper-controller)) (sphand_driver::baxterlgv8-init :type type) (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1500) + (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1000) (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1500) + (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1000) (send *baxter* :rotate-gripper *arm* 0 :relative nil) (send *baxter* :slide-gripper *arm* 0 :relative nil) (send *ri* :angle-vector (send *baxter* :angle-vector)) @@ -25,33 +25,33 @@ (when move-fingers ;; test of abduction/adduction (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1500) + (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1000) (unix::sleep 2) (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1500) + (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1000) (unix::sleep 2) (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pose *arm* :opposed :angle 0) 1500) + (send *baxter* :hand-grasp-pose *arm* :opposed :angle 0) 1000) (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pose *arm* :cylindrical :angle 0) 1500) + (send *baxter* :hand-grasp-pose *arm* :cylindrical :angle 0) 1000) (unix::sleep 2) (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pose *arm* :opposed :angle 0) 1500) + (send *baxter* :hand-grasp-pose *arm* :opposed :angle 0) 1000) (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1500) + (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1000) (unix::sleep 2) ;; test of closing fingers (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pose *arm* :opposed :angle 80) 1500) + (send *baxter* :hand-grasp-pose *arm* :opposed :angle 80) 1000) (unix::sleep 1) (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1500) + (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1000) (unix::sleep 1) (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pose *arm* :opposed :angle 100) 1500) + (send *baxter* :hand-grasp-pose *arm* :opposed :angle 100) 1000) (unix::sleep 1) (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1500)) + (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1000)) ;; test of prismatic joint (send *baxter* :slide-gripper *arm* 206 :relative nil) (send *ri* :angle-vector (send *baxter* :angle-vector) :fast nil 0) diff --git a/demos/sphand_ros/sphand_driver/euslisp/lib/baxterlgv8-interface.l b/demos/sphand_ros/sphand_driver/euslisp/lib/baxterlgv8-interface.l index 02a63bac5..e2c2f9b54 100644 --- a/demos/sphand_ros/sphand_driver/euslisp/lib/baxterlgv8-interface.l +++ b/demos/sphand_ros/sphand_driver/euslisp/lib/baxterlgv8-interface.l @@ -250,7 +250,7 @@ (list (instance trajectory_msgs::JointTrajectoryPoint :init :positions (mapcar #'deg2rad av-list) - :time_from_start (ros::time (/ tm 1000))))) + :time_from_start (ros::time (/ tm 1000.0))))) (send goal :goal :trajectory traj) (send (gethash arm hand-actions-) :send-goal goal) (setq res (send (gethash arm hand-actions-) :get-result)) diff --git a/jsk_2016_01_baxter_apc/euslisp/examples/right-gripper-v5/fixed-picking.l b/jsk_2016_01_baxter_apc/euslisp/examples/right-gripper-v5/fixed-picking.l index 413e142c0..150b399a2 100644 --- a/jsk_2016_01_baxter_apc/euslisp/examples/right-gripper-v5/fixed-picking.l +++ b/jsk_2016_01_baxter_apc/euslisp/examples/right-gripper-v5/fixed-picking.l @@ -34,7 +34,7 @@ (send *baxter* :rotate-gripper *arm* 0 :relative nil) 1500) (send *ri* :move-hand *arm* - (send *baxter* :hand *arm* :angle-vector) 1500) + (send *baxter* :hand *arm* :angle-vector) 1000) (send *ri* :spin-off-by-wrist *arm* :times 20) (send *ri* :gripper-servo-on *arm*) (send *ri* :wait-interpolation) @@ -43,7 +43,7 @@ (send *ri* :wait-interpolation) (send *baxter* :hand-grasp-pre-pose *arm* :opposed) (send *ri* :move-hand *arm* - (send *baxter* :hand *arm* :angle-vector) 1500) + (send *baxter* :hand *arm* :angle-vector) 1000) ) @@ -55,7 +55,7 @@ (send *baxter* :hand-grasp-pre-pose *arm* :opposed) (send *ri* :move-hand *arm* - (send *baxter* :hand *arm* :angle-vector) 1500) + (send *baxter* :hand *arm* :angle-vector) 1000) (unix::sleep 3) (send *ri* :move-arm-body->bin *arm* *bin*) (send *ri* :wait-interpolation) @@ -77,7 +77,7 @@ (send *ri* :wait-interpolation) (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) (send *ri* :move-hand *arm* - (send *baxter* :hand *arm* :angle-vector) 1500) + (send *baxter* :hand *arm* :angle-vector) 1000) ;; approach to object (pushback (send *baxter* *arm* :move-end-pos #f(50 0 0) :world) avs) (pushback (send *baxter* *arm* :move-end-pos #f(50 0 0) :world) avs) @@ -105,7 +105,7 @@ (send *ri* :wait-interpolation) (send *baxter* :hand-grasp-pose *arm* :cylindrical) (send *ri* :move-hand *arm* - (send *baxter* :hand *arm* :angle-vector) 1500) + (send *baxter* :hand *arm* :angle-vector) 1000) ;; draw out object (pushback (send *baxter* *arm* :move-end-pos #f(0 0 50) :world) avs) (pushback (send *baxter* *arm* :move-end-pos #f(0 0 50) :world) avs) @@ -137,7 +137,7 @@ (send *baxter* :hand-grasp-pre-pose *arm* :opposed) (send *ri* :move-hand *arm* - (send *baxter* :hand *arm* :angle-vector) 1500) + (send *baxter* :hand *arm* :angle-vector) 1000) (unix::sleep 3) (send *ri* :move-arm-body->bin *arm* *bin*) (send *ri* :wait-interpolation) @@ -181,7 +181,7 @@ ;; grasp object by hand (send *baxter* :hand-grasp-pose *arm* :opposed) (send *ri* :move-hand *arm* - (send *baxter* :hand *arm* :angle-vector) 1500) + (send *baxter* :hand *arm* :angle-vector) 1000) ;; draw out object (pushback (send *baxter* *arm* :move-end-pos #f(0 0 50) :world) avs) (pushback (send *baxter* *arm* :move-end-pos #f(0 0 10) :world) avs) @@ -217,7 +217,7 @@ (send *baxter* :hand-grasp-pre-pose *arm* :opposed) (send *ri* :move-hand *arm* - (send *baxter* :hand *arm* :angle-vector) 1500) + (send *baxter* :hand *arm* :angle-vector) 1000) (unix::sleep 3) (send *ri* :move-arm-body->bin *arm* *bin*) (send *ri* :wait-interpolation) @@ -261,7 +261,7 @@ ;; grasp object by hand (send *baxter* :hand-grasp-pose *arm* :opposed) (send *ri* :move-hand *arm* - (send *baxter* :hand *arm* :angle-vector) 1500) + (send *baxter* :hand *arm* :angle-vector) 1000) ;; draw out object (pushback (send *baxter* *arm* :move-end-pos #f(0 0 50) :world) avs) (pushback (send *baxter* *arm* :move-end-pos #f(0 0 10) :world) avs) @@ -297,7 +297,7 @@ (send *baxter* :hand-grasp-pre-pose *arm* :opposed) (send *ri* :move-hand *arm* - (send *baxter* :hand *arm* :angle-vector) 1500) + (send *baxter* :hand *arm* :angle-vector) 1000) (unix::sleep 3) (send *ri* :move-arm-body->bin *arm* *bin*) (send *ri* :wait-interpolation) @@ -331,7 +331,7 @@ ;; prepare fingers (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) (send *ri* :move-hand *arm* - (send *baxter* :hand *arm* :angle-vector) 1500) + (send *baxter* :hand *arm* :angle-vector) 1000) (unix::sleep 3) ;; approach and drag object by sliding gripper (pushback (send *baxter* *arm* :move-end-pos #f(0 40 0) :world) avs) @@ -344,7 +344,7 @@ ;; grasp object by hand (send *baxter* :hand-grasp-pose *arm* :cylindrical) (send *ri* :move-hand *arm* - (send *baxter* :hand *arm* :angle-vector) 1500) + (send *baxter* :hand *arm* :angle-vector) 1000) (unix::sleep 3) ;; draw out object ;(pushback (send *baxter* *arm* :move-end-pos #f(0 0 50) :world) avs) diff --git a/jsk_2016_01_baxter_apc/euslisp/examples/right-gripper-v5/picking-with-sib.l b/jsk_2016_01_baxter_apc/euslisp/examples/right-gripper-v5/picking-with-sib.l index 824803c55..da87520ae 100755 --- a/jsk_2016_01_baxter_apc/euslisp/examples/right-gripper-v5/picking-with-sib.l +++ b/jsk_2016_01_baxter_apc/euslisp/examples/right-gripper-v5/picking-with-sib.l @@ -26,7 +26,7 @@ (progn ;; gripper-v5 (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pre-pose *arm* (send *ri* :get-val '_grasp-style)) 1500) + (send *baxter* :hand-grasp-pre-pose *arm* (send *ri* :get-val '_grasp-style)) 1000) (send *baxter* :slide-gripper *arm* 100 :relative nil) (send *ri* :send-av 1000) (send *ri* :wait-interpolation) @@ -34,7 +34,7 @@ (send *ri* :send-av 1000) (send *ri* :wait-interpolation) (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1500) + (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1000) (send *ri* :spin-off-by-wrist *arm* :times 5) ) (send *ri* :spin-off-by-wrist arm :times 20) diff --git a/jsk_2016_01_baxter_apc/euslisp/examples/right-gripper-v5/simple-grasping.l b/jsk_2016_01_baxter_apc/euslisp/examples/right-gripper-v5/simple-grasping.l index 44707441a..2b0920293 100644 --- a/jsk_2016_01_baxter_apc/euslisp/examples/right-gripper-v5/simple-grasping.l +++ b/jsk_2016_01_baxter_apc/euslisp/examples/right-gripper-v5/simple-grasping.l @@ -13,7 +13,7 @@ (send *baxter* :slide-gripper *arm* 0 :relative nil) (send *baxter* :rotate-gripper *arm* 0 :relative nil) (send *ri* :angle-vector (send *baxter* :angle-vector) 1500) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1000) (send *ri* :wait-interpolation) ) @@ -36,9 +36,9 @@ (unless (wait-user "Start grasp?") (return-from opposed-with-suction nil)) (send *ri* :start-grasp *arm*) (unix::sleep 3) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :opposed) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :opposed) 1000) (unless (wait-user "Release object?") (return-from opposed-with-suction nil)) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1000) (send *ri* :stop-grasp *arm*) ) @@ -47,9 +47,9 @@ (send *ri* :angle-vector (send *baxter* :rotate-gripper *arm* 90 :relative nil) 1500) (send *ri* :wait-interpolation) (unless (wait-user "Start grasp?") (return-from opposed-without-suction nil)) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :opposed) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :opposed) 1000) (unless (wait-user "Release object?") (return-from opposed-without-suction nil)) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1000) ) (defun spherical-with-suction () @@ -57,13 +57,13 @@ (send *baxter* :slide-gripper *arm* 0 :relative nil) (send *baxter* :rotate-gripper *arm* -45 :relative nil) (send *ri* :angle-vector (send *baxter* :angle-vector) 1500) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :spherical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :spherical) 1000) (send *ri* :wait-interpolation) (send *ri* :start-grasp *arm*) (unless (wait-user "Start grasp?") (return-from spherical-with-suction nil)) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :spherical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :spherical) 1000) (unless (wait-user "Release object?") (return-from spherical-with-suction nil)) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :spherical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :spherical) 1000) (send *ri* :stop-grasp *arm*) ) @@ -72,12 +72,12 @@ (send *baxter* :slide-gripper *arm* 60 :relative nil) (send *baxter* :rotate-gripper *arm* -45 :relative nil) (send *ri* :angle-vector (send *baxter* :angle-vector) 1500) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :spherical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :spherical) 1000) (send *ri* :wait-interpolation) (unless (wait-user "Start grasp?") (return-from spherical-without-suction nil)) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :spherical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :spherical) 1000) (unless (wait-user "Release object?") (return-from spherical-without-suction nil)) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :spherical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :spherical) 1000) ) (defun cylindrical-with-suction @@ -93,14 +93,14 @@ (send *baxter* :rotate-gripper *arm* -45 :relative nil) (send *ri* :angle-vector (send *baxter* :angle-vector) 1500)) (t (error "Don't know ~A~%" object))) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1000) (send *ri* :wait-interpolation) (unless (wait-user "Start sucking?") (return-from cylindrical-with-suction nil)) (send *ri* :start-grasp *arm*) (unless (wait-user "Start grasp?") (return-from cylindrical-with-suction nil)) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :cylindrical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :cylindrical) 1000) (unless (wait-user "Release object?") (return-from cylindrical-with-suction nil)) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1000) (send *ri* :stop-grasp *arm*) ) @@ -112,7 +112,7 @@ (send *baxter* :slide-gripper *arm* 80 :relative nil) (send *baxter* :rotate-gripper *arm* 0 :relative nil) (send *ri* :angle-vector (send *baxter* :angle-vector) 1500) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1000) (send *ri* :wait-interpolation) (unless (wait-user "Start grasp?") (return-from cylindrical-without-suction nil)) (send *baxter* :rotate-gripper *arm* angle :relative nil) @@ -120,13 +120,13 @@ (send *ri* :wait-interpolation) (send *baxter* :slide-gripper *arm* 0 :relative nil) (send *ri* :angle-vector (send *baxter* :angle-vector) 1500) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :cylindrical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :cylindrical) 1000) (send *ri* :wait-interpolation) (unless (wait-user "Release object?") (return-from cylindrical-without-suction nil)) (send *baxter* :rotate-gripper *arm* 0 :relative nil) (send *baxter* :slide-gripper *arm* 80 :relative nil) (send *ri* :angle-vector (send *baxter* :angle-vector) 1500) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1000) (send *ri* :wait-interpolation) ) (t (error "Don't know ~A~%" object))) @@ -139,7 +139,7 @@ -72.1804 -37.2978 106.601 -126.204 -54.2365 -91.7015 121.418 -45.0) ) (send *ri* :angle-vector (send *baxter* :angle-vector av-before-approach) 10000) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1000) (send *ri* :wait-interpolation) (send *ri* :angle-vector (send *baxter* *arm* :move-end-pos (float-vector 0 0 (+ -150 object-h))) @@ -156,14 +156,14 @@ (send *ri* :angle-vector (send *baxter* :angle-vector) 5000) (send *ri* :wait-interpolation) (unix::sleep 1) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :cylindrical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :cylindrical) 1000) (unix::sleep 1) (send *ri* :spin-off-by-wrist *arm* :times 5) (send *ri* :wait-interpolation) (unless (wait-user "Release object?") (return-from pick-flat-object-on-table nil)) (send *ri* :stop-grasp *arm*) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1500) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1000) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1000) ) (defun pick-tall-object-on-table (&key (z-offset 0)) @@ -172,14 +172,14 @@ -89.7188 44.2261 54.6332 75.6437 -173.833 -43.4475 23.4764 3.75518 0.0) ) (send *ri* :angle-vector (send *baxter* :angle-vector av-before-approach) 10000) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1000) (send *ri* :wait-interpolation) (send *ri* :angle-vector (send *baxter* *arm* :move-end-pos (float-vector 50 0 z-offset) :world) 3000) (send *ri* :start-grasp *arm*) (send *ri* :wait-interpolation) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :opposed) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :opposed) 1000) (unix::sleep 1) (send *ri* :angle-vector (send *baxter* *arm* :move-end-pos (float-vector 0 0 300) :world) 6000) (send *ri* :wait-interpolation) @@ -188,7 +188,7 @@ (send *ri* :wait-interpolation) (unless (wait-user "Release object?") (return-from pick-tall-object-on-table nil)) (send *ri* :stop-grasp *arm*) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1000) ) (defun pick-thick-object-on-table (&key (z-offset 0)) @@ -197,13 +197,13 @@ -94.856 41.5942 51.4819 77.8052 -174.661 -45.2637 -156.819 0.0 90.0) ) (send *ri* :angle-vector (send *baxter* :angle-vector av-before-approach) 10000) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1000) (send *ri* :wait-interpolation) (send *ri* :angle-vector (send *baxter* *arm* :move-end-pos (float-vector 50 0 z-offset) :world) 3000) (send *ri* :wait-interpolation) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :opposed) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :opposed) 1000) (unix::sleep 1) (send *ri* :angle-vector (send *baxter* *arm* :move-end-pos (float-vector 0 0 300) :world) 6000) (send *ri* :wait-interpolation) @@ -212,7 +212,7 @@ (send *ri* :wait-interpolation) (unless (wait-user "Release object?") (return-from pick-thick-object-on-table nil)) (send *ri* :stop-grasp *arm*) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1000) ) (defun pick-dumbbell-on-table (&key slide-l) @@ -226,12 +226,12 @@ ) (send *baxter* :angle-vector av-before-approach) (send *ri* :angle-vector (send *baxter* *arm* :move-end-pos (float-vector 0 0 100) :world) 10000) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1000) (send *ri* :wait-interpolation) (send *ri* :angle-vector (send *baxter* :angle-vector av-before-approach) 10000) (send *ri* :wait-interpolation) (send *ri* :angle-vector (send *baxter* :rotate-gripper *arm* -90 :relative nil) 2000) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :cylindrical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pose *arm* :cylindrical) 1000) (send *ri* :wait-interpolation) (if slide-l (send *ri* :angle-vector (send *baxter* :slide-gripper *arm* slide-l :relative nil) 3000)) @@ -247,7 +247,7 @@ (send *ri* :spin-off-by-wrist *arm* :times 5) (send *ri* :wait-interpolation) (unless (wait-user "Release object?") (return-from pick-dumbbell-on-table nil)) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1000) ) (defun pick-flat-object-on-table-by-drawing () (setq av-before-approach @@ -255,7 +255,7 @@ -97.4991 21.8628 40.6055 81.2109 -159.873 -62.7759 -158.489 120.0 -90.0) ) (send *ri* :angle-vector (send *baxter* :angle-vector av-before-approach) 10000) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1000) (send *ri* :wait-interpolation) (send *ri* :angle-vector (send *baxter* *arm* :move-end-pos (float-vector 0 0 -120) :world) @@ -277,5 +277,5 @@ (send *ri* :wait-interpolation) (unless (wait-user "Release object?") (return-from pick-thick-object-on-table nil)) (send *ri* :stop-grasp *arm*) - (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1500) + (send *ri* :move-hand *arm* (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1000) ) diff --git a/jsk_2016_01_baxter_apc/euslisp/examples/right-gripper-v5/test-actuators.l b/jsk_2016_01_baxter_apc/euslisp/examples/right-gripper-v5/test-actuators.l index 3280722a3..50bb75a54 100644 --- a/jsk_2016_01_baxter_apc/euslisp/examples/right-gripper-v5/test-actuators.l +++ b/jsk_2016_01_baxter_apc/euslisp/examples/right-gripper-v5/test-actuators.l @@ -11,9 +11,9 @@ (&key (ctype :rgripper-controller)) (jsk_2016_01_baxter_apc::baxterrgv5-init :ctype ctype) (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1500) + (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1000) (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1500) + (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1000) (send *baxter* :rotate-gripper *arm* 0 :relative nil) (send *baxter* :slide-gripper *arm* 0 :relative nil) (send *ri* :angle-vector (send *baxter* :angle-vector)) @@ -24,23 +24,23 @@ (defun solidity-main () ;; test of abduction/adduction (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1500) + (send *baxter* :hand-grasp-pre-pose *arm* :cylindrical) 1000) (unix::sleep 2) (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1500) + (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1000) (unix::sleep 2) ;; test of closing fingers (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pose *arm* :opposed :angle 130) 1500) + (send *baxter* :hand-grasp-pose *arm* :opposed :angle 130) 1000) (unix::sleep 1) (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1500) + (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1000) (unix::sleep 1) (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pose *arm* :opposed :angle 130) 1500) + (send *baxter* :hand-grasp-pose *arm* :opposed :angle 130) 1000) (unix::sleep 1) (send *ri* :move-hand *arm* - (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1500) + (send *baxter* :hand-grasp-pre-pose *arm* :opposed) 1000) ;; test of prismatic joint (send *baxter* :slide-gripper *arm* 100 :relative nil) (send *ri* :angle-vector (send *baxter* :angle-vector) :fast nil 0) diff --git a/jsk_2016_01_baxter_apc/euslisp/lib/baxterrgv5-interface.l b/jsk_2016_01_baxter_apc/euslisp/lib/baxterrgv5-interface.l index 1cc7868a0..3be7d2acf 100644 --- a/jsk_2016_01_baxter_apc/euslisp/lib/baxterrgv5-interface.l +++ b/jsk_2016_01_baxter_apc/euslisp/lib/baxterrgv5-interface.l @@ -182,7 +182,7 @@ (list (instance trajectory_msgs::JointTrajectoryPoint :init :positions (mapcar #'deg2rad av-list) - :time_from_start (ros::time (/ tm 1000))))) + :time_from_start (ros::time (/ tm 1000.0))))) (send goal :goal :trajectory traj) (send _right-hand-action :send-goal goal) (setq res (send _right-hand-action :get-result)) @@ -976,7 +976,7 @@ (send *baxter* :hand-grasp-pose arm _grasp-style :angle 120)) ) (send self :move-hand arm - (send *baxter* :hand arm :angle-vector) 1500) + (send *baxter* :hand arm :angle-vector) 1000) ;; move in front of bin entrance (send self :angle-vector-sequence avs-before-approach :fast nil 0 :scale 5.0) @@ -1413,10 +1413,10 @@ ;; open fingers (send *baxter* :hand-grasp-pre-pose arm :cylindrical) (send self :move-hand arm - (send *baxter* :hand arm :angle-vector) 1500) + (send *baxter* :hand arm :angle-vector) 1000) (send *baxter* :hand-grasp-pre-pose arm :opposed) (send self :move-hand arm - (send *baxter* :hand arm :angle-vector) 1500) + (send *baxter* :hand arm :angle-vector) 1000) ;; initialize _last-av-before-approach (setq _last-av-before-approach (send *baxter* :angle-vector)) ;; initialize _grasp-style @@ -1487,7 +1487,7 @@ (send *baxter* :hand-grasp-pre-pose arm :opposed) (send *baxter* :hand-grasp-pre-pose arm _grasp-style)) (send self :move-hand arm - (send *baxter* :hand arm :angle-vector) 1500) + (send *baxter* :hand arm :angle-vector) 1000) (send self :wait-interpolation) (unix::sleep 1) ;; second grasp @@ -1507,7 +1507,7 @@ ((eq _grasp-style :cylindrical) (send *baxter* :hand-grasp-pose arm _grasp-style) (send self :move-hand arm - (send *baxter* :hand arm :angle-vector) 1500) + (send *baxter* :hand arm :angle-vector) 1000) (let (avs) (if (send *baxter* arm :move-end-pos #f(0 0 50) :world) (pushback (send *baxter* :angle-vector) avs)) @@ -1535,7 +1535,7 @@ (send self :wait-interpolation) (send *baxter* :hand-grasp-pose arm _grasp-style) (send self :move-hand arm - (send *baxter* :hand arm :angle-vector) 1500) + (send *baxter* :hand arm :angle-vector) 1000) (if (send *baxter* arm :move-end-pos #f(0 0 50) :world) (send self :angle-vector-sequence (list (send *baxter* :angle-vector)) @@ -1953,10 +1953,10 @@ ;; fold fingers to avoid collision to shelf (send *baxter* :hand-grasp-pre-pose arm :cylindrical) (send self :move-hand arm - (send *baxter* :hand arm :angle-vector) 1500) + (send *baxter* :hand arm :angle-vector) 1000) (send *baxter* :hand-grasp-pose arm :cylindrical) (send self :move-hand arm - (send *baxter* :hand arm :angle-vector) 1500) + (send *baxter* :hand arm :angle-vector) 1000) ) (let (avs) (pushback (send *baxter* :avoid-shelf-pose arm bin) avs) diff --git a/jsk_2016_01_baxter_apc/euslisp/main-rgv5.l b/jsk_2016_01_baxter_apc/euslisp/main-rgv5.l index 691b87edf..c7f562fa7 100755 --- a/jsk_2016_01_baxter_apc/euslisp/main-rgv5.l +++ b/jsk_2016_01_baxter_apc/euslisp/main-rgv5.l @@ -12,9 +12,9 @@ (jsk_2016_01_baxter_apc::baxterrgv5-init :ctype ctype) (send *ri* :gripper-servo-on) (send *ri* :move-hand :rarm - (send *baxter* :hand-grasp-pre-pose :rarm :cylindrical) 1500) + (send *baxter* :hand-grasp-pre-pose :rarm :cylindrical) 1000) (send *ri* :move-hand :rarm - (send *baxter* :hand-grasp-pre-pose :rarm :opposed) 1500) + (send *baxter* :hand-grasp-pre-pose :rarm :opposed) 1000) (send *baxter* :fold-pose-back) ;; view kiva pose (send *baxter* :larm :shoulder-y :joint-angle 70) @@ -195,9 +195,9 @@ (send *ri* :wait-interpolation) ;; open fingers (send *ri* :move-hand arm - (send *baxter* :hand-grasp-pre-pose arm :cylindrical) 1500) + (send *baxter* :hand-grasp-pre-pose arm :cylindrical) 1000) (send *ri* :move-hand arm - (send *baxter* :hand-grasp-pre-pose arm :opposed) 1500) + (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000) (send *ri* :angle-vector-sequence (list (send *baxter* :fold-pose-back arm)) :fast nil 0 :scale 5.0) @@ -284,7 +284,7 @@ ;; gripper-v5 (send *ri* :move-hand arm (send *baxter* :hand-grasp-pre-pose arm (send *ri* :get-val '_grasp-style)) - 1500) + 1000) (send *baxter* :slide-gripper arm 100 :relative nil) (send *ri* :send-av 1000) (send *ri* :wait-interpolation) @@ -292,7 +292,7 @@ (send *ri* :send-av 1000) (send *ri* :wait-interpolation) (send *ri* :move-hand arm - (send *baxter* :hand-grasp-pre-pose arm :opposed) 1500) + (send *baxter* :hand-grasp-pre-pose arm :opposed) 1000) (send *ri* :spin-off-by-wrist arm :times 5) ) (send *ri* :spin-off-by-wrist arm :times 20) diff --git a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l index d747a76cc..eee2917c9 100644 --- a/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/baxter-interface.l @@ -228,7 +228,7 @@ (list (instance trajectory_msgs::JointTrajectoryPoint :init :positions (mapcar #'deg2rad av-list) - :time_from_start (ros::time (/ tm 1000))))) + :time_from_start (ros::time (/ tm 1000.0))))) (send goal :goal :trajectory traj) (send (gethash arm hand-actions-) :send-goal goal) (setq res (send (gethash arm hand-actions-) :get-result)) diff --git a/jsk_arc2017_baxter/euslisp/lib/baxterlgv7-interface.l b/jsk_arc2017_baxter/euslisp/lib/baxterlgv7-interface.l index 4ee7e0649..2bf240d70 100644 --- a/jsk_arc2017_baxter/euslisp/lib/baxterlgv7-interface.l +++ b/jsk_arc2017_baxter/euslisp/lib/baxterlgv7-interface.l @@ -245,7 +245,7 @@ (list (instance trajectory_msgs::JointTrajectoryPoint :init :positions (mapcar #'deg2rad av-list) - :time_from_start (ros::time (/ tm 1000))))) + :time_from_start (ros::time (/ tm 1000.0))))) (send goal :goal :trajectory traj) (send (gethash arm hand-actions-) :send-goal goal) (setq res (send (gethash arm hand-actions-) :get-result))