def __init__(self, platform, matrix=np.eye(3), offset=np.zeros((3,1)),
**kwargs):
"""
@param matrix: 3x3 matrix to transform true values.
@param offset: 3x1 offset to be added after matrix multiplication.
"""
Sensor.__init__(self, platform, **kwargs)
self._transform = AffineTransform(transform=matrix, translation=offset)
def testVideoGeneration():
samp = loadBVHFile(os.path.join(os.path.dirname(__file__),
'../system/walk.bvh'), 0.01)
spl = SplinedBodyModel(samp)
positionEstimator = ContactTrackingKalmanFilter(
SampledBodyModel.structureCopy(samp),
initialTime = spl.startTime,
initialPosition = spl.position(spl.startTime),
initialVelocity = spl.velocity(spl.startTime))
dt = 0.01
sampleTimes = np.arange(spl.startTime + dt, spl.endTime, dt)
for t in sampleTimes:
data = [{'jointName' : p.name,
'linearAcceleration' : p.acceleration(t)}
for p in spl.joints]
positionEstimator(data, t)
outdir = tempfile.mkdtemp()
filename = os.path.join(outdir, 'movie.avi')
autoPositionCamera()
renderSolenoidCoil(
SolenoidMagneticField(200,20,0.05,0.2, AffineTransform()))
createVideo(filename, [BodyModelRenderer(spl),
VelocityVectorRenderer(spl.getPoint('ltoes_end')),
ContactProbabilityRenderer(spl)],
spl.startTime, spl.startTime + 1, 3, (320, 200))
assert os.path.exists(filename)
shutil.rmtree(outdir)
def __init__(self, platform, matrix=np.eye(3), offset=np.zeros((3,1)),
**kwargs):
"""
@param matrix: 3x3 matrix to transform true values.
@param offset: 3x1 offset to be added after matrix multiplication.
"""
Sensor.__init__(self, platform, **kwargs)
self._transform = AffineTransform(transform=matrix, translation=offset)
def testDistortedField():
random.seed(0)
field = DistortedMagneticField(EarthMagneticField())
field.addDistortion(
SolenoidMagneticField(200, 20, 0.05, 0.2, AffineTransform()))
for i in range(10):
yield checkNonZeroHeadingVariation, field, random.uniform(size=(3, 1),
low=-1,
high=1)
class TransformedSensor(Sensor):
"""
An sensor with an affine transform transfer function.
"""
@prepend_method_doc(Sensor)
def __init__(self, platform, matrix=np.eye(3), offset=np.zeros((3,1)),
**kwargs):
"""
@param matrix: 3x3 matrix to transform true values.
@param offset: 3x1 offset to be added after matrix multiplication.
"""
Sensor.__init__(self, platform, **kwargs)
self._transform = AffineTransform(transform=matrix, translation=offset)
def sensedVoltages(self, t):
return self._transform.apply(self.trueValues(t))
class TransformedSensor(Sensor):
"""
An sensor with an affine transform transfer function.
"""
@prepend_method_doc(Sensor)
def __init__(self, platform, matrix=np.eye(3), offset=np.zeros((3,1)),
**kwargs):
"""
@param matrix: 3x3 matrix to transform true values.
@param offset: 3x1 offset to be added after matrix multiplication.
"""
Sensor.__init__(self, platform, **kwargs)
self._transform = AffineTransform(transform=matrix, translation=offset)
def sensedVoltages(self, t):
return self._transform.apply(self.trueValues(t))
def testCG_to_NED_Quat():
testing.assert_equal(
transforms.convertCGtoNED(Quaternion()).components,
Quaternion(0.5, -0.5, 0.5, -0.5).components)
def testNED_to_CG_Quat():
testing.assert_equal(
transforms.convertNEDtoCG(Quaternion()).components,
Quaternion(0.5, 0.5, -0.5, 0.5).components)
AFFINE_TRANSFORM_TESTS = [
(AffineTransform(transform=np.eye(3)), vector(1, 0, 0), vector(1, 0, 0)),
(AffineTransform(rx=np.pi / 2), vector(0, 0, 1), vector(0, -1, 0)),
(AffineTransform(ry=np.pi / 2), vector(0, 0, 1), vector(1, 0, 0)),
(AffineTransform(rz=np.pi / 2), vector(1, 0, 0), vector(0, 1, 0)),
(AffineTransform(scale=2), vector(1, 0, 0), vector(2, 0, 0)),
(AffineTransform(translation=vector(1, 0, 0)), vector(0, 0,
1), vector(1, 0, 1))
]
def checkAffineTransform_apply(transform, input, expectedOutput):
assert_almost_equal(transform.apply(input), expectedOutput)
def checkAffineTransform_reverse(transform, input, expectedOutput):
assert_almost_equal(transform.reverse(expectedOutput), input)
