rbdlsim/3rdparty/rbdl/addons/geometry/Function.h

526 lines
18 KiB
C
Raw Normal View History

2020-10-03 22:55:14 +02:00
#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_