Esempio n. 1
0
  void YouBotBaseService::update()
  {
    if(is_in_visualization_mode)
    {
      in_odometry_state.read(m_odometry_state);

      simxFloat pos[3];
      pos[0] = m_odometry_state.pose.pose.position.x;
      pos[1] = m_odometry_state.pose.pose.position.y;
      pos[2] = m_odometry_state.pose.pose.position.z;

      simxSetObjectPosition(clientID,all_robot_handle,-1,pos,simx_opmode_oneshot);

      double euler[3];
      simxFloat euler_s[3];

      //different coordinate systems, fixing it here
      KDL::Rotation orientation  = KDL::Rotation::Quaternion(
                          m_odometry_state.pose.pose.orientation.x,
                          m_odometry_state.pose.pose.orientation.y,
                          m_odometry_state.pose.pose.orientation.z,
                          m_odometry_state.pose.pose.orientation.w);

      // Instead of transforming for the inverse of this rotation,
      // we should find the right transform. (this is legacy code that
      // transforms vrep coords to rviz coords)
      KDL::Rotation rotation = KDL::Rotation::RPY(0,-M_PI/2,M_PI).Inverse();
      
      orientation = orientation * rotation;
      orientation.GetRPY(euler[0],euler[1],euler[2]); 

      euler_s[0] = euler[0];
      euler_s[1] = euler[1];
      euler_s[2] = euler[2];

      simxSetObjectOrientation(clientID,all_robot_handle,-1,euler_s,simx_opmode_oneshot);

      simxSetJointPosition(clientID,vrep_joint_handle[0],m_joint_state.position[0], simx_opmode_oneshot);
      simxSetJointPosition(clientID,vrep_joint_handle[1],m_joint_state.position[1], simx_opmode_oneshot);
      simxSetJointPosition(clientID,vrep_joint_handle[2],m_joint_state.position[2], simx_opmode_oneshot);
      simxSetJointPosition(clientID,vrep_joint_handle[3],m_joint_state.position[3], simx_opmode_oneshot);
      return;
    }

      Hilas::IRobotBaseService::update();
  }
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);
}