arma::mat adiis::compute_jac(const gsl_vector * x) { // Compute jacobian of transformation: jac(i,j) = dc_i / dx_j // Compute coefficients std::vector<double> c(x->size); double xnorm=0.0; for(size_t i=0;i<x->size;i++) { c[i]=gsl_vector_get(x,i); c[i]=c[i]*c[i]; // c_i = x_i^2 xnorm+=c[i]; } for(size_t i=0;i<x->size;i++) c[i]/=xnorm; // c_i = x_i^2 / \sum_j x_j^2 arma::mat jac(c.size(),c.size()); for(size_t i=0;i<c.size();i++) { double xi=gsl_vector_get(x,i); for(size_t j=0;j<c.size();j++) { double xj=gsl_vector_get(x,j); jac(i,j)=-c[i]*2.0*xj/xnorm; } // Extra term on diagonal jac(i,i)+=2.0*xi/xnorm; } return jac; }
Real HeatConductionKernel::computeQpJacobian() { Real jac(0); jac = _k[_qp]*_grad_phi[_j][_qp] * _grad_test[_i][_qp]; jac += _grad_u[_qp] * _grad_test[_i][_qp]*_k_dT[_qp]*_phi[_j][_qp]; return jac; }
void ReactorNet::evalJacobian(doublereal t, doublereal* y, doublereal* ydot, doublereal* p, Array2D* j) { doublereal ysave, dy; Array2D& jac = *j; //evaluate the unperturbed ydot eval(t, y, ydot, p); for (size_t n = 0; n < m_nv; n++) { // perturb x(n) ysave = y[n]; dy = m_atol[n] + fabs(ysave)*m_rtol; y[n] = ysave + dy; dy = y[n] - ysave; // calculate perturbed residual eval(t, y, DATA_PTR(m_ydot), p); // compute nth column of Jacobian for (size_t m = 0; m < m_nv; m++) { jac(m,n) = (m_ydot[m] - ydot[m])/dy; } y[n] = ysave; } }
/// Elementwise operator / AutoDiffBlock operator/(const AutoDiffBlock& rhs) const { if (jac_.empty() && rhs.jac_.empty()) { return constant(val_ / rhs.val_); } if (jac_.empty()) { return val_ / rhs; } if (rhs.jac_.empty()) { return *this / rhs.val_; } int num_blocks = numBlocks(); std::vector<M> jac(num_blocks); assert(numBlocks() == rhs.numBlocks()); typedef Eigen::DiagonalMatrix<Scalar, Eigen::Dynamic> D; D D1 = val_.matrix().asDiagonal(); D D2 = rhs.val_.matrix().asDiagonal(); D D3 = (1.0/(rhs.val_*rhs.val_)).matrix().asDiagonal(); for (int block = 0; block < num_blocks; ++block) { assert(jac_[block].rows() == rhs.jac_[block].rows()); assert(jac_[block].cols() == rhs.jac_[block].cols()); jac[block] = D3 * (D2*jac_[block] - D1*rhs.jac_[block]); } return function(val_ / rhs.val_, jac); }
Real Q2PBorehole::computeQpOffDiagJacobian(unsigned int jvar) { if (jvar == _other_var_num || jvar == _var.number()) return jac(jvar); return 0.0; }
void ReactorNet::evalJacobian(doublereal t, doublereal* y, doublereal* ydot, doublereal* p, Array2D* j) { doublereal ysave, dy; Array2D& jac = *j; // use a try... catch block, since exceptions are not passed // through CVODE, since it is C code try { //evaluate the unperturbed ydot eval(t, y, ydot, p); for (size_t n = 0; n < m_nv; n++) { // perturb x(n) ysave = y[n]; dy = m_atol[n] + fabs(ysave)*m_rtol; y[n] = ysave + dy; dy = y[n] - ysave; // calculate perturbed residual eval(t, y, DATA_PTR(m_ydot), p); // compute nth column of Jacobian for (size_t m = 0; m < m_nv; m++) { jac(m,n) = (m_ydot[m] - ydot[m])/dy; } y[n] = ysave; } } catch (CanteraError& err) { std::cerr << err.what() << std::endl; error("Terminating execution."); } }
static int IDABandSetup(IDAMem IDA_mem, N_Vector yyp, N_Vector ypp, N_Vector rrp, N_Vector tmp1, N_Vector tmp2, N_Vector tmp3) { int retval; long int retfac; IDABandMem idaband_mem; idaband_mem = (IDABandMem) lmem; /* Increment nje counter. */ nje++; /* Zero out JJ; call Jacobian routine jac; return if it failed. */ BandZero(JJ); retval = jac(neq, mu, ml, tn, yyp, ypp, rrp, cj, jacdata, JJ, tmp1, tmp2, tmp3); last_flag = retval; if (retval < 0) return(-1); if (retval > 0) return(+1); /* Do LU factorization of JJ; return success or fail flag. */ retfac = BandFactor(JJ, pivots); if (retfac != 0) { last_flag = 1; return(+1); } last_flag = 0; return(0); }
Vector ADFun<Base>::Jacobian(const Vector &x) { size_t i; size_t n = Domain(); size_t m = Range(); CPPAD_ASSERT_KNOWN( size_t(x.size()) == n, "Jacobian: length of x not equal domain dimension for F" ); // point at which we are evaluating the Jacobian Forward(0, x); // work factor for forward mode size_t workForward = n; // work factor for reverse mode size_t workReverse = 0; for(i = 0; i < m; i++) { if( ! Parameter(i) ) ++workReverse; } // choose the method with the least work Vector jac( n * m ); if( workForward <= workReverse ) JacobianFor(*this, x, jac); else JacobianRev(*this, x, jac); return jac; }
/**------------------------------------------------- * Make a Gauss-Chebyshev quadrature. */ void MakeQuadrature::makeChebyshev() { const double startX = get("StartX"); const double endX = get("EndX"); const int n = get("N"); JacobiPolynomial jac( 0.5, 0.5, n ); }
static int KINDenseSetup(KINMem kin_mem) { KINDenseMem kindense_mem; long int ier; int retval; kindense_mem = (KINDenseMem) lmem; nje++; DenseZero(J); retval = jac(n, J, uu, fval, J_data, vtemp1, vtemp2); if (retval != 0) { last_flag = -1; return(-1); } /* Do LU factorization of J */ ier = DenseGETRF(J, pivots); /* Return 0 if the LU was complete; otherwise return -1 */ last_flag = ier; if (ier > 0) return(-1); return(0); }
Real RichardsPiecewiseLinearSink::computeQpOffDiagJacobian(unsigned int jvar) { if (_richards_name_UO.not_richards_var(jvar)) return 0.0; unsigned int dvar = _richards_name_UO.richards_var_num(jvar); return jac(dvar); }
Real Q2PPiecewiseLinearSink::computeQpOffDiagJacobian(unsigned int jvar) { if (jvar == _var.number() || jvar == _other_var_num) return jac(jvar); return 0.0; }
int ChainIkSolverVel_pinv_givens::CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out) { toggle!=toggle; jnt2jac.JntToJac(q_in,jac); for(unsigned int i=0;i<6;i++) v_in_eigen(i)=v_in(i); for(unsigned int i=0;i<m;i++){ for(unsigned int j=0;j<n;j++) if(transpose) jac_eigen(i,j)=jac(j,i); else jac_eigen(i,j)=jac(i,j); } int ret = svd_eigen_Macie(jac_eigen,U,S,V,B,tempi,1e-15,toggle); //std::cout<<"# sweeps: "<<ret<<std::endl; if(transpose) UY = (V.transpose() * v_in_eigen).lazy(); else UY = (U.transpose() * v_in_eigen).lazy(); for (unsigned int i = 0; i < n; i++){ double wi = UY(i); double alpha = S(i); if (alpha != 0) alpha = 1.0 / alpha; else alpha = 0.0; SUY(i)= alpha * wi; } if(transpose) qdot_eigen = (U * SUY).lazy(); else qdot_eigen = (V * SUY).lazy(); for (unsigned int j=0;j<chain.getNrOfJoints();j++) qdot_out(j)=qdot_eigen(j); return ret; }
double TRAC_IK::ManipValue2(const KDL::JntArray& arr) { KDL::Jacobian jac(arr.data.size()); jacsolver->JntToJac(arr,jac); Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jac.data); Eigen::MatrixXd singular_values = svdsolver.singularValues(); return singular_values.minCoeff()/singular_values.maxCoeff(); }
void System::rhs_combined(const state_t &state, state_t & out, double time) { assert(out.size() == dim*(dim+1)); rhs(state, out, time); jac(ublas::subrange(state,0,dim), jacobian, time, dfdt); for (int i=1; i<out.size() / dim ; i++) { auto st = ublas::subrange(state,dim*i,dim*(i+1)); ublas::subrange(out, dim*i, dim*(i+1)) = ublas::prod(jacobian, st); } }
Real HeatConductionKernel::computeQpJacobian() { Real jac(0); jac = _diffusion_coefficient[_qp] * Diffusion::computeQpJacobian(); if ( _diffusion_coefficient_dT ) { jac += (*_diffusion_coefficient_dT)[_qp] * _phi[_j][_qp] * Diffusion::computeQpResidual(); } return jac; }
//bilateral constraint between two dynamic objects void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, btRigidBody& body2, const btVector3& pos2, btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep) { (void)timeStep; (void)distance; btScalar normalLenSqr = normal.length2(); btAssert(btFabs(normalLenSqr) < btScalar(1.1)); if (normalLenSqr > btScalar(1.1)) { impulse = btScalar(0.); return; } btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition(); //this jacobian entry could be re-used for all iterations btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); btVector3 vel = vel1 - vel2; btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(), body2.getCenterOfMassTransform().getBasis().transpose(), rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(), body2.getInvInertiaDiagLocal(),body2.getInvMass()); btScalar jacDiagAB = jac.getDiagonal(); btScalar jacDiagABInv = btScalar(1.) / jacDiagAB; btScalar rel_vel = jac.getRelativeVelocity( body1.getLinearVelocity(), body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(), body2.getLinearVelocity(), body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); btScalar a; a=jacDiagABInv; rel_vel = normal.dot(vel); //todo: move this into proper structure btScalar contactDamping = btScalar(0.2); #ifdef ONLY_USE_LINEAR_MASS btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass()); impulse = - contactDamping * rel_vel * massTerm; #else btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; impulse = velocityImpulse; #endif }
VectorBase ADFun<Base>::SparseJacobian( const VectorBase& x, const VectorSet& p ) { size_t m = Range(); size_t n = Domain(); VectorBase jac(m * n); typedef typename VectorSet::value_type Set_type; SparseJacobianCase( Set_type(), x, p, jac); return jac; }
AutoDiffBlock<Scalar> operator*(const typename AutoDiffBlock<Scalar>::M& lhs, const AutoDiffBlock<Scalar>& rhs) { int num_blocks = rhs.numBlocks(); std::vector<typename AutoDiffBlock<Scalar>::M> jac(num_blocks); assert(lhs.cols() == rhs.value().rows()); for (int block = 0; block < num_blocks; ++block) { jac[block] = lhs*rhs.derivative()[block]; } typename AutoDiffBlock<Scalar>::V val = lhs*rhs.value().matrix(); return AutoDiffBlock<Scalar>::function(val, jac); }
int main(int argc,char *argv[]){ Reaching reach; BodySchema *body = new KChainBodySchema(); Matrix jac(cartesian_dim,joint_angle_dim); joint_vec_t angle,angle1; cart_vec_t pos; srand(time(NULL)); body->Load(argv[1]); // body->Print(cout); reach.SetBodySchema(body); for(int j=1;j<2;j++){ body->SetRandomAngle(angle); // body->SetAnglesInRange(angle1); // body->SetPosition(angle1); // angle.Print(); body->Angle2Cart(pos); if(!reach.SetLocalTarget(pos)){ cout<<"can't reach"<<endl ; } // cout<<pos<<endl<<endl; body->SetRandomAngle(angle1); // body->SetAnglesInRange(angle1); // body->SetPosition(angle1); reach.MatchToBodySchema(); // while(!reach.TargetReached()){ for(int i=0;i<2;i++){ reach.ReachingStep(); reach.GetPosition(pos,angle); if(reach.TargetReached()){break;} if(i==999){ cout<<endl<<"failed"<<endl; reach.Print(); // for(int k=0;k<300;k++){ // reach.ReachingStep(); // reach.GetPosition(pos,angle); // cout<<pos<<" "<<angle<<endl; // } } } } // cout<<pos<<" "<<angle<<endl; // cout<<(KinematicChain)(*body)<<endl; // ifstream file(argv[1]); // file>>r; // cout<<r<<endl; cout<<"done"<<endl; return 1; }
static int CVDenseSetup(CVodeMem cv_mem, int convfail, N_Vector ypred, N_Vector fpred, booleantype * jcurPtr, N_Vector vtemp1, N_Vector vtemp2, N_Vector vtemp3) { booleantype jbad, jok; realtype dgamma; integertype ier; CVDenseMem cvdense_mem; cvdense_mem = (CVDenseMem) lmem; /* Use nst, gamma/gammap, and convfail to set J eval. flag jok */ dgamma = ABS((gamma / gammap) - ONE); jbad = (nst == 0) || (nst > nstlj + CVD_MSBJ) || ((convfail == FAIL_BAD_J) && (dgamma < CVD_DGMAX)) || (convfail == FAIL_OTHER); jok = !jbad; if (jok) { /* If jok = TRUE, use saved copy of J */ *jcurPtr = FALSE; DenseCopy(savedJ, M); } else { /* If jok = FALSE, call jac routine for new J value */ nje++; if (iopt != NULL) iopt[DENSE_NJE] = nje; nstlj = nst; *jcurPtr = TRUE; DenseZero(M); jac(N, M, f, f_data, tn, ypred, fpred, ewt, h, uround, J_data, &nfe, vtemp1, vtemp2, vtemp3); DenseCopy(M, savedJ); } /* Scale and add I to get M = I - gamma*J */ DenseScale(-gamma, M); DenseAddI(M); /* Do LU factorization of M */ ier = DenseFactor(M, pivots); /* Return 0 if the LU was complete; otherwise return 1 */ if (ier > 0) return (1); return (0); }
double TRAC_IK::ManipValue1(const KDL::JntArray& arr) { KDL::Jacobian jac(arr.data.size()); jacsolver->JntToJac(arr,jac); Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jac.data); Eigen::MatrixXd singular_values = svdsolver.singularValues(); double error = 1.0; for(unsigned int i=0; i < singular_values.rows(); ++i) error *= singular_values(i,0); return error; }
//---------------------------------------------------- boost::shared_ptr<Eigen::MatrixXd> TaskObject::getJacobian(Eigen::Vector3d& base_AB) const { //change the reference point KDL::Jacobian c_jac = (*chain_jacobian_); c_jac.changeRefPoint(KDL::Vector(base_AB(0), base_AB(1), base_AB(2))); //map to the controlled joints jacobian boost::shared_ptr<Eigen::MatrixXd> jac(new Eigen::MatrixXd(jacobian_->rows(), jacobian_->cols())); jac->setZero(); for(unsigned int i=0; i<joint_map_->rows(); i++) jac->col((*joint_map_)(i)) = c_jac.data.col(i); return jac; }
Real AnisoHeatConduction::computeQpJacobian() { Real jac(0); for (unsigned i(0); i < _dim; ++i) { jac += _grad_test[_i][_qp](i) * (*_k_i[i])[_qp] * _grad_phi[_j][_qp](i); if (_k_i_dT[i]) { jac += (*_k_i_dT[i])[_qp] * _phi[_j][_qp] * (_grad_test[_i][_qp](i) * (*_k_i[i])[_qp] * _grad_u[_qp](i)); } } return jac; }
static int CVBandSetup(CVodeMem cv_mem, int convfail, N_Vector ypred, N_Vector fpred, booleantype *jcurPtr, N_Vector vtemp1, N_Vector vtemp2, N_Vector vtemp3) { booleantype jbad, jok; realtype dgamma; long int ier; CVBandMem cvband_mem; cvband_mem = (CVBandMem) lmem; /* Use nst, gamma/gammap, and convfail to set J eval. flag jok */ dgamma = ABS((gamma/gammap) - ONE); jbad = (nst == 0) || (nst > nstlj + CVB_MSBJ) || ((convfail == CV_FAIL_BAD_J) && (dgamma < CVB_DGMAX)) || (convfail == CV_FAIL_OTHER); jok = !jbad; if (jok) { /* If jok = TRUE, use saved copy of J */ *jcurPtr = FALSE; BandCopy(savedJ, M, mu, ml); } else { /* If jok = FALSE, call jac routine for new J value */ nje++; nstlj = nst; *jcurPtr = TRUE; BandZero(M); jac(n, mu, ml, M, tn, ypred, fpred, J_data, vtemp1, vtemp2, vtemp3); BandCopy(M, savedJ, mu, ml); } /* Scale and add I to get M = I - gamma*J */ BandScale(-gamma, M); BandAddI(M); /* Do LU factorization of M */ ier = BandFactor(M, pivots); /* Return 0 if the LU was complete; otherwise return 1 */ if (ier > 0) { last_flag = ier; return(1); } last_flag = CVBAND_SUCCESS; return(0); }
/// This function is not provided by AutoDiffBlock, so we must add it here. inline CollOfScalar sqrt(const CollOfScalar& x) { // d(sqrt(x))/dy = 1/(2*sqrt(x)) * dx/dy const auto& xjac = x.derivative(); if (xjac.empty()) { return CollOfScalar(sqrt(x.value())); } const int num_blocks = xjac.size(); std::vector<CollOfScalar::M> jac(num_blocks); const auto sqrt_x = sqrt(x.value()); const CollOfScalar::M one_over_two_sqrt_x((0.5/sqrt_x).matrix().asDiagonal()); for (int block = 0; block < num_blocks; ++block) { jac[block] = one_over_two_sqrt_x * xjac[block]; } return CollOfScalar::ADB::function(sqrt_x, jac); }
static void BM_JacobianDot(benchmark::State & state) { rbd::MultiBody mb; rbd::MultiBodyConfig mbc; rbd::MultiBodyGraph mbg; std::tie(mb, mbc, mbg) = makeTree30Dof(false); rbd::Jacobian jac(mb, "LARM6"); rbd::forwardKinematics(mb, mbc); rbd::forwardVelocity(mb, mbc); for(auto _ : state) { jac.jacobianDot(mb, mbc); } }
static void BM_VectorBodyJacobian(benchmark::State & state) { rbd::MultiBody mb; rbd::MultiBodyConfig mbc; rbd::MultiBodyGraph mbg; std::tie(mb, mbc, mbg) = makeTree30Dof(false); rbd::Jacobian jac(mb, "LARM6"); rbd::forwardKinematics(mb, mbc); rbd::forwardVelocity(mb, mbc); for(auto _ : state) { jac.vectorBodyJacobian(mb, mbc, Eigen::Vector3d(1., 0., 0.)); } }
template <class T> double GaussNewton<T>::Fit(DArray &c) { double sum = 0.; DArray res(x_.size(), 0.); DMatrix jac(x_.size(), t_.GetC()), dc(1, t_.GetC()); do { //break; ComputeResiduals(res); ComputeJacobian(jac); ComputeDeltas(jac, res, dc); cerr << "Deltas: " << endl; PrintMatrix(dc); } while(RoundToZero(UpdateParameters(dc, c)) != 0.); ComputeResiduals(res); for(int i = 0; i < res.size(); ++i) { sum += res[i] * res[i]; } return sqrt(sum); }
void test_central() { VectorXd x(3); MatrixXd jac(15,3); MatrixXd actual_jac(15,3); my_functor functor; x << 0.082, 1.13, 2.35; // real one functor.actual_df(x, actual_jac); // using NumericalDiff NumericalDiff<my_functor,Central> numDiff(functor); numDiff.df(x, jac); VERIFY_IS_APPROX(jac, actual_jac); }