Skip to content

Commit

Permalink
[hrpsys_choreonoid/JAXONCustomizer.cpp] remove assumption for bush
Browse files Browse the repository at this point in the history
  • Loading branch information
@Naoki-Hiraoka
Naoki-Hiraoka committed Jun 13, 2021
1 parent ea1e0f0 commit 253eeca
Showing 1 changed file with 59 additions and 50 deletions.
109 changes: 59 additions & 50 deletions hrpsys_choreonoid/src/JAXONCustomizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,8 @@

#include <cmath>
#include <cstring>
#include <string>
#include <vector>
#include <fstream>
#include <boost/function.hpp>
#include <boost/bind.hpp>
Expand Down Expand Up @@ -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<JointValSet> jointValSets;

double tilt_upper_bound;
double tilt_lower_bound;
Expand Down Expand Up @@ -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<double>();
if (param["bush"]["dampingT"]) default_dampingT = param["bush"]["dampingT"].as<double>();
if (param["bush"]["springR"]) default_springR = param["bush"]["springR"].as<double>();
if (param["bush"]["dampingR"]) default_dampingR = param["bush"]["dampingR"].as<double>();
}

int bushIndices[2][3];
std::vector<std::string> bushes{"RLEG", "LLEG", "RARM", "LARM"};
if (param && param["bush"] && param["bush"]["bushes"]) bushes = param["bush"]["bushes"].as<std::vector<std::string>>();
std::vector<std::string> 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<double>();
if (param["bush"][bushes[i]]["dampingT"]) jointValSet.damping = param["bush"][bushes[i]]["dampingT"].as<double>();
}
} 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<double>();
if (param["bush"][bushes[i]]["dampingR"]) jointValSet.damping = param["bush"][bushes[i]]["dampingR"].as<double>();
}
}

customizer->jointValSets.push_back(jointValSet);
}
}
}
Expand Down Expand Up @@ -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<double>();
customizer->dampingT = param["bush"]["dampingT"].as<double>();
customizer->springR = param["bush"]["springR"].as<double>();
customizer->dampingR = param["bush"]["dampingR"].as<double>();
customizer->tilt_upper_bound = param["tilt_laser"]["TILT_UPPER_BOUND"].as<double>();
customizer->tilt_positive_speed = param["tilt_laser"]["TILT_POSITIVE_SPEED"].as<double>();
customizer->tilt_lower_bound = param["tilt_laser"]["TILT_LOWER_BOUND"].as<double>();
customizer->tilt_negative_speed = param["tilt_laser"]["TILT_NEGATIVE_SPEED"].as<double>();
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<double>();
if (param["tilt_laser"]["TILT_POSITIVE_SPEED"]) customizer->tilt_positive_speed = param["tilt_laser"]["TILT_POSITIVE_SPEED"].as<double>();
if (param["tilt_laser"]["TILT_LOWER_BOUND"]) customizer->tilt_lower_bound = param["tilt_laser"]["TILT_LOWER_BOUND"].as<double>();
if (param["tilt_laser"]["TILT_NEGATIVE_SPEED"]) customizer->tilt_negative_speed = param["tilt_laser"]["TILT_NEGATIVE_SPEED"].as<double>();
}
} else {
std::cerr << "[JAXONCustomizer] " << config_file_path << " is not found" << std::endl;
}
}

getVirtualbushJoints(customizer, bodyHandle);
getVirtualbushJoints(customizer, bodyHandle, param);

return static_cast<BodyCustomizerHandle>(customizer);
}
Expand All @@ -215,18 +232,10 @@ static void setVirtualJointForces(BodyCustomizerHandle customizerHandle)
{
JAXONCustomizer* customizer = static_cast<JAXONCustomizer*>(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) {
Expand Down

0 comments on commit 253eeca

Please sign in to comment.