162 lines
5.4 KiB
C++
162 lines
5.4 KiB
C++
/***********************************************************************
|
|
*
|
|
* File name: prigoIK.cpp
|
|
* Author : Wayne Glettig, Marco Salathe
|
|
* Version : Feb. 2012
|
|
*
|
|
* Module that contains the geometric model and does the inverse kinematics
|
|
*
|
|
**********************************************************************/
|
|
|
|
#include <DLCInterface.hpp>
|
|
#include "prigoGM/prigo.h"
|
|
|
|
INITIALIZE_BEGIN
|
|
ALLOCATE_EXTRA(0, 1, prigo);
|
|
|
|
//read parameter file from PARAMS File (prigoIK.xmp)
|
|
//pass PARAMeters to Prigo GM.
|
|
EXTRA(0,0).offsetR1 = PARAM("offsetR1",0);
|
|
EXTRA(0,0).offsetR2 = PARAM("offsetR2",0);
|
|
EXTRA(0,0).offsetR3 = PARAM("offsetR3",0);
|
|
EXTRA(0,0).offsetR4 = PARAM("offsetR4",0);
|
|
EXTRA(0,0).offsetS1 = PARAM("offsetS1",0);
|
|
EXTRA(0,0).offsetS2 = PARAM("offsetS2",0);
|
|
EXTRA(0,0).offsetS3 = PARAM("offsetS3",0);
|
|
EXTRA(0,0).offsetS4 = PARAM("offsetS4",0);
|
|
EXTRA(0,0).l11 = PARAM("l11",0);
|
|
EXTRA(0,0).l12 = PARAM("l12",0);
|
|
EXTRA(0,0).l13 = PARAM("l13",0);
|
|
EXTRA(0,0).l14 = PARAM("l14",0);
|
|
EXTRA(0,0).l24 = PARAM("l24",0);
|
|
EXTRA(0,0).l31 = PARAM("l31",0);
|
|
EXTRA(0,0).l34 = PARAM("l34",0);
|
|
EXTRA(0,0).l35 = PARAM("l35",0);
|
|
EXTRA(0,0).l41 = PARAM("l41",0);
|
|
EXTRA(0,0).l42 = PARAM("l42",0);
|
|
EXTRA(0,0).l43 = PARAM("l43",0);
|
|
EXTRA(0,0).l44 = PARAM("l44",0);
|
|
EXTRA(0,0).l51 = PARAM("l51",0);
|
|
EXTRA(0,0).l52 = PARAM("l52",0);
|
|
EXTRA(0,0).l53 = PARAM("l53",0);
|
|
EXTRA(0,0).l54 = PARAM("l54",0);
|
|
EXTRA(0,0).l55 = PARAM("l55",0);
|
|
EXTRA(0,0).l56 = PARAM("l56",0);
|
|
EXTRA(0,0).a5 = PARAM("a5",0);
|
|
EXTRA(0,0).printParams();
|
|
|
|
//run geometrical model to test if it works;
|
|
double UCS[10] = {0., 0.,0.,20., 0.,0.,0., 0.,0.,275.};
|
|
double MCS[10] = {0., 0.,0.,0., 0.,0.,0., 0.,0.,0.};
|
|
char msg[200];
|
|
int errNo;
|
|
printf("IK Model: Sending:\n");
|
|
printf("UCS: %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf\n", UCS[1],UCS[2],UCS[3],UCS[4],UCS[5],UCS[6],UCS[7],UCS[8],UCS[9]);
|
|
errNo = EXTRA(0,0).mgi(UCS, MCS,msg);
|
|
printf("MCS: %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf\n", MCS[1],MCS[2],MCS[3],MCS[4],MCS[5],MCS[6],MCS[7],MCS[8],MCS[9]);
|
|
printf("IK Model: errNo('0' means OK): %d , msg: %s\n", errNo, msg);
|
|
printf("FK Model: Sending:\n");
|
|
printf("MCS: %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf\n", MCS[1],MCS[2],MCS[3],MCS[4],MCS[5],MCS[6],MCS[7],MCS[8],MCS[9]);
|
|
errNo = EXTRA(0,0).mgd(MCS, UCS,msg);
|
|
printf("UCS: %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf\n", UCS[1],UCS[2],UCS[3],UCS[4],UCS[5],UCS[6],UCS[7],UCS[8],UCS[9]);
|
|
printf("FK Model: errNo('0' means OK): %d , msg: %s\n", errNo, msg);
|
|
INITIALIZE_END
|
|
|
|
STEP_BEGIN
|
|
//pass PARAMeters to Prigo GM.
|
|
EXTRA(0,0).offsetR1 = PARAM("offsetR1",0);
|
|
EXTRA(0,0).offsetR2 = PARAM("offsetR2",0);
|
|
EXTRA(0,0).offsetR3 = PARAM("offsetR3",0);
|
|
EXTRA(0,0).offsetR4 = PARAM("offsetR4",0);
|
|
EXTRA(0,0).offsetS1 = PARAM("offsetS1",0);
|
|
EXTRA(0,0).offsetS2 = PARAM("offsetS2",0);
|
|
EXTRA(0,0).offsetS3 = PARAM("offsetS3",0);
|
|
EXTRA(0,0).offsetS4 = PARAM("offsetS4",0);
|
|
EXTRA(0,0).l11 = PARAM("l11",0);
|
|
EXTRA(0,0).l12 = PARAM("l12",0);
|
|
EXTRA(0,0).l13 = PARAM("l13",0);
|
|
EXTRA(0,0).l14 = PARAM("l14",0);
|
|
EXTRA(0,0).l24 = PARAM("l24",0);
|
|
EXTRA(0,0).l31 = PARAM("l31",0);
|
|
EXTRA(0,0).l34 = PARAM("l34",0);
|
|
EXTRA(0,0).l35 = PARAM("l35",0);
|
|
EXTRA(0,0).l41 = PARAM("l41",0);
|
|
EXTRA(0,0).l42 = PARAM("l42",0);
|
|
EXTRA(0,0).l43 = PARAM("l43",0);
|
|
EXTRA(0,0).l44 = PARAM("l44",0);
|
|
EXTRA(0,0).l51 = PARAM("l51",0);
|
|
EXTRA(0,0).l52 = PARAM("l52",0);
|
|
EXTRA(0,0).l53 = PARAM("l53",0);
|
|
EXTRA(0,0).l54 = PARAM("l54",0);
|
|
EXTRA(0,0).l55 = PARAM("l55",0);
|
|
EXTRA(0,0).l56 = PARAM("l56",0);
|
|
EXTRA(0,0).a5 = PARAM("a5",0);
|
|
|
|
// INPUTs
|
|
double I_ENABLED = INPUT(0,0);
|
|
double I_SHx = INPUT(1,0);
|
|
double I_SHy = INPUT(2,0);
|
|
double I_SHz = INPUT(3,0);
|
|
double I_CHI = INPUT(4,0);
|
|
double I_PHI = INPUT(5,0);
|
|
double I_OMEGA = INPUT(6,0);
|
|
double I_Ox = INPUT(7,0);
|
|
double I_Oy = INPUT(8,0);
|
|
double I_Oz = INPUT(9,0);
|
|
|
|
double O_s1 = OUTPUT(0,0);
|
|
double O_s2 = OUTPUT(1,0);
|
|
double O_s3 = OUTPUT(2,0);
|
|
double O_s4 = OUTPUT(3,0);
|
|
double O_phimotor = OUTPUT(4,0);
|
|
double O_omegamotor = OUTPUT(5,0);
|
|
|
|
// =====================================================================================================
|
|
// CALCULATE MODEL (Call prigo.mgi)
|
|
if (I_ENABLED == 1.) {
|
|
double UCS[10] = {0., I_SHx, I_SHy, I_SHz, I_CHI, I_PHI, I_OMEGA, I_Ox, I_Oy, I_Oz};
|
|
double MCS[10] = {0., 0.,0.,0., 0.,0.,0., 0.,0.,300.};
|
|
int err = EXTRA(0,0).mgi(UCS, MCS);
|
|
if (err){
|
|
err += 100;
|
|
ALARM_WITH_MESSAGE(err, "prigoIK: Kinematic Model error");
|
|
}
|
|
O_s1 = MCS[1];
|
|
O_s2 = MCS[2];
|
|
O_s3 = MCS[3];
|
|
O_s4 = MCS[4];
|
|
O_phimotor = MCS[5];
|
|
O_omegamotor = MCS[6];
|
|
}
|
|
//If module disabled, simply send 0s to the ouput.
|
|
if (I_ENABLED == 0.) {
|
|
O_s1 = 0.;
|
|
O_s2 = 0.;
|
|
O_s3 = 0.;
|
|
O_s4 = 0.;
|
|
O_phimotor = 0.;
|
|
O_omegamotor = 0.;
|
|
}
|
|
|
|
// =====================================================================================================
|
|
//Update OUTPUTs
|
|
OUTPUT(0,0) = O_s1;
|
|
OUTPUT(1,0) = O_s2;
|
|
OUTPUT(2,0) = O_s3;
|
|
OUTPUT(3,0) = O_s4;
|
|
OUTPUT(4,0) = O_phimotor;
|
|
OUTPUT(5,0) = O_omegamotor;
|
|
STEP_END
|
|
|
|
FINALIZE_BEGIN
|
|
DEALLOCATE_EXTRA(0);
|
|
FINALIZE_END
|
|
|
|
SAFETY_BEGIN
|
|
//how to behave when a error of the kinematic model transformation happens
|
|
if(ALARM_CODE>100 && ALARM_CODE<200){
|
|
ALARM_SKIP_RESET;
|
|
}
|
|
SAFETY_END
|
|
|