Skip to content

[hrp2jsknts] Simple footcoords #337

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
Naoki-Hiraoka wants to merge 14 commits into from
Closed

[hrp2jsknts] Simple footcoords #337

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
f419d1c
[hrpsys_choreonoid/iob/iob.cpp] fix bug on pdgains_sim_file_name
Naoki-Hiraoka Apr 20, 2020
5332aa9
add HRP2JSK HRP2JSKNT HRP2JSKNTS
Naoki-Hiraoka Apr 23, 2020
89a9f7c
[hrpsys_choreonoid_tutorials] use HRP2JSK*_LOAD_OBJ.cnoid.in
Naoki-Hiraoka May 8, 2020
b338fdb
[hrpsys_choreonoid_tutorials] use robot_choreonoid.launch
Naoki-Hiraoka May 8, 2020
8681af1
[hrpsys_choreonoid_tutorials] fix initial joint angle of HRP2JSKNT*
Naoki-Hiraoka May 8, 2020
310840a
[hrpsys_choreonoid_tutorials] refine hrp2 codes
Naoki-Hiraoka May 12, 2020
96a3365
[hrpsys_choreonoid][hrpsys_choreonoid_tutorials] add SampleRobot chor…
Naoki-Hiraoka May 12, 2020
5fe6f66
[hrpsys_choreonoid_tutorials] fix HRP2 Customizer params and initial …
Naoki-Hiraoka May 16, 2020
00d3204
[hrpsys_choreonoid_tutorials] enable HRP2JSKNTS vision
Naoki-Hiraoka May 23, 2020
c1f846a
[hrpsys_choreonoid_tutorials] fix camera parameter of HRP2JSKNTS
Naoki-Hiraoka May 24, 2020
d7b32fa
[JAXONCustomizer] enable to set arm bush parameters
Naoki-Hiraoka Oct 14, 2020
bcc42f8
[hrpsys_choreonoid_tutorials] fix parameters for hrp2jsknts
Naoki-Hiraoka Nov 18, 2020
8ac0557
[hrp2jsknts_choreonoid.launch] add more args
Naoki-Hiraoka Nov 28, 2020
9146192
[hrp2jsknts_choreonoid] use simple_footcoords
Naoki-Hiraoka Nov 28, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
101 changes: 83 additions & 18 deletions hrpsys_choreonoid/iob/iob.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,8 @@
#include <rtm/DataInPort.h>
#include <rtm/DataOutPort.h>
#include <rtm/idl/BasicDataTypeSkel.h>
#include <hrpModel/ModelLoaderUtil.h>
#include <hrpModel/Sensor.h>

#include "RobotHardware_choreonoid.h"

Expand Down Expand Up @@ -82,13 +84,18 @@ static InPort<TimedDoubleSeq> *ip_lhsensor_sim;
static InPort<TimedAcceleration3D> *ip_gsensor_sim;
static InPort<TimedAngularVelocity3D> *ip_gyrometer_sim;

static int rfsensor_id;
static int lfsensor_id;
static int rhsensor_id;
static int lhsensor_id;

static void readGainFile();

static double dt;
static std::ifstream gain;
static std::string gain_fname;
static std::vector<double> qold, qold_ref, Pgain, Dgain;
static std::vector<double> tqold, tqold_ref, tqPgain, tqDgain;
static std::vector<double> qold, qold_ref, Pgain, Dgain, initial_Pgain, initial_Dgain;
static std::vector<double> tqold, tqold_ref, tqPgain, tqDgain, initial_tqPgain, initial_tqDgain;
static std::vector<double> Pgain_orig, Dgain_orig;
static std::vector<double> tlimit;
static size_t dof, loop;
Expand Down Expand Up @@ -408,7 +415,11 @@ int read_pgain(int id, double *gain)
if (id == 9)
std::cerr << "read_pgain: [" << id << "] " << Pgain[id] << std::endl;
#endif
*gain = Pgain[id];
if (initial_Pgain[id]==0) {
*gain = 0;
} else {
*gain = Pgain[id] / initial_Pgain[id];
}
return TRUE;
}

Expand All @@ -419,7 +430,7 @@ int write_pgain(int id, double gain)
if (id == 9)
std::cerr << "write_pgain: [" << id << "] " << gain << std::endl;
#endif
Pgain[id] = gain;
Pgain[id] = gain * initial_Pgain[id];
return TRUE;
}

Expand All @@ -430,7 +441,11 @@ int read_dgain(int id, double *gain)
if (id == 9)
std::cerr << "read_dgain: [" << id << "] " << Dgain[id] << std::endl;
#endif
*gain = Dgain[id];
if (initial_Dgain[id]==0) {
*gain = 0;
} else {
*gain = Dgain[id] / initial_Dgain[id];
}
return TRUE;
}

Expand All @@ -441,7 +456,7 @@ int write_dgain(int id, double gain)
if (id == 9)
std::cerr << "write_dgain: [" << id << "] " << gain << std::endl;
#endif
Dgain[id] = gain;
Dgain[id] = gain * initial_Dgain[id];
return TRUE;
}

Expand Down Expand Up @@ -614,6 +629,44 @@ int open_iob(void)
servo[i] = 1;
}

// get sensorId of force sensors
{
RTC::Manager& rtcManager = RTC::Manager::instance();
std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
int comPos = nameServer.find(",");
if (comPos < 0){
comPos = nameServer.length();
}
nameServer = nameServer.substr(0, comPos);
RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
RTC::Properties& prop = self_ptr->getProperties();
hrp::BodyPtr m_robot = hrp::BodyPtr(new hrp::Body());
if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(),
CosNaming::NamingContext::_duplicate(naming.getRootContext())
)){
std::cerr << "failed to load model[" << prop["model"] << "]" << std::endl;
return RTC::RTC_ERROR;
}

if (m_robot->sensor<hrp::ForceSensor>("rfsensor") && m_robot->sensor<hrp::ForceSensor>("lfsensor") && m_robot->sensor<hrp::ForceSensor>("rhsensor") && m_robot->sensor<hrp::ForceSensor>("lhsensor") ){
rfsensor_id = m_robot->sensor<hrp::ForceSensor>("rfsensor")->id;
lfsensor_id = m_robot->sensor<hrp::ForceSensor>("lfsensor")->id;
rhsensor_id = m_robot->sensor<hrp::ForceSensor>("rhsensor")->id;
lhsensor_id = m_robot->sensor<hrp::ForceSensor>("lhsensor")->id;
} else if (m_robot->sensor<hrp::ForceSensor>("rfsensor") && m_robot->sensor<hrp::ForceSensor>("lfsensor") ){
rfsensor_id = m_robot->sensor<hrp::ForceSensor>("rfsensor")->id;
lfsensor_id = m_robot->sensor<hrp::ForceSensor>("lfsensor")->id;
rhsensor_id = 2;
lhsensor_id = 3;
} else {
std::cerr << "could not find rfsensor/lfsensor/rhsensor/lhsensor. use default sensorId" << std::endl;
rfsensor_id = 0;
lfsensor_id = 1;
rhsensor_id = 2;
lhsensor_id = 3;
}
}

std::cerr << "choreonoid IOB is opened" << std::endl;
return TRUE;
}
Expand Down Expand Up @@ -659,7 +712,7 @@ void iob_update(void)
if(number_of_force_sensors() >= 1) {
for(int i = 0; i < 6; i++) {
//forces[0][i] = m_rfsensor_sim.data[i];
force_queue[force_counter][0][i] = m_rfsensor_sim.data[i];
force_queue[force_counter][rfsensor_id][i] = m_rfsensor_sim.data[i];
}
}
}
Expand All @@ -668,7 +721,7 @@ void iob_update(void)
if(number_of_force_sensors() >= 2) {
for(int i = 0; i < 6; i++) {
//forces[1][i] = m_lfsensor_sim.data[i];
force_queue[force_counter][1][i] = m_lfsensor_sim.data[i];
force_queue[force_counter][lfsensor_id][i] = m_lfsensor_sim.data[i];
}
}
}
Expand All @@ -677,7 +730,7 @@ void iob_update(void)
if(number_of_force_sensors() >= 3) {
for(int i = 0; i < 6; i++) {
//forces[2][i] = m_rhsensor_sim.data[i];
force_queue[force_counter][2][i] = m_rhsensor_sim.data[i];
force_queue[force_counter][rhsensor_id][i] = m_rhsensor_sim.data[i];
}
}
}
Expand All @@ -686,7 +739,7 @@ void iob_update(void)
if(number_of_force_sensors() >= 4) {
for(int i = 0; i < 6; i++) {
//forces[3][i] = m_lhsensor_sim.data[i];
force_queue[force_counter][3][i] = m_lhsensor_sim.data[i];
force_queue[force_counter][lhsensor_id][i] = m_lhsensor_sim.data[i];
}
}
}
Expand Down Expand Up @@ -814,6 +867,10 @@ static void readGainFile()
Dgain.resize(dof);
tqPgain.resize(dof);
tqDgain.resize(dof);
initial_Pgain.resize(dof);
initial_Dgain.resize(dof);
initial_tqPgain.resize(dof);
initial_tqDgain.resize(dof);
gain.open(gain_fname.c_str());
if (gain.is_open()) {
std::cerr << "[iob] Gain file [" << gain_fname << "] opened" << std::endl;
Expand All @@ -830,16 +887,16 @@ static void readGainFile()

std::istringstream sstrm(str);
sstrm >> tmp;
Pgain[i] = tmp;
Pgain[i] = initial_Pgain[i] = tmp;
if(sstrm.eof()) goto next;
sstrm >> tmp;
Dgain[i] = tmp;
Dgain[i] = initial_Dgain[i] = tmp;
if(sstrm.eof()) goto next;
sstrm >> tmp;
tqPgain[i] = tmp;
tqPgain[i] = initial_tqPgain[i] = tmp;
if(sstrm.eof()) goto next;
sstrm >> tmp;
tqDgain[i] = tmp;
tqDgain[i] = initial_tqDgain[i] = tmp;
} else {
i--;
break;
Expand Down Expand Up @@ -1090,7 +1147,11 @@ int read_torque_pgain(int id, double *gain)
if (id == 9)
std::cerr << "read_pgain: [" << id << "] " << Pgain[id] << std::endl;
#endif
*gain = tqPgain[id];
if (initial_tqPgain[id]==0) {
*gain = 0;
} else {
*gain = tqPgain[id] / initial_tqPgain[id];
}
return TRUE;
}

Expand All @@ -1101,7 +1162,7 @@ int write_torque_pgain(int id, double gain)
if (id == 9)
std::cerr << "write_pgain: [" << id << "] " << gain << std::endl;
#endif
tqPgain[id] = gain;
tqPgain[id] = gain * initial_tqPgain[id];
return TRUE;
}

Expand All @@ -1112,7 +1173,11 @@ int read_torque_dgain(int id, double *gain)
if (id == 9)
std::cerr << "read_dgain: [" << id << "] " << Dgain[id] << std::endl;
#endif
*gain = tqDgain[id];
if (initial_tqDgain[id]==0) {
*gain = 0;
} else {
*gain = tqDgain[id] / initial_tqDgain[id];
}
return TRUE;
}

Expand All @@ -1123,7 +1188,7 @@ int write_torque_dgain(int id, double gain)
if (id == 9)
std::cerr << "write_dgain: [" << id << "] " << gain << std::endl;
#endif
tqDgain[id] = gain;
tqDgain[id] = gain * initial_tqDgain[id];
return TRUE;
}
#endif
Expand Down
82 changes: 52 additions & 30 deletions hrpsys_choreonoid/src/JAXONCustomizer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

#include <cmath>
#include <cstring>
#include <string>
#include <fstream>
#include <boost/function.hpp>
#include <boost/bind.hpp>
Expand Down Expand Up @@ -53,6 +54,7 @@ static BodyCustomizerInterface bodyCustomizerInterface;

struct JointValSet
{
int index;
double* valuePtr;
double* velocityPtr;
double* torqueForcePtr;
Expand All @@ -63,12 +65,17 @@ struct JAXONCustomizer
BodyHandle bodyHandle;

bool hasVirtualBushJoints;
JointValSet jointValSets[2][3];
JointValSet jointValSets[4][6];
double springT;
double dampingT;
double springR;
double dampingR;

double arm_springT;
double arm_dampingT;
double arm_springR;
double arm_dampingR;

double tilt_upper_bound;
double tilt_lower_bound;
double tilt_positive_speed;
Expand Down Expand Up @@ -102,28 +109,26 @@ static const char** getTargetModelNames()

static void getVirtualbushJoints(JAXONCustomizer* customizer, BodyHandle body)
{
customizer->hasVirtualBushJoints = true;

int bushIndices[2][3];

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 < 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];
jointValSet.valuePtr = bodyInterface->getJointValuePtr(body, bushIndex);
jointValSet.velocityPtr = bodyInterface->getJointVelocityPtr(body, bushIndex);
jointValSet.torqueForcePtr = bodyInterface->getJointForcePtr(body, bushIndex);
customizer->hasVirtualBushJoints = false;

static const char* limbs[] = {"RLEG", "LLEG", "RARM", "LARM"};
static const char* types[] = {"X", "Y", "Z", "ROLL", "PITCH", "YAW"};
char bush_name[30];

for(int i=0; i < 4; ++i){
for(int j=0; j < 6; ++j){
JointValSet& jointValSet = customizer->jointValSets[i][j];
sprintf(bush_name, "%s_BUSH_%s", limbs[i], types[j]);
jointValSet.index = bodyInterface->getLinkIndexFromName(body, bush_name);
if(jointValSet.index < 0){
std::cerr << "[Customizer] failed to find out : " << bush_name << std::endl;
}else{
std::cerr << "[Customizer] find out : " << bush_name << std::endl;
customizer->hasVirtualBushJoints = true;

jointValSet.valuePtr = bodyInterface->getJointValuePtr(body, jointValSet.index);
jointValSet.velocityPtr = bodyInterface->getJointVelocityPtr(body, jointValSet.index);
jointValSet.torqueForcePtr = bodyInterface->getJointForcePtr(body, jointValSet.index);
}
}
}
Expand Down Expand Up @@ -172,6 +177,11 @@ static BodyCustomizerHandle create(BodyHandle bodyHandle, const char* modelName)
customizer->springR = 2.5e3; // Nm / rad
customizer->dampingR = 2.5; // Nm / (rad/s)

customizer->arm_springT = 1.1e6; // N/m
customizer->arm_dampingT = 1.1e3; // N/(m/s)
customizer->arm_springR = 2.5e3; // Nm / rad
customizer->arm_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
Expand All @@ -187,6 +197,10 @@ static BodyCustomizerHandle create(BodyHandle bodyHandle, const char* modelName)
customizer->dampingT = param["bush"]["dampingT"].as<double>();
customizer->springR = param["bush"]["springR"].as<double>();
customizer->dampingR = param["bush"]["dampingR"].as<double>();
customizer->arm_springT = param["armbush"]["springT"].as<double>();
customizer->arm_dampingT = param["armbush"]["dampingT"].as<double>();
customizer->arm_springR = param["armbush"]["springR"].as<double>();
customizer->arm_dampingR = param["armbush"]["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>();
Expand Down Expand Up @@ -215,15 +229,23 @@ 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 i=0; i < 4; ++i){
for(int j=0; j < 3; ++j){
JointValSet& trans = customizer->jointValSets[i][j];
if(trans.index >= 0){
if(i<2) *(trans.torqueForcePtr) = - customizer->springT * (*trans.valuePtr) - customizer->dampingT * (*trans.velocityPtr);
else *(trans.torqueForcePtr) = - customizer->arm_springT * (*trans.valuePtr) - customizer->arm_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){
for(int j=3; j < 6; ++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;
if(rot.index >= 0){
if(i<2) *(rot.torqueForcePtr) = - customizer->springR * (*rot.valuePtr) - customizer->dampingR * (*rot.velocityPtr);
else *(rot.torqueForcePtr) = - customizer->arm_springR * (*rot.valuePtr) - customizer->arm_dampingR * (*rot.velocityPtr);
//std::cerr << i << " " << j << " " << *(rot.torqueForcePtr) << " = " << -customizer->springR << " x " << *rot.valuePtr << " + " << - customizer->dampingR << " x " << *rot.velocityPtr << std::endl;
}
}
}
}
Expand Down
Loading