Sensors

Basics

There are currently 5 sensors:

1. Position - senses x,y,z position of a body.
2. Ray - senses the distance and color component of the body the ray hits.
3. Proprioceptive - senses the current angle of a joint.
4. Touch - senses if a body is touching another body
5. Vestibular - senses a bodies orientation in space

After the simulation terminates, the results of the sensors from every time step is collected and returned as a numpy matrix. The matrix has indices (sensor_id, svi, time). Sensor ID is the returned ID tag of the sensor. Svi stands for sensor value index. Some sensors sense more than one value. For example: the position sensor returns the x,y,z position of a body which corresponds to svi=0,1,2 respectively.

Double Pendulum Cont.

We add sensors to the double pendulum and evaluate the print the output. Insert the following code after the joints have been sent.

dp-sensors.py

pos_sensor = sim.send_position_sensor(cyl_1)
vestib_sensor = sim.send_vestibular_sensor(cyl_2)
prop_sensor = sim.send_proprioceptive_sensor(slider_joint)

sim.start()
results = sim.wait_to_finish()

# we can access the results matrix directly
print results, '\n'

# or use built in commands to get a specific sensor and svi
# value for every time step

pos_x_sensor_results = sim.get_sensor_data(pos_sensor, svi=0)
pos_y_sensor_results = sim.get_sensor_data(pos_sensor, svi=1)
pos_z_sensor_results = sim.get_sensor_data(pos_sensor, svi=2)
vestib_sensor_results = sim.get_sensor_data(vestib_sensor)
prop_sensor_results = sim.get_sensor_data(prop_sensor)

for t in range(500):
    x = pos_x_sensor_results[t]
    y = pos_y_sensor_results[t]
    z = pos_z_sensor_results[t]
    v = vestib_sensor_results[t]
    p = prop_sensor_results[t]

    output = ('{:3d}:: x:{: 3.1f}, y:{: 3.1f}, z:{: 3.1f}, ' +
              'vestib:{: 3.1f}, prop:{: 3.1f}').format(t, x, y, z, v, p)
    print output

The output should be as follows

[[[ -2.50000000e-01  -2.50000000e-01  -2.49983996e-01 ...,  -2.18925998e-01
    -2.19732001e-01  -2.19999000e-01]
  [  0.00000000e+00   0.00000000e+00  -0.00000000e+00 ...,   0.00000000e+00
     0.00000000e+00  -0.00000000e+00]
  [  2.00000000e+00   1.99860096e+00   1.99580204e+00 ...,   1.87864006e+00
     1.88022494e+00   1.88082099e+00]
  [  0.00000000e+00   0.00000000e+00   0.00000000e+00 ...,   0.00000000e+00
     0.00000000e+00   0.00000000e+00]]

 [[  1.57079601e+00   1.57132196e+00   1.57237196e+00 ...,   1.09842098e+00
     1.10756195e+00   1.11671805e+00]
  [  0.00000000e+00   0.00000000e+00   0.00000000e+00 ...,   0.00000000e+00
     0.00000000e+00   0.00000000e+00]
  [  0.00000000e+00   0.00000000e+00   0.00000000e+00 ...,   0.00000000e+00
     0.00000000e+00   0.00000000e+00]
  [  0.00000000e+00   0.00000000e+00   0.00000000e+00 ...,   0.00000000e+00
     0.00000000e+00   0.00000000e+00]]

 [[  0.00000000e+00   0.00000000e+00   3.30000003e-05 ...,   5.00409007e-01
     5.00358999e-01   5.00317991e-01]
  [  0.00000000e+00   0.00000000e+00   0.00000000e+00 ...,   0.00000000e+00
     0.00000000e+00   0.00000000e+00]
  [  0.00000000e+00   0.00000000e+00   0.00000000e+00 ...,   0.00000000e+00
     0.00000000e+00   0.00000000e+00]
  [  0.00000000e+00   0.00000000e+00   0.00000000e+00 ...,   0.00000000e+00
     0.00000000e+00   0.00000000e+00]]]

  0:: x:-0.2, y: 0.0, z: 2.0, vestib: 1.6, prop: 0.0
  1:: x:-0.2, y: 0.0, z: 2.0, vestib: 1.6, prop: 0.0
  2:: x:-0.2, y:-0.0, z: 2.0, vestib: 1.6, prop: 0.0
  3:: x:-0.2, y:-0.0, z: 2.0, vestib: 1.6, prop: 0.0
  4:: x:-0.2, y:-0.0, z: 2.0, vestib: 1.6, prop: 0.0
  5:: x:-0.2, y: 0.0, z: 2.0, vestib: 1.6, prop: 0.0
  6:: x:-0.2, y: 0.0, z: 2.0, vestib: 1.6, prop: 0.0
  7:: x:-0.2, y: 0.0, z: 2.0, vestib: 1.6, prop: 0.0
  8:: x:-0.2, y: 0.0, z: 1.9, vestib: 1.6, prop: 0.0
  9:: x:-0.2, y: 0.0, z: 1.9, vestib: 1.6, prop: 0.0
 10:: x:-0.2, y:-0.0, z: 1.9, vestib: 1.6, prop: 0.0
 11:: x:-0.2, y: 0.0, z: 1.9, vestib: 1.6, prop: 0.0
 12:: x:-0.2, y: 0.0, z: 1.9, vestib: 1.6, prop: 0.0
 13:: x:-0.2, y: 0.0, z: 1.9, vestib: 1.6, prop: 0.1
 14:: x:-0.2, y:-0.0, z: 1.9, vestib: 1.6, prop: 0.1
 15:: x:-0.2, y: 0.0, z: 1.8, vestib: 1.6, prop: 0.1
 16:: x:-0.2, y: 0.0, z: 1.8, vestib: 1.6, prop: 0.1
 17:: x:-0.2, y: 0.0, z: 1.8, vestib: 1.6, prop: 0.2
 18:: x:-0.1, y: 0.0, z: 1.8, vestib: 1.6, prop: 0.2
 19:: x:-0.1, y:-0.0, z: 1.8, vestib: 1.5, prop: 0.3
 20:: x:-0.1, y: 0.0, z: 1.8, vestib: 1.5, prop: 0.3
 21:: x:-0.1, y:-0.0, z: 1.8, vestib: 1.5, prop: 0.4
 22:: x:-0.0, y:-0.0, z: 1.7, vestib: 1.4, prop: 0.4
----------------------------------------------------
472:: x:-0.1, y: 0.0, z: 1.8, vestib: 0.4, prop: 0.5
473:: x:-0.1, y: 0.0, z: 1.8, vestib: 0.4, prop: 0.5
474:: x:-0.1, y: 0.0, z: 1.8, vestib: 0.5, prop: 0.5
475:: x:-0.1, y: 0.0, z: 1.8, vestib: 0.5, prop: 0.5
476:: x:-0.1, y: 0.0, z: 1.8, vestib: 0.6, prop: 0.5
477:: x:-0.1, y: 0.0, z: 1.8, vestib: 0.6, prop: 0.5
478:: x:-0.1, y: 0.0, z: 1.8, vestib: 0.7, prop: 0.5
479:: x:-0.2, y: 0.0, z: 1.8, vestib: 0.7, prop: 0.5
480:: x:-0.2, y:-0.0, z: 1.8, vestib: 0.8, prop: 0.5
481:: x:-0.2, y:-0.0, z: 1.8, vestib: 0.8, prop: 0.5
482:: x:-0.2, y: 0.0, z: 1.8, vestib: 0.8, prop: 0.5
483:: x:-0.2, y: 0.0, z: 1.8, vestib: 0.9, prop: 0.5
484:: x:-0.2, y: 0.0, z: 1.8, vestib: 0.9, prop: 0.5
485:: x:-0.2, y: 0.0, z: 1.8, vestib: 0.9, prop: 0.5
486:: x:-0.2, y: 0.0, z: 1.8, vestib: 1.0, prop: 0.5
487:: x:-0.2, y: 0.0, z: 1.8, vestib: 1.0, prop: 0.5
488:: x:-0.2, y: 0.0, z: 1.8, vestib: 1.0, prop: 0.5
489:: x:-0.2, y: 0.0, z: 1.8, vestib: 1.0, prop: 0.5
490:: x:-0.2, y: 0.0, z: 1.8, vestib: 1.0, prop: 0.5
491:: x:-0.2, y: 0.0, z: 1.9, vestib: 1.0, prop: 0.5
492:: x:-0.2, y: 0.0, z: 1.9, vestib: 1.1, prop: 0.5
493:: x:-0.2, y: 0.0, z: 1.9, vestib: 1.1, prop: 0.5
494:: x:-0.2, y:-0.0, z: 1.9, vestib: 1.1, prop: 0.5
495:: x:-0.2, y: 0.0, z: 1.9, vestib: 1.1, prop: 0.5
496:: x:-0.2, y: 0.0, z: 1.9, vestib: 1.1, prop: 0.5
497:: x:-0.2, y: 0.0, z: 1.9, vestib: 1.1, prop: 0.5
498:: x:-0.2, y: 0.0, z: 1.9, vestib: 1.1, prop: 0.5
499:: x:-0.2, y:-0.0, z: 1.9, vestib: 1.1, prop: 0.5