From f5881faaf2476bc46924f26a5f028efda35668c1 Mon Sep 17 00:00:00 2001 From: tongtybj Date: Sat, 3 Nov 2018 04:57:38 +0900 Subject: [PATCH] Fix the problem of not working function ":wait-interpolation" in uselisp by: update the constaints for joint trajectory controller, relaxing the path tolerance --- .../config/dxl_armed_turtlebot_controllers.yaml | 14 +++++++------- dxl_armed_turtlebot/euslisp/gazebo-grasp-test.l | 7 ++----- .../euslisp/gazebo-grasp-with-recognition.l | 5 ++--- 3 files changed, 11 insertions(+), 15 deletions(-) diff --git a/dxl_armed_turtlebot/config/dxl_armed_turtlebot_controllers.yaml b/dxl_armed_turtlebot/config/dxl_armed_turtlebot_controllers.yaml index dca22cdd..70310151 100644 --- 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 index 224088f6..9ec29ef9 100644 --- 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 index 0541999b..015cfd8d 100644 --- 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