Enable to change weight caluclation. If use_inside_joint_weight_retri…

…eval=true (true by default), inward joint weight retrievs to 1.0 (original). Otherwise, always weight is calculated from joint limit to solve fkanehiro#516
snozawa · Apr 29, 2015 · aae66ac · aae66ac
1 parent e3eab88
commit aae66ac
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 4 deletions.
diff --git a/rtc/ImpedanceController/JointPathEx.cpp b/rtc/ImpedanceController/JointPathEx.cpp
@@ -86,8 +86,8 @@ Vector3 omegaFromRotEx(const Matrix33& r)
     }
 }
 
-JointPathEx::JointPathEx(BodyPtr& robot, Link* base, Link* end, double control_cycle)
-    : JointPath(base, end), sr_gain(1.0), manipulability_limit(0.1), manipulability_gain(0.001), maxIKPosErrorSqr(1.0e-8), maxIKRotErrorSqr(1.0e-6), maxIKIteration(50) {
+JointPathEx::JointPathEx(BodyPtr& robot, Link* base, Link* end, double control_cycle, bool _use_inside_joint_weight_retrieval)
+    : JointPath(base, end), sr_gain(1.0), manipulability_limit(0.1), manipulability_gain(0.001), maxIKPosErrorSqr(1.0e-8), maxIKRotErrorSqr(1.0e-6), maxIKIteration(50), use_inside_joint_weight_retrieval(_use_inside_joint_weight_retrieval) {
   for (int i = 0 ; i < numJoints(); i++ ) {
     joints.push_back(joint(i));
   }
@@ -150,10 +150,15 @@ bool JointPathEx::calcJacobianInverseNullspace(dmatrix &J, dmatrix &Jinv, dmatri
             if (isnan(r)) r = 0;
         }
 
+        // If use_inside_joint_weight_retrieval = true (true by default), use T. F. Chang and R.-V. Dubeby weight retrieval inward.
+        // Otherwise, joint weight is always calculated from limit value to resolve https://github.com/fkanehiro/hrpsys-base/issues/516.
         if (( r - avoid_weight_gain[j] ) >= 0 ) {
 	  w(j, j) = optional_weight_vector[j] * ( 1.0 / ( 1.0 + r) );
 	} else {
-	  w(j, j) = optional_weight_vector[j] * 1.0;
+            if (use_inside_joint_weight_retrieval)
+                w(j, j) = optional_weight_vector[j] * 1.0;
+            else
+                w(j, j) = optional_weight_vector[j] * ( 1.0 / ( 1.0 + r) );
 	}
         avoid_weight_gain[j] = r;
     }

diff --git a/rtc/ImpedanceController/JointPathEx.h b/rtc/ImpedanceController/JointPathEx.h
@@ -15,7 +15,7 @@ namespace hrp {
 namespace hrp {
     class JointPathEx : public JointPath {
   public:
-    JointPathEx(BodyPtr& robot, Link* base, Link* end, double control_cycle);
+    JointPathEx(BodyPtr& robot, Link* base, Link* end, double control_cycle, bool _use_inside_joint_weight_retrieval = true);
     bool calcJacobianInverseNullspace(dmatrix &J, dmatrix &Jinv, dmatrix &Jnull);
     bool calcInverseKinematics2Loop(const Vector3& dp, const Vector3& omega, const double LAMBDA, const double avoid_gain = 0.0, const double reference_gain = 0.0, const dvector* reference_q = NULL);
     bool calcInverseKinematics2(const Vector3& end_p, const Matrix33& end_R, const double avoid_gain = 0.0, const double reference_gain = 0.0, const dvector* reference_q = NULL);
@@ -45,6 +45,7 @@ namespace hrp {
         std::vector<Link*> joints;
         std::vector<double> avoid_weight_gain, optional_weight_vector;
         double sr_gain, manipulability_limit, manipulability_gain, dt;
+        bool use_inside_joint_weight_retrieval;
     };
 
     typedef boost::shared_ptr<JointPathEx> JointPathExPtr;