Merge branch 'kindsenior/PR-servo-error-limit-with-jointId' into auto…

…-stabilizer
YutaKojio · Jun 14, 2022 · 932498c · 932498c
2 parents 70010b1 + 996c598
commit 932498c
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 7 deletions.
diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp
@@ -374,7 +374,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++;}
@@ -433,6 +433,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);
@@ -763,7 +764,7 @@ bool robot::setServoGainPercentage(const char *i_jname, double i_percentage, int
             max_gain_count[i] = max_count;
         }
         std::cerr << "[RobotHardware] setServoGainPercentage " << i_percentage << "[%] for all joints in " << max_count << " loop" << 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];
         if (mode / 2 == 0) pgain[l->jointId] = default_pgain[l->jointId] * i_percentage/100.0;// 0 or 1
         if (!read_dgain(l->jointId, &old_dgain[l->jointId])) old_dgain[l->jointId] = dgain[l->jointId];
@@ -816,7 +817,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;
@@ -854,7 +855,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++;}
@@ -1005,6 +1006,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;
@@ -1056,7 +1058,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 {

diff --git a/rtc/SequencePlayer/SequencePlayer.cpp b/rtc/SequencePlayer/SequencePlayer.cpp
@@ -395,6 +395,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;

diff --git 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;