// ----------------------------------- 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;
}
// \delta v = (I - h J_v - h^2 J_q)^{-1} (h a + h^2 J_q v)
// \delta q = h (\delta v + v)
void vpSystem::IntegrateDynamicsBackwardEuler(scalar time_step)
{
if ( m_pRoot->m_bIsGround )
{
int i, j, n = m_sState.size();
if ( !n ) return;
ForwardDynamics();
RMatrix _a(n,1), _v(n,1);
RMatrix _Jq(n,n), _Jv(n,n);
for ( i = 0; i < n; i++ )
{
_a[i] = m_sState[i].GetAcceleration();
_v[i] = m_sState[i].GetVelocity();
}
for ( i = 0; i < n; i++ )
{
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jq(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() - LIE_EPS);
}
for ( i = 0; i < n; i++ )
{
m_sState[i].SetVelocity(m_sState[i].GetVelocity() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jv(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
m_sState[i].SetVelocity(m_sState[i].GetVelocity() - LIE_EPS);
}
_Jq *= (time_step * time_step);
_Jv *= -time_step;
_Jv -= _Jq;
for ( i = 0; i < n; i++ ) _Jv[i*(n+1)] += SCALAR_1;
_a *= time_step;
_a += _Jq * _v;
SolveAxEqualB_(_Jv, _a);
for ( i = 0; i < n; i++ ) m_sState[i].SetVelocity(_v[i] + _a[i]);
_a += _v;
_a *= time_step;
for ( i = 0; i < n; i++ ) m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + _a[i]);
} else
{
int i, j, n = m_sState.size(), m = n + 6;
ForwardDynamics();
RMatrix _a(m,1), _v(m,1), _dx(m,1);
RMatrix _Jq(m,m), _Jv(m,m), _M = Eye<scalar>(m,m);
SE3 _T;
se3 gv(SCALAR_0);
for ( i = 0; i < n; i++ )
{
_a[i] = m_sState[i].GetAcceleration();
_v[i] = m_sState[i].GetVelocity();
}
memcpy(&_a[n], &m_pRoot->m_sDV[0], sizeof(se3));
memcpy(&_v[n], &m_pRoot->m_sV[0], sizeof(se3));
for ( i = 0; i < n; i++ )
{
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jq(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jq(n+j,i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() - LIE_EPS);
}
for ( i = 0; i < 6; i++ )
{
gv[i] += LIE_EPS;
_T = m_pRoot->m_sFrame;
m_pRoot->m_sFrame *= Exp(gv);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jq(j,n+i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jq(n+j,n+i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_pRoot->m_sFrame = _T;
gv[i] = SCALAR_0;
}
for ( i = 0; i < n; i++ )
{
m_sState[i].SetVelocity(m_sState[i].GetVelocity() + LIE_EPS);
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jv(j,i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jv(n+j,i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_sState[i].SetVelocity(m_sState[i].GetVelocity() - LIE_EPS);
}
for ( i = 0; i < 6; i++ )
{
m_pRoot->m_sV[i] += LIE_EPS;
ForwardDynamics();
for ( j = 0; j < n; j++ ) _Jv(j,n+i) = (m_sState[j].GetAcceleration() - _a[j]) / LIE_EPS;
for ( j = 0; j < 6; j++ ) _Jv(n+j,n+i) = (m_pRoot->m_sDV[j] - _a[n+j]) / LIE_EPS;
m_pRoot->m_sV[i] -= LIE_EPS;
}
_Jq *= (time_step * time_step);
_Jv *= time_step;
_M -= _Jq;
_M -= _Jv;
_a *= time_step;
_a += _Jq * _v;
SolveAxEqualB(_M, _dx, _a);
for ( i = 0; i < n; i++ ) m_sState[i].SetVelocity(m_sState[i].GetVelocity() + _dx[i]);
for ( i = 0; i < 6; i++ ) m_pRoot->m_sV[i] += _dx[n+i];
_dx += _v;
_dx *= time_step;
for ( i = 0; i < n; i++ ) m_sState[i].SetDisplacement(m_sState[i].GetDisplacement() + _dx[i]);
m_pRoot->m_sFrame *= Exp(se3(_dx[n], _dx[n+1], _dx[n+2], _dx[n+3], _dx[n+4], _dx[n+5]));
}
Reparameterize();
}
