naoqi-interface.l: enable to pass string as joint-name (but not suppo…

…rted onQi side)
jsk-ros-pkg · Jun 21, 2024 · dd50787 · dd50787
1 parent 25dd087
commit dd50787
Showing 1 changed file with 5 additions and 1 deletion.
diff --git 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)
      ))