void addCylinderToScene(std::string name,
                        double x,
                        double y,
                        double z,
                        double height,
                        double width)
{
    std::vector<moveit_msgs::CollisionObject> col_objects;
    moveit_msgs::CollisionObject target_obj;
    target_obj.id = name;

    // define object primitives (size and shape)
    shape_msgs::SolidPrimitive cylinder_primitives;
    cylinder_primitives.type = cylinder_primitives.CYLINDER;
    cylinder_primitives.dimensions.resize(2);
    cylinder_primitives.dimensions[0] = height;
    cylinder_primitives.dimensions[1] = width;

    target_obj.primitives.push_back(cylinder_primitives);

    // define object position and orientation
    geometry_msgs::Pose target_pose;
    target_pose.position.x = x;
    target_pose.position.y = y;
    target_pose.position.z = z;

    target_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0.0,0.0,0.0);

    target_obj.primitive_poses.push_back(target_pose);

    target_obj.operation = target_obj.ADD;
    target_obj.header.frame_id = "/base_footprint";

    col_objects.push_back(target_obj);
    planning_scene_ptr->addCollisionObjects(col_objects);
}