SMC_SolveTripodRodsForward (FUN) ¶ FUNCTION SMC_SolveTripodRodsForward : BOOL Calculates the position of the TCP of a tripod given the length of the rods and their three connecting points InOut: Scope Name Type Inout vTCP SMC_VECTOR3D Inout Const c0 SMC_VECTOR3D c1 SMC_VECTOR3D c2 SMC_VECTOR3D Input rodLength LREAL Return SMC_SolveTripodRodsForward BOOL
SMC_SolveTwoRotJoints (FUN) ¶ FUNCTION SMC_SolveTwoRotJoints : BOOL Solves the inverse kinematics for two parallel rotatory joints in sequence. The following situation is assumed: v _ \ _ b \ _ J2 / x q2 / \ . / \ q1 . a \ . \ . \ . \. J1 x <-----+ 0 | | v y Note q1 and q2 are measured around the z-axis, i.e. counter-clockwise in the above diagram. If the links a and b are perpendicular, q2 is zero. The function returns true if (and only if) a solution exists. Otherwise, the point v is not in the working-space. q1 is assumed to be in the range ]-pi, pi], q2 in ]-pi, pi/2[. InOut: Scope Name Type Comment Input dVX LREAL The x-position of the target dVY LREAL The y-position of the target dA LREAL The length of the first link, must be positive dB LREAL The length of the second link, must be positive bElbowLow BOOL To disambiguate the two possible configurations Return SMC_SolveTwoRotJoints BOOL Output q1 LREAL The angle of joint 1 in radian, in the range ]-p2, pi] q2 LREAL The angle of joint 2 in radian, in the range ]-pi, pi/2[
SMC_Trafo_HandleWristSingularity (FUN) ¶ FUNCTION SMC_Trafo_HandleWristSingularity : BOOL Computes angles for q3 and q5 of an 6DOF articulated robot in a singular wrist configuration (q4 ~ 0). The angles are computed so that (q3 - q3_)^2 + (q5 - q5_)^2 is minimized under the following constraints: q3 + q5 = alpha (the orientation is correct) q3_min <= q3 <= q3_max, q5_min <= q5 <= q5_max (the angles are in the valid range) It is assumed that q3_ and q5_ are in the valid range. Returns FALSE if (and only if) there is no solution in the valid range. InOut: Scope Name Type Comment Return SMC_Trafo_HandleWristSingularity BOOL Input q3_ LREAL The old orientation of axis 3 q5_ LREAL The old orientation of axis 5 alpha LREAL The desired sum of q3 and q5 q3_min LREAL The minimum value for axis 3 q3_max LREAL The maximum value for axis 3 q5_min LREAL The minimum value for axis 5 q5_max LREAL The maximum value for axis 5 Output q3 LREAL The new value for axis 3 q5 LREAL The new value for axis 5
SMC_Vec_Sum_Kahan (FB) ¶ FUNCTION_BLOCK SMC_Vec_Sum_Kahan Implements Kahan summation for vectors. InOut: Scope Name Type Input bEnable BOOL v SMC_Vec Output vSum SMC_Vec
SMC_Vec_Zero (FUN) ¶ FUNCTION SMC_Vec_Zero : BOOL Creates a zero-vector of dimension n. InOut: Scope Name Type Inout v SMC_Vec Input n DINT Return SMC_Vec_Zero BOOL
SinCosEquations ¶ SMC_SolveCosLin (Function) SMC_SolveSinCos (Function) SMC_SolveSinCos2 (Function) SMC_SolveSinCosLin (Function)
SMC_SolveCosLin (FUN) ¶ FUNCTION SMC_SolveCosLin : BOOL Solves the following equation for alpha: cos(alpha) + b0 * alpha + b1 = 0 (1) given a range [alpha_start, alpha_end] for alpha. An error is returned in one of the following cases: pSolutions is 0 or capacitySolutions is 0 or an internal error occurred (numerical root finding error) The solutions are returned in radian (same unit as alpha). If alpha_start <= alpha_end, the solutions are returned in ascending order, otherwise in descending order. Only up to capacitySolutions solutions are returned. If more solutions exist, they are ignored. Returns the number of solutions stored in pSolutions. InOut: Scope Name Type Comment Input b0 LREAL The linear factor b1 LREAL The constant term alpha_start LREAL The start of the range for alpha alpha_end LREAL The end of the range for alpha pSolutions POINTER TO LREAL Pointer to an array of capacity capacitySolutions LREAL values. capacitySolutions UDINT The capacity of the array pSolutions. Must be > 0. Return SMC_SolveCosLin BOOL Output numSolutions UDINT The number of computed solutions
SMC_SolveSinCos (FUN) ¶ FUNCTION SMC_SolveSinCos : BOOL Solves the following equation for alpha: a * cos(alpha) + b * sin(alpha) + c = 0. Precondition: (a,b) <> (0,0), |c| <= sqrt(a²+b²). Returns FALSE if (and only if) the precondition does not hold. Two solutions are returned. They are in the range ]-2*pi, 2*pi] and dAlpha1 <= dAlpha2 holds. Note that dAplha1 + 2*n*SMC_PI and dAlpha2 + 2*n*SMC_PI are also solutions of the above equation for every integral value of n. (These are all possible solutions.) InOut: Scope Name Type Comment Return SMC_SolveSinCos BOOL Input da LREAL The factors of the equation db LREAL The factors of the equation dc LREAL The factors of the equation Output dAlpha1 LREAL The first (smaller) solution in radian dAlpha2 LREAL The second (larger) solution in radian
SMC_SolveSinCos2 (FUN) ¶ FUNCTION SMC_SolveSinCos2 : BOOL Solves the following system of equations for alpha: / a b\ / sin(alpha) \ _ / c \ \-b a/ \ cos(alpha) / - \ d / Precondition: (a,b) <> (0,0) and (c,d) <> (0,0). Returns FALSE if (and only if) the precondition does not hold. Note: a linear equation system is solved, and if the unique non-zero solution (which exists if the precondition holds) does not have unit length, it is scaled to unit length implicitly. This means that we compute an alpha which satisfies the original equations up to a multiplicative factor in this case. InOut: Scope Name Type Comment Return SMC_SolveSinCos2 BOOL Input da LREAL The coefficients of the matrix db LREAL The coefficients of the matrix dc LREAL The right hand side dd LREAL The right hand side pdAlpha POINTER TO LREAL The result is written here
SMC_SolveSinCosLin (FUN) ¶ FUNCTION SMC_SolveSinCosLin : BOOL Solves the following equation for alpha: a0 * cos(alpha) + a1 * sin(alpha) + a2 * alpha + a3 = 0 (1) given a range [alpha_start, alpha_end] for alpha. The solutions are returned in radian (same unit as alpha). If alpha_start <= alpha_end, the solutions are returned in ascending order, otherwise in descending order. Only up to capacitySolutions solutions are returned. If more solutions exist, they are ignored. Returns an error in one of the following situations: pSolutions = 0 or capacitySolitions = 0, internal error (numeric error during root-finding) Returns the number of solutions stored in pSolutions. Note: if infinitely many solutions exist, which is the case only for a0 = a1 = a2 = a3 = 0, then only two are returned at alpha=alpha_start and alpha=alpha_end. InOut: Scope Name Type Comment Input a0 LREAL The factors of the equation a1 LREAL a2 LREAL a3 LREAL alpha_start LREAL The start of the range for alpha alpha_end LREAL The end of the range for alpha pSolutions POINTER TO LREAL Pointer to an array of capacity capacitySolutions LREAL values. capacitySolutions UDINT The capacity of the array pSolutions. Return SMC_SolveSinCosLin BOOL Output numSolutions UDINT