Arm's end effector clears octomap instead of going into collision

edit

During planning the arm generates a plan which is not wrong but it ends up clearing the octomap where it should not. This results in a situation where when we try to plan to the next location it thinks it's in free space and hits the environment. The moment I bring the arm away from the octomap manually the octomap fills up again. The following images show what I mean.

Here is my sensors_3d.yaml:

sensors:
  - filtered_cloud_topic: filtered_cloud
    max_range: 5.0
    max_update_rate: 0.1
    padding_offset: 0.1
    padding_scale: 1.0
    point_cloud_topic: /narrow_stereo/points_filtered2
    point_subsample: 1
    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater

Any help is appreciated!