void RTKRobotArm::copy(const MathLib::Matrix& from, Eigen::MatrixXd& to) {
	int r = from.RowSize();
	int c = from.ColumnSize();
	for(int i=0; i<r; ++i) {
		for(int j=0; j<c; ++j) {
			to(i,j) = from.At(i,j);
		}
	}
}
/******************************************************************
 * Get jacobian full direction
 *
 * @ J : return direction jacobian T (9 X dof)
 ******************************************************************/
void sKinematics::getJacobianFullDirection(int axis1, int axis2, MathLib::Matrix &J)
{
	int lColSize = J.ColumnSize();
	MathLib::Matrix lJ3(3,lColSize);

	getJacobianPos(lJ3);
	J.SetRow(lJ3.GetRow(0), 0);
	J.SetRow(lJ3.GetRow(1), 1);
	J.SetRow(lJ3.GetRow(2), 2);

	getJacobianDirection(axis1, lJ3);
	J.SetRow(lJ3.GetRow(0), 3);
	J.SetRow(lJ3.GetRow(1), 4);
	J.SetRow(lJ3.GetRow(2), 5);

	getJacobianDirection(axis2, lJ3);
	J.SetRow(lJ3.GetRow(0), 6);
	J.SetRow(lJ3.GetRow(1), 7);
	J.SetRow(lJ3.GetRow(2), 8);
}