From d92cca5a67a73e545580d0ebdba170df328e2afb Mon Sep 17 00:00:00 2001
From: kindsenior <kunio.einstein@gmail.com>
Date: Sun, 12 Jun 2022 15:02:14 +0900
Subject: [PATCH 1/3] [SoftErrorLimiter/robot.cpp] check jointId >=0 in
 setServoErrorLimit

---
 rtc/SoftErrorLimiter/robot.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/rtc/SoftErrorLimiter/robot.cpp b/rtc/SoftErrorLimiter/robot.cpp
index 47d17796f7d..51a3bcd0ed1 100644
--- a/rtc/SoftErrorLimiter/robot.cpp
+++ b/rtc/SoftErrorLimiter/robot.cpp
@@ -25,11 +25,11 @@ bool robot::setServoErrorLimit(const char *i_jname, double i_limit) {
       m_servoErrorLimit[i] = i_limit;
     }
     std::cerr << "[el] setServoErrorLimit " << i_limit << "[rad] for all joints" << std::endl;
-  }else if ((l = link(i_jname))){
+  }else if ((l = link(i_jname)) && (l->jointId >= 0)){
     m_servoErrorLimit[l->jointId] = i_limit;
     std::cerr << "[el] setServoErrorLimit " << i_limit << "[rad] for " << i_jname << std::endl;
   }else{
-    std::cerr << "[el] Invalid joint name of setServoErrorLimit " << i_jname << "!" << std::endl;
+    std::cerr << "[el] Invalid joint name of jointId of setServoErrorLimit name:" << i_jname << " jointId:" << l->jointId << "!" << std::endl;
     return false;
   }
   return true;

From 8ba5e38b4d59e03b6c6906c6b795e74481667c6f Mon Sep 17 00:00:00 2001
From: Naoki-Hiraoka <hiraoka@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 12 Jun 2022 16:44:18 +0900
Subject: [PATCH 2/3] [SequencePlayer/SequencePlayer.cpp] check jointId in
 setJointAngle

---
 rtc/SequencePlayer/SequencePlayer.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/rtc/SequencePlayer/SequencePlayer.cpp b/rtc/SequencePlayer/SequencePlayer.cpp
index 9bde7dd5c2f..2b3852494bc 100644
--- a/rtc/SequencePlayer/SequencePlayer.cpp
+++ b/rtc/SequencePlayer/SequencePlayer.cpp
@@ -391,6 +391,10 @@ bool SequencePlayer::setJointAngle(short id, double angle, double tm)
     }
     Guard guard(m_mutex);
     if (!setInitialState()) return false;
+    if (id < 0 || id >= m_robot->numJoints()){
+        std::cerr << "[setJointAngle] Invalid jointId " << id << std::endl;
+        return false;
+    }
     dvector q(m_robot->numJoints());
     m_seq->getJointAngles(q.data());
     q[id] = angle;

From 0642b81a78af1eb99cd213343a654670934e57c6 Mon Sep 17 00:00:00 2001
From: Naoki-Hiraoka <hiraoka@jsk.imi.i.u-tokyo.ac.jp>
Date: Sun, 12 Jun 2022 16:45:24 +0900
Subject: [PATCH 3/3] [RobotHardware/robot.cpp] check jointId for services

---
 rtc/RobotHardware/robot.cpp | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp
index 2027dd405db..ec5737370e0 100644
--- a/rtc/RobotHardware/robot.cpp
+++ b/rtc/RobotHardware/robot.cpp
@@ -372,7 +372,7 @@ bool robot::servo(const char *jname, bool turnon)
         }
         m_reportedEmergency = false;
         return ret;
-    }else if ((l = link(jname))){
+    }else if ((l = link(jname)) && (l->jointId >= 0)){
         return servo(l->jointId, turnon);
     }else{
         char *s = (char *)jname; while(*s) {*s=toupper(*s);s++;}
@@ -428,6 +428,7 @@ bool robot::power(const char *jname, bool turnon)
     }else{
         Link *l = link(jname);
         if (!l) return false;
+        if (l->jointId < 0) return false;
         jid = l->jointId;
     }
     return power(jid, turnon);
@@ -749,7 +750,7 @@ bool robot::setServoGainPercentage(const char *i_jname, double i_percentage)
             gain_counter[i] = 0;
         }
         std::cerr << "[RobotHardware] setServoGainPercentage " << i_percentage << "[%] for all joints" << std::endl;
-    }else if ((l = link(i_jname))){
+    }else if ((l = link(i_jname)) && (l->jointId >= 0)){
         if (!read_pgain(l->jointId, &old_pgain[l->jointId])) old_pgain[l->jointId] = pgain[l->jointId];
         pgain[l->jointId] = default_pgain[l->jointId] * i_percentage/100.0;
         if (!read_dgain(l->jointId, &old_dgain[l->jointId])) old_dgain[l->jointId] = dgain[l->jointId];
@@ -800,7 +801,7 @@ bool robot::setServoTorqueGainPercentage(const char *i_jname, double i_percentag
             gain_counter[i] = 0;
         }
         std::cerr << "[RobotHardware] setServoTorqueGainPercentage " << i_percentage << "[%] for all joints" << std::endl;
-    }else if ((l = link(i_jname))){
+    }else if ((l = link(i_jname)) && (l->jointId >= 0)){
 #if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4
         if (!read_torque_pgain(l->jointId, &old_tqpgain[l->jointId])) old_tqpgain[l->jointId] = tqpgain[l->jointId];
         tqpgain[l->jointId] = default_tqpgain[l->jointId] * i_percentage/100.0;
@@ -838,7 +839,7 @@ bool robot::setServoErrorLimit(const char *i_jname, double i_limit)
         for (unsigned int i=0; i<numJoints(); i++){
             m_servoErrorLimit[i] = i_limit;
         }
-    }else if ((l = link(i_jname))){
+    }else if ((l = link(i_jname)) && (l->jointId >= 0)){
         m_servoErrorLimit[l->jointId] = i_limit;
     }else{
         char *s = (char *)i_jname; while(*s) {*s=toupper(*s);s++;}
@@ -989,6 +990,7 @@ bool robot::setJointInertia(const char *jname, double mn)
     Link *l = link(jname);
     if (!l) return false;
     int jid = l->jointId;
+    if (jid < 0) return false;
     return write_joint_inertia(jid, mn);
 #else
     return false;
@@ -1040,7 +1042,7 @@ bool robot::setJointControlMode(const char *i_jname, joint_control_mode mode)
             write_control_mode(i, mode);
         }
         std::cerr << "[RobotHardware] setJointControlMode for all joints : " << mode << std::endl;
-    } else if ((l = link(i_jname))) {
+    } else if ((l = link(i_jname)) && (l->jointId >= 0)) {
         write_control_mode(l->jointId, mode);
         std::cerr << "[RobotHardware] setJointControlMode for " << i_jname << " : " << mode << std::endl;
     } else {