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