ROBOOP, A Robotics Object Oriented Package in C++: utils.h File Reference

#include <stdio.h>
#include <limits>
#include "newmatap.h"
#include "newmatio.h"


Defines
#define	WANT_STRING
#define	WANT_STREAM
#define	WANT_FSTREAM
#define	WANT_MATH
#define	M_PI 3.14159265358979
#define	GRAVITY 9.81
Functions
double	deg2rad (const double angle_deg)
double	rad2deg (const double angle_rad)
ReturnMatrix	x_prod_matrix (const ColumnVector &x)
	Cross product matrix.
ReturnMatrix	pinv (const Matrix &M)
	Matrix pseudo inverse using SVD.
ReturnMatrix	Integ_Trap (const ColumnVector &present, ColumnVector &past, const Real dt)
	Trapezoidal integration.
void	Runge_Kutta4 (ReturnMatrix(*xdot)(Real time, const Matrix &xin), const Matrix &xo, Real to, Real tf, int nsteps, RowVector &tout, Matrix &xout)
	Fixed step size fourth-order Runge-Kutta integrator.
void	Runge_Kutta4_Real_time (ReturnMatrix(*xdot)(Real time, const Matrix &xin), const Matrix &xo, Real to, Real tf, int nsteps)
void	Runge_Kutta4_Real_time (ReturnMatrix(*xdot)(Real time, const Matrix &xin, bool &exit, bool &init), const Matrix &xo, Real to, Real tf, int nsteps)
	Fixed step size fourth-order Runge-Kutta integrator.
void	odeint (ReturnMatrix(*xdot)(Real time, const Matrix &xin), Matrix &xo, Real to, Real tf, Real eps, Real h1, Real hmin, int &nok, int &nbad, RowVector &tout, Matrix &xout, Real dtsav)
	Integrate the ordinary differential equation xdot from time to to time tf using an adaptive step size strategy.
ReturnMatrix	sign (const Matrix &x)
	Sign of a matrix.
short	sign (const Real x)
	Sign of real.
bool	isZero (const double x)
ReturnMatrix	trans (const ColumnVector &a)
	Translation.
ReturnMatrix	rotx (const Real alpha)
	Rotation around x axis.
ReturnMatrix	roty (const Real beta)
	Rotation around x axis.
ReturnMatrix	rotz (const Real gamma)
	Rotation around z axis.
ReturnMatrix	rotk (const Real theta, const ColumnVector &k)
	Rotation around arbitrary axis.
ReturnMatrix	rpy (const ColumnVector &a)
	Roll Pitch Yaw rotation.
ReturnMatrix	eulzxz (const ColumnVector &a)
	Euler ZXZ rotation.
ReturnMatrix	rotd (const Real theta, const ColumnVector &k1, const ColumnVector &k2)
	Rotation around an arbitrary line.
ReturnMatrix	irotk (const Matrix &R)
	Obtain axis from a rotation matrix.
ReturnMatrix	irpy (const Matrix &R)
	Obtain Roll, Pitch and Yaw from a rotation matrix.
ReturnMatrix	ieulzxz (const Matrix &R)
	Obtain Roll, Pitch and Yaw from a rotation matrix.
Variables
static const char	header_utils_rcsid [] = "$Id: utils.h,v 1.10 2006/05/16 16:11:15 gourdeau Exp $"
	RCS/CVS version.
Real	fourbyfourident []
	Used to initialize a $4\times 4$ matrix.
Real	threebythreeident []
	Used to initialize a $3\times 3$ matrix.
const double	epsilon = 0.0000001

Function Documentation

void odeint	(	ReturnMatrix(*)(Real time, const Matrix &xin)	xdot,
		Matrix &	xo,
		Real	to,
		Real	tf,
		Real	eps,
		Real	h1,
		Real	hmin,
		int &	nok,
		int &	nbad,
		RowVector &	tout,
		Matrix &	xout,
		Real	dtsav
	)

Integrate the ordinary differential equation xdot from time to to time tf using an adaptive step size strategy.

adapted from: Numerical Recipes in C, The Art of Scientific Computing, Press, William H. and Flannery, Brian P. and Teukolsky, Saul A. and Vetterling, William T., Cambridge University Press, 1988.

Definition at line 347 of file utils.cpp.

References MAXSTP, rkqc(), and TINY.

Referenced by dynamics_demo().

ReturnMatrix pinv ( const Matrix & M )

Matrix pseudo inverse using SVD.

If $A = U^{*}QV$ is a singular value decomposition of A, then $A^{\dagger} = V^{*}Q^{\dagger}U$ where $X^{*}$ is the conjugate transpose of $ X $ and $Q^{\dagger} = \left [ \begin{array}{cccc} 1/\sigma_1 & & & \\ & 1/\sigma_2& & \\ & & \ddots & \\ & & & 0 \end{array} \right ]$ where the $1/\sigma_i$ are replaced by 0 when $1/\sigma_i < tol$

Definition at line 99 of file utils.cpp.

References epsilon, and pinv().

Referenced by pinv().

utils.h File Reference

Detailed Description

Defines

Functions

Variables

Function Documentation