Fix the problem of not working function ":wait-interpolation" in usel…

…isp by: update the constaints for joint trajectory controller, relaxing the path tolerance
jsk-enshu · Nov 2, 2018 · f5881fa · f5881fa
1 parent b36b65e
commit f5881fa
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 15 deletions.
diff --git a/dxl_armed_turtlebot/config/dxl_armed_turtlebot_controllers.yaml b/dxl_armed_turtlebot/config/dxl_armed_turtlebot_controllers.yaml
@@ -17,22 +17,22 @@ fullbody_controller:
     goal_time: 0.5                   # Override default
     stopped_velocity_tolerance: 0.02 # Override default
     arm_joint1:
-      trajectory: 0.05               # Not enforced if unspecified
+      #trajectory: 0.1               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
     arm_joint2:
-      trajectory: 0.05               # Not enforced if unspecified
+      #trajectory: 0.1               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
     arm_joint3:
-      trajectory: 0.05               # Not enforced if unspecified
+      #trajectory: 0.1               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
     arm_joint4:
-      trajectory: 0.05               # Not enforced if unspecified
+      #trajectory: 0.1               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
     arm_joint5:
-      trajectory: 0.05               # Not enforced if unspecified
+      #trajectory: 0.1               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
     arm_joint6:
-      trajectory: 0.05               # Not enforced if unspecified
+      #trajectory: 0.1               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
 
   gains: # Required because we're controlling an effort interface
@@ -55,7 +55,7 @@ gripper_controller:
     goal_time: 0.5                   # Override default
     stopped_velocity_tolerance: 0.02 # Override default
     gripper_joint:
-      trajectory: 0.05               # Not enforced if unspecified
+      #trajectory: 0.05               # Not enforced if unspecified
       goal: 0.02                     # Not enforced if unspecified
 
   gains: # Required because we're controlling an effort interface

diff --git a/dxl_armed_turtlebot/euslisp/gazebo-grasp-test.l b/dxl_armed_turtlebot/euslisp/gazebo-grasp-test.l
@@ -4,19 +4,16 @@
 
 ;; arm approach  pose
 (send *ri* :angle-vector #f(0 0 -90 0 90 0) 3000)
-;;(send *ri* :wait-interpolation) ;; does not work?
-(unix:sleep 4)
+(send *ri* :wait-interpolation)
 
 ;; open gripper
 (send *ri* :stop-grasp)
 
 ;; go-pos
 (send *ri* :go-pos 0.56 0 1)
-(unix:sleep 6)
 
 ;; grasp
 ;; still some bug in start-grasp, so we have to directly use :move-gripper
 (send *ri* :move-gripper -30 :tm 1500 :wait t)
-(unix:sleep 2)
-
+(send *ri* :wait-interpolation)
 (send *ri* :angle-vector #f(90 0 45 0 45 45)) ;;  arm tuck pose
diff --git a/dxl_armed_turtlebot/euslisp/gazebo-grasp-with-recognition.l b/dxl_armed_turtlebot/euslisp/gazebo-grasp-with-recognition.l
@@ -47,7 +47,6 @@
 (send *ri* :stop-grasp)
 
 
-
 (setq found-obj nil)
 (until found-obj
        (x::window-main-one) ;; IRT viewerの視点を変えられる。見にくければ変えよう
@@ -79,14 +78,14 @@
 (ros::ros-warn "the grap arm pose is ~A" (send *dxl-armed-turtlebot* :angle-vector))
 
 (send *ri* :angle-vector  (send *dxl-armed-turtlebot* :angle-vector) 3000)
+(send *ri* :wait-interpolation)
 
-(unix:sleep 5)
 (send *ri* :go-pos 0.1 0 0)
 
 ;; grasp
 ;; still some bug in start-grasp, so we have to directly use :move-gripper
 (send *ri* :move-gripper -30 :tm 1500 :wait t)
-(unix:sleep 2)
+(send *ri* :wait-interpolation)
 
 (send *ri* :angle-vector #f(80 0 45 0 45 45)) ;;  arm tuck pose
 (send *ri* :go-pos -0.6 0 0) ;;  arm tuck pose