void DensityViewerWindow::fillInHKX() {
string hkl = axesNames(densityViewer->data.isDirect);
sectionComboBox->clear();
for (int i : {2,1,0}) {
string t = hkl;
t[i]='x';
sectionComboBox->addItem(QString::fromStdString(t));
}
}
bool JointTorqueControl::loadCouplingMatrix(yarp::os::Searchable& config,
CouplingMatrices & coupling_matrix,
std::string group_name)
{
if( !config.check(group_name) )
{
coupling_matrix.reset(this->axes);
return true;
}
else
{
yarp::os::Bottle couplings_bot = config.findGroup(group_name);
if(!checkVectorExistInConfiguration(couplings_bot,"axesNames",this->axes) )
{
std::cerr << "JointTorqueControl: error in loading axesNames parameter" << std::endl;
return false;
}
if(!checkVectorExistInConfiguration(couplings_bot,"motorNames",this->axes) )
{
std::cerr << "JointTorqueControl: error in loading motorNames parameter" << std::endl;
return false;
}
std::vector<std::string> motorNames(this->axes);
std::vector<std::string> axesNames(this->axes);
for(int j=0; j < this->axes; j++)
{
motorNames[j] = couplings_bot.find("motorNames").asList()->get(j).asString();
axesNames[j] = couplings_bot.find("axesNames").asList()->get(j).asString();
}
for(int axis_id = 0; axis_id < (int)this->axes; axis_id++)
{
//search if coupling information is provided for each axis, if not return false
std::string axis_name = axesNames[axis_id];
if( !couplings_bot.check(axis_name) ||
!(couplings_bot.find(axis_name).isList()) )
{
std::cerr << "[ERR] " << group_name << " group found, but no coupling found for joint "
<< axis_name << ", exiting" << std::endl;
return false;
}
//Check coupling configuration is well formed
yarp::os::Bottle * axis_coupling_bot = couplings_bot.find(axis_name).asList();
for(int coupled_motor=0; coupled_motor < axis_coupling_bot->size(); coupled_motor++ )
{
if( !(axis_coupling_bot->get(coupled_motor).isList()) ||
!(axis_coupling_bot->get(coupled_motor).asList()->size() == 2) ||
!(axis_coupling_bot->get(coupled_motor).asList()->get(0).isDouble()) ||
!(axis_coupling_bot->get(coupled_motor).asList()->get(1).isString()) )
{
std::cerr << "[ERR] " << group_name << " group found, but coupling for axis "
<< axis_name << " is malformed" << std::endl;
return false;
}
std::string motorName = axis_coupling_bot->get(coupled_motor).asList()->get(1).asString().c_str();
if( !contains(motorNames,motorName) )
{
std::cerr << "[ERR] " << group_name << "group found, but motor name "
<< motorName << " is not part of the motor list" << std::endl;
return false;
}
}
// Zero the row of the selected axis in velocity coupling matrix
coupling_matrix.fromJointVelocitiesToMotorVelocities.row(axis_id).setZero();
// Get non-zero coefficient of the coupling matrices
for(int coupled_motor=0; coupled_motor < axis_coupling_bot->size(); coupled_motor++ )
{
double coeff = axis_coupling_bot->get(coupled_motor).asList()->get(0).asDouble();
std::string motorName = axis_coupling_bot->get(coupled_motor).asList()->get(1).asString();
int motorIndex = findAndReturnIndex(motorNames,motorName);
if( motorIndex == -1 )
{
return false;
}
coupling_matrix.fromJointVelocitiesToMotorVelocities(axis_id,motorIndex) = coeff;
}
}
std::cerr << "loadCouplingMatrix DEBUG: " << std::endl;
std::cerr << "loaded kinematic coupling matrix from group " << group_name << std::endl;
std::cerr << coupling_matrix.fromJointVelocitiesToMotorVelocities << std::endl;
// std::cerr << "loaded torque coupling matrix from group " << group_name << std::endl;
// std::cerr << coupling_matrix.fromJointTorquesToMotorTorques << std::endl;
coupling_matrix.fromJointTorquesToMotorTorques = coupling_matrix.fromJointVelocitiesToMotorVelocities.transpose();
coupling_matrix.fromMotorTorquesToJointTorques = coupling_matrix.fromJointTorquesToMotorTorques.inverse();
coupling_matrix.fromJointVelocitiesToMotorVelocities = coupling_matrix.fromJointVelocitiesToMotorVelocities.inverse();
// Compute the torque coupling matrix
return true;
}
}
