From aae66ac8c8e6aafcebd548eca1b595fc3a2bc010 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Thu, 30 Apr 2015 08:09:37 +0900 Subject: [PATCH] Enable to change weight caluclation. If use_inside_joint_weight_retrieval=true (true by default), inward joint weight retrievs to 1.0 (original). Otherwise, always weight is calculated from joint limit to solve https://github.com/fkanehiro/hrpsys-base/issues/516 --- rtc/ImpedanceController/JointPathEx.cpp | 11 ++++++++--- rtc/ImpedanceController/JointPathEx.h | 3 ++- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/rtc/ImpedanceController/JointPathEx.cpp b/rtc/ImpedanceController/JointPathEx.cpp index 2a37b33f47e..ac5e5996ca1 100644 --- 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 index ae90f7be98b..95b52a0fec8 100644 --- 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 joints; std::vector avoid_weight_gain, optional_weight_vector; double sr_gain, manipulability_limit, manipulability_gain, dt; + bool use_inside_joint_weight_retrieval; }; typedef boost::shared_ptr JointPathExPtr;