526 lines
18 KiB
C++
526 lines
18 KiB
C++
#ifndef SimTK_SimTKCOMMON_FUNCTION_H_
|
|
#define SimTK_SimTKCOMMON_FUNCTION_H_
|
|
|
|
/* -------------------------------------------------------------------------- *
|
|
* Simbody(tm): SimTKcommon *
|
|
* -------------------------------------------------------------------------- *
|
|
* This is part of the SimTK biosimulation toolkit originating from *
|
|
* Simbios, the NIH National Center for Physics-Based Simulation of *
|
|
* Biological Structures at Stanford, funded under the NIH Roadmap for *
|
|
* Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
|
|
* *
|
|
* Portions copyright (c) 2008-12 Stanford University and the Authors. *
|
|
* Authors: Peter Eastman *
|
|
* Contributors: 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. *
|
|
* *
|
|
* Unless required by applicable law or agreed to in writing, software *
|
|
* distributed under the License is distributed on an "AS IS" BASIS, *
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
|
|
* See the License for the specific language governing permissions and *
|
|
* limitations under the License. *
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
// Note: this file was moved from Simmath to SimTKcommon 20100601; see the
|
|
// Simmath repository for earlier history.
|
|
|
|
//#include "SimTKcommon/basics.h"
|
|
//#include "SimTKcommon/Simmatrix.h"
|
|
/*
|
|
Update:
|
|
This is a port of the original code so that it will work with
|
|
the multibody code RBDL written by Martin Felis.
|
|
|
|
Author:
|
|
Matthew Millard
|
|
|
|
Date:
|
|
Nov 2015
|
|
|
|
*/
|
|
|
|
#include <cassert>
|
|
#include <rbdl/rbdl_math.h>
|
|
#include <vector>
|
|
#include <cmath>
|
|
#include <cstdio>
|
|
/**
|
|
This abstract class represents a mathematical function that calculates a
|
|
value of arbitrary type based on M real arguments. The output type is set
|
|
as a template argument, while the number of input components may be
|
|
determined at runtime. The name "Function" (with no trailing _) may be
|
|
used as a synonym for Function_<double>.
|
|
|
|
Subclasses define particular mathematical functions. Predefined subclasses
|
|
are provided for several common function types: Function_<T>::Constant,
|
|
Function_<T>::Linear, Function_<T>::Polynomial, and Function_<T>::Step.
|
|
You can define your own subclasses for other function types. The
|
|
Spline_ class also provides a convenient way to create various types of
|
|
Functions.
|
|
*/
|
|
|
|
namespace RigidBodyDynamics {
|
|
namespace Addons {
|
|
namespace Geometry{
|
|
|
|
|
|
template <class T>
|
|
class Function_ {
|
|
public:
|
|
class Constant;
|
|
class Linear;
|
|
class Polynomial;
|
|
class Sinusoid;
|
|
class Step;
|
|
virtual ~Function_() {
|
|
}
|
|
/**
|
|
Calculate the value of this function at a particular point.
|
|
|
|
@param x the RigidBodyDynamics::Math::VectorNd of input arguments. Its
|
|
size must equal the value returned by getArgumentSize().
|
|
*/
|
|
virtual T calcValue(const RigidBodyDynamics::Math::VectorNd& x) const = 0;
|
|
/**
|
|
Calculate a partial derivative of this function at a particular point.
|
|
Which derivative to take is specified by listing the input components
|
|
with which to take it. For example, if derivComponents=={0}, that
|
|
indicates a first derivative with respective to component 0. If
|
|
derivComponents=={0, 0, 0}, that indicates a third derivative with
|
|
respective to component 0. If derivComponents=={4, 7}, that indicates a
|
|
partial second derivative with respect to components 4 and 7.
|
|
|
|
@param derivComponents
|
|
The input components with respect to which the derivative should be
|
|
taken. Its size must be less than or equal to the value returned
|
|
by getMaxDerivativeOrder().
|
|
@param x
|
|
The RigidBodyDynamics::Math::VectorNd of input arguments. Its size must
|
|
equal the value
|
|
returned by getArgumentSize().
|
|
@return
|
|
The value of the selected derivative, which is of type T.
|
|
*/
|
|
virtual T calcDerivative(const std::vector<int>& derivComponents,
|
|
const RigidBodyDynamics::Math::VectorNd& x) const = 0;
|
|
|
|
/** This provides compatibility with std::vector without requiring any
|
|
copying. **/
|
|
/*
|
|
T calcDerivative(const std::vector<int>& derivComponents,
|
|
const RigidBodyDynamics::Math::VectorNd& x) const
|
|
{ return calcDerivative(std::vector<int>(derivComponents),x); }
|
|
*/
|
|
|
|
/**
|
|
* Get the number of components expected in the input vector.
|
|
*/
|
|
virtual int getArgumentSize() const = 0;
|
|
/**
|
|
* Get the maximum derivative order this Function_ object can calculate.
|
|
*/
|
|
virtual int getMaxDerivativeOrder() const = 0;
|
|
};
|
|
|
|
/** This typedef is used for the very common case that the return type of
|
|
the Function object is double. **/
|
|
typedef Function_<double> Function;
|
|
|
|
|
|
|
|
/**
|
|
* This is a Function_ subclass which simply returns a fixed value, independent
|
|
* of its arguments.
|
|
*/
|
|
template <class T>
|
|
class Function_<T>::Constant : public Function_<T> {
|
|
public:
|
|
/**
|
|
* Create a Function_::Constant object.
|
|
*
|
|
* @param value the value which should be returned by calcValue();
|
|
* @param argumentSize the value which should be returned by
|
|
* getArgumentSize(), with a default of 1.
|
|
*/
|
|
explicit Constant(T value, int argumentSize=1)
|
|
: argumentSize(argumentSize), value(value) {
|
|
}
|
|
T calcValue(const RigidBodyDynamics::Math::VectorNd& x) const {
|
|
assert(x.size() == argumentSize);
|
|
return value;
|
|
}
|
|
T calcDerivative(const std::vector<int>& derivComponents,
|
|
const RigidBodyDynamics::Math::VectorNd& x) const {
|
|
return static_cast<T>(0);
|
|
}
|
|
virtual int getArgumentSize() const {
|
|
return argumentSize;
|
|
}
|
|
int getMaxDerivativeOrder() const {
|
|
return std::numeric_limits<int>::max();
|
|
}
|
|
|
|
/** This provides compatibility with std::vector without requiring any
|
|
copying. **/
|
|
/*
|
|
T calcDerivative(const std::vector<int>& derivComponents,
|
|
const RigidBodyDynamics::Math::VectorNd& x) const
|
|
{ return calcDerivative(std::vector<int>(derivComponents),x); }
|
|
*/
|
|
private:
|
|
const int argumentSize;
|
|
const T value;
|
|
};
|
|
|
|
/**
|
|
* This is a Function_ subclass whose output value is a linear function of its
|
|
* arguments: f(x, y, ...) = ax+by+...+c.
|
|
*/
|
|
template <class T>
|
|
class Function_<T>::Linear : public Function_<T> {
|
|
public:
|
|
/**
|
|
* Create a Function_::Linear object.
|
|
*
|
|
* @param coefficients
|
|
* The coefficients of the linear function. The number of arguments
|
|
* expected by the function is equal to coefficients.size()-1.
|
|
* coefficients[0] is the coefficient for the first argument,
|
|
* coefficients[1] is the coefficient for the second argument, etc.
|
|
* The final element of coefficients contains the constant term.
|
|
*/
|
|
explicit Linear(
|
|
const RigidBodyDynamics::Math::VectorNd& coefficients)
|
|
: coefficients(coefficients) {
|
|
}
|
|
T calcValue(const RigidBodyDynamics::Math::VectorNd& x) const {
|
|
assert(x.size() == coefficients.size()-1);
|
|
T value = static_cast<T>(0);
|
|
for (int i = 0; i < x.size(); ++i)
|
|
value += x[i]*coefficients[i];
|
|
value += coefficients[x.size()];
|
|
return value;
|
|
}
|
|
T calcDerivative(const std::vector<int>& derivComponents,
|
|
const RigidBodyDynamics::Math::VectorNd& x) const {
|
|
assert(x.size() == coefficients.size()-1);
|
|
assert(derivComponents.size() > 0);
|
|
if (derivComponents.size() == 1)
|
|
return coefficients(derivComponents[0]);
|
|
return static_cast<T>(0);
|
|
}
|
|
virtual int getArgumentSize() const {
|
|
return coefficients.size()-1;
|
|
}
|
|
int getMaxDerivativeOrder() const {
|
|
return std::numeric_limits<int>::max();
|
|
}
|
|
|
|
/** This provides compatibility with std::vector without requiring any
|
|
copying. **/
|
|
/*
|
|
T calcDerivative(const std::vector<int>& derivComponents,
|
|
const RigidBodyDynamics::Math::VectorNd& x) const
|
|
{ return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
|
|
*/
|
|
private:
|
|
const RigidBodyDynamics::Math::VectorNd coefficients;
|
|
};
|
|
|
|
|
|
/**
|
|
* This is a Function_ subclass whose output value is a polynomial of its
|
|
* argument: f(x) = ax^n+bx^(n-1)+...+c.
|
|
*/
|
|
template <class T>
|
|
class Function_<T>::Polynomial : public Function_<T> {
|
|
public:
|
|
/**
|
|
* Create a Function_::Polynomial object.
|
|
*
|
|
* @param coefficients the polynomial coefficients in order of decreasing
|
|
* powers
|
|
*/
|
|
Polynomial(const RigidBodyDynamics::Math::VectorNd& coefficients)
|
|
: coefficients(coefficients) {
|
|
}
|
|
T calcValue(const RigidBodyDynamics::Math::VectorNd& x) const {
|
|
assert(x.size() == 1);
|
|
double arg = x[0];
|
|
T value = static_cast<T>(0);
|
|
for (int i = 0; i < coefficients.size(); ++i)
|
|
value = value*arg + coefficients[i];
|
|
return value;
|
|
}
|
|
T calcDerivative(const std::vector<int>& derivComponents,
|
|
const RigidBodyDynamics::Math::VectorNd& x) const {
|
|
assert(x.size() == 1);
|
|
assert(derivComponents.size() > 0);
|
|
double arg = x[0];
|
|
T value = static_cast<T>(0);
|
|
const int derivOrder = (int)derivComponents.size();
|
|
const int polyOrder = coefficients.size()-1;
|
|
for (int i = 0; i <= polyOrder-derivOrder; ++i) {
|
|
T coeff = coefficients[i];
|
|
for (int j = 0; j < derivOrder; ++j)
|
|
coeff *= polyOrder-i-j;
|
|
value = value*arg + coeff;
|
|
}
|
|
return value;
|
|
}
|
|
virtual int getArgumentSize() const {
|
|
return 1;
|
|
}
|
|
int getMaxDerivativeOrder() const {
|
|
return std::numeric_limits<int>::max();
|
|
}
|
|
|
|
/** This provides compatibility with std::vector without requiring any
|
|
copying. **/
|
|
/*
|
|
T calcDerivative(const std::vector<int>& derivComponents,
|
|
const RigidBodyDynamics::Math::VectorNd& x) const
|
|
{ return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
|
|
*/
|
|
private:
|
|
const RigidBodyDynamics::Math::VectorNd coefficients;
|
|
};
|
|
|
|
|
|
/**
|
|
* This is a Function_ subclass whose output value is a sinusoid of its
|
|
* argument: f(x) = a*sin(w*x + p) where a is amplitude, w is frequency
|
|
* in radians per unit of x, p is phase in radians.
|
|
*
|
|
* This is only defined for a scalar (double) return value.
|
|
*/
|
|
template <>
|
|
class Function_<double>::Sinusoid : public Function_<double> {
|
|
public:
|
|
/**
|
|
* Create a Function::Sinusoid object, returning a*sin(w*x+p).
|
|
*
|
|
* @param[in] amplitude 'a' in the above formula
|
|
* @param[in] frequency 'w' in the above formula
|
|
* @param[in] phase 'p' in the above formula
|
|
*/
|
|
Sinusoid(double amplitude, double frequency, double phase=0)
|
|
: a(amplitude), w(frequency), p(phase) {}
|
|
|
|
void setAmplitude(double amplitude) {a=amplitude;}
|
|
void setFrequency(double frequency) {w=frequency;}
|
|
void setPhase (double phase) {p=phase;}
|
|
|
|
double getAmplitude() const {return a;}
|
|
double getFrequency() const {return w;}
|
|
double getPhase () const {return p;}
|
|
|
|
// Implementation of Function_<T> virtuals.
|
|
|
|
virtual double calcValue(
|
|
const RigidBodyDynamics::Math::VectorNd& x) const {
|
|
|
|
const double t = x[0]; // we expect just one argument
|
|
return a*std::sin(w*t + p);
|
|
}
|
|
|
|
virtual double calcDerivative(
|
|
const std::vector<int>& derivComponents,
|
|
const RigidBodyDynamics::Math::VectorNd& x) const {
|
|
|
|
const double t = x[0]; // time is the only argument
|
|
const int order = derivComponents.size();
|
|
// The n'th derivative is
|
|
// sign * a * w^n * sc
|
|
// where sign is -1 if floor(order/2) is odd, else 1
|
|
// and sc is cos(w*t+p) if order is odd, else sin(w*t+p)
|
|
switch(order) {
|
|
case 0: return a* std::sin(w*t + p);
|
|
case 1: return a*w* std::cos(w*t + p);
|
|
case 2: return -a*w*w* std::sin(w*t + p);
|
|
case 3: return -a*w*w*w*std::cos(w*t + p);
|
|
default:
|
|
const double sign = double(((order/2) & 0x1) ? -1 : 1);
|
|
const double sc = (order & 0x1) ? std::cos(w*t+p) : std::sin(w*t+p);
|
|
const double wn = std::pow(w, order);
|
|
return sign*a*wn*sc;
|
|
}
|
|
}
|
|
|
|
virtual int getArgumentSize() const {return 1;} // just time
|
|
virtual int getMaxDerivativeOrder() const {
|
|
return std::numeric_limits<int>::max();
|
|
}
|
|
|
|
/** This provides compatibility with std::vector without requiring any
|
|
copying. **/
|
|
/*
|
|
double calcDerivative(const std::vector<int>& derivComponents,
|
|
const RigidBodyDynamics::Math::VectorNd& x) const
|
|
{ return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
|
|
*/
|
|
private:
|
|
double a, w, p;
|
|
};
|
|
|
|
/**
|
|
* This is a Function_ subclass whose output value y=f(x) is smoothly stepped
|
|
* from y=y0 to y1 as its input argument goes from x=x0 to x1. This is
|
|
* an S-shaped function with first and second derivatives y'(x0)=y'(x1)=0
|
|
* and y''(x0)=y''(x1)==0. The third derivative y''' exists and is continuous
|
|
* but we cannot guarantee anything about it at the end points.
|
|
*/
|
|
template <class T>
|
|
class Function_<T>::Step : public Function_<T> {
|
|
public:
|
|
/**
|
|
* Create a Function_::Step object that smoothly interpolates its output
|
|
* through a given range as its input moves through its range.
|
|
*
|
|
* @param y0 Output value when (x-x0)*sign(x1-x0) <= 0.
|
|
* @param y1 Output value when (x-x1)*sign(x1-x0) >= 0.
|
|
* @param x0 Start of switching interval.
|
|
* @param x1 End of switching interval.
|
|
*
|
|
* @tparam T The template type is the type of y0 and y1. This must
|
|
* be a type that supports subtraction and scalar
|
|
* multiplication by a double so that we can compute
|
|
* an expression like y=y0 + f*(y1-y0) for some double scalar f.
|
|
*
|
|
* Note that the numerical values of x0 and x1 can be in either order
|
|
* x0 < x1 or x0 > x1.
|
|
*/
|
|
Step(const T& y0, const T& y1, double x0, double x1)
|
|
: m_y0(y0), m_y1(y1), m_yr(y1-y0), m_zero(double(0)*y0),
|
|
m_x0(x0), m_x1(x1), m_ooxr(1/(x1-x0)), m_sign(copysign(1,m_ooxr))
|
|
{
|
|
/*
|
|
SimTK_ERRCHK1_ALWAYS(x0 != x1, "Function_<T>::Step::ctor()",
|
|
"A zero-length switching interval is illegal; both ends were %g.", x0);
|
|
*/
|
|
assert(x0 != x1);
|
|
std::printf( "Function_<T>::Step::ctor():"
|
|
"A zero-length switching interval "
|
|
"is illegal; both ends were %g.", x0);
|
|
|
|
}
|
|
|
|
T calcValue(const RigidBodyDynamics::Math::VectorNd& xin) const {
|
|
/*
|
|
SimTK_ERRCHK1_ALWAYS(xin.size() == 1,
|
|
"Function_<T>::Step::calcValue()",
|
|
"Expected just one input argument but got %d.", xin.size());
|
|
*/
|
|
assert(xin.size() == 1);
|
|
std::printf( "Function_<T>::Step::calcValue() "
|
|
"Expected just one input argument but got %d.",
|
|
xin.size());
|
|
|
|
|
|
const double x = xin[0];
|
|
if ((x-m_x0)*m_sign <= 0) return m_y0;
|
|
if ((x-m_x1)*m_sign >= 0) return m_y1;
|
|
// f goes from 0 to 1 as x goes from x0 to x1.
|
|
const double f = step01(m_x0,m_ooxr, x);
|
|
return m_y0 + f*m_yr;
|
|
}
|
|
|
|
T calcDerivative(const std::vector<int>& derivComponents,
|
|
const RigidBodyDynamics::Math::VectorNd& xin) const {
|
|
/*
|
|
SimTK_ERRCHK1_ALWAYS(xin.size() == 1,
|
|
"Function_<T>::Step::calcDerivative()",
|
|
"Expected just one input argument but got %d.", xin.size());
|
|
*/
|
|
assert(xin.size() == 1);
|
|
std::printf( "Function_<T>::Step::calcDerivative() "
|
|
"Expected just one input argument but got %d.",
|
|
xin.size());
|
|
|
|
const int derivOrder = (int)derivComponents.size();
|
|
|
|
/*
|
|
SimTK_ERRCHK1_ALWAYS(1 <= derivOrder && derivOrder <= 3,
|
|
"Function_<T>::Step::calcDerivative()",
|
|
"Only 1st, 2nd, and 3rd derivatives of the step are available,"
|
|
" but derivative %d was requested.", derivOrder);
|
|
*/
|
|
assert(1 <= derivOrder && derivOrder <= 3);
|
|
std::printf("Function_<T>::Step::calcDerivative() "
|
|
"Only 1st, 2nd, and 3rd derivatives of the step are available,"
|
|
" but derivative %d was requested.", derivOrder);
|
|
|
|
const double x = xin[0];
|
|
if ((x-m_x0)*m_sign <= 0) return m_zero;
|
|
if ((x-m_x1)*m_sign >= 0) return m_zero;
|
|
switch(derivOrder) {
|
|
case 1: return dstep01(m_x0,m_ooxr, x) * m_yr;
|
|
case 2: return d2step01(m_x0,m_ooxr, x) * m_yr;
|
|
case 3: return d3step01(m_x0,m_ooxr, x) * m_yr;
|
|
default: assert(!"impossible derivOrder");
|
|
}
|
|
return NAN*m_yr; /*NOTREACHED*/
|
|
}
|
|
|
|
virtual int getArgumentSize() const {return 1;}
|
|
int getMaxDerivativeOrder() const {return 3;}
|
|
|
|
/** This provides compatibility with std::vector without requiring any
|
|
copying. **/
|
|
/*
|
|
T calcDerivative(const std::vector<int>& derivComponents,
|
|
const RigidBodyDynamics::Math::VectorNd& x) const
|
|
{ return calcDerivative(ArrayViewConst_<int>(derivComponents),x); }
|
|
*/
|
|
private:
|
|
const T m_y0, m_y1, m_yr; // precalculate yr=(y1-y0)
|
|
const T m_zero; // precalculate T(0)
|
|
const double m_x0, m_x1, m_ooxr; // precalculate ooxr=1/(x1-x0)
|
|
const double m_sign; // sign(x1-x0) is 1 or -1
|
|
|
|
double step01(double x0, double x1, double x){
|
|
double u = (x-x0)/(x1-x0);
|
|
double u2 = u*u;
|
|
double u3 = u2*u;
|
|
return (3*u2 - 2*u3);
|
|
}
|
|
|
|
double dstep01(double x0, double x1, double x){
|
|
double u = (x-x0)/(x1-x0);
|
|
double du = (1)/(x1-x0);
|
|
double du2 = 2*u*du;
|
|
double du3 = 3*u*u*du;
|
|
return (3*du2 - 2*du3);
|
|
}
|
|
|
|
double d2step01(double x0, double x1, double x){
|
|
double u = (x-x0)/(x1-x0);
|
|
double du = (1)/(x1-x0);
|
|
//double ddu = 0;
|
|
double ddu2 = 2*du*du;// + 2*u*ddu since ddu=0;
|
|
double ddu3 = 3*du*u*du + 3*u*du*du;// + 3*u*u*du; ditto
|
|
return (3*ddu2 - 2*ddu3);
|
|
}
|
|
|
|
double d3step01(double x0, double x1, double x){
|
|
double u = (x-x0)/(x1-x0);
|
|
double du = (1)/(x1-x0);
|
|
//double ddu = 0;
|
|
//double dddu = 0;
|
|
double dddu2 = 0; //2*du*du;// since ddu=0;
|
|
double dddu3 = 3*du*du*du + 3*du*du*du;// ditto
|
|
return (3*dddu2 - 2*dddu3);
|
|
}
|
|
};
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
#endif // SimTK_SimTKCOMMON_FUNCTION_H_
|
|
|
|
|