jepler/linuxcnc
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

motion: check for kinematicsInverse failures

Browse source 
.. and NaN and inf returns of joint positions from kinematicsInverse
This commit is contained in:
Jeff Epler 2013-07-18 13:43:23 -05:00 committed by Sebastian Kuzminsky
parent 6f9ae3cf86
commit 4b8995385c
2 changed files with 70 additions and 24 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

7
src/emc/motion/command.c
View file

@ -256,7 +256,12 @@ STATIC int inRange(EmcPose pos, int id, char *move_type)
} }
/* now fill in with real values, for joints that are used */ /* now fill in with real values, for joints that are used */
kinematicsInverse(&pos, joint_pos, &iflags, &fflags); if (kinematicsInverse(&pos, joint_pos, &iflags, &fflags) < 0)
{
reportError(_("%s move on line %d fails kinematicsInverse"),
move_type, id);
return 0;
}
for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) { for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
/* point to joint data */ /* point to joint data */

87
src/emc/motion/control.c
View file

@ -1127,18 +1127,38 @@ static void get_pos_cmds(long period)
tpGetPos(&emcmotDebug->coord_tp, &emcmotStatus->carte_pos_cmd); tpGetPos(&emcmotDebug->coord_tp, &emcmotStatus->carte_pos_cmd);
/* OUTPUT KINEMATICS - convert to joints in local array */ /* OUTPUT KINEMATICS - convert to joints in local array */
kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions, result = kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions,
&iflags, &fflags); &iflags, &fflags);
/* copy to joint structures and spline them up */ if(result == 0)
for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) { {
/* point to joint struct */ /* copy to joint structures and spline them up */
joint = &joints[joint_num]; for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
joint->coarse_pos = positions[joint_num]; if(!isfinite(positions[joint_num]))
/* spline joints up-- note that we may be adding points {
that fail soft limits, but we'll abort at the end of reportError(_("kinematicsInverse gave non-finite joint location on joint %d"), joint_num);
this cycle so it doesn't really matter */ SET_MOTION_ERROR_FLAG(1);
cubicAddPoint(&(joint->cubic), joint->coarse_pos); SET_MOTION_ENABLE_FLAG(0);
emcmotDebug->enabling = 0;
break;
}
/* point to joint struct */
joint = &joints[joint_num];
joint->coarse_pos = positions[joint_num];
/* spline joints up-- note that we may be adding points
that fail soft limits, but we'll abort at the end of
this cycle so it doesn't really matter */
cubicAddPoint(&(joint->cubic), joint->coarse_pos);
}
} }
else
{
reportError(_("kinematicsInverse failed"));
SET_MOTION_ERROR_FLAG(1);
SET_MOTION_ENABLE_FLAG(0);
emcmotDebug->enabling = 0;
break;
}
/* END OF OUTPUT KINS */ /* END OF OUTPUT KINS */
} }
/* there is data in the interpolators */ /* there is data in the interpolators */
@ -1182,21 +1202,42 @@ static void get_pos_cmds(long period)
to compute the next positions of the joints */ to compute the next positions of the joints */
/* OUTPUT KINEMATICS - convert to joints in local array */ /* OUTPUT KINEMATICS - convert to joints in local array */
kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions, &iflags, &fflags); result = kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions, &iflags, &fflags);
/* copy to joint structures and spline them up */ /* copy to joint structures and spline them up */
for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) { if(result == 0)
/* point to joint struct */ {
joint = &joints[joint_num]; for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
joint->coarse_pos = positions[joint_num]; if(!isfinite(positions[joint_num]))
/* spline joints up-- note that we may be adding points {
that fail soft limits, but we'll abort at the end of reportError(_("kinematicsInverse gave non-finite joint location on joint %d"), joint_num);
this cycle so it doesn't really matter */ SET_MOTION_ERROR_FLAG(1);
cubicAddPoint(&(joint->cubic), joint->coarse_pos); SET_MOTION_ENABLE_FLAG(0);
old_pos_cmd = joint->pos_cmd; emcmotDebug->enabling = 0;
/* interpolate to get new one */ break;
joint->pos_cmd = cubicInterpolate(&(joint->cubic), 0, 0, 0, 0); }
joint->vel_cmd = (joint->pos_cmd - old_pos_cmd) * servo_freq; /* point to joint struct */
joint = &joints[joint_num];
joint->coarse_pos = positions[joint_num];
/* spline joints up-- note that we may be adding points
that fail soft limits, but we'll abort at the end of
this cycle so it doesn't really matter */
cubicAddPoint(&(joint->cubic), joint->coarse_pos);
old_pos_cmd = joint->pos_cmd;
/* interpolate to get new one */
joint->pos_cmd = cubicInterpolate(&(joint->cubic), 0, 0, 0, 0);
joint->vel_cmd = (joint->pos_cmd - old_pos_cmd) * servo_freq;
}
} }
else
{
reportError(_("kinematicsInverse failed"));
SET_MOTION_ERROR_FLAG(1);
SET_MOTION_ENABLE_FLAG(0);
emcmotDebug->enabling = 0;
break;
}
/* END OF OUTPUT KINS */ /* END OF OUTPUT KINS */
/* end of teleop mode */ /* end of teleop mode */