mysdopenglview.cpp

机器人运动学动力学及控制的一个小程序,供有兴趣的人参考

// MySDOpenGLView.cpp : implementation of the CMySDOpenGLView class
//

#include "stdafx.h"
#include "MySDOpenGL.h"

#include "MySDOpenGLDoc.h"
#include "MySDOpenGLView.h"
#include <math.h>
#include <string.h>

//#include "md2.h"
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

#define PI 3.1415926



#define MODEL_1 1
#define MODEL_2 2
#define MODEL_3 3
int model_pick=0;
float model1_scaling=0;
float model2_scaling=0;
float model3_scaling=0;
int  drawmodel=RENDER;
//RENDER SELECT
int winWidth,winHeight;

int w,h;
ModelContext *model1;
ModelContext *model2;
ModelContext *model3;
int drawSetting=POLYGONS;
BOOL F=true;
BOOL onetime=true;
CSDDynamics  xx;
CMatrix theta;
CMatrix dtheta;
CMatrix ddtheta;
CMatrix force;
CControl control;
CMatrix beata;
/////////////////////////////////////////////////////////////////////////////
// CMySDOpenGLView

IMPLEMENT_DYNCREATE(CMySDOpenGLView, CView)

BEGIN_MESSAGE_MAP(CMySDOpenGLView, CView)
	//{{AFX_MSG_MAP(CMySDOpenGLView)
	ON_WM_CREATE()
	ON_WM_DESTROY()
	ON_WM_SIZE()
	ON_WM_TIMER()
	ON_WM_LBUTTONDOWN()
	ON_WM_MOUSEMOVE()
	ON_WM_LBUTTONUP()
	ON_COMMAND(ID_TX_SET, OnTxSet)
	ON_UPDATE_COMMAND_UI(ID_MODE, OnUpdateMode)
	ON_COMMAND(ID_MODE, OnMode)
	//}}AFX_MSG_MAP
	// Standard printing commands
	ON_COMMAND(ID_FILE_PRINT, CView::OnFilePrint)
	ON_COMMAND(ID_FILE_PRINT_DIRECT, CView::OnFilePrint)
	ON_COMMAND(ID_FILE_PRINT_PREVIEW, CView::OnFilePrintPreview)
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CMySDOpenGLView construction/destruction

CMySDOpenGLView::CMySDOpenGLView()
{
//	m_pPropFrame = NULL;
	// TODO: add construction code here

}

CMySDOpenGLView::~CMySDOpenGLView()
{
}

BOOL CMySDOpenGLView::PreCreateWindow(CREATESTRUCT& cs)
{
	// TODO: Modify the Window class or styles here by modifying
	//  the CREATESTRUCT cs
 //  cs.style|=WS_CLIPCHILDREN|WS_CLIPSIBLINGS;
	return CView::PreCreateWindow(cs);
}

/////////////////////////////////////////////////////////////////////////////
// CMySDOpenGLView drawing

void CMySDOpenGLView::OnDraw(CDC* pDC)
{
	CMySDOpenGLDoc* pDoc = GetDocument();
	ASSERT_VALID(pDoc);
	// TODO: add draw code for native data here
//	 CMySDOpenGLDoc *temp=GetDocument();
//GetDocument()->m_strgMatrix=g.ToString("   ");
//     F=pDoc->F;

if(pDoc->Reset){
     Angle1=0.0;
	 Angle2=0.0;
	 Angle3=0.0;
	 Angle4=0.0;

     inputAngle1=0.0;
     inputAngle2=0.0;
     inputAngle3=0.0;
     inputAngle4=0.0;

     dtheta=pDoc->lpTransport->dtheta;

     pDoc->Reset=false;

}

	if(pDoc->F){
     inputAngle1=pDoc->inputAngle1;
     inputAngle2=pDoc->inputAngle2;
     inputAngle3=pDoc->inputAngle3;
     inputAngle4=pDoc->inputAngle4;

    MakeSE3();

//	   CString	m_strMatrix3 = g.ToString(" ");
//	AfxMessageBox(m_strMatrix3, MB_OK|MB_ICONINFORMATION);
pDoc->m_strgMatrix=g.ToString("   ");
	CoumpterFKinematics(g);
	 Angle1=0.0;
	 Angle2=0.0;
	 Angle3=0.0;
	 Angle4=0.0;
     deltaAngle1=inputAngle1/100;
     deltaAngle2=inputAngle2/100;
     deltaAngle3=inputAngle3/100;
     deltaAngle4=inputAngle4/100;
	 pDoc->F=false;
	}
//	if(drawmodel==SELECT)

	if(onetime){pDoc->m_strgMatrix=g.ToString("   ");onetime=false;}
	if(pDoc->IKStart){
		g.FromString(pDoc->m_strgMatrix);
	if(CoumpterIKinematics(g))
	{
///* 
     pDoc->inputAngle1=IAngle1[0];
     pDoc->inputAngle2=IAngle2[0];
     pDoc->inputAngle3=IAngle3[0];
     pDoc->inputAngle4=IAngle4[0];
	CoumpterFKinematics(g);
	//*/
     inputAngle1=pDoc->inputAngle1;
     inputAngle2=pDoc->inputAngle2;
     inputAngle3=pDoc->inputAngle3;
     inputAngle4=pDoc->inputAngle4;
     Angle1=0.0;
	 Angle2=0.0;
	 Angle3=0.0;
	 Angle4=0.0;
     deltaAngle1=inputAngle1/100;
     deltaAngle2=inputAngle2/100;
     deltaAngle3=inputAngle3/100;
     deltaAngle4=inputAngle4/100;
}
pDoc->IKStart=false;
//pDoc->F=false;
//F=false;

	}

//pDoc->Dynamics=true;

	if(pDoc->Dynamics)
	{//  CMatrix theta;
		theta.SetElement(0,0,Angle1);
		theta.SetElement(1,0,Angle2);
		theta.SetElement(2,0,Angle3);
		theta.SetElement(3,0,Angle4);


        beata=pDoc->lpTransport->beata;
		xx.MakeMC(theta,dtheta);
 //        pDoc->lpTransport->gravity=9.8;
		xx.MakeN(theta,dtheta,beata,pDoc->lpTransport->gravity);
       if(pDoc->lpTransport->control)
	   {  control.Designx=pDoc->lpTransport->thetad;
          control.Kp=pDoc->lpTransport->Kp;
		  control.Kv=pDoc->lpTransport->Kv;
          force=control.ControlTorqueCL(dtheta,theta);
	   }
	   else force=force*0;

       double testN=xx.N.GetElement(1,0);
		ddtheta=xx.CaculateDdtheta(theta,dtheta,force);
       double testdd=ddtheta.GetElement(3,0);
	   if(testdd<1e-5) ddtheta.SetElement(3,0,0);
        dtheta=dtheta+ddtheta*0.02;
        deltaAngle1=dtheta.GetElement(0,0);
        deltaAngle2=dtheta.GetElement(1,0);
        deltaAngle3=dtheta.GetElement(2,0);
        deltaAngle4=dtheta.GetElement(3,0);

		inputAngle1=1e8;
		inputAngle2=1e8;
		inputAngle3=1e8;
		inputAngle4=1e8;

	}
		RenderScence();
}

/////////////////////////////////////////////////////////////////////////////
// CMySDOpenGLView printing

BOOL CMySDOpenGLView::OnPreparePrinting(CPrintInfo* pInfo)
{
	// default preparation
	return DoPreparePrinting(pInfo);
}

void CMySDOpenGLView::OnBeginPrinting(CDC* /*pDC*/, CPrintInfo* /*pInfo*/)
{
	// TODO: add extra initialization before printing
}

void CMySDOpenGLView::OnEndPrinting(CDC* /*pDC*/, CPrintInfo* /*pInfo*/)
{
	// TODO: add cleanup after printing
}

/////////////////////////////////////////////////////////////////////////////
// CMySDOpenGLView diagnostics

#ifdef _DEBUG
void CMySDOpenGLView::AssertValid() const
{
	CView::AssertValid();
}

void CMySDOpenGLView::Dump(CDumpContext& dc) const
{
	CView::Dump(dc);
}

CMySDOpenGLDoc* CMySDOpenGLView::GetDocument() // non-debug version is inline
{
	ASSERT(m_pDocument->IsKindOf(RUNTIME_CLASS(CMySDOpenGLDoc)));
	return (CMySDOpenGLDoc*)m_pDocument;
}
#endif //_DEBUG

/////////////////////////////////////////////////////////////////////////////
// CMySDOpenGLView message handlers

int CMySDOpenGLView::OnCreate(LPCREATESTRUCT lpCreateStruct) 
{
	if (CView::OnCreate(lpCreateStruct) == -1)
		return -1;
	
	// TODO: Add your specialized creation code here

    X_Angle=0.0;
     Y_Angle=90.0;

	 inputAngle1=0;
     inputAngle2=0;
     inputAngle3=0;
     inputAngle4=0;
	 MakeSE3();
//	CoumpterFKinematics(g);

/*
//  CVector3 p(0,1,0);
//   CVector3 q(0,1,0);
  CVector3 p(-sqrt(2.0)/2.0,sqrt(2.0)/2.0,0);
  CVector3 q(sqrt(2.0)/2.0,0,0);
  CVector3 r(0,0,0);
  CVector3 a2(0,1,0);
  CVector3 a1(1,0,0);
	  
//       p=(0,1,0);q=(0,-1,0);r=(0,0,0);a=(1,0,0);
  double s1[2],s2[2];
  int n;
  double d=1;
//	 InverseKinematics1(&p,&q,&r,&a2,s1);
//     InverseKinematics2(&p,&q,&r,&a1,&a2,&n,s1,s2);
     InverseKinematics3(&p,&q,&r,&a2,&d,&n,s1);
*/
//	 /*
CVector3 x(0,1,0);
CVector3 y(-1,0,0);
CVector3 z(0,0,1);
//CVector3 p(2.3,1.0,6.0);
CVector3 p(2.3,1,6);

g=Makeg(&x,&y,&z,&p);
//	CMySDOpenGLDoc* pDoc = GetDocument();
//	ASSERT_VALID(pDoc);
// GetDocument()->m_strgMatrix=g.ToString("   ");
//*/
	if(CoumpterIKinematics(g))
	{
///* 
     inputAngle1=IAngle1[0];
     inputAngle2=IAngle2[0];
     inputAngle3=IAngle3[0];
     inputAngle4=IAngle4[0];
	CoumpterFKinematics(g);
	//*/
}
////////////////////////////////////////////////////////

//CMatrix     matrix=TwistToHomogeneous(AxisToTwist(x,p,0));
//CMatrix     matrix1=HomogeneousToTwist(matrix);
double v1[]={0,0,0,0,1,0};
double v2[]={0,0,-1,1,0,0};
double v3[]={0,2,-1,1,0,0};
double v4[]={1,-1.5,0,0,0,1};
CMatrix xi1(6,1,v1);
CMatrix xi2(6,1,v2);
CMatrix xi3(6,1,v3);
CMatrix xi4(6,1,v4);
//CMatrix    matrix=TwistExp2(xi2,90);
//CMatrix    matrix1=InvertRigidAdjoint(matrix);
//CMatrix     matrix1=RigidAdjoint(matrix);
//BOOL       B=    xx.Init(2);
xx.Init(4);

CVector3 q1(0,1,0);
CVector3 r1(0,-1,0);
CVector3 q2(0,1,0);
CVector3 r2(1.5,0,1);
CVector3 q3(0,1,2);
CVector3 r3(1.5,0,1);
CVector3 q4(1.5,1,0);
CVector3 r4(0,0,6);

double m1=24000;
double m2=31000;
double m3=21000;
double m4=8000;

double l1=2;

double ix1=40000;
double iy1=3000;
double iz1=40000;
CMatrix it1(3);
        it1.SetElement(0,0,ix1);
        it1.SetElement(1,1,iy1);
        it1.SetElement(2,2,iz1);

double ix2=34000;
double iy2=28000;
double iz2=8000;
CMatrix it2(3);
        it2.SetElement(0,0,ix2);
        it2.SetElement(1,1,iy2);
        it2.SetElement(2,2,iz2);

double ix3=17000;
double iy3=14000;
double iz3=4000;
CMatrix it3(3);
        it3.SetElement(0,0,ix3);
        it3.SetElement(1,1,iy3);
        it3.SetElement(2,2,iz3);

double ix4=16000;
double iy4=16000;
double iz4=400;
CMatrix it4(3);
        it4.SetElement(0,0,ix4);
        it4.SetElement(1,1,iy4);
        it4.SetElement(2,2,iz4);

CRigid rigid1;
       rigid1.q=q1;
       rigid1.r=r1;
	   rigid1.m=m1;
       rigid1.InertiaT=it1;
       rigid1.MakeGInertiaM();

       rigid1.xi=xi1;
//       rigid1.MakeTwistExp(90);
CRigid rigid2;
       rigid2.q=q2;
       rigid2.r=r2;
	   rigid2.m=m2;
       rigid2.l=2;
       rigid2.InertiaT=it2;
       rigid2.MakeGInertiaM();

       rigid2.xi=xi2;

CRigid rigid3;
	   rigid3.q=q3;
       rigid3.r=r3;
	   rigid3.m=m3;
       rigid3.l=4;
       rigid3.InertiaT=it3;
       rigid3.MakeGInertiaM();
       rigid3.xi=xi3;

CRigid rigid4;
	   rigid4.q=q4;
       rigid4.r=r4;
	   rigid4.m=m4;
       rigid4.InertiaT=it4;
       rigid4.MakeGInertiaM();
       rigid4.xi=xi4;

double vtheta[]={0,0,0,0};
//CMatrix theta(4,1,vtheta);
       theta.Init(4,1);
       theta.SetData(vtheta);
double vdtheta[]={0,0,0,0};
       dtheta.Init(4,1);
       dtheta.SetData(vdtheta);
       
       *xx.R=rigid1;
	   *(xx.R+1)=rigid2;
	   *(xx.R+2)=rigid3;
	   *(xx.R+3)=rigid4;

	   xx.MakeMC(theta,dtheta);
//       xx.MakeEMEK(0,1,0);

double vforce[]={0,0,0,0};
//CMatrix force(4,1,vforce);
       force.Init(4,1);
       force.SetData(vforce);
//double vn[]={0,0,0,10};
//CMatrix n(4,1,vn);
//double vbeata[]={0,0,0,0};
//CMatrix force(4,1,vforce);
       beata.MakeUnitMatrix(4);
//       force.SetData(vforce);
       beata=beata*10000000;

        xx.MakeN(theta,theta,beata,9.8);

//        ddtheta=xx.CaculateDdtheta(theta,theta,force);
		
CMatrix kv;
CMatrix kp;
        kv.MakeUnitMatrix(4);
        kp.MakeUnitMatrix(4);

double vDesigntheta[]={0,0,0,0};
CMatrix Designtheta(4,1,vDesigntheta);
//       Designtheta.Init(4,1);
//       Designtheta.SetData(vDesigntheta);

		control.lpDynamics=&xx;