C++ Matrix4d::col方法代码示例

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

// ----------------------------------- 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. 
//.........这里部分代码省略.........