553 lines
19 KiB
C++
553 lines
19 KiB
C++
|
/* -------------------------------------------------------------------------- *
|
||
|
|
* 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 ();
|
||
|
|
}
|