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