/**
*@brief 手先速度から関節角速度を取得
* @param v 手先速度
* @return 関節角速度
*/
Vector3d RobotArm::calcJointVel(Vector3d v)
{
Matrix3d J = calcJacobian(theta);
Matrix3d Jinv = J.inverse();
Vector3d vf(v(0), v(1), v(2));
Vector3d A = Jinv * vf;
//std::cout << Jinv << std::endl << std::endl;
return A;
}
const DenseMatrix<Real> &
BoundaryFluxBase::getJacobian(unsigned int iside,
dof_id_type ielem,
const std::vector<Real> & uvec1,
const RealVectorValue & dwave,
THREAD_ID tid) const
{
Threads::spin_mutex::scoped_lock lock(_mutex);
if (_cached_elem_id != ielem || _cached_side_id != iside)
{
_cached_elem_id = ielem;
_cached_side_id = iside;
calcJacobian(iside, ielem, uvec1, dwave, _jac1[tid]);
}
return _jac1[tid];
}
IK::Scalar IK::performFirstOrderStep(const Transformation& goalTransformation,IK::Scalar currentStepSize)
{
/*******************************************************************
Calculate chain of current global transformations from the root to
the leaf. The transformations are set up such that the leaf's
transformation when reaching the goal is the identity
transformation.
*******************************************************************/
/* Calculate joint transformations: */
calcJointTransformations();
/*******************************************************************
Calculate the joint parameter update vector and the current
residual.
*******************************************************************/
Scalar residual=calcDeltaP();
/*******************************************************************
If the effector joint is not the end effector (bi-directional IK),
the calculated parameter update vector must be modified to not move
the end effector. More precisely, the parameter update vector must
be projected into the full chain's Jacobian matrix' null space.
*******************************************************************/
if(effectorJointIndex<numJoints)
{
/*******************************************************************
Initialize the parameter update values for joints after the effector
to zero.
*******************************************************************/
for(int i=effectorJointIndex;i<numJoints;++i)
deltaP(i,0)=Scalar(0);
/*******************************************************************
Calculate the full chain's Jacobian matrix.
*******************************************************************/
calcJacobian();
/*******************************************************************
Project the calculated parameter update vector into the Jacobian's
null space. If IK were linear, this would avoid any motion of the
end effector.
*******************************************************************/
/* Calculate J * J^T: */
DenseMatrix jjt(6,6);
inplaceTransposed2Multiplication(jjt,jacobian,jacobian);
/* Calculate the parameter update vector's effect on the end effector: */
DenseMatrix jpv(6,1);
inplaceMultiplication(jpv,jacobian,deltaP);
/* First projection step: */
DenseMatrix p1=jjt.solveLinearEquations(jpv);
/* Second projection step: */
DenseMatrix pvn(numJoints,1);
inplaceTransposed1Multiplication(pvn,jacobian,p1);
/* Subtract pvn from deltaP to project deltaP into Jacobian's null space: */
for(int i=0;i<numJoints;++i)
deltaP(i,0)-=pvn(i,0);
}
/*******************************************************************
Perform a first-order integration step towards the goal using the
parameter update vector.
*******************************************************************/
updateJointAngles(currentStepSize);
return residual;
}
void Newton::solve()
{
long int
dimRHS = 1, // Dimension of right hand side of linear system (=b)
info = 0; // Retrun-flag of Fortran code
int
totSteps = 0; // Total number of steps taken
double
atol = _newtonSettings->getAtol(),
rtol = _newtonSettings->getRtol();
// If initialize() was not called yet
if (_firstCall)
initialize();
// Get current values and residuals from system
_algLoop->getReal(_y);
_algLoop->evaluate();
_algLoop->getRHS(_f);
// Reset status flag
_iterationStatus = CONTINUE;
while (_iterationStatus == CONTINUE) {
// Check stopping criterion
if (!_algLoop->isLinear()) {
_iterationStatus = DONE;
for (int i = 0; i < _dimSys; ++i) {
if (std::abs(_f[i]) > atol + rtol * std::abs(_y[i])) {
_iterationStatus = CONTINUE;
break;
}
}
}
if (_iterationStatus == CONTINUE) {
if (totSteps < _newtonSettings->getNewtMax()) {
// Determination of Jacobian (Fortran-format)
if (_algLoop->isLinear() && !_algLoop->isLinearTearing()) {
const matrix_t& A = _algLoop->getSystemMatrix();
const double* jac = A.data().begin();
std::copy(jac, jac + _dimSys*_dimSys, _jac);
dgesv_(&_dimSys, &dimRHS, _jac, &_dimSys, _iHelp, _f, &_dimSys, &info);
std::copy(_f, _f + _dimSys, _y);
_algLoop->setReal(_y);
if (info != 0)
throw ModelicaSimulationError(ALGLOOP_SOLVER,
"error solving linear system (dgesv info: " + to_string(info) + ")");
else
_iterationStatus = DONE;
}
else if (_algLoop->isLinearTearing()) {
_algLoop->setReal(_zeroVec);
_algLoop->evaluate();
_algLoop->getRHS(_f);
const matrix_t& A_sparse = _algLoop->getSystemMatrix();
//m_t A_dense(A_sparse);
const double* jac = A_sparse.data().begin();
std::copy(jac, jac + _dimSys*_dimSys, _jac);
dgesv_(&_dimSys, &dimRHS, _jac, &_dimSys, _iHelp, _f, &_dimSys, &info);
for (int i = 0; i < _dimSys; i++)
_y[i] = -_f[i];
_algLoop->setReal(_y);
_algLoop->evaluate();
if (info != 0)
throw ModelicaSimulationError(ALGLOOP_SOLVER,
"error solving linear tearing system (dgesv info: " + to_string(info) + ")");
else
_iterationStatus = DONE;
}
else {
LogSysVec(_algLoop, "y" + to_string(totSteps), _y);
LogSysVec(_algLoop, "f" + to_string(totSteps), _f);
double phi = 0.0; // line search function
for (int i = 0; i < _dimSys; ++i) {
phi += _f[i] * _f[i];
}
calcJacobian();
// Solve linear System
dgesv_(&_dimSys, &dimRHS, _jac, &_dimSys, _iHelp, _f, &_dimSys, &info);
if (info != 0)
throw ModelicaSimulationError(ALGLOOP_SOLVER,
"error solving nonlinear system (iteration: " + to_string(totSteps)
+ ", dgesv info: " + to_string(info) + ")");
// Increase counter
++ totSteps;
// New iterate
double lambda = 1.0; // step size
double alpha = 1e-4; // guard for sufficient decrease
// first find a feasible step
while (true) {
for (int i = 0; i < _dimSys; i++) {
_yHelp[i] = _y[i] - lambda * _f[i];
_yHelp[i] = std::min(_yMax[i], std::max(_yMin[i], _yHelp[i]));
}
// evaluate function
try {
calcFunction(_yHelp, _fHelp);
}
catch (ModelicaSimulationError& ex) {
if (lambda < 1e-10)
throw ex;
// reduce step size
lambda *= 0.5;
continue;
}
break;
}
// check for solution, e.g. if a linear system is treated here
_iterationStatus = DONE;
for (int i = 0; i < _dimSys; i++) {
if (std::abs(_fHelp[i]) > atol + rtol * std::abs(_yHelp[i])) {
_iterationStatus = CONTINUE;
break;
}
}
// second do line search with quadratic approximation of phi(lambda)
// C.T.Kelley: Solving Nonlinear Equations with Newton's Method,
// no 1 in Fundamentals of Algorithms, SIAM 2003. ISBN 0-89871-546-6.
double phiHelp = 0.0;
for (int i = 0; i < _dimSys; i++)
phiHelp += _fHelp[i] * _fHelp[i];
while (_iterationStatus == CONTINUE) {
// test half step that also serves as max bound for step reduction
double lambdaTest = 0.5*lambda;
for (int i = 0; i < _dimSys; i++) {
_yTest[i] = _y[i] - lambdaTest * _f[i];
_yTest[i] = std::min(_yMax[i], std::max(_yMin[i], _yTest[i]));
}
calcFunction(_yTest, _fTest);
double phiTest = 0.0;
for (int i = 0; i < _dimSys; i++)
phiTest += _fTest[i] * _fTest[i];
// check for sufficient decrease of phiHelp
// and no further decrease with phiTest
// otherwise minimize quadratic approximation of phi(lambda)
if (phiHelp > (1.0 - alpha * lambda) * phi ||
phiTest < (1.0 - alpha * lambda) * phiHelp) {
long int n = 3;
long int ipiv[3];
double bx[] = {phi, phiTest, phiHelp};
double A[] = {1.0, 1.0, 1.0,
0.0, lambdaTest, lambda,
0.0, lambdaTest*lambdaTest, lambda*lambda};
dgesv_(&n, &dimRHS, A, &n, ipiv, bx, &n, &info);
lambda = std::max(0.1*lambda, -0.5*bx[1]/bx[2]);
if (!(lambda >= 1e-10))
throw ModelicaSimulationError(ALGLOOP_SOLVER,
"Can't get sufficient decrease of solution");
if (lambda >= lambdaTest) {
// upper bound 0.5*lambda
lambda = lambdaTest;
std::copy(_yTest, _yTest + _dimSys, _yHelp);
std::copy(_fTest, _fTest + _dimSys, _fHelp);
phiHelp = phiTest;
}
else {
for (int i = 0; i < _dimSys; i++) {
_yHelp[i] = _y[i] - lambda * _f[i];
_yHelp[i] = std::min(_yMax[i], std::max(_yMin[i], _yHelp[i]));
}
calcFunction(_yHelp, _fHelp);
phiHelp = 0.0;
for (int i = 0; i < _dimSys; i++) {
phiHelp += _fHelp[i] * _fHelp[i];
}
}
if (Logger::getInstance()->isOutput(LC_NLS, LL_DEBUG)) {
std::stringstream ss;
ss << "Newton: eq" << to_string(_algLoop->getEquationIndex());
ss << ", time " << _algLoop->getSimTime();
ss << ": lambda = " << lambda;
ss << ", phi = " << phi << " --> " << phiHelp;
Logger::write(ss.str(), LC_NLS, LL_DEBUG);
}
}
// check for sufficient decrease
if (phiHelp <= (1.0 - alpha * lambda) * phi)
break;
}
// take iterate
std::copy(_yHelp, _yHelp + _dimSys, _y);
std::copy(_fHelp, _fHelp + _dimSys, _f);
phi = phiHelp;
}
}
else
throw ModelicaSimulationError(ALGLOOP_SOLVER,
"error solving nonlinear system (iteration limit: " + to_string(totSteps) + ")");
}
}
LogSysVec(_algLoop, "y*", _y);
}
void Newton::solve()
{
long int
dimRHS = 1, // Dimension of right hand side of linear system (=b)
irtrn = 0; // Retrun-flag of Fortran code
int
totStps = 0; // Total number of steps
// If initialize() was not called yet
if (_firstCall)
initialize();
// Get initializeial values from system
_algLoop->getReal(_y);
//_algLoop->evaluate(command);
_algLoop->getRHS(_f);
// Reset status flag
_iterationStatus = CONTINUE;
while(_iterationStatus == CONTINUE)
{
_iterationStatus = DONE;
// Check stopping criterion
calcFunction(_y,_f);
if(totStps)
{
for(int i=0; i<_dimSys; ++i)
{
if(fabs(_f[i]) > _newtonSettings->getAtol() +_newtonSettings->getRtol() * ( fabs(_f[i])))
{
_iterationStatus = CONTINUE;
break;
}
}
}
else
_iterationStatus = CONTINUE;
// New right hand side
//calcFunction(_y,_f);
if(_iterationStatus == CONTINUE)
{
if(totStps < _newtonSettings->getNewtMax())
{
// Determination of Jacobian (Fortran-format)
if(_algLoop->isLinear()&&!_algLoop->isLinearTearing())
{
//calcFunction(_yHelp,_fHelp);
const matrix_t& A = _algLoop->getSystemMatrix();
const double* jac = A.data().begin();
memcpy(_jac, jac, _dimSys*_dimSys*sizeof(double));
dgesv_(&_dimSys,&dimRHS,_jac,&_dimSys,_iHelp,_f,&_dimSys,&irtrn);
memcpy(_y,_f,_dimSys*sizeof(double));
_algLoop->setReal(_y);
if(irtrn != 0)
throw ModelicaSimulationError(ALGLOOP_SOLVER,"error solving linear tearing system");
else
_iterationStatus = DONE;
}
else if(_algLoop->isLinearTearing())
{
long int dimRHS = 1; // Dimension of right hand side of linear system (=b)
long int irtrn = 0; // Retrun-flag of Fortran code
_algLoop->setReal(_zeroVec);
_algLoop->evaluate();
_algLoop->getRHS(_f);
/* adaptor_t f_adaptor(_dimSys,_f);
shared_matrix_t b(_dimSys,1,f_adaptor);*/
//print_m (b, "b vector");
const matrix_t& A_sparse = _algLoop->getSystemMatrix();
//m_t A_dense(A_sparse);
const double* jac = A_sparse.data().begin();
/*double* jac = new double[dimSys*dimSys];
for(int i=0;i<dimSys;i++)
for(int j=0;j<dimSys;j++)
jac[i*_dimSys+j] = A_sparse(i,j);*/
memcpy(_jac, jac, _dimSys*_dimSys*sizeof(double));
dgesv_(&_dimSys, &dimRHS, _jac, &_dimSys, _iHelp, _f,&_dimSys,&irtrn);
// std::vector< int > ipiv (_dimSys); // pivot vector
//lapack::gesv (A_sparse, ipiv,b); // solving the system, b contains x
for(int i=0; i<_dimSys; i++)
_f[i]=-_f[i];
memcpy(_y, _f, _dimSys*sizeof(double));
_algLoop->setReal(_y);
_algLoop->evaluate();
if(irtrn != 0)
throw ModelicaSimulationError(ALGLOOP_SOLVER,"error solving linear tearing system");
else
_iterationStatus = DONE;
}
else
{
calcJacobian();
// Solve linear System
dgesv_(&_dimSys,&dimRHS,_jac,&_dimSys,_iHelp,_f,&_dimSys,&irtrn);
if(irtrn!=0)
{
// TODO: Throw an error message here.
_iterationStatus = SOLVERERROR;
break;
}
// Increase counter
++ totStps;
// New solution
for(int i=0; i<_dimSys; ++i)
_y[i] -= _newtonSettings->getDelta() * _f[i];
}
}
else
_iterationStatus = SOLVERERROR;
}
}
}
