martin
/
protot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
protot/3rdparty/rbdl/addons/geometry/SmoothSegmentedFunction.cc

845 lines
26 KiB
C++
Raw Permalink Normal View History

Added RBDL to 3rdparty, added empty rig model, used better coding style in CharacterModule
2017-02-22 21:35:14 +01:00
/* -------------------------------------------------------------------------- *
* OpenSim: SmoothSegmentedFunction.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.
Author:
Matthew Millard
Date:
Nov 2015
*/
//=============================================================================
// INCLUDES
//=============================================================================
#include "SmoothSegmentedFunction.h"
#include <fstream>
#include <ostream>
//=============================================================================
// STATICS
//=============================================================================
//using namespace SimTK;
//using namespace OpenSim;
using namespace std;
using namespace RigidBodyDynamics::Addons::Geometry;
static bool DEBUG = false;
static double UTOL = std::numeric_limits<double>::epsilon()*1e6;
static double INTTOL = std::numeric_limits<double>::epsilon()*1e2;
static double SQRTEPS = std::sqrt(numeric_limits<double>::epsilon());
static int MAXITER = 20;
static int NUM_SAMPLE_PTS = 100;
//=============================================================================
// UTILITY FUNCTIONS
//=============================================================================
/*
DETAILED COMPUTATIONAL COSTS:
=========================================================================
WITHOUT INTEGRAL
_________________________________________________________________________
Function Comp Div Mult Add Assignments
_________________________________________________________________________
member assign M:2, 9
curve gen: m,m*100 m m*100 m m*100*(4)
+m*100(3) +m*100*(3)
Function detail
Evaluations Function
m SimTK::SplineFitter<double>::
fitForSmoothingParameter(3,x,u,0).getSpline();
Cost: ?
m*100 SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveVal
Cost: Mult Add Assignments
21 20 13
Total ~typically > 2100*m multiplications, additions,
> 1000*m assignments
> 100*m divisions
_________________________________________________________________________
Comp Div Mult Add Assignments
Total: m+m*100(3) m*100 m*100*21 m*100*20 m*100*13
+m+m*100*3 +m*100*4+9+M:2
+ m*Cost(SimTK::SplineFitter ...)
=========================================================================
ADDITIONAL COST OF COMPUTING THE INTEGRAL CURVE
Comp Div Mult Add Assign
RK45 Fn.Eval m*100*(156 12 618 390 456)
RK45 Overhead m*100*(? ? ? ? ? )
Spline cost m*100*(? ? ? ? ? )
Total: ~typically > 100,000's mult, additions, assignments
> 40,000 comparisions
> 3000 divisions
=========================================================================
M: Matrix
V: Vector
N.B. These costs are dependent on SegmentedQuinticBezierToolkit
*/
SmoothSegmentedFunction::
SmoothSegmentedFunction(
const RigidBodyDynamics::Math::MatrixNd& mX,
const RigidBodyDynamics::Math::MatrixNd& mY,
double x0, double x1,
double y0, double y1,
double dydx0, double dydx1,
const std::string& name):
_x0(x0),_x1(x1),_y0(y0),_y1(y1),_dydx0(dydx0),_dydx1(dydx1),
_name(name)
{
_numBezierSections = mX.cols();
_mXVec.resize(_numBezierSections);
_mYVec.resize(_numBezierSections);
for(int s=0; s < _numBezierSections; s++){
_mXVec[s] = mX.col(s);
_mYVec[s] = mY.col(s);
}
}
//==============================================================================
SmoothSegmentedFunction::SmoothSegmentedFunction():
_x0(NAN),_x1(NAN),
_y0(NAN),_y1(NAN),
_dydx0(NAN),_dydx1(NAN),_name("NOT_YET_SET")
{
//_arraySplineUX.resize(0);
_mXVec.resize(0);
_mYVec.resize(0);
//_splineYintX = SimTK::Spline();
_numBezierSections = (int)NAN;
}
//==============================================================================
void SmoothSegmentedFunction::
updSmoothSegmentedFunction(
const RigidBodyDynamics::Math::MatrixNd& mX,
const RigidBodyDynamics::Math::MatrixNd& mY,
double x0, double x1,
double y0, double y1,
double dydx0, double dydx1,
const std::string& name)
{
if(mX.rows() != 6 || mY.rows() != 6 || mX.cols() != mY.cols() ){
cerr<<"SmoothSegmentedFunction::updSmoothSegmentedFunction "
<< _name.c_str()
<<": matrices mX and mY must have 6 rows, and the same"
<<" number of columns."
<< endl;
assert(0);
abort();
}
_x0 =x0;
_x1 =x1;
_y0 =y0;
_y1 =y1;
_dydx0=dydx0;
_dydx1=dydx1;
if(mX.cols() != _mXVec.size()){
_mXVec.resize(mX.cols());
_mYVec.resize(mY.cols());
}
_numBezierSections = mX.cols();
for(int s=0; s < mX.cols(); s++){
_mXVec[s] = mX.col(s);
_mYVec[s] = mY.col(s);
}
_name = name;
}
//==============================================================================
void SmoothSegmentedFunction::shift(double xShift, double yShift)
{
_x0 += xShift;
_x1 += xShift;
_y0 += yShift;
_y1 += yShift;
for(int i=0; i<_mXVec.size();++i){
for(int j=0; j<_mXVec.at(i).rows();++j){
_mXVec.at(i)[j] += xShift;
_mYVec.at(i)[j] += yShift;
}
}
}
void SmoothSegmentedFunction::scale(double xScale, double yScale)
{
if( abs( xScale ) <= SQRTEPS){
cerr<<"SmoothSegmentedFunction::scale "
<< _name.c_str()
<<": xScale must be greater than sqrt(eps). Setting xScale to such"
<<" a small value will cause the slope of the curve to approach "
<<" infinity, or become undefined."
<< endl;
assert(0);
abort();
}
_x0 *= xScale;
_x1 *= xScale;
_y0 *= yScale;
_y1 *= yScale;
_dydx0 *= yScale/xScale;
_dydx1 *= yScale/xScale;
for(int i=0; i<_mXVec.size();++i){
for(int j=0; j<_mXVec.at(i).rows();++j){
_mXVec.at(i)[j] *= xScale;
_mYVec.at(i)[j] *= yScale;
}
}
}
//==============================================================================
/*Detailed Computational Costs
________________________________________________________________________
If x is in the Bezier Curve
Name Comp. Div. Mult. Add. Assign.
_______________________________________________________________________
SegmentedQuinticBezierToolkit::
calcIndex 3*m+2 1*m 3
*calcU 15 2 82 42 60
calcQuinticBezierCurveVal 21 20 13
total 15+3*m+2 2 103 62+1*m 76
*Approximate. Uses iteration
________________________________________________________________________
If x is in the linear region
Name Comp. Div. Mult. Add. Assign.
1 1 2 1
________________________________________________________________________
*/
double SmoothSegmentedFunction::calcValue(double x) const
{
double yVal = 0;
if(x >= _x0 && x <= _x1 )
{
int idx = SegmentedQuinticBezierToolkit::calcIndex(x,_mXVec);
double u = SegmentedQuinticBezierToolkit::
calcU(x,_mXVec[idx], UTOL, MAXITER);
yVal = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveVal(u,_mYVec[idx]);
}else{
if(x < _x0){
yVal = _y0 + _dydx0*(x-_x0);
}else{
yVal = _y1 + _dydx1*(x-_x1);
}
}
return yVal;
}
double SmoothSegmentedFunction::calcValue(
const RigidBodyDynamics::Math::VectorNd& ax) const
{
if( !(ax.size() == 1) ){
cerr<<"SmoothSegmentedFunction::calcValue " << _name.c_str()
<<": Argument x must have only 1 element, as this function is "
<< "designed only for 1D functions, but a function with "
<< ax.size() << " elements was entered"
<< endl;
assert(0);
abort();
}
return calcValue(ax[0]);
}
/*Detailed Computational Costs
________________________________________________________________________
If x is in the Bezier Curve, and dy/dx is being evaluated
Name Comp. Div. Mult. Add. Assign.
_______________________________________________________________________
Overhead:
SegmentedQuinticBezierToolkit::
calcIndex 3*m+2 1*m 3
*calcU 15 2 82 42 60
Derivative Evaluation:
**calcQuinticBezierCurveDYDX 21 20 13
dy/du 20 19 11
dx/du 20 19 11
dy/dx 1
total 17+3*m 3 143 m+100 98
*Approximate. Uses iteration
**Higher order derivatives cost more
________________________________________________________________________
If x is in the linear region
Name Comp. Div. Mult. Add. Assign.
1 1
________________________________________________________________________
*/
double SmoothSegmentedFunction::calcDerivative(double x, int order) const
{
//return calcDerivative( SimTK::Array_<int>(order,0),
// RigidBodyDynamics::Math::VectorNd(1,x));
double yVal = 0;
//QUINTIC SPLINE
if(order==0){
yVal = calcValue(x);
}else{
if(x >= _x0 && x <= _x1){
int idx = SegmentedQuinticBezierToolkit::calcIndex(x,_mXVec);
double u = SegmentedQuinticBezierToolkit::
calcU(x,_mXVec[idx],UTOL,MAXITER);
yVal = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveDerivDYDX(u, _mXVec[idx],
_mYVec[idx], order);
/*
std::cout << _mX(3, idx) << std::endl;
std::cout << _mX(idx) << std::endl;*/
}else{
if(order == 1){
if(x < _x0){
yVal = _dydx0;
}else{
yVal = _dydx1;}
}else{
yVal = 0;}
}
}
return yVal;
}
double SmoothSegmentedFunction::
calcDerivative( const std::vector<int>& derivComponents,
const RigidBodyDynamics::Math::VectorNd& ax) const
{
/*
for(int i=0; i < (signed)derivComponents.size(); i++){
SimTK_ERRCHK2_ALWAYS( derivComponents[i] == 0,
"SmoothSegmentedFunction::calcDerivative",
"%s: derivComponents can only be populated with 0's because "
"SmoothSegmentedFunction is only valid for a 1D function, but "
"derivComponents had a value of %i in it",
_name.c_str(), derivComponents[i]);
}
SimTK_ERRCHK2_ALWAYS( derivComponents.size() <= 6,
"SmoothSegmentedFunction::calcDerivative",
"%s: calcDerivative is only valid up to a 6th order derivative"
" but derivComponents had a size of %i",
_name.c_str(), derivComponents.size());
SimTK_ERRCHK2_ALWAYS( ax.size() == 1,
"SmoothSegmentedFunction::calcValue",
"%s: Argument x must have only 1 element, as this function is "
"designed only for 1D functions, but ax had a size of %i",
_name.c_str(), ax.size());
*/
for(int i=0; i < (signed)derivComponents.size(); i++){
if( !(derivComponents[i] == 0)){
cerr << "SmoothSegmentedFunction::calcDerivative "
<< _name.c_str()
<<": derivComponents can only be populated with 0's because "
<< "SmoothSegmentedFunction is only valid for a 1D function,"
<< " but derivComponents had a value of "
<< derivComponents[i] << " in it"
<< endl;
assert(0);
abort();
}
}
if( !(derivComponents.size() <= 6)){
cerr << "SmoothSegmentedFunction::calcDerivative " << _name.c_str()
<< ": calcDerivative is only valid up to a 6th order derivative"
<< " but derivComponents had a size of "
<< derivComponents.size()
<< endl;
assert(0);
abort();
}
if( !(ax.size() == 1) ){
cerr << "SmoothSegmentedFunction::calcValue " << _name.c_str()
<< ": Argument x must have only 1 element, as this function is "
<< "designed only for 1D functions, but ax had a size of "
<< ax.size()
<< endl;
assert(0);
abort();
}
return calcDerivative(ax[0], derivComponents.size());
}
/*Detailed Computational Costs
________________________________________________________________________
If x is in the Bezier Curve, and dy/dx is being evaluated
Name Comp. Div. Mult. Add. Assign.
_______________________________________________________________________
*spline.calcValue 7 2 3 1
*Approximate cost of evaluating a cubic spline with 100 knots, where
the bisection method is used to find the correct index
________________________________________________________________________
If x is in the linear region
Name Comp. Div. Mult. Add. Assign.
*spline.calcValue 1 2 3 1
integral eval 2 4 5 1
total 3 6 8 2
*Approximate cost of evaluating a cubic spline at its last knot point
________________________________________________________________________
*/
/*
double SmoothSegmentedFunction::calcIntegral(double x) const
{
SimTK_ERRCHK1_ALWAYS(_computeIntegral,
"SmoothSegmentedFunction::calcIntegral",
"%s: This curve was not constructed with its integral because"
"computeIntegral was false",_name.c_str());
double yVal = 0;
if(x >= _x0 && x <= _x1){
yVal = _splineYintX.calcValue(RigidBodyDynamics::Math::VectorNd(1,x));
}else{
//LINEAR EXTRAPOLATION
if(x < _x0){
RigidBodyDynamics::Math::VectorNd tmp(1);
tmp(0) = _x0;
double ic = _splineYintX.calcValue(tmp);
if(_intx0x1){//Integrating left to right
yVal = _y0*(x-_x0)
+ _dydx0*(x-_x0)*(x-_x0)*0.5
+ ic;
}else{//Integrating right to left
yVal = -_y0*(x-_x0)
- _dydx0*(x-_x0)*(x-_x0)*0.5
+ ic;
}
}else{
RigidBodyDynamics::Math::VectorNd tmp(1);
tmp(0) = _x1;
double ic = _splineYintX.calcValue(tmp);
if(_intx0x1){
yVal = _y1*(x-_x1)
+ _dydx1*(x-_x1)*(x-_x1)*0.5
+ ic;
}else{
yVal = -_y1*(x-_x1)
- _dydx1*(x-_x1)*(x-_x1)*0.5
+ ic;
}
}
}
return yVal;
}
*/
/*
bool SmoothSegmentedFunction::isIntegralAvailable() const
{
return _computeIntegral;
}
*/
/*
bool SmoothSegmentedFunction::isIntegralComputedLeftToRight() const
{
return _intx0x1;
}
*/
int SmoothSegmentedFunction::getArgumentSize() const
{
return 1;
}
int SmoothSegmentedFunction::getMaxDerivativeOrder() const
{
return 6;
}
std::string SmoothSegmentedFunction::getName() const
{
return _name;
}
void SmoothSegmentedFunction::setName(const std::string &name)
{
_name = name;
}
RigidBodyDynamics::Math::VectorNd
SmoothSegmentedFunction::getCurveDomain() const
{
RigidBodyDynamics::Math::VectorNd xrange(2);
xrange[0] = 0;
xrange[1] = 0;
if (!_mXVec.empty()) {
xrange[0] = _mXVec[0][0];
xrange[1] = _mXVec[_mXVec.size()-1][_mXVec[0].size()-1];
}
return xrange;
}
///////////////////////////////////////////////////////////////////////////////
// Utility functions
///////////////////////////////////////////////////////////////////////////////
/*Detailed Computational Costs
_______________________________________________________________________
Name Comp. Div. Mult. Add. Assign.
_______________________________________________________________________
*overhead (17+3*m 2 82 42+m 63)*7
119+21*m 14 574 294+7m 441
calcValue 21 20 13
calcDerivative: dy/dx 1 40 38 22
: d2y/dx2 2 78 73 23
: d3y/dx3 4 118 105 58
: d4y/dx4 5 168 137 71
: d5y/dx5 7 236 170 88
: d6y/dx6 9 334 209 106
**calcIntegral 7 2 3 1
total per point 126+21*m 42 1571 1049 823
total per elbow 126k+21k*m 42k 1571k 1049k 823k
*Approximate. Overhead associated with finding the correct Bezier
spline section, and evaluating u(x).
Assumes 2 Newton iterations in calcU
**Approximate. Includes estimated cost of evaluating a cubic spline
with 100 knots
*/
RigidBodyDynamics::Math::MatrixNd
SmoothSegmentedFunction::calcSampledCurve(int maxOrder,
double domainMin,
double domainMax) const{
int pts = 1; //Number of points between each of the spline points used
//to fit u(x), and also the integral spline
if( !(maxOrder <= getMaxDerivativeOrder()) ){
cerr << "SmoothSegmentedFunction::calcSampledCurve "
<< "Derivative order past the maximum computed order requested"
<< endl;
assert(0);
abort();
}
double x0,x1,delta;
//y,dy,d1y,d2y,d3y,d4y,d5y,d6y,iy
RigidBodyDynamics::Math::VectorNd
midX(NUM_SAMPLE_PTS*_numBezierSections-(_numBezierSections-1));
RigidBodyDynamics::Math::VectorNd x(NUM_SAMPLE_PTS);
//Generate a sample of X values inside of the curve that is denser where
//the curve is more curvy.
double u;
int idx = 0;
for(int s=0; s < _numBezierSections; s++){
//Sample the local set for u and x
for(int i=0;i<NUM_SAMPLE_PTS;i++){
u = ( (double)i )/( (double)(NUM_SAMPLE_PTS-1) );
x[i] = SegmentedQuinticBezierToolkit::
calcQuinticBezierCurveVal(u,_mXVec[s]);
if(_numBezierSections > 1){
//Skip the last point of a set that has another set of points
//after it. Why? The last point and the starting point of the
//next set are identical in value.
if(i<(NUM_SAMPLE_PTS-1) || s == (_numBezierSections-1)){
midX[idx] = x[i];
idx++;
}
}else{
midX[idx] = x[i];
idx++;
}
}
}
RigidBodyDynamics::Math::VectorNd xsmpl(pts*(midX.size()-1)+2*10*pts);
RigidBodyDynamics::Math::MatrixNd results;
/*
if(_computeIntegral){
results.resize(pts*(midX.size()-1)+2*10*pts,maxOrder+2+1);
}else{
}
*/
results.resize(pts*(midX.size()-1)+2*10*pts,maxOrder+2);
//Array initialization is so ugly ...
std::vector<int> d1y(1),d2y(2),d3y(3),d4y(4),d5y(5),d6y(6);
d1y[0]=0;
d2y[0] = 0;
d2y[1] = 0;
for(int i=0;i<3;i++)
d3y[i]=0;
for(int i=0;i<4;i++)
d4y[i]=0;
for(int i=0;i<5;i++)
d5y[i]=0;
for(int i=0;i<6;i++)
d6y[i]=0;
//generate some sample points in the extrapolated region
idx = 0;
x0 = _x0 - 0.1*(_x1-_x0);
if(domainMin < x0)
x0 = domainMin;
x1 = _x0;
delta = (0.1)*(x1-x0)/(pts);
for(int j=0; j<pts*10; j++){
xsmpl[idx] = x0 + delta*j;
idx++;
}
//generate some points in the mid region
for(int i=0; i< midX.size()-1;i++){
x0 = midX[i];
x1 = midX[i+1];
delta = (x1-x0)/pts;
for(int j=0;j<pts;j++){
xsmpl[idx] = x0 + delta*j;
idx++;
}
}
//generate some points in the extrapolated region
x0 = _x1;
x1 = _x1 + 0.1*(_x1-_x0);
if(domainMax > x1)
x1 = domainMax;
delta = (1.0/9.0)*(x1-x0)/(pts);
for(int j=0;j<pts*10;j++){
xsmpl[idx] = x0 + delta*j;
idx++;
}
//Populate the results matrix at the sample points
RigidBodyDynamics::Math::VectorNd ax(1);
for(int i=0; i < xsmpl.size(); i++){
ax[0] = xsmpl[i];
results(i,0) = ax[0];
if(i==48){
double here = 1.0;
}
results(i,1) = calcValue(ax);
if(maxOrder>=1)
results(i,2) = calcDerivative(d1y,ax);
if(maxOrder>=2)
results(i,3) = calcDerivative(d2y,ax);
if(maxOrder>=3)
results(i,4) = calcDerivative(d3y,ax);
if(maxOrder>=4)
results(i,5) = calcDerivative(d4y,ax);
if(maxOrder>=5)
results(i,6) = calcDerivative(d5y,ax);
if(maxOrder>=6)
results(i,7) = calcDerivative(d6y,ax);
/*
if(_computeIntegral){
results(i,maxOrder+2) = calcIntegral(ax(0));
}
*/
}
return results;
}
void SmoothSegmentedFunction::getXControlPoints(
RigidBodyDynamics::Math::MatrixNd& mat) const
{
mat.resize(_mXVec.size(), _mXVec.at(0).size());
for(int i=0; i<_mXVec.size();++i){
for(int j=0; j<_mXVec.size();++j){
mat(i,j) = _mXVec.at(i)[j];
}
}
}
void SmoothSegmentedFunction::getYControlPoints(
RigidBodyDynamics::Math::MatrixNd& mat) const
{
mat.resize(_mYVec.size(), _mYVec.at(0).size());
for(int i=0; i<_mYVec.size();++i){
for(int j=0; j<_mYVec.size();++j){
mat(i,j) = _mYVec.at(i)[j];
}
}
}
/*Detailed Computational Costs
_______________________________________________________________________
Name Comp. Div. Mult. Add. Assign.
_______________________________________________________________________
*overhead (17+3*m 2 82 42+m 63)*3
51+9m 6 246 126+3m 189
calcValue 21 20 13
calcDerivative : dy/dx 1 40 38 22
: d2y/dx2 2 78 73 23
**calcIntegral 7 2 3 1
total per point 58+9m 9 387 260+3m 248
total per elbow 5.8k+900m 900 38.7k 26k+300m 24.8k
*Approximate. Overhead associated with finding the correct Bezier
spline section, and evaluating u(x).
Assumes 2 Newton iterations in calcU
**Approximate. Includes estimated cost of evaluating a cubic spline
with 100 knots
*/
void SmoothSegmentedFunction::printCurveToCSVFile(
const std::string& path,
const std::string& fileNameWithoutExtension,
double domainMin,
double domainMax) const
{
//Only compute up to the 2nd derivative
RigidBodyDynamics::Math::MatrixNd results =
calcSampledCurve(2,domainMin,domainMax);
std::vector<std::string> colNames(results.cols());
colNames[0] = "x";
colNames[1] = "y";
colNames[2] = "dy/dx";
colNames[3] = "d2y/dx2";
/*
if(results.cols() == 5){
colNames[4] = "int_y(x)";
}
*/
std::string fname = fileNameWithoutExtension;
fname.append(".csv");
printMatrixToFile(results,colNames,path,fname);
}
/*
This function will print cvs file of the column vector col0 and the matrix data
*/
void SmoothSegmentedFunction::
printMatrixToFile( RigidBodyDynamics::Math::MatrixNd& data,
std::vector<std::string>& colNames,
const std::string& path,
const std::string& filename) const
{
ofstream datafile;
std::string fullpath = path;
if(fullpath.length() > 0)
fullpath.append("/");
fullpath.append(filename);
datafile.open(fullpath.c_str(),std::ios::out);
if(!datafile){
datafile.close();
cerr << "SmoothSegmentedFunction::printMatrixToFile "
<< _name.c_str() << ": Failed to open the file path: "
<< fullpath.c_str()
<< endl;
assert(0);
abort();
}
for(int i = 0; i < (signed)colNames.size(); i++){
if(i < (signed)colNames.size()-1)
datafile << colNames[i] << ",";
else
datafile << colNames[i] << "\n";
}
for(int i = 0; i < data.rows(); i++){
for(int j = 0; j < data.cols(); j++){
if(j<data.cols()-1)
datafile << data(i,j) << ",";
else
datafile << data(i,j) << "\n";
}
}
datafile.close();
}