Foam::scalar Foam::Rosenbrock23::solve
(
const scalar x0,
const scalarField& y0,
const scalarField& dydx0,
const scalar dx,
scalarField& y
) const
{
odes_.jacobian(x0, y0, dfdx_, dfdy_);
for (label i=0; i<n_; i++)
{
for (label j=0; j<n_; j++)
{
a_(i, j) = -dfdy_(i, j);
}
a_(i, i) += 1.0/(gamma*dx);
}
LUDecompose(a_, pivotIndices_);
// Calculate k1:
forAll(k1_, i)
{
k1_[i] = dydx0[i] + dx*d1*dfdx_[i];
}
boost::function<Real (Time)> GeneralizedHullWhite::speed() const {
std::vector<Real> speedvals;
speedvals.push_back(a_(0.0001));
for (Size i=0;i<a_.size()-1;i++)
speedvals.push_back(
a_(
(speedstructure_[i+1]-speedstructure_[0])/365.0
- 0.00001));
return PiecewiseLinearCurve(speedperiods_, speedvals);
}
Precision JumpingCoefficients::getCenter(
const Position,
const Index sx,
const Index sy,
const Index nx,
const Index ny ) const
{
double value=
nx*nx*(a_((sx-0.5)/nx,1.*sy/ny)+a_((sx+0.5)/nx,1.*sy/ny))
+ny*ny*(b_(1.*sx/nx,(sy-0.5)/ny)+b_(1.*sx/nx,(sy+0.5)/ny));
return value;
}
void AdmittanceController::update(const Eigen::VectorXd& tau, Eigen::VectorXd& qdot_reference, const KDL::JntArray& q_current, Eigen::VectorXd& q_reference) {
// Update filters --> as a result desired velocities are known
for (int i = 0; i<tau.size(); i++) {
/// Check input values (and create segfault to debug)
if (!(fabs(tau(i)) < 10000.0 && fabs(qdot_reference_previous_(i)) < 10000.0) ) {
std::cout << "tauin: " << tau(i) << ", qdot: " << qdot_reference_previous_(i) << std::endl;
assert(false);
} else {
qdot_reference(i) = b_(0,i) * tau(i);
qdot_reference(i) += b_(1,i) * tau_previous_(i);
qdot_reference(i) -= a_(1,i) * qdot_reference_previous_(i);
qdot_reference(i) *= k_(i);
// Integrate desired velocities and limit outputs
q_reference(i) = std::min(q_max_(i),std::max(q_min_(i),q_current(i)+Ts_*qdot_reference(i)));
}
}
tau_previous_ = tau;
qdot_reference_previous_ = qdot_reference;
//ROS_INFO("qdr = %f %f %f %f", qdot_reference(0), qdot_reference(1), qdot_reference(2), qdot_reference(3));
///ROS_INFO("qdrspindle = %f", qdot_reference(7));
}
int test_delete1()
{
A_ a_(10); // OK
// A_ b(3.14); // Error: conversion from double to int disabled
// a = b; // Error: assignment operator disabled
return 0;
}
NumericArray JumpingCoefficients::getL(
const Position,
const Index sx,
const Index sy,
const Index nx,
const Index ny ) const
{
NumericArray result( 0.0, 5 );
result[C]=
1.0*nx*nx*(a_((sx-0.5)/nx,1.*sy/ny)+a_((sx+0.5)/nx,1.*sy/ny))
+1.0*ny*ny*(b_(1.*sx/nx,(sy-0.5)/ny)+b_(1.*sx/nx,(sy+0.5)/ny));
result[W]=-1.0*nx*nx * a_((sx-0.5)/nx,1.*sy/ny);
result[E]=-1.0*nx*nx * a_((sx+0.5)/nx,1.*sy/ny);
result[S]=-1.0*ny*ny * b_(1.*sx/nx,(sy-0.5)/ny);
result[N]=-1.0*ny*ny * b_(1.*sx/nx,(sy+0.5)/ny);
return result;
}
Precision JumpingCoefficients::apply(
const NumericArray& u,
const Position,
const Index sx,
const Index sy,
const Index nx,
const Index ny ) const
{
return
( nx*nx*(a_((sx-0.5)/nx,1.*sy/ny)+a_((sx+0.5)/nx,1.*sy/ny))
+ny*ny*(b_(1.*sx/nx,(sy-0.5)/ny)+b_(1.*sx/nx,(sy+0.5)/ny))
)*u[sy*(nx+1)+sx]
-1.0*nx*nx * a_((sx-0.5)/nx,1.*sy/ny) * u[sy*(nx+1)+sx-1]
-1.0*nx*nx * a_((sx+0.5)/nx,1.*sy/ny) * u[sy*(nx+1)+sx+1]
-1.0*ny*ny * b_(1.*sx/nx,(sy-0.5)/ny) * u[(sy-1)*(nx+1)+sx]
-1.0*ny*ny * b_(1.*sx/nx,(sy+0.5)/ny) * u[(sy+1)*(nx+1)+sx];
}
SSMProcess* RepeatedStateContractions::contractSpecificStates(int n , int m , SSMProcess tSSMProcess){
assert(n <= this->m_ptrData->getNumStates());
assert(m <= this->m_ptrData->getNumStates());
assert(n != m);
rmatrix a( mid( tSSMProcess.getDistributions() ) );
rmatrix t( mid( tSSMProcess.getTransitionMatrix() ) );
//setting up initial values
rvector p(tSSMProcess.getStationaryProbabilities());
rmatrix a_( RMatrixUtils::zeros( Lb(a,ROW) , Ub(a,ROW)-1 , Lb(a,COL) , Ub(a,COL) ) );
rmatrix p_( RMatrixUtils::zeros( Lb(t,ROW) , Ub(t,ROW)-1 , Lb(t,COL) , Ub(t,COL)-1 ) );
//contracting state in transitionMatrix
for(int i = Lb(a,ROW); i <= Ub(a,ROW); i++){
int buff = (i > n) ? -1 : 0;
for(int x = Lb( Row(a,1) ); x <= Ub( Row(a,1) );x++){
real value = 0.0;
if(i != m && i != n){
a_[i+buff][x] = a[i][x];
}
else if(i == m){
a_[i+buff][x] = (p[i]*a[i][x] + p[n]*a[n][x]) / (p[n]+p[m]);
}
}
}
for(int i = 1; i < tSSMProcess.getNumStates(); i++){
int buffI = (i > n) ? -1 : 0;
for(int j = 1; j < tSSMProcess.getNumStates();j++){
int buffJ = (j > n) ? -1 : 0;
real value = 0.0;
if(i != m && i != n && j != n && j != m){
value = t[i][j];
}
else if( i != m && i != n && j == m ){
value = t[i][m] + t[i][n];
}
else if( i == m && j != m && j != n){
value = (p[m]*t[m][j] + p[n]*t[n][j]) / (p[m] + p[n]);
}
else if( i == m && j == m){
value = ( (p[m]*(t[m][m]+t[m][n])) + (p[n]*(t[n][m]+t[n][n])) ) / (p[m] + p[n]);
}
p_[i+buffI][j+buffJ] = value;
}
}
return new SSMProcess( p_ , a_ , false );
}
void Foam::inclinedFilmNusseltHeightFvPatchScalarField::write
(
Ostream& os
) const
{
fixedValueFvPatchScalarField::write(os);
writeEntry(os, GammaMean_());
writeEntry(os, a_());
writeEntry(os, omega_());
writeEntry(os, "value", *this);
}
void dtkMatrixOperationsTestCase::testSolve(void)
{
dtkUnderscore _;
//Matrix-vector
dtkDenseMatrix<double> a(4,4);
a(1,_)=8.,6.,9.,9.;
a(2,_)=9.,0.,9.,4.;
a(3,_)=1.,2.,1.,8.;
a(4,_)=9.,5.,9.,1.;
dtkDenseVector<double> b(4); b = 1,2,3,4;
dtkDenseVector<double> x=dtk::lapack::solve(a,b);
dtkDenseVector<double> res(4); res = 5.34263959,0.53553299,-5.22081218,0.22588832;
for(int i=1;i<5;i++)
QVERIFY(abs(res(i)-x(i))<1e-7);
dtkDenseMatrix<double> a_(4,3);
dtkDenseVector<double> b_(4);
a_(_,1)=1.,0.,0.,0.;
a_(_,2)=0.,1.,0.,0.;
a_(_,3)=0.,0.,1.,1.;
b_ = 1, 2, 2, 1;
dtkDenseVector<double> res_(3); res_ = 1, 2, 1.5;
dtkDenseVector<double> x_= dtk::lapack::solve(a_,b_);
for(int i = 1; i < x_.length(); ++i) {
QVERIFY((x_(i) - res_(i)) < 1e-7);
}
}
EstimateEMOS2<T,N>::EstimateEMOS2(
Array< RectDomain<N>, 1>& strata,
Array< Array<T,N>, 1>& psfs,
Array<T,N>& img,
int iterations,
T epsilon
) :
EstimateIterative<T,N>(psfs(0), img, iterations),
epsilon_(epsilon), strata_(strata)
{
a_.resize(this->img_.extent(0));
strata_.resize(strata.length(0));
psfsF_.resize(psfs.length(0));
TinyVector<int,N> extent(this->img_.extent());
s_.resize(extent);
prev_.resize(extent);
extent(0) *= 2;
s2_.resize(extent);
extent(N-1) = extent(N-1)/2+1;
estF_.resize(extent);
sF_.resize(extent);
// resize the psfs and compute the Fourier transform (OTF)
for ( int m = 0; m < psfs.length(0); m++ )
{
psfsF_(m).resize(extent);
Array<T,N> psfResized(s2_.extent());
padCenter(psfs(m), psfResized);
psfsF_(m) = forwardFFT(psfResized);
}
// compute the interpolation constants
a_ = 0;
int index = strata_(0).lbound(int(0));
for ( int m = 0; m < strata.length(0); m++ )
{
int l = strata_(m).lbound(int(0));
int u = strata_(m).ubound(int(0));
int size = u - l + 1;
for ( int j = 0; j < size; j++ )
{
a_(index) = T(1) - T(j)/T(size);
index++;
}
}
// compute the scaling factors
scale_ = T(1.0)/strata.length(0);
}
dvariable betacf(const dvariable& a, const dvariable& b, const dvariable& x, int MAXIT)
{
typedef tiny_ad::variable<1, 3> Float;
Float a_ (value(a), 0);
Float b_ (value(b), 1);
Float x_ (value(x), 2);
Float ans = betacf<Float>(a_, b_, x_, MAXIT);
tiny_vec<double, 3> der = ans.getDeriv();
dvariable hh;
value(hh) = ans.value;
gradient_structure::GRAD_STACK1->set_gradient_stack(default_evaluation3ind, &(value(hh)),
&(value(a)), der[0] ,&(value(b)), der[1], &(value(x)), der[2]);
return hh;
}
inline void
dispatch(A& a,
typename sake::alloc::traits<A>::void_pointer const p_obj,
Emplacer const & e,
sake::int_tag<1>)
{
typedef sake::alloc::traits<A> traits_;
SAKE_USING_TYPEDEF( typename Emplacer, value_type );
SAKE_USING_TYPEDEF( typename traits_::template value_traits< value_type >, pointer );
typedef typename sake::alloc::rebind< A, value_type >::type rebind_type;
BOOST_STATIC_ASSERT((boost::is_same<
rebind_type,
typename sake::alloc::rebind< rebind_type, value_type >::type
>));
rebind_type a_(a);
a_.construct(static_cast< pointer >(p_obj), e.construct());
}
vec Filter::Doit(vec u_){
u__.col(0) = u_;
y__.col(0) = b_(0)*u__.col(0);
for(int i = 1; i <= order; i++)
y__.col(0) += b_(i)*u__.col(i) - a_(i)*y__.col(i);
for(int i = order; i > 0; i--){
u__.col(i) = u__.col(i-1);
y__.col(i) = y__.col(i-1);
}
//u__ = shitf(u__,+1);
//y__ = shitf(u__,+1);
return y__.col(0);
}
void Mesher::remove_dupes()
{
std::map<std::array<float, 3>, size_t> verts;
std::set<std::array<size_t, 3>> tris;
size_t vertex_id = 0;
for (auto itr=triangles.begin(); itr != triangles.end(); ++itr)
{
std::array<size_t, 3> t;
int i=0;
// For each vertex, find whether it's already in our vertex
// set. Create an array t with vertex indicies.
for (auto v : {itr->a_(), itr->b_(), itr->c_()})
{
auto k = verts.find(v);
if (k != verts.end())
{
t[i++] = k->second;
}
else
{
verts[v] = vertex_id;
t[i++] = vertex_id;
vertex_id++;
}
}
// Check to see if there are any other triangles that use these
// three vertices; if so, delete this triangle.
std::sort(t.begin(), t.end());
if (tris.count(t))
{
itr = triangles.erase(itr);
itr--;
}
else
{
tris.insert(t);
}
}
}
CTEST(Reversal, check_reversal)
{
a_str a, b;
a_gc;
a = a_(a_reverse(a_new("")));
ASSERT_EQUAL(0, *a);
a = a_(a_reverse(a_new("a")));
ASSERT_EQUAL(0, strcmp(a, "a"));
a = a_(a_reverse(a_new("aa")));
ASSERT_EQUAL(0, strcmp(a, "aa"));
a = a_(a_reverse(a_new("ΞΞ±ΞΞ²ΞΞ³ΞΞ΄ΞΞ΅ΞΞΆΞΞ·ΞΞΈΞΞΉΞΞΊΞΞ»ΞΞΌΞΞ½ΞΞΎΞΞΏΞ ΟΞ‘ΟΞ£ΟΞ€ΟΞ₯Ο
Ξ¦ΟΞ§ΟΞ¨ΟΞ©Ο")));
ASSERT_EQUAL(0, strcmp(a, "ΟΞ©ΟΞ¨ΟΞ§ΟΞ¦Ο
Ξ₯ΟΞ€ΟΞ£ΟΞ‘ΟΞ ΞΏΞΞΎΞΞ½ΞΞΌΞΞ»ΞΞΊΞΞΉΞΞΈΞΞ·ΞΞΆΞΞ΅ΞΞ΄ΞΞ³ΞΞ²ΞΞ±Ξ"));
a = a_(a_reverse(a_new("ΰ€
ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ₯ ΰ₯‘ΰ€ΰ€")));
ASSERT_EQUAL(0, strcmp(a, "ΰ€ΰ€ΰ₯‘ΰ₯ ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€
"));
a = a_(a_reverse(a_new("ββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββ")));
ASSERT_EQUAL(0, strcmp(a, "ββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββ"));
a = a_(a_reverse_new(a_(a_new(""))));
ASSERT_EQUAL(0, *a);
a = a_(a_reverse_new(a_(a_new("a"))));
ASSERT_EQUAL(0, strcmp(a, "a"));
a = a_(a_reverse_new(a_(a_new("ΞΞ±ΞΞ²ΞΞ³ΞΞ΄ΞΞ΅ΞΞΆΞΞ·ΞΞΈΞΞΉΞΞΊΞΞ»ΞΞΌΞΞ½ΞΞΎΞΞΏΞ ΟΞ‘ΟΞ£ΟΞ€ΟΞ₯Ο
Ξ¦ΟΞ§ΟΞ¨ΟΞ©Ο"))));
ASSERT_EQUAL(0, strcmp(a, "ΟΞ©ΟΞ¨ΟΞ§ΟΞ¦Ο
Ξ₯ΟΞ€ΟΞ£ΟΞ‘ΟΞ ΞΏΞΞΎΞΞ½ΞΞΌΞΞ»ΞΞΊΞΞΉΞΞΈΞΞ·ΞΞΆΞΞ΅ΞΞ΄ΞΞ³ΞΞ²ΞΞ±Ξ"));
a = a_(a_reverse_new(a_(a_new("ΰ€
ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ₯ ΰ₯‘ΰ€ΰ€"))));
ASSERT_EQUAL(0, strcmp(a, "ΰ€ΰ€ΰ₯‘ΰ₯ ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€
"));
a = a_(a_reverse_new(a_(a_new("ββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββ"))));
ASSERT_EQUAL(0, strcmp(a, "ββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββ"));
b = a_new(NULL);
b = a_(a_reverse_str(a_(a_new("ββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββ")), b));
ASSERT_EQUAL(0, strcmp(b, "ββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββββ"));
b = a_new(NULL);
b = a_(a_reverse_str(a_(a_new("ΰ€
ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ₯ ΰ₯‘ΰ€ΰ€")), b));
ASSERT_EQUAL(0, strcmp(b, "ΰ€ΰ€ΰ₯‘ΰ₯ ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€ΰ€
"));
b = a_new(NULL);
b = a_(a_reverse_str(a_(a_new("")), b));
ASSERT_EQUAL(0, *b);
b = a_new(NULL);
b = a_(a_reverse_str(a_(a_new("a")), b));
ASSERT_EQUAL(0, strcmp(b, "a"));
a_gc_done();
}
std::list<Vec3f> Mesher::get_contour()
{
// Find all of the singular edges in this fan
// (edges that aren't shared between multiple triangles).
std::set<std::array<float, 6>> valid_edges;
for (auto itr=voxel_start; itr != voxel_end; ++itr)
{
if (valid_edges.count(itr->ba_()))
valid_edges.erase(itr->ba_());
else
valid_edges.insert(itr->ab_());
if (valid_edges.count(itr->cb_()))
valid_edges.erase(itr->cb_());
else
valid_edges.insert(itr->bc_());
if (valid_edges.count(itr->ac_()))
valid_edges.erase(itr->ac_());
else
valid_edges.insert(itr->ca_());
}
std::set<std::array<float, 3>> in_fan;
std::list<Vec3f> contour = {voxel_start->a};
in_fan.insert(voxel_start->a_());
in_fan.insert(voxel_start->b_());
in_fan.insert(voxel_start->c_());
fan_start = voxel_start;
voxel_start++;
while (contour.size() == 1 || contour.front() != contour.back())
{
std::list<Triangle>::iterator itr;
for (itr=fan_start; itr != voxel_end; ++itr)
{
const auto& t = *itr;
if (contour.back() == t.a && valid_edges.count(t.ab_()))
{
contour.push_back(t.b);
break;
}
if (contour.back() == t.b && valid_edges.count(t.bc_()))
{
contour.push_back(t.c);
break;
}
if (contour.back() == t.c && valid_edges.count(t.ca_()))
{
contour.push_back(t.a);
break;
}
}
// If we broke out of the loop (meaning itr is pointing to a relevant
// triangle which should be moved forward to before voxel_start), then
// push the list around and update iterators appropriately.
if (itr != voxel_end)
{
in_fan.insert(itr->a_());
in_fan.insert(itr->b_());
in_fan.insert(itr->c_());
if (itr == voxel_start)
{
voxel_start++;
}
else if (itr != fan_start)
{
const Triangle t = *itr;
triangles.insert(voxel_start, t);
itr = triangles.erase(itr);
itr--;
}
}
}
// Special case to catch triangles that are part of a particular fan but
// don't have any edges in the contour (which can happen!).
for (auto itr=voxel_start; itr != voxel_end; ++itr)
{
if (in_fan.count(itr->a_()) &&
in_fan.count(itr->b_()) &&
in_fan.count(itr->c_()))
{
if (itr == voxel_start)
{
voxel_start++;
}
else if (itr != fan_start)
{
const Triangle t = *itr;
triangles.insert(voxel_start, t);
itr = triangles.erase(itr);
itr--;
}
}
}
// Remove the last point of the contour, since it's a closed loop.
contour.pop_back();
return contour;
}
double operator()(unsigned i, unsigned j) const {
return alpha_ * a_(i,j);
}
double operator()(unsigned i, unsigned j) const {
return a_(i,j) + b_(i,j);
}
double operator()(unsigned i, unsigned j) const {
double result = 0;
for (unsigned k = 0; k < a_.cols(); ++k)
result += a_(i,k) * b_(k,j);
return result;
}
Real a() const { return a_(0.0); }
BigInteger BigInteger::operator*(BigIntegerArithmetic::BigInteger b) {
BigInteger a (this->data);
BigInteger bigint_product;
if (a.data == "0" || b.data == "0") {
bigint_product.data = "0";
return bigint_product;
}
int sign1 = 1, sign2 = 1;
if (a.data[0] == '-') {
sign1 = -1;
}
if (b.data[0] == '-') {
sign2 = -1;
}
int final_sign = sign1 * sign2;
BigInteger a_ (a.data);
BigInteger b_ (b.data);
if (sign1 == -1) {
a_.data = a_.data.substr(1, a_.data.length() - 1);
}
if (sign2 == -1) {
b_.data = b_.data.substr(1, b_.data.length() - 1);
}
std::vector <BigInteger> levels;
for (int j = b_.data.length() - 1; j >= 0; j --) {
BigInteger level;
int carry = 0;
for (int i = a_.data.length() - 1; i >= 0; i --) {
int x = a_.data[i] - 48;
int y = b_.data[j] - 48;
int temp_product = (x * y) + carry;
carry = temp_product / 10;
int sum = temp_product % 10;
level.data.push_back(sum + 48);
}
if (carry != 0) {
level.data.push_back (carry + 48);
}
level.data = reverse(level.data);
for (int k = 0; k < b_.data.length() - 1 - j; k ++) {
level.data.push_back ('0');
}
levels.push_back (level);
}
bigint_product = levels[0];
for (unsigned int i = 1; i < levels.size(); i ++) {
bigint_product = bigint_product + levels[i];
}
if (final_sign == -1) {
bigint_product.data = "-" + bigint_product.data;
}
return bigint_product;
}
void AdmittanceController::initialize(const double Ts, const KDL::JntArray& q_min, const KDL::JntArray& q_max, const std::vector<double>& mass, const std::vector<double>& damping) {
// Resize relevant parameters
uint num_joints = 15;
Ts_ = Ts;
// ToDo: Make variables (use M and D as inputs?)
m_.resize(num_joints);
d_.resize(num_joints);
k_.resize(num_joints);
om_.resize(num_joints);
a_.resize(2,num_joints);
b_.resize(2,num_joints);
tau_previous_.resize(num_joints);
qdot_reference_previous_.resize(num_joints);
q_min_.resize(num_joints);
q_max_.resize(num_joints);
// Set parameters
/*(hardcoded)
for (uint i = 0; i<num_joints; i++) {
m_(i) = 0.1;
d_(i) = 1;
}
m_(7) = 1; // Torso
d_(7) = 10; // Torso
*/
for (uint i = 0; i < num_joints; i++) {
m_(i) = mass[i];
d_(i) = damping[i];
}
for (uint i = 0; i<num_joints; i++) {
k_(i) = 1/d_(i);
om_(i) = d_(i)/m_(i);
double wp = om_(i) + eps;
double alpha = wp/(tan(wp*Ts_/2));
double x1 = alpha/om_(i)+1;
double x2 = -alpha/om_(i)+1;
// Numerator and denominator of the filter
a_(0,i) = 1;
a_(1,i) = x2 / x1;
b_(0,i) = 1 / x1;
b_(1,i) = 1 / x1;
///ROS_INFO("a %i = %f, %f, b %i = %f, %f", i, a_(0,i), a_(1,i), i, b_(0,1), b_(1,i));
// Set previous in- and outputs to zero
tau_previous_(i) = 0;
qdot_reference_previous_(i) = 0;
// Set joint limits
q_min_(i) = q_min(i);
q_max_(i) = q_max(i);
}
// Set inputs, states, outputs to zero
ROS_INFO("Admittance controller initialized");
}
bool Foam::seulex::seul
(
const scalar x0,
const scalarField& y0,
const scalar dxTot,
const label k,
scalarField& y,
const scalarField& scale
) const
{
label nSteps = nSeq_[k];
scalar dx = dxTot/nSteps;
for (label i=0; i<n_; i++)
{
for (label j=0; j<n_; j++)
{
a_(i, j) = -dfdy_(i, j);
}
a_(i, i) += 1/dx;
}
LUDecompose(a_, pivotIndices_);
scalar xnew = x0 + dx;
odes_.derivatives(xnew, y0, dy_);
LUBacksubstitute(a_, pivotIndices_, dy_);
yTemp_ = y0;
for (label nn=1; nn<nSteps; nn++)
{
yTemp_ += dy_;
xnew += dx;
if (nn == 1 && k<=1)
{
scalar dy1 = 0;
for (label i=0; i<n_; i++)
{
dy1 += sqr(dy_[i]/scale[i]);
}
dy1 = sqrt(dy1);
odes_.derivatives(x0 + dx, yTemp_, dydx_);
for (label i=0; i<n_; i++)
{
dy_[i] = dydx_[i] - dy_[i]/dx;
}
LUBacksubstitute(a_, pivotIndices_, dy_);
const scalar denom = min(1, dy1 + SMALL);
scalar dy2 = 0;
for (label i=0; i<n_; i++)
{
// Test of dy_[i] to avoid overflow
if (mag(dy_[i]) > scale[i]*denom)
{
theta_ = 1;
return false;
}
dy2 += sqr(dy_[i]/scale[i]);
}
dy2 = sqrt(dy2);
theta_ = dy2/denom;
if (theta_ > 1)
{
return false;
}
}
odes_.derivatives(xnew, yTemp_, dy_);
LUBacksubstitute(a_, pivotIndices_, dy_);
}
for (label i=0; i<n_; i++)
{
y[i] = yTemp_[i] + dy_[i];
}
return true;
}
rref_result ( Input& xpr, base_t tol)
: tol_(tol)
, a_(xpr)
, n_( nt2::height(a_) )
, m_( nt2::width(a_) )
, jb_(of_size(1, n_))
{
BOOST_ASSERT_MSG(ismatrix(a_), "input to rref must be matrix");
if (tol < Zero<type_t>()) tol = nt2::max(m_,n_)*nt2::Eps<base_t>()*nt2::norm(a_,'I');
itype_t i = 1, j = 1;
itype_t k = 0;
type_t p;
itype_t cnt = 1;
while(i <= m_ && j <= n_)
{
// tie(p, k) = nt2::max(nt2::abs(a_(_(i, m_),j))); //TODO
p = nt2::max(nt2::abs(a_(_(i, m_),j)))(1);
for(int l = i; l <= m_; ++l) if (nt2::abs(a_(l, j)) == p) { k = l; break; }
//k = k+i-1;
if (p <= tol)
{
// the column is negligible, zero it out.
a_(_(i, m_),j) = nt2::zeros(m_-i+1, 1, meta::as_<type_t>());
++j;
}
else
{
// remember column index
//jb_ = cath(jb_, j); //TODO
jb_(cnt) = j; ++cnt;
// swap i-th and k-th rows.
tab_t tmp = a_(i, _(j, n_));
a_(i, _(j, n_)) = a_(k, _(j, n_));
a_(k, _(j, n_)) = tmp;
// a_(cath(i, k),_(j, n)) = a_(cath(k, i),_(j, n));
// divide the pivot row by the pivot element.
type_t tmp1 = a_(i, j);
a_(i,_(j, n_)) = a_(i,_(j, n_))/tmp1;
// subtract multiples of the pivot row from all the other rows.
for (itype_t kk = 1; kk <= m_; ++kk)//[1:i-1 i+1:m]
{
if (kk!=i)
{
type_t tmp2 = a_(kk,j);
a_(kk,_(j, n_)) = a_(kk,_(j, n_))- tmp2*a_(i,_(j, n_));
}
}
++i; ++j;
}
}
jb_ = nt2::expand(jb_, 1, --cnt);
}
int test2(V& v)
{
typedef typename V::value_type A;
v.resize(2);
for (int i = 0; i < 2; i++) {
v[i] = i;
}
std::vector<A> ar({A(10), A(11), A(12), A(13), A(14)});
v.insert(v.cbegin()+1, ar.begin(), ar.end());
V ref({A(0), A(10), A(11), A(12), A(13), A(14), A(1)});
if (!cmp(v, ref)) {
std::cerr << "Insert begin+1 failed!" << std::endl;
return 1;
}
v.insert(v.cbegin(), ar.begin(), ar.end());
V ref1({A(10), A(11), A(12), A(13), A(14), A(0), A(10), A(11), A(12), A(13), A(14), A(1)});
if (!cmp(v, ref1)) {
std::cerr << "Insert begin failed!" << std::endl;
return 1;
}
v.insert(v.cend(), ar.begin(), ar.end());
V ref2({A(10), A(11), A(12), A(13), A(14), A(0), A(10), A(11), A(12), A(13), A(14), A(1), A(10), A(11), A(12), A(13), A(14)});
if (!cmp(v, ref2)) {
std::cerr << "Insert end failed!" << std::endl;
return 1;
}
v.insert(v.cend(), 2, A(20));
V ref4({A(10), A(11), A(12), A(13), A(14), A(0), A(10), A(11), A(12), A(13), A(14), A(1), A(10), A(11), A(12), A(13), A(14), A(20), A(20)});
if (!cmp(v, ref4)) {
std::cerr << "Insert (count,value) to the end failed!" << std::endl;
return 1;
}
v.emplace(v.cbegin()+5, 21);
V ref5({A(10), A(11), A(12), A(13), A(14), A(21), A(0), A(10), A(11), A(12), A(13), A(14), A(1), A(10), A(11), A(12), A(13), A(14), A(20), A(20)});
if (!cmp(v, ref5)) {
std::cerr << "emplace to begin+5 failed!" << std::endl;
return 1;
}
v.insert(v.cbegin(), 2, A(40));
V ref6({A(40), A(40), A(10), A(11), A(12), A(13), A(14), A(21), A(0), A(10), A(11), A(12), A(13), A(14), A(1), A(10), A(11), A(12), A(13), A(14), A(20), A(20)});
if (!cmp(v, ref6)) {
std::cerr << "emplace to begin+5 failed!" << std::endl;
return 1;
}
v.insert(v.cbegin()+1, 2, A(41));
V ref7({A(40), A(41), A(41), A(40), A(10), A(11), A(12), A(13), A(14), A(21), A(0), A(10), A(11), A(12), A(13), A(14), A(1), A(10), A(11), A(12), A(13), A(14), A(20), A(20)});
if (!cmp(v, ref7)) {
std::cerr << "emplace to begin+5 failed!" << std::endl;
return 1;
}
v.insert(v.cbegin(), std::move(A(100)));
V ref8({A(100), A(40), A(41), A(41), A(40), A(10), A(11), A(12), A(13), A(14), A(21), A(0), A(10), A(11), A(12), A(13), A(14), A(1), A(10), A(11), A(12), A(13), A(14), A(20), A(20)});
if (!cmp(v, ref8)) {
std::cerr << "insert to begin + std::move failed!" << std::endl;
return 1;
}
A a_(101);
v.insert(v.cbegin()+4, a_);
V ref9({A(100), A(40), A(41), A(41), A(101), A(40), A(10), A(11), A(12), A(13), A(14), A(21), A(0), A(10), A(11), A(12), A(13), A(14), A(1), A(10), A(11), A(12), A(13), A(14), A(20), A(20)});
if (!cmp(v, ref9)) {
std::cerr << "insert to begin + copy failed!" << std::endl;
return 1;
}
v.insert(v.cbegin()+7, {A(60), A(61)});
V ref10({A(100), A(40), A(41), A(41), A(101), A(40), A(10), A(60), A(61), A(11), A(12), A(13), A(14), A(21), A(0), A(10), A(11), A(12), A(13), A(14), A(1), A(10), A(11), A(12), A(13), A(14), A(20), A(20)});
if (!cmp(v, ref10)) {
std::cerr << "insert to begin + copy failed!" << std::endl;
return 1;
}
return 0;
}
