From dd507874c260dcd8ee9800e1f8f94c4ab5e9ac42 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 21 Jun 2024 17:00:33 +0900 Subject: [PATCH] naoqi-interface.l: enable to pass string as joint-name (but not supported onQi side) --- jsk_naoqi_robot/naoqieus/naoqi-interface.l | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/jsk_naoqi_robot/naoqieus/naoqi-interface.l b/jsk_naoqi_robot/naoqieus/naoqi-interface.l index 433bbb43b5..3b01d86ce0 100644 --- a/jsk_naoqi_robot/naoqieus/naoqi-interface.l +++ b/jsk_naoqi_robot/naoqieus/naoqi-interface.l @@ -141,13 +141,17 @@ (setq joint-name (list "LHand"))) (:rhand (setq joint-name (list "RHand"))) + (t + (setq joint-name joint) + (if (/= (length joint-name) (length stiffness)) + (ros::ros-warn "length for 'joint' and 'sitffness' must be same"))) ) (send goal :goal :trajectory :joint_names joint-name) (send goal :goal :trajectory :header :stamp (ros::time-now)) (send goal :goal :trajectory :points (list (instance trajectory_msgs::JointTrajectoryPoint :init - :positions (fill (instantiate float-vector (length joint-name)) stiffness) + :positions (if (numberp stiffness) (fill (instantiate float-vector (length joint-name)) stiffness) stiffness) :time_from_start (ros::time 1)))) (send joint-stiffness-trajectory-action :send-goal goal) ))