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