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