fix bugs in :speed calculation method

jsk-ros-pkg · Jun 27, 2024 · d010442 · d010442
1 parent d6923e8
commit d010442
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/jsk_naoqi_robot/naoqieus/naoqi-interface.l b/jsk_naoqi_robot/naoqieus/naoqi-interface.l
@@ -180,8 +180,8 @@
      (send msg :joint_angles positions)
      ;; fastest-tm is the distance to target
      (setq speed (+ 1.0 (* (/ -1.0 200) (min 180 fastest-tm))))
-     (if tm (setq speed (* (/ tm 1000.0) speed)))
-     (send msg :speed speed)
+     (if tm (setq speed (* (/ 1000.0 tm) speed)))
+     (send msg :speed (min 1.0 speed))
      (ros::ros-warn ":send-joint-angles-with-speed tm:~A, fastest:~A, speed:~A" tm fastest-tm speed)
      (ros::publish "/joint_angles" msg)
    ))