void CAgent::print(std::vector<float> subspace)
{
CLog log((char*)"q_map.txt", 3);
u32 i;
float y, x;
std::vector<float> state;
for (i = 0; i < agent.inputs_count; i++)
state.push_back(0.0);
for (i = 0; i < subspace.size(); i++)
state[i] = subspace[i];
printf("\n q max \n");
for (y = -1.0; y < 1.0; y+= agent.state_density)
{
for (x = -1.0; x < 1.0; x+= agent.state_density)
{
state[subspace.size() + 1] = y;
state[subspace.size() + 0] = x;
float res = q_learning->get_max_q(state);
printf("%2.3f ", res);
//u32 state_idx = q_learning->get_state_idx();
//printf("%3u ", state_idx);
log.add(0, x);
log.add(1, y);
log.add(2, res);
}
printf("\n");
}
log.save();
printf("\n\n");
printf("\n q min \n");
for (y = -1.0; y < 1.0; y+= agent.state_density)
{
for (x = -1.0; x < 1.0; x+= agent.state_density)
{
state[subspace.size() + 1] = y;
state[subspace.size() + 0] = x;
float res = q_learning->get_min_q(state);
printf("%2.3f ", res);
}
printf("\n");
}
printf("\n\n");
printf("\n q id \n");
CLog log_action_id((char*)"q_action_id.txt", 3);
for (y = -1.0; y < 1.0; y+= agent.state_density)
{
for (x = -1.0; x < 1.0; x+= agent.state_density)
{
state[subspace.size() + 1] = y;
state[subspace.size() + 0] = x;
u32 res = q_learning->get_max_q_id(state);
printf("%3i ", res);
log_action_id.add(0, x);
log_action_id.add(1, y);
log_action_id.add(2, res);
}
printf("\n");
}
printf("\n\n");
log_action_id.save();
printf("\nbest action matrix \n");
CLog log_action((char*)"q_action.txt", 2 + actions->get_action_size());
for (y = -1.0; y < 1.0; y+= agent.state_density)
{
for (x = -1.0; x < 1.0; x+= agent.state_density)
{
state[subspace.size() + 1] = y;
state[subspace.size() + 0] = x;
u32 action_id = q_learning->get_max_q_id(state);
struct sAction action = actions->get(0, action_id);
vect_print(action.action);
log_action.add(0, x);
log_action.add(1, y);
for (i = 0; i < action.action.size(); i++)
log_action.add(2 + i, action.action[i]);
}
printf("\n");
}
printf("\n\n");
log_action.save();
}
/**
* 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);
}
/**
* Recursive Newton-Euler algorithm.
*
* @Note the parameter \p stride which is used to allow for input and output
* arrays which are 2-dimensional but in column-major (Matlab) order. We
* need to access rows from the arrays.
*
*/
void
newton_euler (
Robot *robot, /*!< robot object */
double *tau, /*!< returned joint torques */
double *qd, /*!< joint velocities */
double *qdd, /*!< joint accelerations */
double *fext, /*!< external force on manipulator tip */
int stride /*!< indexing stride for qd, qdd */
) {
Vect t1, t2, t3, t4;
Vect qdv, qddv;
Vect F, N;
Vect z0 = {0.0, 0.0, 1.0};
Vect zero = {0.0, 0.0, 0.0};
Vect f_tip = {0.0, 0.0, 0.0};
Vect n_tip = {0.0, 0.0, 0.0};
register int j;
double t;
Link *links = robot->links;
/*
* angular rate and acceleration vectors only have finite
* z-axis component
*/
qdv = qddv = zero;
/* setup external force/moment vectors */
if (fext) {
f_tip.x = fext[0];
f_tip.y = fext[1];
f_tip.z = fext[2];
n_tip.x = fext[3];
n_tip.y = fext[4];
n_tip.z = fext[5];
}
/******************************************************************************
* forward recursion --the kinematics
******************************************************************************/
if (robot->dhtype == MODIFIED) {
/*
* MODIFIED D&H CONVENTIONS
*/
for (j = 0; j < robot->njoints; j++) {
/* create angular vector from scalar input */
qdv.z = qd[j*stride];
qddv.z = qdd[j*stride];
switch (links[j].sigma) {
case REVOLUTE:
/*
* calculate angular velocity of link j
*/
if (j == 0)
*OMEGA(j) = qdv;
else {
rot_trans_vect_mult (&t1, ROT(j), OMEGA(j-1));
vect_add (OMEGA(j), &t1, &qdv);
}
/*
* calculate angular acceleration of link j
*/
if (j == 0)
*OMEGADOT(j) = qddv;
else {
rot_trans_vect_mult (&t3, ROT(j), OMEGADOT(j-1));
vect_cross (&t2, &t1, &qdv);
vect_add (&t1, &t2, &t3);
vect_add (OMEGADOT(j), &t1, &qddv);
}
/*
* compute acc[j]
*/
if (j == 0) {
t1 = *robot->gravity;
} else {
vect_cross(&t1, OMEGA(j-1), PSTAR(j));
vect_cross(&t2, OMEGA(j-1), &t1);
vect_cross(&t1, OMEGADOT(j-1), PSTAR(j));
vect_add(&t1, &t1, &t2);
vect_add(&t1, &t1, ACC(j-1));
}
rot_trans_vect_mult(ACC(j), ROT(j), &t1);
break;
case PRISMATIC:
/*
* calculate omega[j]
*/
if (j == 0)
*(OMEGA(j)) = qdv;
else
rot_trans_vect_mult (OMEGA(j), ROT(j), OMEGA(j-1));
/*
* calculate alpha[j]
*/
if (j == 0)
*(OMEGADOT(j)) = qddv;
else
rot_trans_vect_mult (OMEGADOT(j), ROT(j), OMEGADOT(j-1));
/*
* compute acc[j]
*/
if (j == 0) {
*ACC(j) = *robot->gravity;
} else {
vect_cross(&t1, OMEGADOT(j-1), PSTAR(j));
vect_cross(&t3, OMEGA(j-1), PSTAR(j));
vect_cross(&t2, OMEGA(j-1), &t3);
vect_add(&t1, &t1, &t2);
vect_add(&t1, &t1, ACC(j-1));
rot_trans_vect_mult(ACC(j), ROT(j), &t1);
rot_trans_vect_mult(&t2, ROT(j), OMEGA(j-1));
vect_cross(&t1, &t2, &qdv);
scal_mult(&t1, &t1, 2.0);
vect_add(ACC(j), ACC(j), &t1);
vect_add(ACC(j), ACC(j), &qddv);
}
break;
}
/*
* compute abar[j]
*/
vect_cross(&t1, OMEGADOT(j), R_COG(j));
vect_cross(&t2, OMEGA(j), R_COG(j));
vect_cross(&t3, OMEGA(j), &t2);
vect_add(ACC_COG(j), &t1, &t3);
vect_add(ACC_COG(j), ACC_COG(j), ACC(j));
#ifdef DEBUG
vect_print("w", OMEGA(j));
vect_print("wd", OMEGADOT(j));
vect_print("acc", ACC(j));
vect_print("abar", ACC_COG(j));
#endif
}
} else {
/*
* STANDARD D&H CONVENTIONS
*/
for (j = 0; j < robot->njoints; j++) {
/* create angular vector from scalar input */
qdv.z = qd[j*stride];
qddv.z = qdd[j*stride];
switch (links[j].sigma) {
case REVOLUTE:
/*
* calculate omega[j]
*/
if (j == 0)
t1 = qdv;
else
vect_add (&t1, OMEGA(j-1), &qdv);
rot_trans_vect_mult (OMEGA(j), ROT(j), &t1);
/*
* calculate alpha[j]
*/
if (j == 0)
t3 = qddv;
else {
vect_add (&t1, OMEGADOT(j-1), &qddv);
vect_cross (&t2, OMEGA(j-1), &qdv);
vect_add (&t3, &t1, &t2);
}
rot_trans_vect_mult (OMEGADOT(j), ROT(j), &t3);
/*
* compute acc[j]
*/
vect_cross(&t1, OMEGADOT(j), PSTAR(j));
vect_cross(&t2, OMEGA(j), PSTAR(j));
vect_cross(&t3, OMEGA(j), &t2);
vect_add(ACC(j), &t1, &t3);
if (j == 0) {
rot_trans_vect_mult(&t1, ROT(j), robot->gravity);
} else
rot_trans_vect_mult(&t1, ROT(j), ACC(j-1));
vect_add(ACC(j), ACC(j), &t1);
break;
case PRISMATIC:
/*
* calculate omega[j]
*/
if (j == 0)
*(OMEGA(j)) = zero;
else
rot_trans_vect_mult (OMEGA(j), ROT(j), OMEGA(j-1));
/*
* calculate alpha[j]
*/
if (j == 0)
*(OMEGADOT(j)) = zero;
else
rot_trans_vect_mult (OMEGADOT(j), ROT(j), OMEGADOT(j-1));
/*
* compute acc[j]
*/
if (j == 0) {
vect_add(&qddv, &qddv, robot->gravity);
rot_trans_vect_mult(ACC(j), ROT(j), &qddv);
} else {
vect_add(&t1, &qddv, ACC(j-1));
rot_trans_vect_mult(ACC(j), ROT(j), &t1);
}
vect_cross(&t1, OMEGADOT(j), PSTAR(j));
vect_add(ACC(j), ACC(j), &t1);
rot_trans_vect_mult(&t1, ROT(j), &qdv);
vect_cross(&t2, OMEGA(j), &t1);
scal_mult(&t2, &t2, 2.0);
vect_add(ACC(j), ACC(j), &t2);
vect_cross(&t2, OMEGA(j), PSTAR(j));
vect_cross(&t3, OMEGA(j), &t2);
vect_add(ACC(j), ACC(j), &t3);
break;
}
/*
* compute abar[j]
*/
vect_cross(&t1, OMEGADOT(j), R_COG(j));
vect_cross(&t2, OMEGA(j), R_COG(j));
vect_cross(&t3, OMEGA(j), &t2);
vect_add(ACC_COG(j), &t1, &t3);
vect_add(ACC_COG(j), ACC_COG(j), ACC(j));
#ifdef DEBUG
vect_print("w", OMEGA(j));
vect_print("wd", OMEGADOT(j));
vect_print("acc", ACC(j));
vect_print("abar", ACC_COG(j));
#endif
}
}
/******************************************************************************
* backward recursion part --the kinetics
******************************************************************************/
if (robot->dhtype == MODIFIED) {
/*
* MODIFIED D&H CONVENTIONS
*/
for (j = robot->njoints - 1; j >= 0; j--) {
/*
* compute F[j]
*/
scal_mult (&F, ACC_COG(j), M(j));
/*
* compute f[j]
*/
if (j == (robot->njoints-1))
t1 = f_tip;
else
rot_vect_mult (&t1, ROT(j+1), f(j+1));
vect_add (f(j), &t1, &F);
/*
* compute N[j]
*/
mat_vect_mult(&t2, INERTIA(j), OMEGADOT(j));
mat_vect_mult(&t3, INERTIA(j), OMEGA(j));
vect_cross(&t4, OMEGA(j), &t3);
vect_add(&N, &t2, &t4);
/*
* compute n[j]
*/
if (j == (robot->njoints-1))
t1 = n_tip;
else {
rot_vect_mult(&t1, ROT(j+1), n(j+1));
rot_vect_mult(&t4, ROT(j+1), f(j+1));
vect_cross(&t3, PSTAR(j+1), &t4);
vect_add(&t1, &t1, &t3);
}
vect_cross(&t2, R_COG(j), &F);
vect_add(&t1, &t1, &t2);
vect_add(n(j), &t1, &N);
#ifdef DEBUG
vect_print("f", f(j));
vect_print("n", n(j));
#endif
}
} else {
/*
* STANDARD D&H CONVENTIONS
*/
for (j = robot->njoints - 1; j >= 0; j--) {
/*
* compute f[j]
*/
scal_mult (&t4, ACC_COG(j), M(j));
if (j != (robot->njoints-1)) {
rot_vect_mult (&t1, ROT(j+1), f(j+1));
vect_add (f(j), &t4, &t1);
} else
vect_add (f(j), &t4, &f_tip);
/*
* compute n[j]
*/
/* cross(pstar+r,Fm(:,j)) */
vect_add(&t2, PSTAR(j), R_COG(j));
vect_cross(&t1, &t2, &t4);
if (j != (robot->njoints-1)) {
/* cross(R'*pstar,f) */
rot_trans_vect_mult(&t2, ROT(j+1), PSTAR(j));
vect_cross(&t3, &t2, f(j+1));
/* nn += R*(nn + cross(R'*pstar,f)) */
vect_add(&t3, &t3, n(j+1));
rot_vect_mult(&t2, ROT(j+1), &t3);
vect_add(&t1, &t1, &t2);
} else {
/* cross(R'*pstar,f) */
vect_cross(&t2, PSTAR(j), &f_tip);
/* nn += R*(nn + cross(R'*pstar,f)) */
vect_add(&t1, &t1, &t2);
vect_add(&t1, &t1, &n_tip);
}
mat_vect_mult(&t2, INERTIA(j), OMEGADOT(j));
mat_vect_mult(&t3, INERTIA(j), OMEGA(j));
vect_cross(&t4, OMEGA(j), &t3);
vect_add(&t2, &t2, &t4);
vect_add(n(j), &t1, &t2);
#ifdef DEBUG
vect_print("f", f(j));
vect_print("n", n(j));
#endif
}
}
/*
* Compute the torque total for each axis
*
*/
for (j=0; j < robot->njoints; j++) {
double t;
Link *l = &links[j];
if (robot->dhtype == MODIFIED)
t1 = z0;
else
rot_trans_vect_mult(&t1, ROT(j), &z0);
switch (l->sigma) {
case REVOLUTE:
t = vect_dot(n(j), &t1);
break;
case PRISMATIC:
t = vect_dot(f(j), &t1);
break;
}
/*
* add actuator dynamics and friction
*/
t += l->G * l->G * l->Jm * qdd[j*stride]; // inertia
t += l->G * l->G * l->B * qd[j*stride]; // viscous friction
t += fabs(l->G) * (
(qd[j*stride] > 0 ? l->Tc[0] : 0.0) + // Coulomb friction
(qd[j*stride] < 0 ? l->Tc[1] : 0.0)
);
tau[j*stride] = t;
}
}
