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;
}
