From aa3a91e4cc33ec6cfea5f0ba07353e2f54275c00 Mon Sep 17 00:00:00 2001
From: Taiki Endo <taiki@smilerobotics.com>
Date: Mon, 23 Dec 2024 10:26:03 +0900
Subject: [PATCH] Use set_joint_positions_clamped in update_robot &
 move_joint_to_zero (#274)

---
 src/app.rs | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/app.rs b/src/app.rs
index 7794873..43c4f07 100644
--- a/src/app.rs
+++ b/src/app.rs
@@ -54,9 +54,10 @@ fn move_joint_by_random(robot: &mut k::Chain<f32>) -> Result<(), k::Error> {
     robot.set_joint_positions(&angles_vec)
 }
 
-fn move_joint_to_zero(robot: &mut k::Chain<f32>) -> Result<(), k::Error> {
+fn move_joint_to_zero(robot: &mut k::Chain<f32>) {
     let angles_vec = vec![0.0; robot.dof()];
-    robot.set_joint_positions(&angles_vec)
+    // use clamped variant because 0.0 is maybe out-of-limits.
+    robot.set_joint_positions_clamped(&angles_vec)
 }
 
 fn move_joint_by_index(
@@ -332,9 +333,8 @@ impl UrdfViewerApp {
     fn update_robot(&mut self) {
         // this is hack to handle invalid mimic joints, like hsr
         let joint_positions = self.robot.joint_positions();
-        self.robot
-            .set_joint_positions(&joint_positions)
-            .unwrap_or_else(|err| error!("failed to update robot joints {err}"));
+        // Use clamped variant because 0.0 which is the default position value is maybe out-of-limits.
+        self.robot.set_joint_positions_clamped(&joint_positions);
         self.viewer.update(&self.robot);
         self.update_ik_target_marker();
         self.update_frame_markers();
@@ -513,7 +513,7 @@ impl UrdfViewerApp {
             Key::Z => {
                 self.robot.set_origin(na::Isometry::identity());
                 if self.has_joints() {
-                    move_joint_to_zero(&mut self.robot).unwrap_or(());
+                    move_joint_to_zero(&mut self.robot);
                 }
                 self.update_robot();
             }