Ejemplo n.º 1
0
/******************************************************************
 * 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);
}