void YouBotBaseService::setTwistSetpoints() { cmd_twist.read(m_cmd_twist); std::vector<quantity<angular_velocity> > wheelVelocities; quantity<si::velocity> longitudinalVelocity; quantity<si::velocity> transversalVelocity; quantity<si::angular_velocity> angularVelocity; longitudinalVelocity = m_cmd_twist.linear.x * meter_per_second; transversalVelocity = m_cmd_twist.linear.y * meter_per_second; angularVelocity = m_cmd_twist.angular.z * radian_per_second; youBotBaseKinematic.cartesianVelocityToWheelVelocities(longitudinalVelocity, transversalVelocity, angularVelocity, wheelVelocities); // Little hack int sign[4] = {1,-1,1,-1}; simxPauseCommunication(clientID,1); for(unsigned int i=0; i < wheelVelocities.size() ; i++) { simxSetJointTargetVelocity(clientID,vrep_joint_handle[i],wheelVelocities[i].value() * sign[i], simx_opmode_oneshot); } simxPauseCommunication(clientID,0); }
void RobotVREP::pauseCommunication(int action) { simxPauseCommunication(clientID, action); }
void YouBotBaseService::setJointSetpoints() { joint_position_command.read(m_joint_position_command); joint_velocity_command.read(m_joint_velocity_command); joint_effort_command.read(m_joint_effort_command); FlowStatus f = joint_position_command.read(m_joint_position_command); FlowStatus f1 = joint_velocity_command.read(m_joint_velocity_command); FlowStatus f2 = joint_effort_command.read(m_joint_effort_command); simxPauseCommunication(clientID,1); for (unsigned int joint_nr = 0; joint_nr < YouBot::NR_OF_BASE_SLAVES; ++joint_nr) { switch (m_joint_ctrl_modes[joint_nr]) { case(Hilas::PLANE_ANGLE): { //if(f != NewData) break; simxSetJointTargetPosition(clientID,vrep_joint_handle[joint_nr],m_joint_position_command.positions[joint_nr], simx_opmode_oneshot); break; } case(Hilas::ANGULAR_VELOCITY): { //if(f1 != NewData) break; // Little hack int sign[4] = {1,-1,1,-1}; simxSetJointTargetVelocity(clientID,vrep_joint_handle[joint_nr],m_joint_velocity_command.velocities[joint_nr] * sign[joint_nr], simx_opmode_oneshot); break; } case(Hilas::TORQUE): { //if(f2 != NewData) break; simxSetJointForce(clientID,vrep_joint_handle[joint_nr],m_joint_effort_command.efforts[joint_nr], simx_opmode_oneshot); break; } case(Hilas::MOTOR_STOP): { simxSetJointTargetPosition(clientID,vrep_joint_handle[0],m_joint_state.position[0], simx_opmode_oneshot); simxSetJointTargetPosition(clientID,vrep_joint_handle[1],m_joint_state.position[1], simx_opmode_oneshot); simxSetJointTargetPosition(clientID,vrep_joint_handle[2],m_joint_state.position[2], simx_opmode_oneshot); simxSetJointTargetPosition(clientID,vrep_joint_handle[3],m_joint_state.position[3], simx_opmode_oneshot); break; } case(Hilas::TWIST): { log(Error) << "Cannot be in TWIST ctrl_mode (programming error)" << endlog(); this->getOwner()->error(); break; } default: { log(Error) << "Case not recognized." << endlog(); this->getOwner()->error(); break; } } } simxPauseCommunication(clientID,0); }
int main(int argc, char* argv[]) { int portNb=0; int motorHandles[7]; int motorHandles2[7]; if (argc>=9+7) { // We get the port and the motor handles (for the visual feedback, not for IK!) via command-line arguments portNb=atoi(argv[1]); motorHandles[0]=atoi(argv[2]); motorHandles[1]=atoi(argv[3]); motorHandles[2]=atoi(argv[4]); motorHandles[3]=atoi(argv[5]); motorHandles[4]=atoi(argv[6]); motorHandles[5]=atoi(argv[7]); motorHandles[6]=atoi(argv[8]); motorHandles2[0]=atoi(argv[9]); motorHandles2[1]=atoi(argv[10]); motorHandles2[2]=atoi(argv[11]); motorHandles2[3]=atoi(argv[12]); motorHandles2[4]=atoi(argv[13]); motorHandles2[5]=atoi(argv[14]); motorHandles2[6]=atoi(argv[15]); } else { printf("Indicate following arguments: 'portNumber motor1Handle motor2Handle .. motor7Handle'!\n"); extApi_sleepMs(5000); return 0; } // Read the kinematic file: FILE *file; file=fopen("lbr_iiwa_7_r800.ik","rb"); unsigned char* data=NULL; int dataLength=0; if (file) { fseek(file,0,SEEK_END); unsigned long fl=ftell(file); dataLength=(int)fl; fseek(file,0,SEEK_SET); data=new unsigned char[dataLength]; fread((char*)data,dataLength,1,file); fclose(file); } else { printf("The kinematic content file 'lbr.ik' could not be read!\n"); extApi_sleepMs(5000); return 0; } // Initialize the embedded robot model1: int handle1=simEmbLaunch(); simEmbStart(data,dataLength); // Initialize the embedded robot model2: int handle2=simEmbLaunch(); simEmbStart(data,dataLength); delete[] data; simEmbSwitch(handle1); // use robot model1 // Connect to V-REP at the above specified port, via the remote API. V-REP is just used for visual feed-back, not IK calculation! int clientID=simxStart("127.0.0.1",portNb,true,true,2000,5); if (clientID!=-1) { float simulationStep; simxGetFloatingParameter(clientID,sim_floatparam_simulation_time_step,&simulationStep,simx_opmode_streaming); simxSynchronous(clientID,1); // We enable the synchronous mode, so that we can trigger each simulation step from here int embeddedModelMotorHandles[7]; embeddedModelMotorHandles[0]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint1"); embeddedModelMotorHandles[1]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint2"); embeddedModelMotorHandles[2]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint3"); embeddedModelMotorHandles[3]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint4"); embeddedModelMotorHandles[4]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint5"); embeddedModelMotorHandles[5]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint6"); embeddedModelMotorHandles[6]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint7"); int embeddedModelTargetHandle=simEmbGetObjectHandle("LBR_iiwa_7_R800_target"); int embeddedModelBaseHandle=simEmbGetObjectHandle("LBR_iiwa_7_R800"); extIkReal v=0.0; // Get the initial target dummy matrix, of the embedded model: extIkReal matrix[12]; simEmbGetObjectMatrix(embeddedModelTargetHandle,embeddedModelBaseHandle,matrix); int embeddedModelMotorHandles2[7]; embeddedModelMotorHandles2[0]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint1"); embeddedModelMotorHandles2[1]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint2"); embeddedModelMotorHandles2[2]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint3"); embeddedModelMotorHandles2[3]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint4"); embeddedModelMotorHandles2[4]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint5"); embeddedModelMotorHandles2[5]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint6"); embeddedModelMotorHandles2[6]=simEmbGetObjectHandle("LBR_iiwa_7_R800_joint7"); int embeddedModelTargetHandle2=simEmbGetObjectHandle("LBR_iiwa_7_R800_target"); int embeddedModelBaseHandle2=simEmbGetObjectHandle("LBR_iiwa_7_R800"); extIkReal v2=0.0; // Get the initial target dummy matrix, of the embedded model: extIkReal matrix2[12]; simEmbGetObjectMatrix(embeddedModelTargetHandle2,embeddedModelBaseHandle2,matrix2); while (simxGetConnectionId(clientID)!=-1) { // Following 3 commands will slow down the simulation, but garantee that if the simulation time step was changed, // that there won't be any jumps. Following 3 commands are not needed if you don't modify the simulation time step // (i.e. dt) during simulation. simxUChar vrepWaitingForTrigger=0; while ( (vrepWaitingForTrigger==0)&&(simxGetConnectionId(clientID)!=-1) ) simxGetBooleanParameter(clientID,sim_boolparam_waiting_for_trigger,&vrepWaitingForTrigger,simx_opmode_oneshot_wait); simxGetFloatingParameter(clientID,sim_floatparam_simulation_time_step,&simulationStep,simx_opmode_buffer); v+=extIkReal(0.2)*extIkReal(simulationStep); v2+=extIkReal(0.28)*extIkReal(simulationStep); simEmbSwitch(handle1); // use robot model1 // Set the desired tip matrix by setting the target dummy matrix: matrix[3]=extIkReal(-0.3-cos(v)*0.1); matrix[7]=extIkReal(sin(v)*0.1); matrix[11]=extIkReal(0.629+sin(v*9)*0.01); simEmbSetObjectMatrix(embeddedModelTargetHandle,embeddedModelBaseHandle,matrix); // calculate IK: simEmbHandleIkGroup(sim_handle_all); // Read the corresponding motor angles and send them to V-REP: simxPauseCommunication(clientID,1); // Temporarily pause the remote API communication, in order to send all following commands at once extIkReal pos; simEmbGetJointPosition(embeddedModelMotorHandles[0],&pos); simxSetJointPosition(clientID,motorHandles[0],(float)pos,simx_opmode_oneshot); simEmbGetJointPosition(embeddedModelMotorHandles[1],&pos); simxSetJointPosition(clientID,motorHandles[1],(float)pos,simx_opmode_oneshot); simEmbGetJointPosition(embeddedModelMotorHandles[2],&pos); simxSetJointPosition(clientID,motorHandles[2],(float)pos,simx_opmode_oneshot); simEmbGetJointPosition(embeddedModelMotorHandles[3],&pos); simxSetJointPosition(clientID,motorHandles[3],(float)pos,simx_opmode_oneshot); simEmbGetJointPosition(embeddedModelMotorHandles[4],&pos); simxSetJointPosition(clientID,motorHandles[4],(float)pos,simx_opmode_oneshot); simEmbGetJointPosition(embeddedModelMotorHandles[5],&pos); simxSetJointPosition(clientID,motorHandles[5],(float)pos,simx_opmode_oneshot); simEmbGetJointPosition(embeddedModelMotorHandles[6],&pos); simxSetJointPosition(clientID,motorHandles[6],(float)pos,simx_opmode_oneshot); simEmbSwitch(handle2); // use robot model2 // Set the desired tip matrix by setting the target dummy matrix: matrix2[3]=extIkReal(-0.3-cos(v2)*0.1); matrix2[7]=extIkReal(sin(v2)*0.1); simEmbSetObjectMatrix(embeddedModelTargetHandle2,embeddedModelBaseHandle2,matrix2); // calculate IK: simEmbHandleIkGroup(sim_handle_all); // Read the corresponding motor angles and send them to V-REP: simEmbGetJointPosition(embeddedModelMotorHandles2[0],&pos); simxSetJointPosition(clientID,motorHandles2[0],(float)pos,simx_opmode_oneshot); simEmbGetJointPosition(embeddedModelMotorHandles2[1],&pos); simxSetJointPosition(clientID,motorHandles2[1],(float)pos,simx_opmode_oneshot); simEmbGetJointPosition(embeddedModelMotorHandles2[2],&pos); simxSetJointPosition(clientID,motorHandles2[2],(float)pos,simx_opmode_oneshot); simEmbGetJointPosition(embeddedModelMotorHandles2[3],&pos); simxSetJointPosition(clientID,motorHandles2[3],(float)pos,simx_opmode_oneshot); simEmbGetJointPosition(embeddedModelMotorHandles2[4],&pos); simxSetJointPosition(clientID,motorHandles2[4],(float)pos,simx_opmode_oneshot); simEmbGetJointPosition(embeddedModelMotorHandles2[5],&pos); simxSetJointPosition(clientID,motorHandles2[5],(float)pos,simx_opmode_oneshot); simEmbGetJointPosition(embeddedModelMotorHandles2[6],&pos); simxSetJointPosition(clientID,motorHandles2[6],(float)pos,simx_opmode_oneshot); simxPauseCommunication(clientID,0); // Unpause the remote API communication // Now step the simulation on V-REP side: int r=simx_return_remote_error_flag; // means for next remote API function call: step not triggered while (r==simx_return_remote_error_flag) r=simxSynchronousTrigger(clientID); // Trigger next simulation step if (r!=simx_return_ok) break; printf("."); } simEmbShutDown(); // End the external IK simEmbShutDown(); // End the external IK simxFinish(clientID); // End the remote API } return(0); }