diff --git a/hrpsys_choreonoid/src/JAXONCustomizer.cpp b/hrpsys_choreonoid/src/JAXONCustomizer.cpp index 09944c64..9d702322 100644 --- a/hrpsys_choreonoid/src/JAXONCustomizer.cpp +++ b/hrpsys_choreonoid/src/JAXONCustomizer.cpp @@ -2,6 +2,8 @@ #include #include +#include +#include #include #include #include @@ -56,18 +58,15 @@ struct JointValSet double* valuePtr; double* velocityPtr; double* torqueForcePtr; + double spring; + double damping; }; struct JAXONCustomizer { BodyHandle bodyHandle; - bool hasVirtualBushJoints; - JointValSet jointValSets[2][3]; - double springT; - double dampingT; - double springR; - double dampingR; + std::vector jointValSets; double tilt_upper_bound; double tilt_lower_bound; @@ -101,30 +100,55 @@ static const char** getTargetModelNames() return names; } -static void getVirtualbushJoints(JAXONCustomizer* customizer, BodyHandle body) +static void getVirtualbushJoints(JAXONCustomizer* customizer, BodyHandle body, const YAML::Node& param) { - customizer->hasVirtualBushJoints = true; + double default_springT = 1.1e6; // N/m + double default_dampingT = 1.1e3; // N/(m/s) + double default_springR = 2.5e3; // Nm / rad + double default_dampingR = 2.5; // Nm / (rad/s) + if (param && param["bush"]) { + if (param["bush"]["springT"]) default_springT = param["bush"]["springT"].as(); + if (param["bush"]["dampingT"]) default_dampingT = param["bush"]["dampingT"].as(); + if (param["bush"]["springR"]) default_springR = param["bush"]["springR"].as(); + if (param["bush"]["dampingR"]) default_dampingR = param["bush"]["dampingR"].as(); + } - int bushIndices[2][3]; + std::vector bushes{"RLEG", "LLEG", "RARM", "LARM"}; + if (param && param["bush"] && param["bush"]["bushes"]) bushes = param["bush"]["bushes"].as>(); + std::vector types{"X", "Y", "Z", "ROLL", "PITCH", "YAW"}; - bushIndices[0][0] = bodyInterface->getLinkIndexFromName(body, "RLEG_BUSH_Z"); - bushIndices[0][1] = bodyInterface->getLinkIndexFromName(body, "RLEG_BUSH_ROLL"); - bushIndices[0][2] = bodyInterface->getLinkIndexFromName(body, "RLEG_BUSH_PITCH"); - bushIndices[1][0] = bodyInterface->getLinkIndexFromName(body, "LLEG_BUSH_Z"); - bushIndices[1][1] = bodyInterface->getLinkIndexFromName(body, "LLEG_BUSH_ROLL"); - bushIndices[1][2] = bodyInterface->getLinkIndexFromName(body, "LLEG_BUSH_PITCH"); + for(int i=0; i < bushes.size(); ++i){ + for(int j=0; j < types.size(); ++j){ + std::string bush_name = bushes[i] + "_BUSH_" + types[j]; + int bushIndex = bodyInterface->getLinkIndexFromName(body, bush_name.c_str()); - for(int i=0; i < 2; ++i){ - for(int j=0; j < 3; ++j){ - int bushIndex = bushIndices[i][j]; if(bushIndex < 0){ - std::cerr << "[Customizer] failed to find out : " << i << " " << j << std::endl; - customizer->hasVirtualBushJoints = false; - } else { - JointValSet& jointValSet = customizer->jointValSets[i][j]; + std::cerr << "[Customizer] failed to find out : " << bush_name << std::endl; + }else{ + std::cerr << "[Customizer] find out : " << bush_name << std::endl; + + JointValSet jointValSet; jointValSet.valuePtr = bodyInterface->getJointValuePtr(body, bushIndex); jointValSet.velocityPtr = bodyInterface->getJointVelocityPtr(body, bushIndex); jointValSet.torqueForcePtr = bodyInterface->getJointForcePtr(body, bushIndex); + + if (j<3) { + jointValSet.spring = default_springT; + jointValSet.damping = default_dampingT; + if (param && param["bush"] && param["bush"][bushes[i]]){ + if (param["bush"][bushes[i]]["springT"]) jointValSet.spring = param["bush"][bushes[i]]["springT"].as(); + if (param["bush"][bushes[i]]["dampingT"]) jointValSet.damping = param["bush"][bushes[i]]["dampingT"].as(); + } + } else { + jointValSet.spring = default_springR; + jointValSet.damping = default_dampingR; + if (param && param["bush"] && param["bush"][bushes[i]]){ + if (param["bush"][bushes[i]]["springR"]) jointValSet.spring = param["bush"][bushes[i]]["springR"].as(); + if (param["bush"][bushes[i]]["dampingR"]) jointValSet.damping = param["bush"][bushes[i]]["dampingR"].as(); + } + } + + customizer->jointValSets.push_back(jointValSet); } } } @@ -166,38 +190,31 @@ static BodyCustomizerHandle create(BodyHandle bodyHandle, const char* modelName) customizer = new JAXONCustomizer; customizer->bodyHandle = bodyHandle; - customizer->hasVirtualBushJoints = false; - - customizer->springT = 1.1e6; // N/m - customizer->dampingT = 1.1e3; // N/(m/s) - customizer->springR = 2.5e3; // Nm / rad - customizer->dampingR = 2.5; // Nm / (rad/s) customizer->tilt_upper_bound = 1.35; // rad customizer->tilt_lower_bound = -0.7; // rad customizer->tilt_positive_speed = 1.0; // rad/s customizer->tilt_negative_speed = -1.0; // rad/s + YAML::Node param; char* config_file_path = getenv("CUSTOMIZER_CONF_FILE"); if (config_file_path) { ifstream ifs(config_file_path); if (ifs.is_open()) { std::cerr << "[JAXONCustomizer] Config file is: " << config_file_path << std::endl; - YAML::Node param = YAML::LoadFile(config_file_path); - customizer->springT = param["bush"]["springT"].as(); - customizer->dampingT = param["bush"]["dampingT"].as(); - customizer->springR = param["bush"]["springR"].as(); - customizer->dampingR = param["bush"]["dampingR"].as(); - customizer->tilt_upper_bound = param["tilt_laser"]["TILT_UPPER_BOUND"].as(); - customizer->tilt_positive_speed = param["tilt_laser"]["TILT_POSITIVE_SPEED"].as(); - customizer->tilt_lower_bound = param["tilt_laser"]["TILT_LOWER_BOUND"].as(); - customizer->tilt_negative_speed = param["tilt_laser"]["TILT_NEGATIVE_SPEED"].as(); + param = YAML::LoadFile(config_file_path); + if (param["tilt_laser"]) { + if (param["tilt_laser"]["TILT_UPPER_BOUND"]) customizer->tilt_upper_bound = param["tilt_laser"]["TILT_UPPER_BOUND"].as(); + if (param["tilt_laser"]["TILT_POSITIVE_SPEED"]) customizer->tilt_positive_speed = param["tilt_laser"]["TILT_POSITIVE_SPEED"].as(); + if (param["tilt_laser"]["TILT_LOWER_BOUND"]) customizer->tilt_lower_bound = param["tilt_laser"]["TILT_LOWER_BOUND"].as(); + if (param["tilt_laser"]["TILT_NEGATIVE_SPEED"]) customizer->tilt_negative_speed = param["tilt_laser"]["TILT_NEGATIVE_SPEED"].as(); + } } else { std::cerr << "[JAXONCustomizer] " << config_file_path << " is not found" << std::endl; } } - getVirtualbushJoints(customizer, bodyHandle); + getVirtualbushJoints(customizer, bodyHandle, param); return static_cast(customizer); } @@ -215,18 +232,10 @@ static void setVirtualJointForces(BodyCustomizerHandle customizerHandle) { JAXONCustomizer* customizer = static_cast(customizerHandle); - if(customizer->hasVirtualBushJoints){ - for(int i=0; i < 2; ++i){ - JointValSet& trans = customizer->jointValSets[i][0]; - *(trans.torqueForcePtr) = - customizer->springT * (*trans.valuePtr) - customizer->dampingT * (*trans.velocityPtr); - //std::cerr << i << " " << 0 << " " << *(trans.torqueForcePtr) << " = " << -customizer->springT << " x " << *trans.valuePtr << " + " << - customizer->dampingT << " x " << *trans.velocityPtr << std::endl; - - for(int j=1; j < 3; ++j){ - JointValSet& rot = customizer->jointValSets[i][j]; - *(rot.torqueForcePtr) = - customizer->springR * (*rot.valuePtr) - customizer->dampingR * (*rot.velocityPtr); - //std::cerr << i << " " << j << " " << *(rot.torqueForcePtr) << " = " << -customizer->springR << " x " << *rot.valuePtr << " + " << - customizer->dampingR << " x " << *rot.velocityPtr << std::endl; - } - } + for(int i=0; i < customizer->jointValSets.size(); ++i){ + JointValSet& bush = customizer->jointValSets[i]; + *(bush.torqueForcePtr) = - bush.spring * (*bush.valuePtr) - bush.damping * (*bush.velocityPtr); + //std::cerr << i << " " << 0 << " " << *(bush.torqueForcePtr) << " = " << -bush.spring << " x " << *bush.valuePtr << " + " << - bush.damping << " x " << *bush.velocityPtr << std::endl; } if(customizer->hasMultisenseJoint) {