bool qr(const mat &A, mat &Q, mat &R)
{
int info;
int m = A.rows();
int n = A.cols();
int lwork = n;
int k = std::min(m, n);
vec tau(k);
vec work(lwork);
R = A;
// perform workspace query for optimum lwork value
int lwork_tmp = -1;
dgeqrf_(&m, &n, R._data(), &m, tau._data(), work._data(), &lwork_tmp,
&info);
if (info == 0) {
lwork = static_cast<int>(work(0));
work.set_size(lwork, false);
}
dgeqrf_(&m, &n, R._data(), &m, tau._data(), work._data(), &lwork, &info);
Q = R;
Q.set_size(m, m, true);
// construct R
for (int i = 0; i < m; i++)
for (int j = 0; j < std::min(i, n); j++)
R(i, j) = 0;
// perform workspace query for optimum lwork value
lwork_tmp = -1;
dorgqr_(&m, &m, &k, Q._data(), &m, tau._data(), work._data(), &lwork_tmp,
&info);
if (info == 0) {
lwork = static_cast<int>(work(0));
work.set_size(lwork, false);
}
dorgqr_(&m, &m, &k, Q._data(), &m, tau._data(), work._data(), &lwork,
&info);
return (info == 0);
}
/*!
* \brief The second partial derivative of the Gibbs free energy
* to the normalized pressure for IAPWS region 2
* (i.e. sub-critical steam) (dimensionless).
*
* \param temperature temperature of component in \f$\mathrm{[K]}\f$
* \param pressure pressure of component in \f$\mathrm{[Pa]}\f$
*
* IAPWS: "Revised Release on the IAPWS Industrial Formulation
* 1997 for the Thermodynamic Properties of Water and Steam",
* http://www.iapws.org/relguide/IF97-Rev.pdf
*/
static Scalar ddgamma_ddpi(Scalar temperature, Scalar pressure)
{
Scalar tau_ = tau(temperature); /* reduced temperature */
Scalar pi_ = pi(pressure); /* reduced pressure */
// ideal gas part
Scalar result = -1/(pi_*pi_);
// residual part
for (int i = 0; i < 43; i++) {
result +=
n_r(i) *
I_r(i) *
(I_r(i) - 1) *
std::pow(pi_, I_r(i) - 2) *
std::pow(tau_ - 0.5, J_r(i));
}
return result;
}
void main(void) {
cout << "\nTesting constructor";
Perm sigma;
Perm tau(3);
list<Perm> Sn;
cout << "\nGetting list of all permutations of a 4 element set";
Sn = Perms(3);
char ch = getchar();
Sn = Perms(4);
cout << "\nThere were " << Sn.size() << " permutations";
cout << "\n Here they are along with their signs";
for (list<Perm>::iterator iter = Sn.begin(); iter != Sn.end(); ++iter) {
cout << "\n" << *iter;
cout << " has sign " << sign(*iter);
}
cout << "\nThere were " << Sn.size() << " permutations";
// Also will test PermMake here
list<int> A, B;
A.push_back(1);
A.push_back(3);
A.push_back(2);
B.push_back(2);
B.push_back(3);
B.push_back(1);
Perm sigma2 = PermMake(A, B);
cout << sigma2;
int i;
cin >> i;
}
void qr<T>::output(array_2d<T>& q, array_2d<T>& r) const
{
array<T> tau(min_nm);
if(r.rows() != m || r.columns() != n)
r.resize(idx(m, n));
lapack_interface<T>::call_lapack_geqrf(r, tau, a);
//Now construct Q
//Q should be a MxM matrix
if(q.rows() != m || q.columns() != m)
q.resize(idx(m, m));
if(n <= m) {
//Q can hold the MxN values of R needed by lapack's xORGQR routines
for(size_t i = 0; i < m; i++)
for(size_t j = 0; j < n; j++)
q[m*j + i] = r(i,j);
lapack_interface<T>::call_lapack_orgqr(q, tau);
} else {
//create a new array and do a copy since
//Q cannot hold the MxN values of R as needed by lapack's xORGQR routines
array_2d<T> temp_q(r(all(), size_range(0, m - 1)));
lapack_interface<T>::call_lapack_orgqr(temp_q, tau);
//copy the Q values from temp
for(size_t i = 0; i < m; i++)
for(size_t j = 0; j < m; j++)
q[m*j + i] = temp_q[m*j + i];
}
// R is upper triangular
for(size_t i = 0; i < m; i++)
for(size_t j = 0; j < n; j++)
if(i > j)
r(i,j) = 0;
}
/// Fill the nrows by ncols matrix Q (in column-major order, with
/// leading dimension ldq >= nrows) with a random orthogonal
/// matrix.
///
/// \note If you want the resulting Q factor to be from a Haar
/// distribution (the usual one for random orthogonal matrices),
/// Generator must have a normal distribution.
void
explicit_Q (const Ordinal nrows,
const Ordinal ncols,
Scalar Q[],
const Ordinal ldq)
{
// Fill Q with random numbers
this->fill_random (nrows, ncols, Q, ldq);
// Get ready for QR factorization
LAPACK< Ordinal, Scalar > lapack;
std::vector< Scalar > tau (std::min(nrows, ncols));
// Workspace query
Scalar _lwork1, _lwork2;
int info = 0;
lapack.GEQRF (nrows, ncols, Q, ldq, &tau[0], &_lwork1, -1, &info);
if (info != 0)
throw std::logic_error("LAPACK GEQRF LWORK query failed");
lapack.ORGQR (nrows, ncols, ncols, Q, ldq, &tau[0], &_lwork2, -1, &info);
if (info != 0)
throw std::logic_error("LAPACK ORGQR LWORK query failed");
// Allocate workspace. abs() returns a magnitude_type, and we
// can compare those using std::max. If Scalar is complex,
// you can't compare it using max.
const Ordinal lwork = checkedCast (std::max (ScalarTraits< Scalar >::abs (_lwork1),
ScalarTraits< Scalar >::abs (_lwork2)));
std::vector< Scalar > work (lwork);
// Factor the input matrix
lapack.GEQRF (nrows, ncols, Q, ldq, &tau[0], &work[0], lwork, &info);
if (info != 0)
throw std::runtime_error("LAPACK GEQRF failed");
// Compute explicit Q factor in place
lapack.ORGQR (nrows, ncols, ncols, Q, ldq, &tau[0], &work[0], lwork, &info);
if (info != 0)
throw std::runtime_error("LAPACK ORGQR failed");
}
primitive_vars conserved_vars::to_primitive() const {
const integer sz = rho().size();
primitive_vars v(sz);
v.rho() = rho();
v.ek() = real(0);
for (integer dim = 0; dim != NDIM; ++dim) {
v.v(dimension(dim)) = s(dimension(dim)) / rho();
v.ek() += v.v(dimension(dim)) * s(dimension(dim)) / real(2);
}
for (integer i = 0; i != sz; ++i) {
if ((egas()[i] - v.ek()[i]) < dual_energy_switch2 * egas()[i]) {
v.ei()[i] = std::pow(tau()[i], fgamma);
} else {
v.ei()[i] = std::max(egas()[i] - v.ek()[i], real(0));
}
}
v.p() = (fgamma - real(1)) * v.ei();
v.h() = (egas() + v.p()) / rho();
v.c() = std::sqrt(fgamma * v.p() / v.rho());
return v;
}
/*
* Make sure open loop outputs the same as the input.
*/
TEST_F(OpenLoopControllerTest, DirectFeedthrough)
{
uranus_dp::SetControlMode srv;
srv.request.mode = ControlModes::OPEN_LOOP;
if (!client.call(srv))
ROS_ERROR("Failed to call service set_control_mode. New mode OPEN_LOOP not set.");
PublishSetpoint(-9.966, -7.907, 7.626, -6.023, 1.506, 2.805);
WaitForMessage();
EXPECT_NEAR(tau(0), -9.966, EPSILON);
EXPECT_NEAR(tau(1), -7.907, EPSILON);
EXPECT_NEAR(tau(2), 7.626, EPSILON);
EXPECT_NEAR(tau(3), -6.023, EPSILON);
EXPECT_NEAR(tau(4), 1.506, EPSILON);
EXPECT_NEAR(tau(5), 2.805, EPSILON);
}
df1b2variable log_negbinomial_density(double x,const df1b2variable& _xmu,
const df1b2variable& _xtau)
{
ADUNCONST(df1b2variable,xmu)
ADUNCONST(df1b2variable,xtau)
init_df3_two_variable mu(xmu);
init_df3_two_variable tau(xtau);
*mu.get_u_x()=1.0;
*tau.get_u_y()=1.0;
if (value(tau)-1.0<0.0)
{
cerr << "tau <=1 in log_negbinomial_density " << endl;
ad_exit(1);
}
df3_two_variable r=mu/(1.e-120+(tau-1.0));
df3_two_variable tmp;
tmp=gammln(x+r)-gammln(r) -gammln(x+1)
+r*log(r)+x*log(mu)-(r+x)*log(r+mu);
df1b2variable tmp1;
tmp1=tmp;
return tmp1;
}
void mtsIntuitiveResearchKitMTM::RunClutch(void)
{
// J1-J3
vctDoubleVec q(7, 0.0);
vctDoubleVec qd(7, 0.0);
vctDoubleVec tau(7, 0.0);
q.Assign(JointGet.Ref(7));
vctDoubleVec torqueDesired(8, 0.0);
tau.ForceAssign(Manipulator.CCG(q, qd));
tau[0] = q(0) * 0.0564 + 0.08;
torqueDesired.Ref(7).Assign(tau);
TorqueSet.SetForceTorque(torqueDesired);
PID.SetTorqueJoint(TorqueSet);
// J4-J7
JointSet.Assign(JointGet);
CartesianClutched.Translation().Assign(CartesianGet.Translation());
Manipulator.InverseKinematics(JointSet, CartesianClutched);
SetPositionJointLocal(JointSet);
}
void SurfaceL2ProjectionResidual<EvalT, Traits>::
evaluateFields(typename Traits::EvalData workset)
{
// THESE NEED TO BE REMOVED!!!
typedef Intrepid::FunctionSpaceTools FST;
typedef Intrepid::RealSpaceTools<ScalarT> RST;
ScalarT tau(0);
// Initialize the residual
for (int cell(0); cell < workset.numCells; ++cell) {
for (int node(0); node < numPlaneNodes; ++node) {
int topNode = node + numPlaneNodes;
projection_residual_(cell, node) = 0;
projection_residual_(cell, topNode) = 0;
}
}
for (int cell(0); cell < workset.numCells; ++cell) {
for (int node(0); node < numPlaneNodes; ++node) {
int topNode = node + numPlaneNodes;
for (int pt=0; pt < numQPs; ++pt) {
tau = 0.0;
for (int dim=0; dim <numDims; ++dim){
tau += detF_(cell,pt)*Cauchy_stress_(cell, pt, dim, dim)/numDims;
}
projection_residual_(cell, node) += refValues(node,pt)*
(projected_tau_(cell,pt) - tau)*
refArea(cell,pt);
}
projection_residual_(cell, topNode) = projection_residual_(cell, node);
}
}
}
Divisors divisors(const PrimePowers& pp, bool sorted=true) const {
uint32 size = tau(pp);
auto ret = Divisors(size);
ret[0] = 1;
uint32 curr_size = 1;
for (auto p : pp) {
uint32 next_size = curr_size;
num_t q = p.p;
for (uint32 e = 1; e <= p.e; ++e) {
for (uint32 i = 0; i < curr_size; ++i) {
ret[next_size++] = ret[i] * q;
}
q *= p.p;
}
curr_size = next_size;
}
if (sorted) {
std::sort(ret.begin(), ret.end());
}
return ret;
}
int do_memory_type(int n, W workspace) {
typedef typename bindings::remove_imaginary<T>::type real_type ;
typedef std::complex< real_type > complex_type ;
typedef ublas::matrix<T, ublas::column_major> matrix_type ;
typedef ublas::vector<T> vector_type ;
// Set matrix
matrix_type a( n, n );
vector_type tau( n );
randomize( a );
matrix_type a2( a );
matrix_type a3( a );
// Compute QR factorization.
lapack::geqrf( a, tau, workspace ) ;
// Apply the orthogonal transformations to a2
if( boost::is_complex<T>::value ) {
lapack::unmqr( tag::left(), bindings::conj( a ), tau, a2, workspace );
} else {
lapack::unmqr( tag::left(), bindings::trans( a ), tau, a2, workspace );
}
// The upper triangular parts of a and a2 must be equal.
if (norm_frobenius( upper_part( a - a2 ) )
> std::numeric_limits<real_type>::epsilon() * 10.0 * norm_frobenius( upper_part( a ) ) ) return 255 ;
// Generate orthogonal matrix
lapack::ungqr( a, tau, workspace );
// The result of lapack::ormqr and the equivalent matrix product must be equal.
if (norm_frobenius( a2 - prod(herm(a), a3) )
> std::numeric_limits<real_type>::epsilon() * 10.0 * norm_frobenius( a2 ) ) return 255 ;
return 0 ;
} // do_value_type()
double tau(double x)
/*
derivative of psi
*/
{
double y, zz, term ;
int k ;
if (x<=0.0) fatalx("(tau) bad value: %9.3f\n", x) ;
bernload() ;
if (x<10.0) return (tau(x+1.0) + 1.0/(x*x)) ;
y = 1.0/x + 1.0/(2.0*x*x) ;
zz = 1.0/x ;
for (k=1; k<= bernmax/2 ; k++) {
zz /= (x*x) ;
term = bernum(2*k)/(double) (2*k) ;
term *= zz ;
term *= - (double) (2*k) ;
y -= term ;
}
return y ;
}
void Foam::sixDoFRigidBodyMotion::applyRestraints()
{
if (restraints_.empty())
{
return;
}
if (Pstream::master())
{
forAll(restraints_, rI)
{
if (report_)
{
Info<< "Restraint " << restraints_[rI].name() << ": ";
}
// Restraint position
point rP = Zero;
// Restraint force
vector rF = Zero;
// Restraint moment
vector rM = Zero;
// Accumulate the restraints
restraints_[rI].restrain(*this, rP, rF, rM);
// Update the acceleration
a() += rF/mass_;
// Moments are returned in global axes, transforming to
// body local to add to torque.
tau() += Q().T() & (rM + ((rP - centreOfRotation()) ^ rF));
}
}
}
void
Optimize::inter_swap(size_t times) {
msg.log << tau("before sort by size");
sort_by_size();
msg.log << tau("before decrease");
decrease_truck();
msg.log << tau("after decrease");
sort_by_size();
msg.log << tau("after sort by size");
size_t i = 0;
while ((i++ < times) && inter_swap()) {
msg.log << tau("after inter swap");
msg.log << "\n***************************" << i;
std::rotate(fleet.begin(), fleet.begin() + 1, fleet.end());
msg.log << tau("before next cycle");
}
}
void doAvoidance() {
// Start of user code Avoidance
static int x = 100;
std_msgs::Float32 manip;
Eigen::Map<Vector7d> phi(&jointPosition[0]);
Eigen::Map<Vector7d> tau(&jointTorqueCommand[0]);
port_JointPosition.read(jointPosition);
tau = manipulablityG(phi, prop_threshold, 10.0);
if(--x < 0) {
manip.data = manipulability(phi);
port_Manipulability.write(manip);
// std::cout << "manipulability : " << manipulability(phi) << std::endl;
// std::cout << "torque : " << tau.transpose() << std::endl << std::endl;
x = 20;
}
port_JointTorqueCommand.write(jointTorqueCommand);
// End of user code
}
Foam::point Foam::sixDoFRigidBodyMotion::predictedPosition
(
const point& pInitial,
const vector& deltaForce,
const vector& deltaMoment,
scalar deltaT
) const
{
vector vTemp = v() + deltaT*(a() + deltaForce/mass_);
vector piTemp = pi() + deltaT*(tau() + (Q().T() & deltaMoment));
point centreOfMassTemp = centreOfMass() + deltaT*vTemp;
tensor QTemp = Q();
rotate(QTemp, piTemp, deltaT);
return
(
centreOfMassTemp
+ (QTemp & initialQ_.T() & (pInitial - initialCentreOfMass_))
);
}
/* 'Data' Matrix of data containing observation data in rows, one
* row per observation and complying with this format:
* x y z P
* Where x,y,z are satellite coordinates in an ECEF system
* and P is pseudorange (corrected as much as possible,
* specially from satellite clock errors), all expresed
* in meters.
*
* 'X' Vector of position solution, in meters. There may be
* another solution, that may be accessed with vector
* "SecondSolution" if "ChooseOne" is set to "false".
*
* Return values:
* 0 Ok
* -1 Not enough good data
* -2 Singular problem
*/
int Bancroft::Compute( Matrix<double>& Data,
Vector<double>& X )
throw(Exception)
{
try
{
int N = Data.rows();
Matrix<double> B(0,4); // Working matrix
// Let's test the input data
if( testInput )
{
double satRadius = 0.0;
// Check each row of B Matrix
for( int i=0; i < N; i++ )
{
// If Data(i,3) -> Pseudorange is NOT between the allowed
// range, then drop line immediately
if( !( (Data(i,3) >= minPRange) && (Data(i,3) <= maxPRange) ) )
{
continue;
}
// Let's compute distance between Earth center and
// satellite position
satRadius = RSS(Data(i,0), Data(i,1) , Data(i,2));
// If satRadius is NOT between the allowed range, then drop
// line immediately
if( !( (satRadius >= minRadius) && (satRadius <= maxRadius) ) )
{
continue;
}
// If everything is ok so far, then extract the good
// data row and add it to working matrix
MatrixRowSlice<double> goodRow(Data,i);
B = B && goodRow;
}
// Let's redefine "N" and check if we have enough data rows
// left in a single step
if( (N = B.rows()) < 4 )
{
return -1; // We need at least 4 data rows
}
} // End of 'if( testInput )...'
else
{
// No input filtering. Working matrix (B) and
// input matrix (Data) are equal
B = Data;
}
Matrix<double> BT=transpose(B);
Matrix<double> BTBI(4,4), M(4,4,0.0);
Vector<double> aux(4), alpha(N), solution1(4), solution2(4);
// Temporary storage for BT*B. It will be inverted later
BTBI = BT * B;
// Let's try to invert BTB matrix
try
{
BTBI = inverseChol( BTBI );
}
catch(...)
{
return -2;
}
// Now, let's compute alpha vector
for( int i=0; i < N; i++ )
{
// First, fill auxiliar vector with corresponding satellite
// position and pseudorange
aux(0) = B(i,0);
aux(1) = B(i,1);
aux(2) = B(i,2);
aux(3) = B(i,3);
alpha(i) = 0.5 * Minkowski(aux, aux);
}
Vector<double> tau(N,1.0), BTBIBTtau(4), BTBIBTalpha(4);
BTBIBTtau = BTBI * BT * tau;
BTBIBTalpha = BTBI * BT * alpha;
// Now, let's find the coeficients of the second order-equation
double a(Minkowski(BTBIBTtau, BTBIBTtau));
double b(2.0 * (Minkowski(BTBIBTtau, BTBIBTalpha) - 1.0));
double c(Minkowski(BTBIBTalpha, BTBIBTalpha));
// Calculate discriminant and exit if negative
double discriminant = b*b - 4.0 * a * c;
if (discriminant < 0.0)
{
return -2;
}
// Find possible DELTA values
double DELTA1 = ( -b + SQRT(discriminant) ) / ( 2.0 * a );
double DELTA2 = ( -b - SQRT(discriminant) ) / ( 2.0 * a );
// We need to define M matrix
M(0,0) = 1.0;
M(1,1) = 1.0;
M(2,2) = 1.0;
M(3,3) = - 1.0;
// Find possible position solutions with their implicit radii
solution1 = M * BTBI * ( BT * DELTA1 * tau + BT * alpha );
double radius1(RSS(solution1(0), solution1(1), solution1(2)));
solution2 = M * BTBI * ( BT * DELTA2 * tau + BT * alpha );
double radius2(RSS(solution2(0), solution2(1), solution2(2)));
// Let's choose the right solution
if ( ChooseOne )
{
if ( ABS(CloseTo-radius1) < ABS(CloseTo-radius2) )
{
X = solution1;
}
else
{
X = solution2;
}
}
else
{
// Both solutions will be reported
X = solution1;
SecondSolution = solution2;
}
return 0;
} // end of first "try"
catch(Exception& e)
{
GPSTK_RETHROW(e);
}
} // end Bancroft::Compute()
void ISVDMultiCD::makePass() {
Epetra_LAPACK lapack;
Epetra_BLAS blas;
bool firstPass = (curRank_ == 0);
const int numCols = A_->NumVectors();
TEUCHOS_TEST_FOR_EXCEPTION( !firstPass && (numProc_ != numCols), std::logic_error,
"RBGen::ISVDMultiCD::makePass(): after first pass, numProc should be numCols");
// compute W = I - Z T Z^T from current V_
Teuchos::RCP<Epetra_MultiVector> lclAZT, lclZ;
double *Z_A, *AZT_A;
int Z_LDA, AZT_LDA;
int oldRank = 0;
double Rerr = 0.0;
if (!firstPass) {
// copy V_ into workZ_
lclAZT = Teuchos::rcp( new Epetra_MultiVector(::View,*workAZT_,0,curRank_) );
lclZ = Teuchos::rcp( new Epetra_MultiVector(::View,*workZ_,0,curRank_) );
{
Epetra_MultiVector lclV(::View,*V_,0,curRank_);
*lclZ = lclV;
}
// compute the Householder QR factorization of the current right basis
// Vhat = W*R
int info, lwork = curRank_;
std::vector<double> tau(curRank_), work(lwork);
info = lclZ->ExtractView(&Z_A,&Z_LDA);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error,
"RBGen::ISVDMultiCD::makePass(): error calling ExtractView on Epetra_MultiVector Z.");
lapack.GEQRF(numCols,curRank_,Z_A,Z_LDA,&tau[0],&work[0],lwork,&info);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error,
"RBGen::ISVDMultiCD::makePass(): error calling GEQRF on current right basis while constructing next pass coefficients.");
if (debug_) {
// we just took the QR factorization of a set of orthonormal vectors
// they should have an R factor which is diagonal, with unit elements (\pm 1)
// check it
Rerr = 0.0;
for (int j=0; j<curRank_; j++) {
for (int i=0; i<j; i++) {
Rerr += abs(Z_A[j*Z_LDA+i]);
}
Rerr += abs(abs(Z_A[j*Z_LDA+j]) - 1.0);
}
}
// compute the block representation
// W = I - Z T Z^T
lapack.LARFT('F','C',numCols,curRank_,Z_A,Z_LDA,&tau[0],workT_->A(),workT_->LDA());
// LARFT left upper tri block of Z unchanged
// note: it should currently contain R factor of V_, which is very close to
// diag(\pm 1, ..., \pm 1)
//
// we need to set it to:
// [1 0 0 ... 0]
// [ 1 0 ... 0]
// [ .... ]
// [ 1]
//
// see documentation for LARFT
//
for (int j=0; j<curRank_; j++) {
Z_A[j*Z_LDA+j] = 1.0;
for (int i=0; i<j; i++) {
Z_A[j*Z_LDA+i] = 0.0;
}
}
// compute part of A W: A Z T
// put this in workAZT_
// first, A Z
info = lclAZT->Multiply('N','N',1.0,*A_,*lclZ,0.0);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0,std::logic_error,
"RBGen::ISVDMultiCD::makePass(): Error calling Epetra_MultiVector::Multiply() for A*Z");
// second, (A Z) T (in situ, as T is upper triangular)
info = lclAZT->ExtractView(&AZT_A,&AZT_LDA);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error,
"RBGen::ISVDMultiCD::makePass(): error calling ExtractView on Epetra_MultiVector AZ.");
blas.TRMM('R','U','N','N',numCols,curRank_,1.0,workT_->A(),workT_->LDA(),AZT_A,AZT_LDA);
// save oldRank: it tells us the width of Z
oldRank = curRank_;
curRank_ = 0;
numProc_ = 0;
}
else { // firstPass == true
curRank_ = 0;
numProc_ = 0;
}
while (numProc_ < numCols) {
//
// determine lup
//
// want lup >= lmin
// lup <= lmax
// need lup <= numCols - numProc
// lup <= maxBasisSize - curRank
//
int lup;
if (curRank_ == 0) {
// first step uses startRank_
// this is not affected by lmin,lmax
lup = startRank_;
}
else {
// this value minimizes overall complexity, assuming fixed rank
lup = (int)(curRank_ / Teuchos::ScalarTraits<double>::squareroot(2.0));
// contrain to [lmin,lmax]
lup = (lup < lmin_ ? lmin_ : lup);
lup = (lup > lmax_ ? lmax_ : lup);
}
//
// now cap lup via maxBasisSize and the available data
// these caps apply to all lup, as a result of memory and data constraints
//
// available data
lup = (lup > numCols - numProc_ ? numCols - numProc_ : lup);
// available memory
lup = (lup > maxBasisSize_ - curRank_ ? maxBasisSize_ - curRank_ : lup);
// get view of new vectors
{
const Epetra_MultiVector Aplus(::View,*A_,numProc_,lup);
Epetra_MultiVector Unew(::View,*U_,curRank_,lup);
// put them in U
if (firstPass) {
// new vectors are just Aplus
Unew = Aplus;
}
else {
// new vectors are Aplus - (A Z T) Z_i^T
// specifically, Aplus - (A Z T) Z(numProc:numProc+lup-1,1:oldRank)^T
Epetra_LocalMap lclmap(lup,0,A_->Comm());
Epetra_MultiVector Zi(::View,lclmap,&Z_A[numProc_],Z_LDA,oldRank);
Unew = Aplus;
int info = Unew.Multiply('N','T',-1.0,*lclAZT,Zi,1.0);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0,std::logic_error,
"RBGen::ISVDMultiCD::makePass(): Error calling Epetra_MultiVector::Multiply() for A*Wi");
}
}
// perform the incremental step
incStep(lup);
}
// compute W V = V - Z T Z^T V
// Z^T V is oldRank x curRank
// T Z^T V is oldRank x curRank
// we need T Z^T V in a local Epetra_MultiVector
if (!firstPass) {
Teuchos::RCP<Epetra_MultiVector> lclV;
double *TZTV_A;
int TZTV_LDA;
int info;
Epetra_LocalMap lclmap(oldRank,0,A_->Comm());
// get pointer to current V
lclV = Teuchos::rcp( new Epetra_MultiVector(::View,*V_,0,curRank_) );
// create space for T Z^T V
Epetra_MultiVector TZTV(lclmap,curRank_,false);
// multiply Z^T V
info = TZTV.Multiply('T','N',1.0,*lclZ,*lclV,0.0);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0,std::logic_error,
"RBGen::ISVDMultiCD::makePass(): Error calling Epetra_MultiVector::Multiply() for Z^T V.");
// get pointer to data in Z^T V
info = TZTV.ExtractView(&TZTV_A,&TZTV_LDA);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error,
"RBGen::ISVDMultiCD::makePass(): error calling ExtractView on Epetra_MultiVector TZTV.");
// multiply T (Z^T V)
blas.TRMM('L','U','N','N',oldRank,curRank_,1.0,workT_->A(),workT_->LDA(),TZTV_A,TZTV_LDA);
// multiply V - Z (T Z^T V)
info = lclV->Multiply('N','N',-1.0,*lclZ,TZTV,1.0);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0,std::logic_error,
"RBGen::ISVDMultiCD::makePass(): Error calling Epetra_MultiVector::Multiply() for W V.");
}
//
// compute the new residuals
// we know that A V = U S
// if, in addition, A^T U = V S, then have singular subspaces
// check residuals A^T U - V S, scaling the i-th column by sigma[i]
//
{
// make these static, because makePass() will be likely be called again
static Epetra_LocalMap lclmap(A_->NumVectors(),0,A_->Comm());
static Epetra_MultiVector ATU(lclmap,maxBasisSize_,false);
// we know that A V = U S
// if, in addition, A^T U = V S, then have singular subspaces
// check residuals A^T U - V S, scaling the i-th column by sigma[i]
Epetra_MultiVector ATUlcl(::View,ATU,0,curRank_);
Epetra_MultiVector Ulcl(::View,*U_,0,curRank_);
Epetra_MultiVector Vlcl(::View,*V_,0,curRank_);
// compute A^T U
int info = ATUlcl.Multiply('T','N',1.0,*A_,Ulcl,0.0);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error,
"RBGen::ISVDMultiCD::makePass(): Error calling Epetra_MultiVector::Multiply for A^T U.");
Epetra_LocalMap rankmap(curRank_,0,A_->Comm());
Epetra_MultiVector S(rankmap,curRank_,true);
for (int i=0; i<curRank_; i++) {
S[i][i] = sigma_[i];
}
// subtract V S from A^T U
info = ATUlcl.Multiply('N','N',-1.0,Vlcl,S,1.0);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error,
"RBGen::ISVDMultiCD::computeBasis(): Error calling Epetra_MultiVector::Multiply for V S.");
resNorms_.resize(curRank_);
ATUlcl.Norm2(&resNorms_[0]);
// scale by sigmas
for (int i=0; i<curRank_; i++) {
if (sigma_[i] != 0.0) {
resNorms_[i] /= sigma_[i];
}
}
}
// debugging checks
std::vector<double> errnorms(curRank_);
if (debug_) {
int info;
// Check that A V = U Sigma
// get pointers to current U and V, create workspace for A V - U Sigma
Epetra_MultiVector work(U_->Map(),curRank_,false),
curU(::View,*U_,0,curRank_),
curV(::View,*V_,0,curRank_);
// create local MV for sigmas
Epetra_LocalMap lclmap(curRank_,0,A_->Comm());
Epetra_MultiVector curS(lclmap,curRank_,true);
for (int i=0; i<curRank_; i++) {
curS[i][i] = sigma_[i];
}
info = work.Multiply('N','N',1.0,curU,curS,0.0);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0,std::logic_error,
"RBGen::ISVDMultiCD::makePass(): Error calling Epetra_MultiVector::Multiply() for debugging U S.");
info = work.Multiply('N','N',-1.0,*A_,curV,1.0);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0,std::logic_error,
"RBGen::ISVDMultiCD::makePass(): Error calling Epetra_MultiVector::Multiply() for debugging U S - A V.");
work.Norm2(&errnorms[0]);
for (int i=0; i<curRank_; i++) {
if (sigma_[i] != 0.0) {
errnorms[i] /= sigma_[i];
}
}
}
// update pass counter
curNumPasses_++;
// print out some info
const Epetra_Comm *comm = &A_->Comm();
if (comm->MyPID() == 0 && verbLevel_ >= 1) {
std::cout
<< "------------- ISVDMultiCD::makePass() -----------" << std::endl
<< "| Number of passes: " << curNumPasses_ << std::endl
<< "| Current rank: " << curRank_ << std::endl
<< "| Current sigmas: " << std::endl;
for (int i=0; i<curRank_; i++) {
std::cout << "| " << sigma_[i] << std::endl;
}
if (debug_) {
std::cout << "|DBG US-AV norms: " << std::endl;
for (int i=0; i<curRank_; i++) {
std::cout << "|DBG " << errnorms[i] << std::endl;
}
if (!firstPass) {
std::cout << "|DBG R-I norm: " << Rerr << std::endl;
}
}
}
return;
}
int
main(int argc, char** argv)
{
// DEFINE A MODEL FOR THE AUV
double cog[3] = {0.0, 0.0, 0.01};
double addedmass[6] = {-1, -16, -16, 0, 0, 0};
double inertia[3] = {0.04, 2.1, 2.1};
double lindrag[10] = {-2.4, -23.0, -23.0, -0.3, -9.7, -9.7, 11.5, -11.5, 3.1, -3.1};
double quadrag[10] = {-2.4, -80.0, -80.0, -0.0006, -9.1, -9.1, 0.3, -0.3, 1.5, -1.5};
double liftcoef[8] = {-20.6, -20.6, -1.53, -1.53, 3.84, -3.84, -6, 6};
// double liftcoef[8] = {0.0, 0.0, -1.53, -1.53, 0.0, 0.0, -6, 6};
double fincoef[5] = {9.6, -9.6, 1.82, -3.84, -3.84};
DUNE::Control::ModelParameters prmtrs;
prmtrs.mass = 18.0;
prmtrs.density = 1020;
prmtrs.volume = 0.0177;
prmtrs.motor_friction = 0.06;
prmtrs.max_thrust = 1.0;
prmtrs.cog = Matrix(cog, 3, 1);
prmtrs.addedmass = Matrix(addedmass, 6, 1);
prmtrs.inertia = Matrix(inertia, 3, 1);
prmtrs.linear_drag = Matrix(lindrag, 10, 1);
prmtrs.quadratic_drag = Matrix(quadrag, 10, 1);
prmtrs.lift = Matrix(liftcoef, 8, 1);
prmtrs.fin_lift = Matrix(fincoef, 5, 1);
AUVModel* a = new AUVModel(prmtrs);
// Initial conditions
double elements_nu[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
double elements_eta[6] = {10.0, 10.0, 0.0, 0.0, 0.0, 1.5};
Matrix nu(elements_nu, 6, 1);
Matrix eta(elements_eta, 6, 1);
// DEFINE A DYN SM CONTROLLER
Matrix Lambda = Matrix(3, 3, 0.0);
Lambda(0, 0) = 0.5;
// Lambda(1,1) = 20;
// Lambda(2,2) = 4;
Lambda(1, 1) = 0.5;
Lambda(2, 2) = 0.5;
Matrix Kd = Matrix(3, 3, 0.0);
Kd(0, 0) = 0.1;
Kd(1, 1) = 1;
Kd(2, 2) = 1;
// Matrix Ksigma = Matrix(3)*0.5;
Matrix Ksigma = Matrix(3) * 0.0;
Ksigma(0, 0) = 0.0;
float sigma = 0.2;
Matrix T = Matrix(3) * 0.4;
DSC* d = new DSC(Lambda, Kd, Ksigma, sigma, T, *a);
d->setOutputLimits(-45 * Math::c_pi / 180, 45 * Math::c_pi / 180);
// open file to save data in
std::ofstream log("Data_log.txt");
// file format: timestamp x y z roll pitch yaw
log << "timestamp" << " | "
<< "x" << " | "
<< "y" << " | "
<< "z" << " | "
<< "phi" << " | "
<< "theta" << " | "
<< "psi" << " | "
<< "u" << " | "
<< "v" << " | "
<< "w" << " | "
<< "p" << " | "
<< "q" << " | "
<< "r" << " | "
<< "X" << " | "
<< "Y" << " | "
<< "Z" << " | "
<< "K" << " | "
<< "M" << " | "
<< "N" << " | "
<< "s1_phi" << " | "
<< "s1_theta" << " | "
<< "s1_psi" << " | "
<< "s2_p" << " | "
<< "s2_q" << " | "
<< "s2_r" << " | "
<< std::endl;
double timestamp = 0.0;
double timestep = 0.1;
unsigned maxsteps = 2000;
// initial tau if necessary
double elements_tau[6] = {5.0, 0.0, 0.0, 0.0, 0.0, 0.0};
Matrix tau(elements_tau, 6, 1);
// body fixed acceleration
Matrix nu_dot(6, 1, 0.0);
// Begin the simulation
for (unsigned i = 0; i < maxsteps; i++)
{
if (0)
{
std::cout << "------------" << std::endl;
std::cout << "Step " << i << std::endl;
std::cout << "------------" << std::endl;
}
timestamp += timestep;
// nu_dot = a->step_inv(tau, nu, eta);
nu_dot = a->stepInv(tau.get(0, 2, 0, 0), tau.get(3, 5, 0, 0), nu, eta);
Matrix control = d->step(DSC::StepParam(eta, nu, nu_dot, 0.0, 0.0, 0.0, timestep));
tau.set(3, 5, 0, 0, control);
Matrix s1 = d->getControlSurfaces();
nu += nu_dot * timestep;
eta += MatrixJ(eta(3), eta(4), eta(5)) * nu * timestep;
eta(5) = Angles::normalizeRadian(eta(5));
log << timestamp << ", "
<< eta(0) << ", "
<< eta(1) << ", "
<< eta(2) << ", "
<< eta(3) << ", "
<< eta(4) << ", "
<< eta(5) << ", "
<< nu(0) << ", "
<< nu(1) << ", "
<< nu(2) << ", "
<< nu(3) << ", "
<< nu(4) << ", "
<< nu(5) << ", "
<< tau(0) << ", "
<< tau(1) << ", "
<< tau(2) << ", "
<< tau(3) << ", "
<< tau(4) << ", "
<< tau(5) << ", "
<< s1(0) << ", "
<< s1(1) << ", "
<< s1(2) << ", "
<< s1(3) << ", "
<< s1(4) << ", "
<< s1(5) << ", "
<< std::endl;
}
log.close();
if (d)
delete d;
if (a)
delete a;
return 0;
}
int main(int argc, char* argv[]){
// Input for initial condition
input* inputData = new input;
// Read input file
inputData->readInputFile();
N = inputData->getN(); // Number of CVs
CFL = inputData->getCFL(); // CFL Number
double h = 1.0/N; // delta x
double curr_time = 0; // Keep track of current time
int count=0; // Count for no. of time steps
int M = 3*N/CFL; // Estimated number of time-levels
// Allocate the arrays
double** w = AllocateDynamicArray(3,N); // State vector
double** f = AllocateDynamicArray(3,N); // Flux vector
double** rho = AllocateDynamicArray(M,N); // Density
double** E = AllocateDynamicArray(M,N); // Total energy
double** e = AllocateDynamicArray(M,N); // Specific internal energy
double** p = AllocateDynamicArray(M,N); // Pressure
double** Mach = AllocateDynamicArray(M,N); // Mach Number
double** u = AllocateDynamicArray(M,N); // Velocity
//Initial condition
Initialize_w(inputData,w,h);
Initialize_f(inputData,f,h);
cout << "\nInitial time step: "<<tau(w,h)<<endl;
//Calculation of w at next time steps
while(curr_time<=inputData->getTime()){
//Current time
curr_time = curr_time + tau(w,h);
//Solve for vector w
solve(inputData, w, f, h);
//Update vector f with new values of vector w
update_f(f,w);
//Extract flow variables from vector w
for(int j=0;j<N;j++){
rho[count][j] = w[0][j];
u[count][j] = w[1][j]/rho[count][j];
E[count][j] = w[2][j];
p[count][j] = 0.4*(E[count][j] - 0.5*rho[count][j]*u[count][j]*u[count][j]);
e[count][j] = w[2][j]/rho[count][j] - 0.5*u[count][j]*u[count][j];
Mach[count][j] = u[count][j]/SQRT(1.4*p[count][j]/rho[count][j]);
}
//Count number of time steps
count++;
cout<<"\nTime step No. : "<<count<<"\tTime = "<<curr_time<<endl;
}
PrintFlowVariables(inputData, rho, u, E, e, p, Mach, count, h);
cout<<"\nNumber of time steps = "<<count<<endl;
cout<<"\nCurrent time = "<<curr_time<<endl;
FreeDynamicArray(w);
FreeDynamicArray(f);
FreeDynamicArray(rho);
FreeDynamicArray(E);
FreeDynamicArray(e);
FreeDynamicArray(p);
FreeDynamicArray(u);
FreeDynamicArray(Mach);
delete inputData;
return 0;
}
Real NSEnergyViscousBC::computeQpOffDiagJacobian(unsigned jvar)
{
// Note: This function requires both _vst_derivs *and* _temp_derivs
// Convenience variables
const RealTensorValue & tau = _viscous_stress_tensor[_qp];
Real rho = _rho[_qp];
Real phij = _phi[_j][_qp];
RealVectorValue U(_rho_u[_qp], _rho_v[_qp], _rho_w[_qp]);
// Map jvar into the variable m for our problem, regardless of
// how Moose has numbered things.
unsigned m = mapVarNumber(jvar);
//
// 1.) Thermal term derivatives
//
// See notes for this term, involves temperature Hessian
Real thermal_term = 0.;
for (unsigned ell=0; ell<LIBMESH_DIM; ++ell)
{
Real intermediate_result = 0.;
// The temperature Hessian contribution
for (unsigned n=0; n<5; ++n)
intermediate_result += _temp_derivs.get_hess(m, n) * (*_gradU[n])[_qp](ell);
// Hit Hessian contribution with test function
intermediate_result *= _phi[_j][_qp];
// Add in the temperature gradient contribution
intermediate_result += _temp_derivs.get_grad(m) * _grad_phi[_j][_qp](ell);
// Hit the result with the normal component, accumulate in thermal_term
thermal_term += intermediate_result * _normals[_qp](ell);
}
// Hit thermal_term with thermal conductivity
thermal_term *= _thermal_conductivity[_qp];
//
// 2.) Viscous term derivatives
//
// Compute viscous term derivatives
Real visc_term = 0.;
switch ( m )
{
case 0: // density
{
// Loop over k and ell as in the notes...
for (unsigned int k = 0; k < LIBMESH_DIM; ++k)
{
Real intermediate_value = 0.0;
for (unsigned int ell = 0; ell < LIBMESH_DIM; ++ell)
intermediate_value += (U(ell) / rho * (-tau(k,ell) * phij / rho + _vst_derivs.dtau(k,ell,m)));
// Hit accumulated value with normal component k. We will multiply by test function at
// the end of this routine...
visc_term += intermediate_value * _normals[_qp](k);
} // end for k
break;
} // end case 0
case 1:
case 2:
case 3: // momentums
{
// Map m -> 0,1,2 as usual...
unsigned int m_local = m - 1;
// Loop over k and ell as in the notes...
for (unsigned int k = 0; k < LIBMESH_DIM; ++k)
{
Real intermediate_value = tau(k,m_local) * phij / rho;
for (unsigned int ell = 0; ell < LIBMESH_DIM; ++ell)
intermediate_value += _vst_derivs.dtau(k,ell,m) * U(ell) / rho; // Note: pass 'm' to dtau, it will convert it internally
// Hit accumulated value with normal component k.
visc_term += intermediate_value * _normals[_qp](k);
} // end for k
break;
} // end case 1,2,3
case 4: // energy
mooseError("Shouldn't get here, this is the on-diagonal entry!");
break;
default:
mooseError("Invalid m value.");
break;
}
// Finally, sum up the different contributions (with appropriate
// sign) multiply by the test function, and return.
return (-thermal_term - visc_term) * _test[_i][_qp];
}
Stokhos::MonoProjPCEBasis<ordinal_type, value_type>::
MonoProjPCEBasis(
ordinal_type p,
const Stokhos::OrthogPolyApprox<ordinal_type, value_type>& pce,
const Stokhos::Quadrature<ordinal_type, value_type>& quad,
const Stokhos::Sparse3Tensor<ordinal_type, value_type>& Cijk,
bool limit_integration_order_) :
RecurrenceBasis<ordinal_type, value_type>("Monomial Projection", p, true),
limit_integration_order(limit_integration_order_),
pce_sz(pce.basis()->size()),
pce_norms(pce.basis()->norm_squared()),
a(pce_sz),
b(pce_sz),
basis_vecs(pce_sz, p+1),
new_pce(p+1)
{
// If the original basis is normalized, we can use the standard QR
// factorization. For simplicity, we renormalize the PCE coefficients
// for a normalized basis
Stokhos::OrthogPolyApprox<ordinal_type, value_type> normalized_pce(pce);
for (ordinal_type i=0; i<pce_sz; i++) {
pce_norms[i] = std::sqrt(pce_norms[i]);
normalized_pce[i] *= pce_norms[i];
}
// Evaluate PCE at quad points
ordinal_type nqp = quad.size();
Teuchos::Array<value_type> pce_vals(nqp);
const Teuchos::Array<value_type>& weights = quad.getQuadWeights();
const Teuchos::Array< Teuchos::Array<value_type> >& quad_points =
quad.getQuadPoints();
const Teuchos::Array< Teuchos::Array<value_type> >& basis_values =
quad.getBasisAtQuadPoints();
for (ordinal_type i=0; i<nqp; i++) {
pce_vals[i] = normalized_pce.evaluate(quad_points[i], basis_values[i]);
}
// Form Kylov matrix up to order pce_sz
matrix_type K(pce_sz, pce_sz);
// Compute matrix
matrix_type A(pce_sz, pce_sz);
typedef Stokhos::Sparse3Tensor<ordinal_type, value_type> Cijk_type;
for (typename Cijk_type::k_iterator k_it = Cijk.k_begin();
k_it != Cijk.k_end(); ++k_it) {
ordinal_type k = index(k_it);
for (typename Cijk_type::kj_iterator j_it = Cijk.j_begin(k_it);
j_it != Cijk.j_end(k_it); ++j_it) {
ordinal_type j = index(j_it);
value_type val = 0;
for (typename Cijk_type::kji_iterator i_it = Cijk.i_begin(j_it);
i_it != Cijk.i_end(j_it); ++i_it) {
ordinal_type i = index(i_it);
value_type c = value(i_it) / (pce_norms[j]*pce_norms[k]);
val += pce[i]*c;
}
A(k,j) = val;
}
}
// Each column i is given by projection of the i-th order monomial
// onto original basis
vector_type u0 = Teuchos::getCol(Teuchos::View, K, 0);
u0(0) = 1.0;
for (ordinal_type i=1; i<pce_sz; i++)
u0(i) = 0.0;
for (ordinal_type k=1; k<pce_sz; k++) {
vector_type u = Teuchos::getCol(Teuchos::View, K, k);
vector_type up = Teuchos::getCol(Teuchos::View, K, k-1);
u.multiply(Teuchos::NO_TRANS, Teuchos::NO_TRANS, 1.0, A, up, 0.0);
}
/*
for (ordinal_type j=0; j<pce_sz; j++) {
for (ordinal_type i=0; i<pce_sz; i++) {
value_type val = 0.0;
for (ordinal_type k=0; k<nqp; k++)
val += weights[k]*std::pow(pce_vals[k],j)*basis_values[k][i];
K(i,j) = val;
}
}
*/
std::cout << K << std::endl << std::endl;
// Compute QR factorization of K
ordinal_type ws_size, info;
value_type ws_size_query;
Teuchos::Array<value_type> tau(pce_sz);
Teuchos::LAPACK<ordinal_type,value_type> lapack;
lapack.GEQRF(pce_sz, pce_sz, K.values(), K.stride(), &tau[0],
&ws_size_query, -1, &info);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error,
"GEQRF returned value " << info);
ws_size = static_cast<ordinal_type>(ws_size_query);
Teuchos::Array<value_type> work(ws_size);
lapack.GEQRF(pce_sz, pce_sz, K.values(), K.stride(), &tau[0],
&work[0], ws_size, &info);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error,
"GEQRF returned value " << info);
// Get Q
lapack.ORGQR(pce_sz, pce_sz, pce_sz, K.values(), K.stride(), &tau[0],
&ws_size_query, -1, &info);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error,
"ORGQR returned value " << info);
ws_size = static_cast<ordinal_type>(ws_size_query);
work.resize(ws_size);
lapack.ORGQR(pce_sz, pce_sz, pce_sz, K.values(), K.stride(), &tau[0],
&work[0], ws_size, &info);
TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error,
"ORGQR returned value " << info);
// Get basis vectors
for (ordinal_type j=0; j<p+1; j++)
for (ordinal_type i=0; i<pce_sz; i++)
basis_vecs(i,j) = K(i,j);
// Compute T = Q'*A*Q
matrix_type prod(pce_sz,pce_sz);
prod.multiply(Teuchos::TRANS, Teuchos::NO_TRANS, 1.0, K, A, 0.0);
matrix_type T(pce_sz,pce_sz);
T.multiply(Teuchos::NO_TRANS, Teuchos::NO_TRANS, 1.0, prod, K, 0.0);
//std::cout << T << std::endl;
// Recursion coefficients are diagonal and super diagonal
b[0] = 1.0;
for (ordinal_type i=0; i<pce_sz-1; i++) {
a[i] = T(i,i);
b[i+1] = T(i,i+1);
}
a[pce_sz-1] = T(pce_sz-1,pce_sz-1);
// Setup rest of basis
this->setup();
// Project original PCE into the new basis
vector_type u(pce_sz);
for (ordinal_type i=0; i<pce_sz; i++)
u[i] = normalized_pce[i];
new_pce.multiply(Teuchos::TRANS, Teuchos::NO_TRANS, 1.0, basis_vecs, u,
0.0);
for (ordinal_type i=0; i<=p; i++)
new_pce[i] /= this->norms[i];
}
/*! One possible version of the GFO problem.
Given a matrix of joint torques applied to the robot joints, this will check
if there exists a set of legal contact forces that balance them. If so, it
will compute the set of contact forces that adds up to the wrench of least
magnitude on the object.
For now, this output of this function is to set the computed contact forces
on each contact as dynamic forces, and also to accumulate the resulting
wrench on the object in its external wrench accumulator.
Return codes: 0 is success, >0 means finger slip, no legal contact forces
exist; <0 means error in computation
*/
int
Grasp::computeQuasistaticForces(const Matrix &robotTau)
{
//WARNING: for now, this ignores contacts on the palm. That might change in the future
//for now, if the hand is touching anything other then the object, abort
for (int c=0; c<hand->getNumChains(); c++) {
if ( hand->getChain(c)->getNumContacts(NULL) !=
hand->getChain(c)->getNumContacts(object) ) {
DBGA("Hand contacts not on object");
return 1;
}
}
std::list<Contact*> contacts;
std::list<Joint*> joints;
bool freeChainForces = false;
for(int c=0; c<hand->getNumChains(); c++) {
//for now, we look at all contacts
std::list<Contact*> chainContacts = hand->getChain(c)->getContacts(object);
contacts.insert(contacts.end(), chainContacts.begin(), chainContacts.end());
if (!chainContacts.empty()) {
std::list<Joint*> chainJoints = hand->getChain(c)->getJoints();
joints.insert(joints.end(), chainJoints.begin(), chainJoints.end());
} else {
//the chain has no contacts
//check if any joint forces are not 0
Matrix chainTau = hand->getChain(c)->jointTorquesVector(robotTau);
//torque units should be N * 1.0e6 * mm
if (chainTau.absMax() > 1.0e3) {
DBGA("Joint torque " << chainTau.absMax() << " on chain " << c
<< " with no contacts");
freeChainForces = true;
}
}
}
//if there are no contacts, nothing to compute!
if (contacts.empty()) return 0;
//assemble the joint forces matrix
Matrix tau((int)joints.size(), 1);
int jc; std::list<Joint*>::iterator jit;
for (jc=0, jit = joints.begin(); jit!=joints.end(); jc++,jit++) {
tau.elem(jc,0) = robotTau.elem( (*jit)->getNum(), 0 );
}
//if all the joint forces we care about are zero, do an early exit
//as zero contact forces are guaranteed to balance the chain
//we should probably be able to use a much larger threshold here, if
//units are what I think they are
if (tau.absMax() < 1.0e-3) {
return 0;
}
//if there are forces on chains with no contacts, we have no hope to balance them
if (freeChainForces) {
return 1;
}
Matrix J(contactJacobian(joints, contacts));
Matrix D(Contact::frictionForceBlockMatrix(contacts));
Matrix F(Contact::frictionConstraintsBlockMatrix(contacts));
Matrix R(Contact::localToWorldWrenchBlockMatrix(contacts));
//grasp map G = S*R*D
Matrix G(graspMapMatrix(R, D));
//left-hand equality matrix JTD = JTran * D
Matrix JTran(J.transposed());
Matrix JTD(JTran.rows(), D.cols());
matrixMultiply(JTran, D, JTD);
//matrix of zeroes for right-hand of friction inequality
Matrix zeroes(Matrix::ZEROES<Matrix>(F.rows(), 1));
//matrix of unknowns
Matrix c_beta(D.cols(), 1);
//bounds: all variables greater than 0
Matrix lowerBounds(Matrix::ZEROES<Matrix>(D.cols(),1));
Matrix upperBounds(Matrix::MAX_VECTOR(D.cols()));
//solve QP
double objVal;
int result = factorizedQPSolver(G, JTD, tau, F, zeroes, lowerBounds, upperBounds,
c_beta, &objVal);
if (result) {
if( result > 0) {
DBGP("Grasp: problem unfeasible");
} else {
DBGA("Grasp: QP solver error");
}
return result;
}
//retrieve contact wrenchs in local contact coordinate systems
Matrix cWrenches(D.rows(), 1);
matrixMultiply(D, c_beta, cWrenches);
DBGP("Contact wrenches:\n" << cWrenches);
//compute wrenches relative to object origin and expressed in world coordinates
Matrix objectWrenches(R.rows(), cWrenches.cols());
matrixMultiply(R, cWrenches, objectWrenches);
DBGP("Object wrenches:\n" << objectWrenches);
//display them on the contacts and accumulate them on the object
displayContactWrenches(&contacts, cWrenches);
accumulateAndDisplayObjectWrenches(&contacts, objectWrenches);
//simple sanity check: JT * c = tau
Matrix fCheck(tau.rows(), 1);
matrixMultiply(JTran, cWrenches, fCheck);
for (int j=0; j<tau.rows(); j++) {
//I am not sure this works well for universal and ball joints
double err = fabs(tau.elem(j, 0) - fCheck.elem(j,0));
//take error as a percentage of desired force, if force is non-zero
if ( fabs(tau.elem(j,0)) > 1.0e-2) {
err = err / fabs(tau.elem(j, 0));
} else {
//for zero desired torque, out of thin air we pull an error threshold of 1.0e2
//which is 0.1% of the normal range of torques at 1.0e6
if (err < 1.0e2) err = 0;
}
// 0.1% error is considered too much
if ( err > 1.0e-1) {
DBGA("Desired torque not obtained on joint " << j << ", error " << err <<
" out of " << fabs(tau.elem(j, 0)) );
return -1;
}
}
//complex sanity check: is object force same as QP optimization result?
//this is only expected to work if all contacts are on the same object
double* extWrench = object->getExtWrenchAcc();
vec3 force(extWrench[0], extWrench[1], extWrench[2]);
vec3 torque(extWrench[3], extWrench[4], extWrench[5]);
//take into account the scaling that has taken place
double wrenchError = objVal*1.0e-6 - (force.len_sq() + torque.len_sq()) * 1.0e6;
//units here are N * 1.0e-6; errors should be in the range on miliN
if (wrenchError > 1.0e3) {
DBGA("Wrench sanity check error: " << wrenchError);
return -1;
}
return 0;
}
/*! This function is the equivalent of the Grasp Force Optimization, but done with
the quasi-static formulation cast as a Quadratic Program.
It can perform four types of computation:
- contact force existence: are there contact forces that balance out on the object
- contact force optimization: what are the optimal contact forces (as far as possible
from the edges of the friction cones) that balance out on the object
- grasp force existence: are there joint forces which produce contact forces which
balance out on the object
- grasp force optimization: what are the optimal joint forces, producing contact
forces that are as far as possible from the edges of the friction cones and
balance out on the object.
See individual computation routines for more details.
There might exist cases of grasps that are reported as form-closed where grasp force
existence fails, as this function also asks that the contact forces that balance
the object must be possible to apply from actuated DOF's.
For now, this function displays the computed contact forces on the contacts,
rather than returning them. It also copies the computed joint forces in the
matrix \a robotTau which is assumed to be large enough for all the joints of
the robot and use the robot's numbering scheme.
Return codes: 0 is success, >0 means problem is unfeasible, no equilibrium forces
exist; <0 means error in computation
*/
int
Grasp::computeQuasistaticForcesAndTorques(Matrix *robotTau, int computation)
{
//use the pre-set list of contacts. This includes contacts on the palm, but
//not contacts with other objects or obstacles
std::list<Contact*> contacts;
contacts.insert(contacts.begin(),contactVec.begin(), contactVec.end());
//if there are no contacts we are done
if (contacts.empty()) return 0;
//get only the joints on chains that make contact;
std::list<Joint*> joints = getJointsOnContactChains();
//build the Jacobian and the other matrices that are needed.
//this is the same as in the equilibrium function above.
Matrix J(contactJacobian(joints, contacts));
Matrix D(Contact::frictionForceBlockMatrix(contacts));
Matrix F(Contact::frictionConstraintsBlockMatrix(contacts));
Matrix R(Contact::localToWorldWrenchBlockMatrix(contacts));
Matrix N(Contact::normalForceSumMatrix(contacts));
//grasp map that relates contact amplitudes to object wrench G = S*R*D
Matrix G(graspMapMatrix(R,D));
//matrix that relates contact forces to joint torques JTD = JTran * D
Matrix JTran(J.transposed());
Matrix JTD(JTran.rows(), D.cols());
matrixMultiply(JTran, D, JTD);
// vectors of unknowns
Matrix beta( D.cols(), 1);
Matrix tau( (int)joints.size(), 1);
double objVal;
/* This is the core computation. There are many ways of combining the
optimization criterion and the constraints. Four of them are presented
here, each encapsulated in its own helper function.
*/
int result;
switch(computation)
{
case GRASP_FORCE_EXISTENCE:
result = graspForceExistence(JTD, D, F, G, beta, tau, &objVal);
break;
case GRASP_FORCE_OPTIMIZATION:
result = graspForceOptimization(JTD, D, F, G, beta, tau, &objVal);
break;
case CONTACT_FORCE_EXISTENCE:
result = contactForceExistence(F, N, G, beta, &objVal);
matrixMultiply(JTD, beta, tau);
break;
case CONTACT_FORCE_OPTIMIZATION:
result = contactForceOptimization(F, N, G, beta, &objVal);
matrixMultiply(JTD, beta, tau);
break;
default:
DBGA("Unknown computation type requested");
return -1;
}
if (result) {
if( result > 0) {
DBGA("Grasp: problem unfeasible");
} else {
DBGA("Grasp: solver error");
}
return result;
}
DBGA("Optimization solved; objective: " << objVal);
DBGP("beta:\n" << beta);
DBGP("tau:\n" << tau);
DBGP("Joint forces sum: " << tau.elementSum());
Matrix Gbeta(G.rows(), beta.cols());
matrixMultiply(G, beta, Gbeta);
DBGP("Total object wrench:\n" << Gbeta);
//retrieve contact wrenches in local contact coordinate systems
Matrix cWrenches(D.rows(), 1);
matrixMultiply(D, beta, cWrenches);
DBGP("Contact forces:\n " << cWrenches);
//compute object wrenches relative to object origin and expressed in world coordinates
Matrix objectWrenches(R.rows(), cWrenches.cols());
matrixMultiply(R, cWrenches, objectWrenches);
DBGP("Object wrenches:\n" << objectWrenches);
//display them on the contacts and accumulate them on the object
displayContactWrenches(&contacts, cWrenches);
accumulateAndDisplayObjectWrenches(&contacts, objectWrenches);
//set the robot joint values for the return
std::list<Joint*>::iterator it;
int jc;
for(it=joints.begin(), jc=0; it!=joints.end(); it++,jc++) {
robotTau->elem( (*it)->getNum(), 0 ) = 1.0 * tau.elem(jc,0);
}
//sanity check: contact forces balance joint forces
//sanity check: resultant object wrench is 0
return 0;
}
/**
* Description not yet available.
* \param
*/
double do_gauss_hermite_block_diagonal_multi(const dvector& x,
const dvector& u0,const dmatrix& Hess,const dvector& _xadjoint,
const dvector& _uadjoint,const dmatrix& _Hessadjoint,
function_minimizer * pmin)
{
ADUNCONST(dvector,xadjoint)
ADUNCONST(dvector,uadjoint)
//ADUNCONST(dmatrix,Hessadjoint)
dvector & w= *(pmin->multinomial_weights);
const int xs=x.size();
const int us=u0.size();
gradient_structure::set_NO_DERIVATIVES();
int nsc=pmin->lapprox->num_separable_calls;
const ivector lrea = (*pmin->lapprox->num_local_re_array)(1,nsc);
int hroom = sum(square(lrea));
int nvar=x.size()+u0.size()+hroom;
independent_variables y(1,nvar);
// need to set random effects active together with whatever
// init parameters should be active in this phase
initial_params::set_inactive_only_random_effects();
initial_params::set_active_random_effects();
/*int onvar=*/initial_params::nvarcalc();
initial_params::xinit(y); // get the initial values into the
// do we need this next line?
y(1,xs)=x;
int i,j;
// contribution for quadratic prior
if (quadratic_prior::get_num_quadratic_prior()>0)
{
//Hess+=quadratic_prior::get_cHessian_contribution();
int & vxs = (int&)(xs);
quadratic_prior::get_cHessian_contribution(Hess,vxs);
}
// Here need hooks for sparse matrix structures
dvar3_array & block_diagonal_vhessian=
*pmin->lapprox->block_diagonal_vhessian;
block_diagonal_vhessian.initialize();
dvar3_array& block_diagonal_ch=
*pmin->lapprox->block_diagonal_vch;
//dvar3_array(*pmin->lapprox->block_diagonal_ch);
int ii=xs+us+1;
d3_array& bdH=(*pmin->lapprox->block_diagonal_hessian);
int ic;
for (ic=1;ic<=nsc;ic++)
{
int lus=lrea(ic);
for (i=1;i<=lus;i++)
for (j=1;j<=lus;j++)
y(ii++)=bdH(ic)(i,j);
}
dvector g(1,nvar);
gradcalc(0,g);
gradient_structure::set_YES_DERIVATIVES();
dvar_vector vy=dvar_vector(y);
//initial_params::stddev_vscale(d,vy);
ii=xs+us+1;
if (initial_df1b2params::have_bounded_random_effects)
{
cerr << "can't do importance sampling with bounded random effects"
" at present" << endl;
ad_exit(1);
}
else
{
for (int ic=1;ic<=nsc;ic++)
{
int lus=lrea(ic);
if (lus>0)
{
for (i=1;i<=lus;i++)
{
for (j=1;j<=lus;j++)
{
block_diagonal_vhessian(ic,i,j)=vy(ii++);
}
}
block_diagonal_ch(ic)=
choleski_decomp(inv(block_diagonal_vhessian(ic)));
}
}
}
int nsamp=pmin->lapprox->use_gauss_hermite;
pmin->lapprox->in_gauss_hermite_phase=1;
dvar_vector sample_value(1,nsamp);
sample_value.initialize();
dvar_vector tau(1,us);;
// !!! This only works for one random efect in each separable call
// at present.
if (pmin->lapprox->gh->mi)
{
delete pmin->lapprox->gh->mi;
pmin->lapprox->gh->mi=0;
}
pmin->lapprox->gh->mi=new multi_index(1,nsamp,
pmin->lapprox->multi_random_effects);
multi_index & mi = *(pmin->lapprox->gh->mi);
//for (int is=1;is<=nsamp;is++)
dvector& xx=pmin->lapprox->gh->x;
do
{
int offset=0;
pmin->lapprox->num_separable_calls=0;
//pmin->lapprox->gh->is=is;
for (ic=1;ic<=nsc;ic++)
{
int lus=lrea(ic);
// will need vector stuff here when more than one random effect
if (lus>0)
{
//tau(offset+1,offset+lus).shift(1)=block_diagonal_ch(ic)(1,1)*
// pmin->lapprox->gh->x(is);
dvector xv(1,lus);
for (int iu=1;iu<=lus;iu++)
{
xv(iu)= xx(mi()(iu));
}
tau(offset+1,offset+lus).shift(1)=block_diagonal_ch(ic)*xv;
offset+=lus;
}
}
// have to reorder the terms to match the block diagonal hessian
imatrix & ls=*(pmin->lapprox->block_diagonal_re_list);
int mmin=ls.indexmin();
int mmax=ls.indexmax();
int ii=1;
int i;
for (i=mmin;i<=mmax;i++)
{
int cmin=ls(i).indexmin();
int cmax=ls(i).indexmax();
for (int j=cmin;j<=cmax;j++)
{
vy(ls(i,j))+=tau(ii++);
}
}
if (ii-1 != us)
{
cerr << "error in interface" << endl;
ad_exit(1);
}
initial_params::reset(vy); // get the values into the model
ii=1;
for (i=mmin;i<=mmax;i++)
{
int cmin=ls(i).indexmin();
int cmax=ls(i).indexmax();
for (int j=cmin;j<=cmax;j++)
{
vy(ls(i,j))-=tau(ii++);
}
}
*objective_function_value::pobjfun=0.0;
pmin->AD_uf_outer();
++mi;
}
while(mi.get_depth()<=pmin->lapprox->multi_random_effects);
nsc=pmin->lapprox->num_separable_calls;
dvariable vf=pmin->do_gauss_hermite_integration();
int sgn=0;
dvariable ld=0.0;
if (ad_comm::no_ln_det_choleski_flag)
{
for (int ic=1;ic<=nsc;ic++)
{
if (allocated(block_diagonal_vhessian(ic)))
{
ld+=w(2*ic)*ln_det(block_diagonal_vhessian(ic),sgn);
}
}
ld*=0.5;
}
else
{
for (int ic=1;ic<=nsc;ic++)
{
if (allocated(block_diagonal_vhessian(ic)))
{
ld+=w(2*ic)*ln_det_choleski(block_diagonal_vhessian(ic));
}
}
ld*=0.5;
}
vf+=ld;
//vf+=us*0.91893853320467241;
double f=value(vf);
gradcalc(nvar,g);
// put uhat back into the model
gradient_structure::set_NO_DERIVATIVES();
vy(xs+1,xs+us).shift(1)=u0;
initial_params::reset(vy); // get the values into the model
gradient_structure::set_YES_DERIVATIVES();
pmin->lapprox->in_gauss_hermite_phase=0;
ii=1;
for (i=1;i<=xs;i++)
xadjoint(i)=g(ii++);
for (i=1;i<=us;i++)
uadjoint(i)=g(ii++);
for (ic=1;ic<=nsc;ic++)
{
int lus=lrea(ic);
for (i=1;i<=lus;i++)
{
for (j=1;j<=lus;j++)
{
(*pmin->lapprox->block_diagonal_vhessianadjoint)(ic)(i,j)=g(ii++);
}
}
}
return f;
}
int gfx_thread_jf(void *foo)
{
unsigned int i, j;
const Uint32 trace = SDL_MapRGB(meter->format, 0x70, 0xFF, 0x70);
const Uint32 met_red = SDL_MapRGB(meter->format, 0x30, 0x30, 0xFF);
const Uint32 met_green = SDL_MapRGB(meter->format, 0x30, 0xFF, 0x30);
for (i=0; i<num_meters; i++) {
lp[i] = 0.0f;
}
for (i=0; i<num_scopes; i++) {
buf_rect[i].x = dest[i].x + 10;
buf_rect[i].y = dest[i].y + 10;
buf_rect[i].w = 200;
buf_rect[i].h = 227;
}
while (1) {
for (i=0; i<num_scopes; i++) {
int x=0, y=0, xm1, ym1;
SDL_Rect cor_rect;
SDL_FillRect(meter, buf_rect, trace);
SDL_BlitSurface(meter_buf, buf_rect, screen, buf_rect+i);
for (j=0; j<RING_BUF_SIZE; j++) {
xm1 = x;
ym1 = y;
/* A touch of lowpass filter to make it easier
* to read */
lp[i*2] = lp[i*2] * 0.9f + ring_buf[i*2][j] *
bias * 0.1f;
lp[i*2+1] = lp[i*2+1] * 0.9f +
ring_buf[i*2+1][j] * bias * 0.1f;
x = lp[i*2+1] * 80.0f * 0.707f -
lp[i*2] * 80.0f * 0.707f;
if (x > 100) {
x = 100;
continue;
} else if (x < -100) {
x = -100;
continue;
}
y = lp[i*2+1] * -80.0f * 0.707f +
lp[i*2] * -80.0f * 0.707f;
if (y > 99) {
y = 99;
continue;
} else if (y < -96) {
y = -96;
continue;
}
set_rgba(screen, buf_rect[i].x+100+x, buf_rect[i].y+100+y, trace);
#if 0
if (j > 0) {
draw_line(screen, buf_rect[i].x+100+x, buf_rect[i].y+100+y, buf_rect[i].x+100+xm1, buf_rect[i].y+100+ym1, trace);
}
#endif
}
/* Draw correlation meter */
tau_lp[i] = tau_lp[i] * 0.8f + tau((float *)ring_buf[i*2], (float *)ring_buf[i*2+1], RING_BUF_SIZE) * 0.2f;
cor_rect.y = buf_rect[i].y + 210;
cor_rect.h = 6;
if (tau_lp[i] > 0.0f) {
cor_rect.x = buf_rect[i].x+100;
cor_rect.w = tau_lp[i] * 92.0f;
SDL_FillRect(screen, &cor_rect, met_green);
} else {
cor_rect.x = buf_rect[i].x+100 + tau_lp[i] * 92.0f;
cor_rect.w = (int)(tau_lp[i] * -92.0f) + 1;
SDL_FillRect(screen, &cor_rect, met_red);
}
if (cor_rect.w > 100) printf("%d\t%f\n", cor_rect.w, tau_lp[i]);
}
SDL_UpdateRects(screen, 1, &win);
SDL_Delay(25);
}
return 0;
}
int
main(int argc, char** argv)
{
if (argc < 2)
{
std::cout << "Usage: rlDynamics2Demo MODELFILE Q1 ... Qn QD1 ... QDn QDD1 ... QDDn" << std::endl;
return 1;
}
try
{
rl::mdl::XmlFactory factory;
boost::shared_ptr< rl::mdl::Model > model(factory.create(argv[1]));
rl::mdl::Dynamic* dynamic = dynamic_cast< rl::mdl::Dynamic* >(model.get());
rl::math::Vector q(dynamic->getDof());
rl::math::Vector qd(dynamic->getDof());
rl::math::Vector qdd(dynamic->getDof());
rl::math::Vector tau(dynamic->getDof());
for (std::size_t i = 0; i < dynamic->getDof(); ++i)
{
q(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2]);
qd(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2 + dynamic->getDof()]);
qdd(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2 + 2 * dynamic->getDof()]);
}
rl::math::Vector g(3);
dynamic->getWorldGravity(g);
rl::math::Vector tmp(dynamic->getDof());
rl::math::Vector tmp2(6 * dynamic->getOperationalDof());
std::cout << "===============================================================================" << std::endl;
// FP
dynamic->setPosition(q);
dynamic->forwardPosition();
const rl::math::Transform::ConstTranslationPart& position = dynamic->getOperationalPosition(0).translation();
rl::math::Vector3 orientation = dynamic->getOperationalPosition(0).rotation().eulerAngles(2, 1, 0).reverse();
std::cout << "x = " << position.x() << " m, y = " << position.y() << " m, z = " << position.z() << " m, a = " << orientation.x() * rl::math::RAD2DEG << " deg, b = " << orientation.y() * rl::math::RAD2DEG << " deg, c = " << orientation.z() * rl::math::RAD2DEG << " deg" << std::endl;
std::cout << "===============================================================================" << std::endl;
// FV
dynamic->setPosition(q);
dynamic->setVelocity(qd);
dynamic->forwardVelocity();
std::cout << "xd = " << dynamic->getOperationalVelocity(0).linear().transpose() << " " << dynamic->getOperationalVelocity(0).angular().transpose() << std::endl;
std::cout << "-------------------------------------------------------------------------------" << std::endl;
// J
rl::math::Matrix J(6 * dynamic->getOperationalDof(), dynamic->getDof());
dynamic->calculateJacobian(J);
std::cout << "J = " << std::endl << J << std::endl;
// J * qd
tmp2 = J * qd;
std::cout << "xd = " << tmp2.transpose() << std::endl;
// invJ
rl::math::Matrix invJ(dynamic->getDof(), 6 * dynamic->getOperationalDof());
dynamic->calculateJacobianInverse(J, invJ);
std::cout << "J^{-1} = " << std::endl << invJ << std::endl;
std::cout << "===============================================================================" << std::endl;
// FA
dynamic->setPosition(q);
dynamic->setVelocity(qd);
dynamic->setAcceleration(qdd);
dynamic->setWorldGravity(g);
dynamic->forwardVelocity();
dynamic->forwardAcceleration();
std::cout << "xdd = " << dynamic->getOperationalAcceleration(0).linear().transpose() << " " << dynamic->getOperationalAcceleration(0).angular().transpose() << std::endl;
std::cout << "-------------------------------------------------------------------------------" << std::endl;
// Jd * qd
rl::math::Vector Jdqd(6 * dynamic->getOperationalDof());
dynamic->calculateJacobianDerivative(Jdqd);
std::cout << "Jd*qd = " << Jdqd.transpose() << std::endl;
// J * qdd + Jd * qd
tmp2 = J * qdd + Jdqd;
std::cout << "xdd = " << tmp2.transpose() << std::endl;
std::cout << "===============================================================================" << std::endl;
// RNE
dynamic->setPosition(q);
dynamic->setVelocity(qd);
dynamic->setAcceleration(qdd);
dynamic->inverseDynamics();
dynamic->getTorque(tau);
std::cout << "tau = " << tau.transpose() << std::endl;
std::cout << "-------------------------------------------------------------------------------" << std::endl;
// M
rl::math::Matrix M(dynamic->getDof(), dynamic->getDof());
dynamic->setPosition(q);
dynamic->calculateMassMatrix(M);
std::cout << "M = " << std::endl << M << std::endl;
// V
rl::math::Vector V(dynamic->getDof());
dynamic->setPosition(q);
dynamic->setVelocity(qd);
dynamic->calculateCentrifugalCoriolis(V);
std::cout << "V = " << V.transpose() << std::endl;
// G
rl::math::Vector G(dynamic->getDof());
dynamic->setPosition(q);
dynamic->setWorldGravity(g);
dynamic->calculateGravity(G);
std::cout << "G = " << G.transpose() << std::endl;
// M * qdd + V + G
tmp = M * qdd + V + G;
std::cout << "tau = " << tmp.transpose() << std::endl;
std::cout << "===============================================================================" << std::endl;
// FD
dynamic->setPosition(q);
dynamic->setVelocity(qd);
dynamic->setTorque(tau);
dynamic->forwardDynamics();
dynamic->getAcceleration(tmp);
std::cout << "qdd = " << tmp.transpose() << std::endl;
std::cout << "-------------------------------------------------------------------------------" << std::endl;
// M^{-1}
rl::math::Matrix invM(dynamic->getDof(), dynamic->getDof());
dynamic->setPosition(q);
dynamic->calculateMassMatrixInverse(invM);
std::cout << "M^{-1} = " << std::endl << invM << std::endl;
// V
std::cout << "V = " << V.transpose() << std::endl;
// G
std::cout << "G = " << G.transpose() << std::endl;
// M^{-1} * ( tau - V - G )
tmp = invM * (tau - V - G);
std::cout << "qdd = " << tmp.transpose() << std::endl;
std::cout << "===============================================================================" << std::endl;
}
catch (const std::exception& e)
{
std::cout << e.what() << std::endl;
return 1;
}
return 0;
}
void ElasticCableRayleighDamping<StressStrainLaw>::damping_residual( bool compute_jacobian,
AssemblyContext& context,
CachedValues& /*cache*/)
{
// First, do the "mass" contribution
this->mass_residual_impl(compute_jacobian,
context,
&libMesh::FEMContext::interior_rate,
&libMesh::DiffContext::get_elem_solution_rate_derivative,
_mu_factor);
// Now do the stiffness contribution
const unsigned int n_u_dofs = context.get_dof_indices(this->_disp_vars.u()).size();
const std::vector<libMesh::Real> &JxW =
this->get_fe(context)->get_JxW();
// Residuals that we're populating
libMesh::DenseSubVector<libMesh::Number> &Fu = context.get_elem_residual(this->_disp_vars.u());
libMesh::DenseSubVector<libMesh::Number> &Fv = context.get_elem_residual(this->_disp_vars.v());
libMesh::DenseSubVector<libMesh::Number> &Fw = context.get_elem_residual(this->_disp_vars.w());
//Grab the Jacobian matrix as submatrices
//libMesh::DenseMatrix<libMesh::Number> &K = context.get_elem_jacobian();
libMesh::DenseSubMatrix<libMesh::Number> &Kuu = context.get_elem_jacobian(this->_disp_vars.u(),this->_disp_vars.u());
libMesh::DenseSubMatrix<libMesh::Number> &Kuv = context.get_elem_jacobian(this->_disp_vars.u(),this->_disp_vars.v());
libMesh::DenseSubMatrix<libMesh::Number> &Kuw = context.get_elem_jacobian(this->_disp_vars.u(),this->_disp_vars.w());
libMesh::DenseSubMatrix<libMesh::Number> &Kvu = context.get_elem_jacobian(this->_disp_vars.v(),this->_disp_vars.u());
libMesh::DenseSubMatrix<libMesh::Number> &Kvv = context.get_elem_jacobian(this->_disp_vars.v(),this->_disp_vars.v());
libMesh::DenseSubMatrix<libMesh::Number> &Kvw = context.get_elem_jacobian(this->_disp_vars.v(),this->_disp_vars.w());
libMesh::DenseSubMatrix<libMesh::Number> &Kwu = context.get_elem_jacobian(this->_disp_vars.w(),this->_disp_vars.u());
libMesh::DenseSubMatrix<libMesh::Number> &Kwv = context.get_elem_jacobian(this->_disp_vars.w(),this->_disp_vars.v());
libMesh::DenseSubMatrix<libMesh::Number> &Kww = context.get_elem_jacobian(this->_disp_vars.w(),this->_disp_vars.w());
unsigned int n_qpoints = context.get_element_qrule().n_points();
// All shape function gradients are w.r.t. master element coordinates
const std::vector<std::vector<libMesh::Real> >& dphi_dxi = this->get_fe(context)->get_dphidxi();
const libMesh::DenseSubVector<libMesh::Number>& u_coeffs = context.get_elem_solution( this->_disp_vars.u() );
const libMesh::DenseSubVector<libMesh::Number>& v_coeffs = context.get_elem_solution( this->_disp_vars.v() );
const libMesh::DenseSubVector<libMesh::Number>& w_coeffs = context.get_elem_solution( this->_disp_vars.w() );
const libMesh::DenseSubVector<libMesh::Number>& dudt_coeffs = context.get_elem_solution_rate( this->_disp_vars.u() );
const libMesh::DenseSubVector<libMesh::Number>& dvdt_coeffs = context.get_elem_solution_rate( this->_disp_vars.v() );
const libMesh::DenseSubVector<libMesh::Number>& dwdt_coeffs = context.get_elem_solution_rate( this->_disp_vars.w() );
// Need these to build up the covariant and contravariant metric tensors
const std::vector<libMesh::RealGradient>& dxdxi = this->get_fe(context)->get_dxyzdxi();
const unsigned int dim = 1; // The cable dimension is always 1 for this physics
for (unsigned int qp=0; qp != n_qpoints; qp++)
{
// Gradients are w.r.t. master element coordinates
libMesh::Gradient grad_u, grad_v, grad_w;
libMesh::Gradient dgradu_dt, dgradv_dt, dgradw_dt;
for( unsigned int d = 0; d < n_u_dofs; d++ )
{
libMesh::RealGradient u_gradphi( dphi_dxi[d][qp] );
grad_u += u_coeffs(d)*u_gradphi;
grad_v += v_coeffs(d)*u_gradphi;
grad_w += w_coeffs(d)*u_gradphi;
dgradu_dt += dudt_coeffs(d)*u_gradphi;
dgradv_dt += dvdt_coeffs(d)*u_gradphi;
dgradw_dt += dwdt_coeffs(d)*u_gradphi;
}
libMesh::RealGradient grad_x( dxdxi[qp](0) );
libMesh::RealGradient grad_y( dxdxi[qp](1) );
libMesh::RealGradient grad_z( dxdxi[qp](2) );
libMesh::TensorValue<libMesh::Real> a_cov, a_contra, A_cov, A_contra;
libMesh::Real lambda_sq = 0;
this->compute_metric_tensors( qp, *(this->get_fe(context)), context,
grad_u, grad_v, grad_w,
a_cov, a_contra, A_cov, A_contra,
lambda_sq );
// Compute stress tensor
libMesh::TensorValue<libMesh::Real> tau;
ElasticityTensor C;
this->_stress_strain_law.compute_stress_and_elasticity(dim,a_contra,a_cov,A_contra,A_cov,tau,C);
libMesh::Real jac = JxW[qp];
for (unsigned int i=0; i != n_u_dofs; i++)
{
libMesh::RealGradient u_gradphi( dphi_dxi[i][qp] );
libMesh::Real u_diag_factor = _lambda_factor*this->_A*jac*tau(0,0)*dgradu_dt(0)*u_gradphi(0);
libMesh::Real v_diag_factor = _lambda_factor*this->_A*jac*tau(0,0)*dgradv_dt(0)*u_gradphi(0);
libMesh::Real w_diag_factor = _lambda_factor*this->_A*jac*tau(0,0)*dgradw_dt(0)*u_gradphi(0);
const libMesh::Real C1 = _lambda_factor*this->_A*jac*C(0,0,0,0)*u_gradphi(0);
const libMesh::Real gamma_u = (grad_x(0)+grad_u(0));
const libMesh::Real gamma_v = (grad_y(0)+grad_v(0));
const libMesh::Real gamma_w = (grad_z(0)+grad_w(0));
const libMesh::Real x_term = C1*gamma_u;
const libMesh::Real y_term = C1*gamma_v;
const libMesh::Real z_term = C1*gamma_w;
const libMesh::Real dt_term = dgradu_dt(0)*gamma_u + dgradv_dt(0)*gamma_v + dgradw_dt(0)*gamma_w;
Fu(i) += u_diag_factor + x_term*dt_term;
Fv(i) += v_diag_factor + y_term*dt_term;
Fw(i) += w_diag_factor + z_term*dt_term;
}
if( compute_jacobian )
{
for(unsigned int i=0; i != n_u_dofs; i++)
{
libMesh::RealGradient u_gradphi_I( dphi_dxi[i][qp] );
for(unsigned int j=0; j != n_u_dofs; j++)
{
libMesh::RealGradient u_gradphi_J( dphi_dxi[j][qp] );
libMesh::Real common_factor = _lambda_factor*this->_A*jac*u_gradphi_I(0);
const libMesh::Real diag_term_1 = common_factor*tau(0,0)*u_gradphi_J(0)*context.get_elem_solution_rate_derivative();
const libMesh::Real dgamma_du = ( u_gradphi_J(0)*(grad_x(0)+grad_u(0)) );
const libMesh::Real dgamma_dv = ( u_gradphi_J(0)*(grad_y(0)+grad_v(0)) );
const libMesh::Real dgamma_dw = ( u_gradphi_J(0)*(grad_z(0)+grad_w(0)) );
const libMesh::Real diag_term_2_factor = common_factor*C(0,0,0,0)*context.get_elem_solution_derivative();
Kuu(i,j) += diag_term_1 + dgradu_dt(0)*diag_term_2_factor*dgamma_du;
Kuv(i,j) += dgradu_dt(0)*diag_term_2_factor*dgamma_dv;
Kuw(i,j) += dgradu_dt(0)*diag_term_2_factor*dgamma_dw;
Kvu(i,j) += dgradv_dt(0)*diag_term_2_factor*dgamma_du;
Kvv(i,j) += diag_term_1 + dgradv_dt(0)*diag_term_2_factor*dgamma_dv;
Kvw(i,j) += dgradv_dt(0)*diag_term_2_factor*dgamma_dw;
Kwu(i,j) += dgradw_dt(0)*diag_term_2_factor*dgamma_du;
Kwv(i,j) += dgradw_dt(0)*diag_term_2_factor*dgamma_dv;
Kww(i,j) += diag_term_1 + dgradw_dt(0)*diag_term_2_factor*dgamma_dw;
const libMesh::Real C1 = common_factor*C(0,0,0,0);
const libMesh::Real gamma_u = (grad_x(0)+grad_u(0));
const libMesh::Real gamma_v = (grad_y(0)+grad_v(0));
const libMesh::Real gamma_w = (grad_z(0)+grad_w(0));
const libMesh::Real x_term = C1*gamma_u;
const libMesh::Real y_term = C1*gamma_v;
const libMesh::Real z_term = C1*gamma_w;
const libMesh::Real ddtterm_du = u_gradphi_J(0)*(gamma_u*context.get_elem_solution_rate_derivative()
+ dgradu_dt(0)*context.get_elem_solution_derivative());
const libMesh::Real ddtterm_dv = u_gradphi_J(0)*(gamma_v*context.get_elem_solution_rate_derivative()
+ dgradv_dt(0)*context.get_elem_solution_derivative());
const libMesh::Real ddtterm_dw = u_gradphi_J(0)*(gamma_w*context.get_elem_solution_rate_derivative()
+ dgradw_dt(0)*context.get_elem_solution_derivative());
Kuu(i,j) += x_term*ddtterm_du;
Kuv(i,j) += x_term*ddtterm_dv;
Kuw(i,j) += x_term*ddtterm_dw;
Kvu(i,j) += y_term*ddtterm_du;
Kvv(i,j) += y_term*ddtterm_dv;
Kvw(i,j) += y_term*ddtterm_dw;
Kwu(i,j) += z_term*ddtterm_du;
Kwv(i,j) += z_term*ddtterm_dv;
Kww(i,j) += z_term*ddtterm_dw;
const libMesh::Real dt_term = dgradu_dt(0)*gamma_u + dgradv_dt(0)*gamma_v + dgradw_dt(0)*gamma_w;
// Here, we're missing derivatives of C(0,0,0,0) w.r.t. strain
// Nonzero for hyperelasticity models
const libMesh::Real dxterm_du = C1*u_gradphi_J(0)*context.get_elem_solution_derivative();
const libMesh::Real dyterm_dv = dxterm_du;
const libMesh::Real dzterm_dw = dxterm_du;
Kuu(i,j) += dxterm_du*dt_term;
Kvv(i,j) += dyterm_dv*dt_term;
Kww(i,j) += dzterm_dw*dt_term;
} // end j-loop
} // end i-loop
} // end if(compute_jacobian)
} // end qp loop
}
int main( int argc, char** argv ){
#if (CISST_OS == CISST_LINUX_XENOMAI)
mlockall(MCL_CURRENT | MCL_FUTURE);
RT_TASK task;
rt_task_shadow( &task, "GroupTest", 99, 0 );
#endif
cmnLogger::SetMask( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL );
if( argc != 2 ){
std::cout << "Usage: " << argv[0] << " can[0-1]" << std::endl;
return -1;
}
#if (CISST_OS == CISST_LINUX_XENOMAI)
osaRTSocketCAN can( argv[1], osaCANBus::RATE_1000 );
#else
osaSocketCAN can( argv[1], osaCANBus::RATE_1000 );
#endif
if( can.Open() != osaCANBus::ESUCCESS ){
CMN_LOG_RUN_ERROR << argv[0] << "Failed to open " << argv[1] << std::endl;
return -1;
}
osaWAM WAM( &can );
if( WAM.Initialize() != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to initialize WAM" << std::endl;
return -1;
}
vctDynamicVector<double> qinit( 7, 0.0 );
qinit[1] = -cmnPI_2;
qinit[3] = cmnPI;
if( WAM.SetPositions( qinit ) != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to set position: " << qinit << std::endl;
return -1;
}
cmnPath path;
path.AddRelativeToCisstShare("/models/WAM");
std::string fname = path.Find("wam7.rob", cmnPath::READ);
// Rotate the base
vctMatrixRotation3<double> Rw0( 0.0, 0.0, -1.0,
0.0, 1.0, 0.0,
1.0, 0.0, 0.0 );
vctFixedSizeVector<double,3> tw0(0.0);
vctFrame4x4<double> Rtw0( Rw0, tw0 );
// Gain matrices
vctDynamicMatrix<double> Kp(7, 7, 0.0), Kd(7, 7, 0.0);
Kp[0][0] = 250; Kd[0][0] = 3.0;
Kp[1][1] = 250; Kd[1][1] = 3.0;
Kp[2][2] = 250; Kd[2][2] = 3.0;
Kp[3][3] = 200; Kd[3][3] = 3;
Kp[4][4] = 50; Kd[4][4] = 0.8;
Kp[5][5] = 50; Kd[5][5] = 0.8;
Kp[6][6] = 10; Kd[6][6] = .1;
osaPDGC PDGC( fname, Rtw0, Kp, Kd, qinit );
std::cout << "Activate the WAM" << std::endl;
bool activated = false;
double t1 = osaGetTime();
while( 1 ){
// Get the positions
vctDynamicVector<double> q;
if( WAM.GetPositions( q ) != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to get positions" << std::endl;
return -1;
}
// Check if the pucks are activated
if( !activated ) {
osaWAM::Mode mode;
if( WAM.GetMode( mode ) != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to get mode" << std::endl;
return -1;
}
if( mode == osaWAM::MODE_ACTIVATED )
{ activated = true; }
}
// if pucks are activated, run the controller
vctDynamicVector<double> tau( q.size(), 0.0 );
double t2 = osaGetTime();
if( activated ){
if( PDGC.Evaluate( qinit, q, tau, t2-t1 ) != osaPDGC::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to evaluate controller" << std::endl;
return -1;
}
}
t1 = t2;
// apply torques
if( WAM.SetTorques( tau ) != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to set torques" << std::endl;
return -1;
}
std::cout << "q: " << q << std::endl;
std::cout << "tau: " << tau << std::endl;
}
return 0;
}
