void ObjectModelGrid::addContactPoint(const yarp::sig::Vector fingertipPosition){

    assert(fingertipPosition.size() == 3);
    cout << "Contact position: " << fingertipPosition.toString() << endl;
    _contactPoints.push_back(fingertipPosition);


}
bool reactCtrlThread::controlArm(const yarp::sig::Vector &_vels)
{   
    VectorOf<int> jointsToSetA;
    VectorOf<int> jointsToSetT;
    if (!areJointsHealthyAndSet(jointsToSetA,"arm","velocity"))
    {
        yWarning("[reactCtrlThread]Stopping control because arm joints are not healthy!");
        stopControlHelper();
        return false;
    }

    if (useTorso)
    {
        if (!areJointsHealthyAndSet(jointsToSetT,"torso","velocity"))
        {
            yWarning("[reactCtrlThread]Stopping control because torso joints are not healthy!");
            stopControlHelper();
            return false;
        }
    }

    if (!setCtrlModes(jointsToSetA,"arm","velocity"))
    {
        yError("[reactCtrlThread]I am not able to set the arm joints to velocity mode!");
        return false;
    }   

    if (useTorso)
    {
        if (!setCtrlModes(jointsToSetT,"torso","velocity"))
        {
            yError("[reactCtrlThread]I am not able to set the torso joints to velocity mode!");
            return false;
        }
    }

    printMessage(1,"Moving the robot with velocities: %s\n",_vels.toString(3,3).c_str());
    if (useTorso)
    {
        Vector velsT(3,0.0);
        velsT[0] = _vels[2]; //swapping pitch and yaw as per iKin vs. motor interface convention
        velsT[1] = _vels[1];
        velsT[2] = _vels[0]; //swapping pitch and yaw as per iKin vs. motor interface convention
        
        ivelT->velocityMove(velsT.data());
        ivelA->velocityMove(_vels.subVector(3,9).data()); //indexes 3 to 9 are the arm joints velocities
    }
    else
    {
        ivelA->velocityMove(_vels.data()); //if there is not torso, _vels has only the 7 arm joints
    }

    return true;
}
Beispiel #3
0
/**
 * Write the calibration matrix to a file suitable to be used by the IIT FTsens sensor.
 * calibration_matrix should map the raw straing gauge values to SI (Newton,NewtonMeters)
 * units.
 *
 * Full scale vector should contain the desired full scales in SI units.
 */
bool writeFTsensCalibrationToFile(std::string         filename,
                                  yarp::sig::Matrix & calibration_matrix,
                                  yarp::sig::Vector & full_scale)
{
    if( calibration_matrix.rows() != 6 ||
        calibration_matrix.cols() != 6 ||
        full_scale.size()         != 6 )
    {
        return false;
    }

    std::ofstream myfile;
    myfile.open (filename.c_str());
    if( !myfile.is_open() )
    {
        std::cerr << "[ERROR] Error in writing calibration matrix to " << filename << std::endl;
        return false;
    }

    std::cout << "insituFTSensorCalibration module: writing to file calibration matrix: " << std::endl;
    std::cout << calibration_matrix.toString() << std::endl;
    std::cout << "insituFTSensorCalibration module: with full scale: " << std::endl;
    std::cout << full_scale.toString() << std::endl;

    //It seems that full_scale is required to be an integer, TODO check
    std::vector<int> full_scale_int;
    full_scale_int.resize(6);

    for(int i=0; i < 6; i++ )
    {
        full_scale_int[i] = round(full_scale[i]);
    }

    for(int i=0; i < 6; i++ )
    {
        for(int j=0; j < 6; j++ )
        {
            //The matrix that is passed to the actual sensor use
            //coefficients gains that are encoded with respect to
            //the full scale
            double firmware_coeff = calibration_matrix(i,j)/((double)full_scale_int[i]);
            //Then this firmware coefficient is expressed in 1.15 two complement fixed point way
            //that we encode in the file in hex format TODO CHECK endianess problems
            std::string hex;
            if( !convert_onedotfifteen(firmware_coeff,hex) )
            {
                std::cerr << "[ERROR] Error in writing calibration matrix to file, for the given choice"
                          << " of fullscale the " << i << " " << j << " coefficient is not in [-1.0,1.0]" << std::endl;
                myfile.close();
                return false;
            }

            myfile << hex << "\r\n";
        }
    }

    myfile << 1 << "\r\n";

    for(int i=0; i < 6; i++ )
    {
        myfile << full_scale_int[i] << "\r\n";
    }

    myfile.close();

    return true;
}
Beispiel #4
0
void wysiwyd::wrdac::SubSystem_iKart::goToWayPoint(const yarp::sig::Vector &wp)
{
    refreshOdometryValues();
    printOdometryValues();

    double tolerance = 0.01;
    double distanceToTarget = sqrt(pow(wp[0] - odometry_x, 2.0) + pow(wp[1] - odometry_y, 2.0));
    double angleToTarget = fabs(wp[2] - odometry_orientation);
    double toleranceAngle = 2.0;
    while (distanceToTarget > tolerance || angleToTarget > toleranceAngle)
    {
        //Refresh the heading based on current odometry
        yarp::sig::Vector currentWp(3, 0.0);
        currentWp[0] = odometry_x;
        currentWp[1] = odometry_y;
        currentWp[2] = odometry_orientation;
        double a = currentWp[2] * M_PI / 180.0;
        std::cout << "alpha=" << a << std::endl;
        yarp::sig::Matrix H(4, 4);
        H.eye();
        H(0, 0) = H(1, 1) = cos(a);
        H(0, 1) = sin(a);
        H(1, 0) = -H(0, 1);
        H(0, 3) = currentWp[0];
        H(1, 3) = currentWp[1];

        yarp::sig::Vector localWp(4, 0.0);
        localWp[0] = wp[0];
        localWp[1] = wp[1];
        localWp[3] = 1.0;

        localWp = yarp::math::operator*(yarp::math::pinv(H), localWp);
        std::cout << "H=" << H.toString(3, 3).c_str() << std::endl;
        std::cout << "Going to a waypoint at : " << wp.toString(3, 3).c_str() << std::endl;
        std::cout << "Current position is : " << currentWp.toString(3, 3).c_str() << std::endl;
        std::cout << "Local expression of waypoint : " << localWp.toString(3, 3).c_str() << std::endl;

        double dX = localWp[0];
        double dY = localWp[1];
        double current_heading = M_PI / 2.0 - atan2(dY, dX);
        current_heading = 180.0 * current_heading / M_PI;

        //Calculate angular speed
        double angularSpeed = std::max(-5.0, std::min(5.0, 0.8*(wp[2] - currentWp[2])));
        double linearSpeed = std::min(0.1, 0.8*(distanceToTarget));

        yarp::os::Bottle bCmd;
        bCmd.addInt(2);
        bCmd.addDouble(current_heading); //heading
        bCmd.addDouble(linearSpeed); //linear speed
        bCmd.addDouble(angularSpeed); //angular speed
        portCmd.write(bCmd);
        yarp::os::Time::delay(0.02);

        refreshOdometryValues();
        distanceToTarget = sqrt(pow(wp[0] - odometry_x, 2.0) + pow(wp[1] - odometry_y, 2.0));
        angleToTarget = fabs(wp[2] - currentWp[2]);
        printOdometryValues();
        std::cout << "Speed : " << linearSpeed << "m/s \t" << angularSpeed << "deg/s" << std::endl;
        std::cout << "Distance to target : " << distanceToTarget << std::endl;
    }

    //Zero the velocity
    yarp::os::Bottle bCmd;
    bCmd.addInt(2);
    bCmd.addDouble(0.0); //heading
    bCmd.addDouble(0.0); //linear speed
    bCmd.addDouble(0.0); //angular speed
    portCmd.write(bCmd);
    std::cout << "ok" << std::endl;
}