Real ExtendedOrnsteinUhlenbeckProcess::expectation( Time t0, Real x0, Time dt) const { switch (discretization_) { case MidPoint: return ouProcess_->expectation(t0, x0, dt) + b_(t0+0.5*dt)*(1.0 - std::exp(-speed_*dt)); break; case Trapezodial: { const Time t = t0+dt; const Time u = t0; const Real bt = b_(t); const Real bu = b_(u); const Real ex = std::exp(-speed_*dt); return ouProcess_->expectation(t0, x0, dt) + bt-ex*bu - (bt-bu)/(speed_*dt)*(1-ex); } break; case GaussLobatto: return ouProcess_->expectation(t0, x0, dt) + speed_*std::exp(-speed_*(t0+dt)) * GaussLobattoIntegral(100000, intEps_)( boost::lambda::bind(b_, boost::lambda::_1) *boost::lambda::bind(std::ptr_fun<Real, Real>(std::exp), speed_*boost::lambda::_1), t0, t0+dt); break; default: QL_FAIL("unknown discretization scheme"); } }
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)); }
void HumanoidFootConstraint<Scalar>::computeGeneralConstraintsMatrices(int sizeVec) { computeNbGeneralConstraints(); int N = feetSupervisor_.getNbSamples(); int M = feetSupervisor_.getNbPreviewedSteps(); A_.setZero(nbGeneralConstraints_,sizeVec); b_.setConstant(nbGeneralConstraints_, Constant<Scalar>::MAXIMUM_BOUND_VALUE); //Number of general constraints at current step int nbGeneralConstraintsAtCurrentStep(0); //Sum of general constraints numbers since first step int nbGeneralConstraintsSinceFirstStep(0); for(int i = 0; i<M; ++i) { const ConvexPolygon<Scalar>& cp = feetSupervisor_.getKinematicConvexPolygon(i); nbGeneralConstraintsAtCurrentStep = cp.getNbGeneralConstraints(); for(int j = 0; j<nbGeneralConstraintsAtCurrentStep; ++j) { // Filling matrix A and vector b to create a general constraint of the form // AX + b <=0 A_(nbGeneralConstraintsSinceFirstStep + j, 2*N + i) = cp.getGeneralConstraintsMatrixCoefsForX()(j); A_(nbGeneralConstraintsSinceFirstStep + j, 2*N + M + i) = cp.getGeneralConstraintsMatrixCoefsForY()(j); b_(nbGeneralConstraintsSinceFirstStep + j) = cp.getGeneralConstraintsConstantPart()(j); // This if-else statement adds the required terms to matrices A and b so that // constraints are expressed in world frame if(i==0) { b_(nbGeneralConstraintsSinceFirstStep + j) -= cp.getGeneralConstraintsMatrixCoefsForX()(j) *feetSupervisor_.getSupportFootStateX()(0) + cp.getGeneralConstraintsMatrixCoefsForY()(j) *feetSupervisor_.getSupportFootStateY()(0); } else { A_(nbGeneralConstraintsSinceFirstStep + j, 2*N + i - 1) -= cp.getGeneralConstraintsMatrixCoefsForX()(j); A_(nbGeneralConstraintsSinceFirstStep + j, 2*N + i - 1 + M) -= cp.getGeneralConstraintsMatrixCoefsForY()(j); } } nbGeneralConstraintsSinceFirstStep += nbGeneralConstraintsAtCurrentStep; } }
void Interpolator<Scalar>::computePolynomialNormalisedFactors( VectorX &factor, const Vector3 &initialstate, const Vector3 &finalState, Scalar T ) const { factor(3) = initialstate(0); factor(2) = T*initialstate(1); factor(1) = T*T*initialstate(2)/2; b_(0) = - T*T*initialstate(2)/18 - T*initialstate(1)/3 - initialstate(0); b_(1) = - T*T*initialstate(2)/3 - T*initialstate(1); b_(2) = - T*T*initialstate(2); b_(3) = 0; b_(4) = 0; b_(5) = 0; b_(6) = finalState(0); b_(7) = T*finalState(1); b_(8) = T*T*finalState(2); abc_=aInvNorm_*b_; factor(0) = abc_(0); factor.template segment<8>(4) = abc_.template segment<8>(1); }
static void DoMask(autoBlock_t *fileData) { autoBlock_t *mask = makeBlockHexLine(S_MASK); uint index; for(index = 0; index < getSize(fileData); index++) { b_(fileData)[index] ^= b_(mask)[index % getSize(mask)]; } releaseAutoBlock(mask); }
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; }
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); }
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]; }
void SinglePvtDead::B(const int n, const double* p, const double* /*z*/, double* output_B) const { // #pragma omp parallel for // B = 1/b for (int i = 0; i < n; ++i) { output_B[i] = 1.0/b_(p[i]); } }
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; }
void SinglePvtDead::b(const int n, const double* p, const double* /*r*/, double* output_b, double* output_dbdp, double* output_dbdr) const { // #pragma omp parallel for for (int i = 0; i < n; ++i) { output_b[i] = b_(p[i]); output_dbdp[i] = b_.derivative(p[i]); } std::fill(output_dbdr, output_dbdr + n, 0.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); } } }
T eigen_power_iteration( const matrix<std::complex<T>,D,_A_>& A, O output, const T eps = T(1.0e-5) ) { assert( A.row() == A.col() ); matrix<std::complex<T>,D,_A_> b( A.col(), 1 ); matrix<std::complex<T>,D,_A_> b_( A.col(), 1 ); std::copy( A.diag_cbegin(), A.diag_cend(), b.begin() ); // random initialize matrix<std::complex<T>, D, _A_> A_(A); std::for_each( A_.begin(), A_.end(), [](std::complex<T>& c) { c = std::conj(c); } ); for (;;) { auto const old_b = b; b = A_*b; std::transform( b.begin(), b.end(), b_.begin(), [](std::complex<T> v) { return std::conj(v); } ); auto const u_ = std::inner_product(b.begin(), b.end(), b_.begin(), std::complex<T>(0,0)); auto const u = real(u_); auto const norm = std::sqrt(u); b /= norm; std::transform( b.begin(), b.end(), b_.begin(), [](std::complex<T> v) { return std::conj(v); } ); auto const U_ = std::inner_product(b.begin(), b.end(), b_.begin(), std::complex<T>(0,0)); auto const U = real(U_); std::transform( old_b.begin(), old_b.end(), b_.begin(), [](std::complex<T> v) { return std::conj(v); } ); auto const V_ = std::inner_product(old_b.begin(), old_b.end(), b.begin(), std::complex<T>(0,0)); auto const V = real(V_); auto const UV_ = std::inner_product(b.begin(), b.end(), b_.begin(), std::complex<T>(0,0)); auto const UV = real(UV_); if ( UV * UV > U*V*(T(1)-eps) ) { std::copy( b.begin(), b.end(), output ); return norm; } } assert( !"eigen_power_iteration:: should never reach here!" ); return T(0); //just to kill warnings }
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); } }
inline result_type operator()(T1 t1, T2 t2) { return b_(t1, t2); }
inline result_type operator()(T t) { return b_(t); }
/*--------------------------------------------------------------*/ void nrn_scopmath_solve_thread(int n, double** a, double* b, int* perm, double* p, int* y) #define y_(arg) p[y[arg]] #define b_(arg) b[arg] { int i, j, pivot; double sum; /* Perform forward substitution with pivoting */ if (y) { for (i = 0; i < n; i++) { pivot = perm[i]; sum = 0.0; for (j = 0; j < i; j++) sum += a[pivot][j] * (y_(j)); y_(i) = (b_(pivot) - sum) / a[pivot][i]; } /* * Note that the y vector is already in the correct order for back * substitution. Perform back substitution, pivoting the matrix but not * the y vector. There is no need to divide by the diagonal element as * this is assumed to be unity. */ for (i = n - 1; i >= 0; i--) { pivot = perm[i]; sum = 0.0; for (j = i + 1; j < n; j++) sum += a[pivot][j] * (y_(j)); y_(i) -= sum; } }else{ for (i = 0; i < n; i++) { pivot = perm[i]; sum = 0.0; for (j = 0; j < i; j++) sum += a[pivot][j] * (p[j]); p[i] = (b_(pivot) - sum) / a[pivot][i]; } /* * Note that the y vector is already in the correct order for back * substitution. Perform back substitution, pivoting the matrix but not * the y vector. There is no need to divide by the diagonal element as * this is assumed to be unity. */ for (i = n - 1; i >= 0; i--) { pivot = perm[i]; sum = 0.0; for (j = i + 1; j < n; j++) sum += a[pivot][j] * (p[j]); p[i] -= sum; } } }
Real ExtendedOrnsteinUhlenbeckProcess::drift(Time t, Real x) const { return ouProcess_->drift(t, x) + speed_*b_(t); }
void test_matrix() { //matrix.h vs. Eigen //for timing int n = (int) 1e6; timeval t0, t1; const int SIZE = 10; //matrix.h matrices Real A[SIZE*SIZE]; Real B[SIZE*SIZE]; Real B0[SIZE*SIZE]; //backup, to initialize Eigen Real b[SIZE]; for (int i=0; i<SIZE*SIZE; i++) { A[i] = (Real) i+1; B[i] = 1; B0[i] = B[i]; } for (int i=0; i<SIZE; i++) { b[i] = 1; } //std::cout << "A=\n"; printMatReal(SIZE,SIZE,A,-1,-1); //std::cout << "B=\n"; printMatReal(SIZE,SIZE,B,-1,-1); //std::cout << "b=\n"; printMatReal(SIZE,1,b,-1,-1); //matrix.h int ri = 2; int ci = 3; int brows = SIZE/2; int bcols = SIZE/2; Real val = 2.0; //Real m = 1.0 + 1e-6; Real m = 1.0; gettimeofday(&t0, NULL); for (int i=0; i<n; i++) { //setMat(SIZE,SIZE,val,B); //setMatRow(SIZE,SIZE,ri,val,B); //setMatCol(SIZE,ci,val,B); //setMatBlock(SIZE,ri,ci,brows,bcols,val,B); //mulcMat(SIZE,SIZE,m,B); //mulcMatRow(SIZE,SIZE,ri,m,B); //mulcMatCol(SIZE,ci,m,B); //mulcMatBlock(SIZE,ri,ci,brows,bcols,m,B); //copyMat(SIZE,SIZE,A,B); //copyMatRow(SIZE,SIZE,ri,A,SIZE,ri,B); //copyMatCol(SIZE,ci,A,ci,B); //copyMatBlock(SIZE,ri,ci,brows,bcols,A, SIZE,ri,ci,B); //copyTMat(SIZE,SIZE,A,B); //addmMat(SIZE,SIZE,A,m,B); //addmMatRow(SIZE,SIZE,ri,A,SIZE,ri,m,B); //addmMatCol(SIZE,ci,A,ci,m,B); //addmMatBlock(SIZE,ri,ci,brows,bcols,A, SIZE,ri,ci,m,B); //multMatVec(SIZE,SIZE,A,b,m,B); //multMatTVec(SIZE,SIZE,A,b,m,B); //multMatMat(SIZE,SIZE,A,SIZE,A,m,B); multMatTMat(SIZE,SIZE,A,SIZE,A,m,B); } gettimeofday(&t1, NULL); std::cout << "(matrix.h) B=\n"; printMatReal(SIZE,SIZE,B,0,-1); std::cout << "iterations: " << (Real) n << std::endl; std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl; std::cout << std::endl; if (1) { //Eigen //dynamic Eigen matrices Eigen::Matrix<Real,Eigen::Dynamic,Eigen::Dynamic> A_(SIZE,SIZE); Eigen::Matrix<Real,Eigen::Dynamic,Eigen::Dynamic> B_(SIZE,SIZE); Eigen::Matrix<Real,Eigen::Dynamic,Eigen::Dynamic> b_(SIZE,1); //fixed Eigen matrices //Eigen::Matrix<Real,SIZE,SIZE> A_; //Eigen::Matrix<Real,SIZE,SIZE> B_; //Eigen::Matrix<Real,SIZE,1> b_; memcpy(A_.data(), A, sizeof(Real)*SIZE*SIZE); memcpy(B_.data(), B0, sizeof(Real)*SIZE*SIZE); memcpy(b_.data(), b, sizeof(Real)*SIZE); //std::cout << "Eigen:\n"; //std::cout << "A_=\n" << A_ << std::endl; //std::cout << "B_=\n" << B_ << std::endl; //std::cout << "b_=\n" << b_ << std::endl; gettimeofday(&t0, NULL); for (int i=0; i<n; i++) { //B_.setConstant(val); //B_.row(ri).setConstant(val); //B_.col(ci).setConstant(val); //B_.block(ri,ci,brows,bcols).setConstant(val); //B_ *= m; //B_.row(ri) *= m; //B_.col(ci) *= m; //B_.block(ri,ci,brows,bcols) *= m; //B_ = A_; //B_.row(ri) = A_.row(ri); //B_.col(ci) = A_.col(ci); //B_.block(ri,ci,brows,bcols) = A_.block(ri,ci,brows,bcols); //B_ = A_.transpose(); //B_ += (A_ * m); //B_.row(ri) += (A_.row(ri) * m); //B_.col(ci) += (A_.col(ci) * m); //B_.block(ri,ci,brows,bcols) += (A_.block(ri,ci,brows,bcols) * m); //B_.col(0) = A_*b_; //B_.col(0) = A_.transpose()*b_; //B_ = A_*A_; B_ = A_.transpose()*A_; } gettimeofday(&t1, NULL); std::cout << "(Eigen) B=\n" << B_ << std::endl; std::cout << "iterations: " << (Real) n << std::endl; std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl; } }
unsigned int MethodGear<ODESystemKernel>::FindCorrection(const double t, const Eigen::VectorXd& errorWeights) { // Current solution to be updated vb_ = v_[0]; // Check Constraints CheckConstraints(vb_); // Call the equations this->Equations(vb_, t, f_); numberOfFunctionCalls_++; // Force recalculation of Jacobian matrix (every maxIterationsJacobian_) if (numberOfSteps_ >= stepOfLastJacobian_ + maxIterationsJacobian_) jacobianStatus_ = JACOBIAN_STATUS_HAS_TO_BE_CHANGED; if (jacobianStatus_ == JACOBIAN_STATUS_HAS_TO_BE_CHANGED) { // The Jacobian is evaluated at the current step stepOfLastJacobian_ = numberOfSteps_; // Evaluation of the Jacobian matrix switch (jacobianType_) { case JACOBIAN_TYPE_CONST: jacobianStatus_ = JACOBIAN_STATUS_OK; break; case JACOBIAN_TYPE_USERDEFINED: jacobianStatus_ = JACOBIAN_STATUS_MODIFIED; this->UserDefinedJacobian(vb_, t); numberOfJacobians_++; factorizationStatus_ = MATRIX_HAS_TO_BE_FACTORIZED; break; case JACOBIAN_TYPE_NUMERICAL: jacobianStatus_ = JACOBIAN_STATUS_MODIFIED; this->NumericalJacobian(vb_, t, f_, h_, errorWeights, max_constraints_, max_values_); numberOfJacobians_++; factorizationStatus_ = MATRIX_HAS_TO_BE_FACTORIZED; break; } } // The hr0 value is used to build the G matrix (see Eq. 29.214) const double hr0 = h_*r_[0](p_ - 1); // If the order was changed, the matrix is factorized (of course!) if (factorizationStatus_ == MATRIX_FACTORIZED) { if (iterOrder_ == 0) factorizationStatus_ = MATRIX_HAS_TO_BE_FACTORIZED; } // The matrix is assembled and factorized if (factorizationStatus_ == MATRIX_HAS_TO_BE_FACTORIZED) { stepOfLastFactorization_ = numberOfSteps_; numberOfMatrixFactorizations_++; this->BuildAndFactorizeMatrixG(hr0); factorizationStatus_ = MATRIX_FACTORIZED; } // The firs iteration of the Newton's method is performed // The first guess value is assumed b=0, which means that the following linear system is solved: G*d = -v1+h*f // The solution d, since b=0 on first guess, is equal to the new b { deltab_ = f_*h_; deltab_ -= v_[1]; this->SolveLinearSystem(deltab_); numberOfLinearSystemSolutions_++; b_ = deltab_; } // Newton's iterations double oldCorrectionControl; for (unsigned int k = 1; k <= maxConvergenceIterations_; k++) { // Estimation of the error const double correctionControl = r_[0](p_ - 1)*OpenSMOKE::ErrorControl(deltab_, errorWeights); // If the error is sufficiently small, the convergence is ok if (correctionControl < 1.) { convergenceStatus_ = CONVERGENCE_STATUS_OK; return k; } // If the new error is 2 times larger than the previous one, it is better to stop the Newton's method // and try to restart with a smaller step if (k > 1 && correctionControl > 2.*oldCorrectionControl) { convergenceStatus_ = CONVERGENCE_STATUS_FAILURE; return k; } // Store the error oldCorrectionControl = correctionControl; // Update the solution by adding the vector b: v0 = v0+r0*b va_ = b_*r_[0](p_ - 1); va_ += v_[0]; // Check constraints for minimum and maximum values { // Minimum constraints if (min_constraints_ == true) { for (unsigned int j = 0; j < this->ne_; j++) if (va_(j) < min_values_(j)) { va_(j) = min_values_(j); b_(j) = (min_values_(j) - z_[0](j)) / r_[0](p_ - 1); } } // Maximum constraints if (max_constraints_ == true) { for (unsigned int j = 0; j < this->ne_; j++) if (va_(j) > max_values_(j)) { va_(j) = max_values_(j); b_(j) = (max_values_(j) - z_[0](j)) / r_[0](p_ - 1); } } } // Call the equations this->Equations(va_, t, f_); numberOfFunctionCalls_++; // Apply the Newton's iteration { deltab_ = f_*h_; deltab_ -= v_[1]; deltab_ -= b_; this->SolveLinearSystem(deltab_); numberOfLinearSystemSolutions_++; b_ += deltab_; } } convergenceStatus_ = CONVERGENCE_STATUS_FAILURE; return maxConvergenceIterations_; }
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"); }
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 a_(i,j) + b_(i,j); }
inline result_type operator()(T1 t1, T2 t2, T3 t3) { return b_(t1, t2, t3); }
/*! Least squares method used to make the tracking more robust. It ensures that the points taken into account to compute the right equation belong to the ellipse. */ void vpMeEllipse::leastSquare() { // Construction du systeme Ax=b //! i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4 // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 ) unsigned int i ; vpMeSite p_me ; unsigned int iter =0 ; vpColVector b_(numberOfSignal()) ; vpRobust r(numberOfSignal()) ; r.setThreshold(2); r.setIteration(0) ; vpMatrix D(numberOfSignal(),numberOfSignal()) ; D.setIdentity() ; vpMatrix DA, DAmemory ; vpColVector DAx ; vpColVector w(numberOfSignal()) ; w =1 ; unsigned int nos_1 = numberOfSignal() ; if (list.size() < 3) { vpERROR_TRACE("Not enough point") ; throw(vpTrackingException(vpTrackingException::notEnoughPointError, "not enough point")) ; } if (circle ==false) { vpMatrix A(numberOfSignal(),5) ; vpColVector x(5); unsigned int k =0 ; for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){ p_me = *it; if (p_me.getState() == vpMeSite::NO_SUPPRESSION) { A[k][0] = vpMath::sqr(p_me.jfloat) ; A[k][1] = 2 * p_me.ifloat * p_me.jfloat ; A[k][2] = 2 * p_me.ifloat ; A[k][3] = 2 * p_me.jfloat ; A[k][4] = 1 ; b_[k] = - vpMath::sqr(p_me.ifloat) ; k++ ; } } while (iter < 4 ) { DA = D*A ; vpMatrix DAp ; x = DA.pseudoInverse(1e-26) *D*b_ ; vpColVector residu(nos_1); residu = b_ - A*x; r.setIteration(iter) ; r.MEstimator(vpRobust::TUKEY,residu,w) ; k = 0; for (i=0 ; i < nos_1 ; i++) { D[k][k] =w[k] ; k++; } iter++; } k =0 ; for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){ p_me = *it; if (p_me.getState() == vpMeSite::NO_SUPPRESSION) { if (w[k] < thresholdWeight) { p_me.setState(vpMeSite::M_ESTIMATOR); *it = p_me; } k++ ; } } for(i = 0; i < 5; i ++) K[i] = x[i]; } else { vpMatrix A(numberOfSignal(),3) ; vpColVector x(3); unsigned int k =0 ; for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){ p_me = *it; if (p_me.getState() == vpMeSite::NO_SUPPRESSION) { A[k][0] = 2* p_me.ifloat ; A[k][1] = 2 * p_me.jfloat ; A[k][2] = 1 ; b_[k] = - vpMath::sqr(p_me.ifloat) - vpMath::sqr(p_me.jfloat) ; k++ ; } } while (iter < 4 ) { DA = D*A ; vpMatrix DAp ; x = DA.pseudoInverse(1e-26) *D*b_ ; vpColVector residu(nos_1); residu = b_ - A*x; r.setIteration(iter) ; r.MEstimator(vpRobust::TUKEY,residu,w) ; k = 0; for (i=0 ; i < nos_1 ; i++) { D[k][k] =w[k]; k++; } iter++; } k =0 ; for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){ p_me = *it; if (p_me.getState() == vpMeSite::NO_SUPPRESSION) { if (w[k] < thresholdWeight) { p_me.setState(vpMeSite::M_ESTIMATOR); *it = p_me; } k++ ; } } for(i = 0; i < 3; i ++) K[i+2] = x[i]; } getParameters() ; }
void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const unsigned int n, unsigned *i, unsigned *j) { vpCDEBUG(1) <<" begin vpMeEllipse::initTracking()"<<std::endl ; if (circle==false) { vpMatrix A(n,5) ; vpColVector b_(n) ; vpColVector x(5) ; // Construction du systeme Ax=b //! i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4 // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 ) for (unsigned int k =0 ; k < n ; k++) { A[k][0] = vpMath::sqr(j[k]) ; A[k][1] = 2* i[k] * j[k] ; A[k][2] = 2* i[k] ; A[k][3] = 2* j[k] ; A[k][4] = 1 ; b_[k] = - vpMath::sqr(i[k]) ; } K = A.pseudoInverse(1e-26)*b_ ; std::cout << K << std::endl; } else { vpMatrix A(n,3) ; vpColVector b_(n) ; vpColVector x(3) ; vpColVector Kc(3) ; for (unsigned int k =0 ; k < n ; k++) { A[k][0] = 2* i[k] ; A[k][1] = 2* j[k] ; A[k][2] = 1 ; b_[k] = - vpMath::sqr(i[k]) - vpMath::sqr(j[k]) ; } Kc = A.pseudoInverse(1e-26)*b_ ; K[0] = 1 ; K[1] = 0 ; K[2] = Kc[0] ; K[3] = Kc[1] ; K[4] = Kc[2] ; std::cout << K << std::endl; } iP1.set_i( i[0] ); iP1.set_j( j[0] ); iP2.set_i( i[n-1] ); iP2.set_j( j[n-1] ); getParameters() ; computeAngle(iP1, iP2) ; display(I, vpColor::green) ; sample(I) ; // 2. On appelle ce qui n'est pas specifique { vpMeTracker::initTracking(I) ; } try{ track(I) ; } catch(...) { vpERROR_TRACE("Error caught") ; throw ; } vpMeTracker::display(I) ; vpDisplay::flush(I) ; }
Real b() const { return b_(0.0); }
/*! Initialization of the tracking. The ellipse is defined thanks to the coordinates of n points. \warning It is better to use at least five points to well estimate the K parameters. \param I : Image in which the ellipse appears. \param n : The number of points in the list. \param iP : A pointer to a list of pointsbelonging to the ellipse edge. */ void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const unsigned int n, vpImagePoint *iP) { vpCDEBUG(1) <<" begin vpMeEllipse::initTracking()"<<std::endl ; if (circle==false) { vpMatrix A(n,5) ; vpColVector b_(n) ; vpColVector x(5) ; // Construction du systeme Ax=b //! i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4 // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 ) for (unsigned int k =0 ; k < n ; k++) { A[k][0] = vpMath::sqr(iP[k].get_j()) ; A[k][1] = 2* iP[k].get_i() * iP[k].get_j() ; A[k][2] = 2* iP[k].get_i() ; A[k][3] = 2* iP[k].get_j() ; A[k][4] = 1 ; b_[k] = - vpMath::sqr(iP[k].get_i()) ; } K = A.pseudoInverse(1e-26)*b_ ; std::cout << K << std::endl; } else { vpMatrix A(n,3) ; vpColVector b_(n) ; vpColVector x(3) ; vpColVector Kc(3) ; for (unsigned int k =0 ; k < n ; k++) { A[k][0] = 2* iP[k].get_i() ; A[k][1] = 2* iP[k].get_j() ; A[k][2] = 1 ; b_[k] = - vpMath::sqr(iP[k].get_i()) - vpMath::sqr(iP[k].get_j()) ; } Kc = A.pseudoInverse(1e-26)*b_ ; K[0] = 1 ; K[1] = 0 ; K[2] = Kc[0] ; K[3] = Kc[1] ; K[4] = Kc[2] ; std::cout << K << std::endl; } iP1 = iP[0]; iP2 = iP[n-1]; getParameters() ; std::cout << "vpMeEllipse::initTracking() ellipse avant: " << iPc << " " << a << " " << b << " " << vpMath::deg(e) << " alpha: " << alpha1 << " " << alpha2 << std::endl; computeAngle(iP1, iP2) ; std::cout << "vpMeEllipse::initTracking() ellipse apres: " << iPc << " " << a << " " << b << " " << vpMath::deg(e) << " alpha: " << alpha1 << " " << alpha2 << std::endl; expecteddensity = (alpha2-alpha1) / vpMath::rad((double)me->getSampleStep()); display(I, vpColor::green) ; sample(I) ; // 2. On appelle ce qui n'est pas specifique { vpMeTracker::initTracking(I) ; } try{ track(I) ; } catch(...) { vpERROR_TRACE("Error caught") ; throw ; } vpMeTracker::display(I) ; vpDisplay::flush(I) ; }
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; }