Real Quad3D::area(const NodesT& nodes)
{
JacobianT jac = jacobian(MappedCoordsT::Zero(), nodes);
CoordsT face_normal;
face_normal[XX] = jac(KSI,YY)*jac(ETA,ZZ) - jac(KSI,ZZ)*jac(ETA,YY);
face_normal[YY] = jac(KSI,ZZ)*jac(ETA,XX) - jac(KSI,XX)*jac(ETA,ZZ);
face_normal[ZZ] = jac(KSI,XX)*jac(ETA,YY) - jac(KSI,YY)*jac(ETA,XX);
return 4.*face_normal.norm();
}
void Triag3DLagrangeP1::normal(const MappedCoordsT& mapped_coord, const NodeMatrixT& nodes, CoordsT& result)
{
JacobianT jac;
jacobian(mapped_coord, nodes, jac);
result[XX] = jac(KSI,YY)*jac(ETA,ZZ) - jac(KSI,ZZ)*jac(ETA,YY);
result[YY] = jac(KSI,ZZ)*jac(ETA,XX) - jac(KSI,XX)*jac(ETA,ZZ);
result[ZZ] = jac(KSI,XX)*jac(ETA,YY) - jac(KSI,YY)*jac(ETA,XX);
}
void speed_ode(size_t n, size_t repeat)
{ // free statically allocated memory
if( n == 0 && repeat == 0 )
return;
//
CppAD::vector<double> x(n);
CppAD::vector<double> jacobian(n * n);
link_ode(n, repeat, x, jacobian);
return;
}
void Quad3D::compute_normal(const NodesT& nodes , CoordsT& normal)
{
JacobianT jac = jacobian(MappedCoordsT::Zero(), nodes);
normal[XX] = jac(KSI,YY)*jac(ETA,ZZ) - jac(KSI,ZZ)*jac(ETA,YY);
normal[YY] = jac(KSI,ZZ)*jac(ETA,XX) - jac(KSI,XX)*jac(ETA,ZZ);
normal[ZZ] = jac(KSI,XX)*jac(ETA,YY) - jac(KSI,YY)*jac(ETA,XX);
normal.normalize();
}
bool available_sparse_jacobian(void)
{ size_t n = 10;
size_t repeat = 1;
size_t ell = 3 * n;
CppAD::vector<double> x(n);
CppAD::vector<size_t> i(ell), j(ell);
CppAD::vector<double> jacobian(ell * n);
return link_sparse_jacobian(repeat, x, i, j, jacobian);
}
void TPSLaplaceResid<EvalT, Traits>::
evaluateFields(typename Traits::EvalData workset) {
// Note that all the integration operations are ScalarT! We need the partials of the Jacobian to show up in the
// system Jacobian as the solution is a function of coordinates (it IS the coordinates!).
// This adds significant time to the compile
Intrepid2::CellTools<ScalarT>::setJacobian(jacobian, refPoints, solnVec, *cellType);
// Since Intrepid2 will perform calculations on the entire workset size and not
// just the used portion, we must fill the excess with reasonable values.
// Leaving this out leads to a floating point exception in
// Intrepid2::RealSpaceTools<Scalar>::det(ArrayDet & detArray,
// const ArrayIn & inMats).
for (std::size_t cell = workset.numCells; cell < worksetSize; ++cell)
for (std::size_t qp = 0; qp < numQPs; ++qp)
for (std::size_t i = 0; i < numDims; ++i)
jacobian(cell, qp, i, i) = 1.0;
Intrepid2::CellTools<ScalarT>::setJacobianDet(jacobian_det, jacobian);
// Straight Laplace's equation evaluation for the nodal coord solution
for(std::size_t cell = 0; cell < workset.numCells; ++cell) {
for(std::size_t node_a = 0; node_a < numNodes; ++node_a) {
for(std::size_t eq = 0; eq < numDims; eq++) {
solnResidual(cell, node_a, eq) = 0.0;
}
for(std::size_t qp = 0; qp < numQPs; ++qp) {
for(std::size_t node_b = 0; node_b < numNodes; ++node_b) {
ScalarT kk = 0.0;
for(std::size_t i = 0; i < numDims; i++) {
kk += grad_at_cub_points(node_a, qp, i) * grad_at_cub_points(node_b, qp, i);
}
for(std::size_t eq = 0; eq < numDims; eq++) {
solnResidual(cell, node_a, eq) +=
kk * solnVec(cell, node_b, eq) * jacobian_det(cell, qp) * refWeights(qp);
}
}
}
}
}
}
static void nloptEqualityFunction(unsigned m, double* result, unsigned n, const double* x, double* grad, void* f_data)
{
context &ctx = *(context *)f_data;
GradientOptimizerContext &goc = ctx.goc;
assert(n == goc.fc->numParam);
Eigen::Map< Eigen::VectorXd > Epoint((double*)x, n);
Eigen::VectorXd Eresult(ctx.origeq);
Eigen::MatrixXd jacobian(n, ctx.origeq);
equality_functional ff(goc);
ff(Epoint, Eresult);
if (grad) {
fd_jacobian(goc.gradientAlgo, goc.gradientIterations, goc.gradientStepSize,
ff, Eresult, Epoint, jacobian);
if (ctx.eqmask.size() == 0) {
ctx.eqmask.assign(m, false);
for (int c1=0; c1 < int(m-1); ++c1) {
for (int c2=c1+1; c2 < int(m); ++c2) {
bool match = (Eresult[c1] == Eresult[c2] &&
(jacobian.col(c1) == jacobian.col(c2)));
if (match && !ctx.eqmask[c2]) {
ctx.eqmask[c2] = match;
++ctx.eqredundent;
if (goc.verbose >= 2) {
mxLog("nlopt: eq constraint %d is redundent with %d",
c1, c2);
}
}
}
}
if (ctx.eqredundent) {
if (goc.verbose >= 1) {
mxLog("nlopt: detected %d redundent equality constraints; retrying",
ctx.eqredundent);
}
nlopt_opt opt = (nlopt_opt) goc.extraData;
nlopt_force_stop(opt);
}
}
}
Eigen::Map< Eigen::VectorXd > Uresult(result, m);
Eigen::Map< Eigen::MatrixXd > Ujacobian(grad, n, m);
int dx=0;
for (int cx=0; cx < int(m); ++cx) {
if (ctx.eqmask[cx]) continue;
Uresult[dx] = Eresult[cx];
if (grad) {
Ujacobian.col(dx) = jacobian.col(cx);
}
++dx;
}
if (goc.verbose >= 4 && grad) {
mxPrintMat("eq result", Uresult);
mxPrintMat("eq jacobian", Ujacobian);
}
}
PyObject* Jacobian(double t, double *x, double *p) {
PyObject *OutObj = NULL;
PyObject *JacOut = NULL;
double *jactual = NULL, **jtemp = NULL;
int i, n;
_init_numpy();
if( (gIData == NULL) || (gIData->isInitBasic == 0) || (gIData->hasJac == 0) ) {
Py_INCREF(Py_None);
return Py_None;
}
else if( (gIData->nExtInputs > 0) && (gIData->isInitExtInputs == 0) ) {
Py_INCREF(Py_None);
return Py_None;
}
else {
OutObj = PyTuple_New(1);
assert(OutObj);
n = gIData->phaseDim;
jactual = (double *)PyMem_Malloc(n*n*sizeof(double));
assert(jactual);
jtemp = (double **)PyMem_Malloc(n*sizeof(double *));
assert(jtemp);
for( i = 0; i < n; i++ ) {
jtemp[i] = jactual + i * n;
}
if( gIData->nExtInputs > 0 ) {
FillCurrentExtInputValues( gIData, t );
}
/* Assume jacobian is returned in column-major format */
jacobian(gIData->phaseDim, gIData->paramDim, t, x, p, jtemp,
gIData->extraSpaceSize, gIData->gExtraSpace,
gIData->nExtInputs, gIData->gCurrentExtInputVals);
PyMem_Free(jtemp);
npy_intp dims[2] = {gIData->phaseDim, gIData->phaseDim};
JacOut = PyArray_SimpleNewFromData(2, dims, NPY_DOUBLE, jactual);
if(JacOut) {
PyArray_UpdateFlags((PyArrayObject *)JacOut, NPY_ARRAY_CARRAY | NPY_ARRAY_OWNDATA);
PyTuple_SetItem(OutObj, 0, PyArray_Transpose((PyArrayObject *)JacOut, NULL));
return OutObj;
}
else {
PyMem_Free(jactual);
Py_INCREF(Py_None);
return Py_None;
}
}
}
KOKKOS_INLINE_FUNCTION
void operator()( int ielem )const {
scalar_type elem_vec[8] = { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 };
scalar_type elem_mat[8][8] =
{ { 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 } ,
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 } ,
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 } ,
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 } ,
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 } ,
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 } ,
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 } ,
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 } };
ScalarCoordType x[8], y[8], z[8];
for ( int i = 0 ; i < 8 ; ++i ) {
const int node_index = elem_node_ids( ielem , i );
x[i] = node_coords( node_index , 0 );
y[i] = node_coords( node_index , 1 );
z[i] = node_coords( node_index , 2 );
}
// This loop could be parallelized; however,
// it would require additional per-thread temporaries
// of 'elem_vec' and 'elem_mat' which would
// consume more local memory and have to be reduced.
for ( unsigned i = 0 ; i < shape_function_data::PointCount ; ++i ) {
scalar_type J[SpatialDim*SpatialDim] = { 0, 0, 0, 0, 0, 0, 0, 0, 0 };
jacobian( x, y, z, shape_eval.gradient[i] , J );
// Overwrite J with its inverse to save scratch memory space.
const scalar_type detJ_w = shape_eval.weight[i] * inverse_and_determinant3x3(J);
const scalar_type k_detJ_w = coeff_K * detJ_w ;
const scalar_type Q_detJ_w = coeff_Q * detJ_w ;
contributeDiffusionMatrix( k_detJ_w , shape_eval.gradient[i] , J , elem_mat );
contributeSourceVector( Q_detJ_w , shape_eval.value[i] , elem_vec );
}
for( size_type i=0; i< ElementNodeCount ; ++i) {
element_vectors(ielem, i) = elem_vec[i] ;
}
for( size_type i = 0; i < ElementNodeCount ; i++){
for( size_type j = 0; j < ElementNodeCount ; j++){
element_matrices(ielem, i, j) = elem_mat[i][j] ;
}
}
}
void speed_sparse_jacobian(size_t n, size_t repeat)
{
size_t ell = 3 * n;
CppAD::vector<double> x(n);
CppAD::vector<size_t> i(ell), j(ell);
CppAD::vector<double> jacobian(ell * n);
// note that cppad/sparse_jacobian.cpp assumes that x.size() == size
link_sparse_jacobian(repeat, x, i, j, jacobian);
return;
}
MatrixXf LinkedStructure::pseudoInverse()
{
// Simple math that represents the mathematics
// explained on the website to computing the
// pseudo inverse. this is exactly the math
// discussed in the tutorial!!!
MatrixXf j = jacobian();
MatrixXf jjtInv = (j * j.transpose());
jjtInv = jjtInv.inverse();
return (j.transpose() * jjtInv);
}
void vfieldjac(int *n, double *t, double *x, double *df, int *ldf,
double *rpar, int *ipar) {
double **f = NULL;
setJacPtrs(gIData, df);
f = gIData->gJacPtrs;
FillCurrentExtInputValues(gIData, *t);
jacobian(*n, (unsigned) gIData->paramDim, *t, x, rpar, f,
(unsigned) gIData->extraSpaceSize, gIData->gExtraSpace,
(unsigned) gIData->nExtInputs, gIData->gCurrentExtInputVals);
}
void Tetra3D::compute_jacobian_adjoint(const MappedCoordsT& mapped_coord, const NodesT& nodes, JacobianT& result)
{
JacobianT J = jacobian(mapped_coord, nodes);
result(0, 0) = (J(1, 1)*J(2, 2) - J(1, 2)*J(2, 1));
result(0, 1) = -(J(0, 1)*J(2, 2) - J(0, 2)*J(2, 1));
result(0, 2) = (J(0, 1)*J(1, 2) - J(1, 1)*J(0, 2));
result(1, 0) = -(J(1, 0)*J(2, 2) - J(1, 2)*J(2, 0));
result(1, 1) = (J(0, 0)*J(2, 2) - J(0, 2)*J(2, 0));
result(1, 2) = -(J(0, 0)*J(1, 2) - J(0, 2)*J(1, 0));
result(2, 0) = (J(1, 0)*J(2, 1) - J(1, 1)*J(2, 0));
result(2, 1) = -(J(0, 0)*J(2, 1) - J(0, 1)*J(2, 0));
result(2, 2) = (J(0, 0)*J(1, 1) - J(0, 1)*J(1, 0));
}
void Triag3D::compute_normal(const NodesT& nodes, CoordsT& result)
{
/// @todo this could be simpler for this application
/// Jacobian could be avoided
JacobianT jac = jacobian(MappedCoordsT::Zero(),nodes);
result[XX] = jac(KSI,YY)*jac(ETA,ZZ) - jac(KSI,ZZ)*jac(ETA,YY);
result[YY] = jac(KSI,ZZ)*jac(ETA,XX) - jac(KSI,XX)*jac(ETA,ZZ);
result[ZZ] = jac(KSI,XX)*jac(ETA,YY) - jac(KSI,YY)*jac(ETA,XX);
// turn into unit vector
result.normalize();
}
point twoLinkArm::getQDDot(point q, point qdot, point xddot)
{
double s12=std::sin(q.X()+q.Y());
double c12=std::cos(q.X()+q.Y());
double fJt1dx2=-params.l1*std::cos(q.X())-params.l2*c12;
double fJt1dy2=-params.l2*std::cos(q.X()+q.Y());
double fJt2dx2=-params.l1*std::sin(q.X())-params.l2*s12;
double fJt2dy2=-params.l2*s12;
double fJt1dxdy=-params.l2*c12;
double fJt2dxdy=-params.l2*s12;
return jacobian(q)/(xddot-(mat2(fJt1dx2*qdot.X()+fJt1dxdy*qdot.Y(),fJt1dy2*qdot.Y()+fJt1dxdy*qdot.X(),
fJt2dx2*qdot.X()+fJt2dxdy*qdot.Y(),fJt2dy2*qdot.Y()+fJt2dxdy*qdot.X())*qdot));
}
void LaplaceResid<EvalT, Traits>::
evaluateFields(typename Traits::EvalData workset) {
// setJacobian only needs to be RealType since the data type is only
// used internally for Basis Fns on reference elements, which are
// not functions of coordinates. This save 18min of compile time!!!
Intrepid::CellTools<MeshScalarT>::setJacobian(jacobian, refPoints, coordVec, *cellType);
// Since Intrepid will perform calculations on the entire workset size and not
// just the used portion, we must fill the excess with reasonable values.
// Leaving this out leads to a floating point exception in
// Intrepid::RealSpaceTools<Scalar>::det(ArrayDet & detArray,
// const ArrayIn & inMats).
for (std::size_t cell = workset.numCells; cell < worksetSize; ++cell)
for (std::size_t qp = 0; qp < numQPs; ++qp)
for (std::size_t i = 0; i < numDims; ++i)
jacobian(cell, qp, i, i) = 1.0;
Intrepid::CellTools<MeshScalarT>::setJacobianDet(jacobian_det, jacobian);
// Straight Laplace's equation evaluation for the nodal coord solution
for(std::size_t cell = 0; cell < workset.numCells; ++cell) {
for(std::size_t node_a = 0; node_a < numNodes; ++node_a) {
for(std::size_t eq = 0; eq < numDims; eq++) {
solnResidual(cell, node_a, eq) = 0.0;
}
for(std::size_t qp = 0; qp < numQPs; ++qp) {
for(std::size_t node_b = 0; node_b < numNodes; ++node_b) {
ScalarT kk = 0.0;
for(std::size_t i = 0; i < numDims; i++) {
kk += grad_at_cub_points(node_a, qp, i) * grad_at_cub_points(node_b, qp, i);
}
for(std::size_t eq = 0; eq < numDims; eq++) {
solnResidual(cell, node_a, eq) +=
kk * solnVec(cell, node_b, eq) * jacobian_det(cell, qp) * refWeights(qp);
}
}
}
}
}
}
void Tetra3DLagrangeP1::jacobian_adjoint(const MappedCoordsT& mapped_coord, const NodeMatrixT& nodes, JacobianT& result)
{
JacobianT J;
jacobian(mapped_coord, nodes, J);
result(0, 0) = (J(1, 1)*J(2, 2) - J(1, 2)*J(2, 1));
result(0, 1) = -(J(0, 1)*J(2, 2) - J(0, 2)*J(2, 1));
result(0, 2) = (J(0, 1)*J(1, 2) - J(1, 1)*J(0, 2));
result(1, 0) = -(J(1, 0)*J(2, 2) - J(1, 2)*J(2, 0));
result(1, 1) = (J(0, 0)*J(2, 2) - J(0, 2)*J(2, 0));
result(1, 2) = -(J(0, 0)*J(1, 2) - J(0, 2)*J(1, 0));
result(2, 0) = (J(1, 0)*J(2, 1) - J(1, 1)*J(2, 0));
result(2, 1) = -(J(0, 0)*J(2, 1) - J(0, 1)*J(2, 0));
result(2, 2) = (J(0, 0)*J(1, 1) - J(0, 1)*J(1, 0));
}
Eigen::VectorXd controlEnv::manipulator_inverse_kinematics_simple(void){
unsigned int n_manip_joints = mobile_manip_.n_manip_joints();
unsigned int n_joints = mobile_manip_.dim();
VectorXd manip_joint_inc(n_manip_joints);
manip_joint_inc = pinv(jacobian(mobile_manip_), 0.001)*scaled_pose_error_.v();
VectorXd joint_inc(n_joints);
for (unsigned int i=0; i<n_joints-n_manip_joints; i++) joint_inc(i) = 0.0;
for (unsigned int i=0; i<n_manip_joints; i++) joint_inc(i+n_joints-n_manip_joints) = manip_joint_inc(i);
return joint_inc;
}
void
GlobalStrainUserObject::finalize()
{
std::vector<Real> residual(9);
std::vector<Real> jacobian(81);
std::copy(&_residual(0, 0), &_residual(0, 0) + 9, residual.begin());
std::copy(&_jacobian(0, 0, 0, 0), &_jacobian(0, 0, 0, 0) + 81, jacobian.begin());
gatherSum(residual);
gatherSum(jacobian);
std::copy(residual.begin(), residual.end(), &_residual(0, 0));
std::copy(jacobian.begin(), jacobian.end(), &_jacobian(0, 0, 0, 0));
}
/* jacobian(tag, m, n, x[n], J[m][n]) */
fint jacobian_(fint* ftag,
fint* fdepen,
fint* findep,
fdouble *fargument,
fdouble *fjac) {
int rc= -1;
int tag=*ftag, depen=*fdepen, indep=*findep;
double** Jac = myalloc2(depen,indep);
double* argument = myalloc1(indep);
spread1(indep,fargument,argument);
rc= jacobian(tag,depen,indep,argument,Jac);
pack2(depen,indep,Jac,fjac);
free((char*)*Jac);
free((char*)Jac);
free((char*)argument);
return rc;
}
bool NoxSolver<Scalar>::computeJacobian(const Epetra_Vector &x, Epetra_Operator &op)
{
Epetra_RowMatrix *jac = dynamic_cast<Epetra_RowMatrix *>(&op);
assert(jac != NULL);
EpetraVector<Scalar> xx(x); // wrap our structures around core Epetra objects
EpetraMatrix<Scalar> jacobian(*jac);
jacobian.zero();
Scalar* coeff_vec = new Scalar[xx.length()];
xx.extract(coeff_vec);
this->dp->assemble(coeff_vec, &jacobian, NULL); // NULL is for the right-hand side.
delete [] coeff_vec;
//jacobian.finish();
return true;
}
//----------------------------------------------------
void TaskObject::computeKinematics()
{
//read the chain joint positions from the controlled joints
unsigned int n_jnts = joint_map_->rows();
KDL::JntArray q(n_jnts);
for(unsigned int i=0; i<n_jnts; i++)
q.data(i) = joints_->at((*joint_map_)(i)).getPosition();
//compute the chain jacobian
KDL::Frame pose;
KDL::Jacobian jacobian(n_jnts);
if(fk_solver_->JntToCart(q,pose) < 0)
ROS_ERROR("Could not compute forward kinematics of task object with link %s.",link_.c_str());
if(j_solver_->JntToJac(q, *chain_jacobian_) < 0)
ROS_ERROR("Could not compute jacobian of task object with link %s.",link_.c_str());
KDLToEigen(pose, *trans_l_r_);
//map the chain jacobian to the controlled joint jacobian
jacobian_->setZero();
for(unsigned int i=0; i<n_jnts; i++)
jacobian_->col((*joint_map_)(i)) = chain_jacobian_->data.col(i);
//Transform the associated geometries
for(unsigned int i=0; i<geometries_->size(); i++)
geometries_->at(i)->setLinkTransform( *trans_l_r_);
// std::cout<<"number of joints: "<<n_jnts<<std::endl;
// std::cout<<"Task object w. root: "<<root_<<", link: "<<link_<<" and id: "<<id_<<std::endl;
// std::cout<<"Pose translation: "<<std::endl<< trans_l_r_->translation().transpose()<<std::endl;
// std::cout<<"Pose rotation: "<<std::endl<< trans_l_r_->rotation()<<std::endl;
// std::cout<<"Chain jacobian: "<<std::endl<<chain_jacobian_->data<<std::endl;
// std::cout<<"Jacobian: "<<std::endl<< *jacobian_<<std::endl;
// std::cout<<std::endl;
// //========= DEBUG PRINT CHAIN ==========
// std::ostringstream out;
// printKDLChain(out, *chain_);
// std::cout<<"CHAIN:"<<std::endl;
// std::cout<<out.str()<<std::endl;
// //========= DEBUG PRINT CHAIN END ==========
}
std::string IPeakFunction::getCentreParameterName() const {
FunctionParameterDecorator_sptr fn =
boost::dynamic_pointer_cast<FunctionParameterDecorator>(
FunctionFactory::Instance().createFunction("PeakParameterFunction"));
if (!fn) {
throw std::runtime_error(
"PeakParameterFunction could not be created successfully.");
}
fn->setDecoratedFunction(this->name());
FunctionDomain1DVector domain(std::vector<double>(4, 0.0));
TempJacobian jacobian(4, fn->nParams());
fn->functionDeriv(domain, jacobian);
return parameterName(jacobian.maxParam(0));
}
bool twoLinkArm::unitTests()
{
point testpoint=point(-.0908,-2.2786);
point fcheck=fkin(testpoint);
std::cout<<"Fcheck: "<<std::endl;
crapPoint(fcheck);
std::cout<<std::endl;
point fcheck2;
std::cout<<"Fcheck2: "<<std::endl;
fkin(testpoint, fcheck, fcheck2);
crapPoint(fcheck);
crapPoint(fcheck2);
std::cout<<std::endl;
point icheck;
std::cout<<"Icheck: ";
std::cout<<ikin(fcheck,icheck)<<std::endl;;
crapPoint(icheck);
std::cout<<std::endl;
std::cout<<"Jacobian: "<<std::endl;
mat2 testjac=jacobian(point(5,6));
crapMat(testjac);
std::cout<<std::endl;
std::cout<<"Q double dot: "<<std::endl;
point qdd=getQDDot(point(1,2),point(3,4),point(5,6));
crapPoint(qdd);
std::cout<<std::endl;
std::cout<<"DC: "<<std::endl;
mat2 D;
point C;
computeInertiaCoriolis(point(1,2),point(3,4),D,C);
crapMat(D);
crapPoint(C);
std::cout<<std::endl;
return 1;
}
int main(void){
float x, y, error, max;
float a, b, dx, dy, jac;
printf("Enter initial approximation for x and y:\n");
scanf("%f %f", &x, &y);
do{
printf("x: %f;\t dx: %f;\ny: %f;\t dy: %f\n\n", x, dx, y, dy);
a = f1(x,y);
b = f2(x,y);
jac = jacobian(x,y);
dx = (b*df1y(x,y) - a*df2y(x,y))/jac;
dy = (a*df2x(x,y) - b*df1x(x,y))/jac;
x = x + dx;
y = y + dy;
printf("x: %f;\t dx: %f;\ny: %f;\t dy: %f;\n\n", x, dx, y, dy);
max = (fabs(dx) > fabs(dy))?fabs(dx):fabs(dy);
}while(max > EPSILON);
return 0;
}
void Function::hansen_matrix(const IntervalVector& box, IntervalMatrix& H, const VarSet& set) const {
int n=set.nb_var;
int m=image_dim();
assert(H.nb_cols()==n);
assert(box.size()==nb_var());
assert(H.nb_rows()==m);
IntervalVector var_box=set.var_box(box);
IntervalVector param_box=set.param_box(box);
IntervalVector x=var_box.mid();
IntervalMatrix J(m,n);
for (int var=0; var<n; var++) {
//var=tab[i];
x[var]=var_box[var];
jacobian(set.full_box(x,param_box),J,set);
H.set_col(var,J.col(var));
}
}
bool correct_sparse_jacobian(bool is_package_double)
{ size_t n = 10;
size_t repeat = 1;
size_t ell = 3 * n;
CppAD::vector<double> x(n);
CppAD::vector<size_t> i(ell), j(ell);
CppAD::vector<double> jacobian(ell * n);
link_sparse_jacobian(repeat, x, i, j, jacobian);
size_t order, size, m;
if( is_package_double)
{ order = 1; // check g(x) using f'(x)
size = n;
}
else
{ order = 2; // check g'(x) using f''(x)
size = n * n;
}
CppAD::vector<double> check(size);
CppAD::sparse_evaluate(x, i, j, order, check);
bool ok = true;
size_t k;
if( is_package_double )
{ for( k = 0; k < ell; k++)
{ double u = check[ i[k] ];
double v = jacobian[k];
ok &= CppAD::NearEqual(u, v, 1e-10, 1e-10);
}
return ok;
}
for(k = 0; k < ell; k++)
{ for(m = 0; m < n; m++)
{ double u = check[ i[k] * n + m ];
double v = jacobian[ k * n + m ];
ok &= CppAD::NearEqual(u, v, 1e-10, 1e-10);
}
}
return ok;
}
double Element::Jdmasterdx(SpaceIndex i, SpaceIndex j, const GaussPoint &g)
const
{
#if DIM==2
// | J11 -J01 |
// J^-1 = | | / |J|
// |-J10 J00 |
double sum = 0;
if(i == j) {
int ii = 1 - i;
for(ElementMapNodeIterator ni=mapnode_iterator(); !ni.end(); ++ni) {
sum += (ni.node()->position())(ii) * ni.masterderiv(ii, g);
}
}
else { // i != j
for(ElementMapNodeIterator ni=mapnode_iterator(); !ni.end(); ++ni)
sum -= (ni.node()->position())(i) * ni.masterderiv(j, g);
}
return sum;
#elif DIM==3
// typing out a closed form in code is messy for 3d
// TODO 3D: it might be worth it to type it out eventually as this is inefficient
double m[DIM][DIM], inverse[DIM][DIM];
int ii, jj;
for(ii=0; ii<DIM; ++ii) {
for(jj=0; jj<DIM; ++jj) {
m[ii][jj] = jacobian(ii,jj,g);
}
}
double dj = vtkMath::Determinant3x3(m);
vtkMath::Invert3x3(m,inverse);
return dj*inverse[i][j];
#endif
}
MatrixXf Arm::psuedo_inv_jacobian(VectorXf theta) {
MatrixXf J = jacobian(theta);
float epsilon = 0.0001f;
MatrixXf result (12, 3);
Matrix3f sigma(3, 3);
JacobiSVD<MatrixXf> svd(J, ComputeThinU | ComputeThinV);
Vector3f singularValues = svd.singularValues();
float sigma1, sigma2, sigma3;
if(singularValues(0) < epsilon) {
sigma1 = 0.0f;
} else {
sigma1 = 1/singularValues(0);
}
if(singularValues(1) < epsilon) {
sigma2 = 0.0f;
} else {
sigma2 = 1/singularValues(1);
}
if(singularValues(2) < epsilon) {
sigma3 = 0.0f;
} else {
sigma3 = 1/singularValues(2);
}
sigma << sigma1, 0, 0,
0, sigma2, 0,
0, 0, sigma3;
result = svd.matrixV() * sigma * svd.matrixU().transpose();
// cout << "psuedo inverse: " << endl << result;
return result;
}
void Function::hansen_matrix(const IntervalVector& box, IntervalMatrix& H) const {
int n=nb_var();
int m=image_dim();
assert(H.nb_cols()==n);
assert(box.size()==n);
assert(H.nb_rows()==m);
IntervalVector x=box.mid();
IntervalMatrix J(m,n);
// test!
// int tab[box.size()];
// box.sort_indices(false,tab);
// int var;
for (int var=0; var<n; var++) {
//var=tab[i];
x[var]=box[var];
jacobian(x,J);
H.set_col(var,J.col(var));
}
}
