/******************************************************************************
* ArmRobotSimulation.c *
* -------------------- *
* copyright : (C) 2013 by Richard R. Carrillo and Niceto R. Luque *
* email : rcarrillo,nluque at ugr.es *
******************************************************************************/
/***************************************************************************
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 3 of the License, or *
* (at your option) any later version. *
***************************************************************************/
/*!
* \file ArmRobotSimulation.c
*
* \author Richard R. Carrillo
* \author Niceto R. Luque
* \date 11 of July 2013
*
* This file defines the robot dynamics functions.
*/
#include <mex.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <mat.h>
#include <stdlib.h>
#include <time.h>
#include "../../include/arm_robot_simulator/RobotRecursiveInverseDynamics.h"
#include "../../include/arm_robot_simulator/ArmRobotSimulation.h"
/* default values for gravity and external load */
const double GRAVITY[3]={0, 0, 9.81};
const double EXTERNAL_FORCE[6]={0, 0, 0, 0, 0, 0};
/* default values for simulation */
#define NUMSTEPS 500 // Number of integration steps
#define STEPSIZE 0.002 // Integration step size
void test_simulated_robot_dynamics(void)
/*PURPOSE:
This function is for debugging purposes. It simulates the robot during NUMSTEPS*STEPSIZE and creates tra.txt log file
This log file can be plotted with the tra.m Matlab script
CALLING SEQUENCE:
test_simulated_robot_dynamics()
INPUTS:
OUTPUT:
log file tra.txt is created
*/
{
static int first=0;
double *qentry,*qdentry,*qddentry;
double *qtray,*qdtray, *qddtray,*TorqueOutput;
double *qddoutput,*qdoutput,*qoutput;
struct integrator_buffers robot_integr_buffers;
double Amplitude=0.1;
const char *robotfilename = "MANIPULATORS.mat";
const char *robot = "RRedKuKa"; // "RRedKuKadet10";
const char *robotDet="RRedKuKa";
/*Params */
mxArray *robotdata;
mxArray *robotdataDet;
int n_joints,i,j;
FILE *result_trayec;
double entry[3*3];
double output[3*3];
double tray[3*3];
result_trayec = fopen ("tra.txt","wt");
if(result_trayec==NULL) {perror("traj file");exit(-1);}
/* Charging the robot */
if(load_robot(robotfilename, robot, &n_joints, &robotdata)) {perror("robot file");exit(-1);}
if(load_robot(robotfilename, robotDet, NULL, &robotdataDet)) {perror("robot file");exit(-1);}
/* Initializing variables */
qentry=entry;
qdentry=entry+n_joints;
qddentry=entry+2*n_joints;
TorqueOutput = (double*)malloc(n_joints*sizeof(double));
qtray=tray;
qdtray=tray+n_joints;
qddtray=tray+2*n_joints;
qoutput=output;
qdoutput=output+n_joints;
qddoutput=output+2*n_joints;
for (i=0;i<n_joints;i++){
qentry[i]=Amplitude*sin((-4*M_PI*0*0*0 + 6*M_PI*0*0) + i*M_PI/4);
qdentry[i]=Amplitude*12*M_PI*0*(1 - 0)*cos(4*M_PI*0*0*0 - 6*M_PI*0*0 - M_PI*i/4);
qddentry[i]=Amplitude*(12*M_PI*(1-2*0)*cos(4*M_PI*0*0*0 - 6*M_PI*0*0 - M_PI*i/4) + 144*M_PI*M_PI*0*0*(0 - 1)*(0 - 1)*sin(4*M_PI*0*0*0 - 6*M_PI*0*0 - M_PI*i/4));
}
/*First time loading file and allocating memory*/
allocate_integration_buffers(&robot_integr_buffers,n_joints);
initialize_integration_buffers(entry,&robot_integr_buffers,n_joints);
for(j=1;j<NUMSTEPS;j++){
/*INVERSE DYNAMICS COMPUTATION*/
trajectory(qtray,qdtray,qddtray,j*STEPSIZE,n_joints);
compute_robot_inv_dynamics(robotdata,tray,EXTERNAL_FORCE,GRAVITY,TorqueOutput);
/*DIRECT DYNAMICS COMPUTATION*/
compute_robot_dir_dynamics(robotdataDet, entry, TorqueOutput, output, &robot_integr_buffers, EXTERNAL_FORCE, GRAVITY, STEPSIZE);
memcpy(entry,output,3*n_joints*sizeof(double));
fprintf(result_trayec,"%g ",j*STEPSIZE);
for(i=0;i<6;i++) {
const double *log[]={qtray,qdtray,qddtray,qoutput,qdoutput,qddoutput};
int i2;
for(i2=0;i2<n_joints;i2++)
fprintf(result_trayec,"%g ",log[i][i2]);
}
for(i=0;i<n_joints;i++)
fprintf(result_trayec,"%g ",TorqueOutput[i]);
fprintf(result_trayec,"\n");
}
free_integration_buffers(&robot_integr_buffers);
free(TorqueOutput);
free_robot(robotdata);
fclose(result_trayec);
}
#define NRHS_FASTRNE 6
#define NLHS_FASTRNE 1
void compute_robot_inv_dynamics(mxArray *robot,double *tray,const double *ExternalForce,const double *Gravity,double *TorqueOutput)
/*
PURPOSE:
FASTRNE Compute inverse dynamics via recursive Newton-Euler formulation
TAU = RNE(ROBOT, Q, QD, QDD)
TAU = RNE(ROBOT, [Q QD QDD])
Returns the joint torque required to achieve the specified joint position,
velocity and acceleration state.
Gravity vector is an attribute of the robot object but this may be
overriden by providing a gravity acceleration vector [gx gy gz].
TAU = RNE(ROBOT, Q, QD, QDD, GRAV)
TAU = RNE(ROBOT, [Q QD QDD], GRAV)
An external force/moment acting on the end of the manipulator may also be
specified by a 6-element vector [Fx Fy Fz Mx My Mz].
TAU = RNE(ROBOT, Q, QD, QDD, GRAV, FEXT)
TAU = RNE(ROBOT, [Q QD QDD], GRAV, FEXT)
where Q, QD and QDD are row vectors of the manipulator state; pos, vel, and accel.
The torque computed also contains a contribution due to armature
inertia.
CALLING SEQUENCE:
compute_robot_inv_dynamics(mxArray *robot,double *tray,const double *ExternalForce,const double *Gravity,double *TorqueOutput)
INPUTS:
robot robot variable to be used
tray position velocity and acceleration values per link from the trajectory generator
ExternalForce external applied Force per link
Gravity gravity acceleration vector
OUTPUT:
TorqueOutput Obtained torque per link
*/
{
int i;
mxArray *prhs[NRHS_FASTRNE];
mxArray *plhs[NLHS_FASTRNE];
double *tmp, *tmp2;
int n_rows=1;
int n_cols;
double *qtray, *qdtray, *qddtray;
/*
* Creating the entry of the mexfuntion
*/
n_cols=mstruct_getint(robot, 0, "n");
qtray=tray;
qdtray=tray+n_cols;
qddtray=tray+2*n_cols;
prhs[0]=robot;
prhs[1] = mxCreateDoubleMatrix(n_rows, n_cols, mxREAL);
tmp = mxGetPr(prhs[1]);
/*
* position
*/
memcpy(tmp, qtray, n_cols*sizeof(double));
/*
* velocity
*/
prhs[2] = mxCreateDoubleMatrix(n_rows, n_cols,mxREAL);
tmp = mxGetPr(prhs[2]);
memcpy(tmp, qdtray, n_cols*sizeof(double));
/*
* acceleration
*/
prhs[3] = mxCreateDoubleMatrix(n_rows, n_cols,mxREAL);
tmp = mxGetPr(prhs[3]);
memcpy(tmp, qddtray, n_cols*sizeof(double));
/*
* gravity[Gx Gy Gz]
*/
prhs[4] = mxCreateDoubleMatrix(n_rows,3 ,mxREAL);
tmp = mxGetPr(prhs[4]);
memcpy(tmp, Gravity, 3*sizeof(double));
/*Gx*//*Gy//*Gz*/
/*
* External force[Fx Fy Fz Mx My Mz]
*/
prhs[5] = mxCreateDoubleMatrix(n_rows,6,mxREAL);
tmp = mxGetPr(prhs[5]);
memcpy(tmp, ExternalForce, 6*sizeof(double));
/*Fx*/ /*Fy*/ /*Fz*/ /*MOMENTUMx*/ /*MOMENTUMy*/ /*MOMENTUMz*/
frnecFunction(NLHS_FASTRNE,plhs,NRHS_FASTRNE,prhs);
tmp2 = mxGetPr(plhs[0]);
memcpy(TorqueOutput,tmp2, n_cols*sizeof(double));
//for(i=0;i< n_cols;i++)
// mexPrintf("Torque Output %lf \n",tmp2[i]);
for(i=1;i<NRHS_FASTRNE;i++)
mxDestroyArray(prhs[i]);
for(i=0;i<NLHS_FASTRNE;i++)
mxDestroyArray(plhs[i]);
}
/**
* Computing the direct dynamics
**/
#define NRHS_ACCEL 5
#define NLHS_ACCEL 1
#define NRHS_ACCEL_TAU 6
#define NLHS_ACCEL_TAU 1
void compute_robot_dir_dynamics(mxArray *robot, double *robot_initial_state, double *external_torque, double *robot_resultant_state, struct integrator_buffers *integr_buffers, const double *external_force, const double *gravity, double stepsize)
/* PURPOSE:
Returns a vector of joint accelerations that result from applying the
actuator TORQUE to the manipulator ROBOT in state Q and QD.
Uses the method 1 of Walker and Orin to compute the forward dynamics.
The accelerations of the coordinates are obtained first
with the method of Walker-Orin and, later,it is joining to obtain speed and position.
This form is useful for simulation of manipulator dynamics, in
conjunction with a numerical integration function.
Walker and Orin is a numerical method used to obtain the acceleration of the
articular coordinates from the torque vector.For it, Newton-Euler's
algorithm uses when articular aceleration is zero
B= 0+H(q,q')+C(q); tau=D(q)q''+B; q''=inv(D(q))[tau-B]
CALLING SEQUENCE:
compute_robot_dir_dynamics(Robot,Robot_initial_state, ExternalTorque,Robot_resultant_state, integr_buffers, External_Force, Gravity, Stepsize)
INPUTS:
robot robot variable to be used
robot_initial_state position velocity and acceleration values per link previously obtained
external_torque external applied torque per link
robot_resultant_state Actual position velocity and acceleration values per link obtained
integr_buffers Activity buffer
Gravity Gravity vector
Stepsize integration step size
OUTPUT:
robot_resultant_state Actual position velocity and acceleration values per link obtained
*/
{
static int onetime=0;
int i,k;
mxArray *prhs[NRHS_ACCEL];
mxArray *plhs[NLHS_ACCEL];
mxArray *prhs_tau[NRHS_ACCEL_TAU];
mxArray *plhs_tau[NLHS_ACCEL_TAU];
double *tmp, *tmp2;
double *Minertia,*InvMinertia, *ExternalTorqueAux;
double *qentrymatrix;
double *qdentrymatrix;
double *qddentrymatrix;
int n_rows=3, n_rows_tau=1;
int n_cols ;
double *qentry, *qdentry, *qddentry;
double *qddoutput, *qdoutput, *qoutput;
/*
****************************************************************************
compute current manipulator inertia
torques resulting from unit acceleration of each joint with
no gravity.
****************************************************************************
*/
/*
* Creating the entry of the mexfuntion and initializing Data
*/
n_cols=mstruct_getint(robot, 0, "n");
qentry=robot_initial_state;
qdentry=robot_initial_state+n_cols;
qddentry=robot_initial_state+2*n_cols;
qoutput=robot_resultant_state;
qdoutput=robot_resultant_state+n_cols;
qddoutput=robot_resultant_state+2*n_cols;
qentrymatrix = (double*)malloc(n_cols*n_cols*sizeof(double));
qdentrymatrix = (double*)malloc(n_cols*n_cols*sizeof(double));
qddentrymatrix = (double*)malloc(n_cols*n_cols*sizeof(double));
ExternalTorqueAux = (double*)malloc(n_cols*sizeof(double));
Minertia = (double*)malloc(n_cols*n_cols*sizeof(double));
InvMinertia=(double*)malloc(n_cols*n_cols*sizeof(double));
/*
* position to calculate Matrix Inertia q=[q1 q1 q1;q2 q2 q2;q3 q3 q3]
*/
for (i=0;i<n_cols;i++)
for (k=0;k<n_cols;k++){
qentrymatrix[i*n_cols+k] = qentry[i];/*position*/
qdentrymatrix[i*n_cols+k] = 0;/*velocity*/
if (k==i)
qddentrymatrix[i*n_cols+k] = 1;
else
qddentrymatrix[i*n_cols+k] = 0;
}
prhs[0] = robot;
prhs[1] = mxCreateDoubleMatrix(n_cols, n_cols, mxREAL);
tmp = mxGetPr(prhs[1]);
memcpy(tmp, qentrymatrix, n_cols*n_cols*sizeof(double));
/*
* velocity to calculate Matrix Inertia qd=[0 0 0;0 0 0;0 0 0]
*/
prhs[2] = mxCreateDoubleMatrix(n_cols, n_cols,mxREAL);
tmp = mxGetPr(prhs[2]);
memcpy(tmp, qdentrymatrix, n_cols*n_cols*sizeof(double));
/*
* acceleration to calculate Matrix Inertia qdd=[1 0 0;0 1 0;0 0 1]
*/
prhs[3] = mxCreateDoubleMatrix(n_cols, n_cols,mxREAL);
tmp = mxGetPr(prhs[3]);
memcpy(tmp, qddentrymatrix, n_cols*n_cols*sizeof(double));
/*
* gravity[Gx Gy Gz], to calculate Matrix Inertia G=[0;0;0]
*/
prhs[4] = mxCreateDoubleMatrix(n_rows,1 ,mxREAL);
tmp = mxGetPr(prhs[4]);
tmp[0] = 0.0; /*Gx*/
tmp[1] = 0.0; /*Gy*/
tmp[2] = 0.0; /*Gz*/
/*
* Calculating Matrix Inertia
*/
frnecFunction(NLHS_ACCEL,plhs,NRHS_ACCEL,prhs);
tmp2 = mxGetPr(plhs[0]);
memcpy(Minertia,tmp2,n_cols*n_cols*sizeof(double));
invermat(n_cols, Minertia, InvMinertia);
for(i=1;i<NRHS_ACCEL;i++)
mxDestroyArray(prhs[i]);
for(i=0;i<NLHS_ACCEL;i++)
mxDestroyArray(plhs[i]);
/*
****************************************************************************
compute gravity and coriolis torque
torques resulting from zero acceleration at given velocity &
with gravity acting.
****************************************************************************
*/
prhs_tau[0] = robot;
/*
* position to calculate coriolis q=[q1 q2 q3]
*/
prhs_tau[1] = mxCreateDoubleMatrix(n_rows_tau, n_cols, mxREAL);
tmp = mxGetPr(prhs_tau[1]);
memcpy(tmp, qentry, n_cols*sizeof(double));
/*
* velocity to calculate coriolis qd=[qd1 qd2 qd3]
*/
prhs_tau[2] = mxCreateDoubleMatrix(n_rows_tau, n_cols, mxREAL);
tmp = mxGetPr(prhs_tau[2]);
memcpy(tmp, qdentry, n_cols*sizeof(double));
/*
* acceleration to calculate coriolis qd=[0 0 0]
*/
prhs_tau[3] = mxCreateDoubleMatrix(n_rows_tau, n_cols, mxREAL);
tmp = mxGetPr(prhs_tau[3]);
for(k=0;k<n_cols;k++){
tmp[k] =0.0;
}
/*
* gravity[Gx Gy Gz]
*/
prhs_tau[4] = mxCreateDoubleMatrix(n_rows_tau, 3, mxREAL);
tmp = mxGetPr(prhs_tau[4]);
memcpy(tmp,gravity,sizeof(double)*3);
/*
* External force[Fx Fy Fz Mx My Mz]
*/
prhs_tau[5] = mxCreateDoubleMatrix(n_rows_tau, 6, mxREAL);
tmp = mxGetPr(prhs_tau[5]);
memcpy(tmp, external_force, 6*sizeof(double));
frnecFunction(NLHS_ACCEL_TAU,plhs_tau,NRHS_ACCEL_TAU,prhs_tau);
tmp2 = mxGetPr(plhs_tau[0]);
for(i=0;i< n_cols;i++)
ExternalTorqueAux[i]=external_torque[i]-tmp2[i];
/*
****************************************************************************
compute the acc value within the initial values of qd, qdd
and torque
****************************************************************************
*/
multiplyMatrices(InvMinertia, ExternalTorqueAux,qddoutput,n_rows, n_cols, n_cols, n_rows_tau);
/*
****************************************************************************
Integrating the acc value within the initial values of q, qd
obtaining q and qd of the robot plant
****************************************************************************
*/
integrationprocedure(integr_buffers->accbuffer, integr_buffers->velbuffer, &integr_buffers->occupation, integr_buffers->sumacc, integr_buffers->sumvel, qddoutput,qdoutput,qoutput,integr_buffers->qvinit,integr_buffers->qinit,n_cols,stepsize);
for(i=1;i<NRHS_ACCEL_TAU;i++)
mxDestroyArray(prhs_tau[i]);
for(i=0;i<NLHS_ACCEL_TAU;i++)
mxDestroyArray(plhs_tau[i]);
free(qentrymatrix);
free(qdentrymatrix);
free(qddentrymatrix);
free(InvMinertia);
free(Minertia);
free(ExternalTorqueAux);
}
/**
* RNE can be either an M-file or a MEX-file. This is the Mexfile for C
* See the robotic toolbox manual for details on how to configure the MEX-file.
* The M-file is a wrapper which calls either
* RNE_DH or RNE_MDH depending on the kinematic conventions used by the robot
* object.
*
*
**/
/**
* Mex function entry point.
*/
void
frnecFunction(int nlhs, mxArray **plhs, int nrhs, const mxArray **prhs)
/*
PURPOSE:
The plhs[] and prhs[] parameters are vectors that contain pointers to each left-hand side (output)
variable and each right-hand side (input) variable, respectively.
Accordingly, plhs[0] contains a pointer to the first left-hand side argument, plhs[1] contains a pointer
to the second left-hand side argument, and so on.
Likewise, prhs[0] contains a pointer to the first right-hand side argument, prhs[1] points to the second, and so on.
CALLING SEQUENCE:
Arguments
nlhs MATLAB sets nlhs with the number of expected mxArrays.
plhs MATLAB sets plhs to a pointer to an array of NULL pointers.
nrhs MATLAB sets nrhs to the number of input mxArrays.
prhs MATLAB sets prhs to a pointer to an array of input mxArrays.
Description
These mxArrays are declared as constant,
they are read only and should not be modified by your MEX-file.
Changing the data in these mxArrays may produce undesired side effects.
mexFunction is not a routine you call. Rather, mexFunction is the generic name of the function
entry point that must exist in every C source MEX-file.
When you invoke a MEX-function, MATLAB finds and loads the corresponding MEX-file of the same name.
MATLAB then searches for a symbol named mexFunction within the MEX-file. If it finds one,
it calls the MEX-function using the address of the mexFunction symbol.
If MATLAB cannot find a routine named mexFunction inside the MEX-file, it issues an error message.
When you invoke a MEX-file, MATLAB automatically seeds nlhs, plhs, nrhs, and prhs with the caller's information.
In the syntax of the MATLAB language, functions have the general form
[a,b,c,...] = fun(d,e,f,...)
where the denotes more items of the same format.
The a,b,c... are left-hand side arguments and the
d,e,f... are right-hand side arguments.
The arguments nlhs and nrhs contain the number of left-hand side and right-hand side arguments,
respectively, with which the MEX-function is called. prhs is a pointer to a length nrhs array
of pointers to the right-hand side mxArrays. plhs is a pointer to a length nlhs array where your
C function must put pointers for the returned left-hand side mxArrays.
*/
{
double *q, *qd, *qdd;
double *tau;
// unsigned int m,n;
int j, njoints, p, nq; // l
double *fext = NULL;
double *grav = NULL;
Robot robot;
mxArray *link0;
mxArray *mx_robot;
mxArray *mx_links;
static int firstime = 0;
int fieldmap[FLD_MAX];
if ( !mxIsStruct(ROBOT_IN) ||
(strcmp(mxGetClassName(ROBOT_IN), "robot") != 0)
)
mexErrMsgTxt("first argument is not a robot structure\n");
mx_robot = (mxArray *)ROBOT_IN;
njoints = mstruct_getint(mx_robot, 0, "n");
/*
**********************************************************************
* Handle the different calling formats.
* Setup pointers to q, qd and qdd inputs
**********************************************************************
*/
switch (nrhs) {
case 2:
/*
* TAU = RNE(ROBOT, [Q QD QDD])
*/
if (NUMCOLS(A1_IN) != 3 * njoints)
mexErrMsgTxt("RNE too few cols in [Q QD QDD]");
q = POINTER(A1_IN);
nq = NUMROWS(A1_IN);
qd = &q[njoints*nq];
qdd = &q[2*njoints*nq];
break;
case 3:
/*
* TAU = RNE(ROBOT, [Q QD QDD], GRAV)
*/
if (NUMCOLS(A1_IN) != (3 * njoints))
mexErrMsgTxt("RNE too few cols in [Q QD QDD]");
q = POINTER(A1_IN);
nq = NUMROWS(A1_IN);
qd = &q[njoints*nq];
qdd = &q[2*njoints*nq];
if (NUMELS(A2_IN) != 3)
mexErrMsgTxt("RNE gravity vector expected");
grav = POINTER(A2_IN);
break;
case 4:
/*
* TAU = RNE(ROBOT, Q, QD, QDD)
* TAU = RNE(ROBOT, [Q QD QDD], GRAV, FEXT)
*/
if (NUMCOLS(A1_IN) == (3 * njoints)) {
q = POINTER(A1_IN);
nq = NUMROWS(A1_IN);
qd = &q[njoints*nq];
qdd = &q[2*njoints*nq];
if (NUMELS(A2_IN) != 3)
mexErrMsgTxt("RNE gravity vector expected");
grav = POINTER(A2_IN);
if (NUMELS(A3_IN) != 6)
mexErrMsgTxt("RNE Fext vector expected");
fext = POINTER(A3_IN);
} else {
int nqd = NUMROWS(A2_IN),
nqdd = NUMROWS(A3_IN);
nq = NUMROWS(A1_IN);
if ((nq != nqd) || (nqd != nqdd))
mexErrMsgTxt("RNE Q QD QDD must be same length");
if ( (NUMCOLS(A1_IN) != njoints) ||
(NUMCOLS(A2_IN) != njoints) ||
(NUMCOLS(A3_IN) != njoints)
)
mexErrMsgTxt("RNE Q must have Naxis columns");
q = POINTER(A1_IN);
qd = POINTER(A2_IN);
qdd = POINTER(A3_IN);
}
break;
case 5: {
/*
* TAU = RNE(ROBOT, Q, QD, QDD, GRAV)
*/
int nqd = NUMROWS(A2_IN),
nqdd = NUMROWS(A3_IN);
nq = NUMROWS(A1_IN);
if ((nq != nqd) || (nqd != nqdd))
mexErrMsgTxt("RNE Q QD QDD must be same length");
if ( (NUMCOLS(A1_IN) != njoints) ||
(NUMCOLS(A2_IN) != njoints) ||
(NUMCOLS(A3_IN) != njoints)
)
mexErrMsgTxt("RNE Q must have Naxis columns");
q = POINTER(A1_IN);
qd = POINTER(A2_IN);
qdd = POINTER(A3_IN);
if (NUMELS(A4_IN) != 3)
mexErrMsgTxt("RNE gravity vector expected");
grav = POINTER(A4_IN);
break;
}
case 6: {
/*
* TAU = RNE(ROBOT, Q, QD, QDD, GRAV, FEXT)
*/
int nqd = NUMROWS(A2_IN),
nqdd = NUMROWS(A3_IN);
nq = NUMROWS(A1_IN);
if ((nq != nqd) || (nqd != nqdd))
mexErrMsgTxt("RNE Q QD QDD must be same length");
if ( (NUMCOLS(A1_IN) != njoints) ||
(NUMCOLS(A2_IN) != njoints) ||
(NUMCOLS(A3_IN) != njoints)
)
mexErrMsgTxt("RNE Q must have Naxis columns");
q = POINTER(A1_IN);
qd = POINTER(A2_IN);
qdd = POINTER(A3_IN);
if (NUMELS(A4_IN) != 3)
mexErrMsgTxt("RNE gravity vector expected");
grav = POINTER(A4_IN);
if (NUMELS(A5_IN) != 6)
mexErrMsgTxt("RNE Fext vector expected");
fext = POINTER(A5_IN);
break;
}
default:
mexErrMsgTxt("RNE wrong number of arguments.");
}
/*
* fill out the robot structure
*/
robot.njoints = njoints;
if (grav)
robot.gravity = (Vect *)grav;
else
robot.gravity = (Vect *)mxGetPr( mxGetField(mx_robot, 0, "gravity") );
robot.dhtype = mstruct_getint(mx_robot, 0, "mdh");
/* build link structure */
robot.links = (Link *)mxCalloc(njoints, sizeof(Link));
/*
**********************************************************************
* Now we have to get pointers to data spread all across a cell-array
* of Matlab structures.
*
* Matlab structure elements can be found by name (slow) or by number (fast).
* We assume that since the link structures are all created by the same
* constructor, the index number for each element will be the same for all
* links. However we make no assumption about the numbers themselves.
**********************************************************************
*/
/* get pointer to link structures */
mx_links = mxGetField(mx_robot, 0, "link");
if (mx_links == NULL)
mexErrMsgTxt("couldnt find element link in robot structure");
/*
* Elements of the link structure are:
*
* alpha:
* A:
* theta:
* D:
* offset:
* sigma:
* mdh:
* m:
* r:
* I:
* Jm:
* G:
* B:
* Tc:
*/
/* take the first link as the template */
link0 = mxGetCell(mx_links, 0);
/* and lookup each structure element, and save the index */
fieldmap[FLD_ALPHA] = mstruct_getfield_number(link0, "alpha");
fieldmap[FLD_A] = mstruct_getfield_number(link0, "A");
fieldmap[FLD_THETA] = mstruct_getfield_number(link0, "theta");
fieldmap[FLD_D] = mstruct_getfield_number(link0, "D");
fieldmap[FLD_SIGMA] = mstruct_getfield_number(link0, "sigma");
fieldmap[FLD_OFFSET] = mstruct_getfield_number(link0, "offset");
fieldmap[FLD_M] = mstruct_getfield_number(link0, "m");
fieldmap[FLD_R] = mstruct_getfield_number(link0, "r");
fieldmap[FLD_I] = mstruct_getfield_number(link0, "I");
fieldmap[FLD_JM] = mstruct_getfield_number(link0, "Jm");
fieldmap[FLD_G] = mstruct_getfield_number(link0, "G");
fieldmap[FLD_B] = mstruct_getfield_number(link0, "B");
fieldmap[FLD_TC] = mstruct_getfield_number(link0, "Tc");
/*
* now for each link structure, use the saved index to copy the
* data into the corresponding Link structure.
*/
for (j=0; j<njoints; j++) {
Link *l = &robot.links[j];
mxArray *m = mxGetCell(mx_links, j);
l->alpha = mxGetScalar( mxGetFieldByNumber(m, 0, fieldmap[FLD_ALPHA]) );
l->A = mxGetScalar( mxGetFieldByNumber(m, 0, fieldmap[FLD_A]) );
l->theta = mxGetScalar( mxGetFieldByNumber(m, 0, fieldmap[FLD_THETA]) );
l->D = mxGetScalar( mxGetFieldByNumber(m, 0, fieldmap[FLD_D]) );
l->sigma = (int)mxGetScalar( mxGetFieldByNumber(m, 0, fieldmap[FLD_SIGMA]) );
l->offset = mxGetScalar( mxGetFieldByNumber(m, 0, fieldmap[FLD_OFFSET]) );
l->m = mxGetScalar( mxGetFieldByNumber(m, 0, fieldmap[FLD_M]) );
l->rbar = (Vect *)mxGetPr( mxGetFieldByNumber(m, 0, fieldmap[FLD_R]) );
l->I = mxGetPr( mxGetFieldByNumber(m, 0, fieldmap[FLD_I]) );
l->Jm = mxGetScalar( mxGetFieldByNumber(m, 0, fieldmap[FLD_JM]) );
l->G = mxGetScalar( mxGetFieldByNumber(m, 0, fieldmap[FLD_G]) );
l->B = mxGetScalar( mxGetFieldByNumber(m, 0, fieldmap[FLD_B]) );
l->Tc = mxGetPr( mxGetFieldByNumber(m, 0, fieldmap[FLD_TC]) );
}
/* Create a matrix for the return argument */
TAU_OUT = mxCreateDoubleMatrix(nq, njoints, mxREAL);
tau = mxGetPr(TAU_OUT);
#define MEL(x,R,C) (x[(R)+(C)*nq])
/* for each point in the input trajectory */
for (p=0; p<nq; p++) {
/*
* update all position dependent variables
*/
for (j = 0; j < njoints; j++) {
Link *l = &robot.links[j];
switch (l->sigma) {
case REVOLUTE:
rot_mat(l, MEL(q,p,j)+l->offset, l->D, robot.dhtype);
break;
case PRISMATIC:
rot_mat(l, l->theta, MEL(q,p,j)+l->offset, robot.dhtype);
break;
}
#ifdef DEBUG
rot_print("R", &l->R);
vect_print("p*", &l->r);
#endif
}
newton_euler(&robot, &tau[p], &qd[p], &qdd[p], fext, nq);
}
mxFree(robot.links);
}
/*
* Written by;
*
* Peter I. Corke
* CSIRO Division of Manufacturing Technology
* Preston, Melbourne. Australia. 3072.
* pic@mlb.dmt.csiro.au
* Niceto R. Luque Sola
* Univerity of Granada. Spain
* nluque@ugr.es
*
*
*
*
*/
/**
* Return the link rotation matrix and translation vector.
*
* @param l Link object for which R and p* are required.
* @param th Joint angle, overrides value in link object
* @param d Link extension, overrides value in link object
* @param type Kinematic convention.
*/
static void
rot_mat (
Link *l,
double th,
double d,
DHType type
) {
double st, ct, sa, ca;
#ifdef sun
sincos(th, &st, &ct);
sincos(l->alpha, &sa, &ca);
#else
st = sin(th);
ct = cos(th);
sa = sin(l->alpha);
ca = cos(l->alpha);
#endif
switch (type) {
case STANDARD:
l->R.n.x = ct; l->R.o.x = -ca*st; l->R.a.x = sa*st;
l->R.n.y = st; l->R.o.y = ca*ct; l->R.a.y = -sa*ct;
l->R.n.z = 0.0; l->R.o.z = sa; l->R.a.z = ca;
l->r.x = l->A;
l->r.y = d * sa;
l->r.z = d * ca;
break;
case MODIFIED:
l->R.n.x = ct; l->R.o.x = -st; l->R.a.x = 0.0;
l->R.n.y = st*ca; l->R.o.y = ca*ct; l->R.a.y = -sa;
l->R.n.z = st*sa; l->R.o.z = ct*sa; l->R.a.z = ca;
l->r.x = l->A;
l->r.y = -d * sa;
l->r.z = d * ca;
break;
}
}
/*************************************************************************
* Matlab structure access methods
*************************************************************************/
static mxArray *
mstruct_get_element(mxArray *m, int i, char *field)
{
mxArray *e;
if (mxIsCell(m)) {
#ifdef DEBUG
mexPrintf("%d x %d\n", mxGetM(m), mxGetN(m));
#endif
/* get the i'th cell from the cell array */
if ((e = mxGetCell(m, i)) == NULL)
error("get_element: field %s: cant get cell element %d",field, i);
} else
e = m;
if (!mxIsStruct(e))
mexErrMsgTxt("get_element: expecting a structure");
if ((e = mxGetField(e, 0, field)) != NULL)
return e;
else {
error("No such field as %s", field);
return NULL;
}
}
static int
mstruct_getfield_number(mxArray *m, char *field)
{
int f;
if ((f = mxGetFieldNumber(m, field)) < 0)
error("no element %s in link structure");
return f;
}
static int
mstruct_getint(mxArray *m, int i, char *field)
{
mxArray *e;
e = mstruct_get_element(m, i, field);
return (int) mxGetScalar(e);
}
static double
mstruct_getreal(mxArray *m, int i, char *field)
{
mxArray *e;
e = mstruct_get_element(m, i, field);
return mxGetScalar(e);
}
static double *
mstruct_getrealvect(mxArray *m, int i, char *field)
{
mxArray *e;
e = mstruct_get_element(m, i, field);
return mxGetPr(e);
}
#include <stdarg.h>
/**
* Error message handler. Takes printf() style format string and variable
* arguments and sends resultant string to Matlab via \t mexErrMsgTxt().
*
* @param s Error message string, \t printf() style.
*/
void
error(char *s, ...)
{
char b[BUFSIZ];
va_list ap;
va_start(ap, s);
vsprintf(b, s, ap);
mexErrMsgTxt(b);
}
/**
* Inverse Matrix for calculating the acceleration
* arguments and sends resultant string to main via and single vector.
* entries: size of the matrix, the original matrix and the inverse matrix
* */
void
invermat(int sizeMatrix, double *a, double *ainv)
/*PURPOSE:
Inverting two Matrices needed for obtaining acceleration value of the robot plan
when a torque value is applied
CALLING SEQUENCE:
invermat(sizeMatrix, M1,INVM1)
INPUTS:
sizeMatrix size of M1 must be squared
M1 PointerEntry Matrix to be multiplied
INVM1 Pointer to the result Matrix
OUTPUT:
INVM1 Inverse Matrix
*/
{
double coef, value,element;
double *aux= (double*)malloc(sizeMatrix*sizeMatrix*sizeof(double));
int i,j,n,m,s;
for (i=0;i<sizeMatrix;i++){
for(j=0;j<sizeMatrix;j++){
if(i==j)
ainv[i*sizeMatrix+j]=1.0;
else
ainv[i*sizeMatrix+j]=0.0;
}
}
/*Iterations*/
for (s=0;s<sizeMatrix;s++){
element=a[s*sizeMatrix+s];
if(element==0){
for(n=s+1; n<sizeMatrix; n++){
element=a[n*sizeMatrix+s];
if(element!=0){
for(m=0; m<sizeMatrix; m++){
value=a[n*sizeMatrix+m];
a[n*sizeMatrix+m]=a[s*sizeMatrix+m];
a[s*sizeMatrix+m]=value;
}
break;
}
if(n==(sizeMatrix-1)){
printf("This matrix is not invertible\n");
exit(0);
}
}
}
for (j=0;j<sizeMatrix;j++){
a[s*sizeMatrix+j]/=element;
ainv[s*sizeMatrix+j]/=element;
}
for(i=0;i<sizeMatrix;i++)
{
if (i!=s){
coef=a[i*sizeMatrix+s];
for (j=0;j<sizeMatrix;j++){
aux[j]=a[s*sizeMatrix+j]*(coef*-1);
aux[sizeMatrix+j]=ainv[s*sizeMatrix+j]*(coef*-1);
}
for (j=0;j<sizeMatrix;j++){
a[i*sizeMatrix+j]+=aux[j];
ainv[i*sizeMatrix+j]+=aux[sizeMatrix+j];
}
}
}
}
free(aux);
return;
}
/**
*
* This is for multiplying Matrix.
*
* */
void multiplyMatrices(double *M1, double *M2,double *Result,int oneRow, int oneCol, int twoRow, int twoCol)
/*
PURPOSE:
Multiplying two Matrices needed for obtaining acceleration value of the robot plan
when a torque value is applied
CALLING SEQUENCE:
multiplyingMatrices( M1,M2, &Result, oneRow, oneCol, twoRow, twoCol)
INPUTS:
M1 Entry Matrix to be multiplied
M2 Entry Matrix to be multiplied
Result Pointer to the result Matrix
oneRow number of rows M1
oneCol number of cols M1
twoRow number of rows M2
twoCol number of cols M2
OUTPUT:
Result result Matrix
*/
/*Start of multiplyMatrices function*/
{
/*Declare variables*/
int i, j, k;
/*Create the arrays in the heap memory*/
/*The number of columns in oneMatrix must equal the number of rows in twoMatrix*/
if(oneCol != twoRow)
{
printf("The number of columns in the first matrix must equal the number of rows in the second matrix.\n");
exit(EXIT_FAILURE);
}
/*Declare pointer variables to store the initial locations of my arrays for later use
for loop while i is less than oneRow(number of rows in first matrix)*/
for(i = 0; i < oneRow; i++)
{ /*A for loop while j is less than twoCol*/
for(j = 0; j < twoCol; j++)
{
/* for loop while k is less than oneCol*/
Result[i*twoCol+j]=0.0;
for(k = 0; k < oneCol; k++)
{
/*Multiply the current entries of the matrices and add the answer to the current entry for the multiplied matrix*/
Result[ i*twoCol+j ] += M1[ i*oneCol+ k ]*M2[ k*twoCol + j ] ;
}
}
}
return;
}
/**
*
* This is the integration method for approximating the value of acc
*
*
* */
double trap ( double *f, double h )
/*
PURPOSE:
approximate the value of a definite integral using
the composite trapezoidal rule
CALLING SEQUENCE:
y = trap ( *f, h);
INPUTS:
f buffer containing function points
h h(upper limit of integration step-lower limit of integration)
OUTPUT:
y approximate value of definite integral
*/
{
double sum;
sum = 0.5 * ( f[0] + f[1] );
return ( h*sum );
}
double simp ( double *f, double h )
/*
PURPOSE:
approximate the value of a definite integral using
the composite Simpson's rule
CALLING SEQUENCE:
y = simp ( *f, h);
INPUTS:
f buffer containing function points
h h(upper limit of integration step-lower limit of integration)
OUTPUT:
y approximate value of definite integral
*/
{
double haux,
sum,m;
haux = 2.0*h;
m = haux /6.0;
sum =m* (f[0] + 4.0*f[1]+ f[2]);
return ( sum );
}
double simp3_8 ( double *f, double h )
/*
PURPOSE:
approximate the value of a definite integral using
the composite 3/8 Simpson's rule
CALLING SEQUENCE:
y = simp ( *f, h);
INPUTS:
f buffer containing function points
h h(upper limit of integration step-lower limit of integration)
OUTPUT:
y approximate value of definite integral
*/
{
double haux,
sum,m;
haux = 3.0*h;
m = haux /8.0;
sum =m* (f[0] + 3.0*f[1]+ 3.0*f[2]+ f[3]);
return ( sum );
}
double boolerule ( double *f, double h)
/*
PURPOSE:
approximate the value of a definite integral using
the composite boolerule
CALLING SEQUENCE:
y = bool ( *f, h );
INPUTS:
f buffer containing function points
h h(upper limit of integration step-lower limit of integration)
OUTPUT:
y approximate value of definite integral
*/
{
double haux,
sum,m;
haux = 4.0*h;
m = haux /90.0;
sum =m* (7.0*f[0] + 32.0*f[1]+ 12.0*f[2]+ 32.0*f[3]+7.0*f[4]);
return ( sum );
}
double fifth ( double *f, double h )
/*
PURPOSE:
approximate the value of a definite integral using
the composite boolerule
CALLING SEQUENCE:
y = fifth ( *f, h);
INPUTS:
f buffer containing function points
h h(upper limit of integration step-lower limit of integration)
OUTPUT:
y approximate value of definite integral
*/
{
double haux,
sum,m;
haux = 5.0*h;
m = haux /288.0;
sum =m* (19.0*f[0] + 75.0*f[1]+ 50.0*f[2]+ 50.0*f[3]+75.0*f[4]+19.0*f[5]);
return ( sum );
}
double sixth ( double *f, double h )
/*
PURPOSE:
approximate the value of a definite integral using
the composite boolerule
CALLING SEQUENCE:
y = sixth ( *f, h);
INPUTS:
f buffer containing function points
h h(upper limit of integration step-lower limit of integration)
OUTPUT:
y approximate value of definite integral
*/
{
double haux,
sum,m;
haux = 6.0*h;
m = haux /840.0;
sum =m* (41.0*f[0] + 216.0*f[1]+ 27.0*f[2]+ 272.0*f[3]+27.0*f[4]+216.0*f[5]+41.0*f[6]);
return ( sum );
}
int allocate_integration_buffers(struct integrator_buffers *integr_buffers, int njoints)
/* PURPOSE:
This is to allocate the memory used in the integrative process
CALLING SEQUENCE:
allocate_integration_buffers(integr_buffers)
INPUTS:
integr_buffers allocate buffer memory sumvel,sumacc,accbuffer,velbuffer,qinit and qvinit
OUTPUT:
return 1 if error
*/
{
int ret;
integr_buffers->sumvel=(double*)malloc(njoints*sizeof(double));
integr_buffers->sumacc=(double*)malloc(njoints*sizeof(double));
integr_buffers->accbuffer=(double*)malloc(INTEGR_BUFFERSIZE*njoints*sizeof(double));
integr_buffers->velbuffer=(double*)malloc(INTEGR_BUFFERSIZE*njoints*sizeof(double));
integr_buffers->qinit=(double*)malloc(njoints*sizeof(double));
integr_buffers->qvinit=(double*)malloc(njoints*sizeof(double));
if(integr_buffers->sumvel==NULL || integr_buffers->sumacc==NULL || integr_buffers->accbuffer==NULL || integr_buffers->velbuffer==NULL || integr_buffers->qinit==NULL || integr_buffers->qvinit==NULL)
{
perror("Allocating memory for the buffers");
free(integr_buffers->sumvel);
free(integr_buffers->sumacc);
free(integr_buffers->accbuffer);
free(integr_buffers->velbuffer);
free(integr_buffers->qinit);
free(integr_buffers->qvinit);
ret=1;
}
else
ret=0;
return(ret);
}
/**
* Initializing buffer accel and velocity
*
**/
void initialize_integration_buffers(double *entry, struct integrator_buffers *integr_buffers, int njoints)
/*PURPOSE:
In order to compute the integral value of the given acceleration and velocity to
solve the direct robot dinamic a buffer of 5 elemnts per joint is needed. This is for initializing
these buffers
CALLING SEQUENCE:
initialize_integration_buffers(entry,*integr_buffers,njoints)
INPUTS:
entry: initial entry values to be accumulated
integrator_buffers: struct with the buffer to be initialized
njoints: number of joints
OUTPUT:
struct with the initialized buffers
*/
{
int i,j;
double *qentry, *qdentry, *qddentry;
qentry=entry;
qdentry=entry+njoints;
qddentry=entry+2*njoints;
for (i=0;i<njoints;i++){
(integr_buffers->sumvel)[i]=0.0;
(integr_buffers->sumacc)[i]=0.0;
(integr_buffers->qinit)[i]=qentry[i];
(integr_buffers->qvinit)[i]=qdentry[i];
for(j=0;j<INTEGR_BUFFERSIZE;j++){
if (j==0){
(integr_buffers->accbuffer)[i*INTEGR_BUFFERSIZE+j]=qddentry[i];
(integr_buffers->velbuffer)[i*INTEGR_BUFFERSIZE+j]=qdentry[i];
}
else{
(integr_buffers->accbuffer)[i*INTEGR_BUFFERSIZE+j]=0.0;
(integr_buffers->velbuffer)[i*INTEGR_BUFFERSIZE+j]=0.0;
}
}
}
integr_buffers->occupation=1;
}
/**
* FREE THE MEMORY
* */
void free_integration_buffers(struct integrator_buffers *integr_buffers)
/*PURPOSE:
This is to free the memory used in the integrative process
CALLING SEQUENCE:
free_integration_buffers(integr_buffers)
INPUTS:
integr_buffers free activity sumvel,sumacc,accbuffer,velbuffer,qinit and qvinit
*/
{
free(integr_buffers->sumvel);
free(integr_buffers->sumacc);
free(integr_buffers->accbuffer);
free(integr_buffers->velbuffer);
free(integr_buffers->qinit);
free(integr_buffers->qvinit);
}
/**
* managing the activity buffer accel and velocity,making the integrative process
* */
void integrationprocedure(double *accbuffer, double *velbuffer,int *buff_occup,double *sumacc, double *sumvel, double *qddoutput_1 ,double *qdoutput,double *qoutput,double *qvinit, double *qinit,int njoints,double stepsize)
/*PURPOSE:
In order to compute the integral value of the given acceleration and velocity to
solve the direct robot dinamic a buffer of 7 elemnts per joint is used to integrate
the acceleration and velocity. Solving the direct dynamic
CALLING SEQUENCE:
integrationprocedure(accbuffer, velbuffer,buff_occup,&sumacc, &sumvel, qddoutput_1,qdoutput,qoutput,qvinit,qinit,njoints,stepsize)
INPUTS:
accbuffer activity acc buffer
velbuffer activity velocity buffer
occupation buffer occupation
sumacc accumulated integral
sumvel accumulated integral
qddoutput_1 qdd to be integrated
qdoutput accumulated integral
qoutput accumulated integral
qvinit initial conditions velocity
qinit initial conditions possition
njoints number of robot joints
stepsize integration step
OUTPUT:
qdoutput accumulated integral
qoutput accumulated integral
*/
{
int i;
switch(*buff_occup) {
case 1:
for (i=0;i<njoints;i++){
accbuffer[i*INTEGR_BUFFERSIZE+*buff_occup]=qddoutput_1[i];
qdoutput[i]=trap(accbuffer+i*INTEGR_BUFFERSIZE, stepsize)+qvinit[i];
velbuffer[i*INTEGR_BUFFERSIZE+*buff_occup]=qdoutput[i];
qoutput[i]=trap(velbuffer+i*INTEGR_BUFFERSIZE, stepsize)+qinit[i];
}
(*buff_occup)++;
break;
case 2:
for (i=0;i<njoints;i++){
accbuffer[i*INTEGR_BUFFERSIZE+*buff_occup]=qddoutput_1[i];
qdoutput[i]=simp(accbuffer+i*INTEGR_BUFFERSIZE, stepsize)+qvinit[i];
velbuffer[i*INTEGR_BUFFERSIZE+*buff_occup]=qdoutput[i];
qoutput[i]=simp(velbuffer+i*INTEGR_BUFFERSIZE, stepsize)+qinit[i];
}
(*buff_occup)++;
break;
case 3:
for (i=0;i<njoints;i++){
accbuffer[i*INTEGR_BUFFERSIZE+*buff_occup]=qddoutput_1[i];
qdoutput[i]=simp3_8(accbuffer+i*INTEGR_BUFFERSIZE, stepsize)+qvinit[i];
velbuffer[i*INTEGR_BUFFERSIZE+*buff_occup]=qdoutput[i];
qoutput[i]=simp3_8(velbuffer+i*INTEGR_BUFFERSIZE, stepsize)+qinit[i];
}
(*buff_occup)++;
break;
case 4:
for (i=0;i<njoints;i++){
accbuffer[i*INTEGR_BUFFERSIZE+*buff_occup]=qddoutput_1[i];
qdoutput[i]=boolerule(accbuffer+i*INTEGR_BUFFERSIZE, stepsize)+qvinit[i];
velbuffer[i*INTEGR_BUFFERSIZE+*buff_occup]=qdoutput[i];
qoutput[i]=boolerule(velbuffer+i*INTEGR_BUFFERSIZE, stepsize)+qinit[i];
}
(*buff_occup)++;
break;
case 5:
for (i=0;i<njoints;i++){
accbuffer[i*INTEGR_BUFFERSIZE+*buff_occup]=qddoutput_1[i];
qdoutput[i]=fifth(accbuffer+i*INTEGR_BUFFERSIZE, stepsize)+qvinit[i];
velbuffer[i*INTEGR_BUFFERSIZE+*buff_occup]=qdoutput[i];
qoutput[i]=fifth(velbuffer+i*INTEGR_BUFFERSIZE, stepsize)+qinit[i];
}
(*buff_occup)++;
break;
case 6:
for (i=0;i<njoints;i++){
accbuffer[i*INTEGR_BUFFERSIZE+*buff_occup]=qddoutput_1[i];
qdoutput[i]=sixth(accbuffer+i*INTEGR_BUFFERSIZE, stepsize)+qvinit[i];
sumacc[i]=qdoutput[i];
velbuffer[i*INTEGR_BUFFERSIZE+*buff_occup]=qdoutput[i];
qoutput[i]=sixth(velbuffer+i*INTEGR_BUFFERSIZE, stepsize)+qinit[i];
sumvel[i]=qoutput[i];
}
(*buff_occup)++;
break;
case 7:
for (i=0;i<njoints;i++){
/*Sliding window the elements are desplaced*/
memmove(accbuffer+i*INTEGR_BUFFERSIZE,accbuffer+i*INTEGR_BUFFERSIZE+1,(INTEGR_BUFFERSIZE-1)*sizeof(double));
accbuffer[i*INTEGR_BUFFERSIZE+6]=qddoutput_1[i];
/*Computing the new vector*/
sumacc[i]+=sixth(accbuffer+i*INTEGR_BUFFERSIZE, stepsize)-fifth(accbuffer+i*INTEGR_BUFFERSIZE, stepsize);
qdoutput[i]=sumacc[i];
/*Sliding window the elements are desplaced*/
memmove(velbuffer+i*INTEGR_BUFFERSIZE,velbuffer+i*INTEGR_BUFFERSIZE+1,(INTEGR_BUFFERSIZE-1)*sizeof(double));
velbuffer[i*INTEGR_BUFFERSIZE+6]=qdoutput[i];
/*Computing the new vector*/
sumvel[i]+=sixth(velbuffer+i*INTEGR_BUFFERSIZE, stepsize)-fifth(velbuffer+i*INTEGR_BUFFERSIZE, stepsize);
qoutput[i]=sumvel[i];
}
break;
default:
mexErrMsgTxt("Internal error of the integration buffer.");
}
return;
}
/**
* Obtaining the size of the robot to create the needed variables
* */
int load_robot(const char *robotfile, const char *robotname, int *size, mxArray **robot)
/*PURPOSE:
This is for obtaining robot object and size in order to dynamically create all the variables needed
CALLING SEQUENCE:
error=load_robot(robotfile, robotname, size, robot)
INPUTS:
robotfile File name from where the robot object is loaded
robotname Robot's variable name in the file
robot Robot variable in which the robot is returned
size Variable pointer where the number of joints are stored
OUTPUT:
error =0 if no error
*/
{
int nerror;
MATFile *pmat;
// Open MAT-file
pmat=matOpen(robotfile, "r");
if(pmat)
{
*robot = matGetVariable(pmat, robotname);
if(*robot)
{
if(size)
*size=mstruct_getint(*robot, 0, "n");
nerror=0;
}
else
nerror=1;
matClose(pmat);
}
else
nerror=2;
return(nerror);
}
void free_robot(mxArray *robot)
/*PURPOSE:
This is for free the robot memory array
CALLING SEQUENCE:
free_robot(robot)
INPUTS:
robot Robot variable in which the robot is allocated
*/
{
mxDestroyArray(robot);
}
/**
* Generating the trajectory
* */
void trajectory(double *q, double *qd, double *qdd, double tsimul,int n_joints)
/*PURPOSE:
This is for generating an eight like trajectory in cartesian space by means of using sinusoidal curves
in joint space
CALLING SEQUENCE:
trajectory(q, qd, qdd,tsimul,n_joints)
INPUTS:
tsimul simulation time
n_joint number of links the robot has
OUTPUT:
q position per link at t= tsimul
qd velocity per link at t=tsimul
qdd acceleration per link at t=tsimul
*/
{
int i;
double Amplitude=0.1;
for (i=0;i<n_joints;i++){
// trajectory initially proposed (qd[0] <> 0 and qd[1] <> 0)
/* q[i]=Amplitude*sin(2*M_PI*tsimul+(1.0/4.0)*i*M_PI);
* qd[i]=2*M_PI*Amplitude*cos(2*M_PI*tsimul+(1.0/4.0)*i*M_PI);
* qdd[i]=-4*pow(M_PI,2)*Amplitude*sin(2*M_PI*tsimul+(1.0/4.0)*i*M_PI);
*/
// modified trajectory by means of a cubic spline: -4sum(pisum(t^3)) + 6sum(pisum(t^2)) (qd[0] = 0 and qd[1] = 0 and q[0] = 0 and q[1] = 2*pi)
q[i]=Amplitude*sin((-4*M_PI*tsimul*tsimul*tsimul + 6*M_PI*tsimul*tsimul) + i*M_PI/4);
qd[i]=Amplitude*12*M_PI*tsimul*(1 - tsimul)*cos(4*M_PI*tsimul*tsimul*tsimul - 6*M_PI*tsimul*tsimul - M_PI*i/4);
qdd[i]=Amplitude*(12*M_PI*(1-2*tsimul)*cos(4*M_PI*tsimul*tsimul*tsimul - 6*M_PI*tsimul*tsimul - M_PI*i/4) + 144*M_PI*M_PI*tsimul*tsimul*(tsimul - 1)*(tsimul - 1)*sin(4*M_PI*tsimul*tsimul*tsimul - 6*M_PI*tsimul*tsimul - M_PI*i/4));
}
}