/******************************************************************** * Description: blendmath.c * Circular arc blend math functions * * Author: Robert W. Ellenberg * License: GPL Version 2 * System: Linux * * Copyright (c) 2014 All rights reserved. * * Last change: ********************************************************************/ #include "posemath.h" #include "tc_types.h" #include "tc.h" #include "tp_types.h" #include "rtapi_math.h" #include "spherical_arc.h" #include "blendmath.h" #include "tp_debug.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 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; } } 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; } } 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 */ 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; } } /** 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 /** * @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; pmCartScalMult(u_tan, dr, center_out); 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; } int checkTangentAngle(PmCircle const * const circ, SphericalArc const * const arc, BlendGeom3 const * const geom, BlendParameters const * const param, double cycle_time, int at_end) { // 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); //TODO fail if theta is too large double dot; pmCartCartDot(&u_circ, &u_arc, &dot); double blend_angle = acos(saturate(dot,1.0)); 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; } /** * Check if two cartesian vectors are 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 v1 input vector 1 * @param v2 input vector 2 * @param tol angle tolerance for parallelism */ int pmCartCartParallel(PmCartesian const * const v1, PmCartesian const * const v2, double tol) { PmCartesian u1,u2; pmCartUnit(v1,&u1); pmCartUnit(v2,&u2); double dot; pmCartCartDot(&u1, &u2, &dot); double theta = acos(fabs(dot)); if (theta < tol) { return 1; } else { return 0; } } /** * Check if a Circle and line are coplanar. * * @param circ PmCircle input * @param line PmCartLine input * @param tol deviation tolerance (magnitude of error component) */ int pmCircLineCoplanar(PmCircle const * const circ, PmCartLine const * const line, double tol) { double dot; pmCartCartDot(&circ->normal, &line->uVec, &dot); if (fabs(dot) < tol) { return 1; } else { return 0; } } /** * Somewhat redundant function to calculate the segment intersection angle. * The intersection angle is half of the supplement of the "divergence" angle * between unit vectors. If two unit vectors are pointing in the same * 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 dot; pmCartCartDot(u1, u2, &dot); /*tp_debug_print("u1 = %f %f %f u2 = %f %f %f\n", u1->x, u1->y, u1->z, u2->x, u2->y, u2->z);*/ if (dot > 1.0 || dot < -1.0) { tp_debug_print("dot product %f outside domain of acos!\n",dot); sat_inplace(&dot,1.0); } *theta = acos(-dot)/2.0; return TP_ERR_OK; } /** 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); } /** * 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) { if (!normal ) { return TP_ERR_MISSING_INPUT; } PmCartesian planar_x,planar_y,planar_z; //Find perpendicular component of unit directions // FIXME Assumes normal is unit length // FIXME use plane project? 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); PmCartesian planar_scales; pmCartMag(&planar_x, &planar_scales.x); pmCartMag(&planar_y, &planar_scales.y); pmCartMag(&planar_z, &planar_scales.z); PmCartesian extents; pmCartCartDiv(bounds, &planar_scales, &extents); *diameter = pmCartMin(&extents); return TP_ERR_OK; } /** Find real roots of a quadratic equation in standard form. */ int quadraticFormula(double A, double B, double C, double * const root0, double * const root1) { double disc = pmSq(B) - 4.0 * A * C; if (disc < 0) { tp_debug_print("discriminant < 0\n"); return TP_ERR_FAIL; } double t1 = pmSqrt(disc); if (root0) { *root0 = ( -B + t1) / (2.0 * A); } if (root1) { *root1 = ( -B - t1) / (2.0 * A); } return TP_ERR_OK; } /** * @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) { 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); 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 common velocity and acceleration param->v_req = fmax(prev_tc->reqvel, tc->reqvel); param->v_goal = param->v_req * maxFeedScale; // Calculate the maximum planar velocity double v_planar_max; 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)); tp_debug_print("v_max1 = %f, v_max2 = %f\n", v_max1, v_max2); // Get "altitude" double v_area = v_max1 * v_max2 / 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); 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("vr1 = %f, vr2 = %f\n", prev_tc->reqvel, tc->reqvel); tp_debug_print("v_goal = %f, max scale = %f\n", param->v_goal, maxFeedScale); return res_dia; } /** * 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) { if (tc->motion_type != TC_CIRCULAR || 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; } // Handle convexity param->convex2 = arcConvexTest(&tc->coords.circle.xyz.center, &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); param->phi = (PM_PI - param->theta * 2.0); param->L1 = fmin(prev_tc->target, prev_tc->nominal_length / 2.0); if (param->convex2) { //use half of the length of the chord param->L2 = sin(param->phi2_max/4.0) * tc->coords.circle.xyz.radius; } else { param->L2 = param->phi2_max * tc->coords.circle.xyz.radius; } tp_debug_print("L1 = %f, L2 = %f\n", param->L1, param->L2); // Setup common parameters blendParamKinematics(geom, param, prev_tc, tc, acc_bound, vel_bound, maxFeedScale); return TP_ERR_OK; } 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) { if (tc->motion_type != TC_LINEAR || 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; } param->convex1 = arcConvexTest(&prev_tc->coords.circle.xyz.center, &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); } 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); } tp_debug_print("theta = %f\n", param->theta); param->L1 = param->phi1_max * prev_tc->coords.circle.xyz.radius; param->L2 = tc->nominal_length / 2.0; if (param->convex1) { //use half of the length of the chord param->L1 = sin(param->phi1_max/4.0) * prev_tc->coords.circle.xyz.radius; } tp_debug_print("L1 = %f, L2 = %f\n", param->L1, param->L2); // Setup common parameters blendParamKinematics(geom, param, prev_tc, tc, acc_bound, vel_bound, maxFeedScale); return TP_ERR_OK; } /** * Setup blend paramaters 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) */ 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) { 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; } //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(&prev_tc->coords.circle.xyz.center, &geom->P, &geom->u_tan2, false); param->convex2 = arcConvexTest(&tc->coords.circle.xyz.center, &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); } 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); // 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; } tp_debug_print("theta = %f\n", param->theta); param->phi = (PM_PI - param->theta * 2.0); param->L1 = param->phi1_max * prev_tc->coords.circle.xyz.radius; param->L2 = param->phi2_max * tc->coords.circle.xyz.radius; if (param->convex1) { //use half of the length of the chord param->L1 = sin(param->phi1_max/4.0) * prev_tc->coords.circle.xyz.radius; } if (param->convex2) { //use half of the length of the chord param->L2 = sin(param->phi2_max/4.0) * tc->coords.circle.xyz.radius; } 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 blendParamKinematics(geom, param, prev_tc, tc, acc_bound, vel_bound, maxFeedScale); return TP_ERR_OK; } /** * Setup blend paramaters 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) */ 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) { 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_debug_print("theta = %f\n", param->theta); param->phi = (PM_PI - param->theta * 2.0); blendGeom3Print(geom); const double greediness = 0.5; //Nominal length restriction prevents gobbling too much of parabolic blends param->L1 = fmin(prev_tc->target, prev_tc->nominal_length * greediness); param->L2 = tc->target * greediness; 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 blendParamKinematics(geom, param, prev_tc, tc, acc_bound, vel_bound, maxFeedScale); return TP_ERR_OK; } /** * Calculate plane normal and binormal based on unit direction vectors. */ int blendCalculateNormals3(BlendGeom3 * const geom) { int err_cross = pmCartCartCross(&geom->u_tan1, &geom->u_tan2, &geom->binormal); int err_unit_b = pmCartUnitEq(&geom->binormal); tp_debug_print("binormal = [%f %f %f]\n", geom->binormal.x, geom->binormal.y, geom->binormal.z); pmCartCartSub(&geom->u_tan2, &geom->u_tan1, &geom->normal); int err_unit_n = pmCartUnitEq(&geom->normal); 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); } /** * 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; } else { param->v_actual = param->v_plan; } // Store arc length of blend arc for future checks param->s_arc = param->R_plan * param->phi; if (param->R_plan < TP_POS_EPSILON) { tp_debug_print("#Blend radius too small, aborting arc\n"); return TP_ERR_FAIL; } 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; } if (prev_tc->motion_type != TC_LINEAR) { return 0; } //Check for segment length limits double L_prev = prev_tc->target - points->trim1; double prev_seg_time = L_prev / param->v_plan; param->consume = (prev_seg_time < gap_cycles * prev_tc->cycle_time); if (param->consume) { tp_debug_print("consuming prev line, L_prev = %g\n", L_prev); param->line_length = L_prev; } 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) { // 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; } /** * 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) { PmCartesian center2; double radius2; findSpiralApproximation(circ2, &geom->P, &geom->u_tan2, ¢er2, &radius2); // Define distances from actual center to circle centers double d2 = negate(param->R_plan, param->convex2) + 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(¢er2, &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; } 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; } points->trim1 = d_L; points->trim2 = findTrimAngle(&geom->P, &points->arc_center, ¢er2); return TP_ERR_OK; } /** * 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) { //Create "shifted center" approximation of spiral circles PmCartesian center1; double radius1; findSpiralApproximation(circ1, &geom->P, &geom->u_tan1, ¢er1, &radius1); // Define distance from actual arc center to circle center double d1 = negate(param->R_plan, param->convex1) + 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(¢er1, &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; } 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; } 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; } tp_debug_print("T_final = %f\n",T_final); points->trim1 = findTrimAngle(&geom->P, &points->arc_center, ¢er1); points->trim2 = d_L; return TP_ERR_OK; } /** * "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. */ 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) { // Create "shifted center" approximation of spiral circles // TODO refers to u1 instead of utan? PmCartesian center1, center2; double radius1, radius2; findSpiralApproximation(circ1, &geom->P, &geom->u_tan1, ¢er1, &radius1); findSpiralApproximation(circ2, &geom->P, &geom->u_tan2, ¢er2, &radius2); // Define distances from actual center to adjusted circle centers double d1 = negate(param->R_plan, param->convex1) + radius1; double d2 = negate(param->R_plan, param->convex2) + radius2; tp_debug_print("d1 = %f, d2 = %f\n", d1, d2); //Find "x" distance between C1 and C2 PmCartesian r_C1C2; pmCartCartSub(¢er2, ¢er1, &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); //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; } //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(¢er1, &geom->P, &r_PC1); // Get "test vectors, relative distance from solution center to P PmCartesian test1, test2; pmCartCartAdd(&r_PC1, &c_x, &test1); test2=test1; //Add and subtract c_y component to get equivalent of two Y solutions pmCartCartAddEq(&test1, &c_y); pmCartCartSubEq(&test2, &c_y); double mag1,mag2; pmCartMag(&test1, &mag1); pmCartMag(&test2, &mag2); if (mag2 < mag1) { //negative solution is closer pmCartNegEq(&c_y); } //Continue with correct solution, get actual center PmCartesian r_C1C; pmCartCartAdd(&c_x, &c_y, &r_C1C); pmCartCartAdd(¢er1, &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, ¢er2, &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); points->trim1 = findTrimAngle(&geom->P, &points->arc_center, ¢er1); points->trim2 = findTrimAngle(&geom->P, &points->arc_center, ¢er2); tp_debug_print("trim1 = %f, trim2 = %f\n", points->trim1, points->trim2); return TP_ERR_OK; } /** * Setup the spherical arc struct based on the blend arc data. */ int arcFromBlendPoints3(SphericalArc * const arc, BlendPoints3 const * const points, BlendGeom3 const * const geom, BlendParameters const * const param) { // 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); } int blendGeom3Print(BlendGeom3 const * const 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; } int blendPoints3Print(BlendPoints3 const * const 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); return 0; } double pmCircleActualMaxVel(PmCircle * const circle, double v_max, double a_max, int parabolic) { if (parabolic) { a_max /= 2.0; } double a_n_max = BLEND_ACC_RATIO_NORMAL * a_max; double v_max_acc = pmSqrt(a_n_max * circle->radius); if (v_max_acc < v_max) { tp_debug_print("Maxvel limited from %f to %f for tangential acceleration\n", v_max, v_max_acc); return v_max_acc; } else { tp_debug_print("v_max %f is within limit of v_max_acc %f\n",v_max, v_max_acc); return v_max; } }