diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index e1956ad92eb..88c8c76804a 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -19,370 +19,19 @@ #include "spherical_arc.h" #include "blendmath.h" #include "tp_debug.h" +#include "tp_enums.h" +#include "stdlib.h" +#include "math_util.h" +#include "emcpose.h" -/** @section utilityfuncs Utility functions */ - -/** - * Find the maximum angle allowed between "tangent" segments. - * @param v speed of motion in worst case (i.e. at max feed). - * @param acc magnitude of acceleration allowed during "kink". - * - * Since we are discretized by a timestep, the maximum allowable - * "kink" in a trajectory is bounded by normal acceleration. A small - * kink will effectively be one step along the tightest radius arc - * possible at a given speed. - */ -double findMaxTangentAngle(double v_plan, double acc_limit, double cycle_time) -{ - //Find acc hiccup we're allowed to get - //TODO somewhat redundant with findKinkAccel, should refactor - double acc_margin = BLEND_ACC_RATIO_NORMAL * BLEND_KINK_FACTOR * acc_limit; - double dx = v_plan / cycle_time; - if (dx > 0.0) { - return (acc_margin / dx); - } else { - tp_debug_print(" Velocity or period is negative!\n"); - //Should not happen... - return TP_ANGLE_EPSILON; - } -} - - -/** - * Find the acceleration required to create a specific change in path - * direction, assuming constant speed. - * This determines how much of a "spike" in acceleration will occur due to a - * slight mismatch between tangent directions at the start / end of a segment. - */ -double findKinkAccel(double kink_angle, double v_plan, double cycle_time) -{ - double dx = v_plan / cycle_time; - if (dx > 0.0) { - return (dx * kink_angle); - } else { - rtapi_print_msg(RTAPI_MSG_ERR, "dx < 0 in KinkAccel\n"); - return 0; - } -} - - -/** - * Sign function that returns a valid numerical result for sign(0), rather than NaN. - */ -double fsign(double f) -{ - if (f>0) { - return 1.0; - } else if (f < 0) { - return -1.0; - } else { - //Technically this should be NAN but that's a useless result for tp purposes - return 0; - } -} - -/** negate a value (or not) based on a bool parameter */ -static inline double negate(double f, int neg) -{ - return (neg) ? -f : f; -} - -/** Clip the input at the specified minimum (in place). */ -int clip_min(double * const x, double min) { - if ( *x < min ) { - *x = min; - return 1; - } - return 0; -} - - -/** Clip the input at the specified maximum (in place). */ -int clip_max(double * const x, double max) { - if ( *x > max ) { - *x = max; - return 1; - } - return 0; -} - -/** - * Saturate a value x to be within +/- max. - */ -double saturate(double x, double max) { - if ( x > max ) { - return max; - } - else if ( x < (-max) ) { - return -max; - } - else { - return x; - } -} - -/** - * Saturate a value x to be within max and min. - */ -double bisaturate(double x, double max, double min) { - if ( x > max ) { - return max; - } - else if ( x < min ) { - return min; - } - else { - return x; - } -} - - -/** - * Apply bounds to a value x. - */ -inline double bound(double x, double max, double min) { - if ( x > max ) { - return max; - } - else if ( x < (min) ) { - return min; - } - else { - return x; - } -} +#include "tp_call_wrappers.h" - -/** In-place saturation function */ -int sat_inplace(double * const x, double max) { - if ( *x > max ) { - *x = max; - return 1; - } - else if ( *x < -max ) { - *x = -max; - return -1; - } - return 0; -} - -#if 0 -static int pmCirclePrint(PmCircle const * const circ) { - tp_debug_print(" center = %f %f %f\n", - circ->center.x, - circ->center.y, - circ->center.z); - tp_debug_print(" radius = %f\n", circ->radius); - tp_debug_print(" spiral = %f\n", circ->spiral); - tp_debug_print(" angle = %f\n", circ->angle); - //TODO add other debug data here as needed - return TP_ERR_OK; -} -#endif +static const double NEAR_INFINITY=1e300; /** * @section geomfuncs Geometry check functions */ -/** - * Calculate the best-fit circle to the spiral segment. - * @param circ spiral to be approximated - * @param base_pt the point about which the circle is fit - * @param u_tan tangent unit vector at the base point of the approximation - * @param[out] center_out displaced center for circular approximation - * @param[out] radius_out adjusted radius - * - * The adjusted center for the circle fit is found by displacing the center - * along the spiral tangent by the spiral coefficient. The adjusted radius is - * the distance between the base point and this new center. - * - */ -static inline int findSpiralApproximation(PmCircle const * const circ, - PmCartesian const * const base_pt, - PmCartesian const * const u_tan, - PmCartesian * const center_out, - double * const radius_out) -{ - double dr = circ->spiral / circ->angle; - - /*tp_debug_print("In findSpiralApproximation\n");*/ - /*tp_debug_print(" dr = %f\n",dr);*/ - /*tp_debug_print(" utan = %f %f %f\n",*/ - /*u_tan->x,*/ - /*u_tan->y,*/ - /*u_tan->z);*/ - pmCartScalMult(u_tan, dr, center_out); - /*tp_debug_print(" circcenter = %f %f %f\n",*/ - /*circ->center.x,*/ - /*circ->center.y,*/ - /*circ->center.z);*/ - pmCartCartAddEq(center_out, &circ->center); - - PmCartesian r_adjust; - pmCartCartSub(base_pt, center_out, &r_adjust); - pmCartMag(&r_adjust, radius_out); - - tp_debug_print(" adjusted center = %f %f %f\n", - center_out->x, - center_out->y, - center_out->z); - - tp_debug_print(" adjusted radius = %f\n", *radius_out); - - return TP_ERR_OK; -} - - -/** - * Calculate the angle to trim from a circle based on the blend geometry. - * - * @param P intersection point - * @param arc_center calculated center of blend arc - * @param center actual center of circle (not spiral approximated center) - * @return trim angle - */ -static inline double findTrimAngle(PmCartesian const * const P, - PmCartesian const * const arc_center, - PmCartesian const * const center) -{ - //Define vectors relative to circle center - PmCartesian u_P; - pmCartCartSub(P, center, &u_P); - pmCartUnitEq(&u_P); - - PmCartesian u_arccenter; - pmCartCartSub(arc_center, center, &u_arccenter); - pmCartUnitEq(&u_arccenter); - - double dot; - pmCartCartDot(&u_arccenter, &u_P, &dot); - double dphi = acos(saturate(dot,1.0)); - tp_debug_print(" dphi = %g\n",dphi); - return dphi; -} - - -/** - * Verify that a blend arc is tangent to a circular arc. - */ -int checkTangentAngle(PmCircle const * const circ, SphericalArc const * const arc, BlendGeom3 const * const geom, BlendParameters const * const param, double cycle_time, int at_end) -{ - (void)geom; - // Debug Information to diagnose tangent issues - PmCartesian u_circ, u_arc; - arcTangent(arc, &u_arc, at_end); - - if (at_end) { - pmCircleTangentVector(circ, 0, &u_circ); - } else { - pmCircleTangentVector(circ, circ->angle, &u_circ); - } - - pmCartUnitEq(&u_arc); - - // Find angle between tangent unit vectors - double dot; - pmCartCartDot(&u_circ, &u_arc, &dot); - double blend_angle = acos(saturate(dot,1.0)); - - // Check against the maximum allowed tangent angle for the given velocity and acceleration - double angle_max = findMaxTangentAngle(param->v_plan, param->a_max, cycle_time); - - tp_debug_print("tangent angle = %f, max = %f\n", - blend_angle, - angle_max); - - tp_debug_print("circ_tan = [%g %g %g]\n", - u_circ.x, - u_circ.y, - u_circ.z); - tp_debug_print("arc_tan = [%g %g %g]\n", - u_arc.x, - u_arc.y, - u_arc.z); - - PmCartesian diff; - pmCartCartSub(&u_arc,&u_circ,&diff); - tp_debug_print("diff = [%g %g %g]\n", - diff.x, - diff.y, - diff.z); - - if (blend_angle > angle_max) { - tp_debug_print("angle too large\n"); - return TP_ERR_FAIL; - } - - return TP_ERR_OK; -} - - -/** - * Checks if two UNIT vectors are parallel to the given angle tolerance (in radians). - * @warning tol depends on the small angle approximation and will not be - * accurate for angles larger than about 10 deg. This function is meant for - * small tolerances! - */ -int pmCartCartParallel(PmCartesian const * const u1, - PmCartesian const * const u2, - double tol) -{ - double d_diff; - { - PmCartesian u_diff; - pmCartCartSub(u1, u2, &u_diff); - pmCartMagSq(&u_diff, &d_diff); - } - - tp_debug_json_start(pmCartCartParallel); - tp_debug_json_double(d_diff); - tp_debug_json_end(); - - return d_diff < tol; -} - -/** - * Checks if two UNIT vectors are anti-parallel to the given angle tolerance (in radians). - * @warning tol depends on the small angle approximation and will not be - * accurate for angles larger than about 10 deg. This function is meant for - * small tolerances! - */ -int pmCartCartAntiParallel(PmCartesian const * const u1, - PmCartesian const * const u2, - double tol) -{ - double d_sum; - { - PmCartesian u_sum; - pmCartCartAdd(u1, u2, &u_sum); - pmCartMagSq(&u_sum, &d_sum); - } - - tp_debug_json_start(pmCartCartAntiParallel); - tp_debug_json_double(d_sum); - tp_debug_json_end(); - - return d_sum < tol; -} - - -/** - * Check if two cartesian vectors are parallel or anti-parallel - * The input tolerance specifies what the maximum angle between the - * lines containing two vectors is. Note that vectors pointing in - * opposite directions are still considered parallel, since their - * containing lines are parallel. - * @param u1 input unit vector 1 - * @param u2 input unit vector 2 - * @pre BOTH u1 and u2 must be unit vectors or calculation may be skewed. - */ -int pmUnitCartsColinear(PmCartesian const * const u1, - PmCartesian const * const u2) -{ - return pmCartCartParallel(u1, u2, TP_ANGLE_EPSILON_SQ) || pmCartCartAntiParallel(u1, u2, TP_ANGLE_EPSILON_SQ); -} - - /** * Somewhat redundant function to calculate the segment intersection angle. * The intersection angle is half of the supplement of the "divergence" angle @@ -390,166 +39,83 @@ int pmUnitCartsColinear(PmCartesian const * const u1, * direction, then the intersection angle is PI/2. This is based on the * simple_tp formulation for tolerances. */ -int findIntersectionAngle(PmCartesian const * const u1, - PmCartesian const * const u2, double * const theta) +double findIntersectionAngle(PmVector const * const u1, + PmVector const * const u2) { - double dot; - pmCartCartDot(u1, u2, &dot); - - if (dot > 1.0 || dot < -1.0) { - tp_debug_print("dot product %.16g outside domain of acos! u1 = %.16g %.16g %.16g, u2 = %.16g %.16g %.16g\n", - dot, - u1->x, - u1->y, - u1->z, - u2->x, - u2->y, - u2->z); - sat_inplace(&dot,1.0); - } - - *theta = acos(-dot)/2.0; - return TP_ERR_OK; -} + double dot = VecVecDot(u1, u2); + sat_inplace(&dot,1.0); -/** Calculate the minimum of the three values in a PmCartesian. */ -double pmCartMin(PmCartesian const * const in) -{ - return fmin(fmin(in->x,in->y),in->z); + return acos(-dot)/2.0; } - -/** - * Calculate the diameter of a circle incscribed on a central cross section of a 3D - * rectangular prism. - * - * @param normal normal direction of plane slicing prism. - * @param extents distance from center to one corner of the prism. - * @param diameter diameter of inscribed circle on cross section. - * - */ -int calculateInscribedDiameter(PmCartesian const * const normal, - PmCartesian const * const bounds, double * const diameter) +int findAccelScale(PmVector const * const acc, + PmVector const * const bounds, + PmVector * const scale) { - if (!normal ) { + if (!acc || !bounds ) { return TP_ERR_MISSING_INPUT; } - double n_mag; - pmCartMagSq(normal, &n_mag); - double mag_err = fabs(1.0 - n_mag); - if (mag_err > pmSqrt(TP_POS_EPSILON)) { - /*rtapi_print_msg(RTAPI_MSG_ERR,"normal vector <%.12g,%.12f,%.12f> has magnitude error = %e\n",*/ - /*normal->x,*/ - /*normal->y,*/ - /*normal->z,*/ - /*mag_err);*/ - return TP_ERR_FAIL; - } - - PmCartesian planar_x,planar_y,planar_z; - - //Find perpendicular component of unit directions - // FIXME Assumes normal is unit length - - /* This section projects the X / Y / Z unit vectors onto the plane - * containing the motions. The operation is done "backwards" here due to a - * quirk with posemath. - * - */ - pmCartScalMult(normal, -normal->x, &planar_x); - pmCartScalMult(normal, -normal->y, &planar_y); - pmCartScalMult(normal, -normal->z, &planar_z); - - planar_x.x += 1.0; - planar_y.y += 1.0; - planar_z.z += 1.0; - - pmCartAbs(&planar_x, &planar_x); - pmCartAbs(&planar_y, &planar_y); - pmCartAbs(&planar_z, &planar_z); - - // Crude way to prevent divide-by-zero-error - planar_x.x = fmax(planar_x.x,TP_POS_EPSILON); - planar_y.y = fmax(planar_y.y,TP_POS_EPSILON); - planar_z.z = fmax(planar_z.z,TP_POS_EPSILON); - - double x_scale, y_scale, z_scale; - pmCartMag(&planar_x, &x_scale); - pmCartMag(&planar_y, &y_scale); - pmCartMag(&planar_z, &z_scale); - - double x_extent=0, y_extent=0, z_extent=0; - if (bounds->x != 0) { - x_extent = bounds->x / x_scale; - } - if (bounds->y != 0) { - y_extent = bounds->y / y_scale; - } - if (bounds->z != 0) { - z_extent = bounds->z / z_scale; + if (!scale ) { + return TP_ERR_MISSING_OUTPUT; } - // Find the highest value to start from - *diameter = fmax(fmax(x_extent, y_extent),z_extent); - - // Only for active axes, find the minimum extent - if (bounds->x != 0) { - *diameter = fmin(*diameter, x_extent); - } - if (bounds->y != 0) { - *diameter = fmin(*diameter, y_extent); - } - if (bounds->z != 0) { - *diameter = fmin(*diameter, z_extent); + for (int i=0; i < PM_VECTOR_SIZE; ++i) { + double b = bounds->ax[i]; + scale->ax[i] = b ? fabs(acc->ax[i] / b) : 0.0; } return TP_ERR_OK; } - - -int findAccelScale(PmCartesian const * const acc, - PmCartesian const * const bounds, - PmCartesian * const scale) +// KLUDGE this is a messy workaround for named fields in PmVector +static inline void minBoundRatioIfNonzero(double s, double b, double *m2) { - if (!acc || !bounds ) { - return TP_ERR_MISSING_INPUT; - } - - if (!scale ) { - return TP_ERR_MISSING_OUTPUT; + if (s > 0.0 && b > 0.0) { + //Have to square b here since the scale is also squared + *m2 = fmin(*m2, pmSq(b) / s); } +} - // Find the scale of acceleration vs. machine accel bounds - if (bounds->x != 0) { - scale->x = fabs(acc->x / bounds->x); - } else { - scale->x = 0; - } - if (bounds->y != 0) { - scale->y = fabs(acc->y / bounds->y); - } else { - scale->y = 0; +/** + * Finds the maximum allowed value of the bounded quantity on the given plane. + * + * @param plane_envelope_sq is the square of the maximal value of the ith component of a unit + * vector rotated through the plane. This basically specifies the largest + * possible contribution of each axis to a direction in the plane. We use squared values here + * because they are easier to compute, and require only one square root at the end. + * @param bounds Specifies the per-axis maximum value (e.g. acceleration, velocity) + * @param max_planar_value maximum length of a vector in the plane that is always within limits + * @return -1 on error, 0 if completed successfully + */ +int findMaxValueOnPlane(PmVector const * plane_envelope_sq, + PmVector const * bounds, + double * max_planar_value) +{ + if (!plane_envelope_sq || !bounds || !max_planar_value) { + return -1; } + double m2 = NEAR_INFINITY; - if (bounds->z != 0) { - scale->z = fabs(acc->z / bounds->z); - } else { - scale->z = 0; + for (int i = 0; i < PM_VECTOR_SIZE; ++i) { + minBoundRatioIfNonzero(plane_envelope_sq->ax[i], bounds->ax[i], &m2); } - return TP_ERR_OK; + // One square root at the end to get the actual max accel + *max_planar_value = pmSqrt(m2); + return 0; } - - - /** Find real roots of a quadratic equation in standard form. */ int quadraticFormula(double A, double B, double C, double * const root0, double * const root1) { + if (A < 0.0) { + A *=-1; + B *=-1; + C *=-1; + } double disc = pmSq(B) - 4.0 * A * C; if (disc < 0) { tp_debug_print("discriminant %.12g < 0, A=%.12g, B=%.12g,C=%.12g\n", disc, A, B, C); @@ -569,1305 +135,921 @@ int quadraticFormula(double A, double B, double C, double * const root0, * @section blending blend math functions */ -/** - * Setup common geom parameters based on trajectory segments. - * This function populates the geom structure and "input" fields of - * the blend parameter structure. It returns an error if the segments - * are not coplanar, or if one or both segments is not a circular arc. - * - * @param geom Stores simplified geometry used to calculate blend params. - * @param prev_tc first linear move to blend - * @param tc second linear move to blend - */ -int blendGeom3Init(BlendGeom3 * const geom, - TC_STRUCT const * const prev_tc, - TC_STRUCT const * const tc) +void findVMaxByAltitude( + PmVector const * const u1, + PmVector const * const u2, + BlendControls const * const controls, + BlendParameters * const param) { - geom->v_max1 = prev_tc->maxvel; - geom->v_max2 = tc->maxvel; - - // Get tangent unit vectors to each arc at the intersection point - int res_u1 = tcGetEndTangentUnitVector(prev_tc, &geom->u_tan1); - int res_u2 = tcGetStartTangentUnitVector(tc, &geom->u_tan2); - - // Initialize u1 and u2 by assuming they match the tangent direction - geom->u1 = geom->u_tan1; - geom->u2 = geom->u_tan2; - - int res_intersect = tcGetIntersectionPoint(prev_tc, tc, &geom->P); - - tp_debug_print("Intersection point P = %f %f %f\n", - geom->P.x, - geom->P.y, - geom->P.z); - - // Find angle between tangent vectors - int res_angle = findIntersectionAngle(&geom->u_tan1, - &geom->u_tan2, - &geom->theta_tan); - - // Test for intersection angle errors - if(PM_PI / 2.0 - geom->theta_tan < TP_ANGLE_EPSILON) { - tp_debug_print("Intersection angle too close to pi/2, can't compute normal\n"); - return TP_ERR_TOLERANCE; + if (fabs(param->phi) < TP_ANGLE_EPSILON) { + param->v_max_altitude = fmin(controls->v_max_geom1, controls->v_max_geom2); + return; } - - if(geom->theta_tan < TP_ANGLE_EPSILON) { - tp_debug_print("Intersection angle too small for arc fit\n"); - return TP_ERR_TOLERANCE; - } - - blendCalculateNormals3(geom); - - return res_u1 | - res_u2 | - res_intersect | - res_angle; -} - - -/** - * Initialize common fields in parameters structure. - * - * @param geom Stores simplified geometry used to calculate blend params. - * @param param Abstracted parameters for blending calculations - * @param acc_bound maximum X, Y, Z machine acceleration - * @param vel_bound maximum X, Y, Z machine velocity - * @param maxFeedScale maximum allowed feed override (set in INI) - */ -int blendParamKinematics(BlendGeom3 * const geom, - BlendParameters * const param, - TC_STRUCT const * const prev_tc, - TC_STRUCT const * const tc, - PmCartesian const * const acc_bound, - PmCartesian const * const vel_bound, - double maxFeedScale) -{ - - // KLUDGE: common operations, but not exactly kinematics - param->phi = (PM_PI - param->theta * 2.0); - - double nominal_tolerance; - tcFindBlendTolerance(prev_tc, tc, ¶m->tolerance, &nominal_tolerance); - - // Calculate max acceleration based on plane containing lines - int res_dia = calculateInscribedDiameter(&geom->binormal, acc_bound, ¶m->a_max); - - // Store max normal acceleration - param->a_n_max = param->a_max * BLEND_ACC_RATIO_NORMAL; - tp_debug_print("a_max = %f, a_n_max = %f\n", param->a_max, - param->a_n_max); - - // Find the nominal velocity for the blend segment with no overrides - double v_req_prev = tcGetMaxTargetVel(prev_tc, 1.0); - double v_req_this = tcGetMaxTargetVel(tc, 1.0); - tp_debug_print("vr_prev = %f, vr_this = %f\n", v_req_prev, v_req_this); - param->v_req = fmax(v_req_prev, v_req_this); - - // Find the worst-case velocity we should reach for either segment - param->v_goal = fmax(tcGetMaxTargetVel(prev_tc, maxFeedScale), - tcGetMaxTargetVel(tc, maxFeedScale)); - - // Calculate the maximum planar velocity - double v_planar_max = 0; - //FIXME sloppy handling of return value - res_dia |= calculateInscribedDiameter(&geom->binormal, vel_bound, &v_planar_max); - tp_debug_print("v_planar_max = %f\n", v_planar_max); - // Clip the angle at a reasonable value (less than 90 deg), to prevent div by zero double phi_effective = fmin(param->phi, PM_PI * 0.49); // Copy over maximum velocities, clipping velocity to place altitude within base - double v_max1 = fmin(prev_tc->maxvel, tc->maxvel / cos(phi_effective)); - double v_max2 = fmin(tc->maxvel, prev_tc->maxvel / cos(phi_effective)); + double Cp = cos(phi_effective); + double v_effective1 = fmin(controls->v_max_geom1, controls->v_max_geom2 / Cp); + double v_effective2 = fmin(controls->v_max_geom2, controls->v_max_geom1 / Cp); - tp_debug_print("v_max1 = %f, v_max2 = %f\n", v_max1, v_max2); + tp_debug_print("v_max1 = %f, v_max2 = %f\n", v_effective1, v_effective2); // Get "altitude" - double v_area = v_max1 * v_max2 / 2.0 * sin(param->phi); + double v_area = v_effective1 * v_effective2 / 2.0 * sin(param->phi); tp_debug_print("phi = %f\n", param->phi); tp_debug_print("v_area = %f\n", v_area); // Get "base" of triangle - PmCartesian tmp1, tmp2, diff; - pmCartScalMult(&geom->u1, v_max1, &tmp1); - pmCartScalMult(&geom->u2, v_max2, &tmp2); - pmCartCartSub(&tmp2, &tmp1, &diff); - double base; - pmCartMag(&diff, &base); + PmVector tmp1, tmp2, diff; + VecScalMult(u1, v_effective1, &tmp1); + VecScalMult(u2, v_effective2, &tmp2); + VecVecSub(&tmp2, &tmp1, &diff); + double base = VecMag(&diff); tp_debug_print("v_base = %f\n", base); - double v_max_alt = 2.0 * v_area / base; - - // Can't do altitude-based velocity calculation if we have arcs - if (prev_tc->motion_type != TC_LINEAR || tc->motion_type != TC_LINEAR) { - v_max_alt = 0.0; - } - - tp_debug_print("v_max_alt = %f\n", v_max_alt); - double v_max = fmax(v_max_alt, v_planar_max); - - tp_debug_print("v_max = %f\n", v_max); - param->v_goal = fmin(param->v_goal, v_max); - - tp_debug_print("v_goal = %f, max scale = %f\n", param->v_goal, maxFeedScale); - - return res_dia; + param->v_max_altitude = 2.0 * v_area / base; } -/** - * Setup blend parameters based on a line and an arc. - * This function populates the geom structure and "input" fields of - * the blend parameter structure. It returns an error if the segments - * are not coplanar, or if one or both segments is not a circular arc. - * - * @param geom Stores simplified geometry used to calculate blend params. - * @param param Abstracted parameters for blending calculations - * @param prev_tc first linear move to blend - * @param tc second linear move to blend - * @param acc_bound maximum X, Y, Z machine acceleration - * @param vel_bound maximum X, Y, Z machine velocity - * @param maxFeedScale maximum allowed feed override (set in INI) - */ -int blendInit3FromLineArc(BlendGeom3 * const geom, BlendParameters * const param, - TC_STRUCT const * const prev_tc, - TC_STRUCT const * const tc, - PmCartesian const * const acc_bound, - PmCartesian const * const vel_bound, - double maxFeedScale) +static inline double normalAccelFromMax(double a_max) { + return a_max * BLEND_ACC_RATIO_NORMAL; +} - if (tc->motion_type != TC_CIRCULAR || prev_tc->motion_type != TC_LINEAR) { - return TP_ERR_INPUT_TYPE; - } - - int res_init = blendGeom3Init(geom, prev_tc, tc); - if (res_init != TP_ERR_OK) { - return res_init; - } - - //Fit spiral approximation - findSpiralApproximation(&tc->coords.circle.xyz, - &geom->P, - &geom->u_tan2, - &geom->center2, - &geom->radius2); - // Handle convexity - param->convex2 = arcConvexTest(&geom->center2, &geom->P, &geom->u_tan1, true); - tp_debug_print("circ2 convex: %d\n", - param->convex2); - - //Identify max angle for first arc by blend limits - // TODO better name? - double blend_angle_2 = param->convex2 ? geom->theta_tan : PM_PI / 2.0; - - param->phi2_max = fmin(tc->coords.circle.xyz.angle / 3.0, blend_angle_2); - param->theta = geom->theta_tan; - - if (param->convex2) { - PmCartesian blend_point; - pmCirclePoint(&tc->coords.circle.xyz, - param->phi2_max / 2.0, - &blend_point); - //Create new unit vector based on secant line - // Direction is away from P (at start of segment) - pmCartCartSub(&blend_point, &geom->P, &geom->u2); - pmCartUnitEq(&geom->u2); - //Reduce theta proportionally to the angle between the secant and the normal - param->theta = fmin(param->theta, geom->theta_tan - param->phi2_max / 4.0); - } - - tp_debug_print("phi2_max = %f\n", param->phi2_max); - blendGeom3Print(geom); - - // Check that we're not below the minimum intersection angle (making too tight an arc) - // FIXME make this an INI setting? - const double theta_min = PM_PI / 6.0; - if (param->theta < theta_min) { - tp_debug_print("theta = %f < min %f, aborting arc...\n", - param->theta, - theta_min); - } - - tp_debug_print("theta = %f\n", param->theta); +int find_blend_parameters( + PmVector const * const u_tan1, + PmVector const * const u_tan2, + PmVector const * const acc_bound, + PmVector const * const vel_bound, + BlendControls const * const controls, + BlendParameters * const param) +{ + findVMaxByAltitude( + u_tan1, + u_tan2, + controls, + param); + // KLUDGE: common operations, but not exactly kinematics param->phi = (PM_PI - param->theta * 2.0); - param->L1 = fmin(prev_tc->target, prev_tc->nominal_length / 2.0); + int res_limits = find_blend_vel_accel_planar_limits( + u_tan1, + u_tan2, + acc_bound, + vel_bound, + ¶m->a_max_planar, + ¶m->v_max_planar); - if (param->convex2) { - //use half of the length of the chord - param->L2 = sin(param->phi2_max/4.0) * geom->radius2; - } else { - param->L2 = param->phi2_max * geom->radius2; - } - - tp_debug_print("L1 = %f, L2 = %f\n", param->L1, param->L2); - - // Setup common parameters - int res_kin = blendParamKinematics(geom, - param, - prev_tc, - tc, - acc_bound, - vel_bound, - maxFeedScale); + // Store max normal acceleration + param->a_n_max = normalAccelFromMax(param->a_max_planar); - return res_kin; + return res_limits; } -int blendInit3FromArcLine(BlendGeom3 * const geom, BlendParameters * const param, - TC_STRUCT const * const prev_tc, - TC_STRUCT const * const tc, - PmCartesian const * const acc_bound, - PmCartesian const * const vel_bound, - double maxFeedScale) +tp_err_t init_blend_segment_from_points( + TC_STRUCT * const blend_tc, + BlendPoints const *points, + BlendParameters * const param) { + blend_tc->motion_type = points->motion_type; - if (tc->motion_type != TC_LINEAR || prev_tc->motion_type != TC_CIRCULAR) { - return TP_ERR_INPUT_TYPE; - } - - int res_init = blendGeom3Init(geom, prev_tc, tc); - if (res_init != TP_ERR_OK) { - return res_init; - } - - findSpiralApproximation(&prev_tc->coords.circle.xyz, - &geom->P, - &geom->u_tan1, - &geom->center1, - &geom->radius1); - - param->convex1 = arcConvexTest(&geom->center1, &geom->P, &geom->u_tan2, false); - tp_debug_print("circ1 convex: %d\n", - param->convex1); - - //Identify max angle for first arc by blend limits - // TODO better name? - double blend_angle_1 = param->convex1 ? geom->theta_tan : PM_PI / 2.0; - - param->phi1_max = fmin(prev_tc->coords.circle.xyz.angle * 2.0 / 3.0, blend_angle_1); - param->theta = geom->theta_tan; - - // Build the correct unit vector for the linear approximation - if (param->convex1) { - PmCartesian blend_point; - pmCirclePoint(&prev_tc->coords.circle.xyz, - prev_tc->coords.circle.xyz.angle - param->phi1_max / 2.0 , - &blend_point); - //Create new unit vector based on secant line - // Direction is toward P (at end of segment) - pmCartCartSub(&geom->P, &blend_point, &geom->u1); - pmCartUnitEq(&geom->u1); - - //Reduce theta proportionally to the angle between the secant and the normal - param->theta = fmin(param->theta, geom->theta_tan - param->phi1_max / 4.0); + switch (points->motion_type) { + case TC_LINEAR: + pmLine9Init(&blend_tc->coords.line, &points->arc_start, &points->arc_end); + break; + case TC_SPHERICAL: + { + int res_arc = arc9InitFromPoints( + &blend_tc->coords.arc, + &points->arc_start, + &points->arc_end, + &points->arc_center, + &points->u_tan, + param->line_length); + if (res_arc != 0) { + return res_arc; + } + double v_max = pmSqrt(blend_tc->coords.arc.radius * param->a_n_max); + param->v_plan = fmin(v_max, param->v_plan); + } + break; + default: + return TP_ERR_FAIL; } + return TP_ERR_OK; +} - blendGeom3Print(geom); - tp_debug_print("phi1_max = %f\n", param->phi1_max); - - // Check that we're not below the minimum intersection angle (making too tight an arc) - // FIXME make this an INI setting? - const double theta_min = PM_PI / 6.0; - if (param->theta < theta_min) { - tp_debug_print("theta = %f < min %f, aborting arc...\n", - param->theta, - theta_min); - } +int blendParamInitVelocities( + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + double override_allowance, + BlendControls * const controls) +{ + // Copy over geometric maximum velocities + controls->v_max_geom1 = prev_tc->maxvel_geom; + controls->v_max_geom2 = tc->maxvel_geom; - tp_debug_print("theta = %f\n", param->theta); + // Find the nominal velocity for the blend segment with no overrides + double v_req_prev = tcGetPlanMaxTargetVel(prev_tc, 1.0); + double v_req_this = tcGetPlanMaxTargetVel(tc, 1.0); + controls->v_req = fmax(v_req_prev, v_req_this); - // Use end radius here - param->L1 = param->phi1_max * (geom->radius1); - param->L2 = tc->nominal_length / 2.0; + // Find the worst-case velocity we should reach for either segment + controls->v_goal = fmax(tcGetPlanMaxTargetVel(prev_tc, override_allowance), + tcGetPlanMaxTargetVel(tc, override_allowance)); - if (param->convex1) { - //use half of the length of the chord - param->L1 = sin(param->phi1_max/4.0) * geom->radius1; - } - tp_debug_print("L1 = %f, L2 = %f\n", param->L1, param->L2); - - // Setup common parameters - int res_kin = blendParamKinematics(geom, - param, - prev_tc, - tc, - acc_bound, - vel_bound, - maxFeedScale); - - return res_kin; + return 0; } - /** - * Setup blend parameters based on two circular arc segments. - * This function populates the geom structure and "input" fields of - * the blend parameter structure. It returns an error if the segments - * are not coplanar, or if one or both segments is not a circular arc. - * - * @param geom Stores simplified geometry used to calculate blend params. - * @param param Abstracted parameters for blending calculations - * @param prev_tc first linear move to blend - * @param tc second linear move to blend - * @param acc_bound maximum X, Y, Z machine acceleration - * @param vel_bound maximum X, Y, Z machine velocity - * @param maxFeedScale maximum allowed feed override (set in INI) + * Check if the previous line segment will be consumed based on the blend arc parameters. + * @pre assumes non-null arguments */ -int blendInit3FromArcArc(BlendGeom3 * const geom, BlendParameters * const param, - TC_STRUCT const * const prev_tc, - TC_STRUCT const * const tc, - PmCartesian const * const acc_bound, - PmCartesian const * const vel_bound, - double maxFeedScale) +int blendCheckConsume( + BlendParameters * const param, + double L_prev, + TC_STRUCT const * const prev_tc, + int gap_cycles) { - if (tc->motion_type != TC_CIRCULAR || prev_tc->motion_type != TC_CIRCULAR) { - return TP_ERR_FAIL; - } - - int res_init = blendGeom3Init(geom, prev_tc, tc); - if (res_init != TP_ERR_OK) { - return res_init; - } + double prev_seg_time = L_prev / param->v_plan; + double consume_time = fmax(gap_cycles, 1.0) * prev_tc->cycle_time; - findSpiralApproximation(&prev_tc->coords.circle.xyz, - &geom->P, - &geom->u_tan1, - &geom->center1, - &geom->radius1); - - findSpiralApproximation(&tc->coords.circle.xyz, - &geom->P, - &geom->u_tan2, - &geom->center2, - &geom->radius2); - - - //Do normal calculation here since we need this information for accel / vel limits - blendCalculateNormals3(geom); - - // Get intersection point from circle start - pmCirclePoint(&tc->coords.circle.xyz, 0.0, &geom->P); - tp_debug_print("Intersection point P = %f %f %f\n", - geom->P.x, - geom->P.y, - geom->P.z); - - param->convex1 = arcConvexTest(&geom->center1, &geom->P, &geom->u_tan2, false); - param->convex2 = arcConvexTest(&geom->center2, &geom->P, &geom->u_tan1, true); - tp_debug_print("circ1 convex: %d, circ2 convex: %d\n", - param->convex1, - param->convex2); - - //Identify max angle for first arc by blend limits - // TODO better name? - double blend_angle_1 = param->convex1 ? geom->theta_tan : PM_PI / 2.0; - double blend_angle_2 = param->convex2 ? geom->theta_tan : PM_PI / 2.0; - - param->phi1_max = fmin(prev_tc->coords.circle.xyz.angle * 2.0 / 3.0, blend_angle_1); - param->phi2_max = fmin(tc->coords.circle.xyz.angle / 3.0, blend_angle_2); - - param->theta = geom->theta_tan; - - // Build the correct unit vector for the linear approximation - if (param->convex1) { - PmCartesian blend_point; - pmCirclePoint(&prev_tc->coords.circle.xyz, - prev_tc->coords.circle.xyz.angle - param->phi1_max / 2.0, - &blend_point); - //Create new unit vector based on secant line - // Direction is toward P (at end of segment) - pmCartCartSub(&geom->P, &blend_point, &geom->u1); - pmCartUnitEq(&geom->u1); - - //Reduce theta proportionally to the angle between the secant and the normal - param->theta = fmin(param->theta, geom->theta_tan - param->phi1_max / 4.0); + param->is_consumable = tcCanConsume(prev_tc); + param->consume = ((prev_seg_time <= consume_time) && param->is_consumable); + param->line_length = param->consume ? L_prev : 0.0; + return 0; +} - } +/** @section spiralfuncs Functions to approximate spiral arc length */ - if (param->convex2) { - PmCartesian blend_point; - pmCirclePoint(&tc->coords.circle.xyz, - param->phi2_max / 2.0, - &blend_point); - //Create new unit vector based on secant line - // Direction is away from P (at start of segment) - pmCartCartSub(&blend_point, &geom->P, &geom->u2); - pmCartUnitEq(&geom->u2); - - //Reduce theta proportionally to the angle between the secant and the normal - param->theta = fmin(param->theta, geom->theta_tan - param->phi2_max / 4.0); - } - blendGeom3Print(geom); +double findTrapezoidalDesiredVel(double a_max, + double dx, + double v_final, + double v_current, + double cycle_time) +{ + double dt = fmax(cycle_time, TP_TIME_EPSILON); + // Discriminant is 3 terms (when final velocity is non-zero) + double discr_term1 = pmSq(v_final); + double discr_term2 = a_max * (2.0 * dx - v_current * dt); + double tmp_adt = a_max * dt * 0.5; + double discr_term3 = pmSq(tmp_adt); + double discr = discr_term1 + discr_term2 + discr_term3; + //Start with -B/2 portion of quadratic formula + double maxnewvel = -tmp_adt; - // Check that we're not below the minimum intersection angle (making too tight an arc) - // FIXME make this an INI setting? - const double theta_min = PM_PI / 12.0; - if (param->theta < theta_min) { - tp_debug_print("theta = %f < min %f, aborting arc...\n", - param->theta, - theta_min); - return TP_ERR_FAIL; + //If the discriminant term brings our velocity above zero, add it to the total + //We can ignore the calculation otherwise because negative velocities are clipped to zero + if (discr > discr_term3) { + maxnewvel += pmSqrt(discr); } - tp_debug_print("theta = %f\n", param->theta); + return maxnewvel; +} - param->phi = (PM_PI - param->theta * 2.0); +EndCondition checkEndCondition(double cycleTime, + double dtg, + double currentvel, + double v_f, + double a_max) +{ + // Start with safe defaults (segment will not end next cycle + EndCondition out = { + v_f, + TP_BIG_NUM * cycleTime, + }; - param->L1 = param->phi1_max * geom->radius1; - param->L2 = param->phi2_max * geom->radius2; + double v_avg = (currentvel + v_f) / 2.0; + + //Check that we have a non-zero "average" velocity between now and the + //finish. If not, it means that we have to accelerate from a stop, which + //will take longer than the minimum 2 timesteps that each segment takes, so + //we're safely far form the end. + + //Get dt assuming that we can magically reach the final velocity at + //the end of the move. + // + //KLUDGE: start with a value below the cutoff + double dt = TP_TIME_EPSILON / 2.0; + if (v_avg > TP_VEL_EPSILON) { + //Get dt from distance and velocity (avoid div by zero) + dt = fmax(dt, dtg / v_avg); + } else { + if ( dtg > (v_avg * cycleTime) && dtg > TP_POS_EPSILON) { + tc_pdebug_print(" below velocity threshold, assuming far from end\n"); + return out; + } + } + + // Assuming a full timestep: + double dv = v_f - currentvel; + double a_f = dv / fmax(dt, TP_TIME_EPSILON); + + //If this is a valid acceleration, then we're done. If not, then we solve + //for v_f and dt given the max acceleration allowed. + //If we exceed the maximum acceleration, then the dt estimate is too small. + double a = a_f; + int recalc = sat_inplace(&a, a_max); + + //Need to recalculate vf and above + if (recalc) { + double disc = pmSq(currentvel / a) + 2.0 / a * dtg; + if (disc < 0) { + //Should mean that dx is too big, i.e. we're not close enough + tc_pdebug_print(" dx = %f, too large, not at end yet\n",dtg); + return out; + } + + if (disc < TP_TIME_EPSILON * TP_TIME_EPSILON) { + tc_pdebug_print("disc too small, skipping sqrt\n"); + dt = -currentvel / a; + } else if (a > 0) { + tc_pdebug_print("using positive sqrt\n"); + dt = -currentvel / a + pmSqrt(disc); + } else { + tc_pdebug_print("using negative sqrt\n"); + dt = -currentvel / a - pmSqrt(disc); + } + + tc_pdebug_print(" revised dt = %f\n", dt); + //Update final velocity with actual result + out.v_f = currentvel + dt * a; + } + + out.dt = dt; + return out; +} - if (param->convex1) { - //use half of the length of the chord - param->L1 = sin(param->phi1_max/4.0) * geom->radius1; - } - if (param->convex2) { - //use half of the length of the chord - param->L2 = sin(param->phi2_max/4.0) * geom->radius2; - } - tp_debug_print("L1 = %f, L2 = %f\n", param->L1, param->L2); - tp_debug_print("phi1_max = %f\n",param->phi1_max); - tp_debug_print("phi2_max = %f\n",param->phi2_max); - - // Setup common parameters - int res_kin = blendParamKinematics(geom, - param, - prev_tc, - tc, - acc_bound, - vel_bound, - maxFeedScale); - - return res_kin; +double pseudo_sqrt(double x) +{ + const double sqrt_c = sqrt(PSEUDO_SQRT_EPSILON); + const double den = (sqrt(PSEUDO_SQRT_EPSILON + 1) - sqrt_c); + const double b0 = -sqrt_c; + const double b1 = 1; + return (b1 * pmSqrt(x + PSEUDO_SQRT_EPSILON) + b0) / den; } /** - * Setup blend parameters based on two linear segments. - * This function populates the geom structure and "input" fields of the blend parameter structure based. - * @param geom Stores simplified geometry used to calculate blend params. - * @param param Abstracted parameters for blending calculations - * @param prev_tc first linear move to blend - * @param tc second linear move to blend - * @param acc_bound maximum X, Y, Z machine acceleration - * @param vel_bound maximum X, Y, Z machine velocity - * @param maxFeedScale maximum allowed feed override (set in INI) + * Force a value to be within the range specified, with that range shrunk by the relative margin. + * Example: If rel_margin is 0.1, upper = 2, lower = 1, value must be within [1.05,1.95] + * @pre result is undefined if rel_margin exceeds [0,1], or lower > upper */ -int blendInit3FromLineLine(BlendGeom3 * const geom, BlendParameters * const param, - TC_STRUCT const * const prev_tc, - TC_STRUCT const * const tc, - PmCartesian const * const acc_bound, - PmCartesian const * const vel_bound, - double maxFeedScale) +double clamp(double upper, double lower, double rel_margin, double value) { + double m = (upper - lower) * rel_margin / 2.0; + double u = upper - m; + double l = lower + m; + return fmin(fmax(value, l), u); +} - if (tc->motion_type != TC_LINEAR || prev_tc->motion_type != TC_LINEAR) { - return TP_ERR_FAIL; - } - - int res_init = blendGeom3Init(geom, prev_tc, tc); - if (res_init != TP_ERR_OK) { - return res_init; - } - - param->theta = geom->theta_tan; +tp_err_t find_blend_points_and_tangents( + double Rb, + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + blend_boundary_t * const out) +{ + out->s1 = prev_tc->target - Rb; + out->s2 = Rb; - tp_debug_print("theta = %f\n", param->theta); + tcGetPosReal(prev_tc, out->s1, &out->P1); + tcGetPosReal(tc, out->s2, &out->P2); - param->phi = (PM_PI - param->theta * 2.0); + tcGetTangentUnitVector(prev_tc, out->s1, &out->u1); + tcGetTangentUnitVector(tc, out->s2, &out->u2); - blendGeom3Print(geom); - - //Nominal length restriction prevents gobbling too much of parabolic blends - param->L1 = fmin(prev_tc->target, prev_tc->nominal_length * BLEND_DIST_FRACTION); - param->L2 = tc->target * BLEND_DIST_FRACTION; - tp_debug_print("prev. nominal length = %f, next nominal_length = %f\n", - prev_tc->nominal_length, tc->nominal_length); - tp_debug_print("L1 = %f, L2 = %f\n", param->L1, param->L2); - - // Setup common parameters - int res_kin = blendParamKinematics(geom, - param, - prev_tc, - tc, - acc_bound, - vel_bound, - maxFeedScale); - - return res_kin; + return TP_ERR_OK; } - -/** - * Calculate plane normal and binormal based on unit direction vectors. - */ -int blendCalculateNormals3(BlendGeom3 * const geom) +int find_blend_intermediate_segments( + blend_boundary_t const * const blend_params, + biarc_control_points_t * const control_pts) { + // A coefficient + PmVector du12; + VecVecAdd(&blend_params->u1, &blend_params->u2, &du12); - int err_cross = pmCartCartCross(&geom->u_tan1, - &geom->u_tan2, - &geom->binormal); - int err_unit_b = pmCartUnitEq(&geom->binormal); + PmVector dP12; + VecVecSub(&blend_params->P1, &blend_params->P2, &dP12); - tp_debug_print("binormal = [%f %f %f]\n", geom->binormal.x, - geom->binormal.y, - geom->binormal.z); + // Find B / C first since A could be zero if u1 / u2 are colinear + double B = VecVecDot(&dP12, &du12); + B *= 2.; - pmCartCartSub(&geom->u_tan2, &geom->u_tan1, &geom->normal); - int err_unit_n = pmCartUnitEq(&geom->normal); + double C =VecMagSq(&dP12); - tp_debug_print("normal = [%f %f %f]\n", geom->normal.x, - geom->normal.y, - geom->normal.z); - return (err_cross || err_unit_b || err_unit_n); -} + double A = VecMagSq(&du12); + A -= 4; -/** - * Compute blend parameters based on line data. - * Blend arc parameters such as radius and velocity are calculated here. These - * parameters are later used to create the actual arc geometry in other - * functions. - */ -int blendComputeParameters(BlendParameters * const param) -{ - - // Find maximum distance h from arc center to intersection point - double h_tol = param->tolerance / (1.0 - sin(param->theta)); - - // Find maximum distance along lines allowed by tolerance - double d_tol = cos(param->theta) * h_tol; - tp_debug_print(" d_tol = %f\n", d_tol); - - // Find minimum distance by blend length constraints - double d_lengths = fmin(param->L1, param->L2); - double d_geom = fmin(d_lengths, d_tol); - // Find radius from the limiting length - double R_geom = tan(param->theta) * d_geom; - - // Find maximum velocity allowed by accel and radius - double v_normal = pmSqrt(param->a_n_max * R_geom); - tp_debug_print("v_normal = %f\n", v_normal); - - param->v_plan = fmin(v_normal, param->v_goal); - - /*Get the limiting velocity of the equivalent parabolic blend. We use the - * time it would take to do a "stock" parabolic blend as a metric for how - * much of the segment to consume. A long segment will have a high - * "triangle" velocity, so the radius will only be as large as is needed to - * reach the cornering speed. A short segment will have a low triangle - * velocity, much lower than the actual curvature limit, which can be used - * to calculate an equivalent blend radius. - * */ - double a_parabolic = param->a_max * 0.5; - double v_triangle = pmSqrt(2.0 * a_parabolic * d_geom); - double t_blend = fmin(v_triangle, param->v_plan) / (a_parabolic); - double s_blend = t_blend * param->v_plan; - double R_blend = fmin(s_blend / param->phi, R_geom); //Clamp by limiting radius - - param->R_plan = fmax(pmSq(param->v_plan) / param->a_n_max, R_blend); - param->d_plan = param->R_plan / tan(param->theta); - - tp_debug_print("v_plan = %f\n", param->v_plan); - tp_debug_print("R_plan = %f\n", param->R_plan); - tp_debug_print("d_plan = %f\n", param->d_plan); - - /* "Actual" velocity means the velocity when feed override is 1.0. Recall - * that v_plan may be greater than v_req by the max feed override. If our - * worst-case planned velocity is higher than the requested velocity, then - * clip at the requested velocity. This allows us to increase speed above - * the feed override limits. - */ - if (param->v_plan > param->v_req) { - param->v_actual = param->v_req; + // true iff u1 / u2 are nearly parallel or anti-parallel + if (fabs(A) < 1e-12) + { + if (fabs(B) < TP_POS_EPSILON) { + // u vectors are perpendicular to displacement vector between P1/P2 + double dot = VecVecDot(&blend_params->u1, &blend_params->u2); + if (dot > 0) { + // u1 and -u2 are in opposite directions, no biarc solution exists + return -2; + } else { + // u1 and -u2 are in the same direction, assume "rectangular" solution + control_pts->d = pmSqrt(C)/2.0; + } + } else { + control_pts->d = -C/B; + } } else { - param->v_actual = param->v_plan; + double other; + int res_qf = quadraticFormula(A, B, C, &control_pts->d, &other); + if (res_qf || control_pts->d < 0) { + return -1; + } } - // Store arc length of blend arc for future checks - param->s_arc = param->R_plan * param->phi; + VecScalMult(&blend_params->u1, control_pts->d, &control_pts->Pb1); + VecVecAddEq(&control_pts->Pb1, &blend_params->P1); - if (param->R_plan < TP_POS_EPSILON) { - tp_debug_print("#Blend radius too small, aborting arc\n"); - return TP_ERR_FAIL; - } + VecScalMult(&blend_params->u2, -control_pts->d, &control_pts->Pb2); + VecVecAddEq(&control_pts->Pb2, &blend_params->P2); - if (param->s_arc < TP_MIN_ARC_LENGTH) { - tp_debug_print("#Blend arc length too small, aborting arc\n"); - return TP_ERR_FAIL; - } - return TP_ERR_OK; -} - - -/** Check if the previous line segment will be consumed based on the blend arc parameters. */ -int blendCheckConsume(BlendParameters * const param, - BlendPoints3 const * const points, - TC_STRUCT const * const prev_tc, int gap_cycles) -{ - //Initialize values - param->consume = 0; - param->line_length = 0; - if (!prev_tc) { - return -1; - } + VecVecDirection(&control_pts->Pb2, &control_pts->Pb1, &control_pts->u_mid); + VecScalMult(&control_pts->u_mid, control_pts->d, &control_pts->P_mid); + VecVecAddEq(&control_pts->P_mid, &control_pts->Pb1); - if (prev_tc->motion_type != TC_LINEAR) { + if (control_pts->d > 0) { return 0; + } else { + return -3; } +} - //Check for segment length limits - double L_prev = prev_tc->target - points->trim1; - double prev_seg_time = L_prev / param->v_plan; - - bool can_consume = tcCanConsume(prev_tc); - param->consume = (prev_seg_time < gap_cycles * prev_tc->cycle_time && can_consume); - if (param->consume) { - tp_debug_print("consuming prev line, L_prev = %g\n", - L_prev); - param->line_length = L_prev; +static inline double find_max_blend_length(TC_STRUCT const * const tc, double v_goal, bool allow_consumption) +{ + double min_remainder = allow_consumption ? 0.0 : v_goal * tc->cycle_time / 2.0; + double usable_length = fmax(tc->nominal_length / 2.0 - min_remainder, tc->nominal_length / 3.0); + + if (tc->motion_type == TC_CIRCULAR) { + // FIXME use arc length fit for spiral? + double phi_max = PM_PI / 3.0; + double total_arc_length = pmSqrt(pmSq(tc->coords.circle.xyz.height) + pmSq(tc->coords.circle.fit.total_planar_length)); + double l_from_angle = phi_max / tc->coords.circle.xyz.angle * total_arc_length; + double l_from_length = fmin(tc->target, usable_length); + return fmin(l_from_angle, l_from_length); + } else { + return fmin(tc->target, usable_length); } - return 0; } - -/** - * Compute spherical arc points based on blend arc data. - * Once blend parameters are computed, the three arc points are calculated - * here. - */ -int blendFindPoints3(BlendPoints3 * const points, BlendGeom3 const * const geom, - BlendParameters const * const param) +double find_max_blend_region(TC_STRUCT const * const prev_tc, TC_STRUCT const * const tc, double v_goal) { - // Find center of blend arc along normal vector - double center_dist = param->R_plan / sin(param->theta); - tp_debug_print("center_dist = %f\n", center_dist); - - pmCartScalMult(&geom->normal, center_dist, &points->arc_center); - pmCartCartAddEq(&points->arc_center, &geom->P); - tp_debug_print("arc_center = %f %f %f\n", - points->arc_center.x, - points->arc_center.y, - points->arc_center.z); - - // Start point is d_plan away from intersection P in the - // negative direction of u1 - pmCartScalMult(&geom->u1, -param->d_plan, &points->arc_start); - pmCartCartAddEq(&points->arc_start, &geom->P); - tp_debug_print("arc_start = %f %f %f\n", - points->arc_start.x, - points->arc_start.y, - points->arc_start.z); - - // End point is d_plan away from intersection P in the - // positive direction of u1 - pmCartScalMult(&geom->u2, param->d_plan, &points->arc_end); - pmCartCartAddEq(&points->arc_end, &geom->P); - tp_debug_print("arc_end = %f %f %f\n", - points->arc_end.x, - points->arc_end.y, - points->arc_end.z); - - //For line case, just copy over d_plan since it's the same - points->trim1 = param->d_plan; - points->trim2 = param->d_plan; - - return TP_ERR_OK; + double l_prev = find_max_blend_length(prev_tc, v_goal, tcCanConsume(prev_tc)); + double l_this = find_max_blend_length(tc, v_goal, true); + return fmin(l_prev, l_this); } - -/** - * Take results of line blend calculation and project onto circular arc and line - */ -int blendLineArcPostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in, - BlendParameters * const param, BlendGeom3 const * const geom, - PmCartLine const * const line1, PmCircle const * const circ2) +tp_err_t find_blend_size_from_intermediates( + blend_boundary_t const * const blend_boundary, + biarc_control_points_t const * const intermediates, + double *R_geom, + double *arc_len_est) { - (void)points_in; - (void)line1; - (void)circ2; - - // Define distances from actual center to circle centers - double d2 = negate(param->R_plan, param->convex2) + geom->radius2; - tp_debug_print("d2 = %f\n", d2); - - //Get unit vector normal to line in plane, towards arc center - PmCartesian n1; - pmCartCartCross(&geom->binormal, &geom->u1, &n1); - pmCartUnitEq(&n1); - - tp_debug_print("n1 = %f %f %f\n", - n1.x, - n1.y, - n1.z); - - PmCartesian r_PC2; - pmCartCartSub(&geom->center2, &geom->P, &r_PC2); - - double c2_u,c2_n; //Components of C2-P on u1 and n1 - pmCartCartDot(&r_PC2, &geom->u1, &c2_u); - pmCartCartDot(&r_PC2, &n1, &c2_n); - - tp_debug_print("c2_u = %f, c2_n = %f\n", - c2_u, - c2_n); - - double d_L; // badly named distance along line to intersection - double A = 1; - double B = 2.0 * c2_u; - double C = pmSq(c2_u) - pmSq(d2) + pmSq(param->R_plan - c2_n); - double root0,root1; - int res_dist = quadraticFormula(A, B, C, &root0, &root1); - if (res_dist) { - return TP_ERR_FAIL; - } - - tp_debug_print("root0 = %f, root1 = %f\n", root0, - root1); - d_L = fmin(fabs(root0),fabs(root1)); - if (d_L < 0) { - tp_debug_print("d_L can't be < 0, aborting...\n"); - return TP_ERR_FAIL; - } + double dot = VecVecDot(&blend_boundary->u1, &intermediates->u_mid); - PmCartesian C_u, C_n; - - pmCartScalMult(&geom->u1, -d_L, &C_u); - pmCartScalMult(&n1, param->R_plan, &C_n); - - PmCartesian r_PC; - //Continue with correct solution, get actual center - pmCartCartAdd(&C_u, &C_n, &r_PC); - pmCartCartAdd(&geom->P, &r_PC, &points->arc_center); - tp_debug_print("arc center = %f %f %f\n", - points->arc_center.x, - points->arc_center.y, - points->arc_center.z); - - //Verify tolerances - double h; - pmCartMag(&r_PC, &h); - tp_debug_print("center_dist = %f\n", h); - - double T_final = h - param->R_plan; - tp_debug_print("T_final = %f\n",T_final); - if (T_final > param->tolerance) { - tp_debug_print("Projected circle T (%f) exceeds tolerance %f, aborting blend arc\n", - T_final, - param->tolerance); - return TP_ERR_FAIL; - } + double cos_2theta = fmax(-dot, -1.0 + 1.0e-15); - points->trim1 = d_L; + double sin_theta_approx = pmSqrt((1. - cos_2theta) / 2); + double cos_theta_approx = pmSqrt((1. + cos_2theta) / 2); + double tan_theta_approx = sin_theta_approx / fmax(cos_theta_approx, TP_ANGLE_EPSILON); - points->trim2 = findTrimAngle(&geom->P, - &points->arc_center, - &geom->center2); + // TODO optimize out the tangent calculation if necessary + // Find radius from the limiting length (assume length is actually symmetrical + *R_geom = tan_theta_approx * intermediates->d; + *arc_len_est = 2.0 * intermediates->d * sin_theta_approx; return TP_ERR_OK; } +static const biarc_solution_t zero_region_result = {0}; -/** - * Take results of line blend calculation and project onto circular arc and line - */ -int blendArcLinePostProcess(BlendPoints3 * const points, - BlendPoints3 const * const points_in, - BlendParameters * const param, - BlendGeom3 const * const geom, - PmCircle const * const circ1, - PmCartLine const * const line2) +static inline double get_effective_range(blend_solver_constraints_t const * const constraints) { - (void)points_in; - (void)circ1; - (void)line2; - - // Define distance from actual arc center to circle center - double d1 = negate(param->R_plan, param->convex1) + geom->radius1; - tp_debug_print("d1 = %f\n", d1); - - //Get unit vector normal to line in plane, towards arc center - PmCartesian n2; - pmCartCartCross(&geom->binormal, &geom->u2, &n2); - pmCartUnitEq(&n2); - - tp_debug_print("n2 = %f %f %f\n", - n2.x, - n2.y, - n2.z); - - PmCartesian r_PC1; - pmCartCartSub(&geom->center1, &geom->P, &r_PC1); - double c1_u, c1_n; //Components of C1-P on u2 and n2 - pmCartCartDot(&r_PC1, &geom->u2, &c1_u); - pmCartCartDot(&r_PC1, &n2, &c1_n); - - double d_L; // badly named distance along line to intersection - double A = 1; - double B = 2.0 * c1_u; - double C = pmSq(c1_u) - pmSq(d1) + pmSq(param->R_plan - c1_n); - double root0,root1; - int res_dist = quadraticFormula(A, B, C, &root0, &root1); - if (res_dist) { - return TP_ERR_FAIL; - } + return constraints->upper.Rb - constraints->lower.Rb; +} - tp_debug_print("root0 = %f, root1 = %f\n", root0, - root1); - d_L = fmin(fabs(root0),fabs(root1)); - if (d_L < 0) { - tp_debug_print("d_L can't be < 0, aborting...\n"); - return TP_ERR_FAIL; - } +static inline double guess_interpolated_region_size( + blend_solver_constraints_t const * const constraints, + double R_goal, + double T_goal) +{ + double dRb = constraints->upper.Rb - constraints->lower.Rb; + double dR_plan = constraints->upper.R_plan - constraints->lower.R_plan; + double dR_goal = R_goal - constraints->lower.R_plan; + double Rb_interp_R = constraints->lower.Rb + dRb * (dR_goal / fmax(dR_plan, TP_POS_EPSILON)); - PmCartesian C_u, C_n; - - pmCartScalMult(&geom->u2, d_L, &C_u); - pmCartScalMult(&n2, param->R_plan, &C_n); - - PmCartesian r_PC; - //Continue with correct solution, get actual center - pmCartCartAdd(&C_u, &C_n, &r_PC); - pmCartCartAdd(&geom->P, &r_PC, &points->arc_center); - tp_debug_print("arc center = %f %f %f\n", - points->arc_center.x, - points->arc_center.y, - points->arc_center.z); - - //Verify tolerances - double h; - pmCartMag(&r_PC, &h); - tp_debug_print("center_dist = %f\n", h); - - double T_final = h - param->R_plan; - if (T_final > param->tolerance) { - tp_debug_print("Projected circle T (%f) exceeds tolerance %f, aborting blend arc\n", - T_final, - param->tolerance); - return TP_ERR_FAIL; + double dT_plan = constraints->upper.T_plan - constraints->lower.T_plan; + double dT_goal = T_goal - constraints->lower.T_plan; + double Rb_interp_T = constraints->lower.Rb + dRb * (dT_goal / fmax(dT_plan, TP_POS_EPSILON)); + + if (abs(constraints->bias_count) > 3 ) { + return (constraints->upper.Rb + constraints->lower.Rb) / 2.; + } else { + return clamp(constraints->upper.Rb, constraints->lower.Rb, 0.1, fmin(Rb_interp_R, Rb_interp_T)); } - tp_debug_print("T_final = %f\n",T_final); +} - points->trim1 = findTrimAngle(&geom->P, - &points->arc_center, - &geom->center1); - points->trim2 = d_L; +static inline double guess_blend_region_size_slope( + blend_solver_constraints_t * const constraints, + biarc_solution_t const * const solution, + double R_goal, + double T_goal) +{ + if (solution->R_plan < R_goal && solution->T_plan < T_goal) { + constraints->lower = *solution; + constraints->bias_count = constraints->bias_count > 0 ? -1 : constraints->bias_count - 1; + } else { + constraints->bias_count = constraints->bias_count < 0 ? 1 : constraints->bias_count + 1; + constraints->upper = *solution; + } - return TP_ERR_OK; + return guess_interpolated_region_size(constraints, R_goal, T_goal); } - /** - * "Post-process" results from linear approximation to fit the circular segments. - * This step handles the projection from the linear approximation of each - * circle. Given the solved radius and tolerance, this function updates the - * points structure with the exact trim angles for each segment. + * Given an overall tolerance limit and the tangent vectors at the + * intersection, find an approximate blend region size that puts the midpoint + * of the intermediate line within tolerance of the intersection point. + * + * @note this assumes that the areas to blend are straight-ish (i.e. the error + * introduced by circular segments is minimal). This assumes a linear + * approximation about the intersection point, but better approximations are + * possible. */ -int blendArcArcPostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in, - BlendParameters * const param, BlendGeom3 const * const geom, - PmCircle const * const circ1, PmCircle const * const circ2) +double find_blend_region_from_tolerance( + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + double tolerance) { - (void)points_in; - (void)circ1; - (void)circ2; - - // Create "shifted center" approximation of spiral circles - // TODO refers to u1 instead of utan? - // Define distances from actual center to adjusted circle centers - double d1 = negate(param->R_plan, param->convex1) + geom->radius1; - double d2 = negate(param->R_plan, param->convex2) + geom->radius2; - tp_debug_print("d1 = %f, d2 = %f\n", d1, d2); - - //Find "x" distance between C1 and C2 - PmCartesian r_C1C2; - pmCartCartSub(&geom->center2, &geom->center1, &r_C1C2); - double c2x; - pmCartMag(&r_C1C2, &c2x); - - // Compute the new center location - - double Cx = (-pmSq(d1) + pmSq(d2)-pmSq(c2x)) / (-2.0 * c2x); - double Cy = pmSqrt(pmSq(d1) - pmSq(Cx)); - - tp_debug_print("Cx = %f, Cy = %f\n",Cx,Cy); - - // Find the basis vector uc from center1 to center2 - PmCartesian uc; - //TODO catch failures here - int norm_err = pmCartUnit(&r_C1C2, &uc); - if (norm_err) { - return TP_ERR_FAIL; - } - - // Find the basis vector perpendicular to the binormal and uc - PmCartesian nc; - pmCartCartCross(&geom->binormal, &uc, &nc); + PmVector u_tan1, u_tan2; + // Start by assuming that the blend tangents will be at least 1 tolereance + // distance away (should always be true because part of the blend will move + // along the tangential direction until it hits the intermediate segment). + double s1 = fmax(0, prev_tc->target - tolerance); + double s2 = fmin(tc->target / 2.0, tolerance); + tcGetTangentUnitVector(prev_tc, s1, &u_tan1); + tcGetTangentUnitVector(tc, s2, &u_tan2); + + double eta1 = 0, eta2 = 0; + if (prev_tc->motion_type == TC_CIRCULAR) { + eta1 = (prev_tc->target - s1) / prev_tc->coords.circle.xyz.radius; + } + if (tc->motion_type == TC_CIRCULAR) { + eta2 = s2 / tc->coords.circle.xyz.radius; + } + + double theta = findIntersectionAngle(&u_tan1, &u_tan2); + + // TODO optimize + double eta = (eta1 + eta2) / 2.0; + const double theta2_guess = theta + eta; + double F = 0, theta2 = 0; + if (theta2_guess < M_PI_2) { + // "Normal" case where the effective tangent vectors point towards the intersection + theta2 = theta2_guess; + F = (sin(eta) + cos(theta))/(sin(theta2) + 1); + } else { + // Flipped case, where tangent vectors point away from the intersection + // If theta2 > pi/2, then the tolerance region "flips" so that the triangles are opposite + // Redefine theta2 to have the same sense as theta in the flipped triangle + theta2 = M_PI - theta2_guess; - //Check if nc is in the same half-plane as the intersection normal. if not, - //we need to flip it around to choose the correct solution. - double dot1; - pmCartCartDot(&geom->normal, &nc, &dot1); - if (dot1 < 0) { - pmCartNegEq(&nc); - } - norm_err = pmCartUnitEq(&nc); - if (norm_err) { - return TP_ERR_FAIL; + F = sin(theta) / (1 + sin(theta2)) * cos(theta2) + cos(theta); } + double Rb = tolerance / F; + double Rb_max = sin(theta)/sin(theta2) * Rb; - //Find components of center position wrt circle 1 center. - PmCartesian c_x, c_y; - pmCartScalMult(&uc, Cx, &c_x); - pmCartScalMult(&nc, Cy, &c_y); - - //Get vector from P to first center - PmCartesian r_PC1; - pmCartCartSub(&geom->center1, &geom->P, &r_PC1); + return Rb_max; +} - // Get "test vectors, relative distance from solution center to P - PmCartesian test1, test2; - pmCartCartAdd(&r_PC1, &c_x, &test1); - test2=test1; +double find_blend_region_from_tolerance_simple( + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + double tolerance) +{ + PmVector u_tan1, u_tan2; + // Start by assuming that the blend tangents will be at least 1 tolereance + // distance away (should always be true because part of the blend will move + // along the tangential direction until it hits the intermediate segment). + tcGetEndTangentUnitVector(prev_tc, &u_tan1); + tcGetStartTangentUnitVector(tc, &u_tan2); - //Add and subtract c_y component to get equivalent of two Y solutions - pmCartCartAddEq(&test1, &c_y); - pmCartCartSubEq(&test2, &c_y); + double dot = VecVecDot(&u_tan1, &u_tan2); + double cos_2theta = -dot; - double mag1,mag2; - pmCartMag(&test1, &mag1); - pmCartMag(&test2, &mag2); + // avoids acos, sin, cos, but not sure if this is actually faster + double sin_theta = pmSqrt((1. - cos_2theta) / 2); + double cos_theta = pmSqrt((1. + cos_2theta) / 2); - if (mag2 < mag1) - { - //negative solution is closer - pmCartNegEq(&c_y); - } + double d_est = sin_theta / cos_theta * tolerance; - //Continue with correct solution, get actual center - PmCartesian r_C1C; - pmCartCartAdd(&c_x, &c_y, &r_C1C); - pmCartCartAdd(&geom->center1, &r_C1C, &points->arc_center); - tp_debug_print("arc center = %f %f %f\n", - points->arc_center.x, - points->arc_center.y, - points->arc_center.z); - - //Find components of center position wrt circle 2 center. - PmCartesian r_C2C; - pmCartCartSub(&points->arc_center, &geom->center2, &r_C2C); - - PmCartesian r_PC; - pmCartCartSub(&points->arc_center, &geom->P, &r_PC); - - //Verify tolerances - double h; - pmCartMag(&r_PC, &h); - tp_debug_print("center_dist = %f\n", h); - - double T_final = h - param->R_plan; - if (T_final > param->tolerance) { - tp_debug_print("Projected circle T (%f) exceeds tolerance %f, aborting blend arc\n", - T_final, - param->tolerance); - return TP_ERR_FAIL; - } - tp_debug_print("T_final = %f\n",T_final); + return tolerance / cos_theta + d_est; +} - points->trim1 = findTrimAngle(&geom->P, - &points->arc_center, - &geom->center1); - points->trim2 = findTrimAngle(&geom->P, - &points->arc_center, - &geom->center2); +int find_blend_vel_accel_planar_limits( + PmVector const * const u_tan1, + PmVector const * const u_tan2, + PmVector const * const acc_bound, + PmVector const * const vel_bound, + double *a_max_planar, + double *v_max_planar) +{ + // Find an orthonormal basis and "envelope" vector for the plane that represents + // the worst-case contribution of each axis to the overall velocity / acceleration + PmVector u, v, uv_plane_envelope_sq; + VecVecOrthonormal(u_tan1, u_tan2, &u, &v); + VecVecHarmonicAddSq(&u, &v, &uv_plane_envelope_sq); - tp_debug_print("trim1 = %f, trim2 = %f\n", - points->trim1, - points->trim2); + // Reuse the envelope vector (this is why accel and vel are calculated together) + int res_dia = findMaxValueOnPlane(&uv_plane_envelope_sq, acc_bound, a_max_planar); + res_dia |= findMaxValueOnPlane(&uv_plane_envelope_sq, vel_bound, v_max_planar); - return TP_ERR_OK; + return res_dia; } +void find_biarc_solution_error( + double blend_tolerance, + double R_goal, + double R_plan, + double T_plan, + biarc_solution_errors_t * const err) +{ + err->deviation = blend_tolerance - T_plan; + + // Check for solution convergence based on specified tolerances + err->radius_rel = (R_plan - R_goal + TP_POS_EPSILON) / (R_goal + TP_POS_EPSILON); + err->radius_abs = R_plan - R_goal; +} /** - * Setup the spherical arc struct based on the blend arc data. + * Find the optimal Rb to get the smallest blend that can meet the feed rate requirements. + * Choose initial Rb = Rb_max + * Find blend start / end points and tangent vectors, within Rb of intersection P with procedure find_blend_points_and_tangents + * Find blend segment lengths d, L and intermediate points with procedure find_blend_intermediate_segments + * Calculate blend kinematics for the two blends. + * Find blend radii and arc lengths given P1, P2, Pb1, Pb2 + * Compute max velocity on blend arcs from geometry + * Check blend performance: + * Verify distance from blend midpoint to original intersection P < tolerance + * Check if requested velocity is reached (if not, need bigger arc) + * Check if blend radii are larger than they need to be (e.g. worst-case normal acceleration < a_t_max) + * Refine solution as necessary: + * Guess a new Rb based on heuristic function of Rb, Rb_max, v_plan with procedure guess_blend_region_size + * Stop after N iterations or until Rb converges */ -int arcFromBlendPoints3(SphericalArc * const arc, BlendPoints3 const * const points, - BlendGeom3 const * const geom, BlendParameters const * const param) +tp_err_t optimize_biarc_blend_size( + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + biarc_solver_config_t const * const config, + PmVector const * const vel_bound, + PmVector const * const acc_bound, + biarc_solver_results_t * const biarc_results, + BlendControls * const controls, + BlendParameters * const param, + double cycle_time) { - // If we consume the previous line, the remaining line length gets added here - arc->uTan = geom->u_tan1; - arc->line_length = param->line_length; - arc->binormal = geom->binormal; - - // Create the arc from the processed points - return arcInitFromPoints(arc, &points->arc_start, - &points->arc_end, &points->arc_center); -} + if (!prev_tc || !tc || !biarc_results) { + return TP_ERR_MISSING_INPUT; + } + blendParamInitVelocities(prev_tc, tc, config->feed_override_allowance, controls); -int blendGeom3Print(BlendGeom3 const * const geom) -{ - (void)geom; - tp_debug_print("u1 = %f %f %f\n", - geom->u1.x, - geom->u1.y, - geom->u1.z); - - tp_debug_print("u2 = %f %f %f\n", - geom->u2.x, - geom->u2.y, - geom->u2.z); - return 0; -} + // Find net tolerance that's actually usable (so that we're + // not passing 0.0 around if tolerance is not specified) + tcFindBlendTolerance(prev_tc, tc, &controls->tolerance); -int blendPoints3Print(BlendPoints3 const * const points) -{ - (void)points; - tp_debug_print("arc_start = %f %f %f\n", - points->arc_start.x, - points->arc_start.y, - points->arc_start.z); - - tp_debug_print("arc_center = %f %f %f\n", - points->arc_center.x, - points->arc_center.y, - points->arc_center.z); - - tp_debug_print("arc_end = %f %f %f\n", - points->arc_end.x, - points->arc_end.y, - points->arc_end.z); + //double Rb_init = find_blend_region_from_tolerance(prev_tc, tc, param->tolerance); + double Rb_init = find_max_blend_region(prev_tc, tc, controls->v_goal); - return 0; + // Initial solver values for Rb range using somewhat dumb (but cheap) values + biarc_results->solution.Rb = biarc_results->constraints.upper.Rb = Rb_init; + biarc_results->constraints.upper.R_plan = 0; // To be filled in at first pass through solver + biarc_results->constraints.lower = zero_region_result; -} + const unsigned MAX_ITERATIONS = config->max_iterations; -double pmCartAbsMax(PmCartesian const * const v) -{ - return fmax(fmax(fabs(v->x),fabs(v->y)),fabs(v->z)); -} + double s1_max = prev_tc->target - biarc_results->constraints.upper.Rb; + double s2_max = biarc_results->constraints.upper.Rb; + PmVector u1, u2; + tcGetTangentUnitVector(prev_tc, s1_max, &u1); + tcGetTangentUnitVector(tc, s2_max, &u2); + // Approximate the blend kinematics using the initial guess + // Actual limits will be enforced later + CHP(find_blend_parameters( + &u1, + &u2, + acc_bound, + vel_bound, + controls, + param + )); -PmCircleLimits pmCircleActualMaxVel(PmCircle const * circle, - double v_max, - double a_max) -{ - double a_n_max_cutoff = BLEND_ACC_RATIO_NORMAL * a_max; + double v_max = fmax(param->v_max_planar, param->v_max_altitude); + param->v_plan = fmin(controls->v_goal, v_max); - double eff_radius = pmCircleEffectiveMinRadius(circle); - // Find the acceleration necessary to reach the maximum velocity - double a_n_vmax = pmSq(v_max) / fmax(eff_radius, DOUBLE_FUZZ); - // Find the maximum velocity that still obeys our desired tangential / total acceleration ratio - double v_max_cutoff = pmSqrt(a_n_max_cutoff * eff_radius); + double R_goal = pmSq(controls->v_goal) / param->a_n_max; + // Scale up solver tolerance to have the same net effect on velocity + const double solver_radius_rel_tol = config->velocity_rel_tolerance * R_goal / param->v_plan; + const double solver_radius_abs_tol = pmSq(config->velocity_abs_tolerance) / param->a_n_max; // 2% is pretty good - double v_max_actual = v_max; - double acc_ratio_tan = BLEND_ACC_RATIO_TANGENTIAL; + // Intersection is at start of next segment, so use startpoint method since it's much faster + PmVector P; + CHP(tcGetStartpoint(tc, &P)); - if (a_n_vmax > a_n_max_cutoff) { - v_max_actual = v_max_cutoff; + for (biarc_results->iterations = 1; biarc_results->iterations < MAX_ITERATIONS; ++biarc_results->iterations) + { + // Find blend start / end points and tangent vectors, within Rb of intersection P with procedure find_blend_points_and_tangents + find_blend_points_and_tangents(biarc_results->solution.Rb, prev_tc, tc, &biarc_results->solution.boundary); + // Find blend segment lengths d, L and intermediate points with procedure find_blend_intermediate_segments + if (TP_ERR_OK != find_blend_intermediate_segments(&biarc_results->solution.boundary, &biarc_results->solution.control_pts)) + { + biarc_results->result = BIARC_DEGENERATE_SEGMENTS; + return TP_ERR_FAIL; + } + + // Verify distance from blend midpoint to original intersection P < tolerance + biarc_results->solution.T_plan = VecVecDisp(&biarc_results->solution.control_pts.P_mid, &P); + + CHP(find_blend_size_from_intermediates( + &biarc_results->solution.boundary, + &biarc_results->solution.control_pts, + &biarc_results->solution.R_geom, + &biarc_results->solution.arc_len_est + )); + + // Cheat by expressing max radius in terms of v_max even though it's not strictly the limiting factor + double guess_v_max_length = biarc_results->solution.arc_len_est / cycle_time; + double guess_R_effective_max = pmSq(guess_v_max_length) / param->a_n_max; + + // THis clamping throws off the slope-based update. Need to do slopes based on R_geom, or somehow compensate for it + biarc_results->solution.R_plan = fmin(biarc_results->solution.R_geom, guess_R_effective_max); + // Check for solution convergence based on specified tolerances + find_biarc_solution_error( + controls->tolerance, + R_goal, + biarc_results->solution.R_plan, + biarc_results->solution.T_plan, + &biarc_results->solution.err); + + // Need an ok tolerance or we're not done + if (biarc_results->solution.err.deviation >= -config->deviation_abs_tolerance) { + // Check if we're close to the goal "radius", i.e. we've met the blend target velocity limit + if ((fabs(biarc_results->solution.err.radius_rel) < solver_radius_rel_tol + || fabs(biarc_results->solution.err.radius_abs) < solver_radius_abs_tol)) + { + biarc_results->result = BIARC_REACHED_V_GOAL; + return TP_ERR_OK; + } else if (biarc_results->solution.err.radius_rel < 0.0 && biarc_results->solution.err.deviation < config->deviation_abs_tolerance) { + biarc_results->result = BIARC_REACHED_TOLERANCE; + return TP_ERR_OK; + } else if (get_effective_range(&biarc_results->constraints) < config->convergence_tolerance) { + biarc_results->result = BIARC_CONVERGED; + return TP_ERR_OK; + } + } else { + double R_bound = fmax(biarc_results->constraints.upper.R_plan, biarc_results->constraints.lower.R_plan); + double v_bound = pmSqrt(R_bound * param->a_n_max); + if (biarc_results->iterations > 1 && v_bound < config->velocity_cutoff) { + biarc_results->result = BIARC_MIN_LENGTH_LIMITED; + return TP_ERR_FAIL; + } + } + // Refine Rb if solution has not converged + biarc_results->solution.Rb = guess_blend_region_size_slope( + &biarc_results->constraints, + &biarc_results->solution, + R_goal, + controls->tolerance); + } + + // Ran out of iterations, try the best solution we have + + biarc_results->solution = biarc_results->constraints.lower; + + if (biarc_results->solution.err.deviation >= -config->deviation_abs_tolerance) { + biarc_results->result = BIARC_EXCEEDED_MAX_ITERATIONS; + return TP_ERR_OK; } else { - acc_ratio_tan = pmSqrt(1.0 - pmSq(a_n_vmax / a_max)); + biarc_results->result = BIARC_FAILED_TO_CONVERGE; + return TP_ERR_FAIL; } - - tp_debug_json_start(pmCircleActualMaxVel); - tp_debug_json_double(eff_radius); - tp_debug_json_double(v_max); - tp_debug_json_double(v_max_cutoff); - tp_debug_json_double(a_n_max_cutoff); - tp_debug_json_double(a_n_vmax); - tp_debug_json_double(acc_ratio_tan); - tp_debug_json_end(); - - PmCircleLimits limits = { - v_max_actual, - acc_ratio_tan - }; - - return limits; } - -/** @section spiralfuncs Functions to approximate spiral arc length */ - -/** - * Intermediate function to find the angle for a parameter from 0..1 along the - * spiral arc. - */ -static int pmCircleAngleFromParam(PmCircle const * const circle, - SpiralArcLengthFit const * const fit, - double t, - double * const angle) +#ifdef UNIT_TEST +#include +#endif +#ifdef DO_BIARC_SCAN +tp_err_t scan_blend_properties( + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + biarc_solver_config_t const * const config, + PmVector const * const vel_bound, + PmVector const * const acc_bound, + biarc_solver_results_t * const biarc_results, + BlendParameters * const param, + double cycle_time, + double resolution + ) { - if (fit->spiral_in) { - t = 1.0 - t; - } - //TODO error or cleanup input to prevent param outside 0..1 - double s_in = t * fit->total_planar_length; - - // Quadratic formula to invert arc length -> angle - - double A = fit->b0; - double B = fit->b1; - double C = -s_in; - - double disc = pmSq(B) - 4.0 * A * C ; - if (disc < 0) { - rtapi_print_msg(RTAPI_MSG_ERR, "discriminant %f is negative in angle calculation\n",disc); - return TP_ERR_FAIL; + if (!prev_tc || !tc || !biarc_results) { + return TP_ERR_MISSING_INPUT; } - /* - * Stability of inverting the arc-length relationship. - * Since the b1 coefficient is analogous to arc radius, we can be - * reasonably assured that it will be large enough not to cause numerical - * errors. If this is not the case, then the arc itself is degenerate (very - * small radius), and this condition should be caught well before here. - * - * Since an arc with a very small spiral coefficient will have a small b0 - * coefficient in the fit, we use the Citardauq Formula to ensure that the - * positive root does not lose precision due to subtracting near-similar values. - * - * For more information, see: - * http://people.csail.mit.edu/bkph/articles/Quadratics.pdf - */ - - double angle_out = (2.0 * C) / ( -B - pmSqrt(disc)); - - if (fit->spiral_in) { - // Spiral fit assumes that we're spiraling out, so - // parameterize from opposite end - angle_out = circle->angle - angle_out; + // Find net tolerance that's actually usable (so that we're + // not passing 0.0 around if tolerance is not specified) + tcFindBlendTolerance(prev_tc, tc, ¶m->tolerance); + + blend_solver_constraints_t constraints; + + // Initial solver values for Rb range using somewhat dumb (but cheap) values + constraints.upper = find_max_blend_region(prev_tc, tc); + constraints.tolerance_upper = constraints.upper; + + blendParamInitVelocities(prev_tc, tc, config->feed_override_allowance, param); + + double s1_max = prev_tc->target - constraints.tolerance_upper; + double s2_max = constraints.tolerance_upper; + PmVector u1, u2; + tcGetTangentUnitVector(prev_tc, s1_max, &u1); + tcGetTangentUnitVector(tc, s2_max, &u2); + + findVMaxByAltitude( + &u1, + &u2, + param); + //TODO do altitude calculation too? + // Approximate the blend kinematics using the initial guess + // Actual limits will be enforced later + CHP(blendParamKinematics( + &u1, + &u2, + param, + acc_bound, + vel_bound)); + + // Intersection is at start of next segment, so use startpoint method since it's much faster + EmcPose P; + CHP(tcGetStartpoint(tc, &P)); + +#ifdef UNIT_TEST + printf(">a_n_max %f\n", param->a_n_max); + printf("|f,%12s,%12s,%12s,%12s,%12s,%12s\n", + "Rb", + "tol_dev", + "R_geom", + "R_plan", + "v_plan", + "d"); +#endif + const unsigned MAX_ITERATIONS = (unsigned)((double)constraints.upper.Rb / resolution); + for (biarc_results->iterations = 1; biarc_results->iterations < MAX_ITERATIONS; ++biarc_results->iterations) + { + biarc_results->Rb = (double)biarc_results->iterations * resolution; + // Find blend start / end points and tangent vectors, within Rb of intersection P with procedure find_blend_points_and_tangents + find_blend_points_and_tangents(biarc_results->Rb, prev_tc, tc, &biarc_results->boundary); + // Find blend segment lengths d, L and intermediate points with procedure find_blend_intermediate_segments + int failed = 0; + double T; + if (TP_ERR_OK != find_blend_intermediate_segments(&biarc_results->boundary, &biarc_results->control_pts)) + { + failed = 1; + T=-1; + } else { + + // Verify distance from blend midpoint to original intersection P < tolerance + VecVecDisp(&biarc_results->control_pts.P_mid, &P.tran, &T); + + CHP(find_blend_size_from_intermediates( + &biarc_results->boundary, + &biarc_results->control_pts, + &biarc_results->R_geom, + &biarc_results->arc_len_est)); + + // Cheat by expressing max radius in terms of v_max even though it's not strictly the limiting factor + double guess_v_max_length = biarc_results->arc_len_est / cycle_time; + double guess_R_effective_max = pmSq(guess_v_max_length) / param->a_n_max; + + biarc_results->R_plan = fmin(biarc_results->R_geom, guess_R_effective_max); + } +#ifdef UNIT_TEST + printf("|%d,%12f,%12f,%12f,%12f,%12f,%12f\n", + failed, + biarc_results->Rb, + T, + biarc_results->R_geom, + biarc_results->R_plan, + pmSqrt(param->a_n_max * biarc_results->R_plan), + biarc_results->control_pts.d); +#endif } - - *angle = angle_out; - return TP_ERR_OK; + biarc_results->result = BIARC_EXCEEDED_MAX_ITERATIONS; + return TP_ERR_FAIL; } +#endif - -static void printSpiralArcLengthFit(SpiralArcLengthFit const * const fit) +const char * biarc_result_to_str(BiarcSolverResults r) { - (void)fit; - tp_debug_print("Spiral fit: b0 = %.12f, b1 = %.12f, length = %.12f, spiral_in = %d\n", - fit->b0, - fit->b1, - fit->total_planar_length, - fit->spiral_in); + switch (r) { + case BIARC_DEGENERATE_SEGMENTS: + return "degenerate"; + case BIARC_FAILED_TO_CONVERGE: + return "diverged"; + case BIARC_EXCEEDED_MAX_ITERATIONS: + return "max_iterations"; + case BIARC_NOT_SOLVED: + return "not_solved"; + case BIARC_REACHED_V_GOAL: + return "reached_v_goal"; + case BIARC_REACHED_TOLERANCE: + return "reached_tol"; + case BIARC_CONVERGED: + return "converged"; + case BIARC_MIN_LENGTH_LIMITED: + return "hit_min_len"; + } + return ""; } -/** - * Approximate the arc length function of a general spiral. - * - * The closed-form arc length of a general archimedean spiral is rather - * computationally messy to work with. - * See http://mathworld.wolfram.com/ArchimedesSpiral.html for the actual form. - * - * The simplification here is made possible by a few assumptions: - * 1) That the spiral starts with a nonzero radius - * 2) The spiral coefficient (i.e. change in radius / angle) is not too large - * 3) The spiral coefficient has some minimum magnitude ("perfect" circles are handled as a special case) - * - * The 2nd-order fit below works by matching slope at the start and end of the - * arc length vs. angle curve. This completely specifies the 2nd order fit. - * Also, this fit predicts a total arc length >= the true arc length, which - * means the true speed along the curve will be the same or slower than the - * nominal speed. - */ -int findSpiralArcLengthFit(PmCircle const * const circle, - SpiralArcLengthFit * const fit) +int find_biarc_points_from_solution( + PmVector const * const u1, + PmVector const * const u2, + PmVector const * const P1, + PmVector const * const P2, + PmVector const * const P, + double d, + PmVector const * const acc_bound, + PmVector const * const vel_bound, + BlendControls const * const controls, + BlendPoints * const points, + BlendParameters *const param) { - // Additional data for arc length approximation - double spiral_coef = circle->spiral / circle->angle; - double min_radius = circle->radius; - - if (fsign(circle->spiral) < 0.0) { - // Treat as positive spiral, parameterized in opposite - // direction - spiral_coef*=-1.0; - // Treat final radius as starting radius for fit, so we add the - // negative spiral term to get the minimum radius - // - min_radius+=circle->spiral; - fit->spiral_in = true; + CHP(find_blend_parameters( + u1, + u2, + acc_bound, + vel_bound, + controls, + param)); + + param->theta = findIntersectionAngle( + u1, + u2); + + double straightness_margin = M_PI_2 - param->theta; + + if (fabs(straightness_margin) < 1e-6) { + points->motion_type = TC_LINEAR; + double v_max = fmax(param->v_max_planar, param->v_max_altitude); + param->v_plan = fmin(controls->v_goal, v_max); } else { - fit->spiral_in = false; - } - tp_debug_print("radius = %.12f, angle = %.12f\n", min_radius, circle->angle); - tp_debug_print("spiral_coef = %.12f\n", spiral_coef); - - - //Compute the slope of the arc length vs. angle curve at the start and end of the segment - double slope_start = pmSqrt(pmSq(min_radius) + pmSq(spiral_coef)); - double slope_end = pmSqrt(pmSq(min_radius + spiral_coef * circle->angle) + pmSq(spiral_coef)); + points->motion_type = TC_SPHERICAL; + PmVector u_normal; + VecVecSub( + u2, + u1, + &u_normal); + VecUnitEq(&u_normal); - fit->b0 = (slope_end - slope_start) / (2.0 * circle->angle); - fit->b1 = slope_start; - - fit->total_planar_length = fit->b0 * pmSq(circle->angle) + fit->b1 * circle->angle; - printSpiralArcLengthFit(fit); - - // Check against start and end angle - double angle_end_chk = 0.0; - int res_angle = pmCircleAngleFromParam(circle, fit, 1.0, &angle_end_chk); - if (res_angle != TP_ERR_OK) { - //TODO better error message - rtapi_print_msg(RTAPI_MSG_ERR, - "Spiral fit failed\n"); - return TP_ERR_FAIL; - } + double center_dist = d / cos(param->theta); + param->R_plan = d * tan(param->theta); + param->v_plan = pmSqrt(param->R_plan * param->a_n_max); - // Check fit against angle - double fit_err = angle_end_chk - circle->angle; - if (fabs(fit_err) > TP_ANGLE_EPSILON) { - rtapi_print_msg(RTAPI_MSG_ERR, - "Spiral fit angle difference is %e, maximum allowed is %e\n", - fit_err, - TP_ANGLE_EPSILON); - return TP_ERR_FAIL; + VecScalMult(&u_normal, center_dist, &points->arc_center); + VecVecAddEq(&points->arc_center, P); } - return TP_ERR_OK; -} + points->arc_start = *P1; + points->arc_end = *P2; + points->u_tan = *u1; - -/** - * Compute the angle around a circular segment from the total progress along - * the curve. - */ -int pmCircleAngleFromProgress(PmCircle const * const circle, - SpiralArcLengthFit const * const fit, - double progress, - double * const angle) -{ - double h2; - pmCartMagSq(&circle->rHelix, &h2); - double s_end = pmSqrt(pmSq(fit->total_planar_length) + h2); - // Parameterize by total progress along helix - double t = progress / s_end; - return pmCircleAngleFromParam(circle, fit, t, angle); + return 0; } - -/** - * Find the effective minimum radius for acceleration calculations. - * The radius of curvature of a spiral is larger than the circle of the same - * radius. - */ -double pmCircleEffectiveMinRadius(PmCircle const * circle) +ContinuityCheck calc_C1_continuity( + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const next_tc) { - double dr = circle->spiral / circle->angle; - double h2; - pmCartMagSq(&circle->rHelix, &h2); + ContinuityCheck out = {}; + PmVector u_1, u_2; + tcGetEndTangentUnitVector(prev_tc, &u_1); + tcGetStartTangentUnitVector(next_tc, &u_2); + VecVecSub(&u_2, &u_1, &out.u_diff); + out.dot = VecVecDot(&u_1, &u_2); - // Exact representation of spiral arc length flattened into - double n_inner = pmSq(dr) + pmSq(circle->radius); - double den = n_inner+pmSq(dr); - double num = pmSqrt(n_inner * n_inner * n_inner); - double r_spiral = num / den; + PmVector p_1, p_2; + tcGetEndpoint(prev_tc, &p_1); + tcGetStartpoint(next_tc, &p_2); - // Curvature of helix, assuming that helical motion is independent of plane motion - double effective_radius = h2 / r_spiral + r_spiral; + VecVecSub(&p_2, &p_1, &out.p_diff); - return effective_radius; + return out; } - diff --git a/src/emc/tp/blendmath.h b/src/emc/tp/blendmath.h index ccf2cae70ca..c9f660cb6cd 100644 --- a/src/emc/tp/blendmath.h +++ b/src/emc/tp/blendmath.h @@ -15,252 +15,144 @@ #include "posemath.h" #include "tc_types.h" - -#define BLEND_ACC_RATIO_TANGENTIAL 0.5 -#define BLEND_ACC_RATIO_NORMAL (pmSqrt(1.0 - pmSq(BLEND_ACC_RATIO_TANGENTIAL))) -#define BLEND_KINK_FACTOR 0.25 - -typedef enum { - BLEND_NONE, - BLEND_LINE_LINE, - BLEND_LINE_ARC, - BLEND_ARC_LINE, - BLEND_ARC_ARC, -} blend_type_t; - -/** - * 3D Input geometry for a spherical blend arc. - * This structure contains all of the basic geometry in 3D for a blend arc. - */ -typedef struct { - PmCartesian u1; /* unit vector along line 1 */ - PmCartesian u2; /* unit vector along line 2 */ - PmCartesian P; /* Intersection point */ - PmCartesian normal; /* normal unit vector to plane containing lines */ - PmCartesian binormal; /* binormal unit vector to plane containing lines */ - PmCartesian u_tan1; /* Actual tangent vector to 1 (used for arcs only) */ - PmCartesian u_tan2; /* Actual tangent vector to 2 (used for arcs only) */ - PmCartesian center1; /* Local approximation of center for arc 1 */ - PmCartesian center2; /* Local approximation of center for arc 2 */ - double radius1; /* Local approximation of radius */ - double radius2; - double theta_tan; - double v_max1; /* maximum velocity in direction u_tan1 */ - double v_max2; /* maximum velocity in direction u_tan2 */ - -} BlendGeom3; - -/** - * 9D Input geometry for a spherical blend arc. - */ -#ifdef BLEND_9D -typedef struct { -//Not implemented yet -} BlendGeom9; -#endif - - -/** - * Blend arc parameters (abstracted). - * This structure holds blend arc parameters that have been abstracted from the - * physical geometry. This data is used to find the maximum radius given the - * constraints on the blend. By abstracting the parameters from the geometry, - * the same calculations can be used with any input geometry (lines, arcs, 6 or - * 9 dimensional lines). - */ -typedef struct { - double tolerance; /* Net blend tolerance (min of line 1 and 2) */ - double L1; /* Available part of line 1 to blend over */ - double L2; /* Available part of line 2 to blend over */ - double v_req; /* requested velocity for the blend arc */ - double a_max; /* max acceleration allowed for blend */ - - /* These fields are considered "output", and may be refactored into a - * separate structure in the future */ - - double theta; /* Intersection angle, half of angle between -u1 and u2 */ - double phi; /* supplement of intersection angle, angle between u1 and u2 */ - double a_n_max; /* max normal acceleration allowed */ - - double R_plan; /* planned radius for blend arc */ - double d_plan; /* distance along each line to arc endpoints */ - - double v_goal; /* desired velocity at max feed override */ - double v_plan; /* planned max velocity at max feed override */ - double v_actual; /* velocity at feedscale = 1.0 */ - double s_arc; /* arc length */ - int consume; /* Consume the previous segment */ - double line_length; - //Arc specific stuff - int convex1; - int convex2; - double phi1_max; - double phi2_max; - -} BlendParameters; - - -/** - * Output geometry in 3D. - * Stores the three points representing a simple 3D spherical arc. - */ -typedef struct { - PmCartesian arc_start; /* start point for blend arc */ - PmCartesian arc_end; /* end point for blend arc */ - PmCartesian arc_center; /* center point for blend arc */ - double trim1; /* length (line) or angle (arc) to cut from prev_tc */ - double trim2; /* length (line) or angle (arc) to cut from tc */ -} BlendPoints3; - - - -#ifdef BLEND_9D -typedef struct { -//Not implemented yet -} BlendPoints9; -#endif - -double findMaxTangentAngle(double v, double acc, double cycle_time); - -double findKinkAccel(double kink_angle, double v_plan, double cycle_time); - -double fsign(double f); - -int clip_min(double * const x, double min); - -int clip_max(double * const x, double max); - -double saturate(double x, double max); - -double bisaturate(double x, double max, double min); - -int sat_inplace(double * const x, double max); - -int checkTangentAngle(PmCircle const * const circ, SphericalArc const * const arc, BlendGeom3 const * const geom, BlendParameters const * const param, double cycle_time, int at_end); - -int findIntersectionAngle(PmCartesian const * const u1, - PmCartesian const * const u2, double * const theta); - -double pmCartMin(PmCartesian const * const in); - -int calculateInscribedDiameter(PmCartesian const * const normal, - PmCartesian const * const bounds, double * const diameter); - -int findAccelScale(PmCartesian const * const acc, - PmCartesian const * const bounds, - PmCartesian * const scale); - -int pmUnitCartsColinear(PmCartesian const * const u1, - PmCartesian const * const u2); - -int pmCartCartParallel(PmCartesian const * const u1, - PmCartesian const * const u2, - double tol); - -int pmCartCartAntiParallel(PmCartesian const * const u1, - PmCartesian const * const u2, - double tol); - -int pmCircLineCoplanar(PmCircle const * const circ, - PmCartLine const * const line, double tol); - -int blendCoplanarCheck(PmCartesian const * const normal, - PmCartesian const * const u1_tan, - PmCartesian const * const u2_tan, - double tol); - -int blendCalculateNormals3(BlendGeom3 * const geom); - -int blendComputeParameters(BlendParameters * const param); +#include "tp_enums.h" +#include "blendmath_types.h" +#include "pm_vector.h" + +double findIntersectionAngle(PmVector const * const u1, + PmVector const * const u2); + +int findAccelScale(PmVector const * const acc, + PmVector const * const bounds, + PmVector * const scale); + +int findMaxValueOnPlane(PmVector const * plane_envelope_sq, + PmVector const * bounds, + double * max_planar_value); + +int quadraticFormula( + double A, + double B, + double C, + double * const root0, + double * const root1); + +double findTrapezoidalDesiredVel(double a_max, + double dx, + double v_final, + double currentvel, + double cycle_time); int blendCheckConsume(BlendParameters * const param, - BlendPoints3 const * const points, + double L_prev, TC_STRUCT const * const prev_tc, int gap_cycles); -int blendFindPoints3(BlendPoints3 * const points, BlendGeom3 const * const geom, - BlendParameters const * const param); - -int blendGeom3Init(BlendGeom3 * const geom, - TC_STRUCT const * const prev_tc, - TC_STRUCT const * const tc); - -int blendParamKinematics(BlendGeom3 * const geom, - BlendParameters * const param, - TC_STRUCT const * const prev_tc, - TC_STRUCT const * const tc, - PmCartesian const * const acc_bound, - PmCartesian const * const vel_bound, - double maxFeedScale); - -int blendInit3FromLineLine(BlendGeom3 * const geom, BlendParameters * const param, - TC_STRUCT const * const prev_tc, - TC_STRUCT const * const tc, - PmCartesian const * const acc_bound, - PmCartesian const * const vel_bound, - double maxFeedScale); - -int blendInit3FromLineArc(BlendGeom3 * const geom, BlendParameters * const param, - TC_STRUCT const * const prev_tc, - TC_STRUCT const * const tc, - PmCartesian const * const acc_bound, - PmCartesian const * const vel_bound, - double maxFeedScale); - -int blendInit3FromArcLine(BlendGeom3 * const geom, BlendParameters * const param, - TC_STRUCT const * const prev_tc, - TC_STRUCT const * const tc, - PmCartesian const * const acc_bound, - PmCartesian const * const vel_bound, - double maxFeedScale); - -int blendInit3FromArcArc(BlendGeom3 * const geom, BlendParameters * const param, - TC_STRUCT const * const prev_tc, - TC_STRUCT const * const tc, - PmCartesian const * const acc_bound, - PmCartesian const * const vel_bound, - double maxFeedScale); - -int blendArcArcPostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in, - BlendParameters * const param, BlendGeom3 const * const geom, - PmCircle const * const circ1, PmCircle const * const circ2); - -int blendLineArcPostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in, - BlendParameters * const param, BlendGeom3 const * const geom, - PmCartLine const * const line1, PmCircle const * const circ2); - -int blendArcLinePostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in, - BlendParameters * const param, BlendGeom3 const * const geom, - PmCircle const * const circ1, PmCartLine const * const line2); - -int arcFromBlendPoints3(SphericalArc * const arc, BlendPoints3 const * const points, - BlendGeom3 const * const geom, BlendParameters const * const param); - -//Not implemented yet -int blendGeom3Print(BlendGeom3 const * const geom); -int blendParamPrint(BlendParameters const * const param); -int blendPoints3Print(BlendPoints3 const * const points); - -double pmCartAbsMax(PmCartesian const * const v); - -typedef struct { - double v_max; - double acc_ratio; -} PmCircleLimits; - -PmCircleLimits pmCircleActualMaxVel(const PmCircle *circle, - double v_max_nominal, - double a_max_nominal); - -int findSpiralArcLengthFit(PmCircle const * const circle, - SpiralArcLengthFit * const fit); -int pmCircleAngleFromProgress(PmCircle const * const circle, - SpiralArcLengthFit const * const fit, - double progress, - double * const angle); -double pmCircleEffectiveMinRadius(const PmCircle *circle); +int find_blend_parameters( + const PmVector * const u_tan1, + const PmVector * const u_tan2, + PmVector const * const acc_bound, + PmVector const * const vel_bound, + const BlendControls * const controls, + BlendParameters * const param); + +EndCondition checkEndCondition(double cycleTime, + double dtg, + double currentvel, + double v_f, + double a_max); + +#define PSEUDO_SQRT_EPSILON 0.001 +double pseudo_sqrt(double x); + +int find_blend_vel_accel_planar_limits( + PmVector const * const u_tan1, + PmVector const * const u_tan2, + PmVector const * const acc_bound, + PmVector const * const vel_bound, + double *a_max_planar, + double *v_max_planar); + +void findVMaxByAltitude(PmVector const * const u1, + PmVector const * const u2, const BlendControls * const controls, + BlendParameters * const param); + +int blendParamInitVelocities(TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + double override_allowance, + BlendControls * const controls); + +tp_err_t init_blend_segment_from_points( + TC_STRUCT * const blend_tc, + BlendPoints const *points, + BlendParameters * const param); + +// API for biarc blends + +const char * biarc_result_to_str(BiarcSolverResults r); + +double find_blend_region_from_tolerance( + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + double tolerance); + +double find_blend_region_from_tolerance_simple( + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + double tolerance); + +tp_err_t find_blend_points_and_tangents( + double Rb, + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + blend_boundary_t * const out); + +int find_blend_intermediate_segments(blend_boundary_t const * const blend_params, + biarc_control_points_t * const control_pts); + +double find_max_blend_region(TC_STRUCT const * const prev_tc, TC_STRUCT const * const tc, double v_goal); + +tp_err_t find_blend_size_from_intermediates(blend_boundary_t const * const blend_boundary, + biarc_control_points_t const * const intermediates, + double *R_geom, double * arc_len_est); + +tp_err_t optimize_biarc_blend_size(TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + const biarc_solver_config_t * const config, + PmVector const * const vel_bound, + PmVector const * const acc_bound, + biarc_solver_results_t * const biarc_results, BlendControls * const controls, + BlendParameters * const param, + double cycle_time); + +// For testing only +tp_err_t scan_blend_properties( + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const tc, + biarc_solver_config_t const * const config, + PmVector const * const vel_bound, + PmVector const * const acc_bound, + biarc_solver_results_t * const biarc_results, + BlendParameters * const param, + double cycle_time, + double resolution); + +int blend_find_arcpoints3( + BlendPoints * const points, + biarc_solver_results_t const * const); + +int find_biarc_points_from_solution(PmVector const * const u1, + PmVector const * const u2, + PmVector const * const P1, + PmVector const * const P2, + PmVector const * const P, + double d, + PmVector const * const acc_bound, + PmVector const * const vel_bound, const BlendControls * const controls, + BlendPoints * const points, + BlendParameters *const param); + +ContinuityCheck calc_C1_continuity( + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const next_tc); -static inline double findVPeak(double a_t_max, double distance) -{ - return pmSqrt(a_t_max * distance); -} #endif diff --git a/src/emc/tp/blendmath_types.h b/src/emc/tp/blendmath_types.h new file mode 100644 index 00000000000..48d92c1c369 --- /dev/null +++ b/src/emc/tp/blendmath_types.h @@ -0,0 +1,151 @@ +#ifndef BLENDMATH_TYPES_H +#define BLENDMATH_TYPES_H + +#include "posemath.h" +#include "pm_vector.h" +#include "tc_types.h" + +#define BLEND_ACC_RATIO_TANGENTIAL 0.5 +#define BLEND_ACC_RATIO_NORMAL (pmSqrt(1.0 - pmSq(BLEND_ACC_RATIO_TANGENTIAL))) +#define BLEND_KINK_FACTOR 0.25 + +/** + * Blend "controls" imposed by user settings and the segments themselves. + * This captures externally imposed limits like max velocity, the amount of + * each segment to consume, blend tolerance, etc. + */ +typedef struct { + double v_max_geom1; /* ini_maxvel for each segment */ + double v_max_geom2; + double v_req; /* requsted velocity for the blend arc */ + double v_goal; /* desired velocity at max feed override */ + + double tolerance; /* Net blend tolerance (min of line 1 and 2) */ + + double L1; /* Available part of line 1 to blend over */ + double L2; /* Available part of line 2 to blend over */ +} BlendControls; + +/** + * Blend arc parameters (abstracted). + * This structure holds blend arc parameters that have been abstracted from the + * physical geometry. This data is used to find the maximum radius given the + * constraints on the blend. By abstracting the parameters from the geometry, + * the same calculations can be used with any input geometry (lines, arcs, 6 or + * 9 dimensional lines). + */ +typedef struct { + /* These fields are considered "output", and may be refactored into a + * separate structure in the future */ + + double theta; /* Intersection angle, half of angle between -u1 and u2 */ + double phi; /* supplement of intersection angle, angle between u1 and u2 */ + double a_n_max; /* max normal acceleration allowed */ + double a_max_planar; /* max acceleration allowed for blend */ + + double R_plan; /* planned radius for blend arc */ + double d_plan; /* distance along each line to arc endpoints */ + + double v_max_planar; /* maximum possible velocity in blend plane */ + double v_max_altitude; /* overall maximum possible velocity during blend segment */ + double v_plan; /* planned max velocity at max feed override */ + double v_actual; /* velocity at feedscale = 1.0 */ + double s_arc; /* arc length */ + int is_consumable; /* Consume the previous segment */ + int consume; /* Consume the previous segment */ + double line_length; +} BlendParameters; + +/** + * Output geometry in 3D. + * Stores the three points representing a simple 3D spherical arc. + */ +typedef struct { + PmVector arc_start; /* start point for blend arc */ + PmVector arc_end; /* end point for blend arc */ + PmVector arc_center; /* center point for blend arc */ + PmVector u_tan; + tc_motion_type_t motion_type; +} BlendPoints; + +// Start by mimicing the current structure for minimal changes +typedef struct { + double v_f; + double dt; +} EndCondition; + +// BiArc blend structs (solution and intermediate variables) + +typedef struct +{ + PmVector P1; + PmVector u1; + PmVector P2; + PmVector u2; + double s1; + double s2; +} blend_boundary_t; + +typedef struct +{ + PmVector Pb1; + PmVector Pb2; + PmVector u_mid; + PmVector P_mid; + double d; //!< Length of intermediate blend segment +} biarc_control_points_t; + + +typedef enum +{ + BIARC_DEGENERATE_SEGMENTS=-3, + BIARC_FAILED_TO_CONVERGE=-2, + BIARC_EXCEEDED_MAX_ITERATIONS=-1, + BIARC_NOT_SOLVED, + BIARC_REACHED_V_GOAL, + BIARC_REACHED_TOLERANCE, + BIARC_CONVERGED, + BIARC_MIN_LENGTH_LIMITED, +} BiarcSolverResults; + +typedef struct +{ + double deviation; + double radius_rel; + double radius_abs; +} biarc_solution_errors_t; + +typedef struct { + double Rb; //!< size of blend region (i.e. distance to go along motion segments from intersection to blend boundary points) + double R_geom; //!< Estimated blend radius based on geometry + double arc_len_est; //!< Estimated arc length of first blend + double R_plan; //!< Effective blend "radius" for a max blend velocity, based on both arc length and geometry . + double T_plan; //!< Blend tolerance (deviation from intersection point) for Rb + biarc_solution_errors_t err; //!< error terms from tolerance / velocity goals + blend_boundary_t boundary; //!< Boundary of the blend solution corresponding to Rb + biarc_control_points_t control_pts; //!< Control points for the intermediate line segments making up the biarc blend +} biarc_solution_t; + +typedef struct +{ + biarc_solution_t upper; + biarc_solution_t lower; + int bias_count; //!< KLUDGE track number of times the solution has been on the same side of the upper / lower divide +} blend_solver_constraints_t; + +typedef struct +{ + blend_solver_constraints_t constraints; //!< blend solver internals (upper / lower bounds on Rb) + unsigned iterations; //!< solver iterations + BiarcSolverResults result; + biarc_solution_t solution; +} biarc_solver_results_t; + +typedef struct +{ + PmVector p_diff; + PmVector u_diff; + double dot; +} ContinuityCheck; + +#endif // BLENDMATH_TYPES_H diff --git a/src/emc/tp/joint_util.c b/src/emc/tp/joint_util.c new file mode 100644 index 00000000000..f83cff6ceea --- /dev/null +++ b/src/emc/tp/joint_util.c @@ -0,0 +1,144 @@ +#include "joint_util.h" +#include "motion_debug.h" +#include "rtapi_math.h" +#include "pm_vector.h" + +extern emcmot_debug_t *emcmotDebug; + +PmCartesian getXYZAccelBounds() { + PmCartesian acc_bound = { + emcmotDebug->joints[0].acc_limit, + emcmotDebug->joints[1].acc_limit, + emcmotDebug->joints[2].acc_limit, + }; + return acc_bound; +} + +PmCartesian getXYZVelBounds() { + PmCartesian vel_bound = { + emcmotDebug->joints[0].vel_limit, + emcmotDebug->joints[1].vel_limit, + emcmotDebug->joints[2].vel_limit, + }; + return vel_bound; +} + +PmVector getAccelBounds() +{ + PmVector acc_bound={}; + for (int i = 0; i < PM_VECTOR_SIZE; ++i) { + acc_bound.ax[i] = emcmotDebug->joints[i].acc_limit; + } + return acc_bound; +} + +PmVector getVelBounds() +{ + PmVector vel_bound={}; + for (int i = 0; i < PM_VECTOR_SIZE; ++i) { + vel_bound.ax[i] = emcmotDebug->joints[i].vel_limit; + } + return vel_bound; +} + +unsigned jointAccelViolation(int joint_idx, double acc) +{ + // Allow some numerical tolerance here since max acceleration is a bit + // fuzzy anyway. Torque is the true limiting factor, so there has to be + // some slack here anyway due to variations in table load, friction, etc. + static const double ABS_TOL = 1e-3; + const double REL_TOL = 1.0 + ABS_TOL; + const double a_max_nominal = emcmotDebug->joints[joint_idx].acc_limit; + const double a_limit = fmax(a_max_nominal * REL_TOL, a_max_nominal + ABS_TOL); + return (unsigned)(fabs(acc) > a_limit) << joint_idx; +} + +unsigned jointVelocityViolation(int joint_idx, double v_actual) +{ + // Allow some numerical tolerance here since max acceleration is a bit + // fuzzy anyway. Torque is the true limiting factor, so there has to be + // some slack here anyway due to variations in table load, friction, etc. + static const double ABS_TOL = 1e-2; + const double REL_ACC_TOL = 1.0 + ABS_TOL; + const double v_max_nominal = emcmotDebug->joints[joint_idx].vel_limit; + const double v_limit = fmax(v_max_nominal * REL_ACC_TOL, v_max_nominal + ABS_TOL); + return (unsigned)(fabs(v_actual) > v_limit) << joint_idx; +} + +unsigned jointPositionViolation(int joint_idx, double position) +{ + static const double ABS_TOL = 1e-6; + return (unsigned)(position < emcmotDebug->joints[joint_idx].min_pos_limit - ABS_TOL && + position > emcmotDebug->joints[joint_idx].max_pos_limit + ABS_TOL) << joint_idx; +} + +/** + * Checks for any acceleration violations based on axis limits. + * + * @return 0 if all acceleration bounds are respected, or a bit-set of failed axes (in XYZABCUVW bit order). + */ +unsigned findAccelViolations(PmVector axis_accel) +{ + + // Bit-mask each failure so we can report all failed axes + unsigned fail_bits = (unsigned)(0x0); + for (int i=0; i < PM_VECTOR_SIZE; ++i) { + fail_bits |= jointAccelViolation(i, axis_accel.ax[i]); + } + return fail_bits; +} + +unsigned findVelocityViolations(PmVector axis_vel) +{ + + // Bit-mask each failure so we can report all failed axes + unsigned fail_bits = (unsigned)(0x0); + for (int i=0; i < PM_VECTOR_SIZE; ++i) { + fail_bits |= jointVelocityViolation(i, axis_vel.ax[i]); + } + return fail_bits; +} + +unsigned findPositionViolations(PmVector axis_pos) +{ + + // Bit-mask each failure so we can report all failed axes + unsigned fail_bits = (unsigned)(0x0); + for (int i=0; i < PM_VECTOR_SIZE; ++i) { + fail_bits |= jointPositionViolation(i, axis_pos.ax[i]); + } + return fail_bits; +} + +double findMinNonZero(const PmCartesian * const bounds) { + //Start with max accel value + double act_limit = fmax(fmax(bounds->x, bounds->y), bounds->z); + + // Compare only with active axes + if (bounds->x > 0) { + act_limit = fmin(act_limit, bounds->x); + } + if (bounds->y > 0) { + act_limit = fmin(act_limit, bounds->y); + } + if (bounds->z > 0) { + act_limit = fmin(act_limit, bounds->z); + } + return act_limit; +} + +/** + * Checks all axis values in an EmcPose to see if they exceed a magnitude threshold. + * @return a bitmask that is 0 if all axes are within the threshold. Any + * out-of-limit axes set their corresponding bit to 1 in the returned value (X + * is 0th bit, Y is 1st, etc.). + */ +unsigned int findAbsThresholdViolations(PmVector vec, double threshold) +{ + threshold = fabs(threshold); + unsigned fail_bits = (unsigned)(0x0); + for (int i=0; i < PM_VECTOR_SIZE; ++i) { + fail_bits |= fabs(vec.ax[i]) > threshold; + } + return fail_bits; +} diff --git a/src/emc/tp/joint_util.h b/src/emc/tp/joint_util.h new file mode 100644 index 00000000000..dbeb4dd145a --- /dev/null +++ b/src/emc/tp/joint_util.h @@ -0,0 +1,28 @@ +#ifndef JOINT_UTIL_H +#define JOINT_UTIL_H + +#include +#include +#include "pm_vector.h" + +PmCartesian getXYZAccelBounds(); +PmCartesian getXYZVelBounds(); + +PmVector getAccelBounds(); +PmVector getVelBounds(); + +unsigned jointAccelViolation(int joint_idx, double acc); +unsigned jointVelocityViolation(int joint_idx, double v_actual); +unsigned jointPositionViolation(int joint_idx, double position); + +unsigned findAccelViolations(PmVector axis_accel); +unsigned findVelocityViolations(PmVector axis_vel); +unsigned findPositionViolations(PmVector axis_pos); + +/** + * Finds the smallest non-zero component in a non-negative "bounds" vector. + * Used to identify the "slowest" axis, but ignore axes + */ +double findMinNonZero(PmCartesian const * const bounds); + +#endif // JOINT_UTIL_H diff --git a/src/emc/tp/math_util.c b/src/emc/tp/math_util.c new file mode 100644 index 00000000000..afee6374923 --- /dev/null +++ b/src/emc/tp/math_util.c @@ -0,0 +1,92 @@ +#include "math_util.h" +#include "rtapi_math.h" + +double fsign(double f) +{ + if (f>0) { + return 1.0; + } else if (f < 0) { + return -1.0; + } else { + //Technically this should be NAN but that's a useless result for tp purposes + return 0; + } +} + +/** Clip the input at the specified minimum (in place). */ +int clip_min(double * const x, double min) { + if ( *x < min ) { + *x = min; + return 1; + } + return 0; +} + +/** Clip the input at the specified maximum (in place). */ +int clip_max(double * const x, double max) { + if ( *x > max ) { + *x = max; + return 1; + } + return 0; +} + +/** + * Saturate a value x to be within +/- max. + */ +double saturate(double x, double max) { + if ( x > max ) { + return max; + } + else if ( x < (-max) ) { + return -max; + } + else { + return x; + } +} + + +/** + * Apply bounds to a value x. + */ +double bound(double x, double max, double min) { + if ( x > max ) { + return max; + } + else if ( x < (min) ) { + return min; + } + else { + return x; + } +} + + +/** In-place saturation function */ +int sat_inplace(double * const x, double max) { + if ( *x > max ) { + *x = max; + return 1; + } + else if ( *x < -max ) { + *x = -max; + return -1; + } + return 0; +} + +double hypot2(double x, double y) +{ + return sqrt(x*x + y*y); +} + +double hypot3(double x, double y, double z) +{ + return sqrt(x*x + y*y + z*z); +} + +double hypot4(double w, double x, double y, double z) +{ + return sqrt(w*w + x*x + y*y + z*z); +} diff --git a/src/emc/tp/math_util.h b/src/emc/tp/math_util.h new file mode 100644 index 00000000000..d987c032deb --- /dev/null +++ b/src/emc/tp/math_util.h @@ -0,0 +1,32 @@ +#ifndef MATH_UTIL_H +#define MATH_UTIL_H + +static inline long max(long y, long x) { + return y > x ? y : x; +} + +static inline long min(long y, long x) { + return y < x ? y : x; +} + +static inline double signum(double x) { + return (x > 0.0) ? 1.0 : (x < 0.0) ? -1.0 : 0.0; +} + +double fsign(double f); + +int clip_min(double * const x, double min); + +int clip_max(double * const x, double max); + +double saturate(double x, double max); + +double bound(double x, double max, double min); + +int sat_inplace(double * const x, double max); + +double hypot2(double x, double y); +double hypot3(double x, double y, double z); +double hypot4(double w, double x, double y, double z); + +#endif // MATH_UTIL_H diff --git a/src/emc/tp/meson.build b/src/emc/tp/meson.build index 8feef3c0173..dcbd7474b24 100644 --- a/src/emc/tp/meson.build +++ b/src/emc/tp/meson.build @@ -1,8 +1,13 @@ tp_srcs = files([ + 'blendmath.c', + 'joint_util.c', + 'spherical_arc.c', + 'spherical_arc9.c', 'tc.c', 'tcq.c', 'tp.c', - 'spherical_arc.c', - 'blendmath.c', + 'math_util.c', + 'pm_line9.c', + 'pm_circle9.c', ]) tp_inc = include_directories(['.']) diff --git a/src/emc/tp/pm_circle9.c b/src/emc/tp/pm_circle9.c new file mode 100644 index 00000000000..52f7df4ceb9 --- /dev/null +++ b/src/emc/tp/pm_circle9.c @@ -0,0 +1,370 @@ +#include "rtapi.h" +#include "pm_circle9.h" +#include "blendmath_types.h" +#include "rtapi_bool.h" +#include "rtapi_math.h" +#include "math_util.h" + +static inline double sq(double x) {return x*x;} + +double pmCircle9LengthAndRatios(PmCircle9 * const circ9) +{ + circ9->total_length = pmCircle9Length(circ9); + circ9->xyz_ratio = hypot2(circ9->fit.total_planar_length, circ9->xyz.height) / circ9->total_length; + circ9->abc_ratio = circ9->abc.tmag / circ9->total_length; + circ9->uvw_ratio = circ9->uvw.tmag / circ9->total_length; + return circ9->total_length; +} + +int pmCircle9Init( + PmCircle9 * const circ9, + PmVector const * const start, + PmVector const * const end, + PmCartesian const * const center, + PmCartesian const * const normal, + int turn, + double expected_angle_rad) +{ + PmCartesian start_xyz, end_xyz; + PmCartesian start_uvw, end_uvw; + PmCartesian start_abc, end_abc; + + VecToCart(start, &start_xyz, &start_abc, &start_uvw); + VecToCart(end, &end_xyz, &end_abc, &end_uvw); + + int xyz_fail = pmCircleInit(&circ9->xyz, &start_xyz, &end_xyz, center, normal, turn, expected_angle_rad); + //Initialize line parts of Circle9 + + int abc_fail = pmCartLineInit(&circ9->abc, &start_abc, &end_abc); + int uvw_fail = pmCartLineInit(&circ9->uvw, &start_uvw, &end_uvw); + + int res_fit = findSpiralArcLengthFit(&circ9->xyz, &circ9->fit, 1e-6); + + double l = pmCircle9LengthAndRatios(circ9); + + if (xyz_fail || abc_fail || uvw_fail || res_fit) { + rtapi_print_msg(RTAPI_MSG_ERR,"Failed to initialize Circle9, err codes %d, %d, %d, %d with length %f\n", + xyz_fail, abc_fail, uvw_fail, res_fit, l); + return -1; + } + + return 0; +} + +double pmCircle9Length(PmCircle9 const * const circ9) +{ + return hypot4(circ9->fit.total_planar_length, circ9->xyz.height, circ9->abc.tmag, circ9->uvw.tmag); +} + +int pmCircle9Point(PmCircle9 const * const circ9, double progress, PmVector * const point) +{ + double xyz_progress = progress * circ9->xyz_ratio; + double angle = 0; + pmCircleAngleFromProgress(&circ9->xyz, &circ9->fit, xyz_progress, &angle); + PmCartesian xyz; + pmCirclePoint(&circ9->xyz, angle, &xyz); + PmCartesian abc; + pmCartLinePoint(&circ9->abc, circ9->abc_ratio * progress, &abc); + PmCartesian uvw; + pmCartLinePoint(&circ9->uvw, circ9->uvw_ratio * progress, &uvw); + CartToVec(&xyz, &abc, &uvw, point); + return 0; +} + +int pmCircle9Cut(PmCircle9 * const circ9, double cut_progress, SegmentToKeepType keep_pt) +{ + double xyz_new_pt_progress = cut_progress * circ9->xyz_ratio; + double abc_cut_progress = cut_progress * circ9->abc_ratio; + double uvw_cut_progress = cut_progress * circ9->uvw_ratio; + + int res_other_stretch = pmCartLineCut(&circ9->abc, abc_cut_progress, keep_pt); + res_other_stretch |= pmCartLineCut(&circ9->uvw, uvw_cut_progress, keep_pt); + + double cut_angle; + pmCircleAngleFromProgress(&circ9->xyz, &circ9->fit, xyz_new_pt_progress, &cut_angle); + + int res_stretch = pmCircleCut(&circ9->xyz, cut_angle, keep_pt); + int res_fit = findSpiralArcLengthFit(&circ9->xyz, &circ9->fit, 1e-6); + + pmCircle9LengthAndRatios(circ9); + + return res_fit | res_stretch | res_other_stretch; +} + +int pmCircle9TangentVector(PmCircle9 const * const circ9, double progress, PmVector * const out) +{ + // Scale each portion of the motion by it's total displacement to get the 9D tangent vector + PmCircle const *circle = &circ9->xyz; + + double progress_xyz = progress * circ9->xyz_ratio; + double angle_in=0; + pmCircleAngleFromProgress(circle, &circ9->fit, progress_xyz, &angle_in); + + PmCartesian xyz; + pmCircleTangentVector(circle, angle_in, &xyz); + pmCartScalMultEq(&xyz, circ9->xyz_ratio); + + PmCartesian abc; + pmCartScalMult(&circ9->abc.uVec, circ9->abc_ratio, &abc); + + PmCartesian uvw; + pmCartScalMult(&circ9->uvw.uVec, circ9->uvw_ratio, &uvw); + + CartToVec(&xyz, &abc, &uvw, out); + VecUnitEq(out); + return 0; +} + +/** + * Find the geometric tangent vector to a helical arc. + * Unlike the acceleration vector, the result of this calculation is a vector + * tangent to the helical arc. This is called by wrapper functions for the case of a circular or helical arc. + */ +int pmCircleTangentVector( + PmCircle const * const circle, + double angle_in, + PmCartesian * const u_tan_out) +{ + // Cheat: "rHelix" already has helical height built in + double cheat_dz = 1.0 / circle->angle; + double dr = circle->spiral / circle->angle; + PmCartesian tmp, uRadial, uTangential; + + const double Cangle_by_r = cos(angle_in) / circle->radius; + const double Sangle_by_r = sin(angle_in) / circle->radius; + + pmCartScalMult(&circle->rTan, Cangle_by_r, &uRadial); + pmCartScalMult(&circle->rPerp, Sangle_by_r, &tmp); + pmCartCartAddEq(&uRadial, &tmp); + + pmCartScalMult(&circle->rTan, -Sangle_by_r, &uTangential); + pmCartScalMult(&circle->rPerp, Cangle_by_r, &tmp); + pmCartCartAddEq(&uTangential, &tmp); + + // Add spiral component (scaled by radius) + pmCartScalMult(&uTangential, circle->radius + dr * angle_in, u_tan_out); + pmCartScalMult(&uRadial, dr, &tmp); + pmCartCartAddEq(u_tan_out, &tmp); + + // Add helical component (scaled by radius) + pmCartScalMult(&circle->rHelix, cheat_dz, &tmp); + pmCartCartAddEq(u_tan_out, &tmp); + + // Normalize to get a unit vector in tangent direction + pmCartUnitEq(u_tan_out); + return 0; +} + +PmCircleLimits pmCircleActualMaxVel( + PmCircle const * circle, + double v_max, + double a_max) +{ + double a_n_max_cutoff = BLEND_ACC_RATIO_NORMAL * a_max; + + // For debugging only + double eff_radius = pmCircleEffectiveMinRadius(circle); + + // Find the acceleration necessary to reach the maximum velocity + double a_n_vmax = sq(v_max) / fmax(eff_radius, DOUBLE_FUZZ); + // Find the maximum velocity that still obeys our desired tangential / total acceleration ratio + double v_max_cutoff = sqrt(a_n_max_cutoff * eff_radius); + + double v_max_actual = v_max; + double acc_ratio_tan = BLEND_ACC_RATIO_TANGENTIAL; + + if (a_n_vmax > a_n_max_cutoff) { + v_max_actual = v_max_cutoff; + } else { + acc_ratio_tan = sqrt(1.0 - sq(a_n_vmax / a_max)); + } + + PmCircleLimits limits = { + v_max_actual, + acc_ratio_tan + }; + + return limits; +} + +/** + * Approximate the arc length function of a general spiral. + * + * The closed-form arc length of a general archimedean spiral is rather + * computationally messy to work with. + * See http://mathworld.wolfram.com/ArchimedesSpiral.html for the actual form. + * + * The simplification here is made possible by a few assumptions: + * 1) That the spiral starts with a nonzero radius + * 2) The spiral coefficient (i.e. change in radius / angle) is not too large + * 3) The spiral coefficient has some minimum magnitude ("perfect" circles are handled as a special case) + * + * The 2nd-order fit below works by matching slope at the start and end of the + * arc length vs. angle curve. This completely specifies the 2nd order fit. + * Also, this fit predicts a total arc length >= the true arc length, which + * means the true speed along the curve will be the same or slower than the + * nominal speed. + */ +int findSpiralArcLengthFit( + PmCircle const * const circle, + SpiralArcLengthFit * const fit, + double angle_tolerance) +{ + // Additional data for arc length approximation + double spiral_coef = circle->spiral / circle->angle; + double min_radius = circle->radius; + + if (circle->spiral < 0.0) { + // Treat as positive spiral, parameterized in opposite + // direction + spiral_coef*=-1.0; + // Treat final radius as starting radius for fit, so we add the + // negative spiral term to get the minimum radius + // + min_radius+=circle->spiral; + fit->spiral_in = true; + } else { + fit->spiral_in = false; + } + + //Compute the slope of the arc length vs. angle curve at the start and end of the segment + double slope_start = hypot2(min_radius, spiral_coef); + double slope_end = hypot2(min_radius + spiral_coef * circle->angle, spiral_coef); + + fit->b0 = (slope_end - slope_start) / (2.0 * circle->angle); + fit->b1 = slope_start; + + fit->total_planar_length = fit->b0 * sq(circle->angle) + fit->b1 * circle->angle; + + // Check against start and end angle + double angle_end_chk = 0.0; + int res_angle = pmCircleAngleFromParam(circle, fit, 1.0, &angle_end_chk); + if (res_angle != 0) { + //TODO better error message + rtapi_print_msg(RTAPI_MSG_ERR, + "Spiral fit failed\n"); + return -1; + } + + // Check fit against angle + double fit_err = angle_end_chk - circle->angle; + if (fabs(fit_err) > angle_tolerance) { + rtapi_print_msg(RTAPI_MSG_ERR, + "Spiral fit angle difference is %e, maximum allowed is %e\n", + fit_err, + angle_tolerance); + return -1; + } + + return 0; +} + +double spiralEffectiveRadius(PmCircle const * circle) +{ + double dr = circle->spiral / circle->angle; + + // Exact representation of spiral arc length flattened into + double n_inner = sq(dr) + sq(circle->radius); + double den = n_inner+sq(dr); + double num = sqrt(pmCb(n_inner)); + double r_spiral = num / den; + + return r_spiral; +} + +/** + * Find the effective minimum radius for acceleration calculations. + * The radius of curvature of a spiral is larger than the circle of the same + * radius. + */ +double pmCircleEffectiveMinRadius(PmCircle const * circle) +{ + double dh2 = sq(circle->height / circle->angle); + + double r_spiral = spiralEffectiveRadius(circle); + + // Curvature of helix, assuming that helical motion is independent of plane motion + double effective_radius = dh2 / r_spiral + r_spiral; + + return effective_radius; +} + +/** + * Intermediate function to find the angle for a parameter from 0..1 along the + * spiral arc. + */ +int pmCircleAngleFromParam(PmCircle const * const circle, + SpiralArcLengthFit const * const fit, + double t, + double * const angle) +{ + if (fit->spiral_in) { + t = 1.0 - t; + } + //TODO error or cleanup input to prevent param outside 0..1 + double s_in = t * fit->total_planar_length; + + // Quadratic formula to invert arc length -> angle + + double A = fit->b0; + double B = fit->b1; + double C = -s_in; + + double disc = sq(B) - 4.0 * A * C ; + if (disc < 0) { + return -1; + } + + /* + * Stability of inverting the arc-length relationship. + * Since the b1 coefficient is analogous to arc radius, we can be + * reasonably assured that it will be large enough not to cause numerical + * errors. If this is not the case, then the arc itself is degenerate (very + * small radius), and this condition should be caught well before here. + * + * Since an arc with a very small spiral coefficient will have a small b0 + * coefficient in the fit, we use the Citardauq Formula to ensure that the + * positive root does not lose precision due to subtracting near-similar values. + * + * For more information, see: + * http://people.csail.mit.edu/bkph/articles/Quadratics.pdf + */ + + double angle_out = (2.0 * C) / ( -B - sqrt(disc)); + + if (fit->spiral_in) { + // Spiral fit assumes that we're spiraling out, so + // parameterize from opposite end + angle_out = circle->angle - angle_out; + } + + *angle = angle_out; + return 0; +} + +/** + * Compute the angle around a circular segment from the total progress along + * the curve. + */ +int pmCircleAngleFromProgress(PmCircle const * const circle, + SpiralArcLengthFit const * const fit, + double progress, + double * const angle) +{ + double s_end = hypot2(fit->total_planar_length, circle->height); + // Parameterize by total progress along helix + double t = progress / s_end; + return pmCircleAngleFromParam(circle, fit, t, angle); +} + +double pmCircle9VLimit(PmCircle9 const * const circle, double v_target, double v_limit_linear, double v_limit_angular) +{ + double u_linear = fmax(circle->xyz_ratio, circle->uvw_ratio); + if (u_linear > 1e-12) { + return fmin(v_target, v_limit_linear / u_linear); + } else if (circle->abc_ratio > 1e-12) { + return fmin(v_target, v_limit_angular / circle->abc_ratio); + } else { + return 0.0; + } +} diff --git a/src/emc/tp/pm_circle9.h b/src/emc/tp/pm_circle9.h new file mode 100644 index 00000000000..7cc9b302f75 --- /dev/null +++ b/src/emc/tp/pm_circle9.h @@ -0,0 +1,83 @@ +#ifndef PM_CIRCLE9_H +#define PM_CIRCLE9_H + +#include "posemath.h" +#include "emcpose.h" +#include "pm_line9.h" + +/** + * Spiral arc length approximation by quadratic fit. + */ +typedef struct { + double b0; /* 2nd order coefficient */ + double b1; /* 1st order coefficient */ + double total_planar_length; /* total arc length in plane */ + int spiral_in; /* flag indicating spiral is inward, + rather than outward */ +} SpiralArcLengthFit; + +typedef struct { + PmCircle xyz; + PmCartLine abc; + PmCartLine uvw; + SpiralArcLengthFit fit; + double xyz_ratio; + double abc_ratio; + double uvw_ratio; + double total_length; +} PmCircle9; + +typedef struct { + double v_max; + double acc_ratio; +} PmCircleLimits; + +double pmCircle9LengthAndRatios(PmCircle9 * const circ9); + +int pmCircle9Init(PmCircle9 * const circ9, + PmVector const * const start, + PmVector const * const end, + PmCartesian const * const center, + PmCartesian const * const normal, + int turn, double expected_angle_rad); + +double pmCircle9Length(PmCircle9 const * const circ9); + +int pmCircle9Point(PmCircle9 const * const circ9, double progress, PmVector * const point); + +int pmCircle9Cut(PmCircle9 * const circ, double new_pt_progress, SegmentToKeepType keep_pt); + +int pmCircle9TangentVector(PmCircle9 const * const circ9, double progress, PmVector * const out); + +int pmCircleTangentVector( + PmCircle const * const circle, + double angle_in, + PmCartesian * const out); + +PmCircleLimits pmCircleActualMaxVel( + PmCircle const * const circle, + double v_max3, + double a_max3); + +int findSpiralArcLengthFit(PmCircle const * const circle, + SpiralArcLengthFit * const fit, double angle_tolerance); + +double spiralEffectiveRadius(PmCircle const * circle); + +double pmCircleEffectiveMinRadius(const PmCircle *circle); + +int pmCircleAngleFromParam( + PmCircle const * const circle, + SpiralArcLengthFit const * const fit, + double t, + double * const angle); + +int pmCircleAngleFromProgress( + PmCircle const * const circle, + SpiralArcLengthFit const * const fit, + double progress, + double * const angle); + +double pmCircle9VLimit(const PmCircle9 * const circle, double v_target, double v_limit_linear, double v_limit_angular); + +#endif // PM_CIRCLE9_H diff --git a/src/emc/tp/pm_line9.c b/src/emc/tp/pm_line9.c new file mode 100644 index 00000000000..d5003fc959d --- /dev/null +++ b/src/emc/tp/pm_line9.c @@ -0,0 +1,120 @@ +/** + * @file pm_line9.c + * + * @author Robert W. Ellenberg + * + * @copyright Copyright 2019, Robert W. Ellenberg + * + * This source code is released for free distribution under the terms of the + * GNU General Public License (V2) as published by the Free Software Foundation. + */ + +#include "posemath.h" +#include "pm_line9.h" +#include "rtapi_bool.h" +#include "stddef.h" +#include "rtapi_math.h" + +static const PmCartesian zero={0,0,0}; + +int pmLine9Init( + PmLine9 * const line9, + PmVector const * const start, + PmVector const * const end) +{ + line9->start = *start; + line9->end = *end; + + VecVecSub(&line9->end, &line9->start, &line9->uVec); + line9->tmag = VecMag(&line9->uVec); + VecUnitEq(&line9->uVec); + return 0; +} + +double pmLine9Length(PmLine9 const * const line9) +{ + return line9->tmag; +} + +double pmLine9RotaryAxisLength(PmLine9 const * const line9) +{ + PmCartesian abc; + VecToCart(&line9->end, NULL, &abc, NULL); + PmCartesian abc_start; + VecToCart(&line9->start, NULL, &abc_start, NULL); + + pmCartCartSubEq(&abc, &abc_start); + double len=NAN; + pmCartMag(&abc, &len); + return len; +} + +double pmLine9LinearAxisLength(const PmLine9 * const line9) +{ + PmVector no_abc; + VecVecSub(&line9->end, &line9->start, &no_abc); + VecSetABC(&zero, &no_abc); + + return VecMag(&no_abc); +} + +int pmLine9Point(const PmLine9 * const line9, double progress, PmVector * const point) +{ + return + VecScalMult(&line9->uVec, progress, point) + || VecVecAddEq(point, &line9->start); +} + +int pmLine9Cut(PmLine9 * const line9, double cut_pt_progress, SegmentToKeepType keep_pt) +{ + switch (keep_pt) { + case KEEP_END_PT: + { + PmVector tmp; + VecScalMult(&line9->uVec, cut_pt_progress, &tmp); + VecVecAddEq(&line9->start, &tmp); + line9->tmag -= cut_pt_progress; + return 0; + } + case KEEP_START_PT: + VecScalMult(&line9->uVec, cut_pt_progress, &line9->end); + VecVecAddEq(&line9->end, &line9->start); + line9->tmag = cut_pt_progress; + return 0; + } + + return -1; +} + +static inline bool is_fuzz(double a) { + return fabs(a) < (CART_FUZZ) ? true : false; +} + +int pmCartLineCut(PmCartLine * const line, double cut_pt_progress, SegmentToKeepType keep_pt) +{ + switch (keep_pt) { + case KEEP_END_PT: + { + PmCartesian tmp; + pmCartScalMult(&line->uVec, cut_pt_progress, &tmp); + pmCartCartAddEq(&line->start, &tmp); + line->tmag -= cut_pt_progress; + line->tmag_zero = is_fuzz(line->tmag); + return 0; + } + case KEEP_START_PT: + pmCartScalMult(&line->uVec, cut_pt_progress, &line->end); + pmCartCartAddEq(&line->end, &line->start); + line->tmag = cut_pt_progress; + line->tmag_zero = is_fuzz(line->tmag); + return 0; + } + + return -1; +} + + +double pmLine9VLimit(PmLine9 const * const line9, double v_target, double v_limit_linear, double v_limit_angular) +{ + return VecVLimit(&line9->uVec, v_target, v_limit_linear, v_limit_angular); +} diff --git a/src/emc/tp/pm_line9.h b/src/emc/tp/pm_line9.h new file mode 100644 index 00000000000..11f2a9e9e8f --- /dev/null +++ b/src/emc/tp/pm_line9.h @@ -0,0 +1,36 @@ +#ifndef PM_LINE9_H +#define PM_LINE9_H + +#include "pm_vector.h" + +typedef struct { + PmVector start; + PmVector end; + PmVector uVec; + double tmag; +} PmLine9; + +typedef enum { + KEEP_START_PT, + KEEP_END_PT, +} SegmentToKeepType; + +int pmLine9Init(PmLine9 * const line9, + PmVector const * const start, + PmVector const * const end); + +double pmLine9Length(const PmLine9 * const line9); + +double pmLine9RotaryAxisLength(PmLine9 const * const line9); + +double pmLine9LinearAxisLength(PmLine9 const * const line9); + +int pmLine9Point(PmLine9 const * const line9, double progress, PmVector * const point); + +int pmLine9Cut(PmLine9 * const line, double cut_pt_progress, SegmentToKeepType keep_pt); + +int pmCartLineCut(PmCartLine * const line, double cut_pt_progress, SegmentToKeepType keep_pt); + +double pmLine9VLimit(const PmLine9 * const line9, double v_target, double v_limit_linear, double v_limit_angular); + +#endif // PM_LINE9_H diff --git a/src/emc/tp/rtapi_json5.h b/src/emc/tp/rtapi_json5.h new file mode 100644 index 00000000000..ea76f067686 --- /dev/null +++ b/src/emc/tp/rtapi_json5.h @@ -0,0 +1,111 @@ +/** + * @file rtapi_json5.h + * + * @author Robert W. Ellenberg + * @copyright Copyright (c) 2019 All rights reserved. This project is released under the GPL v2 license. + */ + +#ifndef RTAPI_JSON5_H +#define RTAPI_JSON5_H + +#include "rtapi.h" /* printing functions */ + +// Macro wrappers to stringify the variable name +// NOTE: callers can directly call the underlying print function if a custom field name is needed +#define print_json5_double(varname_) print_json5_double_(#varname_, (double)varname_) +#define print_json5_string(varname_) print_json5_string_(#varname_, varname_) +#define print_json5_long(varname_) print_json5_long_(#varname_, (long)varname_) +#define print_json5_int(varname_) print_json5_int_(#varname_, (int)varname_) +#define print_json5_unsigned(varname_) print_json5_unsigned_(#varname_, (unsigned)varname_) + +#define print_json5_double_field(base_, varname_) print_json5_double_(#varname_, (base_)->varname_) +#define print_json5_string_field(base_, varname_) print_json5_string_(#varname_, (base_)->varname_ ?:"NULL") +#define print_json5_int_field(base_, varname_) print_json5_int_(#varname_, (base_)->varname_) +#define print_json5_unsigned_field(base_, varname_) print_json5_unsigned_(#varname_, (base_)->varname_) +#define print_json5_long_field(base_, varname_) print_json5_long_(#varname_, (base_)->varname_) +#define print_json5_long_long_field(base_, varname_) print_json5_long_long_(#varname_, (base_)->varname_) + +// Hacky way to do realtime-logging of +// KLUDGE shove these in the header to avoid linking issues + +static inline void print_json5_start_() +{ + rtapi_print("{"); +} + +static inline void print_json5_end_() +{ + rtapi_print("}\n"); +} + +static inline void print_json5_fieldname_(const char *fname) +{ + rtapi_print("%s: ", fname ?: "NULL"); +} + +static inline void print_json5_array_start_(const char *arr_name) +{ + if (arr_name) { + rtapi_print("%s: [", arr_name); + } else { + + rtapi_print("["); + } +} + +static inline void print_json5_array_end_() +{ + rtapi_print("]\n"); +} + +static inline void print_json5_object_start_(const char *fname) +{ + if (fname) { + rtapi_print("%s: {", fname); + } else { + rtapi_print("{"); + } +} + +static inline void print_json5_object_end_() +{ + rtapi_print("}, "); +} + +static inline void print_json5_double_(const char *varname, double value) +{ + rtapi_print("%s: %0.17g, ", varname ?: "NO_NAME", value); +} + +static inline void print_json5_bool_(const char *varname, double value) +{ + rtapi_print("%s: %s, ", varname ?: "NO_NAME", value ? "true" : "false"); +} + +static inline void print_json5_int_(const char *varname, int value) +{ + rtapi_print("%s: %d, ", varname ?: "NO_NAME", value); +} + +static inline void print_json5_long_(const char *varname, long value) +{ + rtapi_print("%s: %ld, ", varname ?: "NO_NAME", value); +} + +static inline void print_json5_long_long_(const char *varname, long long value) +{ + rtapi_print("%s: %lld, ", varname ?: "NO_NAME", value); +} + +static inline void print_json5_unsigned_(const char *varname, unsigned value) +{ + rtapi_print("%s: %u, ", varname ?: "NO_NAME", value); +} + +static inline void print_json5_string_(const char *field_name, const char *value) +{ + // TODO handle nulls gracefully with proper JSON null + rtapi_print("%s: \"%s\", ", field_name ?: "NULL", value ?: "NULL"); +} + +#endif // RTAPI_JSON5_H diff --git a/src/emc/tp/spherical_arc.c b/src/emc/tp/spherical_arc.c index 2c347dcb211..2e7899b50f7 100644 --- a/src/emc/tp/spherical_arc.c +++ b/src/emc/tp/spherical_arc.c @@ -13,23 +13,34 @@ #include "posemath.h" #include "spherical_arc.h" -#include "tp_types.h" #include "rtapi_math.h" - +#include "tp_enums.h" #include "tp_debug.h" -int arcInitFromPoints(SphericalArc * const arc, PmCartesian const * const start, - PmCartesian const * const end, - PmCartesian const * const center) +int arcInitFromPoints( + SphericalArc * const arc, + PmCartesian const * const start, + PmCartesian const * const end, + PmCartesian const * const center, + PmCartesian const * const uTan, + double prev_line_length) { #ifdef ARC_PEDANTIC - if (!P0 || !P1 || !center) + if (!start || !end || !center) { return TP_ERR_MISSING_INPUT; - - if (!arc) + } + if (!arc) { return TP_ERR_MISSING_OUTPUT; + } #endif + if (prev_line_length < -1e-17) { + return TP_ERR_INVALID; + } + + arc->uTan = *uTan; + arc->line_length = fmax(prev_line_length, 0.0); + // Store the start, end, and center arc->start = *start; arc->end = *end; @@ -85,18 +96,19 @@ int arcInitFromPoints(SphericalArc * const arc, PmCartesian const * const start, int arcPoint(SphericalArc const * const arc, double progress, PmCartesian * const out) { - //TODO pedantic +#ifdef TP_PEDANTIC + if (!arc) {return TP_ERR_MISSING_INPUT;} + if (!out) {return TP_ERR_MISSING_OUTPUT;} +#endif //Convert progress to actual progress around the arc double net_progress = progress - arc->line_length; if (net_progress <= 0.0 && arc->line_length > 0) { - tc_debug_print("net_progress = %f, line_length = %f\n", net_progress, arc->line_length); //Get position on line (not actually an angle in this case) pmCartScalMult(&arc->uTan, net_progress, out); pmCartCartAdd(out, &arc->start, out); } else { double angle_in = net_progress / arc->radius; - tc_debug_print("angle_in = %f, angle_total = %f\n", angle_in, arc->angle); double scale0 = sin(arc->angle - angle_in) / arc->Sangle; double scale1 = sin(angle_in) / arc->Sangle; @@ -110,93 +122,33 @@ int arcPoint(SphericalArc const * const arc, double progress, PmCartesian * cons return TP_ERR_OK; } -int arcLength(SphericalArc const * const arc, double * const length) +double arcLength(SphericalArc const * const arc) { - *length = arc->radius * arc->angle + arc->line_length; - tp_debug_print("arc length = %g\n", *length); - return TP_ERR_OK; -} - -int arcFromLines(SphericalArc * const arc, PmCartLine const * const line1, - PmCartLine const * const line2, double radius, - double blend_dist, double center_dist, PmCartesian * const start, PmCartesian * const end, int consume) { - (void)radius; - - PmCartesian center, normal, binormal; - - // Pointer to middle point of line segment pair - PmCartesian const * const middle = &line1->end; - //TODO assert line1 end = line2 start? - - //Calculate the normal direction of the arc from the difference - //between the unit vectors - pmCartCartSub(&line2->uVec, &line1->uVec, &normal); - pmCartUnitEq(&normal); - pmCartScalMultEq(&normal, center_dist); - pmCartCartAdd(middle, &normal, ¢er); - - //Calculate the binormal (vector perpendicular to the plane of the - //arc) - pmCartCartCross(&line1->uVec, &line2->uVec, &binormal); - pmCartUnitEq(&binormal); - - // Start point is blend_dist away from middle point in the - // negative direction of line1 - pmCartScalMult(&line1->uVec, -blend_dist, start); - pmCartCartAdd(start, middle, start); - - // End point is blend_dist away from middle point in the positive - // direction of line2 - pmCartScalMult(&line2->uVec, blend_dist, end); - pmCartCartAddEq(end, middle); - - //Handle line portion of line-arc - arc->uTan = line1->uVec; - if (consume) { - arc->line_length = line1->tmag - blend_dist; - } else { - arc->line_length = 0; - } - - return arcInitFromPoints(arc, start, end, ¢er); + return arc->radius * arc->angle + arc->line_length; } -int arcConvexTest(PmCartesian const * const center, - PmCartesian const * const P, PmCartesian const * const uVec, int reverse_dir) +int arcTangent(SphericalArc const * const arc, double const t, PmCartesian * const out) { - //Check if an arc-line intersection is concave or convex - double dot; - PmCartesian diff; - pmCartCartSub(P, center, &diff); - pmCartCartDot(&diff, uVec, &dot); + if (!arc || !out) { + return TP_ERR_MISSING_INPUT; + } - tp_debug_print("convex test: dot = %f, reverse_dir = %d\n", dot, reverse_dir); - int convex = (reverse_dir != 0) ^ (dot < 0); - return convex; -} + // This section implements the derivative of the SLERP formula to get a + // local tangent vector. + const double theta = arc->angle; + const double k = theta / arc->Sangle; + const double k0 = -cos( (1.0 - t) * theta); + const double k1 = cos( t * theta); -int arcTangent(SphericalArc const * const arc, PmCartesian * const tan, int at_end) -{ - PmCartesian r_perp; - PmCartesian r_tan; + PmCartesian dp0,dp1; - if (at_end) { - r_perp = arc->rEnd; - } else { - r_perp = arc->rStart; - } + // Ugly sequence to build up tangent vector from components of the derivative + pmCartScalMult(&arc->rStart, k * k0, &dp0); + pmCartScalMult(&arc->rEnd, k * k1, &dp1); + pmCartCartAdd(&dp0, &dp1, out); - pmCartCartCross(&arc->binormal, &r_perp, &r_tan); - //Get spiral component - double dr = arc->spiral / arc->angle; - - //Get perpendicular component due to spiral - PmCartesian d_perp; - pmCartUnit(&r_perp, &d_perp); - pmCartScalMultEq(&d_perp, dr); - //TODO error checks - pmCartCartAdd(&d_perp, &r_tan, tan); - pmCartUnitEq(tan); + // tangential vector complete, now normalize + pmCartUnitEq(out); return TP_ERR_OK; } diff --git a/src/emc/tp/spherical_arc.h b/src/emc/tp/spherical_arc.h index cea336909b9..ccb68d754bb 100644 --- a/src/emc/tp/spherical_arc.h +++ b/src/emc/tp/spherical_arc.h @@ -18,9 +18,6 @@ #define ARC_POS_EPSILON 1e-12 #define ARC_MIN_RADIUS 1e-12 #define ARC_MIN_ANGLE 1e-6 -//FIXME relate this to cornering acceleration? -#define ARC_ABS_ERR 5e-4 -#define ARC_REL_ERR 5e-4 typedef struct { // Three defining points for the arc @@ -33,7 +30,6 @@ typedef struct { PmCartesian rEnd; PmCartesian uTan; /* Tangent vector at start of arc (copied from prev. tangent line)*/ - PmCartesian binormal; double radius; double spiral; // Angle that the arc encloses @@ -42,26 +38,14 @@ typedef struct { double line_length; } SphericalArc; - -int arcInitFromPoints(SphericalArc * const arc, PmCartesian const * const start, - PmCartesian const * const end, PmCartesian const * const center); - -int arcInitFromVectors(SphericalArc * const arc, PmCartesian const * const vec0, - PmCartesian const * const vec1, - PmCartesian const * const center); +int arcInitFromPoints(SphericalArc * const arc, + PmCartesian const * const start, + PmCartesian const * const end, + PmCartesian const * const center, const PmCartesian * const uTan, double prev_line_length); int arcPoint(SphericalArc const * const arc, double angle_in, PmCartesian * const out); -int arcNormalizedSlerp(SphericalArc const * const arc, double t, PmCartesian * const out); - -int arcLength(SphericalArc const * const arc, double * const length); - -int arcFromLines(SphericalArc * const arc, PmCartLine const * const line1, - PmCartLine const * const line2, double radius, - double blend_dist, double center_dist, PmCartesian * const start, PmCartesian * const end, int consume); - -int arcConvexTest(PmCartesian const * const center, - PmCartesian const * const P, PmCartesian const * const uVec, int reverse_dir); +double arcLength(SphericalArc const * const arc); -int arcTangent(SphericalArc const * const arc, PmCartesian * const tan, int at_end); +int arcTangent(SphericalArc const * const arc, const double t, PmCartesian * const out); #endif diff --git a/src/emc/tp/spherical_arc9.c b/src/emc/tp/spherical_arc9.c new file mode 100644 index 00000000000..baecc0cbc68 --- /dev/null +++ b/src/emc/tp/spherical_arc9.c @@ -0,0 +1,147 @@ +/** + * @file spherical_arc9.c + * + * API for 9D version of "spherical arc" (an arc on the surface of a 9D sphere) + * + * @author Robert W. Ellenberg + * + * @copyright Copyright 2019, Robert W. Ellenberg + * + * This source code is released for free distribution under the terms of the + * GNU General Public License (V2) as published by the Free Software Foundation. + */ + +#include "spherical_arc9.h" +#include "posemath.h" +#include "pm_vector.h" +#include "rtapi_math.h" +#include "tp_enums.h" + +int arc9InitFromPoints( + SphericalArc9 * const arc, + PmVector const * const start, + PmVector const * const end, + PmVector const * const center, + PmVector const * const uTan, + double prev_line_length) +{ +#ifdef ARC_PEDANTIC + if (!start || !end || !center) { + return TP_ERR_MISSING_INPUT; + } + if (!arc) { + return TP_ERR_MISSING_OUTPUT; + } +#endif + + if (prev_line_length < -1e-17) { + return TP_ERR_INVALID; + } + + arc->uTan = *uTan; + arc->line_length = fmax(prev_line_length, 0.0); + + // Store the start, end, and center + arc->start = *start; + arc->end = *end; + arc->center = *center; + + VecVecSub(start, center, &arc->rStart); + VecVecSub(end, center, &arc->rEnd); + + // Find the radii at start and end. These are identical for a perfect spherical arc + double radius0 = VecMag(&arc->rStart); + double radius1 = VecMag(&arc->rEnd); + + if (radius0 < ARC_MIN_RADIUS || radius1 < ARC_MIN_RADIUS) { + return TP_ERR_RADIUS_TOO_SMALL; + } + + // Choose initial radius as nominal radius + arc->radius = radius0; + + // Get unit vectors from center to start and center to end + PmVector u0, u1; + VecScalMult(&arc->rStart, 1.0 / radius0, &u0); + VecScalMult(&arc->rEnd, 1.0 / radius1, &u1); + + // Find arc angle + double dot = VecVecDot(&u0, &u1); + arc->angle = acos(dot); + + // Store spiral factor as radial difference. Archimedean spiral coef. a = spiral / angle + arc->spiral = (radius1 - radius0 ); + + if (arc->angle < ARC_MIN_ANGLE) { + return TP_ERR_GEOM; + } + + // Store sin of arc angle since it is reused many times for SLERP + arc->Sangle = sin(arc->angle); + + return TP_ERR_OK; +} + +int arc9Point(SphericalArc9 const * const arc, double progress, PmVector * const out) +{ +#ifdef ARC_PEDANTIC + if (!arc) {return TP_ERR_MISSING_INPUT;} + if (!out) {return TP_ERR_MISSING_OUTPUT;} +#endif + + //Convert progress to actual progress around the arc + double net_progress = progress - arc->line_length; + if (net_progress <= 0.0 && arc->line_length > 0) { + //Get position on line (not actually an angle in this case) + VecScalMult(&arc->uTan, net_progress, out); + VecVecAdd(out, &arc->start, out); + } else { + double angle_in = net_progress / arc->radius; + double scale0 = sin(arc->angle - angle_in) / arc->Sangle; + double scale1 = sin(angle_in) / arc->Sangle; + + PmVector interp0,interp1; + VecScalMult(&arc->rStart, scale0, &interp0); + VecScalMult(&arc->rEnd, scale1, &interp1); + + VecVecAdd(&interp0, &interp1, out); + VecVecAdd(&arc->center, out, out); + } + return TP_ERR_OK; +} + +double arc9Length(SphericalArc9 const * const arc) +{ + return arc->radius * arc->angle + arc->line_length; +} + +int arc9Tangent(SphericalArc9 const * const arc, double const t, PmVector * const out) +{ + if (!arc || !out) { + return TP_ERR_MISSING_INPUT; + } + + // This section implements the derivative of the SLERP formula to get a + // local tangent vector. + const double theta = arc->angle; + const double k = theta / arc->Sangle; + const double k0 = -cos( (1.0 - t) * theta); + const double k1 = cos( t * theta); + + PmVector dp0,dp1; + + // Ugly sequence to build up tangent vector from components of the derivative + VecScalMult(&arc->rStart, k * k0, &dp0); + VecScalMult(&arc->rEnd, k * k1, &dp1); + VecVecAdd(&dp0, &dp1, out); + + // tangential vector complete, now normalize + VecUnitEq(out); + + return TP_ERR_OK; +} + +double arc9VLimit(const SphericalArc9 * const arc, double v_target, double v_limit_linear, double v_limit_angular) +{ + return VecVLimit(&arc->uTan, v_target, v_limit_linear, v_limit_angular); +} diff --git a/src/emc/tp/spherical_arc9.h b/src/emc/tp/spherical_arc9.h new file mode 100644 index 00000000000..485bbde7cce --- /dev/null +++ b/src/emc/tp/spherical_arc9.h @@ -0,0 +1,44 @@ +#ifndef SPHERICAL_ARC9_H +#define SPHERICAL_ARC9_H + +#include "pm_vector.h" + +#define ARC_POS_EPSILON 1e-12 +#define ARC_MIN_RADIUS 1e-12 +#define ARC_MIN_ANGLE 1e-6 + +typedef struct { + // Three defining points for the arc + PmVector start; + PmVector end; + PmVector center; + // Relative vectors from center to start and center to end + // These are cached here since they'll be reused during SLERP + PmVector rStart; + PmVector rEnd; + PmVector uTan; /* Tangent vector at start of arc (copied from + prev. tangent line)*/ + double radius; + double spiral; + // Angle that the arc encloses + double angle; + double Sangle; + double line_length; +} SphericalArc9; + +int arc9InitFromPoints( + SphericalArc9 * const arc, + PmVector const * const start, + PmVector const * const end, + PmVector const * const center, + const PmVector * const uTan, + double prev_line_length); + +int arc9Point(SphericalArc9 const * const arc, double angle_in, PmVector * const out); + +double arc9Length(SphericalArc9 const * const arc); + +int arc9Tangent(SphericalArc9 const * const arc, const double t, PmVector * const out); + +double arc9VLimit(SphericalArc9 const * const arc, double v_target, double v_limit_linear, double v_limit_angular); +#endif // SPHERICAL_ARC9_H diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index 578db4767ec..dee6d6e59e1 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -22,50 +22,60 @@ #include "tc.h" #include "tp_types.h" #include "spherical_arc.h" +#include "spherical_arc9.h" #include "motion_types.h" //Debug output #include "tp_debug.h" +#include "tp_enums.h" +#include "tp_call_wrappers.h" -double tcGetMaxTargetVel(TC_STRUCT const * const tc, - double max_scale) +double tcGetMaxVelFromLength(TC_STRUCT const * const tc) { - double v_max_target; + double sample_maxvel = tc->target / (tc->cycle_time * TP_MIN_SEGMENT_CYCLES); + return fmin(tc->maxvel_geom, sample_maxvel); +} + +/** + * Planning-time maximum velocity for the given segment. + * This function ignores runtime conditions like if a segment is on final deceleration, actively blending, etc. + */ +double tcGetPlanMaxTargetVel( + TC_STRUCT const * const tc, + double max_feed_scale) +{ + double v_max = tcGetMaxVelFromLength(tc); + double v_max_target = v_max; switch (tc->synchronized) { case TC_SYNC_NONE: // Get maximum reachable velocity from max feed override - v_max_target = tc->reqvel * max_scale; + v_max_target = tc->reqvel * max_feed_scale; break; case TC_SYNC_VELOCITY: //Fallthrough - max_scale = 1.0; - /* Fallthrough */ case TC_SYNC_POSITION: // Assume no spindle override during blend target - default: - v_max_target = tc->maxvel; + // max feed override here is variability of the spindle speed, rather than a user feed override + v_max_target = tc->uu_per_rev * tc->tag.speed * max_feed_scale; break; } // Clip maximum velocity by the segment's own maximum velocity - return fmin(v_max_target, tc->maxvel); + return fmin(v_max_target, v_max); } -double tcGetOverallMaxAccel(const TC_STRUCT *tc) +double tcGetAccelScale(const TC_STRUCT *tc) { // Handle any acceleration reduction due to an approximate-tangent "blend" with the previous or next segment double a_scale = (1.0 - fmax(tc->kink_accel_reduce, tc->kink_accel_reduce_prev)); + return a_scale; +} - // Parabolic blending conditions: If the next segment or previous segment - // has a parabolic blend with this one, acceleration is scaled down by 1/2 - // so that the sum of the two does not exceed the maximum. - if (tc->blend_prev || TC_TERM_COND_PARABOLIC == tc->term_cond) { - a_scale *= 0.5; - } - - return tc->maxaccel * a_scale; +double tcGetOverallMaxAccel(const TC_STRUCT *tc) +{ + return tc->maxaccel * tcGetAccelScale(tc); } /** @@ -73,145 +83,29 @@ double tcGetOverallMaxAccel(const TC_STRUCT *tc) */ double tcGetTangentialMaxAccel(TC_STRUCT const * const tc) { - double a_scale = tcGetOverallMaxAccel(tc); + double a_scale = tcGetAccelScale(tc); // Reduce allowed tangential acceleration in circular motions to stay // within overall limits (accounts for centripetal acceleration while // moving along the circular path). if (tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_SPHERICAL) { - //Limit acceleration for circular arcs to allow for normal acceleration + //Limit acceleration for cirular arcs to allow for normal acceleration a_scale *= tc->acc_ratio_tan; } - return a_scale; + return tc->maxaccel * a_scale; } int tcSetKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc, double kink_vel, double accel_reduction) { + // NOTE: use_kink field is not set until later, if we choose tangent blending without an arc connector prev_tc->kink_vel = kink_vel; - // prev_tc->kink_accel_reduce = fmax(accel_reduction, prev_tc->kink_accel_reduce); tc->kink_accel_reduce_prev = fmax(accel_reduction, tc->kink_accel_reduce_prev); return 0; } -int tcInitKinkProperties(TC_STRUCT *tc) -{ - tc->kink_vel = -1.0; - tc->kink_accel_reduce = 0.0; - tc->kink_accel_reduce_prev = 0.0; - return 0; -} - -int tcRemoveKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc) -{ - prev_tc->kink_vel = -1.0; - prev_tc->kink_accel_reduce = 0.0; - tc->kink_accel_reduce_prev = 0.0; - return 0; -} - - -int tcCircleStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) -{ - PmCartesian startpoint; - PmCartesian radius; - PmCartesian tan, perp; - - pmCirclePoint(&tc->coords.circle.xyz, 0.0, &startpoint); - pmCartCartSub(&startpoint, &tc->coords.circle.xyz.center, &radius); - pmCartCartCross(&tc->coords.circle.xyz.normal, &radius, &tan); - pmCartUnitEq(&tan); - //The unit vector's actual direction is adjusted by the normal - //acceleration here. This unit vector is NOT simply the tangent - //direction. - pmCartCartSub(&tc->coords.circle.xyz.center, &startpoint, &perp); - pmCartUnitEq(&perp); - - pmCartScalMult(&tan, tcGetOverallMaxAccel(tc), &tan); - pmCartScalMultEq(&perp, pmSq(0.5 * tc->reqvel)/tc->coords.circle.xyz.radius); - pmCartCartAdd(&tan, &perp, out); - pmCartUnitEq(out); - return 0; -} - -int tcCircleEndAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) -{ - PmCartesian endpoint; - PmCartesian radius; - - pmCirclePoint(&tc->coords.circle.xyz, tc->coords.circle.xyz.angle, &endpoint); - pmCartCartSub(&endpoint, &tc->coords.circle.xyz.center, &radius); - pmCartCartCross(&tc->coords.circle.xyz.normal, &radius, out); - pmCartUnitEq(out); - return 0; -} - -/** - * Get the acceleration direction unit vector for blend velocity calculations. - * This calculates the direction of acceleration at the start of a segment. - */ -int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) { - - switch (tc->motion_type) { - case TC_LINEAR: - case TC_RIGIDTAP: - *out=tc->coords.line.xyz.uVec; - break; - case TC_CIRCULAR: - tcCircleStartAccelUnitVector(tc,out); - break; - case TC_SPHERICAL: - return -1; - default: - return -1; - } - return 0; -} - -/** - * Get the acceleration direction unit vector for blend velocity calculations. - * This calculates the direction of acceleration at the end of a segment. - */ -int tcGetEndAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) { - - switch (tc->motion_type) { - case TC_LINEAR: - *out=tc->coords.line.xyz.uVec; - break; - case TC_RIGIDTAP: - pmCartScalMult(&tc->coords.line.xyz.uVec, -1.0, out); - break; - case TC_CIRCULAR: - tcCircleEndAccelUnitVector(tc,out); - break; - case TC_SPHERICAL: - return -1; - default: - return -1; - } - return 0; -} - -int tcGetIntersectionPoint(TC_STRUCT const * const prev_tc, - TC_STRUCT const * const tc, PmCartesian * const point) -{ - // TODO NULL pointer check? - // Get intersection point from geometry - if (tc->motion_type == TC_LINEAR) { - *point = tc->coords.line.xyz.start; - } else if (prev_tc->motion_type == TC_LINEAR) { - *point = prev_tc->coords.line.xyz.end; - } else if (tc->motion_type == TC_CIRCULAR){ - pmCirclePoint(&tc->coords.circle.xyz, 0.0, point); - } else { - return TP_ERR_FAIL; - } - return TP_ERR_OK; -} - - /** * Check if a segment can be consumed without disrupting motion or synced IO. */ @@ -221,115 +115,102 @@ int tcCanConsume(TC_STRUCT const * const tc) return false; } - if (tc->syncdio.anychanged || tc->blend_prev || tc->atspeed) { + if (tc->motion_type != TC_LINEAR) { + return false; + } + + if (tc->syncdio.anychanged || tc->atspeed) { //TODO add other conditions here (for any segment that should not be consumed by blending return false; } return true; - } /** - * Find the geometric tangent vector to a helical arc. - * Unlike the acceleration vector, the result of this calculation is a vector - * tangent to the helical arc. This is called by wrapper functions for the case of a circular or helical arc. + * Calulate the unit tangent vector at the start of a move for any segment. */ -int pmCircleTangentVector(PmCircle const * const circle, - double angle_in, PmCartesian * const out) +int tcGetStartTangentUnitVector(TC_STRUCT const * const tc, PmVector * const out) { - - PmCartesian startpoint; - PmCartesian radius; - PmCartesian uTan, dHelix, dRadial; - - // Get vector in radial direction - pmCirclePoint(circle, angle_in, &startpoint); - pmCartCartSub(&startpoint, &circle->center, &radius); - - /* Find local tangent vector using planar normal. Assuming a differential - * angle dtheta, the tangential component of the tangent vector is r * - * dtheta. Since we're normalizing the vector anyway, assume dtheta = 1. - */ - pmCartCartCross(&circle->normal, &radius, &uTan); - - /* the binormal component of the tangent vector is (dz / dtheta) * dtheta. - */ - double dz = 1.0 / circle->angle; - pmCartScalMult(&circle->rHelix, dz, &dHelix); - - pmCartCartAddEq(&uTan, &dHelix); - - /* The normal component is (dr / dtheta) * dtheta. - */ - double dr = circle->spiral / circle->angle; - pmCartUnit(&radius, &dRadial); - pmCartScalMultEq(&dRadial, dr); - pmCartCartAddEq(&uTan, &dRadial); - - //Normalize final output vector - pmCartUnit(&uTan, out); - return 0; + // TODO Optimize + return tcGetTangentUnitVector(tc, 0, out); } - /** - * Calculate the unit tangent vector at the start of a move for any segment. + * Calculate the unit tangent vector at the end of a move for any segment. */ -int tcGetStartTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) { +int tcGetEndTangentUnitVector( + TC_STRUCT const * const tc, + PmVector * const out) { + // TODO Optimize + return tcGetTangentUnitVector(tc, tc->target, out); +} + +int tcGetTangentUnitVector(TC_STRUCT const * const tc, double progress, PmVector * const out) { + static const PmCartesian zero={0,0,0}; switch (tc->motion_type) { case TC_LINEAR: - *out=tc->coords.line.xyz.uVec; - break; + *out = tc->coords.line.uVec; + return 0; case TC_RIGIDTAP: - *out=tc->coords.rigidtap.xyz.uVec; - break; - case TC_CIRCULAR: - pmCircleTangentVector(&tc->coords.circle.xyz, 0.0, out); - break; - default: - rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n",tc->motion_type); + // Augment with zero vector, no re-scaling necessary + return CartToVec(&tc->coords.rigidtap.nominal_xyz.uVec, &zero, &zero, out); + case TC_DWELL: return -1; + case TC_CIRCULAR: + { + return pmCircle9TangentVector( + &tc->coords.circle, + progress, + out); + } + case TC_SPHERICAL: + { + double t = progress / tc->target; + return arc9Tangent(&tc->coords.arc, t, out); + } } - return 0; + rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n",tc->motion_type); + return TP_ERR_FAIL; } -/** - * Calculate the unit tangent vector at the end of a move for any segment. - */ -int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) { - +double tcGetVLimit(TC_STRUCT const * const tc, double v_target, double v_limit_linear, double v_limit_angular) +{ + if ((tc->synchronized == TC_SYNC_POSITION)){ + // No limits applied during position sync + return v_target; + } switch (tc->motion_type) { - case TC_LINEAR: - *out=tc->coords.line.xyz.uVec; - break; - case TC_RIGIDTAP: - pmCartScalMult(&tc->coords.rigidtap.xyz.uVec, -1.0, out); - break; - case TC_CIRCULAR: - pmCircleTangentVector(&tc->coords.circle.xyz, - tc->coords.circle.xyz.angle, out); - break; - default: - rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n",tc->motion_type); - return -1; + case TC_LINEAR: + return pmLine9VLimit(&tc->coords.line, v_target, v_limit_linear, v_limit_angular); + case TC_CIRCULAR: + return pmCircle9VLimit(&tc->coords.circle, v_target, v_limit_linear, v_limit_angular); + case TC_SPHERICAL: + return arc9VLimit(&tc->coords.arc, v_target, v_limit_linear, v_limit_angular); + case TC_RIGIDTAP: + case TC_DWELL: + break; + // for the non-synched portion of rigid tapping } - return 0; + return fmin(v_target, v_limit_linear); } - - /** * Calculate the distance left in the trajectory segment in the indicated * direction. */ double tcGetDistanceToGo(TC_STRUCT const * const tc, int direction) { - double distance = tcGetTarget(tc, direction) - tc->progress; - if (direction == TC_DIR_REVERSE) { - distance *=-1.0; + double distance; + if (direction == TC_DIR_FORWARD) { + // Return standard distance to go + distance = tc->target - tc->progress; + } else { + // Reverse direction, distance from zero instead of target + distance = tc->progress; } + return distance; } @@ -338,7 +219,6 @@ double tcGetTarget(TC_STRUCT const * const tc, int direction) return (direction == TC_DIR_REVERSE) ? 0.0 : tc->target; } - /*! tcGetPos() function * * \brief This function calculates the machine position along the motion's path. @@ -351,267 +231,117 @@ double tcGetTarget(TC_STRUCT const * const tc, int direction) * * @param tc the current TC that is being planned * - * @return EmcPose returns a position (\ref EmcPose = datatype carrying XYZABC information + * @return PmVector returns a position (\ref PmVector = datatype carrying XYZABC information */ -int tcGetPos(TC_STRUCT const * const tc, EmcPose * const out) { - tcGetPosReal(tc, TC_GET_PROGRESS, out); +int tcGetPos(TC_STRUCT const * const tc, PmVector * const out) { + tcGetPosReal(tc, tc->progress, out); return 0; } -int tcGetStartpoint(TC_STRUCT const * const tc, EmcPose * const out) { - tcGetPosReal(tc, TC_GET_STARTPOINT, out); - return 0; +int tcGetStartpoint(TC_STRUCT const * const tc, PmVector * const pos) +{ + PmCartesian xyz; + PmCartesian abc; + PmCartesian uvw; + + switch (tc->motion_type){ + case TC_RIGIDTAP: + xyz = tc->coords.rigidtap.nominal_xyz.start; + abc = tc->coords.rigidtap.abc; + uvw = tc->coords.rigidtap.uvw; + return CartToVec(&xyz, &abc, &uvw, pos); + case TC_LINEAR: + *pos = tc->coords.line.start; + return 0; + case TC_CIRCULAR: + pmCircleStartPoint(&tc->coords.circle.xyz, &xyz); + abc = tc->coords.circle.abc.start; + uvw = tc->coords.circle.uvw.start; + return CartToVec(&xyz, &abc, &uvw, pos); + case TC_SPHERICAL: + return arc9Point(&tc->coords.arc, + 0, + pos); + case TC_DWELL: + *pos = tc->coords.dwell.dwell_pos; + return TP_ERR_OK; + } + + return TP_ERR_FAIL; } -int tcGetEndpoint(TC_STRUCT const * const tc, EmcPose * const out) { - tcGetPosReal(tc, TC_GET_ENDPOINT, out); +int tcGetEndpoint(TC_STRUCT const * const tc, PmVector * const out) { + tcGetPosReal(tc, tc->target, out); return 0; } -int tcGetPosReal(TC_STRUCT const * const tc, int of_point, EmcPose * const pos) +int tcGetPosReal(TC_STRUCT const * const tc, double progress, PmVector * const pos) { PmCartesian xyz; PmCartesian abc; PmCartesian uvw; - double progress=0.0; - - switch (of_point) { - case TC_GET_PROGRESS: - progress = tc->progress; - break; - case TC_GET_ENDPOINT: - progress = tc->target; - break; - case TC_GET_STARTPOINT: - progress = 0.0; - break; - } - - - // Used for arc-length to angle conversion with spiral segments - double angle = 0.0; - int res_fit = TP_ERR_OK; switch (tc->motion_type){ case TC_RIGIDTAP: - if(tc->coords.rigidtap.state > REVERSING) { - pmCartLinePoint(&tc->coords.rigidtap.aux_xyz, progress, &xyz); - } else { - pmCartLinePoint(&tc->coords.rigidtap.xyz, progress, &xyz); - } + pmCartLinePoint(&tc->coords.rigidtap.actual_xyz, progress, &xyz); // no rotary move allowed while tapping abc = tc->coords.rigidtap.abc; uvw = tc->coords.rigidtap.uvw; - break; + return CartToVec(&xyz, &abc, &uvw, pos); case TC_LINEAR: - pmCartLinePoint(&tc->coords.line.xyz, - progress * tc->coords.line.xyz.tmag / tc->target, - &xyz); - pmCartLinePoint(&tc->coords.line.uvw, - progress * tc->coords.line.uvw.tmag / tc->target, - &uvw); - pmCartLinePoint(&tc->coords.line.abc, - progress * tc->coords.line.abc.tmag / tc->target, - &abc); - break; + return pmLine9Point(&tc->coords.line, progress, pos); case TC_CIRCULAR: - res_fit = pmCircleAngleFromProgress(&tc->coords.circle.xyz, - &tc->coords.circle.fit, - progress, &angle); - pmCirclePoint(&tc->coords.circle.xyz, - angle, - &xyz); - pmCartLinePoint(&tc->coords.circle.abc, - progress * tc->coords.circle.abc.tmag / tc->target, - &abc); - pmCartLinePoint(&tc->coords.circle.uvw, - progress * tc->coords.circle.uvw.tmag / tc->target, - &uvw); - break; + return pmCircle9Point(&tc->coords.circle, progress, pos); case TC_SPHERICAL: - arcPoint(&tc->coords.arc.xyz, + return arc9Point(&tc->coords.arc, progress, - &xyz); - abc = tc->coords.arc.abc; - uvw = tc->coords.arc.uvw; - break; - } - - if (res_fit == TP_ERR_OK) { - // Don't touch pos unless we know the value is good - pmCartesianToEmcPose(&xyz, &abc, &uvw, pos); - } - return res_fit; -} - - -/** - * Set the terminal condition of a segment. - * This function will eventually handle state changes associated with altering a terminal condition. - */ -int tcSetTermCond(TC_STRUCT *prev_tc, TC_STRUCT *tc, int term_cond) { - switch (term_cond) { - case TC_TERM_COND_STOP: - case TC_TERM_COND_EXACT: - case TC_TERM_COND_TANGENT: - if (tc) {tc->blend_prev = 0;} - break; - case TC_TERM_COND_PARABOLIC: - if (tc) {tc->blend_prev = 1;} - break; - default: - break; - - } - if (prev_tc) { - tp_debug_print("setting term condition %d on tc id %d, type %d\n", term_cond, prev_tc->id, prev_tc->motion_type); - prev_tc->term_cond = term_cond; + pos); + case TC_DWELL: + *pos = tc->coords.dwell.dwell_pos; + return TP_ERR_OK; } - return 0; + return -1; } /** - * Connect a blend arc to the two line segments it blends. - * Starting with two adjacent line segments, this function shortens each - * segment to connect them with the newly created blend arc. The "previous" - * segment gets a new end point, while the next segment gets a new start point. - * After the operation is complete the result is a set of 3 connected segments - * (line-arc-line). + * Set the terminal condition (i.e. blend or stop) for the given motion segment. + * Also sets flags on the next segment relevant to blending (e.g. parabolic blend sets the blend_prev flag). */ -int tcConnectBlendArc(TC_STRUCT * const prev_tc, TC_STRUCT * const tc, - PmCartesian const * const circ_start, - PmCartesian const * const circ_end) { - - /* Only shift XYZ for now*/ - if (prev_tc) { - tp_debug_print("connect: keep prev_tc\n"); - //Have prev line, need to shorten it - pmCartLineInit(&prev_tc->coords.line.xyz, - &prev_tc->coords.line.xyz.start, circ_start); - tp_debug_print("Old target = %f\n", prev_tc->target); - prev_tc->target = prev_tc->coords.line.xyz.tmag; - tp_debug_print("Target = %f\n",prev_tc->target); - //Setup tangent blending constraints - tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); - tp_debug_print(" L1 end : %f %f %f\n",prev_tc->coords.line.xyz.end.x, - prev_tc->coords.line.xyz.end.y, - prev_tc->coords.line.xyz.end.z); - } else { - tp_debug_print("connect: consume prev_tc\n"); +int tcSetTermCond(TC_STRUCT *tc, tc_term_cond_t term_cond) +{ + if (!tc) { + return -1; } - //Shorten next line - pmCartLineInit(&tc->coords.line.xyz, circ_end, &tc->coords.line.xyz.end); - - tp_info_print(" L2: old target = %f\n", tc->target); - tc->target = tc->coords.line.xyz.tmag; - tp_info_print(" L2: new target = %f\n", tc->target); - tp_debug_print(" L2 start : %f %f %f\n",tc->coords.line.xyz.start.x, - tc->coords.line.xyz.start.y, - tc->coords.line.xyz.start.z); - - tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); - - tp_info_print(" Q1: %f %f %f\n",circ_start->x,circ_start->y,circ_start->z); - tp_info_print(" Q2: %f %f %f\n",circ_end->x,circ_end->y,circ_end->z); - + tc->term_cond = term_cond; return 0; } - -/** - * Check if the current segment is actively blending. - * Checks if a blend should start based on acceleration and velocity criteria. - * Also saves this status so that the blend continues until the segment is - * done. - */ -int tcIsBlending(TC_STRUCT * const tc) { - //FIXME Disabling blends for rigid tap cycle until changes can be verified. - int is_blending_next = (tc->term_cond == TC_TERM_COND_PARABOLIC ) && - tc->on_final_decel && (tc->currentvel < tc->blend_vel) && - tc->motion_type != TC_RIGIDTAP; - - //Latch up the blending_next status here, so that even if the prev conditions - //aren't necessarily true we still blend to completion once the blend - //starts. - tc->blending_next |= is_blending_next; - - return tc->blending_next; -} - int tcFindBlendTolerance(TC_STRUCT const * const prev_tc, - TC_STRUCT const * const tc, double * const T_blend, double * const nominal_tolerance) + TC_STRUCT const * const tc, double * const T_blend) { const double tolerance_ratio = 0.25; double T1 = prev_tc->tolerance; double T2 = tc->tolerance; //Detect zero tolerance = no tolerance and force to reasonable maximum - if (T1 == 0) { + if (T1 <= 0) { T1 = prev_tc->nominal_length * tolerance_ratio; } - if (T2 == 0) { + if (T2 <= 0) { T2 = tc->nominal_length * tolerance_ratio; } - *nominal_tolerance = fmin(T1,T2); + double nominal_tolerance = fmin(T1,T2); //Blend tolerance is the limit of what we can reach by blending alone, //consuming half a segment or less (parabolic equivalent) - double blend_tolerance = fmin(fmin(*nominal_tolerance, + double blend_tolerance = fmin(fmin(nominal_tolerance, prev_tc->nominal_length * tolerance_ratio), tc->nominal_length * tolerance_ratio); *T_blend = blend_tolerance; return 0; } - -/** - * Check for early stop conditions. - * If a variety of conditions are true, then we can't do blending as we expect. - * This function checks for any conditions that force us to stop on the current - * segment. This is different from pausing or aborting, which can happen any - * time. - */ -int tcFlagEarlyStop(TC_STRUCT * const tc, - TC_STRUCT * const nexttc) -{ - - if (!tc || !nexttc) { - return TP_ERR_NO_ACTION; - } - - if(tc->synchronized != TC_SYNC_POSITION && nexttc->synchronized == TC_SYNC_POSITION) { - // we'll have to wait for spindle sync; might as well - // stop at the right place (don't blend) - tp_debug_print("waiting on spindle sync for tc %d\n", tc->id); - tcSetTermCond(tc, nexttc, TC_TERM_COND_STOP); - } - - if(nexttc->atspeed) { - // we'll have to wait for the spindle to be at-speed; might as well - // stop at the right place (don't blend), like above - // FIXME change the values so that 0 is exact stop mode - tp_debug_print("waiting on spindle atspeed for tc %d\n", tc->id); - tcSetTermCond(tc, nexttc, TC_TERM_COND_STOP); - } - - return TP_ERR_OK; -} - -double pmLine9Target(PmLine9 * const line9) -{ - if (!line9->xyz.tmag_zero) { - return line9->xyz.tmag; - } else if (!line9->uvw.tmag_zero) { - return line9->uvw.tmag; - } else if (!line9->abc.tmag_zero) { - return line9->abc.tmag; - } else { - return 0.0; - } -} - - /** * Initialize a new trajectory segment with common parameters. * @@ -619,7 +349,7 @@ double pmLine9Target(PmLine9 * const line9) * the struct is properly initialized BEFORE calling this function. */ int tcInit(TC_STRUCT * const tc, - int motion_type, + tc_motion_type_t motion_type, int canon_motion_type, double cycle_time, unsigned char enables, @@ -635,10 +365,8 @@ int tcInit(TC_STRUCT * const tc, tc->enables = enables; tc->cycle_time = cycle_time; - tc->id = -1; //ID to be set when added to queue (may change before due to blend arcs) - /** Segment settings (given values later during setup / optimization) */ - tc->indexer_jnum = -1; + tc->indexrotary = INDEX_NONE; tc->active_depth = 1; @@ -647,7 +375,6 @@ int tcInit(TC_STRUCT * const tc, return TP_ERR_OK; } - /** * Set kinematic properties for a trajectory segment. */ @@ -660,104 +387,39 @@ int tcSetupMotion(TC_STRUCT * const tc, tc->maxaccel = acc; - tc->maxvel = ini_maxvel; + tc->maxvel_geom = ini_maxvel; tc->reqvel = vel; - // To be computed by velocity optimization / spindle-sync calculations - tc->target_vel = 0; - // To be filled in by tangent calculation, negative = invalid (KLUDGE) - tcInitKinkProperties(tc); + // Initial guess at target velocity is just the requested velocity + tc->target_vel = vel; return TP_ERR_OK; } - int tcSetupState(TC_STRUCT * const tc, TP_STRUCT const * const tp) { - tcSetTermCond(tc, NULL, tp->termCond); tc->tolerance = tp->tolerance; tc->synchronized = tp->synchronized; tc->uu_per_rev = tp->uu_per_rev; return TP_ERR_OK; } -int pmLine9Init(PmLine9 * const line9, - EmcPose const * const start, - EmcPose const * const end) +int tcUpdateCircleAccRatio(TC_STRUCT * tc, double v_max_path) { - // Scratch variables - PmCartesian start_xyz, end_xyz; - PmCartesian start_uvw, end_uvw; - PmCartesian start_abc, end_abc; - - // Convert endpoint to cartesian representation - emcPoseToPmCartesian(start, &start_xyz, &start_abc, &start_uvw); - emcPoseToPmCartesian(end, &end_xyz, &end_abc, &end_uvw); - - // Initialize cartesian line members - int xyz_fail = pmCartLineInit(&line9->xyz, &start_xyz, &end_xyz); - int abc_fail = pmCartLineInit(&line9->abc, &start_abc, &end_abc); - int uvw_fail = pmCartLineInit(&line9->uvw, &start_uvw, &end_uvw); - - if (xyz_fail || abc_fail || uvw_fail) { - rtapi_print_msg(RTAPI_MSG_ERR,"Failed to initialize Line9, err codes %d, %d, %d\n", - xyz_fail,abc_fail,uvw_fail); - return TP_ERR_FAIL; + if (tc->motion_type != TC_CIRCULAR) { + return TP_ERR_OK; } - return TP_ERR_OK; -} - -int pmCircle9Init(PmCircle9 * const circ9, - EmcPose const * const start, - EmcPose const * const end, - PmCartesian const * const center, - PmCartesian const * const normal, - int turn) -{ - PmCartesian start_xyz, end_xyz; - PmCartesian start_uvw, end_uvw; - PmCartesian start_abc, end_abc; - - emcPoseToPmCartesian(start, &start_xyz, &start_abc, &start_uvw); - emcPoseToPmCartesian(end, &end_xyz, &end_abc, &end_uvw); - - int xyz_fail = pmCircleInit(&circ9->xyz, &start_xyz, &end_xyz, center, normal, turn); - //Initialize line parts of Circle9 - int abc_fail = pmCartLineInit(&circ9->abc, &start_abc, &end_abc); - int uvw_fail = pmCartLineInit(&circ9->uvw, &start_uvw, &end_uvw); - - int res_fit = findSpiralArcLengthFit(&circ9->xyz,&circ9->fit); - - if (xyz_fail || abc_fail || uvw_fail || res_fit) { - rtapi_print_msg(RTAPI_MSG_ERR,"Failed to initialize Circle9, err codes %d, %d, %d, %d\n", - xyz_fail, abc_fail, uvw_fail, res_fit); + if (tc->coords.circle.xyz_ratio <= 0.0) { return TP_ERR_FAIL; } - return TP_ERR_OK; -} -double pmCircle9Target(PmCircle9 const * const circ9) -{ - - double h2; - pmCartMagSq(&circ9->xyz.rHelix, &h2); - double helical_length = pmSqrt(pmSq(circ9->fit.total_planar_length) + h2); - - return helical_length; -} - -int tcUpdateCircleAccRatio(TC_STRUCT * tc) -{ - if (tc->motion_type == TC_CIRCULAR) { - PmCircleLimits limits = pmCircleActualMaxVel(&tc->coords.circle.xyz, - tc->maxvel, - tcGetOverallMaxAccel(tc)); - tc->maxvel = limits.v_max; - tc->acc_ratio_tan = limits.acc_ratio; - return 0; - } - // TODO handle blend arc here too? - return 1; //nothing to do, but not an error + double v_max3 = v_max_path * tc->coords.circle.xyz_ratio; + PmCircleLimits limits = pmCircleActualMaxVel(&tc->coords.circle.xyz, + v_max3, + tc->acc_normal_max * tcGetAccelScale(tc)); + tc->maxvel_geom = limits.v_max / tc->coords.circle.xyz_ratio; + tc->acc_ratio_tan = limits.acc_ratio; + return TP_ERR_OK; } /** @@ -767,109 +429,79 @@ int tcUpdateCircleAccRatio(TC_STRUCT * tc) * trust that the length will be the same, and so can use the length in the * velocity optimization. */ -int tcFinalizeLength(TC_STRUCT * const tc) +int tcFinalizeLength(TC_STRUCT * const tc, double max_feed_override) { //Apply velocity corrections if (!tc) { - return TP_ERR_FAIL; + return TP_ERR_OK; } + tp_debug_json5_log_start(tcFinalizeLength); if (tc->finalized) { - tp_debug_print("tc %d already finalized\n", tc->id); - return TP_ERR_NO_ACTION; + tp_debug_json5_log_end("tc %d already finalized", tc->id); + return TP_ERR_OK; } - tp_debug_print("Finalizing motion id %d, type %d\n", tc->id, tc->motion_type); - - tcClampVelocityByLength(tc); - - tcUpdateCircleAccRatio(tc); - - tc->finalized = 1; - return TP_ERR_OK; -} +#ifdef TP_DEBUG + double maxvel_old = tc->maxvel_geom; +#endif + CHP(tcUpdateCircleAccRatio(tc, tcGetPlanMaxTargetVel(tc, max_feed_override))); -int tcClampVelocityByLength(TC_STRUCT * const tc) -{ - //Apply velocity corrections - if (!tc) { - return TP_ERR_FAIL; - } - - //Reduce max velocity to match sample rate - //Assume that cycle time is valid here - double sample_maxvel = tc->target / tc->cycle_time; - tp_debug_print("sample_maxvel = %f\n",sample_maxvel); - tc->maxvel = fmin(tc->maxvel, sample_maxvel); - return TP_ERR_OK; -} - -/** - * compute the total arc length of a circle segment - */ -int tcUpdateTargetFromCircle(TC_STRUCT * const tc) -{ - if (!tc || tc->motion_type !=TC_CIRCULAR) { - return TP_ERR_FAIL; + if (tc->term_cond == TC_TERM_COND_PARABOLIC) { + tcSetTermCond(tc, TC_TERM_COND_EXACT); } + tc->finalized = 1; - double h2; - pmCartMagSq(&tc->coords.circle.xyz.rHelix, &h2); - double helical_length = pmSqrt(pmSq(tc->coords.circle.fit.total_planar_length) + h2); +#ifdef TP_DEBUG + print_json5_tc_id_data_(tc); + print_json5_string_("motion_type", tcMotionTypeAsString((tc_motion_type_t)tc->motion_type)); + print_json5_string_("term_cond", tcTermCondAsString((tc_term_cond_t)tc->term_cond)); + print_json5_double(maxvel_old); + print_json5_double_("maxvel", tc->maxvel_geom); + print_json5_double_("acc_ratio_tan", tc->acc_ratio_tan); +#endif - tc->target = helical_length; + tp_debug_json5_log_end("tc %llu finalized", tc->unique_id); return TP_ERR_OK; } - - int pmRigidTapInit(PmRigidTap * const tap, - EmcPose const * const start, - EmcPose const * const end, + PmVector const * const start, + PmVector const * const end, double reversal_scale) { PmCartesian start_xyz, end_xyz; PmCartesian abc, uvw; //Slightly more allocation this way, but much easier to read - emcPoseToPmCartesian(start, &start_xyz, &abc, &uvw); - emcPoseGetXYZ(end, &end_xyz); + VecToCart(start, &start_xyz, &tap->abc, &tap->uvw); + VecToCart(end, &end_xyz, &abc, &uvw); // Setup XYZ motion - pmCartLineInit(&tap->xyz, &start_xyz, &end_xyz); - - // Copy over fixed ABC and UVW points - tap->abc = abc; - tap->uvw = uvw; + pmCartLineInit(&tap->nominal_xyz, &start_xyz, &end_xyz); + tap->actual_xyz = tap->nominal_xyz; // Setup initial tap state - tap->reversal_target = tap->xyz.tmag; + tap->reversal_target = tap->nominal_xyz.tmag; tap->reversal_scale = reversal_scale; - tap->state = RIGIDTAP_START; - return TP_ERR_OK; + tap->state = RIGIDTAP_TAPPING; -} + if (!pmCartCartCompare(&tap->abc, &abc) || !pmCartCartCompare(&tap->uvw, &uvw)) { + return TP_ERR_RANGE; + } + return TP_ERR_OK; -double pmRigidTapTarget(PmRigidTap * const tap, double uu_per_rev) -{ - // allow 10 turns of the spindle to stop - we don't want to just go on forever - double overrun = 10. * uu_per_rev; - double target = tap->xyz.tmag + overrun; - tp_debug_print("initial tmag = %.12g, added %.12g for overrun, target = %.12g\n", - tap->xyz.tmag, overrun,target); - return target; } /** Returns true if segment has ONLY rotary motion, false otherwise. */ int tcPureRotaryCheck(TC_STRUCT const * const tc) { return (tc->motion_type == TC_LINEAR) && - (tc->coords.line.xyz.tmag_zero) && - (tc->coords.line.uvw.tmag_zero); + pmLine9LinearAxisLength(&tc->coords.line) == 0; } - - +#if 0 +// TODO replace with 9D /** * Given a PmCircle and a circular segment, copy the circle in as the XYZ portion of the segment, then update the motion parameters. * NOTE: does not yet support ABC or UVW motion! @@ -881,10 +513,6 @@ int tcSetCircleXYZ(TC_STRUCT * const tc, PmCircle const * const circ) if (!circ || tc->motion_type != TC_CIRCULAR) { return TP_ERR_FAIL; } - if (!tc->coords.circle.abc.tmag_zero || !tc->coords.circle.uvw.tmag_zero) { - rtapi_print_msg(RTAPI_MSG_ERR, "SetCircleXYZ does not supportABC or UVW motion\n"); - return TP_ERR_FAIL; - } // Store the new circular segment (or use the current one) @@ -895,25 +523,78 @@ int tcSetCircleXYZ(TC_STRUCT * const tc, PmCircle const * const circ) tc->coords.circle.xyz = *circ; // Update the arc length fit to this new segment - findSpiralArcLengthFit(&tc->coords.circle.xyz, &tc->coords.circle.fit); + findSpiralArcLengthFit(&tc->coords.circle.xyz, &tc->coords.circle.fit, TP_ANGLE_EPSILON); // compute the new total arc length using the fit and store as new // target distance - tc->target = pmCircle9Target(&tc->coords.circle); + tc->target = pmCircle9Length(&tc->coords.circle); return TP_ERR_OK; } +#endif + +// Helper functions to convert enums to pretty-print for debug output -int tcClearFlags(TC_STRUCT * const tc) +const char *tcTermCondAsString(tc_term_cond_t c) { - if (!tc) { - return TP_ERR_MISSING_INPUT; + switch (c) + { + case TC_TERM_COND_STOP: + return "EXACT_STOP"; + case TC_TERM_COND_EXACT: + return "EXACT_PATH"; + case TC_TERM_COND_PARABOLIC: + return "PARABOLIC"; + case TC_TERM_COND_TANGENT: + return "TANGENT"; } + return "NONE"; +} - //KLUDGE this will need to be updated manually if any other flags are added. - tc->is_blending = false; +const char *tcMotionTypeAsString(tc_motion_type_t c) +{ + switch (c) + { + case TC_LINEAR: + return "Linear"; + case TC_CIRCULAR: + return "Circular"; + case TC_RIGIDTAP: + return "RigidTap"; + case TC_SPHERICAL: + return "SphericalArc"; + case TC_DWELL: + return "Dwell"; + } + return "NONE"; +} - return TP_ERR_OK; +const char *tcSyncModeAsString(tc_spindle_sync_t c) +{ + switch (c) + { + case TC_SYNC_NONE: + return "sync_none"; + case TC_SYNC_VELOCITY: + return "sync_velocity"; + case TC_SYNC_POSITION: + return "sync_position"; + } + return "NONE"; } +int tcSetLine9(TC_STRUCT * const tc, PmLine9 const * const line9) +{ + if (tc->motion_type != TC_LINEAR) { return -1; } + tc->coords.line = *line9; + tc->target = pmLine9Length(line9); + return 0; +} +int tcSetCircle9(TC_STRUCT * const tc, PmCircle9 const * const circle9) +{ + if (tc->motion_type != TC_CIRCULAR) { return -1; } + tc->coords.circle = *circle9; + tc->target = circle9->total_length; + return 0; +} diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index fc3665f620d..c700695d6ec 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -21,82 +21,48 @@ #include "emcmotcfg.h" #include "tc_types.h" #include "tp_types.h" +#include "pm_vector.h" -double tcGetMaxTargetVel(TC_STRUCT const * const tc, - double max_scale); +double tcGetMaxVelFromLength(TC_STRUCT const * const tc); +double tcGetPlanMaxTargetVel(TC_STRUCT const * const tc, + double max_feed_scale); +double tcGetAccelScale(TC_STRUCT const * tc); double tcGetOverallMaxAccel(TC_STRUCT const * tc); double tcGetTangentialMaxAccel(TC_STRUCT const * const tc); int tcSetKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc, double kink_vel, double accel_reduction); -int tcInitKinkProperties(TC_STRUCT *tc); -int tcRemoveKinkProperties(TC_STRUCT *prev_tc, TC_STRUCT *tc); -int tcGetEndpoint(TC_STRUCT const * const tc, EmcPose * const out); -int tcGetStartpoint(TC_STRUCT const * const tc, EmcPose * const out); -int tcGetPos(TC_STRUCT const * const tc, EmcPose * const out); -int tcGetPosReal(TC_STRUCT const * const tc, int of_endpoint, EmcPose * const out); -int tcGetEndAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); -int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); -int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); -int tcGetStartTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); +int tcGetEndpoint(TC_STRUCT const * const tc, PmVector * const out); +int tcGetStartpoint(TC_STRUCT const * const tc, PmVector * const pos); +int tcGetPos(TC_STRUCT const * const tc, PmVector * const out); +int tcGetPosReal(TC_STRUCT const * const tc, double progress, PmVector * const out); +int tcGetStartTangentUnitVector(TC_STRUCT const * const tc, PmVector * const out); +int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmVector * const out); +int tcGetTangentUnitVector(TC_STRUCT const * const tc, double progress, PmVector * const out); +double tcGetVLimit(TC_STRUCT const * const tc, double v_target, double v_limit_linear, double v_limit_angular); double tcGetDistanceToGo(TC_STRUCT const * const tc, int direction); double tcGetTarget(TC_STRUCT const * const tc, int direction); -int tcGetIntersectionPoint(TC_STRUCT const * const prev_tc, - TC_STRUCT const * const tc, PmCartesian * const point); - int tcCanConsume(TC_STRUCT const * const tc); -int tcSetTermCond(TC_STRUCT * prev_tc, TC_STRUCT * tc, int term_cond); - -int tcConnectBlendArc(TC_STRUCT * const prev_tc, TC_STRUCT * const tc, - PmCartesian const * const circ_start, - PmCartesian const * const circ_end); - -int tcIsBlending(TC_STRUCT * const tc); - +int tcSetTermCond(TC_STRUCT * tc, tc_term_cond_t term_cond); int tcFindBlendTolerance(TC_STRUCT const * const prev_tc, - TC_STRUCT const * const tc, double * const T_blend, double * const nominal_tolerance); - -int pmCircleTangentVector(PmCircle const * const circle, - double angle_in, PmCartesian * const out); - -int tcFlagEarlyStop(TC_STRUCT * const tc, - TC_STRUCT * const nexttc); - -double pmLine9Target(PmLine9 * const line9); - -int pmLine9Init(PmLine9 * const line9, - EmcPose const * const start, - EmcPose const * const end); - -double pmCircle9Target(PmCircle9 const * const circ9); - -int pmCircle9Init(PmCircle9 * const circ9, - EmcPose const * const start, - EmcPose const * const end, - PmCartesian const * const center, - PmCartesian const * const normal, - int turn); + TC_STRUCT const * const tc, double * const T_blend); int pmRigidTapInit(PmRigidTap * const tap, - EmcPose const * const start, - EmcPose const * const end, + PmVector const * const start, + PmVector const * const end, double reversal_scale); -double pmRigidTapTarget(PmRigidTap * const tap, double uu_per_rev); - int tcInit(TC_STRUCT * const tc, - int motion_type, + tc_motion_type_t motion_type, int canon_motion_type, double cycle_time, unsigned char enables, char atspeed); -int tcSetupFromTP(TC_STRUCT * const tc, TP_STRUCT const * const tp); - int tcSetupMotion(TC_STRUCT * const tc, double vel, double ini_maxvel, @@ -104,15 +70,22 @@ int tcSetupMotion(TC_STRUCT * const tc, int tcSetupState(TC_STRUCT * const tc, TP_STRUCT const * const tp); -int tcUpdateCircleAccRatio(TC_STRUCT * tc); - -int tcFinalizeLength(TC_STRUCT * const tc); +int tcUpdateCircleAccRatio(TC_STRUCT * tc, double v_max); -int tcClampVelocityByLength(TC_STRUCT * const tc); +int tcFinalizeLength(TC_STRUCT * const tc, double max_feed_override); int tcPureRotaryCheck(TC_STRUCT const * const tc); +int tcSetLineXYZ(TC_STRUCT * const tc, PmCartLine const * const line); + int tcSetCircleXYZ(TC_STRUCT * const tc, PmCircle const * const circ); -int tcClearFlags(TC_STRUCT * const tc); +int tcSetLine9(TC_STRUCT * const tc, PmLine9 const * const line9); + +int tcSetCircle9(TC_STRUCT * const tc, PmCircle9 const * const circ9); + +const char *tcTermCondAsString(tc_term_cond_t c); +const char *tcMotionTypeAsString(tc_motion_type_t c); +const char *tcSyncModeAsString(tc_spindle_sync_t c); + #endif /* TC_H */ diff --git a/src/emc/tp/tc_types.h b/src/emc/tp/tc_types.h index 6b0dd8d534a..18779601113 100644 --- a/src/emc/tp/tc_types.h +++ b/src/emc/tp/tc_types.h @@ -9,15 +9,22 @@ * System: Linux * * Copyright (c) 2004 All rights reserved. +* +* Last change: ********************************************************************/ #ifndef TC_TYPES_H #define TC_TYPES_H -#include "spherical_arc.h" +#include "spherical_arc9.h" #include "posemath.h" #include "emcpos.h" #include "emcmotcfg.h" #include "state_tag.h" +#include "motion_types.h" +#include "pm_circle9.h" +#include "pm_line9.h" + +typedef unsigned long long tc_unique_id_t; #define BLEND_DIST_FRACTION 0.5 /* values for endFlag */ @@ -32,7 +39,8 @@ typedef enum { TC_LINEAR = 1, TC_CIRCULAR = 2, TC_RIGIDTAP = 3, - TC_SPHERICAL = 4 + TC_SPHERICAL = 4, + TC_DWELL = 5 } tc_motion_type_t; typedef enum { @@ -46,53 +54,25 @@ typedef enum { TC_DIR_REVERSE } tc_direction_t; -#define TC_GET_PROGRESS 0 -#define TC_GET_STARTPOINT 1 -#define TC_GET_ENDPOINT 2 - -#define TC_OPTIM_UNTOUCHED 0 -#define TC_OPTIM_AT_MAX 1 - -#define TC_ACCEL_TRAPZ 0 -#define TC_ACCEL_RAMP 1 - -/** - * Spiral arc length approximation by quadratic fit. - */ -typedef struct { - double b0; /* 2nd order coefficient */ - double b1; /* 1st order coefficient */ - double total_planar_length; /* total arc length in plane */ - int spiral_in; /* flag indicating spiral is inward, - rather than outward */ -} SpiralArcLengthFit; +typedef enum { + TC_OPTIM_UNTOUCHED, + TC_OPTIM_AT_MAX, +} TCOptimizationLevel; +typedef enum { + TC_ACCEL_TRAPZ, + TC_ACCEL_RAMP, +} TCAccelMode; /* structure for individual trajectory elements */ typedef struct { - PmCartLine xyz; - PmCartLine abc; - PmCartLine uvw; -} PmLine9; - -typedef struct { - PmCircle xyz; - PmCartLine abc; - PmCartLine uvw; - SpiralArcLengthFit fit; -} PmCircle9; - -typedef struct { - SphericalArc xyz; - PmCartesian abc; - PmCartesian uvw; -} Arc9; - -typedef enum { - RIGIDTAP_START, - TAPPING, REVERSING, RETRACTION, FINAL_REVERSAL, FINAL_PLACEMENT -} RIGIDTAP_STATE; + PmVector dwell_pos; + double dwell_time_req; // Pre-calculated, fixed dwell time + double delta_rpm_req; // Use to compute a dwell based on the spindle acceleration at run-time + double dwell_time; // actual dwell time (determined at run-time when dwell starts) + double remaining_time; +} TPDwell; typedef unsigned long long iomask_t; // 64 bits on both x86 and x86_64 @@ -104,18 +84,37 @@ typedef struct { double aios[EMCMOT_MAX_AIO]; } syncdio_t; + typedef struct { - PmCartLine xyz; // original, but elongated, move down - PmCartLine aux_xyz; // this will be generated on the fly, for the other - // two moves: retraction, final placement + double position; //!< Reference position for displacement calculations + double direction; //!< Direction of "positive" spindle motion (as -1, 1, or 0) +} spindle_origin_t; + +typedef struct { + PmCartLine nominal_xyz; // nominal tapping motion from start to end) + PmCartLine actual_xyz; // this will be generated on the fly, for the actual rigid tapping motion PmCartesian abc; PmCartesian uvw; double reversal_target; double reversal_scale; - double spindlerevs_at_reversal; - RIGIDTAP_STATE state; + double tap_uu_per_rev; + double retract_uu_per_rev; + rigid_tap_state_t state; } PmRigidTap; +typedef enum { + INDEX_NONE=-1, + INDEX_X_AXIS=0, + INDEX_Y_AXIS, + INDEX_Z_AXIS, + INDEX_A_AXIS, + INDEX_B_AXIS, + INDEX_C_AXIS, + INDEX_U_AXIS, + INDEX_V_AXIS, + INDEX_W_AXIS, +} IndexRotaryAxis; + typedef struct { double cycle_time; //Position stuff @@ -126,51 +125,54 @@ typedef struct { //Velocity double reqvel; // vel requested by F word, calc'd by task double target_vel; // velocity to actually track, limited by other factors - double maxvel; // max possible vel (feed override stops here) + double maxvel_geom; // max possible vel from segment geometry (feed override stops here) double currentvel; // keep track of current step (vel * cycle_time) double finalvel; // velocity to aim for at end of segment double term_vel; // actual velocity at termination of segment + int use_kink; double kink_vel; // Temporary way to store our calculation of maximum velocity we can handle if this segment is declared tangent with the next double kink_accel_reduce_prev; // How much to reduce the allowed tangential acceleration to account for the extra acceleration at an approximate tangent intersection. double kink_accel_reduce; // How much to reduce the allowed tangential acceleration to account for the extra acceleration at an approximate tangent intersection. + double parabolic_equiv_vel; // Estimated equivalent velocity for a parabolic blend with the next segment + + double v_limit_linear_ratio; + double v_limit_angular_ratio; //Acceleration double maxaccel; // accel calc'd by task double acc_ratio_tan;// ratio between normal and tangential accel + double acc_normal_max; // Max acceleration allowed in normal direction (worst-case for the whole curve) int id; // segment's serial number - struct state_tag_t tag; // state tag corresponding to running motion + tc_unique_id_t unique_id; //!< "Unique" identifier for a motion command that's not related to source line + struct state_tag_t tag; /* state tag corresponding to running motion */ union { // describes the segment's start and end positions PmLine9 line; PmCircle9 circle; PmRigidTap rigidtap; - Arc9 arc; + SphericalArc9 arc; + TPDwell dwell; } coords; - int motion_type; // TC_LINEAR (coords.line) or + tc_motion_type_t motion_type; // TC_LINEAR (coords.line) or // TC_CIRCULAR (coords.circle) or // TC_RIGIDTAP (coords.rigidtap) int active; // this motion is being executed int canon_motion_type; // this motion is due to which canon function? - int term_cond; // gcode requests continuous feed at the end of + tc_term_cond_t term_cond; // gcode requests continuous feed at the end of // this segment (g64 mode) - int blending_next; // segment is being blended into following segment - double blend_vel; // velocity below which we should start blending double tolerance; // during the blend at the end of this move, // stay within this distance from the path. - int synchronized; // spindle sync state + tc_spindle_sync_t synchronized; // spindle sync state double uu_per_rev; // for sync, user units per rev (e.g. 0.0625 for 16tpi) - double vel_at_blend_start; - int sync_accel; // we're accelerating up to sync with the spindle unsigned char enables; // Feed scale, etc, enable bits for this move int atspeed; // wait for the spindle to be at-speed before starting this move syncdio_t syncdio; // synched DIO's for this move. what to turn on/off - int indexer_jnum; // which joint to unlock (for a locking indexer) to make this move, -1 for none - int optimization_state; // At peak velocity during blends) + IndexRotaryAxis indexrotary; // which rotary axis to unlock to make this move, -1 for none + TCOptimizationLevel optimization_state; // At peak velocity during blends) int on_final_decel; - int blend_prev; int accel_mode; int splitting; // the segment is less than 1 cycle time // away from the end. @@ -180,8 +182,6 @@ typedef struct { * speed) */ int finalized; - // Temporary status flags (reset each cycle) - int is_blending; } TC_STRUCT; #endif /* TC_TYPES_H */ diff --git a/src/emc/tp/tcq.c b/src/emc/tp/tcq.c index ce2a711c9e3..c6b6ab12c3e 100644 --- a/src/emc/tp/tcq.c +++ b/src/emc/tp/tcq.c @@ -45,14 +45,18 @@ static inline int tcqCheck(TC_QUEUE_STRUCT const * const tcq) */ int tcqCreate(TC_QUEUE_STRUCT * const tcq, int _size, TC_STRUCT * const tcSpace) { - if (!tcq || !tcSpace || _size < 1) { - return -1; - } + if (_size <= 0 || 0 == tcq) { + return -1; + } else { tcq->queue = tcSpace; - tcq->size = _size; + tcq->size = _size; tcqInit(tcq); + if (0 == tcq->queue) { + return -1; + } return 0; + } } /*! tcqDelete() function @@ -97,7 +101,6 @@ int tcqInit(TC_QUEUE_STRUCT * const tcq) tcq->_len = 0; tcq->start = tcq->end = 0; tcq->rend = 0; - tcq->_rlen = 0; tcq->allFull = 0; return 0; @@ -166,7 +169,7 @@ int tcqPopBack(TC_QUEUE_STRUCT * const tcq) return 0; } -#define TCQ_REVERSE_MARGIN 200 +#define TCQ_REVERSE_MARGIN 100 int tcqPop(TC_QUEUE_STRUCT * const tcq) { @@ -230,20 +233,19 @@ int tcqRemove(TC_QUEUE_STRUCT * const tcq, int n) return 0; } - /** * Step backward into the reverse history. */ int tcqBackStep(TC_QUEUE_STRUCT * const tcq) { - if (tcqCheck(tcq)) { return -1; } // start == end means that queue is empty - if ( tcq->start == tcq->rend) { + int rempty = (tcq->start == tcq->rend); + if ( rempty && !tcq->allFull) { return -1; } /* update start ptr and reset allFull flag and len */ diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index bedf9d4a218..8ae115b525e 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -12,21 +12,26 @@ ********************************************************************/ #include "rtapi.h" /* rtapi_print_msg */ #include "posemath.h" /* Geometry types & functions */ +#include "tc.h" +#include "tp.h" #include "emcpose.h" #include "rtapi_math.h" -#include "motion.h" -#include "tp.h" -#include "tc.h" +#include "mot_priv.h" +#include "motion_debug.h" #include "motion_types.h" #include "spherical_arc.h" #include "blendmath.h" -#include "axis.h" -//KLUDGE Don't include all of emc.hh here, just hand-copy the TERM COND -//definitions until we can break the emc constants out into a separate file. -//#include "emc.hh" -#define EMC_TRAJ_TERM_COND_STOP 0 -#define EMC_TRAJ_TERM_COND_EXACT 1 -#define EMC_TRAJ_TERM_COND_BLEND 2 +#include "math_util.h" +#include "joint_util.h" +#include "string.h" +#include "tp_priv.h" +#include "motion_shared.h" +#include "tp_call_wrappers.h" +#include "error_util.h" + +// TODO make these configurable as machine parameters via INI to give integrators more control +// Absolute maximum distance (in revolutions) to add to the end of a rigidtap move to account for spindle reversal +static const double RIGIDTAP_MAX_OVERSHOOT_REVS = 10.0; /** * @section tpdebugflags TP debugging flags @@ -39,68 +44,32 @@ #include "tp_debug.h" -// FIXME: turn off this feature, which causes blends between rapids to -// use the feed override instead of the rapid override -#undef TP_SHOW_BLENDS - -#define TP_OPTIMIZATION_LAZY - -#define MAKE_TP_HAL_PINS -#undef MAKE_TP_HAL_PINS - -// api for tpCreate() inherits a component id provision to include hal pins: -// (not used by the this default tp implementation but may -// be used in alternate user-built implementations) -#ifdef MAKE_TP_HAL_PINS // { -#include "hal.h" -#endif // } +#ifdef TP_DEBUG +static const bool _tp_debug = true; +#else +static const bool _tp_debug = false; +#endif -// Only gcc/g++ supports the #pragma -#if __GNUC__ && !defined(__clang__) -// tpHandleBlendArc() is 2512 - #pragma GCC diagnostic warning "-Wframe-larger-than=2600" +#ifdef TC_DEBUG +static const bool _tc_debug = true; +#else +static const bool _tc_debug = false; #endif -static emcmot_status_t *emcmotStatus; -static emcmot_config_t *emcmotConfig; +// NOTE: turned off this feature, which causes blends between rapids to +// use the feed override instead of the rapid override +#undef TP_SHOW_BLENDS -//========================================================== -// tp module interface -// motmod function ptrs for functions called by tp: -static void( *_DioWrite)(int,char); -static void( *_AioWrite)(int,double); -static void( *_SetRotaryUnlock)(int,int); -static int ( *_GetRotaryIsUnlocked)(int); -static double(*_axis_get_vel_limit)(int); -static double(*_axis_get_acc_limit)(int); +#define TP_OPTIMIZATION_LAZY -void tpMotFunctions(void( *pDioWrite)(int,char) - ,void( *pAioWrite)(int,double) - ,void( *pSetRotaryUnlock)(int,int) - ,int ( *pGetRotaryIsUnlocked)(int) - ,double(*paxis_get_vel_limit)(int) - ,double(*paxis_get_acc_limit)(int) - ) -{ - _DioWrite = pDioWrite; - _AioWrite = pAioWrite; - _SetRotaryUnlock = pSetRotaryUnlock; - _GetRotaryIsUnlocked = pGetRotaryIsUnlocked; - _axis_get_vel_limit = paxis_get_vel_limit; - _axis_get_acc_limit = paxis_get_acc_limit; -} +static TC_STRUCT blend_tc1 = {}; +static TC_STRUCT blend_tc2 = {}; -void tpMotData(emcmot_status_t *pstatus - ,emcmot_config_t *pconfig - ) -{ - emcmotStatus = pstatus; - emcmotConfig = pconfig; -} -//========================================================= +void clearPositionSyncErrors(void); /** static function primitives (ugly but less of a pain than moving code around)*/ -STATIC int tpComputeBlendVelocity( + +int tpComputeBlendVelocity( TC_STRUCT const *tc, TC_STRUCT const *nexttc, double v_target_this, @@ -109,56 +78,29 @@ STATIC int tpComputeBlendVelocity( double *v_blend_next, double *v_blend_net); -STATIC double estimateParabolicBlendPerformance( - TP_STRUCT const *tp, - TC_STRUCT const *tc, - TC_STRUCT const *nexttc); - -STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc); - -STATIC int tpUpdateCycle(TP_STRUCT * const tp, - TC_STRUCT * const tc, TC_STRUCT const * const nexttc); - -STATIC int tpRunOptimization(TP_STRUCT * const tp); - -STATIC inline int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc, int inc_id); - -STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc); - -/** - * @section tpcheck Internal state check functions. - * These functions compartmentalize some of the messy state checks. - * Hopefully this makes changes easier to track as much of the churn will be on small functions. - */ +static inline double findVPeak(double a_t_max, double distance) +{ + return pmSqrt(a_t_max * distance); +} -/** - * Returns true if there is motion along ABC or UVW axes, false otherwise. - */ -STATIC int tcRotaryMotionCheck(TC_STRUCT const * const tc) { - switch (tc->motion_type) { - //Note lack of break statements due to every path returning - case TC_RIGIDTAP: - return false; - case TC_LINEAR: - if (tc->coords.line.abc.tmag_zero && tc->coords.line.uvw.tmag_zero) { - return false; - } else { - return true; - } - case TC_CIRCULAR: - if (tc->coords.circle.abc.tmag_zero && tc->coords.circle.uvw.tmag_zero) { - return false; - } else { - return true; - } - case TC_SPHERICAL: - return true; - default: - tp_debug_print("Unknown motion type!\n"); - return false; +void reportTPAxisError(TP_STRUCT const *tp, unsigned failed_axes, const char *msg_prefix) +{ + if (failed_axes) + { + AxisMaskString failed_axes_str = axisBitMaskToString(failed_axes); + rtapi_print_msg(RTAPI_MSG_ERR, "%s, ax%cs [%s], line %d, %g sec\n", + msg_prefix ?: "unknown error", + failed_axes_str.len > 1 ? 'e' : 'i', // Ugly workaround for english grammar + failed_axes_str.axes, + tp->execTag.fields[GM_FIELD_LINE_NUMBER], + tp->time_elapsed_sec); } } +static bool needConsistencyCheck(ConsistencyCheckMask mask) +{ + return emcmotConfig->consistencyCheckConfig.extraConsistencyChecks & mask; +} /** * @section tpgetset Internal Get/Set functions @@ -167,60 +109,14 @@ STATIC int tcRotaryMotionCheck(TC_STRUCT const * const tc) { * segment's feed override, while taking into account the status of tp itself. */ - - /** * Wrapper to bounds-check the tangent kink ratio from HAL. */ -STATIC double tpGetTangentKinkRatio(void) { +double tpGetTangentKinkRatio(void) { const double max_ratio = 0.7071; const double min_ratio = 0.001; - return fmax(fmin(emcmotConfig->arcBlendTangentKinkRatio,max_ratio),min_ratio); -} - -STATIC int tpGetMachineAccelBounds(PmCartesian * const acc_bound) { - if (!acc_bound) { - return TP_ERR_FAIL; - } - - acc_bound->x = _axis_get_acc_limit(0); //0==>x - acc_bound->y = _axis_get_acc_limit(1); //1==>y - acc_bound->z = _axis_get_acc_limit(2); //2==>z - return TP_ERR_OK; -} - - -STATIC int tpGetMachineVelBounds(PmCartesian * const vel_bound) { - if (!vel_bound) { - return TP_ERR_FAIL; - } - - vel_bound->x = _axis_get_vel_limit(0); //0==>x - vel_bound->y = _axis_get_vel_limit(1); //1==>y - vel_bound->z = _axis_get_vel_limit(2); //2==>z - return TP_ERR_OK; -} - -STATIC int tpGetMachineActiveLimit(double * const act_limit, PmCartesian const * const bounds) { - if (!act_limit) { - return TP_ERR_FAIL; - } - //Start with max accel value - *act_limit = fmax(fmax(bounds->x,bounds->y),bounds->z); - - // Compare only with active axes - if (bounds->x > 0) { - *act_limit = fmin(*act_limit, bounds->x); - } - if (bounds->y > 0) { - *act_limit = fmin(*act_limit, bounds->y); - } - if (bounds->z > 0) { - *act_limit = fmin(*act_limit, bounds->z); - } - tp_debug_print(" arc blending a_max=%f\n", *act_limit); - return TP_ERR_OK; + return fmax(fmin(emcmotConfig->arc_blend_cfg.tangent_kink_ratio,max_ratio),min_ratio); } @@ -228,36 +124,34 @@ STATIC int tpGetMachineActiveLimit(double * const act_limit, PmCartesian const * * Get a segment's feed scale based on the current planner state and emcmotStatus. * @note depends on emcmotStatus for system information. */ -STATIC double tpGetFeedScale(TP_STRUCT const * const tp, +double tpGetRealAbsFeedScale(TP_STRUCT const * const tp, TC_STRUCT const * const tc) { if (!tc) { return 0.0; } + + double net_feed_scale = fabs(emcmotStatus->net_feed_scale); + //All reasons to disable feed override go here bool pausing = tp->pausing && (tc->synchronized == TC_SYNC_NONE || tc->synchronized == TC_SYNC_VELOCITY); bool aborting = tp->aborting; if (pausing) { - tc_debug_print("pausing\n"); return 0.0; } else if (aborting) { - tc_debug_print("aborting\n"); return 0.0; } else if (tc->synchronized == TC_SYNC_POSITION ) { return 1.0; - } else if (tc->is_blending) { - //KLUDGE: Don't allow feed override to keep blending from overruning max velocity - return fmin(emcmotStatus->net_feed_scale, 1.0); } else { - return emcmotStatus->net_feed_scale; + return net_feed_scale; } } - /** * Get target velocity for a tc based on the trajectory planner state. * This gives the requested velocity, capped by the segments maximum velocity. + * @note returns the magnitude of velocity (reverse run is handled at a higher level) */ -STATIC inline double tpGetRealTargetVel(TP_STRUCT const * const tp, +double tpGetRealAbsTargetVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc) { if (!tc) { @@ -265,35 +159,39 @@ STATIC inline double tpGetRealTargetVel(TP_STRUCT const * const tp, } // Start with the scaled target velocity based on the current feed scale double v_target = tc->synchronized ? tc->target_vel : tc->reqvel; + /*tc_debug_print("Initial v_target = %f\n",v_target);*/ // Get the maximum allowed target velocity, and make sure we're below it - return fmin(v_target * tpGetFeedScale(tp,tc), tpGetMaxTargetVel(tp, tc)); + return fmin(v_target * tpGetRealAbsFeedScale(tp,tc), tpGetRealMaxTargetVel(tp, tc)); } - -STATIC inline double getMaxFeedScale(TC_STRUCT const * tc) +double getMaxBlendFeedScale(TC_STRUCT const * prev_tc, TC_STRUCT const * tc) { //All reasons to disable feed override go here - if (tc && tc->synchronized == TC_SYNC_POSITION ) { + if ((tc && tc->synchronized == TC_SYNC_POSITION) || + (prev_tc && prev_tc->synchronized == TC_SYNC_POSITION)) { return 1.0; } else { - return emcmotConfig->maxFeedScale; + return fmin(emcmotConfig->biarc_solver_cfg.feed_override_allowance, emcmotConfig->maxFeedScale); } } - /** * Get the worst-case target velocity for a segment based on the trajectory planner state. * Note that this factors in the user-specified velocity limit. */ -STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc) +double tpGetRealMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc) { - double max_scale = emcmotConfig->maxFeedScale; - if (tc->is_blending) { - //KLUDGE: Don't allow feed override to keep blending from overruning max velocity - max_scale = fmin(max_scale, 1.0); + if (!tc) { + return 0.0; } - double v_max_target = tcGetMaxTargetVel(tc, max_scale); + + double max_scale = (tc->synchronized == TC_SYNC_POSITION) ? 1.0 : emcmotConfig->maxFeedScale; + + double v_max = tcGetMaxVelFromLength(tc); + + // Get maximum reachable velocity from max feed override + double v_max_target = tc->target_vel * max_scale; /* Check if the cartesian velocity limit applies and clip the maximum * velocity. The vLimit is from the max velocity slider, and should @@ -302,12 +200,8 @@ STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT con * computed in the TP, so it would disrupt position tracking to apply this * limit here. */ - if (!tcPureRotaryCheck(tc) && (tc->synchronized != TC_SYNC_POSITION)){ - /*tc_debug_print("Cartesian velocity limit active\n");*/ - v_max_target = fmin(v_max_target, tp->vLimit); - } - - return v_max_target; + double v_limited = tcGetVLimit(tc, v_max_target, tp->vLimit, tp->vLimitAng); + return fmin(v_limited, v_max); } @@ -316,94 +210,62 @@ STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT con * This function factors in the feed override and TC limits. It clamps the * final velocity to the maximum velocity and the next segment's target velocity */ -STATIC inline double tpGetRealFinalVel(TP_STRUCT const * const tp, +double tpGetRealFinalVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc, TC_STRUCT const * const nexttc) { /* If we're stepping, then it doesn't matter what the optimization says, we want to end at a stop. * If the term_cond gets changed out from under us, detect this and force final velocity to zero */ - - if (emcmotStatus->stepping || tc->term_cond != TC_TERM_COND_TANGENT || tp->reverse_run) { + if (emcmotDebug->stepping || tc->term_cond != TC_TERM_COND_TANGENT || tp->reverse_run) { return 0.0; - } - + } + // Get target velocities for this segment and next segment - double v_target_this = tpGetRealTargetVel(tp, tc); + double v_target_this = tpGetRealAbsTargetVel(tp, tc); double v_target_next = 0.0; if (nexttc) { - v_target_next = tpGetRealTargetVel(tp, nexttc); + v_target_next = tpGetRealAbsTargetVel(tp, nexttc); } - tc_debug_print("v_target_next = %f\n",v_target_next); // Limit final velocity to minimum of this and next target velocities double v_target = fmin(v_target_this, v_target_next); - return fmin(tc->finalvel, v_target); + double finalvel = fmin(tc->finalvel, v_target); + return finalvel; } - /** - * Convert the 2-part spindle position and sign to a signed double. + * Set up a spindle origin based on the current spindle COMMANDED direction and the given position. + * + * The origin is used to calculate displacements used in spindle position tracking. + * The direction is stored as part of the origin to prevent discontinuous + * changes in displacement due to sign flips */ -STATIC inline double tpGetSignedSpindlePosition(spindle_status_t *status) { - int spindle_dir; - double spindle_pos; - spindle_dir = status->direction; - spindle_pos = status->spindleRevs; - if (spindle_dir < 0.0) { - spindle_pos*=-1.0; +void setSpindleOrigin(spindle_origin_t *origin, double position) +{ + if (!origin) { + return; + } + origin->position = position; + origin->direction = get_spindle_command_direction(emcmotStatus); +} + +void updateSpindlePositionFromProgress(spindle_origin_t *origin, TC_STRUCT const * const tc) +{ + if (!origin || !tc) { + return; } - return spindle_pos; + origin->position += tc->progress * origin->direction / tc->uu_per_rev; + origin->direction = get_spindle_command_direction(emcmotStatus); } /** * @section tpaccess tp class-like API */ -/* space for trajectory planner queues, plus 10 more for safety */ -/*! \todo FIXME-- default is used; dynamic is not honored */ - TC_STRUCT queueTcSpace[DEFAULT_TC_QUEUE_SIZE + 10]; - /** * Create the trajectory planner structure with an empty queue. */ - -#ifdef MAKE_TP_HAL_PINS // { -static struct tp_haldata { - // Example pin pointers - hal_u32_t *in; - hal_u32_t *out; - // Example parameters - hal_float_t param_rw; - hal_float_t param_ro; -} *tp_haldata; - -static int makepins(int id) { -#define HAL_PREFIX "tp" - int res=0; - if (id < 0) goto error; - tp_haldata = hal_malloc(sizeof(struct tp_haldata)); - if (!tp_haldata) goto error; - - // hal pin examples: - res += hal_pin_u32_newf(HAL_IN ,&(tp_haldata->in) ,id,"%s.in" ,HAL_PREFIX); - res += hal_pin_u32_newf(HAL_OUT,&(tp_haldata->out),id,"%s.out",HAL_PREFIX); - - // hal parameter examples: - res += hal_param_float_newf(HAL_RW, &tp_haldata->param_rw,id,"%s.param-rw",HAL_PREFIX); - res += hal_param_float_newf(HAL_RO, &tp_haldata->param_ro,id,"%s.param-ro",HAL_PREFIX); - - if (res) goto error; - rtapi_print("@@@ %s:%s: ok\n",__FILE__,__FUNCTION__); - return 0; // caller issues hal_ready() -error: - rtapi_print("\n!!! %s:%s: failed res=%d\n\n",__FILE__,__FUNCTION__,res); - return -1; -#undef HAL_PREFIX -} -#endif // } - -int tpCreate(TP_STRUCT * const tp, int _queueSize,int id) -{ - (void)id; +int tpCreate(TP_STRUCT * const tp, int _queueSize, TC_STRUCT * const tcSpace) +{ if (0 == tp) { return TP_ERR_FAIL; } @@ -413,19 +275,13 @@ int tpCreate(TP_STRUCT * const tp, int _queueSize,int id) } else { tp->queueSize = _queueSize; } - TC_STRUCT * const tcSpace = queueTcSpace; /* create the queue */ if (-1 == tcqCreate(&tp->queue, tp->queueSize, tcSpace)) { return TP_ERR_FAIL; } -#ifdef MAKE_TP_HAL_PINS // { - if (-1 == makepins(id)) { - return TP_ERR_FAIL; - } -#endif // } - + tp->nextUniqueId = 0; /* init the rest of our data */ return tpInit(tp); } @@ -441,16 +297,22 @@ int tpClearDIOs(TP_STRUCT * const tp) { tp->syncdio.anychanged = 0; tp->syncdio.dio_mask = 0; tp->syncdio.aio_mask = 0; - for (i = 0; i < emcmotConfig->numDIO; i++) { + for (i = 0; i < num_dio; i++) { tp->syncdio.dios[i] = 0; } - for (i = 0; i < emcmotConfig->numAIO; i++) { + for (i = 0; i < num_aio; i++) { tp->syncdio.aios[i] = 0; } return TP_ERR_OK; } +void clearPosTrackingStatus() +{ + emcmotStatus->pos_tracking_error = 0; + emcmotStatus->pos_tracking_velocity = 0; +} + /** * "Soft initialize" the trajectory planner tp. * This is a "soft" initialization in that TP_STRUCT configuration @@ -462,30 +324,59 @@ int tpClearDIOs(TP_STRUCT * const tp) { */ int tpClear(TP_STRUCT * const tp) { + if (!tp) { + return 0; + } + // Soft-reset the queue (previous pointers to TC_STRUCTS should be considered invalid even though the memory still exists) tcqInit(&tp->queue); - tp->queueSize = 0; + + // Reset the TP goal position to whereever it is (since it's not moving), and all the side-effects of that (zero velocity / acceleration) tp->goalPos = tp->currentPos; - // Clear out status ID's + emcmotStatus->requested_vel = 0.0; + emcmotStatus->current_vel = 0.0; + tp->currentVel = PmVector_zero; + emcmotStatus->distance_to_go = 0.0; + ZERO_EMC_POSE(emcmotStatus->dtg); + + // Clear out elapsed time (for debugging) + tp->time_elapsed_sec = 0.0; + tp->time_elapsed_ticks = 0; + + // Clear out all status related to motion IDs / tags since the queue is cleared tp->nextId = 0; tp->execId = 0; - struct state_tag_t tag = {}; - tp->execTag = tag; + tp->nextexecId = 0; + static const struct state_tag_t empty_tag = {}; + tp->execTag = empty_tag; tp->motionType = 0; tp->done = 1; tp->depth = tp->activeDepth = 0; + tp->aborting = 0; tp->pausing = 0; - tp->reverse_run = 0; - tp->synchronized = 0; - tp->uu_per_rev = 0.0; - emcmotStatus->current_vel = 0.0; - emcmotStatus->requested_vel = 0.0; - emcmotStatus->distance_to_go = 0.0; - ZERO_EMC_POSE(emcmotStatus->dtg); + tp->reverse_run = TC_DIR_FORWARD; + + // Clear spindle synchronization and related status + tpSetSpindleSync(tp, 0.0, 0); + emcmotStatus->spindle_fb.synced = 0; + emcmotStatus->dwell_time_remaining = 0.0; + clearPosTrackingStatus(); + + // Clear any waits for spindle index / at-speed (since the motions that would be waiting no longer exist) + tp->spindle.waiting_for_index = MOTION_INVALID_ID; + tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID; - // equivalent to: SET_MOTION_INPOS_FLAG(1): - emcmotStatus->motionFlag |= EMCMOT_MOTION_INPOS_BIT; + // TODO determine if this really needs to be here (because later in get_pos_cmds() it will be set anyway). + SET_MOTION_INPOS_FLAG(1); +#ifdef TP_DEBUG + PmCartesian axis_vel_limit = getXYZVelBounds(); + PmCartesian axis_accel_limit = getXYZAccelBounds(); + print_json5_log_start(tpClear); + print_json5_PmCartesian(axis_vel_limit); + print_json5_PmCartesian(axis_accel_limit); + print_json5_log_end(); +#endif return tpClearDIOs(tp); } @@ -499,35 +390,20 @@ int tpInit(TP_STRUCT * const tp) tp->cycleTime = 0.0; //Velocity limits tp->vLimit = 0.0; - tp->ini_maxvel = 0.0; - //Accelerations - tp->aLimit = 0.0; - PmCartesian acc_bound; - //FIXME this acceleration bound isn't valid (nor is it used) - if (emcmotStatus == 0) { - rtapi_print("!!!tpInit: NULL emcmotStatus, bye\n\n"); - return -1; - } - tpGetMachineAccelBounds(&acc_bound); - tpGetMachineActiveLimit(&tp->aMax, &acc_bound); - //Angular limits - tp->wMax = 0.0; - tp->wDotMax = 0.0; - - tp->spindle.offset = 0.0; - tp->spindle.revs = 0.0; - tp->spindle.waiting_for_index = MOTION_INVALID_ID; - tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID; + tp->vLimitAng = 0.0; + + setSpindleOrigin(&tp->spindle.origin, 0.0); + tp->spindle.trigger_revs = 0; tp->reverse_run = TC_DIR_FORWARD; - tp->termCond = TC_TERM_COND_PARABOLIC; - tp->tolerance = 0.0; - ZERO_EMC_POSE(tp->currentPos); + // Initialize the current state (used during tpClear to initialize other state) + tp->currentPos = PmVector_zero; + tp->currentVel = PmVector_zero; - PmCartesian vel_bound; - tpGetMachineVelBounds(&vel_bound); - tpGetMachineActiveLimit(&tp->vMax, &vel_bound); + // Only set up TP defaults for tolerance once (after that, it must match what interp / task specifies) + tp->termCond = TC_TERM_COND_PARABOLIC; + tp->tolerance = 0.0; return tpClear(tp); } @@ -547,51 +423,18 @@ int tpSetCycleTime(TP_STRUCT * const tp, double secs) } /** - * Set requested velocity and absolute maximum velocity (bounded by machine). - * This is called before adding lines or circles, specifying vMax (the velocity - * requested by the F word) and ini_maxvel, the max velocity possible before - * meeting a machine constraint caused by an AXIS's max velocity. (the TP is - * allowed to go up to this high when feed override >100% is requested) These - * settings apply to subsequent moves until changed. - */ -int tpSetVmax(TP_STRUCT * const tp, double vMax, double ini_maxvel) -{ - if (0 == tp || vMax <= 0.0 || ini_maxvel <= 0.0) { - return TP_ERR_FAIL; - } - - tp->vMax = vMax; - tp->ini_maxvel = ini_maxvel; - - return TP_ERR_OK; -} - -/** - * Set the tool tip maximum velocity. - * This is the [TRAJ]MAX_LINEAR_VELOCITY. This should be the max velocity of + * Set the maximum velocity for linear and rotary-only moves. + * I think this is the [TRAJ] max velocity. This should be the max velocity of * const the TOOL TIP, not necessarily any particular axis. This applies to * subsequent moves until changed. */ -int tpSetVlimit(TP_STRUCT * const tp, double vLimit) +int tpSetVlimit(TP_STRUCT * const tp, double vLimit, double vLimitAng) { if (!tp) return TP_ERR_FAIL; - if (vLimit < 0.) - tp->vLimit = 0.; - else - tp->vLimit = vLimit; - - return TP_ERR_OK; -} - -/** Sets the max acceleration for the trajectory planner. */ -int tpSetAmax(TP_STRUCT * const tp, double aMax) -{ - if (0 == tp || aMax <= 0.0) { - return TP_ERR_FAIL; - } - - tp->aMax = aMax; + tp->vLimit = fmax(vLimit, 0.0); + tp->vLimitAng = fmax(vLimitAng, 0.0); + tp_debug_print("Setting Vlimit %f %f\n", tp->vLimit, tp->vLimitAng); return TP_ERR_OK; } @@ -620,9 +463,27 @@ int tpSetId(TP_STRUCT * const tp, int id) return TP_ERR_OK; } +tc_unique_id_t tpGetNextUniqueId(TP_STRUCT * const tp) +{ + return tp->nextUniqueId++; +} + +void tcSetId(TP_STRUCT * const tp, TC_STRUCT * const tc, struct state_tag_t tag) +{ + // KLUDGE always number the possible blend arcs sequentially + blend_tc1.unique_id = tpGetNextUniqueId(tp); + blend_tc2.unique_id = tpGetNextUniqueId(tp); + tc->unique_id = tpGetNextUniqueId(tp); + blend_tc1.id = blend_tc2.id = tc->id = tp->nextId; + tc->tag = tag; + blend_tc1.tag = tag; + blend_tc2.tag = tag; +} + + /** Returns the id of the last motion that is currently executing.*/ -int tpGetExecId(TP_STRUCT * const tp) +int tpGetExecId(const TP_STRUCT * const tp) { if (0 == tp) { return TP_ERR_FAIL; @@ -631,7 +492,31 @@ int tpGetExecId(TP_STRUCT * const tp) return tp->execId; } -struct state_tag_t tpGetExecTag(TP_STRUCT * const tp) +int tpGetCompletedId(const TP_STRUCT * const tp) +{ + if (!tp) { + return 0; + } + + //Ugly but direct approach + //Alternative is to store ID locally + return tp->tc_completed_id; +} + +int tpGetQueuedId(TP_STRUCT const * const tp) +{ + if (!tp) { + return 0; + } + + TC_STRUCT const * const tc_last = tcqLast(&tp->queue); + if (!tc_last) { + return 0; + } + return tc_last->id; +} + +struct state_tag_t tpGetExecTag(TP_STRUCT const * const tp) { if (0 == tp) { struct state_tag_t empty = {}; @@ -641,6 +526,22 @@ struct state_tag_t tpGetExecTag(TP_STRUCT * const tp) return tp->execTag; } +int tpGetExecSrcLine(const TP_STRUCT * const tp) +{ + if (tp) { + return tp->execTag.fields[GM_FIELD_LINE_NUMBER]; + } + return 0; +} + +int tpGetNextExecId(const TP_STRUCT * const tp) +{ + if (0 == tp) { + return TP_ERR_FAIL; + } + + return tp->nextexecId; +} /** * Sets the termination condition for all subsequent queued moves. @@ -648,11 +549,9 @@ struct state_tag_t tpGetExecTag(TP_STRUCT * const tp) * begins. If cond is TC_TERM_COND_PARABOLIC, the following move is begun when the * current move slows below a calculated blend velocity. */ -int tpSetTermCond(TP_STRUCT * const tp, int cond, double tolerance) +int tpSetTermCond(TP_STRUCT * const tp, tc_term_cond_t cond, double tolerance) { - if (!tp) { - return TP_ERR_FAIL; - } + tp_err_t res = TP_ERR_FAIL; switch (cond) { //Purposeful waterfall for now @@ -660,15 +559,15 @@ int tpSetTermCond(TP_STRUCT * const tp, int cond, double tolerance) case TC_TERM_COND_TANGENT: case TC_TERM_COND_EXACT: case TC_TERM_COND_STOP: + if (tp) { tp->termCond = cond; tp->tolerance = tolerance; + res = TP_ERR_OK; + } break; - default: - //Invalid condition - return -1; } - return TP_ERR_OK; + return res; } /** @@ -687,7 +586,7 @@ int tpSetPos(TP_STRUCT * const tp, EmcPose const * const pos) return TP_ERR_FAIL; } - tp->goalPos = *pos; + emcPoseToPmVector(pos, &tp->goalPos); return TP_ERR_OK; } @@ -704,7 +603,7 @@ int tpSetCurrentPos(TP_STRUCT * const tp, EmcPose const * const pos) } if (emcPoseValid(pos)) { - tp->currentPos = *pos; + emcPoseToPmVector(pos, &tp->currentPos); return TP_ERR_OK; } else { rtapi_print_msg(RTAPI_MSG_ERR, "Tried to set invalid pose in tpSetCurrentPos on id %d!" @@ -717,28 +616,26 @@ int tpSetCurrentPos(TP_STRUCT * const tp, EmcPose const * const pos) } } - -int tpAddCurrentPos(TP_STRUCT * const tp, EmcPose const * const disp) +int tpAddCurrentPos(TP_STRUCT * const tp, PmVector const * const disp) { if (!tp || !disp) { return TP_ERR_MISSING_INPUT; } - if (emcPoseValid(disp)) { - emcPoseSelfAdd(&tp->currentPos, disp); + if (!VecHasNAN(disp)) { + VecVecAddEq(&tp->currentPos, disp); return TP_ERR_OK; } else { rtapi_print_msg(RTAPI_MSG_ERR, "Tried to set invalid pose in tpAddCurrentPos on id %d!" "disp is %.12g, %.12g, %.12g\n", tp->execId, - disp->tran.x, - disp->tran.y, - disp->tran.z); + disp->ax[0], + disp->ax[1], + disp->ax[2]); return TP_ERR_INVALID; } } - /** * Check for valid tp before queueing additional moves. */ @@ -761,7 +658,7 @@ int tpErrorCheck(TP_STRUCT const * const tp) { * This is used to estimate blend velocity, though by itself is not enough * (since requested velocity and max velocity could be lower). */ -STATIC double tpCalculateTriangleVel(TC_STRUCT const *tc) { +double tpCalculateTriangleVel(TC_STRUCT const *tc) { //Compute peak velocity for blend calculations double acc_scaled = tcGetTangentialMaxAccel(tc); double length = tc->target; @@ -772,55 +669,60 @@ STATIC double tpCalculateTriangleVel(TC_STRUCT const *tc) { return findVPeak(acc_scaled, length); } -/** - * Handles the special case of blending into an unfinalized segment. - * The problem here is that the last segment in the queue can always be cut - * short by a blend to the next segment. However, we can only ever consume at - * most 1/2 of the segment. This function computes the worst-case final - * velocity the previous segment can have, if we want to exactly stop at the - * halfway point. - */ -STATIC double tpCalculateOptimizationInitialVel(TP_STRUCT const * const tp, TC_STRUCT * const tc) + +double calculateOptimizationInitialVel(TC_STRUCT const * const prev_tc, TC_STRUCT const * const tc) { double acc_scaled = tcGetTangentialMaxAccel(tc); double triangle_vel = findVPeak(acc_scaled, tc->target); - double max_vel = tpGetMaxTargetVel(tp, tc); - tp_debug_json_start(tpCalculateOptimizationInitialVel); - tp_debug_json_double(triangle_vel); - tp_debug_json_end(); - return fmin(triangle_vel, max_vel); + double v_max_this = tcGetPlanMaxTargetVel(tc, emcmotConfig->maxFeedScale); + double v_max_prev = tcGetPlanMaxTargetVel(prev_tc, emcmotConfig->maxFeedScale); + double optim_init_vel = fmin(fmin(triangle_vel, v_max_this), v_max_prev); + +#ifdef TP_DEBUG + print_json5_log_start(calculateOptimizationInitialVel); + print_json5_double(triangle_vel); + print_json5_double(optim_init_vel); + print_json5_log_end(); +#endif + return optim_init_vel; } /** - * Initialize a blend arc from its parent segments. - * This copies and initializes properties from the previous and next segments to + * Initialize a blend arc from its parent lines. + * This copies and initializes properties from the previous and next lines to * initialize a blend arc. This function does not handle connecting the * segments together, however. */ -STATIC int tpInitBlendArcFromPrev(TP_STRUCT const * const tp, - TC_STRUCT const * const prev_tc, - TC_STRUCT* const blend_tc, - double vel, - double ini_maxvel, - double acc) -{ +int tpInitBlendArcFromAdjacent( + TP_STRUCT const * const tp, + TC_STRUCT const * const adjacent_tc, + TC_STRUCT* const blend_tc, + double vel, + double ini_maxvel, + double acc, + tc_motion_type_t motion_type) { #ifdef TP_SHOW_BLENDS int canon_motion_type = EMC_MOTION_TYPE_ARC; #else - int canon_motion_type = prev_tc->canon_motion_type; + int canon_motion_type = adjacent_tc->canon_motion_type; #endif tcInit(blend_tc, - TC_SPHERICAL, + motion_type, canon_motion_type, tp->cycleTime, - prev_tc->enables, + adjacent_tc->enables, false); // NOTE: blend arc never needs the atspeed flag, since the previous line will have it (and cannot be consumed). + blend_tc->tag = adjacent_tc->tag; + // KLUDGE tell an observer that the current segment is generated by the TP as part of a blend + blend_tc->tag.packed_flags |= (0x1 << GM_FLAG_TP_BLEND); + // Copy over state data from TP tcSetupState(blend_tc, tp); + blend_tc->term_cond = TC_TERM_COND_TANGENT; // Set kinematics parameters from blend calculations tcSetupMotion(blend_tc, @@ -828,31 +730,32 @@ STATIC int tpInitBlendArcFromPrev(TP_STRUCT const * const tp, ini_maxvel, acc); - // Skip syncdio setup since this blend extends the previous line - blend_tc->syncdio = // enqueue the list of DIOs - prev_tc->syncdio; // that need toggling - // find "helix" length for target double length; - arcLength(&blend_tc->coords.arc.xyz, &length); - tp_info_print("blend tc length = %f\n",length); + switch (motion_type) { + case TC_LINEAR: + length = blend_tc->coords.line.tmag; + break; + case TC_SPHERICAL: + length = arc9Length(&blend_tc->coords.arc); + break; + default: + return TP_ERR_FAIL; + } blend_tc->target = length; blend_tc->nominal_length = length; - - // Set the blend arc to be tangent to the next segment - tcSetTermCond(blend_tc, NULL, TC_TERM_COND_TANGENT); + blend_tc->syncdio = adjacent_tc->syncdio; //NOTE: blend arc radius and everything else is finalized, so set this to 1. //In the future, radius may be adjustable. - tcFinalizeLength(blend_tc); - - // copy state tag from previous segment during blend motion - blend_tc->tag = prev_tc->tag; + CHP(tcFinalizeLength(blend_tc, emcmotConfig->maxFeedScale)); return TP_ERR_OK; } -STATIC int tcSetLineXYZ(TC_STRUCT * const tc, PmCartLine const * const line) +#if 0 +//TODO +int tcSetLineXYZ(TC_STRUCT * const tc, PmCartLine const * const line) { //Update targets with new arc length @@ -864,21 +767,20 @@ STATIC int tcSetLineXYZ(TC_STRUCT * const tc, PmCartLine const * const line) return TP_ERR_FAIL; } - tc->coords.line.xyz = *line; + tc->coords.line = *line; tc->target = line->tmag; return TP_ERR_OK; } +#endif - -static inline int find_max_element(double arr[], int sz) +int find_max_element(double arr[], int sz) { if (sz < 1) { return -1; } // Assumes at least one element int max_idx = 0; - int idx; - for (idx = 0; idx < sz; ++idx) { + for (int idx = 0; idx < sz; ++idx) { if (arr[idx] > arr[max_idx]) { max_idx = idx; } @@ -886,6 +788,26 @@ static inline int find_max_element(double arr[], int sz) return max_idx; } +const char *blendTypeAsString(tc_blend_type_t c) +{ + switch(c) { + case NO_BLEND: + return "NO_BLEND"; + case PARABOLIC_BLEND: + return "PARABOLIC_BLEND"; + case TANGENT_SEGMENTS_BLEND: + return "TANGENT_SEGMENTS"; + case ARC_BLEND: + return "ARC_BLEND"; + } + return ""; +} + +bool isArcBlendFaster(TC_STRUCT const * const prev_tc, double expected_v_max) +{ + return fmax(prev_tc->parabolic_equiv_vel, prev_tc->kink_vel) < expected_v_max; +} + /** * Compare performance of blend arc and equivalent tangent speed. * If we can go faster by assuming the segments are already tangent (and @@ -893,694 +815,498 @@ static inline int find_max_element(double arr[], int sz) * mostly useful for some odd arc-to-arc cases where the blend arc becomes very * short (and therefore slow). */ -STATIC tc_blend_type_t tpChooseBestBlend(TP_STRUCT const * const tp, +tc_blend_type_t tpChooseBestBlend( TC_STRUCT * const prev_tc, TC_STRUCT * const tc, - TC_STRUCT * const blend_tc) + double arc_blend_maxvel) { if (!tc || !prev_tc) { return NO_BLEND; } - // Can't blend segments that are explicitly disallowed switch (prev_tc->term_cond) { - case TC_TERM_COND_EXACT: case TC_TERM_COND_STOP: + case TC_TERM_COND_EXACT: + // Can't blend segments that are explicitly disallowed return NO_BLEND; + case TC_TERM_COND_PARABOLIC: + case TC_TERM_COND_TANGENT: + break; } - // Compute performance measures ("perf_xxx") for each method. This is - // basically the blend velocity. However, because parabolic blends require - // halving the acceleration of both blended segments, they in effect slow - // down the next and previous blends as well. We model this loss by scaling - // the blend velocity down to find an "equivalent" velocity. - double perf_parabolic = estimateParabolicBlendPerformance(tp, prev_tc, tc) / 2.0; - double perf_tangent = prev_tc->kink_vel; - double perf_arc_blend = blend_tc ? blend_tc->maxvel : 0.0; - - tp_debug_print("Blend performance: parabolic %f, tangent %f, arc_blend %f, ", - perf_parabolic, - perf_tangent, - perf_arc_blend); - // KLUDGE Order the performance measurements so that they match the enum values - double perf[3] = {perf_parabolic, perf_tangent, perf_arc_blend}; - tc_blend_type_t best_blend = find_max_element(perf, 3); + double perf[] = { + 0.0, // No parabolic blends anymore + prev_tc->kink_vel, + arc_blend_maxvel, + }; + + tc_blend_type_t best_blend = find_max_element(perf, sizeof (perf)/sizeof (double)); switch (best_blend) { - case PARABOLIC_BLEND: // parabolic - tp_debug_print("using parabolic blend\n"); - tcRemoveKinkProperties(prev_tc, tc); - tcSetTermCond(prev_tc, tc, TC_TERM_COND_PARABOLIC); - break; case TANGENT_SEGMENTS_BLEND: // tangent - tp_debug_print("using approximate tangent blend\n"); // NOTE: acceleration / velocity reduction is done dynamically in functions that access TC_STRUCT properties - tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); + tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); + prev_tc->use_kink = 1; break; case ARC_BLEND: // arc blend - tp_debug_print("using blend arc\n"); - tcRemoveKinkProperties(prev_tc, tc); - break; case NO_BLEND: + case PARABOLIC_BLEND: + tcSetTermCond(prev_tc, TC_TERM_COND_STOP); break; } + +#ifdef TP_DEBUG + print_json5_log_start(tpChooseBestBlend); + print_json5_double_("perf_parabolic", perf[PARABOLIC_BLEND]); + print_json5_double_("perf_tangent", perf[TANGENT_SEGMENTS_BLEND]); + print_json5_double_("perf_arc_blend", perf[ARC_BLEND]); + print_json5_string_("best_blend", blendTypeAsString(best_blend)); + print_json5_log_end(); +#endif + return best_blend; } - -STATIC tp_err_t tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc) +bool checkBiarcContinuity( + TC_STRUCT const * const prev_tc, + TC_STRUCT const * const this_tc) { - tp_debug_print("-- Starting LineArc blend arc --\n"); - - PmCartesian acc_bound, vel_bound; - - //Get machine limits - tpGetMachineAccelBounds(&acc_bound); - tpGetMachineVelBounds(&vel_bound); - - //Populate blend geometry struct - BlendGeom3 geom; - BlendParameters param; - BlendPoints3 points_approx; - BlendPoints3 points_exact; - - int res_init = blendInit3FromLineArc(&geom, ¶m, - prev_tc, - tc, - &acc_bound, - &vel_bound, - emcmotConfig->maxFeedScale); + ContinuityCheck c1 = calc_C1_continuity(prev_tc, &blend_tc1); + ContinuityCheck cmid = calc_C1_continuity(&blend_tc1, &blend_tc2); + ContinuityCheck c2 = calc_C1_continuity(&blend_tc2, this_tc); + double const min_dot_limit = 1.0 - emcmotConfig->consistencyCheckConfig.continuityAngleLimit_rad; + bool c1_error = c1.dot < min_dot_limit + || cmid.dot < min_dot_limit + || c2.dot < min_dot_limit; + + if (c1_error) { + rtapi_print_msg(RTAPI_MSG_WARN, "BiArc blend C1 continuity exceeds limits at line %d, unique_id %lld\n", prev_tc->tag.fields[GM_FIELD_LINE_NUMBER], prev_tc->unique_id); + } + if (_tp_debug || c1_error) + { + print_json5_log_start(checkBiarcContinuity); + print_json5_long_long_("prev_unique_id", prev_tc->unique_id); + print_json5_string_("b1_motion_type", tcMotionTypeAsString(prev_tc->motion_type)); + print_json5_string_("b2_motion_type", tcMotionTypeAsString(this_tc->motion_type)); + print_json5_PmVector_("u1_diff", c1.u_diff); + print_json5_PmVector_("p1_diff", c1.p_diff); + print_json5_double_("u1_diff_dot", c1.dot); + print_json5_PmVector_("u_mid_diff", cmid.u_diff); + print_json5_PmVector_("p_mid_diff", cmid.p_diff); + print_json5_double_("u_mid_diff_dot", cmid.dot); + print_json5_PmVector_("u2_diff", c2.u_diff); + print_json5_double_("u2_diff_dot", c2.dot); + print_json5_PmVector_("p2_diff", c2.p_diff); + print_json5_log_end(); + } + return !c1_error; +} + +tp_err_t tpCreateBiarcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const this_tc) +{ +#ifdef TP_DEBUG + double prev_len = prev_tc->target; + double this_len = this_tc->target; + int prev_can_consume = tcCanConsume(prev_tc); + int this_can_consume = tcCanConsume(this_tc); + + double maxvel = fmin( + tcGetPlanMaxTargetVel(prev_tc, emcmotConfig->biarc_solver_cfg.feed_override_allowance), + tcGetPlanMaxTargetVel(this_tc, emcmotConfig->biarc_solver_cfg.feed_override_allowance)); + double reqvel = fmin(prev_tc->reqvel, this_tc->reqvel); + tc_motion_type_t prev_motion_type = prev_tc->motion_type; + tc_motion_type_t this_motion_type = this_tc->motion_type; + PmVector u1_stretch={}, u2_stretch={}; + PmVector p1_stretch={}, p2_stretch={}; +#endif - if (res_init != TP_ERR_OK) { - tp_debug_print("blend init failed with code %d, aborting blend arc\n", - res_init); - return res_init; + PmVector vel_bound = getVelBounds(); + PmVector acc_bound = getAccelBounds(); + + static biarc_solver_results_t biarc_results = {}; + memset(&biarc_results, 0, sizeof(biarc_solver_results_t)); + + BlendParameters param_guess = {}; + BlendControls controls = {}; + CHP(optimize_biarc_blend_size( + prev_tc, + this_tc, + &emcmotConfig->biarc_solver_cfg, + &vel_bound, + &acc_bound, + &biarc_results, + &controls, + ¶m_guess, + tp->cycleTime)); + + biarc_control_points_t const * const control_pts = &biarc_results.solution.control_pts; + blend_boundary_t const * const boundary = &biarc_results.solution.boundary; + + BlendPoints points1 = {}, points2 = {}; + BlendParameters param1 = {}, param2 = {}; + + CHP(find_biarc_points_from_solution( + &boundary->u1, + &control_pts->u_mid, + &boundary->P1, + &control_pts->P_mid, + &control_pts->Pb1, + control_pts->d, + &acc_bound, + &vel_bound, + &controls, + &points1, + ¶m1)); + + // Check if previous segment can be consumed + blendCheckConsume(¶m1, boundary->s1, prev_tc, emcmotConfig->arc_blend_cfg.gap_cycles); + if (param1.consume && blend_tc1.motion_type == TC_LINEAR) { + // KLUDGE force the start point to be the previous line's starting point since it's going to be consumed + points1.arc_start = prev_tc->coords.line.start; + } + CHP(init_blend_segment_from_points(&blend_tc1, &points1, ¶m1)); + + CHP(find_biarc_points_from_solution( + &control_pts->u_mid, + &boundary->u2, + &control_pts->P_mid, + &boundary->P2, + &control_pts->Pb2, + control_pts->d, + &acc_bound, + &vel_bound, + &controls, + &points2, + ¶m2)); + CHP(init_blend_segment_from_points(&blend_tc2, &points2, ¶m2)); + + // Don't bother if another solution is better + double expected_v_max = fmin(param1.v_plan, param2.v_plan); + if (emcmotConfig->arc_blend_cfg.allow_fallback && !isArcBlendFaster(prev_tc, expected_v_max)) { + return TP_ERR_NO_ACTION; } - // Check for coplanarity based on binormal and tangents - int coplanar = pmUnitCartsColinear(&geom.binormal, - &tc->coords.circle.xyz.normal); + union {PmLine9 line9; PmCircle9 circle9;} prev_geom={}, this_geom={}; - if (!coplanar) { - tp_debug_print("aborting arc, not coplanar\n"); - return TP_ERR_FAIL; + switch (prev_tc->motion_type) { + case TC_LINEAR: + prev_geom.line9 = prev_tc->coords.line; + double new_len = boundary->s1; + pmLine9Cut(&prev_geom.line9, new_len, 0); + break; + case TC_CIRCULAR: + { + prev_geom.circle9 = prev_tc->coords.circle; + pmCircle9Cut(&prev_geom.circle9, boundary->s1, 0); + break; } - - int res_param = blendComputeParameters(¶m); - - int res_points = blendFindPoints3(&points_approx, &geom, ¶m); - - int res_post = blendLineArcPostProcess(&points_exact, - &points_approx, - ¶m, - &geom, &prev_tc->coords.line.xyz, - &tc->coords.circle.xyz); - - //Catch errors in blend setup - if (res_init || res_param || res_points || res_post) { - tp_debug_print("Got %d, %d, %d, %d for init, param, points, post, aborting arc\n", - res_init, - res_param, - res_points, - res_post); + default: return TP_ERR_FAIL; } - /* If blend calculations were successful, then we're ready to create the - * blend arc. - */ + switch (this_tc->motion_type) { + case TC_LINEAR: + this_geom.line9 = this_tc->coords.line; + double new_len = boundary->s2; + pmLine9Cut(&this_geom.line9, new_len, 1); + break; + case TC_CIRCULAR: + { + this_geom.circle9 = this_tc->coords.circle; + pmCircle9Cut(&this_geom.circle9, boundary->s2, 1); + break; + } + default: + return TP_ERR_FAIL; + } + + CHP(tpInitBlendArcFromAdjacent( + tp, + prev_tc, + &blend_tc1, + controls.v_req, + fmin(param1.v_plan, fmax(param1.v_max_planar, param1.v_max_altitude)), + param1.a_max_planar, + blend_tc1.motion_type)); + CHP(tpInitBlendArcFromAdjacent( + tp, + this_tc, + &blend_tc2, + controls.v_req, + fmin(param2.v_plan, fmax(param2.v_max_planar, param2.v_max_altitude)), + param2.a_max_planar, + blend_tc2.motion_type)); + + // Passed all the pre-checks, ready to commit to changing the line segments + if (prev_tc->motion_type == TC_LINEAR) { + tcSetLine9(prev_tc, &prev_geom.line9); + } else if (prev_tc->motion_type == TC_CIRCULAR) { + tcSetCircle9(prev_tc, &prev_geom.circle9); + } + + if (this_tc->motion_type == TC_LINEAR) { + tcSetLine9(this_tc, &this_geom.line9); + } else if (this_tc->motion_type == TC_CIRCULAR) { + tcSetCircle9(this_tc, &this_geom.circle9); + } + + // Update end conditions + tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); + tcSetTermCond(&blend_tc1,TC_TERM_COND_TANGENT); + tcSetTermCond(&blend_tc2, TC_TERM_COND_TANGENT); + + // The DIO's only change if the previous segment is consumed + blend_tc1.syncdio.anychanged &= param1.consume; + // Second arc should follow DIO's of the following segment (since the midpoint is the closest to the ideal endpoint) + // DIOs have already changed by the time the next segment starts, so ignore changes here + this_tc->syncdio.anychanged = 0; + + if (_tp_debug || needConsistencyCheck(CCHECK_C1_CONTINUITY)) { + checkBiarcContinuity(prev_tc, this_tc); + } + +#ifdef TP_DEBUG + tp_debug_json5_log_start(tpCreateBiarcBlend); + if (param1.consume) { + tcGetStartTangentUnitVector(prev_tc, &u1_stretch); + tcGetStartpoint(prev_tc, &p1_stretch); + } else { + tcGetEndTangentUnitVector(prev_tc, &u1_stretch); + tcGetEndpoint(prev_tc, &p1_stretch); + } + tcGetStartTangentUnitVector(this_tc, &u2_stretch); + tcGetStartpoint(this_tc, &p2_stretch); + print_json5_double(prev_len); + print_json5_double(this_len); + print_json5_double_("prev_len_new", prev_tc->target); + print_json5_double_("this_len_new", this_tc->target); + print_json5_int(prev_can_consume); + print_json5_int(this_can_consume); + print_json5_double_("param_line_length", param1.line_length); + print_json5_double_("line_length1", blend_tc1.coords.arc.line_length); + print_json5_double_("line_length2", blend_tc2.coords.arc.line_length); + print_json5_int(prev_motion_type); + print_json5_int(this_motion_type); + print_json5_long_long_("src_line", this_tc->tag.fields[GM_FIELD_LINE_NUMBER]); + print_json5_long_long_("unique_id1", blend_tc1.unique_id); + print_json5_long_long_("unique_id2", blend_tc2.unique_id); + print_json5_biarc_solution_t("solution", &biarc_results.solution); + print_json5_PmVector(u1_stretch); + print_json5_PmVector(u2_stretch); + print_json5_PmVector_("p1_stretch", p1_stretch); + PmVector p11_blend; + tcGetStartpoint(&blend_tc1, &p11_blend); + PmVector p12_blend; + tcGetEndpoint(&blend_tc1, &p12_blend); + PmVector p21_blend; + tcGetStartpoint(&blend_tc2, &p21_blend); + PmVector p22_blend; + tcGetEndpoint(&blend_tc2, &p22_blend); + print_json5_PmVector_("p11_blend", p11_blend); + print_json5_PmVector_("p12_blend", p12_blend); + print_json5_PmVector_("p21_blend", p21_blend); + print_json5_PmVector_("p22_blend", p22_blend); + print_json5_PmVector_("p2_stretch", p2_stretch); + print_json5_biarc_control_points_t("control_pts", &biarc_results.solution.control_pts); + print_json5_blend_boundary_t("boundary", &biarc_results.solution.boundary); + print_json5_int_("iterations", biarc_results.iterations); + print_json5_string_("result", biarc_result_to_str(biarc_results.result)); + print_json5_TC_STRUCT_kinematics("blend1", &blend_tc1); + print_json5_TC_STRUCT_kinematics("blend2", &blend_tc2); + print_json5_double(maxvel); + print_json5_double(reqvel); + print_json5_bool_("consume", param1.consume); + print_json5_SphericalArc9("arc1", &blend_tc1.coords.arc); + print_json5_SphericalArc9("arc2", &blend_tc2.coords.arc); + print_json5_end_(); +#endif - if (points_exact.trim2 > param.phi2_max) { - tp_debug_print("trim2 %f > phi2_max %f, aborting arc...\n", - points_exact.trim2, - param.phi2_max); - return TP_ERR_FAIL; + if (param1.consume) { + if (tcqPopBack(&tp->queue)) { + //This is unrecoverable since we've already changed the line. Something is wrong if we get here... + tpStopWithError(tp, "Motion internal error: previous motion segment at line %d (id %d) is missing, trajectory is invalid", this_tc->tag.fields[GM_FIELD_LINE_NUMBER], this_tc->id); + return TP_ERR_UNRECOVERABLE; + } } - blendCheckConsume(¶m, &points_exact, prev_tc, emcmotConfig->arcBlendGapCycles); - //Store working copies of geometry - PmCartLine line1_temp = prev_tc->coords.line.xyz; - PmCircle circ2_temp = tc->coords.circle.xyz; - - // Change lengths of circles - double new_len1 = line1_temp.tmag - points_exact.trim1; - int res_stretch1 = pmCartLineStretch(&line1_temp, - new_len1, - false); + return TP_ERR_OK; +} - double phi2_new = tc->coords.circle.xyz.angle - points_exact.trim2; +/** + * Add a newly created motion segment to the tp queue. + * Returns an error code if the queue operation fails, otherwise adds a new + * segment to the queue and updates the end point of the trajectory planner. + */ +int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc) { - tp_debug_print("phi2_new = %f\n",phi2_new); - int res_stretch2 = pmCircleStretch(&circ2_temp, - phi2_new, - true); - //TODO create blends - if (res_stretch1 || res_stretch2) { - tp_debug_print("segment resize failed, aborting arc\n"); + if (tcqPut(&tp->queue, tc) == -1) { + tpStopWithError(tp, "Motion internal error: tcqPut() failed"); return TP_ERR_FAIL; } - //Get exact start and end points to account for spiral in arcs - pmCartLinePoint(&line1_temp, - line1_temp.tmag, - &points_exact.arc_start); - pmCirclePoint(&circ2_temp, - 0.0, - &points_exact.arc_end); - //TODO deal with large spiral values, or else detect and fall back? - - blendPoints3Print(&points_exact); - int res_arc = arcFromBlendPoints3(&blend_tc->coords.arc.xyz, - &points_exact, - &geom, - ¶m); - if (res_arc < 0) { - tp_debug_print("arc creation failed, aborting arc\n"); - return TP_ERR_FAIL; + // Store end of current move as new final goal of TP + // KLUDGE: endpoint is garbage for rigid tap since it's supposed to retract past the start point. + if (tc->motion_type != TC_RIGIDTAP) { + tcGetEndpoint(tc, &tp->goalPos); } - - // Note that previous restrictions don't allow ABC or UVW movement, so the - // end and start points should be identical - blend_tc->coords.arc.abc = prev_tc->coords.line.abc.end; - blend_tc->coords.arc.uvw = prev_tc->coords.line.uvw.end; - - //set the max velocity to v_plan, since we'll violate constraints otherwise. - tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req, - param.v_plan, param.a_max); - - int res_tangent = checkTangentAngle(&circ2_temp, - &blend_tc->coords.arc.xyz, - &geom, - ¶m, - tp->cycleTime, - true); - - if (res_tangent < 0) { - tp_debug_print("failed tangent check, aborting arc...\n"); - return TP_ERR_FAIL; - } - - if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) { - return TP_ERR_NO_ACTION; - } - - tp_debug_print("Passed all tests, updating segments\n"); - //TODO refactor to pass consume to connect function - if (param.consume) { - //Since we're consuming the previous segment, pop the last line off of the queue - int res_pop = tcqPopBack(&tp->queue); - if (res_pop) { - tp_debug_print("failed to pop segment, aborting arc\n"); - return TP_ERR_FAIL; - } - } else { - tcSetLineXYZ(prev_tc, &line1_temp); - //KLUDGE the previous segment is still there, so we don't need the at-speed flag on the blend too - blend_tc->atspeed=0; - } - tcSetCircleXYZ(tc, &circ2_temp); - - tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); + tp->done = 0; + tp->depth = tcqLen(&tp->queue); + //Fixing issue with duplicate id's? +#ifdef TP_DEBUG + print_json5_log_start(Enqueue); + print_json5_tc_id_data_(tc); + print_json5_string_("motion_type", tcMotionTypeAsString(tc->motion_type)); + print_json5_double_("target", tc->target); + print_json5_string_("term_cond", tcTermCondAsString(tc->term_cond)); + print_json5_end_(); +#endif return TP_ERR_OK; } - -STATIC tp_err_t tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc) +int handlePrevTermCondition(TC_STRUCT *prev_tc, TC_STRUCT *tc) { - - tp_debug_print("-- Starting ArcLine blend arc --\n"); - PmCartesian acc_bound, vel_bound; - - //Get machine limits - tpGetMachineAccelBounds(&acc_bound); - tpGetMachineVelBounds(&vel_bound); - - //Populate blend geometry struct - BlendGeom3 geom; - BlendParameters param; - BlendPoints3 points_approx; - BlendPoints3 points_exact; - param.consume = 0; - - int res_init = blendInit3FromArcLine(&geom, ¶m, - prev_tc, - tc, - &acc_bound, - &vel_bound, - emcmotConfig->maxFeedScale); - if (res_init != TP_ERR_OK) { - tp_debug_print("blend init failed with code %d, aborting blend arc\n", - res_init); - return res_init; - } - - // Check for coplanarity based on binormal - int coplanar = pmUnitCartsColinear(&geom.binormal, - &prev_tc->coords.circle.xyz.normal); - - if (!coplanar) { - tp_debug_print("aborting arc, not coplanar\n"); - return TP_ERR_FAIL; - } - - int res_param = blendComputeParameters(¶m); - - int res_points = blendFindPoints3(&points_approx, &geom, ¶m); - - int res_post = blendArcLinePostProcess(&points_exact, - &points_approx, - ¶m, - &geom, &prev_tc->coords.circle.xyz, - &tc->coords.line.xyz); - - //Catch errors in blend setup - if (res_init || res_param || res_points || res_post) { - tp_debug_print("Got %d, %d, %d, %d for init, param, points, post\n", - res_init, - res_param, - res_points, - res_post); - return TP_ERR_FAIL; - } - - blendCheckConsume(¶m, &points_exact, prev_tc, emcmotConfig->arcBlendGapCycles); - - /* If blend calculations were successful, then we're ready to create the - * blend arc. - */ - - // Store working copies of geometry - PmCircle circ1_temp = prev_tc->coords.circle.xyz; - PmCartLine line2_temp = tc->coords.line.xyz; - - // Update start and end points of segment copies - double phi1_new = circ1_temp.angle - points_exact.trim1; - - if (points_exact.trim1 > param.phi1_max) { - tp_debug_print("trim1 %f > phi1_max %f, aborting arc...\n", - points_exact.trim1, - param.phi1_max); - return TP_ERR_FAIL; - } - - int res_stretch1 = pmCircleStretch(&circ1_temp, - phi1_new, - false); - if (res_stretch1 != TP_ERR_OK) { - return TP_ERR_FAIL; - } - - double new_len2 = tc->target - points_exact.trim2; - int res_stretch2 = pmCartLineStretch(&line2_temp, - new_len2, - true); - - if (res_stretch1 || res_stretch2) { - tp_debug_print("segment resize failed, aborting arc\n"); - return TP_ERR_FAIL; - } - - pmCirclePoint(&circ1_temp, - circ1_temp.angle, - &points_exact.arc_start); - - pmCartLinePoint(&line2_temp, - 0.0, - &points_exact.arc_end); - - blendPoints3Print(&points_exact); - - int res_arc = arcFromBlendPoints3(&blend_tc->coords.arc.xyz, &points_exact, &geom, ¶m); - if (res_arc < 0) { + if (!tc) { return TP_ERR_FAIL; } - // Note that previous restrictions don't allow ABC or UVW movement, so the - // end and start points should be identical - blend_tc->coords.arc.abc = tc->coords.line.abc.start; - blend_tc->coords.arc.uvw = tc->coords.line.uvw.start; - - //set the max velocity to v_plan, since we'll violate constraints otherwise. - tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req, - param.v_plan, param.a_max); - - int res_tangent = checkTangentAngle(&circ1_temp, &blend_tc->coords.arc.xyz, &geom, ¶m, tp->cycleTime, false); - if (res_tangent) { - tp_debug_print("failed tangent check, aborting arc...\n"); - return TP_ERR_FAIL; + switch (tc->motion_type) { + case TC_RIGIDTAP: + if (prev_tc && prev_tc->motion_type != TC_RIGIDTAP) { + tcSetTermCond(prev_tc, TC_TERM_COND_STOP); + } + // Rigidtap motions are always exact-path to allow pecking + tcSetTermCond(tc, TC_TERM_COND_EXACT); + break; + case TC_LINEAR: + case TC_CIRCULAR: + case TC_SPHERICAL: + { + tc_term_cond_t prev_term = prev_tc ? prev_tc->term_cond : TC_TERM_COND_STOP; + tcSetTermCond(prev_tc, prev_term); } - - if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) { - return TP_ERR_NO_ACTION; + break; + case TC_DWELL: + // Previous motion must do an exact stop at the dwell point since that's the whole point + tcSetTermCond(tc, TC_TERM_COND_STOP); + return TP_ERR_OK; } - - tp_debug_print("Passed all tests, updating segments\n"); - - tcSetCircleXYZ(prev_tc, &circ1_temp); - tcSetLineXYZ(tc, &line2_temp); - - //Cleanup any mess from parabolic - tc->blend_prev = 0; - blend_tc->atspeed=0; - tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); return TP_ERR_OK; } -STATIC tp_err_t tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc) +int handleModeChange(TC_STRUCT *prev_tc, TC_STRUCT *tc) { - - tp_debug_print("-- Starting ArcArc blend arc --\n"); - //TODO type checks - int colinear = pmUnitCartsColinear(&prev_tc->coords.circle.xyz.normal, - &tc->coords.circle.xyz.normal); - if (!colinear) { - // Fail out if not collinear - tp_debug_print("arc abort: not coplanar\n"); - return TP_ERR_FAIL; - } - - PmCartesian acc_bound, vel_bound; - - //Get machine limits - tpGetMachineAccelBounds(&acc_bound); - tpGetMachineVelBounds(&vel_bound); - - //Populate blend geometry struct - BlendGeom3 geom; - BlendParameters param; - BlendPoints3 points_approx; - BlendPoints3 points_exact; - - int res_init = blendInit3FromArcArc(&geom, ¶m, - prev_tc, - tc, - &acc_bound, - &vel_bound, - emcmotConfig->maxFeedScale); - - if (res_init != TP_ERR_OK) { - tp_debug_print("blend init failed with code %d, aborting blend arc\n", - res_init); - return res_init; - } - - int coplanar1 = pmUnitCartsColinear(&geom.binormal, - &prev_tc->coords.circle.xyz.normal); - - if (!coplanar1) { - tp_debug_print("aborting blend arc, arc id %d is not coplanar with binormal\n", prev_tc->id); - return TP_ERR_FAIL; - } - - int coplanar2 = pmUnitCartsColinear(&geom.binormal, - &tc->coords.circle.xyz.normal); - if (!coplanar2) { - tp_debug_print("aborting blend arc, arc id %d is not coplanar with binormal\n", tc->id); - return TP_ERR_FAIL; - } - - - - int res_param = blendComputeParameters(¶m); - int res_points = blendFindPoints3(&points_approx, &geom, ¶m); - - int res_post = blendArcArcPostProcess(&points_exact, - &points_approx, - ¶m, - &geom, &prev_tc->coords.circle.xyz, - &tc->coords.circle.xyz); - - //Catch errors in blend setup - if (res_init || res_param || res_points || res_post) { - tp_debug_print("Got %d, %d, %d, %d for init, param, points, post\n", - res_init, - res_param, - res_points, - res_post); - - return TP_ERR_FAIL; - } - - blendCheckConsume(¶m, &points_exact, prev_tc, emcmotConfig->arcBlendGapCycles); - - /* If blend calculations were successful, then we're ready to create the - * blend arc. Begin work on temp copies of each circle here: - */ - - double phi1_new = prev_tc->coords.circle.xyz.angle - points_exact.trim1; - double phi2_new = tc->coords.circle.xyz.angle - points_exact.trim2; - - // TODO pare down this debug output - tp_debug_print("phi1_new = %f, trim1 = %f\n", phi1_new, points_exact.trim1); - tp_debug_print("phi2_new = %f, trim2 = %f\n", phi2_new, points_exact.trim2); - - if (points_exact.trim1 > param.phi1_max) { - tp_debug_print("trim1 %f > phi1_max %f, aborting arc...\n", - points_exact.trim1, - param.phi1_max); - return TP_ERR_FAIL; - } - - if (points_exact.trim2 > param.phi2_max) { - tp_debug_print("trim2 %f > phi2_max %f, aborting arc...\n", - points_exact.trim2, - param.phi2_max); - return TP_ERR_FAIL; - } - - //Store working copies of geometry - PmCircle circ1_temp = prev_tc->coords.circle.xyz; - PmCircle circ2_temp = tc->coords.circle.xyz; - - int res_stretch1 = pmCircleStretch(&circ1_temp, - phi1_new, - false); - if (res_stretch1 != TP_ERR_OK) { - return TP_ERR_FAIL; - } - - int res_stretch2 = pmCircleStretch(&circ2_temp, - phi2_new, - true); - if (res_stretch1 || res_stretch2) { - tp_debug_print("segment resize failed, aborting arc\n"); + if (!tc || !prev_tc) { return TP_ERR_FAIL; } - //Get exact start and end points to account for spiral in arcs - pmCirclePoint(&circ1_temp, - circ1_temp.angle, - &points_exact.arc_start); - pmCirclePoint(&circ2_temp, - 0.0, - &points_exact.arc_end); - - tp_debug_print("Modified arc points\n"); - blendPoints3Print(&points_exact); - int res_arc = arcFromBlendPoints3(&blend_tc->coords.arc.xyz, &points_exact, &geom, ¶m); - if (res_arc < 0) { - return TP_ERR_FAIL; + // Can't blend across feed / rapid transitions + if ((prev_tc->canon_motion_type == EMC_MOTION_TYPE_TRAVERSE) ^ + (tc->canon_motion_type == EMC_MOTION_TYPE_TRAVERSE)) { + tp_debug_print("Blending disabled: rapid / feed transition must follow exact path\n"); + tcSetTermCond(prev_tc, TC_TERM_COND_EXACT); + if (tc->canon_motion_type == EMC_MOTION_TYPE_TRAVERSE) { + // Force retraction moves to follow exact path (to avoid gouging after a hole retraction) + tcSetTermCond(tc, TC_TERM_COND_EXACT); + } } - // Note that previous restrictions don't allow ABC or UVW movement, so the - // end and start points should be identical - blend_tc->coords.arc.abc = prev_tc->coords.circle.abc.end; - blend_tc->coords.arc.uvw = prev_tc->coords.circle.uvw.end; - - //set the max velocity to v_plan, since we'll violate constraints otherwise. - tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req, - param.v_plan, param.a_max); - - int res_tangent1 = checkTangentAngle(&circ1_temp, &blend_tc->coords.arc.xyz, &geom, ¶m, tp->cycleTime, false); - int res_tangent2 = checkTangentAngle(&circ2_temp, &blend_tc->coords.arc.xyz, &geom, ¶m, tp->cycleTime, true); - if (res_tangent1 || res_tangent2) { - tp_debug_print("failed tangent check, aborting arc...\n"); - return TP_ERR_FAIL; + if (prev_tc->synchronized != TC_SYNC_POSITION && + tc->synchronized == TC_SYNC_POSITION) { + tp_debug_print("Blending disabled: entering position-sync mode\n"); + // NOTE: blending IS allowed when exiting position sync mode, so be + // careful of tolerances here to avoid making thicker threads + // (particularly if ;the lead-out is perpendicular) + return tcSetTermCond(prev_tc, TC_TERM_COND_STOP); } - if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) { - return TP_ERR_NO_ACTION; + if(tc->atspeed) { + // Need to wait for spindle before starting tc, so it's not safe to + // allow any blending from the previous to current segment + tp_debug_print("Blending disabled: waiting on spindle atspeed for tc %d, unique_id %llu\n", + tc->id, tc->unique_id); + return tcSetTermCond(prev_tc, TC_TERM_COND_STOP); } - tp_debug_print("Passed all tests, updating segments\n"); - - tcSetCircleXYZ(prev_tc, &circ1_temp); - tcSetCircleXYZ(tc, &circ2_temp); - - //Cleanup any mess from parabolic - tc->blend_prev = 0; - blend_tc->atspeed=0; - tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); return TP_ERR_OK; } - -STATIC tp_err_t tpCreateLineLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, - TC_STRUCT * const tc, TC_STRUCT * const blend_tc) -{ - - tp_debug_print("-- Starting LineLine blend arc --\n"); - PmCartesian acc_bound, vel_bound; - - //Get machine limits - tpGetMachineAccelBounds(&acc_bound); - tpGetMachineVelBounds(&vel_bound); - - // Setup blend data structures - BlendGeom3 geom; - BlendParameters param; - BlendPoints3 points; - - int res_init = blendInit3FromLineLine(&geom, ¶m, - prev_tc, - tc, - &acc_bound, - &vel_bound, - emcmotConfig->maxFeedScale); - - if (res_init != TP_ERR_OK) { - tp_debug_print("blend init failed with code %d, aborting blend arc\n", - res_init); - return res_init; - } - - int res_blend = blendComputeParameters(¶m); - if (res_blend != TP_ERR_OK) { - return res_blend; - } - - blendFindPoints3(&points, &geom, ¶m); - - blendCheckConsume(¶m, &points, prev_tc, emcmotConfig->arcBlendGapCycles); - - // Set up actual blend arc here - int res_arc = arcFromBlendPoints3(&blend_tc->coords.arc.xyz, &points, &geom, ¶m); - if (res_arc < 0) { - return TP_ERR_FAIL; - } - - // Note that previous restrictions don't allow ABC or UVW movement, so the - // end and start points should be identical - blend_tc->coords.arc.abc = prev_tc->coords.line.abc.end; - blend_tc->coords.arc.uvw = prev_tc->coords.line.uvw.end; - - //set the max velocity to v_plan, since we'll violate constraints otherwise. - tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req, - param.v_plan, param.a_max); - - tp_debug_print("blend_tc target_vel = %g\n", blend_tc->target_vel); - - if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) { - return TP_ERR_NO_ACTION; - } - - int retval = TP_ERR_FAIL; - - //TODO refactor to pass consume to connect function - if (param.consume) { - //Since we're consuming the previous segment, pop the last line off of the queue - retval = tcqPopBack(&tp->queue); - if (retval) { - //This is unrecoverable since we've already changed the line. Something is wrong if we get here... - rtapi_print_msg(RTAPI_MSG_ERR, "PopBack failed\n"); - return TP_ERR_FAIL; - } - //Since the blend arc meets the end of the previous line, we only need - //to "connect" to the next line - retval = tcConnectBlendArc(NULL, tc, &points.arc_start, &points.arc_end); +int tpSetupSyncedIO(TP_STRUCT * const tp, TC_STRUCT * const tc) { + if (tp->syncdio.anychanged != 0) { + tc->syncdio = tp->syncdio; //enqueue the list of DIOs that need toggling + tpClearDIOs(tp); // clear out the list, in order to prepare for the next time we need to use it + return TP_ERR_OK; } else { - //TODO refactor connect function to stretch lines and check for bad stretching - tp_debug_print("keeping previous line\n"); - retval = tcConnectBlendArc(prev_tc, tc, &points.arc_start, &points.arc_end); - blend_tc->atspeed=0; + tc->syncdio.anychanged = 0; + return TP_ERR_NO_ACTION; } - return retval; } +tp_err_t tpFinalizeAndEnqueue(TP_STRUCT * const tp, TC_STRUCT * const tc, PmVector const *nominal_goal) +{ + //TODO refactor this into its own function + TC_STRUCT *prev_tc; + prev_tc = tcqLast(&tp->queue); -/** - * Add a newly created motion segment to the tp queue. - * Returns an error code if the queue operation fails, otherwise adds a new - * segment to the queue and updates the end point of the trajectory planner. - */ -STATIC inline int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc, int inc_id) { + // Make sure the blending flags are consistent w/ previous segment + handlePrevTermCondition(prev_tc, tc); - tc->id = tp->nextId; - if (tcqPut(&tp->queue, tc) == -1) { - rtapi_print_msg(RTAPI_MSG_ERR, "tcqPut failed.\n"); - return TP_ERR_FAIL; - } - if (inc_id) { - tp->nextId++; - } + // Prevent blends for specific mode changes (where blending isn't possible anyway) + handleModeChange(prev_tc, tc); - // Store end of current move as new final goal of TP - // KLUDGE: endpoint is garbage for rigid tap since it's supposed to retract past the start point. - if (tc->motion_type != TC_RIGIDTAP) { - tcGetEndpoint(tc, &tp->goalPos); + CHP(tpSetupSegmentBlend(tp, prev_tc, tc)); + // KLUDGE order is important here, the parabolic blend check has to + // happen after all other steps that affect the terminal condition + CHP(tcFinalizeLength(prev_tc, emcmotConfig->maxFeedScale)); + + CHP(tpAddSegmentToQueue(tp, tc)); + //Run speed optimization (will abort safely if there are no tangent segments) + tpRunOptimization(tp); + if (nominal_goal) { + // Update the goal position so that future motions know what their planned start point is + tp->goalPos = *nominal_goal; } - tp->done = 0; - tp->depth = tcqLen(&tp->queue); - //Fixing issue with duplicate id's? - tp_debug_print("Adding TC id %d of type %d, total length %0.08f\n",tc->id,tc->motion_type,tc->target); return TP_ERR_OK; } -STATIC int handleModeChange(TC_STRUCT * const prev_tc, TC_STRUCT * const tc) +void clearBlendStructs() { - if (!tc || !prev_tc) { - return TP_ERR_FAIL; - } - if ((prev_tc->canon_motion_type == EMC_MOTION_TYPE_TRAVERSE) ^ - (tc->canon_motion_type == EMC_MOTION_TYPE_TRAVERSE)) { - tp_debug_print("Blending disabled: can't blend between rapid and feed motions\n"); - tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP); - } - if (prev_tc->synchronized != TC_SYNC_POSITION && - tc->synchronized == TC_SYNC_POSITION) { - tp_debug_print("Blending disabled: changing spindle sync mode from %d to %d\n", - prev_tc->synchronized, - tc->synchronized); - tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP); - } - return TP_ERR_OK; + memset(&blend_tc1, 0, sizeof(TC_STRUCT)); + memset(&blend_tc2, 0, sizeof(TC_STRUCT)); } -STATIC int tpSetupSyncedIO(TP_STRUCT * const tp, TC_STRUCT * const tc) { - if (tp->syncdio.anychanged != 0) { - tc->syncdio = tp->syncdio; //enqueue the list of DIOs that need toggling - tpClearDIOs(tp); // clear out the list, in order to prepare for the next time we need to use it - return TP_ERR_OK; - } else { - tc->syncdio.anychanged = 0; - return TP_ERR_NO_ACTION; - } - - +static void addRigidTapOverrun(TC_STRUCT * const tc, double revolutions, double uu_per_rev) +{ + PmCartLine *actual_xyz = &tc->coords.rigidtap.actual_xyz; + pmCartLineStretch(actual_xyz, actual_xyz->tmag + revolutions * uu_per_rev, 0); + tc->target = actual_xyz->tmag; } - /** * Adds a rigid tap cycle to the motion queue. */ -int tpAddRigidTap(TP_STRUCT * const tp, - EmcPose end, - double vel, - double ini_maxvel, - double acc, - unsigned char enables, - double scale, - struct state_tag_t tag) { - +int tpAddRigidTap( + TP_STRUCT * const tp, + EmcPose end_p, + double tap_uu_per_rev, + double retract_uu_per_rev, + double ini_maxvel, + double acc, + unsigned char enables, + char atspeed, + double scale, + struct state_tag_t tag) { if (tpErrorCheck(tp)) { return TP_ERR_FAIL; } + PmVector end; + emcPoseToPmVector(&end_p, &end); + tp_info_print("== AddRigidTap ==\n"); if(!tp->synchronized) { @@ -1588,19 +1314,20 @@ int tpAddRigidTap(TP_STRUCT * const tp, return TP_ERR_FAIL; } - TC_STRUCT tc = {0}; + TC_STRUCT tc = {}; + clearBlendStructs(); + tcSetId(tp, &tc, tag); /* Initialize rigid tap move. * NOTE: rigid tapping does not have a canonical type. * NOTE: always need atspeed since this is a synchronized movement. * */ tcInit(&tc, TC_RIGIDTAP, - 2, + 0, tp->cycleTime, enables, - 1); - tc.tag = tag; + atspeed); // Setup any synced IO for this move tpSetupSyncedIO(tp, &tc); @@ -1610,72 +1337,60 @@ int tpAddRigidTap(TP_STRUCT * const tp, // Copy in motion parameters tcSetupMotion(&tc, - vel, + ini_maxvel, ini_maxvel, acc); + tc.coords.rigidtap.tap_uu_per_rev = tap_uu_per_rev; + tc.coords.rigidtap.retract_uu_per_rev = retract_uu_per_rev; + // Setup rigid tap geometry - pmRigidTapInit(&tc.coords.rigidtap, - &tp->goalPos, - &end, scale); - tc.target = pmRigidTapTarget(&tc.coords.rigidtap, tp->uu_per_rev); + CHP(pmRigidTapInit( + &tc.coords.rigidtap, + &tp->goalPos, + &end, + scale)); - // Force exact stop mode after rigid tapping regardless of TP setting - tcSetTermCond(&tc, NULL, TC_TERM_COND_STOP); + addRigidTapOverrun(&tc, RIGIDTAP_MAX_OVERSHOOT_REVS, tc.coords.rigidtap.tap_uu_per_rev); - TC_STRUCT *prev_tc; - //Assume non-zero error code is failure - prev_tc = tcqLast(&tp->queue); - tcFinalizeLength(prev_tc); - tcFlagEarlyStop(prev_tc, &tc); - int retval = tpAddSegmentToQueue(tp, &tc, true); - tpRunOptimization(tp); - return retval; + // Do NOT update the goal position with a rigid tap move + return tpFinalizeAndEnqueue(tp, &tc, NULL); } -STATIC blend_type_t tpCheckBlendArcType( - TC_STRUCT const * const prev_tc, - TC_STRUCT const * const tc) { - - if (!prev_tc || !tc) { - tp_debug_print("prev_tc or tc doesn't exist\n"); - return BLEND_NONE; - } +int tpAddDwell(TP_STRUCT * const tp, double time_sec, double delta_rpm, struct state_tag_t tag) +{ + // NOTE: it's not possible to dwell for less than 1 servo timestep due to how dwells are implemented + TC_STRUCT tc={}; + tcInit(&tc, + TC_DWELL, + 0, + tp->cycleTime, + 0, + 0); + tc.motion_type = TC_DWELL; + tc.coords.dwell.dwell_time_req = fmax(time_sec, tp->cycleTime); + tc.coords.dwell.delta_rpm_req = fabs(delta_rpm); + tc.coords.dwell.dwell_pos = tp->goalPos; + // Initialize with invalid values (dwell time computed at segment activation) + tc.coords.dwell.dwell_time = -1; + tc.coords.dwell.remaining_time = -1; + tc.term_cond = TC_TERM_COND_STOP; + tc.tag = tag; + tpAddSegmentToQueue(tp, &tc); - //If exact stop, we don't compute the arc - if (prev_tc->term_cond != TC_TERM_COND_PARABOLIC) { - tp_debug_print("Wrong term cond = %d\n", prev_tc->term_cond); - return BLEND_NONE; - } + return TP_ERR_OK; +} - //If we have any rotary axis motion, then don't create a blend arc - if (tcRotaryMotionCheck(tc) || tcRotaryMotionCheck(prev_tc)) { - tp_debug_print("One of the segments has rotary motion, aborting blend arc\n"); - return BLEND_NONE; - } - if (tc->finalized || prev_tc->finalized) { - tp_debug_print("Can't create blend when segment lengths are finalized\n"); - return BLEND_NONE; - } - - tp_debug_print("Motion types: prev_tc = %d, tc = %d\n", - prev_tc->motion_type,tc->motion_type); - //If not linear blends, we can't easily compute an arc - if ((prev_tc->motion_type == TC_LINEAR) && (tc->motion_type == TC_LINEAR)) { - return BLEND_LINE_LINE; - } else if (prev_tc->motion_type == TC_LINEAR && tc->motion_type == TC_CIRCULAR) { - return BLEND_LINE_ARC; - } else if (prev_tc->motion_type == TC_CIRCULAR && tc->motion_type == TC_LINEAR) { - return BLEND_ARC_LINE; - } else if (prev_tc->motion_type == TC_CIRCULAR && tc->motion_type == TC_CIRCULAR) { - return BLEND_ARC_ARC; - } else { - return BLEND_NONE; +static double applyKinkVelLimit(TC_STRUCT const * const tc, double vel_in) +{ + if (tc->use_kink && tc->kink_vel >= 0 && tc->term_cond == TC_TERM_COND_TANGENT) { + // Only care about kink_vel with tangent segments that have not been arc blended + return fmin(vel_in, tc->kink_vel); } + return vel_in; } - /** * Based on the nth and (n-1)th segment, find a safe final velocity for the (n-1)th segment. * This function also caps the target velocity if velocity ramping is enabled. If we @@ -1683,7 +1398,7 @@ STATIC blend_type_t tpCheckBlendArcType( * acceleration) will speed up and slow down to reach their target velocity, * creating "humps" in the velocity profile. */ -STATIC int tpComputeOptimalVelocity(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT * const prev1_tc) { +int tpComputeOptimalVelocity(TC_STRUCT * const tc, TC_STRUCT * const prev1_tc) { //Calculate the maximum starting velocity vs_back of segment tc, given the //trajectory parameters double acc_this = tcGetTangentialMaxAccel(tc); @@ -1692,12 +1407,9 @@ STATIC int tpComputeOptimalVelocity(TP_STRUCT const * const tp, TC_STRUCT * cons double vs_back = pmSqrt(pmSq(tc->finalvel) + 2.0 * acc_this * tc->target); // Find the reachable velocity of prev1_tc, moving forwards in time - double vf_limit_this = tc->maxvel; - double vf_limit_prev = prev1_tc->maxvel; - if (prev1_tc->kink_vel >=0 && prev1_tc->term_cond == TC_TERM_COND_TANGENT) { - // Only care about kink_vel with tangent segments - vf_limit_prev = fmin(vf_limit_prev, prev1_tc->kink_vel); - } + double vf_limit_this = tcGetPlanMaxTargetVel(tc, emcmotConfig->maxFeedScale); + double v_max_prev = tcGetPlanMaxTargetVel(prev1_tc, emcmotConfig->maxFeedScale); + double vf_limit_prev = applyKinkVelLimit(prev1_tc, v_max_prev); //Limit the PREVIOUS velocity by how much we can overshoot into double vf_limit = fmin(vf_limit_this, vf_limit_prev); @@ -1711,10 +1423,6 @@ STATIC int tpComputeOptimalVelocity(TP_STRUCT const * const tp, TC_STRUCT * cons //Limit tc's target velocity to avoid creating "humps" in the velocity profile prev1_tc->finalvel = vs_back; - //Reduce max velocity to match sample rate - double sample_maxvel = tc->target / (tp->cycleTime * TP_MIN_SEGMENT_CYCLES); - tc->maxvel = fmin(tc->maxvel, sample_maxvel); - tp_info_print(" prev1_tc-> fv = %f, tc->fv = %f\n", prev1_tc->finalvel, tc->finalvel); @@ -1727,13 +1435,13 @@ STATIC int tpComputeOptimalVelocity(TP_STRUCT const * const tp, TC_STRUCT * cons * Walk along the queue from the back to the front. Based on the "current" * segment's final velocity, calculate the previous segment's maximum allowable * final velocity. The depth we walk along the queue is controlled by the - * TP_LOOKAHEAD_DEPTH constant for now. The process safely aborts early due to + * TP_LOOKAHEAD_DEPTH constant for now. The process safetly aborts early due to * a short queue or other conflicts. */ -STATIC int tpRunOptimization(TP_STRUCT * const tp) { +int tpRunOptimization(TP_STRUCT * const tp) { // Pointers to the "current", previous, and 2nd previous trajectory // components. Current in this context means the segment being optimized, - // NOT the currently executing segment. + // NOT the currently excecuting segment. TC_STRUCT *tc; TC_STRUCT *prev1_tc; @@ -1750,7 +1458,7 @@ STATIC int tpRunOptimization(TP_STRUCT * const tp) { * the front. We can't do anything with the very last element because its * length may change if a new line is added to the queue.*/ - for (x = 1; x < emcmotConfig->arcBlendOptDepth + 2; ++x) { + for (x = 1; x < emcmotConfig->arc_blend_cfg.optimization_depth + 2; ++x) { tp_info_print("==== Optimization step %d ====\n",x); // Update the pointers to the trajectory segments in use @@ -1771,7 +1479,7 @@ STATIC int tpRunOptimization(TP_STRUCT * const tp) { tp_debug_print("Found 2nd non-tangent segment, stopping optimization\n"); return TP_ERR_OK; } else { - tp_debug_print("Found first non-tangent segment, continuing\n"); + tp_debug_print("Found first non-tangent segment, contining\n"); hit_non_tangent = true; continue; } @@ -1788,7 +1496,7 @@ STATIC int tpRunOptimization(TP_STRUCT * const tp) { } //Somewhat pedantic check for other conditions that would make blending unsafe - if (prev1_tc->splitting || prev1_tc->blending_next) { + if (prev1_tc->splitting) { tp_debug_print("segment %d is already blending, cannot optimize safely!\n", ind-1); return TP_ERR_OK; @@ -1802,23 +1510,21 @@ STATIC int tpRunOptimization(TP_STRUCT * const tp) { if (tc->atspeed) { //Assume worst case that we have a stop at this point. This may cause a //slight hiccup, but the alternative is a sudden hard stop. - tp_debug_print("Found atspeed at id %d\n",tc->id); - tc->finalvel = 0.0; - } - - if (!tc->finalized) { - tp_debug_print("Segment %d, type %d not finalized, continuing\n",tc->id,tc->motion_type); + tp_debug_print("Found atspeed at segment %lld\n", tc->unique_id); + prev1_tc->finalvel = 0.0; + } else if (tc->finalized) { + tpComputeOptimalVelocity(tc, prev1_tc); + } else { + tp_debug_print("Segment %lld, type %d not finalized, continuing\n", tc->unique_id, tc->motion_type); // use worst-case final velocity that allows for up to 1/2 of a segment to be consumed. - prev1_tc->finalvel = fmin(prev1_tc->maxvel, tpCalculateOptimizationInitialVel(tp,tc)); + prev1_tc->finalvel = calculateOptimizationInitialVel(prev1_tc, tc); // Fixes acceleration violations when last segment is not finalized, and previous segment is tangent. - if (prev1_tc->kink_vel >=0 && prev1_tc->term_cond == TC_TERM_COND_TANGENT) { - prev1_tc->finalvel = fmin(prev1_tc->finalvel, prev1_tc->kink_vel); - } + tc->finalvel = 0.0; - } else { - tpComputeOptimalVelocity(tp, tc, prev1_tc); + prev1_tc->finalvel = applyKinkVelLimit(prev1_tc, prev1_tc->finalvel); + } tc->active_depth = x - 2 - hit_peaks; @@ -1843,146 +1549,139 @@ STATIC int tpRunOptimization(TP_STRUCT * const tp) { * segment as tangent, and limit the current segment's velocity by the sampling * rate. */ -STATIC int tpSetupTangent(TP_STRUCT const * const tp, +TCIntersectType tpSetupTangent(TP_STRUCT const * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc) { + tp_debug_json5_log_start(tpSetupTangent); if (!tc || !prev_tc) { - tp_debug_print("missing tc or prev tc in tangent check\n"); - return TP_ERR_FAIL; + tp_debug_json5_log_end("missing tc or prev tc in tangent check"); + return TC_INTERSECT_INCOMPATIBLE; } - //If we have ABCUVW movement, then don't check for tangency - if (tcRotaryMotionCheck(tc) || tcRotaryMotionCheck(prev_tc)) { - tp_debug_print("found rotary axis motion\n"); - return TP_ERR_FAIL; - } - - if (emcmotConfig->arcBlendOptDepth < 2) { - tp_debug_print("Optimization depth %d too low for tangent optimization\n", - emcmotConfig->arcBlendOptDepth); - return TP_ERR_FAIL; + if (emcmotConfig->arc_blend_cfg.optimization_depth < 2) { + tp_debug_json5_log_end("Optimization depth %d too low for tangent optimization", + emcmotConfig->arc_blend_cfg.optimization_depth); + return TC_INTERSECT_INCOMPATIBLE; } if (prev_tc->term_cond == TC_TERM_COND_STOP) { - tp_debug_print("Found exact stop condition\n"); - return TP_ERR_FAIL; + tp_debug_json5_log_end("Found exact stop condition"); + return TC_INTERSECT_INCOMPATIBLE; } - PmCartesian prev_tan, this_tan; + if (prev_tc->indexrotary != INDEX_NONE || tc->indexrotary != INDEX_NONE) { + tp_debug_json5_log_end("rotary axis move requires indexing"); + tcSetTermCond(prev_tc, TC_TERM_COND_STOP); + return TC_INTERSECT_INCOMPATIBLE; + } - int res_endtan = tcGetEndTangentUnitVector(prev_tc, &prev_tan); - int res_starttan = tcGetStartTangentUnitVector(tc, &this_tan); - if (res_endtan || res_starttan) { - tp_debug_print("Got %d and %d from tangent vector calc\n", - res_endtan, res_starttan); + // TODO see if this can go after the tangent check + if (prev_tc->progress > prev_tc->target / 2.0) { + tp_debug_json5_log_end(" prev_tc progress (%f) is too large, aborting blend arc\n", prev_tc->progress); + return TC_INTERSECT_INCOMPATIBLE; } + PmVector prev_tan, this_tan; - tp_debug_print("prev tangent vector: %f %f %f\n", prev_tan.x, prev_tan.y, prev_tan.z); - tp_debug_print("this tangent vector: %f %f %f\n", this_tan.x, this_tan.y, this_tan.z); + CHP(tcGetEndTangentUnitVector(prev_tc, &prev_tan)); + CHP(tcGetStartTangentUnitVector(tc, &this_tan)); + + tp_debug_json5_PmVector(prev_tan); + tp_debug_json5_PmVector(this_tan); // Assume small angle approximation here const double SHARP_CORNER_DEG = 2.0; const double SHARP_CORNER_EPSILON = pmSq(PM_PI * ( SHARP_CORNER_DEG / 180.0)); - if (pmCartCartAntiParallel(&prev_tan, &this_tan, SHARP_CORNER_EPSILON)) + + if (VecVecUnitAntiParallel(&prev_tan, &this_tan, SHARP_CORNER_EPSILON)) { - tp_debug_print("Found sharp corner\n"); - tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP); - return TP_ERR_FAIL; + tp_debug_json5_log_end("Found sharp corner"); + tcSetTermCond(prev_tc, TC_TERM_COND_STOP); + return TC_INTERSECT_INCOMPATIBLE; } // Calculate instantaneous acceleration required for change in direction // from v1 to v2, assuming constant speed - double v_max1 = tcGetMaxTargetVel(prev_tc, getMaxFeedScale(prev_tc)); - double v_max2 = tcGetMaxTargetVel(tc, getMaxFeedScale(tc)); + double v_max1 = tcGetPlanMaxTargetVel(prev_tc, emcmotConfig->maxFeedScale); + double v_max2 = tcGetPlanMaxTargetVel(tc, emcmotConfig->maxFeedScale); // Note that this is a minimum since the velocity at the intersection must // be the slower of the two segments not to violate constraints. - double v_max = fmin(v_max1, v_max2); - tp_debug_print("tangent v_max = %f\n",v_max); + double v_max_tangent = fmin(v_max1, v_max2); + tp_debug_json5_double(v_max_tangent); // Account for acceleration past final velocity during a split cycle // (e.g. next segment starts accelerating again so the average velocity is higher at the end of the split cycle) - double a_inst = v_max / tp->cycleTime + tc->maxaccel; + double a_inst = v_max_tangent / tp->cycleTime + tc->maxaccel; + // Set up worst-case final velocity // Compute the actual magnitude of acceleration required given the tangent directions // Do this by assuming that we decelerate to a stop on the previous segment, // and simultaneously accelerate up to the maximum speed on the next one. - PmCartesian acc1, acc2, acc_diff; - pmCartScalMult(&prev_tan, a_inst, &acc1); - pmCartScalMult(&this_tan, a_inst, &acc2); - pmCartCartSub(&acc2,&acc1,&acc_diff); - - //TODO store this in TP struct instead? - PmCartesian acc_bound; - tpGetMachineAccelBounds(&acc_bound); - - PmCartesian acc_scale; - findAccelScale(&acc_diff,&acc_bound,&acc_scale); - tp_debug_print("acc_diff: %f %f %f\n", - acc_diff.x, - acc_diff.y, - acc_diff.z); - tp_debug_print("acc_scale: %f %f %f\n", - acc_scale.x, - acc_scale.y, - acc_scale.z); - - //FIXME this ratio is arbitrary, should be more easily tunable - double acc_scale_max = pmCartAbsMax(&acc_scale); - //KLUDGE lumping a few calculations together here + PmVector acc1, acc2, acc_diff; + VecScalMult(&prev_tan, a_inst, &acc1); + VecScalMult(&this_tan, a_inst, &acc2); + VecVecSub(&acc2, &acc1, &acc_diff); + + tp_debug_json5_PmVector(acc1); + tp_debug_json5_PmVector(acc2); + tp_debug_json5_PmVector(acc_diff); + + PmVector acc_bound = getAccelBounds(); + + PmVector acc_scales; + findAccelScale(&acc_diff, &acc_bound, &acc_scales); + double acc_scale_max = VecAbsMax(&acc_scales); + if (prev_tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_CIRCULAR) { acc_scale_max /= BLEND_ACC_RATIO_TANGENTIAL; } + tp_debug_json5_double(acc_scale_max); // Controls the tradeoff between reduction of final velocity, and reduction of allowed segment acceleration // TODO: this should ideally depend on some function of segment length and acceleration for better optimization const double kink_ratio = tpGetTangentKinkRatio(); + tp_debug_json5_double(kink_ratio); + if (acc_scale_max < kink_ratio) { - tp_debug_print(" Kink acceleration within %g, using tangent blend\n", kink_ratio); - tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT); - tcSetKinkProperties(prev_tc, tc, v_max, acc_scale_max); - return TP_ERR_OK; + tp_debug_json5_log_end(" Kink acceleration within %g, using tangent blend", kink_ratio); + tcSetTermCond(prev_tc, TC_TERM_COND_TANGENT); + tcSetKinkProperties(prev_tc, tc, v_max_tangent, acc_scale_max); + return TC_INTERSECT_TANGENT; } else { - tcSetKinkProperties(prev_tc, tc, v_max * kink_ratio / acc_scale_max, kink_ratio); - tp_debug_print("Kink acceleration scale %f above %f, kink vel = %f, blend arc may be faster\n", - acc_scale_max, - kink_ratio, - prev_tc->kink_vel); - // NOTE: acceleration will be reduced later if tangent blend is used - return TP_ERR_NO_ACTION; + switch (prev_tc->term_cond) { + case TC_TERM_COND_STOP: + case TC_TERM_COND_EXACT: + tp_debug_print(" Corner too sharp for exact-path, forcing exact stop "); + return TC_INTERSECT_INCOMPATIBLE; + case TC_TERM_COND_PARABOLIC: + case TC_TERM_COND_TANGENT: + break; + } + } + if (prev_tc->term_cond == TC_TERM_COND_EXACT) { + tp_debug_json5_log_end("Found exact path condition with non-tangent intersection"); + return TC_INTERSECT_INCOMPATIBLE; } -} -static bool tpCreateBlendIfPossible( - TP_STRUCT *tp, - TC_STRUCT *prev_tc, - TC_STRUCT *tc, - TC_STRUCT *blend_tc) -{ - tp_err_t res_create = TP_ERR_FAIL; - blend_type_t blend_requested = tpCheckBlendArcType(prev_tc, tc); + tcSetKinkProperties(prev_tc, tc, v_max_tangent * kink_ratio / acc_scale_max, kink_ratio); - switch (blend_requested) { - case BLEND_LINE_LINE: - res_create = tpCreateLineLineBlend(tp, prev_tc, tc, blend_tc); - break; - case BLEND_LINE_ARC: - res_create = tpCreateLineArcBlend(tp, prev_tc, tc, blend_tc); - break; - case BLEND_ARC_LINE: - res_create = tpCreateArcLineBlend(tp, prev_tc, tc, blend_tc); - break; - case BLEND_ARC_ARC: - res_create = tpCreateArcArcBlend(tp, prev_tc, tc, blend_tc); - break; - case BLEND_NONE: - default: - tp_debug_print("intersection type not recognized, aborting arc\n"); - res_create = TP_ERR_FAIL; - break; +#ifdef TP_DEBUG + print_json5_double_field(prev_tc, kink_vel); +#endif + + if (!emcmotConfig->arc_blend_cfg.enable) { + tp_debug_json5_log_end("Arc blending disabled"); + return TC_INTERSECT_INCOMPATIBLE; } - return res_create == TP_ERR_OK; -} + if (tc->finalized || prev_tc->finalized) { + tp_debug_json5_log_end("Can't create blend when segment lengths are finalized"); + return TC_INTERSECT_INCOMPATIBLE; + } + tp_debug_json5_log_end("Nontangent intersection needs blend"); + + // NOTE: acceleration will be reduced later if tangent blend is used + return TC_INTERSECT_NONTANGENT; +} /** * Handle creating a blend arc when a new line segment is about to enter the queue. @@ -1990,127 +1689,119 @@ static bool tpCreateBlendIfPossible( * blend arc. Essentially all of the blend arc functions are called through * here to isolate the process. */ -STATIC tc_blend_type_t tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) { +int tpSetupSegmentBlend( + TP_STRUCT * const tp, + TC_STRUCT * const prev_tc, + TC_STRUCT * const tc) { - tp_debug_print("*****************************************\n** Handle Blend Arc **\n"); + // Check the intersection type and handle tangency if the two segments are very nearly tangent + TCIntersectType res_tan = tpSetupTangent(tp, prev_tc, tc); - TC_STRUCT *prev_tc; - prev_tc = tcqLast(&tp->queue); - - //If the previous segment has already started, then don't create a blend - //arc for the next pair. - // TODO May be able to lift this restriction if we can ensure that we leave - // 1 timestep's worth of distance in prev_tc - if ( !prev_tc) { - tp_debug_print(" queue empty\n"); - return NO_BLEND; - } - if (prev_tc->progress > prev_tc->target / 2.0) { - tp_debug_print(" prev_tc progress (%f) is too large, aborting blend arc\n", prev_tc->progress); - return NO_BLEND; + int blends_added = 0; + if (res_tan == TC_INTERSECT_NONTANGENT) { + blends_added = tpCreateBiarcBlend(tp, prev_tc, tc) == TP_ERR_OK ? 2 : 0; } - // Check for tangency between segments and handle any errors - // TODO possibly refactor this into a macro? - int res_tan = tpSetupTangent(tp, prev_tc, tc); - switch (res_tan) { - // Abort blend arc creation in these cases - case TP_ERR_FAIL: - tp_debug_print(" tpSetupTangent failed, aborting blend arc\n"); - case TP_ERR_OK: - return res_tan; - case TP_ERR_NO_ACTION: - default: - //Continue with creation - break; - } - - TC_STRUCT blend_tc = {0}; - - tc_blend_type_t blend_used = NO_BLEND; - - bool arc_blend_ok = tpCreateBlendIfPossible(tp, prev_tc, tc, &blend_tc); - - if (arc_blend_ok) { - //Need to do this here since the length changed - blend_used = ARC_BLEND; - tpAddSegmentToQueue(tp, &blend_tc, false); - } else { - // If blend arc creation failed early on, catch it here and find the best blend - blend_used = tpChooseBestBlend(tp, prev_tc, tc, NULL) ; + switch (blends_added) { + case 0: + tpChooseBestBlend(prev_tc, tc, 0.0); + return TP_ERR_OK; + case 1: + CHP(tpAddSegmentToQueue(tp, &blend_tc1)); + return TP_ERR_OK; + case 2: + CHP(tpAddSegmentToQueue(tp, &blend_tc1)); + CHP(tpAddSegmentToQueue(tp, &blend_tc2)); + return TP_ERR_OK; } - - return blend_used; + return TP_ERR_FAIL; } -//TODO final setup steps as separate functions -// /** * Add a straight line to the tc queue. * end of the previous move to the new end specified here at the * currently-active accel and vel settings from the tp struct. */ - -int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, - double vel, double ini_maxvel, double acc, unsigned char enables, - char atspeed, int indexer_jnum, struct state_tag_t tag) +int tpAddLine( + TP_STRUCT * const tp, + EmcPose end_p, + EMCMotionTypes canon_motion_type, + double vel, + double ini_maxvel, + double acc, + unsigned char enables, + char atspeed, + IndexRotaryAxis indexrotary, + struct state_tag_t tag) { + PmVector end; + emcPoseToPmVector(&end_p, &end); if (tpErrorCheck(tp) < 0) { return TP_ERR_FAIL; } - tp_info_print("== AddLine ==\n"); - // Initialize new tc struct for the line segment - TC_STRUCT tc = {0}; + TC_STRUCT tc = {}; + + clearBlendStructs(); + tcSetId(tp, &tc, tag); + tcInit(&tc, TC_LINEAR, canon_motion_type, tp->cycleTime, enables, atspeed); - tc.tag = tag; // Setup any synced IO for this move tpSetupSyncedIO(tp, &tc); // Copy over state data from the trajectory planner tcSetupState(&tc, tp); + tcSetTermCond(&tc, tp->termCond); // Copy in motion parameters tcSetupMotion(&tc, vel, ini_maxvel, acc); + // Setup line geometry - pmLine9Init(&tc.coords.line, - &tp->goalPos, - &end); - tc.target = pmLine9Target(&tc.coords.line); - if (tc.target < TP_POS_EPSILON) { - rtapi_print_msg(RTAPI_MSG_DBG,"failed to create line id %d, zero-length segment\n",tp->nextId); - return TP_ERR_ZERO_LENGTH; - } + pmLine9Init( + &tc.coords.line, + &tp->goalPos, + &end); + tc.target = pmLine9Length(&tc.coords.line); tc.nominal_length = tc.target; - tcClampVelocityByLength(&tc); + tc.indexrotary = indexrotary; - // For linear move, set joint corresponding to a locking indexer axis - tc.indexer_jnum = indexer_jnum; - - //TODO refactor this into its own function - TC_STRUCT *prev_tc; - prev_tc = tcqLast(&tp->queue); - handleModeChange(prev_tc, &tc); - if (emcmotConfig->arcBlendEnable){ - tpHandleBlendArc(tp, &tc); +#ifdef TP_DEBUG + { + // macros use the variable name, need a plain name to please the JSON5 parser + print_json5_log_start(tpAddLine); + print_json5_tc_id_data_(&tc); + print_json5_PmVector_("start", tp->goalPos); + print_json5_PmVector(end); + print_json5_double(vel); + print_json5_double(ini_maxvel); + print_json5_double(acc); + print_json5_unsigned(enables); + print_json5_long(indexrotary); + print_json5_long(atspeed); + print_json5_long(canon_motion_type); + PmVector delta = tp->goalPos; + VecVecSub(&end, &tp->goalPos, &delta); + print_json5_PmVector(delta); + print_json5_PmLine9_("line", tc.coords.line); + print_json5_log_end(); } - tcFinalizeLength(prev_tc); - tcFlagEarlyStop(prev_tc, &tc); - - int retval = tpAddSegmentToQueue(tp, &tc, true); - //Run speed optimization (will abort safely if there are no tangent segments) - tpRunOptimization(tp); +#endif - return retval; + if (tc.target < TP_POS_EPSILON) { + rtapi_print_msg(RTAPI_MSG_DBG,"failed to create line id %d, zero-length segment\n",tp->nextId); + return TP_ERR_ZERO_LENGTH; + } else { + return tpFinalizeAndEnqueue(tp, &tc, &end); + } } @@ -2125,14 +1816,16 @@ int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, * xyz so the target is always the circle/arc/helical length. */ int tpAddCircle(TP_STRUCT * const tp, - EmcPose end, + EmcPose end_p, PmCartesian center, PmCartesian normal, int turn, - int canon_motion_type, + double expected_angle_rad, + EMCMotionTypes canon_motion_type, double vel, double ini_maxvel, double acc, + double acc_normal, unsigned char enables, char atspeed, struct state_tag_t tag) @@ -2140,11 +1833,14 @@ int tpAddCircle(TP_STRUCT * const tp, if (tpErrorCheck(tp)<0) { return TP_ERR_FAIL; } + PmVector end; + emcPoseToPmVector(&end_p, &end); + + TC_STRUCT tc = {}; - tp_info_print("== AddCircle ==\n"); - tp_debug_print("ini_maxvel = %f\n",ini_maxvel); + clearBlendStructs(); - TC_STRUCT tc = {0}; + tcSetId(tp, &tc, tag); tcInit(&tc, TC_CIRCULAR, @@ -2152,12 +1848,12 @@ int tpAddCircle(TP_STRUCT * const tp, tp->cycleTime, enables, atspeed); - tc.tag = tag; // Setup any synced IO for this move tpSetupSyncedIO(tp, &tc); // Copy over state data from the trajectory planner tcSetupState(&tc, tp); + tcSetTermCond(&tc, tp->termCond); // Setup circle geometry int res_init = pmCircle9Init(&tc.coords.circle, @@ -2165,17 +1861,12 @@ int tpAddCircle(TP_STRUCT * const tp, &end, ¢er, &normal, - turn); + turn, + expected_angle_rad); if (res_init) return res_init; - // Update tc target with existing circular segment - tc.target = pmCircle9Target(&tc.coords.circle); - if (tc.target < TP_POS_EPSILON) { - return TP_ERR_ZERO_LENGTH; - } - tp_debug_print("tc.target = %f\n",tc.target); - tc.nominal_length = tc.target; + tc.acc_normal_max = acc_normal; // Copy in motion parameters tcSetupMotion(&tc, @@ -2183,147 +1874,48 @@ int tpAddCircle(TP_STRUCT * const tp, ini_maxvel, acc); - //Reduce max velocity to match sample rate - tcClampVelocityByLength(&tc); - - TC_STRUCT *prev_tc; - prev_tc = tcqLast(&tp->queue); - - handleModeChange(prev_tc, &tc); - if (emcmotConfig->arcBlendEnable){ - tpHandleBlendArc(tp, &tc); - findSpiralArcLengthFit(&tc.coords.circle.xyz, &tc.coords.circle.fit); - } - tcFinalizeLength(prev_tc); - tcFlagEarlyStop(prev_tc, &tc); - - int retval = tpAddSegmentToQueue(tp, &tc, true); - - tpRunOptimization(tp); - return retval; -} - + pmCircle9LengthAndRatios(&tc.coords.circle); + // Update tc target with existing circular segment + tc.target = tc.coords.circle.total_length; + tc.nominal_length = tc.target; -/** - * Adjusts blend velocity and acceleration to safe limits. - * If we are blending between tc and nexttc, then we need to figure out what a - * safe blend velocity is based on the known trajectory parameters. This - * function updates the TC_STRUCT data with a safe blend velocity. - * - * @note This function will compute the parabolic blend start / end velocities - * regardless of the current terminal condition (useful for planning). - */ -STATIC int tpComputeBlendVelocity( - TC_STRUCT const *tc, - TC_STRUCT const *nexttc, - double target_vel_this, - double target_vel_next, - double *v_blend_this, - double *v_blend_next, - double *v_blend_net) -{ - /* Pre-checks for valid pointers */ - if (!nexttc || !tc || !v_blend_this || !v_blend_next ) { - return TP_ERR_FAIL; +#ifdef TP_DEBUG + { + // macros use the variable name, need a plain name to please the JSON5 parser + print_json5_log_start(tpAddCircle); + print_json5_tc_id_data_(&tc); + print_json5_PmVector_("start", tp->goalPos); + print_json5_PmVector(end); + print_json5_PmCartesian(center); + print_json5_PmCartesian(normal); + print_json5_long(turn); + print_json5_double(expected_angle_rad); + print_json5_double(vel); + print_json5_double(ini_maxvel); + print_json5_double(acc); + print_json5_double(acc_normal); + print_json5_unsigned(enables); + print_json5_long(atspeed); + print_json5_long(canon_motion_type); + PmVector delta = tp->goalPos; + VecVecSub(&end, &tp->goalPos, &delta); + print_json5_PmVector(delta); + print_json5_PmCircle_("xyz_circle", tc.coords.circle.xyz); + print_json5_log_end(); } +#endif - double acc_this = tcGetTangentialMaxAccel(tc); - double acc_next = tcGetTangentialMaxAccel(nexttc); - - double v_reachable_this = fmin(tpCalculateTriangleVel(tc), target_vel_this); - double v_reachable_next = fmin(tpCalculateTriangleVel(nexttc), target_vel_next); - - /* Compute the maximum allowed blend time for each segment. - * This corresponds to the minimum acceleration that will just barely reach - * max velocity as we are 1/2 done the segment. - */ - - double t_max_this = tc->target / v_reachable_this; - double t_max_next = nexttc->target / v_reachable_next; - double t_max_reachable = fmin(t_max_this, t_max_next); - - // How long the blend phase would be at maximum acceleration - double t_min_blend_this = v_reachable_this / acc_this; - double t_min_blend_next = v_reachable_next / acc_next; - - double t_max_blend = fmax(t_min_blend_this, t_min_blend_next); - // The longest blend time we can get that's still within the 1/2 segment restriction - double t_blend = fmin(t_max_reachable, t_max_blend); - - // Now, use this blend time to find the best acceleration / velocity for each segment - *v_blend_this = fmin(v_reachable_this, t_blend * acc_this); - *v_blend_next = fmin(v_reachable_next, t_blend * acc_next); - - double theta; - - PmCartesian v1, v2; - - tcGetEndAccelUnitVector(tc, &v1); - tcGetStartAccelUnitVector(nexttc, &v2); - findIntersectionAngle(&v1, &v2, &theta); - - double cos_theta = cos(theta); - - if (tc->tolerance > 0) { - /* see diagram blend.fig. T (blend tolerance) is given, theta - * is calculated from dot(s1, s2) - * - * blend criteria: we are decelerating at the end of segment s1 - * and we pass distance d from the end. - * find the corresponding velocity v when passing d. - * - * in the drawing note d = 2T/cos(theta) - * - * when v1 is decelerating at a to stop, v = at, t = v/a - * so required d = .5 a (v/a)^2 - * - * equate the two expressions for d and solve for v - */ - double tblend_vel; - /* Minimum value of cos(theta) to prevent numerical instability */ - const double min_cos_theta = cos(PM_PI / 2.0 - TP_MIN_ARC_ANGLE); - if (cos_theta > min_cos_theta) { - tblend_vel = 2.0 * pmSqrt(acc_this * tc->tolerance / cos_theta); - *v_blend_this = fmin(*v_blend_this, tblend_vel); - *v_blend_next = fmin(*v_blend_next, tblend_vel); - } - } - if (v_blend_net) { - /* - * Find net velocity in the direction tangent to the blend. - * When theta ~ 0, net velocity in tangent direction is very small. - * When the segments are nearly tangent (theta ~ pi/2), the blend - * velocity is almost entirely in the tangent direction. - */ - *v_blend_net = sin(theta) * (*v_blend_this + *v_blend_next) / 2.0; + if (tc.target < TP_POS_EPSILON) { + return TP_ERR_ZERO_LENGTH; + } else { + return tpFinalizeAndEnqueue(tp, &tc, &end); } - - return TP_ERR_OK; } -STATIC double estimateParabolicBlendPerformance( - TP_STRUCT const *tp, - TC_STRUCT const *tc, - TC_STRUCT const *nexttc) -{ - double v_this = 0.0, v_next = 0.0; - - // Use maximum possible target velocity to get best-case performance - double target_vel_this = tpGetMaxTargetVel(tp, tc); - double target_vel_next = tpGetMaxTargetVel(tp, nexttc); - - double v_net = 0.0; - tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, &v_net); - - return v_net; -} - - - /** * Calculate distance update from velocity and acceleration. */ -STATIC int tcUpdateDistFromAccel(TC_STRUCT * const tc, double acc, double vel_desired, int reverse_run) +int tcUpdateDistFromAccel(TC_STRUCT * const tc, double acc, double vel_desired, int reverse_run) { // If the resulting velocity is less than zero, than we're done. This // causes a small overshoot, but in practice it is very small. @@ -2347,7 +1939,7 @@ STATIC int tcUpdateDistFromAccel(TC_STRUCT * const tc, double acc, double vel_de tc->progress += (disp_sign * displacement); //Progress has to be within the allowable range - tc->progress = bisaturate(tc->progress, tcGetTarget(tc, TC_DIR_FORWARD), tcGetTarget(tc, TC_DIR_REVERSE)); + tc->progress = bound(tc->progress, tc->target, 0.0); } tc->currentvel = v_next; @@ -2357,31 +1949,47 @@ STATIC int tcUpdateDistFromAccel(TC_STRUCT * const tc, double acc, double vel_de return TP_ERR_OK; } -STATIC void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const tc, TC_STRUCT const * const nexttc, double acc) { +const char *cycleModeToString(UpdateCycleMode mode) +{ + switch (mode) { + case UPDATE_NORMAL: + return "normal"; + case UPDATE_PARABOLIC_BLEND: + return "parabolic_blend"; + case UPDATE_SPLIT: + return "split_cycle"; + } + return "unknown"; +} + +void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const tc, TC_STRUCT const * const nexttc, double acc, int accel_mode, UpdateCycleMode cycle) { #ifdef TC_DEBUG // Find maximum allowed velocity from feed and machine limits - double tc_target_vel = tpGetRealTargetVel(tp, tc); + const double v_target = tpGetRealAbsTargetVel(tp, tc); // Store a copy of final velocity - double tc_finalvel = tpGetRealFinalVel(tp, tc, nexttc); - - /* Debug Output */ - tc_debug_print("tc state: vr = %f, vf = %f, maxvel = %f\n", - tc_target_vel, tc_finalvel, tc->maxvel); - tc_debug_print(" currentvel = %f, fs = %f, tc = %f, term = %d\n", - tc->currentvel, tpGetFeedScale(tp,tc), tc->cycle_time, tc->term_cond); - tc_debug_print(" acc = %f, T = %f, DTG = %.12g\n", acc, - tcGetTarget(tc,tp->reverse_run), tcGetDistanceToGo(tc,tp->reverse_run)); - tc_debug_print(" reverse_run = %d\n", tp->reverse_run); - tc_debug_print(" motion type %d\n", tc->motion_type); - - if (tc->on_final_decel) { - rtapi_print(" on final decel\n"); - } -#else - (void)tp; - (void)tc; - (void)nexttc; - (void)acc; + const double v_final = tpGetRealFinalVel(tp, tc, nexttc); + const double v_max = tpGetRealMaxTargetVel(tp, tc); + + /* Debug Output (inserted into caller's output)*/ + print_json5_long_long_("time_ticks", tp->time_elapsed_ticks); + print_json5_tc_id_data_(tc); + print_json5_string_("motion_type", tcMotionTypeAsString(tc->motion_type)); + print_json5_double(v_target); + print_json5_double(v_final); + print_json5_double(v_max); + print_json5_double_("target", tcGetTarget(tc, tp->reverse_run)); + print_json5_double_("distance_to_go", tcGetDistanceToGo(tc, tp->reverse_run)); + print_json5_double_("v_current", tc->currentvel); + print_json5_double_("a_current", acc); + print_json5_double_("feed_scale", tpGetRealAbsFeedScale(tp, tc)); + print_json5_double_("dt", tc->cycle_time); + print_json5_bool_("reverse_run", tp->reverse_run); + print_json5_string_("term_cond", tcTermCondAsString((tc_term_cond_t)tc->term_cond)); + print_json5_bool_("final_decel", tc->on_final_decel); + print_json5_bool_("need_split", tc->splitting); + print_json5_long_("canon_type", tc->canon_motion_type); + print_json5_string_("accel_mode",accel_mode ? "ramp" : "trapezoidal"); + print_json5_string_("cycle", cycleModeToString(cycle)); #endif } @@ -2397,10 +2005,8 @@ STATIC void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const void tpCalculateTrapezoidalAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc, double * const acc, double * const vel_desired) { - tc_debug_print("using trapezoidal acceleration\n"); - // Find maximum allowed velocity from feed and machine limits - double tc_target_vel = tpGetRealTargetVel(tp, tc); + double tc_target_vel = tpGetRealAbsTargetVel(tp, tc); // Store a copy of final velocity double tc_finalvel = tpGetRealFinalVel(tp, tc, nexttc); @@ -2415,30 +2021,8 @@ void tpCalculateTrapezoidalAccel(TP_STRUCT const * const tp, TC_STRUCT * const t double dx = tcGetDistanceToGo(tc, tp->reverse_run); double maxaccel = tcGetTangentialMaxAccel(tc); - double discr_term1 = pmSq(tc_finalvel); - double discr_term2 = maxaccel * (2.0 * dx - tc->currentvel * tc->cycle_time); - double tmp_adt = maxaccel * tc->cycle_time * 0.5; - double discr_term3 = pmSq(tmp_adt); - - double discr = discr_term1 + discr_term2 + discr_term3; - - // Discriminant is a little more complicated with final velocity term. If - // discriminant < 0, we've overshot (or are about to). Do the best we can - // in this situation -#ifdef TP_PEDANTIC - if (discr < 0.0) { - rtapi_print_msg(RTAPI_MSG_ERR, - "discriminant %f < 0 in velocity calculation!\n", discr); - } -#endif - //Start with -B/2 portion of quadratic formula - double maxnewvel = -tmp_adt; - - //If the discriminant term brings our velocity above zero, add it to the total - //We can ignore the calculation otherwise because negative velocities are clipped to zero - if (discr > discr_term3) { - maxnewvel += pmSqrt(discr); - } + double maxnewvel = findTrapezoidalDesiredVel( + maxaccel, dx, tc_finalvel, tc->currentvel, tc->cycle_time); // Find bounded new velocity based on target velocity // Note that we use a separate variable later to check if we're on final decel @@ -2454,20 +2038,15 @@ void tpCalculateTrapezoidalAccel(TP_STRUCT const * const tp, TC_STRUCT * const t /** * Calculate "ramp" acceleration for a cycle. */ -STATIC int tpCalculateRampAccel(TP_STRUCT const * const tp, +int tpCalculateRampAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc, double * const acc, double * const vel_desired) { - tc_debug_print("using ramped acceleration\n"); // displacement remaining in this segment double dx = tcGetDistanceToGo(tc, tp->reverse_run); - if (!tc->blending_next) { - tc->vel_at_blend_start = tc->currentvel; - } - double vel_final = tpGetRealFinalVel(tp, tc, nexttc); /* Check if the final velocity is too low to properly ramp up.*/ @@ -2504,19 +2083,88 @@ void tpToggleDIOs(TC_STRUCT * const tc) { int i=0; if (tc->syncdio.anychanged != 0) { // we have DIO's to turn on or off - for (i=0; i < emcmotConfig->numDIO; i++) { + for (i=0; i < num_dio; i++) { if (!(tc->syncdio.dio_mask & (1 << i))) continue; - if (tc->syncdio.dios[i] > 0) _DioWrite(i, 1); // turn DIO[i] on - if (tc->syncdio.dios[i] < 0) _DioWrite(i, 0); // turn DIO[i] off + if (tc->syncdio.dios[i] > 0) emcmotDigitalOutWrite(i, 1); // turn DIO[i] on + if (tc->syncdio.dios[i] < 0) emcmotDigitalOutWrite(i, 0); // turn DIO[i] off } - for (i=0; i < emcmotConfig->numAIO; i++) { + for (i=0; i < num_aio; i++) { if (!(tc->syncdio.aio_mask & (1 << i))) continue; - _AioWrite(i, tc->syncdio.aios[i]); // set AIO[i] + emcmotAioWrite(i, tc->syncdio.aios[i]); // set AIO[i] } tc->syncdio.anychanged = 0; //we have turned them all on/off, nothing else to do for this TC the next time } } +/** + * The displacement is always computed with respect to a specie + */ +double findSpindleDisplacement( + double new_pos, + spindle_origin_t origin + ) +{ + return origin.direction * (new_pos - origin.position); +} +double findSpindleVelocity( + double spindle_velocity, + spindle_origin_t origin + ) +{ + return origin.direction * spindle_velocity; +} + +/** + * Helper function to compare commanded and actual spindle velocity. + * If the signs of velocity don't match, then the spindle is reversing direction. + */ +bool spindleReversed(spindle_origin_t origin, double prev_pos, double current_pos) +{ + return origin.direction * (current_pos - prev_pos) < 0; +} + +/** + * Commands a spindle reverse during rigid tapping. + * Does not affect the commanded spindle speed, + */ +void cmdReverseSpindle(double reversal_scale) +{ + // Flip sign on commanded velocity + emcmotStatus->spindle_cmd.velocity_rpm_out *= -1 * reversal_scale; + +} + +/** + * Safely reverses rigid tap motion towards the starting point, preserving the existing tracking error. + * @note extra motion distance for overrun must be added separately + */ +static inline void reverseRigidTapMotion(TC_STRUCT * const tc, + spindle_origin_t * const spindle_origin) +{ + // we've stopped, so set a new target at the original position + PmCartesian start, end; + PmCartLine *actual_xyz = &tc->coords.rigidtap.actual_xyz; + + // Set a new spindle origin at the approximate reversal point, and keep the current tracking error as the new offset + updateSpindlePositionFromProgress(spindle_origin, tc); + + pmCartLinePoint(&tc->coords.rigidtap.actual_xyz, tc->progress, &start); + end = tc->coords.rigidtap.nominal_xyz.start; + pmCartLineInit(actual_xyz, &start, &end); + tc->coords.rigidtap.reversal_target = tc->target = actual_xyz->tmag; + // NOTE: reset both progress and sync location: + // At the point of reversal, the spindle is already synchronized, so + // store the current position tracking error (in user units) as the sync offset + // This way any accumulated error is not forgotten during the retraction + tc->progress = 0.0; +} + +double estimate_rigidtap_decel_distance(double vel, double uu_per_rev) +{ + double cmd_latency_dist = fmax(vel * emcmotStatus->spindle_cmd_latency_sec, 0.); + double decel_distance = emcmotStatus->spindle_max_acceleration_rps2 > 0.0 ? pmSq(vel) / (2.0 * fabs(uu_per_rev) * emcmotStatus->spindle_max_acceleration_rps2) : 0.0; + return cmd_latency_dist + decel_distance; +} /** * Handle special cases for rigid tapping. @@ -2524,79 +2172,88 @@ void tpToggleDIOs(TC_STRUCT * const tc) { * during a rigid tap cycle. In particular, the target and spindle goal need to * be carefully handled since we're reversing direction. */ -STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, - TC_STRUCT * const tc) { - - static double old_spindlepos; - double new_spindlepos = emcmotStatus->spindle_status[tp->spindle.spindle_num].spindleRevs; - if (emcmotStatus->spindle_status[tp->spindle.spindle_num].direction < 0) - new_spindlepos = -new_spindlepos; - - switch (tc->coords.rigidtap.state) { - case RIGIDTAP_START: - old_spindlepos = new_spindlepos; - tc->coords.rigidtap.state = TAPPING; - /* Fallthrough */ - case TAPPING: - tc_debug_print("TAPPING\n"); - if (tc->progress >= tc->coords.rigidtap.reversal_target) { - // command reversal - emcmotStatus->spindle_status[tp->spindle.spindle_num].speed *= -1.0 * tc->coords.rigidtap.reversal_scale; - tc->coords.rigidtap.state = REVERSING; +void tpUpdateRigidTapState( + TP_STRUCT * const tp, + TC_STRUCT * const tc, + TC_STRUCT * const nexttc) +{ + static double old_spindle_pos = 0.0; + double spindle_pos = emcmotStatus->spindle_fb.position_rev; + static double retract_allowance = 0.0; + + rigid_tap_state_t const initial_state = tc->coords.rigidtap.state; + switch (initial_state) { + case RIGIDTAP_INACTIVE: + return; + case RIGIDTAP_TAPPING: + { + tc->uu_per_rev = tc->coords.rigidtap.tap_uu_per_rev; + double decel_distance = estimate_rigidtap_decel_distance(tc->currentvel, tc->uu_per_rev); + double reversal_target = tc->coords.rigidtap.reversal_target - decel_distance; + if (tc->progress >= reversal_target) { + // command reversal to stop / reverse at the bottom of the hole + emcmotStatus->rigid_tap_reversal_vel_rps = emcmotStatus->spindle_fb.velocity_rps; + cmdReverseSpindle(tc->coords.rigidtap.reversal_scale); + tc->coords.rigidtap.state = RIGIDTAP_REVERSING; + retract_allowance = estimate_rigidtap_decel_distance(tc->currentvel, tc->coords.rigidtap.retract_uu_per_rev); } break; - case REVERSING: - tc_debug_print("REVERSING\n"); - if (new_spindlepos < old_spindlepos) { - PmCartesian start, end; - PmCartLine *aux = &tc->coords.rigidtap.aux_xyz; - // we've stopped, so set a new target at the original position - tc->coords.rigidtap.spindlerevs_at_reversal = new_spindlepos + tp->spindle.offset; - - pmCartLinePoint(&tc->coords.rigidtap.xyz, tc->progress, &start); - end = tc->coords.rigidtap.xyz.start; - pmCartLineInit(aux, &start, &end); - rtapi_print_msg(RTAPI_MSG_DBG, "old target = %f", tc->target); - tc->coords.rigidtap.reversal_target = aux->tmag; - tc->target = aux->tmag + 10. * tc->uu_per_rev; - tc->progress = 0.0; - rtapi_print_msg(RTAPI_MSG_DBG, "new target = %f", tc->target); - - tc->coords.rigidtap.state = RETRACTION; + } + case RIGIDTAP_REVERSING: + if (spindleReversed(tp->spindle.origin, old_spindle_pos, spindle_pos) && tc->currentvel <= 0.0) { + emcmotStatus->rigid_tap_overshoot = tc->progress - tc->coords.rigidtap.reversal_target; + reverseRigidTapMotion(tc, &tp->spindle.origin); + double expected_reversal_overshoot = fmax(retract_allowance / tc->coords.rigidtap.retract_uu_per_rev, 0); + // Anticipate the overshoot at the top and add a safety factor for spindle uncertainty + addRigidTapOverrun(tc, RIGIDTAP_MAX_OVERSHOOT_REVS + expected_reversal_overshoot, tc->coords.rigidtap.retract_uu_per_rev); + tc->coords.rigidtap.state = RIGIDTAP_RETRACTION; + tc->uu_per_rev = tc->coords.rigidtap.retract_uu_per_rev; } - old_spindlepos = new_spindlepos; - tc_debug_print("Spindlepos = %f\n", new_spindlepos); break; - case RETRACTION: - tc_debug_print("RETRACTION\n"); + case RIGIDTAP_RETRACTION: if (tc->progress >= tc->coords.rigidtap.reversal_target) { - emcmotStatus->spindle_status[tp->spindle.spindle_num].speed *= -1 / tc->coords.rigidtap.reversal_scale; - tc->coords.rigidtap.state = FINAL_REVERSAL; + // Flip spindle direction again to start final reversal + cmdReverseSpindle(1.0/tc->coords.rigidtap.reversal_scale); + tc->coords.rigidtap.state = RIGIDTAP_FINAL_REVERSAL; + // Once we've cleared the hole, there's no reason to keep synched with the spindle unless we're peck tapping + if (!nexttc + || nexttc->motion_type != TC_RIGIDTAP + || tc->term_cond != TC_TERM_COND_TANGENT) { + // Stop synchronized motion if not peck tapping + tc->synchronized = 0; + tc->target_vel = 0; // Stop as fast as possible during final reversal + tc->canon_motion_type = EMC_MOTION_TYPE_TRAVERSE; + } } break; - case FINAL_REVERSAL: - tc_debug_print("FINAL_REVERSAL\n"); - if (new_spindlepos > old_spindlepos) { - PmCartesian start, end; - PmCartLine *aux = &tc->coords.rigidtap.aux_xyz; - pmCartLinePoint(aux, tc->progress, &start); - end = tc->coords.rigidtap.xyz.start; - pmCartLineInit(aux, &start, &end); - tc->target = aux->tmag; - tc->progress = 0.0; - //No longer need spindle sync at this point - tc->synchronized = 0; - tc->target_vel = tc->maxvel; - - tc->coords.rigidtap.state = FINAL_PLACEMENT; + case RIGIDTAP_FINAL_REVERSAL: + if (spindleReversed(tp->spindle.origin, old_spindle_pos, spindle_pos) && tc->currentvel <= 0.0) { + reverseRigidTapMotion(tc, &tp->spindle.origin); + tc->uu_per_rev = tc->coords.rigidtap.tap_uu_per_rev; + tc->coords.rigidtap.state = RIGIDTAP_FINAL_PLACEMENT; + if (!nexttc + || nexttc->motion_type != TC_RIGIDTAP + || tc->term_cond != TC_TERM_COND_TANGENT) { + // Stop synchronized motion if not peck tapping + tc->synchronized = 0; + tc->target_vel = tc->maxvel_geom; // Move as fast as possible to the final position + tc->canon_motion_type = EMC_MOTION_TYPE_TRAVERSE; + } } - old_spindlepos = new_spindlepos; break; - case FINAL_PLACEMENT: - tc_debug_print("FINAL_PLACEMENT\n"); + case RIGIDTAP_FINAL_PLACEMENT: // this is a regular move now, it'll stop at target above. + old_spindle_pos = 0.0; + retract_allowance = 0.0; break; } + old_spindle_pos = spindle_pos; +#ifdef TC_DEBUG + rigid_tap_state_t current_state = tc->coords.rigidtap.state; + print_json5_log_start(tpUpdateRigidTapState); + print_json5_unsigned(current_state); + print_json5_log_end(); +#endif } @@ -2605,8 +2262,10 @@ STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp, * Based on the specified trajectory segment tc, read its progress and status * flags. Then, update the emcmotStatus structure with this information. */ -STATIC int tpUpdateMovementStatus(TP_STRUCT * const tp, TC_STRUCT const * const tc ) { - +int tpUpdateMovementStatus(TP_STRUCT * const tp, + TC_STRUCT const * const tc, + TC_STRUCT const * const nexttc) +{ if (!tp) { return TP_ERR_FAIL; @@ -2618,8 +2277,7 @@ STATIC int tpUpdateMovementStatus(TP_STRUCT * const tp, TC_STRUCT const * const emcmotStatus->enables_queued = emcmotStatus->enables_new; emcmotStatus->requested_vel = 0; emcmotStatus->current_vel = 0; - emcmotStatus->spindleSync = 0; - + emcmotStatus->dwell_time_remaining = 0.0; emcPoseZero(&emcmotStatus->dtg); tp->motionType = 0; @@ -2627,64 +2285,44 @@ STATIC int tpUpdateMovementStatus(TP_STRUCT * const tp, TC_STRUCT const * const return TP_ERR_STOPPED; } - EmcPose tc_pos; + PmVector tc_pos; tcGetEndpoint(tc, &tc_pos); - tc_debug_print("tc id = %u canon_type = %u motion_type = %u\n", - tc->id, tc->canon_motion_type, tc->motion_type); tp->motionType = tc->canon_motion_type; tp->activeDepth = tc->active_depth; emcmotStatus->distance_to_go = tc->target - tc->progress; emcmotStatus->enables_queued = tc->enables; // report our line number to the guis - tp->execId = tc->id; - emcmotStatus->requested_vel = tc->reqvel; - emcmotStatus->current_vel = tc->currentvel; - - emcPoseSub(&tc_pos, &tp->currentPos, &emcmotStatus->dtg); - return TP_ERR_OK; -} - - -/** - * Do a parabolic blend by updating the nexttc. - * Perform the actual blending process by updating the target velocity for the - * next segment, then running a cycle update. - */ -STATIC void tpUpdateBlend(TP_STRUCT * const tp, TC_STRUCT * const tc, - TC_STRUCT * const nexttc) { - - if (!nexttc) { - return; - } - double save_vel = nexttc->target_vel; - - if (tpGetFeedScale(tp, nexttc) > TP_VEL_EPSILON) { - double dv = tc->vel_at_blend_start - tc->currentvel; - double vel_start = fmax(tc->vel_at_blend_start, TP_VEL_EPSILON); - // Clip the ratio at 1 and 0 - double blend_progress = fmax(fmin(dv / vel_start, 1.0), 0.0); - double blend_scale = tc->vel_at_blend_start / tc->blend_vel; - nexttc->target_vel = blend_progress * nexttc->blend_vel * blend_scale; - // Mark the segment as blending so we handle the new target velocity properly - nexttc->is_blending = true; + tp->execId = tc->tag.fields[GM_FIELD_LINE_NUMBER]; + // Store the next ID as well to show what will execute next to the GUI + int nextexecId = tp->nextexecId = tc->tag.fields[GM_FIELD_NEXT_LINE]; + if (0 == nextexecId && nexttc) { + tp->nextexecId = nexttc->tag.fields[GM_FIELD_LINE_NUMBER]; } else { - // Drive the target velocity to zero since we're stopping - nexttc->target_vel = 0.0; + tp->nextexecId = nextexecId; } - tpUpdateCycle(tp, nexttc, NULL); - //Restore the original target velocity - nexttc->target_vel = save_vel; -} + emcmotStatus->requested_vel = tc->reqvel; + emcmotStatus->dwell_time_remaining = (tc->motion_type == TC_DWELL) ? tc->coords.dwell.remaining_time : 0.0; + emcmotStatus->waiting_on_spindle = tpIsWaitingOnSpindle(tp); + // KLUDGE Zero out any EMC status that doesn't apply + if (tc->synchronized != TC_SYNC_POSITION) { + clearPosTrackingStatus(); + } + emcmotStatus->rigid_tap_state = tc->motion_type == TC_RIGIDTAP ? tc->coords.rigidtap.state : RIGIDTAP_INACTIVE; + PmVector dtg; + VecVecSub(&tc_pos, &tp->currentPos, &dtg); + pmVectorToEmcPose(&dtg, &emcmotStatus->dtg); + return TP_ERR_OK; +} /** * Cleanup if tc is not valid (empty queue). * If the program ends, or we hit QUEUE STARVATION, do a soft reset on the trajectory planner. * TODO merge with tpClear? */ -STATIC void tpHandleEmptyQueue(TP_STRUCT * const tp) +void tpHandleEmptyQueue(TP_STRUCT * const tp) { tcqInit(&tp->queue); @@ -2695,29 +2333,30 @@ STATIC void tpHandleEmptyQueue(TP_STRUCT * const tp) tp->execId = 0; tp->motionType = 0; - tpUpdateMovementStatus(tp, NULL); + tpUpdateMovementStatus(tp, NULL, NULL); tpResume(tp); + // when not executing a move, use the current enable flags + emcmotStatus->enables_queued = emcmotStatus->enables_new; } /** Wrapper function to unlock rotary axes */ -STATIC void tpSetRotaryUnlock(int axis, int unlock) { - _SetRotaryUnlock(axis, unlock); +void tpSetRotaryUnlock(IndexRotaryAxis axis, int unlock) { + emcmotSetRotaryUnlock(axis, unlock); } /** Wrapper function to check rotary axis lock */ -STATIC int tpGetRotaryIsUnlocked(int axis) { - return _GetRotaryIsUnlocked(axis); +int tpGetRotaryIsUnlocked(IndexRotaryAxis axis) { + return emcmotGetRotaryIsUnlocked(axis); } - /** * Cleanup after a trajectory segment is complete. * If the current move is complete and we're not waiting on the spindle for * const this move, then pop if off the queue and perform cleanup operations. * Finally, get the next move in the queue. */ -STATIC int tpCompleteSegment(TP_STRUCT * const tp, +int tpCompleteSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { if (tp->spindle.waiting_for_atspeed == tc->id) { @@ -2728,78 +2367,85 @@ STATIC int tpCompleteSegment(TP_STRUCT * const tp, // spindle position so the next synced move can be in // the right place. if(tc->synchronized != TC_SYNC_NONE) { - tp->spindle.offset += tc->target / tc->uu_per_rev; + updateSpindlePositionFromProgress(&tp->spindle.origin, tc); } else { - tp->spindle.offset = 0.0; + setSpindleOrigin(&tp->spindle.origin, 0.0); } - if(tc->indexer_jnum != -1) { + if(tc->indexrotary != INDEX_NONE) { // this was an indexing move, so before we remove it we must - // relock the joint for the locking indexer axis - tpSetRotaryUnlock(tc->indexer_jnum, 0); + // relock the axis + tpSetRotaryUnlock(tc->indexrotary, 0); // if it is now locked, fall through and remove the finished move. // otherwise, just come back later and check again - if(tpGetRotaryIsUnlocked(tc->indexer_jnum)) + if(tpGetRotaryIsUnlocked(tc->indexrotary)) { return TP_ERR_FAIL; + } } //Clear status flags associated since segment is done //TODO stuff into helper function? tc->active = 0; tc->remove = 0; - tc->is_blending = 0; tc->splitting = 0; tc->cycle_time = tp->cycleTime; //Velocities are by definition zero for a non-active segment tc->currentvel = 0.0; + tp->tc_completed_id = tc->id; tc->term_vel = 0.0; //TODO make progress to match target? // done with this move if (tp->reverse_run) { tcqBackStep(&tp->queue); - tp_debug_print("Finished reverse run of tc id %d\n", tc->id); + tp_debug_print("Finished reverse segment %lld\n", tc->unique_id); } else { int res_pop = tcqPop(&tp->queue); if (res_pop) rtapi_print_msg(RTAPI_MSG_ERR,"Got error %d from tcqPop!\n", res_pop); - tp_debug_print("Finished tc id %d\n", tc->id); + tp_debug_print("Finished forward segment %lld\n", tc->unique_id); } return TP_ERR_OK; } - /** * Handle an abort command. * Based on the current motion state, handle the consequences of an abort command. */ -STATIC tp_err_t tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc, +tp_err_t tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT * const nexttc) { if(!tp->aborting) { //Don't need to do anything if not aborting return TP_ERR_NO_ACTION; } + + int movement_stopped = (!tc || tc->currentvel == 0.0) && (!nexttc || nexttc->currentvel == 0.0); + //If the motion has stopped, then it's safe to reset the TP struct. if( MOTION_ID_VALID(tp->spindle.waiting_for_index) || MOTION_ID_VALID(tp->spindle.waiting_for_atspeed) || - (tc->currentvel == 0.0 && (!nexttc || nexttc->currentvel == 0.0))) { - tcqInit(&tp->queue); - tp->goalPos = tp->currentPos; - tp->done = 1; - tp->depth = tp->activeDepth = 0; - tp->aborting = 0; - tp->execId = 0; - tp->motionType = 0; - tp->synchronized = 0; - tp->reverse_run = 0; - tp->spindle.waiting_for_index = MOTION_INVALID_ID; - tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID; - tpResume(tp); + movement_stopped) { + tpClear(tp); return TP_ERR_STOPPED; - } //FIXME consistent error codes + } return TP_ERR_SLOWING; } +void setSpindleTrigger(TP_STRUCT * const tp, TC_STRUCT * const tc) +{ + const double spindle_vel_rps_raw = get_spindle_speed_out_rpm(emcmotStatus) / 60.0; + + // WARNING: this assumes that max acceleration is constant over the segment to estimate the position error + // This won't be true in the future if we switch to s-curve planning + double a_max = tcGetTangentialMaxAccel(tc); + double accel_revs_est = fabs(tc->uu_per_rev * pmSq(spindle_vel_rps_raw) / (2.0 * a_max)); + // Advance the spindle origin by at least enough + double spindle_offset_turns = ceil(accel_revs_est + 0.25); + setSpindleOrigin(&tp->spindle.origin, spindle_offset_turns * get_spindle_command_direction(emcmotStatus)); + + // Setup spindle trigger conditions + tp->spindle.trigger_revs = -accel_revs_est; +} /** * Check if the spindle has reached the required speed for a move. @@ -2807,9 +2453,8 @@ STATIC tp_err_t tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc, * has not reached the requested speed, or the spindle index has not been * detected. */ -STATIC tp_err_t tpCheckAtSpeed(TP_STRUCT * const tp, TC_STRUCT * const tc) +tp_err_t tpCheckAtSpeed(TP_STRUCT * const tp, TC_STRUCT * const tc) { - int s; // this is no longer the segment we were waiting_for_index for if (MOTION_ID_VALID(tp->spindle.waiting_for_index) && tp->spindle.waiting_for_index != tc->id) { @@ -2829,49 +2474,140 @@ STATIC tp_err_t tpCheckAtSpeed(TP_STRUCT * const tp, TC_STRUCT * const tc) } if (MOTION_ID_VALID(tp->spindle.waiting_for_atspeed)) { - for (s = 0; s < emcmotConfig->numSpindles; s++){ - if(!emcmotStatus->spindle_status[s].at_speed) { - // spindle is still not at the right speed, so wait another cycle + if(!emcmotStatus->spindle_is_atspeed) { + // spindle is still not at the right speed, so wait another cycle + double waited_time = (double)(tp->time_elapsed_ticks - tp->time_at_wait)*tp->cycleTime; + if ( waited_time > emcmotConfig->timeout_cfg.atspeed_wait_timeout_sec) { + LineDescriptor linedesc; + formatLinePrefix(&tc->tag, &linedesc); + tpStopWithError(tp, "%sSpindle did not reach desired speed %0.2f RPM within %0.2f seconds (spindle-at-speed-timeout)", + linedesc.buf, + emcmotStatus->spindle_cmd.velocity_rpm_out, + emcmotConfig->timeout_cfg.atspeed_wait_timeout_sec); + } else { return TP_ERR_WAITING; } + } else { + tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID; } - // not waiting any more - tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID; } if (MOTION_ID_VALID(tp->spindle.waiting_for_index)) { - if (emcmotStatus->spindle_status[tp->spindle.spindle_num].spindle_index_enable) { - /* haven't passed index yet */ - return TP_ERR_WAITING; + if (emcmotStatus->spindle_fb.index_enable) { + double waited_time = (double)(tp->time_elapsed_ticks - tp->time_at_wait)*tp->cycleTime; + if (waited_time > emcmotConfig->timeout_cfg.index_wait_timeout_sec) { + LineDescriptor linedesc; + formatLinePrefix(&tc->tag, &linedesc); + tpStopWithError(tp, "%sSpindle index signal was not detected within %.2f seconds (spindle-index-timeout)", + linedesc.buf, + waited_time); + } else { + /* haven't passed index yet */ + return TP_ERR_WAITING; + } } else { - rtapi_print_msg(RTAPI_MSG_DBG, "Index seen on spindle %d\n", tp->spindle.spindle_num); /* passed index, start the move */ - emcmotStatus->spindleSync = 1; + emcmotStatus->spindle_fb.synced = 1; tp->spindle.waiting_for_index = MOTION_INVALID_ID; - tc->sync_accel = 1; - tp->spindle.revs = 0; + setSpindleTrigger(tp, tc); + emcmotStatus->spindle_sync_state = SYNC_TRIGGER_WAIT; } } + return TP_ERR_OK; } +/** + * Return true if a spindle-synched move is waiting on the spindle + */ +inline int tpIsWaitingOnSpindle(TP_STRUCT const * const tp) +{ + if (MOTION_ID_VALID(tp->spindle.waiting_for_index)) { + return tp->spindle.waiting_for_index; + } else if (MOTION_ID_VALID(tp->spindle.waiting_for_atspeed)) { + return tp->spindle.waiting_for_atspeed; + } + return 0; +} + +void checkPositionMatch(TP_STRUCT const *tp, TC_STRUCT const *tc) +{ + unsigned need_print = _tp_debug; + PmVector tp_position_error={}; + if (needConsistencyCheck(CCHECK_C0_CONTINUITY)){ + tcGetPos(tc, &tp_position_error); + VecVecSubEq(&tp_position_error, &tp->currentPos); + + unsigned position_mismatch_axes = findAbsThresholdViolations(tp_position_error, emcmotConfig->consistencyCheckConfig.maxPositionDriftError); + need_print |= position_mismatch_axes; + reportTPAxisError(tp, position_mismatch_axes, "Motion start position difference exceeds threshold"); + } + + // Log a bunch of TP internal state if required by debug level or position error + if (need_print) { + print_json5_log_start(tpActivateSegment); + print_json5_long_long_("time_ticks", tp->time_elapsed_ticks); + print_json5_tc_id_data_(tc); + print_json5_string_("motion_type_name", tcMotionTypeAsString(tc->motion_type)); + print_json5_int_("motion_type", tc->motion_type); + print_json5_int_("active_axes", tc->tag.fields[GM_FIELD_FEED_AXES]); + + // Position settings + + print_json5_PmVector(tp_position_error); + print_json5_double_field(tc, target); + print_json5_double_field(tc, progress); + if (tc->motion_type == TC_SPHERICAL) { + print_json5_double_field(&tc->coords.arc, line_length); + } + // Velocity settings + print_json5_double_field(tc, reqvel); + print_json5_double_field(tc, target_vel); + print_json5_double_field(tc, finalvel); + print_json5_double_field(tc, kink_vel); + print_json5_int_field(tc, use_kink); + print_json5_double_field(tc, parabolic_equiv_vel); + int blend_mode = 0; + if (tc->term_cond == TC_TERM_COND_PARABOLIC) { + blend_mode = 2; + } else if (tc->term_cond == TC_TERM_COND_TANGENT) { + blend_mode = tc->use_kink ? 3 : 4; + } + print_json5_int(blend_mode); + // Acceleration settings + print_json5_double_("accel_scale", tcGetAccelScale(tc)); + print_json5_double_("acc_overall", tcGetOverallMaxAccel(tc)); + print_json5_double_("acc_tangential", tcGetTangentialMaxAccel(tc)); + print_json5_bool_("accel_ramp", tc->accel_mode); + print_json5_string_("sync_mode", tcSyncModeAsString(tc->synchronized)); + + print_json5_int_field(tc, finalized); + print_json5_int_field(tc, term_cond); + print_json5_int_field(tc, id); + print_json5_int_field(tc, atspeed); + + print_json5_log_end(); + } +} + /** * "Activate" a segment being read for the first time. * This function handles initial setup of a new segment read off of the queue * for the first time. */ -STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { - +tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { //Check if already active if (!tc || tc->active) { return TP_ERR_OK; } +#ifdef TP_PEDANTIC if (!tp) { return TP_ERR_MISSING_INPUT; } +#endif - if (tp->reverse_run && (tc->motion_type == TC_RIGIDTAP || tc->synchronized != TC_SYNC_NONE)) { + if (tp->reverse_run && (tc->motion_type == TC_RIGIDTAP || tc->synchronized != TC_SYNC_NONE || tc->atspeed)) { //Can't activate a segment with synced motion in reverse return TP_ERR_REVERSE_EMPTY; } @@ -2881,92 +2617,118 @@ STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) { * of a trapezoidal motion. This leads to fewer jerk spikes, at a slight * performance cost. * */ - double cutoff_time = 1.0 / (fmax(emcmotConfig->arcBlendRampFreq, TP_TIME_EPSILON)); + double cutoff_time = 1.0 / (fmax(emcmotConfig->arc_blend_cfg.ramp_frequency, TP_TIME_EPSILON)); double length = tcGetDistanceToGo(tc, tp->reverse_run); // Given what velocities we can actually reach, estimate the total time for the segment under ramp conditions - double segment_time = 2.0 * length / (tc->currentvel + fmin(tc->finalvel,tpGetRealTargetVel(tp,tc))); - + double segment_time = 2.0 * length / (tc->currentvel + fmin(tc->finalvel,tpGetRealAbsTargetVel(tp,tc))); if (segment_time < cutoff_time && tc->canon_motion_type != EMC_MOTION_TYPE_TRAVERSE && tc->term_cond == TC_TERM_COND_TANGENT && - tc->motion_type != TC_RIGIDTAP && - length != 0) + !tc->synchronized) { - tp_debug_print("segment_time = %f, cutoff_time = %f, ramping\n", - segment_time, cutoff_time); tc->accel_mode = TC_ACCEL_RAMP; } // Do at speed checks that only happen once int needs_atspeed = tc->atspeed || - (tc->synchronized == TC_SYNC_POSITION && !(emcmotStatus->spindleSync)); + (tc->synchronized == TC_SYNC_POSITION && !(emcmotStatus->spindle_fb.synced)); - if (needs_atspeed){ - int s; - for (s = 0; s < emcmotConfig->numSpindles; s++){ - if (!emcmotStatus->spindle_status[s].at_speed) { - tp->spindle.waiting_for_atspeed = tc->id; - return TP_ERR_WAITING; - } + if ( needs_atspeed && !(emcmotStatus->spindle_is_atspeed)) { + tp->spindle.waiting_for_atspeed = tc->id; + return TP_ERR_WAITING; + } + +// Disable this check until we can do a controlled stop during peck tapping, or do this check earlier +#if 0 + if (tc->motion_type == TC_RIGIDTAP) { + double tap_uu_per_rev = tc->coords.rigidtap.tap_uu_per_rev; + double nominal_axis_vel = emcmotStatus->spindle_fb.velocity_rps * tap_uu_per_rev; + // How long will it take at the current spindle speed (in axis distance) to fully decelerate to a stop + double decel_distance = estimate_rigidtap_decel_distance(nominal_axis_vel, tap_uu_per_rev); + // Nominal depth of the hole with no overshoot + // TODO add adjustable overshoot warning cutoff + double nominal_depth = tc->coords.rigidtap.nominal_xyz.tmag; + + if (decel_distance > nominal_depth) { + // Add a bit of padding to the suggested margin + double height_margin = decel_distance * 1.05 - nominal_depth; + LineDescriptor linedesc; + formatLinePrefix(&tc->tag, &linedesc); + tpStopWithError(tp, "%sRigid tap cycle depth %.3f in is not enough to fully reverse the spindle. Increase starting height by at least %.3f in or reduce spindle speed", + linedesc.buf, + nominal_depth, + height_margin); + return TP_ERR_FAIL; } } +#endif - if (tc->indexer_jnum != -1) { - // request that the joint for the locking indexer axis unlock - tpSetRotaryUnlock(tc->indexer_jnum, 1); + if (tc->indexrotary != INDEX_NONE) { + // request that the axis unlock + tpSetRotaryUnlock(tc->indexrotary, 1); // if it is unlocked, fall through and start the move. // otherwise, just come back later and check again - if (!tpGetRotaryIsUnlocked(tc->indexer_jnum)) { + if (!tpGetRotaryIsUnlocked(tc->indexrotary)) return TP_ERR_WAITING; - } } - // Temporary debug message - tp_debug_print("Activate tc id = %d target_vel = %f req_vel = %f final_vel = %f length = %f\n", - tc->id, - tc->target_vel, - tc->reqvel, - tc->finalvel, - tc->target); - tc->active = 1; //Do not change initial velocity here, since tangent blending already sets this up tp->motionType = tc->canon_motion_type; - tc->blending_next = 0; tc->on_final_decel = 0; - if (TC_SYNC_POSITION == tc->synchronized && !(emcmotStatus->spindleSync)) { + tp_err_t res = TP_ERR_OK; + + if (TC_SYNC_POSITION == tc->synchronized && !(emcmotStatus->spindle_fb.synced)) { tp_debug_print("Setting up position sync\n"); // if we aren't already synced, wait tp->spindle.waiting_for_index = tc->id; // ask for an index reset - emcmotStatus->spindle_status[tp->spindle.spindle_num].spindle_index_enable = 1; - tp->spindle.offset = 0.0; - rtapi_print_msg(RTAPI_MSG_DBG, "Waiting on sync. spindle_num %d..\n", tp->spindle.spindle_num); - return TP_ERR_WAITING; + emcmotStatus->spindle_fb.index_enable = 1; + + + setSpindleOrigin(&tp->spindle.origin, 0.0); + emcmotStatus->spindle_sync_state = SYNC_SEEK_INDEX; + rtapi_print_msg(RTAPI_MSG_DBG, "Waiting on sync...\n"); + res = TP_ERR_WAITING; + } + + if (tc->motion_type == TC_DWELL) { + // Dwell the requested amount of time, plus any extra time required by the spindle rpm delta + double dwell_time = tc->coords.dwell.dwell_time_req; + if (tc->coords.dwell.delta_rpm_req > 0 + && emcmotStatus->spindle_max_acceleration_rps2 > 0) { + // Get the current spindle acceleration (changes based on belt position, for example) and compute a dwell time to wait for the spindle to reach the speed delta + // This avoids the at-speed handshake which may not be consistent enough for soft-tapping + double delta_rps = tc->coords.dwell.delta_rpm_req / 60.0; + dwell_time += delta_rps / emcmotStatus->spindle_max_acceleration_rps2 + emcmotStatus->spindle_cmd_latency_sec; + } + tc->coords.dwell.dwell_time = tc->coords.dwell.remaining_time = dwell_time; } // Update the modal state displayed by the TP tp->execTag = tc->tag; + clearPositionSyncErrors(); - return TP_ERR_OK; + checkPositionMatch(tp, tc); + return res; } - /** * Run velocity mode synchronization. * Update requested velocity to follow the spindle's velocity (scaled by feed rate). */ -STATIC void tpSyncVelocityMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT * const nexttc) { - double speed = emcmotStatus->spindle_status[tp->spindle.spindle_num].spindleSpeedIn; - double pos_error = fabs(speed) * tc->uu_per_rev; +void tpSyncVelocityMode(TC_STRUCT * const tc, TC_STRUCT * const nexttc) { + double speed = fabs(emcmotStatus->spindle_fb.velocity_rps); + double pos_error = speed * tc->uu_per_rev; // Account for movement due to parabolic blending with next segment if(nexttc) { pos_error -= nexttc->progress; } tc->target_vel = pos_error; + emcmotStatus->spindle_sync_state = SYNC_VELOCITY; if (nexttc && nexttc->synchronized) { //If the next move is synchronized too, then match it's @@ -2975,65 +2737,189 @@ STATIC void tpSyncVelocityMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_ST } } +#if 0 +/** + * A function that looks like sqrt but is flatter, and does not have infinite slope at x = 0 for c > 0 + * @pre c >= 0, x >= 0 + * @return + */ +static inline double pseudo_sqrt_cexpr(double x, double c) +{ + const double den = (pmSqrt(c + 1) - pmSqrt(c)); + const double b0 = -pmSqrt(c + const double b1 = 1; + return (b1 * pmSqrt(x+c) + b0) / den; +} +#endif + +static int pos_sync_error_reported = 0; +void checkPositionSyncError(TP_STRUCT const *tp, TC_STRUCT const *tc) +{ + const double max_allowed_error = emcmotConfig->maxPositionTrackingError; + if (!pos_sync_error_reported + && emcmotStatus->spindle_sync_state == SYNC_POSITION + && fabs(emcmotStatus->pos_tracking_error) > max_allowed_error) { + rtapi_print_msg(RTAPI_MSG_ERR,"Spindle position tracking error exceeds limit of %f on line %d\n", + max_allowed_error, + tc->id); + print_json5_log_start(tpSyncPosition); + print_json5_long_long_("time_ticks", tp->time_elapsed_ticks); + print_json5_double_("current_vel", emcmotStatus->current_vel); + print_json5_double_("time", tp->time_elapsed_sec); + print_json5_double_("spindle_revs", emcmotStatus->spindle_fb.position_rev); + print_json5_double_("spindle_speed_rps", get_spindle_speed_out_rpm(emcmotStatus) / 60.); + print_json5_double_("spindle_speed_out", get_spindle_speed_out_rpm(emcmotStatus)); + print_json5_double_("spindle_speed_cmd_rpm", emcmotStatus->spindle_cmd.velocity_rpm_out); + print_json5_double_("spindle_tracking_lookahead_steps", emcmotStatus->spindle_tracking_lookahead_steps); + print_json5_long_("pos_tracking_mode", emcmotStatus->pos_tracking_mode); + print_json5_double_("pos_tracking_velocity", emcmotStatus->pos_tracking_velocity); + print_json5_double_("pos_tracking_error", emcmotStatus->pos_tracking_error); + print_json5_end_(); + pos_sync_error_reported = 1; + } +} + +void clearPositionSyncErrors() +{ + pos_sync_error_reported = 0; +} /** * Run position mode synchronization. * Updates requested velocity for a trajectory segment to track the spindle's position. */ -STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, - TC_STRUCT * const nexttc ) { - - double spindle_pos = tpGetSignedSpindlePosition(&emcmotStatus->spindle_status[tp->spindle.spindle_num]); - tp_debug_print("Spindle at %f\n",spindle_pos); - double spindle_vel, target_vel; - double oldrevs = tp->spindle.revs; - - if ((tc->motion_type == TC_RIGIDTAP) && (tc->coords.rigidtap.state == RETRACTION || - tc->coords.rigidtap.state == FINAL_REVERSAL)) { - tp->spindle.revs = tc->coords.rigidtap.spindlerevs_at_reversal - - spindle_pos; - } else { - tp->spindle.revs = spindle_pos; - } +void tpSyncPositionMode( + TP_STRUCT * const tp, + TC_STRUCT * const tc, + TC_STRUCT * const nexttc ) +{ + + // Start with raw spindle position and our saved offset + double spindle_pos_raw = emcmotStatus->spindle_fb.position_rev; + + const double spindle_vel_rps = findSpindleVelocity(emcmotStatus->spindle_fb.velocity_rps, tp->spindle.origin); + // Estimate spindle position delta (2 steps seems to minimize physical position error + const double lookahead_steps = bound(emcmotStatus->spindle_tracking_lookahead_steps, 100.0, 0); + const double spindle_lookahead_delta = spindle_vel_rps * tp->cycleTime * lookahead_steps; + + // Note that this quantity should be non-negative under normal conditions. + double spindle_displacement_measured = findSpindleDisplacement(spindle_pos_raw, + tp->spindle.origin); + double spindle_displacement = spindle_displacement_measured + spindle_lookahead_delta; + + double v_final = spindle_vel_rps * tc->uu_per_rev; - double pos_desired = (tp->spindle.revs - tp->spindle.offset) * tc->uu_per_rev; - double pos_error = pos_desired - tc->progress; + // Multiply by user feed rate to get equivalent desired position + const double pos_desired = (spindle_displacement) * tc->uu_per_rev; + double net_progress = tc->progress; if(nexttc) { - pos_error -= nexttc->progress; + // If we're in a parabolic blend, the next segment will be active too, + // so make sure to account for its progress + net_progress += nexttc->progress; } + const double pos_error = pos_desired - net_progress; - if(tc->sync_accel) { - // detect when velocities match, and move the target accordingly. - // acceleration will abruptly stop and we will be on our new target. - // FIX: this is driven by TP cycle time, not the segment cycle time - double dt = fmax(tp->cycleTime, TP_TIME_EPSILON); - spindle_vel = tp->spindle.revs / ( dt * tc->sync_accel++); - target_vel = spindle_vel * tc->uu_per_rev; - if(tc->currentvel >= target_vel) { - tc_debug_print("Hit accel target in pos sync\n"); - // move target so as to drive pos_error to 0 next cycle - tp->spindle.offset = tp->spindle.revs - tc->progress / tc->uu_per_rev; - tc->sync_accel = 0; - tc->target_vel = target_vel; - } else { - tc_debug_print("accelerating in pos_sync\n"); - // beginning of move and we are behind: accel as fast as we can - tc->target_vel = tc->maxvel; + double a_max = tcGetTangentialMaxAccel(tc); + +#ifdef TC_DEBUG + print_json5_log_start(tpSyncPositionMode); + const double vel_error_before = v_final - tc->target_vel; + print_json5_double(vel_error_before); + print_json5_double(pos_error); + print_json5_double(spindle_displacement); + print_json5_double(spindle_lookahead_delta); + print_json5_double(spindle_pos_raw); + print_json5_double(pos_desired); + print_json5_double(net_progress); +#endif + + switch (emcmotStatus->spindle_sync_state) { + case SYNC_INACTIVE: + case SYNC_VELOCITY: + case SYNC_SEEK_INDEX: + // not valid here + break; + + case SYNC_TRIGGER_WAIT: + tc->target_vel = 0.0; + if (spindle_displacement < tp->spindle.trigger_revs) { + break; } - } else { + emcmotStatus->spindle_sync_state = SYNC_ACCEL_RAMP; + case SYNC_ACCEL_RAMP: + // Axis has caught up to spindle once position error goes positive + // note that it may not be perfectly synced at this point, but it should be pretty close + if (pos_error >= 0) { + emcmotStatus->spindle_sync_state = SYNC_POSITION; + } + case SYNC_POSITION: + if (tc->on_final_decel) { + emcmotStatus->spindle_sync_state = SYNC_FINAL_DECEL; + } + case SYNC_FINAL_DECEL: // we have synced the beginning of the move as best we can - // track position (minimize pos_error). - tc_debug_print("tracking in pos_sync\n"); - double errorvel; - spindle_vel = (tp->spindle.revs - oldrevs) / tp->cycleTime; - target_vel = spindle_vel * tc->uu_per_rev; - errorvel = pmSqrt(fabs(pos_error) * tcGetTangentialMaxAccel(tc)); - if(pos_error<0) { - errorvel *= -1.0; + // This is the velocity we should be at when the position error is c0 + + /* + * In general, position tracking works by perturbing the + * velocity-synced feed up or down to correct for transient position + * errors. If position error is 0 (e.g. a perfect spindle and + * encoder), then the behavior is identical to velocity-synced motion. + * + * velocity + * | v_p + * | /\ + * | /..\ v_0 + * |--------....----------- + * | .... + * | .... + * |_________________________ + * |----| t time + * + * To correct a position error x_err (shaded area above), we need to + * momentarily increase the velocity to catch up, then drop back to the + * sync velocity. + * + * In effect, this is the trapezoidal velocity planning problem, if: + * 1) remaining distance dx = x_err + * 2) "final" velocity = v_0 + * 3) max velocity / acceleration from motion segment + */ + + switch(emcmotStatus->pos_tracking_mode) { + case POS_TRACK_MODE_LEGACY: + { + // LinuxCNC 2.6 approach to spindle tracking (high jitter when + // position error is very small due to slope of sqrt for small + // values) + double v_sq = a_max * pos_error; + double v_target_stock = signum(v_sq) * pmSqrt(fabs(v_sq)) + v_final; + tc->target_vel = v_target_stock; + break; + } + case POS_TRACK_MODE_FLATTENED: + { + // Experimental spindle tracking that adds correction terms using a pythagorean sum + double v_sq = a_max * pos_error; + double v_target_flat = signum(v_sq) * pseudo_sqrt(fabs(v_sq)) + v_final; + tc->target_vel = v_target_flat; + break; + } + case POS_TRACK_MODE_TRAPEZOIDAL: + { + double v_max = tc->maxvel_geom; + // Use trapezoidal velocity calculation to find target velocity + // NOTE: this tracking method is smoother but has larger average tracking errors; it's here mostly for compatibility + double v_target_trapz = fmin(findTrapezoidalDesiredVel(a_max, pos_error, v_final, tc->currentvel, tc->cycle_time), v_max); + tc->target_vel = v_target_trapz; + break; + } } - tc->target_vel = target_vel + errorvel; } + emcmotStatus->pos_tracking_velocity = tc->target_vel; + emcmotStatus->pos_tracking_error = pos_error; //Finally, clip requested velocity at zero if (tc->target_vel < 0.0) { @@ -3045,62 +2931,32 @@ STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc, //requested velocity to the current move nexttc->target_vel = tc->target_vel; } -} - - -/** - * Perform parabolic blending if needed between segments and handle status updates. - * This isolates most of the parabolic blend stuff to make the code path - * between tangent and parabolic blends easier to follow. - */ -STATIC int tpDoParabolicBlending(TP_STRUCT * const tp, TC_STRUCT * const tc, - TC_STRUCT * const nexttc) { - - tc_debug_print("in DoParabolicBlend\n"); - tpUpdateBlend(tp,tc,nexttc); - - /* Status updates */ - //Decide which segment we're in depending on which is moving faster - if(tc->currentvel > nexttc->currentvel) { - tpUpdateMovementStatus(tp, tc); - } else { - tpToggleDIOs(nexttc); - tpUpdateMovementStatus(tp, nexttc); - } -#ifdef TP_SHOW_BLENDS - // hack to show blends in axis - tp->motionType = 0; +#ifdef TC_DEBUG + print_json5_double_("target_vel", tc->target_vel); + tp_debug_json5_log_end("sync position updated"); #endif - - //Update velocity status based on both tc and nexttc - emcmotStatus->current_vel = tc->currentvel + nexttc->currentvel; - - return TP_ERR_OK; } - /** + TC_STRUCT *next2tc = tcqItem(&tp->queue, 2); * Do a complete update on one segment. * Handles the majority of updates on a single segment for the current cycle. */ -STATIC int tpUpdateCycle(TP_STRUCT * const tp, - TC_STRUCT * const tc, TC_STRUCT const * const nexttc) { - +int tpUpdateCycle(TP_STRUCT * const tp, + TC_STRUCT * const tc, + TC_STRUCT const * const nexttc, + UpdateCycleMode cycle_mode) +{ //placeholders for position for this update - EmcPose before; + PmVector before; //Store the current position due to this TC tcGetPos(tc, &before); - // Update the start velocity if we're not blending yet - if (!tc->blending_next) { - tc->vel_at_blend_start = tc->currentvel; - } - // Run cycle update with stored cycle time int res_accel = 1; double acc=0, vel_desired=0; - + // If the slowdown is not too great, use velocity ramping instead of trapezoidal velocity // Also, don't ramp up for parabolic blends if (tc->accel_mode && tc->term_cond == TC_TERM_COND_TANGENT) { @@ -3112,57 +2968,61 @@ STATIC int tpUpdateCycle(TP_STRUCT * const tp, tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired); } + if ( tc->motion_type == TC_RIGIDTAP + && tc->coords.rigidtap.state == RIGIDTAP_REVERSING + && !tp->aborting + && (tc->on_final_decel || tcGetDistanceToGo(tc, tp->reverse_run) < tc->coords.rigidtap.tap_uu_per_rev)) { + // We're going to hit the bottom of the hole (spindle has overshot) and are losing synchronization + LineDescriptor linedesc; + formatLinePrefix(&tc->tag, &linedesc); + tpStopWithError(tp, "%sSpindle reversal exceeded maximum overshoot in rigid tapping cycle. Increase cycle depth or reduce spindle speed.", + linedesc.buf); + } + + int accel_mode_ramp = (res_accel == TP_ERR_OK); tcUpdateDistFromAccel(tc, acc, vel_desired, tp->reverse_run); - tpDebugCycleInfo(tp, tc, nexttc, acc); + tpDebugCycleInfo(tp, tc, nexttc, acc, accel_mode_ramp, cycle_mode); //Check if we're near the end of the cycle and set appropriate changes tpCheckEndCondition(tp, tc, nexttc); - EmcPose displacement; + PmVector displacement; // Calculate displacement tcGetPos(tc, &displacement); - emcPoseSelfSub(&displacement, &before); + VecVecSubEq(&displacement, &before); //Store displacement (checking for valid pose) int res_set = tpAddCurrentPos(tp, &displacement); - -#ifdef TC_DEBUG - double mag; - emcPoseMagnitude(&displacement, &mag); - tc_debug_print("cycle movement = %f\n", mag); -#endif - return res_set; } - /** * Send default values to status structure. */ -STATIC int tpUpdateInitialStatus(TP_STRUCT const * const tp) { +int tpUpdateInitialStatus(TP_STRUCT const * const tp) { // Update queue length emcmotStatus->tcqlen = tcqLen(&tp->queue); // Set default value for requested speed emcmotStatus->requested_vel = 0.0; - //FIXME test if we can do this safely emcmotStatus->current_vel = 0.0; + emcmotStatus->dwell_time_remaining = 0.0; + // Set synched move waiting on spindle flag + emcmotStatus->waiting_on_spindle = tpIsWaitingOnSpindle(tp); return TP_ERR_OK; } - /** * Flag a segment as needing a split cycle. * In addition to flagging a segment as splitting, do any preparations to store * data for the next cycle. */ -STATIC inline int tcSetSplitCycle(TC_STRUCT * const tc, double split_time, +int tcSetSplitCycle(TC_STRUCT * const tc, double split_time, double v_f) { - tp_debug_print("split time for id %d is %.16g\n", tc->id, split_time); if (tc->splitting != 0 && split_time > 0.0) { - rtapi_print_msg(RTAPI_MSG_ERR,"already splitting on id %d with cycle time %.16g, dx = %.16g, split time %.12g\n", - tc->id, + rtapi_print_msg(RTAPI_MSG_ERR,"already splitting on segment %lld with cycle time %.16g, dx = %.16g, split time %.12g\n", + tc->unique_id, tc->cycle_time, tc->target-tc->progress, split_time); @@ -3174,234 +3034,178 @@ STATIC inline int tcSetSplitCycle(TC_STRUCT * const tc, double split_time, return 0; } - /** * Check remaining time in a segment and calculate split cycle if necessary. * This function estimates how much time we need to complete the next segment. * If it's greater than one timestep, then we do nothing and carry on. If not, * then we flag the segment as "splitting", so that during the next cycle, * it handles the transition to the next segment. + * + * @warning not reentrant since this is where we process TP-based dwells */ -STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc) { - - //Assume no split time unless we find otherwise - tc->cycle_time = tp->cycleTime; - //Initial guess at dt for next round - double dx = tcGetDistanceToGo(tc, tp->reverse_run); - tc_debug_print("tpCheckEndCondition: dx = %e\n",dx); - - if (dx <= TP_POS_EPSILON) { - //If the segment is close to the target position, then we assume that it's done. - tp_debug_print("close to target, dx = %.12f\n",dx); - //Force progress to land exactly on the target to prevent numerical errors. - tc->progress = tcGetTarget(tc, tp->reverse_run); - - if (!tp->reverse_run) { - tcSetSplitCycle(tc, 0.0, tc->currentvel); - } - if (tc->term_cond == TC_TERM_COND_STOP || tc->term_cond == TC_TERM_COND_EXACT || tp->reverse_run) { - tc->remove = 1; - } - return TP_ERR_OK; - } else if (tp->reverse_run) { - return TP_ERR_NO_ACTION; - } else if (tc->term_cond == TC_TERM_COND_STOP || tc->term_cond == TC_TERM_COND_EXACT) { - return TP_ERR_NO_ACTION; - } - - - double v_f = tpGetRealFinalVel(tp, tc, nexttc); - double v_avg = (tc->currentvel + v_f) / 2.0; - - //Check that we have a non-zero "average" velocity between now and the - //finish. If not, it means that we have to accelerate from a stop, which - //will take longer than the minimum 2 timesteps that each segment takes, so - //we're safely far form the end. - - //Get dt assuming that we can magically reach the final velocity at - //the end of the move. - // - //KLUDGE: start with a value below the cutoff - double dt = TP_TIME_EPSILON / 2.0; - if (v_avg > TP_VEL_EPSILON) { - //Get dt from distance and velocity (avoid div by zero) - dt = fmax(dt, dx / v_avg); +int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc) +{ + EndCondition ec; + if (tc->motion_type == TC_DWELL) { + // Knock one cycle off the remaining dwell time + ec.dt = fmax(tc->coords.dwell.remaining_time -= tp->cycleTime, 0.0); + ec.v_f = 0.0; } else { - if ( dx > (v_avg * tp->cycleTime) && dx > TP_POS_EPSILON) { - tc_debug_print(" below velocity threshold, assuming far from end\n"); - return TP_ERR_NO_ACTION; + ec = checkEndCondition( + tp->cycleTime, + tcGetDistanceToGo(tc, tp->reverse_run), + tc->currentvel, + tpGetRealFinalVel(tp, tc, nexttc), + tcGetTangentialMaxAccel(tc) + ); + } + + double dt = ec.dt; + int splitting = dt < tp->cycleTime; + if (splitting) { + if (dt < TP_TIME_EPSILON) { + dt = 0.0; } + tcSetSplitCycle(tc, dt, ec.v_f); } - - //Calculate the acceleration this would take: - - double dv = v_f - tc->currentvel; - double a_f = dv / dt; - - //If this is a valid acceleration, then we're done. If not, then we solve - //for v_f and dt given the max acceleration allowed. - double a_max = tcGetTangentialMaxAccel(tc); - - //If we exceed the maximum acceleration, then the dt estimate is too small. - double a = a_f; - int recalc = sat_inplace(&a, a_max); - - //Need to recalculate vf and above - if (recalc) { - tc_debug_print(" recalculating with a_f = %f, a = %f\n", a_f, a); - double disc = pmSq(tc->currentvel / a) + 2.0 / a * dx; - if (disc < 0) { - //Should mean that dx is too big, i.e. we're not close enough - tc_debug_print(" dx = %f, too large, not at end yet\n",dx); - return TP_ERR_NO_ACTION; - } - - if (disc < TP_TIME_EPSILON * TP_TIME_EPSILON) { - tc_debug_print("disc too small, skipping sqrt\n"); - dt = -tc->currentvel / a; - } else if (a > 0) { - tc_debug_print("using positive sqrt\n"); - dt = -tc->currentvel / a + pmSqrt(disc); - } else { - tc_debug_print("using negative sqrt\n"); - dt = -tc->currentvel / a - pmSqrt(disc); - } - - tc_debug_print(" revised dt = %f\n", dt); - //Update final velocity with actual result - v_f = tc->currentvel + dt * a; +#ifdef TC_DEBUG + { + print_json5_object_start_("tpCheckEndCondition"); + print_json5_tc_id_data_(tc); + print_json5_double_("v_final", ec.v_f); + print_json5_double_("t_remaining", ec.dt); + print_json5_double_("dt_used", dt); + print_json5_bool_("remove", tc->remove); + print_json5_bool_("need_split", splitting); + print_json5_object_end_(); } +#endif - if (dt < TP_TIME_EPSILON) { - //Close enough, call it done - tc_debug_print("revised dt small, finishing tc\n"); - tc->progress = tcGetTarget(tc, tp->reverse_run); - tcSetSplitCycle(tc, 0.0, v_f); - } else if (dt < tp->cycleTime ) { - tc_debug_print(" corrected v_f = %f, a = %f\n", v_f, a); - tcSetSplitCycle(tc, dt, v_f); - } else { - tc_debug_print(" dt = %f, not at end yet\n",dt); - return TP_ERR_NO_ACTION; - } return TP_ERR_OK; } - -STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, +int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT * const nexttc) { + tp_debug_json5_log_start(tpHandleSplitCycle); if (tc->remove) { + tp_debug_json5_log_end("segment flagged for removal"); //Don't need to update since this segment is flagged for removal return TP_ERR_NO_ACTION; } //Pose data to calculate movement due to finishing current TC - EmcPose before; + PmVector before; tcGetPos(tc, &before); - tp_debug_print("tc id %d splitting\n",tc->id); +#ifdef TP_DEBUG + /* Debug Output */ + print_json5_tc_id_data_(tc); + print_json5_double_("target", tc->target); + print_json5_double_("progress", tc->progress); + print_json5_double_("v_terminal", tc->term_vel); + print_json5_double_("dt", tc->cycle_time); +#endif + //Shortcut tc update by assuming we arrive at end tc->progress = tcGetTarget(tc,tp->reverse_run); //Get displacement from prev. position - EmcPose displacement; + PmVector displacement; tcGetPos(tc, &displacement); - emcPoseSelfSub(&displacement, &before); + VecVecSubEq(&displacement, &before); // Update tp's position (checking for valid pose) tpAddCurrentPos(tp, &displacement); -#ifdef TC_DEBUG - double mag; - emcPoseMagnitude(&displacement, &mag); - tc_debug_print("cycle movement = %f\n",mag); -#endif - // Trigger removal of current segment at the end of the cycle tc->remove = 1; if (!nexttc) { - tp_debug_print("no nexttc in split cycle\n"); + tp_debug_json5_log_end("no nexttc in split cycle"); return TP_ERR_OK; } + // Handle various cases of updates for split cycles + // Tangent: next segment gets a partial update for the remaining cycle time + // Exact: NO motion in next segment here since current segment must stop completely switch (tc->term_cond) { case TC_TERM_COND_TANGENT: nexttc->cycle_time = tp->cycleTime - tc->cycle_time; nexttc->currentvel = tc->term_vel; - tp_debug_print("Doing tangent split\n"); - break; + { + // TODO allow access to next segment when reverse blending is implemented + TC_STRUCT *next2tc = tp->reverse_run ? NULL : tcqItem(&tp->queue, 2); + tpUpdateCycle(tp, nexttc, next2tc, UPDATE_SPLIT); + } + break; case TC_TERM_COND_PARABOLIC: - break; + tp_debug_print("can't split with parabolic blends"); + return -1; case TC_TERM_COND_STOP: - break; case TC_TERM_COND_EXACT: break; - default: - rtapi_print_msg(RTAPI_MSG_ERR,"unknown term cond %d in segment %d\n", - tc->term_cond, - tc->id); } - // Run split cycle update with remaining time in nexttc - // KLUDGE: use next cycle after nextc to prevent velocity dip (functions fail gracefully w/ NULL) - int queue_dir_step = tp->reverse_run ? -1 : 1; - TC_STRUCT *next2tc = tcqItem(&tp->queue, queue_dir_step*2); - - tpUpdateCycle(tp, nexttc, next2tc); - // Update status for the split portion // FIXME redundant tangent check, refactor to switch - if (tc->cycle_time > nexttc->cycle_time && tc->term_cond == TC_TERM_COND_TANGENT) { + if (tc->term_cond == TC_TERM_COND_STOP + || tc->term_cond == TC_TERM_COND_EXACT + || (tc->cycle_time > nexttc->cycle_time && tc->term_cond == TC_TERM_COND_TANGENT)) { //Majority of time spent in current segment tpToggleDIOs(tc); - tpUpdateMovementStatus(tp, tc); + tpUpdateMovementStatus(tp, tc, nexttc); } else { tpToggleDIOs(nexttc); - tpUpdateMovementStatus(tp, nexttc); + // TODO allow access to next segment when reverse blending is implemented + TC_STRUCT *next2tc = tp->reverse_run ? NULL : tcqItem(&tp->queue, 2); + tpUpdateMovementStatus(tp, nexttc, next2tc); } + tp_debug_json5_log_end("splitting with next segment"); return TP_ERR_OK; } -STATIC int tpHandleRegularCycle(TP_STRUCT * const tp, +int tpHandleRegularCycle(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT * const nexttc) { + if (!tc) { + return TP_ERR_FAIL; + } if (tc->remove) { //Don't need to update since this segment is flagged for removal return TP_ERR_NO_ACTION; } + tp_debug_json5_log_start(tpHandleRegularCycle); //Run with full cycle time - tc_debug_print("Normal cycle\n"); tc->cycle_time = tp->cycleTime; - tpUpdateCycle(tp, tc, nexttc); - - /* Parabolic blending */ + tpUpdateCycle(tp, tc, nexttc, UPDATE_NORMAL); - double v_this = 0.0, v_next = 0.0; + //Update status for a normal step + tpToggleDIOs(tc); + tpUpdateMovementStatus(tp, tc, nexttc); + tp_debug_json5_log_end("cycle done"); + return TP_ERR_OK; +} - // cap the blend velocity at the current requested speed (factoring in feed override) - double target_vel_this = tpGetRealTargetVel(tp, tc); - double target_vel_next = tpGetRealTargetVel(tp, nexttc); +int tpSteppingCheck(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT * const nexttc) +{ + if (!tc || !tp) { + return TP_ERR_MISSING_INPUT; + } - tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL); - tc->blend_vel = v_this; - if (nexttc) { - nexttc->blend_vel = v_next; + if (nexttc && nexttc->tag.fields[GM_FIELD_STEPPING_ID] == tc->tag.fields[GM_FIELD_STEPPING_ID]) { + //Not stepping since next segment is the same ID, so don't enforce exact stop + return TP_ERR_NO_ACTION; } - if (nexttc && tcIsBlending(tc)) { - tpDoParabolicBlending(tp, tc, nexttc); - } else { - //Update status for a normal step - tpToggleDIOs(tc); - tpUpdateMovementStatus(tp, tc); + if (tc->motion_type != TC_LINEAR && tc->motion_type != TC_CIRCULAR) { + //No special case for stepping + return TP_ERR_NO_ACTION; } + return TP_ERR_OK; } - - /** * Calculate an updated goal position for the next timestep. * This is the brains of the operation. It's called every TRAJ period and is @@ -3410,50 +3214,53 @@ STATIC int tpHandleRegularCycle(TP_STRUCT * const tp, * status; I think those are spelled out here correctly and I can't clean it up * without breaking the API that the TP presents to motion. */ -int tpRunCycle(TP_STRUCT * const tp, long period) +tp_err_t updateSyncTargets(TP_STRUCT *tp, TC_STRUCT *tc, TC_STRUCT *nexttc) { - (void)period; - //Pointers to current and next trajectory component - TC_STRUCT *tc; - TC_STRUCT *nexttc; + // Clear old tracking status (will be updated in the appopriate handler) + clearPosTrackingStatus(); + + switch (tc->synchronized) { + case TC_SYNC_NONE: + emcmotStatus->spindle_sync_state = SYNC_INACTIVE; + emcmotStatus->spindle_fb.synced = 0; + return TP_ERR_OK; + case TC_SYNC_VELOCITY: + tpSyncVelocityMode(tc, nexttc); + return TP_ERR_OK; + case TC_SYNC_POSITION: + tpSyncPositionMode(tp, tc, nexttc); + checkPositionSyncError(tp, tc); + + return TP_ERR_OK; + } + emcmotStatus->spindle_sync_state = SYNC_INACTIVE; + return TP_ERR_INVALID; +} +int tpRunCycleInternal(TP_STRUCT * const tp) +{ /* Get pointers to current and relevant future segments. It's ok here if * future segments don't exist (NULL pointers) as we check for this later). */ - - int queue_dir_step = tp->reverse_run ? -1 : 1; - tc = tcqItem(&tp->queue, 0); - nexttc = tcqItem(&tp->queue, queue_dir_step * 1); + TC_STRUCT *tc = tcqItem(&tp->queue, 0); //!< Pointer to current motion segment or NULL + // TODO allow access to next segment when reverse blending is implemented + TC_STRUCT *nexttc = tp->reverse_run ? NULL : tcqItem(&tp->queue, 1); //!< Pointer to "next" motion segment or NULL //Set GUI status to "zero" state tpUpdateInitialStatus(tp); -#ifdef TC_DEBUG - //Hack debug output for timesteps - // NOTE: need to track every timestep, even those where the trajectory planner is idle - static double time_elapsed = 0; - time_elapsed+=tp->cycleTime; -#endif - - //If we have a NULL pointer, then the queue must be empty, so we're done. - if(!tc) { - tpHandleEmptyQueue(tp); - return TP_ERR_WAITING; - } - - tc_debug_print("-------------------\n"); - - - /* If the queue empties enough, assume that the program is near the end. - * This forces the last segment to be "finalized" to let the optimizer run.*/ - /*tpHandleLowQueue(tp);*/ - /* If we're aborting or pausing and the velocity has reached zero, then we * don't need additional planning and can abort here. */ if (tpHandleAbort(tp, tc, nexttc) == TP_ERR_STOPPED) { return TP_ERR_STOPPED; } + // If the TP is not aborting, then check for an empty queue (which means there's nothing to do) + if(!tc) { + tpHandleEmptyQueue(tp); + return TP_ERR_WAITING; + } + //Return early if we have a reason to wait (i.e. not ready for motion) if (tpCheckAtSpeed(tp, tc) != TP_ERR_OK){ return TP_ERR_WAITING; @@ -3461,40 +3268,20 @@ int tpRunCycle(TP_STRUCT * const tp, long period) int res_activate = tpActivateSegment(tp, tc); if (res_activate != TP_ERR_OK ) { + tp->time_at_wait = tp->time_elapsed_ticks; return res_activate; } // Preprocess rigid tap move (handles threading direction reversals) if (tc->motion_type == TC_RIGIDTAP) { - tpUpdateRigidTapState(tp, tc); - } - - /** If synchronized with spindle, calculate requested velocity to track - * spindle motion.*/ - switch (tc->synchronized) { - case TC_SYNC_NONE: - emcmotStatus->spindleSync = 0; - break; - case TC_SYNC_VELOCITY: - tp_debug_print("sync velocity\n"); - tpSyncVelocityMode(tp, tc, nexttc); - break; - case TC_SYNC_POSITION: - tp_debug_print("sync position\n"); - tpSyncPositionMode(tp, tc, nexttc); - break; - default: - tp_debug_print("unrecognized spindle sync state!\n"); - break; + tpUpdateRigidTapState(tp, tc, nexttc); } -#ifdef TC_DEBUG - EmcPose pos_before = tp->currentPos; -#endif + // If synchronized with spindle, calculate requested velocity to track spindle motion + updateSyncTargets(tp, tc, nexttc); + tpSteppingCheck(tp, tc, nexttc); - tcClearFlags(tc); - tcClearFlags(nexttc); // Update the current tc if (tc->splitting) { tpHandleSplitCycle(tp, tc, nexttc); @@ -3502,22 +3289,6 @@ int tpRunCycle(TP_STRUCT * const tp, long period) tpHandleRegularCycle(tp, tc, nexttc); } -#ifdef TC_DEBUG - double mag; - EmcPose disp; - emcPoseSub(&tp->currentPos, &pos_before, &disp); - emcPoseMagnitude(&disp, &mag); - tc_debug_print("time: %.12e total movement = %.12e vel = %.12e\n", - time_elapsed, - mag, emcmotStatus->current_vel); - - tc_debug_print("tp_displacement = %.12e %.12e %.12e time = %.12e\n", - disp.tran.x, - disp.tran.y, - disp.tran.z, - time_elapsed); -#endif - // If TC is complete, remove it from the queue. if (tc->remove) { tpCompleteSegment(tp, tc); @@ -3526,17 +3297,93 @@ int tpRunCycle(TP_STRUCT * const tp, long period) return TP_ERR_OK; } -int tpSetSpindleSync(TP_STRUCT * const tp, int spindle, double sync, int mode) { - if(sync) { - if (mode) { +double tpGetCurrentVel(TP_STRUCT const * const tp, PmVector const * const v_current, int *pure_angular) +{ + PmCartesian xyz, abc, uvw; + VecToCart(v_current, &xyz, &abc, &uvw); + double v_out = 0.0; + *pure_angular = 0; + switch (tpGetExecTag(tp).fields[GM_FIELD_FEED_AXES]) { + case 0: + break; + case 1: + pmCartMag(&xyz, &v_out); + break; + case 2: + pmCartMag(&uvw, &v_out); + break; + case 3: + pmCartMag(&abc, &v_out); + *pure_angular = 1; + break; + } + return v_out; +} + +int tpRunCycle(TP_STRUCT *tp) +{ + // Before every TP update, ensure that elapsed time and + // TP measurements are stored for error checks + tp->time_elapsed_sec+=tp->cycleTime; + ++tp->time_elapsed_ticks; + PmVector const axis_pos_old = tp->currentPos; + + int res = tpRunCycleInternal(tp); + + // After update (even a no-op), update pos / vel / accel + PmVector const axis_vel_old = tp->currentVel; + PmVector const axis_pos = tp->currentPos; + + PmVector axis_vel; + VecVecSub(&axis_pos, &axis_pos_old, &axis_vel); + VecScalMultEq(&axis_vel, 1.0 / tp->cycleTime); + tp->currentVel = axis_vel; + + emcmotStatus->current_vel = tpGetCurrentVel(tp, &tp->currentVel, &emcmotStatus->pure_angular_move); + + if (needConsistencyCheck(CCHECK_AXIS_LIMITS)) { + PmVector axis_accel; + VecVecSub(&axis_vel, &axis_vel_old, &axis_accel); + VecScalMultEq(&axis_accel, 1.0 / tp->cycleTime); + + unsigned accel_error_mask = findAccelViolations(axis_accel); + unsigned vel_error_mask = findVelocityViolations(axis_vel); + unsigned pos_limit_error_mask = findPositionViolations(axis_pos); + + reportTPAxisError(tp, accel_error_mask, "Acceleration limit exceeded"); + reportTPAxisError(tp, vel_error_mask, "Velocity limit exceeded"); + reportTPAxisError(tp, pos_limit_error_mask, "Position limits exceeded"); + + if (_tc_debug || ( + accel_error_mask | vel_error_mask | pos_limit_error_mask) + ) { + print_json5_log_start(tpRunCycle); + print_json5_long_long_("time_ticks", tp->time_elapsed_ticks); + print_json5_PmVector(axis_pos); + print_json5_PmVector(axis_vel); + print_json5_PmVector(axis_accel); + double current_vel = emcmotStatus->current_vel; + print_json5_double(current_vel); + print_json5_double_("time", tp->time_elapsed_sec); + print_json5_end_(); + } + } + return res; +} + +int tpSetSpindleSync(TP_STRUCT * const tp, double sync, int velocity_mode) { + // WARNING assumes positive sync + if(sync > 0) { + if (velocity_mode) { tp->synchronized = TC_SYNC_VELOCITY; } else { tp->synchronized = TC_SYNC_POSITION; } tp->uu_per_rev = sync; - tp->spindle.spindle_num = spindle; - } else - tp->synchronized = 0; + } else { + tp->synchronized = TC_SYNC_NONE; + tp->uu_per_rev = sync; + } return TP_ERR_OK; } @@ -3585,12 +3432,22 @@ int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos) ZERO_EMC_POSE((*pos)); return TP_ERR_FAIL; } else { - *pos = tp->currentPos; + pmVectorToEmcPose(&tp->currentPos, pos); } return TP_ERR_OK; } +EmcPose tpGetGoalPos(TP_STRUCT const * const tp) +{ + EmcPose out={}; + if (tp) { + pmVectorToEmcPose(&tp->goalPos, &out); + } + + return out; +} + int tpIsDone(TP_STRUCT * const tp) { if (0 == tp) { @@ -3619,7 +3476,6 @@ int tpActiveDepth(TP_STRUCT * const tp) } int tpSetAout(TP_STRUCT * const tp, unsigned char index, double start, double end) { - (void)end; if (0 == tp) { return TP_ERR_FAIL; } @@ -3630,7 +3486,6 @@ int tpSetAout(TP_STRUCT * const tp, unsigned char index, double start, double en } int tpSetDout(TP_STRUCT * const tp, int index, unsigned char start, unsigned char end) { - (void)end; if (0 == tp) { return TP_ERR_FAIL; } @@ -3645,7 +3500,6 @@ int tpSetDout(TP_STRUCT * const tp, int index, unsigned char start, unsigned cha int tpSetRunDir(TP_STRUCT * const tp, tc_direction_t dir) { - // Can't change direction while moving if (tpIsMoving(tp)) { return TP_ERR_FAIL; } @@ -3655,10 +3509,9 @@ int tpSetRunDir(TP_STRUCT * const tp, tc_direction_t dir) case TC_DIR_REVERSE: tp->reverse_run = dir; return TP_ERR_OK; - default: - rtapi_print_msg(RTAPI_MSG_ERR,"Invalid direction flag in SetRunDir"); - return TP_ERR_FAIL; } + rtapi_print_msg(RTAPI_MSG_ERR,"Invalid direction flag in SetRunDir"); + return TP_ERR_FAIL; } int tpIsMoving(TP_STRUCT const * const tp) @@ -3675,38 +3528,28 @@ int tpIsMoving(TP_STRUCT const * const tp) return false; } -// api: functions called by motion: -EXPORT_SYMBOL(tpMotFunctions); -EXPORT_SYMBOL(tpMotData); - -EXPORT_SYMBOL(tpAbort); -EXPORT_SYMBOL(tpActiveDepth); -EXPORT_SYMBOL(tpAddCircle); -EXPORT_SYMBOL(tpAddLine); -EXPORT_SYMBOL(tpAddRigidTap); -EXPORT_SYMBOL(tpClear); -EXPORT_SYMBOL(tpCreate); -EXPORT_SYMBOL(tpGetExecId); -EXPORT_SYMBOL(tpGetExecTag); -EXPORT_SYMBOL(tpGetMotionType); -EXPORT_SYMBOL(tpGetPos); -EXPORT_SYMBOL(tpIsDone); -EXPORT_SYMBOL(tpPause); -EXPORT_SYMBOL(tpQueueDepth); -EXPORT_SYMBOL(tpResume); -EXPORT_SYMBOL(tpRunCycle); -EXPORT_SYMBOL(tpSetAmax); -EXPORT_SYMBOL(tpSetAout); -EXPORT_SYMBOL(tpSetCycleTime); -EXPORT_SYMBOL(tpSetDout); -EXPORT_SYMBOL(tpSetId); -EXPORT_SYMBOL(tpSetPos); -EXPORT_SYMBOL(tpSetRunDir); -EXPORT_SYMBOL(tpSetSpindleSync); -EXPORT_SYMBOL(tpSetTermCond); -EXPORT_SYMBOL(tpSetVlimit); -EXPORT_SYMBOL(tpSetVmax); - -EXPORT_SYMBOL(tcqFull); - -#undef MAKE_TP_HAL_PINS +void formatLinePrefix(struct state_tag_t const *tag, LineDescriptor *linebuf) +{ + linebuf->buf[0]='\0'; + int line = tag->fields[GM_FIELD_LINE_NUMBER]; + const int len = sizeof(linebuf->buf); + if (line > 0) { + rtapi_snprintf(linebuf->buf, len, "Line %d: ", line); + } + linebuf->buf[len-1]='\0'; +} + +void tpStopWithError(TP_STRUCT *tp, const char *fmt, ...) +{ + if (fmt) { + va_list args; + va_start(args, fmt); + enqueueError(fmt, args); + va_end(args); + } + tpAbort(tp); + SET_MOTION_ERROR_FLAG(1); +} + + +// vim:sw=4:sts=4:et: diff --git a/src/emc/tp/tp.h b/src/emc/tp/tp.h index a50e39413b8..e0c095548fd 100644 --- a/src/emc/tp/tp.h +++ b/src/emc/tp/tp.h @@ -7,7 +7,7 @@ * Author: * License: GPL Version 2 * System: Linux -* +* * Copyright (c) 2004 All rights reserved. * ********************************************************************/ @@ -17,74 +17,94 @@ #include "posemath.h" #include "tc_types.h" #include "tp_types.h" +#include "tp_enums.h" #include "tcq.h" -// functions not used by motmod: -int tpAddCurrentPos(TP_STRUCT * const tp, EmcPose const * const disp); -int tpSetCurrentPos(TP_STRUCT * const tp, EmcPose const * const pos); -void tpToggleDIOs(TC_STRUCT * const tc); //gets called when a new tc is - //taken from the queue. it checks - //and toggles all needed DIO's -int tpIsMoving(TP_STRUCT const * const tp); -int tpInit(TP_STRUCT * const tp); - -// functions used by motmod: -int tpCreate(TP_STRUCT * const tp, int _queueSize,int id); +int tpCreate(TP_STRUCT * const tp, int _queueSize, TC_STRUCT * const tcSpace); int tpClear(TP_STRUCT * const tp); +int tpInit(TP_STRUCT * const tp); int tpClearDIOs(TP_STRUCT * const tp); -int tpSetCycleTime(TP_STRUCT * tp, double secs); -int tpSetVmax(TP_STRUCT * tp, double vmax, double ini_maxvel); -int tpSetVlimit(TP_STRUCT * tp, double limit); -int tpSetAmax(TP_STRUCT * tp, double amax); -int tpSetId(TP_STRUCT * tp, int id); -int tpGetExecId(TP_STRUCT * tp); -struct state_tag_t tpGetExecTag(TP_STRUCT * const tp); -int tpSetTermCond(TP_STRUCT * tp, int cond, double tolerance); -int tpSetPos(TP_STRUCT * tp, EmcPose const * const pos); -int tpRunCycle(TP_STRUCT * tp, long period); -int tpPause(TP_STRUCT * tp); -int tpResume(TP_STRUCT * tp); -int tpAbort(TP_STRUCT * tp); +int tpSetCycleTime(TP_STRUCT * const tp, double secs); +int tpSetVlimit(TP_STRUCT * const tp, double vLimit, double vLimitAng); +int tpSetId(TP_STRUCT * const tp, int id); +tc_unique_id_t tpGetNextUniqueId(TP_STRUCT * const tp); +void tcSetId(TP_STRUCT * const tp, TC_STRUCT * const tc, struct state_tag_t tag); +int tpGetExecId(TP_STRUCT const * const tp); +int tpGetCompletedId(TP_STRUCT const * const tp); +int tpGetQueuedId(TP_STRUCT const * const tp); +struct state_tag_t tpGetExecTag(const TP_STRUCT * const tp); +int tpGetExecSrcLine(const TP_STRUCT * const tp); +int tpGetNextExecId(TP_STRUCT const * const tp); +int tpSetTermCond(TP_STRUCT * const tp, tc_term_cond_t cond, double tolerance); +int tpSetPos(TP_STRUCT * const tp, EmcPose const * const pos); +int tpAddCurrentPos(TP_STRUCT * const tp, const PmVector * const disp); +int tpSetCurrentPos(TP_STRUCT * const tp, EmcPose const * const pos); + int tpAddRigidTap(TP_STRUCT * const tp, EmcPose end, - double vel, + double tap_uu_per_rev, + double retract_uu_per_rev, double ini_maxvel, double acc, unsigned char enables, + char atspeed, double scale, struct state_tag_t tag); -int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, - double vel, double ini_maxvel, double acc, unsigned char enables, - char atspeed, int indexrotary, struct state_tag_t tag); -int tpAddCircle(TP_STRUCT * const tp, EmcPose end, PmCartesian center, - PmCartesian normal, int turn, int canon_motion_type, double vel, - double ini_maxvel, double acc, unsigned char enables, - char atspeed, struct state_tag_t tag); + +int tpAddLine(TP_STRUCT * const tp, + EmcPose end, + EMCMotionTypes canon_motion_type, + double vel, + double ini_maxvel, + double acc, + unsigned char enables, + char atspeed, + IndexRotaryAxis indexrotary, + struct state_tag_t tag); + +int tpAddCircle(TP_STRUCT * const tp, + EmcPose end, + PmCartesian center, + PmCartesian normal, + int turn, double expected_angle_rad, + EMCMotionTypes canon_motion_type, + double vel, + double ini_maxvel, + double acc, + double acc_normal, + unsigned char enables, + char atspeed, + struct state_tag_t tag); + +int tpAddDwell(TP_STRUCT * const tp, + double time_sec, double delta_rpm, + struct state_tag_t tag); + +tp_err_t tpRunCycle(TP_STRUCT * const tp); +int tpPause(TP_STRUCT * const tp); +int tpResume(TP_STRUCT * const tp); +int tpAbort(TP_STRUCT * const tp); int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos); +EmcPose tpGetGoalPos(TP_STRUCT const * const tp); int tpIsDone(TP_STRUCT * const tp); int tpQueueDepth(TP_STRUCT * const tp); int tpActiveDepth(TP_STRUCT * const tp); int tpGetMotionType(TP_STRUCT * const tp); -int tpSetSpindleSync(TP_STRUCT * const tp, int spindle, double sync, int wait); +int tpSetSpindleSync(TP_STRUCT * const tp, double sync, int velocity_mode); +void tpToggleDIOs(TC_STRUCT * const tc); //gets called when a new tc is taken from the queue. it checks and toggles all needed DIO's int tpSetAout(TP_STRUCT * const tp, unsigned char index, double start, double end); int tpSetDout(TP_STRUCT * const tp, int index, unsigned char start, unsigned char end); //gets called to place DIO toggles on the TC queue int tpSetRunDir(TP_STRUCT * const tp, tc_direction_t dir); +int tpIsMoving(TP_STRUCT const * const tp); -//--------------------------------------------------------------------- -// Module interface -void tpMotFunctions(void(*pDioWrite)(int,char) - ,void(*pAioWrite)(int,double) - ,void(*pSetRotaryUnlock)(int,int) - ,int( *pGetRotaryUnlock)(int) - ,double(*paxis_get_vel_limit)(int) - ,double(*paxis_get_acc_limit)(int) - ); +typedef struct { + char buf[20]; +} LineDescriptor; -void tpMotData(emcmot_status_t * - ,emcmot_config_t * - ); -//--------------------------------------------------------------------- +void formatLinePrefix(struct state_tag_t const *tag, LineDescriptor *linebuf); +void tpStopWithError(TP_STRUCT * tp, const char *fmt, ...) + __attribute__((format(printf,2,3))); #endif /* TP_H */ diff --git a/src/emc/tp/tp_call_wrappers.h b/src/emc/tp/tp_call_wrappers.h new file mode 100644 index 00000000000..52789a841c1 --- /dev/null +++ b/src/emc/tp/tp_call_wrappers.h @@ -0,0 +1,19 @@ +#ifndef TP_CALL_WRAPPERS_H +#define TP_CALL_WRAPPERS_H + +#include "tp_enums.h" + +#define CH_ERR(expect__, mycall__) \ +do {\ + int expt = expect__; \ + int res = mycall__; \ + if (expt != res) { \ + tp_debug_print("%s failed with %d (expect %d) at %s:%d\n", #mycall__, res, expt, __FUNCTION__, __LINE__); \ + return res; \ + } \ +} while (0) + +#define CHP(mycall__) CH_ERR(TP_ERR_OK, mycall__) + + +#endif // TP_CALL_WRAPPERS_H diff --git a/src/emc/tp/tp_debug.h b/src/emc/tp/tp_debug.h index d14c5e5e511..afc17194348 100644 --- a/src/emc/tp/tp_debug.h +++ b/src/emc/tp/tp_debug.h @@ -6,7 +6,7 @@ * License: GPL Version 2 * System: Linux * -* Copyright (c) 2013 All rights reserved. +* Copyright (c) 2013-2019 All rights reserved. * * Last change: ********************************************************************/ @@ -14,23 +14,50 @@ #define TP_DEBUG_H #include "rtapi.h" /* printing functions */ +#include "posemath.h" +#include "emcpos.h" +#include "rtapi_json5.h" +#include "tp_json5_print.h" /** TP debug stuff */ #ifdef TP_DEBUG //Kludge because I didn't know any better at the time //FIXME replace these with better names? #define tp_debug_print(...) rtapi_print(__VA_ARGS__) -#elif defined(UNIT_TEST) +#define tp_debug_only(funcname,...) funcname(__VA_ARGS__) +#elif defined(UNIT_TEST_VERBOSE) #include #define tp_debug_print(...) printf(__VA_ARGS__) +#define tp_debug_only(funcname,...) funcname(__VA_ARGS__) #else #define tp_debug_print(...) +#define tp_debug_only(funcname,...) #endif -// Verbose but effective wrappers for building faux-JSON debug output for a function -#define tp_debug_json_double(varname_) tp_debug_print("%s: %g, ", #varname_, varname_) -#define tp_debug_json_start(fname_) tp_debug_print("%s: {", #fname_) -#define tp_debug_json_end() tp_debug_print("}\n") +#define print_json5_log_start(fname_) do { \ + print_json5_start_(); \ + print_json5_string_("log_entry", #fname_); \ +} while (0) + +#define print_json5_log_end() print_json5_end_() + +// Another layer of macro wrappers to conditionally compile debug-only statements +#define tp_debug_json5_double(varname_) tp_debug_only(print_json5_double, varname_) +#define tp_debug_json5_string(varname_) tp_debug_only(print_json5_string, varname_) +#define tp_debug_json5_long(varname_) tp_debug_only(print_json5_long, varname_) +#define tp_debug_json5_unsigned(varname_) tp_debug_only(print_json5_unsigned, varname_) +#define tp_debug_json5_PmCartesian(varname_) tp_debug_only(print_json5_PmCartesian, varname_) +#define tp_debug_json5_PmVector(varname_) tp_debug_only(print_json5_PmVector, varname_) +#define tp_debug_json5_EmcPose(varname_) tp_debug_only(print_json5_EmcPose, varname_) +#define tp_debug_json5_PmCartLine(name_, value_) tp_debug_only(print_json5_PmCartLine_, name_, value_) +#define tp_debug_json5_PmCircle(name_, value_) tp_debug_only(print_json5_PmCircle_, name_, value_) +#define tp_debug_json5_log_start(fname_) tp_debug_only(print_json5_log_start, fname_) +#define tp_debug_json5_log_end(...) do { \ + tp_debug_print("reason: \""); \ + tp_debug_print(__VA_ARGS__); \ + tp_debug_print("\""); \ + tp_debug_only(print_json5_end_); \ +} while (0) /** Use for profiling to make static function names visible */ #ifdef TP_PROFILE @@ -46,6 +73,13 @@ #define tc_debug_print(...) #endif +/** "TC" debug info for inspecting trajectory planner output at each timestep */ +#ifdef TP_PEDANTIC_DEBUG +#define tc_pdebug_print(...) rtapi_print(__VA_ARGS__) +#else +#define tc_pdebug_print(...) +#endif + /** TP position data output to debug acceleration spikes */ #ifdef TP_POSEMATH_DEBUG #define tp_posemath_debug(...) rtapi_print(__VA_ARGS__) diff --git a/src/emc/tp/tp_enums.h b/src/emc/tp/tp_enums.h new file mode 100644 index 00000000000..53e128335f9 --- /dev/null +++ b/src/emc/tp/tp_enums.h @@ -0,0 +1,57 @@ +#ifndef TP_ENUMS_H +#define TP_ENUMS_H + +/** + * TP return codes. + * This enum is a catch-all for useful return statuses from TP + * internal functions. This may be replaced with a better system in + * the future. + */ +typedef enum { + TP_ERR_UNRECOVERABLE = -10, + TP_ERR_INVALID = -9, + TP_ERR_INPUT_TYPE = -8, + TP_ERR_TOLERANCE = -7, + TP_ERR_RADIUS_TOO_SMALL = -6, + TP_ERR_GEOM = -5, + TP_ERR_RANGE = -4, + TP_ERR_MISSING_OUTPUT = -3, + TP_ERR_MISSING_INPUT = -2, + TP_ERR_FAIL = -1, + TP_ERR_OK = 0, + TP_ERR_NO_ACTION, + TP_ERR_SLOWING, + TP_ERR_STOPPED, + TP_ERR_WAITING, + TP_ERR_ZERO_LENGTH, + TP_ERR_REVERSE_EMPTY, + TP_ERR_LAST +} tp_err_t; + +typedef enum { + TC_INTERSECT_INCOMPATIBLE=-1, + TC_INTERSECT_TANGENT, + TC_INTERSECT_NONTANGENT, +} TCIntersectType; + +typedef enum { + UPDATE_NORMAL, + UPDATE_PARABOLIC_BLEND, + UPDATE_SPLIT +} UpdateCycleMode; + +/** + * Describes blend modes used in the trajectory planner. + * @note these values are used as array indices, so make sure valid options + * start at 0 and increase by one. + */ +typedef enum { + NO_BLEND = -1, + PARABOLIC_BLEND, + TANGENT_SEGMENTS_BLEND, + ARC_BLEND +} tc_blend_type_t; + + + +#endif // TP_ENUMS_H diff --git a/src/emc/tp/tp_json5_print.h b/src/emc/tp/tp_json5_print.h new file mode 100644 index 00000000000..7f36026d8c2 --- /dev/null +++ b/src/emc/tp/tp_json5_print.h @@ -0,0 +1,206 @@ +#ifndef TP_JSON5_PRINT_H +#define TP_JSON5_PRINT_H + +#include "rtapi_json5.h" +#include "posemath.h" +#include "emcpos.h" +#include "tc_types.h" +#include "blendmath_types.h" + +#define print_json5_PmCartesian(varname_) print_json5_PmCartesian_(#varname_, varname_) +#define print_json5_PmVector(varname_) print_json5_PmVector_(#varname_, varname_) +#define print_json5_EmcPose(varname_) print_json5_EmcPose_(#varname_, varname_) +#define print_json5_PmCartLine(varname_) print_json5_PmCartLine_( #varname_, varname_) +#define print_json5_PmCircle(varname_) print_json5_PmCircle_(#varname_, varname_) + +#define print_json5_PmCartesian_field(base_, varname_) print_json5_PmCartesian_(#varname_, (base_)->varname_) +#define print_json5_PmVector_field(base_, varname_) print_json5_PmVector_(#varname_, (base_)->varname_) +#define print_json5_EmcPose_field(base_, varname_) print_json5_EmcPose_(#varname_, (base_)->varname_) +#define print_json5_PmCartLine_field(base_, varname_) print_json5_PmCartLine_( #varname_, (base_)->varname_) +#define print_json5_PmCircle_field(base_, varname_) print_json5_PmCircle_(#varname_, (base_)->varname_) + + +static inline void print_json5_tc_id_data_(TC_STRUCT const * const tc) +{ + print_json5_int_("src_line", tc->tag.fields[GM_FIELD_LINE_NUMBER]); + print_json5_int_field(tc, id); + print_json5_long_long_field(tc, unique_id); +} + + +static inline void print_json5_PmCartesian_(const char *varname, PmCartesian value) +{ + + rtapi_print("%s: [%0.17g, %0.17g, %0.17g], ", varname ?: "NO_NAME", value.x, value.y, value.z); +} + +static inline void print_json5_PmVector_(const char *varname, PmVector value) +{ + rtapi_print("%s: [%0.17g, %0.17g, %0.17g, %0.17g, %0.17g, %0.17g, %0.17g, %0.17g, %0.17g], ", + varname, + value.ax[0], + value.ax[1], + value.ax[2], + value.ax[3], + value.ax[4], + value.ax[5], + value.ax[6], + value.ax[7], + value.ax[8] + ); +} + +static inline void print_json5_EmcPose_(const char *varname, EmcPose value) +{ + rtapi_print("%s: [%0.17g, %0.17g, %0.17g, %0.17g, %0.17g, %0.17g, %0.17g, %0.17g, %0.17g], ", + varname, + value.tran.x, + value.tran.y, + value.tran.z, + value.a, + value.b, + value.c, + value.u, + value.v, + value.w); +} + +static inline void print_json5_PmCartLine_(const char *name, PmCartLine value) +{ + print_json5_object_start_(name); + // Need manual field names here + print_json5_PmCartesian_("start", value.start); + print_json5_PmCartesian_("end", value.start); + print_json5_PmCartesian_("uVec", value.uVec); + print_json5_double_("tmag", value.tmag); + print_json5_double_("tmag_zero", value.tmag_zero); + print_json5_object_end_(); +} + +static inline void print_json5_PmCircle_(const char *name, PmCircle circle) +{ + print_json5_object_start_(name); + print_json5_PmCartesian_("center", circle.center); + print_json5_PmCartesian_("normal", circle.normal); + print_json5_PmCartesian_("rTan", circle.rTan); + print_json5_PmCartesian_("rPerp", circle.rPerp); + print_json5_PmCartesian_("rHelix", circle.rHelix); + print_json5_double_("radius", circle.radius); + print_json5_double_("angle", circle.angle); + print_json5_double_("spiral", circle.spiral); + print_json5_object_end_(); +} + +static inline void print_json5_PmLine9_(const char *name, PmLine9 value) +{ + //TODO +} + +static inline void print_json5_PmCircle9_(const char *name, PmCircle9 value) +{ + //TODO +} + +static inline void print_json5_SpiralArcLengthFit_(const char *fname, SpiralArcLengthFit fit) +{ + print_json5_object_start_(fname); + print_json5_double_("b0", fit.b0); + print_json5_double_("b1", fit.b1); + print_json5_double_("total_planar_length", fit.total_planar_length); + print_json5_long_("spiral_in", fit.spiral_in); + print_json5_object_end_(); +} + +static inline void print_json5_blend_boundary_t(const char *fname, blend_boundary_t const * const r) +{ + print_json5_object_start_(fname); + print_json5_PmVector_field(r, P1); + print_json5_PmVector_field(r, P2); + print_json5_PmVector_field(r, u1); + print_json5_PmVector_field(r, u2); + print_json5_double_field(r, s1); + print_json5_double_field(r, s2); + print_json5_object_end_(); +} + +static inline void print_json5_biarc_control_points_t(const char *fname, biarc_control_points_t const * const c) +{ + print_json5_object_start_(fname); + print_json5_PmVector_field(c, Pb1); + print_json5_PmVector_field(c, Pb2); + print_json5_PmVector_field(c, u_mid); + print_json5_PmVector_field(c, P_mid); + print_json5_double_field(c, d); + print_json5_object_end_(); +} + +static inline void print_json5_biarc_solution_errors_t(const char *fname, biarc_solution_errors_t const * const r) +{ + print_json5_object_start_(fname); + print_json5_double_field(r, deviation); + print_json5_double_field(r, radius_rel); + print_json5_double_field(r, radius_abs); + print_json5_object_end_(); +} + +static inline void print_json5_biarc_solution_t(const char *fname, biarc_solution_t const * const r) +{ + print_json5_object_start_(fname); + print_json5_double_field(r, Rb); + print_json5_double_field(r, R_geom); + print_json5_double_field(r, arc_len_est); + print_json5_double_field(r, R_plan); + print_json5_double_field(r, T_plan); + print_json5_biarc_solution_errors_t("err", &r->err); + print_json5_object_end_(); +} + +static inline void print_json5_SphericalArc9(const char *fname, SphericalArc9 const * const arc) { + print_json5_object_start_(fname); + print_json5_PmVector_field(arc, start); + print_json5_PmVector_field(arc, end); + print_json5_PmVector_field(arc, center); + print_json5_PmVector_field(arc, rStart); + print_json5_PmVector_field(arc, rEnd); + print_json5_PmVector_field(arc, uTan); + print_json5_double_field(arc, radius); + print_json5_double_field(arc, spiral); + print_json5_double_field(arc, angle); + print_json5_double_field(arc, Sangle); + print_json5_double_field(arc, line_length); + print_json5_object_end_(); +} + +static inline void print_json5_TC_STRUCT_kinematics(const char *fname, TC_STRUCT const * const tc) +{ + print_json5_object_start_(fname); + print_json5_double_("maxvel", tc->maxvel_geom); + print_json5_double_field(tc, maxaccel); + print_json5_double_field(tc, acc_normal_max); + print_json5_double_field(tc, acc_ratio_tan); + print_json5_double_field(tc, reqvel); + print_json5_double_field(tc, target); + print_json5_int_field(tc, motion_type); + print_json5_object_end_(); +} + +static inline void print_json5_SpiralArcLengthFit( + const char *fname, + SpiralArcLengthFit const * const fit, + double min_radius, + double total_angle, + double spiral_coef) +{ + print_json5_object_start_(fname); + print_json5_double(min_radius); + print_json5_double(total_angle); + print_json5_double(spiral_coef); + print_json5_double_("b0", fit->b0); + print_json5_double_("b1", fit->b1); + print_json5_double_("total_planar_length", fit->total_planar_length); + print_json5_bool_("spiral_in", fit->spiral_in); + print_json5_object_end_(); +} + + +#endif // TP_JSON5_PRINT_H diff --git a/src/emc/tp/tp_priv.h b/src/emc/tp/tp_priv.h new file mode 100644 index 00000000000..147ca7530bf --- /dev/null +++ b/src/emc/tp/tp_priv.h @@ -0,0 +1,199 @@ +#ifndef TP_PRIV_H +#define TP_PRIV_H + +#include "tp_enums.h" +#include "tc_types.h" +#include "tp_types.h" +#include "stdbool.h" +#include "error_util.h" + +int tpComputeBlendVelocity( + TC_STRUCT const *tc, + TC_STRUCT const *nexttc, + double v_target_this, + double v_target_next, + double *v_blend_this, + double *v_blend_next, + double *v_blend_net); + +double estimateParabolicBlendPerformance( + TC_STRUCT const *tc, + TC_STRUCT const *nexttc); + +int tpCheckEndCondition(TP_STRUCT const * const tp, + TC_STRUCT * const tc, + TC_STRUCT const * const nexttc); + +int tpUpdateCycle(TP_STRUCT * const tp, + TC_STRUCT * const tc, + TC_STRUCT const * const nexttc, + UpdateCycleMode cycle_mode); + +double tpGetCurrentVel(TP_STRUCT const * const tp, PmVector const * const v_current, int * pure_angular); +int tpRunOptimization(TP_STRUCT * const tp); + +int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc); + +int tpIsWaitingOnSpindle(TP_STRUCT const * const tp); + +int tpSetupSegmentBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc); + +tp_err_t tpCreateBiarcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const this_tc); + +int tcRotaryMotionCheck(TC_STRUCT const * const tc); + +double tpGetTangentKinkRatio(void); + +double tpGetRealAbsFeedScale(TP_STRUCT const * const tp, + TC_STRUCT const * const tc); + +double tpGetRealAbsTargetVel(TP_STRUCT const * const tp, + TC_STRUCT const * const tc); + +double getMaxBlendFeedScale(TC_STRUCT const * prev_tc, TC_STRUCT const * tc); + +double tpGetRealMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc); + +double tpGetRealFinalVel(TP_STRUCT const * const tp, + TC_STRUCT const * const tc, TC_STRUCT const * const nexttc); + +void setSpindleOrigin(spindle_origin_t *origin, double position); + +double tpCalculateTriangleVel(TC_STRUCT const *tc); + +double calculateOptimizationInitialVel(const TC_STRUCT * const prev_tc, const TC_STRUCT * const tc); + +bool isArcBlendFaster(TC_STRUCT const * const prev_tc, double expected_v_max); + +tc_blend_type_t tpChooseBestBlend(TC_STRUCT * const prev_tc, + TC_STRUCT * const tc, + double arc_blend_maxvel); + +tp_err_t tpCreateLineLineBlend(TP_STRUCT * const tp, + TC_STRUCT * const prev_tc, + TC_STRUCT * const tc, + TC_STRUCT * const blend_tc); + +int handlePrevTermCondition(TC_STRUCT *prev_tc, TC_STRUCT *tc); +int handleModeChange(TC_STRUCT *prev_tc, TC_STRUCT *tc); +int tpSetupSyncedIO(TP_STRUCT * const tp, TC_STRUCT * const tc); + +tp_err_t tpFinalizeAndEnqueue(TP_STRUCT * const tp, TC_STRUCT * const tc, const PmVector * nominal_goal); + +int tpComputeOptimalVelocity(TC_STRUCT * const tc, + TC_STRUCT * const prev1_tc); + +TCIntersectType tpSetupTangent( + TP_STRUCT const * const tp, + TC_STRUCT * const prev_tc, + TC_STRUCT * const tc); + +int tcUpdateDistFromAccel(TC_STRUCT * const tc, double acc, double vel_desired, int reverse_run); + +void tpDebugCycleInfo(TP_STRUCT const * const tp, + TC_STRUCT const * const tc, + TC_STRUCT const * const nexttc, + double acc, + int accel_mode, + UpdateCycleMode cycle); + +int tpCalculateRampAccel(TP_STRUCT const * const tp, + TC_STRUCT * const tc, + TC_STRUCT const * const nexttc, + double * const acc, + double * const vel_desired); + +double estimate_rigidtap_decel_distance(double vel, double uu_per_rev); + +void tpUpdateRigidTapState( + TP_STRUCT * const tp, + TC_STRUCT * const tc, + TC_STRUCT * const nexttc); + +int tpUpdateMovementStatus(TP_STRUCT * const tp, + TC_STRUCT const * const tc, + TC_STRUCT const * const nexttc); + +void tpHandleEmptyQueue(TP_STRUCT * const tp); + +void tpSetRotaryUnlock(IndexRotaryAxis axis, int unlock); + +int tpGetRotaryIsUnlocked(IndexRotaryAxis axis); + +int tpCompleteSegment(TP_STRUCT * const tp, + TC_STRUCT * const tc); + +tp_err_t tpHandleAbort(TP_STRUCT * const tp, + TC_STRUCT * const tc, + TC_STRUCT * const nexttc); + +tp_err_t tpCheckAtSpeed(TP_STRUCT * const tp, TC_STRUCT * const tc); + +tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc); + +void tpSyncVelocityMode(TC_STRUCT * const tc, TC_STRUCT * const nexttc); + +void checkPositionSyncError(TP_STRUCT const *tp, TC_STRUCT const *tc); + +void clearPositionSyncErrors(); +void clearPosTrackingStatus(); + +double findSpindleDisplacement( + double new_pos, + spindle_origin_t origin + ); + +double findSpindleVelocity( + double spindle_velocity, + spindle_origin_t origin + ); + +bool spindleReversed(spindle_origin_t origin, double prev_pos, double current_pos); + +void cmdReverseSpindle(double reversal_scale); + +void reportTPAxisError(TP_STRUCT const *tp, unsigned failed_axes, const char *msg_prefix); + + +void tpSyncPositionMode(TP_STRUCT * const tp, + TC_STRUCT * const tc, + TC_STRUCT * const nexttc ); + +int tpDoParabolicBlending(TP_STRUCT * const tp, + TC_STRUCT * const tc, + TC_STRUCT * const nexttc); + +int tpUpdateInitialStatus(TP_STRUCT const * const tp); + +int tpUpdateCycle(TP_STRUCT * const tp, + TC_STRUCT * const tc, + TC_STRUCT const * const nexttc, + UpdateCycleMode cycle_mode); + +int tcSetSplitCycle(TC_STRUCT * const tc, + double split_time, + double v_f); + +int tpHandleRegularCycle(TP_STRUCT * const tp, + TC_STRUCT * const tc, + TC_STRUCT * const nexttc); + +int tpHandleSplitCycle(TP_STRUCT * const tp, + TC_STRUCT * const tc, + TC_STRUCT * const nexttc); + +int tpSteppingCheck(TP_STRUCT * const tp, + TC_STRUCT * const tc, + TC_STRUCT * const nexttc); + +int tpInitBlendArcFromAdjacent(TP_STRUCT const * const tp, + TC_STRUCT const * const prev_tc, + TC_STRUCT * const blend_tc, + double vel, + double ini_maxvel, + double acc, + tc_motion_type_t motion_type); + +int find_max_element(double arr[], int sz); + +#endif // TP_PRIV_H diff --git a/src/emc/tp/tp_types.h b/src/emc/tp/tp_types.h index bfc227b62f7..28fc6598524 100644 --- a/src/emc/tp/tp_types.h +++ b/src/emc/tp/tp_types.h @@ -37,7 +37,7 @@ /* "neighborhood" size (if two values differ by less than the epsilon, * then they are effectively equal.)*/ #define TP_ACCEL_EPSILON 1e-4 -#define TP_VEL_EPSILON 1e-8 +#define TP_VEL_EPSILON DOUBLE_FUZZ #define TP_POS_EPSILON 1e-12 #define TP_TIME_EPSILON 1e-12 #define TP_ANGLE_EPSILON 1e-6 @@ -47,76 +47,45 @@ #define TP_BIG_NUM 1e10 /** - * TP return codes. - * This enum is a catch-all for useful return statuses from TP - * internal functions. This may be replaced with a better system in - * the future. - */ -typedef enum { - TP_ERR_INVALID = -9, - TP_ERR_INPUT_TYPE = -8, - TP_ERR_TOLERANCE = -7, - TP_ERR_RADIUS_TOO_SMALL = -6, - TP_ERR_GEOM = -5, - TP_ERR_RANGE = -4, - TP_ERR_MISSING_OUTPUT = -3, - TP_ERR_MISSING_INPUT = -2, - TP_ERR_FAIL = -1, - TP_ERR_OK = 0, - TP_ERR_NO_ACTION, - TP_ERR_SLOWING, - TP_ERR_STOPPED, - TP_ERR_WAITING, - TP_ERR_ZERO_LENGTH, - TP_ERR_REVERSE_EMPTY, - TP_ERR_LAST -} tp_err_t; - -/** - * Persistent data for spindle status within tpRunCycle. + * Persistant data for spindle status within tpRunCycle. * This structure encapsulates some static variables to simplify refactoring of * synchronized motion code. */ typedef struct { - int spindle_num; - double offset; - double revs; - int waiting_for_index; - int waiting_for_atspeed; + spindle_origin_t origin; //!< initial position of spindle during synchronization (direction-aware) + + double trigger_revs; + int waiting_for_index; + int waiting_for_atspeed; } tp_spindle_t; /** * Trajectory planner state structure. - * Stores persistent data for the trajectory planner that should be accessible + * Stores persistant data for the trajectory planner that should be accessible * by outside functions. */ typedef struct { TC_QUEUE_STRUCT queue; tp_spindle_t spindle; //Spindle data - EmcPose currentPos; - EmcPose goalPos; + int tc_completed_id; /* ID of most recent completed segment, i.e. "-1" in the queue"*/ + + PmVector currentPos; + PmVector goalPos; + PmVector currentVel; int queueSize; double cycleTime; - double vMax; /* vel for subsequent moves */ - double ini_maxvel; /* max velocity allowed by machine - constraints (INI file) for - subsequent moves */ - double vLimit; /* absolute upper limit on all vels */ - - double aMax; /* max accel (unused) */ - //FIXME this shouldn't be a separate limit, - double aMaxCartesian; /* max cartesian acceleration by machine bounds */ - double aLimit; /* max accel (unused) */ + double vLimit; /* absolute upper limit on all linear vels */ + double vLimitAng; /* absolute upper limit on all angular vels */ - double wMax; /* rotational velocity max */ - double wDotMax; /* rotational acceleration max */ int nextId; int execId; + tc_unique_id_t nextUniqueId; struct state_tag_t execTag; /* state tag corresponding to running motion */ - int termCond; + int nextexecId; + tc_term_cond_t termCond; int done; int depth; /* number of total queued motions */ int activeDepth; /* number of motions blending */ @@ -127,27 +96,16 @@ typedef struct { double tolerance; /* for subsequent motions, stay within this distance of the programmed path during blends */ - int synchronized; // spindle sync required for this move - int velocity_mode; /* TRUE if spindle sync is in velocity mode, - FALSE if in position mode */ + tc_spindle_sync_t synchronized; // spindle sync required for this move double uu_per_rev; /* user units per spindle revolution */ syncdio_t syncdio; //record tpSetDout's here -} TP_STRUCT; - + double time_elapsed_sec; // Total elapsed TP run time in seconds + long long time_elapsed_ticks; // Total elapsed TP run time in cycles (ticks) + long long time_at_wait; // Time when TP started to wait for spindle -/** - * Describes blend modes used in the trajectory planner. - * @note these values are used as array indices, so make sure valid options - * start at 0 and increase by one. - */ -typedef enum { - NO_BLEND = -1, - PARABOLIC_BLEND, - TANGENT_SEGMENTS_BLEND, - ARC_BLEND -} tc_blend_type_t; +} TP_STRUCT; #endif /* TP_TYPES_H */ diff --git a/src/libnml/posemath/Submakefile b/src/libnml/posemath/Submakefile index cc1048c62cb..2565f732f1c 100644 --- a/src/libnml/posemath/Submakefile +++ b/src/libnml/posemath/Submakefile @@ -1,5 +1,5 @@ INCLUDES += libnml/posemath -POSEMATHSRCS := $(addprefix libnml/posemath/, _posemath.c posemath.cc gomath.c sincos.c) +POSEMATHSRCS := $(addprefix libnml/posemath/, posemath.c posemath_ext.cc gomath.c sincos.c pm_vector.c) $(call TOOBJSDEPS, $(POSEMATHSRCS)) : EXTRAFLAGS=-fPIC USERSRCS += $(POSEMATHSRCS) TARGETS += ../lib/libposemath.so ../lib/libposemath.so.0 diff --git a/src/libnml/posemath/gomath.c b/src/libnml/posemath/gomath.c index 93cb5249b1f..367158136d3 100644 --- a/src/libnml/posemath/gomath.c +++ b/src/libnml/posemath/gomath.c @@ -19,7 +19,6 @@ /* for debugging */ extern int printf(const char * fmt, ...); #include /* NULL */ -#include /* memset */ #include "rtapi_math.h" #include @@ -72,8 +71,8 @@ int go_sph_cart_convert(const go_sph * s, go_cart * v) { go_real sth, cth, sph, cph; - pm_sincos(s->theta, &sth, &cth); - pm_sincos(s->phi, &sph, &cph); + sincos(s->theta, &sth, &cth); + sincos(s->phi, &sph, &cph); v->x = s->r * cth * sph; v->y = s->r * sth * sph; @@ -86,7 +85,7 @@ int go_sph_cyl_convert(const go_sph * s, go_cyl * c) { go_real sph, cph; - pm_sincos(s->phi, &sph, &cph); + sincos(s->phi, &sph, &cph); c->theta = s->theta; c->r = s->r * sph; @@ -139,7 +138,7 @@ int go_rvec_quat_convert(const go_rvec * r, go_quat * q) (void) go_cart_mag(&vec, &mag); - pm_sincos(0.5 * mag, &sh, &(q->s)); + sincos(0.5 * mag, &sh, &(q->s)); if (q->s >= 0) { q->x = uvec.x * sh; @@ -176,7 +175,7 @@ int go_rvec_mat_convert(const go_rvec * r, go_mat * m) (void) go_cart_mag(&vec, &mag); - pm_sincos(mag, &s, &c); + sincos(mag, &s, &c); omc = 1 - c; m->x.x = c + go_sq(uvec.x) * omc; @@ -330,7 +329,7 @@ int go_mat_rvec_convert(const go_mat * m, go_rvec * r) 3) else if e2 is largest then if c21 < 0 then take the negative for e1 if c32 < 0 then take the negative for e3 - 4) else if e3 is larger then + 4) else if e3 is larget then if c31 < 0 then take the negative for e1 if c32 < 0 then take the negative for e2 @@ -1766,8 +1765,6 @@ int go_cart_cart_pose(const go_cart * v1, const go_cart * v2, go_real eigenval; int retval; - memset(&Nspace,0,sizeof(Nspace)); - Sxx = Sxy = Sxz = 0.0; Syx = Syy = Syz = 0.0; Szx = Szy = Szz = 0.0; @@ -2577,7 +2574,7 @@ int go_poGO_RESULT_plane_distance(const go_cart * point, const go_plane * plane, int go_plane_evaluate(const go_plane * plane, go_real u, go_real v, go_cart * point) { - go_cart v1, v2; /* orthogonal vectors in plane */ + go_cart v1, v2; /* othogonal vectors in plane */ go_cart p; /* point in plane closest to origin */ if (GO_RESULT_OK != go_cart_normal(&plane->normal, &v1)) return GO_RESULT_ERROR; @@ -3632,8 +3629,8 @@ int go_dh_pose_convert(const go_dh * dh, go_pose * p) go_real sth, cth; /* sin, cos theta[i] */ go_real sal, cal; /* sin, cos alpha[i-1] */ - pm_sincos(dh->theta, &sth, &cth); - pm_sincos(dh->alpha, &sal, &cal); + sincos(dh->theta, &sth, &cth); + sincos(dh->alpha, &sal, &cal); h.rot.x.x = cth, h.rot.y.x = -sth, h.rot.z.x = 0.0; h.rot.x.y = sth*cal, h.rot.y.y = cth*cal, h.rot.z.y = -sal; diff --git a/src/libnml/posemath/gomath.h b/src/libnml/posemath/gomath.h index 8b1fa34496e..1aa77a2b626 100644 --- a/src/libnml/posemath/gomath.h +++ b/src/libnml/posemath/gomath.h @@ -293,7 +293,7 @@ extern go_flag go_plane_plane_compare(const go_plane * plane1, const go_plane * extern int go_point_plane_distance(const go_cart * point, const go_plane * plane, go_real * distance); /*! Fills in \a point with the point located distances \a u and \a v along - some orthogonal planar coordinate system in \a plane */ + some othogonal planar coordinate system in \a plane */ extern int go_plane_evaluate(const go_plane * plane, go_real u, go_real v, go_cart * point); /*! Fills in \a point with the intersection point of @@ -687,7 +687,7 @@ extern int go_mat6_vec6_mult(const go_real a[6][6], /* Denavit-Hartenberg to pose conversions */ /* - The link frame is assumed to be + The link frams is assumed to be | i-1 | T diff --git a/src/libnml/posemath/meson.build b/src/libnml/posemath/meson.build index 20062692f1d..f6fef791e7b 100644 --- a/src/libnml/posemath/meson.build +++ b/src/libnml/posemath/meson.build @@ -1,10 +1,11 @@ posemath_cpp_srcs = files([ - 'posemath.cc', + 'posemath_ext.cc', ]) posemath_srcs = files([ - '_posemath.c', + 'posemath.c', 'sincos.c', 'gomath.c', + 'pm_vector.c', ]) posemath_inc = include_directories('.') diff --git a/src/libnml/posemath/pm_vector.c b/src/libnml/posemath/pm_vector.c new file mode 100644 index 00000000000..e35597c8208 --- /dev/null +++ b/src/libnml/posemath/pm_vector.c @@ -0,0 +1,418 @@ +/** + * @file pm_vector.c + * + * API for PmVector (N-dimensional, compile-time sized vector object similar to PmCartesian) + * +* Derived from work by Fred Proctor & Will Shackleford (PmCartesian API) + * @author Robert W. Ellenberg + * + * @copyright Copyright 2019, Robert W. Ellenberg + * + * This source code is released for free distribution under the terms of the + * GNU General Public License (V2) as published by the Free Software Foundation. + */ + +#include "posemath.h" +#include "pm_vector.h" +#include "rtapi_math.h" +#include "string.h" + +// For internal use only +typedef enum { + X, + Y, + Z, + A, + B, + C, + U, + V, + W, +} PmVectorAxes; + +static const double PM_MIN_VECTOR_MAG = 1e-9; +const PmVector PmVector_zero = {}; + +int VecCopyFrom(PmVector * const v, const PmVector * const from) +{ + memcpy(v, from, sizeof(PmVector)); + return 0; +} + +int VecVecAdd(PmVector const * const v1, PmVector const * const v2, PmVector * const out) +{ + int i; + for (i = 0; i < PM_VECTOR_SIZE; ++i) { + out->ax[i] = v1->ax[i] + v2->ax[i]; + } + return 0; +} + +int VecVecAddEq(PmVector * const v, PmVector const * const v2) +{ + int i; + for (i = 0; i < PM_VECTOR_SIZE; ++i) { + v->ax[i] += v2->ax[i]; + } + return 0; +} + +int VecVecSub(PmVector const * const v1, PmVector const * const v2, PmVector * const out) +{ + if (!v1 || !v2 || !out) { + return -1; + } + int i; + for (i = 0; i < PM_VECTOR_SIZE; ++i) { + out->ax[i] = v1->ax[i] - v2->ax[i]; + } + return 0; +} + +int VecVecSubEq(PmVector * const v, PmVector const * const v2) +{ + if (!v || !v2) { + return -1; + } + int i; + for (i = 0; i < PM_VECTOR_SIZE; ++i) { + v->ax[i] -= v2->ax[i]; + } + return 0; +} + +int VecScalMult(PmVector const * const v1, double s, PmVector * const out) +{ + if (!v1 || !out) { + return -1; + } + int i; + for (i = 0; i < PM_VECTOR_SIZE; ++i) { + out->ax[i] = v1->ax[i]*s; + } + return 0; +} + +int VecScalMultEq(PmVector * const v, double s) +{ + if (!v) { + return -1; + } + int i; + for (i = 0; i < PM_VECTOR_SIZE; ++i) { + v->ax[i] *= s; + } + return 0; +} + +double VecVecDot(PmVector const * const v1, PmVector const * const v2) +{ + if (!v1 || !v2) { + return NAN; + } + double dot = 0.0; + for (int i = 0; i < PM_VECTOR_SIZE; ++i) { + dot += v1->ax[i] * v2->ax[i]; + } + return dot; +} + +double VecMagSq(PmVector const * const v1) +{ + return VecVecDot(v1,v1); +} + +double VecMag(PmVector const * const v1) +{ + double mag_sq = VecMagSq(v1); + return pmSqrt(mag_sq); +} + +double VecVecDisp(PmVector const * const v1, PmVector const * const v2) +{ + int i; + double d_sq = 0; + for (i = 0; i < PM_VECTOR_SIZE; ++i) { + d_sq += pmSq(v2->ax[i]-v1->ax[i]); + } + + return pmSqrt(d_sq); +} + +int VecUnit(PmVector const * const v1, PmVector * const out) +{ + *out = *v1; + return VecUnitEq(out); +} + +int VecUnitEq(PmVector * const v1) +{ + double mag = VecMag(v1); + if (fabs(mag) < PM_MIN_VECTOR_MAG) + return -1; + + VecScalMult(v1, 1.0 / mag, v1); + return 0; +} + + +int VecAbs(const PmVector * const v1, PmVector * const out) +{ + if (!v1 || !out) { + return -1; + } + + int i; + for (i = 0; i < PM_VECTOR_SIZE; ++i) { + out->ax[i] = fabs(v1->ax[i]); + } + return 0; +} + +int VecVecDirection( + PmVector const * const to, + PmVector const * const from, + PmVector * const u) +{ + VecVecSub(to, from, u); + return VecUnitEq(u); +} + +double VecMin(PmVector const * const v1) +{ + if (!v1) { + return NAN; + } + + int i; + double v_min = v1->ax[0]; + for (i = 0; i < PM_VECTOR_SIZE; ++i) { + v_min = fmin(v1->ax[i], v_min); + } + return v_min; +} + + +double VecMax(PmVector const * const v1) +{ + if (!v1) { + return NAN; + } + + int i; + double v_max = v1->ax[0]; + for (i = 1; i < PM_VECTOR_SIZE; ++i) { + v_max = fmax(v1->ax[i], v_max); + } + return v_max; +} + +double VecAbsMax(const PmVector * const v1) +{ + if (!v1) { + return NAN; + } + + int i; + double v_max = fabs(v1->ax[0]); + for (i = 1; i < PM_VECTOR_SIZE; ++i) { + v_max = fmax(fabs(v1->ax[i]), v_max); + } + return v_max; +} + +int CartToVec( + PmCartesian const * p_xyz, + PmCartesian const * p_abc, + PmCartesian const * p_uvw, + PmVector * const out) +{ + *out = PmVector_zero; + if (p_xyz) { + // Copy out elements into our array + out->ax[X] = p_xyz->x; + out->ax[Y] = p_xyz->y; + out->ax[Z] = p_xyz->z; + } + + if (p_abc) { + out->ax[A] = p_abc->x; + out->ax[B] = p_abc->y; + out->ax[C] = p_abc->z; + } + + if (p_uvw) { + out->ax[U] = p_uvw->x; + out->ax[V] = p_uvw->y; + out->ax[W] = p_uvw->z; + } + + return 0; +} + +int VecSetXYZ(const PmCartesian * const p_xyz, PmVector * const out) +{ + if (p_xyz) { + // Copy out elements into our array + out->ax[X] = p_xyz->x; + out->ax[Y] = p_xyz->y; + out->ax[Z] = p_xyz->z; + } + return 0; +} + +int VecSetABC(const PmCartesian * const p_abc, PmVector * const out) +{ + if (p_abc) { + // Copy out elements into our array + out->ax[X] = p_abc->x; + out->ax[Y] = p_abc->y; + out->ax[Z] = p_abc->z; + } + return 0; +} + +int VecToCart( + PmVector const * const vec, + PmCartesian * p_xyz, + PmCartesian * p_abc, + PmCartesian * p_uvw) +{ + if (!vec) { + return -1; + } + + if (p_xyz) { + p_xyz->x = vec->ax[X]; + p_xyz->y = vec->ax[Y]; + p_xyz->z = vec->ax[Z]; + } + + if (p_abc) { + p_abc->x = vec->ax[A]; + p_abc->y = vec->ax[B]; + p_abc->z = vec->ax[C]; + } + + if (p_uvw) { + p_uvw->x = vec->ax[U]; + p_uvw->y = vec->ax[V]; + p_uvw->z = vec->ax[W]; + } + + return 0; +} + +int VecVecOrthonormal( + PmVector const * const a, + PmVector const * const b, + PmVector * const u, + PmVector * const v) +{ + *u = *a; + + // Find component of b in direction of a + // NOTE assumes both a and b are already unit + double dot = VecVecDot(a, b); + //Build the component in the direction of a + VecScalMult(u, -dot, v); + VecVecAddEq(v, b); + + // Subtract component of a in direction of b + VecUnitEq(v); + + return 0; +} + +/** + * Performs "harmonic" addition elementwise on the vector and returns the square of the result. + * out(i) = arg_max(cos(theta_i) * a(i) + sin(theta_i) * b(i)) + * + * According to the harmonic addition theorem, this is equivalent to + * out(i) = arg_max(c(i) * cos(theta_i + d_i)), where d_i is a phase angle, and c(i)^2 = a(i)^2+b(i)^2 + * + * Therfore, the square of the harmonic addition out(i)^2 = a(i)^2 + b(i)^2. + * + * Returning the square term saves 3 square root operations (which can sometimes be avoided depending on the final calculation). + */ +int VecVecHarmonicAddSq(PmVector const * const u, + PmVector const * const v, + PmVector * const scales) +{ + if (!u || !v || !scales) { + return -1; + } + + int i; + for (i = 0; i < PM_VECTOR_SIZE; ++i) { + scales->ax[i] = pmSq(u->ax[i]) + pmSq(v->ax[i]); + } + return 0; +} + +/** + * Check if two UNIT vectors are parallel to within a numerical threshold. + * In this case, parallel means the magnitude of their difference is below the specified threshold. + */ +int VecVecUnitParallel( + PmVector const * const u, //!< first vector to compare + PmVector const * const v, //!< second vector to compare + double threshold_sq //!< square of the magnitude threshold + ) +{ + if (!u || !v) { + return -1; + } + PmVector u_diff; + VecVecSub(u, v, &u_diff); + return VecMagSq(&u_diff) < threshold_sq; +} + +/** + * Check if two UNIT vectors are anti-parallel to within a numerical threshold. + * In this case, parallel means the magnitude of their sum is below the specified threshold. + */ +int VecVecUnitAntiParallel( + PmVector const * const u, //!< first vector + PmVector const * const v, //!< second vector + double threshold_sq //!< square of the magnitude threshold + ) +{ + if (!u || !v) { + return -1; + } + PmVector u_sum; + VecVecAdd(u, v, &u_sum); + return VecMagSq(&u_sum) < threshold_sq; +} + +bool VecHasNAN(const PmVector * const v) +{ + int i; + bool has_nan = false; + for (i = 0; i < PM_VECTOR_SIZE; ++i) { + has_nan |= isnan(v->ax[i]); + } + return has_nan; +} + +double VecVLimit(PmVector const * const uVec, double v_target, double v_limit_linear, double v_limit_angular) +{ + PmCartesian xyz, abc, uvw; + VecToCart(uVec, &xyz, &abc, &uvw); + double m_xyz; + pmCartMag(&xyz, &m_xyz); + double m_abc; + pmCartMag(&abc, &m_abc); + double m_uvw; + pmCartMag(&uvw, &m_uvw); + + double u_linear = fmax(m_xyz, m_uvw); + if (u_linear > 1e-12) { + return fmin(v_target, v_limit_linear / u_linear); + } else if (m_abc > 1e-12) { + return fmin(v_target, v_limit_angular / m_abc); + } else { + return 0.0; + } +} diff --git a/src/libnml/posemath/pm_vector.h b/src/libnml/posemath/pm_vector.h new file mode 100644 index 00000000000..c4c2b551bb6 --- /dev/null +++ b/src/libnml/posemath/pm_vector.h @@ -0,0 +1,97 @@ +/******************************************************************** +* Description: pm_vector.h +* Vector structs and functions (generalizes PmCartesian) +* +* Derived from a work by Fred Proctor & Will Shackleford +* +* Author: Robert W. Ellenberg +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2015 All rights reserved. +********************************************************************/ +#ifndef PM_VECTOR_H +#define PM_VECTOR_H +#include "posemath.h" +#include "rtapi_bool.h" + +enum {PM_VECTOR_SIZE=9}; + +// WARNING this is an array under the hood! +typedef struct { + double ax[PM_VECTOR_SIZE]; +} PmVector; + +extern const PmVector PmVector_zero; + +int VecCopyFrom(PmVector * const v, PmVector const * const from); + +int VecVecAdd(PmVector const * const v1, PmVector const * const v2, PmVector * const out); +int VecVecAddEq(PmVector * const v, PmVector const * const v2); + +int VecVecSub(PmVector const * const v1, PmVector const * const v2, PmVector * const out); +int VecVecSubEq(PmVector * const v, PmVector const * const v2); +int VecScalMult(PmVector const * const v1, double s, PmVector * const out); +int VecScalMultEq(PmVector * const v, double s); + +double VecVecDot(PmVector const * const v1, PmVector const * const v2); +double VecMag(PmVector const * const v1); +double VecMagSq(PmVector const * const v1); +double VecVecDisp(PmVector const * const v1, PmVector const * const v2); + +int VecUnit(PmVector const * const v1, PmVector * const out); +int VecUnitEq(PmVector * const v1); +int VecAbs(PmVector const * const v1, PmVector * const out); + +int VecVecDirection( + PmVector const * const to, + PmVector const * const from, + PmVector * const u); + +double VecMin(PmVector const * const v1); +double VecMax(PmVector const * const v1); +double VecAbsMax(PmVector const * const v1); + +int CartToVec( + PmCartesian const * const p_xyz, + PmCartesian const * const p_abc, + PmCartesian const * const p_uvw, + PmVector * const out); + +int VecSetXYZ( + PmCartesian const * const p_xyz, + PmVector * const out); + +int VecSetABC( + PmCartesian const * const p_abc, + PmVector * const out); + +int VecToCart( + PmVector const * const vec, + PmCartesian * p_xyz, + PmCartesian * p_abc, + PmCartesian * p_uvw); + +int VecVecOrthonormal( + PmVector const * const a, + PmVector const * const b, + PmVector * const u, + PmVector * const v); + +int VecVecHarmonicAddSq( + PmVector const * const u, + PmVector const * const v, + PmVector * const scales); + +int VecVecUnitParallel(PmVector const * const u, + PmVector const * const v, + double threshold_sq); + +int VecVecUnitAntiParallel( + PmVector const * const u, + PmVector const * const v, + double threshold); + +bool VecHasNAN(PmVector const * const v); +double VecVLimit(PmVector const * const uVec, double v_target, double v_limit_linear, double v_limit_angular); +#endif diff --git a/src/libnml/posemath/posemath.c b/src/libnml/posemath/posemath.c new file mode 100644 index 00000000000..1a97d15e63f --- /dev/null +++ b/src/libnml/posemath/posemath.c @@ -0,0 +1,2135 @@ +/******************************************************************** +* Description: _posemath.c +* C definitions for pose math library data types and manipulation +* functions. +* +* Derived from a work by Fred Proctor & Will Shackleford +* +* Author: +* License: LGPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +* +* Last change: +********************************************************************/ + +#if defined(PM_PRINT_ERROR) && defined(rtai) +#undef PM_PRINT_ERROR +#endif + +#if defined(PM_DEBUG) && defined(rtai) +#undef PM_DEBUG +#endif + +#ifdef PM_PRINT_ERROR +#define PM_DEBUG /* have to have debug with printing */ +#include +#include +#endif +#include "posemath.h" + +#include "rtapi_math.h" +#include + +#include "sincos.h" + +/* global error number */ +int pmErrno = 0; + +#ifdef PM_PRINT_ERROR + +void pmPrintError(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + vfprintf(stderr, fmt, args); + va_end(args); +} + +/* error printing function */ +void pmPerror(const char *s) +{ + char *pmErrnoString; + + switch (pmErrno) { + case 0: + /* no error */ + return; + + case PM_ERR: + pmErrnoString = "unspecified error"; + break; + + case PM_IMPL_ERR: + pmErrnoString = "not implemented"; + break; + + case PM_NORM_ERR: + pmErrnoString = "expected normalized value"; + break; + + case PM_DIV_ERR: + pmErrnoString = "divide by zero"; + break; + + default: + pmErrnoString = "unassigned error"; + break; + } + + if (s != 0 && s[0] != 0) { + fprintf(stderr, "%s: %s\n", s, pmErrnoString); + } else { + fprintf(stderr, "%s\n", pmErrnoString); + } +} + +#endif /* PM_PRINT_ERROR */ + +/* fuzz checker */ +#define IS_FUZZ(a,fuzz) (fabs(a) < (fuzz) ? 1 : 0) + +/* Pose Math Basis Functions */ + +/* Scalar functions */ + +double pmSqrt(double x) +{ + if (x > 0.0) { + return sqrt(x); + } + + if (x > SQRT_FUZZ) { + return 0.0; + } +#ifdef PM_PRINT_ERROR + pmPrintError("sqrt of large negative number\n"); +#endif + + return 0.0; +} + +/* Translation rep conversion functions */ + +int pmCartSphConvert(PmCartesian const * const v, PmSpherical * const s) +{ + double _r; + + s->theta = atan2(v->y, v->x); + s->r = pmSqrt(pmSq(v->x) + pmSq(v->y) + pmSq(v->z)); + _r = pmSqrt(pmSq(v->x) + pmSq(v->y)); + s->phi = atan2(_r, v->z); + + return pmErrno = 0; +} + +int pmCartCylConvert(PmCartesian const * const v, PmCylindrical * const c) +{ + c->theta = atan2(v->y, v->x); + c->r = pmSqrt(pmSq(v->x) + pmSq(v->y)); + c->z = v->z; + + return pmErrno = 0; +} + +int pmSphCartConvert(PmSpherical const * const s, PmCartesian * const v) +{ + double _r; + + _r = s->r * sin(s->phi); + v->z = s->r * cos(s->phi); + v->x = _r * cos(s->theta); + v->y = _r * sin(s->theta); + + return pmErrno = 0; +} + +int pmSphCylConvert(PmSpherical const * const s, PmCylindrical * const c) +{ + c->theta = s->theta; + c->r = s->r * cos(s->phi); + c->z = s->r * sin(s->phi); + return pmErrno = 0; +} + +int pmCylCartConvert(PmCylindrical const * const c, PmCartesian * const v) +{ + v->x = c->r * cos(c->theta); + v->y = c->r * sin(c->theta); + v->z = c->z; + return pmErrno = 0; +} + +int pmCylSphConvert(PmCylindrical const * const c, PmSpherical * const s) +{ + s->theta = c->theta; + s->r = pmSqrt(pmSq(c->r) + pmSq(c->z)); + s->phi = atan2(c->z, c->r); + return pmErrno = 0; +} + +/* Rotation rep conversion functions */ + +int pmAxisAngleQuatConvert(PmAxis axis, double a, PmQuaternion * const q) +{ + double sh; + + a *= 0.5; + sincos(a, &sh, &(q->s)); + + switch (axis) { + case PM_X: + q->x = sh; + q->y = 0.0; + q->z = 0.0; + break; + + case PM_Y: + q->x = 0.0; + q->y = sh; + q->z = 0.0; + break; + + case PM_Z: + q->x = 0.0; + q->y = 0.0; + q->z = sh; + break; + + default: +#ifdef PM_PRINT_ERROR + pmPrintError("error: bad axis in pmAxisAngleQuatConvert\n"); +#endif + return -1; + } + + if (q->s < 0.0) { + q->s *= -1.0; + q->x *= -1.0; + q->y *= -1.0; + q->z *= -1.0; + } + + return 0; +} + +int pmRotQuatConvert(PmRotationVector const * const r, PmQuaternion * const q) +{ + double sh; + +#ifdef PM_DEBUG + /* make sure r is normalized */ + //FIXME breaks const promise + PmRotationVector r_raw = *r; + if (0 != pmRotNorm(&r_raw, r)) { +#ifdef PM_PRINT_ERROR + pmPrintError + ("error: pmRotQuatConvert rotation vector not normalized\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + if (pmClose(r->s, 0.0, QS_FUZZ)) { + q->s = 1.0; + q->x = q->y = q->z = 0.0; + + return pmErrno = 0; + } + + sincos(r->s / 2.0, &sh, &(q->s)); + + if (q->s >= 0.0) { + q->x = r->x * sh; + q->y = r->y * sh; + q->z = r->z * sh; + } else { + q->s *= -1; + q->x = -r->x * sh; + q->y = -r->y * sh; + q->z = -r->z * sh; + } + + return pmErrno = 0; +} + +int pmRotMatConvert(PmRotationVector const * const r, PmRotationMatrix * const m) +{ + double s, c, omc; + +#ifdef PM_DEBUG + if (!pmRotIsNorm(r)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad vector in pmRotMatConvert\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + sincos(r->s, &s, &c); + + /* from space book */ + m->x.x = c + pmSq(r->x) * (omc = 1 - c); /* omc = One Minus Cos */ + m->y.x = -r->z * s + r->x * r->y * omc; + m->z.x = r->y * s + r->x * r->z * omc; + + m->x.y = r->z * s + r->y * r->x * omc; + m->y.y = c + pmSq(r->y) * omc; + m->z.y = -r->x * s + r->y * r->z * omc; + + m->x.z = -r->y * s + r->z * r->x * omc; + m->y.z = r->x * s + r->z * r->y * omc; + m->z.z = c + pmSq(r->z) * omc; + + return pmErrno = 0; +} + +int pmRotZyzConvert(PmRotationVector const * const r, PmEulerZyz * const zyz) +{ +#ifdef PM_DEBUG +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmRotZyzConvert not implemented\n"); +#endif + return pmErrno = PM_IMPL_ERR; +#else + return PM_IMPL_ERR; +#endif +} + +int pmRotZyxConvert(PmRotationVector const * const r, PmEulerZyx * const zyx) +{ + PmRotationMatrix m; + int r1, r2; + + r1 = pmRotMatConvert(r, &m); + r2 = pmMatZyxConvert(&m, zyx); + + return pmErrno = r1 || r2 ? PM_NORM_ERR : 0; +} + +int pmRotRpyConvert(PmRotationVector const * const r, PmRpy * const rpy) +{ + PmQuaternion q; + int r1, r2; + + q.s = q.x = q.y = q.z = 0.0; + + r1 = pmRotQuatConvert(r, &q); + r2 = pmQuatRpyConvert(&q, rpy); + + return r1 || r2 ? pmErrno : 0; +} + +int pmQuatRotConvert(PmQuaternion const * const q, PmRotationVector * const r) +{ + double sh; + +#ifdef PM_DEBUG + if (!pmQuatIsNorm(q)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmQuatRotConvert\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + if (r == 0) { + return (pmErrno = PM_ERR); + } + + sh = pmSqrt(pmSq(q->x) + pmSq(q->y) + pmSq(q->z)); + + if (sh > QSIN_FUZZ) { + r->s = 2.0 * atan2(sh, q->s); + r->x = q->x / sh; + r->y = q->y / sh; + r->z = q->z / sh; + } else { + r->s = 0.0; + r->x = 0.0; + r->y = 0.0; + r->z = 0.0; + } + + return pmErrno = 0; +} + +int pmQuatMatConvert(PmQuaternion const * const q, PmRotationMatrix * const m) +{ +#ifdef PM_DEBUG + if (!pmQuatIsNorm(q)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmQuatMatConvert\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + /* from space book where e1=q->x e2=q->y e3=q->z e4=q->s */ + m->x.x = 1.0 - 2.0 * (pmSq(q->y) + pmSq(q->z)); + m->y.x = 2.0 * (q->x * q->y - q->z * q->s); + m->z.x = 2.0 * (q->z * q->x + q->y * q->s); + + m->x.y = 2.0 * (q->x * q->y + q->z * q->s); + m->y.y = 1.0 - 2.0 * (pmSq(q->z) + pmSq(q->x)); + m->z.y = 2.0 * (q->y * q->z - q->x * q->s); + + m->x.z = 2.0 * (q->z * q->x - q->y * q->s); + m->y.z = 2.0 * (q->y * q->z + q->x * q->s); + m->z.z = 1.0 - 2.0 * (pmSq(q->x) + pmSq(q->y)); + + return pmErrno = 0; +} + +int pmQuatZyzConvert(PmQuaternion const * const q, PmEulerZyz * const zyz) +{ + PmRotationMatrix m; + int r1, r2; + + /*! \todo FIXME-- need direct equations */ + r1 = pmQuatMatConvert(q, &m); + r2 = pmMatZyzConvert(&m, zyz); + + return pmErrno = r1 || r2 ? PM_NORM_ERR : 0; +} + +int pmQuatZyxConvert(PmQuaternion const * const q, PmEulerZyx * const zyx) +{ + PmRotationMatrix m; + int r1, r2; + + /*! \todo FIXME-- need direct equations */ + r1 = pmQuatMatConvert(q, &m); + r2 = pmMatZyxConvert(&m, zyx); + + return pmErrno = r1 || r2 ? PM_NORM_ERR : 0; +} + +int pmQuatRpyConvert(PmQuaternion const * const q, PmRpy * const rpy) +{ + PmRotationMatrix m; + int r1, r2; + + /*! \todo FIXME-- need direct equations */ + r1 = pmQuatMatConvert(q, &m); + r2 = pmMatRpyConvert(&m, rpy); + + return pmErrno = r1 || r2 ? PM_NORM_ERR : 0; +} + +int pmMatRotConvert(PmRotationMatrix const * const m, PmRotationVector * const r) +{ + PmQuaternion q; + int r1, r2; + + /*! \todo FIXME-- need direct equations */ + r1 = pmMatQuatConvert(m, &q); + r2 = pmQuatRotConvert(&q, r); + + return pmErrno = r1 || r2 ? PM_NORM_ERR : 0; +} + +int pmMatQuatConvert(PmRotationMatrix const * const m, PmQuaternion * const q) +{ + /* + from Stephe's "space" book e1 = (c32 - c23) / 4*e4 e2 = (c13 - c31) / + 4*e4 e3 = (c21 - c12) / 4*e4 e4 = sqrt(1 + c11 + c22 + c33) / 2 + + if e4 == 0 e1 = sqrt(1 + c11 - c33 - c22) / 2 e2 = sqrt(1 + c22 - c33 + - c11) / 2 e3 = sqrt(1 + c33 - c11 - c22) / 2 to determine whether to + take the positive or negative sqrt value since e4 == 0 indicates a + 180* rotation then (0 x y z) = (0 -x -y -z). Thus some generallities + can be used: 1) find which of e1, e2, or e3 has the largest magnitude + and leave it pos. 2) if e1 is largest then if c21 < 0 then take the + negative for e2 if c31 < 0 then take the negative for e3 3) else if e2 + is largest then if c21 < 0 then take the negative for e1 if c32 < 0 + then take the negative for e3 4) else if e3 is larget then if c31 < 0 + then take the negative for e1 if c32 < 0 then take the negative for e2 + + Note: c21 in the space book is m->x.y in this C code */ + + double a; + +#ifdef PM_DEBUG + if (!pmMatIsNorm(m)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad matrix in pmMatQuatConvert\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + q->s = 0.5 * pmSqrt(1.0 + m->x.x + m->y.y + m->z.z); + + if (fabs(q->s) > QS_FUZZ) { + q->x = (m->y.z - m->z.y) / (a = 4 * q->s); + q->y = (m->z.x - m->x.z) / a; + q->z = (m->x.y - m->y.x) / a; + } else { + q->s = 0; + q->x = pmSqrt(1.0 + m->x.x - m->y.y - m->z.z) / 2.0; + q->y = pmSqrt(1.0 + m->y.y - m->x.x - m->z.z) / 2.0; + q->z = pmSqrt(1.0 + m->z.z - m->y.y - m->x.x) / 2.0; + + if (q->x > q->y && q->x > q->z) { + if (m->x.y < 0.0) { + q->y *= -1; + } + if (m->x.z < 0.0) { + q->z *= -1; + } + } else if (q->y > q->z) { + if (m->x.y < 0.0) { + q->x *= -1; + } + if (m->y.z < 0.0) { + q->z *= -1; + } + } else { + if (m->x.z < 0.0) { + q->x *= -1; + } + if (m->y.z < 0.0) { + q->y *= -1; + } + } + } + + return pmQuatNorm(q, q); +} + +int pmMatZyzConvert(PmRotationMatrix const * const m, PmEulerZyz * const zyz) +{ + zyz->y = atan2(pmSqrt(pmSq(m->x.z) + pmSq(m->y.z)), m->z.z); + + if (fabs(zyz->y) < ZYZ_Y_FUZZ) { + zyz->z = 0.0; + zyz->y = 0.0; /* force Y to 0 */ + zyz->zp = atan2(-m->y.x, m->x.x); + } else if (fabs(zyz->y - PM_PI) < ZYZ_Y_FUZZ) { + zyz->z = 0.0; + zyz->y = PM_PI; /* force Y to 180 */ + zyz->zp = atan2(m->y.x, -m->x.x); + } else { + zyz->z = atan2(m->z.y, m->z.x); + zyz->zp = atan2(m->y.z, -m->x.z); + } + + return pmErrno = 0; +} + +int pmMatZyxConvert(PmRotationMatrix const * const m, PmEulerZyx * const zyx) +{ + zyx->y = atan2(-m->x.z, pmSqrt(pmSq(m->x.x) + pmSq(m->x.y))); + + if (fabs(zyx->y - (2 * PM_PI)) < ZYX_Y_FUZZ) { + zyx->z = 0.0; + zyx->y = (2 * PM_PI); /* force it */ + zyx->x = atan2(m->y.x, m->y.y); + } else if (fabs(zyx->y + (2 * PM_PI)) < ZYX_Y_FUZZ) { + zyx->z = 0.0; + zyx->y = -(2 * PM_PI); /* force it */ + zyx->x = -atan2(m->y.z, m->y.y); + } else { + zyx->z = atan2(m->x.y, m->x.x); + zyx->x = atan2(m->y.z, m->z.z); + } + + return pmErrno = 0; +} + +int pmMatRpyConvert(PmRotationMatrix const * const m, PmRpy * const rpy) +{ + rpy->p = atan2(-m->x.z, pmSqrt(pmSq(m->x.x) + pmSq(m->x.y))); + + if (fabs(rpy->p - (2 * PM_PI)) < RPY_P_FUZZ) { + rpy->r = atan2(m->y.x, m->y.y); + rpy->p = (2 * PM_PI); /* force it */ + rpy->y = 0.0; + } else if (fabs(rpy->p + (2 * PM_PI)) < RPY_P_FUZZ) { + rpy->r = -atan2(m->y.z, m->y.y); + rpy->p = -(2 * PM_PI); /* force it */ + rpy->y = 0.0; + } else { + rpy->r = atan2(m->y.z, m->z.z); + rpy->y = atan2(m->x.y, m->x.x); + } + + return pmErrno = 0; +} + +int pmZyzRotConvert(PmEulerZyz const * const zyz, PmRotationVector * const r) +{ +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmZyzRotConvert not implemented\n"); +#endif + return pmErrno = PM_IMPL_ERR; +} + +int pmZyzQuatConvert(PmEulerZyz const * const zyz, PmQuaternion * const q) +{ + PmRotationMatrix m; + int r1, r2; + + /*! \todo FIXME-- need direct equations */ + r1 = pmZyzMatConvert(zyz, &m); + r2 = pmMatQuatConvert(&m, q); + + return pmErrno = r1 || r2 ? PM_NORM_ERR : 0; +} + +int pmZyzMatConvert(PmEulerZyz const * const zyz, PmRotationMatrix * const m) +{ + double sa, sb, sg; + double ca, cb, cg; + + sa = sin(zyz->z); + sb = sin(zyz->y); + sg = sin(zyz->zp); + + ca = cos(zyz->z); + cb = cos(zyz->y); + cg = cos(zyz->zp); + + m->x.x = ca * cb * cg - sa * sg; + m->y.x = -ca * cb * sg - sa * cg; + m->z.x = ca * sb; + + m->x.y = sa * cb * cg + ca * sg; + m->y.y = -sa * cb * sg + ca * cg; + m->z.y = sa * sb; + + m->x.z = -sb * cg; + m->y.z = sb * sg; + m->z.z = cb; + + return pmErrno = 0; +} + +int pmZyzRpyConvert(PmEulerZyz const * const zyz, PmRpy * const rpy) +{ +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmZyzRpyConvert not implemented\n"); +#endif + return pmErrno = PM_IMPL_ERR; +} + +int pmZyxRotConvert(PmEulerZyx const * const zyx, PmRotationVector * const r) +{ + PmRotationMatrix m; + int r1, r2; + + /*! \todo FIXME-- need direct equations */ + r1 = pmZyxMatConvert(zyx, &m); + r2 = pmMatRotConvert(&m, r); + + return pmErrno = r1 || r2 ? PM_NORM_ERR : 0; +} + +int pmZyxQuatConvert(PmEulerZyx const * const zyx, PmQuaternion * const q) +{ + PmRotationMatrix m; + int r1, r2; + + /*! \todo FIXME-- need direct equations */ + r1 = pmZyxMatConvert(zyx, &m); + r2 = pmMatQuatConvert(&m, q); + + return pmErrno = r1 || r2 ? PM_NORM_ERR : 0; +} + +int pmZyxMatConvert(PmEulerZyx const * const zyx, PmRotationMatrix * const m) +{ + double sa, sb, sg; + double ca, cb, cg; + + sa = sin(zyx->z); + sb = sin(zyx->y); + sg = sin(zyx->x); + + ca = cos(zyx->z); + cb = cos(zyx->y); + cg = cos(zyx->x); + + m->x.x = ca * cb; + m->y.x = ca * sb * sg - sa * cg; + m->z.x = ca * sb * cg + sa * sg; + + m->x.y = sa * cb; + m->y.y = sa * sb * sg + ca * cg; + m->z.y = sa * sb * cg - ca * sg; + + m->x.z = -sb; + m->y.z = cb * sg; + m->z.z = cb * cg; + + return pmErrno = 0; +} + +int pmZyxZyzConvert(PmEulerZyx const * const zyx, PmEulerZyz * const zyz) +{ +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmZyxZyzConvert not implemented\n"); +#endif + return pmErrno = PM_IMPL_ERR; +} + +int pmZyxRpyConvert(PmEulerZyx const * const zyx, PmRpy * const rpy) +{ +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmZyxRpyConvert not implemented\n"); +#endif + return pmErrno = PM_IMPL_ERR; +} + +int pmRpyRotConvert(PmRpy const * const rpy, PmRotationVector * const r) +{ + PmQuaternion q; + int r1, r2; + + q.s = q.x = q.y = q.z = 0.0; + r->s = r->x = r->y = r->z = 0.0; + + r1 = pmRpyQuatConvert(rpy, &q); + r2 = pmQuatRotConvert(&q, r); + + return r1 || r2 ? pmErrno : 0; +} + +int pmRpyQuatConvert(PmRpy const * const rpy, PmQuaternion * const q) +{ + PmRotationMatrix m; + int r1, r2; + + /*! \todo FIXME-- need direct equations */ + r1 = pmRpyMatConvert(rpy, &m); + r2 = pmMatQuatConvert(&m, q); + + return pmErrno = r1 || r2 ? PM_NORM_ERR : 0; +} + +int pmRpyMatConvert(PmRpy const * const rpy, PmRotationMatrix * const m) +{ + double sa, sb, sg; + double ca, cb, cg; + + sa = sin(rpy->y); + sb = sin(rpy->p); + sg = sin(rpy->r); + + ca = cos(rpy->y); + cb = cos(rpy->p); + cg = cos(rpy->r); + + m->x.x = ca * cb; + m->y.x = ca * sb * sg - sa * cg; + m->z.x = ca * sb * cg + sa * sg; + + m->x.y = sa * cb; + m->y.y = sa * sb * sg + ca * cg; + m->z.y = sa * sb * cg - ca * sg; + + m->x.z = -sb; + m->y.z = cb * sg; + m->z.z = cb * cg; + + return pmErrno = 0; +} + +int pmRpyZyzConvert(PmRpy const * const rpy, PmEulerZyz * const zyz) +{ +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmRpyZyzConvert not implemented\n"); +#endif + return pmErrno = PM_IMPL_ERR; +} + +int pmRpyZyxConvert(PmRpy const * const rpy, PmEulerZyx * const zyx) +{ +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmRpyZyxConvert not implemented\n"); +#endif + return pmErrno = PM_IMPL_ERR; +} + +int pmPoseHomConvert(PmPose const * const p, PmHomogeneous * const h) +{ + int r1; + + h->tran = p->tran; + r1 = pmQuatMatConvert(&p->rot, &h->rot); + + return pmErrno = r1; +} + +int pmHomPoseConvert(PmHomogeneous const * const h, PmPose * const p) +{ + int r1; + + p->tran = h->tran; + r1 = pmMatQuatConvert(&h->rot, &p->rot); + + return pmErrno = r1; +} + +/* PmCartesian functions */ + +int pmCartCartCompare(PmCartesian const * const v1, PmCartesian const * const v2) +{ + return fabs(v1->x - v2->x) < V_FUZZ + && fabs(v1->y - v2->y) < V_FUZZ + && fabs(v1->z - v2->z) < V_FUZZ; +} + +int pmCartCartDot(PmCartesian const * const v1, PmCartesian const * const v2, double *d) +{ + *d = v1->x * v2->x + v1->y * v2->y + v1->z * v2->z; + + return pmErrno = 0; +} + +int pmCartCartMult(PmCartesian const * const v1, PmCartesian const * const v2, + PmCartesian * const out) +{ + out->x = v1->x * v2->x; + out->y = v1->y * v2->y; + out->z = v1->z * v2->z; + + return pmErrno = 0; +} + +int pmCartCartDiv(PmCartesian const * const v1, PmCartesian const * const v2, + PmCartesian * const out) +{ + out->x = v1->x / v2->x; + out->y = v1->y / v2->y; + out->z = v1->z / v2->z; + + return pmErrno = 0; +} + +int pmCartCartCross(PmCartesian const * const v1, PmCartesian const * const v2, + PmCartesian * const vout) +{ + if (vout == v1 || vout == v2) { + return pmErrno = PM_IMPL_ERR; + } + vout->x = v1->y * v2->z - v1->z * v2->y; + vout->y = v1->z * v2->x - v1->x * v2->z; + vout->z = v1->x * v2->y - v1->y * v2->x; + + return pmErrno = 0; +} + +int pmCartInfNorm(PmCartesian const * v, double * out) +{ + *out = fmax(fabs(v->x),fmax(fabs(v->y),fabs(v->z))); + return pmErrno = 0; +} + +int pmCartMag(PmCartesian const * const v, double *d) +{ + *d = pmSqrt(pmSq(v->x) + pmSq(v->y) + pmSq(v->z)); + + return pmErrno = 0; +} + +/** Find square of magnitude of a vector (useful for some calculations to save a sqrt).*/ +int pmCartMagSq(PmCartesian const * const v, double *d) +{ + *d = pmSq(v->x) + pmSq(v->y) + pmSq(v->z); + + return pmErrno = 0; +} + +int pmCartCartDisp(PmCartesian const * const v1, PmCartesian const * const v2, + double *d) +{ + *d = pmSqrt(pmSq(v2->x - v1->x) + pmSq(v2->y - v1->y) + pmSq(v2->z - v1->z)); + + return pmErrno = 0; +} + +int pmCartCartAdd(PmCartesian const * const v1, PmCartesian const * const v2, + PmCartesian * const vout) +{ + vout->x = v1->x + v2->x; + vout->y = v1->y + v2->y; + vout->z = v1->z + v2->z; + + return pmErrno = 0; +} + +int pmCartCartSub(PmCartesian const * const v1, PmCartesian const * const v2, + PmCartesian * const vout) +{ + vout->x = v1->x - v2->x; + vout->y = v1->y - v2->y; + vout->z = v1->z - v2->z; + + return pmErrno = 0; +} + +int pmCartCartElemDivNonZero( + PmCartesian const * const v, + PmCartesian const * const d, + PmCartesian * const scale) +{ + if (!v || !d || !scale ) { + return pmErrno = PM_ERR; + } + + scale->x = d->x ? fabs(v->x / d->x) : 0.0; + scale->y = d->y ? fabs(v->y / d->y) : 0.0; + scale->z = d->z ? fabs(v->z / d->z) : 0.0; + + return pmErrno = 0; +} + + +int pmCartScalAdd(PmCartesian const * v, double d, PmCartesian *vout) +{ + vout->x = v->x + d; + vout->y = v->y + d; + vout->z = v->z + d; + return pmErrno = 0; +} + +int pmCartScalSub(PmCartesian const * v, double d, PmCartesian *vout) +{ + vout->x = v->x - d; + vout->y = v->y - d; + vout->z = v->z - d; + return pmErrno = 0; +} + +int pmCartScalMult(PmCartesian const * const v1, double d, PmCartesian * const vout) +{ + if (v1 != vout) { + *vout = *v1; + } + return pmCartScalMultEq(vout, d); +} + +int pmCartScalDiv(PmCartesian const * const v1, double d, PmCartesian * const vout) +{ + if (v1 != vout) { + *vout = *v1; + } + return pmCartScalDivEq(vout, d); +} + +int pmCartNeg(PmCartesian const * const v1, PmCartesian * const vout) +{ + if (v1 != vout) { + *vout = *v1; + } + + return pmCartNegEq(vout); +} + +int pmCartNegEq(PmCartesian * const v1) +{ + v1->x = -v1->x; + v1->y = -v1->y; + v1->z = -v1->z; + + return pmErrno = 0; +} + +int pmCartCartDirection( + PmCartesian const * const to, + PmCartesian const * const from, + PmCartesian * const u) +{ + pmCartCartSub(to, from, u); + return pmCartUnitEq(u); +} + +/** + * Checks if two UNIT vectors are parallel to the given angle tolerance (in radians). + * @warning tol depends on the small angle approximation and will not be + * accurate for angles larger than about 10 deg. This function is meant for + * small tolerances! + */ +int pmCartCartParallel(PmCartesian const * const u1, + PmCartesian const * const u2, + double tol) +{ + double d_diff; + { + PmCartesian u_diff; + pmCartCartSub(u1, u2, &u_diff); + pmCartMagSq(&u_diff, &d_diff); + } + + return d_diff < tol; +} + +/** + * Checks if two UNIT vectors are anti-parallel to the given angle tolerance (in radians). + * @warning tol depends on the small angle approximation and will not be + * accurate for angles larger than about 10 deg. This function is meant for + * small tolerances! + */ +int pmCartCartAntiParallel(PmCartesian const * const u1, + PmCartesian const * const u2, + double tol) +{ + double d_sum; + { + PmCartesian u_sum; + pmCartCartAdd(u1, u2, &u_sum); + pmCartMagSq(&u_sum, &d_sum); + } + + return d_sum < tol; +} + +int pmCartInv(PmCartesian const * const v1, PmCartesian * const vout) +{ + if (v1 != vout) { + *vout = *v1; + } + + return pmCartInvEq(vout); +} + +int pmCartInvEq(PmCartesian * const v) +{ + double size_sq; + pmCartMagSq(v,&size_sq); + + if (size_sq == 0.0) { +#ifdef PM_PRINT_ERROR + pmPrintError("Zero vector in pmCartInv\n"); +#endif + return pmErrno = PM_NORM_ERR; + } + + v->x /= size_sq; + v->y /= size_sq; + v->z /= size_sq; + + return pmErrno = 0; +} + +// This used to be called pmCartNorm. + +int pmCartUnit(PmCartesian const * const v, PmCartesian * const vout) +{ + if (vout != v) { + *vout = *v; + } + return pmCartUnitEq(vout); +} + +int pmCartAbs(PmCartesian const * const v, PmCartesian * const vout) +{ + + vout->x = fabs(v->x); + vout->y = fabs(v->y); + vout->z = fabs(v->z); + + return pmErrno = 0; +} + +int pmCartAbsMax(PmCartesian const * const v, double * out) +{ + *out = fmax(fmax(fabs(v->x),fabs(v->y)),fabs(v->z)); + return pmErrno = 0; +} + +/* Compound assign operator equivalent functions. These are to prevent issues with passing the same variable as both input (const) and output */ + +int pmCartCartAddEq(PmCartesian * const v, PmCartesian const * const v_add) +{ + v->x += v_add->x; + v->y += v_add->y; + v->z += v_add->z; + + return pmErrno = 0; +} + +int pmCartCartSubEq(PmCartesian * const v, PmCartesian const * const v_sub) +{ + v->x -= v_sub->x; + v->y -= v_sub->y; + v->z -= v_sub->z; + + return pmErrno = 0; +} + +int pmCartScalAddEq(PmCartesian * v, double d) +{ + + v->x += d; + v->y += d; + v->z += d; + + return pmErrno = 0; +} + +int pmCartScalSubEq(PmCartesian * const v, double d) +{ + + v->x -= d; + v->y -= d; + v->z -= d; + + return pmErrno = 0; +} + +int pmCartScalMultEq(PmCartesian * const v, double d) +{ + + v->x *= d; + v->y *= d; + v->z *= d; + + return pmErrno = 0; +} + +int pmCartScalDivEq(PmCartesian * const v, double d) +{ + + if (d == 0.0) { +#ifdef PM_PRINT_ERROR + pmPrintError("Divide by 0 in pmCartScalDiv\n"); +#endif + + return pmErrno = PM_DIV_ERR; + } + + v->x /= d; + v->y /= d; + v->z /= d; + + return pmErrno = 0; +} + +int pmCartUnitEq(PmCartesian * const v) +{ + double size = pmSqrt(pmSq(v->x) + pmSq(v->y) + pmSq(v->z)); + + if (size == 0.0) { +#ifdef PM_PRINT_ERROR + pmPrintError("Zero vector in pmCartUnit\n"); +#endif + return pmErrno = PM_NORM_ERR; + } + + v->x /= size; + v->y /= size; + v->z /= size; + + return pmErrno = 0; +} + +/*! \todo This is if 0'd out so we can find all the pmCartNorm calls that should + be renamed pmCartUnit. + Later we'll put this back. */ +#if 0 + +int pmCartNorm(PmCartesian const * const v, PmCartesian * const vout) +{ + + vout->x = v->x; + vout->y = v->y; + vout->z = v->z; + + return pmErrno = 0; +} +#endif + +int pmCartIsNorm(PmCartesian const * const v) +{ + return pmSqrt(pmSq(v->x) + pmSq(v->y) + pmSq(v->z)) - 1.0 < UNIT_VEC_FUZZ ? 1 : 0; +} + +int pmCartCartProj(PmCartesian const * const v1, PmCartesian const * const v2, PmCartesian * const vout) +{ + int r1, r2; + int r3=1; + double d12; + double d22; + + r1 = pmCartCartDot(v1, v2, &d12); + r2 = pmCartCartDot(v2, v2, &d22); + if (!(r1 || r1)){ + r3 = pmCartScalMult(v2, d12/d22, vout); + } + + return pmErrno = r1 || r2 || r3 ? PM_NORM_ERR : 0; +} + +int pmCartPlaneProj(PmCartesian const * const v, PmCartesian const * const normal, PmCartesian * const vout) +{ + int r1, r2; + PmCartesian par; + + r1 = pmCartCartProj(v, normal, &par); + r2 = pmCartCartSub(v, &par, vout); + + return pmErrno = r1 || r2 ? PM_NORM_ERR : 0; +} + +/** + * Finds an in-plane orthonormal basis for two unit vectors based on a simplified Gram-Schmidt method. + */ +int pmCartCartOrthonormal( + PmCartesian const * const a_hat, //!< First unit vector (becomes first basis vector) + PmCartesian const * const b_hat, //!< Second unit vector + PmCartesian * const u, //!< [out] first orthonormal vector in direction of a_hat + PmCartesian * const v) //!< [out] second orthonormal vector +{ + *u = *a_hat; + + // Find component of a in direction of b + // NOTE assumes both a and b are already unit + double dot; + pmCartCartDot(a_hat, b_hat, &dot); + + //Build the component in the opposite direction of b + pmCartScalMult(a_hat, -dot, v); + + // Subtract component of a in direction of b (by adding opposite) + pmCartCartAddEq(v, b_hat); + + double dot_range = 1.0 - fabs(dot); + if ( dot_range > CART_FUZZ) { + // Only unitize vector if is has a nonzero magnitude, otherwise leave it zero + // This is correct-ish since colinear vectors don't span a plane, so we + // only need one vector as a basis. Leaving the second vector small or + // zero-lenth will be well-behaved for algorithms that anticipate this + // possibility. + pmCartUnitEq(v); + } + return pmErrno; +} + +/** + * Performs "harmonic" addition elementwise on the vector and returns the square of the result. + * out(i) = arg_max(cos(theta_i) * a(i) + sin(theta_i) * b(i)) + * + * According to the harmonic addition theorem, this is equivalent to + * out(i) = arg_max(c(i) * cos(theta_i + d_i)), where d_i is a phase angle, and c(i)^2 = a(i)^2+b(i)^2 + * + * Therfore, the square of the harmonic addition out(i)^2 = a(i)^2 + b(i)^2. + * + * Returning the square term saves 3 square root operations (which can sometimes be avoided depending on the final calculation). + */ +int pmCartCartHarmonicAddSq( + PmCartesian const * const a, + PmCartesian const * const b, + PmCartesian * const out) +{ + out->x = pmSq(a->x) + pmSq(b->x); + out->y = pmSq(a->y) + pmSq(b->y); + out->z = pmSq(a->z) + pmSq(b->z); + return 0; +} + + +/* angle-axis functions */ + +int pmQuatAxisAngleMult(PmQuaternion const * const q, PmAxis axis, double angle, + PmQuaternion * const pq) +{ + double sh, ch; + +#ifdef PM_DEBUG + if (!pmQuatIsNorm(q)) { +#ifdef PM_PRINT_ERROR + pmPrintError("error: non-unit quaternion in pmQuatAxisAngleMult\n"); +#endif + return -1; + } +#endif + + angle *= 0.5; + sincos(angle, &sh, &ch); + + switch (axis) { + case PM_X: + pq->s = ch * q->s - sh * q->x; + pq->x = ch * q->x + sh * q->s; + pq->y = ch * q->y + sh * q->z; + pq->z = ch * q->z - sh * q->y; + break; + + case PM_Y: + pq->s = ch * q->s - sh * q->y; + pq->x = ch * q->x - sh * q->z; + pq->y = ch * q->y + sh * q->s; + pq->z = ch * q->z + sh * q->x; + break; + + case PM_Z: + pq->s = ch * q->s - sh * q->z; + pq->x = ch * q->x + sh * q->y; + pq->y = ch * q->y - sh * q->x; + pq->z = ch * q->z + sh * q->s; + break; + + default: +#ifdef PM_PRINT_ERROR + pmPrintError("error: bad axis in pmQuatAxisAngleMult\n"); +#endif + return -1; + } + + if (pq->s < 0.0) { + pq->s *= -1.0; + pq->x *= -1.0; + pq->y *= -1.0; + pq->z *= -1.0; + } + + return 0; +} + +/* PmRotationVector functions */ + +int pmRotScalMult(PmRotationVector const * const r, double s, PmRotationVector * const rout) +{ + rout->s = r->s * s; + rout->x = r->x; + rout->y = r->y; + rout->z = r->z; + + return pmErrno = 0; +} + +int pmRotScalDiv(PmRotationVector const * const r, double s, PmRotationVector * const rout) +{ + if (s == 0.0) { +#ifdef PM_PRINT_ERROR + pmPrintError("Divide by zero in pmRotScalDiv\n"); +#endif + + rout->s = DBL_MAX; + rout->x = r->x; + rout->y = r->y; + rout->z = r->z; + + return pmErrno = PM_NORM_ERR; + } + + rout->s = r->s / s; + rout->x = r->x; + rout->y = r->y; + rout->z = r->z; + + return pmErrno = 0; +} + +int pmRotIsNorm(PmRotationVector const * const r) +{ + if (fabs(r->s) < RS_FUZZ || + fabs(pmSqrt(pmSq(r->x) + pmSq(r->y) + pmSq(r->z))) - 1.0 < UNIT_VEC_FUZZ) + { + return 1; + } + + return 0; +} + +int pmRotNorm(PmRotationVector const * const r, PmRotationVector * const rout) +{ + double size; + + size = pmSqrt(pmSq(r->x) + pmSq(r->y) + pmSq(r->z)); + + if (fabs(r->s) < RS_FUZZ) { + rout->s = 0.0; + rout->x = 0.0; + rout->y = 0.0; + rout->z = 0.0; + + return pmErrno = 0; + } + + if (size == 0.0) { +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmRotNorm size is zero\n"); +#endif + + rout->s = 0.0; + rout->x = 0.0; + rout->y = 0.0; + rout->z = 0.0; + + return pmErrno = PM_NORM_ERR; + } + + rout->s = r->s; + rout->x = r->x / size; + rout->y = r->y / size; + rout->z = r->z / size; + + return pmErrno = 0; +} + +/* PmRotationMatrix functions */ + +int pmMatNorm(PmRotationMatrix const * const m, PmRotationMatrix * const mout) +{ + /*! \todo FIXME */ + *mout = *m; + +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmMatNorm not implemented\n"); +#endif + return pmErrno = PM_IMPL_ERR; +} + +int pmMatIsNorm(PmRotationMatrix const * const m) +{ + PmCartesian u; + + pmCartCartCross(&m->x, &m->y, &u); + + return (pmCartIsNorm(&m->x) && pmCartIsNorm(&m->y) && pmCartIsNorm(&m->z) && pmCartCartCompare(&u, &m->z)); +} + +int pmMatInv(PmRotationMatrix const * const m, PmRotationMatrix * const mout) +{ + /* inverse of a rotation matrix is the transpose */ + + mout->x.x = m->x.x; + mout->x.y = m->y.x; + mout->x.z = m->z.x; + + mout->y.x = m->x.y; + mout->y.y = m->y.y; + mout->y.z = m->z.y; + + mout->z.x = m->x.z; + mout->z.y = m->y.z; + mout->z.z = m->z.z; + + return pmErrno = 0; +} + +int pmMatCartMult(PmRotationMatrix const * const m, PmCartesian const * const v, PmCartesian * const vout) +{ + vout->x = m->x.x * v->x + m->y.x * v->y + m->z.x * v->z; + vout->y = m->x.y * v->x + m->y.y * v->y + m->z.y * v->z; + vout->z = m->x.z * v->x + m->y.z * v->y + m->z.z * v->z; + + return pmErrno = 0; +} + +int pmMatMatMult(PmRotationMatrix const * const m1, PmRotationMatrix const * const m2, + PmRotationMatrix * const mout) +{ + mout->x.x = m1->x.x * m2->x.x + m1->y.x * m2->x.y + m1->z.x * m2->x.z; + mout->x.y = m1->x.y * m2->x.x + m1->y.y * m2->x.y + m1->z.y * m2->x.z; + mout->x.z = m1->x.z * m2->x.x + m1->y.z * m2->x.y + m1->z.z * m2->x.z; + + mout->y.x = m1->x.x * m2->y.x + m1->y.x * m2->y.y + m1->z.x * m2->y.z; + mout->y.y = m1->x.y * m2->y.x + m1->y.y * m2->y.y + m1->z.y * m2->y.z; + mout->y.z = m1->x.z * m2->y.x + m1->y.z * m2->y.y + m1->z.z * m2->y.z; + + mout->z.x = m1->x.x * m2->z.x + m1->y.x * m2->z.y + m1->z.x * m2->z.z; + mout->z.y = m1->x.y * m2->z.x + m1->y.y * m2->z.y + m1->z.y * m2->z.z; + mout->z.z = m1->x.z * m2->z.x + m1->y.z * m2->z.y + m1->z.z * m2->z.z; + + return pmErrno = 0; +} + +/* PmQuaternion functions */ + +int pmQuatQuatCompare(PmQuaternion const * const q1, PmQuaternion const * const q2) +{ +#ifdef PM_DEBUG + if (!pmQuatIsNorm(q1) || !pmQuatIsNorm(q2)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmQuatQuatCompare\n"); +#endif + } +#endif + + if (fabs(q1->s - q2->s) < Q_FUZZ && + fabs(q1->x - q2->x) < Q_FUZZ && + fabs(q1->y - q2->y) < Q_FUZZ && fabs(q1->z - q2->z) < Q_FUZZ) { + return 1; + } + + /* note (0, x, y, z) = (0, -x, -y, -z) */ + if (fabs(q1->s) >= QS_FUZZ || + fabs(q1->x + q2->x) >= Q_FUZZ || + fabs(q1->y + q2->y) >= Q_FUZZ || fabs(q1->z + q2->z) >= Q_FUZZ) { + return 0; + } + + return 1; +} + +int pmQuatMag(PmQuaternion const * const q, double *d) +{ + PmRotationVector r; + int r1; + + if (0 == d) { + return (pmErrno = PM_ERR); + } + + r1 = pmQuatRotConvert(q, &r); + *d = r.s; + + return pmErrno = r1; +} + +int pmQuatNorm(PmQuaternion const * const q1, PmQuaternion * const qout) +{ + double size = pmSqrt(pmSq(q1->s) + pmSq(q1->x) + pmSq(q1->y) + pmSq(q1->z)); + + if (size == 0.0) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmQuatNorm\n"); +#endif + qout->s = 1; + qout->x = 0; + qout->y = 0; + qout->z = 0; + + return pmErrno = PM_NORM_ERR; + } + + if (q1->s >= 0.0) { + qout->s = q1->s / size; + qout->x = q1->x / size; + qout->y = q1->y / size; + qout->z = q1->z / size; + + return pmErrno = 0; + } else { + qout->s = -q1->s / size; + qout->x = -q1->x / size; + qout->y = -q1->y / size; + qout->z = -q1->z / size; + + return pmErrno = 0; + } +} + +int pmQuatInv(PmQuaternion const * const q1, PmQuaternion * const qout) +{ + if (qout == 0) { + return pmErrno = PM_ERR; + } + + qout->s = q1->s; + qout->x = -q1->x; + qout->y = -q1->y; + qout->z = -q1->z; + +#ifdef PM_DEBUG + if (!pmQuatIsNorm(q1)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmQuatInv\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + return pmErrno = 0; +} + +int pmQuatIsNorm(PmQuaternion const * const q1) +{ + return (fabs(pmSq(q1->s) + pmSq(q1->x) + pmSq(q1->y) + pmSq(q1->z) - 1.0) < + UNIT_QUAT_FUZZ); +} + +int pmQuatScalMult(PmQuaternion const * const q, double s, PmQuaternion * const qout) +{ + /*! \todo FIXME-- need a native version; this goes through a rotation vector */ + PmRotationVector r; + int r1, r2, r3; + + r1 = pmQuatRotConvert(q, &r); + r2 = pmRotScalMult(&r, s, &r); + r3 = pmRotQuatConvert(&r, qout); + + return pmErrno = (r1 || r2 || r3) ? PM_NORM_ERR : 0; +} + +int pmQuatScalDiv(PmQuaternion const * const q, double s, PmQuaternion * const qout) +{ + /*! \todo FIXME-- need a native version; this goes through a rotation vector */ + PmRotationVector r; + int r1, r2, r3; + + r1 = pmQuatRotConvert(q, &r); + r2 = pmRotScalDiv(&r, s, &r); + r3 = pmRotQuatConvert(&r, qout); + + return pmErrno = (r1 || r2 || r3) ? PM_NORM_ERR : 0; +} + +int pmQuatQuatMult(PmQuaternion const * const q1, PmQuaternion const * const q2, PmQuaternion * const qout) +{ + if (qout == 0) { + return pmErrno = PM_ERR; + } + + qout->s = q1->s * q2->s - q1->x * q2->x - q1->y * q2->y - q1->z * q2->z; + + if (qout->s >= 0.0) { + qout->x = q1->s * q2->x + q1->x * q2->s + q1->y * q2->z - q1->z * q2->y; + qout->y = q1->s * q2->y - q1->x * q2->z + q1->y * q2->s + q1->z * q2->x; + qout->z = q1->s * q2->z + q1->x * q2->y - q1->y * q2->x + q1->z * q2->s; + } else { + qout->s *= -1; + qout->x = -q1->s * q2->x - q1->x * q2->s - q1->y * q2->z + q1->z * q2->y; + qout->y = -q1->s * q2->y + q1->x * q2->z - q1->y * q2->s - q1->z * q2->x; + qout->z = -q1->s * q2->z - q1->x * q2->y + q1->y * q2->x - q1->z * q2->s; + } + +#ifdef PM_DEBUG + if (!pmQuatIsNorm(q1) || !pmQuatIsNorm(q2)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmQuatQuatMult\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + return pmErrno = 0; +} + +int pmQuatCartMult(PmQuaternion const * const q1, PmCartesian const * const v2, PmCartesian * const vout) +{ + PmCartesian c; + + c.x = q1->y * v2->z - q1->z * v2->y; + c.y = q1->z * v2->x - q1->x * v2->z; + c.z = q1->x * v2->y - q1->y * v2->x; + + vout->x = v2->x + 2.0 * (q1->s * c.x + q1->y * c.z - q1->z * c.y); + vout->y = v2->y + 2.0 * (q1->s * c.y + q1->z * c.x - q1->x * c.z); + vout->z = v2->z + 2.0 * (q1->s * c.z + q1->x * c.y - q1->y * c.x); + +#ifdef PM_DEBUG + if (!pmQuatIsNorm(q1)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmQuatCartMult\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + return pmErrno = 0; +} + +/* PmPose functions*/ + +int pmPosePoseCompare(PmPose const * const p1, PmPose const * const p2) +{ +#ifdef PM_DEBUG + if (!pmQuatIsNorm(&p1->rot) || !pmQuatIsNorm(&p2->rot)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmPosePoseCompare\n"); +#endif + } +#endif + + return pmErrno = (pmQuatQuatCompare(&p1->rot, &p2->rot) && pmCartCartCompare(&p1->tran, &p2->tran)); +} + +int pmPoseInv(PmPose const * const p1, PmPose * const p2) +{ + int r1, r2; + +#ifdef PM_DEBUG + if (!pmQuatIsNorm(&p1->rot)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmPoseInv\n"); +#endif + } +#endif + + r1 = pmQuatInv(&p1->rot, &p2->rot); + r2 = pmQuatCartMult(&p2->rot, &p1->tran, &p2->tran); + + p2->tran.x *= -1.0; + p2->tran.y *= -1.0; + p2->tran.z *= -1.0; + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0; +} + +int pmPoseCartMult(PmPose const * const p1, PmCartesian const * const v2, PmCartesian * const vout) +{ + int r1, r2; + +#ifdef PM_DEBUG + if (!pmQuatIsNorm(&p1->rot)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmPoseCartMult\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + r1 = pmQuatCartMult(&p1->rot, v2, vout); + r2 = pmCartCartAdd(&p1->tran, vout, vout); + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0; +} + +int pmPosePoseMult(PmPose const * const p1, PmPose const * const p2, PmPose * const pout) +{ + int r1, r2, r3; + +#ifdef PM_DEBUG + if (!pmQuatIsNorm(&p1->rot) || !pmQuatIsNorm(&p2->rot)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad quaternion in pmPosePoseMult\n"); +#endif + return pmErrno = PM_NORM_ERR; + } +#endif + + r1 = pmQuatCartMult(&p1->rot, &p2->tran, &pout->tran); + r2 = pmCartCartAdd(&p1->tran, &pout->tran, &pout->tran); + r3 = pmQuatQuatMult(&p1->rot, &p2->rot, &pout->rot); + + return pmErrno = (r1 || r2 || r3) ? PM_NORM_ERR : 0; +} + +/* homogeneous transform functions */ + +int pmHomInv(PmHomogeneous const * const h1, PmHomogeneous * const h2) +{ + int r1, r2; + +#ifdef PM_DEBUG + if (!pmMatIsNorm(&h1->rot)) { +#ifdef PM_PRINT_ERROR + pmPrintError("Bad rotation matrix in pmHomInv\n"); +#endif + } +#endif + + r1 = pmMatInv(&h1->rot, &h2->rot); + r2 = pmMatCartMult(&h2->rot, &h1->tran, &h2->tran); + + h2->tran.x *= -1.0; + h2->tran.y *= -1.0; + h2->tran.z *= -1.0; + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0; +} + +/* line functions */ + +int pmLineInit(PmLine * const line, PmPose const * const start, PmPose const * const end) +{ + int r1 = 0, r2 = 0, r3 = 0, r4 = 0, r5 = 0; + double tmag = 0.0; + double rmag = 0.0; + PmQuaternion startQuatInverse; + + if (0 == line) { + return (pmErrno = PM_ERR); + } + + r3 = pmQuatInv(&start->rot, &startQuatInverse); + if (r3) { + return r3; + } + + r4 = pmQuatQuatMult(&startQuatInverse, &end->rot, &line->qVec); + if (r4) { + return r4; + } + + pmQuatMag(&line->qVec, &rmag); + if (rmag > Q_FUZZ) { + r5 = pmQuatScalMult(&line->qVec, 1 / rmag, &(line->qVec)); + if (r5) { + return r5; + } + } + + line->start = *start; + line->end = *end; + r1 = pmCartCartSub(&end->tran, &start->tran, &line->uVec); + if (r1) { + return r1; + } + + pmCartMag(&line->uVec, &tmag); + if (IS_FUZZ(tmag, CART_FUZZ)) { + line->uVec.x = 1.0; + line->uVec.y = 0.0; + line->uVec.z = 0.0; + } else { + r2 = pmCartUnit(&line->uVec, &line->uVec); + } + line->tmag = tmag; + line->rmag = rmag; + line->tmag_zero = (line->tmag <= CART_FUZZ); + line->rmag_zero = (line->rmag <= Q_FUZZ); + + /* return PM_NORM_ERR if uVec has been set to 1, 0, 0 */ + return pmErrno = (r1 || r2 || r3 || r4 || r5) ? PM_NORM_ERR : 0; +} + +int pmLinePoint(PmLine const * const line, double len, PmPose * const point) +{ + int r1 = 0, r2 = 0, r3 = 0, r4 = 0; + + if (line->tmag_zero) { + point->tran = line->end.tran; + } else { + /* return start + len * uVec */ + r1 = pmCartScalMult(&line->uVec, len, &point->tran); + r2 = pmCartCartAdd(&line->start.tran, &point->tran, &point->tran); + } + + if (line->rmag_zero) { + point->rot = line->end.rot; + } else { + if (line->tmag_zero) { + r3 = pmQuatScalMult(&line->qVec, len, &point->rot); + } else { + r3 = pmQuatScalMult(&line->qVec, len * line->rmag / line->tmag, + &point->rot); + } + r4 = pmQuatQuatMult(&line->start.rot, &point->rot, &point->rot); + } + + return pmErrno = (r1 || r2 || r3 || r4) ? PM_NORM_ERR : 0; +} + + +/* pure cartesian line functions */ + +int pmCartLineInit(PmCartLine * const line, PmCartesian const * const start, PmCartesian const * const end) +{ + int r1 = 0, r2 = 0; + + if (0 == line) { + return (pmErrno = PM_ERR); + } + + line->start = *start; + line->end = *end; + r1 = pmCartCartSub(end, start, &line->uVec); + if (r1) { + return r1; + } + + pmCartMag(&line->uVec, &line->tmag); + // NOTE: use the same criteria for "zero" length vectors as used by canon + double max_xyz=0; + pmCartInfNorm(&line->uVec, &max_xyz); + + if (IS_FUZZ(max_xyz, CART_FUZZ)) { + line->uVec.x = 1.0; + line->uVec.y = 0.0; + line->uVec.z = 0.0; + line->tmag_zero = 1; + } else { + r2 = pmCartUnitEq(&line->uVec); + line->tmag_zero = 0; + } + + /* return PM_NORM_ERR if uVec has been set to 1, 0, 0 */ + return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0; +} + +int pmCartLinePoint(PmCartLine const * const line, double len, PmCartesian * const point) +{ + int r1 = 0, r2 = 0; + + if (line->tmag_zero) { + *point = line->end; + } else { + /* return start + len * uVec */ + r1 = pmCartScalMult(&line->uVec, len, point); + r2 = pmCartCartAdd(&line->start, point, point); + } + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0; +} + + +int pmCartLineStretch(PmCartLine * const line, double new_len, int from_end) +{ + int r1 = 0, r2 = 0; + + if (!line || line->tmag_zero || new_len <= DOUBLE_FUZZ) { + return PM_ERR; + } + + if (from_end) { + // Store the new relative position from end in the start point + r1 = pmCartScalMult(&line->uVec, -new_len, &line->start); + // Offset the new start point by the current end point + r2 = pmCartCartAddEq(&line->start, &line->end); + } else { + // Store the new relative position from start in the end point: + r1 = pmCartScalMult(&line->uVec, new_len, &line->end); + // Offset the new end point by the current start point + r2 = pmCartCartAdd(&line->start, &line->end, &line->end); + } + line->tmag = new_len; + + return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0; +} + +/* circle functions */ + +/* + pmCircleInit() takes the defining parameters of a generalized circle + and sticks them in the structure. It also computes the radius and vectors + in the plane that are useful for other functions and that don't need + to be recomputed every time. + + Note that the end can be placed arbitrarily, resulting in a combination of + spiral and helical motion. There is an overconstraint between the start, + center, and normal vector: the center vector and start vector are assumed + to be in the plane defined by the normal vector. If this is not true, then + it will be made true by moving the center vector onto the plane. + */ +int pmCircleInit( + PmCircle * const circle, + PmCartesian const * const start, + PmCartesian const * const end, + PmCartesian const * const center, + PmCartesian const * const normal, + int turn, + double expected_angle_rad) +{ + double dot; + PmCartesian rEnd; + PmCartesian v; + double d; + int r1; + +#ifdef PM_DEBUG + if (0 == circle) { +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmCircleInit circle pointer is null\n"); +#endif + return pmErrno = PM_ERR; + } +#endif + + /* adjust center */ + pmCartCartSub(start, center, &v); + r1 = pmCartCartProj(&v, normal, &v); + if (PM_NORM_ERR == r1) { + /* bad normal vector-- abort */ +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmCircleInit normal vector is 0\n"); +#endif + return -1; + } + pmCartCartAdd(&v, center, &circle->center); + + /* normalize and redirect normal vector based on turns. If turn is less + than 0, point normal vector in other direction and make turn positive, + -1 -> 0, -2 -> 1, etc. */ + pmCartUnit(normal, &circle->normal); + if (turn < 0) { + turn = -1 - turn; + pmCartScalMult(&circle->normal, -1.0, &circle->normal); + } + + /* radius */ + pmCartCartDisp(start, &circle->center, &circle->radius); + + /* vector in plane of circle from center to start, magnitude radius */ + pmCartCartSub(start, &circle->center, &circle->rTan); + /* vector in plane of circle perpendicular to rTan, magnitude radius */ + pmCartCartCross(&circle->normal, &circle->rTan, &circle->rPerp); + + /* do rHelix, rEnd */ + pmCartCartSub(end, &circle->center, &circle->rHelix); + pmCartPlaneProj(&circle->rHelix, &circle->normal, &rEnd); + pmCartMag(&rEnd, &circle->spiral); + circle->spiral -= circle->radius; + pmCartCartSub(&circle->rHelix, &rEnd, &circle->rHelix); + pmCartUnit(&rEnd, &rEnd); + pmCartScalMult(&rEnd, circle->radius, &rEnd); + pmCartMag(&circle->rHelix, &circle->height); + + /* Patch for error spiral end same as spiral center */ + pmCartMag(&rEnd, &d); + if (d == 0.0) { + pmCartScalMult(&circle->normal, DOUBLE_FUZZ, &v); + pmCartCartAdd(&rEnd, &v, &rEnd); + } + /* end patch 03-mar-1999 Dirk Maij */ + + /* angle */ + pmCartCartDot(&circle->rTan, &rEnd, &dot); + dot = dot / (circle->radius * circle->radius); + if (dot > 1.0) { + circle->angle = 0.0; + } else if (dot < -1.0) { + circle->angle = PM_PI; + } else { + circle->angle = acos(dot); + } + /* now angle is in range 0..PI . Check if cross is antiparallel to + normal. If so, true angle is between PI..2PI. Need to subtract from + 2PI. */ + pmCartCartCross(&circle->rTan, &rEnd, &v); + pmCartCartDot(&v, &circle->normal, &d); + if (d < 0.0 && circle->angle >= CIRCLE_FUZZ ) { + circle->angle = PM_TAU - circle->angle; + } + + /* now add more angle for multi turns (also includes the extra turn for the case when the start / end points are coincident) */ + if (turn > 0) { + circle->angle += turn * PM_TAU; + } + + // KLUDGE explicit +0 is the default so this comparison is safe + if (expected_angle_rad != 0.) { + double missing_turns = round((fabs(expected_angle_rad) - circle->angle)/(PM_TAU)); + circle->angle += missing_turns * PM_TAU; + } + + // Ensure that circle angle is positive or this will fail + if (circle->angle < DOUBLE_FUZZ) { + return pmErrno = PM_DIV_ERR; + } + + //Default to invalid +/* if 0'ed out while not debugging*/ +#if 0 + printf("\n\n"); + printf("pmCircleInit:\n"); + printf(" \t start : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + start->x, start->y, start->z); + printf(" \t end : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + end->x, end->y, end->z); + printf(" \t center : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + center->x, center->y, center->z); + printf(" \t normal : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + normal->x, normal->y, normal->z); + printf(" \t rEnd : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + rEnd.x, rEnd.y, rEnd.z); + printf(" \t turn=%d\n", turn); + printf(" \t dot=%9.9f\n", dot); + printf(" \t d=%9.9f\n", d); + printf(" \t circle \t{angle=%9.9f, radius=%9.9f, spiral=%9.9f}\n", + circle->angle, circle->radius, circle->spiral); + printf(" \t circle->normal : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + circle->normal.x, circle->normal.y, circle->normal.z); + printf(" \t circle->center : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + circle->center.x, circle->center.y, circle->center.z); + printf(" \t circle->rTan : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + circle->rTan.x, circle->rTan.y, circle->rTan.z); + printf(" \t circle->rPerp : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + circle->rPerp.x, circle->rPerp.y, circle->rPerp.z); + printf(" \t circle->rHelix : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n", + circle->rHelix.x, circle->rHelix.y, circle->rHelix.z); + printf("\n\n"); +#endif + + return pmErrno = 0; +} + + +/* + pmCirclePoint() returns the point at the given angle along + the circle. If the circle is a helix or spiral or combination, the + point will include interpolation off the actual circle. + */ +int pmCirclePoint(PmCircle const * const circle, double angle, PmCartesian * const point) +{ + PmCartesian perp; + double scale; + +#ifdef PM_DEBUG + if (0 == circle || 0 == point) { +#ifdef PM_PRINT_ERROR + pmPrintError + ("error: pmCirclePoint circle or point pointer is null\n"); +#endif + return pmErrno = PM_ERR; + } +#endif + + /* compute components rel to center */ + pmCartScalMult(&circle->rTan, cos(angle), point); + pmCartScalMult(&circle->rPerp, sin(angle), &perp); + + /* add to get radius vector rel to center */ + pmCartCartAddEq(point, &perp); + + /* get scale for spiral, helix interpolation */ + if (circle->angle == 0.0) { +#ifdef PM_PRINT_ERROR + pmPrintError("error: pmCirclePoint angle is zero\n"); +#endif + return pmErrno = PM_DIV_ERR; + } + scale = angle / circle->angle; + + /* add scaled vector in radial dir for spiral */ + double spiral_scale = circle->spiral * scale / circle->radius; + pmCartScalMultEq(point, (1.0 + spiral_scale)); + + /* add scaled vector in helix dir */ + pmCartScalMult(&circle->rHelix, scale, &perp); + pmCartCartAddEq(point, &perp); + + /* add to center vector for final result */ + pmCartCartAddEq(point, &circle->center); + + return pmErrno = 0; +} + +int pmCircleCut(PmCircle * const circ, double cut_angle, int keep_end_pt) +{ + if (!circ) { + return pmErrno = PM_ERR; + } + double new_angle = keep_end_pt ? (circ->angle - cut_angle) : cut_angle; + if (new_angle < DOUBLE_FUZZ) { + return pmErrno = PM_NORM_ERR; + } + + if (keep_end_pt) { + // Need some extra juggling if we're creating a new start point + PmCartesian new_start; + pmCirclePoint(circ, cut_angle, &new_start); + PmCartesian tmp; + pmCartCartSub(&new_start, &circ->center, &tmp); + // Need to find new components perpendicular to circle normal, and to each other + pmCartCartCross(&circ->normal, &tmp, &circ->rPerp); + pmCartCartCross(&circ->rPerp, &circ->normal, &circ->rTan); + // Center is displaced if there is a helical component + pmCartCartSubEq(&tmp, &circ->rTan); + pmCartCartAddEq(&circ->center, &tmp); + // New initial radius in case there is non-zero spiral component + pmCartMag(&circ->rTan, &circ->radius); + } + // Reduce the spiral / helical components proportionally + double angle_ratio = (new_angle / circ->angle); + circ->spiral *= angle_ratio; + pmCartScalMultEq(&circ->rHelix, angle_ratio); + circ->height *= angle_ratio; + circ->angle = new_angle; + + return 0; +} + +int pmCircleStartPoint(PmCircle const * const circle, PmCartesian * const point) +{ + pmCartCartAdd(&circle->rTan, &circle->center, point); + + return pmErrno = 0; +} diff --git a/src/libnml/posemath/posemath.h b/src/libnml/posemath/posemath.h index 6a3d5a1ec72..5d4c11e6408 100644 --- a/src/libnml/posemath/posemath.h +++ b/src/libnml/posemath/posemath.h @@ -56,6 +56,7 @@ * operations should be coded and the overloaded C++ functions or operators * should be added. * +* NOTE: posemath C / C++ libraries are now split into c/h and cc/hh file pairs to make the bookeeping easier. * * Derived from a work by Fred Proctor & Will Shackleford * @@ -71,464 +72,8 @@ #ifndef POSEMATH_H #define POSEMATH_H -#ifdef __cplusplus - -#define USE_CONST -#define USE_CCONST -#define USE_REF - -#ifdef USE_CCONST -#define PM_CCONST const -#else -#define PM_CCONST -#endif - -#ifdef USE_CONST -#define PM_CONST const -#else -#define PM_CONST -#endif - -#ifdef USE_REF -#define PM_REF & -#else -#define PM_REF -#endif - -#if __cplusplus < 201103L -/* - * Not required in C++11 and beyond. It will generate a warning: - * "implicitly-declared 'constexpr ...' is deprecated [-Wdeprecated-copy]" - * If, somehow, somewhere, it is required, then you probably must implement - * both copy-constructor, destructor and operator= override (rule of three). - * Otherwise, if you do not have anything special in the members, then the - * compiler will do a good job at doing the right thing for you. - */ -#define INCLUDE_POSEMATH_COPY_CONSTRUCTORS -#endif - -/* forward declarations-- conversion ctors will need these */ - -/* translation types */ -struct PM_CARTESIAN; /* Cart */ -struct PM_SPHERICAL; /* Sph */ -struct PM_CYLINDRICAL; /* Cyl */ - -/* rotation types */ -struct PM_ROTATION_VECTOR; /* Rot */ -struct PM_ROTATION_MATRIX; /* Mat */ -struct PM_QUATERNION; /* Quat */ -struct PM_EULER_ZYZ; /* Zyz */ -struct PM_EULER_ZYX; /* Zyx */ -struct PM_RPY; /* Rpy */ - -/* pose types */ -struct PM_POSE; /* Pose */ -struct PM_HOMOGENEOUS; /* Hom */ - -/* PM_CARTESIAN */ - -struct PM_CARTESIAN { - /* ctors/dtors */ - PM_CARTESIAN() { - }; - PM_CARTESIAN(double _x, double _y, double _z); -#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS - PM_CARTESIAN(PM_CCONST PM_CARTESIAN & cart); // added 7-May-1997 - // by WPS -#endif - - PM_CARTESIAN(PM_CONST PM_CYLINDRICAL PM_REF c); /* conversion */ - PM_CARTESIAN(PM_CONST PM_SPHERICAL PM_REF s); /* conversion */ - - /* operators */ - double &operator[] (int n); /* this[n] */ - PM_CARTESIAN & operator += (const PM_CARTESIAN &o); - PM_CARTESIAN & operator -= (const PM_CARTESIAN &o); - - // Scalar operations - PM_CARTESIAN & operator *= (double o); - PM_CARTESIAN & operator /= (double o); - - /* data */ - double x, y, z; /* this.x, etc. */ -}; - -/* PM_SPHERICAL */ - -struct PM_SPHERICAL { - /* ctors/dtors */ - PM_SPHERICAL() { - }; -#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS - PM_SPHERICAL(PM_CCONST PM_SPHERICAL & s); -#endif - PM_SPHERICAL(double _theta, double _phi, double _r); - PM_SPHERICAL(PM_CONST PM_CYLINDRICAL PM_REF v); /* conversion */ - PM_SPHERICAL(PM_CONST PM_CARTESIAN PM_REF v); /* conversion */ - - /* operators */ - double &operator[] (int n); /* this[n] */ - - /* data */ - double theta, phi, r; -}; - -/* PM_CYLINDRICAL */ - -struct PM_CYLINDRICAL { - /* ctors/dtors */ - PM_CYLINDRICAL() { - }; -#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS - PM_CYLINDRICAL(PM_CCONST PM_CYLINDRICAL & c); -#endif - PM_CYLINDRICAL(double _theta, double _r, double _z); - PM_CYLINDRICAL(PM_CONST PM_CARTESIAN PM_REF v); /* conversion */ - PM_CYLINDRICAL(PM_CONST PM_SPHERICAL PM_REF v); /* conversion */ - - /* operators */ - double &operator[] (int n); /* this[n] */ - - /* data */ - double theta, r, z; -}; - -/* PM_ROTATION_VECTOR */ - -struct PM_ROTATION_VECTOR { - /* ctors/dtors */ - PM_ROTATION_VECTOR() { - }; -#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS - PM_ROTATION_VECTOR(PM_CCONST PM_ROTATION_VECTOR & r); -#endif - PM_ROTATION_VECTOR(double _r, double _x, double _y, double _z); - PM_ROTATION_VECTOR(PM_CONST PM_QUATERNION PM_REF q); /* conversion - */ - - /* operators */ - double &operator[] (int n); /* this[n] */ - - /* data */ - double s, x, y, z; -}; - -/* PM_ROTATION_MATRIX */ - -struct PM_ROTATION_MATRIX { - /* ctors/dtors */ - PM_ROTATION_MATRIX() { - }; -#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS - PM_ROTATION_MATRIX(PM_CCONST PM_ROTATION_MATRIX & mat); /* added - 7-May-1997 - by WPS */ -#endif - PM_ROTATION_MATRIX(double xx, double xy, double xz, - double yx, double yy, double yz, double zx, double zy, double zz); - PM_ROTATION_MATRIX(const PM_CARTESIAN& _x, const PM_CARTESIAN& _y, const PM_CARTESIAN& _z); - PM_ROTATION_MATRIX(PM_CONST PM_ROTATION_VECTOR PM_REF v); /* conversion - */ - PM_ROTATION_MATRIX(PM_CONST PM_QUATERNION PM_REF q); /* conversion - */ - PM_ROTATION_MATRIX(PM_CONST PM_EULER_ZYZ PM_REF zyz); /* conversion - */ - PM_ROTATION_MATRIX(PM_CONST PM_EULER_ZYX PM_REF zyx); /* conversion - */ - PM_ROTATION_MATRIX(PM_CONST PM_RPY PM_REF rpy); /* conversion */ - - /* operators */ - PM_CARTESIAN & operator[](int n); /* this[n] */ - - /* data */ - PM_CARTESIAN x, y, z; -}; - -/* PM_QUATERNION */ - -enum PM_AXIS { PM_X, PM_Y, PM_Z }; - -struct PM_QUATERNION { - /* ctors/dtors */ - PM_QUATERNION() { - }; -#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS - PM_QUATERNION(PM_CCONST PM_QUATERNION & quat); /* added 7-May-1997 - by WPS */ -#endif - PM_QUATERNION(double _s, double _x, double _y, double _z); - PM_QUATERNION(PM_CONST PM_ROTATION_VECTOR PM_REF v); /* conversion - */ - PM_QUATERNION(PM_CONST PM_ROTATION_MATRIX PM_REF m); /* conversion - */ - PM_QUATERNION(PM_CONST PM_EULER_ZYZ PM_REF zyz); /* conversion */ - PM_QUATERNION(PM_CONST PM_EULER_ZYX PM_REF zyx); /* conversion */ - PM_QUATERNION(PM_CONST PM_RPY PM_REF rpy); /* conversion */ - PM_QUATERNION(PM_AXIS axis, double angle); /* conversion */ - - /* operators */ - double &operator[] (int n); /* this[n] */ - - /* functions */ - void axisAngleMult(PM_AXIS axis, double angle); - - /* data */ - double s, x, y, z; /* this.s, etc. */ -}; - -/* PM_EULER_ZYZ */ - -struct PM_EULER_ZYZ { - /* ctors/dtors */ - PM_EULER_ZYZ() { - }; -#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS - PM_EULER_ZYZ(PM_CCONST PM_EULER_ZYZ & zyz); -#endif - PM_EULER_ZYZ(double _z, double _y, double _zp); - PM_EULER_ZYZ(PM_CONST PM_QUATERNION PM_REF q); /* conversion */ - PM_EULER_ZYZ(PM_CONST PM_ROTATION_MATRIX PM_REF m); /* conversion */ - - /* operators */ - double &operator[] (int n); - - /* data */ - double z, y, zp; -}; - -/* PM_EULER_ZYX */ - -struct PM_EULER_ZYX { - /* ctors/dtors */ - PM_EULER_ZYX() { - }; -#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS - PM_EULER_ZYX(PM_CCONST PM_EULER_ZYX & zyx); -#endif - PM_EULER_ZYX(double _z, double _y, double _x); - PM_EULER_ZYX(PM_CONST PM_QUATERNION PM_REF q); /* conversion */ - PM_EULER_ZYX(PM_CONST PM_ROTATION_MATRIX PM_REF m); /* conversion */ - - /* operators */ - double &operator[] (int n); - - /* data */ - double z, y, x; -}; - -/* PM_RPY */ - -struct PM_RPY { - /* ctors/dtors */ - PM_RPY() { - }; -#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS - PM_RPY(PM_CCONST PM_RPY PM_REF rpy); /* added 7-May-1997 by WPS */ -#endif - PM_RPY(double _r, double _p, double _y); - PM_RPY(PM_CONST PM_QUATERNION PM_REF q); /* conversion */ - PM_RPY(PM_CONST PM_ROTATION_MATRIX PM_REF m); /* conversion */ - - /* operators */ - double &operator[] (int n); - - /* data */ - double r, p, y; -}; - -/* PM_POSE */ - -struct PM_POSE { - /* ctors/dtors */ - PM_POSE() { - }; -#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS - PM_POSE(PM_CCONST PM_POSE & p); -#endif - PM_POSE(const PM_CARTESIAN& v, const PM_QUATERNION& q); - PM_POSE(double x, double y, double z, - double s, double sx, double sy, double sz); - PM_POSE(PM_CONST PM_HOMOGENEOUS PM_REF h); /* conversion */ - - /* operators */ - double &operator[] (int n); /* this[n] */ - - /* data */ - PM_CARTESIAN tran; - PM_QUATERNION rot; -}; - -/* PM_HOMOGENEOUS */ - -struct PM_HOMOGENEOUS { - /* ctors/dtors */ - PM_HOMOGENEOUS() { - }; -#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS - PM_HOMOGENEOUS(PM_CCONST PM_HOMOGENEOUS & h); -#endif - PM_HOMOGENEOUS(const PM_CARTESIAN& v, const PM_ROTATION_MATRIX& m); - PM_HOMOGENEOUS(PM_CONST PM_POSE PM_REF p); /* conversion */ - - /* operators */ - PM_CARTESIAN & operator[](int n); /* column vector */ - - /* data ( [ 0 0 0 1 ] element is manually returned by [] if needed ) */ - PM_CARTESIAN tran; - PM_ROTATION_MATRIX rot; -}; - -/* PM_LINE */ - -struct PM_LINE { - /* ctors/dtors */ - PM_LINE() { - }; -#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS - PM_LINE(PM_CCONST PM_LINE &); -#endif - - /* functions */ - int init(const PM_POSE& start, const PM_POSE& end); - int point(double len, PM_POSE * point); - - /* data */ - PM_POSE start; /* where motion was started */ - PM_POSE end; /* where motion is going */ - PM_CARTESIAN uVec; /* unit vector from start to end */ -}; - -/* PM_CIRCLE */ - -struct PM_CIRCLE { - /* ctors/dtors */ - PM_CIRCLE() - : radius(0.0), - angle(0.0), - spiral(0.0) - {}; -#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS - PM_CIRCLE(PM_CCONST PM_CIRCLE &); -#endif - - /* functions */ - int init(const PM_POSE& start, const PM_POSE& end, - const PM_CARTESIAN& center, const PM_CARTESIAN& normal, int turn); - int point(double angle, PM_POSE * point); - - /* data */ - PM_CARTESIAN center; - PM_CARTESIAN normal; - PM_CARTESIAN rTan; - PM_CARTESIAN rPerp; - PM_CARTESIAN rHelix; - double radius; - double angle; - double spiral; -}; - -/* overloaded external functions */ - -/* dot */ -extern double dot(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); - -/* cross */ -extern PM_CARTESIAN cross(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); - -#if 0 -/* norm */ -extern PM_CARTESIAN norm(PM_CARTESIAN v); -extern PM_QUATERNION norm(PM_QUATERNION q); -extern PM_ROTATION_VECTOR norm(PM_ROTATION_VECTOR r); -extern PM_ROTATION_MATRIX norm(PM_ROTATION_MATRIX m); -#endif - -/* unit */ -extern PM_CARTESIAN unit(const PM_CARTESIAN &v); -extern PM_QUATERNION unit(const PM_QUATERNION &q); -extern PM_ROTATION_VECTOR unit(const PM_ROTATION_VECTOR &r); -extern PM_ROTATION_MATRIX unit(const PM_ROTATION_MATRIX &m); - -/* isNorm */ -extern int isNorm(const PM_CARTESIAN &v); -extern int isNorm(const PM_QUATERNION &q); -extern int isNorm(const PM_ROTATION_VECTOR &r); -extern int isNorm(const PM_ROTATION_MATRIX &m); - -/* mag */ -extern double mag(const PM_CARTESIAN &v); - -/* disp */ -extern double disp(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); - -/* inv */ -extern PM_CARTESIAN inv(const PM_CARTESIAN &v); -extern PM_ROTATION_MATRIX inv(const PM_ROTATION_MATRIX &m); -extern PM_QUATERNION inv(const PM_QUATERNION &q); -extern PM_POSE inv(const PM_POSE &p); -extern PM_HOMOGENEOUS inv(const PM_HOMOGENEOUS &h); - -/* project */ -extern PM_CARTESIAN proj(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); - -/* overloaded arithmetic functions */ - -/* unary +, - for translation, rotation, pose */ -extern PM_CARTESIAN operator + (const PM_CARTESIAN &v); -extern PM_CARTESIAN operator - (const PM_CARTESIAN &v); -extern PM_QUATERNION operator + (const PM_QUATERNION &q); -extern PM_QUATERNION operator - (const PM_QUATERNION &q); -extern PM_POSE operator + (const PM_POSE &p); -extern PM_POSE operator - (const PM_POSE &p); - -/* compare operators */ -extern int operator == (const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); -extern int operator == (const PM_QUATERNION &q1, const PM_QUATERNION &q2); -extern int operator == (const PM_POSE &p1, const PM_POSE &p2); -extern int operator != (const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); -extern int operator != (const PM_QUATERNION &q1, const PM_QUATERNION &q2); -extern int operator != (const PM_POSE &p1, const PM_POSE &p2); - -/* translation +, -, scalar *, - */ - -/* v + v */ -extern PM_CARTESIAN operator + (PM_CARTESIAN v1, const PM_CARTESIAN &v2); -/* v - v */ -extern PM_CARTESIAN operator - (PM_CARTESIAN v1, const PM_CARTESIAN &v2); -/* v * s */ -extern PM_CARTESIAN operator *(PM_CARTESIAN v, double s); -/* s * v */ -extern PM_CARTESIAN operator *(double s, PM_CARTESIAN v); -/* v / s */ -extern PM_CARTESIAN operator / (const PM_CARTESIAN &v, double s); - -/* rotation * by scalar, translation, and rotation */ - -/* s * q */ -extern PM_QUATERNION operator *(double s, const PM_QUATERNION &q); -/* q * s */ -extern PM_QUATERNION operator *(const PM_QUATERNION &q, double s); -/* q / s */ -extern PM_QUATERNION operator / (const PM_QUATERNION &q, double s); -/* q * v */ -extern PM_CARTESIAN operator *(const PM_QUATERNION &q, const PM_CARTESIAN &v); -/* q * q */ -extern PM_QUATERNION operator *(const PM_QUATERNION &q1, const PM_QUATERNION &q2); -/* m * m */ -extern PM_ROTATION_MATRIX operator *(const PM_ROTATION_MATRIX &m1, - const PM_ROTATION_MATRIX &m2); - -/* pose operators */ - -/* q * p */ -extern PM_POSE operator *(const PM_QUATERNION &q, const PM_POSE &p); -/* p * p */ -extern PM_POSE operator *(const PM_POSE &p1, const PM_POSE &p2); -/* p * v */ -extern PM_CARTESIAN operator *(const PM_POSE &p, const PM_CARTESIAN &v); - -#endif /* __cplusplus */ +#include "posemath_fwd.h" +// #include "config.h" /* now comes the C stuff */ @@ -538,52 +83,49 @@ extern "C" { /* PmCartesian */ - typedef struct { + struct PmCartesian { double x, y, z; /* this.x, etc. */ - } PmCartesian; + }; + /* PmSpherical */ - typedef struct { + struct PmSpherical{ double theta, phi, r; - } PmSpherical; + }; /* PmCylindrical */ - typedef struct { + struct PmCylindrical { double theta, r, z; - } PmCylindrical; + }; /* PmAxis */ -#ifdef __cplusplus - typedef PM_AXIS PmAxis; -#else typedef enum { PM_X, PM_Y, PM_Z } PmAxis; -#endif /* PmRotationVector */ - typedef struct { + struct PmRotationVector { double s, x, y, z; - } PmRotationVector; + }; /* PmRotationMatrix */ - typedef struct { + struct PmRotationMatrix{ PmCartesian x, y, z; - } PmRotationMatrix; + }; /* PmQuaternion */ - typedef struct { + struct PmQuaternion { double s, x, y, z; /* this.s, etc. */ - } PmQuaternion; + }; /* PmEulerZyz */ @@ -608,11 +150,11 @@ extern "C" { /* PmPose */ - typedef struct { + struct PmPose { PmCartesian tran; PmQuaternion rot; - } PmPose; + }; /* PmCartLine */ typedef struct { @@ -625,11 +167,11 @@ extern "C" { /* Homogeneous transform PmHomogeneous */ - typedef struct { + struct PmHomogeneous{ PmCartesian tran; PmRotationMatrix rot; - } PmHomogeneous; + }; /* line structure */ @@ -654,7 +196,8 @@ extern "C" { PmCartesian rPerp; PmCartesian rHelix; double radius; - double angle; + double height; + double angle; double spiral; } PmCircle; @@ -670,7 +213,7 @@ extern "C" { #define PM_PI 3.14159265358979323846 #define PM_PI_2 1.57079632679489661923 #define PM_PI_4 0.78539816339744830962 -#define PM_2_PI 6.28318530717958647692 +#define PM_TAU 6.28318530717958647692 #ifdef PM_LOOSE_NAMESPACE @@ -695,11 +238,9 @@ extern "C" { /* quicky macros */ -//#define pmClose(a, b, eps) ((fabs((a) - (b)) < (eps)) ? 1 : 0) -//#define pmSq(x) ((x)*(x)) - -int pmClose(double a, double b, double eps); -__attribute__((always_inline)) static inline double pmSq(double x) { return x*x; } +#define pmClose(a, b, eps) ((fabs((a) - (b)) < (eps)) ? 1 : 0) +#define pmSq(x) ((x)*(x)) +#define pmCb(x) ((x)*(x)*(x)) #ifdef TO_DEG #undef TO_DEG @@ -713,71 +254,6 @@ __attribute__((always_inline)) static inline double pmSq(double x) { return x*x; /*! \todo FIXME-- fix these */ -/* DOUBLE_FUZZ is the smallest double, d, such that (1+d != 1) w/o FPC. - DOUBLECP_FUZZ is the same only with the Floating Point CoProcessor */ - -#define DOUBLE_FUZZ 2.2204460492503131e-16 -#define DOUBLECP_FUZZ 1.0842021724855044e-19 - - -/** - * FIXME sloppily defined constants here. - * These constants are quite large compared to the DOUBLE_FUZZ limitation. They - * seem like an ugly band-aid for floating point problems. - */ - -// FIXME setting this to be an order of magnitude smaller than canon's shortest -// allowed segment. This is still larger than TP's smallest position, so it may -// be silently causing trouble. -// andypugh 5/2/22 This seems to be interpreted to be in config units. -#define CART_FUZZ (1.0e-8) -/* how close a cartesian vector's magnitude must be for it to be considered - a zero vector */ - -#define Q_FUZZ (1.0e-06) -/* how close elements of a Q must be to be equal */ - -#define QS_FUZZ (1.0e-6) -/* how close q.s is to 0 to be 180 deg rotation */ - -#define RS_FUZZ (1.0e-6) -/* how close r.s is for a rotation vector to be considered 0 */ - -#define QSIN_FUZZ (1.0e-6) -/* how close sin(a/2) is to 0 to be zero rotation */ - -#define V_FUZZ (1.0e-8) -/* how close elements of a V must be to be equal */ - -#define SQRT_FUZZ (-1.0e-6) -/* how close to 0 before math_sqrt() is error */ - -#define UNIT_VEC_FUZZ (1.0e-6) -/* how close mag of vec must be to 1.00 */ - -#define UNIT_QUAT_FUZZ (1.0e-6) -/* how close mag of quat must be to 1.00 */ - -#define UNIT_SC_FUZZ (1.0e-6) -/* how close mag of sin, cos must be to 1.00 */ - -#define E_EPSILON (1.0e-6) -/* how close second ZYZ euler angle must be to 0/PI for degeneration */ - -#define SINGULAR_EPSILON (1.0e-6) -/* how close to zero the determinate of a matrix must be for singularity */ - -#define RPY_P_FUZZ (1.0e-6) -/* how close pitch is to zero for RPY to degenerate */ - -#define ZYZ_Y_FUZZ (1.0e-6) -/* how close Y is to zero for ZYZ Euler to degenerate */ - -#define ZYX_Y_FUZZ (1.0e-6) -/* how close Y is to zero for ZYX Euler to degenerate */ - -#define CIRCLE_FUZZ (1.0e-6) -/* Bug fix for the missing circles problem */ /* debug output printing */ extern void pmPrintError(const char *fmt, ...) __attribute__((format(printf,1,2))); @@ -785,11 +261,14 @@ __attribute__((always_inline)) static inline double pmSq(double x) { return x*x; /* global error number and errors */ extern int pmErrno; extern void pmPerror(const char *fmt); -#define PM_OK 0 /* no error */ -#define PM_ERR -1 /* unspecified error */ -#define PM_IMPL_ERR -2 /* not implemented */ -#define PM_NORM_ERR -3 /* arg should have been norm */ -#define PM_DIV_ERR -4 /* divide by zero error */ + + typedef enum { + PM_DIV_ERR = -4, /* divide by zero error */ + PM_NORM_ERR = -3, /* arg should have been norm */ + PM_IMPL_ERR = -2, /* not implemented */ + PM_ERR = -1, /* unspecified error */ + PM_OK = 0 + } PosemathErrCode; /* Scalar functions */ @@ -874,18 +353,29 @@ __attribute__((always_inline)) static inline double pmSq(double x) { return x*x; extern int pmCartCartDisp(PmCartesian const * const v1, PmCartesian const * const v2, double *d); extern int pmCartCartAdd(PmCartesian const * const, PmCartesian const * const, PmCartesian * const); extern int pmCartCartSub(PmCartesian const * const, PmCartesian const * const, PmCartesian * const); + extern int pmCartCartElemDivNonZero( + PmCartesian const * const v, + PmCartesian const * const divisor, + PmCartesian * const scale); + + extern int pmCartScalAdd(const PmCartesian * v, double d, PmCartesian * vout); + extern int pmCartScalSub(const PmCartesian * v, double d, PmCartesian * vout); extern int pmCartScalMult(PmCartesian const * const, double, PmCartesian * const); extern int pmCartScalDiv(PmCartesian const * const, double, PmCartesian * const); extern int pmCartNeg(PmCartesian const * const, PmCartesian * const); extern int pmCartUnit(PmCartesian const * const, PmCartesian * const); extern int pmCartAbs(PmCartesian const * const, PmCartesian * const); + extern int pmCartAbsMax(PmCartesian const * const v, double * out); // Equivalent of compound operators like +=, -=, etc. Basically, these functions work directly on the first PmCartesian extern int pmCartCartAddEq(PmCartesian * const, PmCartesian const * const); extern int pmCartCartSubEq(PmCartesian * const, PmCartesian const * const); + extern int pmCartScalAddEq(PmCartesian * v, double d); + extern int pmCartScalSubEq(PmCartesian * const v, double d); extern int pmCartScalMultEq(PmCartesian * const, double); extern int pmCartScalDivEq(PmCartesian * const, double); extern int pmCartUnitEq(PmCartesian * const); extern int pmCartNegEq(PmCartesian * const); + extern int pmCartCartDirection(PmCartesian const * const to, PmCartesian const * const, PmCartesian * const); /*! \todo Another #if 0 */ #if 0 extern int pmCartNorm(PmCartesian const * const v, PmCartesian * const vout); @@ -894,12 +384,29 @@ __attribute__((always_inline)) static inline double pmSq(double x) { return x*x; #define pmCartNorm(a,b,c,d,e) bad{a.b.c.d.e} #endif + extern int pmCartCartParallel(PmCartesian const * const u1, + PmCartesian const * const u2, + double tol); + + extern int pmCartCartAntiParallel(PmCartesian const * const u1, + PmCartesian const * const u2, + double tol); + extern int pmCartIsNorm(PmCartesian const * const v); extern int pmCartInv(PmCartesian const * const, PmCartesian * const); extern int pmCartInvEq(PmCartesian * const); extern int pmCartCartProj(PmCartesian const * const, PmCartesian const * const, PmCartesian * const); extern int pmCartPlaneProj(PmCartesian const * const v, PmCartesian const * const normal, PmCartesian * vout); + extern int pmCartCartOrthonormal( + PmCartesian const * const a, + PmCartesian const * const b, + PmCartesian * const u, + PmCartesian * const v); + extern int pmCartCartHarmonicAddSq( + PmCartesian const * const a, + PmCartesian const * const b, + PmCartesian * const out); /* rotation functions */ @@ -960,56 +467,20 @@ __attribute__((always_inline)) static inline double pmSq(double x) { return x*x; /* circle functions */ - extern int pmCircleInit(PmCircle * const circle, - PmCartesian const * const start, PmCartesian const * const end, - PmCartesian const * const center, PmCartesian const * const normal, int turn); + extern int pmCircleInit( + PmCircle * const circle, + PmCartesian const * const start, + PmCartesian const * const end, + PmCartesian const * const center, + PmCartesian const * const normal, + int turn, + double expected_angle_rad); extern int pmCirclePoint(PmCircle const * const circle, double angle, PmCartesian * const point); - extern int pmCircleStretch(PmCircle * const circ, double new_angle, int from_end); - -/* slicky macros for item-by-item copying between C and C++ structs */ + extern int pmCircleCut(PmCircle * const circ, double new_angle, int keep_end_pt); + extern int pmCircleStartPoint(PmCircle const * const circle, PmCartesian * const point); #ifdef __cplusplus } /* matches extern "C" for C++ */ - -template -void toCart(const A& src, B* dst) {(dst)->x = (src).x; (dst)->y = (src).y; (dst)->z = (src).z;} - -template -void toCyl(const A& src, B* dst) {(dst)->theta = (src).theta; (dst)->r = (src).r; (dst)->z = (src).z;} - -template -void toSph(const A& src, B* dst) {(dst)->theta = (src).theta; (dst)->phi = (src).phi; (dst)->r = (src).r;} - -template -void toQuat(const A& src, B* dst) {(dst)->s = (src).s; (dst)->x = (src).x; (dst)->y = (src).y; (dst)->z = (src).z;} - -template -void toRot(const A& src, B* dst) {(dst)->s = (src).s; (dst)->x = (src).x; (dst)->y = (src).y; (dst)->z = (src).z;} - -template -void toMat(const A& src, B* dst) {toCart((src).x, &((dst)->x)); toCart((src).y, &((dst)->y)); toCart((src).z, &((dst)->z));} - -template -void toEulerZyz(const A& src, B* dst) {(dst)->z = (src).z; (dst)->y = (src).y; (dst)->zp = (src).zp;} - -template -void toEulerZyx(const A& src, B* dst) {(dst)->z = (src).z; (dst)->y = (src).y; (dst)->x = (src).x;} - -template -void toRpy(const A& src, B* dst) {(dst)->r = (src).r; (dst)->p = (src).p; (dst)->y = (src).y;} - -template -void toPose(const A& src, B* dst) {toCart((src).tran, &((dst)->tran)); toQuat((src).rot, &((dst)->rot));} - -template -void toHom(const A& src, B* dst) {toCart((src).tran, &((dst)->tran)); toMat((src).rot, &((dst)->rot));} - -template -void toLine(const A& src, B* dst) {toPose((src).start, &((dst)->start)); toPose((src).end, &((dst)->end)); toCart((src).uVec, &((dst)->uVec));} - -template -void toCircle(const A& src, B* dst) {toCart((src).center, &((dst)->center)); toCart((src).normal, &((dst)->normal)); toCart((src).rTan, &((dst)->rTan)); toCart((src).rPerp, &((dst)->rPerp)); toCart((src).rHelix, &((dst)->rHelix)); (dst)->radius = (src).radius; (dst)->angle = (src).angle; (dst)->spiral = (src).spiral;} - #endif #endif /* #ifndef POSEMATH_H */ diff --git a/src/libnml/posemath/posemath_ext.cc b/src/libnml/posemath/posemath_ext.cc new file mode 100644 index 00000000000..ef9eecfa81d --- /dev/null +++ b/src/libnml/posemath/posemath_ext.cc @@ -0,0 +1,1415 @@ +/******************************************************************** +* Description: posemath_ext.cc +* C++ definitions for pose math library data types and manipulation +* functions. +* +* Derived from a work by Fred Proctor & Will Shackleford +* +* Author: +* License: LGPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +* +* Last change: +********************************************************************/ + +#include "posemath_ext.hh" + +#ifdef PM_PRINT_ERROR +#define PM_DEBUG // need debug with printing +#endif + +// place to reference arrays when bounds are exceeded +static double noElement = 0.0; +static PM_CARTESIAN *noCart = nullptr; + + +// PM_CARTESIAN class + +PM_CARTESIAN::PM_CARTESIAN(double _x, double _y, double _z) +{ + x = _x; + y = _y; + z = _z; +} + +PM_CARTESIAN::PM_CARTESIAN(PM_CONST PM_CYLINDRICAL PM_REF c) +{ + PmCylindrical cyl; + + toCyl(c, &cyl); + pmCylCartConvert(&cyl, (PmCartesian*)this); +} + +PM_CARTESIAN::PM_CARTESIAN(PM_CONST PM_SPHERICAL PM_REF s) +{ + PmSpherical sph; + + toSph(s, &sph); + pmSphCartConvert(&sph, (PmCartesian*)this); +} + +double &PM_CARTESIAN::operator [] (int n) { + switch (n) { + case 0: + return x; + case 1: + return y; + case 2: + return z; + default: + return noElement; // need to return a double & + } +} + +PM_CARTESIAN & PM_CARTESIAN::operator -= (const PM_CARTESIAN &o) { + x-=o.x; + y-=o.y; + z-=o.z; + return *this; +} +PM_CARTESIAN & PM_CARTESIAN::operator += (const PM_CARTESIAN &o) { + x+=o.x; + y+=o.y; + z+=o.z; + return *this; +} +/* +const PM_CARTESIAN PM_CARTESIAN::operator+(const PM_CARTESIAN &o) const { + PM_CARTESIAN result = *this; + result += o; + return result; +} + +const PM_CARTESIAN PM_CARTESIAN::operator-(const PM_CARTESIAN &o) const { + PM_CARTESIAN result = *this; + result -= o; + return result; +} +*/ + +PM_CARTESIAN & PM_CARTESIAN::operator *= (double o) +{ + x*=o; + y*=o; + z*=o; + return *this; + +} + +PM_CARTESIAN & PM_CARTESIAN::operator /= (double o) +{ + x/=o; + y/=o; + z/=o; + return *this; +} + +void PM_CARTESIAN::shift(int n) +{ + switch (n % 3) { + case -3: + case 0: + case 3: + return; + case -2: + case 1: + { + double xp = x; + x = z; + double yp = y; + y = xp; + z = yp; + return; + } + case -1: + case 2: + { + double zp = z; + z = x; + double yp = y; + y = zp; + x = yp; + return; + } + } +} + + +// PM_SPHERICAL + +PM_SPHERICAL::PM_SPHERICAL(double _theta, double _phi, double _r) +{ + theta = _theta; + phi = _phi; + r = _r; +} + +PM_SPHERICAL::PM_SPHERICAL(PM_CONST PM_CARTESIAN PM_REF v) +{ + PmSpherical sph; + + pmCartSphConvert(&v, &sph); + toSph(sph, this); +} + +PM_SPHERICAL::PM_SPHERICAL(PM_CONST PM_CYLINDRICAL PM_REF c) +{ + PmCylindrical cyl; + PmSpherical sph; + + toCyl(c, &cyl); + pmCylSphConvert(&cyl, &sph); + toSph(sph, this); +} + +double &PM_SPHERICAL::operator [] (int n) { + switch (n) { + case 0: + return theta; + case 1: + return phi; + case 2: + return r; + default: + return noElement; // need to return a double & + } +} + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_SPHERICAL::PM_SPHERICAL(PM_CCONST PM_SPHERICAL & s) +{ + theta = s.theta; + phi = s.phi; + r = s.r; +} +#endif +// PM_CYLINDRICAL + +PM_CYLINDRICAL::PM_CYLINDRICAL(double _theta, double _r, double _z) +{ + theta = _theta; + r = _r; + z = _z; +} + +PM_CYLINDRICAL::PM_CYLINDRICAL(PM_CONST PM_CARTESIAN PM_REF v) +{ + PmCylindrical cyl; + + pmCartCylConvert(&v, &cyl); + toCyl(cyl, this); +} + +PM_CYLINDRICAL::PM_CYLINDRICAL(PM_CONST PM_SPHERICAL PM_REF s) +{ + PmSpherical sph; + PmCylindrical cyl; + + toSph(s, &sph); + pmSphCylConvert(&sph, &cyl); + toCyl(cyl, this); +} + +double &PM_CYLINDRICAL::operator [] (int n) { + switch (n) { + case 0: + return theta; + case 1: + return r; + case 2: + return z; + default: + return noElement; // need to return a double & + } +} + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_CYLINDRICAL::PM_CYLINDRICAL(PM_CCONST PM_CYLINDRICAL & c) +{ + theta = c.theta; + r = c.r; + z = c.z; +} +#endif +// PM_ROTATION_VECTOR + +PM_ROTATION_VECTOR::PM_ROTATION_VECTOR(double _s, double _x, + double _y, double _z) +{ + PmRotationVector rv; + + rv.s = _s; + rv.x = _x; + rv.y = _y; + rv.z = _z; + + pmRotNorm(&rv, &rv); + toRot(rv, this); +} + +PM_ROTATION_VECTOR::PM_ROTATION_VECTOR(PM_CONST PM_QUATERNION PM_REF q) +{ + PmQuaternion quat; + PmRotationVector rv; + + toQuat(q, &quat); + pmQuatRotConvert(&quat, &rv); + toRot(rv, this); +} + +double &PM_ROTATION_VECTOR::operator [] (int n) { + switch (n) { + case 0: + return s; + case 1: + return x; + case 2: + return y; + case 3: + return z; + default: + return noElement; // need to return a double & + } +} + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_ROTATION_VECTOR::PM_ROTATION_VECTOR(PM_CCONST PM_ROTATION_VECTOR & r) +{ + s = r.s; + x = r.x; + y = r.y; + z = r.z; +} +#endif +// PM_ROTATION_MATRIX class + +// ctors/dtors + +PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(double xx, double xy, double xz, + double yx, double yy, double yz, double zx, double zy, double zz) +{ + x.x = xx; + x.y = xy; + x.z = xz; + + y.x = yx; + y.y = yy; + y.z = yz; + + z.x = zx; + z.y = zy; + z.z = zz; + + /*! \todo FIXME-- need a matrix orthonormalization function pmMatNorm() */ +} + +PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CARTESIAN _x, PM_CARTESIAN _y, + PM_CARTESIAN _z) +{ + x = _x; + y = _y; + z = _z; +} + +PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_ROTATION_VECTOR PM_REF v) +{ + PmRotationVector rv; + PmRotationMatrix mat; + + toRot(v, &rv); + pmRotMatConvert(&rv, &mat); + toMat(mat, this); +} + +PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_QUATERNION PM_REF q) +{ + PmQuaternion quat; + PmRotationMatrix mat; + + toQuat(q, &quat); + pmQuatMatConvert(&quat, &mat); + toMat(mat, this); +} + +PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_RPY PM_REF rpy) +{ + PmRpy _rpy; + PmRotationMatrix mat; + + toRpy(rpy, &_rpy); + pmRpyMatConvert(&_rpy, &mat); + toMat(mat, this); +} + +PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_EULER_ZYZ PM_REF zyz) +{ + PmEulerZyz _zyz; + PmRotationMatrix mat; + + toEulerZyz(zyz, &_zyz); + pmZyzMatConvert(&_zyz, &mat); + toMat(mat, this); +} + +PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_EULER_ZYX PM_REF zyx) +{ + PmEulerZyx _zyx; + PmRotationMatrix mat; + + toEulerZyx(zyx, &_zyx); + pmZyxMatConvert(&_zyx, &mat); + toMat(mat, this); +} + +// operators + +PM_CARTESIAN & PM_ROTATION_MATRIX::operator [](int n) { + switch (n) { + case 0: + return x; + case 1: + return y; + case 2: + return z; + default: + if (0 == noCart) { + noCart = new PM_CARTESIAN(0.0, 0.0, 0.0); + } + return (*noCart); // need to return a PM_CARTESIAN & + } +} + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CCONST PM_ROTATION_MATRIX & m) +{ + x = m.x; + y = m.y; + z = m.z; +} +#endif +// PM_QUATERNION class + +PM_QUATERNION::PM_QUATERNION(double _s, double _x, double _y, double _z) +{ + PmQuaternion quat; + + quat.s = _s; + quat.x = _x; + quat.y = _y; + quat.z = _z; + + pmQuatNorm(&quat, &quat); + + s = quat.s; + x = quat.x; + y = quat.y; + z = quat.z; +} + +PM_QUATERNION::PM_QUATERNION(PM_CONST PM_ROTATION_VECTOR PM_REF v) +{ + PmRotationVector rv; + PmQuaternion quat; + + toRot(v, &rv); + pmRotQuatConvert(&rv, &quat); + toQuat(quat, this); +} + +PM_QUATERNION::PM_QUATERNION(PM_CONST PM_ROTATION_MATRIX PM_REF m) +{ + PmRotationMatrix mat; + PmQuaternion quat; + + toMat(m, &mat); + pmMatQuatConvert(&mat, &quat); + toQuat(quat, this); +} + +PM_QUATERNION::PM_QUATERNION(PM_CONST PM_EULER_ZYZ PM_REF zyz) +{ + PmEulerZyz _zyz; + PmQuaternion quat; + + toEulerZyz(zyz, &_zyz); + pmZyzQuatConvert(&_zyz, &quat); + toQuat(quat, this); +} + +PM_QUATERNION::PM_QUATERNION(PM_CONST PM_EULER_ZYX PM_REF zyx) +{ + PmEulerZyx _zyx; + PmQuaternion quat; + + toEulerZyx(zyx, &_zyx); + pmZyxQuatConvert(&_zyx, &quat); + toQuat(quat, this); +} + +PM_QUATERNION::PM_QUATERNION(PM_CONST PM_RPY PM_REF rpy) +{ + PmRpy _rpy; + PmQuaternion quat; + + toRpy(rpy, &_rpy); + pmRpyQuatConvert(&_rpy, &quat); + toQuat(quat, this); +} + +PM_QUATERNION::PM_QUATERNION(PM_AXIS _axis, double _angle) +{ + PmQuaternion quat; + + pmAxisAngleQuatConvert((PmAxis) _axis, _angle, &quat); + toQuat(quat, this); +} + +void PM_QUATERNION::axisAngleMult(PM_AXIS _axis, double _angle) +{ + PmQuaternion quat; + + toQuat((*this), &quat); + pmQuatAxisAngleMult(&quat, (PmAxis) _axis, _angle, &quat); + toQuat(quat, this); +} + +double &PM_QUATERNION::operator [] (int n) { + switch (n) { + case 0: + return s; + case 1: + return x; + case 2: + return y; + case 3: + return z; + default: + return noElement; // need to return a double & + } +} +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_QUATERNION::PM_QUATERNION(PM_CCONST PM_QUATERNION & q) +{ + s = q.s; + x = q.x; + y = q.y; + z = q.z; +} +#endif + +// PM_EULER_ZYZ class + +PM_EULER_ZYZ::PM_EULER_ZYZ(double _z, double _y, double _zp) +{ + z = _z; + y = _y; + zp = _zp; +} + +PM_EULER_ZYZ::PM_EULER_ZYZ(PM_CONST PM_QUATERNION PM_REF q) +{ + PmQuaternion quat; + PmEulerZyz zyz; + + toQuat(q, &quat); + pmQuatZyzConvert(&quat, &zyz); + toEulerZyz(zyz, this); +} + +PM_EULER_ZYZ::PM_EULER_ZYZ(PM_CONST PM_ROTATION_MATRIX PM_REF m) +{ + PmRotationMatrix mat; + PmEulerZyz zyz; + + toMat(m, &mat); + pmMatZyzConvert(&mat, &zyz); + toEulerZyz(zyz, this); +} + +double &PM_EULER_ZYZ::operator [] (int n) { + switch (n) { + case 0: + return z; + case 1: + return y; + case 2: + return zp; + default: + return noElement; // need to return a double & + } +} + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_EULER_ZYZ::PM_EULER_ZYZ(PM_CCONST PM_EULER_ZYZ & zyz) +{ + z = zyz.z; + y = zyz.y; + zp = zyz.zp; +} +#endif + +// PM_EULER_ZYX class + +PM_EULER_ZYX::PM_EULER_ZYX(double _z, double _y, double _x) +{ + z = _z; + y = _y; + x = _x; +} + +PM_EULER_ZYX::PM_EULER_ZYX(PM_CONST PM_QUATERNION PM_REF q) +{ + pmQuatZyxConvert(&q, this); +} + +PM_EULER_ZYX::PM_EULER_ZYX(PM_CONST PM_ROTATION_MATRIX PM_REF m) +{ + PmRotationMatrix mat; + PmEulerZyx zyx; + + toMat(m, &mat); + pmMatZyxConvert(&mat, &zyx); + toEulerZyx(zyx, this); +} + +double &PM_EULER_ZYX::operator [] (int n) { + switch (n) { + case 0: + return z; + case 1: + return y; + case 2: + return x; + default: + return noElement; // need to return a double & + } +} + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_EULER_ZYX::PM_EULER_ZYX(PM_CCONST PM_EULER_ZYX & zyx) +{ + z = zyx.z; + y = zyx.y; + x = zyx.x; +} +#endif +// PM_RPY class + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_RPY::PM_RPY(PM_CCONST PM_RPY & rpy) +{ + r = rpy.r; + p = rpy.p; + y = rpy.y; +} +#endif + +PM_RPY::PM_RPY(double _r, double _p, double _y) +{ + r = _r; + p = _p; + y = _y; +} + +PM_RPY::PM_RPY(PM_CONST PM_QUATERNION PM_REF q) +{ + PmQuaternion quat; + PmRpy rpy; + + toQuat(q, &quat); + pmQuatRpyConvert(&quat, &rpy); + toRpy(rpy, this); +} + +PM_RPY::PM_RPY(PM_CONST PM_ROTATION_MATRIX PM_REF m) +{ + PmRotationMatrix mat; + PmRpy rpy; + + toMat(m, &mat); + pmMatRpyConvert(&mat, &rpy); + toRpy(rpy, this); +} + +double &PM_RPY::operator [] (int n) { + switch (n) { + case 0: + return r; + case 1: + return p; + case 2: + return y; + default: + return noElement; // need to return a double & + } +} + +// PM_POSE class + +PM_POSE::PM_POSE(PM_CARTESIAN v, PM_QUATERNION q) +{ + tran = v; + rot = q; +} + +PM_POSE::PM_POSE(double x, double y, double z, + double s, double sx, double sy, double sz) +{ + tran.x = x; + tran.y = y; + tran.z = z; + rot.s = s; + rot.x = sx; + rot.y = sy; + rot.z = sz; +} + +PM_POSE::PM_POSE(PM_CONST PM_HOMOGENEOUS PM_REF h) +{ + PmHomogeneous hom; + PmPose pose; + + toHom(h, &hom); + pmHomPoseConvert(&hom, &pose); + toPose(pose, this); +} + +double &PM_POSE::operator [] (int n) { + switch (n) { + case 0: + return tran.x; + case 1: + return tran.y; + case 2: + return tran.z; + case 3: + return rot.s; + case 4: + return rot.x; + case 5: + return rot.y; + case 6: + return rot.z; + default: + return noElement; // need to return a double & + } +} + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_POSE::PM_POSE(PM_CCONST PM_POSE & p) +{ + tran = p.tran; + rot = p.rot; +} +#endif +// PM_HOMOGENEOUS class + +PM_HOMOGENEOUS::PM_HOMOGENEOUS(PM_CARTESIAN v, PM_ROTATION_MATRIX m) +{ + tran = v; + rot = m; +} + +PM_HOMOGENEOUS::PM_HOMOGENEOUS(PM_CONST PM_POSE PM_REF p) +{ + PmPose pose; + PmHomogeneous hom; + + toPose(p, &pose); + pmPoseHomConvert(&pose, &hom); + toHom(hom, this); +} + +PM_CARTESIAN & PM_HOMOGENEOUS::operator [](int n) { + // if it is a rotation vector, stuff 0 as default bottom + // if it is a translation vector, stuff 1 as default bottom + + switch (n) { + case 0: + noElement = 0.0; + return rot.x; + case 1: + noElement = 0.0; + return rot.y; + case 2: + noElement = 0.0; + return rot.z; + case 3: + noElement = 1.0; + return tran; + default: + if (0 == noCart) { + noCart = new PM_CARTESIAN(0.0, 0.0, 0.0); + } + return (*noCart); // need to return a PM_CARTESIAN & + } +} + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_HOMOGENEOUS::PM_HOMOGENEOUS(PM_CCONST PM_HOMOGENEOUS & h) +{ + tran = h.tran; + rot = h.rot; +} +#endif + +// PM_LINE class + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_LINE::PM_LINE(PM_CCONST PM_LINE & l) +{ + start = l.start; + end = l.end; + uVec = l.uVec; +} +#endif + +int PM_LINE::init(PM_POSE start, PM_POSE end) +{ + PmLine _line; + PmPose _start, _end; + int retval; + + toPose(start, &_start); + toPose(end, &_end); + + retval = pmLineInit(&_line, &_start, &_end); + + toLine(_line, this); + + return retval; +} + +int PM_LINE::point(double len, PM_POSE * point) +{ + PmLine _line; + PmPose _point; + int retval; + + toLine(*this, &_line); + + retval = pmLinePoint(&_line, len, &_point); + + toPose(_point, point); + + return retval; +} + +// PM_CIRCLE class + +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS +PM_CIRCLE::PM_CIRCLE(PM_CCONST PM_CIRCLE & c) +{ + center = c.center; + normal = c.normal; + rTan = c.rTan; + rPerp = c.rPerp; + rHelix = c.rHelix; + radius = c.radius; + angle = c.angle; + spiral = c.spiral; +} +#endif + +int PM_CIRCLE::init(PM_POSE start, PM_POSE end, + PM_CARTESIAN _center, PM_CARTESIAN _normal, int turn, double expected_angle_rad) +{ + int retval; + retval = pmCircleInit(this, &start.tran, &end.tran, &_center, &_normal, turn, expected_angle_rad); + return retval; +} + +int PM_CIRCLE::point(double angle_, PM_POSE * point) +{ + return pmCirclePoint(this, angle_, &point->tran); +} + +// overloaded external functions + +// dot + +double dot(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2) +{ + double d; + PmCartesian _v1, _v2; + + toCart(v1, &_v1); + toCart(v2, &_v2); + + pmCartCartDot(&_v1, &_v2, &d); + + return d; +} + +// cross + +PM_CARTESIAN cross(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2) +{ + PM_CARTESIAN ret; + PmCartesian _v1, _v2, _v3; + + toCart(v1, &_v1); + toCart(v2, &_v2); + + pmCartCartCross(&_v1, &_v2, &_v3); + + toCart(_v3, &ret); + + return ret; +} + +//unit + +PM_CARTESIAN unit(const PM_CARTESIAN &v) +{ + PM_CARTESIAN vout; + PmCartesian _v; + + toCart(v, &_v); + + pmCartUnitEq(&_v); + + toCart(_v, &vout); + + return vout; +} + +/*! \todo Another #if 0 */ +#if 0 + +PM_CARTESIAN norm(PM_CARTESIAN v) +{ + PM_CARTESIAN vout; + PmCartesian _v; + + toCart(v, &_v); + + pmCartNorm(&_v, &_v); + + toCart(_v, &vout); + + return vout; +} + +PM_QUATERNION norm(PM_QUATERNION q) +{ + PM_QUATERNION qout; + PmQuaternion _q; + + toQuat(q, &_q); + pmQuatNorm(_q, &_q); + + toQuat(_q, &qout); + + return qout; +} + +PM_ROTATION_VECTOR norm(PM_ROTATION_VECTOR r) +{ + PM_ROTATION_VECTOR rout; + PmRotationVector _r; + + toRot(r, &_r); + + pmRotNorm(_r, &_r); + + toRot(_r, &rout); + + return rout; +} + +PM_ROTATION_MATRIX norm(PM_ROTATION_MATRIX m) +{ + PM_ROTATION_MATRIX mout; + PmRotationMatrix _m; + + toMat(m, &_m); + + pmMatNorm(_m, &_m); + + toMat(_m, &mout); + + return mout; +} +#endif + +// isNorm + +int isNorm(PM_CARTESIAN v) +{ + PmCartesian _v; + + toCart(v, &_v); + + return pmCartIsNorm(&_v); +} + +int isNorm(PM_QUATERNION q) +{ + PmQuaternion _q; + + toQuat(q, &_q); + + return pmQuatIsNorm(&_q); +} + +int isNorm(PM_ROTATION_VECTOR r) +{ + PmRotationVector _r; + + toRot(r, &_r); + + return pmRotIsNorm(&_r); +} + +int isNorm(PM_ROTATION_MATRIX m) +{ + PmRotationMatrix _m; + + toMat(m, &_m); + + return pmMatIsNorm(&_m); +} + +// mag + +double mag(const PM_CARTESIAN &v) +{ + double ret; + PmCartesian _v; + + toCart(v, &_v); + + pmCartMag(&_v, &ret); + + return ret; +} + +// disp + +double disp(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2) +{ + double ret; + PmCartesian _v1, _v2; + + toCart(v1, &_v1); + toCart(v2, &_v2); + + pmCartCartDisp(&_v1, &_v2, &ret); + + return ret; +} + +// inv + +PM_CARTESIAN inv(const PM_CARTESIAN &v) +{ + PM_CARTESIAN ret; + PmCartesian _v; + + toCart(v, &_v); + + pmCartInv(&_v, &_v); + + toCart(_v, &ret); + + return ret; +} + +PM_ROTATION_MATRIX inv(const PM_ROTATION_MATRIX &m) +{ + PM_ROTATION_MATRIX ret; + PmRotationMatrix _m; + + toMat(m, &_m); + + pmMatInv(&_m, &_m); + + toMat(_m, &ret); + + return ret; +} + +PM_QUATERNION inv(const PM_QUATERNION &q) +{ + PM_QUATERNION ret; + PmQuaternion _q; + + toQuat(q, &_q); + + pmQuatInv(&_q, &_q); + + toQuat(_q, &ret); + + return ret; +} + +PM_POSE inv(const PM_POSE &p) +{ + PM_POSE ret; + PmPose _p; + + toPose(p, &_p); + + pmPoseInv(&_p, &_p); + + toPose(_p, &ret); + + return ret; +} + +PM_HOMOGENEOUS inv(const PM_HOMOGENEOUS &h) +{ + PM_HOMOGENEOUS ret; + PmHomogeneous _h; + + toHom(h, &_h); + + pmHomInv(&_h, &_h); + + toHom(_h, &ret); + + return ret; +} + +// project + +PM_CARTESIAN proj(const PM_CARTESIAN &v1, PM_CARTESIAN &v2) +{ + PM_CARTESIAN ret; + PmCartesian _v1, _v2; + + toCart(v1, &_v1); + toCart(v2, &_v2); + + pmCartCartProj(&_v1, &_v2, &_v1); + + toCart(_v1, &ret); + + return ret; +} + +// overloaded arithmetic operators + +PM_CARTESIAN operator +(const PM_CARTESIAN &v) +{ + return v; +} + +PM_CARTESIAN operator -(const PM_CARTESIAN &v) +{ + PM_CARTESIAN ret; + + ret.x = -v.x; + ret.y = -v.y; + ret.z = -v.z; + + return ret; +} + +PM_QUATERNION operator +(const PM_QUATERNION &q) +{ + return q; +} + +PM_QUATERNION operator -(const PM_QUATERNION &q) +{ + PM_QUATERNION ret; + PmQuaternion _q; + + toQuat(q, &_q); + + pmQuatInv(&_q, &_q); + + toQuat(_q, &ret); + + return ret; +} + +PM_POSE operator +(const PM_POSE &p) +{ + return p; +} + +PM_POSE operator -(const PM_POSE &p) +{ + PM_POSE ret; + PmPose _p; + + toPose(p, &_p); + + pmPoseInv(&_p, &_p); + + toPose(_p, &ret); + + return ret; +} + +int operator ==(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2) +{ + PmCartesian _v1, _v2; + + toCart(v1, &_v1); + toCart(v2, &_v2); + + return pmCartCartCompare(&_v1, &_v2); +} + +int operator ==(const PM_QUATERNION &q1, PM_QUATERNION &q2) +{ + PmQuaternion _q1, _q2; + + toQuat(q1, &_q1); + toQuat(q2, &_q2); + + return pmQuatQuatCompare(&_q1, &_q2); +} + +int operator ==(const PM_POSE &p1, const PM_POSE &p2) +{ + PmPose _p1, _p2; + + toPose(p1, &_p1); + toPose(p2, &_p2); + + return pmPosePoseCompare(&_p1, &_p2); +} + +int operator !=(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2) +{ + PmCartesian _v1, _v2; + + toCart(v1, &_v1); + toCart(v2, &_v2); + + return !pmCartCartCompare(&_v1, &_v2); +} + +int operator !=(const PM_QUATERNION &q1, const PM_QUATERNION &q2) +{ + PmQuaternion _q1, _q2; + + toQuat(q1, &_q1); + toQuat(q2, &_q2); + + return !pmQuatQuatCompare(&_q1, &_q2); +} + +int operator !=(const PM_POSE &p1, const PM_POSE &p2) +{ + PmPose _p1, _p2; + + toPose(p1, &_p1); + toPose(p2, &_p2); + + return !pmPosePoseCompare(&_p1, &_p2); +} + +PM_CARTESIAN operator +(PM_CARTESIAN v1, const PM_CARTESIAN &v2) +{ + v1 += v2; + return v1; +} + +PM_CARTESIAN operator -(PM_CARTESIAN v1, const PM_CARTESIAN &v2) +{ + v1 -= v2; + return v1; +} + +PM_CARTESIAN operator *(PM_CARTESIAN v, double s) +{ + v *= s; + return v; +} + +PM_CARTESIAN operator *(double s, PM_CARTESIAN v) +{ + v *= s; + return v; +} + +PM_CARTESIAN operator /(const PM_CARTESIAN &v, double s) +{ + PM_CARTESIAN ret; + +#ifdef PM_DEBUG + if (s == 0.0) { +#ifdef PM_PRINT_ERROR + pmPrintError("PM_CARTESIAN::operator / : divide by 0\n"); +#endif + pmErrno = PM_DIV_ERR; + return ret; + } +#endif + + ret.x = v.x / s; + ret.y = v.y / s; + ret.z = v.z / s; + + return ret; +} + +PM_QUATERNION operator *(double s, const PM_QUATERNION &q) +{ + PM_QUATERNION qout; + PmQuaternion _q; + + toQuat(q, &_q); + + pmQuatScalMult(&_q, s, &_q); + + toQuat(_q, &qout); + + return qout; +} + +PM_QUATERNION operator *(const PM_QUATERNION &q, double s) +{ + PM_QUATERNION qout; + PmQuaternion _q; + + toQuat(q, &_q); + + pmQuatScalMult(&_q, s, &_q); + + toQuat(_q, &qout); + + return qout; +} + +PM_QUATERNION operator /(const PM_QUATERNION &q, double s) +{ + PM_QUATERNION qout; + PmQuaternion _q; + + toQuat(q, &_q); + +#ifdef PM_DEBUG + if (s == 0.0) { +#ifdef PM_PRINT_ERROR + pmPrintError("Divide by 0 in operator /\n"); +#endif + pmErrno = PM_NORM_ERR; + +/*! \todo Another #if 0 */ +#if 0 + // g++/gcc versions 2.8.x and 2.9.x + // will complain that the call to PM_QUATERNION(PM_QUATERNION) is + // ambigous. (2.7.x and some others allow it) + return qout = + PM_QUATERNION((double) 0, (double) 0, (double) 0, (double) 0); +#else + + PmQuaternion quat; + + quat.s = 0; + quat.x = 0; + quat.y = 0; + quat.z = 0; + + pmQuatNorm(&quat, &quat); + + qout.s = quat.s; + qout.x = quat.x; + qout.y = quat.y; + qout.z = quat.z; + return qout; +#endif + + } +#endif + + pmQuatScalMult(&_q, 1.0 / s, &_q); + toQuat(_q, &qout); + + pmErrno = 0; + return qout; +} + +PM_CARTESIAN operator *(const PM_QUATERNION &q, const PM_CARTESIAN &v) +{ + PM_CARTESIAN vout; + PmQuaternion _q; + PmCartesian _v; + + toQuat(q, &_q); + toCart(v, &_v); + + pmQuatCartMult(&_q, &_v, &_v); + + toCart(_v, &vout); + + return vout; +} + +PM_QUATERNION operator *(const PM_QUATERNION &q1, const PM_QUATERNION &q2) +{ + PM_QUATERNION ret; + PmQuaternion _q1, _q2; + + toQuat(q1, &_q1); + toQuat(q2, &_q2); + + pmQuatQuatMult(&_q1, &_q2, &_q1); + + toQuat(_q1, &ret); + + return ret; +} + +PM_ROTATION_MATRIX operator *(const PM_ROTATION_MATRIX &m1, const PM_ROTATION_MATRIX &m2) +{ + PM_ROTATION_MATRIX ret; + PmRotationMatrix _m1, _m2; + + toMat(m1, &_m1); + toMat(m2, &_m2); + + pmMatMatMult(&_m1, &_m2, &_m1); + + toMat(_m1, &ret); + + return ret; +} + +PM_POSE operator *(const PM_POSE &p1, const PM_POSE &p2) +{ + PM_POSE ret; + PmPose _p1, _p2; + + toPose(p1, &_p1); + toPose(p2, &_p2); + + pmPosePoseMult(&_p1, &_p2, &_p1); + + toPose(_p1, &ret); + + return ret; +} + +PM_CARTESIAN operator *(const PM_POSE &p, const PM_CARTESIAN &v) +{ + PM_CARTESIAN ret; + PmPose _p; + PmCartesian _v; + + toPose(p, &_p); + toCart(v, &_v); + + pmPoseCartMult(&_p, &_v, &_v); + + toCart(_v, &ret); + + return ret; +} + +PM_CARTESIAN shift(const PM_CARTESIAN & p, int n) +{ + // TODO optimize + auto out = p; + out.shift(n); + return out; +} diff --git a/src/libnml/posemath/posemath_ext.hh b/src/libnml/posemath/posemath_ext.hh new file mode 100644 index 00000000000..fdbc55499e6 --- /dev/null +++ b/src/libnml/posemath/posemath_ext.hh @@ -0,0 +1,454 @@ +#ifndef POSEMATH_HH +#define POSEMATH_HH + +#include "posemath.h" + +#define USE_CONST +#define USE_CCONST +#define USE_REF + +#ifdef USE_CCONST +#define PM_CCONST const +#else +#define PM_CCONST +#endif + +#ifdef USE_CONST +#define PM_CONST const +#else +#define PM_CONST +#endif + +#ifdef USE_REF +#define PM_REF & +#else +#define PM_REF +#endif + +#define INCLUDE_POSEMATH_COPY_CONSTRUCTORS + +/* forward declarations-- conversion ctors will need these */ + +/* translation types */ +struct PM_CARTESIAN; /* Cart */ +struct PM_SPHERICAL; /* Sph */ +struct PM_CYLINDRICAL; /* Cyl */ + +/* rotation types */ +struct PM_ROTATION_VECTOR; /* Rot */ +struct PM_ROTATION_MATRIX; /* Mat */ +struct PM_QUATERNION; /* Quat */ +struct PM_EULER_ZYZ; /* Zyz */ +struct PM_EULER_ZYX; /* Zyx */ +struct PM_RPY; /* Rpy */ + +/* pose types */ +struct PM_POSE; /* Pose */ +struct PM_HOMOGENEOUS; /* Hom */ + +/* PM_CARTESIAN */ + +struct PM_CARTESIAN : public PmCartesian { + /* ctors/dtors */ + PM_CARTESIAN() = default; + PM_CARTESIAN(double _x, double _y, double _z); + PM_CARTESIAN(const PmCartesian & cart) : PmCartesian(cart) {}; + + PM_CARTESIAN(PM_CONST PM_CYLINDRICAL PM_REF c); /* conversion */ + PM_CARTESIAN(PM_CONST PM_SPHERICAL PM_REF s); /* conversion */ + + /* operators */ + double &operator[] (int n); /* this[n] */ + PM_CARTESIAN & operator += (const PM_CARTESIAN &o); + PM_CARTESIAN & operator -= (const PM_CARTESIAN &o); + + // Scalar operations + PM_CARTESIAN & operator *= (double o); + PM_CARTESIAN & operator /= (double o); + + void shift(int n); +}; + +PM_CARTESIAN shift(PM_CARTESIAN const &p, int n); + +/* PM_SPHERICAL */ + +struct PM_SPHERICAL { + /* ctors/dtors */ + PM_SPHERICAL() { + } +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS + PM_SPHERICAL(PM_CCONST PM_SPHERICAL & s); +#endif + PM_SPHERICAL(double _theta, double _phi, double _r); + PM_SPHERICAL(PM_CONST PM_CYLINDRICAL PM_REF v); /* conversion */ + PM_SPHERICAL(PM_CONST PM_CARTESIAN PM_REF v); /* conversion */ + + /* operators */ + double &operator[] (int n); /* this[n] */ + + /* data */ + double theta, phi, r; +}; + +/* PM_CYLINDRICAL */ + +struct PM_CYLINDRICAL { + /* ctors/dtors */ + PM_CYLINDRICAL() { + } +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS + PM_CYLINDRICAL(PM_CCONST PM_CYLINDRICAL & c); +#endif + PM_CYLINDRICAL(double _theta, double _r, double _z); + PM_CYLINDRICAL(PM_CONST PM_CARTESIAN PM_REF v); /* conversion */ + PM_CYLINDRICAL(PM_CONST PM_SPHERICAL PM_REF v); /* conversion */ + + /* operators */ + double &operator[] (int n); /* this[n] */ + + /* data */ + double theta, r, z; +}; + +/* PM_ROTATION_VECTOR */ + +struct PM_ROTATION_VECTOR { + /* ctors/dtors */ + PM_ROTATION_VECTOR() { + } +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS + PM_ROTATION_VECTOR(PM_CCONST PM_ROTATION_VECTOR & r); +#endif + PM_ROTATION_VECTOR(double _r, double _x, double _y, double _z); + PM_ROTATION_VECTOR(PM_CONST PM_QUATERNION PM_REF q); /* conversion + */ + + /* operators */ + double &operator[] (int n); /* this[n] */ + + /* data */ + double s, x, y, z; +}; + +/* PM_ROTATION_MATRIX */ + +struct PM_ROTATION_MATRIX { + /* ctors/dtors */ + PM_ROTATION_MATRIX() { + } +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS + PM_ROTATION_MATRIX(PM_CCONST PM_ROTATION_MATRIX & mat); /* added + 7-May-1997 + by WPS */ +#endif + PM_ROTATION_MATRIX(double xx, double xy, double xz, + double yx, double yy, double yz, double zx, double zy, double zz); + PM_ROTATION_MATRIX(PM_CARTESIAN _x, PM_CARTESIAN _y, PM_CARTESIAN _z); + PM_ROTATION_MATRIX(PM_CONST PM_ROTATION_VECTOR PM_REF v); /* conversion + */ + PM_ROTATION_MATRIX(PM_CONST PM_QUATERNION PM_REF q); /* conversion + */ + PM_ROTATION_MATRIX(PM_CONST PM_EULER_ZYZ PM_REF zyz); /* conversion + */ + PM_ROTATION_MATRIX(PM_CONST PM_EULER_ZYX PM_REF zyx); /* conversion + */ + PM_ROTATION_MATRIX(PM_CONST PM_RPY PM_REF rpy); /* conversion */ + + /* operators */ + PM_CARTESIAN & operator[](int n); /* this[n] */ + + /* data */ + PM_CARTESIAN x, y, z; +}; + +/* PM_QUATERNION */ + +using PM_AXIS = PmAxis; + +struct PM_QUATERNION : public PmQuaternion { + /* ctors/dtors */ + PM_QUATERNION() { + } +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS + PM_QUATERNION(PM_CCONST PM_QUATERNION & quat); /* added 7-May-1997 + by WPS */ +#endif + PM_QUATERNION(double _s, double _x, double _y, double _z); + PM_QUATERNION(PM_CONST PM_ROTATION_VECTOR PM_REF v); /* conversion + */ + PM_QUATERNION(PM_CONST PM_ROTATION_MATRIX PM_REF m); /* conversion + */ + PM_QUATERNION(PM_CONST PM_EULER_ZYZ PM_REF zyz); /* conversion */ + PM_QUATERNION(PM_CONST PM_EULER_ZYX PM_REF zyx); /* conversion */ + PM_QUATERNION(PM_CONST PM_RPY PM_REF rpy); /* conversion */ + PM_QUATERNION(PM_AXIS axis, double angle); /* conversion */ + + /* operators */ + double &operator[] (int n); /* this[n] */ + + /* functions */ + void axisAngleMult(PM_AXIS axis, double angle); +}; + +/* PM_EULER_ZYZ */ + +struct PM_EULER_ZYZ { + /* ctors/dtors */ + PM_EULER_ZYZ() { + } +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS + PM_EULER_ZYZ(PM_CCONST PM_EULER_ZYZ & zyz); +#endif + PM_EULER_ZYZ(double _z, double _y, double _zp); + PM_EULER_ZYZ(PM_CONST PM_QUATERNION PM_REF q); /* conversion */ + PM_EULER_ZYZ(PM_CONST PM_ROTATION_MATRIX PM_REF m); /* conversion */ + + /* operators */ + double &operator[] (int n); + + /* data */ + double z, y, zp; +}; + +/* PM_EULER_ZYX */ + +struct PM_EULER_ZYX : public PmEulerZyx { + /* ctors/dtors */ + PM_EULER_ZYX() { + } +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS + PM_EULER_ZYX(PM_CCONST PM_EULER_ZYX & zyx); +#endif + PM_EULER_ZYX(double _z, double _y, double _x); + PM_EULER_ZYX(PM_CONST PM_QUATERNION PM_REF q); /* conversion */ + PM_EULER_ZYX(PM_CONST PM_ROTATION_MATRIX PM_REF m); /* conversion */ + + /* operators */ + double &operator[] (int n); +}; + +/* PM_RPY */ + +struct PM_RPY { + /* ctors/dtors */ + PM_RPY() { + } +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS + PM_RPY(PM_CCONST PM_RPY PM_REF rpy); /* added 7-May-1997 by WPS */ +#endif + PM_RPY(double _r, double _p, double _y); + PM_RPY(PM_CONST PM_QUATERNION PM_REF q); /* conversion */ + PM_RPY(PM_CONST PM_ROTATION_MATRIX PM_REF m); /* conversion */ + + /* operators */ + double &operator[] (int n); + + /* data */ + double r, p, y; +}; + +/* PM_POSE */ + +struct PM_POSE : public PmPose { + /* ctors/dtors */ + PM_POSE() { + } +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS + PM_POSE(PM_CCONST PM_POSE & p); +#endif + PM_POSE(PM_CARTESIAN v, PM_QUATERNION q); + PM_POSE(double x, double y, double z, + double s, double sx, double sy, double sz); + PM_POSE(PM_CONST PM_HOMOGENEOUS PM_REF h); /* conversion */ + + /* operators */ + double &operator[] (int n); /* this[n] */ +}; + +/* PM_HOMOGENEOUS */ + +struct PM_HOMOGENEOUS { + /* ctors/dtors */ + PM_HOMOGENEOUS() { + } +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS + PM_HOMOGENEOUS(PM_CCONST PM_HOMOGENEOUS & h); +#endif + PM_HOMOGENEOUS(PM_CARTESIAN v, PM_ROTATION_MATRIX m); + PM_HOMOGENEOUS(PM_CONST PM_POSE PM_REF p); /* conversion */ + + /* operators */ + PM_CARTESIAN & operator[](int n); /* column vector */ + + /* data ( [ 0 0 0 1 ] element is manually returned by [] if needed ) */ + PM_CARTESIAN tran; + PM_ROTATION_MATRIX rot; +}; + +/* PM_LINE */ + +struct PM_LINE { + /* ctors/dtors */ + PM_LINE() { + } +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS + PM_LINE(PM_CCONST PM_LINE &); +#endif + + /* functions */ + int init(PM_POSE start, PM_POSE end); + int point(double len, PM_POSE * point); + + /* data */ + PM_POSE start; /* where motion was started */ + PM_POSE end; /* where motion is going */ + PM_CARTESIAN uVec; /* unit vector from start to end */ +}; + +/* PM_CIRCLE */ + +struct PM_CIRCLE : public PmCircle { + /* ctors/dtors */ + PM_CIRCLE() { + } +#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS + PM_CIRCLE(PM_CCONST PM_CIRCLE &); +#endif + + /* functions */ + int init(PM_POSE start, PM_POSE end, + PM_CARTESIAN center, PM_CARTESIAN normal, int turn, double expected_angle_rad); + int point(double angle_, PM_POSE * point); +}; + +/* overloaded external functions */ + +/* dot */ +extern double dot(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); + +/* cross */ +extern PM_CARTESIAN cross(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); + +#if 0 +/* norm */ +extern PM_CARTESIAN norm(PM_CARTESIAN v); +extern PM_QUATERNION norm(PM_QUATERNION q); +extern PM_ROTATION_VECTOR norm(PM_ROTATION_VECTOR r); +extern PM_ROTATION_MATRIX norm(PM_ROTATION_MATRIX m); +#endif + +/* unit */ +extern PM_CARTESIAN unit(const PM_CARTESIAN &v); +extern PM_QUATERNION unit(const PM_QUATERNION &q); +extern PM_ROTATION_VECTOR unit(const PM_ROTATION_VECTOR &r); +extern PM_ROTATION_MATRIX unit(const PM_ROTATION_MATRIX &m); + +/* isNorm */ +extern int isNorm(const PM_CARTESIAN &v); +extern int isNorm(const PM_QUATERNION &q); +extern int isNorm(const PM_ROTATION_VECTOR &r); +extern int isNorm(const PM_ROTATION_MATRIX &m); + +/* mag */ +extern double mag(const PM_CARTESIAN &v); + +/* disp */ +extern double disp(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); + +/* inv */ +extern PM_CARTESIAN inv(const PM_CARTESIAN &v); +extern PM_ROTATION_MATRIX inv(const PM_ROTATION_MATRIX &m); +extern PM_QUATERNION inv(const PM_QUATERNION &q); +extern PM_POSE inv(const PM_POSE &p); +extern PM_HOMOGENEOUS inv(const PM_HOMOGENEOUS &h); + +/* project */ +extern PM_CARTESIAN proj(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); + +/* overloaded arithmetic functions */ + +/* unary +, - for translation, rotation, pose */ +extern PM_CARTESIAN operator + (const PM_CARTESIAN &v); +extern PM_CARTESIAN operator - (const PM_CARTESIAN &v); +extern PM_QUATERNION operator + (const PM_QUATERNION &q); +extern PM_QUATERNION operator - (const PM_QUATERNION &q); +extern PM_POSE operator + (const PM_POSE &p); +extern PM_POSE operator - (const PM_POSE &p); + +/* compare operators */ +extern int operator == (const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); +extern int operator == (const PM_QUATERNION &q1, const PM_QUATERNION &q2); +extern int operator == (const PM_POSE &p1, const PM_POSE &p2); +extern int operator != (const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); +extern int operator != (const PM_QUATERNION &q1, const PM_QUATERNION &q2); +extern int operator != (const PM_POSE &p1, const PM_POSE &p2); + +/* translation +, -, scalar *, - */ + +/* v + v */ +extern PM_CARTESIAN operator + (PM_CARTESIAN v1, const PM_CARTESIAN &v2); +/* v - v */ +extern PM_CARTESIAN operator - (PM_CARTESIAN v1, const PM_CARTESIAN &v2); +/* v * s */ +extern PM_CARTESIAN operator *(PM_CARTESIAN v, double s); +/* s * v */ +extern PM_CARTESIAN operator *(double s, PM_CARTESIAN v); +/* v / s */ +extern PM_CARTESIAN operator / (const PM_CARTESIAN &v, double s); + +/* rotation * by scalar, translation, and rotation */ + +/* s * q */ +extern PM_QUATERNION operator *(double s, const PM_QUATERNION &q); +/* q * s */ +extern PM_QUATERNION operator *(const PM_QUATERNION &q, double s); +/* q / s */ +extern PM_QUATERNION operator / (const PM_QUATERNION &q, double s); +/* q * v */ +extern PM_CARTESIAN operator *(const PM_QUATERNION &q, const PM_CARTESIAN &v); +/* q * q */ +extern PM_QUATERNION operator *(const PM_QUATERNION &q1, const PM_QUATERNION &q2); +/* m * m */ +extern PM_ROTATION_MATRIX operator *(const PM_ROTATION_MATRIX &m1, + const PM_ROTATION_MATRIX &m2); + +/* pose operators */ + +/* q * p */ +extern PM_POSE operator *(const PM_QUATERNION &q, const PM_POSE &p); +/* p * p */ +extern PM_POSE operator *(const PM_POSE &p1, const PM_POSE &p2); +/* p * v */ +extern PM_CARTESIAN operator *(const PM_POSE &p, const PM_CARTESIAN &v); + +/* slicky macros for item-by-item copying between C and C++ structs */ + +#define toCart(src,dst) do {*dst = PM_CARTESIAN(src);} while (0) + +#define toCyl(src,dst) do {(dst)->theta = (src).theta; (dst)->r = (src).r; (dst)->z = (src).z;} while (0) + +#define toSph(src,dst) do {(dst)->theta = (src).theta; (dst)->phi = (src).phi; (dst)->r = (src).r;} while (0) + +#define toQuat(src,dst) do {(dst)->s = (src).s; (dst)->x = (src).x; (dst)->y = (src).y; (dst)->z = (src).z;} while (0) + +#define toRot(src,dst) do {(dst)->s = (src).s; (dst)->x = (src).x; (dst)->y = (src).y; (dst)->z = (src).z;} while (0) + +#define toMat(src,dst) do {toCart((src).x, &((dst)->x)); toCart((src).y, &((dst)->y)); toCart((src).z, &((dst)->z));} while (0) + +#define toEulerZyz(src,dst) do {(dst)->z = (src).z; (dst)->y = (src).y; (dst)->zp = (src).zp;} while (0) + +#define toEulerZyx(src,dst) do {(dst)->z = (src).z; (dst)->y = (src).y; (dst)->x = (src).x;} while (0) + +#define toRpy(src,dst) do {(dst)->r = (src).r; (dst)->p = (src).p; (dst)->y = (src).y;} while (0) + +#define toPose(src,dst) do {toCart((src).tran, &((dst)->tran)); toQuat((src).rot, &((dst)->rot));} while (0) + +#define toHom(src,dst) do {toCart((src).tran, &((dst)->tran)); toMat((src).rot, &((dst)->rot));} while (0) + +#define toLine(src,dst) do {toPose((src).start, &((dst)->start)); toPose((src).end, &((dst)->end)); toCart((src).uVec, &((dst)->uVec));} while (0) + +#define toCircle(src,dst) do {toCart((src).center, &((dst)->center)); toCart((src).normal, &((dst)->normal)); toCart((src).rTan, &((dst)->rTan)); toCart((src).rPerp, &((dst)->rPerp)); toCart((src).rHelix, &((dst)->rHelix)); (dst)->radius = (src).radius; (dst)->angle = (src).angle; (dst)->spiral = (src).spiral;} while (0) + + +#endif // POSEMATH_HH diff --git a/src/libnml/posemath/posemath_fwd.h b/src/libnml/posemath/posemath_fwd.h new file mode 100644 index 00000000000..8322e435028 --- /dev/null +++ b/src/libnml/posemath/posemath_fwd.h @@ -0,0 +1,94 @@ +#ifndef POSEMATH_FWD_H +#define POSEMATH_FWD_H + +struct PmCartesian; +struct PmCylindrical; +struct PmEuler_ZYX; +struct PmEuler_ZYZ; +struct PmHomogeneous; +struct PmPose; +struct PmQuaternion; +struct PmRotationMatrix; +struct PmRotationVector; +struct PmRPY; +struct PmSpherical; + +typedef struct PmCartesian PmCartesian; +typedef struct PmCylindrical PmCylindrical; +typedef struct PmEuler_ZYX PmEuler_ZYX; +typedef struct PmEuler_ZYZ PmEuler_ZYZ; +typedef struct PmHomogeneous PmHomogeneous; +typedef struct PmPose PmPose; +typedef struct PmQuaternion PmQuaternion; +typedef struct PmRotationMatrix PmRotationMatrix; +typedef struct PmRotationVector PmRotationVector; +typedef struct PmRPY PmRPY; +typedef struct PmSpherical PmSpherical; + +/* DOUBLE_FUZZ is the smallest double, d, such that (1+d != 1) w/o FPC. + DOUBLECP_FUZZ is the same only with the Floating Point CoProcessor */ + +#define DOUBLE_FUZZ 2.2204460492503131e-16 +#define DOUBLECP_FUZZ 1.0842021724855044e-19 + + +/** + * FIXME sloppily defined constants here. + * These constants are quite large compared to the DOUBLE_FUZZ limitation. They + * seem like an ugly band-aid for floating point problems. + */ + +// FIXME setting this to be an order of magnitude smaller than canon's shortest +// allowed segment. This is still larger than TP's smallest position, so it may +// be silently causing trouble. +#define CART_FUZZ (1.0e-8) +/* how close a cartesian vector's magnitude must be for it to be considered + a zero vector */ + +#define Q_FUZZ (1.0e-06) +/* how close elements of a Q must be to be equal */ + +#define QS_FUZZ (1.0e-6) +/* how close q.s is to 0 to be 180 deg rotation */ + +#define RS_FUZZ (1.0e-6) +/* how close r.s is for a rotation vector to be considered 0 */ + +#define QSIN_FUZZ (1.0e-6) +/* how close sin(a/2) is to 0 to be zero rotat */ + +#define V_FUZZ (1.0e-8) +/* how close elements of a V must be to be equal */ + +#define SQRT_FUZZ (-1.0e-6) +/* how close to 0 before math_sqrt() is error */ + +#define UNIT_VEC_FUZZ (1.0e-6) +/* how close mag of vec must be to 1.00 */ + +#define UNIT_QUAT_FUZZ (1.0e-6) +/* how close mag of quat must be to 1.00 */ + +#define UNIT_SC_FUZZ (1.0e-6) +/* how close mag of sin, cos must be to 1.00 */ + +#define E_EPSILON (1.0e-6) +/* how close second ZYZ euler angle must be to 0/PI for degeneration */ + +#define SINGULAR_EPSILON (1.0e-6) +/* how close to zero the determinate of a matrix must be for singularity */ + +#define RPY_P_FUZZ (1.0e-6) +/* how close pitch is to zero for RPY to degenerate */ + +#define ZYZ_Y_FUZZ (1.0e-6) +/* how close Y is to zero for ZYZ Euler to degenerate */ + +#define ZYX_Y_FUZZ (1.0e-6) +/* how close Y is to zero for ZYX Euler to degenerate */ + +#define CIRCLE_FUZZ (1.0e-6) +/* Bug fix for the missing circles problem */ + + +#endif // POSEMATH_FWD_H diff --git a/src/libnml/posemath/posemath_fwd.hh b/src/libnml/posemath/posemath_fwd.hh new file mode 100644 index 00000000000..126bfeaa7b7 --- /dev/null +++ b/src/libnml/posemath/posemath_fwd.hh @@ -0,0 +1,18 @@ +#ifndef POSEMATH_FWD_HH +#define POSEMATH_FWD_HH + +#include "posemath_fwd.h" + +struct PM_CARTESIAN; +struct PM_CYLINDRICAL; +struct PM_EULER_ZYX; +struct PM_EULER_ZYZ; +struct PM_HOMOGENEOUS; +struct PM_POSE; +struct PM_QUATERNION; +struct PM_ROTATION_MATRIX; +struct PM_ROTATION_VECTOR; +struct PM_RPY; +struct PM_SPHERICAL; + +#endif // POSEMATH_FWD_HH diff --git a/src/libnml/posemath/sincos.c b/src/libnml/posemath/sincos.c index 649e4df6a8a..4ea47b83646 100644 --- a/src/libnml/posemath/sincos.c +++ b/src/libnml/posemath/sincos.c @@ -17,11 +17,27 @@ 21-Jan-2004 P.C. Moved across from the original EMC source tree. */ +#include "config.h" + +#if defined(__KERNEL__) +#undef HAVE_SINCOS +#endif + +#ifndef HAVE_SINCOS + #include "rtapi_math.h" #include "sincos.h" -void pm_sincos(double x, double *sx, double *cx) +#ifndef HAVE___SINCOS + +#include "posemath.h" + +void sincos(double x, double *sx, double *cx) { *sx = sin(x); *cx = cos(x); } + +#endif + +#endif diff --git a/src/libnml/posemath/sincos.h b/src/libnml/posemath/sincos.h index 86fb61a090d..a9745c33615 100644 --- a/src/libnml/posemath/sincos.h +++ b/src/libnml/posemath/sincos.h @@ -15,6 +15,27 @@ #ifndef SINCOS_H #define SINCOS_H -extern void pm_sincos(double x, double *sx, double *cx); +#include "config.h" +/* + for each platform that has built-in support for the sincos function, + that should be discovered by ./configure +*/ + +/* testing for sincos now supported by ./configure */ +#ifdef HAVE___SINCOS +#define sincos __sincos +#endif + + +/* + all other platforms will not have __sincos, and will + get the declaration for the explicit function +*/ + +#ifndef HAVE___SINCOS + +extern void sincos(double x, double *sx, double *cx); + +#endif #endif /* #ifndef SINCOS_H */