Matrix and general kinematics

Functions
void	c_matmul (int m, int n, int k, double alpha, const double *a, int lda, const double *b, int ldb, double beta, double *c, int ldc)
	Multiply two column-major matrices: c = alpha*a*b + beta*c.
double	c_q_factor (double zeta)
	Compute the Q factor from a damping ratio.
double	c_estimate_bandwidth (double fn, double zeta)
	Estimate the half-power bandwidth.
double	c_logarithmic_decrement (double x1, double x2, int n)
	Compute logarithmic decrement from two peaks.
double	c_damping_from_log_decrement (double delta)
	Convert logarithmic decrement to damping ratio.
void	c_find_free_response_properties (int n, const double *t, const double *x, double s, int np, double *delta, double *fn, double *x1, double *x2, double *t1, double *t2)
	Extract damping and frequency properties from a free response.
double	c_rise_time (double wn, double zeta)
	Compute the 10-to-90 percent rise time.
double	c_find_settling_amplitude (int n, const double *x)
	Find the settled response amplitude.
double	c_damping_from_fractional_overshoot (int n, const double *x)
	Estimate damping from fractional overshoot.
void	c_evaluate_step_response (int n, double wn, double zeta, double xs, const double *t, double *x)
	Evaluate a second-order step response.
void	c_rotate_x (double angle, double *r, int ldr)
	Build a rotation matrix about the x axis.
void	c_rotate_y (double angle, double *r, int ldr)
	Build a rotation matrix about the y axis.
void	c_rotate_z (double angle, double *r, int ldr)
	Build a rotation matrix about the z axis.
void	c_rotate (const double i[3], const double j[3], const double k[3], double *r, int ldr)
	Build a rotation matrix from three basis vectors.
void	c_acceleration_transform (const double alpha[3], const double omega[3], const double a[3], const double x[3], double *r, int ldr)
	Build an acceleration transformation matrix.
void	c_velocity_transform (const double omega[3], const double v[3], const double x[3], double *r, int ldr)
	Build a velocity transformation matrix.
void	c_determine_local_stability (int n, const double *a, int lda, double complex *ev, int *flag)
	Classify the eigenvalues of a linearized system for local stability.
void	c_dh_forward_kinematics_table (const c_dh_table *tbl, double *T, int ldt)
	Compute forward kinematics from a Denavit-Hartenberg table.
void	c_dh_forward_kinematics (int n, const double *alpha, const double *a, const double *theta, const double *d, double *T, int ldt)
	Compute serial Denavit-Hartenberg forward kinematics.
void	c_dh_forward_kinematics_2 (const double *T1, int ldt1, const double *T2, int ldt2, double *T, int ldt)
	Multiply two homogeneous transforms.
void	c_dh_forward_kinematics_3 (const double *T1, int ldt1, const double *T2, int ldt2, const double *T3, int ldt3, double *T, int ldt)
	Multiply three homogeneous transforms.
void	c_dh_forward_kinematics_4 (const double *T1, int ldt1, const double *T2, int ldt2, const double *T3, int ldt3, const double *T4, double *T, int ldt)
	Multiply four homogeneous transforms.
void	c_dh_forward_kinematics_5 (const double *T1, int ldt1, const double *T2, int ldt2, const double *T3, int ldt3, const double *T4, int ldt4, const double *T5, int ldt5, double *T, int ldt)
	Multiply five homogeneous transforms.
void	c_dh_forward_kinematics_6 (const double *T1, int ldt1, const double *T2, int ldt2, const double *T3, int ldt3, const double *T4, int ldt4, const double *T5, int ldt5, const double *T6, int ldt6, double *T, int ldt)
	Multiply six homogeneous transforms.
void	c_dh_forward_kinematics_7 (const double *T1, int ldt1, const double *T2, int ldt2, const double *T3, int ldt3, const double *T4, int ldt4, const double *T5, int ldt5, const double *T6, int ldt6, const double *T7, int ldt7, double *T, int ldt)
	Multiply seven homogeneous transforms.
void	c_dh_forward_kinematics_8 (const double *T1, int ldt1, const double *T2, int ldt2, const double *T3, int ldt3, const double *T4, int ldt4, const double *T5, int ldt5, const double *T6, int ldt6, const double *T7, int ldt7, const double *T8, int ldt8, double *T, int ldt)
	Multiply eight homogeneous transforms.
void	c_dh_jacobian (int n, const double *alpha, const double *a, const double *theta, const double *d, const int *jtypes, double *jac, int ldjac)
	Compute a Denavit-Hartenberg linkage Jacobian.
void	c_dh_matrix (double alpha, double a, double theta, double d, double *T, int ldt)
	Build one Denavit-Hartenberg homogeneous transform.
void	c_dh_rotate_x (double alpha, double *T, int ldt)
	Build the Denavit-Hartenberg x-axis rotation matrix.
void	c_dh_rotate_z (double theta, double *T, int ldt)
	Build the Denavit-Hartenberg z-axis rotation matrix.
void	c_dh_translate_x (double a, double *T, int ldt)
	Build the Denavit-Hartenberg x translation matrix.
void	c_dh_translate_z (double d, double *T, int ldt)
	Build the Denavit-Hartenberg z translation matrix.
void	c_jacobian_generating_vector (const double *d, const double *k, const double *R, int ldr, int jtype, double jvec[6])
	Generate one joint Jacobian column.
void	c_solve_inverse_kinematics (int njoints, int neqn, const c_vecfcn mdl, const double *qo, const double *constraints, const double *qmax, const double *qmin, double *jvar, double *resid, c_iteration_behavior *ib, void *user_data)
	Solve a nonlinear inverse-kinematics problem.
void	c_to_angle_axis (const double *r, int ldr, double *angle, double axis[3])
	Convert a rotation matrix to angle-axis form.

Detailed Description

Function Documentation

c_matmul()

void c_matmul	(	int	m,
		int	n,
		int	k,
		double	alpha,
		const double *	a,
		int	lda,
		const double *	b,
		int	ldb,
		double	beta,
		double *	c,
		int	ldc
	)

Multiply two column-major matrices: c = alpha*a*b + beta*c.

Parameters

m	Rows of a and c.
n	Columns of b and c.
k	Inner dimension.
alpha	Scale factor for a*b.
a	First matrix.
lda	Leading dimension of a.
b	Second matrix.
ldb	Leading dimension of b.
beta	Scale factor for c.
c	Matrix updated in place.
ldc	Leading dimension of c.

c_q_factor()

double c_q_factor

(

double

zeta

)

Compute the Q factor from a damping ratio.

Parameters

zeta	Damping ratio.

Returns

Q factor.

c_estimate_bandwidth()

double c_estimate_bandwidth	(	double	fn,
		double	zeta
	)

Estimate the half-power bandwidth.

Parameters

fn	Natural frequency.
zeta	Damping ratio.

Returns

Bandwidth.

c_logarithmic_decrement()

double c_logarithmic_decrement	(	double	x1,
		double	x2,
		int	n
	)

Compute logarithmic decrement from two peaks.

Parameters

x1	First peak.
x2	Second peak.
n	Peak separation in cycles.

Returns

Logarithmic decrement.

c_damping_from_log_decrement()

double c_damping_from_log_decrement

(

double

delta

)

Convert logarithmic decrement to damping ratio.

Parameters

delta

Logarithmic decrement.

Returns

Damping ratio.

c_find_free_response_properties()

void c_find_free_response_properties	(	int	n,
		const double *	t,
		const double *	x,
		double	s,
		int	np,
		double *	delta,
		double *	fn,
		double *	x1,
		double *	x2,
		double *	t1,
		double *	t2
	)

Extract damping and frequency properties from a free response.

Parameters

n	Sample count.
t	Sample times.
x	Response samples.
s	Settling threshold.
np	Number of periods to use.
delta	Output logarithmic decrement.
fn	Output natural frequency.
x1	Output first peak.
x2	Output second peak.
t1	Output first peak time.
t2	Output second peak time.

c_rise_time()

double c_rise_time	(	double	wn,
		double	zeta
	)

Compute the 10-to-90 percent rise time.

Parameters

wn	Undamped natural frequency.
zeta	Damping ratio.

Returns

Rise time.

c_find_settling_amplitude()

double c_find_settling_amplitude	(	int	n,
		const double *	x
	)

Find the settled response amplitude.

Parameters

n	Sample count.
x	Response samples.

Returns

Settling amplitude.

c_damping_from_fractional_overshoot()

double c_damping_from_fractional_overshoot	(	int	n,
		const double *	x
	)

Estimate damping from fractional overshoot.

Parameters

n	Sample count.
x	Response samples.

Returns

Damping ratio.

c_evaluate_step_response()

void c_evaluate_step_response	(	int	n,
		double	wn,
		double	zeta,
		double	xs,
		const double *	t,
		double *	x
	)

Evaluate a second-order step response.

Parameters

n	Sample count.
wn	Undamped natural frequency.
zeta	Damping ratio.
xs	Step amplitude.
t	Sample times.
x	Output response samples.

c_rotate_x()

void c_rotate_x	(	double	angle,
		double *	r,
		int	ldr
	)

Build a rotation matrix about the x axis.

Parameters

angle	Rotation angle in radians.
r	Output 3-by-3 matrix.
ldr	Leading dimension of r.

c_rotate_y()

void c_rotate_y	(	double	angle,
		double *	r,
		int	ldr
	)

Build a rotation matrix about the y axis.

Parameters

angle	Rotation angle in radians.
r	Output 3-by-3 matrix.
ldr	Leading dimension of r.

c_rotate_z()

void c_rotate_z	(	double	angle,
		double *	r,
		int	ldr
	)

Build a rotation matrix about the z axis.

Parameters

angle	Rotation angle in radians.
r	Output 3-by-3 matrix.
ldr	Leading dimension of r.

c_rotate()

void c_rotate	(	const double	i[3],
		const double	j[3],
		const double	k[3],
		double *	r,
		int	ldr
	)

Build a rotation matrix from three basis vectors.

Parameters

i	First basis vector.
j	Second basis vector.
k	Third basis vector.
r	Output matrix.
ldr	Leading dimension of r.

c_acceleration_transform()

void c_acceleration_transform	(	const double	alpha[3],
		const double	omega[3],
		const double	a[3],
		const double	x[3],
		double *	r,
		int	ldr
	)

Build an acceleration transformation matrix.

Parameters

alpha	Angular acceleration.
omega	Angular velocity.
a	Linear acceleration.
x	Position vector.
r	Output matrix.
ldr	Leading dimension of r.

c_velocity_transform()

void c_velocity_transform	(	const double	omega[3],
		const double	v[3],
		const double	x[3],
		double *	r,
		int	ldr
	)

Build a velocity transformation matrix.

Parameters

omega	Angular velocity.
v	Linear velocity.
x	Position vector.
r	Output matrix.
ldr	Leading dimension of r.

c_determine_local_stability()

void c_determine_local_stability	(	int	n,
		const double *	a,
		int	lda,
		double complex *	ev,
		int *	flag
	)

Classify the eigenvalues of a linearized system for local stability.

Parameters

n	Matrix order.
a	System matrix.
lda	Leading dimension of a.
ev	Output eigenvalues.
flag	Output stability classification.

c_dh_forward_kinematics_table()

void c_dh_forward_kinematics_table	(	const c_dh_table *	tbl,
		double *	T,
		int	ldt
	)

Compute forward kinematics from a Denavit-Hartenberg table.

Parameters

tbl	Parameter table.
T	Output transform.
ldt	Leading dimension of T.

c_dh_forward_kinematics()

void c_dh_forward_kinematics	(	int	n,
		const double *	alpha,
		const double *	a,
		const double *	theta,
		const double *	d,
		double *	T,
		int	ldt
	)

Compute serial Denavit-Hartenberg forward kinematics.

Parameters

n	Number of joints.
alpha	Link twists.
a	Link lengths.
theta	Joint angles.
d	Link offsets.
T	Output transform.
ldt	Leading dimension of T.

c_dh_forward_kinematics_2()

void c_dh_forward_kinematics_2	(	const double *	T1,
		int	ldt1,
		const double *	T2,
		int	ldt2,
		double *	T,
		int	ldt
	)

Multiply two homogeneous transforms.

Parameters

T1	First transform.
ldt1	Leading dimension of T1.
T2	Second transform.
ldt2	Leading dimension of T2.
T	Output transform.
ldt	Leading dimension of T.

c_dh_forward_kinematics_3()

void c_dh_forward_kinematics_3	(	const double *	T1,
		int	ldt1,
		const double *	T2,
		int	ldt2,
		const double *	T3,
		int	ldt3,
		double *	T,
		int	ldt
	)

Multiply three homogeneous transforms.

Parameters

T1	First transform.
ldt1	Leading dimension of T1.
T2	Second transform.
ldt2	Leading dimension of T2.
T3	Third transform.
ldt3	Leading dimension of T3.
T	Output transform.
ldt	Leading dimension of T.

c_dh_forward_kinematics_4()

void c_dh_forward_kinematics_4	(	const double *	T1,
		int	ldt1,
		const double *	T2,
		int	ldt2,
		const double *	T3,
		int	ldt3,
		const double *	T4,
		double *	T,
		int	ldt
	)

Multiply four homogeneous transforms.

Arguments follow c_dh_forward_kinematics_3 and add T4/ldt4.

c_dh_forward_kinematics_5()

void c_dh_forward_kinematics_5	(	const double *	T1,
		int	ldt1,
		const double *	T2,
		int	ldt2,
		const double *	T3,
		int	ldt3,
		const double *	T4,
		int	ldt4,
		const double *	T5,
		int	ldt5,
		double *	T,
		int	ldt
	)

Multiply five homogeneous transforms.

Arguments follow c_dh_forward_kinematics_3 and add T4/ldt4 and T5/ldt5.

c_dh_forward_kinematics_6()

void c_dh_forward_kinematics_6	(	const double *	T1,
		int	ldt1,
		const double *	T2,
		int	ldt2,
		const double *	T3,
		int	ldt3,
		const double *	T4,
		int	ldt4,
		const double *	T5,
		int	ldt5,
		const double *	T6,
		int	ldt6,
		double *	T,
		int	ldt
	)

Multiply six homogeneous transforms.

Arguments follow c_dh_forward_kinematics_3 and add T4/ldt4, T5/ldt5, and T6/ldt6.

c_dh_forward_kinematics_7()

void c_dh_forward_kinematics_7	(	const double *	T1,
		int	ldt1,
		const double *	T2,
		int	ldt2,
		const double *	T3,
		int	ldt3,
		const double *	T4,
		int	ldt4,
		const double *	T5,
		int	ldt5,
		const double *	T6,
		int	ldt6,
		const double *	T7,
		int	ldt7,
		double *	T,
		int	ldt
	)

Multiply seven homogeneous transforms.

Arguments follow c_dh_forward_kinematics_3 and add T4/ldt4 through T7/ldt7.

c_dh_forward_kinematics_8()

void c_dh_forward_kinematics_8	(	const double *	T1,
		int	ldt1,
		const double *	T2,
		int	ldt2,
		const double *	T3,
		int	ldt3,
		const double *	T4,
		int	ldt4,
		const double *	T5,
		int	ldt5,
		const double *	T6,
		int	ldt6,
		const double *	T7,
		int	ldt7,
		const double *	T8,
		int	ldt8,
		double *	T,
		int	ldt
	)

Multiply eight homogeneous transforms.

Arguments follow c_dh_forward_kinematics_3 and add T4/ldt4 through T8/ldt8.

c_dh_jacobian()

void c_dh_jacobian	(	int	n,
		const double *	alpha,
		const double *	a,
		const double *	theta,
		const double *	d,
		const int *	jtypes,
		double *	jac,
		int	ldjac
	)

Compute a Denavit-Hartenberg linkage Jacobian.

Parameters

n	Joint count.
alpha	Link twists.
a	Link lengths.
theta	Joint angles.
d	Link offsets.
jtypes	Joint types.
jac	Output Jacobian.
ldjac	Leading dimension of jac.

c_dh_matrix()

void c_dh_matrix	(	double	alpha,
		double	a,
		double	theta,
		double	d,
		double *	T,
		int	ldt
	)

Build one Denavit-Hartenberg homogeneous transform.

Parameters

alpha	Link twist.
a	Link length.
theta	Joint angle.
d	Link offset.
T	Output transform.
ldt	Leading dimension of T.

c_dh_rotate_x()

void c_dh_rotate_x	(	double	alpha,
		double *	T,
		int	ldt
	)

Build the Denavit-Hartenberg x-axis rotation matrix.

Parameters

alpha	Rotation angle.
T	Output matrix.
ldt	Leading dimension of T.

c_dh_rotate_z()

void c_dh_rotate_z	(	double	theta,
		double *	T,
		int	ldt
	)

Build the Denavit-Hartenberg z-axis rotation matrix.

Parameters

theta	Rotation angle.
T	Output matrix.
ldt	Leading dimension of T.

c_dh_translate_x()

void c_dh_translate_x	(	double	a,
		double *	T,
		int	ldt
	)

Build the Denavit-Hartenberg x translation matrix.

Parameters

a	Translation distance.
T	Output matrix.
ldt	Leading dimension of T.

c_dh_translate_z()

void c_dh_translate_z	(	double	d,
		double *	T,
		int	ldt
	)

Build the Denavit-Hartenberg z translation matrix.

Parameters

d	Translation distance.
T	Output matrix.
ldt	Leading dimension of T.

c_jacobian_generating_vector()

void c_jacobian_generating_vector	(	const double *	d,
		const double *	k,
		const double *	R,
		int	ldr,
		int	jtype,
		double	jvec[6]
	)

Generate one joint Jacobian column.

Parameters

d	Joint position vector.
k	Joint axis.
R	Rotation matrix.
ldr	Leading dimension of R.
jtype	Joint type.
jvec	Output six-vector.

c_solve_inverse_kinematics()

void c_solve_inverse_kinematics	(	int	njoints,
		int	neqn,
		const c_vecfcn	mdl,
		const double *	qo,
		const double *	constraints,
		const double *	qmax,
		const double *	qmin,
		double *	jvar,
		double *	resid,
		c_iteration_behavior *	ib,
		void *	user_data
	)

Solve a nonlinear inverse-kinematics problem.

Parameters

njoints	Number of joint variables.
neqn	Number of equations.
mdl	Residual callback.
qo	Initial joint variables.
constraints	Constraint values.
qmax	Upper bounds.
qmin	Lower bounds.
jvar	Output joint variables.
resid	Output residual.
ib	Output iteration statistics.
user_data	Opaque caller data forwarded to mdl.

c_to_angle_axis()

void c_to_angle_axis	(	const double *	r,
		int	ldr,
		double *	angle,
		double	axis[3]
	)

Convert a rotation matrix to angle-axis form.

Parameters

r	Rotation matrix.
ldr	Leading dimension of r.
angle	Output angle in radians.
axis	Output unit axis.