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;
}
/**
* 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;
}
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;
}