diff --git a/examples/c/sphere.c b/examples/c/sphere.c index b345edb..1efb410 100644 --- a/examples/c/sphere.c +++ b/examples/c/sphere.c @@ -118,9 +118,9 @@ * SLIPPING ROLLING Relative slip velocity drops below EPS_MINSLIPVEL, * and is decreasing. */ - -#include #include +#include +#include /* Sphere radii. */ #define R_INNER 1. diff --git a/sdfast_api.h b/sdfast_api.h new file mode 100644 index 0000000..5c555de --- /dev/null +++ b/sdfast_api.h @@ -0,0 +1,565 @@ +/* sdfast_api.h - Modern C/C++ header for SD/FAST generated functions. + * + * Copyright 2026 Michael Sherman + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not + * use this file except in compliance with the License. You may obtain a copy + * of the License at http://www.apache.org/licenses/LICENSE-2.0 + * + * This header declares all functions generated by SD/FAST (the dynamics file, + * the simplified analysis file, and the library file). It can be used from + * both C and C++ code. + * + * PRECISION HANDLING + * ------------------ + * SD/FAST can generate code in single or double precision (controlled by the + * "single" keyword in the System Description File). By default, double + * precision is used. All floating-point parameters use the type `sdreal_t`. + * + * Define SDFAST_SINGLE_PRECISION before including this header if your + * generated code uses single precision (float). Otherwise double is assumed. + * + * ARRAY SIZE PARAMETERS + * --------------------- + * The generated functions use model-specific array sizes (NQ, NU, NC, etc.) + * but in C, array parameters decay to pointers, so a fixed header works for + * any model. The caller is responsible for passing correctly-sized arrays. + * + * USER-SUPPLIED ROUTINES + * ---------------------- + * Several routines must be supplied by the user (not generated by SD/FAST). + * These are declared at the end of this header. + */ + +#ifndef SDFAST_API_H_ +#define SDFAST_API_H_ + +#include + +#ifdef SDFAST_SINGLE_PRECISION +typedef float sdreal_t; +#else +typedef double sdreal_t; +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +/* ======================================================================== + * INITIALIZATION + * ======================================================================== */ + +/* Initialize the SD/FAST system. Must be called before any other routine. */ +void sdinit(void); + +/* ======================================================================== + * STATE AND DERIVATIVES (General Analysis Routines) + * ======================================================================== */ + +/* Set the system time, positions (q), and velocities (u). */ +void sdstate(sdreal_t timein, sdreal_t q[], sdreal_t u[]); + +/* Compute position (qdot) and velocity (udot) derivatives. */ +void sdderiv(sdreal_t oqdot[], sdreal_t oudot[]); + +/* Compute the DAE residuals (for implicit integration). */ +void sdresid(sdreal_t eqdot[], sdreal_t eudot[], sdreal_t emult[], + sdreal_t resid[]); + +/* ======================================================================== + * EULER PARAMETER / ANGLE CONVERSIONS + * ======================================================================== */ + +/* Convert Euler-parameter state to 1-2-3 angle state. */ +void sdst2ang(sdreal_t st[], sdreal_t stang[]); + +/* Convert 1-2-3 angle state to Euler-parameter state. */ +void sdang2st(sdreal_t stang[], sdreal_t st[]); + +/* Check Euler parameter normalization error. */ +void sdnrmsterr(sdreal_t st[], sdreal_t normst[], int routine); + +/* Normalize Euler parameters in state vector. */ +void sdnormst(sdreal_t st[], sdreal_t normst[]); + +/* ======================================================================== + * COORDINATE DERIVATIVES AND CONVERSIONS + * ======================================================================== */ + +/* Return position coordinate derivatives (qdots) for tree joints. */ +void sdqdot(sdreal_t oqdot[]); + +/* Convert velocities (u) to position coordinate derivatives (qdot). */ +void sdu2qdot(sdreal_t uin[], sdreal_t oqdot[]); + +/* Set the pseudo-joint (loop joint) state. */ +void sdpsstate(sdreal_t lqin[]); + +/* Return pseudo (loop) joint coordinates and velocities. */ +void sdpseudo(sdreal_t lqout[], sdreal_t luout[]); + +/* Return pseudo (loop) joint coordinate derivatives. */ +void sdpsqdot(sdreal_t lqdout[]); + +/* Return pseudo (loop) joint velocity derivatives. */ +void sdpsudot(sdreal_t ludout[]); + +/* ======================================================================== + * CONSTRAINT ERRORS + * ======================================================================== */ + +/* Return position constraint errors. */ +void sdperr(sdreal_t errs[]); + +/* Return velocity constraint errors. */ +void sdverr(sdreal_t errs[]); + +/* Return acceleration constraint errors. */ +void sdaerr(sdreal_t errs[]); + +/* ======================================================================== + * CONSTRAINT STABILIZATION + * ======================================================================== */ + +/* Set constraint stabilization gains (Baumgarte parameters). */ +void sdstab(sdreal_t velin, sdreal_t posin); + +/* Get constraint stabilization gains. */ +void sdgetstab(sdreal_t *velout, sdreal_t *posout); + +/* ======================================================================== + * CONSTRAINT MULTIPLIERS + * ======================================================================== */ + +/* Return constraint multipliers, rank, and multiplier map. */ +void sdmult(sdreal_t omults[], int *orank, int multmap[]); + +/* ======================================================================== + * DYNAMICS INTERMEDIATE ROUTINES + * ======================================================================== */ + +/* Compute partial velocities (kinematics stage). */ +void sddoping(void); + +/* Compute gyroscopic torques (related to loop joints). */ +void sddoltau(void); + +/* Compute articulated-body inertias. */ +void sddoiner(void); + +/* Compute acceleration-independent force terms. */ +void sddofs0(void); + +/* Compute and factor the mass matrix. */ +void sddomm(int routine); + +/* Factor/solve the left-hand side. */ +void sdlhs(int routine); + +/* Compute constraint force contributions. */ +void sdmfrc(sdreal_t imult[]); + +/* Compute right-hand side force terms. */ +void sdrhs(void); + +/* Compute force matrix (acceleration-independent part). */ +void sdfs0(void); + +/* Compute force to multiplier contributions. */ +void sdfsmult(void); + +/* Compute full force matrix. */ +void sdfsfull(void); + +/* Compute generalized force-to-multiplier matrix. */ +void sdfsgenmult(void); + +/* Compute full generalized force matrix. */ +void sdfsgenfull(void); + +/* ======================================================================== + * EQUATIONS OF MOTION: UDOTS + * ======================================================================== */ + +/* Compute unconstrained accelerations (udot with no constraint forces). */ +void sdudot0(sdreal_t oudot0[]); + +/* Set the velocity derivatives (udots) directly. */ +void sdsetudot(sdreal_t iudot[]); + +/* Compute acceleration due to constraint multipliers. */ +void sdudotm(sdreal_t imult[], sdreal_t oudotm[]); + +/* ======================================================================== + * TORQUE / FORCE COMPUTATIONS + * ======================================================================== */ + +/* Compute equivalent hinge torques. */ +void sdequivht(sdreal_t tau[]); + +/* Compute full generalized forces (including constraint contributions). */ +void sdfulltrq(sdreal_t udotin[], sdreal_t multin[], sdreal_t trqout[]); + +/* Compute hinge torques to produce given udots (ignoring constraints). */ +void sdcomptrq(sdreal_t udotin[], sdreal_t trqout[]); + +/* Compute hinge torques due to constraint multipliers. */ +void sdmulttrq(sdreal_t multin[], sdreal_t trqout[]); + +/* Return the system mass matrix. */ +void sdmassmat(sdreal_t *mmat); /* actually mmat[NU][NU] */ + +/* Return the force matrix (RHS without udot terms). */ +void sdfrcmat(sdreal_t fmat[]); + +/* ======================================================================== + * REACTION FORCES AND TORQUES + * ======================================================================== */ + +/* Return reaction forces and torques at all tree joints. + * forces and torques are [NJNT][3] arrays. */ +void sdreac(sdreal_t *forces, sdreal_t *torques); + +/* ======================================================================== + * MOMENTUM AND ENERGY + * ======================================================================== */ + +/* Compute linear momentum, angular momentum, and kinetic energy. */ +void sdmom(sdreal_t lm[3], sdreal_t am[3], sdreal_t *ke); + +/* Compute system mass, COM position, and COM velocity. */ +void sdsys(sdreal_t *mtoto, sdreal_t cell[3], sdreal_t cell_vel[3]); + +/* ======================================================================== + * KINEMATIC INFORMATION + * ======================================================================== */ + +/* Return position of a body-fixed point in the ground frame. */ +void sdpos(int body, sdreal_t point[3], sdreal_t loc[3]); + +/* Return velocity of a body-fixed point in the ground frame. */ +void sdvel(int body, sdreal_t point[3], sdreal_t vel[3]); + +/* Return acceleration of a body-fixed point in the ground frame. */ +void sdacc(int body, sdreal_t point[3], sdreal_t acc[3]); + +/* Return orientation (direction cosine matrix) of body w.r.t. ground. */ +void sdorient(int body, sdreal_t dircos[3][3]); + +/* Return angular velocity of body in the ground frame. */ +void sdangvel(int body, sdreal_t angvel[3]); + +/* Return angular acceleration of body in the ground frame. */ +void sdangacc(int body, sdreal_t aacc[3]); + +/* Transform a vector from one body's frame to another's. */ +void sdtrans(int frbod, sdreal_t ivec[3], int tobod, sdreal_t ovec[3]); + +/* Compute Cartesian velocity change due to a coordinate change. */ +void sdrel2cart(int coord, int body, sdreal_t point[3], + sdreal_t linchg[3], sdreal_t rotchg[3]); + +/* ======================================================================== + * DIRECTION COSINE / ANGLE / QUATERNION CONVERSIONS + * ======================================================================== */ + +/* Convert direction cosine matrix to 1-2-3 Euler angles. */ +void sddc2ang(sdreal_t dircos[3][3], + sdreal_t *a1, sdreal_t *a2, sdreal_t *a3); + +/* Convert direction cosine matrix to Euler parameters (quaternion). */ +void sddc2quat(sdreal_t dircos[3][3], + sdreal_t *e1, sdreal_t *e2, sdreal_t *e3, sdreal_t *e4); + +/* Convert 1-2-3 Euler angles to direction cosine matrix. */ +void sdang2dc(sdreal_t a1, sdreal_t a2, sdreal_t a3, + sdreal_t dircos[3][3]); + +/* Convert Euler parameters (quaternion) to direction cosine matrix. */ +void sdquat2dc(sdreal_t ie1, sdreal_t ie2, sdreal_t ie3, sdreal_t ie4, + sdreal_t dircos[3][3]); + +/* ======================================================================== + * BODY/JOINT PARAMETER SETTING ROUTINES + * ======================================================================== */ + +/* Set the gravity vector. */ +void sdgrav(sdreal_t gravin[3]); + +/* Set the mass of a body. */ +void sdmass(int body, sdreal_t mass); + +/* Set the inertia of a body (3x3 matrix about COM in body frame). */ +void sdiner(int body, sdreal_t iner[3][3]); + +/* Set body-to-joint vector for a tree joint. */ +void sdbtj(int joint, sdreal_t btj[3]); + +/* Set inboard-to-joint vector for a tree joint. */ +void sditj(int joint, sdreal_t itj[3]); + +/* Set a pin (axis) vector for a tree joint. */ +void sdpin(int joint, int pinno, sdreal_t pin[3]); + +/* Enable/disable prescribed motion for a joint axis. */ +void sdpres(int joint, int axis, int predession); + +/* Notify SD/FAST that constraint configuration has changed. */ +void sdconschg(void); + +/* ======================================================================== + * BODY/JOINT PARAMETER GETTING ROUTINES + * ======================================================================== */ + +/* Get the gravity vector. */ +void sdgetgrav(sdreal_t gravout[3]); + +/* Get the mass of a body. */ +void sdgetmass(int body, sdreal_t *massout); + +/* Get the inertia of a body. */ +void sdgetiner(int body, sdreal_t iner[3][3]); + +/* Get body-to-joint vector for a tree joint. */ +void sdgetbtj(int joint, sdreal_t btj[3]); + +/* Get inboard-to-joint vector for a tree joint. */ +void sdgetitj(int joint, sdreal_t itj[3]); + +/* Get a pin (axis) vector for a tree joint. */ +void sdgetpin(int joint, int pinno, sdreal_t pin[3]); + +/* Get prescribed motion status of a joint axis. */ +void sdgetpres(int joint, int axis, int *presout); + +/* ======================================================================== + * PRESCRIBED MOTION + * ======================================================================== */ + +/* Set prescribed position for a joint axis. */ +void sdprespos(int joint, int axis, sdreal_t pos); + +/* Set prescribed velocity for a joint axis. */ +void sdpresvel(int joint, int axis, sdreal_t vel); + +/* Set prescribed acceleration for a joint axis. */ +void sdpresacc(int joint, int axis, sdreal_t acc); + +/* ======================================================================== + * APPLIED LOADS + * ======================================================================== */ + +/* Get the current hinge torque/force for a joint axis. */ +void sdgetht(int joint, int axis, sdreal_t *torque); + +/* Apply a hinge torque/force to a joint axis. */ +void sdhinget(int joint, int axis, sdreal_t torque); + +/* Apply a force at a point on a body. */ +void sdpointf(int body, sdreal_t point[3], sdreal_t force[3]); + +/* Apply a torque to a body. */ +void sdbodyt(int body, sdreal_t torque[3]); + +/* ======================================================================== + * TOPOLOGY AND SYSTEM INFORMATION + * ======================================================================== */ + +/* Return the state vector index for a joint axis. */ +int sdindx(int joint, int axis); + +/* Return system-wide information in a 50-element array. */ +void sdinfo(int info[50]); + +/* Return joint information. */ +void sdjnt(int joint, int info[50]); + +/* Return constraint information. */ +void sdcons(int consno, int info[50]); + +/* ======================================================================== + * ERROR HANDLING + * ======================================================================== */ + +/* Return the last error number and routine number. */ +void sderror(int *routine, int *errnum); + +/* Print accumulated error messages to a file. */ +void sdprinterr(FILE *fnum); + +/* Clear the error flag. */ +void sdclearerr(void); + +/* Set an error (for user use). */ +void sdseterr(int routine, int errnum); + +/* ======================================================================== + * SERIAL NUMBER AND GENERATION TIME + * ======================================================================== */ + +/* Return the SD/FAST serial number. */ +void sdserialno(int *serno); + +/* Return the generation time stamp. */ +void sdgentime(int *gentm); + +/* ======================================================================== + * INTEGRATION ROUTINES (from sdlib) + * ======================================================================== */ + +/* Variable-step integrator with event detection. */ +void sdvinteg(int (*f)(sdreal_t, sdreal_t[], sdreal_t[], sdreal_t[], int*), + sdreal_t *t, sdreal_t y[], sdreal_t dy[], sdreal_t param[], + sdreal_t dt, sdreal_t *step, int neq, sdreal_t tol, + sdreal_t work[], int *err, int *which); + +/* Fixed-step Runge-Kutta integrator. */ +void sdfinteg(int (*f)(sdreal_t, sdreal_t[], sdreal_t[], sdreal_t[], int*), + sdreal_t *t, sdreal_t y[], sdreal_t dy[], sdreal_t param[], + sdreal_t dt, int neq, sdreal_t work[], sdreal_t *errest, + int *fstatus); + +/* Nonlinear root finder. */ +void sdroot(int (*f)(sdreal_t[], sdreal_t[], sdreal_t[]), + sdreal_t vars[], sdreal_t param[], + int nvar, int nfunc, int ndes, int lock[], + sdreal_t rtol, sdreal_t dtol, int maxeval, + sdreal_t jw[], sdreal_t dw[], sdreal_t rw[], int iw[], + sdreal_t fret[], int *fcnt, int *err); + +/* Linear least-squares solver. */ +void sdlsslv(int nr, int nc, int nra, int nca, int ndes, + int mapr[], int mapc[], sdreal_t tol, + sdreal_t dw[], sdreal_t rw[], int iw[], + sdreal_t *w, sdreal_t b[], sdreal_t x[]); + +/* ======================================================================== + * SIMPLIFIED ANALYSIS ROUTINES + * ======================================================================== */ + +/* Assembly analysis: find coordinates satisfying constraints. */ +void sdassemble(sdreal_t time, sdreal_t state[], int lock[], + sdreal_t tol, int maxevals, int *fcnt, int *err); + +/* Velocity analysis: find velocities satisfying velocity constraints. */ +void sdinitvel(sdreal_t time, sdreal_t state[], int lock[], + sdreal_t tol, int maxevals, int *fcnt, int *err); + +/* Static analysis: find equilibrium configuration. */ +void sdstatic(sdreal_t time, sdreal_t state[], int lock[], + sdreal_t ctol, sdreal_t tol, + int maxevals, int *fcnt, int *err); + +/* Steady motion analysis. */ +void sdsteady(sdreal_t time, sdreal_t state[], int lock[], + sdreal_t ctol, sdreal_t tol, + int maxevals, int *fcnt, int *err); + +/* Variable-step motion (dynamic) analysis. */ +void sdmotion(sdreal_t *time, sdreal_t state[], sdreal_t dstate[], + sdreal_t dt, sdreal_t ctol, sdreal_t tol, + int *flag, int *err); + +/* Fixed-step motion (dynamic) analysis. */ +void sdfmotion(sdreal_t *time, sdreal_t state[], sdreal_t dstate[], + sdreal_t dt, sdreal_t ctol, + int *flag, sdreal_t *errest, int *err); + +/* ======================================================================== + * USER-SUPPLIED CALLBACK FUNCTIONS FOR SIMPLIFIED ANALYSIS + * These are called internally by the simplified analysis routines. + * The user provides implementations; they are model-specific. + * ======================================================================== */ + +/* User-supplied position function for assembly. */ +void sdposfunc(sdreal_t vars[], sdreal_t param[], sdreal_t resid[]); + +/* User-supplied velocity function for velocity analysis. */ +void sdvelfunc(sdreal_t vars[], sdreal_t param[], sdreal_t resid[]); + +/* User-supplied static analysis function. */ +void sdstatfunc(sdreal_t vars[], sdreal_t param[], sdreal_t resid[]); + +/* User-supplied steady motion function. */ +void sdstdyfunc(sdreal_t vars[], sdreal_t param[], sdreal_t resid[]); + +/* User-supplied motion derivative function. */ +void sdmotfunc(sdreal_t time, sdreal_t state[], sdreal_t dstate[], + sdreal_t param[], int *status); + +/* ======================================================================== + * VECTOR UTILITY ROUTINES (from sdlib) + * ======================================================================== */ + +/* Copy a 3-vector. */ +void sdvcopy(sdreal_t ivec[3], sdreal_t ovec[3]); + +/* Set a 3-vector from three scalars. */ +void sdvset(sdreal_t s1, sdreal_t s2, sdreal_t s3, sdreal_t ovec[3]); + +/* Add two 3-vectors: ovec = ivec1 + ivec2. */ +void sdvadd(sdreal_t ivec1[3], sdreal_t ivec2[3], sdreal_t ovec[3]); + +/* Subtract two 3-vectors: diff = ivec1 - ivec2. */ +void sdvsub(sdreal_t ivec1[3], sdreal_t ivec2[3], sdreal_t diff[3]); + +/* Multiply a 3-vector by a scalar: ovec = sclr * ivec. */ +void sdvmul(sdreal_t sclr, sdreal_t ivec[3], sdreal_t ovec[3]); + +/* Scalar * vector + vector: ovec = sclr*ivec1 + ivec2. */ +void sdvaxpy(sdreal_t sclr, sdreal_t ivec1[3], sdreal_t ivec2[3], + sdreal_t ovec[3]); + +/* Cross product: ovec = ivec1 x ivec2. */ +void sdvcross(sdreal_t ivec1[3], sdreal_t ivec2[3], sdreal_t ovec[3]); + +/* Dot product of two 3-vectors. */ +sdreal_t sdvdot(sdreal_t ivec1[3], sdreal_t ivec2[3]); + +/* Norm (length) of a 3-vector. */ +sdreal_t sdvnorm(sdreal_t ivec[3]); + +/* Rotate a vector about an axis: ovec = rotate(ivec, rvec, theta). */ +void sdvrot(sdreal_t ivec[3], sdreal_t rvec[3], sdreal_t theta, + sdreal_t ovec[3]); + +/* ======================================================================== + * USER-WRITTEN ROUTINES + * + * These must be supplied by the user. They are called by SD/FAST's + * generated code during derivative evaluation. + * ======================================================================== */ + +/* Apply user forces and torques. Called during derivative computation. */ +void sduforce(sdreal_t t, sdreal_t q[], sdreal_t u[]); + +/* Specify prescribed motion values. Called during derivative computation. */ +void sdumotion(sdreal_t t, sdreal_t q[], sdreal_t u[]); + +/* User derivative routine (for use with general analysis routines). */ +void sduderiv(sdreal_t t, sdreal_t state[], sdreal_t dstate[], + sdreal_t param[], int *status); + +/* User evaluation routine (for use with root finder). */ +void sdueval(sdreal_t vars[], sdreal_t param[], sdreal_t resid[]); + +/* User constraint position errors (for user constraints). */ +void sduperr(sdreal_t t, sdreal_t q[], sdreal_t errs[]); + +/* User constraint velocity errors (for user constraints). */ +void sduverr(sdreal_t t, sdreal_t q[], sdreal_t u[], sdreal_t errs[]); + +/* User constraint acceleration errors (for user constraints). */ +void sduaerr(sdreal_t t, sdreal_t q[], sdreal_t u[], sdreal_t udot[], + sdreal_t errs[]); + +/* Apply user constraint forces. */ +void sduconsfrc(sdreal_t t, sdreal_t q[], sdreal_t u[], sdreal_t mults[]); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif /* SDFAST_API_H_ */ +