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