int MovetoPoint(float *GoalPosition, float minDistance, int clientID, int leftMotorHandle, int rightMotorHandle, int cuboidHandle) { float radius=0.25; float axis=0.5; float P=0.7; float LinVel=0.3; float AngVel; float ObjectPosition[3]; float ObjectOrientation[3]; //rotation about Z --> ObjectOrientation[2] float OrientationError; simxGetObjectPosition(clientID,cuboidHandle,-1,ObjectPosition,simx_opmode_oneshot_wait); float distance=sqrt(pow(ObjectPosition[0]-GoalPosition[0],2)+pow(ObjectPosition[1]-GoalPosition[1],2)+pow(ObjectPosition[2]-GoalPosition[2],2)); auto snow = std::chrono::system_clock::now(); auto sduration = snow.time_since_epoch(); auto smillis = std::chrono::duration_cast<std::chrono::milliseconds>(sduration).count(); while (distance>minDistance) { simxGetObjectOrientation(clientID, cuboidHandle, -1, ObjectOrientation, simx_opmode_streaming); simxGetObjectPosition(clientID,cuboidHandle,-1,ObjectPosition,simx_opmode_oneshot_wait); OrientationError=orientationError(ObjectPosition[0], ObjectPosition[1], ObjectOrientation[2], GoalPosition[0], GoalPosition[1]); AngVel=P*OrientationError; MoveandRotate(LinVel, AngVel, radius, axis, clientID, leftMotorHandle, rightMotorHandle); printf("Distance: %f Robot: %f Error: %f\n", distance, ObjectOrientation[2], OrientationError); distance=sqrt(pow(ObjectPosition[0]-GoalPosition[0],2)+pow(ObjectPosition[1]-GoalPosition[1],2)); //printing data to file auto now = std::chrono::system_clock::now(); auto duration = now.time_since_epoch(); auto millis = std::chrono::duration_cast<std::chrono::milliseconds>(duration).count(); //std::cout << millis-smillis << "\t" << ObjectPosition[0] << "\t" << ObjectPosition[1] << std::endl; } MoveandRotate(0, 0, radius, axis, clientID, leftMotorHandle, rightMotorHandle); }
std::tuple<float, float, float> Object::globalPosition() const { simxFloat *positionVec = new simxFloat[3]; try { ReturnCodesExceptions::handleReturnCode( simxGetObjectPosition(m_ClientId, m_Handle, -1, positionVec, simx_opmode_oneshot_wait)); return std::make_tuple(static_cast<float>(positionVec[0]), positionVec[1], positionVec[2]); } catch (ReturnCodesExceptions *ex) { throw ex; } }
vector < double > RobotVREP::getObjectPosition(Object * object, int relativeTo, simxInt operationMode) { int uniqueId = object->getUniqueObjectId(); int vrepHandlerVectorPosition = objectIdToVrepHandler_map.at(uniqueId); int * handler = VrepHandlerVector.at(vrepHandlerVectorPosition); float * pos = new float[3]; int error = simxGetObjectPosition(clientID, *handler, relativeTo, pos, operationMode); if(error != 0) vrep_error << "simxGetObjectPosition - " << *handler << " : "<< error << endl; vector < double > position; position.push_back((double)pos[0]); position.push_back((double)pos[1]); position.push_back((double)pos[2]); delete[] pos; return position; }
void rys::setTarget() { simxGetObjectPosition(clientID,goalHandle,-1,goalPosition,simx_opmode_oneshot_wait); }
void Individuo::initStreamingSensors() { //cout << this->robotName << ": Conexao com motores, sensores e corpo do robo #" + to_string(this->id) + "\n"; sensorName = "Pioneer_p3dx_robot_" + to_string(this->id); if (simxGetObjectHandle(this->clientId, sensorName.c_str(), &body, simx_opmode_oneshot_wait) == simx_return_ok) { //cout << this->robotName << ": Conectado corpo do motor." << std::endl; simxGetObjectPosition(clientId, body, -1, position, simx_opmode_streaming); } else { cout << this->robotName << ": Nao conectado ao corpo do motor." << std::endl; } sensorName = "Pioneer_p3dx_leftMotor_robot_" + to_string(this->id); if (simxGetObjectHandle(this->clientId, sensorName.c_str(), &motorLeft, simx_opmode_oneshot_wait) == simx_return_ok) { //cout << this->robotName << ": Conectado no motor esquerdo." << std::endl; } else cout << this->robotName << ": Nao conectado no motor esquerdo." << std::endl; sensorName = "Pioneer_p3dx_rightMotor_robot_" + to_string(this->id); if (simxGetObjectHandle(this->clientId, sensorName.c_str(), &motorRight, simx_opmode_oneshot_wait) == simx_return_ok){ //cout << this->robotName << ": Conectado no motor direito." << std::endl; } else cout << this->robotName << ": Nao conectado no motor direito." << std::endl; sensorName = "Pioneer_p3dx_caster_freeJoint_robot_" + to_string(this->id); if (simxGetObjectHandle(this->clientId, sensorName.c_str(), &casterWheel, simx_opmode_oneshot_wait) == simx_return_ok) { // cout << this->robotName << ": Conectado a rodinha livre." << std::endl; } else cout << this->robotName << ": Nao conectado a rodinha livre." << std::endl; for (int i = 0; i < 2; i++) { sensorName = "LaserPointer_sensor_front_" + to_string(i + 1)+"_robot_"+to_string(this->id); if (simxGetObjectHandle(this->clientId, sensorName.c_str(), &laserPointerFront[i], simx_opmode_oneshot_wait) == simx_return_ok) { //cout << this->robotName << ": Conectado ao laser pointer front #" << i + 1 << endl; simxReadProximitySensor(this->clientId, laserPointerFront[i], NULL, NULL, NULL, NULL, simx_opmode_streaming); }else { cout << this->robotName << ": Nao conectado ao laser pointer front #" << i + 1 << endl; } sensorName = "LaserPointer_sensor_back_" + to_string(i + 1) + "_robot_" + to_string(this->id); if (simxGetObjectHandle(this->clientId, sensorName.c_str(), &laserPointerBack[i], simx_opmode_oneshot_wait) == simx_return_ok) { //cout << this->robotName << ": Conectado ao laser pointer back #" << i + 1 << endl; simxReadProximitySensor(this->clientId, laserPointerBack[i], NULL, NULL, NULL, NULL, simx_opmode_streaming); }else { cout << this->robotName << ": Nao conectado ao laser pointer back" << i + 1 << endl; } sensorName = "LaserPointer_sensor_right_" + to_string(i + 1) + "_robot_" + to_string(this->id); if (simxGetObjectHandle(this->clientId, sensorName.c_str(), &laserPointerRight[i], simx_opmode_oneshot_wait) == simx_return_ok) { //cout << this->robotName << ": Conectado ao laser pointer right #" << i + 1 << endl; simxReadProximitySensor(this->clientId, laserPointerRight[i], NULL, NULL, NULL, NULL, simx_opmode_streaming); }else { cout << this->robotName << ": Nao conectado ao laser pointer right" << i + 1 << endl; } sensorName = "LaserPointer_sensor_left_" + to_string(i + 1) + "_robot_" + to_string(this->id); if (simxGetObjectHandle(this->clientId, sensorName.c_str(), &laserPointerLeft[i], simx_opmode_oneshot_wait) == simx_return_ok) { //cout << this->robotName << ": Conectado ao laser pointer back #" << i + 1 << endl; simxReadProximitySensor(this->clientId, laserPointerLeft[i], NULL, NULL, NULL, NULL, simx_opmode_streaming); }else { cout << this->robotName << ": Nao conectado ao laser pointer back" << i + 1 << endl; } } /*simxGetObjectPosition(clientId, body, -1, initPosBody, simx_opmode_oneshot_wait); simxGetObjectPosition(clientId, motorLeft, -1, initPosLeftWheel, simx_opmode_oneshot_wait); simxGetObjectPosition(clientId, motorRight, -1, initPosRightWheel, simx_opmode_oneshot_wait); simxGetObjectPosition(clientId, casterWheel, -1, initPosCasterWheel, simx_opmode_oneshot_wait);*/ /*cout << "X: " << initPosBody[0] << " Y: " << initPosBody[1] << " Z: " << initPosBody[2] << endl; cout << "X: " << initPosLeftWheel[0] << " Y: " << initPosLeftWheel[1] << " Z: " << initPosLeftWheel[2] << endl; cout << "X: " << initPosRightWheel[0] << " Y: " << initPosRightWheel[1] << " Z: " << initPosRightWheel[2] << endl; cout << "X: " << initPosCasterWheel[0] << " Y: " << initPosCasterWheel[1] << " Z: " << initPosCasterWheel[2] << endl;*/ }
bool Individuo::freeFall() { float pos[3]; simxGetObjectPosition(clientId, body, -1, position, simx_opmode_buffer); return (position[2] < MIN_HEIGHT) ? true : false; }
int main(int argc,char* argv[]) { int portNb=0; int leftMotorHandle; int rightMotorHandle; int cuboidHandle; int goalHandle; if (argc>=6) { portNb=atoi(argv[1]); leftMotorHandle=atoi(argv[2]); rightMotorHandle=atoi(argv[3]); cuboidHandle=atoi(argv[4]); goalHandle=atoi(argv[5]); } else { extApi_sleepMs(5000); return 0; } int clientID=simxStart((simxChar*)"127.0.0.1",portNb,true,true,2000,5); if (clientID!=-1) { int driveBackStartTime=-99000; float motorSpeeds[2]; float leftMotorAngle; float ObjectPosition[3]; float GoalPosition[3]; float ObjectOrientation[3]; float minDistance=0.3; if (simxGetConnectionId(clientID)!=-1) { simxUChar sensorTrigger=0; simxGetObjectHandle(clientID, argv[2], &leftMotorHandle, simx_opmode_blocking); simxGetObjectHandle(clientID, argv[3], &rightMotorHandle, simx_opmode_blocking); simxGetObjectHandle(clientID, argv[4], &cuboidHandle, simx_opmode_blocking); simxGetObjectHandle(clientID, argv[5], &goalHandle, simx_opmode_blocking); simxGetObjectPosition(clientID,goalHandle,-1,GoalPosition,simx_opmode_oneshot_wait); MovetoPoint(GoalPosition, minDistance, clientID, leftMotorHandle, rightMotorHandle, cuboidHandle); simxSetJointTargetVelocity(clientID,leftMotorHandle,0,simx_opmode_blocking); simxSetJointTargetVelocity(clientID,rightMotorHandle,0,simx_opmode_blocking); extApi_sleepMs(5); } printf("Fin!\n"); simxFinish(clientID); } return(0); }