From b4c596a4a7d56c06d189a5c87f9e4f9f29c481b6 Mon Sep 17 00:00:00 2001 From: Hiroto Suzuki Date: Thu, 8 Dec 2016 20:33:50 +0900 Subject: [PATCH] fix gripper-controller --- .../euslisp/dxl-7dof-arm-interface-common.l | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface-common.l b/dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface-common.l index b624f4e4..a3daf93b 100644 --- a/dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface-common.l +++ b/dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface-common.l @@ -100,8 +100,8 @@ ) (:default-controller () - (send self :fullbody-controller) - ;;(append (send self :fullbody-controller) (send self :gripper-controller)) + ;;(send self :fullbody-controller) + (append (send self :fullbody-controller) (send self :gripper-controller)) ) ;; raw dynamixel command ;; TODO : define these methods by considering pr2eus? @@ -166,8 +166,9 @@ (:start-grasp (&optional (arm :arm) &key ((:gain g) 0.5) ((:objects objs) objects)) "Start grasp mode." - (send self :set-compliance-slope 7 1023) - (send self :set-torque-limit 7 g) + (unless (send self :simulation-modep) + (send self :set-compliance-slope 7 1023) + (send self :set-torque-limit 7 g)) (send robot :gripper arm :angle-vector (send-all (send robot :gripper arm :joint-list) :min-angle)) (send self :angle-vector (send robot :angle-vector) 1000 :gripper-controller) @@ -185,8 +186,9 @@ (send robot :gripper arm :angle-vector (send-all (send robot :gripper arm :joint-list) :max-angle)) (send self :angle-vector (send robot :angle-vector) 1000 :gripper-controller) - (send self :set-compliance-slope 7 32) - (send self :set-torque-limit 7 1.0) + (unless (send self :simulation-modep) + (send self :set-compliance-slope 7 32) + (send self :set-torque-limit 7 1.0)) (send self :wait-interpolation :gripper-controller) ) )