void DfEriX_Parallel::getJ(const TlDenseVector_Lapack& rho,
TlDenseSymmetricMatrix_Scalapack* pJ) {
assert(pJ != NULL);
const TlOrbitalInfo orbitalInfo((*(this->pPdfParam_))["coordinates"],
(*(this->pPdfParam_))["basis_set"]);
const TlOrbitalInfo_Density orbitalInfo_Density(
(*(this->pPdfParam_))["coordinates"],
(*(this->pPdfParam_))["basis_set_j"]);
const ShellArrayTable shellArrayTable =
this->makeShellArrayTable(orbitalInfo);
const ShellArrayTable shellArrayTable_Density =
this->makeShellArrayTable(orbitalInfo_Density);
pJ->resize(this->m_nNumOfAOs);
TlSparseSymmetricMatrix tmpJ(this->m_nNumOfAOs);
// pJ->zeroClear();
this->createEngines();
DfTaskCtrl* pDfTaskCtrl = this->getDfTaskCtrlObject();
std::vector<DfTaskCtrl::Task2> taskList;
bool hasTask = pDfTaskCtrl->getQueue2(orbitalInfo, true, this->grainSize_,
&taskList, true);
while (hasTask == true) {
this->getJ_part(orbitalInfo, orbitalInfo_Density, shellArrayTable_Density,
taskList, rho, &tmpJ);
hasTask =
pDfTaskCtrl->getQueue2(orbitalInfo, true, this->grainSize_, &taskList);
}
// this->finalize(pJ);
pJ->mergeSparseMatrix(tmpJ);
pDfTaskCtrl->cutoffReport();
delete pDfTaskCtrl;
pDfTaskCtrl = NULL;
this->destroyEngines();
}
RobotInterface::Status Test_IK_QP::RobotUpdate(){
//updates the kinematic chain
mSKinematicChain->setJoints(mJointKinematics.Array());
mSKinematicChain->getEndPos(lPos.Array());
mSKinematicChain->getJoints(mJointDesPos.Array());
//gets the jacobian from the kinematic chain and gives it to the solver
Matrix tmpJ(6,KUKA_DOF);
mSKinematicChain->getJacobian(tmpJ);
mSolver.setJacobian(tmpJ);
Vector TargetPos(6);
if(mStep==0){
//initial position of the end effector
mInitPos = lPos;
mStep = 1;
mTIME=0;
}
if(mStep == 1){
//The end effector goes to the desired position to follow the trajectory
double TimeToInitialPosition(10);
//Target position of the end effector
TargetPos[0] = (1-mTIME/TimeToInitialPosition)* mInitPos[0] ;
TargetPos[1] = (1-mTIME/TimeToInitialPosition)* mInitPos[1] + 0.5* mTIME/TimeToInitialPosition ;
TargetPos[2] = (1-mTIME/TimeToInitialPosition)* mInitPos[2] + 0.6* mTIME/TimeToInitialPosition ;
//Velocity of the end effector if the trajectory is perfectly followed, this is the (if possible analytical) time derivative of TargetPos
mTargetVelocity[0] = (- mInitPos[0] )/TimeToInitialPosition ;
mTargetVelocity[1] = (- mInitPos[1] + 0.5)/TimeToInitialPosition ;
mTargetVelocity[2] = (- mInitPos[2] + 0.6)/TimeToInitialPosition ;
if(mTIME>TimeToInitialPosition){
mTIME=0;
mStep = 2;
}
}
if(mStep ==2){
//"infinity motion
//Proportional to the frequency of the loop
double k = 0.4;
TargetPos[0] = 0.5 *sin(sin(k*mTIME) * sin(k*mTIME));
TargetPos[1] = 0.5* cos(sin(k*mTIME) * sin(k*mTIME));
TargetPos[2] = 0.6 + 0.2 * sin(4*k*mTIME);
//the target velocity has been found with wolfram, numerical differentiation of TargetPos would also work
mTargetVelocity[0] = 0.5*k*sin(2*k*mTIME) * cos(sin(k*mTIME)*sin(k*mTIME));
mTargetVelocity[1] = -0.5*2*k*sin(k*mTIME) * sin(sin(k*mTIME)*sin(k*mTIME))*cos(k* mTIME);
mTargetVelocity[2] = 4* 0.2 * k* cos(4*k*mTIME);
}
Vector tmpRdot(6);
//Velocity of the end effector that will be given to the solver, it includes a gain to do positional tracking in addition to velocity tracking
double gain (0.2);
for(unsigned int i(0); i<3; ++i){
tmpRdot[i] = mTargetVelocity[i]+ gain*(TargetPos[i]- lPos[i])/_dt;
}
//Solves the inverse kinematics
mSolver.Solve(tmpRdot, mSensorsGroup.GetJointAngles());
//gets the joints velocity
mJointDesVel = mSolver.getOutput();
//update the position of the joints by forward Euler numerical integration
mSKinematicChain->getJoints(lJoints.Array());
mJointDesPos = lJoints + mJointDesVel*_dt;
//sets the updated joints positions
mSKinematicChain->setJoints(mJointDesPos.Array());
mJointKinematics.Set(mJointDesPos);
mTIME += _dt;
return STATUS_OK;
}
