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