static inline Matrix4d cubicHermiteCoefficients(const Vector4d& p0, const Vector4d& p1, const Vector4d& v0, const Vector4d& v1) { Matrix4d coeff; coeff.col(0) = p0; coeff.col(1) = v0; coeff.col(2) = 3.0 * (p1 - p0) - (2.0 * v0 + v1); coeff.col(3) = 2.0 * (p0 - p1) + (v1 + v0); return coeff; }
Vector7d Sim3:: vee(const Matrix4d & Omega) { Vector7d upsilon_omega_sigma; upsilon_omega_sigma.head<3>() = Omega.col(3).head<3>(); upsilon_omega_sigma.tail<4>() = ScSO3::vee(Omega.topLeftCorner<3,3>()); return upsilon_omega_sigma; }
Matrix4d Sim3:: hat(const Vector7d & v) { Matrix4d Omega; Omega.topLeftCorner<3,3>() = ScSO3::hat(v.tail<4>()); Omega.col(3).head<3>() = v.head<3>(); Omega.row(3) = Vector4d(0., 0., 0., 0.); return Omega; }
// ----------------------------------- 2015.01.07 ------------------------------------------// // Staubli.cpp void CStaubli::GetInverseKin(Rparam *m_Robot, Matrix4d &_des_T, VectorXd &_dq, double _damp_param) { //// _x가 몇 자유도로 들어오는 지 잘 생각해 보아야 한다. VectorXd _dist_x(3); // Xdiff = (Xdes - Xcurr) _dist_x = _des_T.col(3).segment(0, 3) - m_Robot->T06.col(3).segment(0, 3); // x, y, z값에 대한 것만 //cout<<"dist_x: "<<_dist_x(0)<<" "<<_dist_x(1)<<" "<<_dist_x(2)<<endl; VectorXd _dx(6); //while(_dist_x.norm() > 0.01){ // orientation (direction cosine) Vector3d _s1, _s2, _s3, _s1d, _s2d, _s3d; _s1 = m_Robot->T06.col(0).segment(0, 3), _s2 = m_Robot->T06.col(1).segment(0, 3), _s3 = m_Robot->T06.col(2).segment(0, 3); // 현재 로봇의 direction cosine _s1d = _des_T.col(0).segment(0, 3), _s2d = _des_T.col(1).segment(0, 3), _s3d = _des_T.col(2).segment(0, 3); // goal_T 의 direction cosine ////// Generate dx. // goal_T와 현재 로봇 position 과의 차이에다가 현재 로봇의 direction cosine column을 dot product 해줌 _dx(0) = _s1.dot(_dist_x); _dx(1) = _s2.dot(_dist_x); _dx(2) = _s3.dot(_dist_x); //// 일단 orientation은 잘 수렴이 안된다. //// Ossama method //VectorXd pi_d(3); //pi_d.setZero(); //pi_d = -0.5*(_s1.cross(_s1d) + _s2.cross(_s2d) + _s3.cross(_s3d)); // //_dx(3) = pi_d(0); //_dx(4) = pi_d(1); //_dx(5) = pi_d(2); // Jong hwa method _dx(3) = 0.5*(_s3.dot(_s2d) - _s3d.dot(_s2)); _dx(4) = 0.5*(_s1.dot(_s3d) - _s1d.dot(_s3)); _dx(5) = 0.5*(_s2.dot(_s1d) - _s2d.dot(_s1)); //cout << "pi_d: " << pi_d(0) << " " << pi_d(1) << " " << pi_d(2) << endl; //// Change Coordinate.(orientation은 일단 0으로 설정?) MatrixXd Rot(6, 6); Rot.setZero(); for(int i=0; i<3; i++){ Rot.col(i).segment(0, 3) = m_Robot->T06.col(i).segment(0, 3); Rot.col(i+3).segment(3, 3) = m_Robot->T06.col(i).segment(0, 3); } _dx = Rot * _dx; //cout << "dx-----------------\n" << _dx << endl; //JacobiSVD<MatrixXd> svd(m_Robot->Jacobian, ComputeFullU | ComputeFullV); //matrix가 square가 아니면 ComputeThinU ////cout << "A least-squares solution of m*x = rhs is:" << endl << svd.solve(_dx) << endl; //MatrixXd _Jacobian_pinv(6, 6); //MatrixXd singularvals(6, 6); //singularvals = svd.singularValues().asDiagonal(); //double pinvtoler = max(m_Robot->Jacobian.rows(), m_Robot->Jacobian.cols()) * m_Robot->Jacobian.norm() * 2.22*exp(-16.0); ///tolerence 없으면 발산하는 부분이 발생한다. //MatrixXd singularvals_inv(6, 6); //singularvals_inv.setZero(); //for(int i=0; i<6; i++){ // if(singularvals(i, i) > pinvtoler) // diagonal term of singular values // singularvals_inv(i, i) = 1/singularvals(i, i); //} //cout<<"Its singular values are : "<<endl<<svd.singularValues()<<endl; //for(int i=0; i<6; i++){ // for(int j=0; j<6; j++) // cout<<singularvals(i, j)<<" "; // cout<<endl; //} //cout<<"Its singular values inverse are : "<<endl<<svd.singularValues()<<endl; //for(int i=0; i<6; i++){ // for(int j=0; j<6; j++) // cout<<singularvals_inv(i, j)<<" "; // cout<<endl; //} //// 아래 jacobian pinv는 matlab과 비교하여 옳다는 것을 검증했음. //_Jacobian_pinv = svd.matrixV() * singularvals_inv * svd.matrixU().transpose(); //// 이거는 보통 사용하는 것들.. //MatrixXd _jacobian_square(6, 6); //_jacobian_square = m_Robot->Jacobian * m_Robot->Jacobian.transpose(); //_Jacobian_pinv = m_Robot->Jacobian.transpose() * _jacobian_square.inverse(); _dq.resize(6); //_dq = _Jacobian_pinv * _dx; //// use damped least square method. //// It can be easily seen that the joint speeds are only zero if e has become zero. //// A problem arises, however, when the end-effector has to go through a singularity to get to its goal. //// Then, the solution to J^+ “explodes” and joint speeds go to infinity. //// In order to work around this, we can introduce damping to the controller. //// 출처 : 에이치티티피:://correll.cs.colorado.edu/?p=1958 MatrixXd _mat_Identity(6, 6); _mat_Identity.setIdentity(); //// damping 튜닝하는 방법에 대한 스터디도 많다고 함.. 그래도 대부분 휴리스틱 하다고 함. //// 해보니깐 5.0은 약간 왔다리갔다리 하는 편이고 10.0은 지나치게 damped 되는 느낌이 있음. 느림. //// adaptive하게 튜닝해 주는 것이 좋겠다. 초반에는 damping을 크게 하다가 나중에는 적게 하는 방식으로! //// 글구 이건 논문 거리가 될 지는 모르겠는데, damping이 작으면 부드러운 곡선으로 다가가질 않고 이쪽저쪽 튀는 느낌이다. //// damping이 지나치게 작으면 오히려 값에 수렴하는 데 너무 오래 걸 //// damping에 0을 넣으면 jacobian의 inverse와 똑같아져 버리는데, 이건 존재하지 않을 경우가 많다. (pseudo inverse가 아니게 됨) //// 목표지점과의 거리차 * 0.01을 하면 적당한 속도로 도착하는 것 같고, 0.1을 하면 오히려 안좋다. 기준이 뭘까? //// 0.05일 때에도 도달 못하네.. 숫자의 차이에서 오는 차이는 이해를 못 하겠지만, 일단 짐작으로는 거의 도달했을 때 너무 damping이 //// 작아지면 이쪽저쪽 발산하느라 안되는 것으로 생각된다. 일정 값 이하일 때에는 미니멈 리밋 값을 줘야겠다. //// _dq = (m_Robot->Jacobian.transpose() * m_Robot->Jacobian + _damp_param*_damp_param*_mat_Identity).inverse() * _Jacobian_pinv * _dx; if(_damp_param < 2) _damp_param = 2; // 아래 식은 출처논문 : Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares methods _dq = m_Robot->Jacobian.transpose() * (m_Robot->Jacobian * m_Robot->Jacobian.transpose() + _damp_param*_damp_param*_mat_Identity).inverse() * _dx; //cout<<"_dx : "<<_dx<<endl; //cout<<"_dq : "<<_dq<<endl; //} //MatrixXd Jacobian_inv(6, 6); //if(m_Robot->_Jacobian.determinant() < 0.1){ // Jacobian_inv = m_Robot->_Jacobian.transpose(); // cout<<"transpose"<<endl; //} //else{ // Jacobian_inv = m_Robot->_Jacobian.inverse() * m_Robot->_Jacobian.transpose(); // cout<<"pseudo inverse"<<endl; //} //JacobiSVD<MatrixXd> svd(m_Robot->_Jacobian, ComputeFullU | ComputeFullV); //matrix가 square가 아니면 ComputeThinU //cout<<"Its singular values are : "<<endl<<svd.singularValues()<<endl; //cout<<"Its left singular vectors are the columns of the U matrix : " <<endl<<svd.matrixU()<<endl; //cout<<"Its right singular vectors are the columns of the V matrix : " <<endl<<svd.matrixV()<<endl; //cout << "A least-squares solution of m*x = rhs is:" << endl << svd.solve(_dx) << endl; }