static void calculSolutionDementhon(double xi0, double yi0, vpColVector &I, vpColVector &J, vpHomogeneousMatrix &cMo ) { #if (DEBUG_LEVEL1) std::cout << "begin (Dementhon.cc)CalculSolutionDementhon() " << std::endl; #endif double normI, normJ, normk, Z0; vpColVector k(3); // normalisation de I et J normI = sqrt(I.sumSquare()) ; normJ = sqrt(J.sumSquare()) ; I/=normI; J/=normJ; k = vpColVector::cross(I,J) ; // k = I^I Z0=2.0/(normI+normJ); normk = sqrt(k.sumSquare()) ; k /= normk ; J = vpColVector::cross(k,I) ; //calcul de la matrice de passage cMo[0][0]=I[0]; cMo[0][1]=I[1]; cMo[0][2]=I[2]; cMo[0][3]=xi0*Z0; cMo[1][0]=J[0]; cMo[1][1]=J[1]; cMo[1][2]=J[2]; cMo[1][3]=yi0*Z0; cMo[2][0]=k[0]; cMo[2][1]=k[1]; cMo[2][2]=k[2]; cMo[2][3]=Z0; #if (DEBUG_LEVEL1) std::cout << "end (Dementhon.cc)CalculSolutionDementhon() " << std::endl; #endif }
// Fill in the plot message for publishing data to be plotted in the /VS_errors topic void makePlotMSG(std_msgs::Float64MultiArray &plotMsg, vpColVector &taskError, vpColVector &qdot, vpColVector &q1dot, vpColVector &q2dot, vpMatrix &fJe, vpVelocityTwistMatrix &eVf, KDL::JntArray &kdlArmPosition ) { // add qdot - total joint velocity for(unsigned int i=0; i<qdot.getRows(); i++) { plotMsg.data.push_back(qdot[i]); } // add q1dot - main task for(unsigned int i=0; i<q1dot.getRows(); i++) { plotMsg.data.push_back(q1dot[i]); } // add q2dot - main task for(unsigned int i=0; i<q2dot.getRows(); i++) { plotMsg.data.push_back(q2dot[i]); } // add the end-effector velocity vpColVector vel; vel = eVf*fJe*qdot; for(unsigned int i=0; i<vel.getRows(); i++) { plotMsg.data.push_back(vel[i]); } // add the joint positions for(unsigned int i=0; i<kdlArmPosition.rows(); i++) { plotMsg.data.push_back(kdlArmPosition.data[i]); } // add the error norm plotMsg.data.push_back(taskError.sumSquare()); // add the errors for(unsigned int i=0; i<taskError.getRows(); i++) { ROS_INFO_STREAM("=> makePlotMSG: adding taskError at position " << plotMsg.data.size()); plotMsg.data.push_back(taskError[i]); } }
void vpMbKltTracker::computeVVSCheckLevenbergMarquardtKlt(const unsigned int iter, const unsigned int nbInfos, const vpHomogeneousMatrix &cMoPrev, const vpColVector &error_prev, const vpHomogeneousMatrix &ctTc0_Prev, double &mu, bool &reStartFromLastIncrement) { if(iter != 0 && m_optimizationMethod == vpMbTracker::LEVENBERG_MARQUARDT_OPT){ if(m_error.sumSquare()/(double)(2*nbInfos) > error_prev.sumSquare()/(double)(2*nbInfos)){ mu *= 10.0; if(mu > 1.0) throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged"); cMo = cMoPrev; m_error = error_prev; ctTc0 = ctTc0_Prev; reStartFromLastIncrement = true; } } }