def handlePositionConstraint(self, c, fields):
bodyId = self.bodyNameToId[c.linkName]
pointInBodyFrame = np.asarray(c.pointInLink, dtype=float)
referenceFrame = transformUtils.getNumpyFromTransform(c.referenceFrame)
lowerBound = np.asarray(c.positionTarget, dtype=float) + c.lowerBound
upperBound = np.asarray(c.positionTarget, dtype=float) + c.upperBound
tspan = np.asarray(c.tspan, dtype=float)
pc = pydrakeik.WorldPositionInFrameConstraint(self.rigidBodyTree, bodyId, pointInBodyFrame, referenceFrame, lowerBound, upperBound, tspan)
return pc
def handlePositionConstraint(self, c, fields):
bodyId = self.bodyNameToId[c.linkName]
pointInBodyFrame = np.asarray(c.pointInLink, dtype=float)
referenceFrame = transformUtils.getNumpyFromTransform(c.referenceFrame)
lowerBound = np.asarray(c.positionTarget, dtype=float) + c.lowerBound
upperBound = np.asarray(c.positionTarget, dtype=float) + c.upperBound
tspan = np.asarray(c.tspan, dtype=float)
pc = pydrakeik.WorldPositionInFrameConstraint(self.rigidBodyTree, bodyId, pointInBodyFrame, referenceFrame, lowerBound, upperBound, tspan)
return pc
def testEulerToFrame():
'''
Test some euler converions
'''
rpy = transformations.euler_from_quaternion(transformations.random_quaternion())
frame = transformUtils.frameFromPositionAndRPY([0,0,0], np.degrees(rpy))
mat = transformUtils.getNumpyFromTransform(frame)
mat2 = transformations.euler_matrix(rpy[0], rpy[1], rpy[2])
print mat
print mat2
assert np.allclose(mat, mat2)
def handleQuatConstraint(self, c, fields):
bodyId = self.bodyNameToId[c.linkName]
tolerance = np.radians(c.angleToleranceInDegrees)
tspan = np.asarray(c.tspan, dtype=float)
if isinstance(c.quaternion, vtk.vtkTransform):
quat = transformUtils.getNumpyFromTransform(c.quaternion)
else:
quat = np.asarray(c.quaternion, dtype=float)
if quat.shape == (4,4):
quat = transformUtils.transformations.quaternion_from_matrix(quat)
qc = pydrakeik.WorldQuatConstraint(self.rigidBodyTree, bodyId, quat, tolerance, tspan)
return qc
def testEuler():
'''
Test some euler conversions
'''
quat = transformations.random_quaternion()
pos = np.random.rand(3)
frame = transformUtils.transformFromPose(pos, quat)
mat = transformUtils.getNumpyFromTransform(frame)
rpy = transformUtils.rollPitchYawFromTransform(frame)
rpy2 = transformations.euler_from_matrix(mat)
print rpy
print rpy2
assert np.allclose(rpy, rpy2)
def testEulerToFrame():
'''
Test some euler converions
'''
rpy = transformations.euler_from_quaternion(
transformations.random_quaternion())
frame = transformUtils.frameFromPositionAndRPY([0, 0, 0], np.degrees(rpy))
mat = transformUtils.getNumpyFromTransform(frame)
mat2 = transformations.euler_matrix(rpy[0], rpy[1], rpy[2])
print(mat)
print(mat2)
assert np.allclose(mat, mat2)
def testEuler():
'''
Test some euler conversions
'''
quat = transformations.random_quaternion()
pos = np.random.rand(3)
frame = transformUtils.transformFromPose(pos, quat)
mat = transformUtils.getNumpyFromTransform(frame)
rpy = transformUtils.rollPitchYawFromTransform(frame)
rpy2 = transformations.euler_from_matrix(mat)
print(rpy)
print(rpy2)
assert np.allclose(rpy, rpy2)
def handleQuatConstraint(self, c, fields):
bodyId = self.bodyNameToId[c.linkName]
tolerance = np.radians(c.angleToleranceInDegrees)
tspan = np.asarray(c.tspan, dtype=float)
if isinstance(c.quaternion, vtk.vtkTransform):
quat = transformUtils.getNumpyFromTransform(c.quaternion)
else:
quat = np.asarray(c.quaternion, dtype=float)
if quat.shape == (4,4):
quat = transformUtils.transformations.quaternion_from_matrix(quat)
qc = pydrakeik.WorldQuatConstraint(self.rigidBodyTree, bodyId, quat, tolerance, tspan)
return qc
def testTransform():
'''
test transformFromPose --> getAxesFromTransform is same as quat --> matrix
'''
quat = transformations.random_quaternion()
pos = np.random.rand(3)
frame = transformUtils.transformFromPose(pos, quat)
axes = transformUtils.getAxesFromTransform(frame)
mat = transformUtils.getNumpyFromTransform(frame)
assert np.allclose(mat[:3,:3], np.array(axes).transpose())
mat2 = transformations.quaternion_matrix(quat)
mat2[:3,3] = pos
print mat
print mat2
assert np.allclose(mat, mat2)
def testTransform():
'''
test transformFromPose --> getAxesFromTransform is same as quat --> matrix
'''
quat = transformations.random_quaternion()
pos = np.random.rand(3)
frame = transformUtils.transformFromPose(pos, quat)
axes = transformUtils.getAxesFromTransform(frame)
mat = transformUtils.getNumpyFromTransform(frame)
assert np.allclose(mat[:3, :3], np.array(axes).transpose())
mat2 = transformations.quaternion_matrix(quat)
mat2[:3, 3] = pos
print(mat)
print(mat2)
assert np.allclose(mat, mat2)
