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;
}
