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