/* Algoritmo di Metropolis */
double HOmetropolis (double* state, int state_dim)
{
int i;
double u[2];
double x_new, temp1, temp2;
double DS = 0;
for(i=0; i<state_dim; i++)
{
ranlxd(u,2);
x_new = state[i] + DELTA*(2*u[1] - 1);
temp1 = deltaS(state,state_dim,x_new,i);
temp2 = exp(-temp1);
/* scelta nuova configurazione */
if(temp2 >= u[0])
{
state[i] = x_new;
DS += temp1;
}
}
return DS;
}
void
PLC_LSH::computeStress()
{
// Given the stretching, compute the stress increment and add it to the old stress. Also update the creep strain
// stress = stressOld + stressIncrement
// creep_strain = creep_strainOld + creep_strainIncrement
if (_t_step == 0 && !_app.isRestarting())
return;
if (_output_iteration_info == true)
{
_console
<< std::endl
<< "iteration output for combined creep-plasticity solve:"
<< " time=" <<_t
<< " temperature=" << _temperature[_qp]
<< " int_pt=" << _qp
<< std::endl;
}
// compute trial stress
SymmTensor stress_new( *elasticityTensor() * _strain_increment );
stress_new += _stress_old;
SymmTensor creep_strain_increment;
SymmTensor plastic_strain_increment;
SymmTensor elastic_strain_increment;
SymmTensor stress_new_last( stress_new );
Real delS(_absolute_stress_tolerance+1);
Real first_delS(delS);
unsigned int counter(0);
while (counter < _max_its &&
delS > _absolute_stress_tolerance &&
(delS/first_delS) > _relative_tolerance)
{
elastic_strain_increment = _strain_increment;
elastic_strain_increment -= plastic_strain_increment;
stress_new = *elasticityTensor() * elastic_strain_increment;
stress_new += _stress_old;
elastic_strain_increment = _strain_increment;
computeCreep( elastic_strain_increment, creep_strain_increment, stress_new );
// now use stress_new to calculate a new effective_trial_stress and determine if
// yield has occured and if so, calculate the corresponding plastic strain
elastic_strain_increment -= creep_strain_increment;
computeLSH( elastic_strain_increment, plastic_strain_increment, stress_new );
elastic_strain_increment -= plastic_strain_increment;
// now check convergence
SymmTensor deltaS(stress_new_last - stress_new);
delS = std::sqrt(deltaS.doubleContraction(deltaS));
if (counter == 0)
{
first_delS = delS;
}
stress_new_last = stress_new;
if (_output_iteration_info == true)
{
_console
<< "stress_it=" << counter
<< " rel_delS=" << delS/first_delS
<< " rel_tol=" << _relative_tolerance
<< " abs_delS=" << delS
<< " abs_tol=" << _absolute_stress_tolerance
<< std::endl;
}
++counter;
}
if (counter == _max_its &&
delS > _absolute_stress_tolerance &&
(delS/first_delS) > _relative_tolerance)
{
mooseError("Max stress iteration hit during plasticity-creep solve!");
}
_strain_increment = elastic_strain_increment;
_stress[_qp] = stress_new;
}
void
CombinedCreepPlasticity::computeStress( const Elem & current_elem,
unsigned qp, const SymmElasticityTensor & elasticityTensor,
const SymmTensor & stress_old, SymmTensor & strain_increment,
SymmTensor & stress_new )
{
// Given the stretching, compute the stress increment and add it to the old stress. Also update the creep strain
// stress = stressOld + stressIncrement
// creep_strain = creep_strainOld + creep_strainIncrement
if(_t_step == 0) return;
if (_output_iteration_info == true)
{
Moose::out
<< std::endl
<< "iteration output for CombinedCreepPlasticity solve:"
<< " time=" <<_t
<< " temperature=" << _temperature[qp]
<< " int_pt=" << qp
<< std::endl;
}
// compute trial stress
stress_new = elasticityTensor * strain_increment;
stress_new += stress_old;
const SubdomainID current_block = current_elem.subdomain_id();
const std::vector<ReturnMappingModel*> & rmm( _submodels[current_block] );
const unsigned num_submodels = rmm.size();
SymmTensor inelastic_strain_increment;
SymmTensor elastic_strain_increment;
SymmTensor stress_new_last( stress_new );
Real delS(_absolute_tolerance+1);
Real first_delS(delS);
unsigned int counter(0);
while(counter < _max_its &&
delS > _absolute_tolerance &&
(delS/first_delS) > _relative_tolerance &&
(num_submodels != 1 || counter < 1))
{
elastic_strain_increment = strain_increment;
stress_new = elasticityTensor * (elastic_strain_increment - inelastic_strain_increment);
stress_new += stress_old;
for (unsigned i_rmm(0); i_rmm < num_submodels; ++i_rmm)
{
rmm[i_rmm]->computeStress( current_elem, qp, elasticityTensor, stress_old, elastic_strain_increment,
stress_new, inelastic_strain_increment );
}
// now check convergence
SymmTensor deltaS(stress_new_last - stress_new);
delS = std::sqrt(deltaS.doubleContraction(deltaS));
if (counter == 0)
{
first_delS = delS;
}
stress_new_last = stress_new;
if (_output_iteration_info == true)
{
Moose::out
<< "stress_it=" << counter
<< " rel_delS=" << (0 == first_delS ? 0 : delS/first_delS)
<< " rel_tol=" << _relative_tolerance
<< " abs_delS=" << delS
<< " abs_tol=" << _absolute_tolerance
<< std::endl;
}
++counter;
}
if(counter == _max_its &&
delS > _absolute_tolerance &&
(delS/first_delS) > _relative_tolerance)
{
mooseError("Max stress iteration hit during CombinedCreepPlasticity solve!");
}
strain_increment = elastic_strain_increment;
}
bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double endEffectorTargetOrientation[4],
const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4],
const double* q_current, int numQ,int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
{
bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) ? true : false;
Jacobian ikJacobian(useAngularPart,numQ);
ikJacobian.Reset();
bool UseJacobianTargets1 = false;
if ( UseJacobianTargets1 ) {
ikJacobian.SetJtargetActive();
}
else {
ikJacobian.SetJendActive();
}
VectorR3 targets;
targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]);
ikJacobian.ComputeJacobian(&targets); // Set up Jacobian and deltaS vectors
// Set one end effector world position from Bullet
VectorRn deltaS(3);
for (int i = 0; i < 3; ++i)
{
deltaS.Set(i,dampIk[i]*(endEffectorTargetPosition[i]-endEffectorWorldPosition[i]));
}
// Set one end effector world orientation from Bullet
VectorRn deltaR(3);
if (useAngularPart)
{
btQuaternion startQ(endEffectorWorldOrientation[0],endEffectorWorldOrientation[1],endEffectorWorldOrientation[2],endEffectorWorldOrientation[3]);
btQuaternion endQ(endEffectorTargetOrientation[0],endEffectorTargetOrientation[1],endEffectorTargetOrientation[2],endEffectorTargetOrientation[3]);
btQuaternion deltaQ = endQ*startQ.inverse();
float angle = deltaQ.getAngle();
btVector3 axis = deltaQ.getAxis();
if (angle > PI) {
angle -= 2.0*PI;
}
else if (angle < -PI) {
angle += 2.0*PI;
}
float angleDot = angle;
btVector3 angularVel = angleDot*axis.normalize();
for (int i = 0; i < 3; ++i)
{
deltaR.Set(i,dampIk[i+3]*angularVel[i]);
}
}
{
if (useAngularPart)
{
VectorRn deltaC(6);
MatrixRmn completeJacobian(6,numQ);
for (int i = 0; i < 3; ++i)
{
deltaC.Set(i,deltaS[i]);
deltaC.Set(i+3,deltaR[i]);
for (int j = 0; j < numQ; ++j)
{
completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]);
completeJacobian.Set(i+3,j,angular_jacobian[i*numQ+j]);
}
}
ikJacobian.SetDeltaS(deltaC);
ikJacobian.SetJendTrans(completeJacobian);
} else
{
VectorRn deltaC(3);
MatrixRmn completeJacobian(3,numQ);
for (int i = 0; i < 3; ++i)
{
deltaC.Set(i,deltaS[i]);
for (int j = 0; j < numQ; ++j)
{
completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]);
}
}
ikJacobian.SetDeltaS(deltaC);
ikJacobian.SetJendTrans(completeJacobian);
}
}
// Calculate the change in theta values
switch (ikMethod) {
case IK2_JACOB_TRANS:
ikJacobian.CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK2_DLS:
case IK2_VEL_DLS:
case IK2_VEL_DLS_WITH_ORIENTATION:
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength()==numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
break;
case IK2_VEL_DLS_WITH_NULLSPACE:
case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
assert(m_data->m_nullSpaceVelocity.GetLength()==numQ);
ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
break;
case IK2_DLS_SVD:
ikJacobian.CalcDeltaThetasDLSwithSVD();
break;
case IK2_PURE_PSEUDO:
ikJacobian.CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break;
case IK2_SDLS:
ikJacobian.CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
default:
ikJacobian.ZeroDeltaThetas();
break;
}
// Use for velocity IK, update theta dot
//ikJacobian.UpdateThetaDot();
// Use for position IK, incrementally update theta
//ikJacobian.UpdateThetas();
// Apply the change in the theta values
//ikJacobian.UpdatedSClampValue(&targets);
for (int i=0;i<numQ;i++)
{
// Use for velocity IK
q_new[i] = ikJacobian.dTheta[i] + q_current[i];
// Use for position IK
//q_new[i] = m_data->m_ikNodes[i]->GetTheta();
}
return true;
}
