robotlabmate.cpp

多机器人合作中的动态角色分配仿真算法是多机器人合作领域的一个比较著名的仿真软件

// RobotLabmate.cpp: implementation of the CRobotLabmate class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "simulator.h"
#include "RobotLabmate.h"
#include "const.h"
#include <math.h>

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif


#define MAX_ARM_DISP 35


//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
IMPLEMENT_SERIAL(CRobotLabmate, CRobotNonHolonomic, 1 )

// onstructor
CRobotLabmate::CRobotLabmate(double x, double y, double theta, short status, short id, CString name) : CRobotNonHolonomic(x, y, theta, status, id, name)
{
	m_sizex = 70;
	m_sizey = 30;
	m_beginLead = 0;
		
	for (short i=0; i<3; i++){
		m_force[i] = 0;
		m_force_w[i] = 0;
	}

	// transformations between World (W), Robot (R) amd Arm (A) coordinates
	m_tR2A[0] = -m_sizex/2;
	m_tR2A[1] = 0;
	m_tR2A[2] = 0;

	m_tA2R[0] = +m_sizex/2;
	m_tA2R[1] = 0;
	m_tA2R[2] = 0;

	// Position of the ARM in the three coordinates
	m_armA[0] = MAX_ARM_DISP;
	m_armA[1] = 0;
	m_armA[2] = 0;

	m_armR[0] = m_armA[0] * cos(m_tA2R[2]) - m_armA[1] * sin(m_tA2R[2]) + m_tA2R[0];
	m_armR[1] = m_armA[0] * sin(m_tA2R[2]) + m_armA[1] * cos(m_tA2R[2]) + m_tA2R[1];
	m_armR[2] = m_armA[2] + m_tA2R[2];

	m_armW[0] = m_armR[0] * cos(m_theta) - m_armR[1] * sin(m_theta) + m_x;
	m_armW[1] = m_armR[0] * sin(m_theta) + m_armR[1] * cos(m_theta) + m_y;
	m_armW[2] = m_armR[2] + m_theta;

	ComputeArmPos();

	double a = m_sizex/2, b = m_sizey; // robot dimensions
	m_polygon.AddPoint( CPoint( round( m_x - a*cos(m_theta) + b*sin(m_theta) ), round( m_y - a*sin(m_theta) - b*cos(m_theta) ) ) );
	m_polygon.AddPoint( CPoint( round( m_x + a*cos(m_theta) + b*sin(m_theta) ), round( m_y - b*cos(m_theta) ) ) );
	m_polygon.AddPoint( CPoint( round( m_x + a*cos(m_theta) - b*sin(m_theta) ), round( m_y + b*cos(m_theta) ) ) );
	m_polygon.AddPoint( CPoint( round( m_x - a*cos(m_theta) - b*sin(m_theta) ), round( m_y - a*sin(m_theta) + b*cos(m_theta) ) ) );
	m_polygon.ComputeCenter();
	m_polygon.SetPointsCenterFrame();
	m_rect = m_polygon.m_rect;
}

// Empty constructor
CRobotLabmate::CRobotLabmate()
{	
	m_sizex = 70;
	m_sizey = 30;
}

CRobotLabmate::~CRobotLabmate()
{

}

// Update robot's position
void CRobotLabmate::Update(CArray<CRobot*, CRobot*> *robots, double simTime, double dt, CBox* box, CMapPath *globalMap)
{
	CDataMsg *dataMsg;
	CControlMsg *controlMsg;

	// calling the sensor method (from CRobot)
	Sensor(globalMap,box);

	// If there is no box, Put the robot in the LEAD mode
	if (!box->m_exist)
		m_status = LEAD;

	// Docking
	if (m_status == DOCK) {
		m_v = 80;
		m_x += m_v * cos(m_theta) * dt;

		// If not in ontact, test if in contact
		if (!m_inContactBox) {
			DetectBox(box);
			
			// if in contact, Initialize arm position related to the box and send message DOCKOK
			if (m_inContactBox) {
				InitializeArmBox(CPoint(round(m_armW[0]), round(m_armW[1])), box, m_theta);

				m_v = 0;
				m_status = WAIT;
				controlMsg = new CControlMsg;
				controlMsg->m_code = DOCKOK;
				controlMsg->m_from = m_id;
				controlMsg->m_tstamp = simTime;
				SendControlMsg(controlMsg);
				return;
			}
		}
	}

	// Pushing the box. When the arm is in position, send message PUSHOK 
	if (m_status == PUSH) {
		if (m_armA[0] >= MAX_ARM_DISP - 10){
			m_v = 10;
			m_x += m_v * cos(m_theta) * dt;
		}
		else {
			m_status = WAIT;
			controlMsg = new CControlMsg;
			controlMsg->m_code = PUSHOK;
			controlMsg->m_tstamp = simTime;
			controlMsg->m_from = m_id;
			controlMsg->m_x = m_x;
			controlMsg->m_y = m_y;
			SendControlMsg(controlMsg);
		}
	}
	
	// If status is leading, the robot follow the planned path. If backing up, it has
	// a special planner. In both cases it broadcasts a data mesage to the other robots.
	if ( (m_status == LEAD) || (m_status == BACKUP) ){

		m_x += m_v * cos(m_theta) * dt;
		m_y += m_v * sin(m_theta) * dt;
		m_theta += m_w * dt;

		if (m_status == LEAD)
			FollowPath(simTime, m_localMap);
		else
			BackupManeuver(simTime, dt);

		// send data message each 0.01 "seconds"
		m_timer += dt;
		if ( m_timer > 0.01 ) {
			m_timer = 0;
			dataMsg = new CDataMsg;
			dataMsg->m_v = m_v;
			dataMsg->m_w = m_w;
			dataMsg->m_theta = m_theta;
			dataMsg->m_tstamp = simTime;
			SendDataMsg(dataMsg);
		}

	}
	// for the follow case, the velocity is set when the robot receives a data msg.
	else if (m_status == FOLLOW) {
	
		m_x += m_v * cos(m_theta) * dt;
		m_y += m_v * sin(m_theta) * dt;
		m_theta += m_w * dt;

	}

	// compute forces applied by the compliant arm
	ComputeForces(box);

	// recompute robot shape (polygon)
	m_polygon.Recompute(m_x, m_y, m_theta);
}


// compute forces applied by the compliant arm
void CRobotLabmate::ComputeForces(CBox* box)
{
	double k, c;
	double delta_p[3];	
	double delta_v[3];	
	double delta_va[3];
	double vbox_r[3];
	
	if ( (m_inContactBox) ) { 

		// computing arm position in the world frame, using the position of the box.
		m_armW[0] = m_armB[0] * cos(box->m_theta) - m_armB[1] * sin(box->m_theta) + box->m_x;
		m_armW[1] = m_armB[0] * sin(box->m_theta) + m_armB[1] * cos(box->m_theta) + box->m_y;
		m_armW[2] = m_armB[2] + box->m_theta;
		
		// Converting arm position from world coordinates to robot coordinates
		m_armR[0] = cos(m_theta) * m_armW[0] + sin(m_theta) * m_armW[1] - m_x * cos(m_theta) - m_y * sin(m_theta);
		m_armR[1] = -sin(m_theta) * m_armW[0] + cos(m_theta) * m_armW[1] - m_y * cos(m_theta) + m_x * sin(m_theta);
		m_armR[2] = m_armW[2] - m_theta;
		
		// Converting arm position from robot coordinates to arm coordinates
		m_armA[0] = m_armR[0] * cos(m_tR2A[2]) - m_armR[1] * sin(m_tR2A[2]) + m_tR2A[0];
		m_armA[1] = m_armR[0] * sin(m_tR2A[2]) + m_armR[1] * cos(m_tR2A[2]) + m_tR2A[1];
		m_armA[2] = m_armR[2] + m_tR2A[2];
		
		//Computing the arm displacements relative to the arm frame
		delta_p[0] =  MAX_ARM_DISP - m_armA[0];
		delta_p[1] =  0 - m_armA[1];
		delta_p[2] =  0 - m_armA[2];

		// Rotating box velocity from world coordinates to robot coordinates
		vbox_r[0] = cos(m_theta) * box->m_v[0] + sin(m_theta) * box->m_v[1];
		vbox_r[1] = -sin(m_theta) * box->m_v[0] + cos(m_theta) * box->m_v[1];
		vbox_r[2] = box->m_v[2];
		
		//Computing velocities relative to the robot
		delta_v[0] =  m_v - vbox_r[0];
		delta_v[1] =  -vbox_r[1];
		delta_v[2] =  -vbox_r[2];

		//Rotating resultant velocities to arm coordinates
		delta_va[0] = cos(m_tR2A[2]) * delta_v[0] - sin(m_tR2A[2]) * delta_v[1];
		delta_va[1] = sin(m_tR2A[2]) * delta_v[0] + cos(m_tR2A[2]) * delta_v[1];
		delta_va[2] = delta_v[2];

		k = 50;
		c = 1;
		
		m_force[0] = k * delta_p[0] + c * delta_va[0]; 
		m_force[1] = k * delta_p[1] + c * delta_va[1]; 
		m_force[2] = k * delta_p[2] + c * delta_va[2]; 
		
		if ( m_force[0] < 0 )	// the robots onlt apply positive forces in x
			m_force[0] = 0;
		
		// Putting the forces in world coordinates to compute the resultant forces.
		m_force_w[0] = m_force[0] * cos(m_theta - m_tR2A[2]) - m_force[1] * sin(m_theta - m_tR2A[2]);  //-m_tR2A[2] = +m_tA2R[2]
		m_force_w[1] = m_force[0] * sin(m_theta - m_tR2A[2]) + m_force[1] * cos(m_theta - m_tR2A[2]);
		m_force_w[2] = m_force[2];
	}
	else
		ComputeArmPos();
}

// Compute th arm position when the robots are not in contact with the box
void CRobotLabmate::ComputeArmPos()
{
	m_force_w[0] = 0;
	m_force_w[1] = 0;
	m_force_w[2] = 0;

	m_armA[0] = MAX_ARM_DISP;
	m_armA[1] = 0;
	m_armA[2] = 0;

	m_armR[0] = m_armA[0] * cos(m_tA2R[2]) - m_armA[1] * sin(m_tA2R[2]) + m_tA2R[0];
	m_armR[1] = m_armA[0] * sin(m_tA2R[2]) + m_armA[1] * cos(m_tA2R[2]) + m_tA2R[1];
	m_armR[2] = m_armA[2] + m_tA2R[2];

	m_armW[0] = m_armR[0] * cos(m_theta) - m_armR[1] * sin(m_theta) + m_x;
	m_armW[1] = m_armR[0] * sin(m_theta) + m_armR[1] * cos(m_theta) + m_y;
	m_armW[2] = m_armR[2] + m_theta;
}

// Draw the robot and the compliant arm
void CRobotLabmate::Draw(CDC *pDC)
{
	CPoint arm[5];

	if (m_tA2R[2] == 0) {
		arm[0] = m_polygon.GetPoint(1);
		arm[1] = CPoint( round( m_armW[0] + m_sizey * sin(m_armW[2]) ),
			             round( m_armW[1] - m_sizey * cos(m_armW[2]) ) );
		arm[2] = CPoint( round( m_armW[0] - m_sizey * sin(m_armW[2]) ),
			             round( m_armW[1] + m_sizey * cos(m_armW[2]) ) );
		arm[3] = m_polygon.GetPoint(2);
		arm[4] = arm[2];

	}
	else {
		double x_r = m_tA2R[0];
		double y_r = m_tA2R[1];

		double x_w = x_r * cos(m_theta) - y_r * sin(m_theta) + m_x;
		double y_w = x_r * sin(m_theta) + y_r * cos(m_theta) + m_y;

		arm[0] = CPoint( round( x_w ),
			             round( y_w ) );
		arm[1] = CPoint( round( m_armW[0] + m_sizey * sin(m_armW[2]) ),
			             round( m_armW[1] - m_sizey * cos(m_armW[2]) ) );
		arm[2] = CPoint( round( m_armW[0] - m_sizey * sin(m_armW[2]) ),
			             round( m_armW[1] + m_sizey * cos(m_armW[2]) ) );
		arm[3] = arm[0];