phschoen
/
lwr_server
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
70 Commits
1 Branch
0 Tags
31 MiB
 
 
 
 
 
 
Tree: 165281e612
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '165281e612'
${ noResults }
lwr_server/lwrserv/matlab/rvctools/robot/mex/frne.c

496 lines
13 KiB
Raw Blame History

/**
* \file frne.c
* \author Peter Corke
* \brief MEX file body
*
*
* FRNE MEX file version of RNE.M
*
* TAU = FRNE(ROBOT, Q, QD, QDD)
* TAU = FRNE(ROBOT, [Q QD QDD])
*
* where Q, QD and QDD are row vectors of the manipulator state; pos,
* vel, and accel.
*
* Returns the joint torque required to achieve the specified joint
* position, velocity and acceleration state. Gravity is taken
* from the robot object.
*
* TAU = RNE(ROBOT, Q, QD, QDD, GRAV)
* TAU = RNE(ROBOT, [Q QD QDD], GRAV)
*
* GRAV overrides the gravity vector in the robot object.
*
* 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)
*
*/
/*
* Copyright (C) 1999-2008, by Peter I. Corke
*
* This file is part of The Robotics Toolbox for Matlab (RTB).
*
* RTB is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTB is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Leser General Public License
* along with RTB. If not, see <http://www.gnu.org/licenses/>.
*
*/
#include "mex.h"
#include <math.h>
#include "frne.h"
/*
#define DEBUG
*/
/* Input Arguments */
#define ROBOT_IN prhs[0]
#define A1_IN prhs[1]
#define A2_IN prhs[2]
#define A3_IN prhs[3]
#define A4_IN prhs[4]
#define A5_IN prhs[5]
/* Output Arguments */
#define TAU_OUT plhs[0]
/* Some useful things */
#define NUMROWS(x) mxGetM(x)
#define NUMCOLS(x) mxGetN(x)
#define NUMELS(x) (mxGetN(x)*mxGetM(x))
#define POINTER(x) mxGetPr(x)
/* forward defines */
static void rot_mat (Link *l, double th, double d, DHType type);
static int mstruct_getfield_number(mxArray *m, char *field);
static int mstruct_getint(mxArray *m, int i, char *field);
static double mstruct_getreal(mxArray *m, int i, char *field);
static double * mstruct_getrealvect(mxArray *m, int i, char *field);
void error(char *s, ...);
/* default values for gravity and external load */
/**
* MEX function entry point.
*/
void
mexFunction(
int nlhs,
mxArray *plhs[],
int nrhs,
const mxArray *prhs[]
) {
double *q, *qd, *qdd;
double *tau;
unsigned int m,n;
int j, njoints, p, nq;
double *fext = NULL;
double *grav = NULL;
Robot robot;
mxArray *link0;
mxArray *mx_robot;
mxArray *mx_links;
static int first_time = 0;
/*
fprintf(stderr, "Fast RNE: (c) Peter Corke 2002-2011\n");
*/
if ( !mxIsClass(ROBOT_IN, "SerialLink") )
mexErrMsgTxt("frne: 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("frne: 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("frne: 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("frne: 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("frne: gravity vector expected");
grav = POINTER(A2_IN);
if (NUMELS(A3_IN) != 6)
mexErrMsgTxt("frne: 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("frne: Q QD QDD must be same length");
if ( (NUMCOLS(A1_IN) != njoints) ||
(NUMCOLS(A2_IN) != njoints) ||
(NUMCOLS(A3_IN) != njoints)
)
mexErrMsgTxt("frne: 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("frne: Q QD QDD must be same length");
if ( (NUMCOLS(A1_IN) != njoints) ||
(NUMCOLS(A2_IN) != njoints) ||
(NUMCOLS(A3_IN) != njoints)
)
mexErrMsgTxt("frne: Q must have Naxis columns");
q = POINTER(A1_IN);
qd = POINTER(A2_IN);
qdd = POINTER(A3_IN);
if (NUMELS(A4_IN) != 3)
mexErrMsgTxt("frne: 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("frne: Q QD QDD must be same length");
if ( (NUMCOLS(A1_IN) != njoints) ||
(NUMCOLS(A2_IN) != njoints) ||
(NUMCOLS(A3_IN) != njoints)
)
mexErrMsgTxt("frne: Q must have Naxis columns");
q = POINTER(A1_IN);
qd = POINTER(A2_IN);
qdd = POINTER(A3_IN);
if (NUMELS(A4_IN) != 3)
mexErrMsgTxt("frne: gravity vector expected");
grav = POINTER(A4_IN);
if (NUMELS(A5_IN) != 6)
mexErrMsgTxt("frne: Fext vector expected");
fext = POINTER(A5_IN);
break;
}
default:
mexErrMsgTxt("frne: wrong number of arguments.");
}
/*
* fill out the robot structure
*/
robot.njoints = njoints;
if (grav)
robot.gravity = (Vect *)grav;
else
robot.gravity = (Vect *)mxGetPr( mxGetProperty(mx_robot, (mwIndex)0, "gravity") );
robot.dhtype = mstruct_getint(mx_robot, 0, "mdh");
/* build link structure */
robot.links = (Link *)mxCalloc((mwSize) njoints, (mwSize) 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 the first link structure */
link0 = mxGetProperty(mx_robot, (mwIndex) 0, "links");
if (link0 == NULL)
mexErrMsgTxt("couldnt find element link in robot object");
/*
* Elements of the link structure are:
*
* alpha:
* A:
* theta:
* D:
* offset:
* sigma:
* mdh:
* m:
* r:
* I:
* Jm:
* G:
* B:
* Tc:
*/
if (first_time == 0) {
mexPrintf("Fast RNE: (c) Peter Corke 2002-2012\n");
first_time = 1;
}
/*
* copy data from the Link objects into the local links structure
* to save function calls later
*/
for (j=0; j<njoints; j++) {
Link *l = &robot.links[j];
mxArray *links = mxGetProperty(mx_robot, (mwIndex) 0, "links"); // links array
l->alpha = mxGetScalar( mxGetProperty(links, (mwIndex) j, "alpha") );
l->A = mxGetScalar( mxGetProperty(links, (mwIndex) j, "a") );
l->theta = mxGetScalar( mxGetProperty(links, (mwIndex) j, "theta") );
l->D = mxGetScalar( mxGetProperty(links, (mwIndex) j, "d") );
l->sigma = mxGetScalar( mxGetProperty(links, (mwIndex) j, "sigma") );
l->offset = mxGetScalar( mxGetProperty(links, (mwIndex) j, "offset") );
l->m = mxGetScalar( mxGetProperty(links, (mwIndex) j, "m") );
l->rbar = (Vect *)mxGetPr( mxGetProperty(links, (mwIndex) j, "r") );
l->I = mxGetPr( mxGetProperty(links, (mwIndex) j, "I") );
l->Jm = mxGetScalar( mxGetProperty(links, (mwIndex) j, "Jm") );
l->G = mxGetScalar( mxGetProperty(links, (mwIndex) j, "G") );
l->B = mxGetScalar( mxGetProperty(links, (mwIndex) j, "B") );
l->Tc = mxGetPr( mxGetProperty(links, (mwIndex) j, "Tc") );
}
/* Create a matrix for the return argument */
TAU_OUT = mxCreateDoubleMatrix((mwSize) nq, (mwSize) 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
*
* Permission to use and distribute is granted, provided that this message
* is retained, and due credit given when the results are incorporated in
* publised work.
*
*/
/**
* 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, get the field from joint i
*************************************************************************/
static mxArray *
mstruct_get_element(mxArray *m, int j, char *field)
{
mxArray *e;
if ((e = mxGetProperty(m, (mwIndex)j, field)) != NULL)
return e;
else {
error("No such field as %s", field);
}
}
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);
}