590 lines
22 KiB
C++
590 lines
22 KiB
C++
#ifndef SMOOTHSEGMENTEDFUNCTION_H_
|
|
#define SMOOTHSEGMENTEDFUNCTION_H_
|
|
/* -------------------------------------------------------------------------- *
|
|
* OpenSim: SmoothSegmentedFunction.h *
|
|
* -------------------------------------------------------------------------- *
|
|
* 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
|
|
|
|
*/
|
|
|
|
#include "Function.h"
|
|
#include "SegmentedQuinticBezierToolkit.h"
|
|
#include <vector>
|
|
#include <rbdl/rbdl_math.h>
|
|
|
|
|
|
|
|
/**
|
|
This class contains the quintic Bezier curves, x(u) and y(u), that have been
|
|
created by SmoothSegmentedFunctionFactory to follow a physiologically
|
|
meaningful muscle characteristic. A SmoothSegmentedFunction cannot be
|
|
created directly,you must use SmoothSegmentedFunctionFactory to create the
|
|
muscle curve of interest.
|
|
|
|
<B>Computational Cost Details</B>
|
|
All computational costs assume the following operation costs:
|
|
|
|
\verbatim
|
|
Operation Type : #flops
|
|
+,-,=,Boolean Op : 1
|
|
/ : 10
|
|
sqrt: 20
|
|
trig: 40
|
|
\endverbatim
|
|
|
|
These relative weightings will vary processor to processor, and so any
|
|
of the quoted computational costs are approximate.
|
|
|
|
|
|
<B> RBDL Port Notes </B>
|
|
The port of this code from OpenSim has been accompanied by a few changes:
|
|
|
|
1. The 'calcIntegral' method has been removed. Why? This function
|
|
relied on having access to a variable-step error controlled
|
|
integrator. There is no such integrator built into RBDL. Rather
|
|
than add a dependency (by using Boost perhaps) this functionality
|
|
has been removed.
|
|
|
|
2. The function name .printMuscleCurveToFile(...) has been changed
|
|
to .printCurveToFile().
|
|
|
|
|
|
@author Matt Millard
|
|
@version 0.0
|
|
|
|
|
|
*/
|
|
|
|
namespace RigidBodyDynamics {
|
|
namespace Addons {
|
|
namespace Geometry{
|
|
|
|
class SmoothSegmentedFunction : public Function_<double>
|
|
{
|
|
|
|
|
|
public:
|
|
|
|
///The default constructor, which populates the member data fields with
|
|
///NaN's
|
|
SmoothSegmentedFunction();
|
|
|
|
//SmoothSegmentedFunction();
|
|
/**
|
|
Creates a set of quintic Bezier curves.
|
|
|
|
@param mX The matrix of quintic Bezier x point locations (6xn).
|
|
Each column vector is the 6 control points required
|
|
for each quintic Bezier curve. For C0 continuity
|
|
adjacent columns must share the last and first control
|
|
points. For C1 continuity the last 2 and first two
|
|
control points of adjacent curves should be on the same
|
|
curve.
|
|
|
|
@param mY The matrix of quintic Bezier y point locations (6xn).
|
|
|
|
@param x0 The minimum x value. This is used for the linear
|
|
extrapolation of the Bezier curve. This parameter is
|
|
explicitly asked for, rather than computed, to prevent
|
|
rounding error from reducing the accuracy of the
|
|
linear extrapolation.
|
|
@param x1 The maximum x value. This is used for the linear
|
|
extrapolation and is required for the same reasons
|
|
as x0.
|
|
@param y0 The value of y(x) at x=x0. This is used for the linear
|
|
extrapolation and is required for the same reasons
|
|
as x0.
|
|
@param y1 The value of y(x) at x=x1. This is used for the linear
|
|
extrapolation and is required for the same reasons
|
|
as x0.
|
|
@param dydx0 The value of dy/dx at x=x0. This is used for the linear
|
|
extrapolation and is required for the same reasons
|
|
as x0.
|
|
@param dydx1 The value of dy/dx at x=x1. This is used for the linear
|
|
extrapolation and is required for the same reasons
|
|
as x0.
|
|
@param name The name of the data this SmoothSegmentedFunction. This name
|
|
is used to make human-readable error messages and to
|
|
generate sensible names when printing the curve to file.
|
|
|
|
<B>Computational Costs</B>
|
|
Generating the integral curve is not cheap, and so should only be used
|
|
when if it will be evaluated during a simulation.
|
|
\verbatim
|
|
Computatonal Cost Per Bezier Section:
|
|
Without Integral : 4,100 flops
|
|
\endverbatim
|
|
|
|
*/
|
|
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);
|
|
|
|
|
|
/**
|
|
Updates the Bezier curve conrol points. The updated curve must have
|
|
the same number of control points as the original curve.
|
|
|
|
@param mX The matrix of quintic Bezier x point locations (6xn).
|
|
Each column vector is the 6 control points required
|
|
for each quintic Bezier curve. For C0 continuity
|
|
adjacent columns must share the last and first control
|
|
points. For C1 continuity the last 2 and first two
|
|
control points of adjacent curves should be on the same
|
|
curve.
|
|
|
|
@param mY The matrix of quintic Bezier y point locations (6xn).
|
|
|
|
@param x0 The minimum x value. This is used for the linear
|
|
extrapolation of the Bezier curve. This parameter is
|
|
explicitly asked for, rather than computed, to prevent
|
|
rounding error from reducing the accuracy of the
|
|
linear extrapolation.
|
|
@param x1 The maximum x value. This is used for the linear
|
|
extrapolation and is required for the same reasons
|
|
as x0.
|
|
@param y0 The value of y(x) at x=x0. This is used for the linear
|
|
extrapolation and is required for the same reasons
|
|
as x0.
|
|
@param y1 The value of y(x) at x=x1. This is used for the linear
|
|
extrapolation and is required for the same reasons
|
|
as x0.
|
|
@param dydx0 The value of dy/dx at x=x0. This is used for the linear
|
|
extrapolation and is required for the same reasons
|
|
as x0.
|
|
@param dydx1 The value of dy/dx at x=x1. This is used for the linear
|
|
extrapolation and is required for the same reasons
|
|
as x0.
|
|
@param name The name of the data this SmoothSegmentedFunction. This name
|
|
is used to make human-readable error messages and to
|
|
generate sensible names when printing the curve to file.
|
|
|
|
*/
|
|
void 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);
|
|
|
|
/**
|
|
This function will shift the entire SmoothSegmentedFunction by xShift
|
|
and yShift. Setting xShift = yShift = 0.0 will leave the curve unchanged.
|
|
|
|
@param xShift - the amount to shift the curve in the x-direction.
|
|
@param yShift - the amount to shift the curve in the y-direction
|
|
*/
|
|
void shift(double xShift, double yShift);
|
|
|
|
/**
|
|
This function will scale the curve in the x and y directions. Setting
|
|
xScale=yScale=1.0 will leave the curve unchanged.
|
|
|
|
\b aborts \b
|
|
-If abs(xScale) < sqrt(eps)
|
|
|
|
@param xScale: the amount to scale the curve in the x direction
|
|
@param yScale: the amount to scale the curve in the y direction
|
|
*/
|
|
void scale(double xScale, double yScale);
|
|
|
|
|
|
|
|
/**Calculates the value of the curve this object represents.
|
|
|
|
@param x The domain point of interest
|
|
|
|
\b aborts \b
|
|
-If ax does not have a size of 1
|
|
@returns The value of the curve
|
|
|
|
|
|
The curve is parameterized as a set of Bezier curves. If x is within the
|
|
domain of these Bezier curves they will be evaluated. If x is outside
|
|
of the domain of these Bezier curves a linear extrapolation will be
|
|
evalulated
|
|
|
|
|
|
<B>Computational Costs</B>
|
|
\verbatim
|
|
x in curve domain : ~282 flops
|
|
x in linear section: ~5 flops
|
|
\endverbatim
|
|
*/
|
|
double calcValue(double x) const;
|
|
|
|
|
|
|
|
/**Calculates the value of the derivative of the curve this object
|
|
represents.
|
|
|
|
@param x The domain point of interest.
|
|
|
|
@param order The order of the derivative to compute. Note that order must
|
|
be between 0 and 2. Calling 0 just calls calcValue.
|
|
|
|
\b aborts \b
|
|
-If anything but 0's are stored in derivComponents
|
|
-If more than the 6th derivative is asked for
|
|
-If ax has a size other than 1
|
|
|
|
@return The value of the d^ny/dx^n th derivative evaluated at x
|
|
|
|
|
|
|
|
<B>Computational Costs</B>
|
|
\verbatim
|
|
x in curve domain : ~391 flops
|
|
x in linear section: ~2 flops
|
|
\endverbatim
|
|
|
|
*/
|
|
double calcDerivative(double x, int order) const;
|
|
|
|
|
|
|
|
|
|
/*This will return the value of the integral of this objects curve
|
|
evaluated at x.
|
|
|
|
@param x the domain point of interest
|
|
|
|
\b aborts \b
|
|
-If the function does not have a pre-computed integral
|
|
@return the value of the functions integral evaluated at x
|
|
|
|
The integral is approximate, though its errors are small.
|
|
The integral is computed by numerically integrating the function when
|
|
the constructor for this class is called (if computeIntegral is true) and
|
|
then splining the result, thus the regions between the knot points may
|
|
have some error in them. A very fine mesh of points is used to create the
|
|
spline so the errors will be small
|
|
|
|
<B>Computational Costs</B>
|
|
\verbatim
|
|
x in curve domain : ~13 flops
|
|
x in linear section: ~19 flops
|
|
\endverbatim
|
|
|
|
*/
|
|
//double calcIntegral(double x) const;
|
|
|
|
/*
|
|
Returns a bool that indicates if the integral curve has been computed.
|
|
|
|
@return true if the integral of this function is available, false if
|
|
it has not been computed.
|
|
*/
|
|
//bool isIntegralAvailable() const;
|
|
|
|
/*
|
|
Returns a bool that indicates if the integral computed is compuated left
|
|
to right, or right to left.
|
|
|
|
@return true if the integral was computed left to right, and false if the
|
|
integral was computed right to left. Note that the output of
|
|
this function is only valid if isIntegralAvailable() returns
|
|
true.
|
|
*/
|
|
//bool isIntegralComputedLeftToRight() const;
|
|
|
|
/**
|
|
Returns a string that is the name for this curve, which is set at the
|
|
time of construction and cannot be changed after construction.
|
|
|
|
@return The string name this object was given during construction*/
|
|
std::string getName() const;
|
|
|
|
/**
|
|
Sets the name of the SmoothSegmentedFunction object.
|
|
|
|
@param name The name of the data this SmoothSegmentedFunction. This name
|
|
is used to make human-readable error messages and to
|
|
generate sensible names when printing the curve to file.
|
|
*/
|
|
void setName(const std::string &name);
|
|
|
|
/**
|
|
This function returns a SimTK::Vec2 that contains in its 0th element
|
|
the lowest value of the curve domain, and in its 1st element the highest
|
|
value in the curve domain of the curve. Outside of this domain the curve
|
|
is approximated using linear extrapolation.
|
|
|
|
@return The minimum and maximum value of the domain, x, of the curve
|
|
y(x). Within this range y(x) is a curve, outside of this range
|
|
the function y(x) is a C2 (continuous to the second
|
|
derivative) linear extrapolation*/
|
|
RigidBodyDynamics::Math::VectorNd getCurveDomain() const;
|
|
|
|
/**This function will generate a csv file (of 'name_curveName.csv', where
|
|
name is the one used in the constructor) of the muscle curve, and
|
|
'curveName' corresponds to the function that was called from
|
|
SmoothSegmentedFunctionFactory to create the curve.
|
|
|
|
@param path The full path to the location. Note '/' slashes must be used,
|
|
and do not put a '/' after the last folder.
|
|
@param fileNameWithoutExtension The name of the file to write, not
|
|
including the file extension
|
|
@param domainMin
|
|
the left most domain point of the curve to print. The curve
|
|
will extend to at least this point.
|
|
@param domainMax
|
|
the right most domain point of the curve to print. The
|
|
printed curve will extend at least to this point, perhaps
|
|
beyond.
|
|
\b aborts \b
|
|
-If the filename is empty
|
|
|
|
For example the tendon
|
|
curve for a muscle named 'glutmax' will be:
|
|
|
|
'glutmax_tendonForceLengthCurve.csv'
|
|
|
|
The file will contain the following columns:
|
|
|
|
\verbatim
|
|
Col# 1, 2, 3, 4, 5
|
|
x, y, dy/dx, d2y/dx2, iy
|
|
\endverbatim
|
|
|
|
Where iy is the integral of y(x). If the curve has been set not to have
|
|
an integral, this column will not exist.
|
|
|
|
The curve will be sampled from its linear extrapolation region, through
|
|
the curve, out to the other linear extrapolation region. The width of
|
|
each linear extrapolation region is 10% of the entire range of x, or
|
|
0.1*(x1-x0).
|
|
|
|
The number of rows used will vary from curve to curve. Each quintic
|
|
Bezier curve section will have 100 samples. Each linearily extrapolated
|
|
region will have 10 samples each. Some muscle curves (the tendon,
|
|
parallel elements, compressive elements) consist of only 1 elbow, and so
|
|
these matrices will have only 100+20 rows. The force velocity curve is
|
|
made up of 2 elbows and will have 200+20 rows. The active force length
|
|
curve has 5 elbows, and so its sampled matrix will have 500+20 rows
|
|
|
|
<B>Computational Costs</B>
|
|
This varies depending on the curve (as mentioned above).
|
|
\verbatim
|
|
~97,400 to 487,000 flops
|
|
\endverbatim
|
|
|
|
<B>Example</B>
|
|
To read the csv file with a header in from Matlab, you need to use
|
|
csvread set so that it will ignore the header row. This is accomplished
|
|
by using the extra two numerical arguments for csvread to tell the
|
|
function to begin reading from the 1st row, and the 0th index (csvread
|
|
is 0 indexed).
|
|
\verbatim
|
|
data=csvread('test_tendonForceLengthCurve.csv',1,0);
|
|
\endverbatim
|
|
|
|
*/
|
|
void printCurveToCSVFile(const std::string& path,
|
|
const std::string& fileNameWithoutExtension,
|
|
double domainMin,
|
|
double domainMax) const;
|
|
|
|
|
|
/**
|
|
@param maxOrder The maximum derivative order to compute
|
|
@param domainMin
|
|
@param domainMax
|
|
\b aborts \b
|
|
-If the requested derivatve order is greater than
|
|
getMaxDerivativeOrder()
|
|
@returns a matrix populated with x,y,dy/dx ... d^ny/dx^n,iy
|
|
|
|
|
|
This function will generate a RigidBodyDynamics::Math::MatrixNd populated
|
|
with samples of the muscle curves values, derivatives (up to 6) and its
|
|
first integral (if available). The matrix has the following columns:
|
|
|
|
\verbatim
|
|
Col# 1, 2, 3, 4, 5, 6, 7, 8, 9,
|
|
x, y, dy/dx, d2y/dx2, d3y/dx3, d4y/dx4, d5y/dx5, d6y/dx6, iy
|
|
\endverbatim
|
|
|
|
Where iy is the integral of y(x). If the curve has been set not to have
|
|
an integral, this column will not exist.
|
|
|
|
The curve will be sampled from its
|
|
linear extrapolation region, through the curve, out to the other linear
|
|
extrapolation region. The width of each linear extrapolation region is
|
|
10% of the entire range of x, or 0.1*(x1-x0).
|
|
|
|
The rows used will vary from curve to curve. Each quintic Bezier curve
|
|
section will have 100 samples + 20 samples for the linear extrapolation
|
|
region. Some muscle curves (the tendon, parallel elements, compressive
|
|
elements) consist of only 1 elbow, and so these matrices will have only
|
|
100+20 rows. The force velocity curve is made up of 2 elbows and will
|
|
have 200+20 rows. The active force length curve has 5 elbows, and so its
|
|
sampled matrix will have 500+20 rows
|
|
|
|
*/
|
|
RigidBodyDynamics::Math::MatrixNd calcSampledCurve(int maxOrder,
|
|
double domainMin,
|
|
double domainMax) const;
|
|
|
|
|
|
void getXControlPoints(RigidBodyDynamics::Math::MatrixNd& mat) const;
|
|
void getYControlPoints(RigidBodyDynamics::Math::MatrixNd& mat) const;
|
|
|
|
private:
|
|
|
|
|
|
/**Nov 2015: Not needed in the RBDL port.
|
|
Array of spline fit functions X(u) for each Bezier elbow*/
|
|
//Nov 2015: Not needed in the RBDL port.
|
|
//SimTK::Array_<SimTK::Spline> _arraySplineUX;
|
|
/**Nov 2015: Not included in the RBDL port (RBDL doesn't have an
|
|
error controlled integrator to generate this curve)
|
|
Spline fit of the integral of the curve y(x)*/
|
|
//SimTK::Spline _splineYintX;
|
|
|
|
/**Bezier X1,...,Xn control point locations. Control points are
|
|
stored in 6x1 vectors in the order above*/
|
|
std::vector<RigidBodyDynamics::Math::VectorNd> _mXVec;
|
|
/**Bezier Y1,...,Yn control point locations. Control points are
|
|
stored in 6x1 vectors in the order above*/
|
|
std::vector<RigidBodyDynamics::Math::VectorNd> _mYVec;
|
|
|
|
/**The number of quintic Bezier curves that describe the relation*/
|
|
int _numBezierSections;
|
|
|
|
/**The minimum value of the domain*/
|
|
double _x0;
|
|
/**The maximum value of the domain*/
|
|
double _x1;
|
|
/**The minimum value of the range*/
|
|
double _y0;
|
|
/**The maximum value of the range*/
|
|
double _y1;
|
|
/**The slope at _x0*/
|
|
double _dydx0;
|
|
/**The slope at _x1*/
|
|
double _dydx1;
|
|
/*This is the users */
|
|
//bool _computeIntegral;
|
|
|
|
/*This variable, when true, indicates that the user wants the integral
|
|
from left to right (x0 to x1). If it is false, the integral from right
|
|
to left (x1 to x0) is computed*/
|
|
//bool _intx0x1;
|
|
/**The name of the function**/
|
|
std::string _name;
|
|
|
|
/**No human should be constructing a SmoothSegmentedFunction, so the
|
|
constructor is made private so that mere mortals cannot look at it.
|
|
SmoothSegmentedFunctionFactory should be used to create
|
|
MuscleCurveFunctions and that's why its a friend*/
|
|
friend class SmoothSegmentedFunctionFactory;
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
This function will print cvs file of the column vector col0 and the
|
|
matrix data
|
|
|
|
@param data A matrix of data
|
|
@param colnames Array of column headings
|
|
@param path The desired path to the folder to write the file
|
|
@param filename The name of the file to print
|
|
\b aborts \b
|
|
-If the desired file cannot be created and openened, perhaps
|
|
because the path doesn't exist.
|
|
*/
|
|
void printMatrixToFile( RigidBodyDynamics::Math::MatrixNd& data,
|
|
std::vector<std::string>& colnames,
|
|
const std::string& path,
|
|
const std::string& filename) const;
|
|
|
|
/**
|
|
Refer to the documentation for calcValue(double x)
|
|
because this function is identical in function to
|
|
calcValue(double x), but requires different inputs.
|
|
This is a required virtual function required because this class extends
|
|
the Function interface.
|
|
*/
|
|
double calcValue(const RigidBodyDynamics::Math::VectorNd& x) const;
|
|
/*virtual*/
|
|
|
|
/** Refer to the documentation for calcDerivative(double x, int order)
|
|
because this function is identical in function to
|
|
calcDerivative(double x, int order), but requires different inputs.
|
|
This is a required virtual function required because this class extends
|
|
the Function interface.*/
|
|
double calcDerivative(const std::vector<int>& derivComponents,
|
|
const RigidBodyDynamics::Math::VectorNd& x) const;
|
|
/*virtual*/
|
|
|
|
/**This will return the size of the vector that the
|
|
calcValue(const RigidBodyDynamics::Math::VectorNd& x) require. This is a
|
|
required virtual function required because this class extends the
|
|
Function interface, though is only needed if you call
|
|
|
|
double calcValue(const RigidBodyDynamics::Math::VectorNd& x) const;
|
|
|
|
or
|
|
|
|
double calcDerivative( const SimTK::Array_<int>& derivComponents,
|
|
const RigidBodyDynamics::Math::VectorNd& x) const;
|
|
|
|
Since this class is implementing strictly scalar functions you can use
|
|
the simplified versions of calcValue(double x) and
|
|
calcDerivative(double x, int order) instead.
|
|
|
|
*/
|
|
int getArgumentSize() const; /*virtual*/
|
|
|
|
/**@return The maximum order derivative that this object is capable of
|
|
returning*/
|
|
/*virtual*/
|
|
int getMaxDerivativeOrder() const;
|
|
|
|
|
|
};
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
#endif
|