Merge pull request #610 from snozawa/enable_change_weight_calculation

Enable change weight calculation
fkanehiro · Apr 30, 2015 · cb815be · cb815be
2 parents a1962c1 + c10f3ef
commit cb815be
Show file tree

Hide file tree

Showing 5 changed files with 13 additions and 7 deletions.
diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp
@@ -193,7 +193,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
           coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str());
         }
         tp.localR = Eigen::AngleAxis<double>(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle
-        tp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt));
+        tp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false));
         // Fix for toe joint
         if (ee_name.find("leg") != std::string::npos && tp.manip->numJoints() == 7) { // leg and 7dof joint (6dof leg +1dof toe)
             std::vector<double> optw;

diff --git a/rtc/ImpedanceController/ImpedanceController.cpp b/rtc/ImpedanceController/ImpedanceController.cpp
@@ -226,7 +226,7 @@ RTC::ReturnCode_t ImpedanceController::onInitialize()
         // 3. Check whether joint path is adequate.
         hrp::Link* target_link = m_robot->link(ee_map[ee_name].target_name);
         ImpedanceParam p;
-        p.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(base_name_map[ee_name]), target_link, m_dt));
+        p.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(base_name_map[ee_name]), target_link, m_dt, false));
         if ( ! p.manip ) {
             std::cerr << "[" << m_profile.instance_name << "]   Invalid joint path from " << base_name_map[ee_name] << " to " << target_link->name << "!! Impedance param for " << sensor_name << " cannot be added!!" << std::endl;
             continue;

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;

diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp
@@ -209,7 +209,7 @@ RTC::ReturnCode_t Stabilizer::onInitialize()
       else if (ee_name == "lleg") ikp.sensor_name = "lfsensor";
       else ikp.sensor_name = "";
       stikp.push_back(ikp);
-      jpe_v.push_back(hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), dt)));
+      jpe_v.push_back(hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), dt, false)));
       // Fix for toe joint
       if (ee_name.find("leg") != std::string::npos && jpe_v.back()->numJoints() == 7) { // leg and has 7dof joint (6dof leg +1dof toe)
           std::vector<double> optw;