def pose(self):
"""
>>> j = RzJoint(gpos = 3.14/2., gvel = 1.)
>>> j.pose
array([[ 7.96326711e-04, -9.99999683e-01, 0.00000000e+00,
0.00000000e+00],
[ 9.99999683e-01, 7.96326711e-04, 0.00000000e+00,
0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00,
0.00000000e+00],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
1.00000000e+00]])
"""
return homogeneousmatrix.rotz(self.gpos[0])
def ipose(self):
return homogeneousmatrix.rotz(-self.gpos[0])