rbdlsim/3rdparty/rbdl/addons/geometry/tests/testSmoothSegmentedFunction.cc

553 lines
19 KiB
C++
Raw Normal View History

2020-10-03 22:55:14 +02:00
/* -------------------------------------------------------------------------- *
* OpenSim: testSmoothSegmentedFunctionFactory.cpp *
* -------------------------------------------------------------------------- *
* The OpenSim API is a toolkit for musculoskeletal modeling and simulation. *
* See http://opensim.stanford.edu and the NOTICE file for more information. *
* OpenSim is developed at Stanford University and supported by the US *
* National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA *
* through the Warrior Web program. *
* *
* Copyright (c) 2005-2012 Stanford University and the Authors *
* Author(s): Matthew Millard *
* *
* 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. *
* -------------------------------------------------------------------------- */
/*
Update:
This is a port of the original code so that it will work with
the multibody code RBDL written by Martin Felis.
This also includes additional curves (the Anderson2007 curves)
which are not presently in OpenSim.
Author:
Matthew Millard
Date:
Nov 2015
*/
/*
Below is a basic bench mark simulation for the SmoothSegmentedFunctionFactory
class, a class that enables the easy generation of C2 continuous curves
that define the various characteristic curves required in a muscle model
*/
// Author: Matthew Millard
//==============================================================================
// INCLUDES
//==============================================================================
#include "geometry.h"
#include "numericalTestFunctions.h"
#include <UnitTest++.h>
#include <rbdl/rbdl_math.h>
#include <ctime>
#include <string>
#include <stdio.h>
#include <exception>
#include <cassert>
#include <fstream>
using namespace RigidBodyDynamics::Addons::Geometry;
using namespace std;
/**
This function will create a quintic Bezier curve y(x) and sample it, its
first derivative w.r.t. U (dx(u)/du and dy(u)/du), and its first derivative
w.r.t. to X and print it to the screen.
*/
void testSegmentedQuinticBezierDerivatives(
int maximumNumberOfToleranceViolations)
{
//cout <<"**************************************************"<<endl;
//cout << " TEST: Bezier Curve Derivative DU" << endl;
string name = "testSegmentedQuinticBezierDerivatives()";
RigidBodyDynamics::Math::VectorNd xPts(6);
RigidBodyDynamics::Math::VectorNd yPts(6);
xPts[0] = 0;
xPts[1] = 0.5;
xPts[2] = 0.5;
xPts[3] = 0.75;
xPts[4] = 0.75;
xPts[5] = 1;
yPts[0] = 0;
yPts[1] = 0.125;
yPts[2] = 0.125;
yPts[3] = 0.5;
yPts[4] = 0.5;
yPts[5] = 1;
double val = 0;
double d1 = 0;
double d2 = 0;
double d3 = 0;
double d4 = 0;
double d5 = 0;
double d6 = 0;
double u = 0;
int steps = 100;
RigidBodyDynamics::Math::MatrixNd analyticDerXU(steps,8);
RigidBodyDynamics::Math::MatrixNd analyticDerYU(steps,8);
RigidBodyDynamics::Math::VectorNd uV(steps);
for(int i = 0; i<steps; i++){
//int i = 10;
u = (double)i/(steps-1);
uV[i] = u;
val= SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveVal(u,xPts);
d1 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,xPts,1);
d2 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,xPts,2);
d3 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,xPts,3);
d4 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,xPts,4);
d5 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,xPts,5);
d6 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,xPts,6);
analyticDerXU(i,0) = u;
analyticDerXU(i,1) = val;
analyticDerXU(i,2) = d1;
analyticDerXU(i,3) = d2;
analyticDerXU(i,4) = d3;
analyticDerXU(i,5) = d4;
analyticDerXU(i,6) = d5;
analyticDerXU(i,7) = d6;
val= SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveVal(u,yPts);
d1 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,yPts,1);
d2 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,yPts,2);
d3 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,yPts,3);
d4 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,yPts,4);
d5 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,yPts,5);
d6 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivU(u,yPts,6);
analyticDerYU(i,0) = u;
analyticDerYU(i,1) = val;
analyticDerYU(i,2) = d1;
analyticDerYU(i,3) = d2;
analyticDerYU(i,4) = d3;
analyticDerYU(i,5) = d4;
analyticDerYU(i,6) = d5;
analyticDerYU(i,7) = d6;
}
int mxDU = 6-1;
RigidBodyDynamics::Math::MatrixNd numericDer(analyticDerXU.rows(), mxDU);
RigidBodyDynamics::Math::MatrixNd errorDer(analyticDerXU.rows(), mxDU);
double tol = (double)(1.0/steps);
tol = tol*tol*50;
//Numerical error in a central difference increases with the
//square of h.
//http://en.wikipedia.org/wiki/Finite_difference
RigidBodyDynamics::Math::VectorNd domainX =
RigidBodyDynamics::Math::VectorNd::Zero(analyticDerXU.rows());
RigidBodyDynamics::Math::VectorNd rangeY =
RigidBodyDynamics::Math::VectorNd::Zero(analyticDerXU.rows());
RigidBodyDynamics::Math::VectorNd analyticDerYX =
RigidBodyDynamics::Math::VectorNd::Zero(analyticDerXU.rows());
for(int j=0; j<analyticDerXU.rows(); j++){
domainX[j] = analyticDerXU(j,0);
}
for(int i=0;i<mxDU;i++){
for(int j=0; j<analyticDerXU.rows(); j++){
rangeY[j] = analyticDerXU(j,i+1);
analyticDerYX[j] = analyticDerXU(j,i+2);
}
numericDer.col(i) = calcCentralDifference(domainX,
rangeY,true);
for(int j=0; j<analyticDerYX.rows();++j){
errorDer(j,i) = analyticDerYX[j]-numericDer(j,i);
}
//errorDer(i)= abs( errorDer(i).elementwiseDivide(numericDer(i)) );
//The end points can't be tested because a central difference
//cannot be accurately calculated at these locations
for(int j=1; j<analyticDerXU.rows()-1; j++){
assert( abs(errorDer(j,i))<tol );
//if(errorDer(j,i)>tol)
//printf("Error > Tol: (%i,%i): %f > %f\n",j,i,errorDer(j,i),tol);
}
}
//errorDer.cwiseAbs();
//cout << errorDer << endl;
//printf("...absolute tolerance of %f met\n", tol);
//cout << " TEST: Bezier Curve Derivative DYDX to d6y/dx6" << endl;
RigidBodyDynamics::Math::MatrixNd numericDerXY(analyticDerXU.rows(), 6);
RigidBodyDynamics::Math::MatrixNd analyticDerXY(analyticDerXU.rows(),6);
for(int i=0; i< analyticDerXU.rows(); i++)
{
analyticDerXY(i,0) = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,1);
analyticDerXY(i,1) = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,2);
analyticDerXY(i,2) = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,3);
analyticDerXY(i,3) = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,4);
analyticDerXY(i,4) = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,5);
analyticDerXY(i,5) = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivDYDX(uV[i],xPts,yPts,6);
}
for(int j=0; j<numericDerXY.cols();j++){
for(int k=0; k<numericDerXY.rows(); k++){
domainX[k] = analyticDerXU(k,1);
if(j == 0){
rangeY[k] = analyticDerYU(k,1);
}else{
rangeY[k] = analyticDerXY(k,j-1);
}
}
numericDerXY.col(j) = calcCentralDifference(domainX,
rangeY,true);
}
//Generate numerical derivative curves for the first 3 derivatives
/*
numericDerXY.col(0) = calcCentralDifference(analyticDerXU.col(1),
analyticDerYU.col(1),true);
numericDerXY.col(1) = calcCentralDifference(analyticDerXU.col(1),
analyticDerXY.col(0),true);
numericDerXY.col(2) = calcCentralDifference(analyticDerXU.col(1),
analyticDerXY.col(1),true);
numericDerXY.col(3) = calcCentralDifference(analyticDerXU.col(1),
analyticDerXY.col(2),true);
numericDerXY.col(4) = calcCentralDifference(analyticDerXU.col(1),
analyticDerXY.col(3),true);
numericDerXY.col(5) = calcCentralDifference(analyticDerXU.col(1),
analyticDerXY.col(4),true);
*/
//Create the matrix of errors
RigidBodyDynamics::Math::MatrixNd errorDerXYNum(analyticDerXU.rows(), 6);
RigidBodyDynamics::Math::MatrixNd errorDerXYDen(analyticDerXU.rows(), 6);
RigidBodyDynamics::Math::MatrixNd errorDerXY(analyticDerXU.rows(), 6);
for(int i = 0; i < errorDerXYNum.rows(); ++i){
for(int j = 0; j < errorDerXYNum.cols(); ++j){
errorDerXYNum(i,j) = abs( analyticDerXY(i,j)-numericDerXY(i,j));
errorDerXYDen(i,j) = abs( analyticDerXY(i,j)+numericDerXY(i,j));
errorDerXY(i,j) = errorDerXYNum(i,j)/errorDerXYDen(i,j);
}
}
double relTol = 5e-2;
int relTolExceeded = 0;
for(int j=0;j<6;j++){
//can't test the first and last entries because a central diff.
//cannot calculate these values accurately.
for(int i=1;i<analyticDerXU.rows()-1;i++){
if(errorDerXY(i,j)>relTol){
//printf("Error > Tol: (%i,%i): %f > %f\n",i,j,
// errorDerXY(i,j),relTol);
relTolExceeded++;
}
}
}
//cout << relTolExceeded << endl;
//The relative tolerance gets exceeded occasionally in locations of
//rapid change in the curve. Provided there are only a few locations
//where the relative tolerance of 5% is broken, the curves should be
//regarded as being good. Ten errors out of a possible 100*6 data points
//seems relatively small.
CHECK(relTolExceeded < maximumNumberOfToleranceViolations);
//std::string fname = "analyticDerXY.csv";
//printMatrixToFile(analyticDerXY,fname);
//fname = "numericDerXY.csv";
//printMatrixToFile(numericDerXY,fname);
//fname = "errorDerXY.csv";
//printMatrixToFile(errorDerXY,fname);
//printf(" ...relative tolerance of %f not exceeded more than %i times\n"
// " across all 6 derivatives, with 100 samples each\n",
// relTol, 10);
//cout <<"**************************************************"<<endl;
}
TEST(QuinticBezierToolKitDerivatives)
{
int maximumNumberOfToleranceViolations = 10;
testSegmentedQuinticBezierDerivatives(10);
}
TEST(SmoothSegmentedFunctionProperties)
{
//1. Make a fake monotonic curve
RigidBodyDynamics::Math::VectorNd x(5);
RigidBodyDynamics::Math::VectorNd y(5);
RigidBodyDynamics::Math::VectorNd dydx(5);
for(int i=0; i<x.size();++i){
x[i] = i*0.5*M_PI/(x.size()-1);
y[i] = sin(x[i]) + x[i];
dydx[i] = cos(x[i]) + 1.0;
}
double c = 0.5;
RigidBodyDynamics::Math::MatrixNd mX(6,4), mY(6,4);
RigidBodyDynamics::Math::MatrixNd p0(6,2);
for(int i=0; i < 4; ++i){
p0 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCornerControlPoints( x[i], y[i], dydx[i],
x[i+1],y[i+1],dydx[i+1],c);
mX.col(i) = p0.col(0);
mY.col(i) = p0.col(1);
}
SmoothSegmentedFunction testCurve = SmoothSegmentedFunction();
testCurve.updSmoothSegmentedFunction( mX, mY,
x[0], x[4],
y[0], y[4],
dydx[0],dydx[4],
"testCurve");
//2. Test key points.
RigidBodyDynamics::Math::VectorNd yErr(5);
RigidBodyDynamics::Math::VectorNd dydxErr(5);
for(int i=0; i<x.size(); ++i){
yErr[i] = testCurve.calcValue(x[i]) - y[i];
dydxErr[i] = testCurve.calcDerivative(x[i],1) - dydx[i];
CHECK( abs(yErr[i]) < TOL_SMALL );
CHECK( abs(dydxErr[i]) < TOL_SMALL );
}
//3. Test derivatives for numerical consistency
RigidBodyDynamics::Math::MatrixNd testCurveSample
= testCurve.calcSampledCurve( 6, x[0]-0.1, x[4]+0.1);
bool areCurveDerivativesGood =
areCurveDerivativesCloseToNumericDerivatives(
testCurve,
testCurveSample,
TOL_DX);
CHECK(areCurveDerivativesGood);
//4. Test C2 continuity
bool curveIsContinuous = isCurveC2Continuous(testCurve,
testCurveSample,
TOL_BIG);
CHECK(curveIsContinuous);
//5. Test monotonicity
bool curveIsMonotonic = isCurveMontonic(testCurveSample);
CHECK(curveIsMonotonic);
}
TEST(ShiftScale)
{
//1. Make a curve
RigidBodyDynamics::Math::VectorNd xV(5);
RigidBodyDynamics::Math::VectorNd yV(5);
RigidBodyDynamics::Math::VectorNd dydxV(5);
for(int i=0; i<xV.size();++i){
xV[i] = i*0.5*M_PI/(xV.size()-1);
yV[i] = sin(xV[i]) + xV[i];
dydxV[i] = cos(xV[i]) + 1.0;
}
double c = 0.5;
RigidBodyDynamics::Math::MatrixNd mX(6,4), mY(6,4);
RigidBodyDynamics::Math::MatrixNd p0(6,2);
for(int i=0; i < 4; ++i){
p0 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCornerControlPoints( xV[i], yV[i], dydxV[i],
xV[i+1],yV[i+1],dydxV[i+1],c);
mX.col(i) = p0.col(0);
mY.col(i) = p0.col(1);
}
SmoothSegmentedFunction curve = SmoothSegmentedFunction();
curve.updSmoothSegmentedFunction( mX, mY,
xV[0], xV[4],
yV[0], yV[4],
dydxV[0],dydxV[4],
"testCurve");
SmoothSegmentedFunction shiftedCurve = SmoothSegmentedFunction();
shiftedCurve = curve;
double xShift = 1.0/3.0;
double yShift = 2.0/3.0;
shiftedCurve.shift(xShift,yShift);
//Test shift
double x ,y, dydx, d2ydx2;
double xS ,yS, dydxS, d2ydx2S;
double xmin = -0.1;
double xmax = 0.5*M_PI+0.1;
for(int i=0; i<xV.size();++i){
x = xmin + i*(xmax-xmin)/(xV.size()-1);
y = curve.calcValue(x);
dydx = curve.calcDerivative(x,1);
d2ydx2 = curve.calcDerivative(x,2);
xS = x + xShift;
yS = shiftedCurve.calcValue(xS) - yShift;
dydxS = shiftedCurve.calcDerivative(xS,1);
d2ydx2S = shiftedCurve.calcDerivative(xS,2);
CHECK( abs(y-yS) < TOL_SMALL );
CHECK( abs( dydx-dydxS) < TOL_SMALL );
CHECK( abs(d2ydx2-d2ydx2S) < TOL_SMALL );
}
//Test scale
SmoothSegmentedFunction scaledCurve = SmoothSegmentedFunction();
scaledCurve = curve;
double xScale = 1.0/2.0;
double yScale = 5.0/3.0;
double dydxScale = yScale/xScale;
scaledCurve.scale(xScale,yScale);
for(int i=0; i<xV.size();++i){
x = xmin + i*(xmax-xmin)/(xV.size()-1);
y = curve.calcValue(x);
dydx = curve.calcDerivative(x,1);
d2ydx2 = curve.calcDerivative(x,2);
xS = x*xScale;
yS = scaledCurve.calcValue(xS)/yScale;
dydxS = scaledCurve.calcDerivative(xS,1)/dydxScale ;
d2ydx2S = scaledCurve.calcDerivative(xS,2)*xScale/(dydxScale);
CHECK( abs(y-yS) < TOL_SMALL );
CHECK( abs( dydx-dydxS) < TOL_SMALL );
CHECK( abs(d2ydx2-d2ydx2S) < TOL_SMALL );
}
}
TEST(CalcFunctionInverseValue){
//1. Make a curve with multiple maxima
unsigned int n = 10;
RigidBodyDynamics::Math::VectorNd xV(n);
RigidBodyDynamics::Math::VectorNd yV(n);
RigidBodyDynamics::Math::VectorNd dydxV(n);
for(int i=0; i<xV.size();++i){
xV[i] = i*2*M_PI/(xV.size()-1);
yV[i] = sin(xV[i]);
dydxV[i] = cos(xV[i]);
}
double c = 0.5;
unsigned int m =n-1;
RigidBodyDynamics::Math::MatrixNd mX(6,m), mY(6,m);
RigidBodyDynamics::Math::MatrixNd p0(6,2);
for(int i=0; i < m; ++i){
p0 = SegmentedQuinticBezierToolkit::
calcQuinticBezierCornerControlPoints( xV[i], yV[i], dydxV[i],
xV[i+1],yV[i+1],dydxV[i+1],c);
mX.col(i) = p0.col(0);
mY.col(i) = p0.col(1);
}
SmoothSegmentedFunction curve = SmoothSegmentedFunction();
curve.updSmoothSegmentedFunction( mX, mY,
xV[0], xV[n-1],
yV[0], yV[n-1],
dydxV[0],dydxV[n-1],
"testCurve");
//Testing within the curve: make sure it returns an x value that
//evaluates to the desired y and that it is the correct x.
double xGuess = M_PI*0.25;
double y0 = sin(xGuess);
double x0 = curve.calcInverseValue(y0,xGuess);
double err = curve.calcValue(x0)-y0;
CHECK( abs(err) < TOL_SMALL);
CHECK( abs(x0-xGuess) < 0.1);
//Testing within the curve: test the other x.
xGuess = M_PI*(0.25) + M_PI*(0.5);
y0 = sin(xGuess);
x0 = curve.calcInverseValue(y0,xGuess);
err = curve.calcValue(x0)-y0;
CHECK( abs(err) < TOL_SMALL);
CHECK( abs(x0-xGuess) < 0.1);
//Test outside of the interval.
xGuess = -1.0;
y0 = dydxV[0]*xGuess;
x0 = curve.calcInverseValue(y0,xGuess);
err = curve.calcValue(x0)-y0;
CHECK( abs(err) < TOL_SMALL);
CHECK( abs(x0-xGuess) < 0.1);
//Test outside of the interval.
xGuess = M_PI*2.0 + 1.0;
y0 = dydxV[n-1]*1.0;
x0 = curve.calcInverseValue(y0,xGuess);
err = curve.calcValue(x0)-y0;
CHECK( abs(err) < TOL_SMALL);
CHECK( abs(x0-xGuess) < 0.1);
}
int main (int argc, char *argv[])
{
return UnitTest::RunAllTests ();
}