예제 #1
0
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;
    }

}